Blender  V3.3
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 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 
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 
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
168  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  {
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  {
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
ATTR_WARN_UNUSED_RESULT const BMVert * v
bool btGjkEpaCalcPenDepth(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVector3 &v, btVector3 &wWitnessOnA, btVector3 &wWitnessOnB)
int btComputeGjkEpaPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btVoronoiSimplexSolver &simplexSolver, btGjkDistanceTemplate *distInfo)
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:931
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:898
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
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. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE btScalar distance2(const btVector3 &v) const
Return the distance squared between the ends of this and another vector This is symantically treating...
Definition: btVector3.h:939
btVoronoiSimplexSolver
SyclQueue void void size_t num_bytes void
static unsigned a[3]
Definition: RandGen.cpp:78
T length(const vec_base< T, Size > &a)
T distance(const T &a, const T &b)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
btVector3 witnesses[2]
Definition: btGjkEpa3.h:43