Blender  V3.3
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 
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);
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,
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 }
#define MAX_ITERATIONS
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btSimplexSolverInterface
btStaticPlaneShape(const btVector3 &planeNormal, btScalar planeConstant)
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
btContinuousConvexCollision(const btConvexShape *shapeA, const btConvexShape *shapeB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
virtual bool calcTimeOfImpact(const btTransform &fromA, const btTransform &toA, const btTransform &fromB, const btTransform &toB, CastResult &result)
cast a convex against another convex object
ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
ccl_global KernelShaderEvalInput * input
static unsigned c
Definition: RandGen.cpp:83
T length(const vec_base< T, Size > &a)
T distance(const T &a, const T &b)
btVector3 m_normalOnBInWorld
btVector3 m_pointInWorld
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)