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
calcPenDepth(btSimplexSolverInterface & simplexSolver,const btConvexShape * convexA,const btConvexShape * convexB,const btTransform & transA,const btTransform & transB,btVector3 & v,btVector3 & pa,btVector3 & pb,class btIDebugDraw * debugDraw)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;///scale dependent
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
getPenetrationDirections()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
362