23 #if defined(DEBUG) || defined (_DEBUG)
27 #include <spu_printf.h>
28 #define printf spu_printf
33 #define REL_ERROR2 btScalar(1.0e-6)
42 m_penetrationDepthSolver(penetrationDepthSolver),
43 m_simplexSolver(simplexSolver),
44 m_minkowskiA(objectA),
45 m_minkowskiB(objectB),
46 m_shapeTypeA(objectA->getShapeType()),
47 m_shapeTypeB(objectB->getShapeType()),
48 m_marginA(objectA->getMargin()),
49 m_marginB(objectB->getMargin()),
50 m_ignoreMargin(false),
52 m_catchDegeneracies(1),
53 m_fixContactNormalDirection(1)
58 m_penetrationDepthSolver(penetrationDepthSolver),
59 m_simplexSolver(simplexSolver),
60 m_minkowskiA(objectA),
61 m_minkowskiB(objectB),
62 m_shapeTypeA(shapeTypeA),
63 m_shapeTypeB(shapeTypeB),
66 m_ignoreMargin(false),
68 m_catchDegeneracies(1),
69 m_fixContactNormalDirection(1)
86 m_cachedSeparatingDistance = 0.f;
95 localTransA.
getOrigin() -= positionOffset;
96 localTransB.
getOrigin() -= positionOffset;
98 bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
113 int gGjkMaxIter = 1000;
114 m_cachedSeparatingAxis.setValue(0,1,0);
116 bool isValid =
false;
117 bool checkSimplex =
false;
118 bool checkPenetration =
true;
119 m_degenerateSimplex = 0;
121 m_lastUsedMethod = -1;
127 btScalar margin = marginA + marginB;
131 m_simplexSolver->reset();
141 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
142 btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
155 delta = m_cachedSeparatingAxis.
dot(w);
160 m_degenerateSimplex = 10;
167 if (m_simplexSolver->inSimplex(w))
169 m_degenerateSimplex = 1;
174 btScalar f0 = squaredDistance - delta;
181 m_degenerateSimplex = 2;
184 m_degenerateSimplex = 11;
191 m_simplexSolver->addVertex(w, pWorld, qWorld);
195 if (!m_simplexSolver->closest(newCachedSeparatingAxis))
197 m_degenerateSimplex = 3;
204 m_cachedSeparatingAxis = newCachedSeparatingAxis;
205 m_degenerateSimplex = 6;
210 btScalar previousSquaredDistance = squaredDistance;
211 squaredDistance = newCachedSeparatingAxis.
length2();
213 if (squaredDistance>previousSquaredDistance)
216 m_degenerateSimplex = 7;
217 squaredDistance = previousSquaredDistance;
218 checkSimplex =
false;
227 if (previousSquaredDistance - squaredDistance <=
SIMD_EPSILON * previousSquaredDistance)
231 m_degenerateSimplex = 12;
236 m_cachedSeparatingAxis = newCachedSeparatingAxis;
239 if (m_curIter++ > gGjkMaxIter)
241 #if defined(DEBUG) || defined (_DEBUG)
243 printf(
"btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
244 printf(
"sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
245 m_cachedSeparatingAxis.getX(),
246 m_cachedSeparatingAxis.getY(),
247 m_cachedSeparatingAxis.getZ(),
249 m_minkowskiA->getShapeType(),
250 m_minkowskiB->getShapeType());
258 bool check = (!m_simplexSolver->fullSimplex());
265 m_degenerateSimplex = 13;
272 m_simplexSolver->compute_points(pointOnA, pointOnB);
273 normalInB = m_cachedSeparatingAxis;
280 m_degenerateSimplex = 5;
290 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
291 pointOnB += m_cachedSeparatingAxis * (marginB / s);
292 distance = ((
btScalar(1.)/rlen) - margin);
295 m_lastUsedMethod = 1;
298 m_lastUsedMethod = 2;
302 bool catchDegeneratePenetrationCase =
303 (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
306 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
311 if (m_penetrationDepthSolver)
317 m_cachedSeparatingAxis.setZero();
319 bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
321 m_minkowskiA,m_minkowskiB,
322 localTransA,localTransB,
323 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
330 btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
334 tmpNormalInB = m_cachedSeparatingAxis;
335 lenSqr = m_cachedSeparatingAxis.
length2();
338 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
340 tmpNormalInB /=
btSqrt(lenSqr);
343 if (!isValid || (distance2 < distance))
345 distance = distance2;
346 pointOnA = tmpPointOnA;
347 pointOnB = tmpPointOnB;
348 normalInB = tmpNormalInB;
351 m_lastUsedMethod = 3;
354 m_lastUsedMethod = 8;
358 m_lastUsedMethod = 9;
370 if (m_cachedSeparatingAxis.length2() >
btScalar(0.))
374 if (!isValid || (distance2 < distance))
376 distance = distance2;
377 pointOnA = tmpPointOnA;
378 pointOnB = tmpPointOnB;
379 pointOnA -= m_cachedSeparatingAxis * marginA ;
380 pointOnB += m_cachedSeparatingAxis * marginB ;
381 normalInB = m_cachedSeparatingAxis;
385 m_lastUsedMethod = 6;
388 m_lastUsedMethod = 5;
403 m_cachedSeparatingAxis = normalInB;
404 m_cachedSeparatingDistance = distance;
408 pointOnB+positionOffset,
btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
btScalar btSqrt(btScalar y)
btScalar dot(const btVector3 &v) const
Return the dot product.
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
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 length2() const
Return the length of the vector squared.
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)=0
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)