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