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