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 length2() const
Return the length of the vector squared. 
btScalar btSqrt(btScalar y)
const btScalar & getY() const
Return the y value. 
btScalar m_maximumDistanceSquared
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)
btScalar dot(const btVector3 &v) const
Return the dot product. 
void addVertex(const btVector3 &w, const btVector3 &p, const btVector3 &q)
btVector3 can be used to represent 3D points and vectors. 
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)
const btScalar & getX() const
Return the x value. 
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
bool closest(btVector3 &v)