1 /*
2 * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
3 * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org
5 Bullet Continuous Collision Detection and Physics Library
6 Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
7
8 This software is provided 'as-is', without any express or implied warranty.
9 In no event will the authors be held liable for any damages arising from the use of this software.
10 Permission is granted to anyone to use this software for any purpose,
11 including commercial applications, and to alter it and redistribute it freely,
12 subject to the following restrictions:
13
14 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
15 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
16 3. This notice may not be removed or altered from any source distribution.
17 */
18
19 ///ODE box-box collision detection is adapted to work with Bullet
20
21 #include "btBoxBoxDetector.h"
22 #include "BulletCollision/CollisionShapes/btBoxShape.h"
23
24 #include <float.h>
25 #include <string.h>
26
btBoxBoxDetector(const btBoxShape * box1,const btBoxShape * box2)27 btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2)
28 : m_box1(box1),
29 m_box2(box2)
30 {
31
32 }
33
34
35 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
36 // generate contact points. this returns 0 if there is no contact otherwise
37 // it returns the number of contacts generated.
38 // `normal' returns the contact normal.
39 // `depth' returns the maximum penetration depth along that normal.
40 // `return_code' returns a number indicating the type of contact that was
41 // detected:
42 // 1,2,3 = box 2 intersects with a face of box 1
43 // 4,5,6 = box 1 intersects with a face of box 2
44 // 7..15 = edge-edge contact
45 // `maxc' is the maximum number of contacts allowed to be generated, i.e.
46 // the size of the `contact' array.
47 // `contact' and `skip' are the contact array information provided to the
48 // collision functions. this function only fills in the position and depth
49 // fields.
50 struct dContactGeom;
51 #define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
52 #define dInfinity FLT_MAX
53
54
55 /*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
56 PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
57 PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
58 PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
59 */
dDOT(const btScalar * a,const btScalar * b)60 static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
dDOT44(const btScalar * a,const btScalar * b)61 static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
dDOT41(const btScalar * a,const btScalar * b)62 static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
dDOT14(const btScalar * a,const btScalar * b)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);
dLineClosestApproach(const btVector3 & pa,const btVector3 & ua,const btVector3 & pb,const btVector3 & ub,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
intersectRectQuad2(btScalar h[2],btScalar p[8],btScalar ret[16])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[]);
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);
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)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
getClosestPoints(const ClosestPointInput & input,Result & output,class btIDebugDraw *,bool)673 void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
674 {
675
676 const btTransform& transformA = input.m_transformA;
677 const btTransform& transformB = input.m_transformB;
678
679 int skip = 0;
680 dContactGeom *contact = 0;
681
682 dMatrix3 R1;
683 dMatrix3 R2;
684
685 for (int j=0;j<3;j++)
686 {
687 R1[0+4*j] = transformA.getBasis()[j].x();
688 R2[0+4*j] = transformB.getBasis()[j].x();
689
690 R1[1+4*j] = transformA.getBasis()[j].y();
691 R2[1+4*j] = transformB.getBasis()[j].y();
692
693
694 R1[2+4*j] = transformA.getBasis()[j].z();
695 R2[2+4*j] = transformB.getBasis()[j].z();
696
697 }
698
699
700
701 btVector3 normal;
702 btScalar depth;
703 int return_code;
704 int maxc = 4;
705
706
707 dBoxBox2 (transformA.getOrigin(),
708 R1,
709 2.f*m_box1->getHalfExtentsWithMargin(),
710 transformB.getOrigin(),
711 R2,
712 2.f*m_box2->getHalfExtentsWithMargin(),
713 normal, &depth, &return_code,
714 maxc, contact, skip,
715 output
716 );
717
718 }
719