21#if defined(DEBUG) || defined(_DEBUG)
25#include <spu_printf.h>
26#define printf spu_printf
31#ifdef BT_USE_DOUBLE_PRECISION
32#define REL_ERROR2 btScalar(1.0e-12)
35#define REL_ERROR2 btScalar(1.0e-6)
91 supAworld = localTransA(pInA);
92 supBworld = localTransB(qInB);
100 aMinb = supAworld - supBworld;
350 double u, v, w, p, q, r;
351 double s, t, dist, dist2;
365 s = (q * r - w * p) / (w * v - r * r);
366 t = (-s * r - q) / w;
573 int B_on_ACD, C_on_ADB, D_on_ABC;
574 int AB_O, AC_O, AD_O;
630 if (AB_O && AC_O && AD_O)
700 localTransA.
getOrigin() -= positionOffset;
701 localTransB.
getOrigin() -= positionOffset;
717 int gGjkMaxIter = 1000;
720 bool isValid =
false;
721 bool checkSimplex =
false;
722 bool checkPenetration =
true;
728 btScalar margin = marginA + marginB;
762 for (
int iterations = 0; iterations < gGjkMaxIter; iterations++)
791 if (do_simplex_res == 1)
796 else if (do_simplex_res == -1)
877 btScalar f0 = squaredDistance - delta;
914 btScalar previousSquaredDistance = squaredDistance;
915 squaredDistance = newCachedSeparatingAxis.
length2();
918 if (squaredDistance > previousSquaredDistance)
921 squaredDistance = previousSquaredDistance;
922 checkSimplex =
false;
930 if (previousSquaredDistance - squaredDistance <=
SIMD_EPSILON * previousSquaredDistance)
944#if defined(DEBUG) || defined(_DEBUG)
946 printf(
"btGjkPairDetector maxIter exceeded:%i\n",
m_curIter);
947 printf(
"sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
993 distance = ((
btScalar(1.) / rlen) - margin);
995 orgNormalInB = normalInB;
1006 bool catchDegeneratePenetrationCase =
1010 if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
1025 localTransA, localTransB,
1033 btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
1043 tmpNormalInB /=
btSqrt(lenSqr);
1047 if (!isValid || (distance2 < distance))
1049 distance = distance2;
1050 pointOnA = tmpPointOnA;
1051 pointOnB = tmpPointOnB;
1052 normalInB = tmpNormalInB;
1076 btScalar distance2 = (tmpPointOnA - tmpPointOnB).
length() - margin;
1078 if (!isValid || (distance2 < distance))
1080 distance = distance2;
1081 pointOnA = tmpPointOnA;
1082 pointOnB = tmpPointOnB;
1128 d2 = orgNormalInB.
dot(w) - margin;
1142 d1 = (-normalInB).
dot(w) - margin;
1155 d0 = normalInB.
dot(w) - margin;
1166 if (d2 > d0 && d2 > d1 && d2 > distance)
1168 normalInB = orgNormalInB;
1176 pointOnB + positionOffset,
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
const btSupportVector * btSimplexPoint(const btSimplex *s, int idx)
btScalar btVec3PointTriDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *B, const btVector3 *C, btVector3 *witness)
int btVec3Eq(const btVector3 *a, const btVector3 *b)
int btSimplexSize(const btSimplex *s)
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)
void ccdVec3Sub(btVector3 *v, const btVector3 *w)
btScalar ccdVec3X(const btVector3 *v)
void btSupportCopy(btSupportVector *d, const btSupportVector *s)
void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
btScalar btVec3PointSegmentDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *b, btVector3 *witness)
btScalar gGjkEpaPenetrationTolerance
void btVec3Copy(btVector3 *v, const btVector3 *w)
void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
void btSimplexInit(btSimplex *s)
static btVector3 ccd_vec3_origin(0, 0, 0)
void btTripleCross(const btVector3 *a, const btVector3 *b, const btVector3 *c, btVector3 *d)
btScalar ccdVec3Z(const btVector3 *v)
void btVec3Scale(btVector3 *d, btScalar k)
void btSimplexAdd(btSimplex *s, const btSupportVector *v)
const btSupportVector * ccdSimplexLast(const btSimplex *s)
static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
void ccdVec3Add(btVector3 *v, const btVector3 *w)
static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
btScalar ccdVec3Y(const btVector3 *v)
int ccdSign(btScalar val)
btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b)
void btSimplexSetSize(btSimplex *s, int size)
static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
int ccdEq(btScalar _a, btScalar _b)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btSqrt(btScalar y)
btScalar btFabs(btScalar x)
bool btFuzzyZero(btScalar x)
#define btSimplexSolverInterface
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
const btConvexShape * m_minkowskiB
const btConvexShape * m_minkowskiA
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
int m_fixContactNormalDirection
btScalar m_cachedSeparatingDistance
btSimplexSolverInterface * m_simplexSolver
btVector3 m_cachedSeparatingAxis
void getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
btConvexPenetrationDepthSolver * m_penetrationDepthSolver
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
btVector3 can be used to represent 3D points and vectors.
const btScalar & z() const
Return the z value.
btScalar dot(const btVector3 &v) const
Return the dot product.
btScalar length2() const
Return the length of the vector squared.
const btScalar & x() const
Return the x value.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
const btScalar & y() const
Return the y value.
int last
index of last added point
btVector3 v2
Support point in obj2.
btVector3 v1
Support point in obj1.
btVector3 v
Support point in minkowski sum.