/addon/ofxBox2d/src/Box2D/Collision/b2Distance.cpp

/*

* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com

*

* 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 "b2Distance.h"

#include "b2CircleShape.h"

#include "b2PolygonShape.h"



// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.

int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;



void b2DistanceProxy::Set(const b2Shape* shape)

{

	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;



	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.R, -d));

		vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));

		b2Vec2 wBLocal;

		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.R, 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;

		}

	}

}