Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @ 2833

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

Merged presentation branch back to trunk.

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