Bullet Collision Detection & Physics Library: btComputeGjkEpaPenetration.h Source File

Bullet Collision Detection & Physics Library

btComputeGjkEpaPenetration.h
Go to the documentation of this file.
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 template <typename btConvexTemplate>
25 bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
26 const btGjkCollisionDescription& colDesc,
27 btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
28 {
29 (void)v;
30
31 // const btScalar radialmargin(btScalar(0.));
32
33 btVector3 guessVector(b.getWorldTransform().getOrigin() - a.getWorldTransform().getOrigin()); //?? why not use the GJK input?
34
35 btGjkEpaSolver3::sResults results;
36
37 if (btGjkEpaSolver3_Penetration(a, b, guessVector, results))
38
39 {
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];
44 v = results.normal;
45 return true;
46 }
47 else
48 {
49 if (btGjkEpaSolver3_Distance(a, b, guessVector, results))
50 {
51 wWitnessOnA = results.witnesses[0];
52 wWitnessOnB = results.witnesses[1];
53 v = results.normal;
54 return false;
55 }
56 }
57 return false;
58 }
59
60 template <typename btConvexTemplate, typename btGjkDistanceTemplate>
61 int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
62 {
63 bool m_catchDegeneracies = true;
64 btScalar m_cachedSeparatingDistance = 0.f;
65
66 btScalar distance = btScalar(0.);
67 btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
68
69 btVector3 pointOnA, pointOnB;
70 btTransform localTransA = a.getWorldTransform();
71 btTransform localTransB = b.getWorldTransform();
72
73 btScalar marginA = a.getMargin();
74 btScalar marginB = b.getMargin();
75
76 int m_curIter = 0;
77 int gGjkMaxIter = colDesc.m_maxGjkIterations; //this is to catch invalid input, perhaps check for #NaN?
78 btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
79
80 bool isValid = false;
81 bool checkSimplex = false;
82 bool checkPenetration = true;
83 int m_degenerateSimplex = 0;
84
85 int m_lastUsedMethod = -1;
86
87 {
88 btScalar squaredDistance = BT_LARGE_FLOAT;
89 btScalar delta = btScalar(0.);
90
91 btScalar margin = marginA + marginB;
92
93 simplexSolver.reset();
94
95 for (;;)
96 //while (true)
97 {
98 btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
99 btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
100
101 btVector3 pInA = a.getLocalSupportWithoutMargin(separatingAxisInA);
102 btVector3 qInB = b.getLocalSupportWithoutMargin(separatingAxisInB);
103
104 btVector3 pWorld = localTransA(pInA);
105 btVector3 qWorld = localTransB(qInB);
106
107 btVector3 w = pWorld - qWorld;
108 delta = m_cachedSeparatingAxis.dot(w);
109
110 // potential exit, they don't overlap
111 if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
112 {
113 m_degenerateSimplex = 10;
114 checkSimplex = true;
115 //checkPenetration = false;
116 break;
117 }
118
119 //exit 0: the new point is already in the simplex, or we didn't come any closer
120 if (simplexSolver.inSimplex(w))
121 {
122 m_degenerateSimplex = 1;
123 checkSimplex = true;
124 break;
125 }
126 // are we getting any closer ?
127 btScalar f0 = squaredDistance - delta;
128 btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
129
130 if (f0 <= f1)
131 {
132 if (f0 <= btScalar(0.))
133 {
134 m_degenerateSimplex = 2;
135 }
136 else
137 {
138 m_degenerateSimplex = 11;
139 }
140 checkSimplex = true;
141 break;
142 }
143
144 //add current vertex to simplex
145 simplexSolver.addVertex(w, pWorld, qWorld);
146 btVector3 newCachedSeparatingAxis;
147
148 //calculate the closest point to the origin (update vector v)
149 if (!simplexSolver.closest(newCachedSeparatingAxis))
150 {
151 m_degenerateSimplex = 3;
152 checkSimplex = true;
153 break;
154 }
155
156 if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2)
157 {
158 m_cachedSeparatingAxis = newCachedSeparatingAxis;
159 m_degenerateSimplex = 6;
160 checkSimplex = true;
161 break;
162 }
163
164 btScalar previousSquaredDistance = squaredDistance;
165 squaredDistance = newCachedSeparatingAxis.length2();
166 #if 0
167 if (squaredDistance>previousSquaredDistance)
169 {
170 m_degenerateSimplex = 7;
171 squaredDistance = previousSquaredDistance;
172 checkSimplex = false;
173 break;
174 }
175 #endif //
176
177 //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
178
179 //are we getting any closer ?
180 if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
181 {
182 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
183 checkSimplex = true;
184 m_degenerateSimplex = 12;
185
186 break;
187 }
188
189 m_cachedSeparatingAxis = newCachedSeparatingAxis;
190
191 //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
192 if (m_curIter++ > gGjkMaxIter)
193 {
194 #if defined(DEBUG) || defined(_DEBUG)
195
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(),
201 squaredDistance);
202 #endif
203
204 break;
205 }
206
207 bool check = (!simplexSolver.fullSimplex());
208 //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
209
210 if (!check)
211 {
212 //do we need this backup_closest here ?
213 // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
214 m_degenerateSimplex = 13;
215 break;
216 }
217 }
218
219 if (checkSimplex)
220 {
221 simplexSolver.compute_points(pointOnA, pointOnB);
222 normalInB = m_cachedSeparatingAxis;
223
224 btScalar lenSqr = m_cachedSeparatingAxis.length2();
225
226 //valid normal
227 if (lenSqr < 0.0001)
228 {
229 m_degenerateSimplex = 5;
230 }
231 if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
232 {
233 btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
234 normalInB *= rlen; //normalize
235
236 btScalar s = btSqrt(squaredDistance);
237
238 btAssert(s > btScalar(0.0));
239 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
240 pointOnB += m_cachedSeparatingAxis * (marginB / s);
241 distance = ((btScalar(1.) / rlen) - margin);
242 isValid = true;
243
244 m_lastUsedMethod = 1;
245 }
246 else
247 {
248 m_lastUsedMethod = 2;
249 }
250 }
251
252 bool catchDegeneratePenetrationCase =
253 (m_catchDegeneracies && m_degenerateSimplex && ((distance + margin) < 0.01));
254
255 //if (checkPenetration && !isValid)
256 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
257 {
258 //penetration case
259
260 //if there is no way to handle penetrations, bail out
261
262 // Penetration depth case.
263 btVector3 tmpPointOnA, tmpPointOnB;
264
265 m_cachedSeparatingAxis.setZero();
266
267 bool isValid2 = btGjkEpaCalcPenDepth(a, b,
268 colDesc,
269 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
270
271 if (isValid2)
272 {
273 btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
274 btScalar lenSqr = tmpNormalInB.length2();
275 if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
276 {
277 tmpNormalInB = m_cachedSeparatingAxis;
278 lenSqr = m_cachedSeparatingAxis.length2();
279 }
280
281 if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
282 {
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))
287 {
288 distance = distance2;
289 pointOnA = tmpPointOnA;
290 pointOnB = tmpPointOnB;
291 normalInB = tmpNormalInB;
292
293 isValid = true;
294 m_lastUsedMethod = 3;
295 }
296 else
297 {
298 m_lastUsedMethod = 8;
299 }
300 }
301 else
302 {
303 m_lastUsedMethod = 9;
304 }
305 }
306 else
307
308 {
314
315 if (m_cachedSeparatingAxis.length2() > btScalar(0.))
316 {
317 btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
318 //only replace valid distances when the distance is less
319 if (!isValid || (distance2 < distance))
320 {
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();
328
329 isValid = true;
330 m_lastUsedMethod = 6;
331 }
332 else
333 {
334 m_lastUsedMethod = 5;
335 }
336 }
337 }
338 }
339 }
340
341 if (isValid && ((distance < 0) || (distance * distance < colDesc.m_maximumDistanceSquared)))
342 {
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;
349 return 0;
350 }
351 return -m_lastUsedMethod;
352 }
353
354 #endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btGjkCollisionDescription
Definition: btGjkCollisionDescription.h:22
btVoronoiSimplexSolver::fullSimplex
bool fullSimplex() const
Definition: btVoronoiSimplexSolver.h:152
btGjkEpaCalcPenDepth
bool btGjkEpaCalcPenDepth(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVector3 &v, btVector3 &wWitnessOnA, btVector3 &wWitnessOnB)
Definition: btComputeGjkEpaPenetration.h:25
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btGjkCollisionDescription::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition: btGjkCollisionDescription.h:25
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btVoronoiSimplexSolver::inSimplex
bool inSimplex(const btVector3 &w)
Definition: btVoronoiSimplexSolver.cpp:269
btGjkEpaSolver3::sResults
Definition: btGjkEpa3.h:35
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btGjkCollisionDescription::m_firstDir
btVector3 m_firstDir
Definition: btGjkCollisionDescription.h:23
btGjkEpaSolver3::sResults::normal
btVector3 normal
Definition: btGjkEpa3.h:44
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btVoronoiSimplexSolver
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
Definition: btVoronoiSimplexSolver.h:99
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
btGjkEpa3.h
btGjkCollisionDescription.h
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVoronoiSimplexSolver.h
btVoronoiSimplexSolver::reset
void reset()
Definition: btVoronoiSimplexSolver.cpp:59
btTransform.h
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btGjkEpaSolver3_Penetration
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:931
btGjkCollisionDescription::m_gjkRelError2
btScalar m_gjkRelError2
Definition: btGjkCollisionDescription.h:26
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btVoronoiSimplexSolver::addVertex
void addVertex(const btVector3 &w, const btVector3 &p, const btVector3 &q)
Definition: btVoronoiSimplexSolver.cpp:69
btComputeGjkEpaPenetration
int btComputeGjkEpaPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVoronoiSimplexSolver &simplexSolver, btGjkDistanceTemplate *distInfo)
Definition: btComputeGjkEpaPenetration.h:61
btGjkEpaSolver3_Distance
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:898
btVoronoiSimplexSolver::compute_points
void compute_points(btVector3 &p1, btVector3 &p2)
Definition: btVoronoiSimplexSolver.cpp:306
btGjkEpaSolver3::sResults::witnesses
btVector3 witnesses[2]
Definition: btGjkEpa3.h:43
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btGjkCollisionDescription::m_maxGjkIterations
int m_maxGjkIterations
Definition: btGjkCollisionDescription.h:24
btVoronoiSimplexSolver::closest
bool closest(btVector3 &v)
Definition: btVoronoiSimplexSolver.cpp:236
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251