2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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:
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.
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"
23 #define NUM_UNITSPHERE_POINTS 42
24 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
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))
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)
82 struct btIntermediateResult : public SpuContactResult
85 btIntermediateResult():m_hasResult(false)
89 btVector3 m_normalOnBInWorld;
90 btVector3 m_pointInWorld;
94 virtual void setShapeIdentifiersA(int partId0,int index0)
100 virtual void setShapeIdentifiersB(int partId1,int index1)
105 void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
107 m_normalOnBInWorld = normalOnBInWorld;
108 m_pointInWorld = pointInWorld;
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);
119 btVector3 seperatingAxisInA,seperatingAxisInB;
120 btVector3 pInA,qInB,pWorld,qWorld,w;
122 //#define USE_BATCHED_SUPPORT 1
123 #ifdef USE_BATCHED_SUPPORT
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];
131 int numSampleDirections = NUM_UNITSPHERE_POINTS;
133 for (i=0;i<numSampleDirections;i++)
135 const btVector3& norm = sPenetrationDirections[i];
136 seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
137 seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
141 int numPDA = convexA->getNumPreferredPenetrationDirections();
144 for (int i=0;i<numPDA;i++)
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++;
158 int numPDB = convexB->getNumPreferredPenetrationDirections();
161 for (int i=0;i<numPDB;i++)
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++;
176 convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
177 convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
179 for (i=0;i<numSampleDirections;i++)
181 const btVector3& norm = sPenetrationDirections[i];
182 seperatingAxisInA = seperatingAxisInABatch[i];
183 seperatingAxisInB = seperatingAxisInBBatch[i];
185 pInA = supportVerticesABatch[i];
186 qInB = supportVerticesBBatch[i];
188 pWorld = transA(pInA);
189 qWorld = transB(qInB);
191 btScalar delta = norm.dot(w);
192 //find smallest delta
203 int numSampleDirections = NUM_UNITSPHERE_POINTS;
205 ///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh
206 #define DO_PREFERRED_DIRECTIONS 1
207 #ifdef DO_PREFERRED_DIRECTIONS
209 int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA);
212 for (int i=0;i<numPDA;i++)
215 spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm);
216 norm = transA.getBasis() * norm;
217 sPenetrationDirections[numSampleDirections] = norm;
218 numSampleDirections++;
224 int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB);
227 for (int i=0;i<numPDB;i++)
230 spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm);
231 norm = transB.getBasis() * norm;
232 sPenetrationDirections[numSampleDirections] = norm;
233 numSampleDirections++;
237 #endif //DO_PREFERRED_DIRECTIONS
239 for (int i=0;i<numSampleDirections;i++)
241 const btVector3& norm = sPenetrationDirections[i];
242 seperatingAxisInA = (-norm)* transA.getBasis();
243 seperatingAxisInB = norm* transB.getBasis();
245 pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual( seperatingAxisInA);//, NULL);
246 qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);//, NULL);
248 // pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
249 // qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
251 pWorld = transA(pInA);
252 qWorld = transB(qInB);
254 btScalar delta = norm.dot(w);
255 //find smallest delta
264 #endif //USE_BATCHED_SUPPORT
268 minA += minNorm*marginA;
269 minB -= minNorm*marginB;
271 if (minProj < btScalar(0.))
274 minProj += (marginA + marginB) + btScalar(1.00);
280 //#define DEBUG_DRAW 1
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);
295 btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
297 btScalar offsetDist = minProj;
298 btVector3 offset = minNorm * offsetDist;
301 SpuClosestPointInput input;
302 input.m_convexVertexData[0] = convexVertexDataA;
303 input.m_convexVertexData[1] = convexVertexDataB;
304 btVector3 newOrg = transA.getOrigin() + offset;
306 btTransform displacedTrans = transA;
307 displacedTrans.setOrigin(newOrg);
309 input.m_transformA = displacedTrans;
310 input.m_transformB = transB;
311 input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
313 btIntermediateResult res;
314 gjkdet.getClosestPoints(input,res,0);
316 btScalar correctedMinNorm = minProj - res.m_depth;
319 //the penetration depth is over-estimated, relax it
320 btScalar penetration_relaxation= btScalar(1.);
321 minNorm*=penetration_relaxation;
326 pa = res.m_pointInWorld - minNorm * correctedMinNorm;
327 pb = res.m_pointInWorld;
332 btVector3 color(1,0,0);
333 debugDraw->drawLine(pa,pb,color);
339 // could not seperate shapes
342 return res.m_hasResult;