Bullet Collision Detection & Physics Library: btMprPenetration.h Source File

Bullet Collision Detection & Physics Library

btMprPenetration.h
Go to the documentation of this file.
1
2 /***
3 * ---------------------------------
4 * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
5 *
6 * This file was ported from mpr.c file, part of libccd.
7 * The Minkoski Portal Refinement implementation was ported
8 * to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
9 * The original MPR idea and implementation is by Gary Snethen
10 * in XenoCollide, see http://github.com/erwincoumans/xenocollide
11 *
12 * Distributed under the OSI-approved BSD License (the "License");
13 * see <http://www.opensource.org/licenses/bsd-license.php>.
14 * This software is distributed WITHOUT ANY WARRANTY; without even the
15 * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16 * See the License for more information.
17 */
18
20
21 #ifndef BT_MPR_PENETRATION_H
22 #define BT_MPR_PENETRATION_H
23
24 #define BT_DEBUG_MPR1
25
26 #include "LinearMath/btTransform.h"
27 #include "LinearMath/btAlignedObjectArray.h"
28
29 //#define MPR_AVERAGE_CONTACT_POSITIONS
30
31 struct btMprCollisionDescription
32 {
33 btVector3 m_firstDir;
34 int m_maxGjkIterations;
35 btScalar m_maximumDistanceSquared;
36 btScalar m_gjkRelError2;
37
38 btMprCollisionDescription()
39 : m_firstDir(0, 1, 0),
40 m_maxGjkIterations(1000),
41 m_maximumDistanceSquared(1e30f),
42 m_gjkRelError2(1.0e-6)
43 {
44 }
45 virtual ~btMprCollisionDescription()
46 {
47 }
48 };
49
50 struct btMprDistanceInfo
51 {
52 btVector3 m_pointOnA;
53 btVector3 m_pointOnB;
54 btVector3 m_normalBtoA;
55 btScalar m_distance;
56 };
57
58 #ifdef __cplusplus
59 #define BT_MPR_SQRT sqrtf
60 #else
61 #define BT_MPR_SQRT sqrt
62 #endif
63 #define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
64 #define BT_MPR_FABS fabs
65
66 #define BT_MPR_TOLERANCE 1E-6f
67 #define BT_MPR_MAX_ITERATIONS 1000
68
69 struct _btMprSupport_t
70 {
71 btVector3 v;
72 btVector3 v1;
73 btVector3 v2;
74 };
75 typedef struct _btMprSupport_t btMprSupport_t;
76
77 struct _btMprSimplex_t
78 {
79 btMprSupport_t ps[4];
80 int last;
81 };
82 typedef struct _btMprSimplex_t btMprSimplex_t;
83
84 inline btMprSupport_t *btMprSimplexPointW(btMprSimplex_t *s, int idx)
85 {
86 return &s->ps[idx];
87 }
88
89 inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
90 {
91 s->last = size - 1;
92 }
93
94 #ifdef DEBUG_MPR
95 inline void btPrintPortalVertex(_btMprSimplex_t *portal, int index)
96 {
97 printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(), portal->ps[index].v.y(), portal->ps[index].v.z(),
98 portal->ps[index].v1.x(), portal->ps[index].v1.y(), portal->ps[index].v1.z(),
99 portal->ps[index].v2.x(), portal->ps[index].v2.y(), portal->ps[index].v2.z());
100 }
101 #endif //DEBUG_MPR
102
103 inline int btMprSimplexSize(const btMprSimplex_t *s)
104 {
105 return s->last + 1;
106 }
107
108 inline const btMprSupport_t *btMprSimplexPoint(const btMprSimplex_t *s, int idx)
109 {
110 // here is no check on boundaries
111 return &s->ps[idx];
112 }
113
114 inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
115 {
116 *d = *s;
117 }
118
119 inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
120 {
121 btMprSupportCopy(s->ps + pos, a);
122 }
123
124 inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
125 {
126 btMprSupport_t supp;
127
128 btMprSupportCopy(&supp, &s->ps[pos1]);
129 btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
130 btMprSupportCopy(&s->ps[pos2], &supp);
131 }
132
133 inline int btMprIsZero(float val)
134 {
135 return BT_MPR_FABS(val) < FLT_EPSILON;
136 }
137
138 inline int btMprEq(float _a, float _b)
139 {
140 float ab;
141 float a, b;
142
143 ab = BT_MPR_FABS(_a - _b);
144 if (BT_MPR_FABS(ab) < FLT_EPSILON)
145 return 1;
146
147 a = BT_MPR_FABS(_a);
148 b = BT_MPR_FABS(_b);
149 if (b > a)
150 {
151 return ab < FLT_EPSILON * b;
152 }
153 else
154 {
155 return ab < FLT_EPSILON * a;
156 }
157 }
158
159 inline int btMprVec3Eq(const btVector3 *a, const btVector3 *b)
160 {
161 return btMprEq((*a).x(), (*b).x()) && btMprEq((*a).y(), (*b).y()) && btMprEq((*a).z(), (*b).z());
162 }
163
164 template <typename btConvexTemplate>
165 inline void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center)
166 {
167 center->v1 = a.getObjectCenterInWorld();
168 center->v2 = b.getObjectCenterInWorld();
169 center->v = center->v1 - center->v2;
170 }
171
172 inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
173 {
174 v->setValue(x, y, z);
175 }
176
177 inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
178 {
179 *v += *w;
180 }
181
182 inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
183 {
184 *v = *w;
185 }
186
187 inline void btMprVec3Scale(btVector3 *d, float k)
188 {
189 *d *= k;
190 }
191
192 inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
193 {
194 float dot;
195
196 dot = btDot(*a, *b);
197 return dot;
198 }
199
200 inline float btMprVec3Len2(const btVector3 *v)
201 {
202 return btMprVec3Dot(v, v);
203 }
204
205 inline void btMprVec3Normalize(btVector3 *d)
206 {
207 float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
208 btMprVec3Scale(d, k);
209 }
210
211 inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
212 {
213 *d = btCross(*a, *b);
214 }
215
216 inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
217 {
218 *d = *v - *w;
219 }
220
221 inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
222 {
223 btVector3 v2v1, v3v1;
224
225 btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v,
226 &btMprSimplexPoint(portal, 1)->v);
227 btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v,
228 &btMprSimplexPoint(portal, 1)->v);
229 btMprVec3Cross(dir, &v2v1, &v3v1);
230 btMprVec3Normalize(dir);
231 }
232
233 inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
234 const btVector3 *dir)
235 {
236 float dot;
237 dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
238 return btMprIsZero(dot) || dot > 0.f;
239 }
240
241 inline int portalReachTolerance(const btMprSimplex_t *portal,
242 const btMprSupport_t *v4,
243 const btVector3 *dir)
244 {
245 float dv1, dv2, dv3, dv4;
246 float dot1, dot2, dot3;
247
248 // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
249
250 dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir);
251 dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir);
252 dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir);
253 dv4 = btMprVec3Dot(&v4->v, dir);
254
255 dot1 = dv4 - dv1;
256 dot2 = dv4 - dv2;
257 dot3 = dv4 - dv3;
258
259 dot1 = BT_MPR_FMIN(dot1, dot2);
260 dot1 = BT_MPR_FMIN(dot1, dot3);
261
262 return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
263 }
264
265 inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
266 const btMprSupport_t *v4,
267 const btVector3 *dir)
268 {
269 float dot;
270 dot = btMprVec3Dot(&v4->v, dir);
271 return btMprIsZero(dot) || dot > 0.f;
272 }
273
274 inline void btExpandPortal(btMprSimplex_t *portal,
275 const btMprSupport_t *v4)
276 {
277 float dot;
278 btVector3 v4v0;
279
280 btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
281 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
282 if (dot > 0.f)
283 {
284 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
285 if (dot > 0.f)
286 {
287 btMprSimplexSet(portal, 1, v4);
288 }
289 else
290 {
291 btMprSimplexSet(portal, 3, v4);
292 }
293 }
294 else
295 {
296 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
297 if (dot > 0.f)
298 {
299 btMprSimplexSet(portal, 2, v4);
300 }
301 else
302 {
303 btMprSimplexSet(portal, 1, v4);
304 }
305 }
306 }
307 template <typename btConvexTemplate>
308 inline void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b,
309 const btMprCollisionDescription &colDesc,
310 const btVector3 &dir, btMprSupport_t *supp)
311 {
312 btVector3 separatingAxisInA = dir * a.getWorldTransform().getBasis();
313 btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis();
314
315 btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA);
316 btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB);
317
318 supp->v1 = a.getWorldTransform()(pInA);
319 supp->v2 = b.getWorldTransform()(qInB);
320 supp->v = supp->v1 - supp->v2;
321 }
322
323 template <typename btConvexTemplate>
324 static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b,
325 const btMprCollisionDescription &colDesc,
326 btMprSimplex_t *portal)
327 {
328 btVector3 dir, va, vb;
329 float dot;
330 int cont;
331
332 // vertex 0 is center of portal
333 btFindOrigin(a, b, colDesc, btMprSimplexPointW(portal, 0));
334
335 // vertex 0 is center of portal
336 btMprSimplexSetSize(portal, 1);
337
338 btVector3 zero = btVector3(0, 0, 0);
339 btVector3 *org = &zero;
340
341 if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org))
342 {
343 // Portal's center lies on origin (0,0,0) => we know that objects
344 // intersect but we would need to know penetration info.
345 // So move center little bit...
346 btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
347 btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va);
348 }
349
350 // vertex 1 = support in direction of origin
351 btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
352 btMprVec3Scale(&dir, -1.f);
353 btMprVec3Normalize(&dir);
354
355 btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 1));
356
357 btMprSimplexSetSize(portal, 2);
358
359 // test if origin isn't outside of v1
360 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);
361
362 if (btMprIsZero(dot) || dot < 0.f)
363 return -1;
364
365 // vertex 2
366 btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
367 &btMprSimplexPoint(portal, 1)->v);
368 if (btMprIsZero(btMprVec3Len2(&dir)))
369 {
370 if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org))
371 {
372 // origin lies on v1
373 return 1;
374 }
375 else
376 {
377 // origin lies on v0-v1 segment
378 return 2;
379 }
380 }
381
382 btMprVec3Normalize(&dir);
383 btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 2));
384
385 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
386 if (btMprIsZero(dot) || dot < 0.f)
387 return -1;
388
389 btMprSimplexSetSize(portal, 3);
390
391 // vertex 3 direction
392 btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
393 &btMprSimplexPoint(portal, 0)->v);
394 btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
395 &btMprSimplexPoint(portal, 0)->v);
396 btMprVec3Cross(&dir, &va, &vb);
397 btMprVec3Normalize(&dir);
398
399 // it is better to form portal faces to be oriented "outside" origin
400 dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
401 if (dot > 0.f)
402 {
403 btMprSimplexSwap(portal, 1, 2);
404 btMprVec3Scale(&dir, -1.f);
405 }
406
407 while (btMprSimplexSize(portal) < 4)
408 {
409 btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 3));
410
411 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
412 if (btMprIsZero(dot) || dot < 0.f)
413 return -1;
414
415 cont = 0;
416
417 // test if origin is outside (v1, v0, v3) - set v2 as v3 and
418 // continue
419 btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v,
420 &btMprSimplexPoint(portal, 3)->v);
421 dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
422 if (dot < 0.f && !btMprIsZero(dot))
423 {
424 btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
425 cont = 1;
426 }
427
428 if (!cont)
429 {
430 // test if origin is outside (v3, v0, v2) - set v1 as v3 and
431 // continue
432 btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v,
433 &btMprSimplexPoint(portal, 2)->v);
434 dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
435 if (dot < 0.f && !btMprIsZero(dot))
436 {
437 btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
438 cont = 1;
439 }
440 }
441
442 if (cont)
443 {
444 btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
445 &btMprSimplexPoint(portal, 0)->v);
446 btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
447 &btMprSimplexPoint(portal, 0)->v);
448 btMprVec3Cross(&dir, &va, &vb);
449 btMprVec3Normalize(&dir);
450 }
451 else
452 {
453 btMprSimplexSetSize(portal, 4);
454 }
455 }
456
457 return 0;
458 }
459
460 template <typename btConvexTemplate>
461 static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc,
462 btMprSimplex_t *portal)
463 {
464 btVector3 dir;
465 btMprSupport_t v4;
466
467 for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
468 //while (1)
469 {
470 // compute direction outside the portal (from v0 through v1,v2,v3
471 // face)
472 btPortalDir(portal, &dir);
473
474 // test if origin is inside the portal
475 if (portalEncapsulesOrigin(portal, &dir))
476 return 0;
477
478 // get next support point
479
480 btMprSupport(a, b, colDesc, dir, &v4);
481
482 // test if v4 can expand portal to contain origin and if portal
483 // expanding doesn't reach given tolerance
484 if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
485 {
486 return -1;
487 }
488
489 // v1-v2-v3 triangle must be rearranged to face outside Minkowski
490 // difference (direction from v0).
491 btExpandPortal(portal, &v4);
492 }
493
494 return -1;
495 }
496
497 static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
498 {
499 btVector3 zero = btVector3(0, 0, 0);
500 btVector3 *origin = &zero;
501
502 btVector3 dir;
503 size_t i;
504 float b[4], sum, inv;
505 btVector3 vec, p1, p2;
506
507 btPortalDir(portal, &dir);
508
509 // use barycentric coordinates of tetrahedron to find origin
510 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
511 &btMprSimplexPoint(portal, 2)->v);
512 b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
513
514 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
515 &btMprSimplexPoint(portal, 2)->v);
516 b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
517
518 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
519 &btMprSimplexPoint(portal, 1)->v);
520 b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
521
522 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
523 &btMprSimplexPoint(portal, 1)->v);
524 b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
525
526 sum = b[0] + b[1] + b[2] + b[3];
527
528 if (btMprIsZero(sum) || sum < 0.f)
529 {
530 b[0] = 0.f;
531
532 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
533 &btMprSimplexPoint(portal, 3)->v);
534 b[1] = btMprVec3Dot(&vec, &dir);
535 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
536 &btMprSimplexPoint(portal, 1)->v);
537 b[2] = btMprVec3Dot(&vec, &dir);
538 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
539 &btMprSimplexPoint(portal, 2)->v);
540 b[3] = btMprVec3Dot(&vec, &dir);
541
542 sum = b[1] + b[2] + b[3];
543 }
544
545 inv = 1.f / sum;
546
547 btMprVec3Copy(&p1, origin);
548 btMprVec3Copy(&p2, origin);
549 for (i = 0; i < 4; i++)
550 {
551 btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
552 btMprVec3Scale(&vec, b[i]);
553 btMprVec3Add(&p1, &vec);
554
555 btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
556 btMprVec3Scale(&vec, b[i]);
557 btMprVec3Add(&p2, &vec);
558 }
559 btMprVec3Scale(&p1, inv);
560 btMprVec3Scale(&p2, inv);
561 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
562 btMprVec3Copy(pos, &p1);
563 btMprVec3Add(pos, &p2);
564 btMprVec3Scale(pos, 0.5);
565 #else
566 btMprVec3Copy(pos, &p2);
567 #endif //MPR_AVERAGE_CONTACT_POSITIONS
568 }
569
570 inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
571 {
572 btVector3 ab;
573 btMprVec3Sub2(&ab, a, b);
574 return btMprVec3Len2(&ab);
575 }
576
577 inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
578 const btVector3 *x0,
579 const btVector3 *b,
580 btVector3 *witness)
581 {
582 // The computation comes from solving equation of segment:
583 // S(t) = x0 + t.d
584 // where - x0 is initial point of segment
585 // - d is direction of segment from x0 (|d| > 0)
586 // - t belongs to <0, 1> interval
587 //
588 // Than, distance from a segment to some point P can be expressed:
589 // D(t) = |x0 + t.d - P|^2
590 // which is distance from any point on segment. Minimization
591 // of this function brings distance from P to segment.
592 // Minimization of D(t) leads to simple quadratic equation that's
593 // solving is straightforward.
594 //
595 // Bonus of this method is witness point for free.
596
597 float dist, t;
598 btVector3 d, a;
599
600 // direction of segment
601 btMprVec3Sub2(&d, b, x0);
602
603 // precompute vector from P to x0
604 btMprVec3Sub2(&a, x0, P);
605
606 t = -1.f * btMprVec3Dot(&a, &d);
607 t /= btMprVec3Len2(&d);
608
609 if (t < 0.f || btMprIsZero(t))
610 {
611 dist = btMprVec3Dist2(x0, P);
612 if (witness)
613 btMprVec3Copy(witness, x0);
614 }
615 else if (t > 1.f || btMprEq(t, 1.f))
616 {
617 dist = btMprVec3Dist2(b, P);
618 if (witness)
619 btMprVec3Copy(witness, b);
620 }
621 else
622 {
623 if (witness)
624 {
625 btMprVec3Copy(witness, &d);
626 btMprVec3Scale(witness, t);
627 btMprVec3Add(witness, x0);
628 dist = btMprVec3Dist2(witness, P);
629 }
630 else
631 {
632 // recycling variables
633 btMprVec3Scale(&d, t);
634 btMprVec3Add(&d, &a);
635 dist = btMprVec3Len2(&d);
636 }
637 }
638
639 return dist;
640 }
641
642 inline float btMprVec3PointTriDist2(const btVector3 *P,
643 const btVector3 *x0, const btVector3 *B,
644 const btVector3 *C,
645 btVector3 *witness)
646 {
647 // Computation comes from analytic expression for triangle (x0, B, C)
648 // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
649 // Then equation for distance is:
650 // D(s, t) = | T(s, t) - P |^2
651 // This leads to minimization of quadratic function of two variables.
652 // The solution from is taken only if s is between 0 and 1, t is
653 // between 0 and 1 and t + s < 1, otherwise distance from segment is
654 // computed.
655
656 btVector3 d1, d2, a;
657 float u, v, w, p, q, r;
658 float s, t, dist, dist2;
659 btVector3 witness2;
660
661 btMprVec3Sub2(&d1, B, x0);
662 btMprVec3Sub2(&d2, C, x0);
663 btMprVec3Sub2(&a, x0, P);
664
665 u = btMprVec3Dot(&a, &a);
666 v = btMprVec3Dot(&d1, &d1);
667 w = btMprVec3Dot(&d2, &d2);
668 p = btMprVec3Dot(&a, &d1);
669 q = btMprVec3Dot(&a, &d2);
670 r = btMprVec3Dot(&d1, &d2);
671
672 btScalar div = (w * v - r * r);
673 if (btMprIsZero(div))
674 {
675 s = -1;
676 }
677 else
678 {
679 s = (q * r - w * p) / div;
680 t = (-s * r - q) / w;
681 }
682
683 if ((btMprIsZero(s) || s > 0.f) && (btMprEq(s, 1.f) || s < 1.f) && (btMprIsZero(t) || t > 0.f) && (btMprEq(t, 1.f) || t < 1.f) && (btMprEq(t + s, 1.f) || t + s < 1.f))
684 {
685 if (witness)
686 {
687 btMprVec3Scale(&d1, s);
688 btMprVec3Scale(&d2, t);
689 btMprVec3Copy(witness, x0);
690 btMprVec3Add(witness, &d1);
691 btMprVec3Add(witness, &d2);
692
693 dist = btMprVec3Dist2(witness, P);
694 }
695 else
696 {
697 dist = s * s * v;
698 dist += t * t * w;
699 dist += 2.f * s * t * r;
700 dist += 2.f * s * p;
701 dist += 2.f * t * q;
702 dist += u;
703 }
704 }
705 else
706 {
707 dist = _btMprVec3PointSegmentDist2(P, x0, B, witness);
708
709 dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
710 if (dist2 < dist)
711 {
712 dist = dist2;
713 if (witness)
714 btMprVec3Copy(witness, &witness2);
715 }
716
717 dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
718 if (dist2 < dist)
719 {
720 dist = dist2;
721 if (witness)
722 btMprVec3Copy(witness, &witness2);
723 }
724 }
725
726 return dist;
727 }
728
729 template <typename btConvexTemplate>
730 static void btFindPenetr(const btConvexTemplate &a, const btConvexTemplate &b,
731 const btMprCollisionDescription &colDesc,
732 btMprSimplex_t *portal,
733 float *depth, btVector3 *pdir, btVector3 *pos)
734 {
735 btVector3 dir;
736 btMprSupport_t v4;
737 unsigned long iterations;
738
739 btVector3 zero = btVector3(0, 0, 0);
740 btVector3 *origin = &zero;
741
742 iterations = 1UL;
743 for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
744 //while (1)
745 {
746 // compute portal direction and obtain next support point
747 btPortalDir(portal, &dir);
748
749 btMprSupport(a, b, colDesc, dir, &v4);
750
751 // reached tolerance -> find penetration info
752 if (portalReachTolerance(portal, &v4, &dir) || iterations == BT_MPR_MAX_ITERATIONS)
753 {
754 *depth = btMprVec3PointTriDist2(origin, &btMprSimplexPoint(portal, 1)->v, &btMprSimplexPoint(portal, 2)->v, &btMprSimplexPoint(portal, 3)->v, pdir);
755 *depth = BT_MPR_SQRT(*depth);
756
757 if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
758 {
759 *pdir = dir;
760 }
761 btMprVec3Normalize(pdir);
762
763 // barycentric coordinates:
764 btFindPos(portal, pos);
765
766 return;
767 }
768
769 btExpandPortal(portal, &v4);
770
771 iterations++;
772 }
773 }
774
775 static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
776 {
777 // Touching contact on portal's v1 - so depth is zero and direction
778 // is unimportant and pos can be guessed
779 *depth = 0.f;
780 btVector3 zero = btVector3(0, 0, 0);
781 btVector3 *origin = &zero;
782
783 btMprVec3Copy(dir, origin);
784 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
785 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
786 btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
787 btMprVec3Scale(pos, 0.5);
788 #else
789 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
790 #endif
791 }
792
793 static void btFindPenetrSegment(btMprSimplex_t *portal,
794 float *depth, btVector3 *dir, btVector3 *pos)
795 {
796 // Origin lies on v0-v1 segment.
797 // Depth is distance to v1, direction also and position must be
798 // computed
799 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
800 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
801 btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
802 btMprVec3Scale(pos, 0.5f);
803 #else
804 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
805 #endif //MPR_AVERAGE_CONTACT_POSITIONS
806
807 btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
808 *depth = BT_MPR_SQRT(btMprVec3Len2(dir));
809 btMprVec3Normalize(dir);
810 }
811
812 template <typename btConvexTemplate>
813 inline int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b,
814 const btMprCollisionDescription &colDesc,
815 float *depthOut, btVector3 *dirOut, btVector3 *posOut)
816 {
817 btMprSimplex_t portal;
818
819 // Phase 1: Portal discovery
820 int result = btDiscoverPortal(a, b, colDesc, &portal);
821
822 //sepAxis[pairIndex] = *pdir;//or -dir?
823
824 switch (result)
825 {
826 case 0:
827 {
828 // Phase 2: Portal refinement
829
830 result = btRefinePortal(a, b, colDesc, &portal);
831 if (result < 0)
832 return -1;
833
834 // Phase 3. Penetration info
835 btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut);
836
837 break;
838 }
839 case 1:
840 {
841 // Touching contact on portal's v1.
842 btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
843 result = 0;
844 break;
845 }
846 case 2:
847 {
848 btFindPenetrSegment(&portal, depthOut, dirOut, posOut);
849 result = 0;
850 break;
851 }
852 default:
853 {
854 //if (res < 0)
855 //{
856 // Origin isn't inside portal - no collision.
857 result = -1;
858 //}
859 }
860 };
861
862 return result;
863 };
864
865 template <typename btConvexTemplate, typename btMprDistanceTemplate>
866 inline int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
867 {
868 btVector3 dir, pos;
869 float depth;
870
871 int res = btMprPenetration(a, b, colDesc, &depth, &dir, &pos);
872 if (res == 0)
873 {
874 distInfo->m_distance = -depth;
875 distInfo->m_pointOnB = pos;
876 distInfo->m_normalBtoA = -dir;
877 distInfo->m_pointOnA = pos - distInfo->m_distance * dir;
878 return 0;
879 }
880
881 return -1;
882 }
883
884 #endif //BT_MPR_PENETRATION_H
BT_MPR_FMIN
#define BT_MPR_FMIN(x, y)
Definition: btMprPenetration.h:63
btMprDistanceInfo::m_pointOnA
btVector3 m_pointOnA
Definition: btMprPenetration.h:52
_btMprSupport_t::v2
btVector3 v2
Support point in obj2.
Definition: btMprPenetration.h:73
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btMprVec3Cross
void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:211
portalReachTolerance
int portalReachTolerance(const btMprSimplex_t *portal, const btMprSupport_t *v4, const btVector3 *dir)
Definition: btMprPenetration.h:241
btMprVec3Eq
int btMprVec3Eq(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:159
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btMprVec3Sub2
void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:216
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
_btMprSupport_t::v
btVector3 v
Support point in minkowski sum.
Definition: btMprPenetration.h:71
btDiscoverPortal
static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal)
Definition: btMprPenetration.h:324
btMprCollisionDescription::~btMprCollisionDescription
virtual ~btMprCollisionDescription()
Definition: btMprPenetration.h:45
_btMprSimplex_t::last
int last
index of last added point
Definition: btMprPenetration.h:80
btMprDistanceInfo
Definition: btMprPenetration.h:51
btMprSupportCopy
void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
Definition: btMprPenetration.h:114
BT_MPR_TOLERANCE
#define BT_MPR_TOLERANCE
Definition: btMprPenetration.h:66
btComputeMprPenetration
int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
Definition: btMprPenetration.h:866
btMprCollisionDescription
Definition: btMprPenetration.h:32
btMprSimplexPointW
btMprSupport_t * btMprSimplexPointW(btMprSimplex_t *s, int idx)
Definition: btMprPenetration.h:84
_btMprVec3PointSegmentDist2
float _btMprVec3PointSegmentDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *b, btVector3 *witness)
Definition: btMprPenetration.h:577
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
btMprDistanceInfo::m_distance
btScalar m_distance
Definition: btMprPenetration.h:55
BT_MPR_FABS
#define BT_MPR_FABS
Definition: btMprPenetration.h:64
btExpandPortal
void btExpandPortal(btMprSimplex_t *portal, const btMprSupport_t *v4)
Definition: btMprPenetration.h:274
btPortalDir
void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
Definition: btMprPenetration.h:221
btMprCollisionDescription::btMprCollisionDescription
btMprCollisionDescription()
Definition: btMprPenetration.h:38
btMprSimplexSet
void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
Definition: btMprPenetration.h:119
btMprVec3Dist2
float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:570
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btFindPenetr
static void btFindPenetr(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal, float *depth, btVector3 *pdir, btVector3 *pos)
Definition: btMprPenetration.h:730
_btMprSupport_t
Definition: btMprPenetration.h:70
_btMprSimplex_t::ps
btMprSupport_t ps[4]
Definition: btMprPenetration.h:79
btMprVec3PointTriDist2
float btMprVec3PointTriDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *B, const btVector3 *C, btVector3 *witness)
Definition: btMprPenetration.h:642
btMprSimplexSize
int btMprSimplexSize(const btMprSimplex_t *s)
Definition: btMprPenetration.h:103
btMprVec3Len2
float btMprVec3Len2(const btVector3 *v)
Definition: btMprPenetration.h:200
btMprSimplexSetSize
void btMprSimplexSetSize(btMprSimplex_t *s, int size)
Definition: btMprPenetration.h:89
btMprSupport
void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, const btVector3 &dir, btMprSupport_t *supp)
Definition: btMprPenetration.h:308
BT_MPR_MAX_ITERATIONS
#define BT_MPR_MAX_ITERATIONS
Definition: btMprPenetration.h:67
btMprCollisionDescription::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition: btMprPenetration.h:35
btTransform.h
btMprVec3Normalize
void btMprVec3Normalize(btVector3 *d)
Definition: btMprPenetration.h:205
portalCanEncapsuleOrigin
int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, const btMprSupport_t *v4, const btVector3 *dir)
Definition: btMprPenetration.h:265
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btMprSimplexSwap
void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
Definition: btMprPenetration.h:124
btMprCollisionDescription::m_firstDir
btVector3 m_firstDir
Definition: btMprPenetration.h:33
btFindPenetrTouch
static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
Definition: btMprPenetration.h:775
btMprEq
int btMprEq(float _a, float _b)
Definition: btMprPenetration.h:138
btMprSimplexPoint
const btMprSupport_t * btMprSimplexPoint(const btMprSimplex_t *s, int idx)
Definition: btMprPenetration.h:108
btFindPos
static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
Definition: btMprPenetration.h:497
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
_btMprSimplex_t
Definition: btMprPenetration.h:78
portalEncapsulesOrigin
int portalEncapsulesOrigin(const btMprSimplex_t *portal, const btVector3 *dir)
Definition: btMprPenetration.h:233
btMprPenetration
int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, float *depthOut, btVector3 *dirOut, btVector3 *posOut)
Definition: btMprPenetration.h:813
btFindOrigin
void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center)
Definition: btMprPenetration.h:165
btMprVec3Dot
float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:192
btRefinePortal
static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal)
Definition: btMprPenetration.h:461
btMprCollisionDescription::m_gjkRelError2
btScalar m_gjkRelError2
Definition: btMprPenetration.h:36
btMprDistanceInfo::m_pointOnB
btVector3 m_pointOnB
Definition: btMprPenetration.h:53
btAlignedObjectArray.h
btMprIsZero
int btMprIsZero(float val)
Definition: btMprPenetration.h:133
btMprVec3Set
void btMprVec3Set(btVector3 *v, float x, float y, float z)
Definition: btMprPenetration.h:172
btMprVec3Add
void btMprVec3Add(btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:177
BT_MPR_SQRT
#define BT_MPR_SQRT
Definition: btMprPenetration.h:61
btFindPenetrSegment
static void btFindPenetrSegment(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
Definition: btMprPenetration.h:793
btMprVec3Scale
void btMprVec3Scale(btVector3 *d, float k)
Definition: btMprPenetration.h:187
btMprDistanceInfo::m_normalBtoA
btVector3 m_normalBtoA
Definition: btMprPenetration.h:54
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
_btMprSupport_t::v1
btVector3 v1
Support point in obj1.
Definition: btMprPenetration.h:72
btMprCollisionDescription::m_maxGjkIterations
int m_maxGjkIterations
Definition: btMprPenetration.h:34
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btMprVec3Copy
void btMprVec3Copy(btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:182