a7c8cf140ce4e97e3b614484c68f59bb62557d96
[blender.git] / extern / bullet2 / 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  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(btBoxShape* box1,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 }