Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

Added Bullet physics engine.

  • Property svn:eol-style set to native
File size: 10.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 "btGjkPairDetector.h"
17#include "BulletCollision/CollisionShapes/btConvexShape.h"
18#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
19#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
20
21
22
23#if defined(DEBUG) || defined (_DEBUG)
24//#define TEST_NON_VIRTUAL 1
25#include <stdio.h> //for debug printf
26#ifdef __SPU__
27#include <spu_printf.h>
28#define printf spu_printf
29//#define DEBUG_SPU_COLLISION_DETECTION 1
30#endif //__SPU__
31#endif
32
33//must be above the machine epsilon
34#define REL_ERROR2 btScalar(1.0e-6)
35
36//temp globals, to improve GJK/EPA/penetration calculations
37int gNumDeepPenetrationChecks = 0;
38int gNumGjkChecks = 0;
39
40
41
42btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*  penetrationDepthSolver)
43:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
44m_penetrationDepthSolver(penetrationDepthSolver),
45m_simplexSolver(simplexSolver),
46m_minkowskiA(objectA),
47m_minkowskiB(objectB),
48m_ignoreMargin(false),
49m_lastUsedMethod(-1),
50m_catchDegeneracies(1)
51{
52}
53
54void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
55{
56        btScalar distance=btScalar(0.);
57        btVector3       normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
58        btVector3 pointOnA,pointOnB;
59        btTransform     localTransA = input.m_transformA;
60        btTransform localTransB = input.m_transformB;
61        btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
62        localTransA.getOrigin() -= positionOffset;
63        localTransB.getOrigin() -= positionOffset;
64
65
66        btScalar marginA = m_minkowskiA->getMarginNonVirtual();
67        btScalar marginB = m_minkowskiB->getMarginNonVirtual();
68
69#ifdef TEST_NON_VIRTUAL
70        btScalar marginAv = m_minkowskiA->getMargin();
71        btScalar marginBv = m_minkowskiB->getMargin();
72
73        btAssert(marginA == marginAv);
74        btAssert(marginB == marginBv);
75#endif //TEST_NON_VIRTUAL
76
77        gNumGjkChecks++;
78
79#ifdef DEBUG_SPU_COLLISION_DETECTION
80        spu_printf("inside gjk\n");
81#endif
82        //for CCD we don't use margins
83        if (m_ignoreMargin)
84        {
85                marginA = btScalar(0.);
86                marginB = btScalar(0.);
87#ifdef DEBUG_SPU_COLLISION_DETECTION
88                spu_printf("ignoring margin\n");
89#endif
90        }
91
92        m_curIter = 0;
93        int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
94        m_cachedSeparatingAxis.setValue(0,1,0);
95
96        bool isValid = false;
97        bool checkSimplex = false;
98        bool checkPenetration = true;
99        m_degenerateSimplex = 0;
100
101        m_lastUsedMethod = -1;
102
103        {
104                btScalar squaredDistance = SIMD_INFINITY;
105                btScalar delta = btScalar(0.);
106               
107                btScalar margin = marginA + marginB;
108               
109               
110
111                m_simplexSolver->reset();
112               
113                for ( ; ; )
114                //while (true)
115                {
116
117                        btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
118                        btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
119
120#ifdef TEST_NON_VIRTUAL
121                        btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
122                        btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
123#endif
124                        btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
125                        btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
126#ifdef TEST_NON_VIRTUAL
127                        btAssert((pInAv-pInA).length() < 0.0001);
128                        btAssert((qInBv-qInB).length() < 0.0001);
129#endif //
130
131                        btPoint3  pWorld = localTransA(pInA);   
132                        btPoint3  qWorld = localTransB(qInB);
133
134#ifdef DEBUG_SPU_COLLISION_DETECTION
135                spu_printf("got local supporting vertices\n");
136#endif
137
138                        btVector3 w     = pWorld - qWorld;
139                        delta = m_cachedSeparatingAxis.dot(w);
140
141                        // potential exit, they don't overlap
142                        if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
143                        {
144                                checkPenetration = false;
145                                break;
146                        }
147
148                        //exit 0: the new point is already in the simplex, or we didn't come any closer
149                        if (m_simplexSolver->inSimplex(w))
150                        {
151                                m_degenerateSimplex = 1;
152                                checkSimplex = true;
153                                break;
154                        }
155                        // are we getting any closer ?
156                        btScalar f0 = squaredDistance - delta;
157                        btScalar f1 = squaredDistance * REL_ERROR2;
158
159                        if (f0 <= f1)
160                        {
161                                if (f0 <= btScalar(0.))
162                                {
163                                        m_degenerateSimplex = 2;
164                                }
165                                checkSimplex = true;
166                                break;
167                        }
168
169#ifdef DEBUG_SPU_COLLISION_DETECTION
170                spu_printf("addVertex 1\n");
171#endif
172                        //add current vertex to simplex
173                        m_simplexSolver->addVertex(w, pWorld, qWorld);
174#ifdef DEBUG_SPU_COLLISION_DETECTION
175                spu_printf("addVertex 2\n");
176#endif
177                        //calculate the closest point to the origin (update vector v)
178                        if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
179                        {
180                                m_degenerateSimplex = 3;
181                                checkSimplex = true;
182                                break;
183                        }
184
185                        if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
186            {
187                m_degenerateSimplex = 6;
188                checkSimplex = true;
189                break;
190            }
191
192                        btScalar previousSquaredDistance = squaredDistance;
193                        squaredDistance = m_cachedSeparatingAxis.length2();
194                       
195                        //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
196
197                        //are we getting any closer ?
198                        if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
199                        { 
200                                m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
201                                checkSimplex = true;
202                                break;
203                        }
204
205                          //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
206              if (m_curIter++ > gGjkMaxIter)   
207              {   
208                      #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
209
210                              printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
211                              printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
212                              m_cachedSeparatingAxis.getX(),   
213                              m_cachedSeparatingAxis.getY(),   
214                              m_cachedSeparatingAxis.getZ(),   
215                              squaredDistance,   
216                              m_minkowskiA->getShapeType(),   
217                              m_minkowskiB->getShapeType());   
218
219                      #endif   
220                      break;   
221
222              } 
223
224
225                        bool check = (!m_simplexSolver->fullSimplex());
226                        //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
227
228                        if (!check)
229                        {
230                                //do we need this backup_closest here ?
231                                m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
232                                break;
233                        }
234                }
235
236                if (checkSimplex)
237                {
238                        m_simplexSolver->compute_points(pointOnA, pointOnB);
239                        normalInB = pointOnA-pointOnB;
240                        btScalar lenSqr = m_cachedSeparatingAxis.length2();
241                        //valid normal
242                        if (lenSqr < 0.0001)
243                        {
244                                m_degenerateSimplex = 5;
245                        } 
246                        if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
247                        {
248                                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
249                                normalInB *= rlen; //normalize
250                                btScalar s = btSqrt(squaredDistance);
251                       
252                                btAssert(s > btScalar(0.0));
253                                pointOnA -= m_cachedSeparatingAxis * (marginA / s);
254                                pointOnB += m_cachedSeparatingAxis * (marginB / s);
255                                distance = ((btScalar(1.)/rlen) - margin);
256                                isValid = true;
257                               
258                                m_lastUsedMethod = 1;
259                        } else
260                        {
261                                m_lastUsedMethod = 2;
262                        }
263                }
264
265                bool catchDegeneratePenetrationCase = 
266                        (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
267
268                //if (checkPenetration && !isValid)
269                if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
270                {
271                        //penetration case
272               
273                        //if there is no way to handle penetrations, bail out
274                        if (m_penetrationDepthSolver)
275                        {
276                                // Penetration depth case.
277                                btVector3 tmpPointOnA,tmpPointOnB;
278                               
279                                gNumDeepPenetrationChecks++;
280
281                                bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
282                                        *m_simplexSolver, 
283                                        m_minkowskiA,m_minkowskiB,
284                                        localTransA,localTransB,
285                                        m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
286                                        debugDraw,input.m_stackAlloc
287                                        );
288
289                                if (isValid2)
290                                {
291                                        btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
292                                        btScalar lenSqr = tmpNormalInB.length2();
293                                        if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
294                                        {
295                                                tmpNormalInB /= btSqrt(lenSqr);
296                                                btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
297                                                //only replace valid penetrations when the result is deeper (check)
298                                                if (!isValid || (distance2 < distance))
299                                                {
300                                                        distance = distance2;
301                                                        pointOnA = tmpPointOnA;
302                                                        pointOnB = tmpPointOnB;
303                                                        normalInB = tmpNormalInB;
304                                                        isValid = true;
305                                                        m_lastUsedMethod = 3;
306                                                } else
307                                                {
308                                                       
309                                                }
310                                        } else
311                                        {
312                                                //isValid = false;
313                                                m_lastUsedMethod = 4;
314                                        }
315                                } else
316                                {
317                                        m_lastUsedMethod = 5;
318                                }
319                               
320                        }
321                }
322        }
323
324        if (isValid)
325        {
326#ifdef __SPU__
327                //spu_printf("distance\n");
328#endif //__CELLOS_LV2__
329
330
331#ifdef DEBUG_SPU_COLLISION_DETECTION
332                spu_printf("output 1\n");
333#endif
334                output.addContactPoint(
335                        normalInB,
336                        pointOnB+positionOffset,
337                        distance);
338
339#ifdef DEBUG_SPU_COLLISION_DETECTION
340                spu_printf("output 2\n");
341#endif
342                //printf("gjk add:%f",distance);
343        }
344
345
346}
347
348
349
350
351
Note: See TracBrowser for help on using the repository browser.