3 * ---------------------------------
4 * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
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
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.
19 ///2014 Oct, Erwin Coumans, Use templates to avoid void* casts
21 #ifndef BT_MPR_PENETRATION_H
22 #define BT_MPR_PENETRATION_H
26 #include "LinearMath/btTransform.h"
27 #include "LinearMath/btAlignedObjectArray.h"
29 //#define MPR_AVERAGE_CONTACT_POSITIONS
31 struct btMprCollisionDescription
34 int m_maxGjkIterations;
35 btScalar m_maximumDistanceSquared;
36 btScalar m_gjkRelError2;
38 btMprCollisionDescription()
39 : m_firstDir(0, 1, 0),
40 m_maxGjkIterations(1000),
41 m_maximumDistanceSquared(1e30f),
42 m_gjkRelError2(1.0e-6)
45 virtual ~btMprCollisionDescription()
50 struct btMprDistanceInfo
54 btVector3 m_normalBtoA;
59 #define BT_MPR_SQRT sqrtf
61 #define BT_MPR_SQRT sqrt
63 #define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
64 #define BT_MPR_FABS fabs
66 #define BT_MPR_TOLERANCE 1E-6f
67 #define BT_MPR_MAX_ITERATIONS 1000
69 struct _btMprSupport_t
71 btVector3 v; //!< Support point in minkowski sum
72 btVector3 v1; //!< Support point in obj1
73 btVector3 v2; //!< Support point in obj2
75 typedef struct _btMprSupport_t btMprSupport_t;
77 struct _btMprSimplex_t
80 int last; //!< index of last added point
82 typedef struct _btMprSimplex_t btMprSimplex_t;
84 inline btMprSupport_t *btMprSimplexPointW(btMprSimplex_t *s, int idx)
89 inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
95 inline void btPrintPortalVertex(_btMprSimplex_t *portal, int index)
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());
103 inline int btMprSimplexSize(const btMprSimplex_t *s)
108 inline const btMprSupport_t *btMprSimplexPoint(const btMprSimplex_t *s, int idx)
110 // here is no check on boundaries
114 inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
119 inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
121 btMprSupportCopy(s->ps + pos, a);
124 inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
128 btMprSupportCopy(&supp, &s->ps[pos1]);
129 btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
130 btMprSupportCopy(&s->ps[pos2], &supp);
133 inline int btMprIsZero(float val)
135 return BT_MPR_FABS(val) < FLT_EPSILON;
138 inline int btMprEq(float _a, float _b)
143 ab = BT_MPR_FABS(_a - _b);
144 if (BT_MPR_FABS(ab) < FLT_EPSILON)
151 return ab < FLT_EPSILON * b;
155 return ab < FLT_EPSILON * a;
159 inline int btMprVec3Eq(const btVector3 *a, const btVector3 *b)
161 return btMprEq((*a).x(), (*b).x()) && btMprEq((*a).y(), (*b).y()) && btMprEq((*a).z(), (*b).z());
164 template <typename btConvexTemplate>
165 inline void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center)
167 center->v1 = a.getObjectCenterInWorld();
168 center->v2 = b.getObjectCenterInWorld();
169 center->v = center->v1 - center->v2;
172 inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
174 v->setValue(x, y, z);
177 inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
182 inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
187 inline void btMprVec3Scale(btVector3 *d, float k)
192 inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
200 inline float btMprVec3Len2(const btVector3 *v)
202 return btMprVec3Dot(v, v);
205 inline void btMprVec3Normalize(btVector3 *d)
207 float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
208 btMprVec3Scale(d, k);
211 inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
213 *d = btCross(*a, *b);
216 inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
221 inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
223 btVector3 v2v1, v3v1;
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);
233 inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
234 const btVector3 *dir)
237 dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
238 return btMprIsZero(dot) || dot > 0.f;
241 inline int portalReachTolerance(const btMprSimplex_t *portal,
242 const btMprSupport_t *v4,
243 const btVector3 *dir)
245 float dv1, dv2, dv3, dv4;
246 float dot1, dot2, dot3;
248 // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
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);
259 dot1 = BT_MPR_FMIN(dot1, dot2);
260 dot1 = BT_MPR_FMIN(dot1, dot3);
262 return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
265 inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
266 const btMprSupport_t *v4,
267 const btVector3 *dir)
270 dot = btMprVec3Dot(&v4->v, dir);
271 return btMprIsZero(dot) || dot > 0.f;
274 inline void btExpandPortal(btMprSimplex_t *portal,
275 const btMprSupport_t *v4)
280 btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
281 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
284 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
287 btMprSimplexSet(portal, 1, v4);
291 btMprSimplexSet(portal, 3, v4);
296 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
299 btMprSimplexSet(portal, 2, v4);
303 btMprSimplexSet(portal, 1, v4);
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)
312 btVector3 separatingAxisInA = dir * a.getWorldTransform().getBasis();
313 btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis();
315 btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA);
316 btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB);
318 supp->v1 = a.getWorldTransform()(pInA);
319 supp->v2 = b.getWorldTransform()(qInB);
320 supp->v = supp->v1 - supp->v2;
323 template <typename btConvexTemplate>
324 static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b,
325 const btMprCollisionDescription &colDesc,
326 btMprSimplex_t *portal)
328 btVector3 dir, va, vb;
332 // vertex 0 is center of portal
333 btFindOrigin(a, b, colDesc, btMprSimplexPointW(portal, 0));
335 // vertex 0 is center of portal
336 btMprSimplexSetSize(portal, 1);
338 btVector3 zero = btVector3(0, 0, 0);
339 btVector3 *org = &zero;
341 if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org))
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);
350 // vertex 1 = support in direction of origin
351 btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
352 btMprVec3Scale(&dir, -1.f);
353 btMprVec3Normalize(&dir);
355 btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 1));
357 btMprSimplexSetSize(portal, 2);
359 // test if origin isn't outside of v1
360 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);
362 if (btMprIsZero(dot) || dot < 0.f)
366 btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
367 &btMprSimplexPoint(portal, 1)->v);
368 if (btMprIsZero(btMprVec3Len2(&dir)))
370 if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org))
377 // origin lies on v0-v1 segment
382 btMprVec3Normalize(&dir);
383 btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 2));
385 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
386 if (btMprIsZero(dot) || dot < 0.f)
389 btMprSimplexSetSize(portal, 3);
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);
399 // it is better to form portal faces to be oriented "outside" origin
400 dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
403 btMprSimplexSwap(portal, 1, 2);
404 btMprVec3Scale(&dir, -1.f);
407 while (btMprSimplexSize(portal) < 4)
409 btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 3));
411 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
412 if (btMprIsZero(dot) || dot < 0.f)
417 // test if origin is outside (v1, v0, v3) - set v2 as v3 and
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))
424 btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
430 // test if origin is outside (v3, v0, v2) - set v1 as v3 and
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))
437 btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
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);
453 btMprSimplexSetSize(portal, 4);
460 template <typename btConvexTemplate>
461 static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc,
462 btMprSimplex_t *portal)
467 for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
470 // compute direction outside the portal (from v0 through v1,v2,v3
472 btPortalDir(portal, &dir);
474 // test if origin is inside the portal
475 if (portalEncapsulesOrigin(portal, &dir))
478 // get next support point
480 btMprSupport(a, b, colDesc, dir, &v4);
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))
489 // v1-v2-v3 triangle must be rearranged to face outside Minkowski
490 // difference (direction from v0).
491 btExpandPortal(portal, &v4);
497 static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
499 btVector3 zero = btVector3(0, 0, 0);
500 btVector3 *origin = &zero;
504 float b[4], sum, inv;
505 btVector3 vec, p1, p2;
507 btPortalDir(portal, &dir);
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);
514 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
515 &btMprSimplexPoint(portal, 2)->v);
516 b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
518 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
519 &btMprSimplexPoint(portal, 1)->v);
520 b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
522 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
523 &btMprSimplexPoint(portal, 1)->v);
524 b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
526 sum = b[0] + b[1] + b[2] + b[3];
528 if (btMprIsZero(sum) || sum < 0.f)
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);
542 sum = b[1] + b[2] + b[3];
547 btMprVec3Copy(&p1, origin);
548 btMprVec3Copy(&p2, origin);
549 for (i = 0; i < 4; i++)
551 btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
552 btMprVec3Scale(&vec, b[i]);
553 btMprVec3Add(&p1, &vec);
555 btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
556 btMprVec3Scale(&vec, b[i]);
557 btMprVec3Add(&p2, &vec);
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);
566 btMprVec3Copy(pos, &p2);
567 #endif //MPR_AVERAGE_CONTACT_POSITIONS
570 inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
573 btMprVec3Sub2(&ab, a, b);
574 return btMprVec3Len2(&ab);
577 inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
582 // The computation comes from solving equation of segment:
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
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.
595 // Bonus of this method is witness point for free.
600 // direction of segment
601 btMprVec3Sub2(&d, b, x0);
603 // precompute vector from P to x0
604 btMprVec3Sub2(&a, x0, P);
606 t = -1.f * btMprVec3Dot(&a, &d);
607 t /= btMprVec3Len2(&d);
609 if (t < 0.f || btMprIsZero(t))
611 dist = btMprVec3Dist2(x0, P);
613 btMprVec3Copy(witness, x0);
615 else if (t > 1.f || btMprEq(t, 1.f))
617 dist = btMprVec3Dist2(b, P);
619 btMprVec3Copy(witness, b);
625 btMprVec3Copy(witness, &d);
626 btMprVec3Scale(witness, t);
627 btMprVec3Add(witness, x0);
628 dist = btMprVec3Dist2(witness, P);
632 // recycling variables
633 btMprVec3Scale(&d, t);
634 btMprVec3Add(&d, &a);
635 dist = btMprVec3Len2(&d);
642 inline float btMprVec3PointTriDist2(const btVector3 *P,
643 const btVector3 *x0, const btVector3 *B,
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
657 float u, v, w, p, q, r;
658 float s, t, dist, dist2;
661 btMprVec3Sub2(&d1, B, x0);
662 btMprVec3Sub2(&d2, C, x0);
663 btMprVec3Sub2(&a, x0, P);
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);
672 btScalar div = (w * v - r * r);
673 if (btMprIsZero(div))
679 s = (q * r - w * p) / div;
680 t = (-s * r - q) / w;
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))
687 btMprVec3Scale(&d1, s);
688 btMprVec3Scale(&d2, t);
689 btMprVec3Copy(witness, x0);
690 btMprVec3Add(witness, &d1);
691 btMprVec3Add(witness, &d2);
693 dist = btMprVec3Dist2(witness, P);
699 dist += 2.f * s * t * r;
707 dist = _btMprVec3PointSegmentDist2(P, x0, B, witness);
709 dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
714 btMprVec3Copy(witness, &witness2);
717 dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
722 btMprVec3Copy(witness, &witness2);
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)
737 unsigned long iterations;
739 btVector3 zero = btVector3(0, 0, 0);
740 btVector3 *origin = &zero;
743 for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
746 // compute portal direction and obtain next support point
747 btPortalDir(portal, &dir);
749 btMprSupport(a, b, colDesc, dir, &v4);
751 // reached tolerance -> find penetration info
752 if (portalReachTolerance(portal, &v4, &dir) || iterations == BT_MPR_MAX_ITERATIONS)
754 *depth = btMprVec3PointTriDist2(origin, &btMprSimplexPoint(portal, 1)->v, &btMprSimplexPoint(portal, 2)->v, &btMprSimplexPoint(portal, 3)->v, pdir);
755 *depth = BT_MPR_SQRT(*depth);
757 if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
761 btMprVec3Normalize(pdir);
763 // barycentric coordinates:
764 btFindPos(portal, pos);
769 btExpandPortal(portal, &v4);
775 static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
777 // Touching contact on portal's v1 - so depth is zero and direction
778 // is unimportant and pos can be guessed
780 btVector3 zero = btVector3(0, 0, 0);
781 btVector3 *origin = &zero;
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);
789 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
793 static void btFindPenetrSegment(btMprSimplex_t *portal,
794 float *depth, btVector3 *dir, btVector3 *pos)
796 // Origin lies on v0-v1 segment.
797 // Depth is distance to v1, direction also and position must be
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);
804 btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
805 #endif //MPR_AVERAGE_CONTACT_POSITIONS
807 btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
808 *depth = BT_MPR_SQRT(btMprVec3Len2(dir));
809 btMprVec3Normalize(dir);
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)
817 btMprSimplex_t portal;
819 // Phase 1: Portal discovery
820 int result = btDiscoverPortal(a, b, colDesc, &portal);
822 //sepAxis[pairIndex] = *pdir;//or -dir?
828 // Phase 2: Portal refinement
830 result = btRefinePortal(a, b, colDesc, &portal);
834 // Phase 3. Penetration info
835 btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut);
841 // Touching contact on portal's v1.
842 btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
848 btFindPenetrSegment(&portal, depthOut, dirOut, posOut);
856 // Origin isn't inside portal - no collision.
865 template <typename btConvexTemplate, typename btMprDistanceTemplate>
866 inline int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
871 int res = btMprPenetration(a, b, colDesc, &depth, &dir, &pos);
874 distInfo->m_distance = -depth;
875 distInfo->m_pointOnB = pos;
876 distInfo->m_normalBtoA = -dir;
877 distInfo->m_pointOnA = pos - distInfo->m_distance * dir;
884 #endif //BT_MPR_PENETRATION_H