Bullet Collision Detection & Physics Library
btBoxBoxDetector.cpp
Go to the documentation of this file.
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 
20 
21 #include "btBoxBoxDetector.h"
23 
24 #include <float.h>
25 #include <string.h>
26 
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,
710  transformB.getOrigin(),
711  R2,
713  normal, &depth, &return_code,
714  maxc, contact, skip,
715  output
716  );
717 
718 }
#define SIMD_EPSILON
Definition: btScalar.h:521
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
static btScalar dDOT44(const btScalar *a, const btScalar *b)
btVector3 getHalfExtentsWithMargin() const
Definition: btBoxShape.h:36
#define btAssert(x)
Definition: btScalar.h:131
#define M__PI
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btBoxBoxDetector(const btBoxShape *box1, const btBoxShape *box2)
ODE box-box collision detection is adapted to work with Bullet.
static btScalar dDOT14(const btScalar *a, const btScalar *b)
static btScalar dDOT41(const btScalar *a, const btScalar *b)
void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
#define output
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:496
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
int dBoxBox2(const btVector3 &p1, const dMatrix3 R1, const btVector3 &side1, const btVector3 &p2, const dMatrix3 R2, const btVector3 &side2, btVector3 &normal, btScalar *depth, int *return_code, int maxc, dContactGeom *, int, btDiscreteCollisionDetectorInterface::Result &output)
#define dMULTIPLY0_331(A, B, C)
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:26
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
#define dMULTIPLY1_331(A, B, C)
void dLineClosestApproach(const btVector3 &pa, const btVector3 &ua, const btVector3 &pb, const btVector3 &ub, btScalar *alpha, btScalar *beta)
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)=0
#define dInfinity
const btBoxShape * m_box2
#define TST(expr1, expr2, norm, cc)
const btBoxShape * m_box1
#define dDOTpq(a, b, p, q)
static btScalar dDOT(const btScalar *a, const btScalar *b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
btScalar dMatrix3[4 *3]