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());  
 
   42                 wWitnessOnA = results.witnesses[0];
 
   43                 wWitnessOnB = results.witnesses[1];
 
   51                         wWitnessOnA = results.witnesses[0];
 
   52                         wWitnessOnB = results.witnesses[1];
 
   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();
 
  101                         btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
 
  102                         btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
 
  108                         delta = m_cachedSeparatingAxis.
dot(w);
 
  113                                 m_degenerateSimplex = 10;
 
  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();
 
  167             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(),
 
  214                                 m_degenerateSimplex = 13;
 
  222                         normalInB = m_cachedSeparatingAxis;
 
  229                                 m_degenerateSimplex = 5;
 
  239                                 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
 
  240                                 pointOnB += m_cachedSeparatingAxis * (marginB / s);
 
  241                                 distance = ((
btScalar(1.) / rlen) - margin);
 
  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;
 
  277                                         tmpNormalInB = m_cachedSeparatingAxis;
 
  278                                         lenSqr = m_cachedSeparatingAxis.
length2();
 
  283                                         tmpNormalInB /= 
btSqrt(lenSqr);
 
  286                                         if (!isValid || (distance2 < distance))
 
  288                                                 distance = distance2;
 
  289                                                 pointOnA = tmpPointOnA;
 
  290                                                 pointOnB = tmpPointOnB;
 
  291                                                 normalInB = tmpNormalInB;
 
  294                                                 m_lastUsedMethod = 3;
 
  298                                                 m_lastUsedMethod = 8;
 
  303                                         m_lastUsedMethod = 9;
 
  317                                         btScalar distance2 = (tmpPointOnA - tmpPointOnB).
length() - margin;
 
  319                                         if (!isValid || (distance2 < distance))
 
  321                                                 distance = distance2;
 
  322                                                 pointOnA = tmpPointOnA;
 
  323                                                 pointOnB = tmpPointOnB;
 
  324                                                 pointOnA -= m_cachedSeparatingAxis * marginA;
 
  325                                                 pointOnB += m_cachedSeparatingAxis * marginB;
 
  326                                                 normalInB = m_cachedSeparatingAxis;
 
  330                                                 m_lastUsedMethod = 6;
 
  334                                                 m_lastUsedMethod = 5;
 
  343                 m_cachedSeparatingAxis = normalInB;
 
  344                 m_cachedSeparatingDistance = distance;
 
  345                 distInfo->m_distance = distance;
 
  346                 distInfo->m_normalBtoA = normalInB;
 
  347                 distInfo->m_pointOnB = pointOnB;
 
  348                 distInfo->m_pointOnA = pointOnB + normalInB * distance;
 
  351         return -m_lastUsedMethod;
 
  354 #endif  //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H