Bullet Collision Detection & Physics Library: btContinuousConvexCollision.cpp Source File

Bullet Collision Detection & Physics Library

btContinuousConvexCollision.cpp
Go to the documentation of this file.
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
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
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
46 #define MAX_ITERATIONS 64
47
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
90 bool btContinuousConvexCollision::calcTimeOfImpact(
91 const btTransform& fromA,
92 const btTransform& toA,
93 const btTransform& fromB,
94 const btTransform& toB,
95 CastResult& result)
96 {
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 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:523
btContinuousConvexCollision::computeClosestPoints
void computeClosestPoints(const btTransform &transA, const btTransform &transB, struct btPointCollector &pointCollector)
Definition: btContinuousConvexCollision.cpp:48
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btTransformUtil.h
btConvexShape::localGetSupportingVertex
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const =0
btConvexCast::CastResult::m_fraction
btScalar m_fraction
Definition: btConvexCast.h:72
btPointCollector.h
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btConvexPenetrationDepthSolver
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
Definition: btConvexPenetrationDepthSolver.h:25
btStaticPlaneShape::getPlaneConstant
const btScalar & getPlaneConstant() const
Definition: btStaticPlaneShape.h:54
btStaticPlaneShape.h
btPointCollector::m_distance
btScalar m_distance
Definition: btPointCollector.h:25
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:46
btConvexShape.h
btGjkPairDetector.h
MAX_ITERATIONS
#define MAX_ITERATIONS
This maximum should not be necessary.
Definition: btContinuousConvexCollision.cpp:46
btConvexCast::CastResult::m_normal
btVector3 m_normal
Definition: btConvexCast.h:70
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
btConvexCast::CastResult::m_allowedPenetration
btScalar m_allowedPenetration
Definition: btConvexCast.h:74
btPointCollector
Definition: btPointCollector.h:21
btConvexCast::CastResult::DebugDraw
virtual void DebugDraw(btScalar fraction)
Definition: btConvexCast.h:50
btConvexCast::CastResult::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btConvexCast.h:73
btConvexCast::CastResult
RayResult stores the closest result alternatively, add a callback method to decide about closest/all ...
Definition: btConvexCast.h:46
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btSimplexSolverInterface
#define btSimplexSolverInterface
Definition: btSimplexSolverInterface.h:24
btContinuousConvexCollision::m_simplexSolver
btSimplexSolverInterface * m_simplexSolver
Definition: btContinuousConvexCollision.h:31
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btTransform::inverseTimes
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:222
btPointCollector::m_pointInWorld
btVector3 m_pointInWorld
Definition: btPointCollector.h:24
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btContinuousConvexCollision::m_convexA
const btConvexShape * m_convexA
Definition: btContinuousConvexCollision.h:33
btContinuousConvexCollision.h
btConvexShape::getMargin
virtual btScalar getMargin() const =0
btContinuousConvexCollision::m_penetrationDepthSolver
btConvexPenetrationDepthSolver * m_penetrationDepthSolver
Definition: btContinuousConvexCollision.h:32
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btTransformUtil::integrateTransform
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
Definition: btTransformUtil.h:32
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btStaticPlaneShape::getPlaneNormal
const btVector3 & getPlaneNormal() const
Definition: btStaticPlaneShape.h:49
btConvexCast::CastResult::m_hitPoint
btVector3 m_hitPoint
Definition: btConvexCast.h:71
btPointCollector::addContactPoint
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
Definition: btPointCollector.h:45
btContinuousConvexCollision::calcTimeOfImpact
virtual bool calcTimeOfImpact(const btTransform &fromA, const btTransform &toA, const btTransform &fromB, const btTransform &toB, CastResult &result)
cast a convex against another convex object
Definition: btContinuousConvexCollision.cpp:90
btGjkPairDetector
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
Definition: btGjkPairDetector.h:27
btContinuousConvexCollision::m_convexB1
const btConvexShape * m_convexB1
Definition: btContinuousConvexCollision.h:35
btCollisionShape::getShapeType
int getShapeType() const
Definition: btCollisionShape.h:106
btPointCollector::m_hasResult
bool m_hasResult
Definition: btPointCollector.h:27
btCollisionShape::getAngularMotionDisc
virtual btScalar getAngularMotionDisc() const
getAngularMotionDisc returns the maximum radius needed for Conservative Advancement to handle time-of...
Definition: btCollisionShape.cpp:47
btContinuousConvexCollision::btContinuousConvexCollision
btContinuousConvexCollision(const btConvexShape *shapeA, const btConvexShape *shapeB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
Definition: btContinuousConvexCollision.cpp:26
btStaticPlaneShape
The btStaticPlaneShape simulates an infinite non-moving (static) collision plane. ...
Definition: btStaticPlaneShape.h:22
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition: btDiscreteCollisionDetectorInterface.h:39
btSimplexSolverInterface.h
btSphereShape.h
btTransformUtil::calculateVelocity
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
Definition: btTransformUtil.h:115
btIDebugDraw::drawSphere
virtual void drawSphere(btScalar radius, const btTransform &transform, const btVector3 &color)
Definition: btIDebugDraw.h:92
btTransform::inverse
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
btConvexCast::CastResult::reportFailure
virtual void reportFailure(int errNo, int numIterations)
Definition: btConvexCast.h:52
btPointCollector::m_normalOnBInWorld
btVector3 m_normalOnBInWorld
Definition: btPointCollector.h:23
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btContinuousConvexCollision::m_planeShape
const btStaticPlaneShape * m_planeShape
Definition: btContinuousConvexCollision.h:36