initial commit. includes PhsyicsBox2dExtension

AndEngine/jni/Box2D/Collision/Shapes/b2CircleShape.cpp

@@ -0,0 +1,89 @@
+/*
+* Copyright (c) 2006-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 "Box2D/Collision/Shapes/b2CircleShape.h"
+#include <new>
+
+b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2CircleShape));
+	b2CircleShape* clone = new (mem) b2CircleShape;
+	*clone = *this;
+	return clone;
+}
+
+bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
+{
+	b2Vec2 center = transform.position + b2Mul(transform.R, m_p);
+	b2Vec2 d = p - center;
+	return b2Dot(d, d) <= m_radius * m_radius;
+}
+
+// Collision Detection in Interactive 3D Environments by Gino van den Bergen
+// From Section 3.1.2
+// x = s + a * r
+// norm(x) = radius
+bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const
+{
+	b2Vec2 position = transform.position + b2Mul(transform.R, m_p);
+	b2Vec2 s = input.p1 - position;
+	float32 b = b2Dot(s, s) - m_radius * m_radius;
+
+	// Solve quadratic equation.
+	b2Vec2 r = input.p2 - input.p1;
+	float32 c =  b2Dot(s, r);
+	float32 rr = b2Dot(r, r);
+	float32 sigma = c * c - rr * b;
+
+	// Check for negative discriminant and short segment.
+	if (sigma < 0.0f || rr < b2_epsilon)
+	{
+		return false;
+	}
+
+	// Find the point of intersection of the line with the circle.
+	float32 a = -(c + b2Sqrt(sigma));
+
+	// Is the intersection point on the segment?
+	if (0.0f <= a && a <= input.maxFraction * rr)
+	{
+		a /= rr;
+		output->fraction = a;
+		output->normal = s + a * r;
+		output->normal.Normalize();
+		return true;
+	}
+
+	return false;
+}
+
+void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform) const
+{
+	b2Vec2 p = transform.position + b2Mul(transform.R, m_p);
+	aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
+	aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
+}
+
+void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+	massData->mass = density * b2_pi * m_radius * m_radius;
+	massData->center = m_p;
+
+	// inertia about the local origin
+	massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p));
+}

AndEngine/jni/Box2D/Collision/Shapes/b2CircleShape.h

@@ -0,0 +1,87 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#ifndef B2_CIRCLE_SHAPE_H
+#define B2_CIRCLE_SHAPE_H
+
+#include "Box2D/Collision/Shapes/b2Shape.h"
+
+/// A circle shape.
+class b2CircleShape : public b2Shape
+{
+public:
+	b2CircleShape();
+
+	/// Implement b2Shape.
+	b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+	/// Implement b2Shape.
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;
+
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const;
+
+	/// Get the supporting vertex index in the given direction.
+	int32 GetSupport(const b2Vec2& d) const;
+
+	/// Get the supporting vertex in the given direction.
+	const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
+
+	/// Get the vertex count.
+	int32 GetVertexCount() const { return 1; }
+
+	/// Get a vertex by index. Used by b2Distance.
+	const b2Vec2& GetVertex(int32 index) const;
+
+	/// Position
+	b2Vec2 m_p;
+};
+
+inline b2CircleShape::b2CircleShape()
+{
+	m_type = e_circle;
+	m_radius = 0.0f;
+	m_p.SetZero();
+}
+
+inline int32 b2CircleShape::GetSupport(const b2Vec2 &d) const
+{
+	B2_NOT_USED(d);
+	return 0;
+}
+
+inline const b2Vec2& b2CircleShape::GetSupportVertex(const b2Vec2 &d) const
+{
+	B2_NOT_USED(d);
+	return m_p;
+}
+
+inline const b2Vec2& b2CircleShape::GetVertex(int32 index) const
+{
+	B2_NOT_USED(index);
+	b2Assert(index == 0);
+	return m_p;
+}
+
+#endif

AndEngine/jni/Box2D/Collision/Shapes/b2PolygonShape.cpp

@@ -0,0 +1,434 @@
+/*
+* Copyright (c) 2006-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 "Box2D/Collision/Shapes/b2PolygonShape.h"
+#include <new>
+
+b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2PolygonShape));
+	b2PolygonShape* clone = new (mem) b2PolygonShape;
+	*clone = *this;
+	return clone;
+}
+
+void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
+{
+	m_vertexCount = 4;
+	m_vertices[0].Set(-hx, -hy);
+	m_vertices[1].Set( hx, -hy);
+	m_vertices[2].Set( hx,  hy);
+	m_vertices[3].Set(-hx,  hy);
+	m_normals[0].Set(0.0f, -1.0f);
+	m_normals[1].Set(1.0f, 0.0f);
+	m_normals[2].Set(0.0f, 1.0f);
+	m_normals[3].Set(-1.0f, 0.0f);
+	m_centroid.SetZero();
+}
+
+void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
+{
+	m_vertexCount = 4;
+	m_vertices[0].Set(-hx, -hy);
+	m_vertices[1].Set( hx, -hy);
+	m_vertices[2].Set( hx,  hy);
+	m_vertices[3].Set(-hx,  hy);
+	m_normals[0].Set(0.0f, -1.0f);
+	m_normals[1].Set(1.0f, 0.0f);
+	m_normals[2].Set(0.0f, 1.0f);
+	m_normals[3].Set(-1.0f, 0.0f);
+	m_centroid = center;
+
+	b2Transform xf;
+	xf.position = center;
+	xf.R.Set(angle);
+
+	// Transform vertices and normals.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		m_vertices[i] = b2Mul(xf, m_vertices[i]);
+		m_normals[i] = b2Mul(xf.R, m_normals[i]);
+	}
+}
+
+void b2PolygonShape::SetAsEdge(const b2Vec2& v1, const b2Vec2& v2)
+{
+	m_vertexCount = 2;
+	m_vertices[0] = v1;
+	m_vertices[1] = v2;
+	m_centroid = 0.5f * (v1 + v2);
+	m_normals[0] = b2Cross(v2 - v1, 1.0f);
+	m_normals[0].Normalize();
+	m_normals[1] = -m_normals[0];
+}
+
+static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
+{
+	b2Assert(count >= 2);
+
+	b2Vec2 c; c.Set(0.0f, 0.0f);
+	float32 area = 0.0f;
+
+	if (count == 2)
+	{
+		c = 0.5f * (vs[0] + vs[1]);
+		return c;
+	}
+
+	// pRef is the reference point for forming triangles.
+	// It's location doesn't change the result (except for rounding error).
+	b2Vec2 pRef(0.0f, 0.0f);
+#if 0
+	// This code would put the reference point inside the polygon.
+	for (int32 i = 0; i < count; ++i)
+	{
+		pRef += vs[i];
+	}
+	pRef *= 1.0f / count;
+#endif
+
+	const float32 inv3 = 1.0f / 3.0f;
+
+	for (int32 i = 0; i < count; ++i)
+	{
+		// Triangle vertices.
+		b2Vec2 p1 = pRef;
+		b2Vec2 p2 = vs[i];
+		b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
+
+		b2Vec2 e1 = p2 - p1;
+		b2Vec2 e2 = p3 - p1;
+
+		float32 D = b2Cross(e1, e2);
+
+		float32 triangleArea = 0.5f * D;
+		area += triangleArea;
+
+		// Area weighted centroid
+		c += triangleArea * inv3 * (p1 + p2 + p3);
+	}
+
+	// Centroid
+	b2Assert(area > b2_epsilon);
+	c *= 1.0f / area;
+	return c;
+}
+
+void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
+{
+	b2Assert(2 <= count && count <= b2_maxPolygonVertices);
+	m_vertexCount = count;
+
+	// Copy vertices.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		m_vertices[i] = vertices[i];
+	}
+
+	// Compute normals. Ensure the edges have non-zero length.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		int32 i1 = i;
+		int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
+		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
+		b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
+		m_normals[i] = b2Cross(edge, 1.0f);
+		m_normals[i].Normalize();
+	}
+
+#ifdef _DEBUG
+	// Ensure the polygon is convex and the interior
+	// is to the left of each edge.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		int32 i1 = i;
+		int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
+		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
+
+		for (int32 j = 0; j < m_vertexCount; ++j)
+		{
+			// Don't check vertices on the current edge.
+			if (j == i1 || j == i2)
+			{
+				continue;
+			}
+			
+			b2Vec2 r = m_vertices[j] - m_vertices[i1];
+
+			// Your polygon is non-convex (it has an indentation) or
+			// has colinear edges.
+			float32 s = b2Cross(edge, r);
+			b2Assert(s > 0.0f);
+		}
+	}
+#endif
+
+	// Compute the polygon centroid.
+	m_centroid = ComputeCentroid(m_vertices, m_vertexCount);
+}
+
+bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+	b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
+
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
+		if (dot > 0.0f)
+		{
+			return false;
+		}
+	}
+
+	return true;
+}
+
+bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf) const
+{
+	// Put the ray into the polygon's frame of reference.
+	b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);
+	b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
+	b2Vec2 d = p2 - p1;
+
+	if (m_vertexCount == 2)
+	{
+		b2Vec2 v1 = m_vertices[0];
+		b2Vec2 v2 = m_vertices[1];
+		b2Vec2 normal = m_normals[0];
+
+		// q = p1 + t * d
+		// dot(normal, q - v1) = 0
+		// dot(normal, p1 - v1) + t * dot(normal, d) = 0
+		float32 numerator = b2Dot(normal, v1 - p1);
+		float32 denominator = b2Dot(normal, d);
+
+		if (denominator == 0.0f)
+		{
+			return false;
+		}
+	
+		float32 t = numerator / denominator;
+		if (t < 0.0f || 1.0f < t)
+		{
+			return false;
+		}
+
+		b2Vec2 q = p1 + t * d;
+
+		// q = v1 + s * r
+		// s = dot(q - v1, r) / dot(r, r)
+		b2Vec2 r = v2 - v1;
+		float32 rr = b2Dot(r, r);
+		if (rr == 0.0f)
+		{
+			return false;
+		}
+
+		float32 s = b2Dot(q - v1, r) / rr;
+		if (s < 0.0f || 1.0f < s)
+		{
+			return false;
+		}
+
+		output->fraction = t;
+		if (numerator > 0.0f)
+		{
+			output->normal = -normal;
+		}
+		else
+		{
+			output->normal = normal;
+		}
+		return true;
+	}
+	else
+	{
+		float32 lower = 0.0f, upper = input.maxFraction;
+
+		int32 index = -1;
+
+		for (int32 i = 0; i < m_vertexCount; ++i)
+		{
+			// p = p1 + a * d
+			// dot(normal, p - v) = 0
+			// dot(normal, p1 - v) + a * dot(normal, d) = 0
+			float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
+			float32 denominator = b2Dot(m_normals[i], d);
+
+			if (denominator == 0.0f)
+			{	
+				if (numerator < 0.0f)
+				{
+					return false;
+				}
+			}
+			else
+			{
+				// Note: we want this predicate without division:
+				// lower < numerator / denominator, where denominator < 0
+				// Since denominator < 0, we have to flip the inequality:
+				// lower < numerator / denominator <==> denominator * lower > numerator.
+				if (denominator < 0.0f && numerator < lower * denominator)
+				{
+					// Increase lower.
+					// The segment enters this half-space.
+					lower = numerator / denominator;
+					index = i;
+				}
+				else if (denominator > 0.0f && numerator < upper * denominator)
+				{
+					// Decrease upper.
+					// The segment exits this half-space.
+					upper = numerator / denominator;
+				}
+			}
+
+			// The use of epsilon here causes the assert on lower to trip
+			// in some cases. Apparently the use of epsilon was to make edge
+			// shapes work, but now those are handled separately.
+			//if (upper < lower - b2_epsilon)
+			if (upper < lower)
+			{
+				return false;
+			}
+		}
+
+		b2Assert(0.0f <= lower && lower <= input.maxFraction);
+
+		if (index >= 0)
+		{
+			output->fraction = lower;
+			output->normal = b2Mul(xf.R, m_normals[index]);
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf) const
+{
+	b2Vec2 lower = b2Mul(xf, m_vertices[0]);
+	b2Vec2 upper = lower;
+
+	for (int32 i = 1; i < m_vertexCount; ++i)
+	{
+		b2Vec2 v = b2Mul(xf, m_vertices[i]);
+		lower = b2Min(lower, v);
+		upper = b2Max(upper, v);
+	}
+
+	b2Vec2 r(m_radius, m_radius);
+	aabb->lowerBound = lower - r;
+	aabb->upperBound = upper + r;
+}
+
+void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+	// Polygon mass, centroid, and inertia.
+	// Let rho be the polygon density in mass per unit area.
+	// Then:
+	// mass = rho * int(dA)
+	// centroid.x = (1/mass) * rho * int(x * dA)
+	// centroid.y = (1/mass) * rho * int(y * dA)
+	// I = rho * int((x*x + y*y) * dA)
+	//
+	// We can compute these integrals by summing all the integrals
+	// for each triangle of the polygon. To evaluate the integral
+	// for a single triangle, we make a change of variables to
+	// the (u,v) coordinates of the triangle:
+	// x = x0 + e1x * u + e2x * v
+	// y = y0 + e1y * u + e2y * v
+	// where 0 <= u && 0 <= v && u + v <= 1.
+	//
+	// We integrate u from [0,1-v] and then v from [0,1].
+	// We also need to use the Jacobian of the transformation:
+	// D = cross(e1, e2)
+	//
+	// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
+	//
+	// The rest of the derivation is handled by computer algebra.
+
+	b2Assert(m_vertexCount >= 2);
+
+	// A line segment has zero mass.
+	if (m_vertexCount == 2)
+	{
+		massData->center = 0.5f * (m_vertices[0] + m_vertices[1]);
+		massData->mass = 0.0f;
+		massData->I = 0.0f;
+		return;
+	}
+
+	b2Vec2 center; center.Set(0.0f, 0.0f);
+	float32 area = 0.0f;
+	float32 I = 0.0f;
+
+	// pRef is the reference point for forming triangles.
+	// It's location doesn't change the result (except for rounding error).
+	b2Vec2 pRef(0.0f, 0.0f);
+#if 0
+	// This code would put the reference point inside the polygon.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		pRef += m_vertices[i];
+	}
+	pRef *= 1.0f / count;
+#endif
+
+	const float32 k_inv3 = 1.0f / 3.0f;
+
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		// Triangle vertices.
+		b2Vec2 p1 = pRef;
+		b2Vec2 p2 = m_vertices[i];
+		b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
+
+		b2Vec2 e1 = p2 - p1;
+		b2Vec2 e2 = p3 - p1;
+
+		float32 D = b2Cross(e1, e2);
+
+		float32 triangleArea = 0.5f * D;
+		area += triangleArea;
+
+		// Area weighted centroid
+		center += triangleArea * k_inv3 * (p1 + p2 + p3);
+
+		float32 px = p1.x, py = p1.y;
+		float32 ex1 = e1.x, ey1 = e1.y;
+		float32 ex2 = e2.x, ey2 = e2.y;
+
+		float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
+		float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
+
+		I += D * (intx2 + inty2);
+	}
+
+	// Total mass
+	massData->mass = density * area;
+
+	// Center of mass
+	b2Assert(area > b2_epsilon);
+	center *= 1.0f / area;
+	massData->center = center;
+
+	// Inertia tensor relative to the local origin.
+	massData->I = density * I;
+}

AndEngine/jni/Box2D/Collision/Shapes/b2PolygonShape.h

@@ -0,0 +1,131 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#ifndef B2_POLYGON_SHAPE_H
+#define B2_POLYGON_SHAPE_H
+
+#include "Box2D/Collision/Shapes/b2Shape.h"
+
+/// A convex polygon. It is assumed that the interior of the polygon is to
+/// the left of each edge.
+class b2PolygonShape : public b2Shape
+{
+public:
+	b2PolygonShape();
+
+	/// Implement b2Shape.
+	b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+	/// Copy vertices. This assumes the vertices define a convex polygon.
+	/// It is assumed that the exterior is the the right of each edge.
+	void Set(const b2Vec2* vertices, int32 vertexCount);
+
+	/// Build vertices to represent an axis-aligned box.
+	/// @param hx the half-width.
+	/// @param hy the half-height.
+	void SetAsBox(float32 hx, float32 hy);
+
+	/// Build vertices to represent an oriented box.
+	/// @param hx the half-width.
+	/// @param hy the half-height.
+	/// @param center the center of the box in local coordinates.
+	/// @param angle the rotation of the box in local coordinates.
+	void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);
+
+	/// Set this as a single edge.
+	void SetAsEdge(const b2Vec2& v1, const b2Vec2& v2);
+
+	/// @see b2Shape::TestPoint
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;
+
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const;
+
+	/// Get the supporting vertex index in the given direction.
+	int32 GetSupport(const b2Vec2& d) const;
+
+	/// Get the supporting vertex in the given direction.
+	const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
+
+	/// Get the vertex count.
+	int32 GetVertexCount() const { return m_vertexCount; }
+
+	/// Get a vertex by index.
+	const b2Vec2& GetVertex(int32 index) const;
+
+	b2Vec2 m_centroid;
+	b2Vec2 m_vertices[b2_maxPolygonVertices];
+	b2Vec2 m_normals[b2_maxPolygonVertices];
+	int32 m_vertexCount;
+};
+
+inline b2PolygonShape::b2PolygonShape()
+{
+	m_type = e_polygon;
+	m_radius = b2_polygonRadius;
+	m_vertexCount = 0;
+	m_centroid.SetZero();
+}
+
+inline int32 b2PolygonShape::GetSupport(const b2Vec2& d) const
+{
+	int32 bestIndex = 0;
+	float32 bestValue = b2Dot(m_vertices[0], d);
+	for (int32 i = 1; i < m_vertexCount; ++i)
+	{
+		float32 value = b2Dot(m_vertices[i], d);
+		if (value > bestValue)
+		{
+			bestIndex = i;
+			bestValue = value;
+		}
+	}
+
+	return bestIndex;
+}
+
+inline const b2Vec2& b2PolygonShape::GetSupportVertex(const b2Vec2& d) const
+{
+	int32 bestIndex = 0;
+	float32 bestValue = b2Dot(m_vertices[0], d);
+	for (int32 i = 1; i < m_vertexCount; ++i)
+	{
+		float32 value = b2Dot(m_vertices[i], d);
+		if (value > bestValue)
+		{
+			bestIndex = i;
+			bestValue = value;
+		}
+	}
+
+	return m_vertices[bestIndex];
+}
+
+inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const
+{
+	b2Assert(0 <= index && index < m_vertexCount);
+	return m_vertices[index];
+}
+
+#endif

AndEngine/jni/Box2D/Collision/Shapes/b2Shape.h

@@ -0,0 +1,95 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#ifndef B2_SHAPE_H
+#define B2_SHAPE_H
+
+#include "Box2D/Common/b2BlockAllocator.h"
+#include "Box2D/Common/b2Math.h"
+#include "Box2D/Collision/b2Collision.h"
+
+/// This holds the mass data computed for a shape.
+struct b2MassData
+{
+	/// The mass of the shape, usually in kilograms.
+	float32 mass;
+
+	/// The position of the shape's centroid relative to the shape's origin.
+	b2Vec2 center;
+
+	/// The rotational inertia of the shape about the local origin.
+	float32 I;
+};
+
+/// A shape is used for collision detection. You can create a shape however you like.
+/// Shapes used for simulation in b2World are created automatically when a b2Fixture
+/// is created.
+class b2Shape
+{
+public:
+	
+	enum Type
+	{
+		e_unknown= -1,
+		e_circle = 0,
+		e_polygon = 1,
+		e_typeCount = 2,
+	};
+
+	b2Shape() { m_type = e_unknown; }
+	virtual ~b2Shape() {}
+
+	/// Clone the concrete shape using the provided allocator.
+	virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0;
+
+	/// Get the type of this shape. You can use this to down cast to the concrete shape.
+	/// @return the shape type.
+	Type GetType() const;
+
+	/// Test a point for containment in this shape. This only works for convex shapes.
+	/// @param xf the shape world transform.
+	/// @param p a point in world coordinates.
+	virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;
+
+	/// Cast a ray against this shape.
+	/// @param output the ray-cast results.
+	/// @param input the ray-cast input parameters.
+	/// @param transform the transform to be applied to the shape.
+	virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const = 0;
+
+	/// Given a transform, compute the associated axis aligned bounding box for this shape.
+	/// @param aabb returns the axis aligned box.
+	/// @param xf the world transform of the shape.
+	virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf) const = 0;
+
+	/// Compute the mass properties of this shape using its dimensions and density.
+	/// The inertia tensor is computed about the local origin.
+	/// @param massData returns the mass data for this shape.
+	/// @param density the density in kilograms per meter squared.
+	virtual void ComputeMass(b2MassData* massData, float32 density) const = 0;
+
+	Type m_type;
+	float32 m_radius;
+};
+
+inline b2Shape::Type b2Shape::GetType() const
+{
+	return m_type;
+}
+
+#endif

AndEngine/jni/Box2D/Collision/b2BroadPhase.cpp

@@ -0,0 +1,116 @@
+/*
+* Copyright (c) 2006-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 "Box2D/Collision/b2BroadPhase.h"
+#include <string.h>
+
+b2BroadPhase::b2BroadPhase()
+{
+	m_proxyCount = 0;
+
+	m_pairCapacity = 16;
+	m_pairCount = 0;
+	m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
+
+	m_moveCapacity = 16;
+	m_moveCount = 0;
+	m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
+}
+
+b2BroadPhase::~b2BroadPhase()
+{
+	b2Free(m_moveBuffer);
+	b2Free(m_pairBuffer);
+}
+
+int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
+{
+	int32 proxyId = m_tree.CreateProxy(aabb, userData);
+	++m_proxyCount;
+	BufferMove(proxyId);
+	return proxyId;
+}
+
+void b2BroadPhase::DestroyProxy(int32 proxyId)
+{
+	UnBufferMove(proxyId);
+	--m_proxyCount;
+	m_tree.DestroyProxy(proxyId);
+}
+
+void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
+{
+	bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement);
+	if (buffer)
+	{
+		BufferMove(proxyId);
+	}
+}
+
+void b2BroadPhase::BufferMove(int32 proxyId)
+{
+	if (m_moveCount == m_moveCapacity)
+	{
+		int32* oldBuffer = m_moveBuffer;
+		m_moveCapacity *= 2;
+		m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
+		memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32));
+		b2Free(oldBuffer);
+	}
+
+	m_moveBuffer[m_moveCount] = proxyId;
+	++m_moveCount;
+}
+
+void b2BroadPhase::UnBufferMove(int32 proxyId)
+{
+	for (int32 i = 0; i < m_moveCount; ++i)
+	{
+		if (m_moveBuffer[i] == proxyId)
+		{
+			m_moveBuffer[i] = e_nullProxy;
+			return;
+		}
+	}
+}
+
+// This is called from b2DynamicTree::Query when we are gathering pairs.
+bool b2BroadPhase::QueryCallback(int32 proxyId)
+{
+	// A proxy cannot form a pair with itself.
+	if (proxyId == m_queryProxyId)
+	{
+		return true;
+	}
+
+	// Grow the pair buffer as needed.
+	if (m_pairCount == m_pairCapacity)
+	{
+		b2Pair* oldBuffer = m_pairBuffer;
+		m_pairCapacity *= 2;
+		m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
+		memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
+		b2Free(oldBuffer);
+	}
+
+	m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId);
+	m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId);
+	++m_pairCount;
+
+	return true;
+}

AndEngine/jni/Box2D/Collision/b2BroadPhase.h

@@ -0,0 +1,264 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#ifndef B2_BROAD_PHASE_H
+#define B2_BROAD_PHASE_H
+
+#include "Box2D/Common/b2Settings.h"
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/b2DynamicTree.h"
+#include <stdlib.h>
+//#include <algorithm>
+
+struct b2Pair
+{
+	int32 proxyIdA;
+	int32 proxyIdB;
+	int32 next;
+};
+
+/// The broad-phase is used for computing pairs and performing volume queries and ray casts.
+/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
+/// It is up to the client to consume the new pairs and to track subsequent overlap.
+class b2BroadPhase
+{
+public:
+
+	enum
+	{
+		e_nullProxy = -1,
+	};
+
+	b2BroadPhase();
+	~b2BroadPhase();
+
+	/// Create a proxy with an initial AABB. Pairs are not reported until
+	/// UpdatePairs is called.
+	int32 CreateProxy(const b2AABB& aabb, void* userData);
+
+	/// Destroy a proxy. It is up to the client to remove any pairs.
+	void DestroyProxy(int32 proxyId);
+
+	/// Call MoveProxy as many times as you like, then when you are done
+	/// call UpdatePairs to finalized the proxy pairs (for your time step).
+	void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);
+
+	/// Get the fat AABB for a proxy.
+	const b2AABB& GetFatAABB(int32 proxyId) const;
+
+	/// Get user data from a proxy. Returns NULL if the id is invalid.
+	void* GetUserData(int32 proxyId) const;
+
+	/// Test overlap of fat AABBs.
+	bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const;
+
+	/// Get the number of proxies.
+	int32 GetProxyCount() const;
+
+	/// Update the pairs. This results in pair callbacks. This can only add pairs.
+	template <typename T>
+	void UpdatePairs(T* callback);
+
+	/// Query an AABB for overlapping proxies. The callback class
+	/// is called for each proxy that overlaps the supplied AABB.
+	template <typename T>
+	void Query(T* callback, const b2AABB& aabb) const;
+
+	/// Ray-cast against the proxies in the tree. This relies on the callback
+	/// to perform a exact ray-cast in the case were the proxy contains a shape.
+	/// The callback also performs the any collision filtering. This has performance
+	/// roughly equal to k * log(n), where k is the number of collisions and n is the
+	/// number of proxies in the tree.
+	/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+	/// @param callback a callback class that is called for each proxy that is hit by the ray.
+	template <typename T>
+	void RayCast(T* callback, const b2RayCastInput& input) const;
+
+	/// Compute the height of the embedded tree.
+	int32 ComputeHeight() const;
+
+private:
+
+	friend class b2DynamicTree;
+
+	void BufferMove(int32 proxyId);
+	void UnBufferMove(int32 proxyId);
+
+	bool QueryCallback(int32 proxyId);
+
+	b2DynamicTree m_tree;
+
+	int32 m_proxyCount;
+
+	int32* m_moveBuffer;
+	int32 m_moveCapacity;
+	int32 m_moveCount;
+
+	b2Pair* m_pairBuffer;
+	int32 m_pairCapacity;
+	int32 m_pairCount;
+
+	int32 m_queryProxyId;
+};
+
+/// This is used to sort pairs.
+inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2)
+{
+	if (pair1.proxyIdA < pair2.proxyIdA)
+	{
+		return true;
+	}
+
+	if (pair1.proxyIdA == pair2.proxyIdA)
+	{
+		return pair1.proxyIdB < pair2.proxyIdB;
+	}
+
+	return false;
+}
+
+inline void* b2BroadPhase::GetUserData(int32 proxyId) const
+{
+	return m_tree.GetUserData(proxyId);
+}
+
+inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const
+{
+	const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA);
+	const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB);
+	return b2TestOverlap(aabbA, aabbB);
+}
+
+inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const
+{
+	return m_tree.GetFatAABB(proxyId);
+}
+
+inline int32 b2BroadPhase::GetProxyCount() const
+{
+	return m_proxyCount;
+}
+
+inline int32 b2BroadPhase::ComputeHeight() const
+{
+	return m_tree.ComputeHeight();
+}
+
+//The return value of this function should represent whether elem1 is considered less than,
+//equal to, or greater than elem2 by returning, respectively, a negative value, zero or a positive value.
+inline int b2PairCompareQSort(const void * elem1, const void * elem2)
+{
+   b2Pair* pair1 = (b2Pair*) elem1;
+   b2Pair* pair2 = (b2Pair*) elem2;
+
+   if (pair1->proxyIdA < pair2->proxyIdA)
+   {
+      return -1;
+   }
+
+   if (pair1->proxyIdA == pair2->proxyIdA)
+   {
+      if( pair1->proxyIdB < pair2->proxyIdB ) {
+         return -1;
+      }
+      else if(pair1->proxyIdB > pair2->proxyIdB) {
+         return 1;
+      }
+      else {
+         return 0;
+      }
+   }
+   else {
+      return 1;
+   }
+
+}
+
+template <typename T>
+void b2BroadPhase::UpdatePairs(T* callback)
+{
+	// Reset pair buffer
+	m_pairCount = 0;
+
+	// Perform tree queries for all moving proxies.
+	for (int32 i = 0; i < m_moveCount; ++i)
+	{
+		m_queryProxyId = m_moveBuffer[i];
+		if (m_queryProxyId == e_nullProxy)
+		{
+			continue;
+		}
+
+		// We have to query the tree with the fat AABB so that
+		// we don't fail to create a pair that may touch later.
+		const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId);
+
+		// Query tree, create pairs and add them pair buffer.
+		m_tree.Query(this, fatAABB);
+	}
+
+	// Reset move buffer
+	m_moveCount = 0;
+
+	// Sort the pair buffer to expose duplicates.
+	//std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan);
+
+	// FIX from http://www.box2d.org/forum/viewtopic.php?f=7&t=4756&start=0 to get rid of stl dependency
+	qsort(m_pairBuffer, sizeof(m_pairBuffer) / sizeof(struct b2Pair) , sizeof(struct b2Pair), b2PairCompareQSort);
+
+	// Send the pairs back to the client.
+	int32 i = 0;
+	while (i < m_pairCount)
+	{
+		b2Pair* primaryPair = m_pairBuffer + i;
+		void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA);
+		void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB);
+
+		callback->AddPair(userDataA, userDataB);
+		++i;
+
+		// Skip any duplicate pairs.
+		while (i < m_pairCount)
+		{
+			b2Pair* pair = m_pairBuffer + i;
+			if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB)
+			{
+				break;
+			}
+			++i;
+		}
+	}
+
+	// Try to keep the tree balanced.
+	m_tree.Rebalance(4);
+}
+
+
+template <typename T>
+inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const
+{
+	m_tree.Query(callback, aabb);
+}
+
+template <typename T>
+inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const
+{
+	m_tree.RayCast(callback, input);
+}
+
+#endif

AndEngine/jni/Box2D/Collision/b2CollideCircle.cpp

@@ -0,0 +1,154 @@
+/*
+* 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 "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+void b2CollideCircles(
+	b2Manifold* manifold,
+	const b2CircleShape* circleA, const b2Transform& xfA,
+	const b2CircleShape* circleB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+
+	b2Vec2 pA = b2Mul(xfA, circleA->m_p);
+	b2Vec2 pB = b2Mul(xfB, circleB->m_p);
+
+	b2Vec2 d = pB - pA;
+	float32 distSqr = b2Dot(d, d);
+	float32 rA = circleA->m_radius, rB = circleB->m_radius;
+	float32 radius = rA + rB;
+	if (distSqr > radius * radius)
+	{
+		return;
+	}
+
+	manifold->type = b2Manifold::e_circles;
+	manifold->localPoint = circleA->m_p;
+	manifold->localNormal.SetZero();
+	manifold->pointCount = 1;
+
+	manifold->points[0].localPoint = circleB->m_p;
+	manifold->points[0].id.key = 0;
+}
+
+void b2CollidePolygonAndCircle(
+	b2Manifold* manifold,
+	const b2PolygonShape* polygonA, const b2Transform& xfA,
+	const b2CircleShape* circleB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+
+	// Compute circle position in the frame of the polygon.
+	b2Vec2 c = b2Mul(xfB, circleB->m_p);
+	b2Vec2 cLocal = b2MulT(xfA, c);
+
+	// Find the min separating edge.
+	int32 normalIndex = 0;
+	float32 separation = -b2_maxFloat;
+	float32 radius = polygonA->m_radius + circleB->m_radius;
+	int32 vertexCount = polygonA->m_vertexCount;
+	const b2Vec2* vertices = polygonA->m_vertices;
+	const b2Vec2* normals = polygonA->m_normals;
+
+	for (int32 i = 0; i < vertexCount; ++i)
+	{
+		float32 s = b2Dot(normals[i], cLocal - vertices[i]);
+
+		if (s > radius)
+		{
+			// Early out.
+			return;
+		}
+
+		if (s > separation)
+		{
+			separation = s;
+			normalIndex = i;
+		}
+	}
+
+	// Vertices that subtend the incident face.
+	int32 vertIndex1 = normalIndex;
+	int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
+	b2Vec2 v1 = vertices[vertIndex1];
+	b2Vec2 v2 = vertices[vertIndex2];
+
+	// If the center is inside the polygon ...
+	if (separation < b2_epsilon)
+	{
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_faceA;
+		manifold->localNormal = normals[normalIndex];
+		manifold->localPoint = 0.5f * (v1 + v2);
+		manifold->points[0].localPoint = circleB->m_p;
+		manifold->points[0].id.key = 0;
+		return;
+	}
+
+	// Compute barycentric coordinates
+	float32 u1 = b2Dot(cLocal - v1, v2 - v1);
+	float32 u2 = b2Dot(cLocal - v2, v1 - v2);
+	if (u1 <= 0.0f)
+	{
+		if (b2DistanceSquared(cLocal, v1) > radius * radius)
+		{
+			return;
+		}
+
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_faceA;
+		manifold->localNormal = cLocal - v1;
+		manifold->localNormal.Normalize();
+		manifold->localPoint = v1;
+		manifold->points[0].localPoint = circleB->m_p;
+		manifold->points[0].id.key = 0;
+	}
+	else if (u2 <= 0.0f)
+	{
+		if (b2DistanceSquared(cLocal, v2) > radius * radius)
+		{
+			return;
+		}
+
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_faceA;
+		manifold->localNormal = cLocal - v2;
+		manifold->localNormal.Normalize();
+		manifold->localPoint = v2;
+		manifold->points[0].localPoint = circleB->m_p;
+		manifold->points[0].id.key = 0;
+	}
+	else
+	{
+		b2Vec2 faceCenter = 0.5f * (v1 + v2);
+		float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
+		if (separation > radius)
+		{
+			return;
+		}
+
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_faceA;
+		manifold->localNormal = normals[vertIndex1];
+		manifold->localPoint = faceCenter;
+		manifold->points[0].localPoint = circleB->m_p;
+		manifold->points[0].id.key = 0;
+	}
+}

AndEngine/jni/Box2D/Collision/b2CollidePolygon.cpp

@@ -0,0 +1,306 @@
+/*
+* Copyright (c) 2006-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 "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+// Find the separation between poly1 and poly2 for a give edge normal on poly1.
+static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
+							  const b2PolygonShape* poly2, const b2Transform& xf2)
+{
+	int32 count1 = poly1->m_vertexCount;
+	const b2Vec2* vertices1 = poly1->m_vertices;
+	const b2Vec2* normals1 = poly1->m_normals;
+
+	int32 count2 = poly2->m_vertexCount;
+	const b2Vec2* vertices2 = poly2->m_vertices;
+
+	b2Assert(0 <= edge1 && edge1 < count1);
+
+	// Convert normal from poly1's frame into poly2's frame.
+	b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
+	b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
+
+	// Find support vertex on poly2 for -normal.
+	int32 index = 0;
+	float32 minDot = b2_maxFloat;
+
+	for (int32 i = 0; i < count2; ++i)
+	{
+		float32 dot = b2Dot(vertices2[i], normal1);
+		if (dot < minDot)
+		{
+			minDot = dot;
+			index = i;
+		}
+	}
+
+	b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
+	b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
+	float32 separation = b2Dot(v2 - v1, normal1World);
+	return separation;
+}
+
+// Find the max separation between poly1 and poly2 using edge normals from poly1.
+static float32 b2FindMaxSeparation(int32* edgeIndex,
+								 const b2PolygonShape* poly1, const b2Transform& xf1,
+								 const b2PolygonShape* poly2, const b2Transform& xf2)
+{
+	int32 count1 = poly1->m_vertexCount;
+	const b2Vec2* normals1 = poly1->m_normals;
+
+	// Vector pointing from the centroid of poly1 to the centroid of poly2.
+	b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
+	b2Vec2 dLocal1 = b2MulT(xf1.R, d);
+
+	// Find edge normal on poly1 that has the largest projection onto d.
+	int32 edge = 0;
+	float32 maxDot = -b2_maxFloat;
+	for (int32 i = 0; i < count1; ++i)
+	{
+		float32 dot = b2Dot(normals1[i], dLocal1);
+		if (dot > maxDot)
+		{
+			maxDot = dot;
+			edge = i;
+		}
+	}
+
+	// Get the separation for the edge normal.
+	float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+
+	// Check the separation for the previous edge normal.
+	int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
+	float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
+
+	// Check the separation for the next edge normal.
+	int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
+	float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
+
+	// Find the best edge and the search direction.
+	int32 bestEdge;
+	float32 bestSeparation;
+	int32 increment;
+	if (sPrev > s && sPrev > sNext)
+	{
+		increment = -1;
+		bestEdge = prevEdge;
+		bestSeparation = sPrev;
+	}
+	else if (sNext > s)
+	{
+		increment = 1;
+		bestEdge = nextEdge;
+		bestSeparation = sNext;
+	}
+	else
+	{
+		*edgeIndex = edge;
+		return s;
+	}
+
+	// Perform a local search for the best edge normal.
+	for ( ; ; )
+	{
+		if (increment == -1)
+			edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
+		else
+			edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
+
+		s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+
+		if (s > bestSeparation)
+		{
+			bestEdge = edge;
+			bestSeparation = s;
+		}
+		else
+		{
+			break;
+		}
+	}
+
+	*edgeIndex = bestEdge;
+	return bestSeparation;
+}
+
+static void b2FindIncidentEdge(b2ClipVertex c[2],
+							 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
+							 const b2PolygonShape* poly2, const b2Transform& xf2)
+{
+	int32 count1 = poly1->m_vertexCount;
+	const b2Vec2* normals1 = poly1->m_normals;
+
+	int32 count2 = poly2->m_vertexCount;
+	const b2Vec2* vertices2 = poly2->m_vertices;
+	const b2Vec2* normals2 = poly2->m_normals;
+
+	b2Assert(0 <= edge1 && edge1 < count1);
+
+	// Get the normal of the reference edge in poly2's frame.
+	b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
+
+	// Find the incident edge on poly2.
+	int32 index = 0;
+	float32 minDot = b2_maxFloat;
+	for (int32 i = 0; i < count2; ++i)
+	{
+		float32 dot = b2Dot(normal1, normals2[i]);
+		if (dot < minDot)
+		{
+			minDot = dot;
+			index = i;
+		}
+	}
+
+	// Build the clip vertices for the incident edge.
+	int32 i1 = index;
+	int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+	c[0].v = b2Mul(xf2, vertices2[i1]);
+	c[0].id.features.referenceEdge = (uint8)edge1;
+	c[0].id.features.incidentEdge = (uint8)i1;
+	c[0].id.features.incidentVertex = 0;
+
+	c[1].v = b2Mul(xf2, vertices2[i2]);
+	c[1].id.features.referenceEdge = (uint8)edge1;
+	c[1].id.features.incidentEdge = (uint8)i2;
+	c[1].id.features.incidentVertex = 1;
+}
+
+// Find edge normal of max separation on A - return if separating axis is found
+// Find edge normal of max separation on B - return if separation axis is found
+// Choose reference edge as min(minA, minB)
+// Find incident edge
+// Clip
+
+// The normal points from 1 to 2
+void b2CollidePolygons(b2Manifold* manifold,
+					  const b2PolygonShape* polyA, const b2Transform& xfA,
+					  const b2PolygonShape* polyB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+	float32 totalRadius = polyA->m_radius + polyB->m_radius;
+
+	int32 edgeA = 0;
+	float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+	if (separationA > totalRadius)
+		return;
+
+	int32 edgeB = 0;
+	float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+	if (separationB > totalRadius)
+		return;
+
+	const b2PolygonShape* poly1;	// reference polygon
+	const b2PolygonShape* poly2;	// incident polygon
+	b2Transform xf1, xf2;
+	int32 edge1;		// reference edge
+	uint8 flip;
+	const float32 k_relativeTol = 0.98f;
+	const float32 k_absoluteTol = 0.001f;
+
+	if (separationB > k_relativeTol * separationA + k_absoluteTol)
+	{
+		poly1 = polyB;
+		poly2 = polyA;
+		xf1 = xfB;
+		xf2 = xfA;
+		edge1 = edgeB;
+		manifold->type = b2Manifold::e_faceB;
+		flip = 1;
+	}
+	else
+	{
+		poly1 = polyA;
+		poly2 = polyB;
+		xf1 = xfA;
+		xf2 = xfB;
+		edge1 = edgeA;
+		manifold->type = b2Manifold::e_faceA;
+		flip = 0;
+	}
+
+	b2ClipVertex incidentEdge[2];
+	b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+
+	int32 count1 = poly1->m_vertexCount;
+	const b2Vec2* vertices1 = poly1->m_vertices;
+
+	b2Vec2 v11 = vertices1[edge1];
+	b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
+
+	b2Vec2 localTangent = v12 - v11;
+	localTangent.Normalize();
+	
+	b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
+	b2Vec2 planePoint = 0.5f * (v11 + v12);
+
+	b2Vec2 tangent = b2Mul(xf1.R, localTangent);
+	b2Vec2 normal = b2Cross(tangent, 1.0f);
+	
+	v11 = b2Mul(xf1, v11);
+	v12 = b2Mul(xf1, v12);
+
+	// Face offset.
+	float32 frontOffset = b2Dot(normal, v11);
+
+	// Side offsets, extended by polytope skin thickness.
+	float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
+	float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
+
+	// Clip incident edge against extruded edge1 side edges.
+	b2ClipVertex clipPoints1[2];
+	b2ClipVertex clipPoints2[2];
+	int np;
+
+	// Clip to box side 1
+	np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
+
+	if (np < 2)
+		return;
+
+	// Clip to negative box side 1
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2);
+
+	if (np < 2)
+	{
+		return;
+	}
+
+	// Now clipPoints2 contains the clipped points.
+	manifold->localNormal = localNormal;
+	manifold->localPoint = planePoint;
+
+	int32 pointCount = 0;
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
+
+		if (separation <= totalRadius)
+		{
+			b2ManifoldPoint* cp = manifold->points + pointCount;
+			cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
+			cp->id = clipPoints2[i].id;
+			cp->id.features.flip = flip;
+			++pointCount;
+		}
+	}
+
+	manifold->pointCount = pointCount;
+}

AndEngine/jni/Box2D/Collision/b2Collision.cpp

@@ -0,0 +1,250 @@
+/*
+* 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 "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/b2Distance.h"
+
+void b2WorldManifold::Initialize(const b2Manifold* manifold,
+						  const b2Transform& xfA, float32 radiusA,
+						  const b2Transform& xfB, float32 radiusB)
+{
+	if (manifold->pointCount == 0)
+	{
+		return;
+	}
+
+	switch (manifold->type)
+	{
+	case b2Manifold::e_circles:
+		{
+			normal.Set(1.0f, 0.0f);
+			b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
+			b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
+			if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
+			{
+				normal = pointB - pointA;
+				normal.Normalize();
+			}
+
+			b2Vec2 cA = pointA + radiusA * normal;
+			b2Vec2 cB = pointB - radiusB * normal;
+			points[0] = 0.5f * (cA + cB);
+		}
+		break;
+
+	case b2Manifold::e_faceA:
+		{
+			normal = b2Mul(xfA.R, manifold->localNormal);
+			b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
+			
+			for (int32 i = 0; i < manifold->pointCount; ++i)
+			{
+				b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
+				b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
+				b2Vec2 cB = clipPoint - radiusB * normal;
+				points[i] = 0.5f * (cA + cB);
+			}
+		}
+		break;
+
+	case b2Manifold::e_faceB:
+		{
+			normal = b2Mul(xfB.R, manifold->localNormal);
+			b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
+
+			for (int32 i = 0; i < manifold->pointCount; ++i)
+			{
+				b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
+				b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
+				b2Vec2 cA = clipPoint - radiusA * normal;
+				points[i] = 0.5f * (cA + cB);
+			}
+
+			// Ensure normal points from A to B.
+			normal = -normal;
+		}
+		break;
+	}
+}
+
+void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
+					  const b2Manifold* manifold1, const b2Manifold* manifold2)
+{
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		state1[i] = b2_nullState;
+		state2[i] = b2_nullState;
+	}
+
+	// Detect persists and removes.
+	for (int32 i = 0; i < manifold1->pointCount; ++i)
+	{
+		b2ContactID id = manifold1->points[i].id;
+
+		state1[i] = b2_removeState;
+
+		for (int32 j = 0; j < manifold2->pointCount; ++j)
+		{
+			if (manifold2->points[j].id.key == id.key)
+			{
+				state1[i] = b2_persistState;
+				break;
+			}
+		}
+	}
+
+	// Detect persists and adds.
+	for (int32 i = 0; i < manifold2->pointCount; ++i)
+	{
+		b2ContactID id = manifold2->points[i].id;
+
+		state2[i] = b2_addState;
+
+		for (int32 j = 0; j < manifold1->pointCount; ++j)
+		{
+			if (manifold1->points[j].id.key == id.key)
+			{
+				state2[i] = b2_persistState;
+				break;
+			}
+		}
+	}
+}
+
+// From Real-time Collision Detection, p179.
+bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
+{
+	float32 tmin = -b2_maxFloat;
+	float32 tmax = b2_maxFloat;
+
+	b2Vec2 p = input.p1;
+	b2Vec2 d = input.p2 - input.p1;
+	b2Vec2 absD = b2Abs(d);
+
+	b2Vec2 normal;
+
+	for (int32 i = 0; i < 2; ++i)
+	{
+		if (absD(i) < b2_epsilon)
+		{
+			// Parallel.
+			if (p(i) < lowerBound(i) || upperBound(i) < p(i))
+			{
+				return false;
+			}
+		}
+		else
+		{
+			float32 inv_d = 1.0f / d(i);
+			float32 t1 = (lowerBound(i) - p(i)) * inv_d;
+			float32 t2 = (upperBound(i) - p(i)) * inv_d;
+
+			// Sign of the normal vector.
+			float32 s = -1.0f;
+
+			if (t1 > t2)
+			{
+				b2Swap(t1, t2);
+				s = 1.0f;
+			}
+
+			// Push the min up
+			if (t1 > tmin)
+			{
+				normal.SetZero();
+				normal(i) = s;
+				tmin = t1;
+			}
+
+			// Pull the max down
+			tmax = b2Min(tmax, t2);
+
+			if (tmin > tmax)
+			{
+				return false;
+			}
+		}
+	}
+
+	// Does the ray start inside the box?
+	// Does the ray intersect beyond the max fraction?
+	if (tmin < 0.0f || input.maxFraction < tmin)
+	{
+		return false;
+	}
+
+	// Intersection.
+	output->fraction = tmin;
+	output->normal = normal;
+	return true;
+}
+
+// Sutherland-Hodgman clipping.
+int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
+						const b2Vec2& normal, float32 offset)
+{
+	// Start with no output points
+	int32 numOut = 0;
+
+	// Calculate the distance of end points to the line
+	float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
+	float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
+
+	// If the points are behind the plane
+	if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
+	if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
+
+	// If the points are on different sides of the plane
+	if (distance0 * distance1 < 0.0f)
+	{
+		// Find intersection point of edge and plane
+		float32 interp = distance0 / (distance0 - distance1);
+		vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
+		if (distance0 > 0.0f)
+		{
+			vOut[numOut].id = vIn[0].id;
+		}
+		else
+		{
+			vOut[numOut].id = vIn[1].id;
+		}
+		++numOut;
+	}
+
+	return numOut;
+}
+
+bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
+				   const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2DistanceInput input;
+	input.proxyA.Set(shapeA);
+	input.proxyB.Set(shapeB);
+	input.transformA = xfA;
+	input.transformB = xfB;
+	input.useRadii = true;
+
+	b2SimplexCache cache;
+	cache.count = 0;
+
+	b2DistanceOutput output;
+
+	b2Distance(&output, &cache, &input);
+
+	return output.distance < 10.0f * b2_epsilon;
+}

AndEngine/jni/Box2D/Collision/b2Collision.h

@@ -0,0 +1,240 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#ifndef B2_COLLISION_H
+#define B2_COLLISION_H
+
+#include "Box2D/Common/b2Math.h"
+#include <limits.h>
+
+/// @file
+/// Structures and functions used for computing contact points, distance
+/// queries, and TOI queries.
+
+class b2Shape;
+class b2CircleShape;
+class b2PolygonShape;
+
+const uint8 b2_nullFeature = UCHAR_MAX;
+
+/// Contact ids to facilitate warm starting.
+union b2ContactID
+{
+	/// The features that intersect to form the contact point
+	struct Features
+	{
+		uint8 referenceEdge;	///< The edge that defines the outward contact normal.
+		uint8 incidentEdge;		///< The edge most anti-parallel to the reference edge.
+		uint8 incidentVertex;	///< The vertex (0 or 1) on the incident edge that was clipped.
+		uint8 flip;				///< A value of 1 indicates that the reference edge is on shape2.
+	} features;
+	uint32 key;					///< Used to quickly compare contact ids.
+};
+
+/// A manifold point is a contact point belonging to a contact
+/// manifold. It holds details related to the geometry and dynamics
+/// of the contact points.
+/// The local point usage depends on the manifold type:
+/// -e_circles: the local center of circleB
+/// -e_faceA: the local center of cirlceB or the clip point of polygonB
+/// -e_faceB: the clip point of polygonA
+/// This structure is stored across time steps, so we keep it small.
+/// Note: the impulses are used for internal caching and may not
+/// provide reliable contact forces, especially for high speed collisions.
+struct b2ManifoldPoint
+{
+	b2Vec2 localPoint;		///< usage depends on manifold type
+	float32 normalImpulse;	///< the non-penetration impulse
+	float32 tangentImpulse;	///< the friction impulse
+	b2ContactID id;			///< uniquely identifies a contact point between two shapes
+};
+
+/// A manifold for two touching convex shapes.
+/// Box2D supports multiple types of contact:
+/// - clip point versus plane with radius
+/// - point versus point with radius (circles)
+/// The local point usage depends on the manifold type:
+/// -e_circles: the local center of circleA
+/// -e_faceA: the center of faceA
+/// -e_faceB: the center of faceB
+/// Similarly the local normal usage:
+/// -e_circles: not used
+/// -e_faceA: the normal on polygonA
+/// -e_faceB: the normal on polygonB
+/// We store contacts in this way so that position correction can
+/// account for movement, which is critical for continuous physics.
+/// All contact scenarios must be expressed in one of these types.
+/// This structure is stored across time steps, so we keep it small.
+struct b2Manifold
+{
+	enum Type
+	{
+		e_circles,
+		e_faceA,
+		e_faceB
+	};
+
+	b2ManifoldPoint points[b2_maxManifoldPoints];	///< the points of contact
+	b2Vec2 localNormal;								///< not use for Type::e_points
+	b2Vec2 localPoint;								///< usage depends on manifold type
+	Type type;
+	int32 pointCount;								///< the number of manifold points
+};
+
+/// This is used to compute the current state of a contact manifold.
+struct b2WorldManifold
+{
+	/// Evaluate the manifold with supplied transforms. This assumes
+	/// modest motion from the original state. This does not change the
+	/// point count, impulses, etc. The radii must come from the shapes
+	/// that generated the manifold.
+	void Initialize(const b2Manifold* manifold,
+					const b2Transform& xfA, float32 radiusA,
+					const b2Transform& xfB, float32 radiusB);
+
+	b2Vec2 normal;						///< world vector pointing from A to B
+	b2Vec2 points[b2_maxManifoldPoints];	///< world contact point (point of intersection)
+};
+
+/// This is used for determining the state of contact points.
+enum b2PointState
+{
+	b2_nullState,		///< point does not exist
+	b2_addState,		///< point was added in the update
+	b2_persistState,	///< point persisted across the update
+	b2_removeState		///< point was removed in the update
+};
+
+/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
+/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
+void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
+					  const b2Manifold* manifold1, const b2Manifold* manifold2);
+
+/// Used for computing contact manifolds.
+struct b2ClipVertex
+{
+	b2Vec2 v;
+	b2ContactID id;
+};
+
+/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+struct b2RayCastInput
+{
+	b2Vec2 p1, p2;
+	float32 maxFraction;
+};
+
+/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
+/// come from b2RayCastInput.
+struct b2RayCastOutput
+{
+	b2Vec2 normal;
+	float32 fraction;
+};
+
+/// An axis aligned bounding box.
+struct b2AABB
+{
+	/// Verify that the bounds are sorted.
+	bool IsValid() const;
+
+	/// Get the center of the AABB.
+	b2Vec2 GetCenter() const
+	{
+		return 0.5f * (lowerBound + upperBound);
+	}
+
+	/// Get the extents of the AABB (half-widths).
+	b2Vec2 GetExtents() const
+	{
+		return 0.5f * (upperBound - lowerBound);
+	}
+
+	/// Combine two AABBs into this one.
+	void Combine(const b2AABB& aabb1, const b2AABB& aabb2)
+	{
+		lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound);
+		upperBound = b2Max(aabb1.upperBound, aabb2.upperBound);
+	}
+
+	/// Does this aabb contain the provided AABB.
+	bool Contains(const b2AABB& aabb) const
+	{
+		bool result = true;
+		result = result && lowerBound.x <= aabb.lowerBound.x;
+		result = result && lowerBound.y <= aabb.lowerBound.y;
+		result = result && aabb.upperBound.x <= upperBound.x;
+		result = result && aabb.upperBound.y <= upperBound.y;
+		return result;
+	}
+
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const;
+
+	b2Vec2 lowerBound;	///< the lower vertex
+	b2Vec2 upperBound;	///< the upper vertex
+};
+
+/// Compute the collision manifold between two circles.
+void b2CollideCircles(b2Manifold* manifold,
+					  const b2CircleShape* circle1, const b2Transform& xf1,
+					  const b2CircleShape* circle2, const b2Transform& xf2);
+
+/// Compute the collision manifold between a polygon and a circle.
+void b2CollidePolygonAndCircle(b2Manifold* manifold,
+							   const b2PolygonShape* polygon, const b2Transform& xf1,
+							   const b2CircleShape* circle, const b2Transform& xf2);
+
+/// Compute the collision manifold between two polygons.
+void b2CollidePolygons(b2Manifold* manifold,
+					   const b2PolygonShape* polygon1, const b2Transform& xf1,
+					   const b2PolygonShape* polygon2, const b2Transform& xf2);
+
+/// Clipping for contact manifolds.
+int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
+							const b2Vec2& normal, float32 offset);
+
+/// Determine if two generic shapes overlap.
+bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
+				   const b2Transform& xfA, const b2Transform& xfB);
+
+// ---------------- Inline Functions ------------------------------------------
+
+inline bool b2AABB::IsValid() const
+{
+	b2Vec2 d = upperBound - lowerBound;
+	bool valid = d.x >= 0.0f && d.y >= 0.0f;
+	valid = valid && lowerBound.IsValid() && upperBound.IsValid();
+	return valid;
+}
+
+inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b)
+{
+	b2Vec2 d1, d2;
+	d1 = b.lowerBound - a.upperBound;
+	d2 = a.lowerBound - b.upperBound;
+
+	if (d1.x > 0.0f || d1.y > 0.0f)
+		return false;
+
+	if (d2.x > 0.0f || d2.y > 0.0f)
+		return false;
+
+	return true;
+}
+
+#endif

AndEngine/jni/Box2D/Collision/b2Distance.cpp

@@ -0,0 +1,571 @@
+/*
+* 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 "Box2D/Collision/b2Distance.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.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)
+{
+	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;
+		}
+	}
+}

AndEngine/jni/Box2D/Collision/b2Distance.h

@@ -0,0 +1,141 @@
+
+/*
+* Copyright (c) 2006-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.
+*/
+
+#ifndef B2_DISTANCE_H
+#define B2_DISTANCE_H
+
+#include "Box2D/Common/b2Math.h"
+#include <limits.h>
+
+class b2Shape;
+
+/// A distance proxy is used by the GJK algorithm.
+/// It encapsulates any shape.
+struct b2DistanceProxy
+{
+	b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {}
+
+	/// Initialize the proxy using the given shape. The shape
+	/// must remain in scope while the proxy is in use.
+	void Set(const b2Shape* shape);
+
+	/// Get the supporting vertex index in the given direction.
+	int32 GetSupport(const b2Vec2& d) const;
+
+	/// Get the supporting vertex in the given direction.
+	const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
+
+	/// Get the vertex count.
+	int32 GetVertexCount() const;
+
+	/// Get a vertex by index. Used by b2Distance.
+	const b2Vec2& GetVertex(int32 index) const;
+
+	const b2Vec2* m_vertices;
+	int32 m_count;
+	float32 m_radius;
+};
+
+/// Used to warm start b2Distance.
+/// Set count to zero on first call.
+struct b2SimplexCache
+{
+	float32 metric;		///< length or area
+	uint16 count;
+	uint8 indexA[3];	///< vertices on shape A
+	uint8 indexB[3];	///< vertices on shape B
+};
+
+/// Input for b2Distance.
+/// You have to option to use the shape radii
+/// in the computation. Even 
+struct b2DistanceInput
+{
+	b2DistanceProxy proxyA;
+	b2DistanceProxy proxyB;
+	b2Transform transformA;
+	b2Transform transformB;
+	bool useRadii;
+};
+
+/// Output for b2Distance.
+struct b2DistanceOutput
+{
+	b2Vec2 pointA;		///< closest point on shapeA
+	b2Vec2 pointB;		///< closest point on shapeB
+	float32 distance;
+	int32 iterations;	///< number of GJK iterations used
+};
+
+/// Compute the closest points between two shapes. Supports any combination of:
+/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
+/// On the first call set b2SimplexCache.count to zero.
+void b2Distance(b2DistanceOutput* output,
+				b2SimplexCache* cache, 
+				const b2DistanceInput* input);
+
+
+//////////////////////////////////////////////////////////////////////////
+
+inline int32 b2DistanceProxy::GetVertexCount() const
+{
+	return m_count;
+}
+
+inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const
+{
+	b2Assert(0 <= index && index < m_count);
+	return m_vertices[index];
+}
+
+inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const
+{
+	int32 bestIndex = 0;
+	float32 bestValue = b2Dot(m_vertices[0], d);
+	for (int32 i = 1; i < m_count; ++i)
+	{
+		float32 value = b2Dot(m_vertices[i], d);
+		if (value > bestValue)
+		{
+			bestIndex = i;
+			bestValue = value;
+		}
+	}
+
+	return bestIndex;
+}
+
+inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const
+{
+	int32 bestIndex = 0;
+	float32 bestValue = b2Dot(m_vertices[0], d);
+	for (int32 i = 1; i < m_count; ++i)
+	{
+		float32 value = b2Dot(m_vertices[i], d);
+		if (value > bestValue)
+		{
+			bestIndex = i;
+			bestValue = value;
+		}
+	}
+
+	return m_vertices[bestIndex];
+}
+
+#endif

AndEngine/jni/Box2D/Collision/b2DynamicTree.cpp

@@ -0,0 +1,365 @@
+/*
+* Copyright (c) 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 "Box2D/Collision/b2DynamicTree.h"
+#include <string.h>
+#include <float.h>
+
+b2DynamicTree::b2DynamicTree()
+{
+	m_root = b2_nullNode;
+
+	m_nodeCapacity = 16;
+	m_nodeCount = 0;
+	m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
+	memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode));
+
+	// Build a linked list for the free list.
+	for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
+	{
+		m_nodes[i].next = i + 1;
+	}
+	m_nodes[m_nodeCapacity-1].next = b2_nullNode;
+	m_freeList = 0;
+
+	m_path = 0;
+
+	m_insertionCount = 0;
+}
+
+b2DynamicTree::~b2DynamicTree()
+{
+	// This frees the entire tree in one shot.
+	b2Free(m_nodes);
+}
+
+// Allocate a node from the pool. Grow the pool if necessary.
+int32 b2DynamicTree::AllocateNode()
+{
+	// Expand the node pool as needed.
+	if (m_freeList == b2_nullNode)
+	{
+		b2Assert(m_nodeCount == m_nodeCapacity);
+
+		// The free list is empty. Rebuild a bigger pool.
+		b2DynamicTreeNode* oldNodes = m_nodes;
+		m_nodeCapacity *= 2;
+		m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
+		memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode));
+		b2Free(oldNodes);
+
+		// Build a linked list for the free list. The parent
+		// pointer becomes the "next" pointer.
+		for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
+		{
+			m_nodes[i].next = i + 1;
+		}
+		m_nodes[m_nodeCapacity-1].next = b2_nullNode;
+		m_freeList = m_nodeCount;
+	}
+
+	// Peel a node off the free list.
+	int32 nodeId = m_freeList;
+	m_freeList = m_nodes[nodeId].next;
+	m_nodes[nodeId].parent = b2_nullNode;
+	m_nodes[nodeId].child1 = b2_nullNode;
+	m_nodes[nodeId].child2 = b2_nullNode;
+	++m_nodeCount;
+	return nodeId;
+}
+
+// Return a node to the pool.
+void b2DynamicTree::FreeNode(int32 nodeId)
+{
+	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+	b2Assert(0 < m_nodeCount);
+	m_nodes[nodeId].next = m_freeList;
+	m_freeList = nodeId;
+	--m_nodeCount;
+}
+
+// Create a proxy in the tree as a leaf node. We return the index
+// of the node instead of a pointer so that we can grow
+// the node pool.
+int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
+{
+	int32 proxyId = AllocateNode();
+
+	// Fatten the aabb.
+	b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
+	m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
+	m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
+	m_nodes[proxyId].userData = userData;
+
+	InsertLeaf(proxyId);
+
+	// Rebalance if necessary.
+	int32 iterationCount = m_nodeCount >> 4;
+	int32 tryCount = 0;
+	int32 height = ComputeHeight();
+	while (height > 64 && tryCount < 10)
+	{
+		Rebalance(iterationCount);
+		height = ComputeHeight();
+		++tryCount;
+	}
+
+	return proxyId;
+}
+
+void b2DynamicTree::DestroyProxy(int32 proxyId)
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+	b2Assert(m_nodes[proxyId].IsLeaf());
+
+	RemoveLeaf(proxyId);
+	FreeNode(proxyId);
+}
+
+bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+
+	b2Assert(m_nodes[proxyId].IsLeaf());
+
+	if (m_nodes[proxyId].aabb.Contains(aabb))
+	{
+		return false;
+	}
+
+	RemoveLeaf(proxyId);
+
+	// Extend AABB.
+	b2AABB b = aabb;
+	b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
+	b.lowerBound = b.lowerBound - r;
+	b.upperBound = b.upperBound + r;
+
+	// Predict AABB displacement.
+	b2Vec2 d = b2_aabbMultiplier * displacement;
+
+	if (d.x < 0.0f)
+	{
+		b.lowerBound.x += d.x;
+	}
+	else
+	{
+		b.upperBound.x += d.x;
+	}
+
+	if (d.y < 0.0f)
+	{
+		b.lowerBound.y += d.y;
+	}
+	else
+	{
+		b.upperBound.y += d.y;
+	}
+
+	m_nodes[proxyId].aabb = b;
+
+	InsertLeaf(proxyId);
+	return true;
+}
+
+void b2DynamicTree::InsertLeaf(int32 leaf)
+{
+	++m_insertionCount;
+
+	if (m_root == b2_nullNode)
+	{
+		m_root = leaf;
+		m_nodes[m_root].parent = b2_nullNode;
+		return;
+	}
+
+	// Find the best sibling for this node.
+	b2Vec2 center = m_nodes[leaf].aabb.GetCenter();
+	int32 sibling = m_root;
+	if (m_nodes[sibling].IsLeaf() == false)
+	{
+		do 
+		{
+			int32 child1 = m_nodes[sibling].child1;
+			int32 child2 = m_nodes[sibling].child2;
+
+			b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center);
+			b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center);
+
+			float32 norm1 = delta1.x + delta1.y;
+			float32 norm2 = delta2.x + delta2.y;
+
+			if (norm1 < norm2)
+			{
+				sibling = child1;
+			}
+			else
+			{
+				sibling = child2;
+			}
+
+		}
+		while(m_nodes[sibling].IsLeaf() == false);
+	}
+
+	// Create a parent for the siblings.
+	int32 node1 = m_nodes[sibling].parent;
+	int32 node2 = AllocateNode();
+	m_nodes[node2].parent = node1;
+	m_nodes[node2].userData = NULL;
+	m_nodes[node2].aabb.Combine(m_nodes[leaf].aabb, m_nodes[sibling].aabb);
+
+	if (node1 != b2_nullNode)
+	{
+		if (m_nodes[m_nodes[sibling].parent].child1 == sibling)
+		{
+			m_nodes[node1].child1 = node2;
+		}
+		else
+		{
+			m_nodes[node1].child2 = node2;
+		}
+
+		m_nodes[node2].child1 = sibling;
+		m_nodes[node2].child2 = leaf;
+		m_nodes[sibling].parent = node2;
+		m_nodes[leaf].parent = node2;
+
+		do 
+		{
+			if (m_nodes[node1].aabb.Contains(m_nodes[node2].aabb))
+			{
+				break;
+			}
+
+			m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);
+			node2 = node1;
+			node1 = m_nodes[node1].parent;
+		}
+		while(node1 != b2_nullNode);
+	}
+	else
+	{
+		m_nodes[node2].child1 = sibling;
+		m_nodes[node2].child2 = leaf;
+		m_nodes[sibling].parent = node2;
+		m_nodes[leaf].parent = node2;
+		m_root = node2;
+	}
+}
+
+void b2DynamicTree::RemoveLeaf(int32 leaf)
+{
+	if (leaf == m_root)
+	{
+		m_root = b2_nullNode;
+		return;
+	}
+
+	int32 node2 = m_nodes[leaf].parent;
+	int32 node1 = m_nodes[node2].parent;
+	int32 sibling;
+	if (m_nodes[node2].child1 == leaf)
+	{
+		sibling = m_nodes[node2].child2;
+	}
+	else
+	{
+		sibling = m_nodes[node2].child1;
+	}
+
+	if (node1 != b2_nullNode)
+	{
+		// Destroy node2 and connect node1 to sibling.
+		if (m_nodes[node1].child1 == node2)
+		{
+			m_nodes[node1].child1 = sibling;
+		}
+		else
+		{
+			m_nodes[node1].child2 = sibling;
+		}
+		m_nodes[sibling].parent = node1;
+		FreeNode(node2);
+
+		// Adjust ancestor bounds.
+		while (node1 != b2_nullNode)
+		{
+			b2AABB oldAABB = m_nodes[node1].aabb;
+			m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);
+
+			if (oldAABB.Contains(m_nodes[node1].aabb))
+			{
+				break;
+			}
+
+			node1 = m_nodes[node1].parent;
+		}
+	}
+	else
+	{
+		m_root = sibling;
+		m_nodes[sibling].parent = b2_nullNode;
+		FreeNode(node2);
+	}
+}
+
+void b2DynamicTree::Rebalance(int32 iterations)
+{
+	if (m_root == b2_nullNode)
+	{
+		return;
+	}
+
+	for (int32 i = 0; i < iterations; ++i)
+	{
+		int32 node = m_root;
+
+		uint32 bit = 0;
+		while (m_nodes[node].IsLeaf() == false)
+		{
+			int32* children = &m_nodes[node].child1;
+			node = children[(m_path >> bit) & 1];
+			bit = (bit + 1) & (8* sizeof(uint32) - 1);
+		}
+		++m_path;
+
+		RemoveLeaf(node);
+		InsertLeaf(node);
+	}
+}
+
+// Compute the height of a sub-tree.
+int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
+{
+	if (nodeId == b2_nullNode)
+	{
+		return 0;
+	}
+
+	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+	b2DynamicTreeNode* node = m_nodes + nodeId;
+	int32 height1 = ComputeHeight(node->child1);
+	int32 height2 = ComputeHeight(node->child2);
+	return 1 + b2Max(height1, height2);
+}
+
+int32 b2DynamicTree::ComputeHeight() const
+{
+	return ComputeHeight(m_root);
+}

AndEngine/jni/Box2D/Collision/b2DynamicTree.h

@@ -0,0 +1,286 @@
+/*
+* Copyright (c) 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.
+*/
+
+#ifndef B2_DYNAMIC_TREE_H
+#define B2_DYNAMIC_TREE_H
+
+#include "Box2D/Collision/b2Collision.h"
+
+/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
+
+#define b2_nullNode (-1)
+
+/// A node in the dynamic tree. The client does not interact with this directly.
+struct b2DynamicTreeNode
+{
+	bool IsLeaf() const
+	{
+		return child1 == b2_nullNode;
+	}
+
+	/// This is the fattened AABB.
+	b2AABB aabb;
+
+	//int32 userData;
+	void* userData;
+
+	union
+	{
+		int32 parent;
+		int32 next;
+	};
+
+	int32 child1;
+	int32 child2;
+};
+
+/// A dynamic tree arranges data in a binary tree to accelerate
+/// queries such as volume queries and ray casts. Leafs are proxies
+/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
+/// so that the proxy AABB is bigger than the client object. This allows the client
+/// object to move by small amounts without triggering a tree update.
+///
+/// Nodes are pooled and relocatable, so we use node indices rather than pointers.
+class b2DynamicTree
+{
+public:
+
+	/// Constructing the tree initializes the node pool.
+	b2DynamicTree();
+
+	/// Destroy the tree, freeing the node pool.
+	~b2DynamicTree();
+
+	/// Create a proxy. Provide a tight fitting AABB and a userData pointer.
+	int32 CreateProxy(const b2AABB& aabb, void* userData);
+
+	/// Destroy a proxy. This asserts if the id is invalid.
+	void DestroyProxy(int32 proxyId);
+
+	/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
+	/// then the proxy is removed from the tree and re-inserted. Otherwise
+	/// the function returns immediately.
+	/// @return true if the proxy was re-inserted.
+	bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
+
+	/// Perform some iterations to re-balance the tree.
+	void Rebalance(int32 iterations);
+
+	/// Get proxy user data.
+	/// @return the proxy user data or 0 if the id is invalid.
+	void* GetUserData(int32 proxyId) const;
+
+	/// Get the fat AABB for a proxy.
+	const b2AABB& GetFatAABB(int32 proxyId) const;
+
+	/// Compute the height of the tree.
+	int32 ComputeHeight() const;
+
+	/// Query an AABB for overlapping proxies. The callback class
+	/// is called for each proxy that overlaps the supplied AABB.
+	template <typename T>
+	void Query(T* callback, const b2AABB& aabb) const;
+
+	/// Ray-cast against the proxies in the tree. This relies on the callback
+	/// to perform a exact ray-cast in the case were the proxy contains a shape.
+	/// The callback also performs the any collision filtering. This has performance
+	/// roughly equal to k * log(n), where k is the number of collisions and n is the
+	/// number of proxies in the tree.
+	/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+	/// @param callback a callback class that is called for each proxy that is hit by the ray.
+	template <typename T>
+	void RayCast(T* callback, const b2RayCastInput& input) const;
+
+private:
+
+	int32 AllocateNode();
+	void FreeNode(int32 node);
+
+	void InsertLeaf(int32 node);
+	void RemoveLeaf(int32 node);
+
+	int32 ComputeHeight(int32 nodeId) const;
+
+	int32 m_root;
+
+	b2DynamicTreeNode* m_nodes;
+	int32 m_nodeCount;
+	int32 m_nodeCapacity;
+
+	int32 m_freeList;
+
+	/// This is used incrementally traverse the tree for re-balancing.
+	uint32 m_path;
+
+	int32 m_insertionCount;
+};
+
+inline void* b2DynamicTree::GetUserData(int32 proxyId) const
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+	return m_nodes[proxyId].userData;
+}
+
+inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+	return m_nodes[proxyId].aabb;
+}
+
+template <typename T>
+inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
+{
+	const int32 k_stackSize = 128;
+	int32 stack[k_stackSize];
+
+	int32 count = 0;
+	stack[count++] = m_root;
+
+	while (count > 0)
+	{
+		int32 nodeId = stack[--count];
+		if (nodeId == b2_nullNode)
+		{
+			continue;
+		}
+
+		const b2DynamicTreeNode* node = m_nodes + nodeId;
+
+		if (b2TestOverlap(node->aabb, aabb))
+		{
+			if (node->IsLeaf())
+			{
+				bool proceed = callback->QueryCallback(nodeId);
+				if (proceed == false)
+				{
+					return;
+				}
+			}
+			else
+			{
+				if (count < k_stackSize)
+				{
+					stack[count++] = node->child1;
+				}
+
+				if (count < k_stackSize)
+				{
+					stack[count++] = node->child2;
+				}
+			}
+		}
+	}
+}
+
+template <typename T>
+inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
+{
+	b2Vec2 p1 = input.p1;
+	b2Vec2 p2 = input.p2;
+	b2Vec2 r = p2 - p1;
+	b2Assert(r.LengthSquared() > 0.0f);
+	r.Normalize();
+
+	// v is perpendicular to the segment.
+	b2Vec2 v = b2Cross(1.0f, r);
+	b2Vec2 abs_v = b2Abs(v);
+
+	// Separating axis for segment (Gino, p80).
+	// |dot(v, p1 - c)| > dot(|v|, h)
+
+	float32 maxFraction = input.maxFraction;
+
+	// Build a bounding box for the segment.
+	b2AABB segmentAABB;
+	{
+		b2Vec2 t = p1 + maxFraction * (p2 - p1);
+		segmentAABB.lowerBound = b2Min(p1, t);
+		segmentAABB.upperBound = b2Max(p1, t);
+	}
+
+	const int32 k_stackSize = 128;
+	int32 stack[k_stackSize];
+
+	int32 count = 0;
+	stack[count++] = m_root;
+
+	while (count > 0)
+	{
+		int32 nodeId = stack[--count];
+		if (nodeId == b2_nullNode)
+		{
+			continue;
+		}
+
+		const b2DynamicTreeNode* node = m_nodes + nodeId;
+
+		if (b2TestOverlap(node->aabb, segmentAABB) == false)
+		{
+			continue;
+		}
+
+		// Separating axis for segment (Gino, p80).
+		// |dot(v, p1 - c)| > dot(|v|, h)
+		b2Vec2 c = node->aabb.GetCenter();
+		b2Vec2 h = node->aabb.GetExtents();
+		float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
+		if (separation > 0.0f)
+		{
+			continue;
+		}
+
+		if (node->IsLeaf())
+		{
+			b2RayCastInput subInput;
+			subInput.p1 = input.p1;
+			subInput.p2 = input.p2;
+			subInput.maxFraction = maxFraction;
+
+			float32 value = callback->RayCastCallback(subInput, nodeId);
+
+			if (value == 0.0f)
+			{
+				// The client has terminated the ray cast.
+				return;
+			}
+
+			if (value > 0.0f)
+			{
+				// Update segment bounding box.
+				maxFraction = value;
+				b2Vec2 t = p1 + maxFraction * (p2 - p1);
+				segmentAABB.lowerBound = b2Min(p1, t);
+				segmentAABB.upperBound = b2Max(p1, t);
+			}
+		}
+		else
+		{
+			if (count < k_stackSize)
+			{
+				stack[count++] = node->child1;
+			}
+
+			if (count < k_stackSize)
+			{
+				stack[count++] = node->child2;
+			}
+		}
+	}
+}
+
+#endif

AndEngine/jni/Box2D/Collision/b2TimeOfImpact.cpp

@@ -0,0 +1,483 @@
+/*
+* 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 "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 <stdio.h>
+
+int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
+int32 b2_toiRootIters, b2_toiMaxRootIters;
+
+int32 b2_toiMaxOptIters;
+
+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)
+	{
+		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, 0.0f);
+		m_sweepB.GetTransform(&xfB, 0.0f);
+
+		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.R, 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.R, 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.R,  m_axis);
+				b2Vec2 axisB = b2MulT(xfB.R, -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.R, m_axis);
+				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+				b2Vec2 axisB = b2MulT(xfB.R, -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.R, m_axis);
+				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+				b2Vec2 axisA = b2MulT(xfA.R, -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.R,  m_axis);
+				b2Vec2 axisB = b2MulT(xfB.R, -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.R, m_axis);
+				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+				b2Vec2 axisB = b2MulT(xfB.R, -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.R, m_axis);
+				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+				b2Vec2 axisA = b2MulT(xfA.R, -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);
+#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);
+}

AndEngine/jni/Box2D/Collision/b2TimeOfImpact.h

@@ -0,0 +1,59 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#ifndef B2_TIME_OF_IMPACT_H
+#define B2_TIME_OF_IMPACT_H
+
+#include "Box2D/Common/b2Math.h"
+#include "Box2D/Collision/b2Distance.h"
+#include <limits.h>
+
+/// Input parameters for b2TimeOfImpact
+struct b2TOIInput
+{
+	b2DistanceProxy proxyA;
+	b2DistanceProxy proxyB;
+	b2Sweep sweepA;
+	b2Sweep sweepB;
+	float32 tMax;		// defines sweep interval [0, tMax]
+};
+
+// Output parameters for b2TimeOfImpact.
+struct b2TOIOutput
+{
+	enum State
+	{
+		e_unknown,
+		e_failed,
+		e_overlapped,
+		e_touching,
+		e_separated
+	};
+
+	State state;
+	float32 t;
+};
+
+/// Compute the upper bound on time before two shapes penetrate. Time is represented as
+/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
+/// non-tunneling collision. If you change the time interval, you should call this function
+/// again.
+/// Note: use b2Distance to compute the contact point and normal at the time of impact.
+void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);
+
+#endif