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