Bullet Collision Detection & Physics Library
btMinkowskiPenetrationDepthSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 #include "btMinkowskiPenetrationDepthSolver.h"
17 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
18 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
19 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
21 
22 #define NUM_UNITSPHERE_POINTS 42
23 
24 
25 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
26  const btConvexShape* convexA,const btConvexShape* convexB,
27  const btTransform& transA,const btTransform& transB,
28  btVector3& v, btVector3& pa, btVector3& pb,
29  class btIDebugDraw* debugDraw
30  )
31 {
32 
33  (void)v;
34 
35  bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
36 
37  struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
38  {
39 
40  btIntermediateResult():m_hasResult(false)
41  {
42  }
43 
44  btVector3 m_normalOnBInWorld;
45  btVector3 m_pointInWorld;
46  btScalar m_depth;
47  bool m_hasResult;
48 
49  virtual void setShapeIdentifiersA(int partId0,int index0)
50  {
51  (void)partId0;
52  (void)index0;
53  }
54  virtual void setShapeIdentifiersB(int partId1,int index1)
55  {
56  (void)partId1;
57  (void)index1;
58  }
59  void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
60  {
61  m_normalOnBInWorld = normalOnBInWorld;
62  m_pointInWorld = pointInWorld;
63  m_depth = depth;
64  m_hasResult = true;
65  }
66  };
67 
68  //just take fixed number of orientation, and sample the penetration depth in that direction
69  btScalar minProj = btScalar(BT_LARGE_FLOAT);
70  btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
71  btVector3 minA,minB;
72  btVector3 seperatingAxisInA,seperatingAxisInB;
73  btVector3 pInA,qInB,pWorld,qWorld,w;
74 
75 #ifndef __SPU__
76 #define USE_BATCHED_SUPPORT 1
77 #endif
78 #ifdef USE_BATCHED_SUPPORT
79 
80  btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
81  btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
82  btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
83  btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
84  int i;
85 
86  int numSampleDirections = NUM_UNITSPHERE_POINTS;
87 
88  for (i=0;i<numSampleDirections;i++)
89  {
90  btVector3 norm = getPenetrationDirections()[i];
91  seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
92  seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
93  }
94 
95  {
96  int numPDA = convexA->getNumPreferredPenetrationDirections();
97  if (numPDA)
98  {
99  for (int i=0;i<numPDA;i++)
100  {
101  btVector3 norm;
102  convexA->getPreferredPenetrationDirection(i,norm);
103  norm = transA.getBasis() * norm;
104  getPenetrationDirections()[numSampleDirections] = norm;
105  seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
106  seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
107  numSampleDirections++;
108  }
109  }
110  }
111 
112  {
113  int numPDB = convexB->getNumPreferredPenetrationDirections();
114  if (numPDB)
115  {
116  for (int i=0;i<numPDB;i++)
117  {
118  btVector3 norm;
119  convexB->getPreferredPenetrationDirection(i,norm);
120  norm = transB.getBasis() * norm;
121  getPenetrationDirections()[numSampleDirections] = norm;
122  seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
123  seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
124  numSampleDirections++;
125  }
126  }
127  }
128 
129 
130 
131 
132  convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
133  convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
134 
135  for (i=0;i<numSampleDirections;i++)
136  {
137  btVector3 norm = getPenetrationDirections()[i];
138  if (check2d)
139  {
140  norm[2] = 0.f;
141  }
142  if (norm.length2()>0.01)
143  {
144 
145  seperatingAxisInA = seperatingAxisInABatch[i];
146  seperatingAxisInB = seperatingAxisInBBatch[i];
147 
148  pInA = supportVerticesABatch[i];
149  qInB = supportVerticesBBatch[i];
150 
151  pWorld = transA(pInA);
152  qWorld = transB(qInB);
153  if (check2d)
154  {
155  pWorld[2] = 0.f;
156  qWorld[2] = 0.f;
157  }
158 
159  w = qWorld - pWorld;
160  btScalar delta = norm.dot(w);
161  //find smallest delta
162  if (delta < minProj)
163  {
164  minProj = delta;
165  minNorm = norm;
166  minA = pWorld;
167  minB = qWorld;
168  }
169  }
170  }
171 #else
172 
173  int numSampleDirections = NUM_UNITSPHERE_POINTS;
174 
175 #ifndef __SPU__
176  {
177  int numPDA = convexA->getNumPreferredPenetrationDirections();
178  if (numPDA)
179  {
180  for (int i=0;i<numPDA;i++)
181  {
182  btVector3 norm;
183  convexA->getPreferredPenetrationDirection(i,norm);
184  norm = transA.getBasis() * norm;
185  getPenetrationDirections()[numSampleDirections] = norm;
186  numSampleDirections++;
187  }
188  }
189  }
190 
191  {
192  int numPDB = convexB->getNumPreferredPenetrationDirections();
193  if (numPDB)
194  {
195  for (int i=0;i<numPDB;i++)
196  {
197  btVector3 norm;
198  convexB->getPreferredPenetrationDirection(i,norm);
199  norm = transB.getBasis() * norm;
200  getPenetrationDirections()[numSampleDirections] = norm;
201  numSampleDirections++;
202  }
203  }
204  }
205 #endif // __SPU__
206 
207  for (int i=0;i<numSampleDirections;i++)
208  {
209  const btVector3& norm = getPenetrationDirections()[i];
210  seperatingAxisInA = (-norm)* transA.getBasis();
211  seperatingAxisInB = norm* transB.getBasis();
212  pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
213  qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
214  pWorld = transA(pInA);
215  qWorld = transB(qInB);
216  w = qWorld - pWorld;
217  btScalar delta = norm.dot(w);
218  //find smallest delta
219  if (delta < minProj)
220  {
221  minProj = delta;
222  minNorm = norm;
223  minA = pWorld;
224  minB = qWorld;
225  }
226  }
227 #endif //USE_BATCHED_SUPPORT
228 
229  //add the margins
230 
231  minA += minNorm*convexA->getMarginNonVirtual();
232  minB -= minNorm*convexB->getMarginNonVirtual();
233  //no penetration
234  if (minProj < btScalar(0.))
235  return false;
236 
237  btScalar extraSeparation = 0.5f;
238  minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
239 
240 
241 
242 
243 
244 //#define DEBUG_DRAW 1
245 #ifdef DEBUG_DRAW
246  if (debugDraw)
247  {
248  btVector3 color(0,1,0);
249  debugDraw->drawLine(minA,minB,color);
250  color = btVector3 (1,1,1);
251  btVector3 vec = minB-minA;
252  btScalar prj2 = minNorm.dot(vec);
253  debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
254 
255  }
256 #endif //DEBUG_DRAW
257 
258 
259 
260  btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
261 
262  btScalar offsetDist = minProj;
263  btVector3 offset = minNorm * offsetDist;
264 
265 
266 
267  btGjkPairDetector::ClosestPointInput input;
268 
269  btVector3 newOrg = transA.getOrigin() + offset;
270 
271  btTransform displacedTrans = transA;
272  displacedTrans.setOrigin(newOrg);
273 
274  input.m_transformA = displacedTrans;
275  input.m_transformB = transB;
276  input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
277 
278  btIntermediateResult res;
279  gjkdet.setCachedSeperatingAxis(-minNorm);
280  gjkdet.getClosestPoints(input,res,debugDraw);
281 
282  btScalar correctedMinNorm = minProj - res.m_depth;
283 
284 
285  //the penetration depth is over-estimated, relax it
286  btScalar penetration_relaxation= btScalar(1.);
287  minNorm*=penetration_relaxation;
288 
289 
290  if (res.m_hasResult)
291  {
292 
293  pa = res.m_pointInWorld - minNorm * correctedMinNorm;
294  pb = res.m_pointInWorld;
295  v = minNorm;
296 
297 #ifdef DEBUG_DRAW
298  if (debugDraw)
299  {
300  btVector3 color(1,0,0);
301  debugDraw->drawLine(pa,pb,color);
302  }
303 #endif//DEBUG_DRAW
304 
305 
306  }
307  return res.m_hasResult;
308 }
309 
310 btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
311 {
312  static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
313  {
314  btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
315  btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
316  btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
317  btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
318  btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
319  btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
320  btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
321  btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
322  btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
323  btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
324  btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
325  btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
326  btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
327  btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
328  btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
329  btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
330  btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
331  btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
332  btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
333  btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
334  btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
335  btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
336  btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
337  btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
338  btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
339  btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
340  btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
341  btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
342  btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
343  btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
344  btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
345  btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
346  btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
347  btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
348  btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
349  btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
350  btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
351  btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
352  btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
353  btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
354  btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
355  btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
356  };
357 
358  return sPenetrationDirections;
359 }
360 
361 
btConvexShape::getNumPreferredPenetrationDirections
virtual int getNumPreferredPenetrationDirections() const =0
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btSubSimplexConvexCast.h
btDiscreteCollisionDetectorInterface::Result
Definition: btDiscreteCollisionDetectorInterface.h:31
btConvexShape.h
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btMinkowskiPenetrationDepthSolver::calcPenDepth
virtual bool calcPenDepth(btSimplexSolverInterface &simplexSolver, const btConvexShape *convexA, const btConvexShape *convexB, const btTransform &transA, const btTransform &transB, btVector3 &v, btVector3 &pa, btVector3 &pb, class btIDebugDraw *debugDraw)
Definition: btMinkowskiPenetrationDepthSolver.cpp:25
btGjkPairDetector.h
btConvexShape::getPreferredPenetrationDirection
virtual void getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const =0
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition: btDiscreteCollisionDetectorInterface.h:51
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:49
btConvexShape::localGetSupportVertexWithoutMarginNonVirtual
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
Definition: btConvexShape.cpp:129
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:29
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition: btDiscreteCollisionDetectorInterface.h:50
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btGjkPairDetector
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
Definition: btGjkPairDetector.h:30
MAX_PREFERRED_PENETRATION_DIRECTIONS
#define MAX_PREFERRED_PENETRATION_DIRECTIONS
Definition: btConvexShape.h:27
btConvexShape::getMarginNonVirtual
btScalar getMarginNonVirtual() const
Definition: btConvexShape.cpp:317
btVoronoiSimplexSolver.h
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btMinkowskiPenetrationDepthSolver.h
btIDebugDraw::drawLine
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
btMinkowskiPenetrationDepthSolver::getPenetrationDirections
static btVector3 * getPenetrationDirections()
Definition: btMinkowskiPenetrationDepthSolver.cpp:310
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
btConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const =0
btCollisionShape::isConvex2d
bool isConvex2d() const
Definition: btCollisionShape.h:68
btGjkPairDetector::setCachedSeperatingAxis
void setCachedSeperatingAxis(const btVector3 &seperatingAxis)
Definition: btGjkPairDetector.h:75
btGjkPairDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition: btGjkPairDetector.cpp:79
btSimplexSolverInterface
#define btSimplexSolverInterface
Definition: btSimplexSolverInterface.h:26
NUM_UNITSPHERE_POINTS
#define NUM_UNITSPHERE_POINTS
Definition: btMinkowskiPenetrationDepthSolver.cpp:22
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition: btDiscreteCollisionDetectorInterface.h:42
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
Generated by   doxygen 1.8.17