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 "btContinuousConvexCollision.h"
17 #include "BulletCollision/CollisionShapes/btConvexShape.h"
18 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
19 #include "LinearMath/btTransformUtil.h"
20 #include "BulletCollision/CollisionShapes/btSphereShape.h"
21
22 #include "btGjkPairDetector.h"
23 #include "btPointCollector.h"
24 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
25
btContinuousConvexCollision(const btConvexShape * convexA,const btConvexShape * convexB,btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * penetrationDepthSolver)26 btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
27 : m_simplexSolver(simplexSolver),
28 m_penetrationDepthSolver(penetrationDepthSolver),
29 m_convexA(convexA),
30 m_convexB1(convexB),
31 m_planeShape(0)
32 {
33 }
34
btContinuousConvexCollision(const btConvexShape * convexA,const btStaticPlaneShape * plane)35 btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btStaticPlaneShape* plane)
36 : m_simplexSolver(0),
37 m_penetrationDepthSolver(0),
38 m_convexA(convexA),
39 m_convexB1(0),
40 m_planeShape(plane)
41 {
42 }
43
44 /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
45 /// You don't want your game ever to lock-up.
46 #define MAX_ITERATIONS 64
47
computeClosestPoints(const btTransform & transA,const btTransform & transB,btPointCollector & pointCollector)48 void btContinuousConvexCollision::computeClosestPoints(const btTransform& transA, const btTransform& transB, btPointCollector& pointCollector)
49 {
50 if (m_convexB1)
51 {
52 m_simplexSolver->reset();
53 btGjkPairDetector gjk(m_convexA, m_convexB1, m_convexA->getShapeType(), m_convexB1->getShapeType(), m_convexA->getMargin(), m_convexB1->getMargin(), m_simplexSolver, m_penetrationDepthSolver);
54 btGjkPairDetector::ClosestPointInput input;
55 input.m_transformA = transA;
56 input.m_transformB = transB;
57 gjk.getClosestPoints(input, pointCollector, 0);
58 }
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
calcTimeOfImpact(const btTransform & fromA,const btTransform & toA,const btTransform & fromB,const btTransform & toB,CastResult & result)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 /// compute linear and angular velocity for this interval, to interpolate
98 btVector3 linVelA, angVelA, linVelB, angVelB;
99 btTransformUtil::calculateVelocity(fromA, toA, btScalar(1.), linVelA, angVelA);
100 btTransformUtil::calculateVelocity(fromB, toB, btScalar(1.), linVelB, angVelB);
101
102 btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
103 btScalar boundingRadiusB = m_convexB1 ? m_convexB1->getAngularMotionDisc() : 0.f;
104
105 btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
106 btVector3 relLinVel = (linVelB - linVelA);
107
108 btScalar relLinVelocLength = (linVelB - linVelA).length();
109
110 if ((relLinVelocLength + maxAngularProjectedVelocity) == 0.f)
111 return false;
112
113 btScalar lambda = btScalar(0.);
114
115 btVector3 n;
116 n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
117 bool hasResult = false;
118 btVector3 c;
119
120 btScalar lastLambda = lambda;
121 //btScalar epsilon = btScalar(0.001);
122
123 int numIter = 0;
124 //first solution, using GJK
125
126 btScalar radius = 0.001f;
127 // result.drawCoordSystem(sphereTr);
128
129 btPointCollector pointCollector1;
130
131 {
132 computeClosestPoints(fromA, fromB, pointCollector1);
133
134 hasResult = pointCollector1.m_hasResult;
135 c = pointCollector1.m_pointInWorld;
136 }
137
138 if (hasResult)
139 {
140 btScalar dist;
141 dist = pointCollector1.m_distance + result.m_allowedPenetration;
142 n = pointCollector1.m_normalOnBInWorld;
143 btScalar projectedLinearVelocity = relLinVel.dot(n);
144 if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
145 return false;
146
147 //not close enough
148 while (dist > radius)
149 {
150 if (result.m_debugDrawer)
151 {
152 result.m_debugDrawer->drawSphere(c, 0.2f, btVector3(1, 1, 1));
153 }
154 btScalar dLambda = btScalar(0.);
155
156 projectedLinearVelocity = relLinVel.dot(n);
157
158 //don't report time of impact for motion away from the contact normal (or causes minor penetration)
159 if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
160 return false;
161
162 dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
163
164 lambda += dLambda;
165
166 if (lambda > btScalar(1.) || lambda < btScalar(0.))
167 return false;
168
169 //todo: next check with relative epsilon
170 if (lambda <= lastLambda)
171 {
172 return false;
173 //n.setValue(0,0,0);
174 //break;
175 }
176 lastLambda = lambda;
177
178 //interpolate to next lambda
179 btTransform interpolatedTransA, interpolatedTransB, relativeTrans;
180
181 btTransformUtil::integrateTransform(fromA, linVelA, angVelA, lambda, interpolatedTransA);
182 btTransformUtil::integrateTransform(fromB, linVelB, angVelB, lambda, interpolatedTransB);
183 relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
184
185 if (result.m_debugDrawer)
186 {
187 result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(), 0.2f, btVector3(1, 0, 0));
188 }
189
190 result.DebugDraw(lambda);
191
192 btPointCollector pointCollector;
193 computeClosestPoints(interpolatedTransA, interpolatedTransB, pointCollector);
194
195 if (pointCollector.m_hasResult)
196 {
197 dist = pointCollector.m_distance + result.m_allowedPenetration;
198 c = pointCollector.m_pointInWorld;
199 n = pointCollector.m_normalOnBInWorld;
200 }
201 else
202 {
203 result.reportFailure(-1, numIter);
204 return false;
205 }
206
207 numIter++;
208 if (numIter > MAX_ITERATIONS)
209 {
210 result.reportFailure(-2, numIter);
211 return false;
212 }
213 }
214
215 result.m_fraction = lambda;
216 result.m_normal = n;
217 result.m_hitPoint = c;
218 return true;
219 }
220
221 return false;
222 }
223