Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 15 years ago

Added Bullet physics engine.

  • 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    a = 1.f/(btScalar(3.0)*(a+q));
211    cx = a*(cx + q*(p[n*2-2]+p[0]));
212    cy = a*(cy + q*(p[n*2-1]+p[1]));
213  }
214
215  // compute the angle of each point w.r.t. the centroid
216  btScalar A[8];
217  for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
218
219  // search for points that have angles closest to A[i0] + i*(2*pi/m).
220  int avail[8];
221  for (i=0; i<n; i++) avail[i] = 1;
222  avail[i0] = 0;
223  iret[0] = i0;
224  iret++;
225  for (j=1; j<m; j++) {
226    a = btScalar(j)*(2*M__PI/m) + A[i0];
227    if (a > M__PI) a -= 2*M__PI;
228    btScalar maxdiff=1e9,diff;
229
230    *iret = i0;                 // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
231
232    for (i=0; i<n; i++) {
233      if (avail[i]) {
234        diff = btFabs (A[i]-a);
235        if (diff > M__PI) diff = 2*M__PI - diff;
236        if (diff < maxdiff) {
237          maxdiff = diff;
238          *iret = i;
239        }
240      }
241    }
242#if defined(DEBUG) || defined (_DEBUG)
243    btAssert (*iret != i0);     // ensure iret got set
244#endif
245    avail[*iret] = 0;
246    iret++;
247  }
248}
249
250
251
252int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
253             const btVector3& side1, const btVector3& p2,
254             const dMatrix3 R2, const btVector3& side2,
255             btVector3& normal, btScalar *depth, int *return_code,
256                 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output);
257int 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{
263  const btScalar fudge_factor = btScalar(1.05);
264  btVector3 p,pp,normalC;
265  const btScalar *normalR = 0;
266  btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
267    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
268  int i,j,invert_normal,code;
269
270  // get vector from centers of box 1 to box 2, relative to box 1
271  p = p2 - p1;
272  dMULTIPLY1_331 (pp,R1,p);             // get pp = p relative to body 1
273
274  // get side lengths / 2
275  A[0] = side1[0]*btScalar(0.5);
276  A[1] = side1[1]*btScalar(0.5);
277  A[2] = side1[2]*btScalar(0.5);
278  B[0] = side2[0]*btScalar(0.5);
279  B[1] = side2[1]*btScalar(0.5);
280  B[2] = side2[2]*btScalar(0.5);
281
282  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
283  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
284  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
285  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
286
287  Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
288  Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
289  Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);
290
291  // for all 15 possible separating axes:
292  //   * see if the axis separates the boxes. if so, return 0.
293  //   * find the depth of the penetration along the separating axis (s2)
294  //   * if this is the largest depth so far, record it.
295  // the normal vector will be set to the separating axis with the smallest
296  // depth. note: normalR is set to point to a column of R1 or R2 if that is
297  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
298  // set to a vector relative to body 1. invert_normal is 1 if the sign of
299  // the normal should be flipped.
300
301#define TST(expr1,expr2,norm,cc) \
302  s2 = btFabs(expr1) - (expr2); \
303  if (s2 > 0) return 0; \
304  if (s2 > s) { \
305    s = s2; \
306    normalR = norm; \
307    invert_normal = ((expr1) < 0); \
308    code = (cc); \
309  }
310
311  s = -dInfinity;
312  invert_normal = 0;
313  code = 0;
314
315  // separating axis = u1,u2,u3
316  TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
317  TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
318  TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
319
320  // separating axis = v1,v2,v3
321  TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
322  TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
323  TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
324
325  // note: cross product axes need to be scaled when s is computed.
326  // normal (n1,n2,n3) is relative to box 1.
327#undef TST
328#define TST(expr1,expr2,n1,n2,n3,cc) \
329  s2 = btFabs(expr1) - (expr2); \
330  if (s2 > 0) return 0; \
331  l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
332  if (l > 0) { \
333    s2 /= l; \
334    if (s2*fudge_factor > s) { \
335      s = s2; \
336      normalR = 0; \
337      normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
338      invert_normal = ((expr1) < 0); \
339      code = (cc); \
340    } \
341  }
342
343  // separating axis = u1 x (v1,v2,v3)
344  TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
345  TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
346  TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
347
348  // separating axis = u2 x (v1,v2,v3)
349  TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
350  TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
351  TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
352
353  // separating axis = u3 x (v1,v2,v3)
354  TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
355  TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
356  TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
357
358#undef TST
359
360  if (!code) return 0;
361
362  // if we get to this point, the boxes interpenetrate. compute the normal
363  // in global coordinates.
364  if (normalR) {
365    normal[0] = normalR[0];
366    normal[1] = normalR[4];
367    normal[2] = normalR[8];
368  }
369  else {
370    dMULTIPLY0_331 (normal,R1,normalC);
371  }
372  if (invert_normal) {
373    normal[0] = -normal[0];
374    normal[1] = -normal[1];
375    normal[2] = -normal[2];
376  }
377  *depth = -s;
378
379  // compute contact point(s)
380
381  if (code > 6) {
382    // an edge from box 1 touches an edge from box 2.
383    // find a point pa on the intersecting edge of box 1
384    btVector3 pa;
385    btScalar sign;
386    for (i=0; i<3; i++) pa[i] = p1[i];
387    for (j=0; j<3; j++) {
388      sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
389      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
390    }
391
392    // find a point pb on the intersecting edge of box 2
393    btVector3 pb;
394    for (i=0; i<3; i++) pb[i] = p2[i];
395    for (j=0; j<3; j++) {
396      sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
397      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
398    }
399
400    btScalar alpha,beta;
401    btVector3 ua,ub;
402    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
403    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
404
405    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
406    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
407    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
408
409        {
410               
411                //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
412                //contact[0].depth = *depth;
413                btVector3 pointInWorld;
414
415#ifdef USE_CENTER_POINT
416            for (i=0; i<3; i++) 
417                        pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
418                output.addContactPoint(-normal,pointInWorld,-*depth);
419#else
420                output.addContactPoint(-normal,pb,-*depth);
421#endif //
422                *return_code = code;
423        }
424    return 1;
425  }
426
427  // okay, we have a face-something intersection (because the separating
428  // axis is perpendicular to a face). define face 'a' to be the reference
429  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
430  // the incident face (the closest face of the other box).
431
432  const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
433  if (code <= 3) {
434    Ra = R1;
435    Rb = R2;
436    pa = p1;
437    pb = p2;
438    Sa = A;
439    Sb = B;
440  }
441  else {
442    Ra = R2;
443    Rb = R1;
444    pa = p2;
445    pb = p1;
446    Sa = B;
447    Sb = A;
448  }
449
450  // nr = normal vector of reference face dotted with axes of incident box.
451  // anr = absolute values of nr.
452  btVector3 normal2,nr,anr;
453  if (code <= 3) {
454    normal2[0] = normal[0];
455    normal2[1] = normal[1];
456    normal2[2] = normal[2];
457  }
458  else {
459    normal2[0] = -normal[0];
460    normal2[1] = -normal[1];
461    normal2[2] = -normal[2];
462  }
463  dMULTIPLY1_331 (nr,Rb,normal2);
464  anr[0] = btFabs (nr[0]);
465  anr[1] = btFabs (nr[1]);
466  anr[2] = btFabs (nr[2]);
467
468  // find the largest compontent of anr: this corresponds to the normal
469  // for the indident face. the other axis numbers of the indicent face
470  // are stored in a1,a2.
471  int lanr,a1,a2;
472  if (anr[1] > anr[0]) {
473    if (anr[1] > anr[2]) {
474      a1 = 0;
475      lanr = 1;
476      a2 = 2;
477    }
478    else {
479      a1 = 0;
480      a2 = 1;
481      lanr = 2;
482    }
483  }
484  else {
485    if (anr[0] > anr[2]) {
486      lanr = 0;
487      a1 = 1;
488      a2 = 2;
489    }
490    else {
491      a1 = 0;
492      a2 = 1;
493      lanr = 2;
494    }
495  }
496
497  // compute center point of incident face, in reference-face coordinates
498  btVector3 center;
499  if (nr[lanr] < 0) {
500    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
501  }
502  else {
503    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
504  }
505
506  // find the normal and non-normal axis numbers of the reference box
507  int codeN,code1,code2;
508  if (code <= 3) codeN = code-1; else codeN = code-4;
509  if (codeN==0) {
510    code1 = 1;
511    code2 = 2;
512  }
513  else if (codeN==1) {
514    code1 = 0;
515    code2 = 2;
516  }
517  else {
518    code1 = 0;
519    code2 = 1;
520  }
521
522  // find the four corners of the incident face, in reference-face coordinates
523  btScalar quad[8];     // 2D coordinate of incident face (x,y pairs)
524  btScalar c1,c2,m11,m12,m21,m22;
525  c1 = dDOT14 (center,Ra+code1);
526  c2 = dDOT14 (center,Ra+code2);
527  // optimize this? - we have already computed this data above, but it is not
528  // stored in an easy-to-index format. for now it's quicker just to recompute
529  // the four dot products.
530  m11 = dDOT44 (Ra+code1,Rb+a1);
531  m12 = dDOT44 (Ra+code1,Rb+a2);
532  m21 = dDOT44 (Ra+code2,Rb+a1);
533  m22 = dDOT44 (Ra+code2,Rb+a2);
534  {
535    btScalar k1 = m11*Sb[a1];
536    btScalar k2 = m21*Sb[a1];
537    btScalar k3 = m12*Sb[a2];
538    btScalar k4 = m22*Sb[a2];
539    quad[0] = c1 - k1 - k3;
540    quad[1] = c2 - k2 - k4;
541    quad[2] = c1 - k1 + k3;
542    quad[3] = c2 - k2 + k4;
543    quad[4] = c1 + k1 + k3;
544    quad[5] = c2 + k2 + k4;
545    quad[6] = c1 + k1 - k3;
546    quad[7] = c2 + k2 - k4;
547  }
548
549  // find the size of the reference face
550  btScalar rect[2];
551  rect[0] = Sa[code1];
552  rect[1] = Sa[code2];
553
554  // intersect the incident and reference faces
555  btScalar ret[16];
556  int n = intersectRectQuad2 (rect,quad,ret);
557  if (n < 1) return 0;          // this should never happen
558
559  // convert the intersection points into reference-face coordinates,
560  // and compute the contact position and depth for each point. only keep
561  // those points that have a positive (penetrating) depth. delete points in
562  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
563  btScalar point[3*8];          // penetrating contact points
564  btScalar dep[8];                      // depths for those points
565  btScalar det1 = 1.f/(m11*m22 - m12*m21);
566  m11 *= det1;
567  m12 *= det1;
568  m21 *= det1;
569  m22 *= det1;
570  int cnum = 0;                 // number of penetrating contact points found
571  for (j=0; j < n; j++) {
572    btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
573    btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
574    for (i=0; i<3; i++) point[cnum*3+i] =
575                          center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
576    dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
577    if (dep[cnum] >= 0) {
578      ret[cnum*2] = ret[j*2];
579      ret[cnum*2+1] = ret[j*2+1];
580      cnum++;
581    }
582  }
583  if (cnum < 1) return 0;       // this should never happen
584
585  // we can't generate more contacts than we actually have
586  if (maxc > cnum) maxc = cnum;
587  if (maxc < 1) maxc = 1;
588
589  if (cnum <= maxc) {
590    // we have less contacts than we need, so we use them all
591    for (j=0; j < cnum; j++) {
592
593                //AddContactPoint...
594
595                //dContactGeom *con = CONTACT(contact,skip*j);
596      //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
597      //con->depth = dep[j];
598
599                btVector3 pointInWorld;
600                for (i=0; i<3; i++) 
601                        pointInWorld[i] = point[j*3+i] + pa[i];
602                output.addContactPoint(-normal,pointInWorld,-dep[j]);
603
604    }
605  }
606  else {
607    // we have more contacts than are wanted, some of them must be culled.
608    // find the deepest point, it is always the first contact.
609    int i1 = 0;
610    btScalar maxdepth = dep[0];
611    for (i=1; i<cnum; i++) {
612      if (dep[i] > maxdepth) {
613        maxdepth = dep[i];
614        i1 = i;
615      }
616    }
617
618    int iret[8];
619    cullPoints2 (cnum,ret,maxc,i1,iret);
620
621    for (j=0; j < maxc; j++) {
622//      dContactGeom *con = CONTACT(contact,skip*j);
623  //    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
624    //  con->depth = dep[iret[j]];
625
626                btVector3 posInWorld;
627                for (i=0; i<3; i++) 
628                        posInWorld[i] = point[iret[j]*3+i] + pa[i];
629                output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
630    }
631    cnum = maxc;
632  }
633
634  *return_code = code;
635  return cnum;
636}
637
638void    btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
639{
640       
641        const btTransform& transformA = input.m_transformA;
642        const btTransform& transformB = input.m_transformB;
643       
644        int skip = 0;
645        dContactGeom *contact = 0;
646
647        dMatrix3 R1;
648        dMatrix3 R2;
649
650        for (int j=0;j<3;j++)
651        {
652                R1[0+4*j] = transformA.getBasis()[j].x();
653                R2[0+4*j] = transformB.getBasis()[j].x();
654
655                R1[1+4*j] = transformA.getBasis()[j].y();
656                R2[1+4*j] = transformB.getBasis()[j].y();
657
658
659                R1[2+4*j] = transformA.getBasis()[j].z();
660                R2[2+4*j] = transformB.getBasis()[j].z();
661
662        }
663
664       
665
666        btVector3 normal;
667        btScalar depth;
668        int return_code;
669        int maxc = 4;
670
671
672        dBoxBox2 (transformA.getOrigin(), 
673        R1,
674        2.f*m_box1->getHalfExtentsWithMargin(),
675        transformB.getOrigin(),
676        R2, 
677        2.f*m_box2->getHalfExtentsWithMargin(),
678        normal, &depth, &return_code,
679        maxc, contact, skip,
680        output
681        );
682
683}
Note: See TracBrowser for help on using the repository browser.