Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/archive/tutorialHS09/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @ 8783

Last change on this file since 8783 was 5738, checked in by landauf, 15 years ago

merged libraries2 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                                checkSimplex=true;
154                                //checkPenetration = false;
155                                break;
156                        }
157
158                        //exit 0: the new point is already in the simplex, or we didn't come any closer
159                        if (m_simplexSolver->inSimplex(w))
160                        {
161                                m_degenerateSimplex = 1;
162                                checkSimplex = true;
163                                break;
164                        }
165                        // are we getting any closer ?
166                        btScalar f0 = squaredDistance - delta;
167                        btScalar f1 = squaredDistance * REL_ERROR2;
168
169                        if (f0 <= f1)
170                        {
171                                if (f0 <= btScalar(0.))
172                                {
173                                        m_degenerateSimplex = 2;
174                                }
175                                checkSimplex = true;
176                                break;
177                        }
178
179#ifdef DEBUG_SPU_COLLISION_DETECTION
180                spu_printf("addVertex 1\n");
181#endif
182                        //add current vertex to simplex
183                        m_simplexSolver->addVertex(w, pWorld, qWorld);
184#ifdef DEBUG_SPU_COLLISION_DETECTION
185                spu_printf("addVertex 2\n");
186#endif
187                        //calculate the closest point to the origin (update vector v)
188                        if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
189                        {
190                                m_degenerateSimplex = 3;
191                                checkSimplex = true;
192                                break;
193                        }
194
195                        if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
196            {
197                m_degenerateSimplex = 6;
198                checkSimplex = true;
199                break;
200            }
201
202                        btScalar previousSquaredDistance = squaredDistance;
203                        squaredDistance = m_cachedSeparatingAxis.length2();
204                       
205                        //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
206
207                        //are we getting any closer ?
208                        if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
209                        { 
210                                m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
211                                checkSimplex = true;
212                                break;
213                        }
214
215                          //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
216              if (m_curIter++ > gGjkMaxIter)   
217              {   
218                      #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
219
220                              printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
221                              printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
222                              m_cachedSeparatingAxis.getX(),   
223                              m_cachedSeparatingAxis.getY(),   
224                              m_cachedSeparatingAxis.getZ(),   
225                              squaredDistance,   
226                              m_minkowskiA->getShapeType(),   
227                              m_minkowskiB->getShapeType());   
228
229                      #endif   
230                      break;   
231
232              } 
233
234
235                        bool check = (!m_simplexSolver->fullSimplex());
236                        //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
237
238                        if (!check)
239                        {
240                                //do we need this backup_closest here ?
241                                m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
242                                break;
243                        }
244                }
245
246                if (checkSimplex)
247                {
248                        m_simplexSolver->compute_points(pointOnA, pointOnB);
249                        normalInB = pointOnA-pointOnB;
250                        btScalar lenSqr = m_cachedSeparatingAxis.length2();
251                        //valid normal
252                        if (lenSqr < 0.0001)
253                        {
254                                m_degenerateSimplex = 5;
255                        } 
256                        if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
257                        {
258                                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
259                                normalInB *= rlen; //normalize
260                                btScalar s = btSqrt(squaredDistance);
261                       
262                                btAssert(s > btScalar(0.0));
263                                pointOnA -= m_cachedSeparatingAxis * (marginA / s);
264                                pointOnB += m_cachedSeparatingAxis * (marginB / s);
265                                distance = ((btScalar(1.)/rlen) - margin);
266                                isValid = true;
267                               
268                                m_lastUsedMethod = 1;
269                        } else
270                        {
271                                m_lastUsedMethod = 2;
272                        }
273                }
274
275                bool catchDegeneratePenetrationCase = 
276                        (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
277
278                //if (checkPenetration && !isValid)
279                if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
280                {
281                        //penetration case
282               
283                        //if there is no way to handle penetrations, bail out
284                        if (m_penetrationDepthSolver)
285                        {
286                                // Penetration depth case.
287                                btVector3 tmpPointOnA,tmpPointOnB;
288                               
289                                gNumDeepPenetrationChecks++;
290
291                                bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
292                                        *m_simplexSolver, 
293                                        m_minkowskiA,m_minkowskiB,
294                                        localTransA,localTransB,
295                                        m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
296                                        debugDraw,input.m_stackAlloc
297                                        );
298
299                                if (isValid2)
300                                {
301                                        btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
302                                        btScalar lenSqr = tmpNormalInB.length2();
303                                        if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
304                                        {
305                                                tmpNormalInB /= btSqrt(lenSqr);
306                                                btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
307                                                //only replace valid penetrations when the result is deeper (check)
308                                                if (!isValid || (distance2 < distance))
309                                                {
310                                                        distance = distance2;
311                                                        pointOnA = tmpPointOnA;
312                                                        pointOnB = tmpPointOnB;
313                                                        normalInB = tmpNormalInB;
314                                                        isValid = true;
315                                                        m_lastUsedMethod = 3;
316                                                } else
317                                                {
318                                                       
319                                                }
320                                        } else
321                                        {
322                                                //isValid = false;
323                                                m_lastUsedMethod = 4;
324                                        }
325                                } else
326                                {
327                                        m_lastUsedMethod = 5;
328                                }
329                               
330                        }
331                }
332        }
333
334        if (isValid)
335        {
336#ifdef __SPU__
337                //spu_printf("distance\n");
338#endif //__CELLOS_LV2__
339
340
341#ifdef DEBUG_SPU_COLLISION_DETECTION
342                spu_printf("output 1\n");
343#endif
344                m_cachedSeparatingAxis = normalInB;
345                m_cachedSeparatingDistance = distance;
346
347                output.addContactPoint(
348                        normalInB,
349                        pointOnB+positionOffset,
350                        distance);
351
352#ifdef DEBUG_SPU_COLLISION_DETECTION
353                spu_printf("output 2\n");
354#endif
355                //printf("gjk add:%f",distance);
356        }
357
358
359}
360
361
362
363
364
Note: See TracBrowser for help on using the repository browser.