Bullet Collision Detection & Physics Library: btBoxBoxDetector.cpp Source File

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"
22 #include "BulletCollision/CollisionShapes/btBoxShape.h"
23
24 #include <float.h>
25 #include <string.h>
26
27 btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2)
28 : m_box1(box1),
29 m_box2(box2)
30 {
31 }
32
33 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
34 // generate contact points. this returns 0 if there is no contact otherwise
35 // it returns the number of contacts generated.
36 // `normal' returns the contact normal.
37 // `depth' returns the maximum penetration depth along that normal.
38 // `return_code' returns a number indicating the type of contact that was
39 // detected:
40 // 1,2,3 = box 2 intersects with a face of box 1
41 // 4,5,6 = box 1 intersects with a face of box 2
42 // 7..15 = edge-edge contact
43 // `maxc' is the maximum number of contacts allowed to be generated, i.e.
44 // the size of the `contact' array.
45 // `contact' and `skip' are the contact array information provided to the
46 // collision functions. this function only fills in the position and depth
47 // fields.
48 struct dContactGeom;
49 #define dDOTpq(a, b, p, q) ((a)[0] * (b)[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)])
50 #define dInfinity FLT_MAX
51
52 /*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
53 PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
54 PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
55 PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
56 */
57 static btScalar dDOT(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 1); }
58 static btScalar dDOT44(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 4); }
59 static btScalar dDOT41(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 1); }
60 static btScalar dDOT14(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 4); }
61 #define dMULTIPLYOP1_331(A, op, B, C) \
62 { \
63 (A)[0] op dDOT41((B), (C)); \
64 (A)[1] op dDOT41((B + 1), (C)); \
65 (A)[2] op dDOT41((B + 2), (C)); \
66 }
67
68 #define dMULTIPLYOP0_331(A, op, B, C) \
69 { \
70 (A)[0] op dDOT((B), (C)); \
71 (A)[1] op dDOT((B + 4), (C)); \
72 (A)[2] op dDOT((B + 8), (C)); \
73 }
74
75 #define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C)
76 #define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C)
77
78 typedef btScalar dMatrix3[4 * 3];
79
80 void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
81 const btVector3& pb, const btVector3& ub,
82 btScalar* alpha, btScalar* beta);
83 void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
84 const btVector3& pb, const btVector3& ub,
85 btScalar* alpha, btScalar* beta)
86 {
87 btVector3 p;
88 p[0] = pb[0] - pa[0];
89 p[1] = pb[1] - pa[1];
90 p[2] = pb[2] - pa[2];
91 btScalar uaub = dDOT(ua, ub);
92 btScalar q1 = dDOT(ua, p);
93 btScalar q2 = -dDOT(ub, p);
94 btScalar d = 1 - uaub * uaub;
95 if (d <= btScalar(0.0001f))
96 {
97 // @@@ this needs to be made more robust
98 *alpha = 0;
99 *beta = 0;
100 }
101 else
102 {
103 d = 1.f / d;
104 *alpha = (q1 + uaub * q2) * d;
105 *beta = (uaub * q1 + q2) * d;
106 }
107 }
108
109 // find all the intersection points between the 2D rectangle with vertices
110 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
111 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
112 //
113 // the intersection points are returned as x,y pairs in the 'ret' array.
114 // the number of intersection points is returned by the function (this will
115 // be in the range 0 to 8).
116
117 static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
118 {
119 // q (and r) contain nq (and nr) coordinate points for the current (and
120 // chopped) polygons
121 int nq = 4, nr = 0;
122 btScalar buffer[16];
123 btScalar* q = p;
124 btScalar* r = ret;
125 for (int dir = 0; dir <= 1; dir++)
126 {
127 // direction notation: xy[0] = x axis, xy[1] = y axis
128 for (int sign = -1; sign <= 1; sign += 2)
129 {
130 // chop q along the line xy[dir] = sign*h[dir]
131 btScalar* pq = q;
132 btScalar* pr = r;
133 nr = 0;
134 for (int i = nq; i > 0; i--)
135 {
136 // go through all points in q and all lines between adjacent points
137 if (sign * pq[dir] < h[dir])
138 {
139 // this point is inside the chopping line
140 pr[0] = pq[0];
141 pr[1] = pq[1];
142 pr += 2;
143 nr++;
144 if (nr & 8)
145 {
146 q = r;
147 goto done;
148 }
149 }
150 btScalar* nextq = (i > 1) ? pq + 2 : q;
151 if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir]))
152 {
153 // this line crosses the chopping line
154 pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) /
155 (nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]);
156 pr[dir] = sign * h[dir];
157 pr += 2;
158 nr++;
159 if (nr & 8)
160 {
161 q = r;
162 goto done;
163 }
164 }
165 pq += 2;
166 }
167 q = r;
168 r = (q == ret) ? buffer : ret;
169 nq = nr;
170 }
171 }
172 done:
173 if (q != ret) memcpy(ret, q, nr * 2 * sizeof(btScalar));
174 return nr;
175 }
176
177 #define M__PI 3.14159265f
178
179 // given n points in the plane (array p, of size 2*n), generate m points that
180 // best represent the whole set. the definition of 'best' here is not
181 // predetermined - the idea is to select points that give good box-box
182 // collision detection behavior. the chosen point indexes are returned in the
183 // array iret (of size m). 'i0' is always the first entry in the array.
184 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
185 // in the range [0..n-1].
186
187 void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]);
188 void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
189 {
190 // compute the centroid of the polygon in cx,cy
191 int i, j;
192 btScalar a, cx, cy, q;
193 if (n == 1)
194 {
195 cx = p[0];
196 cy = p[1];
197 }
198 else if (n == 2)
199 {
200 cx = btScalar(0.5) * (p[0] + p[2]);
201 cy = btScalar(0.5) * (p[1] + p[3]);
202 }
203 else
204 {
205 a = 0;
206 cx = 0;
207 cy = 0;
208 for (i = 0; i < (n - 1); i++)
209 {
210 q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
211 a += q;
212 cx += q * (p[i * 2] + p[i * 2 + 2]);
213 cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
214 }
215 q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
216 if (btFabs(a + q) > SIMD_EPSILON)
217 {
218 a = 1.f / (btScalar(3.0) * (a + q));
219 }
220 else
221 {
222 a = BT_LARGE_FLOAT;
223 }
224 cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
225 cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
226 }
227
228 // compute the angle of each point w.r.t. the centroid
229 btScalar A[8];
230 for (i = 0; i < n; i++) A[i] = btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
231
232 // search for points that have angles closest to A[i0] + i*(2*pi/m).
233 int avail[8];
234 for (i = 0; i < n; i++) avail[i] = 1;
235 avail[i0] = 0;
236 iret[0] = i0;
237 iret++;
238 for (j = 1; j < m; j++)
239 {
240 a = btScalar(j) * (2 * M__PI / m) + A[i0];
241 if (a > M__PI) a -= 2 * M__PI;
242 btScalar maxdiff = 1e9, diff;
243
244 *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
245
246 for (i = 0; i < n; i++)
247 {
248 if (avail[i])
249 {
250 diff = btFabs(A[i] - a);
251 if (diff > M__PI) diff = 2 * M__PI - diff;
252 if (diff < maxdiff)
253 {
254 maxdiff = diff;
255 *iret = i;
256 }
257 }
258 }
259 #if defined(DEBUG) || defined(_DEBUG)
260 btAssert(*iret != i0); // ensure iret got set
261 #endif
262 avail[*iret] = 0;
263 iret++;
264 }
265 }
266
267 int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
268 const btVector3& side1, const btVector3& p2,
269 const dMatrix3 R2, const btVector3& side2,
270 btVector3& normal, btScalar* depth, int* return_code,
271 int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output);
272 int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
273 const btVector3& side1, const btVector3& p2,
274 const dMatrix3 R2, const btVector3& side2,
275 btVector3& normal, btScalar* depth, int* return_code,
276 int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output)
277 {
278 const btScalar fudge_factor = btScalar(1.05);
279 btVector3 p, pp, normalC(0.f, 0.f, 0.f);
280 const btScalar* normalR = 0;
281 btScalar A[3], B[3], R11, R12, R13, R21, R22, R23, R31, R32, R33,
282 Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l;
283 int i, j, invert_normal, code;
284
285 // get vector from centers of box 1 to box 2, relative to box 1
286 p = p2 - p1;
287 dMULTIPLY1_331(pp, R1, p); // get pp = p relative to body 1
288
289 // get side lengths / 2
290 A[0] = side1[0] * btScalar(0.5);
291 A[1] = side1[1] * btScalar(0.5);
292 A[2] = side1[2] * btScalar(0.5);
293 B[0] = side2[0] * btScalar(0.5);
294 B[1] = side2[1] * btScalar(0.5);
295 B[2] = side2[2] * btScalar(0.5);
296
297 // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
298 R11 = dDOT44(R1 + 0, R2 + 0);
299 R12 = dDOT44(R1 + 0, R2 + 1);
300 R13 = dDOT44(R1 + 0, R2 + 2);
301 R21 = dDOT44(R1 + 1, R2 + 0);
302 R22 = dDOT44(R1 + 1, R2 + 1);
303 R23 = dDOT44(R1 + 1, R2 + 2);
304 R31 = dDOT44(R1 + 2, R2 + 0);
305 R32 = dDOT44(R1 + 2, R2 + 1);
306 R33 = dDOT44(R1 + 2, R2 + 2);
307
308 Q11 = btFabs(R11);
309 Q12 = btFabs(R12);
310 Q13 = btFabs(R13);
311 Q21 = btFabs(R21);
312 Q22 = btFabs(R22);
313 Q23 = btFabs(R23);
314 Q31 = btFabs(R31);
315 Q32 = btFabs(R32);
316 Q33 = btFabs(R33);
317
318 // for all 15 possible separating axes:
319 // * see if the axis separates the boxes. if so, return 0.
320 // * find the depth of the penetration along the separating axis (s2)
321 // * if this is the largest depth so far, record it.
322 // the normal vector will be set to the separating axis with the smallest
323 // depth. note: normalR is set to point to a column of R1 or R2 if that is
324 // the smallest depth normal so far. otherwise normalR is 0 and normalC is
325 // set to a vector relative to body 1. invert_normal is 1 if the sign of
326 // the normal should be flipped.
327
328 #define TST(expr1, expr2, norm, cc) \
329 s2 = btFabs(expr1) - (expr2); \
330 if (s2 > 0) return 0; \
331 if (s2 > s) \
332 { \
333 s = s2; \
334 normalR = norm; \
335 invert_normal = ((expr1) < 0); \
336 code = (cc); \
337 }
338
339 s = -dInfinity;
340 invert_normal = 0;
341 code = 0;
342
343 // separating axis = u1,u2,u3
344 TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1);
345 TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2);
346 TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3);
347
348 // separating axis = v1,v2,v3
349 TST(dDOT41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4);
350 TST(dDOT41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5);
351 TST(dDOT41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6);
352
353 // note: cross product axes need to be scaled when s is computed.
354 // normal (n1,n2,n3) is relative to box 1.
355 #undef TST
356 #define TST(expr1, expr2, n1, n2, n3, cc) \
357 s2 = btFabs(expr1) - (expr2); \
358 if (s2 > SIMD_EPSILON) return 0; \
359 l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \
360 if (l > SIMD_EPSILON) \
361 { \
362 s2 /= l; \
363 if (s2 * fudge_factor > s) \
364 { \
365 s = s2; \
366 normalR = 0; \
367 normalC[0] = (n1) / l; \
368 normalC[1] = (n2) / l; \
369 normalC[2] = (n3) / l; \
370 invert_normal = ((expr1) < 0); \
371 code = (cc); \
372 } \
373 }
374
375 btScalar fudge2(1.0e-5f);
376
377 Q11 += fudge2;
378 Q12 += fudge2;
379 Q13 += fudge2;
380
381 Q21 += fudge2;
382 Q22 += fudge2;
383 Q23 += fudge2;
384
385 Q31 += fudge2;
386 Q32 += fudge2;
387 Q33 += fudge2;
388
389 // separating axis = u1 x (v1,v2,v3)
390 TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7);
391 TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8);
392 TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9);
393
394 // separating axis = u2 x (v1,v2,v3)
395 TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10);
396 TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11);
397 TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12);
398
399 // separating axis = u3 x (v1,v2,v3)
400 TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13);
401 TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14);
402 TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15);
403
404 #undef TST
405
406 if (!code) return 0;
407
408 // if we get to this point, the boxes interpenetrate. compute the normal
409 // in global coordinates.
410 if (normalR)
411 {
412 normal[0] = normalR[0];
413 normal[1] = normalR[4];
414 normal[2] = normalR[8];
415 }
416 else
417 {
418 dMULTIPLY0_331(normal, R1, normalC);
419 }
420 if (invert_normal)
421 {
422 normal[0] = -normal[0];
423 normal[1] = -normal[1];
424 normal[2] = -normal[2];
425 }
426 *depth = -s;
427
428 // compute contact point(s)
429
430 if (code > 6)
431 {
432 // an edge from box 1 touches an edge from box 2.
433 // find a point pa on the intersecting edge of box 1
434 btVector3 pa;
435 btScalar sign;
436 for (i = 0; i < 3; i++) pa[i] = p1[i];
437 for (j = 0; j < 3; j++)
438 {
439 sign = (dDOT14(normal, R1 + j) > 0) ? btScalar(1.0) : btScalar(-1.0);
440 for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j];
441 }
442
443 // find a point pb on the intersecting edge of box 2
444 btVector3 pb;
445 for (i = 0; i < 3; i++) pb[i] = p2[i];
446 for (j = 0; j < 3; j++)
447 {
448 sign = (dDOT14(normal, R2 + j) > 0) ? btScalar(-1.0) : btScalar(1.0);
449 for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j];
450 }
451
452 btScalar alpha, beta;
453 btVector3 ua, ub;
454 for (i = 0; i < 3; i++) ua[i] = R1[((code)-7) / 3 + i * 4];
455 for (i = 0; i < 3; i++) ub[i] = R2[((code)-7) % 3 + i * 4];
456
457 dLineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
458 for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha;
459 for (i = 0; i < 3; i++) pb[i] += ub[i] * beta;
460
461 {
462 //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
463 //contact[0].depth = *depth;
464 btVector3 pointInWorld;
465
466 #ifdef USE_CENTER_POINT
467 for (i = 0; i < 3; i++)
468 pointInWorld[i] = (pa[i] + pb[i]) * btScalar(0.5);
469 output.addContactPoint(-normal, pointInWorld, -*depth);
470 #else
471 output.addContactPoint(-normal, pb, -*depth);
472
473 #endif //
474 *return_code = code;
475 }
476 return 1;
477 }
478
479 // okay, we have a face-something intersection (because the separating
480 // axis is perpendicular to a face). define face 'a' to be the reference
481 // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
482 // the incident face (the closest face of the other box).
483
484 const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb;
485 if (code <= 3)
486 {
487 Ra = R1;
488 Rb = R2;
489 pa = p1;
490 pb = p2;
491 Sa = A;
492 Sb = B;
493 }
494 else
495 {
496 Ra = R2;
497 Rb = R1;
498 pa = p2;
499 pb = p1;
500 Sa = B;
501 Sb = A;
502 }
503
504 // nr = normal vector of reference face dotted with axes of incident box.
505 // anr = absolute values of nr.
506 btVector3 normal2, nr, anr;
507 if (code <= 3)
508 {
509 normal2[0] = normal[0];
510 normal2[1] = normal[1];
511 normal2[2] = normal[2];
512 }
513 else
514 {
515 normal2[0] = -normal[0];
516 normal2[1] = -normal[1];
517 normal2[2] = -normal[2];
518 }
519 dMULTIPLY1_331(nr, Rb, normal2);
520 anr[0] = btFabs(nr[0]);
521 anr[1] = btFabs(nr[1]);
522 anr[2] = btFabs(nr[2]);
523
524 // find the largest compontent of anr: this corresponds to the normal
525 // for the indident face. the other axis numbers of the indicent face
526 // are stored in a1,a2.
527 int lanr, a1, a2;
528 if (anr[1] > anr[0])
529 {
530 if (anr[1] > anr[2])
531 {
532 a1 = 0;
533 lanr = 1;
534 a2 = 2;
535 }
536 else
537 {
538 a1 = 0;
539 a2 = 1;
540 lanr = 2;
541 }
542 }
543 else
544 {
545 if (anr[0] > anr[2])
546 {
547 lanr = 0;
548 a1 = 1;
549 a2 = 2;
550 }
551 else
552 {
553 a1 = 0;
554 a2 = 1;
555 lanr = 2;
556 }
557 }
558
559 // compute center point of incident face, in reference-face coordinates
560 btVector3 center;
561 if (nr[lanr] < 0)
562 {
563 for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
564 }
565 else
566 {
567 for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
568 }
569
570 // find the normal and non-normal axis numbers of the reference box
571 int codeN, code1, code2;
572 if (code <= 3)
573 codeN = code - 1;
574 else
575 codeN = code - 4;
576 if (codeN == 0)
577 {
578 code1 = 1;
579 code2 = 2;
580 }
581 else if (codeN == 1)
582 {
583 code1 = 0;
584 code2 = 2;
585 }
586 else
587 {
588 code1 = 0;
589 code2 = 1;
590 }
591
592 // find the four corners of the incident face, in reference-face coordinates
593 btScalar quad[8]; // 2D coordinate of incident face (x,y pairs)
594 btScalar c1, c2, m11, m12, m21, m22;
595 c1 = dDOT14(center, Ra + code1);
596 c2 = dDOT14(center, Ra + code2);
597 // optimize this? - we have already computed this data above, but it is not
598 // stored in an easy-to-index format. for now it's quicker just to recompute
599 // the four dot products.
600 m11 = dDOT44(Ra + code1, Rb + a1);
601 m12 = dDOT44(Ra + code1, Rb + a2);
602 m21 = dDOT44(Ra + code2, Rb + a1);
603 m22 = dDOT44(Ra + code2, Rb + a2);
604 {
605 btScalar k1 = m11 * Sb[a1];
606 btScalar k2 = m21 * Sb[a1];
607 btScalar k3 = m12 * Sb[a2];
608 btScalar k4 = m22 * Sb[a2];
609 quad[0] = c1 - k1 - k3;
610 quad[1] = c2 - k2 - k4;
611 quad[2] = c1 - k1 + k3;
612 quad[3] = c2 - k2 + k4;
613 quad[4] = c1 + k1 + k3;
614 quad[5] = c2 + k2 + k4;
615 quad[6] = c1 + k1 - k3;
616 quad[7] = c2 + k2 - k4;
617 }
618
619 // find the size of the reference face
620 btScalar rect[2];
621 rect[0] = Sa[code1];
622 rect[1] = Sa[code2];
623
624 // intersect the incident and reference faces
625 btScalar ret[16];
626 int n = intersectRectQuad2(rect, quad, ret);
627 if (n < 1) return 0; // this should never happen
628
629 // convert the intersection points into reference-face coordinates,
630 // and compute the contact position and depth for each point. only keep
631 // those points that have a positive (penetrating) depth. delete points in
632 // the 'ret' array as necessary so that 'point' and 'ret' correspond.
633 btScalar point[3 * 8]; // penetrating contact points
634 btScalar dep[8]; // depths for those points
635 btScalar det1 = 1.f / (m11 * m22 - m12 * m21);
636 m11 *= det1;
637 m12 *= det1;
638 m21 *= det1;
639 m22 *= det1;
640 int cnum = 0; // number of penetrating contact points found
641 for (j = 0; j < n; j++)
642 {
643 btScalar k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
644 btScalar k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
645 for (i = 0; i < 3; i++) point[cnum * 3 + i] =
646 center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
647 dep[cnum] = Sa[codeN] - dDOT(normal2, point + cnum * 3);
648 if (dep[cnum] >= 0)
649 {
650 ret[cnum * 2] = ret[j * 2];
651 ret[cnum * 2 + 1] = ret[j * 2 + 1];
652 cnum++;
653 }
654 }
655 if (cnum < 1) return 0; // this should never happen
656
657 // we can't generate more contacts than we actually have
658 if (maxc > cnum) maxc = cnum;
659 if (maxc < 1) maxc = 1;
660
661 if (cnum <= maxc)
662 {
663 if (code < 4)
664 {
665 // we have less contacts than we need, so we use them all
666 for (j = 0; j < cnum; j++)
667 {
668 btVector3 pointInWorld;
669 for (i = 0; i < 3; i++)
670 pointInWorld[i] = point[j * 3 + i] + pa[i];
671 output.addContactPoint(-normal, pointInWorld, -dep[j]);
672 }
673 }
674 else
675 {
676 // we have less contacts than we need, so we use them all
677 for (j = 0; j < cnum; j++)
678 {
679 btVector3 pointInWorld;
680 for (i = 0; i < 3; i++)
681 pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j];
682 //pointInWorld[i] = point[j*3+i] + pa[i];
683 output.addContactPoint(-normal, pointInWorld, -dep[j]);
684 }
685 }
686 }
687 else
688 {
689 // we have more contacts than are wanted, some of them must be culled.
690 // find the deepest point, it is always the first contact.
691 int i1 = 0;
692 btScalar maxdepth = dep[0];
693 for (i = 1; i < cnum; i++)
694 {
695 if (dep[i] > maxdepth)
696 {
697 maxdepth = dep[i];
698 i1 = i;
699 }
700 }
701
702 int iret[8];
703 cullPoints2(cnum, ret, maxc, i1, iret);
704
705 for (j = 0; j < maxc; j++)
706 {
707 // dContactGeom *con = CONTACT(contact,skip*j);
708 // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
709 // con->depth = dep[iret[j]];
710
711 btVector3 posInWorld;
712 for (i = 0; i < 3; i++)
713 posInWorld[i] = point[iret[j] * 3 + i] + pa[i];
714 if (code < 4)
715 {
716 output.addContactPoint(-normal, posInWorld, -dep[iret[j]]);
717 }
718 else
719 {
720 output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
721 }
722 }
723 cnum = maxc;
724 }
725
726 *return_code = code;
727 return cnum;
728 }
729
730 void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* /*debugDraw*/, bool /*swapResults*/)
731 {
732 const btTransform& transformA = input.m_transformA;
733 const btTransform& transformB = input.m_transformB;
734
735 int skip = 0;
736 dContactGeom* contact = 0;
737
738 dMatrix3 R1;
739 dMatrix3 R2;
740
741 for (int j = 0; j < 3; j++)
742 {
743 R1[0 + 4 * j] = transformA.getBasis()[j].x();
744 R2[0 + 4 * j] = transformB.getBasis()[j].x();
745
746 R1[1 + 4 * j] = transformA.getBasis()[j].y();
747 R2[1 + 4 * j] = transformB.getBasis()[j].y();
748
749 R1[2 + 4 * j] = transformA.getBasis()[j].z();
750 R2[2 + 4 * j] = transformB.getBasis()[j].z();
751 }
752
753 btVector3 normal;
754 btScalar depth;
755 int return_code;
756 int maxc = 4;
757
758 dBoxBox2(transformA.getOrigin(),
759 R1,
760 2.f * m_box1->getHalfExtentsWithMargin(),
761 transformB.getOrigin(),
762 R2,
763 2.f * m_box2->getHalfExtentsWithMargin(),
764 normal, &depth, &return_code,
765 maxc, contact, skip,
766 output);
767 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:523
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
dDOT44
static btScalar dDOT44(const btScalar *a, const btScalar *b)
Definition: btBoxBoxDetector.cpp:58
btDiscreteCollisionDetectorInterface::Result
Definition: btDiscreteCollisionDetectorInterface.h:29
btBoxShape.h
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btBoxShape::getHalfExtentsWithMargin
btVector3 getHalfExtentsWithMargin() const
Definition: btBoxShape.h:34
btBoxBoxDetector.h
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:46
M__PI
#define M__PI
Definition: btBoxBoxDetector.cpp:177
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition: btDiscreteCollisionDetectorInterface.h:47
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btBoxBoxDetector::btBoxBoxDetector
btBoxBoxDetector(const btBoxShape *box1, const btBoxShape *box2)
ODE box-box collision detection is adapted to work with Bullet.
Definition: btBoxBoxDetector.cpp:27
dDOT14
static btScalar dDOT14(const btScalar *a, const btScalar *b)
Definition: btBoxBoxDetector.cpp:60
dDOT41
static btScalar dDOT41(const btScalar *a, const btScalar *b)
Definition: btBoxBoxDetector.cpp:59
cullPoints2
void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
Definition: btBoxBoxDetector.cpp:188
output
#define output
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:498
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:26
btBoxBoxDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition: btBoxBoxDetector.cpp:730
dBoxBox2
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)
Definition: btBoxBoxDetector.cpp:272
dMULTIPLY0_331
#define dMULTIPLY0_331(A, B, C)
Definition: btBoxBoxDetector.cpp:76
btBoxShape
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:26
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
intersectRectQuad2
static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
Definition: btBoxBoxDetector.cpp:117
dMULTIPLY1_331
#define dMULTIPLY1_331(A, B, C)
Definition: btBoxBoxDetector.cpp:75
dLineClosestApproach
void dLineClosestApproach(const btVector3 &pa, const btVector3 &ua, const btVector3 &pb, const btVector3 &ub, btScalar *alpha, btScalar *beta)
Definition: btBoxBoxDetector.cpp:83
dInfinity
#define dInfinity
Definition: btBoxBoxDetector.cpp:50
btBoxBoxDetector::m_box2
const btBoxShape * m_box2
Definition: btBoxBoxDetector.h:30
TST
#define TST(expr1, expr2, norm, cc)
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition: btDiscreteCollisionDetectorInterface.h:39
btBoxBoxDetector::m_box1
const btBoxShape * m_box1
Definition: btBoxBoxDetector.h:29
dDOTpq
#define dDOTpq(a, b, p, q)
Definition: btBoxBoxDetector.cpp:49
dDOT
static btScalar dDOT(const btScalar *a, const btScalar *b)
Definition: btBoxBoxDetector.cpp:57
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:477
dMatrix3
btScalar dMatrix3[4 *3]
Definition: btBoxBoxDetector.cpp:78