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@q12.org Web: www.q12.org
5 Bullet Continuous Collision Detection and Physics Library
6 Bullet is Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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:
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.
19 ///ODE box-box collision detection is adapted to work with Bullet
21 #include "btBoxBoxDetector.h"
22 #include "BulletCollision/CollisionShapes/btBoxShape.h"
27 btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2)
33 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
34 // generate contact points. this returns 0 if there is no contact otherwise
35 // it returns the number of contacts generated.
36 // `normal' returns the contact normal.
37 // `depth' returns the maximum penetration depth along that normal.
38 // `return_code' returns a number indicating the type of contact that was
40 // 1,2,3 = box 2 intersects with a face of box 1
41 // 4,5,6 = box 1 intersects with a face of box 2
42 // 7..15 = edge-edge contact
43 // `maxc' is the maximum number of contacts allowed to be generated, i.e.
44 // the size of the `contact' array.
45 // `contact' and `skip' are the contact array information provided to the
46 // collision functions. this function only fills in the position and depth
49 #define dDOTpq(a, b, p, q) ((a)[0] * (b)[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)])
50 #define dInfinity FLT_MAX
52 /*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
53 PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
54 PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
55 PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
57 static btScalar dDOT(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 1); }
58 static btScalar dDOT44(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 4); }
59 static btScalar dDOT41(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 1); }
60 static btScalar dDOT14(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 4); }
61 #define dMULTIPLYOP1_331(A, op, B, C) \
63 (A)[0] op dDOT41((B), (C)); \
64 (A)[1] op dDOT41((B + 1), (C)); \
65 (A)[2] op dDOT41((B + 2), (C)); \
68 #define dMULTIPLYOP0_331(A, op, B, C) \
70 (A)[0] op dDOT((B), (C)); \
71 (A)[1] op dDOT((B + 4), (C)); \
72 (A)[2] op dDOT((B + 8), (C)); \
75 #define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C)
76 #define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C)
78 typedef btScalar dMatrix3[4 * 3];
80 void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
81 const btVector3& pb, const btVector3& ub,
82 btScalar* alpha, btScalar* beta);
83 void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
84 const btVector3& pb, const btVector3& ub,
85 btScalar* alpha, btScalar* beta)
91 btScalar uaub = dDOT(ua, ub);
92 btScalar q1 = dDOT(ua, p);
93 btScalar q2 = -dDOT(ub, p);
94 btScalar d = 1 - uaub * uaub;
95 if (d <= btScalar(0.0001f))
97 // @@@ this needs to be made more robust
104 *alpha = (q1 + uaub * q2) * d;
105 *beta = (uaub * q1 + q2) * d;
109 // find all the intersection points between the 2D rectangle with vertices
110 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
111 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
113 // the intersection points are returned as x,y pairs in the 'ret' array.
114 // the number of intersection points is returned by the function (this will
115 // be in the range 0 to 8).
117 static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
119 // q (and r) contain nq (and nr) coordinate points for the current (and
125 for (int dir = 0; dir <= 1; dir++)
127 // direction notation: xy[0] = x axis, xy[1] = y axis
128 for (int sign = -1; sign <= 1; sign += 2)
130 // chop q along the line xy[dir] = sign*h[dir]
134 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])
139 // this point is inside the chopping line
150 btScalar* nextq = (i > 1) ? pq + 2 : q;
151 if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir]))
153 // this line crosses the chopping line
154 pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) /
155 (nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]);
156 pr[dir] = sign * h[dir];
168 r = (q == ret) ? buffer : ret;
173 if (q != ret) memcpy(ret, q, nr * 2 * sizeof(btScalar));
177 #define M__PI 3.14159265f
179 // given n points in the plane (array p, of size 2*n), generate m points that
180 // best represent the whole set. the definition of 'best' here is not
181 // predetermined - the idea is to select points that give good box-box
182 // collision detection behavior. the chosen point indexes are returned in the
183 // array iret (of size m). 'i0' is always the first entry in the array.
184 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
185 // in the range [0..n-1].
187 void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]);
188 void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
190 // compute the centroid of the polygon in cx,cy
192 btScalar a, cx, cy, q;
200 cx = btScalar(0.5) * (p[0] + p[2]);
201 cy = btScalar(0.5) * (p[1] + p[3]);
208 for (i = 0; i < (n - 1); i++)
210 q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
212 cx += q * (p[i * 2] + p[i * 2 + 2]);
213 cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
215 q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
216 if (btFabs(a + q) > SIMD_EPSILON)
218 a = 1.f / (btScalar(3.0) * (a + q));
224 cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
225 cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
228 // compute the angle of each point w.r.t. the centroid
230 for (i = 0; i < n; i++) A[i] = btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
232 // search for points that have angles closest to A[i0] + i*(2*pi/m).
234 for (i = 0; i < n; i++) avail[i] = 1;
238 for (j = 1; j < m; j++)
240 a = btScalar(j) * (2 * M__PI / m) + A[i0];
241 if (a > M__PI) a -= 2 * M__PI;
242 btScalar maxdiff = 1e9, diff;
244 *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
246 for (i = 0; i < n; i++)
250 diff = btFabs(A[i] - a);
251 if (diff > M__PI) diff = 2 * M__PI - diff;
259 #if defined(DEBUG) || defined(_DEBUG)
260 btAssert(*iret != i0); // ensure iret got set
267 int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
268 const btVector3& side1, const btVector3& p2,
269 const dMatrix3 R2, const btVector3& side2,
270 btVector3& normal, btScalar* depth, int* return_code,
271 int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output);
272 int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
273 const btVector3& side1, const btVector3& p2,
274 const dMatrix3 R2, const btVector3& side2,
275 btVector3& normal, btScalar* depth, int* return_code,
276 int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output)
278 const btScalar fudge_factor = btScalar(1.05);
279 btVector3 p, pp, normalC(0.f, 0.f, 0.f);
280 const btScalar* normalR = 0;
281 btScalar A[3], B[3], R11, R12, R13, R21, R22, R23, R31, R32, R33,
282 Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l;
283 int i, j, invert_normal, code;
285 // get vector from centers of box 1 to box 2, relative to box 1
287 dMULTIPLY1_331(pp, R1, p); // get pp = p relative to body 1
289 // get side lengths / 2
290 A[0] = side1[0] * btScalar(0.5);
291 A[1] = side1[1] * btScalar(0.5);
292 A[2] = side1[2] * btScalar(0.5);
293 B[0] = side2[0] * btScalar(0.5);
294 B[1] = side2[1] * btScalar(0.5);
295 B[2] = side2[2] * btScalar(0.5);
297 // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
298 R11 = dDOT44(R1 + 0, R2 + 0);
299 R12 = dDOT44(R1 + 0, R2 + 1);
300 R13 = dDOT44(R1 + 0, R2 + 2);
301 R21 = dDOT44(R1 + 1, R2 + 0);
302 R22 = dDOT44(R1 + 1, R2 + 1);
303 R23 = dDOT44(R1 + 1, R2 + 2);
304 R31 = dDOT44(R1 + 2, R2 + 0);
305 R32 = dDOT44(R1 + 2, R2 + 1);
306 R33 = dDOT44(R1 + 2, R2 + 2);
318 // for all 15 possible separating axes:
319 // * see if the axis separates the boxes. if so, return 0.
320 // * find the depth of the penetration along the separating axis (s2)
321 // * if this is the largest depth so far, record it.
322 // the normal vector will be set to the separating axis with the smallest
323 // depth. note: normalR is set to point to a column of R1 or R2 if that is
324 // the smallest depth normal so far. otherwise normalR is 0 and normalC is
325 // set to a vector relative to body 1. invert_normal is 1 if the sign of
326 // the normal should be flipped.
328 #define TST(expr1, expr2, norm, cc) \
329 s2 = btFabs(expr1) - (expr2); \
330 if (s2 > 0) return 0; \
335 invert_normal = ((expr1) < 0); \
343 // separating axis = u1,u2,u3
344 TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1);
345 TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2);
346 TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3);
348 // separating axis = v1,v2,v3
349 TST(dDOT41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4);
350 TST(dDOT41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5);
351 TST(dDOT41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6);
353 // note: cross product axes need to be scaled when s is computed.
354 // normal (n1,n2,n3) is relative to box 1.
356 #define TST(expr1, expr2, n1, n2, n3, cc) \
357 s2 = btFabs(expr1) - (expr2); \
358 if (s2 > SIMD_EPSILON) return 0; \
359 l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \
360 if (l > SIMD_EPSILON) \
363 if (s2 * fudge_factor > s) \
367 normalC[0] = (n1) / l; \
368 normalC[1] = (n2) / l; \
369 normalC[2] = (n3) / l; \
370 invert_normal = ((expr1) < 0); \
375 btScalar fudge2(1.0e-5f);
389 // separating axis = u1 x (v1,v2,v3)
390 TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7);
391 TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8);
392 TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9);
394 // separating axis = u2 x (v1,v2,v3)
395 TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10);
396 TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11);
397 TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12);
399 // separating axis = u3 x (v1,v2,v3)
400 TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13);
401 TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14);
402 TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15);
408 // if we get to this point, the boxes interpenetrate. compute the normal
409 // in global coordinates.
412 normal[0] = normalR[0];
413 normal[1] = normalR[4];
414 normal[2] = normalR[8];
418 dMULTIPLY0_331(normal, R1, normalC);
422 normal[0] = -normal[0];
423 normal[1] = -normal[1];
424 normal[2] = -normal[2];
428 // compute contact point(s)
432 // an edge from box 1 touches an edge from box 2.
433 // find a point pa on the intersecting edge of box 1
436 for (i = 0; i < 3; i++) pa[i] = p1[i];
437 for (j = 0; j < 3; j++)
439 sign = (dDOT14(normal, R1 + j) > 0) ? btScalar(1.0) : btScalar(-1.0);
440 for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j];
443 // find a point pb on the intersecting edge of box 2
445 for (i = 0; i < 3; i++) pb[i] = p2[i];
446 for (j = 0; j < 3; j++)
448 sign = (dDOT14(normal, R2 + j) > 0) ? btScalar(-1.0) : btScalar(1.0);
449 for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j];
452 btScalar alpha, beta;
454 for (i = 0; i < 3; i++) ua[i] = R1[((code)-7) / 3 + i * 4];
455 for (i = 0; i < 3; i++) ub[i] = R2[((code)-7) % 3 + i * 4];
457 dLineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
458 for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha;
459 for (i = 0; i < 3; i++) pb[i] += ub[i] * beta;
462 //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
463 //contact[0].depth = *depth;
464 btVector3 pointInWorld;
466 #ifdef USE_CENTER_POINT
467 for (i = 0; i < 3; i++)
468 pointInWorld[i] = (pa[i] + pb[i]) * btScalar(0.5);
469 output.addContactPoint(-normal, pointInWorld, -*depth);
471 output.addContactPoint(-normal, pb, -*depth);
479 // okay, we have a face-something intersection (because the separating
480 // axis is perpendicular to a face). define face 'a' to be the reference
481 // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
482 // the incident face (the closest face of the other box).
484 const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb;
504 // nr = normal vector of reference face dotted with axes of incident box.
505 // anr = absolute values of nr.
506 btVector3 normal2, nr, anr;
509 normal2[0] = normal[0];
510 normal2[1] = normal[1];
511 normal2[2] = normal[2];
515 normal2[0] = -normal[0];
516 normal2[1] = -normal[1];
517 normal2[2] = -normal[2];
519 dMULTIPLY1_331(nr, Rb, normal2);
520 anr[0] = btFabs(nr[0]);
521 anr[1] = btFabs(nr[1]);
522 anr[2] = btFabs(nr[2]);
524 // find the largest compontent of anr: this corresponds to the normal
525 // for the indident face. the other axis numbers of the indicent face
526 // are stored in a1,a2.
559 // compute center point of incident face, in reference-face coordinates
563 for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
567 for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
570 // find the normal and non-normal axis numbers of the reference box
571 int codeN, code1, code2;
592 // find the four corners of the incident face, in reference-face coordinates
593 btScalar quad[8]; // 2D coordinate of incident face (x,y pairs)
594 btScalar c1, c2, m11, m12, m21, m22;
595 c1 = dDOT14(center, Ra + code1);
596 c2 = dDOT14(center, Ra + code2);
597 // optimize this? - we have already computed this data above, but it is not
598 // stored in an easy-to-index format. for now it's quicker just to recompute
599 // the four dot products.
600 m11 = dDOT44(Ra + code1, Rb + a1);
601 m12 = dDOT44(Ra + code1, Rb + a2);
602 m21 = dDOT44(Ra + code2, Rb + a1);
603 m22 = dDOT44(Ra + code2, Rb + a2);
605 btScalar k1 = m11 * Sb[a1];
606 btScalar k2 = m21 * Sb[a1];
607 btScalar k3 = m12 * Sb[a2];
608 btScalar k4 = m22 * Sb[a2];
609 quad[0] = c1 - k1 - k3;
610 quad[1] = c2 - k2 - k4;
611 quad[2] = c1 - k1 + k3;
612 quad[3] = c2 - k2 + k4;
613 quad[4] = c1 + k1 + k3;
614 quad[5] = c2 + k2 + k4;
615 quad[6] = c1 + k1 - k3;
616 quad[7] = c2 + k2 - k4;
619 // find the size of the reference face
624 // intersect the incident and reference faces
626 int n = intersectRectQuad2(rect, quad, ret);
627 if (n < 1) return 0; // this should never happen
629 // convert the intersection points into reference-face coordinates,
630 // and compute the contact position and depth for each point. only keep
631 // those points that have a positive (penetrating) depth. delete points in
632 // the 'ret' array as necessary so that 'point' and 'ret' correspond.
633 btScalar point[3 * 8]; // penetrating contact points
634 btScalar dep[8]; // depths for those points
635 btScalar det1 = 1.f / (m11 * m22 - m12 * m21);
640 int cnum = 0; // number of penetrating contact points found
641 for (j = 0; j < n; j++)
643 btScalar k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
644 btScalar k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
645 for (i = 0; i < 3; i++) point[cnum * 3 + i] =
646 center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
647 dep[cnum] = Sa[codeN] - dDOT(normal2, point + cnum * 3);
650 ret[cnum * 2] = ret[j * 2];
651 ret[cnum * 2 + 1] = ret[j * 2 + 1];
655 if (cnum < 1) return 0; // this should never happen
657 // we can't generate more contacts than we actually have
658 if (maxc > cnum) maxc = cnum;
659 if (maxc < 1) maxc = 1;
665 // we have less contacts than we need, so we use them all
666 for (j = 0; j < cnum; j++)
668 btVector3 pointInWorld;
669 for (i = 0; i < 3; i++)
670 pointInWorld[i] = point[j * 3 + i] + pa[i];
671 output.addContactPoint(-normal, pointInWorld, -dep[j]);
676 // we have less contacts than we need, so we use them all
677 for (j = 0; j < cnum; j++)
679 btVector3 pointInWorld;
680 for (i = 0; i < 3; i++)
681 pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j];
682 //pointInWorld[i] = point[j*3+i] + pa[i];
683 output.addContactPoint(-normal, pointInWorld, -dep[j]);
689 // we have more contacts than are wanted, some of them must be culled.
690 // find the deepest point, it is always the first contact.
692 btScalar maxdepth = dep[0];
693 for (i = 1; i < cnum; i++)
695 if (dep[i] > maxdepth)
703 cullPoints2(cnum, ret, maxc, i1, iret);
705 for (j = 0; j < maxc; j++)
707 // dContactGeom *con = CONTACT(contact,skip*j);
708 // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
709 // con->depth = dep[iret[j]];
711 btVector3 posInWorld;
712 for (i = 0; i < 3; i++)
713 posInWorld[i] = point[iret[j] * 3 + i] + pa[i];
716 output.addContactPoint(-normal, posInWorld, -dep[iret[j]]);
720 output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
730 void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* /*debugDraw*/, bool /*swapResults*/)
732 const btTransform& transformA = input.m_transformA;
733 const btTransform& transformB = input.m_transformB;
736 dContactGeom* contact = 0;
741 for (int j = 0; j < 3; j++)
743 R1[0 + 4 * j] = transformA.getBasis()[j].x();
744 R2[0 + 4 * j] = transformB.getBasis()[j].x();
746 R1[1 + 4 * j] = transformA.getBasis()[j].y();
747 R2[1 + 4 * j] = transformB.getBasis()[j].y();
749 R1[2 + 4 * j] = transformA.getBasis()[j].z();
750 R2[2 + 4 * j] = transformB.getBasis()[j].z();
758 dBoxBox2(transformA.getOrigin(),
760 2.f * m_box1->getHalfExtentsWithMargin(),
761 transformB.getOrigin(),
763 2.f * m_box2->getHalfExtentsWithMargin(),
764 normal, &depth, &return_code,