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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btContinuousConvexCollision.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
26btContinuousConvexCollision::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}
MAX_ITERATIONS
#define MAX_ITERATIONS
This maximum should not be necessary.
Definition btContinuousConvexCollision.cpp:46
btContinuousConvexCollision.h
btConvexShape.h
btGjkPairDetector.h
btPointCollector.h
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition btQuaternion.h:895
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
SIMD_EPSILON
#define SIMD_EPSILON
Definition btScalar.h:543
btSimplexSolverInterface.h
btSimplexSolverInterface
#define btSimplexSolverInterface
Definition btSimplexSolverInterface.h:24
btSphereShape.h
btStaticPlaneShape.h
btTransformUtil.h
btCollisionShape::getShapeType
int getShapeType() const
Definition btCollisionShape.h:107
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
btContinuousConvexCollision::computeClosestPoints
void computeClosestPoints(const btTransform &transA, const btTransform &transB, struct btPointCollector &pointCollector)
Definition btContinuousConvexCollision.cpp:48
btContinuousConvexCollision::m_planeShape
const btStaticPlaneShape * m_planeShape
Definition btContinuousConvexCollision.h:36
btContinuousConvexCollision::m_penetrationDepthSolver
btConvexPenetrationDepthSolver * m_penetrationDepthSolver
Definition btContinuousConvexCollision.h:32
btContinuousConvexCollision::m_convexA
const btConvexShape * m_convexA
Definition btContinuousConvexCollision.h:33
btContinuousConvexCollision::m_simplexSolver
btSimplexSolverInterface * m_simplexSolver
Definition btContinuousConvexCollision.h:31
btContinuousConvexCollision::m_convexB1
const btConvexShape * m_convexB1
Definition btContinuousConvexCollision.h:35
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
btConvexPenetrationDepthSolver
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
Definition btConvexPenetrationDepthSolver.h:26
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition btConvexShape.h:33
btConvexShape::localGetSupportingVertex
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const =0
btConvexShape::getMargin
virtual btScalar getMargin() const =0
btGjkPairDetector
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
Definition btGjkPairDetector.h:28
btGjkPairDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition btGjkPairDetector.cpp:73
btIDebugDraw::drawSphere
virtual void drawSphere(btScalar radius, const btTransform &transform, const btVector3 &color)
Definition btIDebugDraw.h:92
btStaticPlaneShape
The btStaticPlaneShape simulates an infinite non-moving (static) collision plane.
Definition btStaticPlaneShape.h:24
btStaticPlaneShape::getPlaneConstant
const btScalar & getPlaneConstant() const
Definition btStaticPlaneShape.h:54
btStaticPlaneShape::getPlaneNormal
const btVector3 & getPlaneNormal() const
Definition btStaticPlaneShape.h:49
btTransformUtil::integrateTransform
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
Definition btTransformUtil.h:32
btTransformUtil::calculateVelocity
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
Definition btTransformUtil.h:115
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btTransform::inverse
btTransform inverse() const
Return the inverse of this transform.
Definition btTransform.h:183
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition btTransform.h:109
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition btTransform.h:114
btTransform::inverseTimes
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition btTransform.h:223
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::length
btScalar length() const
Return the length of the vector.
Definition btVector3.h:257
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:229
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition btVector3.h:640
btConvexCast::CastResult
RayResult stores the closest result alternatively, add a callback method to decide about closest/all ...
Definition btConvexCast.h:47
btConvexCast::CastResult::m_normal
btVector3 m_normal
Definition btConvexCast.h:70
btConvexCast::CastResult::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition btConvexCast.h:73
btConvexCast::CastResult::m_fraction
btScalar m_fraction
Definition btConvexCast.h:72
btConvexCast::CastResult::reportFailure
virtual void reportFailure(int errNo, int numIterations)
Definition btConvexCast.h:52
btConvexCast::CastResult::m_allowedPenetration
btScalar m_allowedPenetration
Definition btConvexCast.h:74
btConvexCast::CastResult::m_hitPoint
btVector3 m_hitPoint
Definition btConvexCast.h:71
btConvexCast::CastResult::DebugDraw
virtual void DebugDraw(btScalar fraction)
Definition btConvexCast.h:50
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition btDiscreteCollisionDetectorInterface.h:40
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition btDiscreteCollisionDetectorInterface.h:47
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition btDiscreteCollisionDetectorInterface.h:46
btPointCollector
Definition btPointCollector.h:22
btPointCollector::m_normalOnBInWorld
btVector3 m_normalOnBInWorld
Definition btPointCollector.h:23
btPointCollector::m_distance
btScalar m_distance
Definition btPointCollector.h:25
btPointCollector::m_hasResult
bool m_hasResult
Definition btPointCollector.h:27
btPointCollector::m_pointInWorld
btVector3 m_pointInWorld
Definition btPointCollector.h:24
btPointCollector::addContactPoint
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
Definition btPointCollector.h:45