Logo Search packages:      
Sourcecode: blender version File versions  Download package

bool btGjkConvexCast::calcTimeOfImpact ( const btTransform fromA,
const btTransform toA,
const btTransform fromB,
const btTransform toB,
CastResult result 
) [virtual]

cast a convex against another convex object

compute linear velocity for this interval, to interpolate

Implements btConvexCast.

Definition at line 37 of file btGjkConvexCast.cpp.

References btConvexCast::CastResult::DebugDraw(), btGjkPairDetector::getClosestPoints(), btTransform::getOrigin(), btConvexCast::CastResult::m_allowedPenetration, btConvexCast::CastResult::m_fraction, btConvexCast::CastResult::m_hitPoint, btConvexCast::CastResult::m_normal, and btTransform::setIdentity().

Referenced by btConvexConvexAlgorithm::calculateTimeOfImpact().

{


      m_simplexSolver->reset();

      /// compute linear velocity for this interval, to interpolate
      //assume no rotation/angular velocity, assert here?
      btVector3 linVelA,linVelB;
      linVelA = toA.getOrigin()-fromA.getOrigin();
      linVelB = toB.getOrigin()-fromB.getOrigin();

      btScalar radius = btScalar(0.001);
      btScalar lambda = btScalar(0.);
      btVector3 v(1,0,0);

      int maxIter = MAX_ITERATIONS;

      btVector3 n;
      n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
      bool hasResult = false;
      btVector3 c;
      btVector3 r = (linVelA-linVelB);

      btScalar lastLambda = lambda;
      //btScalar epsilon = btScalar(0.001);

      int numIter = 0;
      //first solution, using GJK


      btTransform identityTrans;
      identityTrans.setIdentity();


//    result.drawCoordSystem(sphereTr);

      btPointCollector  pointCollector;

            
      btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);       
      btGjkPairDetector::ClosestPointInput input;

      //we don't use margins during CCD
      //    gjk.setIgnoreMargin(true);

      input.m_transformA = fromA;
      input.m_transformB = fromB;
      gjk.getClosestPoints(input,pointCollector,0);

      hasResult = pointCollector.m_hasResult;
      c = pointCollector.m_pointInWorld;

      if (hasResult)
      {
            btScalar dist;
            dist = pointCollector.m_distance;
            n = pointCollector.m_normalOnBInWorld;

      

            //not close enough
            while (dist > radius)
            {
                  numIter++;
                  if (numIter > maxIter)
                  {
                        return false; //todo: report a failure
                  }
                  btScalar dLambda = btScalar(0.);

                  btScalar projectedLinearVelocity = r.dot(n);
                  
                  dLambda = dist / (projectedLinearVelocity);

                  lambda = lambda - dLambda;

                  if (lambda > btScalar(1.))
                        return false;

                  if (lambda < btScalar(0.))
                        return false;

                  //todo: next check with relative epsilon
                  if (lambda <= lastLambda)
                  {
                        return false;
                        //n.setValue(0,0,0);
                        break;
                  }
                  lastLambda = lambda;

                  //interpolate to next lambda
                  result.DebugDraw( lambda );
                  input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
                  input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
                  
                  gjk.getClosestPoints(input,pointCollector,0);
                  if (pointCollector.m_hasResult)
                  {
                        if (pointCollector.m_distance < btScalar(0.))
                        {
                              result.m_fraction = lastLambda;
                              n = pointCollector.m_normalOnBInWorld;
                              result.m_normal=n;
                              result.m_hitPoint = pointCollector.m_pointInWorld;
                              return true;
                        }
                        c = pointCollector.m_pointInWorld;        
                        n = pointCollector.m_normalOnBInWorld;
                        dist = pointCollector.m_distance;
                  } else
                  {
                        //??
                        return false;
                  }

            }

            //is n normalized?
            //don't report time of impact for motion away from the contact normal (or causes minor penetration)
            if (n.dot(r)>=-result.m_allowedPenetration)
                  return false;

            result.m_fraction = lambda;
            result.m_normal = n;
            result.m_hitPoint = c;
            return true;
      }

      return false;


}


Generated by  Doxygen 1.6.0   Back to index