1 /* 2 * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith 3 * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 4 * All rights reserved. Email: russ (at) q12.org Web: www.q12.org 5 Bullet Continuous Collision Detection and Physics Library 6 Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 7 8 This software is provided 'as-is', without any express or implied warranty. 9 In no event will the authors be held liable for any damages arising from the use of this software. 10 Permission is granted to anyone to use this software for any purpose, 11 including commercial applications, and to alter it and redistribute it freely, 12 subject to the following restrictions: 13 14 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 15 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 16 3. This notice may not be removed or altered from any source distribution. 17 */ 18 19 ///ODE box-box collision detection is adapted to work with Bullet 20 21 #include "btBoxBoxDetector.h" 22 #include "BulletCollision/CollisionShapes/btBoxShape.h" 23 24 #include <float.h> 25 #include <string.h> 26 27 btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2) 28 : m_box1(box1), 29 m_box2(box2) 30 { 31 32 } 33 34 35 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and 36 // generate contact points. this returns 0 if there is no contact otherwise 37 // it returns the number of contacts generated. 38 // `normal' returns the contact normal. 39 // `depth' returns the maximum penetration depth along that normal. 40 // `return_code' returns a number indicating the type of contact that was 41 // detected: 42 // 1,2,3 = box 2 intersects with a face of box 1 43 // 4,5,6 = box 1 intersects with a face of box 2 44 // 7..15 = edge-edge contact 45 // `maxc' is the maximum number of contacts allowed to be generated, i.e. 46 // the size of the `contact' array. 47 // `contact' and `skip' are the contact array information provided to the 48 // collision functions. this function only fills in the position and depth 49 // fields. 50 struct dContactGeom; 51 #define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) 52 #define dInfinity FLT_MAX 53 54 55 /*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } 56 PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } 57 PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } 58 PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } 59 */ 60 static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } 61 static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } 62 static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } 63 static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } 64 #define dMULTIPLYOP1_331(A,op,B,C) \ 65 {\ 66 (A)[0] op dDOT41((B),(C)); \ 67 (A)[1] op dDOT41((B+1),(C)); \ 68 (A)[2] op dDOT41((B+2),(C)); \ 69 } 70 71 #define dMULTIPLYOP0_331(A,op,B,C) \ 72 { \ 73 (A)[0] op dDOT((B),(C)); \ 74 (A)[1] op dDOT((B+4),(C)); \ 75 (A)[2] op dDOT((B+8),(C)); \ 76 } 77 78 #define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) 79 #define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) 80 81 typedef btScalar dMatrix3[4*3]; 82 83 void dLineClosestApproach (const btVector3& pa, const btVector3& ua, 84 const btVector3& pb, const btVector3& ub, 85 btScalar *alpha, btScalar *beta); 86 void dLineClosestApproach (const btVector3& pa, const btVector3& ua, 87 const btVector3& pb, const btVector3& ub, 88 btScalar *alpha, btScalar *beta) 89 { 90 btVector3 p; 91 p[0] = pb[0] - pa[0]; 92 p[1] = pb[1] - pa[1]; 93 p[2] = pb[2] - pa[2]; 94 btScalar uaub = dDOT(ua,ub); 95 btScalar q1 = dDOT(ua,p); 96 btScalar q2 = -dDOT(ub,p); 97 btScalar d = 1-uaub*uaub; 98 if (d <= btScalar(0.0001f)) { 99 // @@@ this needs to be made more robust 100 *alpha = 0; 101 *beta = 0; 102 } 103 else { 104 d = 1.f/d; 105 *alpha = (q1 + uaub*q2)*d; 106 *beta = (uaub*q1 + q2)*d; 107 } 108 } 109 110 111 112 // find all the intersection points between the 2D rectangle with vertices 113 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), 114 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). 115 // 116 // the intersection points are returned as x,y pairs in the 'ret' array. 117 // the number of intersection points is returned by the function (this will 118 // be in the range 0 to 8). 119 120 static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) 121 { 122 // q (and r) contain nq (and nr) coordinate points for the current (and 123 // chopped) polygons 124 int nq=4,nr=0; 125 btScalar buffer[16]; 126 btScalar *q = p; 127 btScalar *r = ret; 128 for (int dir=0; dir <= 1; dir++) { 129 // direction notation: xy[0] = x axis, xy[1] = y axis 130 for (int sign=-1; sign <= 1; sign += 2) { 131 // chop q along the line xy[dir] = sign*h[dir] 132 btScalar *pq = q; 133 btScalar *pr = r; 134 nr = 0; 135 for (int i=nq; i > 0; i--) { 136 // go through all points in q and all lines between adjacent points 137 if (sign*pq[dir] < h[dir]) { 138 // this point is inside the chopping line 139 pr[0] = pq[0]; 140 pr[1] = pq[1]; 141 pr += 2; 142 nr++; 143 if (nr & 8) { 144 q = r; 145 goto done; 146 } 147 } 148 btScalar *nextq = (i > 1) ? pq+2 : q; 149 if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { 150 // this line crosses the chopping line 151 pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / 152 (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); 153 pr[dir] = sign*h[dir]; 154 pr += 2; 155 nr++; 156 if (nr & 8) { 157 q = r; 158 goto done; 159 } 160 } 161 pq += 2; 162 } 163 q = r; 164 r = (q==ret) ? buffer : ret; 165 nq = nr; 166 } 167 } 168 done: 169 if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); 170 return nr; 171 } 172 173 174 #define M__PI 3.14159265f 175 176 // given n points in the plane (array p, of size 2*n), generate m points that 177 // best represent the whole set. the definition of 'best' here is not 178 // predetermined - the idea is to select points that give good box-box 179 // collision detection behavior. the chosen point indexes are returned in the 180 // array iret (of size m). 'i0' is always the first entry in the array. 181 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be 182 // in the range [0..n-1]. 183 184 void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); 185 void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) 186 { 187 // compute the centroid of the polygon in cx,cy 188 int i,j; 189 btScalar a,cx,cy,q; 190 if (n==1) { 191 cx = p[0]; 192 cy = p[1]; 193 } 194 else if (n==2) { 195 cx = btScalar(0.5)*(p[0] + p[2]); 196 cy = btScalar(0.5)*(p[1] + p[3]); 197 } 198 else { 199 a = 0; 200 cx = 0; 201 cy = 0; 202 for (i=0; i<(n-1); i++) { 203 q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; 204 a += q; 205 cx += q*(p[i*2]+p[i*2+2]); 206 cy += q*(p[i*2+1]+p[i*2+3]); 207 } 208 q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; 209 if (btFabs(a+q) > SIMD_EPSILON) 210 { 211 a = 1.f/(btScalar(3.0)*(a+q)); 212 } else 213 { 214 a=BT_LARGE_FLOAT; 215 } 216 cx = a*(cx + q*(p[n*2-2]+p[0])); 217 cy = a*(cy + q*(p[n*2-1]+p[1])); 218 } 219 220 // compute the angle of each point w.r.t. the centroid 221 btScalar A[8]; 222 for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx); 223 224 // search for points that have angles closest to A[i0] + i*(2*pi/m). 225 int avail[8]; 226 for (i=0; i<n; i++) avail[i] = 1; 227 avail[i0] = 0; 228 iret[0] = i0; 229 iret++; 230 for (j=1; j<m; j++) { 231 a = btScalar(j)*(2*M__PI/m) + A[i0]; 232 if (a > M__PI) a -= 2*M__PI; 233 btScalar maxdiff=1e9,diff; 234 235 *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 236 237 for (i=0; i<n; i++) { 238 if (avail[i]) { 239 diff = btFabs (A[i]-a); 240 if (diff > M__PI) diff = 2*M__PI - diff; 241 if (diff < maxdiff) { 242 maxdiff = diff; 243 *iret = i; 244 } 245 } 246 } 247 #if defined(DEBUG) || defined (_DEBUG) 248 btAssert (*iret != i0); // ensure iret got set 249 #endif 250 avail[*iret] = 0; 251 iret++; 252 } 253 } 254 255 256 257 int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, 258 const btVector3& side1, const btVector3& p2, 259 const dMatrix3 R2, const btVector3& side2, 260 btVector3& normal, btScalar *depth, int *return_code, 261 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); 262 int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, 263 const btVector3& side1, const btVector3& p2, 264 const dMatrix3 R2, const btVector3& side2, 265 btVector3& normal, btScalar *depth, int *return_code, 266 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) 267 { 268 const btScalar fudge_factor = btScalar(1.05); 269 btVector3 p,pp,normalC(0.f,0.f,0.f); 270 const btScalar *normalR = 0; 271 btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, 272 Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; 273 int i,j,invert_normal,code; 274 275 // get vector from centers of box 1 to box 2, relative to box 1 276 p = p2 - p1; 277 dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 278 279 // get side lengths / 2 280 A[0] = side1[0]*btScalar(0.5); 281 A[1] = side1[1]*btScalar(0.5); 282 A[2] = side1[2]*btScalar(0.5); 283 B[0] = side2[0]*btScalar(0.5); 284 B[1] = side2[1]*btScalar(0.5); 285 B[2] = side2[2]*btScalar(0.5); 286 287 // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 288 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); 289 R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); 290 R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); 291 292 Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); 293 Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); 294 Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); 295 296 // for all 15 possible separating axes: 297 // * see if the axis separates the boxes. if so, return 0. 298 // * find the depth of the penetration along the separating axis (s2) 299 // * if this is the largest depth so far, record it. 300 // the normal vector will be set to the separating axis with the smallest 301 // depth. note: normalR is set to point to a column of R1 or R2 if that is 302 // the smallest depth normal so far. otherwise normalR is 0 and normalC is 303 // set to a vector relative to body 1. invert_normal is 1 if the sign of 304 // the normal should be flipped. 305 306 #define TST(expr1,expr2,norm,cc) \ 307 s2 = btFabs(expr1) - (expr2); \ 308 if (s2 > 0) return 0; \ 309 if (s2 > s) { \ 310 s = s2; \ 311 normalR = norm; \ 312 invert_normal = ((expr1) < 0); \ 313 code = (cc); \ 314 } 315 316 s = -dInfinity; 317 invert_normal = 0; 318 code = 0; 319 320 // separating axis = u1,u2,u3 321 TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); 322 TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); 323 TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); 324 325 // separating axis = v1,v2,v3 326 TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); 327 TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); 328 TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); 329 330 // note: cross product axes need to be scaled when s is computed. 331 // normal (n1,n2,n3) is relative to box 1. 332 #undef TST 333 #define TST(expr1,expr2,n1,n2,n3,cc) \ 334 s2 = btFabs(expr1) - (expr2); \ 335 if (s2 > SIMD_EPSILON) return 0; \ 336 l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ 337 if (l > SIMD_EPSILON) { \ 338 s2 /= l; \ 339 if (s2*fudge_factor > s) { \ 340 s = s2; \ 341 normalR = 0; \ 342 normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ 343 invert_normal = ((expr1) < 0); \ 344 code = (cc); \ 345 } \ 346 } 347 348 btScalar fudge2 (1.0e-5f); 349 350 Q11 += fudge2; 351 Q12 += fudge2; 352 Q13 += fudge2; 353 354 Q21 += fudge2; 355 Q22 += fudge2; 356 Q23 += fudge2; 357 358 Q31 += fudge2; 359 Q32 += fudge2; 360 Q33 += fudge2; 361 362 // separating axis = u1 x (v1,v2,v3) 363 TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); 364 TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); 365 TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); 366 367 // separating axis = u2 x (v1,v2,v3) 368 TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); 369 TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); 370 TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); 371 372 // separating axis = u3 x (v1,v2,v3) 373 TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); 374 TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); 375 TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); 376 377 #undef TST 378 379 if (!code) return 0; 380 381 // if we get to this point, the boxes interpenetrate. compute the normal 382 // in global coordinates. 383 if (normalR) { 384 normal[0] = normalR[0]; 385 normal[1] = normalR[4]; 386 normal[2] = normalR[8]; 387 } 388 else { 389 dMULTIPLY0_331 (normal,R1,normalC); 390 } 391 if (invert_normal) { 392 normal[0] = -normal[0]; 393 normal[1] = -normal[1]; 394 normal[2] = -normal[2]; 395 } 396 *depth = -s; 397 398 // compute contact point(s) 399 400 if (code > 6) { 401 // an edge from box 1 touches an edge from box 2. 402 // find a point pa on the intersecting edge of box 1 403 btVector3 pa; 404 btScalar sign; 405 for (i=0; i<3; i++) pa[i] = p1[i]; 406 for (j=0; j<3; j++) { 407 sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); 408 for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; 409 } 410 411 // find a point pb on the intersecting edge of box 2 412 btVector3 pb; 413 for (i=0; i<3; i++) pb[i] = p2[i]; 414 for (j=0; j<3; j++) { 415 sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); 416 for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; 417 } 418 419 btScalar alpha,beta; 420 btVector3 ua,ub; 421 for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; 422 for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; 423 424 dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); 425 for (i=0; i<3; i++) pa[i] += ua[i]*alpha; 426 for (i=0; i<3; i++) pb[i] += ub[i]*beta; 427 428 { 429 430 //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); 431 //contact[0].depth = *depth; 432 btVector3 pointInWorld; 433 434 #ifdef USE_CENTER_POINT 435 for (i=0; i<3; i++) 436 pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); 437 output.addContactPoint(-normal,pointInWorld,-*depth); 438 #else 439 output.addContactPoint(-normal,pb,-*depth); 440 441 #endif // 442 *return_code = code; 443 } 444 return 1; 445 } 446 447 // okay, we have a face-something intersection (because the separating 448 // axis is perpendicular to a face). define face 'a' to be the reference 449 // face (i.e. the normal vector is perpendicular to this) and face 'b' to be 450 // the incident face (the closest face of the other box). 451 452 const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; 453 if (code <= 3) { 454 Ra = R1; 455 Rb = R2; 456 pa = p1; 457 pb = p2; 458 Sa = A; 459 Sb = B; 460 } 461 else { 462 Ra = R2; 463 Rb = R1; 464 pa = p2; 465 pb = p1; 466 Sa = B; 467 Sb = A; 468 } 469 470 // nr = normal vector of reference face dotted with axes of incident box. 471 // anr = absolute values of nr. 472 btVector3 normal2,nr,anr; 473 if (code <= 3) { 474 normal2[0] = normal[0]; 475 normal2[1] = normal[1]; 476 normal2[2] = normal[2]; 477 } 478 else { 479 normal2[0] = -normal[0]; 480 normal2[1] = -normal[1]; 481 normal2[2] = -normal[2]; 482 } 483 dMULTIPLY1_331 (nr,Rb,normal2); 484 anr[0] = btFabs (nr[0]); 485 anr[1] = btFabs (nr[1]); 486 anr[2] = btFabs (nr[2]); 487 488 // find the largest compontent of anr: this corresponds to the normal 489 // for the indident face. the other axis numbers of the indicent face 490 // are stored in a1,a2. 491 int lanr,a1,a2; 492 if (anr[1] > anr[0]) { 493 if (anr[1] > anr[2]) { 494 a1 = 0; 495 lanr = 1; 496 a2 = 2; 497 } 498 else { 499 a1 = 0; 500 a2 = 1; 501 lanr = 2; 502 } 503 } 504 else { 505 if (anr[0] > anr[2]) { 506 lanr = 0; 507 a1 = 1; 508 a2 = 2; 509 } 510 else { 511 a1 = 0; 512 a2 = 1; 513 lanr = 2; 514 } 515 } 516 517 // compute center point of incident face, in reference-face coordinates 518 btVector3 center; 519 if (nr[lanr] < 0) { 520 for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; 521 } 522 else { 523 for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; 524 } 525 526 // find the normal and non-normal axis numbers of the reference box 527 int codeN,code1,code2; 528 if (code <= 3) codeN = code-1; else codeN = code-4; 529 if (codeN==0) { 530 code1 = 1; 531 code2 = 2; 532 } 533 else if (codeN==1) { 534 code1 = 0; 535 code2 = 2; 536 } 537 else { 538 code1 = 0; 539 code2 = 1; 540 } 541 542 // find the four corners of the incident face, in reference-face coordinates 543 btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) 544 btScalar c1,c2,m11,m12,m21,m22; 545 c1 = dDOT14 (center,Ra+code1); 546 c2 = dDOT14 (center,Ra+code2); 547 // optimize this? - we have already computed this data above, but it is not 548 // stored in an easy-to-index format. for now it's quicker just to recompute 549 // the four dot products. 550 m11 = dDOT44 (Ra+code1,Rb+a1); 551 m12 = dDOT44 (Ra+code1,Rb+a2); 552 m21 = dDOT44 (Ra+code2,Rb+a1); 553 m22 = dDOT44 (Ra+code2,Rb+a2); 554 { 555 btScalar k1 = m11*Sb[a1]; 556 btScalar k2 = m21*Sb[a1]; 557 btScalar k3 = m12*Sb[a2]; 558 btScalar k4 = m22*Sb[a2]; 559 quad[0] = c1 - k1 - k3; 560 quad[1] = c2 - k2 - k4; 561 quad[2] = c1 - k1 + k3; 562 quad[3] = c2 - k2 + k4; 563 quad[4] = c1 + k1 + k3; 564 quad[5] = c2 + k2 + k4; 565 quad[6] = c1 + k1 - k3; 566 quad[7] = c2 + k2 - k4; 567 } 568 569 // find the size of the reference face 570 btScalar rect[2]; 571 rect[0] = Sa[code1]; 572 rect[1] = Sa[code2]; 573 574 // intersect the incident and reference faces 575 btScalar ret[16]; 576 int n = intersectRectQuad2 (rect,quad,ret); 577 if (n < 1) return 0; // this should never happen 578 579 // convert the intersection points into reference-face coordinates, 580 // and compute the contact position and depth for each point. only keep 581 // those points that have a positive (penetrating) depth. delete points in 582 // the 'ret' array as necessary so that 'point' and 'ret' correspond. 583 btScalar point[3*8]; // penetrating contact points 584 btScalar dep[8]; // depths for those points 585 btScalar det1 = 1.f/(m11*m22 - m12*m21); 586 m11 *= det1; 587 m12 *= det1; 588 m21 *= det1; 589 m22 *= det1; 590 int cnum = 0; // number of penetrating contact points found 591 for (j=0; j < n; j++) { 592 btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); 593 btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); 594 for (i=0; i<3; i++) point[cnum*3+i] = 595 center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; 596 dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); 597 if (dep[cnum] >= 0) { 598 ret[cnum*2] = ret[j*2]; 599 ret[cnum*2+1] = ret[j*2+1]; 600 cnum++; 601 } 602 } 603 if (cnum < 1) return 0; // this should never happen 604 605 // we can't generate more contacts than we actually have 606 if (maxc > cnum) maxc = cnum; 607 if (maxc < 1) maxc = 1; 608 609 if (cnum <= maxc) { 610 611 if (code<4) 612 { 613 // we have less contacts than we need, so we use them all 614 for (j=0; j < cnum; j++) 615 { 616 btVector3 pointInWorld; 617 for (i=0; i<3; i++) 618 pointInWorld[i] = point[j*3+i] + pa[i]; 619 output.addContactPoint(-normal,pointInWorld,-dep[j]); 620 621 } 622 } else 623 { 624 // we have less contacts than we need, so we use them all 625 for (j=0; j < cnum; j++) 626 { 627 btVector3 pointInWorld; 628 for (i=0; i<3; i++) 629 pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j]; 630 //pointInWorld[i] = point[j*3+i] + pa[i]; 631 output.addContactPoint(-normal,pointInWorld,-dep[j]); 632 } 633 } 634 } 635 else { 636 // we have more contacts than are wanted, some of them must be culled. 637 // find the deepest point, it is always the first contact. 638 int i1 = 0; 639 btScalar maxdepth = dep[0]; 640 for (i=1; i<cnum; i++) { 641 if (dep[i] > maxdepth) { 642 maxdepth = dep[i]; 643 i1 = i; 644 } 645 } 646 647 int iret[8]; 648 cullPoints2 (cnum,ret,maxc,i1,iret); 649 650 for (j=0; j < maxc; j++) { 651 // dContactGeom *con = CONTACT(contact,skip*j); 652 // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; 653 // con->depth = dep[iret[j]]; 654 655 btVector3 posInWorld; 656 for (i=0; i<3; i++) 657 posInWorld[i] = point[iret[j]*3+i] + pa[i]; 658 if (code<4) 659 { 660 output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); 661 } else 662 { 663 output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]); 664 } 665 } 666 cnum = maxc; 667 } 668 669 *return_code = code; 670 return cnum; 671 } 672 673 void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) 674 { 675 676 const btTransform& transformA = input.m_transformA; 677 const btTransform& transformB = input.m_transformB; 678 679 int skip = 0; 680 dContactGeom *contact = 0; 681 682 dMatrix3 R1; 683 dMatrix3 R2; 684 685 for (int j=0;j<3;j++) 686 { 687 R1[0+4*j] = transformA.getBasis()[j].x(); 688 R2[0+4*j] = transformB.getBasis()[j].x(); 689 690 R1[1+4*j] = transformA.getBasis()[j].y(); 691 R2[1+4*j] = transformB.getBasis()[j].y(); 692 693 694 R1[2+4*j] = transformA.getBasis()[j].z(); 695 R2[2+4*j] = transformB.getBasis()[j].z(); 696 697 } 698 699 700 701 btVector3 normal; 702 btScalar depth; 703 int return_code; 704 int maxc = 4; 705 706 707 dBoxBox2 (transformA.getOrigin(), 708 R1, 709 2.f*m_box1->getHalfExtentsWithMargin(), 710 transformB.getOrigin(), 711 R2, 712 2.f*m_box2->getHalfExtentsWithMargin(), 713 normal, &depth, &return_code, 714 maxc, contact, skip, 715 output 716 ); 717 718 } 719
