bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / NarrowPhaseCollision / btContinuousConvexCollision.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
17 #include "btContinuousConvexCollision.h"
18 #include "BulletCollision/CollisionShapes/btConvexShape.h"
19 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
20 #include "LinearMath/btTransformUtil.h"
21 #include "BulletCollision/CollisionShapes/btSphereShape.h"
22
23 #include "btGjkPairDetector.h"
24 #include "btPointCollector.h"
25 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
26
27
28
29 btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape*    convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
30 :m_simplexSolver(simplexSolver),
31 m_penetrationDepthSolver(penetrationDepthSolver),
32 m_convexA(convexA),m_convexB1(convexB),m_planeShape(0)
33 {
34 }
35
36
37 btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape*  convexA,const btStaticPlaneShape*       plane)
38 :m_simplexSolver(0),
39 m_penetrationDepthSolver(0),
40 m_convexA(convexA),m_convexB1(0),m_planeShape(plane)
41 {
42 }
43
44
45 /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
46 /// You don't want your game ever to lock-up.
47 #define MAX_ITERATIONS 64
48
49 void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
50 {
51         if (m_convexB1)
52         {
53                 m_simplexSolver->reset();
54                 btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);               
55                 btGjkPairDetector::ClosestPointInput input;
56                 input.m_transformA = transA;
57                 input.m_transformB = transB;
58                 gjk.getClosestPoints(input,pointCollector,0);
59         } else
60         {
61                 //convex versus plane
62                 const btConvexShape* convexShape = m_convexA;
63                 const btStaticPlaneShape* planeShape = m_planeShape;
64                 
65                 const btVector3& planeNormal = planeShape->getPlaneNormal();
66                 const btScalar& planeConstant = planeShape->getPlaneConstant();
67                 
68                 btTransform convexWorldTransform = transA;
69                 btTransform convexInPlaneTrans;
70                 convexInPlaneTrans= transB.inverse() * convexWorldTransform;
71                 btTransform planeInConvex;
72                 planeInConvex= convexWorldTransform.inverse() * transB;
73                 
74                 btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
75
76                 btVector3 vtxInPlane = convexInPlaneTrans(vtx);
77                 btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
78
79                 btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
80                 btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
81                 btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
82
83                 pointCollector.addContactPoint(
84                         normalOnSurfaceB,
85                         vtxInPlaneWorld,
86                         distance);
87         }
88 }
89
90 bool    btContinuousConvexCollision::calcTimeOfImpact(
91                                 const btTransform& fromA,
92                                 const btTransform& toA,
93                                 const btTransform& fromB,
94                                 const btTransform& toB,
95                                 CastResult& result)
96 {
97
98
99         /// compute linear and angular velocity for this interval, to interpolate
100         btVector3 linVelA,angVelA,linVelB,angVelB;
101         btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
102         btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
103
104
105         btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
106         btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f;
107
108         btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
109         btVector3 relLinVel = (linVelB-linVelA);
110
111         btScalar relLinVelocLength = (linVelB-linVelA).length();
112         
113         if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
114                 return false;
115
116
117
118         btScalar lambda = btScalar(0.);
119         btVector3 v(1,0,0);
120
121         int maxIter = MAX_ITERATIONS;
122
123         btVector3 n;
124         n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
125         bool hasResult = false;
126         btVector3 c;
127
128         btScalar lastLambda = lambda;
129         //btScalar epsilon = btScalar(0.001);
130
131         int numIter = 0;
132         //first solution, using GJK
133
134
135         btScalar radius = 0.001f;
136 //      result.drawCoordSystem(sphereTr);
137
138         btPointCollector        pointCollector1;
139
140         {
141         
142                 computeClosestPoints(fromA,fromB,pointCollector1);
143
144                 hasResult = pointCollector1.m_hasResult;
145                 c = pointCollector1.m_pointInWorld;
146         }
147
148         if (hasResult)
149         {
150                 btScalar dist;
151                 dist = pointCollector1.m_distance + result.m_allowedPenetration;
152                 n = pointCollector1.m_normalOnBInWorld;
153                 btScalar projectedLinearVelocity = relLinVel.dot(n);
154                 if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
155                         return false;
156
157                 //not close enough
158                 while (dist > radius)
159                 {
160                         if (result.m_debugDrawer)
161                         {
162                                 result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
163                         }
164                         btScalar dLambda = btScalar(0.);
165
166                         projectedLinearVelocity = relLinVel.dot(n);
167
168                         
169                         //don't report time of impact for motion away from the contact normal (or causes minor penetration)
170                         if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
171                                 return false;
172                         
173                         dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
174
175                         
176                         
177                         lambda = lambda + dLambda;
178
179                         if (lambda > btScalar(1.))
180                                 return false;
181
182                         if (lambda < btScalar(0.))
183                                 return false;
184
185
186                         //todo: next check with relative epsilon
187                         if (lambda <= lastLambda)
188                         {
189                                 return false;
190                                 //n.setValue(0,0,0);
191                                 break;
192                         }
193                         lastLambda = lambda;
194
195                         
196
197                         //interpolate to next lambda
198                         btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
199
200                         btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
201                         btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
202                         relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
203
204                         if (result.m_debugDrawer)
205                         {
206                                 result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
207                         }
208
209                         result.DebugDraw( lambda );
210
211                         btPointCollector        pointCollector;
212                         computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector);
213
214                         if (pointCollector.m_hasResult)
215                         {
216                                 dist = pointCollector.m_distance+result.m_allowedPenetration;
217                                 c = pointCollector.m_pointInWorld;              
218                                 n = pointCollector.m_normalOnBInWorld;
219                         } else
220                         {
221                                 result.reportFailure(-1, numIter);
222                                 return false;
223                         }
224
225                         numIter++;
226                         if (numIter > maxIter)
227                         {
228                                 result.reportFailure(-2, numIter);
229                                 return false;
230                         }
231                 }
232         
233                 result.m_fraction = lambda;
234                 result.m_normal = n;
235                 result.m_hitPoint = c;
236                 return true;
237         }
238
239         return false;
240
241 }
242