0856332d1ca2f2416a7aa2bba91b850edf889dda
[blender.git] / extern / bullet2 / src / BulletCollision / NarrowPhaseCollision / btGjkPairDetector.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. 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
37 int gNumDeepPenetrationChecks = 0;
38 int gNumGjkChecks = 0;
39
40
41
42 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*  penetrationDepthSolver)
43 :m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
44 m_penetrationDepthSolver(penetrationDepthSolver),
45 m_simplexSolver(simplexSolver),
46 m_minkowskiA(objectA),
47 m_minkowskiB(objectB),
48 m_ignoreMargin(false),
49 m_lastUsedMethod(-1),
50 m_catchDegeneracies(1)
51 {
52 }
53
54 void 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