00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "btContinuousConvexCollision.h"
00018 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00019 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
00020 #include "LinearMath/btTransformUtil.h"
00021 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00022
00023 #include "btGjkPairDetector.h"
00024 #include "btPointCollector.h"
00025 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
00026
00027
00028
00029 btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
00030 :m_simplexSolver(simplexSolver),
00031 m_penetrationDepthSolver(penetrationDepthSolver),
00032 m_convexA(convexA),m_convexB1(convexB),m_planeShape(0)
00033 {
00034 }
00035
00036
00037 btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape* convexA,const btStaticPlaneShape* plane)
00038 :m_simplexSolver(0),
00039 m_penetrationDepthSolver(0),
00040 m_convexA(convexA),m_convexB1(0),m_planeShape(plane)
00041 {
00042 }
00043
00044
00047 #define MAX_ITERATIONS 64
00048
00049 void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
00050 {
00051 if (m_convexB1)
00052 {
00053 m_simplexSolver->reset();
00054 btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);
00055 btGjkPairDetector::ClosestPointInput input;
00056 input.m_transformA = transA;
00057 input.m_transformB = transB;
00058 gjk.getClosestPoints(input,pointCollector,0);
00059 } else
00060 {
00061
00062 const btConvexShape* convexShape = m_convexA;
00063 const btStaticPlaneShape* planeShape = m_planeShape;
00064
00065 const btVector3& planeNormal = planeShape->getPlaneNormal();
00066 const btScalar& planeConstant = planeShape->getPlaneConstant();
00067
00068 btTransform convexWorldTransform = transA;
00069 btTransform convexInPlaneTrans;
00070 convexInPlaneTrans= transB.inverse() * convexWorldTransform;
00071 btTransform planeInConvex;
00072 planeInConvex= convexWorldTransform.inverse() * transB;
00073
00074 btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
00075
00076 btVector3 vtxInPlane = convexInPlaneTrans(vtx);
00077 btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
00078
00079 btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
00080 btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
00081 btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
00082
00083 pointCollector.addContactPoint(
00084 normalOnSurfaceB,
00085 vtxInPlaneWorld,
00086 distance);
00087 }
00088 }
00089
00090 bool btContinuousConvexCollision::calcTimeOfImpact(
00091 const btTransform& fromA,
00092 const btTransform& toA,
00093 const btTransform& fromB,
00094 const btTransform& toB,
00095 CastResult& result)
00096 {
00097
00098
00100 btVector3 linVelA,angVelA,linVelB,angVelB;
00101 btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
00102 btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
00103
00104
00105 btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
00106 btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f;
00107
00108 btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
00109 btVector3 relLinVel = (linVelB-linVelA);
00110
00111 btScalar relLinVelocLength = (linVelB-linVelA).length();
00112
00113 if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
00114 return false;
00115
00116
00117
00118 btScalar lambda = btScalar(0.);
00119 btVector3 v(1,0,0);
00120
00121 int maxIter = MAX_ITERATIONS;
00122
00123 btVector3 n;
00124 n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00125 bool hasResult = false;
00126 btVector3 c;
00127
00128 btScalar lastLambda = lambda;
00129
00130
00131 int numIter = 0;
00132
00133
00134
00135 btScalar radius = 0.001f;
00136
00137
00138 btPointCollector pointCollector1;
00139
00140 {
00141
00142 computeClosestPoints(fromA,fromB,pointCollector1);
00143
00144 hasResult = pointCollector1.m_hasResult;
00145 c = pointCollector1.m_pointInWorld;
00146 }
00147
00148 if (hasResult)
00149 {
00150 btScalar dist;
00151 dist = pointCollector1.m_distance + result.m_allowedPenetration;
00152 n = pointCollector1.m_normalOnBInWorld;
00153 btScalar projectedLinearVelocity = relLinVel.dot(n);
00154 if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
00155 return false;
00156
00157
00158 while (dist > radius)
00159 {
00160 if (result.m_debugDrawer)
00161 {
00162 result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
00163 }
00164 btScalar dLambda = btScalar(0.);
00165
00166 projectedLinearVelocity = relLinVel.dot(n);
00167
00168
00169
00170 if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
00171 return false;
00172
00173 dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
00174
00175
00176
00177 lambda = lambda + dLambda;
00178
00179 if (lambda > btScalar(1.))
00180 return false;
00181
00182 if (lambda < btScalar(0.))
00183 return false;
00184
00185
00186
00187 if (lambda <= lastLambda)
00188 {
00189 return false;
00190
00191 break;
00192 }
00193 lastLambda = lambda;
00194
00195
00196
00197
00198 btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
00199
00200 btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
00201 btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
00202 relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
00203
00204 if (result.m_debugDrawer)
00205 {
00206 result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
00207 }
00208
00209 result.DebugDraw( lambda );
00210
00211 btPointCollector pointCollector;
00212 computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector);
00213
00214 if (pointCollector.m_hasResult)
00215 {
00216 dist = pointCollector.m_distance+result.m_allowedPenetration;
00217 c = pointCollector.m_pointInWorld;
00218 n = pointCollector.m_normalOnBInWorld;
00219 } else
00220 {
00221 result.reportFailure(-1, numIter);
00222 return false;
00223 }
00224
00225 numIter++;
00226 if (numIter > maxIter)
00227 {
00228 result.reportFailure(-2, numIter);
00229 return false;
00230 }
00231 }
00232
00233 result.m_fraction = lambda;
00234 result.m_normal = n;
00235 result.m_hitPoint = c;
00236 return true;
00237 }
00238
00239 return false;
00240
00241 }
00242