Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp @ 1966

Last change on this file since 1966 was 1966, checked in by rgrieder, 16 years ago

Let's go for multithreaded physics!
  • Property svn:eol-style set to native
File size: 11.4 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#include "SpuMinkowskiPenetrationDepthSolver.h"
17#include "SpuVoronoiSimplexSolver.h"
18#include "SpuGjkPairDetector.h"
19#include "SpuContactResult.h"
20#include "SpuPreferredPenetrationDirections.h"
21
22
23#include "SpuCollisionShapes.h"
24
25#define NUM_UNITSPHERE_POINTS 42
26static btVector3        sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
27{
28btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
29btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
30btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
31btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
32btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
33btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
34btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
35btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
36btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
37btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
38btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
39btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
40btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
41btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
42btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
43btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
44btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
45btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
46btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
47btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
48btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
49btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
50btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
51btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
52btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
53btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
54btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
55btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
56btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
57btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
58btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
59btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
60btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
61btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
62btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
63btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
64btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
65btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
66btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
67btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
68btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
69btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
70};
71
72bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
73                void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
74            btTransform& transA,const btTransform& transB,
75                        btVector3& v, btPoint3& pa, btPoint3& pb,
76                        class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
77                        struct SpuConvexPolyhedronVertexData* convexVertexDataA,
78                        struct SpuConvexPolyhedronVertexData* convexVertexDataB
79                        ) const
80{
81
82        (void)stackAlloc;
83        (void)v;
84       
85
86        struct btIntermediateResult : public SpuContactResult
87        {
88
89                btIntermediateResult():m_hasResult(false)
90                {
91                }
92               
93                btVector3 m_normalOnBInWorld;
94                btVector3 m_pointInWorld;
95                btScalar m_depth;
96                bool    m_hasResult;
97
98                virtual void setShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
99                {
100                        (void)partId0;
101                        (void)index0;
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(1e30);
116        btVector3 minNorm;
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 = localGetSupportingVertexWithoutMargin(shapeTypeA, convexA, seperatingAxisInA,convexVertexDataA);//, NULL);
246                qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, convexB, seperatingAxisInB,convexVertexDataB);//, 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
296        SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0);
297
298        btScalar offsetDist = minProj;
299        btVector3 offset = minNorm * offsetDist;
300       
301
302        SpuClosestPointInput input;
303        input.m_convexVertexData[0] = convexVertexDataA;
304        input.m_convexVertexData[1] = convexVertexDataB;
305        btVector3 newOrg = transA.getOrigin() + offset;
306
307        btTransform displacedTrans = transA;
308        displacedTrans.setOrigin(newOrg);
309
310        input.m_transformA = displacedTrans;
311        input.m_transformB = transB;
312        input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
313       
314        btIntermediateResult res;
315        gjkdet.getClosestPoints(input,res);
316
317        btScalar correctedMinNorm = minProj - res.m_depth;
318
319
320        //the penetration depth is over-estimated, relax it
321        btScalar penetration_relaxation= btScalar(1.);
322        minNorm*=penetration_relaxation;
323
324        if (res.m_hasResult)
325        {
326
327                pa = res.m_pointInWorld - minNorm * correctedMinNorm;
328                pb = res.m_pointInWorld;
329               
330#ifdef DEBUG_DRAW
331                if (debugDraw)
332                {
333                        btVector3 color(1,0,0);
334                        debugDraw->drawLine(pa,pb,color);
335                }
336#endif//DEBUG_DRAW
337
338
339        } else {
340                // could not seperate shapes
341                btAssert (false);
342        }
343        return res.m_hasResult;
344}
345
346
347
Note: See TracBrowser for help on using the repository browser.