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)
64 m_penetrationDepthSolver(penetrationDepthSolver),
65 m_simplexSolver(simplexSolver),
66 m_minkowskiA(objectA),
67 m_minkowskiB(objectB),
68 m_shapeTypeA(shapeTypeA),
69 m_shapeTypeB(shapeTypeB),
72 m_ignoreMargin(false),
74 m_catchDegeneracies(1),
75 m_fixContactNormalDirection(1)
92 m_cachedSeparatingDistance = 0.f;
101 localTransA.
getOrigin() -= positionOffset;
102 localTransB.
getOrigin() -= positionOffset;
104 bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
119 int gGjkMaxIter = 1000;
120 m_cachedSeparatingAxis.setValue(0,1,0);
122 bool isValid =
false;
123 bool checkSimplex =
false;
124 bool checkPenetration =
true;
125 m_degenerateSimplex = 0;
127 m_lastUsedMethod = -1;
133 btScalar margin = marginA + marginB;
137 m_simplexSolver->reset();
147 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
148 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
161 delta = m_cachedSeparatingAxis.
dot(w);
166 m_degenerateSimplex = 10;
173 if (m_simplexSolver->inSimplex(w))
175 m_degenerateSimplex = 1;
180 btScalar f0 = squaredDistance - delta;
187 m_degenerateSimplex = 2;
190 m_degenerateSimplex = 11;
197 m_simplexSolver->addVertex(w, pWorld, qWorld);
201 if (!m_simplexSolver->closest(newCachedSeparatingAxis))
203 m_degenerateSimplex = 3;
210 m_cachedSeparatingAxis = newCachedSeparatingAxis;
211 m_degenerateSimplex = 6;
216 btScalar previousSquaredDistance = squaredDistance;
217 squaredDistance = newCachedSeparatingAxis.
length2();
219 if (squaredDistance>previousSquaredDistance)
222 m_degenerateSimplex = 7;
223 squaredDistance = previousSquaredDistance;
224 checkSimplex =
false;
233 if (previousSquaredDistance - squaredDistance <=
SIMD_EPSILON * previousSquaredDistance)
237 m_degenerateSimplex = 12;
242 m_cachedSeparatingAxis = newCachedSeparatingAxis;
245 if (m_curIter++ > gGjkMaxIter)
247 #if defined(DEBUG) || defined (_DEBUG) 249 printf(
"btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
250 printf(
"sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
251 m_cachedSeparatingAxis.getX(),
252 m_cachedSeparatingAxis.getY(),
253 m_cachedSeparatingAxis.getZ(),
255 m_minkowskiA->getShapeType(),
256 m_minkowskiB->getShapeType());
264 bool check = (!m_simplexSolver->fullSimplex());
271 m_degenerateSimplex = 13;
278 m_simplexSolver->compute_points(pointOnA, pointOnB);
279 normalInB = m_cachedSeparatingAxis;
286 m_degenerateSimplex = 5;
296 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
297 pointOnB += m_cachedSeparatingAxis * (marginB / s);
298 distance = ((
btScalar(1.)/rlen) - margin);
301 m_lastUsedMethod = 1;
304 m_lastUsedMethod = 2;
308 bool catchDegeneratePenetrationCase =
312 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
317 if (m_penetrationDepthSolver)
323 m_cachedSeparatingAxis.setZero();
325 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
327 m_minkowskiA,m_minkowskiB,
328 localTransA,localTransB,
329 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
336 btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
340 tmpNormalInB = m_cachedSeparatingAxis;
341 lenSqr = m_cachedSeparatingAxis.
length2();
346 tmpNormalInB /=
btSqrt(lenSqr);
348 m_lastUsedMethod = 3;
350 if (!isValid || (distance2 < distance))
352 distance = distance2;
353 pointOnA = tmpPointOnA;
354 pointOnB = tmpPointOnB;
355 normalInB = tmpNormalInB;
361 m_lastUsedMethod = 8;
365 m_lastUsedMethod = 9;
377 if (m_cachedSeparatingAxis.length2() >
btScalar(0.))
381 if (!isValid || (distance2 < distance))
383 distance = distance2;
384 pointOnA = tmpPointOnA;
385 pointOnB = tmpPointOnB;
386 pointOnA -= m_cachedSeparatingAxis * marginA ;
387 pointOnB += m_cachedSeparatingAxis * marginB ;
388 normalInB = m_cachedSeparatingAxis;
392 m_lastUsedMethod = 6;
395 m_lastUsedMethod = 5;
410 m_cachedSeparatingAxis = normalInB;
411 m_cachedSeparatingDistance = distance;
425 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
426 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
431 d1 = (-normalInB).
dot(w);
439 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
440 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
445 d0 = normalInB.
dot(w);
449 m_lastUsedMethod = 10;
456 pointOnB+positionOffset,
btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
btScalar gGjkEpaPenetrationTolerance
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
btScalar length2() const
Return the length of the vector squared.
btScalar btSqrt(btScalar y)
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
int gNumDeepPenetrationChecks
#define btSimplexSolverInterface
btScalar dot(const btVector3 &v) const
Return the dot product.
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
btVector3 can be used to represent 3D points and vectors.
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
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)
void getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)