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