int main() {
for(int t = 0; t < 100000; ++t) {
int n = rand_int(2, 40);
vector<V> points;
for(int i = 0; i < n; ++i) {
points.push_back(V(rand_real(-1.0, 1.0), rand_real(-1.0, 1.0)));
}
double d = computeClosestPoints(points);
double cmpd = 100;
for(int i = 0; i < n; ++i) {
for(int j = i + 1; j < n; ++j) {
cmpd = min(cmpd, abs(points[i] - points[j]));
}
}
if(d != cmpd) fail();
}
return 0;
}
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;
}
