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(btBoxShape * box1,btBoxShape * box2)27 btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,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 // separating axis = u1 x (v1,v2,v3)
349 TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
350 TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
351 TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
352
353 // separating axis = u2 x (v1,v2,v3)
354 TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
355 TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
356 TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
357
358 // separating axis = u3 x (v1,v2,v3)
359 TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
360 TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
361 TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
362
363 #undef TST
364
365 if (!code) return 0;
366
367 // if we get to this point, the boxes interpenetrate. compute the normal
368 // in global coordinates.
369 if (normalR) {
370 normal[0] = normalR[0];
371 normal[1] = normalR[4];
372 normal[2] = normalR[8];
373 }
374 else {
375 dMULTIPLY0_331 (normal,R1,normalC);
376 }
377 if (invert_normal) {
378 normal[0] = -normal[0];
379 normal[1] = -normal[1];
380 normal[2] = -normal[2];
381 }
382 *depth = -s;
383
384 // compute contact point(s)
385
386 if (code > 6) {
387 // an edge from box 1 touches an edge from box 2.
388 // find a point pa on the intersecting edge of box 1
389 btVector3 pa;
390 btScalar sign;
391 for (i=0; i<3; i++) pa[i] = p1[i];
392 for (j=0; j<3; j++) {
393 sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
394 for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
395 }
396
397 // find a point pb on the intersecting edge of box 2
398 btVector3 pb;
399 for (i=0; i<3; i++) pb[i] = p2[i];
400 for (j=0; j<3; j++) {
401 sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
402 for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
403 }
404
405 btScalar alpha,beta;
406 btVector3 ua,ub;
407 for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
408 for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
409
410 dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
411 for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
412 for (i=0; i<3; i++) pb[i] += ub[i]*beta;
413
414 {
415
416 //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
417 //contact[0].depth = *depth;
418 btVector3 pointInWorld;
419
420 #ifdef USE_CENTER_POINT
421 for (i=0; i<3; i++)
422 pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
423 output.addContactPoint(-normal,pointInWorld,-*depth);
424 #else
425 output.addContactPoint(-normal,pb,-*depth);
426
427 #endif //
428 *return_code = code;
429 }
430 return 1;
431 }
432
433 // okay, we have a face-something intersection (because the separating
434 // axis is perpendicular to a face). define face 'a' to be the reference
435 // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
436 // the incident face (the closest face of the other box).
437
438 const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
439 if (code <= 3) {
440 Ra = R1;
441 Rb = R2;
442 pa = p1;
443 pb = p2;
444 Sa = A;
445 Sb = B;
446 }
447 else {
448 Ra = R2;
449 Rb = R1;
450 pa = p2;
451 pb = p1;
452 Sa = B;
453 Sb = A;
454 }
455
456 // nr = normal vector of reference face dotted with axes of incident box.
457 // anr = absolute values of nr.
458 btVector3 normal2,nr,anr;
459 if (code <= 3) {
460 normal2[0] = normal[0];
461 normal2[1] = normal[1];
462 normal2[2] = normal[2];
463 }
464 else {
465 normal2[0] = -normal[0];
466 normal2[1] = -normal[1];
467 normal2[2] = -normal[2];
468 }
469 dMULTIPLY1_331 (nr,Rb,normal2);
470 anr[0] = btFabs (nr[0]);
471 anr[1] = btFabs (nr[1]);
472 anr[2] = btFabs (nr[2]);
473
474 // find the largest compontent of anr: this corresponds to the normal
475 // for the indident face. the other axis numbers of the indicent face
476 // are stored in a1,a2.
477 int lanr,a1,a2;
478 if (anr[1] > anr[0]) {
479 if (anr[1] > anr[2]) {
480 a1 = 0;
481 lanr = 1;
482 a2 = 2;
483 }
484 else {
485 a1 = 0;
486 a2 = 1;
487 lanr = 2;
488 }
489 }
490 else {
491 if (anr[0] > anr[2]) {
492 lanr = 0;
493 a1 = 1;
494 a2 = 2;
495 }
496 else {
497 a1 = 0;
498 a2 = 1;
499 lanr = 2;
500 }
501 }
502
503 // compute center point of incident face, in reference-face coordinates
504 btVector3 center;
505 if (nr[lanr] < 0) {
506 for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
507 }
508 else {
509 for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
510 }
511
512 // find the normal and non-normal axis numbers of the reference box
513 int codeN,code1,code2;
514 if (code <= 3) codeN = code-1; else codeN = code-4;
515 if (codeN==0) {
516 code1 = 1;
517 code2 = 2;
518 }
519 else if (codeN==1) {
520 code1 = 0;
521 code2 = 2;
522 }
523 else {
524 code1 = 0;
525 code2 = 1;
526 }
527
528 // find the four corners of the incident face, in reference-face coordinates
529 btScalar quad[8]; // 2D coordinate of incident face (x,y pairs)
530 btScalar c1,c2,m11,m12,m21,m22;
531 c1 = dDOT14 (center,Ra+code1);
532 c2 = dDOT14 (center,Ra+code2);
533 // optimize this? - we have already computed this data above, but it is not
534 // stored in an easy-to-index format. for now it's quicker just to recompute
535 // the four dot products.
536 m11 = dDOT44 (Ra+code1,Rb+a1);
537 m12 = dDOT44 (Ra+code1,Rb+a2);
538 m21 = dDOT44 (Ra+code2,Rb+a1);
539 m22 = dDOT44 (Ra+code2,Rb+a2);
540 {
541 btScalar k1 = m11*Sb[a1];
542 btScalar k2 = m21*Sb[a1];
543 btScalar k3 = m12*Sb[a2];
544 btScalar k4 = m22*Sb[a2];
545 quad[0] = c1 - k1 - k3;
546 quad[1] = c2 - k2 - k4;
547 quad[2] = c1 - k1 + k3;
548 quad[3] = c2 - k2 + k4;
549 quad[4] = c1 + k1 + k3;
550 quad[5] = c2 + k2 + k4;
551 quad[6] = c1 + k1 - k3;
552 quad[7] = c2 + k2 - k4;
553 }
554
555 // find the size of the reference face
556 btScalar rect[2];
557 rect[0] = Sa[code1];
558 rect[1] = Sa[code2];
559
560 // intersect the incident and reference faces
561 btScalar ret[16];
562 int n = intersectRectQuad2 (rect,quad,ret);
563 if (n < 1) return 0; // this should never happen
564
565 // convert the intersection points into reference-face coordinates,
566 // and compute the contact position and depth for each point. only keep
567 // those points that have a positive (penetrating) depth. delete points in
568 // the 'ret' array as necessary so that 'point' and 'ret' correspond.
569 btScalar point[3*8]; // penetrating contact points
570 btScalar dep[8]; // depths for those points
571 btScalar det1 = 1.f/(m11*m22 - m12*m21);
572 m11 *= det1;
573 m12 *= det1;
574 m21 *= det1;
575 m22 *= det1;
576 int cnum = 0; // number of penetrating contact points found
577 for (j=0; j < n; j++) {
578 btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
579 btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
580 for (i=0; i<3; i++) point[cnum*3+i] =
581 center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
582 dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
583 if (dep[cnum] >= 0) {
584 ret[cnum*2] = ret[j*2];
585 ret[cnum*2+1] = ret[j*2+1];
586 cnum++;
587 }
588 }
589 if (cnum < 1) return 0; // this should never happen
590
591 // we can't generate more contacts than we actually have
592 if (maxc > cnum) maxc = cnum;
593 if (maxc < 1) maxc = 1;
594
595 if (cnum <= maxc) {
596
597 if (code<4)
598 {
599 // we have less contacts than we need, so we use them all
600 for (j=0; j < cnum; j++)
601 {
602 btVector3 pointInWorld;
603 for (i=0; i<3; i++)
604 pointInWorld[i] = point[j*3+i] + pa[i];
605 output.addContactPoint(-normal,pointInWorld,-dep[j]);
606
607 }
608 } else
609 {
610 // we have less contacts than we need, so we use them all
611 for (j=0; j < cnum; j++)
612 {
613 btVector3 pointInWorld;
614 for (i=0; i<3; i++)
615 pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
616 //pointInWorld[i] = point[j*3+i] + pa[i];
617 output.addContactPoint(-normal,pointInWorld,-dep[j]);
618 }
619 }
620 }
621 else {
622 // we have more contacts than are wanted, some of them must be culled.
623 // find the deepest point, it is always the first contact.
624 int i1 = 0;
625 btScalar maxdepth = dep[0];
626 for (i=1; i<cnum; i++) {
627 if (dep[i] > maxdepth) {
628 maxdepth = dep[i];
629 i1 = i;
630 }
631 }
632
633 int iret[8];
634 cullPoints2 (cnum,ret,maxc,i1,iret);
635
636 for (j=0; j < maxc; j++) {
637 // dContactGeom *con = CONTACT(contact,skip*j);
638 // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
639 // con->depth = dep[iret[j]];
640
641 btVector3 posInWorld;
642 for (i=0; i<3; i++)
643 posInWorld[i] = point[iret[j]*3+i] + pa[i];
644 if (code<4)
645 {
646 output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
647 } else
648 {
649 output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
650 }
651 }
652 cnum = maxc;
653 }
654
655 *return_code = code;
656 return cnum;
657 }
658
getClosestPoints(const ClosestPointInput & input,Result & output,class btIDebugDraw *,bool)659 void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
660 {
661
662 const btTransform& transformA = input.m_transformA;
663 const btTransform& transformB = input.m_transformB;
664
665 int skip = 0;
666 dContactGeom *contact = 0;
667
668 dMatrix3 R1;
669 dMatrix3 R2;
670
671 for (int j=0;j<3;j++)
672 {
673 R1[0+4*j] = transformA.getBasis()[j].x();
674 R2[0+4*j] = transformB.getBasis()[j].x();
675
676 R1[1+4*j] = transformA.getBasis()[j].y();
677 R2[1+4*j] = transformB.getBasis()[j].y();
678
679
680 R1[2+4*j] = transformA.getBasis()[j].z();
681 R2[2+4*j] = transformB.getBasis()[j].z();
682
683 }
684
685
686
687 btVector3 normal;
688 btScalar depth;
689 int return_code;
690 int maxc = 4;
691
692
693 dBoxBox2 (transformA.getOrigin(),
694 R1,
695 2.f*m_box1->getHalfExtentsWithMargin(),
696 transformB.getOrigin(),
697 R2,
698 2.f*m_box2->getHalfExtentsWithMargin(),
699 normal, &depth, &return_code,
700 maxc, contact, skip,
701 output
702 );
703
704 }
705