btGjkConvexCast.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 
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         //assume no rotation/angular velocity, assert here?
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         //btScalar epsilon = btScalar(0.001);
00068 
00069         int numIter = 0;
00070         //first solution, using GJK
00071 
00072 
00073         btTransform identityTrans;
00074         identityTrans.setIdentity();
00075 
00076 
00077 //      result.drawCoordSystem(sphereTr);
00078 
00079         btPointCollector        pointCollector;
00080 
00081                 
00082         btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);               
00083         btGjkPairDetector::ClosestPointInput input;
00084 
00085         //we don't use margins during CCD
00086         //      gjk.setIgnoreMargin(true);
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                 //not close enough
00104                 while (dist > radius)
00105                 {
00106                         numIter++;
00107                         if (numIter > maxIter)
00108                         {
00109                                 return false; //todo: report a failure
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                         //todo: next check with relative epsilon
00126                         if (lambda <= lastLambda)
00127                         {
00128                                 return false;
00129                                 //n.setValue(0,0,0);
00130                                 break;
00131                         }
00132                         lastLambda = lambda;
00133 
00134                         //interpolate to next lambda
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                 //is n normalized?
00162                 //don't report time of impact for motion away from the contact normal (or causes minor penetration)
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