00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #include <stdio.h>
00014 #include "posemath.h"
00015 #include "mathprnt.h"
00016
00017 static void test_math_printf()
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 }
00046
00047
00048 int testcase_pmrpyrot(PmRpy rpy)
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 }
00076
00077 int test_pmrpyrot()
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 }
00160
00161 int fullcircle_test(PmRotationVector rv)
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 }
00178
00179
00180 int main()
00181 {
00182
00183
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 }