Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @ 1963

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

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 11.0 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 "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
23static btVector3        sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
24{
25btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
26btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
27btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
28btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
29btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
30btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
31btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
32btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
33btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
34btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
35btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
36btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
37btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
38btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
39btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
40btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
41btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
42btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
43btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
44btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
45btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
46btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
47btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
48btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
49btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
50btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
51btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
52btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
53btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
54btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
55btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
56btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
57btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
58btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
59btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
60btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
61btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
62btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
63btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
64btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
65btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
66btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
67};
68
69
70bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
71                                                                                                   const btConvexShape* convexA,const btConvexShape* convexB,
72                                                                                                   const btTransform& transA,const btTransform& transB,
73                                                                                                   btVector3& v, btPoint3& pa, btPoint3& 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
Note: See TracBrowser for help on using the repository browser.