[216] | 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 | } |
---|