tests/box2d/Box2D/Collision/b2Distance.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/b2Distance.h>
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
 #include <Box2D/Collision/Shapes/b2ChainShape.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>

 // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
 int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;

 void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
 {
 	switch (shape->GetType())
 	{
 	case b2Shape::e_circle:
 		{
 			const b2CircleShape* circle = (b2CircleShape*)shape;
 			m_vertices = &circle->m_p;
 			m_count = 1;
 			m_radius = circle->m_radius;
 		}
 		break;

 	case b2Shape::e_polygon:
 		{
 			const b2PolygonShape* polygon = (b2PolygonShape*)shape;
 			m_vertices = polygon->m_vertices;
 			m_count = polygon->m_vertexCount;
 			m_radius = polygon->m_radius;
 		}
 		break;

 	case b2Shape::e_chain:
 		{
 			const b2ChainShape* chain = (b2ChainShape*)shape;
 			b2Assert(0 <= index && index < chain->m_count);

 			m_buffer[0] = chain->m_vertices[index];
 			if (index + 1 < chain->m_count)
 			{
 				m_buffer[1] = chain->m_vertices[index + 1];
 			}
 			else
 			{
 				m_buffer[1] = chain->m_vertices[0];
 			}

 			m_vertices = m_buffer;
 			m_count = 2;
 			m_radius = chain->m_radius;
 		}
 		break;

 	case b2Shape::e_edge:
 		{
 			const b2EdgeShape* edge = (b2EdgeShape*)shape;
 			m_vertices = &edge->m_vertex1;
 			m_count = 2;
 			m_radius = edge->m_radius;
 		}
 		break;

 	default:
 		b2Assert(false);
 	}
 }


 struct b2SimplexVertex
 {
 	b2Vec2 wA;		// support point in proxyA
 	b2Vec2 wB;		// support point in proxyB
 	b2Vec2 w;		// wB - wA
 	float32 a;		// barycentric coordinate for closest point
 	int32 indexA;	// wA index
 	int32 indexB;	// wB index
 };

 struct b2Simplex
 {
 	void ReadCache(	const b2SimplexCache* cache,
 					const b2DistanceProxy* proxyA, const b2Transform& transformA,
 					const b2DistanceProxy* proxyB, const b2Transform& transformB)
 	{
 		b2Assert(cache->count <= 3);

 		// Copy data from cache.
 		m_count = cache->count;
 		b2SimplexVertex* vertices = &m_v1;
 		for (int32 i = 0; i < m_count; ++i)
 		{
 			b2SimplexVertex* v = vertices + i;
 			v->indexA = cache->indexA[i];
 			v->indexB = cache->indexB[i];
 			b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
 			b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
 			v->wA = b2Mul(transformA, wALocal);
 			v->wB = b2Mul(transformB, wBLocal);
 			v->w = v->wB - v->wA;
 			v->a = 0.0f;
 		}

 		// Compute the new simplex metric, if it is substantially different than
 		// old metric then flush the simplex.
 		if (m_count > 1)
 		{
 			float32 metric1 = cache->metric;
 			float32 metric2 = GetMetric();
 			if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
 			{
 				// Reset the simplex.
 				m_count = 0;
 			}
 		}

 		// If the cache is empty or invalid ...
 		if (m_count == 0)
 		{
 			b2SimplexVertex* v = vertices + 0;
 			v->indexA = 0;
 			v->indexB = 0;
 			b2Vec2 wALocal = proxyA->GetVertex(0);
 			b2Vec2 wBLocal = proxyB->GetVertex(0);
 			v->wA = b2Mul(transformA, wALocal);
 			v->wB = b2Mul(transformB, wBLocal);
 			v->w = v->wB - v->wA;
 			m_count = 1;
 		}
 	}

 	void WriteCache(b2SimplexCache* cache) const
 	{
 		cache->metric = GetMetric();
 		cache->count = uint16(m_count);
 		const b2SimplexVertex* vertices = &m_v1;
 		for (int32 i = 0; i < m_count; ++i)
 		{
 			cache->indexA[i] = uint8(vertices[i].indexA);
 			cache->indexB[i] = uint8(vertices[i].indexB);
 		}
 	}

 	b2Vec2 GetSearchDirection() const
 	{
 		switch (m_count)
 		{
 		case 1:
 			return -m_v1.w;

 		case 2:
 			{
 				b2Vec2 e12 = m_v2.w - m_v1.w;
 				float32 sgn = b2Cross(e12, -m_v1.w);
 				if (sgn > 0.0f)
 				{
 					// Origin is left of e12.
 					return b2Cross(1.0f, e12);
 				}
 				else
 				{
 					// Origin is right of e12.
 					return b2Cross(e12, 1.0f);
 				}
 			}

 		default:
 			b2Assert(false);
 			return b2Vec2_zero;
 		}
 	}

 	b2Vec2 GetClosestPoint() const
 	{
 		switch (m_count)
 		{
 		case 0:
 			b2Assert(false);
 			return b2Vec2_zero;

 		case 1:
 			return m_v1.w;

 		case 2:
 			return m_v1.a * m_v1.w + m_v2.a * m_v2.w;

 		case 3:
 			return b2Vec2_zero;

 		default:
 			b2Assert(false);
 			return b2Vec2_zero;
 		}
 	}

 	void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
 	{
 		switch (m_count)
 		{
 		case 0:
 			b2Assert(false);
 			break;

 		case 1:
 			*pA = m_v1.wA;
 			*pB = m_v1.wB;
 			break;

 		case 2:
 			*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
 			*pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
 			break;

 		case 3:
 			*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
 			*pB = *pA;
 			break;

 		default:
 			b2Assert(false);
 			break;
 		}
 	}

 	float32 GetMetric() const
 	{
 		switch (m_count)
 		{
 		case 0:
 			b2Assert(false);
 			return 0.0;

 		case 1:
 			return 0.0f;

 		case 2:
 			return b2Distance(m_v1.w, m_v2.w);

 		case 3:
 			return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);

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

 	void Solve2();
 	void Solve3();

 	b2SimplexVertex m_v1, m_v2, m_v3;
 	int32 m_count;
 };


 // Solve a line segment using barycentric coordinates.
 //
 // p = a1 * w1 + a2 * w2
 // a1 + a2 = 1
 //
 // The vector from the origin to the closest point on the line is
 // perpendicular to the line.
 // e12 = w2 - w1
 // dot(p, e) = 0
 // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
 //
 // 2-by-2 linear system
 // [1      1     ][a1] = [1]
 // [w1.e12 w2.e12][a2] = [0]
 //
 // Define
 // d12_1 =  dot(w2, e12)
 // d12_2 = -dot(w1, e12)
 // d12 = d12_1 + d12_2
 //
 // Solution
 // a1 = d12_1 / d12
 // a2 = d12_2 / d12
 void b2Simplex::Solve2()
 {
 	b2Vec2 w1 = m_v1.w;
 	b2Vec2 w2 = m_v2.w;
 	b2Vec2 e12 = w2 - w1;

 	// w1 region
 	float32 d12_2 = -b2Dot(w1, e12);
 	if (d12_2 <= 0.0f)
 	{
 		// a2 <= 0, so we clamp it to 0
 		m_v1.a = 1.0f;
 		m_count = 1;
 		return;
 	}

 	// w2 region
 	float32 d12_1 = b2Dot(w2, e12);
 	if (d12_1 <= 0.0f)
 	{
 		// a1 <= 0, so we clamp it to 0
 		m_v2.a = 1.0f;
 		m_count = 1;
 		m_v1 = m_v2;
 		return;
 	}

 	// Must be in e12 region.
 	float32 inv_d12 = 1.0f / (d12_1 + d12_2);
 	m_v1.a = d12_1 * inv_d12;
 	m_v2.a = d12_2 * inv_d12;
 	m_count = 2;
 }

 // Possible regions:
 // - points[2]
 // - edge points[0]-points[2]
 // - edge points[1]-points[2]
 // - inside the triangle
 void b2Simplex::Solve3()
 {
 	b2Vec2 w1 = m_v1.w;
 	b2Vec2 w2 = m_v2.w;
 	b2Vec2 w3 = m_v3.w;

 	// Edge12
 	// [1      1     ][a1] = [1]
 	// [w1.e12 w2.e12][a2] = [0]
 	// a3 = 0
 	b2Vec2 e12 = w2 - w1;
 	float32 w1e12 = b2Dot(w1, e12);
 	float32 w2e12 = b2Dot(w2, e12);
 	float32 d12_1 = w2e12;
 	float32 d12_2 = -w1e12;

 	// Edge13
 	// [1      1     ][a1] = [1]
 	// [w1.e13 w3.e13][a3] = [0]
 	// a2 = 0
 	b2Vec2 e13 = w3 - w1;
 	float32 w1e13 = b2Dot(w1, e13);
 	float32 w3e13 = b2Dot(w3, e13);
 	float32 d13_1 = w3e13;
 	float32 d13_2 = -w1e13;

 	// Edge23
 	// [1      1     ][a2] = [1]
 	// [w2.e23 w3.e23][a3] = [0]
 	// a1 = 0
 	b2Vec2 e23 = w3 - w2;
 	float32 w2e23 = b2Dot(w2, e23);
 	float32 w3e23 = b2Dot(w3, e23);
 	float32 d23_1 = w3e23;
 	float32 d23_2 = -w2e23;

 	// Triangle123
 	float32 n123 = b2Cross(e12, e13);

 	float32 d123_1 = n123 * b2Cross(w2, w3);
 	float32 d123_2 = n123 * b2Cross(w3, w1);
 	float32 d123_3 = n123 * b2Cross(w1, w2);

 	// w1 region
 	if (d12_2 <= 0.0f && d13_2 <= 0.0f)
 	{
 		m_v1.a = 1.0f;
 		m_count = 1;
 		return;
 	}

 	// e12
 	if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
 	{
 		float32 inv_d12 = 1.0f / (d12_1 + d12_2);
 		m_v1.a = d12_1 * inv_d12;
 		m_v2.a = d12_2 * inv_d12;
 		m_count = 2;
 		return;
 	}

 	// e13
 	if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
 	{
 		float32 inv_d13 = 1.0f / (d13_1 + d13_2);
 		m_v1.a = d13_1 * inv_d13;
 		m_v3.a = d13_2 * inv_d13;
 		m_count = 2;
 		m_v2 = m_v3;
 		return;
 	}

 	// w2 region
 	if (d12_1 <= 0.0f && d23_2 <= 0.0f)
 	{
 		m_v2.a = 1.0f;
 		m_count = 1;
 		m_v1 = m_v2;
 		return;
 	}

 	// w3 region
 	if (d13_1 <= 0.0f && d23_1 <= 0.0f)
 	{
 		m_v3.a = 1.0f;
 		m_count = 1;
 		m_v1 = m_v3;
 		return;
 	}

 	// e23
 	if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
 	{
 		float32 inv_d23 = 1.0f / (d23_1 + d23_2);
 		m_v2.a = d23_1 * inv_d23;
 		m_v3.a = d23_2 * inv_d23;
 		m_count = 2;
 		m_v1 = m_v3;
 		return;
 	}

 	// Must be in triangle123
 	float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
 	m_v1.a = d123_1 * inv_d123;
 	m_v2.a = d123_2 * inv_d123;
 	m_v3.a = d123_3 * inv_d123;
 	m_count = 3;
 }

 void b2Distance(b2DistanceOutput* output,
 				b2SimplexCache* cache,
 				const b2DistanceInput* input)
 {
 	++b2_gjkCalls;

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

 	b2Transform transformA = input->transformA;
 	b2Transform transformB = input->transformB;

 	// Initialize the simplex.
 	b2Simplex simplex;
 	simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);

 	// Get simplex vertices as an array.
 	b2SimplexVertex* vertices = &simplex.m_v1;
 	const int32 k_maxIters = 20;

 	// These store the vertices of the last simplex so that we
 	// can check for duplicates and prevent cycling.
 	int32 saveA[3], saveB[3];
 	int32 saveCount = 0;

 	b2Vec2 closestPoint = simplex.GetClosestPoint();
 	float32 distanceSqr1 = closestPoint.LengthSquared();
 	float32 distanceSqr2 = distanceSqr1;

 	// Main iteration loop.
 	int32 iter = 0;
 	while (iter < k_maxIters)
 	{
 		// Copy simplex so we can identify duplicates.
 		saveCount = simplex.m_count;
 		for (int32 i = 0; i < saveCount; ++i)
 		{
 			saveA[i] = vertices[i].indexA;
 			saveB[i] = vertices[i].indexB;
 		}

 		switch (simplex.m_count)
 		{
 		case 1:
 			break;

 		case 2:
 			simplex.Solve2();
 			break;

 		case 3:
 			simplex.Solve3();
 			break;

 		default:
 			b2Assert(false);
 		}

 		// If we have 3 points, then the origin is in the corresponding triangle.
 		if (simplex.m_count == 3)
 		{
 			break;
 		}

 		// Compute closest point.
 		b2Vec2 p = simplex.GetClosestPoint();
 		distanceSqr2 = p.LengthSquared();

 		// Ensure progress
 		if (distanceSqr2 >= distanceSqr1)
 		{
 			//break;
 		}
 		distanceSqr1 = distanceSqr2;

 		// Get search direction.
 		b2Vec2 d = simplex.GetSearchDirection();

 		// Ensure the search direction is numerically fit.
 		if (d.LengthSquared() < b2_epsilon * b2_epsilon)
 		{
 			// The origin is probably contained by a line segment
 			// or triangle. Thus the shapes are overlapped.

 			// We can't return zero here even though there may be overlap.
 			// In case the simplex is a point, segment, or triangle it is difficult
 			// to determine if the origin is contained in the CSO or very close to it.
 			break;
 		}

 		// Compute a tentative new simplex vertex using support points.
 		b2SimplexVertex* vertex = vertices + simplex.m_count;
 		vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
 		vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
 		b2Vec2 wBLocal;
 		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
 		vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
 		vertex->w = vertex->wB - vertex->wA;

 		// Iteration count is equated to the number of support point calls.
 		++iter;
 		++b2_gjkIters;

 		// Check for duplicate support points. This is the main termination criteria.
 		bool duplicate = false;
 		for (int32 i = 0; i < saveCount; ++i)
 		{
 			if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
 			{
 				duplicate = true;
 				break;
 			}
 		}

 		// If we found a duplicate support point we must exit to avoid cycling.
 		if (duplicate)
 		{
 			break;
 		}

 		// New vertex is ok and needed.
 		++simplex.m_count;
 	}

 	b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);

 	// Prepare output.
 	simplex.GetWitnessPoints(&output->pointA, &output->pointB);
 	output->distance = b2Distance(output->pointA, output->pointB);
 	output->iterations = iter;

 	// Cache the simplex.
 	simplex.WriteCache(cache);

 	// Apply radii if requested.
 	if (input->useRadii)
 	{
 		float32 rA = proxyA->m_radius;
 		float32 rB = proxyB->m_radius;

 		if (output->distance > rA + rB && output->distance > b2_epsilon)
 		{
 			// Shapes are still no overlapped.
 			// Move the witness points to the outer surface.
 			output->distance -= rA + rB;
 			b2Vec2 normal = output->pointB - output->pointA;
 			normal.Normalize();
 			output->pointA += rA * normal;
 			output->pointB -= rB * normal;
 		}
 		else
 		{
 			// Shapes are overlapped when radii are considered.
 			// Move the witness points to the middle.
 			b2Vec2 p = 0.5f * (output->pointA + output->pointB);
 			output->pointA = p;
 			output->pointB = p;
 			output->distance = 0.0f;
 		}
 	}
 }
	/*
	* 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/b2Distance.h>
	#include <Box2D/Collision/Shapes/b2CircleShape.h>
	#include <Box2D/Collision/Shapes/b2EdgeShape.h>
	#include <Box2D/Collision/Shapes/b2ChainShape.h>
	#include <Box2D/Collision/Shapes/b2PolygonShape.h>

	// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
	int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;

	void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
	{
	switch (shape->GetType())
	{
	case b2Shape::e_circle:
	{
	const b2CircleShape* circle = (b2CircleShape*)shape;
	m_vertices = &circle->m_p;
	m_count = 1;
	m_radius = circle->m_radius;
	}
	break;

	case b2Shape::e_polygon:
	{
	const b2PolygonShape* polygon = (b2PolygonShape*)shape;
	m_vertices = polygon->m_vertices;
	m_count = polygon->m_vertexCount;
	m_radius = polygon->m_radius;
	}
	break;

	case b2Shape::e_chain:
	{
	const b2ChainShape* chain = (b2ChainShape*)shape;
	b2Assert(0 <= index && index < chain->m_count);

	m_buffer[0] = chain->m_vertices[index];
	if (index + 1 < chain->m_count)
	{
	m_buffer[1] = chain->m_vertices[index + 1];
	}
	else
	{
	m_buffer[1] = chain->m_vertices[0];
	}

	m_vertices = m_buffer;
	m_count = 2;
	m_radius = chain->m_radius;
	}
	break;

	case b2Shape::e_edge:
	{
	const b2EdgeShape* edge = (b2EdgeShape*)shape;
	m_vertices = &edge->m_vertex1;
	m_count = 2;
	m_radius = edge->m_radius;
	}
	break;

	default:
	b2Assert(false);
	}
	}


	struct b2SimplexVertex
	{
	b2Vec2 wA; // support point in proxyA
	b2Vec2 wB; // support point in proxyB
	b2Vec2 w; // wB - wA
	float32 a; // barycentric coordinate for closest point
	int32 indexA; // wA index
	int32 indexB; // wB index
	};

	struct b2Simplex
	{
	void ReadCache( const b2SimplexCache* cache,
	const b2DistanceProxy* proxyA, const b2Transform& transformA,
	const b2DistanceProxy* proxyB, const b2Transform& transformB)
	{
	b2Assert(cache->count <= 3);

	// Copy data from cache.
	m_count = cache->count;
	b2SimplexVertex* vertices = &m_v1;
	for (int32 i = 0; i < m_count; ++i)
	{
	b2SimplexVertex* v = vertices + i;
	v->indexA = cache->indexA[i];
	v->indexB = cache->indexB[i];
	b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
	b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
	v->wA = b2Mul(transformA, wALocal);
	v->wB = b2Mul(transformB, wBLocal);
	v->w = v->wB - v->wA;
	v->a = 0.0f;
	}

	// Compute the new simplex metric, if it is substantially different than
	// old metric then flush the simplex.
	if (m_count > 1)
	{
	float32 metric1 = cache->metric;
	float32 metric2 = GetMetric();
	if (metric2 < 0.5f * metric1 \|\| 2.0f * metric1 < metric2 \|\| metric2 < b2_epsilon)
	{
	// Reset the simplex.
	m_count = 0;
	}
	}

	// If the cache is empty or invalid ...
	if (m_count == 0)
	{
	b2SimplexVertex* v = vertices + 0;
	v->indexA = 0;
	v->indexB = 0;
	b2Vec2 wALocal = proxyA->GetVertex(0);
	b2Vec2 wBLocal = proxyB->GetVertex(0);
	v->wA = b2Mul(transformA, wALocal);
	v->wB = b2Mul(transformB, wBLocal);
	v->w = v->wB - v->wA;
	m_count = 1;
	}
	}

	void WriteCache(b2SimplexCache* cache) const
	{
	cache->metric = GetMetric();
	cache->count = uint16(m_count);
	const b2SimplexVertex* vertices = &m_v1;
	for (int32 i = 0; i < m_count; ++i)
	{
	cache->indexA[i] = uint8(vertices[i].indexA);
	cache->indexB[i] = uint8(vertices[i].indexB);
	}
	}

	b2Vec2 GetSearchDirection() const
	{
	switch (m_count)
	{
	case 1:
	return -m_v1.w;

	case 2:
	{
	b2Vec2 e12 = m_v2.w - m_v1.w;
	float32 sgn = b2Cross(e12, -m_v1.w);
	if (sgn > 0.0f)
	{
	// Origin is left of e12.
	return b2Cross(1.0f, e12);
	}
	else
	{
	// Origin is right of e12.
	return b2Cross(e12, 1.0f);
	}
	}

	default:
	b2Assert(false);
	return b2Vec2_zero;
	}
	}

	b2Vec2 GetClosestPoint() const
	{
	switch (m_count)
	{
	case 0:
	b2Assert(false);
	return b2Vec2_zero;

	case 1:
	return m_v1.w;

	case 2:
	return m_v1.a * m_v1.w + m_v2.a * m_v2.w;

	case 3:
	return b2Vec2_zero;

	default:
	b2Assert(false);
	return b2Vec2_zero;
	}
	}

	void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
	{
	switch (m_count)
	{
	case 0:
	b2Assert(false);
	break;

	case 1:
	*pA = m_v1.wA;
	*pB = m_v1.wB;
	break;

	case 2:
	pA = m_v1.a m_v1.wA + m_v2.a * m_v2.wA;
	pB = m_v1.a m_v1.wB + m_v2.a * m_v2.wB;
	break;

	case 3:
	pA = m_v1.a m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
	pB = pA;
	break;

	default:
	b2Assert(false);
	break;
	}
	}

	float32 GetMetric() const
	{
	switch (m_count)
	{
	case 0:
	b2Assert(false);
	return 0.0;

	case 1:
	return 0.0f;

	case 2:
	return b2Distance(m_v1.w, m_v2.w);

	case 3:
	return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);

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

	void Solve2();
	void Solve3();

	b2SimplexVertex m_v1, m_v2, m_v3;
	int32 m_count;
	};


	// Solve a line segment using barycentric coordinates.
	//
	// p = a1 * w1 + a2 * w2
	// a1 + a2 = 1
	//
	// The vector from the origin to the closest point on the line is
	// perpendicular to the line.
	// e12 = w2 - w1
	// dot(p, e) = 0
	// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
	//
	// 2-by-2 linear system
	// [1 1 ][a1] = [1]
	// [w1.e12 w2.e12][a2] = [0]
	//
	// Define
	// d12_1 = dot(w2, e12)
	// d12_2 = -dot(w1, e12)
	// d12 = d12_1 + d12_2
	//
	// Solution
	// a1 = d12_1 / d12
	// a2 = d12_2 / d12
	void b2Simplex::Solve2()
	{
	b2Vec2 w1 = m_v1.w;
	b2Vec2 w2 = m_v2.w;
	b2Vec2 e12 = w2 - w1;

	// w1 region
	float32 d12_2 = -b2Dot(w1, e12);
	if (d12_2 <= 0.0f)
	{
	// a2 <= 0, so we clamp it to 0
	m_v1.a = 1.0f;
	m_count = 1;
	return;
	}

	// w2 region
	float32 d12_1 = b2Dot(w2, e12);
	if (d12_1 <= 0.0f)
	{
	// a1 <= 0, so we clamp it to 0
	m_v2.a = 1.0f;
	m_count = 1;
	m_v1 = m_v2;
	return;
	}

	// Must be in e12 region.
	float32 inv_d12 = 1.0f / (d12_1 + d12_2);
	m_v1.a = d12_1 * inv_d12;
	m_v2.a = d12_2 * inv_d12;
	m_count = 2;
	}

	// Possible regions:
	// - points[2]
	// - edge points[0]-points[2]
	// - edge points[1]-points[2]
	// - inside the triangle
	void b2Simplex::Solve3()
	{
	b2Vec2 w1 = m_v1.w;
	b2Vec2 w2 = m_v2.w;
	b2Vec2 w3 = m_v3.w;

	// Edge12
	// [1 1 ][a1] = [1]
	// [w1.e12 w2.e12][a2] = [0]
	// a3 = 0
	b2Vec2 e12 = w2 - w1;
	float32 w1e12 = b2Dot(w1, e12);
	float32 w2e12 = b2Dot(w2, e12);
	float32 d12_1 = w2e12;
	float32 d12_2 = -w1e12;

	// Edge13
	// [1 1 ][a1] = [1]
	// [w1.e13 w3.e13][a3] = [0]
	// a2 = 0
	b2Vec2 e13 = w3 - w1;
	float32 w1e13 = b2Dot(w1, e13);
	float32 w3e13 = b2Dot(w3, e13);
	float32 d13_1 = w3e13;
	float32 d13_2 = -w1e13;

	// Edge23
	// [1 1 ][a2] = [1]
	// [w2.e23 w3.e23][a3] = [0]
	// a1 = 0
	b2Vec2 e23 = w3 - w2;
	float32 w2e23 = b2Dot(w2, e23);
	float32 w3e23 = b2Dot(w3, e23);
	float32 d23_1 = w3e23;
	float32 d23_2 = -w2e23;

	// Triangle123
	float32 n123 = b2Cross(e12, e13);

	float32 d123_1 = n123 * b2Cross(w2, w3);
	float32 d123_2 = n123 * b2Cross(w3, w1);
	float32 d123_3 = n123 * b2Cross(w1, w2);

	// w1 region
	if (d12_2 <= 0.0f && d13_2 <= 0.0f)
	{
	m_v1.a = 1.0f;
	m_count = 1;
	return;
	}

	// e12
	if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
	{
	float32 inv_d12 = 1.0f / (d12_1 + d12_2);
	m_v1.a = d12_1 * inv_d12;
	m_v2.a = d12_2 * inv_d12;
	m_count = 2;
	return;
	}

	// e13
	if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
	{
	float32 inv_d13 = 1.0f / (d13_1 + d13_2);
	m_v1.a = d13_1 * inv_d13;
	m_v3.a = d13_2 * inv_d13;
	m_count = 2;
	m_v2 = m_v3;
	return;
	}

	// w2 region
	if (d12_1 <= 0.0f && d23_2 <= 0.0f)
	{
	m_v2.a = 1.0f;
	m_count = 1;
	m_v1 = m_v2;
	return;
	}

	// w3 region
	if (d13_1 <= 0.0f && d23_1 <= 0.0f)
	{
	m_v3.a = 1.0f;
	m_count = 1;
	m_v1 = m_v3;
	return;
	}

	// e23
	if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
	{
	float32 inv_d23 = 1.0f / (d23_1 + d23_2);
	m_v2.a = d23_1 * inv_d23;
	m_v3.a = d23_2 * inv_d23;
	m_count = 2;
	m_v1 = m_v3;
	return;
	}

	// Must be in triangle123
	float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
	m_v1.a = d123_1 * inv_d123;
	m_v2.a = d123_2 * inv_d123;
	m_v3.a = d123_3 * inv_d123;
	m_count = 3;
	}

	void b2Distance(b2DistanceOutput* output,
	b2SimplexCache* cache,
	const b2DistanceInput* input)
	{
	++b2_gjkCalls;

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

	b2Transform transformA = input->transformA;
	b2Transform transformB = input->transformB;

	// Initialize the simplex.
	b2Simplex simplex;
	simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);

	// Get simplex vertices as an array.
	b2SimplexVertex* vertices = &simplex.m_v1;
	const int32 k_maxIters = 20;

	// These store the vertices of the last simplex so that we
	// can check for duplicates and prevent cycling.
	int32 saveA[3], saveB[3];
	int32 saveCount = 0;

	b2Vec2 closestPoint = simplex.GetClosestPoint();
	float32 distanceSqr1 = closestPoint.LengthSquared();
	float32 distanceSqr2 = distanceSqr1;

	// Main iteration loop.
	int32 iter = 0;
	while (iter < k_maxIters)
	{
	// Copy simplex so we can identify duplicates.
	saveCount = simplex.m_count;
	for (int32 i = 0; i < saveCount; ++i)
	{
	saveA[i] = vertices[i].indexA;
	saveB[i] = vertices[i].indexB;
	}

	switch (simplex.m_count)
	{
	case 1:
	break;

	case 2:
	simplex.Solve2();
	break;

	case 3:
	simplex.Solve3();
	break;

	default:
	b2Assert(false);
	}

	// If we have 3 points, then the origin is in the corresponding triangle.
	if (simplex.m_count == 3)
	{
	break;
	}

	// Compute closest point.
	b2Vec2 p = simplex.GetClosestPoint();
	distanceSqr2 = p.LengthSquared();

	// Ensure progress
	if (distanceSqr2 >= distanceSqr1)
	{
	//break;
	}
	distanceSqr1 = distanceSqr2;

	// Get search direction.
	b2Vec2 d = simplex.GetSearchDirection();

	// Ensure the search direction is numerically fit.
	if (d.LengthSquared() < b2_epsilon * b2_epsilon)
	{
	// The origin is probably contained by a line segment
	// or triangle. Thus the shapes are overlapped.

	// We can't return zero here even though there may be overlap.
	// In case the simplex is a point, segment, or triangle it is difficult
	// to determine if the origin is contained in the CSO or very close to it.
	break;
	}

	// Compute a tentative new simplex vertex using support points.
	b2SimplexVertex* vertex = vertices + simplex.m_count;
	vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
	vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
	b2Vec2 wBLocal;
	vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
	vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
	vertex->w = vertex->wB - vertex->wA;

	// Iteration count is equated to the number of support point calls.
	++iter;
	++b2_gjkIters;

	// Check for duplicate support points. This is the main termination criteria.
	bool duplicate = false;
	for (int32 i = 0; i < saveCount; ++i)
	{
	if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
	{
	duplicate = true;
	break;
	}
	}

	// If we found a duplicate support point we must exit to avoid cycling.
	if (duplicate)
	{
	break;
	}

	// New vertex is ok and needed.
	++simplex.m_count;
	}

	b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);

	// Prepare output.
	simplex.GetWitnessPoints(&output->pointA, &output->pointB);
	output->distance = b2Distance(output->pointA, output->pointB);
	output->iterations = iter;

	// Cache the simplex.
	simplex.WriteCache(cache);

	// Apply radii if requested.
	if (input->useRadii)
	{
	float32 rA = proxyA->m_radius;
	float32 rB = proxyB->m_radius;

	if (output->distance > rA + rB && output->distance > b2_epsilon)
	{
	// Shapes are still no overlapped.
	// Move the witness points to the outer surface.
	output->distance -= rA + rB;
	b2Vec2 normal = output->pointB - output->pointA;
	normal.Normalize();
	output->pointA += rA * normal;
	output->pointB -= rB * normal;
	}
	else
	{
	// Shapes are overlapped when radii are considered.
	// Move the witness points to the middle.
	b2Vec2 p = 0.5f * (output->pointA + output->pointB);
	output->pointA = p;
	output->pointB = p;
	output->distance = 0.0f;
	}
	}
	}