tests/box2d/Box2D/Collision/b2TimeOfImpact.cpp - external/github.com/kripken/emscripten - Git at Google

 /*
 * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 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.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

 #include <Box2D/Collision/b2Collision.h>
 #include <Box2D/Collision/b2Distance.h>
 #include <Box2D/Collision/b2TimeOfImpact.h>
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>

 #include <cstdio>
 using namespace std;

 int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
 int32 b2_toiRootIters, b2_toiMaxRootIters;

 struct b2SeparationFunction
 {
 	enum Type
 	{
 		e_points,
 		e_faceA,
 		e_faceB
 	};

 	// TODO_ERIN might not need to return the separation

 	float32 Initialize(const b2SimplexCache* cache,
 		const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
 		const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
 		float32 t1)
 	{
 		m_proxyA = proxyA;
 		m_proxyB = proxyB;
 		int32 count = cache->count;
 		b2Assert(0 < count && count < 3);

 		m_sweepA = sweepA;
 		m_sweepB = sweepB;

 		b2Transform xfA, xfB;
 		m_sweepA.GetTransform(&xfA, t1);
 		m_sweepB.GetTransform(&xfB, t1);

 		if (count == 1)
 		{
 			m_type = e_points;
 			b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
 			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
 			b2Vec2 pointA = b2Mul(xfA, localPointA);
 			b2Vec2 pointB = b2Mul(xfB, localPointB);
 			m_axis = pointB - pointA;
 			float32 s = m_axis.Normalize();
 			return s;
 		}
 		else if (cache->indexA[0] == cache->indexA[1])
 		{
 			// Two points on B and one on A.
 			m_type = e_faceB;
 			b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
 			b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);

 			m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
 			m_axis.Normalize();
 			b2Vec2 normal = b2Mul(xfB.q, m_axis);

 			m_localPoint = 0.5f * (localPointB1 + localPointB2);
 			b2Vec2 pointB = b2Mul(xfB, m_localPoint);

 			b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
 			b2Vec2 pointA = b2Mul(xfA, localPointA);

 			float32 s = b2Dot(pointA - pointB, normal);
 			if (s < 0.0f)
 			{
 				m_axis = -m_axis;
 				s = -s;
 			}
 			return s;
 		}
 		else
 		{
 			// Two points on A and one or two points on B.
 			m_type = e_faceA;
 			b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
 			b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);

 			m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
 			m_axis.Normalize();
 			b2Vec2 normal = b2Mul(xfA.q, m_axis);

 			m_localPoint = 0.5f * (localPointA1 + localPointA2);
 			b2Vec2 pointA = b2Mul(xfA, m_localPoint);

 			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
 			b2Vec2 pointB = b2Mul(xfB, localPointB);

 			float32 s = b2Dot(pointB - pointA, normal);
 			if (s < 0.0f)
 			{
 				m_axis = -m_axis;
 				s = -s;
 			}
 			return s;
 		}
 	}

 	float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
 	{
 		b2Transform xfA, xfB;
 		m_sweepA.GetTransform(&xfA, t);
 		m_sweepB.GetTransform(&xfB, t);

 		switch (m_type)
 		{
 		case e_points:
 			{
 				b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
 				b2Vec2 axisB = b2MulT(xfB.q, -m_axis);

 				*indexA = m_proxyA->GetSupport(axisA);
 				*indexB = m_proxyB->GetSupport(axisB);

 				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
 				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);

 				b2Vec2 pointA = b2Mul(xfA, localPointA);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);

 				float32 separation = b2Dot(pointB - pointA, m_axis);
 				return separation;
 			}

 		case e_faceA:
 			{
 				b2Vec2 normal = b2Mul(xfA.q, m_axis);
 				b2Vec2 pointA = b2Mul(xfA, m_localPoint);

 				b2Vec2 axisB = b2MulT(xfB.q, -normal);

 				*indexA = -1;
 				*indexB = m_proxyB->GetSupport(axisB);

 				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);

 				float32 separation = b2Dot(pointB - pointA, normal);
 				return separation;
 			}

 		case e_faceB:
 			{
 				b2Vec2 normal = b2Mul(xfB.q, m_axis);
 				b2Vec2 pointB = b2Mul(xfB, m_localPoint);

 				b2Vec2 axisA = b2MulT(xfA.q, -normal);

 				*indexB = -1;
 				*indexA = m_proxyA->GetSupport(axisA);

 				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
 				b2Vec2 pointA = b2Mul(xfA, localPointA);

 				float32 separation = b2Dot(pointA - pointB, normal);
 				return separation;
 			}

 		default:
 			b2Assert(false);
 			*indexA = -1;
 			*indexB = -1;
 			return 0.0f;
 		}
 	}

 	float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
 	{
 		b2Transform xfA, xfB;
 		m_sweepA.GetTransform(&xfA, t);
 		m_sweepB.GetTransform(&xfB, t);

 		switch (m_type)
 		{
 		case e_points:
 			{
 				b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
 				b2Vec2 axisB = b2MulT(xfB.q, -m_axis);

 				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
 				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);

 				b2Vec2 pointA = b2Mul(xfA, localPointA);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);
 				float32 separation = b2Dot(pointB - pointA, m_axis);

 				return separation;
 			}

 		case e_faceA:
 			{
 				b2Vec2 normal = b2Mul(xfA.q, m_axis);
 				b2Vec2 pointA = b2Mul(xfA, m_localPoint);

 				b2Vec2 axisB = b2MulT(xfB.q, -normal);

 				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);

 				float32 separation = b2Dot(pointB - pointA, normal);
 				return separation;
 			}

 		case e_faceB:
 			{
 				b2Vec2 normal = b2Mul(xfB.q, m_axis);
 				b2Vec2 pointB = b2Mul(xfB, m_localPoint);

 				b2Vec2 axisA = b2MulT(xfA.q, -normal);

 				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
 				b2Vec2 pointA = b2Mul(xfA, localPointA);

 				float32 separation = b2Dot(pointA - pointB, normal);
 				return separation;
 			}

 		default:
 			b2Assert(false);
 			return 0.0f;
 		}
 	}

 	const b2DistanceProxy* m_proxyA;
 	const b2DistanceProxy* m_proxyB;
 	b2Sweep m_sweepA, m_sweepB;
 	Type m_type;
 	b2Vec2 m_localPoint;
 	b2Vec2 m_axis;
 };

 // CCD via the local separating axis method. This seeks progression
 // by computing the largest time at which separation is maintained.
 void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 {
 	++b2_toiCalls;

 	output->state = b2TOIOutput::e_unknown;
 	output->t = input->tMax;

 	const b2DistanceProxy* proxyA = &input->proxyA;
 	const b2DistanceProxy* proxyB = &input->proxyB;

 	b2Sweep sweepA = input->sweepA;
 	b2Sweep sweepB = input->sweepB;

 	// Large rotations can make the root finder fail, so we normalize the
 	// sweep angles.
 	sweepA.Normalize();
 	sweepB.Normalize();

 	float32 tMax = input->tMax;

 	float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
 	float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
 	float32 tolerance = 0.25f * b2_linearSlop;
 	b2Assert(target > tolerance);

 	float32 t1 = 0.0f;
 	const int32 k_maxIterations = 20;	// TODO_ERIN b2Settings
 	int32 iter = 0;

 	// Prepare input for distance query.
 	b2SimplexCache cache;
 	cache.count = 0;
 	b2DistanceInput distanceInput;
 	distanceInput.proxyA = input->proxyA;
 	distanceInput.proxyB = input->proxyB;
 	distanceInput.useRadii = false;

 	// The outer loop progressively attempts to compute new separating axes.
 	// This loop terminates when an axis is repeated (no progress is made).
 	for(;;)
 	{
 		b2Transform xfA, xfB;
 		sweepA.GetTransform(&xfA, t1);
 		sweepB.GetTransform(&xfB, t1);

 		// Get the distance between shapes. We can also use the results
 		// to get a separating axis.
 		distanceInput.transformA = xfA;
 		distanceInput.transformB = xfB;
 		b2DistanceOutput distanceOutput;
 		b2Distance(&distanceOutput, &cache, &distanceInput);

 		// If the shapes are overlapped, we give up on continuous collision.
 		if (distanceOutput.distance <= 0.0f)
 		{
 			// Failure!
 			output->state = b2TOIOutput::e_overlapped;
 			output->t = 0.0f;
 			break;
 		}

 		if (distanceOutput.distance < target + tolerance)
 		{
 			// Victory!
 			output->state = b2TOIOutput::e_touching;
 			output->t = t1;
 			break;
 		}

 		// Initialize the separating axis.
 		b2SeparationFunction fcn;
 		fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
 #if 0
 		// Dump the curve seen by the root finder
 		{
 			const int32 N = 100;
 			float32 dx = 1.0f / N;
 			float32 xs[N+1];
 			float32 fs[N+1];

 			float32 x = 0.0f;

 			for (int32 i = 0; i <= N; ++i)
 			{
 				sweepA.GetTransform(&xfA, x);
 				sweepB.GetTransform(&xfB, x);
 				float32 f = fcn.Evaluate(xfA, xfB) - target;

 				printf("%g %g\n", x, f);

 				xs[i] = x;
 				fs[i] = f;

 				x += dx;
 			}
 		}
 #endif

 		// Compute the TOI on the separating axis. We do this by successively
 		// resolving the deepest point. This loop is bounded by the number of vertices.
 		bool done = false;
 		float32 t2 = tMax;
 		int32 pushBackIter = 0;
 		for (;;)
 		{
 			// Find the deepest point at t2. Store the witness point indices.
 			int32 indexA, indexB;
 			float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);

 			// Is the final configuration separated?
 			if (s2 > target + tolerance)
 			{
 				// Victory!
 				output->state = b2TOIOutput::e_separated;
 				output->t = tMax;
 				done = true;
 				break;
 			}

 			// Has the separation reached tolerance?
 			if (s2 > target - tolerance)
 			{
 				// Advance the sweeps
 				t1 = t2;
 				break;
 			}

 			// Compute the initial separation of the witness points.
 			float32 s1 = fcn.Evaluate(indexA, indexB, t1);

 			// Check for initial overlap. This might happen if the root finder
 			// runs out of iterations.
 			if (s1 < target - tolerance)
 			{
 				output->state = b2TOIOutput::e_failed;
 				output->t = t1;
 				done = true;
 				break;
 			}

 			// Check for touching
 			if (s1 <= target + tolerance)
 			{
 				// Victory! t1 should hold the TOI (could be 0.0).
 				output->state = b2TOIOutput::e_touching;
 				output->t = t1;
 				done = true;
 				break;
 			}

 			// Compute 1D root of: f(x) - target = 0
 			int32 rootIterCount = 0;
 			float32 a1 = t1, a2 = t2;
 			for (;;)
 			{
 				// Use a mix of the secant rule and bisection.
 				float32 t;
 				if (rootIterCount & 1)
 				{
 					// Secant rule to improve convergence.
 					t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
 				}
 				else
 				{
 					// Bisection to guarantee progress.
 					t = 0.5f * (a1 + a2);
 				}

 				float32 s = fcn.Evaluate(indexA, indexB, t);

 				if (b2Abs(s - target) < tolerance)
 				{
 					// t2 holds a tentative value for t1
 					t2 = t;
 					break;
 				}

 				// Ensure we continue to bracket the root.
 				if (s > target)
 				{
 					a1 = t;
 					s1 = s;
 				}
 				else
 				{
 					a2 = t;
 					s2 = s;
 				}

 				++rootIterCount;
 				++b2_toiRootIters;

 				if (rootIterCount == 50)
 				{
 					break;
 				}
 			}

 			b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);

 			++pushBackIter;

 			if (pushBackIter == b2_maxPolygonVertices)
 			{
 				break;
 			}
 		}

 		++iter;
 		++b2_toiIters;

 		if (done)
 		{
 			break;
 		}

 		if (iter == k_maxIterations)
 		{
 			// Root finder got stuck. Semi-victory.
 			output->state = b2TOIOutput::e_failed;
 			output->t = t1;
 			break;
 		}
 	}

 	b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
 }
	/*
	* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
	*
	* This software is provided 'as-is', without any express or implied
	* warranty. In no event will the authors be held liable for any damages
	* arising from the use of this software.
	* Permission is granted to anyone to use this software for any purpose,
	* including commercial applications, and to alter it and redistribute it
	* freely, subject to the following restrictions:
	* 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.
	* 2. Altered source versions must be plainly marked as such, and must not be
	* misrepresented as being the original software.
	* 3. This notice may not be removed or altered from any source distribution.
	*/

	#include <Box2D/Collision/b2Collision.h>
	#include <Box2D/Collision/b2Distance.h>
	#include <Box2D/Collision/b2TimeOfImpact.h>
	#include <Box2D/Collision/Shapes/b2CircleShape.h>
	#include <Box2D/Collision/Shapes/b2PolygonShape.h>

	#include <cstdio>
	using namespace std;

	int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
	int32 b2_toiRootIters, b2_toiMaxRootIters;

	struct b2SeparationFunction
	{
	enum Type
	{
	e_points,
	e_faceA,
	e_faceB
	};

	// TODO_ERIN might not need to return the separation

	float32 Initialize(const b2SimplexCache* cache,
	const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
	const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
	float32 t1)
	{
	m_proxyA = proxyA;
	m_proxyB = proxyB;
	int32 count = cache->count;
	b2Assert(0 < count && count < 3);

	m_sweepA = sweepA;
	m_sweepB = sweepB;

	b2Transform xfA, xfB;
	m_sweepA.GetTransform(&xfA, t1);
	m_sweepB.GetTransform(&xfB, t1);

	if (count == 1)
	{
	m_type = e_points;
	b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
	b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
	b2Vec2 pointA = b2Mul(xfA, localPointA);
	b2Vec2 pointB = b2Mul(xfB, localPointB);
	m_axis = pointB - pointA;
	float32 s = m_axis.Normalize();
	return s;
	}
	else if (cache->indexA[0] == cache->indexA[1])
	{
	// Two points on B and one on A.
	m_type = e_faceB;
	b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
	b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);

	m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
	m_axis.Normalize();
	b2Vec2 normal = b2Mul(xfB.q, m_axis);

	m_localPoint = 0.5f * (localPointB1 + localPointB2);
	b2Vec2 pointB = b2Mul(xfB, m_localPoint);

	b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
	b2Vec2 pointA = b2Mul(xfA, localPointA);

	float32 s = b2Dot(pointA - pointB, normal);
	if (s < 0.0f)
	{
	m_axis = -m_axis;
	s = -s;
	}
	return s;
	}
	else
	{
	// Two points on A and one or two points on B.
	m_type = e_faceA;
	b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
	b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);

	m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
	m_axis.Normalize();
	b2Vec2 normal = b2Mul(xfA.q, m_axis);

	m_localPoint = 0.5f * (localPointA1 + localPointA2);
	b2Vec2 pointA = b2Mul(xfA, m_localPoint);

	b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
	b2Vec2 pointB = b2Mul(xfB, localPointB);

	float32 s = b2Dot(pointB - pointA, normal);
	if (s < 0.0f)
	{
	m_axis = -m_axis;
	s = -s;
	}
	return s;
	}
	}

	float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
	{
	b2Transform xfA, xfB;
	m_sweepA.GetTransform(&xfA, t);
	m_sweepB.GetTransform(&xfB, t);

	switch (m_type)
	{
	case e_points:
	{
	b2Vec2 axisA = b2MulT(xfA.q, m_axis);
	b2Vec2 axisB = b2MulT(xfB.q, -m_axis);

	*indexA = m_proxyA->GetSupport(axisA);
	*indexB = m_proxyB->GetSupport(axisB);

	b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
	b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);

	b2Vec2 pointA = b2Mul(xfA, localPointA);
	b2Vec2 pointB = b2Mul(xfB, localPointB);

	float32 separation = b2Dot(pointB - pointA, m_axis);
	return separation;
	}

	case e_faceA:
	{
	b2Vec2 normal = b2Mul(xfA.q, m_axis);
	b2Vec2 pointA = b2Mul(xfA, m_localPoint);

	b2Vec2 axisB = b2MulT(xfB.q, -normal);

	*indexA = -1;
	*indexB = m_proxyB->GetSupport(axisB);

	b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
	b2Vec2 pointB = b2Mul(xfB, localPointB);

	float32 separation = b2Dot(pointB - pointA, normal);
	return separation;
	}

	case e_faceB:
	{
	b2Vec2 normal = b2Mul(xfB.q, m_axis);
	b2Vec2 pointB = b2Mul(xfB, m_localPoint);

	b2Vec2 axisA = b2MulT(xfA.q, -normal);

	*indexB = -1;
	*indexA = m_proxyA->GetSupport(axisA);

	b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
	b2Vec2 pointA = b2Mul(xfA, localPointA);

	float32 separation = b2Dot(pointA - pointB, normal);
	return separation;
	}

	default:
	b2Assert(false);
	*indexA = -1;
	*indexB = -1;
	return 0.0f;
	}
	}

	float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
	{
	b2Transform xfA, xfB;
	m_sweepA.GetTransform(&xfA, t);
	m_sweepB.GetTransform(&xfB, t);

	switch (m_type)
	{
	case e_points:
	{
	b2Vec2 axisA = b2MulT(xfA.q, m_axis);
	b2Vec2 axisB = b2MulT(xfB.q, -m_axis);

	b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
	b2Vec2 localPointB = m_proxyB->GetVertex(indexB);

	b2Vec2 pointA = b2Mul(xfA, localPointA);
	b2Vec2 pointB = b2Mul(xfB, localPointB);
	float32 separation = b2Dot(pointB - pointA, m_axis);

	return separation;
	}

	case e_faceA:
	{
	b2Vec2 normal = b2Mul(xfA.q, m_axis);
	b2Vec2 pointA = b2Mul(xfA, m_localPoint);

	b2Vec2 axisB = b2MulT(xfB.q, -normal);

	b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
	b2Vec2 pointB = b2Mul(xfB, localPointB);

	float32 separation = b2Dot(pointB - pointA, normal);
	return separation;
	}

	case e_faceB:
	{
	b2Vec2 normal = b2Mul(xfB.q, m_axis);
	b2Vec2 pointB = b2Mul(xfB, m_localPoint);

	b2Vec2 axisA = b2MulT(xfA.q, -normal);

	b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
	b2Vec2 pointA = b2Mul(xfA, localPointA);

	float32 separation = b2Dot(pointA - pointB, normal);
	return separation;
	}

	default:
	b2Assert(false);
	return 0.0f;
	}
	}

	const b2DistanceProxy* m_proxyA;
	const b2DistanceProxy* m_proxyB;
	b2Sweep m_sweepA, m_sweepB;
	Type m_type;
	b2Vec2 m_localPoint;
	b2Vec2 m_axis;
	};

	// CCD via the local separating axis method. This seeks progression
	// by computing the largest time at which separation is maintained.
	void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
	{
	++b2_toiCalls;

	output->state = b2TOIOutput::e_unknown;
	output->t = input->tMax;

	const b2DistanceProxy* proxyA = &input->proxyA;
	const b2DistanceProxy* proxyB = &input->proxyB;

	b2Sweep sweepA = input->sweepA;
	b2Sweep sweepB = input->sweepB;

	// Large rotations can make the root finder fail, so we normalize the
	// sweep angles.
	sweepA.Normalize();
	sweepB.Normalize();

	float32 tMax = input->tMax;

	float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
	float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
	float32 tolerance = 0.25f * b2_linearSlop;
	b2Assert(target > tolerance);

	float32 t1 = 0.0f;
	const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
	int32 iter = 0;

	// Prepare input for distance query.
	b2SimplexCache cache;
	cache.count = 0;
	b2DistanceInput distanceInput;
	distanceInput.proxyA = input->proxyA;
	distanceInput.proxyB = input->proxyB;
	distanceInput.useRadii = false;

	// The outer loop progressively attempts to compute new separating axes.
	// This loop terminates when an axis is repeated (no progress is made).
	for(;;)
	{
	b2Transform xfA, xfB;
	sweepA.GetTransform(&xfA, t1);
	sweepB.GetTransform(&xfB, t1);

	// Get the distance between shapes. We can also use the results
	// to get a separating axis.
	distanceInput.transformA = xfA;
	distanceInput.transformB = xfB;
	b2DistanceOutput distanceOutput;
	b2Distance(&distanceOutput, &cache, &distanceInput);

	// If the shapes are overlapped, we give up on continuous collision.
	if (distanceOutput.distance <= 0.0f)
	{
	// Failure!
	output->state = b2TOIOutput::e_overlapped;
	output->t = 0.0f;
	break;
	}

	if (distanceOutput.distance < target + tolerance)
	{
	// Victory!
	output->state = b2TOIOutput::e_touching;
	output->t = t1;
	break;
	}

	// Initialize the separating axis.
	b2SeparationFunction fcn;
	fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
	#if 0
	// Dump the curve seen by the root finder
	{
	const int32 N = 100;
	float32 dx = 1.0f / N;
	float32 xs[N+1];
	float32 fs[N+1];

	float32 x = 0.0f;

	for (int32 i = 0; i <= N; ++i)
	{
	sweepA.GetTransform(&xfA, x);
	sweepB.GetTransform(&xfB, x);
	float32 f = fcn.Evaluate(xfA, xfB) - target;

	printf("%g %g\n", x, f);

	xs[i] = x;
	fs[i] = f;

	x += dx;
	}
	}
	#endif

	// Compute the TOI on the separating axis. We do this by successively
	// resolving the deepest point. This loop is bounded by the number of vertices.
	bool done = false;
	float32 t2 = tMax;
	int32 pushBackIter = 0;
	for (;;)
	{
	// Find the deepest point at t2. Store the witness point indices.
	int32 indexA, indexB;
	float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);

	// Is the final configuration separated?
	if (s2 > target + tolerance)
	{
	// Victory!
	output->state = b2TOIOutput::e_separated;
	output->t = tMax;
	done = true;
	break;
	}

	// Has the separation reached tolerance?
	if (s2 > target - tolerance)
	{
	// Advance the sweeps
	t1 = t2;
	break;
	}

	// Compute the initial separation of the witness points.
	float32 s1 = fcn.Evaluate(indexA, indexB, t1);

	// Check for initial overlap. This might happen if the root finder
	// runs out of iterations.
	if (s1 < target - tolerance)
	{
	output->state = b2TOIOutput::e_failed;
	output->t = t1;
	done = true;
	break;
	}

	// Check for touching
	if (s1 <= target + tolerance)
	{
	// Victory! t1 should hold the TOI (could be 0.0).
	output->state = b2TOIOutput::e_touching;
	output->t = t1;
	done = true;
	break;
	}

	// Compute 1D root of: f(x) - target = 0
	int32 rootIterCount = 0;
	float32 a1 = t1, a2 = t2;
	for (;;)
	{
	// Use a mix of the secant rule and bisection.
	float32 t;
	if (rootIterCount & 1)
	{
	// Secant rule to improve convergence.
	t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
	}
	else
	{
	// Bisection to guarantee progress.
	t = 0.5f * (a1 + a2);
	}

	float32 s = fcn.Evaluate(indexA, indexB, t);

	if (b2Abs(s - target) < tolerance)
	{
	// t2 holds a tentative value for t1
	t2 = t;
	break;
	}

	// Ensure we continue to bracket the root.
	if (s > target)
	{
	a1 = t;
	s1 = s;
	}
	else
	{
	a2 = t;
	s2 = s;
	}

	++rootIterCount;
	++b2_toiRootIters;

	if (rootIterCount == 50)
	{
	break;
	}
	}

	b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);

	++pushBackIter;

	if (pushBackIter == b2_maxPolygonVertices)
	{
	break;
	}
	}

	++iter;
	++b2_toiIters;

	if (done)
	{
	break;
	}

	if (iter == k_maxIterations)
	{
	// Root finder got stuck. Semi-victory.
	output->state = b2TOIOutput::e_failed;
	output->t = t1;
	break;
	}
	}

	b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
	}