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)
76 {
77 #if 0
78  (void)v;
79 
80 
81  struct btIntermediateResult : public SpuContactResult
82  {
83 
84  btIntermediateResult():m_hasResult(false)
85  {
86  }
87 
88  btVector3 m_normalOnBInWorld;
89  btVector3 m_pointInWorld;
90  btScalar m_depth;
91  bool m_hasResult;
92 
93  virtual void setShapeIdentifiersA(int partId0,int index0)
94  {
95  (void)partId0;
96  (void)index0;
97  }
98 
99  virtual void setShapeIdentifiersB(int partId1,int index1)
100  {
101  (void)partId1;
102  (void)index1;
103  }
104  void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
105  {
106  m_normalOnBInWorld = normalOnBInWorld;
107  m_pointInWorld = pointInWorld;
108  m_depth = depth;
109  m_hasResult = true;
110  }
111  };
112 
113  //just take fixed number of orientation, and sample the penetration depth in that direction
114  btScalar minProj = btScalar(BT_LARGE_FLOAT);
115  btVector3 minNorm(0.f,0.f,0.f);
116  btVector3 minVertex;
117  btVector3 minA,minB;
118  btVector3 seperatingAxisInA,seperatingAxisInB;
119  btVector3 pInA,qInB,pWorld,qWorld,w;
120 
121 //#define USE_BATCHED_SUPPORT 1
122 #ifdef USE_BATCHED_SUPPORT
123 
124  btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
125  btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
126  btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
127  btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
128  int i;
129 
130  int numSampleDirections = NUM_UNITSPHERE_POINTS;
131 
132  for (i=0;i<numSampleDirections;i++)
133  {
134  const btVector3& norm = sPenetrationDirections[i];
135  seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
136  seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
137  }
138 
139  {
140  int numPDA = convexA->getNumPreferredPenetrationDirections();
141  if (numPDA)
142  {
143  for (int i=0;i<numPDA;i++)
144  {
145  btVector3 norm;
146  convexA->getPreferredPenetrationDirection(i,norm);
147  norm = transA.getBasis() * norm;
148  sPenetrationDirections[numSampleDirections] = norm;
149  seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
150  seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
151  numSampleDirections++;
152  }
153  }
154  }
155 
156  {
157  int numPDB = convexB->getNumPreferredPenetrationDirections();
158  if (numPDB)
159  {
160  for (int i=0;i<numPDB;i++)
161  {
162  btVector3 norm;
163  convexB->getPreferredPenetrationDirection(i,norm);
164  norm = transB.getBasis() * norm;
165  sPenetrationDirections[numSampleDirections] = norm;
166  seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
167  seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
168  numSampleDirections++;
169  }
170  }
171  }
172 
173 
174 
175  convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
176  convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
177 
178  for (i=0;i<numSampleDirections;i++)
179  {
180  const btVector3& norm = sPenetrationDirections[i];
181  seperatingAxisInA = seperatingAxisInABatch[i];
182  seperatingAxisInB = seperatingAxisInBBatch[i];
183 
184  pInA = supportVerticesABatch[i];
185  qInB = supportVerticesBBatch[i];
186 
187  pWorld = transA(pInA);
188  qWorld = transB(qInB);
189  w = qWorld - pWorld;
190  btScalar delta = norm.dot(w);
191  //find smallest delta
192  if (delta < minProj)
193  {
194  minProj = delta;
195  minNorm = norm;
196  minA = pWorld;
197  minB = qWorld;
198  }
199  }
200 #else
201 
202  int numSampleDirections = NUM_UNITSPHERE_POINTS;
203 
205 #define DO_PREFERRED_DIRECTIONS 1
206 #ifdef DO_PREFERRED_DIRECTIONS
207  {
208  int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA);
209  if (numPDA)
210  {
211  for (int i=0;i<numPDA;i++)
212  {
213  btVector3 norm;
214  spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm);
215  norm = transA.getBasis() * norm;
216  sPenetrationDirections[numSampleDirections] = norm;
217  numSampleDirections++;
218  }
219  }
220  }
221 
222  {
223  int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB);
224  if (numPDB)
225  {
226  for (int i=0;i<numPDB;i++)
227  {
228  btVector3 norm;
229  spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm);
230  norm = transB.getBasis() * norm;
231  sPenetrationDirections[numSampleDirections] = norm;
232  numSampleDirections++;
233  }
234  }
235  }
236 #endif //DO_PREFERRED_DIRECTIONS
237 
238  for (int i=0;i<numSampleDirections;i++)
239  {
240  const btVector3& norm = sPenetrationDirections[i];
241  seperatingAxisInA = (-norm)* transA.getBasis();
242  seperatingAxisInB = norm* transB.getBasis();
243 
244  pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual( seperatingAxisInA);//, NULL);
245  qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);//, NULL);
246 
247  // pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
248  // qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
249 
250  pWorld = transA(pInA);
251  qWorld = transB(qInB);
252  w = qWorld - pWorld;
253  btScalar delta = norm.dot(w);
254  //find smallest delta
255  if (delta < minProj)
256  {
257  minProj = delta;
258  minNorm = norm;
259  minA = pWorld;
260  minB = qWorld;
261  }
262  }
263 #endif //USE_BATCHED_SUPPORT
264 
265  //add the margins
266 
267  minA += minNorm*marginA;
268  minB -= minNorm*marginB;
269  //no penetration
270  if (minProj < btScalar(0.))
271  return false;
272 
273  minProj += (marginA + marginB) + btScalar(1.00);
274 
275 
276 
277 
278 
279 //#define DEBUG_DRAW 1
280 #ifdef DEBUG_DRAW
281  if (debugDraw)
282  {
283  btVector3 color(0,1,0);
284  debugDraw->drawLine(minA,minB,color);
285  color = btVector3 (1,1,1);
286  btVector3 vec = minB-minA;
287  btScalar prj2 = minNorm.dot(vec);
288  debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
289 
290  }
291 #endif //DEBUG_DRAW
292 
293 
294  btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
295 
296  btScalar offsetDist = minProj;
297  btVector3 offset = minNorm * offsetDist;
298 
299 
300  SpuClosestPointInput input;
301  input.m_convexVertexData[0] = convexVertexDataA;
302  input.m_convexVertexData[1] = convexVertexDataB;
303  btVector3 newOrg = transA.getOrigin() + offset;
304 
305  btTransform displacedTrans = transA;
306  displacedTrans.setOrigin(newOrg);
307 
308  input.m_transformA = displacedTrans;
309  input.m_transformB = transB;
310  input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
311 
312  btIntermediateResult res;
313  gjkdet.getClosestPoints(input,res,0);
314 
315  btScalar correctedMinNorm = minProj - res.m_depth;
316 
317 
318  //the penetration depth is over-estimated, relax it
319  btScalar penetration_relaxation= btScalar(1.);
320  minNorm*=penetration_relaxation;
321 
322  if (res.m_hasResult)
323  {
324 
325  pa = res.m_pointInWorld - minNorm * correctedMinNorm;
326  pb = res.m_pointInWorld;
327 
328 #ifdef DEBUG_DRAW
329  if (debugDraw)
330  {
331  btVector3 color(1,0,0);
332  debugDraw->drawLine(pa,pb,color);
333  }
334 #endif//DEBUG_DRAW
335 
336 
337  } else {
338  // could not seperate shapes
339  //btAssert (false);
340  }
341  return res.m_hasResult;
342 #endif
343  return false;
344 }
345 
346 
347 
Generated on Wed Apr 9 2014 02:38:25 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2