btContinuousConvexCollision.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
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                 //convex versus plane
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         //btScalar epsilon = btScalar(0.001);
00130 
00131         int numIter = 0;
00132         //first solution, using GJK
00133 
00134 
00135         btScalar radius = 0.001f;
00136 //      result.drawCoordSystem(sphereTr);
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                 //not close enough
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                         //don't report time of impact for motion away from the contact normal (or causes minor penetration)
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                         //todo: next check with relative epsilon
00187                         if (lambda <= lastLambda)
00188                         {
00189                                 return false;
00190                                 //n.setValue(0,0,0);
00191                                 break;
00192                         }
00193                         lastLambda = lambda;
00194 
00195                         
00196 
00197                         //interpolate to next lambda
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