Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/src/external/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @ 12177

Last change on this file since 12177 was 12177, checked in by siramesh, 5 years ago

Super Orxo Bros Final (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 21.0 KB
Line 
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
8This software is provided 'as-is', without any express or implied warranty.
9In no event will the authors be held liable for any damages arising from the use of this software.
10Permission is granted to anyone to use this software for any purpose,
11including commercial applications, and to alter it and redistribute it freely,
12subject to the following restrictions:
13
141. 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.
152. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
163. 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
27btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2)
28: m_box1(box1),
29m_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.
50struct 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); }
56PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
57PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
58PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
59*/
60static btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
61static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
62static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
63static 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
81typedef btScalar dMatrix3[4*3];
82
83void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
84                           const btVector3& pb, const btVector3& ub,
85                           btScalar *alpha, btScalar *beta);
86void 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
120static 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
184void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]);
185void 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
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);
262int 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
673void    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}
Note: See TracBrowser for help on using the repository browser.