aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/demo/demo_ode.cpp
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ode-0.9/ode/demo/demo_ode.cpp1128
1 files changed, 0 insertions, 1128 deletions
diff --git a/libraries/ode-0.9/ode/demo/demo_ode.cpp b/libraries/ode-0.9/ode/demo/demo_ode.cpp
deleted file mode 100644
index bd22ac1..0000000
--- a/libraries/ode-0.9/ode/demo/demo_ode.cpp
+++ /dev/null
@@ -1,1128 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include <setjmp.h>
24#include <ode/ode.h>
25
26#ifdef _MSC_VER
27#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
28#endif
29
30//****************************************************************************
31// matrix accessors
32
33#define _A(i,j) A[(i)*4+(j)]
34#define _I(i,j) I[(i)*4+(j)]
35#define _R(i,j) R[(i)*4+(j)]
36
37//****************************************************************************
38// tolerances
39
40#ifdef dDOUBLE
41const double tol = 1e-10;
42#endif
43
44#ifdef dSINGLE
45const double tol = 1e-5;
46#endif
47
48//****************************************************************************
49// misc messages and error handling
50
51#ifdef __GNUC__
52#define HEADER printf ("%s()\n", __FUNCTION__);
53#else
54#define HEADER printf ("%s:%d\n",__FILE__,__LINE__);
55#endif
56
57static jmp_buf jump_buffer;
58
59
60void myMessageFunction (int num, const char *msg, va_list ap)
61{
62 printf ("(Message %d: ",num);
63 vprintf (msg,ap);
64 printf (")");
65 dSetMessageHandler (0);
66 longjmp (jump_buffer,1);
67}
68
69
70#define TRAP_MESSAGE(do,ifnomsg,ifmsg) \
71 dSetMessageHandler (&myMessageFunction); \
72 if (setjmp (jump_buffer)) { \
73 dSetMessageHandler (0); \
74 ifmsg ; \
75 } \
76 else { \
77 dSetMessageHandler (&myMessageFunction); \
78 do ; \
79 ifnomsg ; \
80 } \
81 dSetMessageHandler (0);
82
83//****************************************************************************
84// utility stuff
85
86// compare two numbers, within a threshhold, return 1 if approx equal
87
88int cmp (dReal a, dReal b)
89{
90 return (fabs(a-b) < tol);
91}
92
93//****************************************************************************
94// matrix utility stuff
95
96// compare a 3x3 matrix with the identity
97
98int cmpIdentityMat3 (dMatrix3 A)
99{
100 return
101 (cmp(_A(0,0),1.0) && cmp(_A(0,1),0.0) && cmp(_A(0,2),0.0) &&
102 cmp(_A(1,0),0.0) && cmp(_A(1,1),1.0) && cmp(_A(1,2),0.0) &&
103 cmp(_A(2,0),0.0) && cmp(_A(2,1),0.0) && cmp(_A(2,2),1.0));
104}
105
106
107// transpose a 3x3 matrix in-line
108
109void transpose3x3 (dMatrix3 A)
110{
111 dReal tmp;
112 tmp=A[4]; A[4]=A[1]; A[1]=tmp;
113 tmp=A[8]; A[8]=A[2]; A[2]=tmp;
114 tmp=A[9]; A[9]=A[6]; A[6]=tmp;
115}
116
117//****************************************************************************
118// test miscellaneous math functions
119
120void testRandomNumberGenerator()
121{
122 HEADER;
123 if (dTestRand()) printf ("\tpassed\n");
124 else printf ("\tFAILED\n");
125}
126
127
128void testInfinity()
129{
130 HEADER;
131 if (1e10 < dInfinity && -1e10 > -dInfinity && -dInfinity < dInfinity)
132 printf ("\tpassed\n");
133 else printf ("\tFAILED\n");
134}
135
136
137void testPad()
138{
139 HEADER;
140 char s[100];
141 s[0]=0;
142 for (int i=0; i<=16; i++) sprintf (s+strlen(s),"%d ",dPAD(i));
143 printf ("\t%s\n", strcmp(s,"0 1 4 4 4 8 8 8 8 12 12 12 12 16 16 16 16 ") ?
144 "FAILED" : "passed");
145}
146
147
148void testCrossProduct()
149{
150 HEADER;
151
152 dVector3 a1,a2,b,c;
153 dMatrix3 B;
154 dMakeRandomVector (b,3,1.0);
155 dMakeRandomVector (c,3,1.0);
156
157 dCROSS (a1,=,b,c);
158
159 dSetZero (B,12);
160 dCROSSMAT (B,b,4,+,-);
161 dMultiply0 (a2,B,c,3,3,1);
162
163 dReal diff = dMaxDifference(a1,a2,3,1);
164 printf ("\t%s\n", diff > tol ? "FAILED" : "passed");
165}
166
167
168void testSetZero()
169{
170 HEADER;
171 dReal a[100];
172 dMakeRandomVector (a,100,1.0);
173 dSetZero (a,100);
174 for (int i=0; i<100; i++) if (a[i] != 0.0) {
175 printf ("\tFAILED\n");
176 return;
177 }
178 printf ("\tpassed\n");
179}
180
181
182void testNormalize3()
183{
184 HEADER;
185 int i,j,bad=0;
186 dVector3 n1,n2;
187 for (i=0; i<1000; i++) {
188 dMakeRandomVector (n1,3,1.0);
189 for (j=0; j<3; j++) n2[j]=n1[j];
190 dNormalize3 (n2);
191 if (dFabs(dDOT(n2,n2) - 1.0) > tol) bad |= 1;
192 if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2;
193 if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4;
194 if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8;
195 if (dFabs(dDOT(n2,n1) - dSqrt(dDOT(n1,n1))) > tol) bad |= 16;
196 if (bad) {
197 printf ("\tFAILED (code=%x)\n",bad);
198 return;
199 }
200 }
201 printf ("\tpassed\n");
202}
203
204
205/*
206void testReorthonormalize()
207{
208 HEADER;
209 dMatrix3 R,I;
210 dMakeRandomMatrix (R,3,3,1.0);
211 for (int i=0; i<30; i++) dReorthonormalize (R);
212 dMultiply2 (I,R,R,3,3,3);
213 printf ("\t%s\n",cmpIdentityMat3 (I) ? "passed" : "FAILED");
214}
215*/
216
217
218void testPlaneSpace()
219{
220 HEADER;
221 dVector3 n,p,q;
222 int bad = 0;
223 for (int i=0; i<1000; i++) {
224 dMakeRandomVector (n,3,1.0);
225 dNormalize3 (n);
226 dPlaneSpace (n,p,q);
227 if (fabs(dDOT(n,p)) > tol) bad = 1;
228 if (fabs(dDOT(n,q)) > tol) bad = 1;
229 if (fabs(dDOT(p,q)) > tol) bad = 1;
230 if (fabs(dDOT(p,p)-1) > tol) bad = 1;
231 if (fabs(dDOT(q,q)-1) > tol) bad = 1;
232 }
233 printf ("\t%s\n", bad ? "FAILED" : "passed");
234}
235
236//****************************************************************************
237// test matrix functions
238
239#define MSIZE 21
240#define MSIZE4 24 // MSIZE rounded up to 4
241
242
243void testMatrixMultiply()
244{
245 // A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4
246 dReal A[8],B[12],A2[12],B2[16],C[8];
247 int i;
248
249 HEADER;
250 dSetZero (A,8);
251 for (i=0; i<3; i++) A[i] = i+2;
252 for (i=0; i<3; i++) A[i+4] = i+3+2;
253 for (i=0; i<12; i++) B[i] = i+8;
254 dSetZero (A2,12);
255 for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3];
256 dSetZero (B2,16);
257 for (i=0; i<12; i++) B2[i+i/3] = B[i];
258
259 dMultiply0 (C,A,B,2,3,4);
260 if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 ||
261 C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278)
262 printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n");
263
264 dMultiply1 (C,A2,B,2,3,4);
265 if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 ||
266 C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241)
267 printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n");
268
269 dMultiply2 (C,A,B2,2,3,4);
270 if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 ||
271 C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326)
272 printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n");
273}
274
275
276void testSmallMatrixMultiply()
277{
278 dMatrix3 A,B,C,A2;
279 dVector3 a,a2,x;
280
281 HEADER;
282 dMakeRandomMatrix (A,3,3,1.0);
283 dMakeRandomMatrix (B,3,3,1.0);
284 dMakeRandomMatrix (C,3,3,1.0);
285 dMakeRandomMatrix (x,3,1,1.0);
286
287 // dMULTIPLY0_331()
288 dMULTIPLY0_331 (a,B,x);
289 dMultiply0 (a2,B,x,3,3,1);
290 printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
291 "passed");
292
293 // dMULTIPLY1_331()
294 dMULTIPLY1_331 (a,B,x);
295 dMultiply1 (a2,B,x,3,3,1);
296 printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
297 "passed");
298
299 // dMULTIPLY0_133
300 dMULTIPLY0_133 (a,x,B);
301 dMultiply0 (a2,x,B,1,3,3);
302 printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" :
303 "passed");
304
305 // dMULTIPLY0_333()
306 dMULTIPLY0_333 (A,B,C);
307 dMultiply0 (A2,B,C,3,3,3);
308 printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
309 "passed");
310
311 // dMULTIPLY1_333()
312 dMULTIPLY1_333 (A,B,C);
313 dMultiply1 (A2,B,C,3,3,3);
314 printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
315 "passed");
316
317 // dMULTIPLY2_333()
318 dMULTIPLY2_333 (A,B,C);
319 dMultiply2 (A2,B,C,3,3,3);
320 printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
321 "passed");
322}
323
324
325void testCholeskyFactorization()
326{
327 dReal A[MSIZE4*MSIZE], B[MSIZE4*MSIZE], C[MSIZE4*MSIZE], diff;
328 HEADER;
329 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
330 dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE);
331 memcpy (A,B,MSIZE4*MSIZE*sizeof(dReal));
332 if (dFactorCholesky (B,MSIZE)) printf ("\tpassed (1)\n");
333 else printf ("\tFAILED (1)\n");
334 dClearUpperTriangle (B,MSIZE);
335 dMultiply2 (C,B,B,MSIZE,MSIZE,MSIZE);
336 diff = dMaxDifference(A,C,MSIZE,MSIZE);
337 printf ("\tmaximum difference = %.6e - %s (2)\n",diff,
338 diff > tol ? "FAILED" : "passed");
339}
340
341
342void testCholeskySolve()
343{
344 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], b[MSIZE],x[MSIZE],btest[MSIZE],diff;
345 HEADER;
346
347 // get A,L = PD matrix
348 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
349 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
350 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
351
352 // get b,x = right hand side
353 dMakeRandomMatrix (b,MSIZE,1,1.0);
354 memcpy (x,b,MSIZE*sizeof(dReal));
355
356 // factor L
357 if (dFactorCholesky (L,MSIZE)) printf ("\tpassed (1)\n");
358 else printf ("\tFAILED (1)\n");
359 dClearUpperTriangle (L,MSIZE);
360
361 // solve A*x = b
362 dSolveCholesky (L,x,MSIZE);
363
364 // compute A*x and compare it with b
365 dMultiply2 (btest,A,x,MSIZE,MSIZE,1);
366 diff = dMaxDifference(b,btest,MSIZE,1);
367 printf ("\tmaximum difference = %.6e - %s (2)\n",diff,
368 diff > tol ? "FAILED" : "passed");
369}
370
371
372void testInvertPDMatrix()
373{
374 int i,j,ok;
375 dReal A[MSIZE4*MSIZE], Ainv[MSIZE4*MSIZE], I[MSIZE4*MSIZE];
376 HEADER;
377
378 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
379 dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE);
380 memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal));
381 dSetZero (Ainv,MSIZE4*MSIZE);
382
383 if (dInvertPDMatrix (A,Ainv,MSIZE))
384 printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n");
385 dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE);
386
387 // compare with identity
388 ok = 1;
389 for (i=0; i<MSIZE; i++) {
390 for (j=0; j<MSIZE; j++) {
391 if (i != j) if (cmp (I[i*MSIZE4+j],0.0)==0) ok = 0;
392 }
393 }
394 for (i=0; i<MSIZE; i++) {
395 if (cmp (I[i*MSIZE4+i],1.0)==0) ok = 0;
396 }
397 if (ok) printf ("\tpassed (2)\n"); else printf ("\tFAILED (2)\n");
398}
399
400
401void testIsPositiveDefinite()
402{
403 dReal A[MSIZE4*MSIZE], B[MSIZE4*MSIZE];
404 HEADER;
405 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
406 dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE);
407 printf ("\t%s\n",dIsPositiveDefinite(A,MSIZE) ? "FAILED (1)":"passed (1)");
408 printf ("\t%s\n",dIsPositiveDefinite(B,MSIZE) ? "passed (2)":"FAILED (2)");
409}
410
411
412void testFastLDLTFactorization()
413{
414 int i,j;
415 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], DL[MSIZE4*MSIZE],
416 ATEST[MSIZE4*MSIZE], d[MSIZE], diff;
417 HEADER;
418 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
419 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
420 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
421
422 dFactorLDLT (L,d,MSIZE,MSIZE4);
423 dClearUpperTriangle (L,MSIZE);
424 for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
425
426 dSetZero (DL,MSIZE4*MSIZE);
427 for (i=0; i<MSIZE; i++) {
428 for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
429 }
430
431 dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);
432 diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
433 printf ("\tmaximum difference = %.6e - %s\n",diff,
434 diff > tol ? "FAILED" : "passed");
435}
436
437
438void testSolveLDLT()
439{
440 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], x[MSIZE],
441 b[MSIZE], btest[MSIZE], diff;
442 HEADER;
443 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
444 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
445 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
446
447 dMakeRandomMatrix (b,MSIZE,1,1.0);
448 memcpy (x,b,MSIZE*sizeof(dReal));
449
450 dFactorLDLT (L,d,MSIZE,MSIZE4);
451 dSolveLDLT (L,d,x,MSIZE,MSIZE4);
452
453 dMultiply2 (btest,A,x,MSIZE,MSIZE,1);
454 diff = dMaxDifference(b,btest,MSIZE,1);
455 printf ("\tmaximum difference = %.6e - %s\n",diff,
456 diff > tol ? "FAILED" : "passed");
457}
458
459
460void testLDLTAddTL()
461{
462 int i,j;
463 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE], a[MSIZE],
464 DL[MSIZE4*MSIZE], ATEST[MSIZE4*MSIZE], diff;
465 HEADER;
466
467 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
468 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
469 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
470 dFactorLDLT (L,d,MSIZE,MSIZE4);
471
472 // delete first row and column of factorization
473 for (i=0; i<MSIZE; i++) a[i] = -A[i*MSIZE4];
474 a[0] += 1;
475 dLDLTAddTL (L,d,a,MSIZE,MSIZE4);
476 for (i=1; i<MSIZE; i++) L[i*MSIZE4] = 0;
477 d[0] = 1;
478
479 // get modified L*D*L'
480 dClearUpperTriangle (L,MSIZE);
481 for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
482 dSetZero (DL,MSIZE4*MSIZE);
483 for (i=0; i<MSIZE; i++) {
484 for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
485 }
486 dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);
487
488 // compare it to A with its first row/column removed
489 for (i=1; i<MSIZE; i++) A[i*MSIZE4] = A[i] = 0;
490 A[0] = 1;
491 diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
492 printf ("\tmaximum difference = %.6e - %s\n",diff,
493 diff > tol ? "FAILED" : "passed");
494}
495
496
497void testLDLTRemove()
498{
499 int i,j,r,p[MSIZE];
500 dReal A[MSIZE4*MSIZE], L[MSIZE4*MSIZE], d[MSIZE],
501 L2[MSIZE4*MSIZE], d2[MSIZE], DL2[MSIZE4*MSIZE],
502 Atest1[MSIZE4*MSIZE], Atest2[MSIZE4*MSIZE], diff, maxdiff;
503 HEADER;
504
505 // make array of A row pointers
506 dReal *Arows[MSIZE];
507 for (i=0; i<MSIZE; i++) Arows[i] = A+i*MSIZE4;
508
509 // fill permutation vector
510 for (i=0; i<MSIZE; i++) p[i]=i;
511
512 dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
513 dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
514 memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
515 dFactorLDLT (L,d,MSIZE,MSIZE4);
516
517 maxdiff = 1e10;
518 for (r=0; r<MSIZE; r++) {
519 // get Atest1 = A with row/column r removed
520 memcpy (Atest1,A,MSIZE4*MSIZE*sizeof(dReal));
521 dRemoveRowCol (Atest1,MSIZE,MSIZE4,r);
522
523 // test that the row/column removal worked
524 int bad = 0;
525 for (i=0; i<MSIZE; i++) {
526 for (j=0; j<MSIZE; j++) {
527 if (i != r && j != r) {
528 int ii = i;
529 int jj = j;
530 if (ii >= r) ii--;
531 if (jj >= r) jj--;
532 if (A[i*MSIZE4+j] != Atest1[ii*MSIZE4+jj]) bad = 1;
533 }
534 }
535 }
536 if (bad) printf ("\trow/col removal FAILED for row %d\n",r);
537
538 // zero out last row/column of Atest1
539 for (i=0; i<MSIZE; i++) {
540 Atest1[(MSIZE-1)*MSIZE4+i] = 0;
541 Atest1[i*MSIZE4+MSIZE-1] = 0;
542 }
543
544 // get L2*D2*L2' = adjusted factorization to remove that row
545 memcpy (L2,L,MSIZE4*MSIZE*sizeof(dReal));
546 memcpy (d2,d,MSIZE*sizeof(dReal));
547 dLDLTRemove (/*A*/ Arows,p,L2,d2,MSIZE,MSIZE,r,MSIZE4);
548
549 // get Atest2 = L2*D2*L2'
550 dClearUpperTriangle (L2,MSIZE);
551 for (i=0; i<(MSIZE-1); i++) L2[i*MSIZE4+i] = 1.0;
552 for (i=0; i<MSIZE; i++) L2[(MSIZE-1)*MSIZE4+i] = 0;
553 d2[MSIZE-1] = 1;
554 dSetZero (DL2,MSIZE4*MSIZE);
555 for (i=0; i<(MSIZE-1); i++) {
556 for (j=0; j<MSIZE-1; j++) DL2[i*MSIZE4+j] = L2[i*MSIZE4+j] / d2[j];
557 }
558
559 dMultiply2 (Atest2,L2,DL2,MSIZE,MSIZE,MSIZE);
560
561 diff = dMaxDifference(Atest1,Atest2,MSIZE,MSIZE);
562 if (diff < maxdiff) maxdiff = diff;
563
564 /*
565 dPrintMatrix (Atest1,MSIZE,MSIZE);
566 printf ("\n");
567 dPrintMatrix (Atest2,MSIZE,MSIZE);
568 printf ("\n");
569 */
570 }
571 printf ("\tmaximum difference = %.6e - %s\n",maxdiff,
572 maxdiff > tol ? "FAILED" : "passed");
573}
574
575//****************************************************************************
576// test mass stuff
577
578#define NUMP 10 // number of particles
579
580
581void printMassParams (dMass *m)
582{
583 printf ("mass = %.4f\n",m->mass);
584 printf ("com = (%.4f,%.4f,%.4f)\n",m->c[0],m->c[1],m->c[2]);
585 printf ("I = [ %10.4f %10.4f %10.4f ]\n"
586 " [ %10.4f %10.4f %10.4f ]\n"
587 " [ %10.4f %10.4f %10.4f ]\n",
588 m->_I(0,0),m->_I(0,1),m->_I(0,2),
589 m->_I(1,0),m->_I(1,1),m->_I(1,2),
590 m->_I(2,0),m->_I(2,1),m->_I(2,2));
591}
592
593
594void compareMassParams (dMass *m1, dMass *m2, char *msg)
595{
596 int i,j,ok = 1;
597 if (!(cmp(m1->mass,m2->mass) && cmp(m1->c[0],m2->c[0]) &&
598 cmp(m1->c[1],m2->c[1]) && cmp(m1->c[2],m2->c[2])))
599 ok = 0;
600 for (i=0; i<3; i++) for (j=0; j<3; j++)
601 if (cmp (m1->_I(i,j),m2->_I(i,j))==0) ok = 0;
602 if (ok) printf ("\tpassed (%s)\n",msg); else printf ("\tFAILED (%s)\n",msg);
603}
604
605
606// compute the mass parameters of a particle set
607
608void computeMassParams (dMass *m, dReal q[NUMP][3], dReal pm[NUMP])
609{
610 int i,j;
611 dMassSetZero (m);
612 for (i=0; i<NUMP; i++) {
613 m->mass += pm[i];
614 for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j];
615 m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]);
616 m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]);
617 m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]);
618 m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]);
619 m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]);
620 m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]);
621 }
622 for (j=0; j<3; j++) m->c[j] /= m->mass;
623 m->_I(1,0) = m->_I(0,1);
624 m->_I(2,0) = m->_I(0,2);
625 m->_I(2,1) = m->_I(1,2);
626}
627
628
629void testMassFunctions()
630{
631 dMass m;
632 int i,j;
633 dReal q[NUMP][3]; // particle positions
634 dReal pm[NUMP]; // particle masses
635 dMass m1,m2;
636 dMatrix3 R;
637
638 HEADER;
639
640 printf ("\t");
641 dMassSetZero (&m);
642 TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6),
643 printf (" FAILED (1)\n"), printf (" passed (1)\n"));
644
645 printf ("\t");
646 dMassSetZero (&m);
647 TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4),
648 printf (" passed (2)\n") , printf (" FAILED (2)\n"));
649 if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) &&
650 m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 &&
651 m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 &&
652 m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4)
653 printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n");
654
655 dMassSetZero (&m);
656 dMassSetSphere (&m,1.4, 0.86);
657 if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
658 cmp(m._I(0,0),1.10349124669826) &&
659 cmp(m._I(1,1),1.10349124669826) &&
660 cmp(m._I(2,2),1.10349124669826) &&
661 m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
662 m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
663 printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n");
664
665 dMassSetZero (&m);
666 dMassSetCapsule (&m,1.3,1,0.76,1.53);
667 if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
668 cmp(m._I(0,0),1.59461986077384) &&
669 cmp(m._I(1,1),4.57537403079093) &&
670 cmp(m._I(2,2),4.57537403079093) &&
671 m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
672 m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
673 printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n");
674
675 dMassSetZero (&m);
676 dMassSetBox (&m,0.27,3,4,5);
677 if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
678 cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) &&
679 m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
680 m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
681 printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n");
682
683 // test dMassAdjust?
684
685 // make random particles and compute the mass, COM and inertia, then
686 // translate and repeat.
687 for (i=0; i<NUMP; i++) {
688 pm[i] = dRandReal()+0.5;
689 for (j=0; j<3; j++) {
690 q[i][j] = 2.0*(dRandReal()-0.5);
691 }
692 }
693 computeMassParams (&m1,q,pm);
694 memcpy (&m2,&m1,sizeof(dMass));
695 dMassTranslate (&m2,1,2,-3);
696 for (i=0; i<NUMP; i++) {
697 q[i][0] += 1;
698 q[i][1] += 2;
699 q[i][2] -= 3;
700 }
701 computeMassParams (&m1,q,pm);
702 compareMassParams (&m1,&m2,"7");
703
704 // rotate the masses
705 _R(0,0) = -0.87919618797635;
706 _R(0,1) = 0.15278881840384;
707 _R(0,2) = -0.45129772879842;
708 _R(1,0) = -0.47307856232664;
709 _R(1,1) = -0.39258064912909;
710 _R(1,2) = 0.78871864932708;
711 _R(2,0) = -0.05666336483842;
712 _R(2,1) = 0.90693771059546;
713 _R(2,2) = 0.41743652473765;
714 dMassRotate (&m2,R);
715 for (i=0; i<NUMP; i++) {
716 dReal a[3];
717 dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1);
718 q[i][0] = a[0];
719 q[i][1] = a[1];
720 q[i][2] = a[2];
721 }
722 computeMassParams (&m1,q,pm);
723 compareMassParams (&m1,&m2,"8");
724}
725
726//****************************************************************************
727// test rotation stuff
728
729void makeRandomRotation (dMatrix3 R)
730{
731 dReal *u1 = R, *u2=R+4, *u3=R+8;
732 dMakeRandomVector (u1,3,1.0);
733 dNormalize3 (u1);
734 dMakeRandomVector (u2,3,1.0);
735 dReal d = dDOT(u1,u2);
736 u2[0] -= d*u1[0];
737 u2[1] -= d*u1[1];
738 u2[2] -= d*u1[2];
739 dNormalize3 (u2);
740 dCROSS (u3,=,u1,u2);
741}
742
743
744void testRtoQandQtoR()
745{
746 HEADER;
747 dMatrix3 R,I,R2;
748 dQuaternion q;
749 int i;
750
751 // test makeRandomRotation()
752 makeRandomRotation (R);
753 dMultiply2 (I,R,R,3,3,3);
754 printf ("\tmakeRandomRotation() - %s (1)\n",
755 cmpIdentityMat3(I) ? "passed" : "FAILED");
756
757 // test QtoR() on random normalized quaternions
758 int ok = 1;
759 for (i=0; i<100; i++) {
760 dMakeRandomVector (q,4,1.0);
761 dNormalize4 (q);
762 dQtoR (q,R);
763 dMultiply2 (I,R,R,3,3,3);
764 if (cmpIdentityMat3(I)==0) ok = 0;
765 }
766 printf ("\tQtoR() orthonormality %s (2)\n", ok ? "passed" : "FAILED");
767
768 // test R -> Q -> R works
769 dReal maxdiff=0;
770 for (i=0; i<100; i++) {
771 makeRandomRotation (R);
772 dRtoQ (R,q);
773 dQtoR (q,R2);
774 dReal diff = dMaxDifference (R,R2,3,3);
775 if (diff > maxdiff) maxdiff = diff;
776 }
777 printf ("\tmaximum difference = %e - %s (3)\n",maxdiff,
778 (maxdiff > tol) ? "FAILED" : "passed");
779}
780
781
782void testQuaternionMultiply()
783{
784 HEADER;
785 dMatrix3 RA,RB,RC,Rtest;
786 dQuaternion qa,qb,qc;
787 dReal diff,maxdiff=0;
788
789 for (int i=0; i<100; i++) {
790 makeRandomRotation (RB);
791 makeRandomRotation (RC);
792 dRtoQ (RB,qb);
793 dRtoQ (RC,qc);
794
795 dMultiply0 (RA,RB,RC,3,3,3);
796 dQMultiply0 (qa,qb,qc);
797 dQtoR (qa,Rtest);
798 diff = dMaxDifference (Rtest,RA,3,3);
799 if (diff > maxdiff) maxdiff = diff;
800
801 dMultiply1 (RA,RB,RC,3,3,3);
802 dQMultiply1 (qa,qb,qc);
803 dQtoR (qa,Rtest);
804 diff = dMaxDifference (Rtest,RA,3,3);
805 if (diff > maxdiff) maxdiff = diff;
806
807 dMultiply2 (RA,RB,RC,3,3,3);
808 dQMultiply2 (qa,qb,qc);
809 dQtoR (qa,Rtest);
810 diff = dMaxDifference (Rtest,RA,3,3);
811 if (diff > maxdiff) maxdiff = diff;
812
813 dMultiply0 (RA,RC,RB,3,3,3);
814 transpose3x3 (RA);
815 dQMultiply3 (qa,qb,qc);
816 dQtoR (qa,Rtest);
817 diff = dMaxDifference (Rtest,RA,3,3);
818 if (diff > maxdiff) maxdiff = diff;
819 }
820 printf ("\tmaximum difference = %e - %s\n",maxdiff,
821 (maxdiff > tol) ? "FAILED" : "passed");
822}
823
824
825void testRotationFunctions()
826{
827 dMatrix3 R1;
828 HEADER;
829
830 printf ("\tdRSetIdentity - ");
831 dMakeRandomMatrix (R1,3,3,1.0);
832 dRSetIdentity (R1);
833 if (cmpIdentityMat3(R1)) printf ("passed\n"); else printf ("FAILED\n");
834
835 printf ("\tdRFromAxisAndAngle - ");
836
837 printf ("\n");
838 printf ("\tdRFromEulerAngles - ");
839
840 printf ("\n");
841 printf ("\tdRFrom2Axes - ");
842
843 printf ("\n");
844}
845
846//****************************************************************************
847
848#include "../src/array.h"
849#include "../src/array.cpp"
850
851// matrix header on the stack
852
853class dMatrixComparison {
854 struct dMatInfo;
855 dArray<dMatInfo*> mat;
856 int afterfirst,index;
857
858public:
859 dMatrixComparison();
860 ~dMatrixComparison();
861
862 dReal nextMatrix (dReal *A, int n, int m, int lower_tri, char *name, ...);
863 // add a new n*m matrix A to the sequence. the name of the matrix is given
864 // by the printf-style arguments (name,...). if this is the first sequence
865 // then this object will simply record the matrices and return 0.
866 // if this the second or subsequent sequence then this object will compare
867 // the matrices with the first sequence, and report any differences.
868 // the matrix error will be returned. if `lower_tri' is 1 then only the
869 // lower triangle of the matrix (including the diagonal) will be compared
870 // (the matrix must be square).
871
872 void end();
873 // end a sequence.
874
875 void reset();
876 // restarts the object, so the next sequence will be the first sequence.
877
878 void dump();
879 // print out info about all the matrices in the sequence
880};
881
882struct dMatrixComparison::dMatInfo {
883 int n,m; // size of matrix
884 char name[128]; // name of the matrix
885 dReal *data; // matrix data
886 int size; // size of `data'
887};
888
889
890
891dMatrixComparison::dMatrixComparison()
892{
893 afterfirst = 0;
894 index = 0;
895}
896
897
898dMatrixComparison::~dMatrixComparison()
899{
900 reset();
901}
902
903
904dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri,
905 char *name, ...)
906{
907 if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix");
908 int num = n*dPAD(m);
909
910 if (afterfirst==0) {
911 dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo));
912 mi->n = n;
913 mi->m = m;
914 mi->size = num * sizeof(dReal);
915 mi->data = (dReal*) dAlloc (mi->size);
916 memcpy (mi->data,A,mi->size);
917
918 va_list ap;
919 va_start (ap,name);
920 vsprintf (mi->name,name,ap);
921 if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long");
922
923 mat.push (mi);
924 return 0;
925 }
926 else {
927 if (lower_tri && n != m)
928 dDebug (0,"dMatrixComparison, lower triangular matrix must be square");
929 if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices");
930 dMatInfo *mp = mat[index];
931 index++;
932
933 dMatInfo mi;
934 va_list ap;
935 va_start (ap,name);
936 vsprintf (mi.name,name,ap);
937 if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long");
938
939 if (strcmp(mp->name,mi.name) != 0)
940 dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")",
941 mp->name,mi.name);
942 if (mp->n != n || mp->m != m)
943 dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)",
944 mp->n,mp->m,n,m);
945
946 dReal maxdiff;
947 if (lower_tri) {
948 maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n);
949 }
950 else {
951 maxdiff = dMaxDifference (A,mp->data,n,m);
952 }
953 if (maxdiff > tol)
954 dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", "
955 "error=%.4e)",n,m,mi.name,maxdiff);
956 return maxdiff;
957 }
958}
959
960
961void dMatrixComparison::end()
962{
963 if (mat.size() <= 0) dDebug (0,"no matrices in sequence");
964 afterfirst = 1;
965 index = 0;
966}
967
968
969void dMatrixComparison::reset()
970{
971 for (int i=0; i<mat.size(); i++) {
972 dFree (mat[i]->data,mat[i]->size);
973 dFree (mat[i],sizeof(dMatInfo));
974 }
975 mat.setSize (0);
976 afterfirst = 0;
977 index = 0;
978}
979
980
981void dMatrixComparison::dump()
982{
983 for (int i=0; i<mat.size(); i++)
984 printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m);
985}
986
987//****************************************************************************
988// unit test
989
990#include <setjmp.h>
991
992// static jmp_buf jump_buffer;
993
994static void myDebug (int num, const char *msg, va_list ap)
995{
996 // printf ("(Error %d: ",num);
997 // vprintf (msg,ap);
998 // printf (")\n");
999 longjmp (jump_buffer,1);
1000}
1001
1002
1003extern "C" void dTestMatrixComparison()
1004{
1005 volatile int i;
1006 printf ("dTestMatrixComparison()\n");
1007 dMessageFunction *orig_debug = dGetDebugHandler();
1008
1009 dMatrixComparison mc;
1010 dReal A[50*50];
1011
1012 // make first sequence
1013 unsigned long seed = dRandGetSeed();
1014 for (i=1; i<49; i++) {
1015 dMakeRandomMatrix (A,i,i+1,1.0);
1016 mc.nextMatrix (A,i,i+1,0,"A%d",i);
1017 }
1018 mc.end();
1019
1020 //mc.dump();
1021
1022 // test identical sequence
1023 dSetDebugHandler (&myDebug);
1024 dRandSetSeed (seed);
1025 if (setjmp (jump_buffer)) {
1026 printf ("\tFAILED (1)\n");
1027 }
1028 else {
1029 for (i=1; i<49; i++) {
1030 dMakeRandomMatrix (A,i,i+1,1.0);
1031 mc.nextMatrix (A,i,i+1,0,"A%d",i);
1032 }
1033 mc.end();
1034 printf ("\tpassed (1)\n");
1035 }
1036 dSetDebugHandler (orig_debug);
1037
1038 // test broken sequences (with matrix error)
1039 dRandSetSeed (seed);
1040 volatile int passcount = 0;
1041 for (i=1; i<49; i++) {
1042 if (setjmp (jump_buffer)) {
1043 passcount++;
1044 }
1045 else {
1046 dSetDebugHandler (&myDebug);
1047 dMakeRandomMatrix (A,i,i+1,1.0);
1048 A[(i-1)*dPAD(i+1)+i] += REAL(0.01);
1049 mc.nextMatrix (A,i,i+1,0,"A%d",i);
1050 dSetDebugHandler (orig_debug);
1051 }
1052 }
1053 mc.end();
1054 printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED");
1055
1056 // test broken sequences (with name error)
1057 dRandSetSeed (seed);
1058 passcount = 0;
1059 for (i=1; i<49; i++) {
1060 if (setjmp (jump_buffer)) {
1061 passcount++;
1062 }
1063 else {
1064 dSetDebugHandler (&myDebug);
1065 dMakeRandomMatrix (A,i,i+1,1.0);
1066 mc.nextMatrix (A,i,i+1,0,"B%d",i);
1067 dSetDebugHandler (orig_debug);
1068 }
1069 }
1070 mc.end();
1071 printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED");
1072
1073 // test identical sequence again
1074 dSetDebugHandler (&myDebug);
1075 dRandSetSeed (seed);
1076 if (setjmp (jump_buffer)) {
1077 printf ("\tFAILED (4)\n");
1078 }
1079 else {
1080 for (i=1; i<49; i++) {
1081 dMakeRandomMatrix (A,i,i+1,1.0);
1082 mc.nextMatrix (A,i,i+1,0,"A%d",i);
1083 }
1084 mc.end();
1085 printf ("\tpassed (4)\n");
1086 }
1087 dSetDebugHandler (orig_debug);
1088}
1089
1090//****************************************************************************
1091
1092// internal unit tests
1093extern "C" void dTestDataStructures();
1094extern "C" void dTestMatrixComparison();
1095extern "C" void dTestSolveLCP();
1096
1097
1098int main()
1099{
1100 dInitODE();
1101 testRandomNumberGenerator();
1102 testInfinity();
1103 testPad();
1104 testCrossProduct();
1105 testSetZero();
1106 testNormalize3();
1107 //testReorthonormalize(); ... not any more
1108 testPlaneSpace();
1109 testMatrixMultiply();
1110 testSmallMatrixMultiply();
1111 testCholeskyFactorization();
1112 testCholeskySolve();
1113 testInvertPDMatrix();
1114 testIsPositiveDefinite();
1115 testFastLDLTFactorization();
1116 testSolveLDLT();
1117 testLDLTAddTL();
1118 testLDLTRemove();
1119 testMassFunctions();
1120 testRtoQandQtoR();
1121 testQuaternionMultiply();
1122 testRotationFunctions();
1123 dTestMatrixComparison();
1124 dTestSolveLCP();
1125 // dTestDataStructures();
1126 dCloseODE();
1127 return 0;
1128}
generated by cgit v1.1 at 2024-09-13 08:08:56 +0000