bool btContinuousConvexCollision::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, const btTransform& fromB, const btTransform& toB, CastResult& result) { m_simplexSolver->reset(); /// compute linear and angular velocity for this interval, to interpolate btVector3 linVelA,angVelA,linVelB,angVelB; btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA); btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB); btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); btScalar boundingRadiusB = m_convexB->getAngularMotionDisc(); btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; btVector3 relLinVel = (linVelB-linVelA); btScalar relLinVelocLength = (linVelB-linVelA).length(); if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) return false; 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; btScalar lastLambda = lambda; //btScalar epsilon = btScalar(0.001); int numIter = 0; //first solution, using GJK btTransform identityTrans; identityTrans.setIdentity(); btSphereShape raySphere(btScalar(0.0)); raySphere.setMargin(btScalar(0.)); // result.drawCoordSystem(sphereTr); btPointCollector pointCollector1; { btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,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,pointCollector1,0); hasResult = pointCollector1.m_hasResult; c = pointCollector1.m_pointInWorld; } if (hasResult) { btScalar dist; dist = pointCollector1.m_distance; n = pointCollector1.m_normalOnBInWorld; btScalar projectedLinearVelocity = relLinVel.dot(n); //not close enough while (dist > radius) { numIter++; if (numIter > maxIter) { return false; //todo: report a failure } btScalar dLambda = btScalar(0.); projectedLinearVelocity = relLinVel.dot(n); //calculate safe moving fraction from distance / (linear+rotational velocity) //btScalar clippedDist = GEN_min(angularConservativeRadius,dist); //btScalar clippedDist = dist; //don't report time of impact for motion away from the contact normal (or causes minor penetration) if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) return false; dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); 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 btTransform interpolatedTransA,interpolatedTransB,relativeTrans; btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA); btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); result.DebugDraw( lambda ); btPointCollector pointCollector; btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; input.m_transformA = interpolatedTransA; input.m_transformB = interpolatedTransB; gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { if (pointCollector.m_distance < btScalar(0.)) { //degenerate ?! result.m_fraction = lastLambda; n = pointCollector.m_normalOnBInWorld; result.m_normal=n;//.setValue(1,1,1);// = n; result.m_hitPoint = pointCollector.m_pointInWorld; return true; } c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; dist = pointCollector.m_distance; } else { //?? return false; } } if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON) return false; result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; return true; } return false; /* //todo: //if movement away from normal, discard result btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin(); if (result.m_fraction < btScalar(1.)) { if (move.dot(result.m_normal) <= btScalar(0.)) { } } */ }
bool btGjkConvexCast::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, const btTransform& fromB, const btTransform& toB, CastResult& result) { 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; }
bool btContinuousConvexCollision::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, const btTransform& fromB, const btTransform& toB, CastResult& result) { /// compute linear and angular velocity for this interval, to interpolate btVector3 linVelA,angVelA,linVelB,angVelB; btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA); btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB); btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f; btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; btVector3 relLinVel = (linVelB-linVelA); btScalar relLinVelocLength = (linVelB-linVelA).length(); if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) return false; btScalar lambda = btScalar(0.); btVector3 n; n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; btScalar lastLambda = lambda; //btScalar epsilon = btScalar(0.001); int numIter = 0; //first solution, using GJK btScalar radius = 0.001f; // result.drawCoordSystem(sphereTr); btPointCollector pointCollector1; { computeClosestPoints(fromA,fromB,pointCollector1); hasResult = pointCollector1.m_hasResult; c = pointCollector1.m_pointInWorld; } if (hasResult) { btScalar dist; dist = pointCollector1.m_distance + result.m_allowedPenetration; n = pointCollector1.m_normalOnBInWorld; btScalar projectedLinearVelocity = relLinVel.dot(n); if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) return false; //not close enough while (dist > radius) { if (result.m_debugDrawer) { result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1)); } btScalar dLambda = btScalar(0.); projectedLinearVelocity = relLinVel.dot(n); //don't report time of impact for motion away from the contact normal (or causes minor penetration) if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) return false; dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); lambda += dLambda; if (lambda > btScalar(1.) || 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 btTransform interpolatedTransA,interpolatedTransB,relativeTrans; btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA); btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); if (result.m_debugDrawer) { result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0)); } result.DebugDraw( lambda ); btPointCollector pointCollector; computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector); if (pointCollector.m_hasResult) { dist = pointCollector.m_distance+result.m_allowedPenetration; c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; } else { result.reportFailure(-1, numIter); return false; } numIter++; if (numIter > MAX_ITERATIONS) { result.reportFailure(-2, numIter); return false; } } result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; return true; } return false; }