Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/netp6/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @ 5819

Last change on this file since 5819 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.

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