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