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"
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 
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
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:902
#define SIMD_EPSILON
Definition: btScalar.h:494
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:853
#define BT_LARGE_FLOAT
Definition: btScalar.h:280
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
#define btAssert(x)
Definition: btScalar.h:113
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
bool inSimplex(const btVector3 &w)
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
bool btGjkEpaCalcPenDepth(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVector3 &v, btVector3 &wWitnessOnA, btVector3 &wWitnessOnB)
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
void setZero()
Definition: btVector3.h:671
void addVertex(const btVector3 &w, const btVector3 &p, const btVector3 &q)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:866
void compute_points(btVector3 &p1, btVector3 &p2)
int btComputeGjkEpaPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVoronoiSimplexSolver &simplexSolver, btGjkDistanceTemplate *distInfo)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278