Fix for bug #19817: cloth simulation with collision slow on Mac.
[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
75                                                                                                    )
76 {
77
78         (void)v;
79         
80
81         struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
82         {
83
84                 btIntermediateResult():m_hasResult(false)
85                 {
86                 }
87                 
88                 btVector3 m_normalOnBInWorld;
89                 btVector3 m_pointInWorld;
90                 btScalar m_depth;
91                 bool    m_hasResult;
92
93                 virtual void setShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
94                 {
95                         (void)partId0;
96                         (void)index0;
97                         (void)partId1;
98                         (void)index1;
99                 }
100                 void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
101                 {
102                         m_normalOnBInWorld = normalOnBInWorld;
103                         m_pointInWorld = pointInWorld;
104                         m_depth = depth;
105                         m_hasResult = true;
106                 }
107         };
108
109         //just take fixed number of orientation, and sample the penetration depth in that direction
110         btScalar minProj = btScalar(1e30);
111         btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
112         btVector3 minA,minB;
113         btVector3 seperatingAxisInA,seperatingAxisInB;
114         btVector3 pInA,qInB,pWorld,qWorld,w;
115
116 #ifndef __SPU__
117 #define USE_BATCHED_SUPPORT 1
118 #endif
119 #ifdef USE_BATCHED_SUPPORT
120
121         btVector3       supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
122         btVector3       supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
123         btVector3       seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
124         btVector3       seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
125         int i;
126
127         int numSampleDirections = NUM_UNITSPHERE_POINTS;
128
129         for (i=0;i<numSampleDirections;i++)
130         {
131                 const btVector3& norm = sPenetrationDirections[i];
132                 seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
133                 seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
134         }
135
136         {
137                 int numPDA = convexA->getNumPreferredPenetrationDirections();
138                 if (numPDA)
139                 {
140                         for (int i=0;i<numPDA;i++)
141                         {
142                                 btVector3 norm;
143                                 convexA->getPreferredPenetrationDirection(i,norm);
144                                 norm  = transA.getBasis() * norm;
145                                 sPenetrationDirections[numSampleDirections] = norm;
146                                 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
147                                 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
148                                 numSampleDirections++;
149                         }
150                 }
151         }
152
153         {
154                 int numPDB = convexB->getNumPreferredPenetrationDirections();
155                 if (numPDB)
156                 {
157                         for (int i=0;i<numPDB;i++)
158                         {
159                                 btVector3 norm;
160                                 convexB->getPreferredPenetrationDirection(i,norm);
161                                 norm  = transB.getBasis() * norm;
162                                 sPenetrationDirections[numSampleDirections] = norm;
163                                 seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
164                                 seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
165                                 numSampleDirections++;
166                         }
167                 }
168         }
169
170
171
172         convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
173         convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
174
175         for (i=0;i<numSampleDirections;i++)
176         {
177                 const btVector3& norm = sPenetrationDirections[i];
178                 seperatingAxisInA = seperatingAxisInABatch[i];
179                 seperatingAxisInB = seperatingAxisInBBatch[i];
180
181                 pInA = supportVerticesABatch[i];
182                 qInB = supportVerticesBBatch[i];
183
184                 pWorld = transA(pInA);  
185                 qWorld = transB(qInB);
186                 w       = qWorld - pWorld;
187                 btScalar delta = norm.dot(w);
188                 //find smallest delta
189                 if (delta < minProj)
190                 {
191                         minProj = delta;
192                         minNorm = norm;
193                         minA = pWorld;
194                         minB = qWorld;
195                 }
196         }       
197 #else
198
199         int numSampleDirections = NUM_UNITSPHERE_POINTS;
200
201 #ifndef __SPU__
202         {
203                 int numPDA = convexA->getNumPreferredPenetrationDirections();
204                 if (numPDA)
205                 {
206                         for (int i=0;i<numPDA;i++)
207                         {
208                                 btVector3 norm;
209                                 convexA->getPreferredPenetrationDirection(i,norm);
210                                 norm  = transA.getBasis() * norm;
211                                 sPenetrationDirections[numSampleDirections] = norm;
212                                 numSampleDirections++;
213                         }
214                 }
215         }
216
217         {
218                 int numPDB = convexB->getNumPreferredPenetrationDirections();
219                 if (numPDB)
220                 {
221                         for (int i=0;i<numPDB;i++)
222                         {
223                                 btVector3 norm;
224                                 convexB->getPreferredPenetrationDirection(i,norm);
225                                 norm  = transB.getBasis() * norm;
226                                 sPenetrationDirections[numSampleDirections] = norm;
227                                 numSampleDirections++;
228                         }
229                 }
230         }
231 #endif // __SPU__
232
233         for (int i=0;i<numSampleDirections;i++)
234         {
235                 const btVector3& norm = sPenetrationDirections[i];
236                 seperatingAxisInA = (-norm)* transA.getBasis();
237                 seperatingAxisInB = norm* transB.getBasis();
238                 pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
239                 qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
240                 pWorld = transA(pInA);  
241                 qWorld = transB(qInB);
242                 w       = qWorld - pWorld;
243                 btScalar delta = norm.dot(w);
244                 //find smallest delta
245                 if (delta < minProj)
246                 {
247                         minProj = delta;
248                         minNorm = norm;
249                         minA = pWorld;
250                         minB = qWorld;
251                 }
252         }
253 #endif //USE_BATCHED_SUPPORT
254
255         //add the margins
256
257         minA += minNorm*convexA->getMarginNonVirtual();
258         minB -= minNorm*convexB->getMarginNonVirtual();
259         //no penetration
260         if (minProj < btScalar(0.))
261                 return false;
262
263         minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
264
265
266
267
268
269 //#define DEBUG_DRAW 1
270 #ifdef DEBUG_DRAW
271         if (debugDraw)
272         {
273                 btVector3 color(0,1,0);
274                 debugDraw->drawLine(minA,minB,color);
275                 color = btVector3 (1,1,1);
276                 btVector3 vec = minB-minA;
277                 btScalar prj2 = minNorm.dot(vec);
278                 debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
279
280         }
281 #endif //DEBUG_DRAW
282
283         
284
285         btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
286
287         btScalar offsetDist = minProj;
288         btVector3 offset = minNorm * offsetDist;
289         
290
291
292         btGjkPairDetector::ClosestPointInput input;
293                 
294         btVector3 newOrg = transA.getOrigin() + offset;
295
296         btTransform displacedTrans = transA;
297         displacedTrans.setOrigin(newOrg);
298
299         input.m_transformA = displacedTrans;
300         input.m_transformB = transB;
301         input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
302         
303         btIntermediateResult res;
304         gjkdet.getClosestPoints(input,res,debugDraw);
305
306         btScalar correctedMinNorm = minProj - res.m_depth;
307
308
309         //the penetration depth is over-estimated, relax it
310         btScalar penetration_relaxation= btScalar(1.);
311         minNorm*=penetration_relaxation;
312
313         if (res.m_hasResult)
314         {
315
316                 pa = res.m_pointInWorld - minNorm * correctedMinNorm;
317                 pb = res.m_pointInWorld;
318                 
319 #ifdef DEBUG_DRAW
320                 if (debugDraw)
321                 {
322                         btVector3 color(1,0,0);
323                         debugDraw->drawLine(pa,pb,color);
324                 }
325 #endif//DEBUG_DRAW
326
327
328         }
329         return res.m_hasResult;
330 }
331
332
333