1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org
4
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:
10
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.
14 */
15
16 #ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
17 #define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
18
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"
23
24
25
26
27
28
29 template <typename btConvexTemplate>
btGjkEpaCalcPenDepth(const btConvexTemplate & a,const btConvexTemplate & b,const btGjkCollisionDescription & colDesc,btVector3 & v,btVector3 & wWitnessOnA,btVector3 & wWitnessOnB)30 bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
31 const btGjkCollisionDescription& colDesc,
32 btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
33 {
34 (void)v;
35
36 // const btScalar radialmargin(btScalar(0.));
37
38 btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input?
39
40 btGjkEpaSolver3::sResults results;
41
42
43 if(btGjkEpaSolver3_Penetration(a,b,guessVector,results))
44
45 {
46 // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
47 //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
48 wWitnessOnA = results.witnesses[0];
49 wWitnessOnB = results.witnesses[1];
50 v = results.normal;
51 return true;
52 } else
53 {
54 if(btGjkEpaSolver3_Distance(a,b,guessVector,results))
55 {
56 wWitnessOnA = results.witnesses[0];
57 wWitnessOnB = results.witnesses[1];
58 v = results.normal;
59 return false;
60 }
61 }
62 return false;
63 }
64
65 template <typename btConvexTemplate, typename btGjkDistanceTemplate>
btComputeGjkEpaPenetration(const btConvexTemplate & a,const btConvexTemplate & b,const btGjkCollisionDescription & colDesc,btVoronoiSimplexSolver & simplexSolver,btGjkDistanceTemplate * distInfo)66 int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
67 {
68
69 bool m_catchDegeneracies = true;
70 btScalar m_cachedSeparatingDistance = 0.f;
71
72 btScalar distance=btScalar(0.);
73 btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
74
75 btVector3 pointOnA,pointOnB;
76 btTransform localTransA = a.getWorldTransform();
77 btTransform localTransB = b.getWorldTransform();
78
79 btScalar marginA = a.getMargin();
80 btScalar marginB = b.getMargin();
81
82 int m_curIter = 0;
83 int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN?
84 btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
85
86 bool isValid = false;
87 bool checkSimplex = false;
88 bool checkPenetration = true;
89 int m_degenerateSimplex = 0;
90
91 int m_lastUsedMethod = -1;
92
93 {
94 btScalar squaredDistance = BT_LARGE_FLOAT;
95 btScalar delta = btScalar(0.);
96
97 btScalar margin = marginA + marginB;
98
99
100
101 simplexSolver.reset();
102
103 for ( ; ; )
104 //while (true)
105 {
106
107 btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
108 btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
109
110 btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
111 btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
112
113 btVector3 pWorld = localTransA(pInA);
114 btVector3 qWorld = localTransB(qInB);
115
116
117
118 btVector3 w = pWorld - qWorld;
119 delta = m_cachedSeparatingAxis.dot(w);
120
121 // potential exit, they don't overlap
122 if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
123 {
124 m_degenerateSimplex = 10;
125 checkSimplex=true;
126 //checkPenetration = false;
127 break;
128 }
129
130 //exit 0: the new point is already in the simplex, or we didn't come any closer
131 if (simplexSolver.inSimplex(w))
132 {
133 m_degenerateSimplex = 1;
134 checkSimplex = true;
135 break;
136 }
137 // are we getting any closer ?
138 btScalar f0 = squaredDistance - delta;
139 btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
140
141 if (f0 <= f1)
142 {
143 if (f0 <= btScalar(0.))
144 {
145 m_degenerateSimplex = 2;
146 } else
147 {
148 m_degenerateSimplex = 11;
149 }
150 checkSimplex = true;
151 break;
152 }
153
154 //add current vertex to simplex
155 simplexSolver.addVertex(w, pWorld, qWorld);
156 btVector3 newCachedSeparatingAxis;
157
158 //calculate the closest point to the origin (update vector v)
159 if (!simplexSolver.closest(newCachedSeparatingAxis))
160 {
161 m_degenerateSimplex = 3;
162 checkSimplex = true;
163 break;
164 }
165
166 if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2)
167 {
168 m_cachedSeparatingAxis = newCachedSeparatingAxis;
169 m_degenerateSimplex = 6;
170 checkSimplex = true;
171 break;
172 }
173
174 btScalar previousSquaredDistance = squaredDistance;
175 squaredDistance = newCachedSeparatingAxis.length2();
176 #if 0
177 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
178 if (squaredDistance>previousSquaredDistance)
179 {
180 m_degenerateSimplex = 7;
181 squaredDistance = previousSquaredDistance;
182 checkSimplex = false;
183 break;
184 }
185 #endif //
186
187
188 //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
189
190 //are we getting any closer ?
191 if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
192 {
193 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
194 checkSimplex = true;
195 m_degenerateSimplex = 12;
196
197 break;
198 }
199
200 m_cachedSeparatingAxis = newCachedSeparatingAxis;
201
202 //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
203 if (m_curIter++ > gGjkMaxIter)
204 {
205 #if defined(DEBUG) || defined (_DEBUG)
206
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(),
212 squaredDistance);
213 #endif
214
215 break;
216
217 }
218
219
220 bool check = (!simplexSolver.fullSimplex());
221 //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
222
223 if (!check)
224 {
225 //do we need this backup_closest here ?
226 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
227 m_degenerateSimplex = 13;
228 break;
229 }
230 }
231
232 if (checkSimplex)
233 {
234 simplexSolver.compute_points(pointOnA, pointOnB);
235 normalInB = m_cachedSeparatingAxis;
236
237 btScalar lenSqr =m_cachedSeparatingAxis.length2();
238
239 //valid normal
240 if (lenSqr < 0.0001)
241 {
242 m_degenerateSimplex = 5;
243 }
244 if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
245 {
246 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
247 normalInB *= rlen; //normalize
248
249 btScalar s = btSqrt(squaredDistance);
250
251 btAssert(s > btScalar(0.0));
252 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
253 pointOnB += m_cachedSeparatingAxis * (marginB / s);
254 distance = ((btScalar(1.)/rlen) - margin);
255 isValid = true;
256
257 m_lastUsedMethod = 1;
258 } else
259 {
260 m_lastUsedMethod = 2;
261 }
262 }
263
264 bool catchDegeneratePenetrationCase =
265 (m_catchDegeneracies && m_degenerateSimplex && ((distance+margin) < 0.01));
266
267 //if (checkPenetration && !isValid)
268 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
269 {
270 //penetration case
271
272 //if there is no way to handle penetrations, bail out
273
274 // Penetration depth case.
275 btVector3 tmpPointOnA,tmpPointOnB;
276
277 m_cachedSeparatingAxis.setZero();
278
279 bool isValid2 = btGjkEpaCalcPenDepth(a,b,
280 colDesc,
281 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
282
283 if (isValid2)
284 {
285 btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
286 btScalar lenSqr = tmpNormalInB.length2();
287 if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
288 {
289 tmpNormalInB = m_cachedSeparatingAxis;
290 lenSqr = m_cachedSeparatingAxis.length2();
291 }
292
293 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
294 {
295 tmpNormalInB /= btSqrt(lenSqr);
296 btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
297 //only replace valid penetrations when the result is deeper (check)
298 if (!isValid || (distance2 < distance))
299 {
300 distance = distance2;
301 pointOnA = tmpPointOnA;
302 pointOnB = tmpPointOnB;
303 normalInB = tmpNormalInB;
304
305 isValid = true;
306 m_lastUsedMethod = 3;
307 } else
308 {
309 m_lastUsedMethod = 8;
310 }
311 } else
312 {
313 m_lastUsedMethod = 9;
314 }
315 } else
316
317 {
318 ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
319 ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
320 ///reports a valid positive distance. Use the results of the second GJK instead of failing.
321 ///thanks to Jacob.Langford for the reproduction case
322 ///http://code.google.com/p/bullet/issues/detail?id=250
323
324
325 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
326 {
327 btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
328 //only replace valid distances when the distance is less
329 if (!isValid || (distance2 < distance))
330 {
331 distance = distance2;
332 pointOnA = tmpPointOnA;
333 pointOnB = tmpPointOnB;
334 pointOnA -= m_cachedSeparatingAxis * marginA ;
335 pointOnB += m_cachedSeparatingAxis * marginB ;
336 normalInB = m_cachedSeparatingAxis;
337 normalInB.normalize();
338
339 isValid = true;
340 m_lastUsedMethod = 6;
341 } else
342 {
343 m_lastUsedMethod = 5;
344 }
345 }
346 }
347 }
348 }
349
350
351
352 if (isValid && ((distance < 0) || (distance*distance < colDesc.m_maximumDistanceSquared)))
353 {
354
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;
361 return 0;
362 }
363 return -m_lastUsedMethod;
364 }
365
366
367
368
369 #endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
370