update Bullet 2.x with latest changes, notice that the integration is not finished...
[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 #if defined(DEBUG) || defined (_DEBUG)
22 #include <stdio.h> //for debug printf
23 #endif
24
25 static const btScalar rel_error = btScalar(1.0e-5);
26 btScalar rel_error2 = rel_error * rel_error;
27 float maxdist2 = 1.e30f;
28
29 #ifdef __SPU__
30 #include <spu_printf.h>
31 #endif //__SPU__
32
33 btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*      penetrationDepthSolver)
34 :m_cachedSeparatingAxis(0.f,0.f,1.f),
35 m_penetrationDepthSolver(penetrationDepthSolver),
36 m_simplexSolver(simplexSolver),
37 m_minkowskiA(objectA),
38 m_minkowskiB(objectB),
39 m_ignoreMargin(false)
40 {
41 }
42
43 void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
44 {
45         btScalar distance=0.f;
46         btVector3       normalInB(0.f,0.f,0.f);
47         btVector3 pointOnA,pointOnB;
48
49         float marginA = m_minkowskiA->getMargin();
50         float marginB = m_minkowskiB->getMargin();
51
52         //for CCD we don't use margins
53         if (m_ignoreMargin)
54         {
55                 marginA = 0.f;
56                 marginB = 0.f;
57         }
58
59         int curIter = 0;
60         int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
61
62         bool isValid = false;
63         bool checkSimplex = false;
64         bool checkPenetration = true;
65
66         {
67                 btScalar squaredDistance = SIMD_INFINITY;
68                 btScalar delta = 0.f;
69                 
70                 btScalar margin = marginA + marginB;
71                 
72                 
73
74                 m_simplexSolver->reset();
75                 
76                 while (true)
77                 {
78
79                         btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
80                         btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
81
82                         btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
83                         btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
84                         btPoint3  pWorld = input.m_transformA(pInA);    
85                         btPoint3  qWorld = input.m_transformB(qInB);
86                         
87                         btVector3 w     = pWorld - qWorld;
88                         delta = m_cachedSeparatingAxis.dot(w);
89
90                         // potential exit, they don't overlap
91                         if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
92                         {
93                                 checkPenetration = false;
94                                 break;
95                         }
96
97                         //exit 0: the new point is already in the simplex, or we didn't come any closer
98                         if (m_simplexSolver->inSimplex(w))
99                         {
100                                 checkSimplex = true;
101                                 break;
102                         }
103                         // are we getting any closer ?
104                         if (squaredDistance - delta <= squaredDistance * rel_error2)
105                         {
106                                 checkSimplex = true;
107                                 break;
108                         }
109                         //add current vertex to simplex
110                         m_simplexSolver->addVertex(w, pWorld, qWorld);
111
112                         //calculate the closest point to the origin (update vector v)
113                         if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
114                         {
115                                 checkSimplex = true;
116                                 break;
117                         }
118
119                         btScalar previousSquaredDistance = squaredDistance;
120                         squaredDistance = m_cachedSeparatingAxis.length2();
121                         
122                         //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
123
124                         //are we getting any closer ?
125                         if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
126                         { 
127                                 m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
128                                 checkSimplex = true;
129                                 break;
130                         }
131
132                           //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
133               if (curIter++ > gGjkMaxIter)   
134               {   
135                       #if defined(DEBUG) || defined (_DEBUG)   
136                               printf("btGjkPairDetector maxIter exceeded:%i\n",curIter);   
137                               printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
138                               m_cachedSeparatingAxis.getX(),   
139                               m_cachedSeparatingAxis.getY(),   
140                               m_cachedSeparatingAxis.getZ(),   
141                               squaredDistance,   
142                               m_minkowskiA->getShapeType(),   
143                               m_minkowskiB->getShapeType());   
144                       #endif   
145                       break;   
146
147               } 
148
149
150                         bool check = (!m_simplexSolver->fullSimplex());
151                         //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
152
153                         if (!check)
154                         {
155                                 //do we need this backup_closest here ?
156                                 m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
157                                 break;
158                         }
159                 }
160
161                 if (checkSimplex)
162                 {
163                         m_simplexSolver->compute_points(pointOnA, pointOnB);
164                         normalInB = pointOnA-pointOnB;
165                         float lenSqr = m_cachedSeparatingAxis.length2();
166                         //valid normal
167                         if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
168                         {
169                                 float rlen = 1.f / btSqrt(lenSqr );
170                                 normalInB *= rlen; //normalize
171                                 btScalar s = btSqrt(squaredDistance);
172                                 ASSERT(s > btScalar(0.0));
173                                 pointOnA -= m_cachedSeparatingAxis * (marginA / s);
174                                 pointOnB += m_cachedSeparatingAxis * (marginB / s);
175                                 distance = ((1.f/rlen) - margin);
176                                 isValid = true;
177                         }
178                 }
179
180                 if (checkPenetration && !isValid)
181                 {
182                         //penetration case
183                 
184                         //if there is no way to handle penetrations, bail out
185                         if (m_penetrationDepthSolver)
186                         {
187                                 // Penetration depth case.
188                                 isValid = m_penetrationDepthSolver->calcPenDepth( 
189                                         *m_simplexSolver, 
190                                         m_minkowskiA,m_minkowskiB,
191                                         input.m_transformA,input.m_transformB,
192                                         m_cachedSeparatingAxis, pointOnA, pointOnB,
193                                         debugDraw
194                                         );
195
196                                 if (isValid)
197                                 {
198                                         normalInB = pointOnB-pointOnA;
199                                         float lenSqr = normalInB.length2();
200                                         if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
201                                         {
202                                                 normalInB /= btSqrt(lenSqr);
203                                                 distance = -(pointOnA-pointOnB).length();
204                                         } else
205                                         {
206                                                 isValid = false;
207                                         }
208                                 }
209                         }
210                 }
211         }
212
213         if (isValid)
214         {
215 #ifdef __SPU__
216                 //spu_printf("distance\n");
217 #endif //__CELLOS_LV2__
218
219
220                 output.addContactPoint(
221                         normalInB,
222                         pointOnB,
223                         distance);
224                 //printf("gjk add:%f",distance);
225         }
226
227
228 }
229
230
231
232
233