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
25
26
27
28
29 template <typename btConvexTemplate>
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>
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 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 {
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
btGjkEpaSolver3_Penetration
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:902
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:495
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:856
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:281
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btVoronoiSimplexSolver::fullSimplex
bool fullSimplex() const
Definition: btVoronoiSimplexSolver.h:157
btGjkCollisionDescription::m_firstDir
btVector3 m_firstDir
Definition: btGjkCollisionDescription.h:24
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:419
btAssert
#define btAssert(x)
Definition: btScalar.h:114
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:573
btTransform.h
btVoronoiSimplexSolver::reset
void reset()
Definition: btVoronoiSimplexSolver.cpp:66
btGjkCollisionDescription::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition: btGjkCollisionDescription.h:26
btVoronoiSimplexSolver::inSimplex
bool inSimplex(const btVector3 &w)
Definition: btVoronoiSimplexSolver.cpp:283
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:307
btGjkCollisionDescription
Definition: btGjkCollisionDescription.h:22
btGjkEpaSolver3::sResults
Definition: btGjkEpa3.h:36
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:575
btVoronoiSimplexSolver
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
Definition: btVoronoiSimplexSolver.h:95
btGjkEpaCalcPenDepth
bool btGjkEpaCalcPenDepth(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVector3 &v, btVector3 &wWitnessOnA, btVector3 &wWitnessOnB)
Definition: btComputeGjkEpaPenetration.h:30
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btVector3::setZero
void setZero()
Definition: btVector3.h:681
btVoronoiSimplexSolver::addVertex
void addVertex(const btVector3 &w, const btVector3 &p, const btVector3 &q)
Definition: btVoronoiSimplexSolver.cpp:78
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btGjkCollisionDescription::m_gjkRelError2
btScalar m_gjkRelError2
Definition: btGjkCollisionDescription.h:27
btGjkEpaSolver3_Distance
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:866
btGjkCollisionDescription::m_maxGjkIterations
int m_maxGjkIterations
Definition: btGjkCollisionDescription.h:25
btGjkCollisionDescription.h
btVoronoiSimplexSolver.h
btVoronoiSimplexSolver::compute_points
void compute_points(btVector3 &p1, btVector3 &p2)
Definition: btVoronoiSimplexSolver.cpp:322
btComputeGjkEpaPenetration
int btComputeGjkEpaPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVoronoiSimplexSolver &simplexSolver, btGjkDistanceTemplate *distInfo)
Definition: btComputeGjkEpaPenetration.h:66
btGjkEpa3.h
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:571
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btVoronoiSimplexSolver::closest
bool closest(btVector3 &v)
Definition: btVoronoiSimplexSolver.cpp:243