#include "rcs_defs.hh"#include <stdio.h>#include <stdlib.h>#include <math.h>#include <float.h>#include <iostream.h>#include "posemath.h"#include "mathprnt.h"Include dependency graph for testpmcpp.cc:
Go to the source code of this file.
Data Structures | |
| struct | VECTOR |
Defines | |
| #define | forceError() fprintf(stderr, "\tv--- forced error on line %d\n", __LINE__) |
| #define | testAssert(cond) ((cond) ? 1 : (fprintf(stderr, "error on line %d\n", __LINE__), numErrors++, 0)) |
Functions | |
| void | testPrint () |
| void | testCart () |
| void | testCyl () |
| void | testQuat () |
| void | testMat () |
| void | testHom () |
| void | testPose () |
| void | testOther () |
| int | main () |
Variables | |
| int | numErrors = 0 |
|
|
Definition at line 46 of file testpmcpp.cc. |
|
|
Definition at line 47 of file testpmcpp.cc. Referenced by testCart(), testCyl(), testHom(), testMat(), testOther(), testPose(), and testQuat().
|
|
|
Definition at line 50 of file testpmcpp.cc. 00051 {
00052 PM_CARTESIAN v(1.0, 2.0, 3.0);
00053 PM_QUATERNION quat(1., 2., 3., 4.);
00054 PM_ROTATION_MATRIX mat(1., 2., 3.,
00055 4., 5., 6.,
00056 7., 8., 9.);
00057 PM_POSE pose(1., 2., 3., 4., 5., 6., 7.);
00058
00059 quat = norm(quat);
00060 pose.rot = norm(pose.rot);
00061
00062 #ifndef NO_IOSTREAM
00063 cout << "vector = " << v << endl;
00064 cout << "quat = " << quat << endl;
00065 cout << "mat = " << endl << mat;
00066 cout << "pose = " << pose << endl;
00067 #endif
00068 }
|
|
|
Definition at line 71 of file testpmcpp.cc. 00072 {
00073 double d;
00074 PM_CARTESIAN v1(1, 2, 3);
00075 PM_CARTESIAN v2(1, 2, 3);
00076 PM_CARTESIAN *pv;
00077
00078 // test arg ctor
00079 testAssert(v1 == v2);
00080
00081 // test new arg ctor
00082 pv = new PM_CARTESIAN(4, 5, 6);
00083 testAssert(PM_CARTESIAN(4, 5, 6) == *pv);
00084
00085 // test indexing
00086 d = v2[-1]; // ERROR
00087 d = v2[3]; // ERROR
00088 d = v2[2];
00089 v2[2] = v2[1];
00090 v2[1] = v2[0];
00091 v2[0] = d;
00092 testAssert(PM_CARTESIAN(3, 1, 2) == v2);
00093
00094 // test assignment
00095 v1 = PM_CARTESIAN(0, 0, 0);
00096 *pv = PM_CARTESIAN(0, 0, 0);
00097 *pv = v1 = v2;
00098 testAssert(PM_CARTESIAN(3, 1, 2) == v1);
00099 testAssert(PM_CARTESIAN(3, 1, 2) == v2);
00100 testAssert(PM_CARTESIAN(3, 1, 2) == *pv);
00101
00102 // test unary operators
00103 *pv = -v1;
00104 testAssert(PM_CARTESIAN(3, 1, 2) == v1);
00105 testAssert(PM_CARTESIAN(-3, -1, -2) == *pv);
00106 *pv = +v1;
00107 testAssert(PM_CARTESIAN(3, 1, 2) == v1);
00108 testAssert(PM_CARTESIAN(3, 1, 2) == *pv);
00109
00110 // test +/-
00111 v1 = v1 + v2;
00112 testAssert(PM_CARTESIAN(6, 2, 4) == v1);
00113 testAssert(PM_CARTESIAN(3, 1, 2) == v2);
00114 v2 = v2 - v1;
00115 testAssert(PM_CARTESIAN(6, 2, 4) == v1);
00116 testAssert(PM_CARTESIAN(-3, -1, -2) == v2);
00117
00118 // test scalar *, /
00119 v1 = v1 / 2;
00120 testAssert(PM_CARTESIAN(3, 1, 2) == v1);
00121 v2 = v2 * 2;
00122 testAssert(PM_CARTESIAN(-6, -2, -4) == v2);
00123
00124 // test ==
00125 v1 = v2;
00126 testAssert(v1 == v2);
00127 v1 = v1 + PM_CARTESIAN(1, 1, 1) * V_FUZZ * 0.99;
00128 testAssert(v1 == v2);
00129 v1 = v2;
00130 v1 = v1 + PM_CARTESIAN(1, 1, 1) * V_FUZZ * 1.01;
00131 testAssert(v1 != v2);
00132
00133 // test dot
00134 v1 = PM_CARTESIAN(1, 2, 3);
00135 v2 = PM_CARTESIAN(4, 5, 6);
00136 testAssert(32 == dot(v1, v2));
00137
00138 // test cross
00139 v1 = PM_CARTESIAN(1, 2, 3);
00140 v2 = PM_CARTESIAN(4, 5, 6);
00141 testAssert(PM_CARTESIAN(-3, 6, -3) == cross(v1, v2));
00142
00143 // test mag
00144 v1 = PM_CARTESIAN(1, 2, 3);
00145 v2 = PM_CARTESIAN(4, 5, 6);
00146 testAssert(sqrt((double) 14.0) == mag(v1));
00147
00148 // test norm
00149 v1 = unit(PM_CARTESIAN(1, 2, 3));
00150 testAssert(1.0 == mag(v1));
00151
00152 // test disp
00153 v1 = PM_CARTESIAN(1, 2, 3);
00154 v2 = PM_CARTESIAN(4, 5, 6);
00155 testAssert(sqrt((double)27.0) == disp(v1, v2));
00156
00157 // test inv
00158 v1 = PM_CARTESIAN(1, 1, 1);
00159 #if 0
00160 // g++/gcc versions 2.8.x and 2.9.x
00161 // will complain that the call to PM_CARTESIAN(PM_CARTESIAN) is
00162 // ambigous. (2.7.x and some others allow it)
00163 testAssert(1 == dot(v1, inv(v1)));
00164 #endif
00165
00166 // test delete
00167 delete pv;
00168 }
|
|
|
Definition at line 170 of file testpmcpp.cc. 00171 {
00172 PM_CYLINDRICAL c1;
00173 PM_CYLINDRICAL c2(PM_PI_2, 2, 3);
00174 PM_CYLINDRICAL *pv;
00175 PM_CARTESIAN v1, v2;
00176
00177 // test arg ctor
00178 testAssert(PM_CYLINDRICAL(PM_PI_2, 2, 3) == c2);
00179
00180 // test new arg ctor
00181 pv = new PM_CYLINDRICAL(PM_PI_4, 5, 6);
00182 testAssert(PM_CYLINDRICAL(PM_PI_4, 5, 6) == *pv);
00183
00184 // test assignment
00185 c1 = PM_CYLINDRICAL(PM_PI_2, 1, 2);
00186 c2 = PM_CYLINDRICAL(0, 0, 0);
00187 *pv = PM_CYLINDRICAL(0, 0, 0);
00188 *pv = c2 = c1;
00189 testAssert(PM_CYLINDRICAL(PM_PI_2, 1, 2) == c1);
00190 testAssert(PM_CYLINDRICAL(PM_PI_2, 1, 2) == c2);
00191 testAssert(PM_CYLINDRICAL(PM_PI_2, 1, 2) == *pv);
00192
00193 // test unary operators
00194 *pv = -c1;
00195 testAssert(PM_CYLINDRICAL(PM_PI_2, 1, 2) == c1);
00196 testAssert(PM_CYLINDRICAL(-PM_PI_2, 1, -2) == *pv);
00197 *pv = +c1;
00198 testAssert(PM_CYLINDRICAL(PM_PI_2, 1, 2) == c1);
00199 testAssert(PM_CYLINDRICAL(PM_PI_2, 1, 2) == *pv);
00200
00201 // test +/-
00202 v1 = PM_CARTESIAN(1, 2, 3);
00203 v2 = PM_CARTESIAN(4, 5, 6);
00204
00205 c1 = v1;
00206 c2 = v2;
00207
00208 v1 = v1 + v2;
00209 c1 = c1 + c2;
00210
00211 testAssert(PM_CARTESIAN(c1) == v1);
00212 testAssert(c1 == PM_CYLINDRICAL(v1));
00213
00214 // test scalar *, /
00215 c1 = PM_CYLINDRICAL(-PM_PI_4, -2, 3);
00216 c2 = c1 / 2.0;
00217 testAssert(PM_CYLINDRICAL(-PM_PI_4, -1, 1.5) == c2);
00218 c2 = c1 * 2.0;
00219 testAssert(PM_CYLINDRICAL(-PM_PI_4, -4, 6) == c2);
00220
00221 // test dot
00222 #ifndef linux_2_2_5
00223 v1 = c1 = PM_CYLINDRICAL(1, 2, 3);
00224 v2 = c2 = PM_CYLINDRICAL(4, 5, 6);
00225 testAssert(dot(c1, c2) == dot(v1, v2));
00226
00227 // test cross
00228 v1 = c1 = PM_CYLINDRICAL(1, 2, 3);
00229 v2 = c2 = PM_CYLINDRICAL(4, 5, 6);
00230 testAssert(cross(c1, c2) == cross(v1, v2));
00231
00232 // test mag
00233 c1 = PM_CYLINDRICAL(1, 2, 3);
00234 testAssert(sqrt((double) 13.0) == mag(c1));
00235
00236 // test norm
00237 // c1 = norm(PM_CYLINDRICAL(1, 2, 3));
00238 // testAssert(1.0 == mag(c1));
00239
00240 // test inv
00241 c1 = PM_CYLINDRICAL(1, 1, 1);
00242 #if 0
00243 // g++/gcc versions 2.8.x and 2.9.x
00244 // will complain that the call to PM_CARTESIAN(PM_CARTESIAN) is
00245 // ambigous. (2.7.x and some others allow it)
00246 testAssert(1 == dot(c1, inv(c1)));
00247 #endif
00248 #endif
00249
00250 // test delete
00251 delete pv;
00252 }
|
|
|
Definition at line 254 of file testpmcpp.cc. 00255 {
00256 double d;
00257 PM_CARTESIAN v1;
00258 PM_CARTESIAN v2;
00259 PM_QUATERNION q1;
00260 PM_QUATERNION q2(1, 0, 0, 0);
00261 PM_QUATERNION *pq;
00262
00263 // test arg ctor
00264 testAssert(PM_QUATERNION(1, 0, 0, 0) == q2);
00265
00266 // test new arg ctor
00267 pq = new PM_QUATERNION(1, 0, 0, 0);
00268 testAssert(PM_QUATERNION(1, 0, 0, 0) == *pq);
00269
00270 // test indexing
00271 d = q2[-1]; // ERROR
00272 d = q2[4]; // ERROR
00273 q1[0] = q2[0];
00274 q1[1] = q2[1];
00275 q1[2] = q2[2];
00276 q1[3] = q2[3];
00277 testAssert(q1 == q2);
00278
00279 // test assignment
00280 q1 = PM_QUATERNION(1, 0, 0, 0);
00281 q2 = PM_QUATERNION(2, 3, 4, 5);
00282 *pq = PM_QUATERNION(6, 7, 8, 9);
00283 *pq = q2 = q1;
00284 testAssert(PM_QUATERNION(1, 0, 0, 0) == q1);
00285 testAssert(PM_QUATERNION(1, 0, 0, 0) == q2);
00286 testAssert(PM_QUATERNION(1, 0, 0, 0) == *pq);
00287
00288 // test rotation vector assignment
00289 /* .
00290 quaternion buffs will know that 90 rotations about X, Y, and Z
00291 axes are
00292 (sqrt(2)/2, sqrt(2)/2, 0, 0),
00293 (sqrt(2)/2, 0, sqrt(2)/2, 0), and
00294 (sqrt(2)/2, 0, 0, sqrt(2)/2), respectively.
00295 */
00296 d = sqrt(2.0) / 2.0;
00297 q1 = PM_ROTATION_VECTOR(PM_PI_2, 1, 0, 0);
00298 testAssert(PM_QUATERNION(d, d, 0, 0) == q1);
00299 q1 = PM_ROTATION_VECTOR(PM_PI_2, 0, 1, 0);
00300 testAssert(PM_QUATERNION(d, 0, d, 0) == q1);
00301 q1 = PM_ROTATION_VECTOR(PM_PI_2, 0, 0, 1);
00302 testAssert(PM_QUATERNION(d, 0, 0, d) == q1);
00303
00304 // test unary operators
00305 q1 = PM_ROTATION_VECTOR(PM_PI_2, 1, 1, 1);
00306 q2 = +q1;
00307 testAssert(q1 == q2);
00308 q2 = -q1;
00309 testAssert(q1 != q2);
00310 q2 = -q2;
00311 testAssert(q1 == q2);
00312
00313 // test Q * Q and Q * V
00314 v1 = PM_CARTESIAN(1, 2, 3);
00315 v2 = PM_CARTESIAN(1, 2, 3);
00316 q1 = PM_ROTATION_VECTOR(PM_PI_2, 0.5, 1, 1);
00317 q2 = PM_ROTATION_VECTOR(PM_PI_2, 1, 0.5, 1);
00318 *pq = PM_ROTATION_VECTOR(PM_PI_2, 1, 1, 0.5);
00319 v1 = q1 * v1;
00320 v1 = q2 * v1;
00321 v1 = *pq * v1;
00322 v2 = *pq * q2 * q1 * v2;
00323 testAssert(v1 == v2);
00324
00325 // test scaling
00326 q1 = PM_ROTATION_VECTOR(PM_PI_2, 1, -1, 0.5);
00327 q2 = PM_ROTATION_VECTOR(PM_PI_2 / 2.0, 1, -1, 0.5);
00328 q1 = q1 / 2.0;
00329 testAssert(q1 == q2);
00330 q1 = PM_ROTATION_VECTOR(PM_PI_2, 1, -1, 0.5);
00331 q2 = PM_ROTATION_VECTOR(PM_PI_2 * 2.0, 1, -1, 0.5);
00332 q1 = q1 * 2.0;
00333 testAssert(q1 == q2);
00334
00335 // test norm, isNorm
00336 q1.s = 1;
00337 q1.x = 2;
00338 q1.y = 3;
00339 q1.z = 4;
00340 testAssert(! isNorm(q1));
00341 q1 = norm(q1);
00342 testAssert(isNorm(q1));
00343
00344 // test inv
00345 q1 = PM_QUATERNION(2, 3, 4, 5);
00346 q2 = inv(q1);
00347 testAssert(q1 * q2 == PM_QUATERNION(1, 0, 0, 0));
00348
00349 // test delete
00350 delete pq;
00351 }
|
|
|
Definition at line 353 of file testpmcpp.cc. 00354 {
00355 PM_ROTATION_MATRIX m1(PM_ROTATION_VECTOR(PM_PI_2, 1, 2, 3));
00356 PM_ROTATION_MATRIX m2 = PM_ROTATION_VECTOR(-PM_PI_4, 2, 0, -1);
00357 PM_ROTATION_MATRIX m3;
00358 PM_QUATERNION q1(PM_ROTATION_VECTOR(PM_PI_2, 1, 2, 3));
00359 PM_QUATERNION q2 = PM_ROTATION_VECTOR(-PM_PI_4, 2, 0, -1);
00360 PM_QUATERNION q3;
00361
00362
00363 // use implicit QQ mult
00364 m3 = m1 * m2;
00365
00366 // use explicit QQ mult
00367 q3 = q1 * q2;
00368
00369 // should be equal
00370 testAssert(q3 == m3);
00371
00372 // set them equal via conversion
00373 q1 = m3;
00374 testAssert(q1 == q3);
00375 }
|
|
|
Definition at line 377 of file testpmcpp.cc. 00378 {
00379 PM_HOMOGENEOUS h1, h2;
00380 PM_POSE p1;
00381
00382 p1 = PM_POSE(PM_CARTESIAN(1, 2, 3), PM_ROTATION_MATRIX(PM_ROTATION_VECTOR(PM_PI_2, 1, 2, 3)));
00383 h1 = p1;
00384 testAssert(p1 == h1);
00385
00386 h2 = -h1;
00387 testAssert(h1 * h2 == PM_POSE(0, 0, 0, 1, 0, 0, 0));
00388 }
|
|
|
Definition at line 390 of file testpmcpp.cc. 00391 {
00392 PM_POSE p;
00393 PM_CARTESIAN v, v1, v2;
00394
00395 p = PM_POSE(PM_CARTESIAN(1, 2, 3), PM_ROTATION_MATRIX(PM_ROTATION_VECTOR(PM_PI_2, 1, 2, 3)));
00396 v = PM_CARTESIAN(1, 1, 2);
00397
00398 v1 = p * v;
00399 v2 = p.rot * v + p.tran;
00400
00401 testAssert(v1 == v2);
00402 }
|
|
|
Definition at line 404 of file testpmcpp.cc. 00405 {
00406 PM_RPY rpy1, rpy2;
00407 PM_EULER_ZYZ zyz1, zyz2;
00408 PM_EULER_ZYX zyx1, zyx2;
00409 PM_ROTATION_VECTOR v1;
00410 PM_QUATERNION q1, q2;
00411
00412 v1 = PM_ROTATION_VECTOR(PM_PI_2, 1, 2, 3);
00413
00414 rpy1 = PM_ROTATION_MATRIX(v1);
00415 zyz1 = PM_ROTATION_MATRIX(v1);
00416 zyx1 = PM_ROTATION_MATRIX(v1);
00417 rpy2 = PM_ROTATION_MATRIX(v1);
00418 zyz2 = PM_ROTATION_MATRIX(v1);
00419 zyx2 = PM_ROTATION_MATRIX(v1);
00420
00421 testAssert(rpy1 == rpy2);
00422 testAssert(zyz1 == zyz2);
00423 testAssert(zyx1 == zyx2);
00424
00425 rpy2 = - rpy1;
00426 zyz2 = - zyz1;
00427 zyx2 = - zyx1;
00428
00429 testAssert(rpy1 != rpy2);
00430 testAssert(zyz1 != zyz2);
00431 testAssert(zyx1 != zyx2);
00432
00433 q1 = rpy1;
00434 q2 = zyz2;
00435 testAssert(q1 * q2 == PM_QUATERNION(1, 0, 0, 0));
00436
00437 q1 = rpy1;
00438 q2 = zyx2;
00439 testAssert(q1 * q2 == PM_QUATERNION(1, 0, 0, 0));
00440
00441 // FIXME-- need assignment operators for all types
00442 // zyz1 = zyx1;
00443 }
|
|
|
Definition at line 459 of file testpmcpp.cc. 00461 {
00462 testPrint();
00463 testCart();
00464 testQuat();
00465 testMat();
00466 testCyl();
00467 testHom();
00468 testPose();
00469 testOther();
00470
00471 if (0 != numErrors)
00472 {
00473 fprintf(stderr, "%d errors\n", numErrors);
00474 exit(1);
00475 }
00476 else
00477 {
00478 exit(0);
00479 }
00480 }
|
|
|
Definition at line 45 of file testpmcpp.cc. |
1.2.11.1 written by Dimitri van Heesch,
© 1997-2001