Bullet Collision Detection & Physics Library
btSubSimplexConvexCast.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 
17 #include "btSubSimplexConvexCast.h"
19 
22 #include "btPointCollector.h"
24 
26 :m_simplexSolver(simplexSolver),
27 m_convexA(convexA),m_convexB(convexB)
28 {
29 }
30 
33 #ifdef BT_USE_DOUBLE_PRECISION
34 #define MAX_ITERATIONS 64
35 #else
36 #define MAX_ITERATIONS 32
37 #endif
39  const btTransform& fromA,
40  const btTransform& toA,
41  const btTransform& fromB,
42  const btTransform& toB,
43  CastResult& result)
44 {
45 
46  m_simplexSolver->reset();
47 
48  btVector3 linVelA,linVelB;
49  linVelA = toA.getOrigin()-fromA.getOrigin();
50  linVelB = toB.getOrigin()-fromB.getOrigin();
51 
52  btScalar lambda = btScalar(0.);
53 
54  btTransform interpolatedTransA = fromA;
55  btTransform interpolatedTransB = fromB;
56 
58  btVector3 r = (linVelA-linVelB);
59  btVector3 v;
60 
61  btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
62  btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
63  v = supVertexA-supVertexB;
64  int maxIter = MAX_ITERATIONS;
65 
66  btVector3 n;
67  n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
68 
69  btVector3 c;
70 
71 
72 
73 
74  btScalar dist2 = v.length2();
75 
76 #ifdef BT_USE_DOUBLE_PRECISION
77  btScalar epsilon = SIMD_EPSILON * 10;
78 #else
79 //todo: epsilon kept for backward compatibility of unit tests.
80 //will need to digg deeper to make the algorithm more robust
81 //since, a large epsilon can cause an early termination with false
82 //positive results (ray intersections that shouldn't be there)
83  btScalar epsilon = btScalar(0.0001);
84 #endif //BT_USE_DOUBLE_PRECISION
85 
86 
87  btVector3 w,p;
88  btScalar VdotR;
89 
90  while ( (dist2 > epsilon) && maxIter--)
91  {
92  supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
93  supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
94  w = supVertexA-supVertexB;
95 
96  btScalar VdotW = v.dot(w);
97 
98  if (lambda > btScalar(1.0))
99  {
100  return false;
101  }
102 
103  if ( VdotW > btScalar(0.))
104  {
105  VdotR = v.dot(r);
106 
107  if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
108  return false;
109  else
110  {
111  lambda = lambda - VdotW / VdotR;
112  //interpolate to next lambda
113  // x = s + lambda * r;
114  interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
115  interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
116  //m_simplexSolver->reset();
117  //check next line
118  w = supVertexA-supVertexB;
119 
120  n = v;
121 
122  }
123  }
125  if (!m_simplexSolver->inSimplex(w))
126  m_simplexSolver->addVertex( w, supVertexA , supVertexB);
127 
128  if (m_simplexSolver->closest(v))
129  {
130  dist2 = v.length2();
131 
132  //todo: check this normal for validity
133  //n=v;
134  //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
135  //printf("DIST2=%f\n",dist2);
136  //printf("numverts = %i\n",m_simplexSolver->numVertices());
137  } else
138  {
139  dist2 = btScalar(0.);
140  }
141  }
142 
143  //int numiter = MAX_ITERATIONS - maxIter;
144 // printf("number of iterations: %d", numiter);
145 
146  //don't report a time of impact when moving 'away' from the hitnormal
147 
148 
149  result.m_fraction = lambda;
150  if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
151  result.m_normal = n.normalized();
152  else
153  result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
154 
155  //don't report time of impact for motion away from the contact normal (or causes minor penetration)
156  if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
157  return false;
158 
159  btVector3 hitA,hitB;
160  m_simplexSolver->compute_points(hitA,hitB);
161  result.m_hitPoint=hitB;
162  return true;
163 }
164 
165 
166 
167 
#define SIMD_EPSILON
Definition: btScalar.h:521
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const =0
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
#define MAX_ITERATIONS
Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degene...
const btConvexShape * m_convexA
btSimplexSolverInterface * m_simplexSolver
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
RayResult stores the closest result alternatively, add a callback method to decide about closest/all ...
Definition: btConvexCast.h:36
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
#define btSimplexSolverInterface
virtual bool calcTimeOfImpact(const btTransform &fromA, const btTransform &toA, const btTransform &fromB, const btTransform &toB, CastResult &result)
SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (...
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btSubsimplexConvexCast(const btConvexShape *shapeA, const btConvexShape *shapeB, btSimplexSolverInterface *simplexSolver)
const btConvexShape * m_convexB
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:966
void setInterpolate3(const btVector3 &v0, const btVector3 &v1, btScalar rt)
Definition: btVector3.h:503
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292