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(separatingAxisInA);
102 btVector3 qInB = b.getLocalSupportWithoutMargin(separatingAxisInB);
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();
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(),
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;