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 | |
---|
25 | collision tests. if this program is run without any arguments it will |
---|
26 | perform all the tests multiple times, with different random data for each |
---|
27 | test. if this program is given a test number it will run that test |
---|
28 | graphically/interactively, in which case the space bar can be used to |
---|
29 | change 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) |
---|
54 | const 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 |
---|
60 | typedef int test_function_t(); |
---|
61 | |
---|
62 | struct 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 | }; |
---|
69 | TestSlot testslot[MAX_TESTS]; |
---|
70 | |
---|
71 | |
---|
72 | // globals used by the test functions |
---|
73 | int graphical_test=0; // show graphical results of this test, 0=none |
---|
74 | int current_test; // currently execiting test |
---|
75 | int 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 | |
---|
97 | void 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 | |
---|
104 | void 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 | |
---|
131 | void 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 | |
---|
219 | int 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 | |
---|
263 | int 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 | |
---|
332 | int 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 | |
---|
419 | int 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 | |
---|
468 | int 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 | |
---|
612 | int 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 | |
---|
740 | int 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 | |
---|
849 | int 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 | |
---|
964 | static 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 | |
---|
997 | static 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 | |
---|
1025 | static 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 | |
---|
1094 | int 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 | |
---|
1146 | int 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 | |
---|
1222 | int space_pressed = 0; |
---|
1223 | |
---|
1224 | |
---|
1225 | // start simulation - set viewpoint |
---|
1226 | |
---|
1227 | static 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 | |
---|
1237 | static void command (int cmd) |
---|
1238 | { |
---|
1239 | if (cmd == ' ') space_pressed = 1; |
---|
1240 | } |
---|
1241 | |
---|
1242 | |
---|
1243 | // simulation loop |
---|
1244 | |
---|
1245 | static 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 | |
---|
1261 | void 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 | |
---|
1348 | int 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 | } |
---|