aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/demo/demo_collision.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/demo/demo_collision.cpp')
-rw-r--r--libraries/ode-0.9/ode/demo/demo_collision.cpp1371
1 files changed, 1371 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/demo/demo_collision.cpp b/libraries/ode-0.9/ode/demo/demo_collision.cpp
new file mode 100644
index 0000000..8f591e9
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_collision.cpp
@@ -0,0 +1,1371 @@
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/*
24
25collision tests. if this program is run without any arguments it will
26perform all the tests multiple times, with different random data for each
27test. if this program is given a test number it will run that test
28graphically/interactively, in which case the space bar can be used to
29change the random test conditions.
30
31*/
32
33
34#include <ode/ode.h>
35#include <drawstuff/drawstuff.h>
36
37#ifdef _MSC_VER
38#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
39#endif
40
41// select correct drawing functions
42#ifdef dDOUBLE
43#define dsDrawSphere dsDrawSphereD
44#define dsDrawBox dsDrawBoxD
45#define dsDrawLine dsDrawLineD
46#define dsDrawCapsule dsDrawCapsuleD
47#endif
48
49//****************************************************************************
50// test infrastructure, including constants and macros
51
52#define TEST_REPS1 1000 // run each test this many times (first batch)
53#define TEST_REPS2 10000 // run each test this many times (second batch)
54const dReal tol = 1e-8; // tolerance used for numerical checks
55#define MAX_TESTS 1000 // maximum number of test slots
56#define Z_OFFSET 2 // z offset for drawing (to get above ground)
57
58
59// test function. returns 1 if the test passed or 0 if it failed
60typedef int test_function_t();
61
62struct TestSlot {
63 int number; // number of test
64 char *name; // name of test
65 int failcount;
66 test_function_t *test_fn;
67 int last_failed_line;
68};
69TestSlot testslot[MAX_TESTS];
70
71
72// globals used by the test functions
73int graphical_test=0; // show graphical results of this test, 0=none
74int current_test; // currently execiting test
75int draw_all_objects_called;
76
77
78#define MAKE_TEST(number,function) \
79 if (testslot[number].name) dDebug (0,"test number already used"); \
80 if (number <= 0 || number >= MAX_TESTS) dDebug (0,"bad test number"); \
81 testslot[number].name = # function; \
82 testslot[number].test_fn = function;
83
84#define FAILED() { if (graphical_test==0) { \
85 testslot[current_test].last_failed_line=__LINE__; return 0; } }
86#define PASSED() { return 1; }
87
88//****************************************************************************
89// globals
90
91/* int dBoxBox (const dVector3 p1, const dMatrix3 R1,
92 const dVector3 side1, const dVector3 p2,
93 const dMatrix3 R2, const dVector3 side2,
94 dVector3 normal, dReal *depth, int *code,
95 int maxc, dContactGeom *contact, int skip); */
96
97void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
98 const dVector3 pb, const dVector3 ub,
99 dReal *alpha, dReal *beta);
100
101//****************************************************************************
102// draw all objects in a space, and draw all the collision contact points
103
104void nearCallback (void *data, dGeomID o1, dGeomID o2)
105{
106 int i,j,n;
107 const int N = 100;
108 dContactGeom contact[N];
109
110 if (dGeomGetClass (o2) == dRayClass) {
111 n = dCollide (o2,o1,N,&contact[0],sizeof(dContactGeom));
112 }
113 else {
114 n = dCollide (o1,o2,N,&contact[0],sizeof(dContactGeom));
115 }
116 if (n > 0) {
117 dMatrix3 RI;
118 dRSetIdentity (RI);
119 const dReal ss[3] = {0.01,0.01,0.01};
120 for (i=0; i<n; i++) {
121 contact[i].pos[2] += Z_OFFSET;
122 dsDrawBox (contact[i].pos,RI,ss);
123 dVector3 n;
124 for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j];
125 dsDrawLine (contact[i].pos,n);
126 }
127 }
128}
129
130
131void draw_all_objects (dSpaceID space)
132{
133 int i, j;
134
135 draw_all_objects_called = 1;
136 if (!graphical_test) return;
137 int n = dSpaceGetNumGeoms (space);
138
139 // draw all contact points
140 dsSetColor (0,1,1);
141 dSpaceCollide (space,0,&nearCallback);
142
143 // draw all rays
144 for (i=0; i<n; i++) {
145 dGeomID g = dSpaceGetGeom (space,i);
146 if (dGeomGetClass (g) == dRayClass) {
147 dsSetColor (1,1,1);
148 dVector3 origin,dir;
149 dGeomRayGet (g,origin,dir);
150 origin[2] += Z_OFFSET;
151 dReal length = dGeomRayGetLength (g);
152 for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j];
153 dsDrawLine (origin,dir);
154 dsSetColor (0,0,1);
155 dsDrawSphere (origin,dGeomGetRotation(g),0.01);
156 }
157 }
158
159 // draw all other objects
160 for (i=0; i<n; i++) {
161 dGeomID g = dSpaceGetGeom (space,i);
162 dVector3 pos;
163 if (dGeomGetClass (g) != dPlaneClass) {
164 memcpy (pos,dGeomGetPosition(g),sizeof(pos));
165 pos[2] += Z_OFFSET;
166 }
167
168 switch (dGeomGetClass (g)) {
169
170 case dSphereClass: {
171 dsSetColorAlpha (1,0,0,0.8);
172 dReal radius = dGeomSphereGetRadius (g);
173 dsDrawSphere (pos,dGeomGetRotation(g),radius);
174 break;
175 }
176
177 case dBoxClass: {
178 dsSetColorAlpha (1,1,0,0.8);
179 dVector3 sides;
180 dGeomBoxGetLengths (g,sides);
181 dsDrawBox (pos,dGeomGetRotation(g),sides);
182 break;
183 }
184
185 case dCapsuleClass: {
186 dsSetColorAlpha (0,1,0,0.8);
187 dReal radius,length;
188 dGeomCapsuleGetParams (g,&radius,&length);
189 dsDrawCapsule (pos,dGeomGetRotation(g),length,radius);
190 break;
191 }
192
193 case dPlaneClass: {
194 dVector4 n;
195 dMatrix3 R,sides;
196 dVector3 pos2;
197 dGeomPlaneGetParams (g,n);
198 dRFromZAxis (R,n[0],n[1],n[2]);
199 for (j=0; j<3; j++) pos[j] = n[j]*n[3];
200 pos[2] += Z_OFFSET;
201 sides[0] = 2;
202 sides[1] = 2;
203 sides[2] = 0.001;
204 dsSetColor (1,0,1);
205 for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j];
206 dsDrawLine (pos,pos2);
207 dsSetColorAlpha (1,0,1,0.8);
208 dsDrawBox (pos,R,sides);
209 break;
210 }
211
212 }
213 }
214}
215
216//****************************************************************************
217// point depth tests
218
219int test_sphere_point_depth()
220{
221 int j;
222 dVector3 p,q;
223 dMatrix3 R;
224 dReal r,d;
225
226 dSimpleSpace space(0);
227 dGeomID sphere = dCreateSphere (0,1);
228 dSpaceAdd (space,sphere);
229
230 // ********** make a random sphere of radius r at position p
231
232 r = dRandReal()+0.1;
233 dGeomSphereSetRadius (sphere,r);
234 dMakeRandomVector (p,3,1.0);
235 dGeomSetPosition (sphere,p[0],p[1],p[2]);
236 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
237 dRandReal()*2-1,dRandReal()*10-5);
238 dGeomSetRotation (sphere,R);
239
240 // ********** test center point has depth r
241
242 if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();
243
244 // ********** test point on surface has depth 0
245
246 for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
247 dNormalize3 (q);
248 for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
249 if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();
250
251 // ********** test point at random depth
252
253 d = (dRandReal()*2-1) * r;
254 for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
255 dNormalize3 (q);
256 for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
257 if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();
258
259 PASSED();
260}
261
262
263int test_box_point_depth()
264{
265 int i,j;
266 dVector3 s,p,q,q2; // s = box sides
267 dMatrix3 R;
268 dReal ss,d; // ss = smallest side
269
270 dSimpleSpace space(0);
271 dGeomID box = dCreateBox (0,1,1,1);
272 dSpaceAdd (space,box);
273
274 // ********** make a random box
275
276 for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
277 dGeomBoxSetLengths (box,s[0],s[1],s[2]);
278 dMakeRandomVector (p,3,1.0);
279 dGeomSetPosition (box,p[0],p[1],p[2]);
280 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
281 dRandReal()*2-1,dRandReal()*10-5);
282 dGeomSetRotation (box,R);
283
284 // ********** test center point has depth of smallest side
285
286 ss = 1e9;
287 for (j=0; j<3; j++) if (s[j] < ss) ss = s[j];
288 if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol)
289 FAILED();
290
291 // ********** test point on surface has depth 0
292
293 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
294 i = dRandInt (3);
295 if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i];
296 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
297 for (j=0; j<3; j++) q2[j] += p[j];
298 if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED();
299
300 // ********** test points outside box have -ve depth
301
302 for (j=0; j<3; j++) {
303 q[j] = 0.5*s[j] + dRandReal() + 0.01;
304 if (dRandReal() > 0.5) q[j] = -q[j];
305 }
306 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
307 for (j=0; j<3; j++) q2[j] += p[j];
308 if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED();
309
310 // ********** test points inside box have +ve depth
311
312 for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5);
313 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
314 for (j=0; j<3; j++) q2[j] += p[j];
315 if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED();
316
317 // ********** test random depth of point aligned along axis (up to ss deep)
318
319 i = dRandInt (3);
320 for (j=0; j<3; j++) q[j] = 0;
321 d = (dRandReal()*(ss*0.5+1)-1);
322 q[i] = s[i]*0.5 - d;
323 if (dRandReal() > 0.5) q[i] = -q[i];
324 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
325 for (j=0; j<3; j++) q2[j] += p[j];
326 if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED();
327
328 PASSED();
329}
330
331
332int test_ccylinder_point_depth()
333{
334 int j;
335 dVector3 p,a;
336 dMatrix3 R;
337 dReal r,l,beta,x,y,d;
338
339 dSimpleSpace space(0);
340 dGeomID ccyl = dCreateCapsule (0,1,1);
341 dSpaceAdd (space,ccyl);
342
343 // ********** make a random ccyl
344
345 r = dRandReal()*0.5 + 0.01;
346 l = dRandReal()*1 + 0.01;
347 dGeomCapsuleSetParams (ccyl,r,l);
348 dMakeRandomVector (p,3,1.0);
349 dGeomSetPosition (ccyl,p[0],p[1],p[2]);
350 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
351 dRandReal()*2-1,dRandReal()*10-5);
352 dGeomSetRotation (ccyl,R);
353
354 // ********** test point on axis has depth of 'radius'
355
356 beta = dRandReal()-0.5;
357 for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2];
358 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol)
359 FAILED();
360
361 // ********** test point on surface (excluding caps) has depth 0
362
363 beta = dRandReal()*2*M_PI;
364 x = r*sin(beta);
365 y = r*cos(beta);
366 beta = dRandReal()-0.5;
367 for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
368 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
369
370 // ********** test point on surface of caps has depth 0
371
372 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
373 dNormalize3 (a);
374 if (dDOT14(a,R+2) > 0) {
375 for (j=0; j<3; j++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2];
376 }
377 else {
378 for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2];
379 }
380 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
381
382 // ********** test point inside ccyl has positive depth
383
384 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
385 dNormalize3 (a);
386 beta = dRandReal()-0.5;
387 for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2];
388 if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED();
389
390 // ********** test point depth (1)
391
392 d = (dRandReal()*2-1) * r;
393 beta = dRandReal()*2*M_PI;
394 x = (r-d)*sin(beta);
395 y = (r-d)*cos(beta);
396 beta = dRandReal()-0.5;
397 for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
398 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
399 FAILED();
400
401 // ********** test point depth (2)
402
403 d = (dRandReal()*2-1) * r;
404 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
405 dNormalize3 (a);
406 if (dDOT14(a,R+2) > 0) {
407 for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2];
408 }
409 else {
410 for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2];
411 }
412 if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
413 FAILED();
414
415 PASSED();
416}
417
418
419int test_plane_point_depth()
420{
421 int j;
422 dVector3 n,p,q,a,b; // n = plane normal
423 dReal d;
424
425 dSimpleSpace space(0);
426 dGeomID plane = dCreatePlane (0,0,0,1,0);
427 dSpaceAdd (space,plane);
428
429 // ********** make a random plane
430
431 for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
432 dNormalize3 (n);
433 d = dRandReal() - 0.5;
434 dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
435 dPlaneSpace (n,p,q);
436
437 // ********** test point on plane has depth 0
438
439 a[0] = dRandReal() - 0.5;
440 a[1] = dRandReal() - 0.5;
441 a[2] = 0;
442 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
443 if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED();
444
445 // ********** test arbitrary depth point
446
447 a[0] = dRandReal() - 0.5;
448 a[1] = dRandReal() - 0.5;
449 a[2] = dRandReal() - 0.5;
450 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
451 if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol)
452 FAILED();
453
454 // ********** test depth-1 point
455
456 a[0] = dRandReal() - 0.5;
457 a[1] = dRandReal() - 0.5;
458 a[2] = -1;
459 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
460 if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED();
461
462 PASSED();
463}
464
465//****************************************************************************
466// ray tests
467
468int test_ray_and_sphere()
469{
470 int j;
471 dContactGeom contact;
472 dVector3 p,q,q2,n,v1;
473 dMatrix3 R;
474 dReal r,k;
475
476 dSimpleSpace space(0);
477 dGeomID ray = dCreateRay (0,0);
478 dGeomID sphere = dCreateSphere (0,1);
479 dSpaceAdd (space,ray);
480 dSpaceAdd (space,sphere);
481
482 // ********** make a random sphere of radius r at position p
483
484 r = dRandReal()+0.1;
485 dGeomSphereSetRadius (sphere,r);
486 dMakeRandomVector (p,3,1.0);
487 dGeomSetPosition (sphere,p[0],p[1],p[2]);
488 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
489 dRandReal()*2-1,dRandReal()*10-5);
490 dGeomSetRotation (sphere,R);
491
492 // ********** test zero length ray just inside sphere
493
494 dGeomRaySetLength (ray,0);
495 dMakeRandomVector (q,3,1.0);
496 dNormalize3 (q);
497 for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
498 dGeomSetPosition (ray,q[0],q[1],q[2]);
499 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
500 dRandReal()*2-1,dRandReal()*10-5);
501 dGeomSetRotation (ray,R);
502 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
503
504 // ********** test zero length ray just outside that sphere
505
506 dGeomRaySetLength (ray,0);
507 dMakeRandomVector (q,3,1.0);
508 dNormalize3 (q);
509 for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
510 dGeomSetPosition (ray,q[0],q[1],q[2]);
511 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
512 dRandReal()*2-1,dRandReal()*10-5);
513 dGeomSetRotation (ray,R);
514 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
515
516 // ********** test finite length ray totally contained inside the sphere
517
518 dMakeRandomVector (q,3,1.0);
519 dNormalize3 (q);
520 k = dRandReal();
521 for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j];
522 dMakeRandomVector (q2,3,1.0);
523 dNormalize3 (q2);
524 k = dRandReal();
525 for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j];
526 for (j=0; j<3; j++) n[j] = q2[j] - q[j];
527 dNormalize3 (n);
528 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
529 dGeomRaySetLength (ray,dDISTANCE (q,q2));
530 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
531
532 // ********** test finite length ray totally outside the sphere
533
534 dMakeRandomVector (q,3,1.0);
535 dNormalize3 (q);
536 do {
537 dMakeRandomVector (n,3,1.0);
538 dNormalize3 (n);
539 }
540 while (dDOT(n,q) < 0); // make sure normal goes away from sphere
541 for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
542 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
543 dGeomRaySetLength (ray,100);
544 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
545
546 // ********** test ray from outside to just above surface
547
548 dMakeRandomVector (q,3,1.0);
549 dNormalize3 (q);
550 for (j=0; j<3; j++) n[j] = -q[j];
551 for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j];
552 dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
553 dGeomRaySetLength (ray,0.99*r);
554 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
555
556 // ********** test ray from outside to just below surface
557
558 dGeomRaySetLength (ray,1.01*r);
559 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
560 for (j=0; j<3; j++) q2[j] = r * q[j] + p[j];
561 if (dDISTANCE (contact.pos,q2) > tol) FAILED();
562
563 // ********** test contact point distance for random rays
564
565 dMakeRandomVector (q,3,1.0);
566 dNormalize3 (q);
567 k = dRandReal()+0.5;
568 for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j];
569 dMakeRandomVector (n,3,1.0);
570 dNormalize3 (n);
571 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
572 dGeomRaySetLength (ray,100);
573 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) {
574 k = dDISTANCE (contact.pos,dGeomGetPosition(sphere));
575 if (dFabs(k - r) > tol) FAILED();
576 // also check normal signs
577 if (dDOT (n,contact.normal) > 0) FAILED();
578 // also check depth of contact point
579 if (dFabs (dGeomSpherePointDepth
580 (sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
581 FAILED();
582
583 draw_all_objects (space);
584 }
585
586 // ********** test tangential grazing - miss
587
588 dMakeRandomVector (q,3,1.0);
589 dNormalize3 (q);
590 dPlaneSpace (q,n,v1);
591 for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
592 for (j=0; j<3; j++) q[j] -= n[j];
593 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
594 dGeomRaySetLength (ray,2);
595 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
596
597 // ********** test tangential grazing - hit
598
599 dMakeRandomVector (q,3,1.0);
600 dNormalize3 (q);
601 dPlaneSpace (q,n,v1);
602 for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
603 for (j=0; j<3; j++) q[j] -= n[j];
604 dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
605 dGeomRaySetLength (ray,2);
606 if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
607
608 PASSED();
609}
610
611
612int test_ray_and_box()
613{
614 int i,j;
615 dContactGeom contact;
616 dVector3 s,p,q,n,q2,q3,q4; // s = box sides
617 dMatrix3 R;
618 dReal k;
619
620 dSimpleSpace space(0);
621 dGeomID ray = dCreateRay (0,0);
622 dGeomID box = dCreateBox (0,1,1,1);
623 dSpaceAdd (space,ray);
624 dSpaceAdd (space,box);
625
626 // ********** make a random box
627
628 for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
629 dGeomBoxSetLengths (box,s[0],s[1],s[2]);
630 dMakeRandomVector (p,3,1.0);
631 dGeomSetPosition (box,p[0],p[1],p[2]);
632 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
633 dRandReal()*2-1,dRandReal()*10-5);
634 dGeomSetRotation (box,R);
635
636 // ********** test zero length ray just inside box
637
638 dGeomRaySetLength (ray,0);
639 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
640 i = dRandInt (3);
641 if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
642 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
643 for (j=0; j<3; j++) q2[j] += p[j];
644 dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
645 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
646 dRandReal()*2-1,dRandReal()*10-5);
647 dGeomSetRotation (ray,R);
648 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
649
650 // ********** test zero length ray just outside box
651
652 dGeomRaySetLength (ray,0);
653 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
654 i = dRandInt (3);
655 if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
656 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
657 for (j=0; j<3; j++) q2[j] += p[j];
658 dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
659 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
660 dRandReal()*2-1,dRandReal()*10-5);
661 dGeomSetRotation (ray,R);
662 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
663
664 // ********** test finite length ray totally contained inside the box
665
666 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
667 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
668 for (j=0; j<3; j++) q2[j] += p[j];
669 for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
670 dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
671 for (j=0; j<3; j++) q4[j] += p[j];
672 for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
673 dNormalize3 (n);
674 dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
675 dGeomRaySetLength (ray,dDISTANCE(q2,q4));
676 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
677
678 // ********** test finite length ray totally outside the box
679
680 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
681 i = dRandInt (3);
682 if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
683 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
684 for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
685 dNormalize3 (q2);
686 dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
687 dGeomRaySetLength (ray,10);
688 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
689
690 // ********** test ray from outside to just above surface
691
692 for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
693 i = dRandInt (3);
694 if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
695 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
696 for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
697 k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
698 for (j=0; j<3; j++) q2[j] = -q2[j];
699 dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
700 dGeomRaySetLength (ray,k*0.99);
701 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
702
703 // ********** test ray from outside to just below surface
704
705 dGeomRaySetLength (ray,k*1.01);
706 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
707
708 // ********** test contact point position for random rays
709
710 for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
711 dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
712 for (j=0; j<3; j++) q2[j] += p[j];
713 for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
714 dNormalize3 (q3);
715 dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
716 dGeomRaySetLength (ray,10);
717 if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
718 // check depth of contact point
719 if (dFabs (dGeomBoxPointDepth
720 (box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
721 FAILED();
722 // check position of contact point
723 for (j=0; j<3; j++) contact.pos[j] -= p[j];
724 dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
725 if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
726 dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
727 dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
728 FAILED();
729 }
730 // also check normal signs
731 if (dDOT (q3,contact.normal) > 0) FAILED();
732
733 draw_all_objects (space);
734 }
735
736 PASSED();
737}
738
739
740int test_ray_and_ccylinder()
741{
742 int j;
743 dContactGeom contact;
744 dVector3 p,a,b,n;
745 dMatrix3 R;
746 dReal r,l,k,x,y;
747
748 dSimpleSpace space(0);
749 dGeomID ray = dCreateRay (0,0);
750 dGeomID ccyl = dCreateCapsule (0,1,1);
751 dSpaceAdd (space,ray);
752 dSpaceAdd (space,ccyl);
753
754 // ********** make a random capped cylinder
755
756 r = dRandReal()*0.5 + 0.01;
757 l = dRandReal()*1 + 0.01;
758 dGeomCapsuleSetParams (ccyl,r,l);
759 dMakeRandomVector (p,3,1.0);
760 dGeomSetPosition (ccyl,p[0],p[1],p[2]);
761 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
762 dRandReal()*2-1,dRandReal()*10-5);
763 dGeomSetRotation (ccyl,R);
764
765 // ********** test ray completely within ccyl
766
767 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
768 dNormalize3 (a);
769 k = (dRandReal()-0.5)*l;
770 for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2];
771 for (j=0; j<3; j++) b[j] = dRandReal()-0.5;
772 dNormalize3 (b);
773 k = (dRandReal()-0.5)*l;
774 for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2];
775 dGeomRaySetLength (ray,dDISTANCE(a,b));
776 for (j=0; j<3; j++) b[j] -= a[j];
777 dNormalize3 (b);
778 dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
779 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
780
781 // ********** test ray outside ccyl that just misses (between caps)
782
783 k = dRandReal()*2*M_PI;
784 x = sin(k);
785 y = cos(k);
786 for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1];
787 k = (dRandReal()-0.5)*l;
788 for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j];
789 dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
790 dGeomRaySetLength (ray,r*0.99);
791 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
792
793 // ********** test ray outside ccyl that just hits (between caps)
794
795 dGeomRaySetLength (ray,r*1.01);
796 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
797 // check depth of contact point
798 if (dFabs (dGeomCapsulePointDepth
799 (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
800 FAILED();
801
802 // ********** test ray outside ccyl that just misses (caps)
803
804 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
805 dNormalize3 (a);
806 if (dDOT14(a,R+2) < 0) {
807 for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2];
808 }
809 else {
810 for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2];
811 }
812 dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
813 dGeomRaySetLength (ray,r*0.99);
814 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
815
816 // ********** test ray outside ccyl that just hits (caps)
817
818 dGeomRaySetLength (ray,r*1.01);
819 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
820 // check depth of contact point
821 if (dFabs (dGeomCapsulePointDepth
822 (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
823 FAILED();
824
825 // ********** test random rays
826
827 for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
828 for (j=0; j<3; j++) n[j] = dRandReal()-0.5;
829 dNormalize3 (n);
830 dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]);
831 dGeomRaySetLength (ray,10);
832
833 if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) {
834 // check depth of contact point
835 if (dFabs (dGeomCapsulePointDepth
836 (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
837 FAILED();
838
839 // check normal signs
840 if (dDOT (n,contact.normal) > 0) FAILED();
841
842 draw_all_objects (space);
843 }
844
845 PASSED();
846}
847
848
849int test_ray_and_plane()
850{
851 int j;
852 dContactGeom contact;
853 dVector3 n,p,q,a,b,g,h; // n,d = plane parameters
854 dMatrix3 R;
855 dReal d;
856
857 dSimpleSpace space(0);
858 dGeomID ray = dCreateRay (0,0);
859 dGeomID plane = dCreatePlane (0,0,0,1,0);
860 dSpaceAdd (space,ray);
861 dSpaceAdd (space,plane);
862
863 // ********** make a random plane
864
865 for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
866 dNormalize3 (n);
867 d = dRandReal() - 0.5;
868 dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
869 dPlaneSpace (n,p,q);
870
871 // ********** test finite length ray below plane
872
873 dGeomRaySetLength (ray,0.09);
874 a[0] = dRandReal()-0.5;
875 a[1] = dRandReal()-0.5;
876 a[2] = -dRandReal()*0.5 - 0.1;
877 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
878 dGeomSetPosition (ray,b[0],b[1],b[2]);
879 dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
880 dRandReal()*2-1,dRandReal()*10-5);
881 dGeomSetRotation (ray,R);
882 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
883
884 // ********** test finite length ray above plane
885
886 a[0] = dRandReal()-0.5;
887 a[1] = dRandReal()-0.5;
888 a[2] = dRandReal()*0.5 + 0.01;
889 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
890 g[0] = dRandReal()-0.5;
891 g[1] = dRandReal()-0.5;
892 g[2] = dRandReal() + 0.01;
893 for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
894 dNormalize3 (h);
895 dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
896 dGeomRaySetLength (ray,10);
897 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
898
899 // ********** test finite length ray that intersects plane
900
901 a[0] = dRandReal()-0.5;
902 a[1] = dRandReal()-0.5;
903 a[2] = dRandReal()-0.5;
904 for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
905 g[0] = dRandReal()-0.5;
906 g[1] = dRandReal()-0.5;
907 g[2] = dRandReal()-0.5;
908 for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
909 dNormalize3 (h);
910 dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
911 dGeomRaySetLength (ray,10);
912 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) {
913 // test that contact is on plane surface
914 if (dFabs (dDOT(contact.pos,n) - d) > tol) FAILED();
915 // also check normal signs
916 if (dDOT (h,contact.normal) > 0) FAILED();
917 // also check contact point depth
918 if (dFabs (dGeomPlanePointDepth
919 (plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
920 FAILED();
921
922 draw_all_objects (space);
923 }
924
925 // ********** test ray that just misses
926
927 for (j=0; j<3; j++) b[j] = (1+d)*n[j];
928 for (j=0; j<3; j++) h[j] = -n[j];
929 dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
930 dGeomRaySetLength (ray,0.99);
931 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
932
933 // ********** test ray that just hits
934
935 dGeomRaySetLength (ray,1.01);
936 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
937
938 // ********** test polarity with typical ground plane
939
940 dGeomPlaneSetParams (plane,0,0,1,0);
941 for (j=0; j<3; j++) a[j] = 0.1;
942 for (j=0; j<3; j++) b[j] = 0;
943 a[2] = 1;
944 b[2] = -1;
945 dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
946 dGeomRaySetLength (ray,2);
947 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
948 if (dFabs (contact.depth - 1) > tol) FAILED();
949 a[2] = -1;
950 b[2] = 1;
951 dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
952 if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
953 if (dFabs (contact.depth - 1) > tol) FAILED();
954
955 PASSED();
956}
957
958//****************************************************************************
959// a really inefficient, but hopefully correct implementation of
960// dBoxTouchesBox(), that does 144 edge-face tests.
961
962// return 1 if edge v1 -> v2 hits the rectangle described by p1,p2,p3
963
964static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
965 dVector3 p1, dVector3 p2, dVector3 p3)
966{
967 int k;
968 dVector3 u1,u2,n,tmp;
969 for (k=0; k<3; k++) u1[k] = p3[k]-p1[k];
970 for (k=0; k<3; k++) u2[k] = p2[k]-p1[k];
971 dReal d1 = dSqrt(dDOT(u1,u1));
972 dReal d2 = dSqrt(dDOT(u2,u2));
973 dNormalize3 (u1);
974 dNormalize3 (u2);
975 if (dFabs(dDOT(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2");
976 dCROSS (n,=,u1,u2);
977 for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k];
978 dReal d = -dDOT(n,p1);
979 if (dFabs(dDOT(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1");
980 if (dFabs(dDOT(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2");
981 if (dFabs(dDOT(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3");
982 dReal alpha = -(d+dDOT(n,v1))/dDOT(n,tmp);
983 for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]);
984 if (dFabs(dDOT(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp");
985 if (alpha < 0) return 0;
986 if (alpha > 1) return 0;
987 for (k=0; k<3; k++) tmp[k] -= p1[k];
988 dReal a1 = dDOT(u1,tmp);
989 dReal a2 = dDOT(u2,tmp);
990 if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0;
991 return 1;
992}
993
994
995// return 1 if box 1 is completely inside box 2
996
997static int box1inside2 (const dVector3 p1, const dMatrix3 R1,
998 const dVector3 side1, const dVector3 p2,
999 const dMatrix3 R2, const dVector3 side2)
1000{
1001 for (int i=-1; i<=1; i+=2) {
1002 for (int j=-1; j<=1; j+=2) {
1003 for (int k=-1; k<=1; k+=2) {
1004 dVector3 v,vv;
1005 v[0] = i*0.5*side1[0];
1006 v[1] = j*0.5*side1[1];
1007 v[2] = k*0.5*side1[2];
1008 dMULTIPLY0_331 (vv,R1,v);
1009 vv[0] += p1[0] - p2[0];
1010 vv[1] += p1[1] - p2[1];
1011 vv[2] += p1[2] - p2[2];
1012 for (int axis=0; axis < 3; axis++) {
1013 dReal z = dDOT14(vv,R2+axis);
1014 if (z < (-side2[axis]*0.5) || z > (side2[axis]*0.5)) return 0;
1015 }
1016 }
1017 }
1018 }
1019 return 1;
1020}
1021
1022
1023// test if any edge from box 1 hits a face from box 2
1024
1025static int testBoxesTouch2 (const dVector3 p1, const dMatrix3 R1,
1026 const dVector3 side1, const dVector3 p2,
1027 const dMatrix3 R2, const dVector3 side2)
1028{
1029 int j,k,j1,j2;
1030
1031 // for 6 faces from box 2
1032 for (int fd=0; fd<3; fd++) { // direction for face
1033
1034 for (int fo=0; fo<2; fo++) { // offset of face
1035 // get four points on the face. first get 2 indexes that are not fd
1036 int k1=0,k2=0;
1037 if (fd==0) { k1 = 1; k2 = 2; }
1038 if (fd==1) { k1 = 0; k2 = 2; }
1039 if (fd==2) { k1 = 0; k2 = 1; }
1040 dVector3 fp[4],tmp;
1041 k=0;
1042 for (j1=-1; j1<=1; j1+=2) {
1043 for (j2=-1; j2<=1; j2+=2) {
1044 fp[k][k1] = j1;
1045 fp[k][k2] = j2;
1046 fp[k][fd] = fo*2-1;
1047 k++;
1048 }
1049 }
1050 for (j=0; j<4; j++) {
1051 for (k=0; k<3; k++) fp[j][k] *= 0.5*side2[k];
1052 dMULTIPLY0_331 (tmp,R2,fp[j]);
1053 for (k=0; k<3; k++) fp[j][k] = tmp[k] + p2[k];
1054 }
1055
1056 // for 8 vertices
1057 dReal v1[3];
1058 for (v1[0]=-1; v1[0] <= 1; v1[0] += 2) {
1059 for (v1[1]=-1; v1[1] <= 1; v1[1] += 2) {
1060 for (v1[2]=-1; v1[2] <= 1; v1[2] += 2) {
1061 // for all possible +ve leading edges from those vertices
1062 for (int ei=0; ei < 3; ei ++) {
1063 if (v1[ei] < 0) {
1064 // get vertex1 -> vertex2 = an edge from box 1
1065 dVector3 vv1,vv2;
1066 for (k=0; k<3; k++) vv1[k] = v1[k] * 0.5*side1[k];
1067 for (k=0; k<3; k++) vv2[k] = (v1[k] + (k==ei)*2)*0.5*side1[k];
1068 dVector3 vertex1,vertex2;
1069 dMULTIPLY0_331 (vertex1,R1,vv1);
1070 dMULTIPLY0_331 (vertex2,R1,vv2);
1071 for (k=0; k<3; k++) vertex1[k] += p1[k];
1072 for (k=0; k<3; k++) vertex2[k] += p1[k];
1073
1074 // see if vertex1 -> vertex2 interesects face
1075 if (edgeIntersectsRect (vertex1,vertex2,fp[0],fp[1],fp[2]))
1076 return 1;
1077 }
1078 }
1079 }
1080 }
1081 }
1082 }
1083 }
1084
1085 if (box1inside2 (p1,R1,side1,p2,R2,side2)) return 1;
1086 if (box1inside2 (p2,R2,side2,p1,R1,side1)) return 1;
1087
1088 return 0;
1089}
1090
1091//****************************************************************************
1092// dBoxTouchesBox() test
1093
1094int test_dBoxTouchesBox()
1095{
1096 int k,bt1,bt2;
1097 dVector3 p1,p2,side1,side2;
1098 dMatrix3 R1,R2;
1099
1100 dSimpleSpace space(0);
1101 dGeomID box1 = dCreateBox (0,1,1,1);
1102 dSpaceAdd (space,box1);
1103 dGeomID box2 = dCreateBox (0,1,1,1);
1104 dSpaceAdd (space,box2);
1105
1106 dMakeRandomVector (p1,3,0.5);
1107 dMakeRandomVector (p2,3,0.5);
1108 for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
1109 for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
1110 dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1111 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1112 dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1113 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1114
1115 dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
1116 dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
1117 dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
1118 dGeomSetRotation (box1,R1);
1119 dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1120 dGeomSetRotation (box2,R2);
1121 draw_all_objects (space);
1122
1123 int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
1124 int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
1125 bt1 = t1 || t2;
1126 bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);
1127
1128 if (bt1 != bt2) FAILED();
1129
1130 /*
1131 // some more debugging info if necessary
1132 if (bt1 && bt2) printf ("agree - boxes touch\n");
1133 if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
1134 if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
1135 "says no\n");
1136 if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
1137 "says yes\n");
1138 */
1139
1140 PASSED();
1141}
1142
1143//****************************************************************************
1144// test box-box collision
1145
1146int test_dBoxBox()
1147{
1148 int k,bt;
1149 dVector3 p1,p2,side1,side2,normal,normal2;
1150 dMatrix3 R1,R2;
1151 dReal depth,depth2;
1152 int code;
1153 dContactGeom contact[48];
1154
1155 dSimpleSpace space(0);
1156 dGeomID box1 = dCreateBox (0,1,1,1);
1157 dSpaceAdd (space,box1);
1158 dGeomID box2 = dCreateBox (0,1,1,1);
1159 dSpaceAdd (space,box2);
1160
1161 dMakeRandomVector (p1,3,0.5);
1162 dMakeRandomVector (p2,3,0.5);
1163 for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
1164 for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
1165
1166 dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1167 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1168 dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
1169 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
1170
1171 // dRSetIdentity (R1); // we can also try this
1172 // dRSetIdentity (R2);
1173
1174 dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
1175 dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
1176 dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
1177 dGeomSetRotation (box1,R1);
1178 dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1179 dGeomSetRotation (box2,R2);
1180
1181 code = 0;
1182 depth = 0;
1183 bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal,&depth,&code,8,contact,
1184 sizeof(dContactGeom));
1185 if (bt==1) {
1186 p2[0] += normal[0] * 0.96 * depth;
1187 p2[1] += normal[1] * 0.96 * depth;
1188 p2[2] += normal[2] * 0.96 * depth;
1189 bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
1190 sizeof(dContactGeom));
1191
1192 /*
1193 dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1194 draw_all_objects (space);
1195 */
1196
1197 if (bt != 1) {
1198 FAILED();
1199 dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1200 draw_all_objects (space);
1201 }
1202
1203 p2[0] += normal[0] * 0.08 * depth;
1204 p2[1] += normal[1] * 0.08 * depth;
1205 p2[2] += normal[2] * 0.08 * depth;
1206 bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
1207 sizeof(dContactGeom));
1208 if (bt != 0) FAILED();
1209
1210 // dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
1211 // draw_all_objects (space);
1212 }
1213
1214 // printf ("code=%2d depth=%.4f ",code,depth);
1215
1216 PASSED();
1217}
1218
1219//****************************************************************************
1220// graphics
1221
1222int space_pressed = 0;
1223
1224
1225// start simulation - set viewpoint
1226
1227static void start()
1228{
1229 static float xyz[3] = {2.4807,-1.8023,2.7600};
1230 static float hpr[3] = {141.5000,-18.5000,0.0000};
1231 dsSetViewpoint (xyz,hpr);
1232}
1233
1234
1235// called when a key pressed
1236
1237static void command (int cmd)
1238{
1239 if (cmd == ' ') space_pressed = 1;
1240}
1241
1242
1243// simulation loop
1244
1245static void simLoop (int pause)
1246{
1247 do {
1248 draw_all_objects_called = 0;
1249 unsigned long seed = dRandGetSeed();
1250 testslot[graphical_test].test_fn();
1251 if (draw_all_objects_called) {
1252 if (space_pressed) space_pressed = 0; else dRandSetSeed (seed);
1253 }
1254 }
1255 while (!draw_all_objects_called);
1256}
1257
1258//****************************************************************************
1259// do all the tests
1260
1261void do_tests (int argc, char **argv)
1262{
1263 int i,j;
1264
1265 // process command line arguments
1266 if (argc >= 2) {
1267 graphical_test = atoi (argv[1]);
1268 }
1269
1270 if (graphical_test) {
1271 // do one test gaphically and interactively
1272
1273 if (graphical_test < 1 || graphical_test >= MAX_TESTS ||
1274 !testslot[graphical_test].name) {
1275 dError (0,"invalid test number");
1276 }
1277
1278 printf ("performing test: %s\n",testslot[graphical_test].name);
1279
1280 // setup pointers to drawstuff callback functions
1281 dsFunctions fn;
1282 fn.version = DS_VERSION;
1283 fn.start = &start;
1284 fn.step = &simLoop;
1285 fn.command = &command;
1286 fn.stop = 0;
1287 fn.path_to_textures = "../../drawstuff/textures";
1288
1289 dsSetSphereQuality (3);
1290 dsSetCapsuleQuality (8);
1291 dsSimulationLoop (argc,argv,1280,900,&fn);
1292 }
1293 else {
1294 // do all tests noninteractively
1295
1296 for (i=0; i<MAX_TESTS; i++) testslot[i].number = i;
1297
1298 // first put the active tests into a separate array
1299 int n=0;
1300 for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) n++;
1301 TestSlot **ts = (TestSlot**) alloca (n * sizeof(TestSlot*));
1302 j = 0;
1303 for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) ts[j++] = testslot+i;
1304 if (j != n) dDebug (0,"internal");
1305
1306 // do two test batches. the first test batch has far fewer reps and will
1307 // catch problems quickly. if all tests in the first batch passes, the
1308 // second batch is run.
1309
1310 for (i=0; i<n; i++) ts[i]->failcount = 0;
1311 int total_reps=0;
1312 for (int batch=0; batch<2; batch++) {
1313 int reps = (batch==0) ? TEST_REPS1 : TEST_REPS2;
1314 total_reps += reps;
1315 printf ("testing batch %d (%d reps)...\n",batch+1,reps);
1316
1317 // run tests
1318 for (j=0; j<reps; j++) {
1319 for (i=0; i<n; i++) {
1320 current_test = ts[i]->number;
1321 if (ts[i]->test_fn() != 1) ts[i]->failcount++;
1322 }
1323 }
1324
1325 // check for failures
1326 int total_fail_count=0;
1327 for (i=0; i<n; i++) total_fail_count += ts[i]->failcount;
1328 if (total_fail_count) break;
1329 }
1330
1331 // print results
1332 for (i=0; i<n; i++) {
1333 printf ("%3d: %-30s: ",ts[i]->number,ts[i]->name);
1334 if (ts[i]->failcount) {
1335 printf ("FAILED (%.2f%%) at line %d\n",
1336 double(ts[i]->failcount)/double(total_reps)*100.0,
1337 ts[i]->last_failed_line);
1338 }
1339 else {
1340 printf ("ok\n");
1341 }
1342 }
1343 }
1344}
1345
1346//****************************************************************************
1347
1348int main (int argc, char **argv)
1349{
1350 // setup all tests
1351
1352 memset (testslot,0,sizeof(testslot));
1353 dInitODE();
1354
1355 MAKE_TEST(1,test_sphere_point_depth);
1356 MAKE_TEST(2,test_box_point_depth);
1357 MAKE_TEST(3,test_ccylinder_point_depth);
1358 MAKE_TEST(4,test_plane_point_depth);
1359
1360 MAKE_TEST(10,test_ray_and_sphere);
1361 MAKE_TEST(11,test_ray_and_box);
1362 MAKE_TEST(12,test_ray_and_ccylinder);
1363 MAKE_TEST(13,test_ray_and_plane);
1364
1365 MAKE_TEST(100,test_dBoxTouchesBox);
1366 MAKE_TEST(101,test_dBoxBox);
1367
1368 do_tests (argc,argv);
1369 dCloseODE();
1370 return 0;
1371}
generated by cgit v1.1 at 2024-12-28 07:18:04 +0000