Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "btSubSimplexConvexCast.h"
00018 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00019
00020 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
00022 #include "btPointCollector.h"
00023 #include "LinearMath/btTransformUtil.h"
00024
00025 btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
00026 :m_simplexSolver(simplexSolver),
00027 m_convexA(convexA),m_convexB(convexB)
00028 {
00029 }
00030
00033 #ifdef BT_USE_DOUBLE_PRECISION
00034 #define MAX_ITERATIONS 64
00035 #else
00036 #define MAX_ITERATIONS 32
00037 #endif
00038 bool btSubsimplexConvexCast::calcTimeOfImpact(
00039 const btTransform& fromA,
00040 const btTransform& toA,
00041 const btTransform& fromB,
00042 const btTransform& toB,
00043 CastResult& result)
00044 {
00045
00046 m_simplexSolver->reset();
00047
00048 btVector3 linVelA,linVelB;
00049 linVelA = toA.getOrigin()-fromA.getOrigin();
00050 linVelB = toB.getOrigin()-fromB.getOrigin();
00051
00052 btScalar lambda = btScalar(0.);
00053
00054 btTransform interpolatedTransA = fromA;
00055 btTransform interpolatedTransB = fromB;
00056
00058 btVector3 r = (linVelA-linVelB);
00059 btVector3 v;
00060
00061 btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
00062 btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
00063 v = supVertexA-supVertexB;
00064 int maxIter = MAX_ITERATIONS;
00065
00066 btVector3 n;
00067 n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00068 bool hasResult = false;
00069 btVector3 c;
00070
00071 btScalar lastLambda = lambda;
00072
00073
00074 btScalar dist2 = v.length2();
00075 #ifdef BT_USE_DOUBLE_PRECISION
00076 btScalar epsilon = btScalar(0.0001);
00077 #else
00078 btScalar epsilon = btScalar(0.0001);
00079 #endif //BT_USE_DOUBLE_PRECISION
00080 btVector3 w,p;
00081 btScalar VdotR;
00082
00083 while ( (dist2 > epsilon) && maxIter--)
00084 {
00085 supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
00086 supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
00087 w = supVertexA-supVertexB;
00088
00089 btScalar VdotW = v.dot(w);
00090
00091 if (lambda > btScalar(1.0))
00092 {
00093 return false;
00094 }
00095
00096 if ( VdotW > btScalar(0.))
00097 {
00098 VdotR = v.dot(r);
00099
00100 if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
00101 return false;
00102 else
00103 {
00104 lambda = lambda - VdotW / VdotR;
00105
00106
00107 interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
00108 interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
00109
00110
00111 w = supVertexA-supVertexB;
00112 lastLambda = lambda;
00113 n = v;
00114 hasResult = true;
00115 }
00116 }
00118 if (!m_simplexSolver->inSimplex(w))
00119 m_simplexSolver->addVertex( w, supVertexA , supVertexB);
00120
00121 if (m_simplexSolver->closest(v))
00122 {
00123 dist2 = v.length2();
00124 hasResult = true;
00125
00126
00127
00128
00129
00130 } else
00131 {
00132 dist2 = btScalar(0.);
00133 }
00134 }
00135
00136
00137
00138
00139
00140
00141
00142 result.m_fraction = lambda;
00143 if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
00144 result.m_normal = n.normalized();
00145 else
00146 result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00147
00148
00149 if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
00150 return false;
00151
00152 btVector3 hitA,hitB;
00153 m_simplexSolver->compute_points(hitA,hitB);
00154 result.m_hitPoint=hitB;
00155 return true;
00156 }
00157
00158
00159
00160