Source code: rcs/posemath/testpm.java
1 /*
2 testpm.java
3
4 Test code for Java version of pose math library
5
6 Modification history:
7
8 7-Feb-1997 FMP removed AXIS; mathQ -> pmQuat
9 30-Jan-1997 FMP created
10 */
11
12 package rcs.posemath;
13
14
15 public class testpm
16 {
17
18 static public void testAssert(boolean test) throws Exception
19 {
20 if(test)
21 {
22 return;
23 }
24 throw new Exception("Assertion Failed!");
25 }
26
27 static public void testAssert(boolean test, String msg)
28 {
29 if(test)
30 {
31 return;
32 }
33 System.out.println("Assertion failed! : "+msg);
34 Thread.dumpStack();
35 }
36
37 static void testPrint()
38 {
39 try
40 {
41 PmCartesian v = new PmCartesian(1.0, 2.0, 3.0);
42 double dbl = 3.14;
43 PmQuaternion quat = new PmQuaternion(1., 2., 3., 4.);
44 PmRotationMatrix mat = new PmRotationMatrix(
45 1., 2., 3.,
46 4., 5., 6.,
47 7., 8., 9.
48 );
49
50 PmPose pose = new PmPose(1., 2., 3., 4., 5., 6., 7.);
51
52 Posemath.pmQuatNorm(quat,quat);
53 Posemath.pmQuatNorm(pose.rot, pose.rot);
54
55 System.out.println("vector = "+v);
56 System.out.println("quat = "+quat);
57 System.out.println("mat = "+mat);
58 System.out.println("pose = "+pose);
59 }
60 catch(Exception e)
61 {
62 e.printStackTrace();
63 }
64
65 }
66
67 // PM_CARTESIAN tests
68 static void testCart()
69 {
70 try
71 {
72 double d;
73 PM_CARTESIAN v1 = new PM_CARTESIAN(1, 2, 3);
74 PM_CARTESIAN v2 = new PM_CARTESIAN(1, 2, 3);
75 PM_CARTESIAN pv;
76
77 // test arg ctor
78 testAssert(v1.equals(v2));
79
80 // test new arg ctor
81 pv = new PM_CARTESIAN(4, 5, 6);
82 testAssert((new PM_CARTESIAN(4, 5, 6)).equals(pv));
83
84
85 d = v2.x;
86 v2.x = v2.z;
87 v2.z = v2.y;
88 v2.y = d;
89
90 // test assignment
91 v1 = new PM_CARTESIAN(0, 0, 0);
92 pv = new PM_CARTESIAN(0, 0, 0);
93 pv = v1 = (PM_CARTESIAN) v2.clone();
94 testAssert((new PM_CARTESIAN(3, 1, 2)).equals(v1));
95 testAssert((new PM_CARTESIAN(3, 1, 2)).equals(v2));
96 testAssert((new PM_CARTESIAN(3, 1, 2)).equals(pv));
97
98 // test unary operators
99 pv = Posemath.neg(v1);
100 testAssert((new PM_CARTESIAN(3, 1, 2)).equals(v1));
101 testAssert((new PM_CARTESIAN(-3, -1, -2)).equals(pv));
102 pv = v1;
103 testAssert((new PM_CARTESIAN(3, 1, 2)).equals(v1));
104 testAssert((new PM_CARTESIAN(3, 1, 2)).equals(pv));
105
106 // test +/-
107 Posemath.pmCartCartAdd(v1, v2, v1);
108 testAssert((new PM_CARTESIAN(6, 2, 4)).equals(v1));
109 testAssert((new PM_CARTESIAN(3, 1, 2)).equals(v2));
110 Posemath.pmCartCartSub(v2, v1, v2);
111 testAssert((new PM_CARTESIAN(6, 2, 4)).equals(v1));
112 testAssert((new PM_CARTESIAN(-3, -1, -2)).equals(v2));
113
114 // test scalar *, /
115 v1 = Posemath.divide(v1,2.);
116 testAssert((new PM_CARTESIAN(3, 1, 2)).equals(v1));
117 v2 = Posemath.multiply(v2,2.);
118 testAssert((new PM_CARTESIAN(-6, -2, -4)).equals(v2));
119
120 // test ==
121 v1 = (PM_CARTESIAN) v2.clone();
122 testAssert(v1.equals(v2));
123 v1 = Posemath.add(v1, Posemath.multiply(new PM_CARTESIAN(1.,1.,1.), Posemath.V_FUZZ * 0.99));
124 //v1 = v1 + PM_CARTESIAN(1, 1, 1) * V_FUZZ * 0.99;
125 testAssert(v1.equals(v2));
126 v1 = v2;
127 v1 = Posemath.add(v1, Posemath.multiply(new PM_CARTESIAN(1.,1.,1.), Posemath.V_FUZZ * 1.01));
128 //v1 = v1 + PM_CARTESIAN(1, 1, 1) * V_FUZZ * 1.01;
129 testAssert(!v1.equals(v2));
130
131 // test dot
132 v1 = new PM_CARTESIAN(1, 2, 3);
133 v2 = new PM_CARTESIAN(4, 5, 6);
134 testAssert(32 == Posemath.dot(v1, v2));
135
136 // test cross
137 v1 = new PM_CARTESIAN(1, 2, 3);
138 v2 = new PM_CARTESIAN(4, 5, 6);
139 testAssert((new PM_CARTESIAN(-3, 6, -3)).equals(Posemath.cross(v1, v2)));
140
141 // test mag
142 v1 = new PM_CARTESIAN(1, 2, 3);
143 v2 = new PM_CARTESIAN(4, 5, 6);
144 testAssert(Math.sqrt(14) == Posemath.mag(v1));
145
146 // test norm
147 v1 = Posemath.norm(new PM_CARTESIAN(1., 2., 3.));
148 testAssert(1.0 == Posemath.mag(v1));
149
150 // test disp
151 v1 = new PM_CARTESIAN(1, 2, 3);
152 v2 = new PM_CARTESIAN(4, 5, 6);
153 testAssert(Math.sqrt(27.) == Posemath.disp(v1, v2));
154
155 // test inv
156 v1 = new PM_CARTESIAN(1, 1, 1);
157 testAssert(1. == Posemath.dot(v1, Posemath.inv(v1)));
158 }
159 catch(Exception e)
160 {
161 e.printStackTrace();
162 }
163
164 }
165
166 static void testCyl()
167 {
168 try
169 {
170
171 PM_CYLINDRICAL c1;
172 PM_CYLINDRICAL c2= new PM_CYLINDRICAL(Math.PI/2, 2, 3);
173 PM_CYLINDRICAL pv;
174 PM_CARTESIAN v1, v2;
175
176 // test arg ctor
177 testAssert((new PM_CYLINDRICAL(Math.PI/2, 2, 3)).equals(c2));
178
179 // test new arg ctor
180 pv = new PM_CYLINDRICAL(Math.PI/4, 5, 6);
181 testAssert((new PM_CYLINDRICAL(Math.PI/4, 5, 6)).equals(pv));
182
183 // test assignment
184 c1 = new PM_CYLINDRICAL(Math.PI/2, 1, 2);
185 c2 = new PM_CYLINDRICAL(0, 0, 0);
186 pv = new PM_CYLINDRICAL(0, 0, 0);
187 pv = c2 = c1;
188 testAssert( (new PM_CYLINDRICAL(Math.PI/2, 1, 2)).equals(c1) );
189 testAssert( (new PM_CYLINDRICAL(Math.PI/2, 1, 2)).equals(c2) );
190 testAssert( (new PM_CYLINDRICAL(Math.PI/2, 1, 2)).equals(pv) );
191
192 // test unary operators
193 pv = Posemath.neg(c1);
194 testAssert((new PM_CYLINDRICAL(Math.PI/2, 1, 2)).equals(c1));
195 testAssert((new PM_CYLINDRICAL(-Math.PI/2, 1, -2)).equals(pv));
196 pv = c1;
197 testAssert((new PM_CYLINDRICAL(Math.PI/2, 1, 2)).equals(c1));
198 testAssert((new PM_CYLINDRICAL(Math.PI/2, 1, 2)).equals(pv));
199
200 // test +/-
201 v1 = new PM_CARTESIAN(1, 2, 3);
202 v2 = new PM_CARTESIAN(4, 5, 6);
203
204 c1 = Posemath.toCyl(v1);
205 c2 = Posemath.toCyl(v2);
206
207 v1 = Posemath.add(v1, v2);
208 c1 = Posemath.add(c1, c2);
209
210 testAssert((new PM_CARTESIAN(c1)).equals(v1));
211 testAssert(c1.equals(new PM_CYLINDRICAL(v1)));
212
213 // test scalar *, /
214 c1 = new PM_CYLINDRICAL(-Math.PI/4, 2., 3.);
215 c2 = Posemath.divide(c1, 2.0);
216 testAssert((new PM_CYLINDRICAL(-Math.PI/4, 1, 1.5)).equals(c2));
217 c2 = Posemath.multiply(c1, 2.0);
218 testAssert((new PM_CYLINDRICAL(-Math.PI/4, 4, 6)).equals(c2));
219
220 // test dot
221 c1 = new PM_CYLINDRICAL(1, 2, 3);
222 v1 = new PM_CARTESIAN(c1);
223 c2 = new PM_CYLINDRICAL(4, 5, 6);
224 v2 = new PM_CARTESIAN(c2);
225 testAssert(Posemath.dot(c1, c2) == Posemath.dot(v1, v2));
226
227 // test cross
228 c1 = new PM_CYLINDRICAL(1, 2, 3);
229 v1 = new PM_CARTESIAN(c1);
230 c2 = new PM_CYLINDRICAL(4, 5, 6);
231 v2 = new PM_CARTESIAN(c2);
232 testAssert(Posemath.cross(c1, c2).equals(Posemath.cross(v1, v2)));
233
234 // test mag
235 c1 = new PM_CYLINDRICAL(1, 2, 3);
236 testAssert(Math.sqrt(13) == Posemath.mag(c1));
237
238 // test norm
239 c1 = Posemath.norm(new PM_CYLINDRICAL(1., 2., 3.));
240 testAssert(1.0 == Posemath.mag(c1));
241
242 // test inv
243 c1 = new PM_CYLINDRICAL(1., 1., 1.);
244 testAssert(1. == Posemath.dot(c1, Posemath.inv(c1)));
245 }
246 catch(Exception e)
247 {
248 e.printStackTrace();
249 }
250
251 }
252
253 static void testQuat()
254 {
255 try
256 {
257 double d;
258 PM_CARTESIAN v1;
259 PM_CARTESIAN v2;
260 PM_QUATERNION q1;
261 PM_QUATERNION q2 = new PM_QUATERNION(1, 0, 0, 0);
262 PM_QUATERNION pq;
263
264 // test arg ctor
265 testAssert((new PM_QUATERNION(1, 0, 0, 0)).equals(q2));
266
267 // test new arg ctor
268 pq = new PM_QUATERNION(1, 0, 0, 0);
269 testAssert((new PM_QUATERNION(1, 0, 0, 0)).equals(pq));
270
271 // test indexing
272 q1 = q2;
273 testAssert(q1.equals(q2));
274
275 // test assignment
276 q1 = new PM_QUATERNION(1, 0, 0, 0);
277 q2 = new PM_QUATERNION(2, 3, 4, 5);
278 pq = new PM_QUATERNION(6, 7, 8, 9);
279 pq = q2 = q1;
280 testAssert((new PM_QUATERNION(1, 0, 0, 0)).equals(q1));
281 testAssert((new PM_QUATERNION(1, 0, 0, 0)).equals(q2));
282 testAssert((new PM_QUATERNION(1, 0, 0, 0)).equals(pq));
283
284 // test rotation vector assignment
285 /* .
286 quaternion buffs will know that 90 rotations about X, Y, and Z
287 axes are
288 (sqrt(2)/2, sqrt(2)/2, 0, 0),
289 (sqrt(2)/2, 0, sqrt(2)/2, 0), and
290 (sqrt(2)/2, 0, 0, sqrt(2)/2), respectively.
291 */
292 d = Math.sqrt(2.0) / 2.0;
293 q1 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 1, 0, 0));
294 testAssert((new PM_QUATERNION(d, d, 0, 0)).equals(q1));
295 q1 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 0, 1, 0));
296 testAssert((new PM_QUATERNION(d, 0, d, 0)).equals(q1));
297 q1 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 0, 0, 1));
298 testAssert((new PM_QUATERNION(d, 0, 0, d)).equals(q1));
299
300 // test unary operators
301 q1 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 1, 1, 1));
302 q2 = q1;
303 testAssert(q1.equals(q2));
304
305 // test Q * Q and Q * V
306 v1 = new PM_CARTESIAN(1, 2, 3);
307 v2 = new PM_CARTESIAN(1, 2, 3);
308 q1 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 0.5, 1, 1));
309 q2 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 1, 0.5, 1));
310 pq = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 1, 1, 0.5));
311 v1 = Posemath.multiply(q1, v1);
312 v1 = Posemath.multiply(q2, v1);
313 v1 = Posemath.multiply(pq, v1);
314 v2 = Posemath.multiply(pq , Posemath.multiply(q2 , Posemath.multiply(q1 , v2)));
315 testAssert(v1.equals(v2));
316
317 // test scaling
318 q1 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 1., -1., 0.5));
319 q2 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2 / 2.0, 1., -1., 0.5));
320 q1 = Posemath.divide(q1, 2.0);
321 testAssert(q1.equals(q2));
322 q1 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2, 1., -1., 0.5));
323 q2 = Posemath.toQuat(new PM_ROTATION_VECTOR(Math.PI/2 * 2.0, 1., -1., 0.5));
324 q1 = Posemath.multiply(q1, 2.0);
325 testAssert(q1.equals(q2));
326
327 // test norm, isNorm
328 q1.s = 1;
329 q1.x = 2;
330 q1.y = 3;
331 q1.z = 4;
332 testAssert(! Posemath.isNorm(q1));
333 q1 = Posemath.norm(q1);
334 testAssert(Posemath.isNorm(q1));
335
336 // test inv
337 q1 = new PM_QUATERNION(2, 3, 4, 5);
338 q2 = Posemath.inv(q1);
339 testAssert(Posemath.multiply(q1,q2).equals(new PM_QUATERNION(1, 0, 0, 0)));
340 }
341 catch(Exception e)
342 {
343 e.printStackTrace();
344 }
345
346 }
347
348 static void testMat()
349 {
350 try
351 {
352
353 PM_ROTATION_MATRIX m1 = new PM_ROTATION_MATRIX(new PM_ROTATION_VECTOR(Math.PI/2, 1., 2., 3.));
354 PM_ROTATION_MATRIX m2 = new PM_ROTATION_MATRIX(new PM_ROTATION_VECTOR(-Math.PI/4, 2., 0., -1.));
355 PM_ROTATION_MATRIX m3;
356 PM_QUATERNION q1 = new PM_QUATERNION(new PM_ROTATION_VECTOR(Math.PI/2, 1., 2., 3.));
357 PM_QUATERNION q2 = new PM_QUATERNION(new PM_ROTATION_VECTOR(-Math.PI/4, 2., 0., -1.));
358 PM_QUATERNION q3;
359
360 // use implicit QQ mult
361 m3 = Posemath.multiply(m1, m2);
362
363 // use explicit QQ mult
364 q3 = Posemath.multiply(q1, q2);
365
366 // should be equal
367 testAssert(q3.equals(m3));
368
369 // set them equal via conversion
370 q1 = Posemath.toQuat(m3);
371 testAssert(q1.equals(q3));
372 }
373 catch(Exception e)
374 {
375 e.printStackTrace();
376 }
377 }
378
379 static void testHom()
380 {
381 try
382 {
383 PM_HOMOGENEOUS h1, h2;
384 PM_POSE p1, p2;
385
386 p1 = new PM_POSE(new PM_CARTESIAN(1., 2., 3.), new PM_QUATERNION(new PM_ROTATION_VECTOR(Math.PI/2, 1., 2., 3.)));
387 h1 = Posemath.toHom(p1);
388 testAssert(p1.equals(h1));
389
390 // FIXME -- Negation function not implemented for PM_HOMOGENEOUS
391 // h2 = Posemath.neg(h1);
392 // testAssert(Posemath.mult(h1, h2).equals(new PM_POSE(0., 0., 0., 1., 0., 0., 0.)));
393 }
394 catch(Exception e)
395 {
396 e.printStackTrace();
397 }
398
399 }
400
401 static void testOther()
402 {
403 try
404 {
405
406 PM_RPY rpy1 = new PM_RPY();
407 PM_RPY rpy2 = new PM_RPY();
408 PM_EULER_ZYZ zyz1 = new PM_EULER_ZYZ();
409 PM_EULER_ZYZ zyz2 = new PM_EULER_ZYZ();
410 PM_EULER_ZYX zyx1 = new PM_EULER_ZYX();
411 PM_EULER_ZYX zyx2 = new PM_EULER_ZYX();
412 PM_ROTATION_VECTOR v1;
413 PM_QUATERNION q1, q2;
414
415 v1 = new PM_ROTATION_VECTOR(Math.PI/2, 1., 2., 3.);
416
417 /* FIXME -- not implemented
418 rpy1 = new PM_RPY(v1);
419 zyz1 = new PM_EULER_ZYZ(v1);
420 zyx1 = new PM_EULER_ZYX(v1);
421 rpy2 = new PM_RPY(v1);
422 zyz2 = new PM_EULER_ZYZ(v1);
423 zyx2 = new PM_EULER_ZYX(v1);
424
425 testAssert(rpy1.equals(rpy2));
426 testAssert(zyz1.equals(zyz2));
427 testAssert(zyx1.equals(zyx2));
428 */
429
430 // FIXME : negation functions not implemented
431 //rpy2 = - rpy1;
432 //zyz2 = - zyz1;
433 //zyx2 = - zyx1;
434
435 //testAssert(rpy1 != rpy2);
436 //testAssert(zyz1 != zyz2);
437 //testAssert(zyx1 != zyx2);
438
439 q1 = Posemath.toQuat(rpy1);
440 q2 = Posemath.toQuat(zyz2);
441 testAssert(Posemath.multiply(q1,q2).equals(new PM_QUATERNION(1, 0, 0, 0)));
442
443 q1 = Posemath.toQuat(rpy1);
444 q2 = Posemath.toQuat(zyx2);
445 testAssert(Posemath.multiply(q1, q2).equals(new PM_QUATERNION(1, 0, 0, 0)));
446
447 // FIXME-- need assignment operators for all types
448 // zyz1 = zyx1;
449 }
450 catch(Exception e)
451 {
452 e.printStackTrace();
453 }
454
455 }
456
457
458 public static void main(String args[])
459 {
460 // testPrint();
461 testCart();
462 testQuat();
463 testMat();
464 testCyl();
465 testHom();
466 testOther();
467 }
468 }
469