Main Page   Class Hierarchy   Alphabetical List   Data Structures   File List   Data Fields   Globals  

testpmc.c File Reference

#include <stdio.h>
#include "posemath.h"
#include "mathprnt.h"

Include dependency graph for testpmc.c:

Include dependency graph

Go to the source code of this file.

Functions

void test_math_printf ()
int testcase_pmrpyrot (PmRpy rpy)
int test_pmrpyrot ()
int fullcircle_test (PmRotationVector rv)
int main ()

Function Documentation

void test_math_printf (    )  [static]
 

Definition at line 17 of file testpmc.c.

00018 {
00019   VECTOR v = {1.0, 2.0, 3.0};
00020   double dbl = 3.14;
00021   QUATERNION quat = {1., 2., 3., 4.};
00022   MATRIX mat = { {1., 2., 3.},
00023                  {4., 5., 6.},
00024                  {7., 8., 9.} };
00025   POSE pose = { {1., 2., 3.}, {4., 5., 6., 7.}};
00026   char string[256];
00027 
00028   pmQuatNorm(quat, &quat);
00029   pmQuatNorm(pose.rot, &pose.rot);
00030 
00031   pmSprintf(string, "Hi there!");
00032   printf("%s\n", string);
00033 
00034   pmSprintf(string, "This is a real test:  vector = %4v \nfloat = %4f",
00035                v, dbl);
00036   printf("%s\n", string);
00037   pmPrintf("Hi this vector: %v\n%f", v, dbl);
00038   pmFprintf(stdout, " number = %f, vector = %v\n", dbl, v);
00039   pmPrintf("vector = %v \nformated vector = %6.3v\n", v, v);
00040   pmPrintf("quaternion = %q \nformated quaternion = %6.3q\n", quat, quat);
00041   pmPrintf("quaternion = %Q \nformated quaternion = %6.3Q\n", quat, quat);
00042   pmPrintf("matrix = %m \nformatted matrix = %6.3m\n", mat, mat);
00043   pmPrintf("pose = %p \nformatted pose = %6.3p\n", pose, pose);
00044   pmPrintf("Pose = %P \nFormatted Pose = %6.3P\n", pose, pose);
00045 }

int testcase_pmrpyrot (  PmRpy    rpy ) 
 

Definition at line 48 of file testpmc.c.

Referenced by test_pmrpyrot(). 

00049 {
00050   PmQuaternion q;
00051   PmRotationVector v1,v2;
00052   v1.x = v1.y = v1.z = v1.s = 0.0;
00053   v2.x = v2.y = v2.z = v2.s = 0.0;
00054   q.x = q.y = q.z = q.s = 0.0;
00055   
00056 
00057   printf("\n********************************\n");
00058   printf("rpy = (r = %f PI, p =  %f PI, y =  %f PI)\n",
00059          rpy.r/PI, rpy.p/PI, rpy.y/PI);
00060   pmRpyRotConvert(rpy,&v1);
00061   printf("direct convert : v1 = (s=%f, x= %f y=%f, z=%f)\n", v1.s, v1.x, v1.y,v1.z);
00062   pmRpyQuatConvert(rpy,&q);
00063   printf("quaternion intermediate : q = (s=%f, x= %f y=%f, z=%f)\n", q.s, q.x, q.y,q.z);
00064   pmQuatRotConvert(q,&v2);
00065   printf("convert via quat: v2 = (s=%f, x=%f, y=%f,  z=%f)\n", v2.s, v2.x, v2.y,v2.z);
00066   pmRotRpyConvert(v2,&rpy);
00067   printf("convert back direct: rpy = (r = %f PI, p =  %f PI, y =  %f PI)\n",
00068          rpy.r/PI, rpy.p/PI, rpy.y/PI);
00069   pmRotQuatConvert(v1,&q);
00070   pmQuatRpyConvert(q,&rpy);
00071   printf("convert back via quat: rpy = (r = %f PI, p =  %f PI, y =  %f PI)\n",
00072          rpy.r/PI, rpy.p/PI, rpy.y/PI);
00073   printf("********************************\n");
00074   return 0;
00075 }

int test_pmrpyrot (    ) 
 

Definition at line 77 of file testpmc.c.

00078 {
00079   PmRpy rpy;
00080 
00081   rpy.r = 0; rpy.p = 0; rpy.y = 0;
00082   testcase_pmrpyrot(rpy);
00083   rpy.r = PI/4; rpy.p = 0; rpy.y = 0;
00084   testcase_pmrpyrot(rpy);
00085   rpy.r = 0; rpy.p = PI/4; rpy.y = 0;
00086   testcase_pmrpyrot(rpy);
00087   rpy.r = 0; rpy.p = 0; rpy.y = PI/4;
00088   testcase_pmrpyrot(rpy);
00089   rpy.r = 0; rpy.p = PI/4; rpy.y = PI/4;
00090   testcase_pmrpyrot(rpy);
00091   rpy.r = PI/4; rpy.p = PI/4; rpy.y = PI/4;
00092   testcase_pmrpyrot(rpy);
00093   rpy.r = PI/4; rpy.p = PI/4; rpy.y = 0;
00094   testcase_pmrpyrot(rpy);
00095   rpy.r = PI/4; rpy.p = 0; rpy.y = PI/4;
00096   testcase_pmrpyrot(rpy);
00097 
00098   printf("\n\n+++++++++++++++++++++++++++++++++++++++++++\n\n");
00099 
00100   rpy.r = 0; rpy.p = 0; rpy.y = 0;
00101   testcase_pmrpyrot(rpy);
00102   rpy.r = PI/3; rpy.p = 0; rpy.y = 0;
00103   testcase_pmrpyrot(rpy);
00104   rpy.r = 0; rpy.p = PI/3; rpy.y = 0;
00105   testcase_pmrpyrot(rpy);
00106   rpy.r = 0; rpy.p = 0; rpy.y = PI/3;
00107   testcase_pmrpyrot(rpy);
00108   rpy.r = 0; rpy.p = PI/3; rpy.y = PI/3;
00109   testcase_pmrpyrot(rpy);
00110   rpy.r = PI/3; rpy.p = PI/3; rpy.y = PI/3;
00111   testcase_pmrpyrot(rpy);
00112   rpy.r = PI/3; rpy.p = PI/3; rpy.y = 0;
00113   testcase_pmrpyrot(rpy);
00114   rpy.r = PI/3; rpy.p = 0; rpy.y = PI/3;
00115   testcase_pmrpyrot(rpy);
00116 
00117 
00118 
00119   printf("\n\n+++++++++++++++++++++++++++++++++++++++++++\n\n");
00120 
00121   rpy.r = 0; rpy.p = 0; rpy.y = 0;
00122   testcase_pmrpyrot(rpy);
00123   rpy.r = PI/2; rpy.p = 0; rpy.y = 0;
00124   testcase_pmrpyrot(rpy);
00125   rpy.r = 0; rpy.p = PI/2; rpy.y = 0;
00126   testcase_pmrpyrot(rpy);
00127   rpy.r = 0; rpy.p = 0; rpy.y = PI/2;
00128   testcase_pmrpyrot(rpy);
00129   rpy.r = 0; rpy.p = PI/2; rpy.y = PI/2;
00130   testcase_pmrpyrot(rpy);
00131   rpy.r = PI/2; rpy.p = PI/2; rpy.y = PI/2;
00132   testcase_pmrpyrot(rpy);
00133   rpy.r = PI/2; rpy.p = PI/2; rpy.y = 0;
00134   testcase_pmrpyrot(rpy);
00135   rpy.r = PI/2; rpy.p = 0; rpy.y = PI/2;
00136   testcase_pmrpyrot(rpy);
00137 
00138 
00139   printf("\n\n+++++++++++++++++++++++++++++++++++++++++++\n\n");
00140 
00141   rpy.r = 0; rpy.p = 0; rpy.y = 0;
00142   testcase_pmrpyrot(rpy);
00143   rpy.r = 2*PI; rpy.p = 0; rpy.y = 0;
00144   testcase_pmrpyrot(rpy);
00145   rpy.r = 0; rpy.p = 2*PI; rpy.y = 0;
00146   testcase_pmrpyrot(rpy);
00147   rpy.r = 0; rpy.p = 0; rpy.y = 2*PI;
00148   testcase_pmrpyrot(rpy);
00149   rpy.r = 0; rpy.p = 2*PI; rpy.y = 2*PI;
00150   testcase_pmrpyrot(rpy);
00151   rpy.r = 2*PI; rpy.p = 2*PI; rpy.y = 2*PI;
00152   testcase_pmrpyrot(rpy);
00153   rpy.r = 2*PI; rpy.p = 2*PI; rpy.y = 0;
00154   testcase_pmrpyrot(rpy);
00155   rpy.r = 2*PI; rpy.p = 0; rpy.y = 2*PI;
00156   testcase_pmrpyrot(rpy);
00157 
00158   return(0);
00159 }

int fullcircle_test (  PmRotationVector    rv ) 
 

Definition at line 161 of file testpmc.c.

Referenced by main(). 

00162 {
00163   PmRotationVector rv_out;
00164   PmEulerZyx zyx;
00165   double rotation_scaler;
00166   int deg;
00167   printf("rv: %f %f %f %f\n\n",rv.s,rv.x,rv.y,rv.z);
00168 
00169   for(deg = 0; deg <= 360; deg +=5)
00170     {
00171       rotation_scaler = deg*2*PI/360/rv.s;
00172       pmRotScalMult(rv,rotation_scaler, &rv_out);
00173       printf("rv_out: %f %f %f %f\n",rv_out.s, rv_out.x, rv_out.y, rv_out.z);
00174       pmRotZyxConvert(rv_out,&zyx);
00175       printf("zyx: %f %f %f\n",zyx.z,zyx.y, zyx.x);
00176     }
00177 }

int main (    ) 
 

Definition at line 180 of file testpmc.c.

00181 {
00182   // test_math_printf();
00183   //test_pmzyxrot();
00184   PmRotationVector rv;
00185   PmEulerZyx zyx;
00186   zyx.z = 0; zyx.y = PI/4; zyx.x = PI/4;
00187   pmZyxRotConvert(zyx,&rv);
00188   fullcircle_test(rv);
00189 
00190   return 0;
00191 }

Generated on Sun Dec 2 15:58:01 2001 for rcslib by doxygen1.2.11.1 written by Dimitri van Heesch, © 1997-2001