Back to index

supertuxkart  0.5+dfsg1
btBoxBoxDetector.cpp
Go to the documentation of this file.
00001 
00002 /*
00003  * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
00004  * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
00005  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
00006  Bullet Continuous Collision Detection and Physics Library
00007  Bullet is Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00008 
00009 This software is provided 'as-is', without any express or implied warranty.
00010 In no event will the authors be held liable for any damages arising from the use of this software.
00011 Permission is granted to anyone to use this software for any purpose, 
00012 including commercial applications, and to alter it and redistribute it freely, 
00013 subject to the following restrictions:
00014 
00015 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.
00016 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00017 3. This notice may not be removed or altered from any source distribution.
00018 */
00019 
00021 
00022 #include "btBoxBoxDetector.h"
00023 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00024 
00025 #include <float.h>
00026 #include <string.h>
00027 
00028 btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2)
00029 : m_box1(box1),
00030 m_box2(box2)
00031 {
00032 
00033 }
00034 
00035 
00036 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
00037 // generate contact points. this returns 0 if there is no contact otherwise
00038 // it returns the number of contacts generated.
00039 // `normal' returns the contact normal.
00040 // `depth' returns the maximum penetration depth along that normal.
00041 // `return_code' returns a number indicating the type of contact that was
00042 // detected:
00043 //        1,2,3 = box 2 intersects with a face of box 1
00044 //        4,5,6 = box 1 intersects with a face of box 2
00045 //        7..15 = edge-edge contact
00046 // `maxc' is the maximum number of contacts allowed to be generated, i.e.
00047 // the size of the `contact' array.
00048 // `contact' and `skip' are the contact array information provided to the
00049 // collision functions. this function only fills in the position and depth
00050 // fields.
00051 struct dContactGeom;
00052 #define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
00053 #define dInfinity FLT_MAX
00054 
00055 
00056 /*PURE_INLINE btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
00057 PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
00058 PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
00059 PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
00060 */
00061 static btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
00062 static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
00063 static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
00064 static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
00065 #define dMULTIPLYOP1_331(A,op,B,C) \
00066 do { \
00067   (A)[0] op dDOT41((B),(C)); \
00068   (A)[1] op dDOT41((B+1),(C)); \
00069   (A)[2] op dDOT41((B+2),(C)); \
00070 } while(0)
00071 #define dMULTIPLYOP0_331(A,op,B,C) \
00072 do { \
00073   (A)[0] op dDOT((B),(C)); \
00074   (A)[1] op dDOT((B+4),(C)); \
00075   (A)[2] op dDOT((B+8),(C)); \
00076 } while(0)
00077 
00078 #define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
00079 #define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
00080 
00081 typedef btScalar dMatrix3[4*3];
00082 
00083 void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
00084                         const btVector3& pb, const btVector3& ub,
00085                         btScalar *alpha, btScalar *beta)
00086 {
00087   btVector3 p;
00088   p[0] = pb[0] - pa[0];
00089   p[1] = pb[1] - pa[1];
00090   p[2] = pb[2] - pa[2];
00091   btScalar uaub = dDOT(ua,ub);
00092   btScalar q1 =  dDOT(ua,p);
00093   btScalar q2 = -dDOT(ub,p);
00094   btScalar d = 1-uaub*uaub;
00095   if (d <= btScalar(0.0001f)) {
00096     // @@@ this needs to be made more robust
00097     *alpha = 0;
00098     *beta  = 0;
00099   }
00100   else {
00101     d = 1.f/d;
00102     *alpha = (q1 + uaub*q2)*d;
00103     *beta  = (uaub*q1 + q2)*d;
00104   }
00105 }
00106 
00107 
00108 
00109 // find all the intersection points between the 2D rectangle with vertices
00110 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
00111 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
00112 //
00113 // the intersection points are returned as x,y pairs in the 'ret' array.
00114 // the number of intersection points is returned by the function (this will
00115 // be in the range 0 to 8).
00116 
00117 static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
00118 {
00119   // q (and r) contain nq (and nr) coordinate points for the current (and
00120   // chopped) polygons
00121   int nq=4,nr;
00122   btScalar buffer[16];
00123   btScalar *q = p;
00124   btScalar *r = ret;
00125   for (int dir=0; dir <= 1; dir++) {
00126     // direction notation: xy[0] = x axis, xy[1] = y axis
00127     for (int sign=-1; sign <= 1; sign += 2) {
00128       // chop q along the line xy[dir] = sign*h[dir]
00129       btScalar *pq = q;
00130       btScalar *pr = r;
00131       nr = 0;
00132       for (int i=nq; i > 0; i--) {
00133        // go through all points in q and all lines between adjacent points
00134        if (sign*pq[dir] < h[dir]) {
00135          // this point is inside the chopping line
00136          pr[0] = pq[0];
00137          pr[1] = pq[1];
00138          pr += 2;
00139          nr++;
00140          if (nr & 8) {
00141            q = r;
00142            goto done;
00143          }
00144        }
00145        btScalar *nextq = (i > 1) ? pq+2 : q;
00146        if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
00147          // this line crosses the chopping line
00148          pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
00149            (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
00150          pr[dir] = sign*h[dir];
00151          pr += 2;
00152          nr++;
00153          if (nr & 8) {
00154            q = r;
00155            goto done;
00156          }
00157        }
00158        pq += 2;
00159       }
00160       q = r;
00161       r = (q==ret) ? buffer : ret;
00162       nq = nr;
00163     }
00164   }
00165  done:
00166   if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
00167   return nr;
00168 }
00169 
00170 #define dAtan2(y,x) ((float)atan2f((y),(x)))     /* arc tangent with 2 args */
00171 #define M__PI 3.14159265f
00172 
00173 // given n points in the plane (array p, of size 2*n), generate m points that
00174 // best represent the whole set. the definition of 'best' here is not
00175 // predetermined - the idea is to select points that give good box-box
00176 // collision detection behavior. the chosen point indexes are returned in the
00177 // array iret (of size m). 'i0' is always the first entry in the array.
00178 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
00179 // in the range [0..n-1].
00180 
00181 void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
00182 {
00183   // compute the centroid of the polygon in cx,cy
00184   int i,j;
00185   btScalar a,cx,cy,q;
00186   if (n==1) {
00187     cx = p[0];
00188     cy = p[1];
00189   }
00190   else if (n==2) {
00191     cx = btScalar(0.5)*(p[0] + p[2]);
00192     cy = btScalar(0.5)*(p[1] + p[3]);
00193   }
00194   else {
00195     a = 0;
00196     cx = 0;
00197     cy = 0;
00198     for (i=0; i<(n-1); i++) {
00199       q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
00200       a += q;
00201       cx += q*(p[i*2]+p[i*2+2]);
00202       cy += q*(p[i*2+1]+p[i*2+3]);
00203     }
00204     q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
00205     a = 1.f/(btScalar(3.0)*(a+q));
00206     cx = a*(cx + q*(p[n*2-2]+p[0]));
00207     cy = a*(cy + q*(p[n*2-1]+p[1]));
00208   }
00209 
00210   // compute the angle of each point w.r.t. the centroid
00211   btScalar A[8];
00212   for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx);
00213 
00214   // search for points that have angles closest to A[i0] + i*(2*pi/m).
00215   int avail[8];
00216   for (i=0; i<n; i++) avail[i] = 1;
00217   avail[i0] = 0;
00218   iret[0] = i0;
00219   iret++;
00220   for (j=1; j<m; j++) {
00221     a = btScalar(j)*(2*M__PI/m) + A[i0];
00222     if (a > M__PI) a -= 2*M__PI;
00223     btScalar maxdiff=1e9,diff;
00224 #ifndef dNODEBUG
00225     *iret = i0;                    // iret is not allowed to keep this value
00226 #endif
00227     for (i=0; i<n; i++) {
00228       if (avail[i]) {
00229        diff = fabsf (A[i]-a);
00230        if (diff > M__PI) diff = 2*M__PI - diff;
00231        if (diff < maxdiff) {
00232          maxdiff = diff;
00233          *iret = i;
00234        }
00235       }
00236     }
00237 #ifndef dNODEBUG
00238     btAssert (*iret != i0); // ensure iret got set
00239 #endif
00240     avail[*iret] = 0;
00241     iret++;
00242   }
00243 }
00244 
00245 
00246 
00247 
00248 int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
00249             const btVector3& side1, const btVector3& p2,
00250             const dMatrix3 R2, const btVector3& side2,
00251             btVector3& normal, btScalar *depth, int *return_code,
00252                int maxc, dContactGeom *contact, int skip,btDiscreteCollisionDetectorInterface::Result& output)
00253 {
00254   const btScalar fudge_factor = btScalar(1.05);
00255   btVector3 p,pp,normalC;
00256   const btScalar *normalR = 0;
00257   btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
00258     Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
00259   int i,j,invert_normal,code;
00260 
00261   // get vector from centers of box 1 to box 2, relative to box 1
00262   p = p2 - p1;
00263   dMULTIPLY1_331 (pp,R1,p);        // get pp = p relative to body 1
00264 
00265   // get side lengths / 2
00266   A[0] = side1[0]*btScalar(0.5);
00267   A[1] = side1[1]*btScalar(0.5);
00268   A[2] = side1[2]*btScalar(0.5);
00269   B[0] = side2[0]*btScalar(0.5);
00270   B[1] = side2[1]*btScalar(0.5);
00271   B[2] = side2[2]*btScalar(0.5);
00272 
00273   // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
00274   R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
00275   R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
00276   R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
00277 
00278   Q11 = fabsf(R11); Q12 = fabsf(R12); Q13 = fabsf(R13);
00279   Q21 = fabsf(R21); Q22 = fabsf(R22); Q23 = fabsf(R23);
00280   Q31 = fabsf(R31); Q32 = fabsf(R32); Q33 = fabsf(R33);
00281 
00282   // for all 15 possible separating axes:
00283   //   * see if the axis separates the boxes. if so, return 0.
00284   //   * find the depth of the penetration along the separating axis (s2)
00285   //   * if this is the largest depth so far, record it.
00286   // the normal vector will be set to the separating axis with the smallest
00287   // depth. note: normalR is set to point to a column of R1 or R2 if that is
00288   // the smallest depth normal so far. otherwise normalR is 0 and normalC is
00289   // set to a vector relative to body 1. invert_normal is 1 if the sign of
00290   // the normal should be flipped.
00291 
00292 #define TST(expr1,expr2,norm,cc) \
00293   s2 = fabsf(expr1) - (expr2); \
00294   if (s2 > 0) return 0; \
00295   if (s2 > s) { \
00296     s = s2; \
00297     normalR = norm; \
00298     invert_normal = ((expr1) < 0); \
00299     code = (cc); \
00300   }
00301 
00302   s = -dInfinity;
00303   invert_normal = 0;
00304   code = 0;
00305 
00306   // separating axis = u1,u2,u3
00307   TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
00308   TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
00309   TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
00310 
00311   // separating axis = v1,v2,v3
00312   TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
00313   TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
00314   TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
00315 
00316   // note: cross product axes need to be scaled when s is computed.
00317   // normal (n1,n2,n3) is relative to box 1.
00318 #undef TST
00319 #define TST(expr1,expr2,n1,n2,n3,cc) \
00320   s2 = fabsf(expr1) - (expr2); \
00321   if (s2 > 0) return 0; \
00322   l = sqrtf((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
00323   if (l > 0) { \
00324     s2 /= l; \
00325     if (s2*fudge_factor > s) { \
00326       s = s2; \
00327       normalR = 0; \
00328       normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
00329       invert_normal = ((expr1) < 0); \
00330       code = (cc); \
00331     } \
00332   }
00333 
00334   // separating axis = u1 x (v1,v2,v3)
00335   TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
00336   TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
00337   TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
00338 
00339   // separating axis = u2 x (v1,v2,v3)
00340   TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
00341   TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
00342   TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
00343 
00344   // separating axis = u3 x (v1,v2,v3)
00345   TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
00346   TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
00347   TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
00348 
00349 #undef TST
00350 
00351   if (!code) return 0;
00352 
00353   // if we get to this point, the boxes interpenetrate. compute the normal
00354   // in global coordinates.
00355   if (normalR) {
00356     normal[0] = normalR[0];
00357     normal[1] = normalR[4];
00358     normal[2] = normalR[8];
00359   }
00360   else {
00361     dMULTIPLY0_331 (normal,R1,normalC);
00362   }
00363   if (invert_normal) {
00364     normal[0] = -normal[0];
00365     normal[1] = -normal[1];
00366     normal[2] = -normal[2];
00367   }
00368   *depth = -s;
00369 
00370   // compute contact point(s)
00371 
00372   if (code > 6) {
00373     // an edge from box 1 touches an edge from box 2.
00374     // find a point pa on the intersecting edge of box 1
00375     btVector3 pa;
00376     btScalar sign;
00377     for (i=0; i<3; i++) pa[i] = p1[i];
00378     for (j=0; j<3; j++) {
00379       sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
00380       for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
00381     }
00382 
00383     // find a point pb on the intersecting edge of box 2
00384     btVector3 pb;
00385     for (i=0; i<3; i++) pb[i] = p2[i];
00386     for (j=0; j<3; j++) {
00387       sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
00388       for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
00389     }
00390 
00391     btScalar alpha,beta;
00392     btVector3 ua,ub;
00393     for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
00394     for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
00395 
00396     dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
00397     for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
00398     for (i=0; i<3; i++) pb[i] += ub[i]*beta;
00399 
00400        {
00401               
00402               //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
00403               //contact[0].depth = *depth;
00404               btVector3 pointInWorld;
00405 
00406 #ifdef USE_CENTER_POINT
00407            for (i=0; i<3; i++) 
00408                      pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
00409               output.addContactPoint(-normal,pointInWorld,-*depth);
00410 #else
00411               output.addContactPoint(-normal,pb,-*depth);
00412 #endif //
00413               *return_code = code;
00414        }
00415     return 1;
00416   }
00417 
00418   // okay, we have a face-something intersection (because the separating
00419   // axis is perpendicular to a face). define face 'a' to be the reference
00420   // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
00421   // the incident face (the closest face of the other box).
00422 
00423   const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
00424   if (code <= 3) {
00425     Ra = R1;
00426     Rb = R2;
00427     pa = p1;
00428     pb = p2;
00429     Sa = A;
00430     Sb = B;
00431   }
00432   else {
00433     Ra = R2;
00434     Rb = R1;
00435     pa = p2;
00436     pb = p1;
00437     Sa = B;
00438     Sb = A;
00439   }
00440 
00441   // nr = normal vector of reference face dotted with axes of incident box.
00442   // anr = absolute values of nr.
00443   btVector3 normal2,nr,anr;
00444   if (code <= 3) {
00445     normal2[0] = normal[0];
00446     normal2[1] = normal[1];
00447     normal2[2] = normal[2];
00448   }
00449   else {
00450     normal2[0] = -normal[0];
00451     normal2[1] = -normal[1];
00452     normal2[2] = -normal[2];
00453   }
00454   dMULTIPLY1_331 (nr,Rb,normal2);
00455   anr[0] = fabsf (nr[0]);
00456   anr[1] = fabsf (nr[1]);
00457   anr[2] = fabsf (nr[2]);
00458 
00459   // find the largest compontent of anr: this corresponds to the normal
00460   // for the indident face. the other axis numbers of the indicent face
00461   // are stored in a1,a2.
00462   int lanr,a1,a2;
00463   if (anr[1] > anr[0]) {
00464     if (anr[1] > anr[2]) {
00465       a1 = 0;
00466       lanr = 1;
00467       a2 = 2;
00468     }
00469     else {
00470       a1 = 0;
00471       a2 = 1;
00472       lanr = 2;
00473     }
00474   }
00475   else {
00476     if (anr[0] > anr[2]) {
00477       lanr = 0;
00478       a1 = 1;
00479       a2 = 2;
00480     }
00481     else {
00482       a1 = 0;
00483       a2 = 1;
00484       lanr = 2;
00485     }
00486   }
00487 
00488   // compute center point of incident face, in reference-face coordinates
00489   btVector3 center;
00490   if (nr[lanr] < 0) {
00491     for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
00492   }
00493   else {
00494     for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
00495   }
00496 
00497   // find the normal and non-normal axis numbers of the reference box
00498   int codeN,code1,code2;
00499   if (code <= 3) codeN = code-1; else codeN = code-4;
00500   if (codeN==0) {
00501     code1 = 1;
00502     code2 = 2;
00503   }
00504   else if (codeN==1) {
00505     code1 = 0;
00506     code2 = 2;
00507   }
00508   else {
00509     code1 = 0;
00510     code2 = 1;
00511   }
00512 
00513   // find the four corners of the incident face, in reference-face coordinates
00514   btScalar quad[8];  // 2D coordinate of incident face (x,y pairs)
00515   btScalar c1,c2,m11,m12,m21,m22;
00516   c1 = dDOT14 (center,Ra+code1);
00517   c2 = dDOT14 (center,Ra+code2);
00518   // optimize this? - we have already computed this data above, but it is not
00519   // stored in an easy-to-index format. for now it's quicker just to recompute
00520   // the four dot products.
00521   m11 = dDOT44 (Ra+code1,Rb+a1);
00522   m12 = dDOT44 (Ra+code1,Rb+a2);
00523   m21 = dDOT44 (Ra+code2,Rb+a1);
00524   m22 = dDOT44 (Ra+code2,Rb+a2);
00525   {
00526     btScalar k1 = m11*Sb[a1];
00527     btScalar k2 = m21*Sb[a1];
00528     btScalar k3 = m12*Sb[a2];
00529     btScalar k4 = m22*Sb[a2];
00530     quad[0] = c1 - k1 - k3;
00531     quad[1] = c2 - k2 - k4;
00532     quad[2] = c1 - k1 + k3;
00533     quad[3] = c2 - k2 + k4;
00534     quad[4] = c1 + k1 + k3;
00535     quad[5] = c2 + k2 + k4;
00536     quad[6] = c1 + k1 - k3;
00537     quad[7] = c2 + k2 - k4;
00538   }
00539 
00540   // find the size of the reference face
00541   btScalar rect[2];
00542   rect[0] = Sa[code1];
00543   rect[1] = Sa[code2];
00544 
00545   // intersect the incident and reference faces
00546   btScalar ret[16];
00547   int n = intersectRectQuad2 (rect,quad,ret);
00548   if (n < 1) return 0;             // this should never happen
00549 
00550   // convert the intersection points into reference-face coordinates,
00551   // and compute the contact position and depth for each point. only keep
00552   // those points that have a positive (penetrating) depth. delete points in
00553   // the 'ret' array as necessary so that 'point' and 'ret' correspond.
00554   btScalar point[3*8];             // penetrating contact points
00555   btScalar dep[8];                 // depths for those points
00556   btScalar det1 = 1.f/(m11*m22 - m12*m21);
00557   m11 *= det1;
00558   m12 *= det1;
00559   m21 *= det1;
00560   m22 *= det1;
00561   int cnum = 0;                    // number of penetrating contact points found
00562   for (j=0; j < n; j++) {
00563     btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
00564     btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
00565     for (i=0; i<3; i++) point[cnum*3+i] =
00566                        center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
00567     dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
00568     if (dep[cnum] >= 0) {
00569       ret[cnum*2] = ret[j*2];
00570       ret[cnum*2+1] = ret[j*2+1];
00571       cnum++;
00572     }
00573   }
00574   if (cnum < 1) return 0;   // this should never happen
00575 
00576   // we can't generate more contacts than we actually have
00577   if (maxc > cnum) maxc = cnum;
00578   if (maxc < 1) maxc = 1;
00579 
00580   if (cnum <= maxc) {
00581     // we have less contacts than we need, so we use them all
00582     for (j=0; j < cnum; j++) {
00583 
00584               //AddContactPoint...
00585 
00586               //dContactGeom *con = CONTACT(contact,skip*j);
00587       //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
00588       //con->depth = dep[j];
00589 
00590               btVector3 pointInWorld;
00591               for (i=0; i<3; i++) 
00592                      pointInWorld[i] = point[j*3+i] + pa[i];
00593               output.addContactPoint(-normal,pointInWorld,-dep[j]);
00594 
00595     }
00596   }
00597   else {
00598     // we have more contacts than are wanted, some of them must be culled.
00599     // find the deepest point, it is always the first contact.
00600     int i1 = 0;
00601     btScalar maxdepth = dep[0];
00602     for (i=1; i<cnum; i++) {
00603       if (dep[i] > maxdepth) {
00604        maxdepth = dep[i];
00605        i1 = i;
00606       }
00607     }
00608 
00609     int iret[8];
00610     cullPoints2 (cnum,ret,maxc,i1,iret);
00611 
00612     for (j=0; j < maxc; j++) {
00613 //      dContactGeom *con = CONTACT(contact,skip*j);
00614   //    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
00615     //  con->depth = dep[iret[j]];
00616 
00617               btVector3 posInWorld;
00618               for (i=0; i<3; i++) 
00619                      posInWorld[i] = point[iret[j]*3+i] + pa[i];
00620               output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
00621     }
00622     cnum = maxc;
00623   }
00624 
00625   *return_code = code;
00626   return cnum;
00627 }
00628 
00629 void   btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
00630 {
00631        
00632        const btTransform& transformA = input.m_transformA;
00633        const btTransform& transformB = input.m_transformB;
00634        
00635        int skip = 0;
00636        dContactGeom *contact = 0;
00637 
00638        dMatrix3 R1;
00639        dMatrix3 R2;
00640 
00641        for (int j=0;j<3;j++)
00642        {
00643               R1[0+4*j] = transformA.getBasis()[j].x();
00644               R2[0+4*j] = transformB.getBasis()[j].x();
00645 
00646               R1[1+4*j] = transformA.getBasis()[j].y();
00647               R2[1+4*j] = transformB.getBasis()[j].y();
00648 
00649 
00650               R1[2+4*j] = transformA.getBasis()[j].z();
00651               R2[2+4*j] = transformB.getBasis()[j].z();
00652 
00653        }
00654 
00655        
00656 
00657        btVector3 normal;
00658        btScalar depth;
00659        int return_code;
00660        int maxc = 4;
00661 
00662 
00663        dBoxBox2 (transformA.getOrigin(), 
00664        R1,
00665        2.f*m_box1->getHalfExtentsWithMargin(),
00666        transformB.getOrigin(),
00667        R2, 
00668        2.f*m_box2->getHalfExtentsWithMargin(),
00669        normal, &depth, &return_code,
00670        maxc, contact, skip,
00671        output
00672        );
00673 
00674 }