16#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
17#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
29template <
typename btConvexTemplate>
38 btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());
65template <
typename btConvexTemplate,
typename btGjkDistanceTemplate>
69 bool m_catchDegeneracies =
true;
70 btScalar m_cachedSeparatingDistance = 0.f;
87 bool checkSimplex =
false;
88 bool checkPenetration =
true;
89 int m_degenerateSimplex = 0;
91 int m_lastUsedMethod = -1;
101 simplexSolver.
reset();
110 btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
111 btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
119 delta = m_cachedSeparatingAxis.
dot(w);
124 m_degenerateSimplex = 10;
133 m_degenerateSimplex = 1;
138 btScalar f0 = squaredDistance - delta;
145 m_degenerateSimplex = 2;
148 m_degenerateSimplex = 11;
155 simplexSolver.
addVertex(w, pWorld, qWorld);
159 if (!simplexSolver.
closest(newCachedSeparatingAxis))
161 m_degenerateSimplex = 3;
168 m_cachedSeparatingAxis = newCachedSeparatingAxis;
169 m_degenerateSimplex = 6;
174 btScalar previousSquaredDistance = squaredDistance;
175 squaredDistance = newCachedSeparatingAxis.
length2();
178 if (squaredDistance>previousSquaredDistance)
180 m_degenerateSimplex = 7;
181 squaredDistance = previousSquaredDistance;
182 checkSimplex =
false;
191 if (previousSquaredDistance - squaredDistance <=
SIMD_EPSILON * previousSquaredDistance)
195 m_degenerateSimplex = 12;
200 m_cachedSeparatingAxis = newCachedSeparatingAxis;
203 if (m_curIter++ > gGjkMaxIter)
205#if defined(DEBUG) || defined (_DEBUG)
207 printf(
"btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
208 printf(
"sepAxis=(%f,%f,%f), squaredDistance = %f\n",
209 m_cachedSeparatingAxis.
getX(),
210 m_cachedSeparatingAxis.
getY(),
211 m_cachedSeparatingAxis.
getZ(),
227 m_degenerateSimplex = 13;
235 normalInB = m_cachedSeparatingAxis;
242 m_degenerateSimplex = 5;
252 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
253 pointOnB += m_cachedSeparatingAxis * (marginB / s);
254 distance = ((
btScalar(1.)/rlen) - margin);
257 m_lastUsedMethod = 1;
260 m_lastUsedMethod = 2;
264 bool catchDegeneratePenetrationCase =
265 (m_catchDegeneracies && m_degenerateSimplex && ((distance+margin) < 0.01));
268 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
277 m_cachedSeparatingAxis.
setZero();
281 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
285 btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
289 tmpNormalInB = m_cachedSeparatingAxis;
290 lenSqr = m_cachedSeparatingAxis.
length2();
295 tmpNormalInB /=
btSqrt(lenSqr);
298 if (!isValid || (distance2 < distance))
300 distance = distance2;
301 pointOnA = tmpPointOnA;
302 pointOnB = tmpPointOnB;
303 normalInB = tmpNormalInB;
306 m_lastUsedMethod = 3;
309 m_lastUsedMethod = 8;
313 m_lastUsedMethod = 9;
329 if (!isValid || (distance2 < distance))
331 distance = distance2;
332 pointOnA = tmpPointOnA;
333 pointOnB = tmpPointOnB;
334 pointOnA -= m_cachedSeparatingAxis * marginA ;
335 pointOnB += m_cachedSeparatingAxis * marginB ;
336 normalInB = m_cachedSeparatingAxis;
340 m_lastUsedMethod = 6;
343 m_lastUsedMethod = 5;
355 m_cachedSeparatingAxis = normalInB;
356 m_cachedSeparatingDistance = distance;
357 distInfo->m_distance = distance;
358 distInfo->m_normalBtoA = normalInB;
359 distInfo->m_pointOnB = pointOnB;
360 distInfo->m_pointOnA = pointOnB+normalInB*distance;
363 return -m_lastUsedMethod;
bool btGjkEpaCalcPenDepth(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVector3 &v, btVector3 &wWitnessOnA, btVector3 &wWitnessOnB)
int btComputeGjkEpaPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVoronoiSimplexSolver &simplexSolver, btGjkDistanceTemplate *distInfo)
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
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)
btVector3 can be used to represent 3D points and vectors.
const btScalar & getZ() 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 & getY() const
Return the y value.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
const btScalar & getX() const
Return the x value.
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
void addVertex(const btVector3 &w, const btVector3 &p, const btVector3 &q)
bool inSimplex(const btVector3 &w)
void compute_points(btVector3 &p1, btVector3 &p2)
bool closest(btVector3 &v)
btScalar m_maximumDistanceSquared