23 #if defined(DEBUG) || defined (_DEBUG) 27 #include <spu_printf.h> 28 #define printf spu_printf 33 #ifdef BT_USE_DOUBLE_PRECISION 34 #define REL_ERROR2 btScalar(1.0e-12) 37 #define REL_ERROR2 btScalar(1.0e-6) 48 m_penetrationDepthSolver(penetrationDepthSolver),
49 m_simplexSolver(simplexSolver),
50 m_minkowskiA(objectA),
51 m_minkowskiB(objectB),
52 m_shapeTypeA(objectA->getShapeType()),
53 m_shapeTypeB(objectB->getShapeType()),
54 m_marginA(objectA->getMargin()),
55 m_marginB(objectB->getMargin()),
56 m_ignoreMargin(false),
58 m_catchDegeneracies(1),
59 m_fixContactNormalDirection(1)
97 supAworld = localTransA(pInA);
98 supBworld = localTransB(qInB);
106 aMinb = supAworld - supBworld;
357 double u, v, w, p, q, r;
358 double s, t, dist, dist2;
372 s = (q * r - w * p) / (w * v - r * r);
373 t = (-s * r - q) / w;
569 int B_on_ACD, C_on_ADB, D_on_ABC;
570 int AB_O, AC_O, AD_O;
625 if (AB_O && AC_O && AD_O) {
688 localTransA.
getOrigin() -= positionOffset;
689 localTransB.
getOrigin() -= positionOffset;
706 int gGjkMaxIter = 1000;
709 bool isValid =
false;
710 bool checkSimplex =
false;
711 bool checkPenetration =
true;
717 btScalar margin = marginA + marginB;
757 for (
int iterations = 0; iterations <gGjkMaxIter; iterations++)
786 if (do_simplex_res == 1)
791 else if (do_simplex_res == -1)
876 btScalar f0 = squaredDistance - delta;
913 btScalar previousSquaredDistance = squaredDistance;
914 squaredDistance = newCachedSeparatingAxis.
length2();
916 if (squaredDistance > previousSquaredDistance)
920 squaredDistance = previousSquaredDistance;
921 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",
995 distance = ((
btScalar(1.) / rlen) - margin);
997 orgNormalInB = normalInB;
1010 bool catchDegeneratePenetrationCase =
1014 if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase )) || (status == 0))
1030 localTransA,localTransB,
1040 btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
1048 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
1050 tmpNormalInB /=
btSqrt(lenSqr);
1054 if (!isValid || (distance2 < distance))
1056 distance = distance2;
1057 pointOnA = tmpPointOnA;
1058 pointOnB = tmpPointOnB;
1059 normalInB = tmpNormalInB;
1085 btScalar distance2 = (tmpPointOnA - tmpPointOnB).
length() - margin;
1087 if (!isValid || (distance2 < distance))
1089 distance = distance2;
1090 pointOnA = tmpPointOnA;
1091 pointOnB = tmpPointOnB;
1142 d2 = orgNormalInB.
dot(w)- margin;
1158 d1 = (-normalInB).
dot(w)- margin;
1173 d0 = normalInB.
dot(w)-margin;
1184 if (d2 > d0 && d2 > d1 && d2 > distance)
1187 normalInB = orgNormalInB;
1196 pointOnB+positionOffset,
btConvexPenetrationDepthSolver * m_penetrationDepthSolver
btVector3 m_cachedSeparatingAxis
btSimplexSolverInterface * m_simplexSolver
void btVec3Scale(btVector3 *d, btScalar k)
btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
btScalar m_cachedSeparatingDistance
void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
btScalar gGjkEpaPenetrationTolerance
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
static btVector3 ccd_vec3_origin(0, 0, 0)
btScalar btVec3PointSegmentDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *b, btVector3 *witness)
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
btScalar btSqrt(btScalar y)
static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
const btSupportVector * btSimplexPoint(const btSimplex *s, int idx)
int ccdEq(btScalar _a, btScalar _b)
void ccdVec3Add(btVector3 *v, const btVector3 *w)
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
int btVec3Eq(const btVector3 *a, const btVector3 *b)
btVector3 v1
Support point in obj1.
void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
btScalar dot(const btVector3 &v) const
Return the dot product.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
const btScalar & x() const
Return the x value.
const btScalar & getZ() const
Return the z value.
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
int gNumDeepPenetrationChecks
virtual bool calcPenDepth(btSimplexSolverInterface &simplexSolver, const btConvexShape *convexA, const btConvexShape *convexB, const btTransform &transA, const btTransform &transB, btVector3 &v, btVector3 &pa, btVector3 &pb, class btIDebugDraw *debugDraw)=0
void btSimplexSetSize(btSimplex *s, int size)
void btSimplexInit(btSimplex *s)
#define btSimplexSolverInterface
int m_fixContactNormalDirection
void btSupportCopy(btSupportVector *d, const btSupportVector *s)
const btScalar & getY() const
Return the y value.
const btScalar & getX() const
Return the x value.
void btSimplexAdd(btSimplex *s, const btSupportVector *v)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
int last
index of last added point
const btConvexShape * m_minkowskiB
const btScalar & y() const
Return the y value.
void btTripleCross(const btVector3 *a, const btVector3 *b, const btVector3 *c, btVector3 *d)
btVector3 v2
Support point in obj2.
void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b)
btVector3 can be used to represent 3D points and vectors.
btScalar length2() const
Return the length of the vector squared.
btScalar ccdVec3Z(const btVector3 *v)
btScalar btVec3PointTriDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *B, const btVector3 *C, btVector3 *witness)
btVector3 v
Support point in minkowski sum.
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)=0
void ccdVec3Sub(btVector3 *v, const btVector3 *w)
int btSimplexSize(const btSimplex *s)
bool btFuzzyZero(btScalar x)
int ccdSign(btScalar val)
const btSupportVector * ccdSimplexLast(const btSimplex *s)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
btScalar ccdVec3Y(const btVector3 *v)
void btVec3Copy(btVector3 *v, const btVector3 *w)
const btConvexShape * m_minkowskiA
btScalar ccdVec3X(const btVector3 *v)
btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
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 int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
void getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
btScalar btFabs(btScalar x)
const btScalar & z() const
Return the z value.