16 #ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
17 #define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
24 template <
typename btConvexTemplate>
33 btVector3 guessVector(
b.getWorldTransform().getOrigin() -
a.getWorldTransform().getOrigin());
60 template <
typename btConvexTemplate,
typename btGjkDistanceTemplate>
63 bool m_catchDegeneracies =
true;
64 btScalar m_cachedSeparatingDistance = 0.f;
81 bool checkSimplex =
false;
82 bool checkPenetration =
true;
83 int m_degenerateSimplex = 0;
85 int m_lastUsedMethod = -1;
93 simplexSolver.reset();
98 btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
99 btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
101 btVector3 pInA =
a.getLocalSupportWithoutMargin(separatingAxisInA);
102 btVector3 qInB =
b.getLocalSupportWithoutMargin(separatingAxisInB);
108 delta = m_cachedSeparatingAxis.dot(
w);
113 m_degenerateSimplex = 10;
120 if (simplexSolver.inSimplex(
w))
122 m_degenerateSimplex = 1;
127 btScalar f0 = squaredDistance - delta;
134 m_degenerateSimplex = 2;
138 m_degenerateSimplex = 11;
145 simplexSolver.addVertex(
w, pWorld, qWorld);
149 if (!simplexSolver.closest(newCachedSeparatingAxis))
151 m_degenerateSimplex = 3;
158 m_cachedSeparatingAxis = newCachedSeparatingAxis;
159 m_degenerateSimplex = 6;
164 btScalar previousSquaredDistance = squaredDistance;
165 squaredDistance = newCachedSeparatingAxis.length2();
168 if (squaredDistance>previousSquaredDistance)
170 m_degenerateSimplex = 7;
171 squaredDistance = previousSquaredDistance;
172 checkSimplex =
false;
180 if (previousSquaredDistance - squaredDistance <=
SIMD_EPSILON * previousSquaredDistance)
184 m_degenerateSimplex = 12;
189 m_cachedSeparatingAxis = newCachedSeparatingAxis;
192 if (m_curIter++ > gGjkMaxIter)
194 #if defined(DEBUG) || defined(_DEBUG)
196 printf(
"btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
197 printf(
"sepAxis=(%f,%f,%f), squaredDistance = %f\n",
198 m_cachedSeparatingAxis.getX(),
199 m_cachedSeparatingAxis.getY(),
200 m_cachedSeparatingAxis.getZ(),
207 bool check = (!simplexSolver.fullSimplex());
214 m_degenerateSimplex = 13;
221 simplexSolver.compute_points(pointOnA, pointOnB);
222 normalInB = m_cachedSeparatingAxis;
224 btScalar lenSqr = m_cachedSeparatingAxis.length2();
229 m_degenerateSimplex = 5;
239 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
240 pointOnB += m_cachedSeparatingAxis * (marginB / s);
244 m_lastUsedMethod = 1;
248 m_lastUsedMethod = 2;
252 bool catchDegeneratePenetrationCase =
253 (m_catchDegeneracies && m_degenerateSimplex && ((
distance + margin) < 0.01));
256 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
265 m_cachedSeparatingAxis.setZero();
269 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
273 btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
274 btScalar lenSqr = tmpNormalInB.length2();
277 tmpNormalInB = m_cachedSeparatingAxis;
278 lenSqr = m_cachedSeparatingAxis.length2();
283 tmpNormalInB /=
btSqrt(lenSqr);
289 pointOnA = tmpPointOnA;
290 pointOnB = tmpPointOnB;
291 normalInB = tmpNormalInB;
294 m_lastUsedMethod = 3;
298 m_lastUsedMethod = 8;
303 m_lastUsedMethod = 9;
315 if (m_cachedSeparatingAxis.length2() >
btScalar(0.))
322 pointOnA = tmpPointOnA;
323 pointOnB = tmpPointOnB;
324 pointOnA -= m_cachedSeparatingAxis * marginA;
325 pointOnB += m_cachedSeparatingAxis * marginB;
326 normalInB = m_cachedSeparatingAxis;
327 normalInB.normalize();
330 m_lastUsedMethod = 6;
334 m_lastUsedMethod = 5;
343 m_cachedSeparatingAxis = normalInB;
344 m_cachedSeparatingDistance =
distance;
346 distInfo->m_normalBtoA = normalInB;
347 distInfo->m_pointOnB = pointOnB;
348 distInfo->m_pointOnA = pointOnB + normalInB *
distance;
351 return -m_lastUsedMethod;
ATTR_WARN_UNUSED_RESULT const BMVert * v
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)
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE btScalar distance2(const btVector3 &v) const
Return the distance squared between the ends of this and another vector This is symantically treating...
SyclQueue void void size_t num_bytes void
T length(const vec_base< T, Size > &a)
T distance(const T &a, const T &b)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
btScalar m_maximumDistanceSquared