Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "btGjkConvexCast.h"
00019 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00020 #include "btGjkPairDetector.h"
00021 #include "btPointCollector.h"
00022 #include "LinearMath/btTransformUtil.h"
00023
00024 #ifdef BT_USE_DOUBLE_PRECISION
00025 #define MAX_ITERATIONS 64
00026 #else
00027 #define MAX_ITERATIONS 32
00028 #endif
00029
00030 btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
00031 :m_simplexSolver(simplexSolver),
00032 m_convexA(convexA),
00033 m_convexB(convexB)
00034 {
00035 }
00036
00037 bool btGjkConvexCast::calcTimeOfImpact(
00038 const btTransform& fromA,
00039 const btTransform& toA,
00040 const btTransform& fromB,
00041 const btTransform& toB,
00042 CastResult& result)
00043 {
00044
00045
00046 m_simplexSolver->reset();
00047
00049
00050 btVector3 linVelA,linVelB;
00051 linVelA = toA.getOrigin()-fromA.getOrigin();
00052 linVelB = toB.getOrigin()-fromB.getOrigin();
00053
00054 btScalar radius = btScalar(0.001);
00055 btScalar lambda = btScalar(0.);
00056 btVector3 v(1,0,0);
00057
00058 int maxIter = MAX_ITERATIONS;
00059
00060 btVector3 n;
00061 n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00062 bool hasResult = false;
00063 btVector3 c;
00064 btVector3 r = (linVelA-linVelB);
00065
00066 btScalar lastLambda = lambda;
00067
00068
00069 int numIter = 0;
00070
00071
00072
00073 btTransform identityTrans;
00074 identityTrans.setIdentity();
00075
00076
00077
00078
00079 btPointCollector pointCollector;
00080
00081
00082 btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);
00083 btGjkPairDetector::ClosestPointInput input;
00084
00085
00086
00087
00088 input.m_transformA = fromA;
00089 input.m_transformB = fromB;
00090 gjk.getClosestPoints(input,pointCollector,0);
00091
00092 hasResult = pointCollector.m_hasResult;
00093 c = pointCollector.m_pointInWorld;
00094
00095 if (hasResult)
00096 {
00097 btScalar dist;
00098 dist = pointCollector.m_distance;
00099 n = pointCollector.m_normalOnBInWorld;
00100
00101
00102
00103
00104 while (dist > radius)
00105 {
00106 numIter++;
00107 if (numIter > maxIter)
00108 {
00109 return false;
00110 }
00111 btScalar dLambda = btScalar(0.);
00112
00113 btScalar projectedLinearVelocity = r.dot(n);
00114
00115 dLambda = dist / (projectedLinearVelocity);
00116
00117 lambda = lambda - dLambda;
00118
00119 if (lambda > btScalar(1.))
00120 return false;
00121
00122 if (lambda < btScalar(0.))
00123 return false;
00124
00125
00126 if (lambda <= lastLambda)
00127 {
00128 return false;
00129
00130 break;
00131 }
00132 lastLambda = lambda;
00133
00134
00135 result.DebugDraw( lambda );
00136 input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
00137 input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
00138
00139 gjk.getClosestPoints(input,pointCollector,0);
00140 if (pointCollector.m_hasResult)
00141 {
00142 if (pointCollector.m_distance < btScalar(0.))
00143 {
00144 result.m_fraction = lastLambda;
00145 n = pointCollector.m_normalOnBInWorld;
00146 result.m_normal=n;
00147 result.m_hitPoint = pointCollector.m_pointInWorld;
00148 return true;
00149 }
00150 c = pointCollector.m_pointInWorld;
00151 n = pointCollector.m_normalOnBInWorld;
00152 dist = pointCollector.m_distance;
00153 } else
00154 {
00155
00156 return false;
00157 }
00158
00159 }
00160
00161
00162
00163 if (n.dot(r)>=-result.m_allowedPenetration)
00164 return false;
00165
00166 result.m_fraction = lambda;
00167 result.m_normal = n;
00168 result.m_hitPoint = c;
00169 return true;
00170 }
00171
00172 return false;
00173
00174
00175 }
00176