2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
16 #ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
17 #define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
19 #include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
20 #include "btGjkEpa3.h"
21 #include "btGjkCollisionDescription.h"
22 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
24 template <typename btConvexTemplate>
25 bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
26 const btGjkCollisionDescription& colDesc,
27 btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
31 // const btScalar radialmargin(btScalar(0.));
33 btVector3 guessVector(b.getWorldTransform().getOrigin() - a.getWorldTransform().getOrigin()); //?? why not use the GJK input?
35 btGjkEpaSolver3::sResults results;
37 if (btGjkEpaSolver3_Penetration(a, b, guessVector, results))
40 // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
41 //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
42 wWitnessOnA = results.witnesses[0];
43 wWitnessOnB = results.witnesses[1];
49 if (btGjkEpaSolver3_Distance(a, b, guessVector, results))
51 wWitnessOnA = results.witnesses[0];
52 wWitnessOnB = results.witnesses[1];
60 template <typename btConvexTemplate, typename btGjkDistanceTemplate>
61 int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
63 bool m_catchDegeneracies = true;
64 btScalar m_cachedSeparatingDistance = 0.f;
66 btScalar distance = btScalar(0.);
67 btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
69 btVector3 pointOnA, pointOnB;
70 btTransform localTransA = a.getWorldTransform();
71 btTransform localTransB = b.getWorldTransform();
73 btScalar marginA = a.getMargin();
74 btScalar marginB = b.getMargin();
77 int gGjkMaxIter = colDesc.m_maxGjkIterations; //this is to catch invalid input, perhaps check for #NaN?
78 btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
81 bool checkSimplex = false;
82 bool checkPenetration = true;
83 int m_degenerateSimplex = 0;
85 int m_lastUsedMethod = -1;
88 btScalar squaredDistance = BT_LARGE_FLOAT;
89 btScalar delta = btScalar(0.);
91 btScalar margin = marginA + marginB;
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);
104 btVector3 pWorld = localTransA(pInA);
105 btVector3 qWorld = localTransB(qInB);
107 btVector3 w = pWorld - qWorld;
108 delta = m_cachedSeparatingAxis.dot(w);
110 // potential exit, they don't overlap
111 if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
113 m_degenerateSimplex = 10;
115 //checkPenetration = false;
119 //exit 0: the new point is already in the simplex, or we didn't come any closer
120 if (simplexSolver.inSimplex(w))
122 m_degenerateSimplex = 1;
126 // are we getting any closer ?
127 btScalar f0 = squaredDistance - delta;
128 btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
132 if (f0 <= btScalar(0.))
134 m_degenerateSimplex = 2;
138 m_degenerateSimplex = 11;
144 //add current vertex to simplex
145 simplexSolver.addVertex(w, pWorld, qWorld);
146 btVector3 newCachedSeparatingAxis;
148 //calculate the closest point to the origin (update vector v)
149 if (!simplexSolver.closest(newCachedSeparatingAxis))
151 m_degenerateSimplex = 3;
156 if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2)
158 m_cachedSeparatingAxis = newCachedSeparatingAxis;
159 m_degenerateSimplex = 6;
164 btScalar previousSquaredDistance = squaredDistance;
165 squaredDistance = newCachedSeparatingAxis.length2();
167 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
168 if (squaredDistance>previousSquaredDistance)
170 m_degenerateSimplex = 7;
171 squaredDistance = previousSquaredDistance;
172 checkSimplex = false;
177 //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
179 //are we getting any closer ?
180 if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
182 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
184 m_degenerateSimplex = 12;
189 m_cachedSeparatingAxis = newCachedSeparatingAxis;
191 //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
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());
208 //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
212 //do we need this backup_closest here ?
213 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
214 m_degenerateSimplex = 13;
221 simplexSolver.compute_points(pointOnA, pointOnB);
222 normalInB = m_cachedSeparatingAxis;
224 btScalar lenSqr = m_cachedSeparatingAxis.length2();
229 m_degenerateSimplex = 5;
231 if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
233 btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
234 normalInB *= rlen; //normalize
236 btScalar s = btSqrt(squaredDistance);
238 btAssert(s > btScalar(0.0));
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));
255 //if (checkPenetration && !isValid)
256 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
260 //if there is no way to handle penetrations, bail out
262 // Penetration depth case.
263 btVector3 tmpPointOnA, tmpPointOnB;
265 m_cachedSeparatingAxis.setZero();
267 bool isValid2 = btGjkEpaCalcPenDepth(a, b,
269 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
273 btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
274 btScalar lenSqr = tmpNormalInB.length2();
275 if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
277 tmpNormalInB = m_cachedSeparatingAxis;
278 lenSqr = m_cachedSeparatingAxis.length2();
281 if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
283 tmpNormalInB /= btSqrt(lenSqr);
284 btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
285 //only replace valid penetrations when the result is deeper (check)
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;
309 ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
310 ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
311 ///reports a valid positive distance. Use the results of the second GJK instead of failing.
312 ///thanks to Jacob.Langford for the reproduction case
313 ///http://code.google.com/p/bullet/issues/detail?id=250
315 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
317 btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
318 //only replace valid distances when the distance is less
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;
327 normalInB.normalize();
330 m_lastUsedMethod = 6;
334 m_lastUsedMethod = 5;
341 if (isValid && ((distance < 0) || (distance * distance < colDesc.m_maximumDistanceSquared)))
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