16 #ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H 17 #define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H 29 template <
typename btConvexTemplate>
38 btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());
48 wWitnessOnA = results.witnesses[0];
49 wWitnessOnB = results.witnesses[1];
56 wWitnessOnA = results.witnesses[0];
57 wWitnessOnB = results.witnesses[1];
65 template <
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();
177 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();
293 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
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;
369 #endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
btScalar btSqrt(btScalar y)
btScalar m_maximumDistanceSquared
btScalar dot(const btVector3 &v) const
Return the dot product.
bool inSimplex(const btVector3 &w)
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
const btScalar & getZ() const
Return the z value.
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
bool btGjkEpaCalcPenDepth(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVector3 &v, btVector3 &wWitnessOnA, btVector3 &wWitnessOnB)
const btScalar & getY() const
Return the y value.
const btScalar & getX() const
Return the x value.
void addVertex(const btVector3 &w, const btVector3 &p, const btVector3 &q)
btVector3 can be used to represent 3D points and vectors.
btScalar length2() const
Return the length of the vector squared.
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
void compute_points(btVector3 &p1, btVector3 &p2)
int btComputeGjkEpaPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVoronoiSimplexSolver &simplexSolver, btGjkDistanceTemplate *distInfo)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
bool closest(btVector3 &v)