dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp

   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@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
   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 // 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
  39 // detected:
  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
  47 // fields.
  48 struct dContactGeom;
  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
  51
  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); }
  56 */
  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)   \
  62         {                                   \
  63                 (A)[0] op dDOT41((B), (C));     \
  64                 (A)[1] op dDOT41((B + 1), (C)); \
  65                 (A)[2] op dDOT41((B + 2), (C)); \
  66         }
  67
  68 #define dMULTIPLYOP0_331(A, op, B, C) \
  69         {                                 \
  70                 (A)[0] op dDOT((B), (C));     \
  71                 (A)[1] op dDOT((B + 4), (C)); \
  72                 (A)[2] op dDOT((B + 8), (C)); \
  73         }
  74
  75 #define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C)
  76 #define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C)
  77
  78 typedef btScalar dMatrix3[4 * 3];
  79
  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)
  86 {
  87         btVector3 p;
  88         p[0] = pb[0] - pa[0];
  89         p[1] = pb[1] - pa[1];
  90         p[2] = pb[2] - pa[2];
  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))
  96         {
  97                 // @@@ this needs to be made more robust
  98                 *alpha = 0;
  99                 *beta = 0;
 100         }
 101         else
 102         {
 103                 d = 1.f / d;
 104                 *alpha = (q1 + uaub * q2) * d;
 105                 *beta = (uaub * q1 + q2) * d;
 106         }
 107 }
 108
 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]).
 112 //
 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).
 116
 117 static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
 118 {
 119         // q (and r) contain nq (and nr) coordinate points for the current (and
 120         // chopped) polygons
 121         int nq = 4, nr = 0;
 122         btScalar buffer[16];
 123         btScalar* q = p;
 124         btScalar* r = ret;
 125         for (int dir = 0; dir <= 1; dir++)
 126         {
 127                 // direction notation: xy[0] = x axis, xy[1] = y axis
 128                 for (int sign = -1; sign <= 1; sign += 2)
 129                 {
 130                         // chop q along the line xy[dir] = sign*h[dir]
 131                         btScalar* pq = q;
 132                         btScalar* pr = r;
 133                         nr = 0;
 134                         for (int i = nq; i > 0; i--)
 135                         {
 136                                 // go through all points in q and all lines between adjacent points
 137                                 if (sign * pq[dir] < h[dir])
 138                                 {
 139                                         // this point is inside the chopping line
 140                                         pr[0] = pq[0];
 141                                         pr[1] = pq[1];
 142                                         pr += 2;
 143                                         nr++;
 144                                         if (nr & 8)
 145                                         {
 146                                                 q = r;
 147                                                 goto done;
 148                                         }
 149                                 }
 150                                 btScalar* nextq = (i > 1) ? pq + 2 : q;
 151                                 if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir]))
 152                                 {
 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];
 157                                         pr += 2;
 158                                         nr++;
 159                                         if (nr & 8)
 160                                         {
 161                                                 q = r;
 162                                                 goto done;
 163                                         }
 164                                 }
 165                                 pq += 2;
 166                         }
 167                         q = r;
 168                         r = (q == ret) ? buffer : ret;
 169                         nq = nr;
 170                 }
 171         }
 172 done:
 173         if (q != ret) memcpy(ret, q, nr * 2 * sizeof(btScalar));
 174         return nr;
 175 }
 176
 177 #define M__PI 3.14159265f
 178
 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].
 186
 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[])
 189 {
 190         // compute the centroid of the polygon in cx,cy
 191         int i, j;
 192         btScalar a, cx, cy, q;
 193         if (n == 1)
 194         {
 195                 cx = p[0];
 196                 cy = p[1];
 197         }
 198         else if (n == 2)
 199         {
 200                 cx = btScalar(0.5) * (p[0] + p[2]);
 201                 cy = btScalar(0.5) * (p[1] + p[3]);
 202         }
 203         else
 204         {
 205                 a = 0;
 206                 cx = 0;
 207                 cy = 0;
 208                 for (i = 0; i < (n - 1); i++)
 209                 {
 210                         q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
 211                         a += q;
 212                         cx += q * (p[i * 2] + p[i * 2 + 2]);
 213                         cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
 214                 }
 215                 q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
 216                 if (btFabs(a + q) > SIMD_EPSILON)
 217                 {
 218                         a = 1.f / (btScalar(3.0) * (a + q));
 219                 }
 220                 else
 221                 {
 222                         a = BT_LARGE_FLOAT;
 223                 }
 224                 cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
 225                 cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
 226         }
 227
 228         // compute the angle of each point w.r.t. the centroid
 229         btScalar A[8];
 230         for (i = 0; i < n; i++) A[i] = btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
 231
 232         // search for points that have angles closest to A[i0] + i*(2*pi/m).
 233         int avail[8];
 234         for (i = 0; i < n; i++) avail[i] = 1;
 235         avail[i0] = 0;
 236         iret[0] = i0;
 237         iret++;
 238         for (j = 1; j < m; j++)
 239         {
 240                 a = btScalar(j) * (2 * M__PI / m) + A[i0];
 241                 if (a > M__PI) a -= 2 * M__PI;
 242                 btScalar maxdiff = 1e9, diff;
 243
 244                 *iret = i0;  // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
 245
 246                 for (i = 0; i < n; i++)
 247                 {
 248                         if (avail[i])
 249                         {
 250                                 diff = btFabs(A[i] - a);
 251                                 if (diff > M__PI) diff = 2 * M__PI - diff;
 252                                 if (diff < maxdiff)
 253                                 {
 254                                         maxdiff = diff;
 255                                         *iret = i;
 256                                 }
 257                         }
 258                 }
 259 #if defined(DEBUG) || defined(_DEBUG)
 260                 btAssert(*iret != i0);  // ensure iret got set
 261 #endif
 262                 avail[*iret] = 0;
 263                 iret++;
 264         }
 265 }
 266
 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)
 277 {
 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;
 284
 285         // get vector from centers of box 1 to box 2, relative to box 1
 286         p = p2 - p1;
 287         dMULTIPLY1_331(pp, R1, p);  // get pp = p relative to body 1
 288
 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);
 296
 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);
 307
 308         Q11 = btFabs(R11);
 309         Q12 = btFabs(R12);
 310         Q13 = btFabs(R13);
 311         Q21 = btFabs(R21);
 312         Q22 = btFabs(R22);
 313         Q23 = btFabs(R23);
 314         Q31 = btFabs(R31);
 315         Q32 = btFabs(R32);
 316         Q33 = btFabs(R33);
 317
 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.
 327
 328 #define TST(expr1, expr2, norm, cc)    \
 329         s2 = btFabs(expr1) - (expr2);      \
 330         if (s2 > 0) return 0;              \
 331         if (s2 > s)                        \
 332         {                                  \
 333                 s = s2;                        \
 334                 normalR = norm;                \
 335                 invert_normal = ((expr1) < 0); \
 336                 code = (cc);                   \
 337         }
 338
 339         s = -dInfinity;
 340         invert_normal = 0;
 341         code = 0;
 342
 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);
 347
 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);
 352
 353         // note: cross product axes need to be scaled when s is computed.
 354         // normal (n1,n2,n3) is relative to box 1.
 355 #undef TST
 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)                                \
 361         {                                                    \
 362                 s2 /= l;                                         \
 363                 if (s2 * fudge_factor > s)                       \
 364                 {                                                \
 365                         s = s2;                                      \
 366                         normalR = 0;                                 \
 367                         normalC[0] = (n1) / l;                       \
 368                         normalC[1] = (n2) / l;                       \
 369                         normalC[2] = (n3) / l;                       \
 370                         invert_normal = ((expr1) < 0);               \
 371                         code = (cc);                                 \
 372                 }                                                \
 373         }
 374
 375         btScalar fudge2(1.0e-5f);
 376
 377         Q11 += fudge2;
 378         Q12 += fudge2;
 379         Q13 += fudge2;
 380
 381         Q21 += fudge2;
 382         Q22 += fudge2;
 383         Q23 += fudge2;
 384
 385         Q31 += fudge2;
 386         Q32 += fudge2;
 387         Q33 += fudge2;
 388
 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);
 393
 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);
 398
 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);
 403
 404 #undef TST
 405
 406         if (!code) return 0;
 407
 408         // if we get to this point, the boxes interpenetrate. compute the normal
 409         // in global coordinates.
 410         if (normalR)
 411         {
 412                 normal[0] = normalR[0];
 413                 normal[1] = normalR[4];
 414                 normal[2] = normalR[8];
 415         }
 416         else
 417         {
 418                 dMULTIPLY0_331(normal, R1, normalC);
 419         }
 420         if (invert_normal)
 421         {
 422                 normal[0] = -normal[0];
 423                 normal[1] = -normal[1];
 424                 normal[2] = -normal[2];
 425         }
 426         *depth = -s;
 427
 428         // compute contact point(s)
 429
 430         if (code > 6)
 431         {
 432                 // an edge from box 1 touches an edge from box 2.
 433                 // find a point pa on the intersecting edge of box 1
 434                 btVector3 pa;
 435                 btScalar sign;
 436                 for (i = 0; i < 3; i++) pa[i] = p1[i];
 437                 for (j = 0; j < 3; j++)
 438                 {
 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];
 441                 }
 442
 443                 // find a point pb on the intersecting edge of box 2
 444                 btVector3 pb;
 445                 for (i = 0; i < 3; i++) pb[i] = p2[i];
 446                 for (j = 0; j < 3; j++)
 447                 {
 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];
 450                 }
 451
 452                 btScalar alpha, beta;
 453                 btVector3 ua, ub;
 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];
 456
 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;
 460
 461                 {
 462                         //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
 463                         //contact[0].depth = *depth;
 464                         btVector3 pointInWorld;
 465
 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);
 470 #else
 471                         output.addContactPoint(-normal, pb, -*depth);
 472
 473 #endif  //
 474                         *return_code = code;
 475                 }
 476                 return 1;
 477         }
 478
 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).
 483
 484         const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb;
 485         if (code <= 3)
 486         {
 487                 Ra = R1;
 488                 Rb = R2;
 489                 pa = p1;
 490                 pb = p2;
 491                 Sa = A;
 492                 Sb = B;
 493         }
 494         else
 495         {
 496                 Ra = R2;
 497                 Rb = R1;
 498                 pa = p2;
 499                 pb = p1;
 500                 Sa = B;
 501                 Sb = A;
 502         }
 503
 504         // nr = normal vector of reference face dotted with axes of incident box.
 505         // anr = absolute values of nr.
 506         btVector3 normal2, nr, anr;
 507         if (code <= 3)
 508         {
 509                 normal2[0] = normal[0];
 510                 normal2[1] = normal[1];
 511                 normal2[2] = normal[2];
 512         }
 513         else
 514         {
 515                 normal2[0] = -normal[0];
 516                 normal2[1] = -normal[1];
 517                 normal2[2] = -normal[2];
 518         }
 519         dMULTIPLY1_331(nr, Rb, normal2);
 520         anr[0] = btFabs(nr[0]);
 521         anr[1] = btFabs(nr[1]);
 522         anr[2] = btFabs(nr[2]);
 523
 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.
 527         int lanr, a1, a2;
 528         if (anr[1] > anr[0])
 529         {
 530                 if (anr[1] > anr[2])
 531                 {
 532                         a1 = 0;
 533                         lanr = 1;
 534                         a2 = 2;
 535                 }
 536                 else
 537                 {
 538                         a1 = 0;
 539                         a2 = 1;
 540                         lanr = 2;
 541                 }
 542         }
 543         else
 544         {
 545                 if (anr[0] > anr[2])
 546                 {
 547                         lanr = 0;
 548                         a1 = 1;
 549                         a2 = 2;
 550                 }
 551                 else
 552                 {
 553                         a1 = 0;
 554                         a2 = 1;
 555                         lanr = 2;
 556                 }
 557         }
 558
 559         // compute center point of incident face, in reference-face coordinates
 560         btVector3 center;
 561         if (nr[lanr] < 0)
 562         {
 563                 for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
 564         }
 565         else
 566         {
 567                 for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
 568         }
 569
 570         // find the normal and non-normal axis numbers of the reference box
 571         int codeN, code1, code2;
 572         if (code <= 3)
 573                 codeN = code - 1;
 574         else
 575                 codeN = code - 4;
 576         if (codeN == 0)
 577         {
 578                 code1 = 1;
 579                 code2 = 2;
 580         }
 581         else if (codeN == 1)
 582         {
 583                 code1 = 0;
 584                 code2 = 2;
 585         }
 586         else
 587         {
 588                 code1 = 0;
 589                 code2 = 1;
 590         }
 591
 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);
 604         {
 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;
 617         }
 618
 619         // find the size of the reference face
 620         btScalar rect[2];
 621         rect[0] = Sa[code1];
 622         rect[1] = Sa[code2];
 623
 624         // intersect the incident and reference faces
 625         btScalar ret[16];
 626         int n = intersectRectQuad2(rect, quad, ret);
 627         if (n < 1) return 0;  // this should never happen
 628
 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);
 636         m11 *= det1;
 637         m12 *= det1;
 638         m21 *= det1;
 639         m22 *= det1;
 640         int cnum = 0;  // number of penetrating contact points found
 641         for (j = 0; j < n; j++)
 642         {
 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);
 648                 if (dep[cnum] >= 0)
 649                 {
 650                         ret[cnum * 2] = ret[j * 2];
 651                         ret[cnum * 2 + 1] = ret[j * 2 + 1];
 652                         cnum++;
 653                 }
 654         }
 655         if (cnum < 1) return 0;  // this should never happen
 656
 657         // we can't generate more contacts than we actually have
 658         if (maxc > cnum) maxc = cnum;
 659         if (maxc < 1) maxc = 1;
 660
 661         if (cnum <= maxc)
 662         {
 663                 if (code < 4)
 664                 {
 665                         // we have less contacts than we need, so we use them all
 666                         for (j = 0; j < cnum; j++)
 667                         {
 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]);
 672                         }
 673                 }
 674                 else
 675                 {
 676                         // we have less contacts than we need, so we use them all
 677                         for (j = 0; j < cnum; j++)
 678                         {
 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]);
 684                         }
 685                 }
 686         }
 687         else
 688         {
 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.
 691                 int i1 = 0;
 692                 btScalar maxdepth = dep[0];
 693                 for (i = 1; i < cnum; i++)
 694                 {
 695                         if (dep[i] > maxdepth)
 696                         {
 697                                 maxdepth = dep[i];
 698                                 i1 = i;
 699                         }
 700                 }
 701
 702                 int iret[8];
 703                 cullPoints2(cnum, ret, maxc, i1, iret);
 704
 705                 for (j = 0; j < maxc; j++)
 706                 {
 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]];
 710
 711                         btVector3 posInWorld;
 712                         for (i = 0; i < 3; i++)
 713                                 posInWorld[i] = point[iret[j] * 3 + i] + pa[i];
 714                         if (code < 4)
 715                         {
 716                                 output.addContactPoint(-normal, posInWorld, -dep[iret[j]]);
 717                         }
 718                         else
 719                         {
 720                                 output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
 721                         }
 722                 }
 723                 cnum = maxc;
 724         }
 725
 726         *return_code = code;
 727         return cnum;
 728 }
 729
 730 void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* /*debugDraw*/, bool /*swapResults*/)
 731 {
 732         const btTransform& transformA = input.m_transformA;
 733         const btTransform& transformB = input.m_transformB;
 734
 735         int skip = 0;
 736         dContactGeom* contact = 0;
 737
 738         dMatrix3 R1;
 739         dMatrix3 R2;
 740
 741         for (int j = 0; j < 3; j++)
 742         {
 743                 R1[0 + 4 * j] = transformA.getBasis()[j].x();
 744                 R2[0 + 4 * j] = transformB.getBasis()[j].x();
 745
 746                 R1[1 + 4 * j] = transformA.getBasis()[j].y();
 747                 R2[1 + 4 * j] = transformB.getBasis()[j].y();
 748
 749                 R1[2 + 4 * j] = transformA.getBasis()[j].z();
 750                 R2[2 + 4 * j] = transformB.getBasis()[j].z();
 751         }
 752
 753         btVector3 normal;
 754         btScalar depth;
 755         int return_code;
 756         int maxc = 4;
 757
 758         dBoxBox2(transformA.getOrigin(),
 759                          R1,
 760                          2.f * m_box1->getHalfExtentsWithMargin(),
 761                          transformB.getOrigin(),
 762                          R2,
 763                          2.f * m_box2->getHalfExtentsWithMargin(),
 764                          normal, &depth, &return_code,
 765                          maxc, contact, skip,
 766                          output);
 767 }