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