Beispiel #1
0
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;
}