summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp')
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp494
1 files changed, 233 insertions, 261 deletions
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index a0b825f0e8..803f6e0671 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -18,75 +18,69 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
-
-
-#if defined(DEBUG) || defined (_DEBUG)
+#if defined(DEBUG) || defined(_DEBUG)
//#define TEST_NON_VIRTUAL 1
-#include <stdio.h> //for debug printf
+#include <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
-#endif //__SPU__
+#endif //__SPU__
#endif
//must be above the machine epsilon
-#ifdef BT_USE_DOUBLE_PRECISION
- #define REL_ERROR2 btScalar(1.0e-12)
- btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define REL_ERROR2 btScalar(1.0e-12)
+btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
#else
- #define REL_ERROR2 btScalar(1.0e-6)
- btScalar gGjkEpaPenetrationTolerance = 0.001;
+#define REL_ERROR2 btScalar(1.0e-6)
+btScalar gGjkEpaPenetrationTolerance = 0.001;
#endif
-//temp globals, to improve GJK/EPA/penetration calculations
-int gNumDeepPenetrationChecks = 0;
-int gNumGjkChecks = 0;
-
-
-btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_simplexSolver(simplexSolver),
-m_minkowskiA(objectA),
-m_minkowskiB(objectB),
-m_shapeTypeA(objectA->getShapeType()),
-m_shapeTypeB(objectB->getShapeType()),
-m_marginA(objectA->getMargin()),
-m_marginB(objectB->getMargin()),
-m_ignoreMargin(false),
-m_lastUsedMethod(-1),
-m_catchDegeneracies(1),
-m_fixContactNormalDirection(1)
+
+btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
+ : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
+ m_penetrationDepthSolver(penetrationDepthSolver),
+ m_simplexSolver(simplexSolver),
+ m_minkowskiA(objectA),
+ m_minkowskiB(objectB),
+ m_shapeTypeA(objectA->getShapeType()),
+ m_shapeTypeB(objectB->getShapeType()),
+ m_marginA(objectA->getMargin()),
+ m_marginB(objectB->getMargin()),
+ m_ignoreMargin(false),
+ m_lastUsedMethod(-1),
+ m_catchDegeneracies(1),
+ m_fixContactNormalDirection(1)
{
}
-btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_simplexSolver(simplexSolver),
-m_minkowskiA(objectA),
-m_minkowskiB(objectB),
-m_shapeTypeA(shapeTypeA),
-m_shapeTypeB(shapeTypeB),
-m_marginA(marginA),
-m_marginB(marginB),
-m_ignoreMargin(false),
-m_lastUsedMethod(-1),
-m_catchDegeneracies(1),
-m_fixContactNormalDirection(1)
+btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
+ : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
+ m_penetrationDepthSolver(penetrationDepthSolver),
+ m_simplexSolver(simplexSolver),
+ m_minkowskiA(objectA),
+ m_minkowskiB(objectB),
+ m_shapeTypeA(shapeTypeA),
+ m_shapeTypeB(shapeTypeB),
+ m_marginA(marginA),
+ m_marginB(marginB),
+ m_ignoreMargin(false),
+ m_lastUsedMethod(-1),
+ m_catchDegeneracies(1),
+ m_fixContactNormalDirection(1)
{
}
-void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults)
{
(void)swapResults;
- getClosestPointsNonVirtual(input,output,debugDraw);
+ getClosestPointsNonVirtual(input, output, debugDraw);
}
-static void btComputeSupport(const btConvexShape* convexA, const btTransform& localTransA, const btConvexShape* convexB, const btTransform& localTransB, const btVector3& dir, bool check2d, btVector3& supAworld, btVector3& supBworld, btVector3& aMinb)
+static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb)
{
- btVector3 seperatingAxisInA = (dir)* localTransA.getBasis();
- btVector3 seperatingAxisInB = (-dir)* localTransB.getBasis();
+ btVector3 seperatingAxisInA = (dir)*localTransA.getBasis();
+ btVector3 seperatingAxisInB = (-dir) * localTransB.getBasis();
btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
@@ -106,22 +100,21 @@ static void btComputeSupport(const btConvexShape* convexA, const btTransform& lo
aMinb = supAworld - supBworld;
}
-struct btSupportVector
+struct btSupportVector
{
- btVector3 v; //!< Support point in minkowski sum
- btVector3 v1; //!< Support point in obj1
- btVector3 v2; //!< Support point in obj2
+ btVector3 v; //!< Support point in minkowski sum
+ btVector3 v1; //!< Support point in obj1
+ btVector3 v2; //!< Support point in obj2
};
-struct btSimplex
+struct btSimplex
{
btSupportVector ps[4];
- int last; //!< index of last added point
+ int last; //!< index of last added point
};
static btVector3 ccd_vec3_origin(0, 0, 0);
-
inline void btSimplexInit(btSimplex *s)
{
s->last = -1;
@@ -142,19 +135,18 @@ inline void btSupportCopy(btSupportVector *d, const btSupportVector *s)
*d = *s;
}
-inline void btVec3Copy(btVector3 *v, const btVector3* w)
+inline void btVec3Copy(btVector3 *v, const btVector3 *w)
{
*v = *w;
}
-inline void ccdVec3Add(btVector3*v, const btVector3*w)
+inline void ccdVec3Add(btVector3 *v, const btVector3 *w)
{
v->m_floats[0] += w->m_floats[0];
v->m_floats[1] += w->m_floats[1];
v->m_floats[2] += w->m_floats[2];
}
-
inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
{
*v -= *w;
@@ -162,24 +154,22 @@ inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
{
*d = (*v) - (*w);
-
}
-inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
+inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
{
btScalar dot;
dot = a->dot(*b);
-
+
return dot;
}
-inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3*b)
+inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b)
{
btVector3 ab;
btVec3Sub2(&ab, a, b);
return btVec3Dot(&ab, &ab);
}
-
inline void btVec3Scale(btVector3 *d, btScalar k)
{
d->m_floats[0] *= k;
@@ -195,7 +185,7 @@ inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
}
inline void btTripleCross(const btVector3 *a, const btVector3 *b,
- const btVector3 *c, btVector3 *d)
+ const btVector3 *c, btVector3 *d)
{
btVector3 e;
btVec3Cross(&e, a, b);
@@ -213,36 +203,35 @@ inline int ccdEq(btScalar _a, btScalar _b)
a = btFabs(_a);
b = btFabs(_b);
- if (b > a) {
+ if (b > a)
+ {
return ab < SIMD_EPSILON * b;
}
- else {
+ else
+ {
return ab < SIMD_EPSILON * a;
}
}
-btScalar ccdVec3X(const btVector3* v)
+btScalar ccdVec3X(const btVector3 *v)
{
return v->x();
}
-btScalar ccdVec3Y(const btVector3* v)
+btScalar ccdVec3Y(const btVector3 *v)
{
return v->y();
}
-btScalar ccdVec3Z(const btVector3* v)
+btScalar ccdVec3Z(const btVector3 *v)
{
return v->z();
}
inline int btVec3Eq(const btVector3 *a, const btVector3 *b)
{
- return ccdEq(ccdVec3X(a), ccdVec3X(b))
- && ccdEq(ccdVec3Y(a), ccdVec3Y(b))
- && ccdEq(ccdVec3Z(a), ccdVec3Z(b));
+ return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b));
}
-
inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
{
// here is no check on boundaries in sake of speed
@@ -250,7 +239,6 @@ inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
btSupportCopy(s->ps + s->last, v);
}
-
inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
{
btSupportCopy(s->ps + pos, a);
@@ -268,27 +256,28 @@ inline const btSupportVector *ccdSimplexLast(const btSimplex *s)
inline int ccdSign(btScalar val)
{
- if (btFuzzyZero(val)) {
+ if (btFuzzyZero(val))
+ {
return 0;
}
- else if (val < btScalar(0)) {
+ else if (val < btScalar(0))
+ {
return -1;
}
return 1;
}
-
inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
- const btVector3 *x0,
- const btVector3 *b,
- btVector3 *witness)
+ const btVector3 *x0,
+ const btVector3 *b,
+ btVector3 *witness)
{
// The computation comes from solving equation of segment:
// S(t) = x0 + t.d
// where - x0 is initial point of segment
// - d is direction of segment from x0 (|d| > 0)
// - t belongs to <0, 1> interval
- //
+ //
// Than, distance from a segment to some point P can be expressed:
// D(t) = |x0 + t.d - P|^2
// which is distance from any point on segment. Minimization
@@ -310,24 +299,29 @@ inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
t = -btScalar(1.) * btVec3Dot(&a, &d);
t /= btVec3Dot(&d, &d);
- if (t < btScalar(0) || btFuzzyZero(t)) {
+ if (t < btScalar(0) || btFuzzyZero(t))
+ {
dist = ccdVec3Dist2(x0, P);
if (witness)
btVec3Copy(witness, x0);
}
- else if (t > btScalar(1) || ccdEq(t, btScalar(1))) {
+ else if (t > btScalar(1) || ccdEq(t, btScalar(1)))
+ {
dist = ccdVec3Dist2(b, P);
if (witness)
btVec3Copy(witness, b);
}
- else {
- if (witness) {
+ else
+ {
+ if (witness)
+ {
btVec3Copy(witness, &d);
btVec3Scale(witness, t);
ccdVec3Add(witness, x0);
dist = ccdVec3Dist2(witness, P);
}
- else {
+ else
+ {
// recycling variables
btVec3Scale(&d, t);
ccdVec3Add(&d, &a);
@@ -338,11 +332,10 @@ inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
return dist;
}
-
btScalar btVec3PointTriDist2(const btVector3 *P,
- const btVector3 *x0, const btVector3 *B,
- const btVector3 *C,
- btVector3 *witness)
+ const btVector3 *x0, const btVector3 *B,
+ const btVector3 *C,
+ btVector3 *witness)
{
// Computation comes from analytic expression for triangle (x0, B, C)
// T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
@@ -372,13 +365,9 @@ btScalar btVec3PointTriDist2(const btVector3 *P,
s = (q * r - w * p) / (w * v - r * r);
t = (-s * r - q) / w;
- if ((btFuzzyZero(s) || s > btScalar(0))
- && (ccdEq(s, btScalar(1)) || s < btScalar(1))
- && (btFuzzyZero(t) || t > btScalar(0))
- && (ccdEq(t, btScalar(1)) || t < btScalar(1))
- && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) {
-
- if (witness)
+ if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1)))
+ {
+ if (witness)
{
btVec3Scale(&d1, s);
btVec3Scale(&d2, t);
@@ -388,7 +377,7 @@ btScalar btVec3PointTriDist2(const btVector3 *P,
dist = ccdVec3Dist2(witness, P);
}
- else
+ else
{
dist = s * s * v;
dist += t * t * w;
@@ -398,18 +387,21 @@ btScalar btVec3PointTriDist2(const btVector3 *P,
dist += u;
}
}
- else {
+ else
+ {
dist = btVec3PointSegmentDist2(P, x0, B, witness);
dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2);
- if (dist2 < dist) {
+ if (dist2 < dist)
+ {
dist = dist2;
if (witness)
btVec3Copy(witness, &witness2);
}
dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
- if (dist2 < dist) {
+ if (dist2 < dist)
+ {
dist = dist2;
if (witness)
btVec3Copy(witness, &witness2);
@@ -419,7 +411,6 @@ btScalar btVec3PointTriDist2(const btVector3 *P,
return dist;
}
-
static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
{
const btSupportVector *A, *B;
@@ -441,18 +432,21 @@ static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
// check if origin doesn't lie on AB segment
btVec3Cross(&tmp, &AB, &AO);
- if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) {
+ if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0))
+ {
return 1;
}
// check if origin is in area where AB segment is
- if (btFuzzyZero(dot) || dot < btScalar(0)) {
+ if (btFuzzyZero(dot) || dot < btScalar(0))
+ {
// origin is in outside are of A
btSimplexSet(simplex, 0, A);
btSimplexSetSize(simplex, 1);
btVec3Copy(dir, &AO);
}
- else {
+ else
+ {
// origin is in area where AB segment is
// keep simplex untouched and set direction to
@@ -463,8 +457,6 @@ static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
return 0;
}
-
-
static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
{
const btSupportVector *A, *B, *C;
@@ -479,13 +471,15 @@ static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
// check touching contact
dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
- if (btFuzzyZero(dist)) {
+ if (btFuzzyZero(dist))
+ {
return 1;
}
// check if triangle is really triangle (has area > 0)
// if not simplex can't be expanded and thus no itersection is found
- if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) {
+ if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v))
+ {
return -1;
}
@@ -500,54 +494,64 @@ static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
btVec3Cross(&tmp, &ABC, &AC);
dot = btVec3Dot(&tmp, &AO);
- if (btFuzzyZero(dot) || dot > btScalar(0)) {
+ if (btFuzzyZero(dot) || dot > btScalar(0))
+ {
dot = btVec3Dot(&AC, &AO);
- if (btFuzzyZero(dot) || dot > btScalar(0)) {
+ if (btFuzzyZero(dot) || dot > btScalar(0))
+ {
// C is already in place
btSimplexSet(simplex, 1, A);
btSimplexSetSize(simplex, 2);
btTripleCross(&AC, &AO, &AC, dir);
}
- else {
-
+ else
+ {
dot = btVec3Dot(&AB, &AO);
- if (btFuzzyZero(dot) || dot > btScalar(0)) {
+ if (btFuzzyZero(dot) || dot > btScalar(0))
+ {
btSimplexSet(simplex, 0, B);
btSimplexSet(simplex, 1, A);
btSimplexSetSize(simplex, 2);
btTripleCross(&AB, &AO, &AB, dir);
}
- else {
+ else
+ {
btSimplexSet(simplex, 0, A);
btSimplexSetSize(simplex, 1);
btVec3Copy(dir, &AO);
}
}
}
- else {
+ else
+ {
btVec3Cross(&tmp, &AB, &ABC);
dot = btVec3Dot(&tmp, &AO);
- if (btFuzzyZero(dot) || dot > btScalar(0))
+ if (btFuzzyZero(dot) || dot > btScalar(0))
{
dot = btVec3Dot(&AB, &AO);
- if (btFuzzyZero(dot) || dot > btScalar(0)) {
+ if (btFuzzyZero(dot) || dot > btScalar(0))
+ {
btSimplexSet(simplex, 0, B);
btSimplexSet(simplex, 1, A);
btSimplexSetSize(simplex, 2);
btTripleCross(&AB, &AO, &AB, dir);
}
- else {
+ else
+ {
btSimplexSet(simplex, 0, A);
btSimplexSetSize(simplex, 1);
btVec3Copy(dir, &AO);
}
}
- else {
+ else
+ {
dot = btVec3Dot(&ABC, &AO);
- if (btFuzzyZero(dot) || dot > btScalar(0)) {
+ if (btFuzzyZero(dot) || dot > btScalar(0))
+ {
btVec3Copy(dir, &ABC);
}
- else {
+ else
+ {
btSupportVector tmp;
btSupportCopy(&tmp, C);
btSimplexSet(simplex, 0, B);
@@ -581,7 +585,8 @@ static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
// if it is not simplex can't be expanded and thus no intersection is
// found
dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
- if (btFuzzyZero(dist)) {
+ if (btFuzzyZero(dist))
+ {
return -1;
}
@@ -622,12 +627,14 @@ static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;
AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;
- if (AB_O && AC_O && AD_O) {
+ if (AB_O && AC_O && AD_O)
+ {
// origin is in tetrahedron
return 1;
// rearrange simplex to triangle and call btDoSimplex3()
}
- else if (!AB_O) {
+ else if (!AB_O)
+ {
// B is farthest from the origin among all of the tetrahedron's
// points, so remove it from the list and go on with the triangle
// case
@@ -636,14 +643,16 @@ static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
btSimplexSet(simplex, 2, A);
btSimplexSetSize(simplex, 3);
}
- else if (!AC_O) {
+ else if (!AC_O)
+ {
// C is farthest
btSimplexSet(simplex, 1, D);
btSimplexSet(simplex, 0, B);
btSimplexSet(simplex, 2, A);
btSimplexSetSize(simplex, 3);
}
- else { // (!AD_O)
+ else
+ { // (!AD_O)
btSimplexSet(simplex, 0, C);
btSimplexSet(simplex, 1, B);
btSimplexSet(simplex, 2, A);
@@ -655,36 +664,39 @@ static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
{
- if (btSimplexSize(simplex) == 2) {
+ if (btSimplexSize(simplex) == 2)
+ {
// simplex contains segment only one segment
return btDoSimplex2(simplex, dir);
}
- else if (btSimplexSize(simplex) == 3) {
+ else if (btSimplexSize(simplex) == 3)
+ {
// simplex contains triangle
return btDoSimplex3(simplex, dir);
}
- else { // btSimplexSize(simplex) == 4
- // tetrahedron - this is the only shape which can encapsule origin
- // so btDoSimplex4() also contains test on it
+ else
+ { // btSimplexSize(simplex) == 4
+ // tetrahedron - this is the only shape which can encapsule origin
+ // so btDoSimplex4() also contains test on it
return btDoSimplex4(simplex, dir);
}
}
#ifdef __SPU__
-void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
#else
-void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
#endif
{
m_cachedSeparatingDistance = 0.f;
- btScalar distance=btScalar(0.);
- btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+ btScalar distance = btScalar(0.);
+ btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
- btVector3 pointOnA,pointOnB;
- btTransform localTransA = input.m_transformA;
+ btVector3 pointOnA, pointOnB;
+ btTransform localTransA = input.m_transformA;
btTransform localTransB = input.m_transformB;
- btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
+ btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
localTransA.getOrigin() -= positionOffset;
localTransB.getOrigin() -= positionOffset;
@@ -693,7 +705,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
btScalar marginA = m_marginA;
btScalar marginB = m_marginB;
- gNumGjkChecks++;
//for CCD we don't use margins
if (m_ignoreMargin)
@@ -703,19 +714,19 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
}
m_curIter = 0;
- int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
- m_cachedSeparatingAxis.setValue(0,1,0);
+ int gGjkMaxIter = 1000; //this is to catch invalid input, perhaps check for #NaN?
+ m_cachedSeparatingAxis.setValue(0, 1, 0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
-
+
m_lastUsedMethod = -1;
int status = -2;
btVector3 orgNormalInB(0, 0, 0);
btScalar margin = marginA + marginB;
-
+
//we add a separate implementation to check if the convex shapes intersect
//See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang
//Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver
@@ -726,22 +737,18 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
btScalar squaredDistance = BT_LARGE_FLOAT;
btScalar delta = btScalar(0.);
-
-
-
btSimplex simplex1;
- btSimplex* simplex = &simplex1;
+ btSimplex *simplex = &simplex1;
btSimplexInit(simplex);
btVector3 dir(1, 0, 0);
{
-
btVector3 lastSupV;
btVector3 supAworld;
btVector3 supBworld;
btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
-
+
btSupportVector last;
last.v = lastSupV;
last.v1 = supAworld;
@@ -751,10 +758,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
dir = -lastSupV;
-
-
// start iterations
- for (int iterations = 0; iterations <gGjkMaxIter; iterations++)
+ for (int iterations = 0; iterations < gGjkMaxIter; iterations++)
{
// obtain support point
btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
@@ -769,7 +774,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
status = -1;
break;
}
-
+
// add last support vector to simplex
last.v = lastSupV;
last.v1 = supAworld;
@@ -781,21 +786,21 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
// intersect and 0 if algorithm should continue
btVector3 newDir;
- int do_simplex_res = btDoSimplex(simplex, &dir);
+ int do_simplex_res = btDoSimplex(simplex, &dir);
- if (do_simplex_res == 1)
+ if (do_simplex_res == 1)
{
- status = 0; // intersection found
+ status = 0; // intersection found
break;
}
- else if (do_simplex_res == -1)
+ else if (do_simplex_res == -1)
{
// intersection not found
status = -1;
break;
}
-
- if (btFuzzyZero(btVec3Dot(&dir, &dir)))
+
+ if (btFuzzyZero(btVec3Dot(&dir, &dir)))
{
// intersection not found
status = -1;
@@ -815,7 +820,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
break;
}
}
-
}
m_simplexSolver->reset();
@@ -825,27 +829,24 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
//printf("Intersect!\n");
}
- if (status==-1)
+ if (status == -1)
{
//printf("not intersect\n");
}
//printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
if (1)
{
- for (; ; )
- //while (true)
+ for (;;)
+ //while (true)
{
-
- btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
- btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
-
+ btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
+ btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
- btVector3 pWorld = localTransA(pInA);
- btVector3 qWorld = localTransB(qInB);
-
+ btVector3 pWorld = localTransA(pInA);
+ btVector3 qWorld = localTransB(qInB);
if (check2d)
{
@@ -921,8 +922,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
checkSimplex = false;
break;
}
-#endif //
-
+#endif //
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
@@ -938,33 +938,31 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
m_cachedSeparatingAxis = newCachedSeparatingAxis;
- //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
+ //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter)
{
-#if defined(DEBUG) || defined (_DEBUG)
+#if defined(DEBUG) || defined(_DEBUG)
printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
- m_cachedSeparatingAxis.getX(),
- m_cachedSeparatingAxis.getY(),
- m_cachedSeparatingAxis.getZ(),
- squaredDistance,
- m_minkowskiA->getShapeType(),
- m_minkowskiB->getShapeType());
-
-#endif
- break;
+ m_cachedSeparatingAxis.getX(),
+ m_cachedSeparatingAxis.getY(),
+ m_cachedSeparatingAxis.getZ(),
+ squaredDistance,
+ m_minkowskiA->getShapeType(),
+ m_minkowskiB->getShapeType());
+#endif
+ break;
}
-
bool check = (!m_simplexSolver->fullSimplex());
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
- // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+ // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
@@ -972,20 +970,20 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
if (checkSimplex)
{
- m_simplexSolver->compute_points(pointOnA, pointOnB);
- normalInB = m_cachedSeparatingAxis;
+ m_simplexSolver->compute_points(pointOnA, pointOnB);
+ normalInB = m_cachedSeparatingAxis;
+
+ btScalar lenSqr = m_cachedSeparatingAxis.length2();
- btScalar lenSqr =m_cachedSeparatingAxis.length2();
-
//valid normal
if (lenSqr < REL_ERROR2)
{
m_degenerateSimplex = 5;
}
- if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+ if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
- normalInB *= rlen; //normalize
+ normalInB *= rlen; //normalize
btScalar s = btSqrt(squaredDistance);
@@ -1005,13 +1003,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
}
}
-
-
- bool catchDegeneratePenetrationCase =
- (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
+ bool catchDegeneratePenetrationCase =
+ (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < gGjkEpaPenetrationTolerance));
//if (checkPenetration && !isValid)
- if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase )) || (status == 0))
+ if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
{
//penetration case
@@ -1019,19 +1015,16 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
if (m_penetrationDepthSolver)
{
// Penetration depth case.
- btVector3 tmpPointOnA,tmpPointOnB;
-
- gNumDeepPenetrationChecks++;
+ btVector3 tmpPointOnA, tmpPointOnB;
+
m_cachedSeparatingAxis.setZero();
- bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
- *m_simplexSolver,
- m_minkowskiA,m_minkowskiB,
- localTransA,localTransB,
+ bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
+ *m_simplexSolver,
+ m_minkowskiA, m_minkowskiB,
+ localTransA, localTransB,
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
- debugDraw
- );
-
+ debugDraw);
if (m_cachedSeparatingAxis.length2())
{
@@ -1039,13 +1032,13 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
{
btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
btScalar lenSqr = tmpNormalInB.length2();
- if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+ if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.length2();
}
- if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+ if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
{
tmpNormalInB /= btSqrt(lenSqr);
btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
@@ -1058,7 +1051,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
isValid = true;
-
}
else
{
@@ -1079,7 +1071,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
-
if (m_cachedSeparatingAxis.length2() > btScalar(0.))
{
btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
@@ -1103,109 +1094,90 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
}
}
}
- } else
+ }
+ else
{
//printf("EPA didn't return a valid value\n");
}
-
}
-
}
}
-
-
- if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
+ if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
{
-
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
if (1)
{
- ///todo: need to track down this EPA penetration solver degeneracy
- ///the penetration solver reports penetration but the contact normal
- ///connecting the contact points is pointing in the opposite direction
- ///until then, detect the issue and revert the normal
+ ///todo: need to track down this EPA penetration solver degeneracy
+ ///the penetration solver reports penetration but the contact normal
+ ///connecting the contact points is pointing in the opposite direction
+ ///until then, detect the issue and revert the normal
btScalar d2 = 0.f;
{
- btVector3 seperatingAxisInA = (-orgNormalInB)* localTransA.getBasis();
- btVector3 seperatingAxisInB = orgNormalInB* localTransB.getBasis();
-
+ btVector3 seperatingAxisInA = (-orgNormalInB) * localTransA.getBasis();
+ btVector3 seperatingAxisInB = orgNormalInB * localTransB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
- btVector3 pWorld = localTransA(pInA);
- btVector3 qWorld = localTransB(qInB);
+ btVector3 pWorld = localTransA(pInA);
+ btVector3 qWorld = localTransB(qInB);
btVector3 w = pWorld - qWorld;
- d2 = orgNormalInB.dot(w)- margin;
+ d2 = orgNormalInB.dot(w) - margin;
}
-
- btScalar d1=0;
+
+ btScalar d1 = 0;
{
-
- btVector3 seperatingAxisInA = (normalInB)* localTransA.getBasis();
- btVector3 seperatingAxisInB = -normalInB* localTransB.getBasis();
-
+ btVector3 seperatingAxisInA = (normalInB)*localTransA.getBasis();
+ btVector3 seperatingAxisInB = -normalInB * localTransB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
- btVector3 pWorld = localTransA(pInA);
- btVector3 qWorld = localTransB(qInB);
- btVector3 w = pWorld - qWorld;
- d1 = (-normalInB).dot(w)- margin;
-
+ btVector3 pWorld = localTransA(pInA);
+ btVector3 qWorld = localTransB(qInB);
+ btVector3 w = pWorld - qWorld;
+ d1 = (-normalInB).dot(w) - margin;
}
btScalar d0 = 0.f;
{
- btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
- btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
-
+ btVector3 seperatingAxisInA = (-normalInB) * input.m_transformA.getBasis();
+ btVector3 seperatingAxisInB = normalInB * input.m_transformB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
- btVector3 pWorld = localTransA(pInA);
- btVector3 qWorld = localTransB(qInB);
- btVector3 w = pWorld - qWorld;
- d0 = normalInB.dot(w)-margin;
+ btVector3 pWorld = localTransA(pInA);
+ btVector3 qWorld = localTransB(qInB);
+ btVector3 w = pWorld - qWorld;
+ d0 = normalInB.dot(w) - margin;
}
-
- if (d1>d0)
+
+ if (d1 > d0)
{
m_lastUsedMethod = 10;
- normalInB*=-1;
- }
+ normalInB *= -1;
+ }
if (orgNormalInB.length2())
{
if (d2 > d0 && d2 > d1 && d2 > distance)
{
-
normalInB = orgNormalInB;
distance = d2;
}
}
}
-
output.addContactPoint(
normalInB,
- pointOnB+positionOffset,
+ pointOnB + positionOffset,
distance);
-
}
else
{
//printf("invalid gjk query\n");
}
-
-
}
-
-
-
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-23 08:39:39 +0000