web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2CollideCircle.cpp

+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/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_count;
+	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 s = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
+		if (s > 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;
+	}
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2CollideEdge.cpp

+/*
+ * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+ *
+ * This software is provided 'as-is', without any express or implied
+ * warranty.  In no event will the authors be held liable for any damages
+ * arising from the use of this software.
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 1. The origin of this software must not be misrepresented; you must not
+ * claim that you wrote the original software. If you use this software
+ * in a product, an acknowledgment in the product documentation would be
+ * appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ * misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2EdgeShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+
+// Compute contact points for edge versus circle.
+// This accounts for edge connectivity.
+void b2CollideEdgeAndCircle(b2Manifold* manifold,
+							const b2EdgeShape* edgeA, const b2Transform& xfA,
+							const b2CircleShape* circleB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+	
+	// Compute circle in frame of edge
+	b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
+	
+	b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
+	b2Vec2 e = B - A;
+	
+	// Barycentric coordinates
+	float32 u = b2Dot(e, B - Q);
+	float32 v = b2Dot(e, Q - A);
+	
+	float32 radius = edgeA->m_radius + circleB->m_radius;
+	
+	b2ContactFeature cf;
+	cf.indexB = 0;
+	cf.typeB = b2ContactFeature::e_vertex;
+	
+	// Region A
+	if (v <= 0.0f)
+	{
+		b2Vec2 P = A;
+		b2Vec2 d = Q - P;
+		float32 dd = b2Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+		
+		// Is there an edge connected to A?
+		if (edgeA->m_hasVertex0)
+		{
+			b2Vec2 A1 = edgeA->m_vertex0;
+			b2Vec2 B1 = A;
+			b2Vec2 e1 = B1 - A1;
+			float32 u1 = b2Dot(e1, B1 - Q);
+			
+			// Is the circle in Region AB of the previous edge?
+			if (u1 > 0.0f)
+			{
+				return;
+			}
+		}
+		
+		cf.indexA = 0;
+		cf.typeA = b2ContactFeature::e_vertex;
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_circles;
+		manifold->localNormal.SetZero();
+		manifold->localPoint = P;
+		manifold->points[0].id.key = 0;
+		manifold->points[0].id.cf = cf;
+		manifold->points[0].localPoint = circleB->m_p;
+		return;
+	}
+	
+	// Region B
+	if (u <= 0.0f)
+	{
+		b2Vec2 P = B;
+		b2Vec2 d = Q - P;
+		float32 dd = b2Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+		
+		// Is there an edge connected to B?
+		if (edgeA->m_hasVertex3)
+		{
+			b2Vec2 B2 = edgeA->m_vertex3;
+			b2Vec2 A2 = B;
+			b2Vec2 e2 = B2 - A2;
+			float32 v2 = b2Dot(e2, Q - A2);
+			
+			// Is the circle in Region AB of the next edge?
+			if (v2 > 0.0f)
+			{
+				return;
+			}
+		}
+		
+		cf.indexA = 1;
+		cf.typeA = b2ContactFeature::e_vertex;
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_circles;
+		manifold->localNormal.SetZero();
+		manifold->localPoint = P;
+		manifold->points[0].id.key = 0;
+		manifold->points[0].id.cf = cf;
+		manifold->points[0].localPoint = circleB->m_p;
+		return;
+	}
+	
+	// Region AB
+	float32 den = b2Dot(e, e);
+	b2Assert(den > 0.0f);
+	b2Vec2 P = (1.0f / den) * (u * A + v * B);
+	b2Vec2 d = Q - P;
+	float32 dd = b2Dot(d, d);
+	if (dd > radius * radius)
+	{
+		return;
+	}
+	
+	b2Vec2 n(-e.y, e.x);
+	if (b2Dot(n, Q - A) < 0.0f)
+	{
+		n.Set(-n.x, -n.y);
+	}
+	n.Normalize();
+	
+	cf.indexA = 0;
+	cf.typeA = b2ContactFeature::e_face;
+	manifold->pointCount = 1;
+	manifold->type = b2Manifold::e_faceA;
+	manifold->localNormal = n;
+	manifold->localPoint = A;
+	manifold->points[0].id.key = 0;
+	manifold->points[0].id.cf = cf;
+	manifold->points[0].localPoint = circleB->m_p;
+}
+
+// This structure is used to keep track of the best separating axis.
+struct b2EPAxis
+{
+	enum Type
+	{
+		e_unknown,
+		e_edgeA,
+		e_edgeB
+	};
+	
+	Type type;
+	int32 index;
+	float32 separation;
+};
+
+// This holds polygon B expressed in frame A.
+struct b2TempPolygon
+{
+	b2Vec2 vertices[b2_maxPolygonVertices];
+	b2Vec2 normals[b2_maxPolygonVertices];
+	int32 count;
+};
+
+// Reference face used for clipping
+struct b2ReferenceFace
+{
+	int32 i1, i2;
+	
+	b2Vec2 v1, v2;
+	
+	b2Vec2 normal;
+	
+	b2Vec2 sideNormal1;
+	float32 sideOffset1;
+	
+	b2Vec2 sideNormal2;
+	float32 sideOffset2;
+};
+
+// This class collides and edge and a polygon, taking into account edge adjacency.
+struct b2EPCollider
+{
+	void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
+				 const b2PolygonShape* polygonB, const b2Transform& xfB);
+	b2EPAxis ComputeEdgeSeparation();
+	b2EPAxis ComputePolygonSeparation();
+	
+	enum VertexType
+	{
+		e_isolated,
+		e_concave,
+		e_convex
+	};
+	
+	b2TempPolygon m_polygonB;
+	
+	b2Transform m_xf;
+	b2Vec2 m_centroidB;
+	b2Vec2 m_v0, m_v1, m_v2, m_v3;
+	b2Vec2 m_normal0, m_normal1, m_normal2;
+	b2Vec2 m_normal;
+	VertexType m_type1, m_type2;
+	b2Vec2 m_lowerLimit, m_upperLimit;
+	float32 m_radius;
+	bool m_front;
+};
+
+// Algorithm:
+// 1. Classify v1 and v2
+// 2. Classify polygon centroid as front or back
+// 3. Flip normal if necessary
+// 4. Initialize normal range to [-pi, pi] about face normal
+// 5. Adjust normal range according to adjacent edges
+// 6. Visit each separating axes, only accept axes within the range
+// 7. Return if _any_ axis indicates separation
+// 8. Clip
+void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
+						   const b2PolygonShape* polygonB, const b2Transform& xfB)
+{
+	m_xf = b2MulT(xfA, xfB);
+	
+	m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
+	
+	m_v0 = edgeA->m_vertex0;
+	m_v1 = edgeA->m_vertex1;
+	m_v2 = edgeA->m_vertex2;
+	m_v3 = edgeA->m_vertex3;
+	
+	bool hasVertex0 = edgeA->m_hasVertex0;
+	bool hasVertex3 = edgeA->m_hasVertex3;
+	
+	b2Vec2 edge1 = m_v2 - m_v1;
+	edge1.Normalize();
+	m_normal1.Set(edge1.y, -edge1.x);
+	float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
+	float32 offset0 = 0.0f, offset2 = 0.0f;
+	bool convex1 = false, convex2 = false;
+	
+	// Is there a preceding edge?
+	if (hasVertex0)
+	{
+		b2Vec2 edge0 = m_v1 - m_v0;
+		edge0.Normalize();
+		m_normal0.Set(edge0.y, -edge0.x);
+		convex1 = b2Cross(edge0, edge1) >= 0.0f;
+		offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
+	}
+	
+	// Is there a following edge?
+	if (hasVertex3)
+	{
+		b2Vec2 edge2 = m_v3 - m_v2;
+		edge2.Normalize();
+		m_normal2.Set(edge2.y, -edge2.x);
+		convex2 = b2Cross(edge1, edge2) > 0.0f;
+		offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
+	}
+	
+	// Determine front or back collision. Determine collision normal limits.
+	if (hasVertex0 && hasVertex3)
+	{
+		if (convex1 && convex2)
+		{
+			m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else if (convex1)
+		{
+			m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else if (convex2)
+		{
+			m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = -m_normal0;
+			}
+		}
+		else
+		{
+			m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = -m_normal0;
+			}
+		}
+	}
+	else if (hasVertex0)
+	{
+		if (convex1)
+		{
+			m_front = offset0 >= 0.0f || offset1 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = -m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else
+		{
+			m_front = offset0 >= 0.0f && offset1 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal0;
+			}
+		}
+	}
+	else if (hasVertex3)
+	{
+		if (convex2)
+		{
+			m_front = offset1 >= 0.0f || offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal1;
+			}
+		}
+		else
+		{
+			m_front = offset1 >= 0.0f && offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = m_normal1;
+			}
+		}		
+	}
+	else
+	{
+		m_front = offset1 >= 0.0f;
+		if (m_front)
+		{
+			m_normal = m_normal1;
+			m_lowerLimit = -m_normal1;
+			m_upperLimit = -m_normal1;
+		}
+		else
+		{
+			m_normal = -m_normal1;
+			m_lowerLimit = m_normal1;
+			m_upperLimit = m_normal1;
+		}
+	}
+	
+	// Get polygonB in frameA
+	m_polygonB.count = polygonB->m_count;
+	for (int32 i = 0; i < polygonB->m_count; ++i)
+	{
+		m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
+		m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
+	}
+	
+	m_radius = polygonB->m_radius + edgeA->m_radius;
+	
+	manifold->pointCount = 0;
+	
+	b2EPAxis edgeAxis = ComputeEdgeSeparation();
+	
+	// If no valid normal can be found than this edge should not collide.
+	if (edgeAxis.type == b2EPAxis::e_unknown)
+	{
+		return;
+	}
+	
+	if (edgeAxis.separation > m_radius)
+	{
+		return;
+	}
+	
+	b2EPAxis polygonAxis = ComputePolygonSeparation();
+	if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
+	{
+		return;
+	}
+	
+	// Use hysteresis for jitter reduction.
+	const float32 k_relativeTol = 0.98f;
+	const float32 k_absoluteTol = 0.001f;
+	
+	b2EPAxis primaryAxis;
+	if (polygonAxis.type == b2EPAxis::e_unknown)
+	{
+		primaryAxis = edgeAxis;
+	}
+	else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
+	{
+		primaryAxis = polygonAxis;
+	}
+	else
+	{
+		primaryAxis = edgeAxis;
+	}
+	
+	b2ClipVertex ie[2];
+	b2ReferenceFace rf;
+	if (primaryAxis.type == b2EPAxis::e_edgeA)
+	{
+		manifold->type = b2Manifold::e_faceA;
+		
+		// Search for the polygon normal that is most anti-parallel to the edge normal.
+		int32 bestIndex = 0;
+		float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
+		for (int32 i = 1; i < m_polygonB.count; ++i)
+		{
+			float32 value = b2Dot(m_normal, m_polygonB.normals[i]);
+			if (value < bestValue)
+			{
+				bestValue = value;
+				bestIndex = i;
+			}
+		}
+		
+		int32 i1 = bestIndex;
+		int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
+		
+		ie[0].v = m_polygonB.vertices[i1];
+		ie[0].id.cf.indexA = 0;
+		ie[0].id.cf.indexB = static_cast<uint8>(i1);
+		ie[0].id.cf.typeA = b2ContactFeature::e_face;
+		ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
+		
+		ie[1].v = m_polygonB.vertices[i2];
+		ie[1].id.cf.indexA = 0;
+		ie[1].id.cf.indexB = static_cast<uint8>(i2);
+		ie[1].id.cf.typeA = b2ContactFeature::e_face;
+		ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
+		
+		if (m_front)
+		{
+			rf.i1 = 0;
+			rf.i2 = 1;
+			rf.v1 = m_v1;
+			rf.v2 = m_v2;
+			rf.normal = m_normal1;
+		}
+		else
+		{
+			rf.i1 = 1;
+			rf.i2 = 0;
+			rf.v1 = m_v2;
+			rf.v2 = m_v1;
+			rf.normal = -m_normal1;
+		}		
+	}
+	else
+	{
+		manifold->type = b2Manifold::e_faceB;
+		
+		ie[0].v = m_v1;
+		ie[0].id.cf.indexA = 0;
+		ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
+		ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
+		ie[0].id.cf.typeB = b2ContactFeature::e_face;
+		
+		ie[1].v = m_v2;
+		ie[1].id.cf.indexA = 0;
+		ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);		
+		ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
+		ie[1].id.cf.typeB = b2ContactFeature::e_face;
+		
+		rf.i1 = primaryAxis.index;
+		rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
+		rf.v1 = m_polygonB.vertices[rf.i1];
+		rf.v2 = m_polygonB.vertices[rf.i2];
+		rf.normal = m_polygonB.normals[rf.i1];
+	}
+	
+	rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
+	rf.sideNormal2 = -rf.sideNormal1;
+	rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
+	rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
+	
+	// Clip incident edge against extruded edge1 side edges.
+	b2ClipVertex clipPoints1[2];
+	b2ClipVertex clipPoints2[2];
+	int32 np;
+	
+	// Clip to box side 1
+	np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
+	
+	if (np < b2_maxManifoldPoints)
+	{
+		return;
+	}
+	
+	// Clip to negative box side 1
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
+	
+	if (np < b2_maxManifoldPoints)
+	{
+		return;
+	}
+	
+	// Now clipPoints2 contains the clipped points.
+	if (primaryAxis.type == b2EPAxis::e_edgeA)
+	{
+		manifold->localNormal = rf.normal;
+		manifold->localPoint = rf.v1;
+	}
+	else
+	{
+		manifold->localNormal = polygonB->m_normals[rf.i1];
+		manifold->localPoint = polygonB->m_vertices[rf.i1];
+	}
+	
+	int32 pointCount = 0;
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		float32 separation;
+		
+		separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
+		
+		if (separation <= m_radius)
+		{
+			b2ManifoldPoint* cp = manifold->points + pointCount;
+			
+			if (primaryAxis.type == b2EPAxis::e_edgeA)
+			{
+				cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
+				cp->id = clipPoints2[i].id;
+			}
+			else
+			{
+				cp->localPoint = clipPoints2[i].v;
+				cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
+				cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
+				cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
+				cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
+			}
+			
+			++pointCount;
+		}
+	}
+	
+	manifold->pointCount = pointCount;
+}
+
+b2EPAxis b2EPCollider::ComputeEdgeSeparation()
+{
+	b2EPAxis axis;
+	axis.type = b2EPAxis::e_edgeA;
+	axis.index = m_front ? 0 : 1;
+	axis.separation = FLT_MAX;
+	
+	for (int32 i = 0; i < m_polygonB.count; ++i)
+	{
+		float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
+		if (s < axis.separation)
+		{
+			axis.separation = s;
+		}
+	}
+	
+	return axis;
+}
+
+b2EPAxis b2EPCollider::ComputePolygonSeparation()
+{
+	b2EPAxis axis;
+	axis.type = b2EPAxis::e_unknown;
+	axis.index = -1;
+	axis.separation = -FLT_MAX;
+
+	b2Vec2 perp(-m_normal.y, m_normal.x);
+
+	for (int32 i = 0; i < m_polygonB.count; ++i)
+	{
+		b2Vec2 n = -m_polygonB.normals[i];
+		
+		float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);
+		float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);
+		float32 s = b2Min(s1, s2);
+		
+		if (s > m_radius)
+		{
+			// No collision
+			axis.type = b2EPAxis::e_edgeB;
+			axis.index = i;
+			axis.separation = s;
+			return axis;
+		}
+		
+		// Adjacency
+		if (b2Dot(n, perp) >= 0.0f)
+		{
+			if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop)
+			{
+				continue;
+			}
+		}
+		else
+		{
+			if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop)
+			{
+				continue;
+			}
+		}
+		
+		if (s > axis.separation)
+		{
+			axis.type = b2EPAxis::e_edgeB;
+			axis.index = i;
+			axis.separation = s;
+		}
+	}
+	
+	return axis;
+}
+
+void b2CollideEdgeAndPolygon(	b2Manifold* manifold,
+							 const b2EdgeShape* edgeA, const b2Transform& xfA,
+							 const b2PolygonShape* polygonB, const b2Transform& xfB)
+{
+	b2EPCollider collider;
+	collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2CollidePolygon.cpp

+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+// 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_count;
+	int32 count2 = poly2->m_count;
+	const b2Vec2* n1s = poly1->m_normals;
+	const b2Vec2* v1s = poly1->m_vertices;
+	const b2Vec2* v2s = poly2->m_vertices;
+	b2Transform xf = b2MulT(xf2, xf1);
+
+	int32 bestIndex = 0;
+	float32 maxSeparation = -b2_maxFloat;
+	for (int32 i = 0; i < count1; ++i)
+	{
+		// Get poly1 normal in frame2.
+		b2Vec2 n = b2Mul(xf.q, n1s[i]);
+		b2Vec2 v1 = b2Mul(xf, v1s[i]);
+
+		// Find deepest point for normal i.
+		float32 si = b2_maxFloat;
+		for (int32 j = 0; j < count2; ++j)
+		{
+			float32 sij = b2Dot(n, v2s[j] - v1);
+			if (sij < si)
+			{
+				si = sij;
+			}
+		}
+
+		if (si > maxSeparation)
+		{
+			maxSeparation = si;
+			bestIndex = i;
+		}
+	}
+
+	*edgeIndex = bestIndex;
+	return maxSeparation;
+}
+
+static void b2FindIncidentEdge(b2ClipVertex c[2],
+							 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
+							 const b2PolygonShape* poly2, const b2Transform& xf2)
+{
+	const b2Vec2* normals1 = poly1->m_normals;
+
+	int32 count2 = poly2->m_count;
+	const b2Vec2* vertices2 = poly2->m_vertices;
+	const b2Vec2* normals2 = poly2->m_normals;
+
+	b2Assert(0 <= edge1 && edge1 < poly1->m_count);
+
+	// Get the normal of the reference edge in poly2's frame.
+	b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, 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.cf.indexA = (uint8)edge1;
+	c[0].id.cf.indexB = (uint8)i1;
+	c[0].id.cf.typeA = b2ContactFeature::e_face;
+	c[0].id.cf.typeB = b2ContactFeature::e_vertex;
+
+	c[1].v = b2Mul(xf2, vertices2[i2]);
+	c[1].id.cf.indexA = (uint8)edge1;
+	c[1].id.cf.indexB = (uint8)i2;
+	c[1].id.cf.typeA = b2ContactFeature::e_face;
+	c[1].id.cf.typeB = b2ContactFeature::e_vertex;
+}
+
+// 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_tol = 0.1f * b2_linearSlop;
+
+	if (separationB > separationA + k_tol)
+	{
+		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_count;
+	const b2Vec2* vertices1 = poly1->m_vertices;
+
+	int32 iv1 = edge1;
+	int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
+
+	b2Vec2 v11 = vertices1[iv1];
+	b2Vec2 v12 = vertices1[iv2];
+
+	b2Vec2 localTangent = v12 - v11;
+	localTangent.Normalize();
+	
+	b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
+	b2Vec2 planePoint = 0.5f * (v11 + v12);
+
+	b2Vec2 tangent = b2Mul(xf1.q, 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, iv1);
+
+	if (np < 2)
+		return;
+
+	// Clip to negative box side 1
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2, iv2);
+
+	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;
+			if (flip)
+			{
+				// Swap features
+				b2ContactFeature cf = cp->id.cf;
+				cp->id.cf.indexA = cf.indexB;
+				cp->id.cf.indexB = cf.indexA;
+				cp->id.cf.typeA = cf.typeB;
+				cp->id.cf.typeB = cf.typeA;
+			}
+			++pointCount;
+		}
+	}
+
+	manifold->pointCount = pointCount;
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2Collision.cpp

+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/b2Distance.h"
+
+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);
+			separations[0] = b2Dot(cB - cA, normal);
+		}
+		break;
+
+	case b2Manifold::e_faceA:
+		{
+			normal = b2Mul(xfA.q, 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);
+				separations[i] = b2Dot(cB - cA, normal);
+			}
+		}
+		break;
+
+	case b2Manifold::e_faceB:
+		{
+			normal = b2Mul(xfB.q, 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);
+				separations[i] = b2Dot(cA - cB, normal);
+			}
+
+			// 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, int32 vertexIndexA)
+{
+	// 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);
+
+		// VertexA is hitting edgeB.
+		vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);
+		vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
+		vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
+		vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;
+		++numOut;
+	}
+
+	return numOut;
+}
+
+bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
+					const b2Shape* shapeB, int32 indexB,
+					const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2DistanceInput input;
+	input.proxyA.Set(shapeA, indexA);
+	input.proxyB.Set(shapeB, indexB);
+	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;
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2Collision.h

+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#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 b2EdgeShape;
+class b2PolygonShape;
+
+const uint8 b2_nullFeature = UCHAR_MAX;
+
+/// The features that intersect to form the contact point
+/// This must be 4 bytes or less.
+struct b2ContactFeature
+{
+	enum Type
+	{
+		e_vertex = 0,
+		e_face = 1
+	};
+
+	uint8 indexA;		///< Feature index on shapeA
+	uint8 indexB;		///< Feature index on shapeB
+	uint8 typeA;		///< The feature type on shapeA
+	uint8 typeB;		///< The feature type on shapeB
+};
+
+/// Contact ids to facilitate warm starting.
+union b2ContactID
+{
+	b2ContactFeature cf;
+	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)
+	float32 separations[b2_maxManifoldPoints];	///< a negative value indicates overlap, in meters
+};
+
+/// 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);
+	}
+
+	/// Get the perimeter length
+	float32 GetPerimeter() const
+	{
+		float32 wx = upperBound.x - lowerBound.x;
+		float32 wy = upperBound.y - lowerBound.y;
+		return 2.0f * (wx + wy);
+	}
+
+	/// Combine an AABB into this one.
+	void Combine(const b2AABB& aabb)
+	{
+		lowerBound = b2Min(lowerBound, aabb.lowerBound);
+		upperBound = b2Max(upperBound, aabb.upperBound);
+	}
+
+	/// 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* circleA, const b2Transform& xfA,
+					  const b2CircleShape* circleB, const b2Transform& xfB);
+
+/// Compute the collision manifold between a polygon and a circle.
+void b2CollidePolygonAndCircle(b2Manifold* manifold,
+							   const b2PolygonShape* polygonA, const b2Transform& xfA,
+							   const b2CircleShape* circleB, const b2Transform& xfB);
+
+/// Compute the collision manifold between two polygons.
+void b2CollidePolygons(b2Manifold* manifold,
+					   const b2PolygonShape* polygonA, const b2Transform& xfA,
+					   const b2PolygonShape* polygonB, const b2Transform& xfB);
+
+/// Compute the collision manifold between an edge and a circle.
+void b2CollideEdgeAndCircle(b2Manifold* manifold,
+							   const b2EdgeShape* polygonA, const b2Transform& xfA,
+							   const b2CircleShape* circleB, const b2Transform& xfB);
+
+/// Compute the collision manifold between an edge and a circle.
+void b2CollideEdgeAndPolygon(b2Manifold* manifold,
+							   const b2EdgeShape* edgeA, const b2Transform& xfA,
+							   const b2PolygonShape* circleB, const b2Transform& xfB);
+
+/// Clipping for contact manifolds.
+int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
+							const b2Vec2& normal, float32 offset, int32 vertexIndexA);
+
+/// Determine if two generic shapes overlap.
+bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
+					const b2Shape* shapeB, int32 indexB,
+					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

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2Distance.cpp

+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Distance.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2EdgeShape.h"
+#include "Box2D/Collision/Shapes/b2ChainShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
+int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
+
+void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
+{
+	switch (shape->GetType())
+	{
+	case b2Shape::e_circle:
+		{
+			const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
+			m_vertices = &circle->m_p;
+			m_count = 1;
+			m_radius = circle->m_radius;
+		}
+		break;
+
+	case b2Shape::e_polygon:
+		{
+			const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
+			m_vertices = polygon->m_vertices;
+			m_count = polygon->m_count;
+			m_radius = polygon->m_radius;
+		}
+		break;
+
+	case b2Shape::e_chain:
+		{
+			const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
+			b2Assert(0 <= index && index < chain->m_count);
+
+			m_buffer[0] = chain->m_vertices[index];
+			if (index + 1 < chain->m_count)
+			{
+				m_buffer[1] = chain->m_vertices[index + 1];
+			}
+			else
+			{
+				m_buffer[1] = chain->m_vertices[0];
+			}
+
+			m_vertices = m_buffer;
+			m_count = 2;
+			m_radius = chain->m_radius;
+		}
+		break;
+
+	case b2Shape::e_edge:
+		{
+			const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
+			m_vertices = &edge->m_vertex1;
+			m_count = 2;
+			m_radius = edge->m_radius;
+		}
+		break;
+
+	default:
+		b2Assert(false);
+	}
+}
+
+
+struct b2SimplexVertex
+{
+	b2Vec2 wA;		// support point in proxyA
+	b2Vec2 wB;		// support point in proxyB
+	b2Vec2 w;		// wB - wA
+	float32 a;		// barycentric coordinate for closest point
+	int32 indexA;	// wA index
+	int32 indexB;	// wB index
+};
+
+struct b2Simplex
+{
+	void ReadCache(	const b2SimplexCache* cache,
+					const b2DistanceProxy* proxyA, const b2Transform& transformA,
+					const b2DistanceProxy* proxyB, const b2Transform& transformB)
+	{
+		b2Assert(cache->count <= 3);
+		
+		// Copy data from cache.
+		m_count = cache->count;
+		b2SimplexVertex* vertices = &m_v1;
+		for (int32 i = 0; i < m_count; ++i)
+		{
+			b2SimplexVertex* v = vertices + i;
+			v->indexA = cache->indexA[i];
+			v->indexB = cache->indexB[i];
+			b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
+			b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
+			v->wA = b2Mul(transformA, wALocal);
+			v->wB = b2Mul(transformB, wBLocal);
+			v->w = v->wB - v->wA;
+			v->a = 0.0f;
+		}
+
+		// Compute the new simplex metric, if it is substantially different than
+		// old metric then flush the simplex.
+		if (m_count > 1)
+		{
+			float32 metric1 = cache->metric;
+			float32 metric2 = GetMetric();
+			if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
+			{
+				// Reset the simplex.
+				m_count = 0;
+			}
+		}
+
+		// If the cache is empty or invalid ...
+		if (m_count == 0)
+		{
+			b2SimplexVertex* v = vertices + 0;
+			v->indexA = 0;
+			v->indexB = 0;
+			b2Vec2 wALocal = proxyA->GetVertex(0);
+			b2Vec2 wBLocal = proxyB->GetVertex(0);
+			v->wA = b2Mul(transformA, wALocal);
+			v->wB = b2Mul(transformB, wBLocal);
+			v->w = v->wB - v->wA;
+			v->a = 1.0f;
+			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.0f;
+
+		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;
+
+	// 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;
+		}
+
+		// Get search direction.
+		b2Vec2 d = simplex.GetSearchDirection();
+
+		// Ensure the search direction is numerically fit.
+		if (d.LengthSquared() < b2_epsilon * b2_epsilon)
+		{
+			// The origin is probably contained by a line segment
+			// or triangle. Thus the shapes are overlapped.
+
+			// We can't return zero here even though there may be overlap.
+			// In case the simplex is a point, segment, or triangle it is difficult
+			// to determine if the origin is contained in the CSO or very close to it.
+			break;
+		}
+
+		// Compute a tentative new simplex vertex using support points.
+		b2SimplexVertex* vertex = vertices + simplex.m_count;
+		vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
+		vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
+		b2Vec2 wBLocal;
+		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
+		vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
+		vertex->w = vertex->wB - vertex->wA;
+
+		// Iteration count is equated to the number of support point calls.
+		++iter;
+		++b2_gjkIters;
+
+		// Check for duplicate support points. This is the main termination criteria.
+		bool duplicate = false;
+		for (int32 i = 0; i < saveCount; ++i)
+		{
+			if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
+			{
+				duplicate = true;
+				break;
+			}
+		}
+
+		// If we found a duplicate support point we must exit to avoid cycling.
+		if (duplicate)
+		{
+			break;
+		}
+
+		// New vertex is ok and needed.
+		++simplex.m_count;
+	}
+
+	b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
+
+	// Prepare output.
+	simplex.GetWitnessPoints(&output->pointA, &output->pointB);
+	output->distance = b2Distance(output->pointA, output->pointB);
+	output->iterations = iter;
+
+	// Cache the simplex.
+	simplex.WriteCache(cache);
+
+	// Apply radii if requested.
+	if (input->useRadii)
+	{
+		float32 rA = proxyA->m_radius;
+		float32 rB = proxyB->m_radius;
+
+		if (output->distance > rA + rB && output->distance > b2_epsilon)
+		{
+			// Shapes are still no overlapped.
+			// Move the witness points to the outer surface.
+			output->distance -= rA + rB;
+			b2Vec2 normal = output->pointB - output->pointA;
+			normal.Normalize();
+			output->pointA += rA * normal;
+			output->pointB -= rB * normal;
+		}
+		else
+		{
+			// Shapes are overlapped when radii are considered.
+			// Move the witness points to the middle.
+			b2Vec2 p = 0.5f * (output->pointA + output->pointB);
+			output->pointA = p;
+			output->pointB = p;
+			output->distance = 0.0f;
+		}
+	}
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2Distance.h

+
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_DISTANCE_H
+#define B2_DISTANCE_H
+
+#include "Box2D/Common/b2Math.h"
+
+class b2Shape;
+
+/// A distance proxy is used by the GJK algorithm.
+/// It encapsulates any shape.
+struct b2DistanceProxy
+{
+	b2DistanceProxy() : m_vertices(nullptr), 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, int32 index);
+
+	/// 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;
+
+	b2Vec2 m_buffer[2];
+	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

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2DynamicTree.cpp

+/*
+* Copyright (c) 2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2DynamicTree.h"
+#include <string.h>
+
+b2DynamicTree::b2DynamicTree()
+{
+	m_root = b2_nullNode;
+
+	m_nodeCapacity = 16;
+	m_nodeCount = 0;
+	m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
+	memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
+
+	// 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[i].height = -1;
+	}
+	m_nodes[m_nodeCapacity-1].next = b2_nullNode;
+	m_nodes[m_nodeCapacity-1].height = -1;
+	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.
+		b2TreeNode* oldNodes = m_nodes;
+		m_nodeCapacity *= 2;
+		m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
+		memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
+		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[i].height = -1;
+		}
+		m_nodes[m_nodeCapacity-1].next = b2_nullNode;
+		m_nodes[m_nodeCapacity-1].height = -1;
+		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_nodes[nodeId].height = 0;
+	m_nodes[nodeId].userData = nullptr;
+	++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_nodes[nodeId].height = -1;
+	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;
+	m_nodes[proxyId].height = 0;
+
+	InsertLeaf(proxyId);
+
+	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
+	b2AABB leafAABB = m_nodes[leaf].aabb;
+	int32 index = m_root;
+	while (m_nodes[index].IsLeaf() == false)
+	{
+		int32 child1 = m_nodes[index].child1;
+		int32 child2 = m_nodes[index].child2;
+
+		float32 area = m_nodes[index].aabb.GetPerimeter();
+
+		b2AABB combinedAABB;
+		combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
+		float32 combinedArea = combinedAABB.GetPerimeter();
+
+		// Cost of creating a new parent for this node and the new leaf
+		float32 cost = 2.0f * combinedArea;
+
+		// Minimum cost of pushing the leaf further down the tree
+		float32 inheritanceCost = 2.0f * (combinedArea - area);
+
+		// Cost of descending into child1
+		float32 cost1;
+		if (m_nodes[child1].IsLeaf())
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child1].aabb);
+			cost1 = aabb.GetPerimeter() + inheritanceCost;
+		}
+		else
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child1].aabb);
+			float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
+			float32 newArea = aabb.GetPerimeter();
+			cost1 = (newArea - oldArea) + inheritanceCost;
+		}
+
+		// Cost of descending into child2
+		float32 cost2;
+		if (m_nodes[child2].IsLeaf())
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child2].aabb);
+			cost2 = aabb.GetPerimeter() + inheritanceCost;
+		}
+		else
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child2].aabb);
+			float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
+			float32 newArea = aabb.GetPerimeter();
+			cost2 = newArea - oldArea + inheritanceCost;
+		}
+
+		// Descend according to the minimum cost.
+		if (cost < cost1 && cost < cost2)
+		{
+			break;
+		}
+
+		// Descend
+		if (cost1 < cost2)
+		{
+			index = child1;
+		}
+		else
+		{
+			index = child2;
+		}
+	}
+
+	int32 sibling = index;
+
+	// Create a new parent.
+	int32 oldParent = m_nodes[sibling].parent;
+	int32 newParent = AllocateNode();
+	m_nodes[newParent].parent = oldParent;
+	m_nodes[newParent].userData = nullptr;
+	m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
+	m_nodes[newParent].height = m_nodes[sibling].height + 1;
+
+	if (oldParent != b2_nullNode)
+	{
+		// The sibling was not the root.
+		if (m_nodes[oldParent].child1 == sibling)
+		{
+			m_nodes[oldParent].child1 = newParent;
+		}
+		else
+		{
+			m_nodes[oldParent].child2 = newParent;
+		}
+
+		m_nodes[newParent].child1 = sibling;
+		m_nodes[newParent].child2 = leaf;
+		m_nodes[sibling].parent = newParent;
+		m_nodes[leaf].parent = newParent;
+	}
+	else
+	{
+		// The sibling was the root.
+		m_nodes[newParent].child1 = sibling;
+		m_nodes[newParent].child2 = leaf;
+		m_nodes[sibling].parent = newParent;
+		m_nodes[leaf].parent = newParent;
+		m_root = newParent;
+	}
+
+	// Walk back up the tree fixing heights and AABBs
+	index = m_nodes[leaf].parent;
+	while (index != b2_nullNode)
+	{
+		index = Balance(index);
+
+		int32 child1 = m_nodes[index].child1;
+		int32 child2 = m_nodes[index].child2;
+
+		b2Assert(child1 != b2_nullNode);
+		b2Assert(child2 != b2_nullNode);
+
+		m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
+		m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+
+		index = m_nodes[index].parent;
+	}
+
+	//Validate();
+}
+
+void b2DynamicTree::RemoveLeaf(int32 leaf)
+{
+	if (leaf == m_root)
+	{
+		m_root = b2_nullNode;
+		return;
+	}
+
+	int32 parent = m_nodes[leaf].parent;
+	int32 grandParent = m_nodes[parent].parent;
+	int32 sibling;
+	if (m_nodes[parent].child1 == leaf)
+	{
+		sibling = m_nodes[parent].child2;
+	}
+	else
+	{
+		sibling = m_nodes[parent].child1;
+	}
+
+	if (grandParent != b2_nullNode)
+	{
+		// Destroy parent and connect sibling to grandParent.
+		if (m_nodes[grandParent].child1 == parent)
+		{
+			m_nodes[grandParent].child1 = sibling;
+		}
+		else
+		{
+			m_nodes[grandParent].child2 = sibling;
+		}
+		m_nodes[sibling].parent = grandParent;
+		FreeNode(parent);
+
+		// Adjust ancestor bounds.
+		int32 index = grandParent;
+		while (index != b2_nullNode)
+		{
+			index = Balance(index);
+
+			int32 child1 = m_nodes[index].child1;
+			int32 child2 = m_nodes[index].child2;
+
+			m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+			m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
+
+			index = m_nodes[index].parent;
+		}
+	}
+	else
+	{
+		m_root = sibling;
+		m_nodes[sibling].parent = b2_nullNode;
+		FreeNode(parent);
+	}
+
+	//Validate();
+}
+
+// Perform a left or right rotation if node A is imbalanced.
+// Returns the new root index.
+int32 b2DynamicTree::Balance(int32 iA)
+{
+	b2Assert(iA != b2_nullNode);
+
+	b2TreeNode* A = m_nodes + iA;
+	if (A->IsLeaf() || A->height < 2)
+	{
+		return iA;
+	}
+
+	int32 iB = A->child1;
+	int32 iC = A->child2;
+	b2Assert(0 <= iB && iB < m_nodeCapacity);
+	b2Assert(0 <= iC && iC < m_nodeCapacity);
+
+	b2TreeNode* B = m_nodes + iB;
+	b2TreeNode* C = m_nodes + iC;
+
+	int32 balance = C->height - B->height;
+
+	// Rotate C up
+	if (balance > 1)
+	{
+		int32 iF = C->child1;
+		int32 iG = C->child2;
+		b2TreeNode* F = m_nodes + iF;
+		b2TreeNode* G = m_nodes + iG;
+		b2Assert(0 <= iF && iF < m_nodeCapacity);
+		b2Assert(0 <= iG && iG < m_nodeCapacity);
+
+		// Swap A and C
+		C->child1 = iA;
+		C->parent = A->parent;
+		A->parent = iC;
+
+		// A's old parent should point to C
+		if (C->parent != b2_nullNode)
+		{
+			if (m_nodes[C->parent].child1 == iA)
+			{
+				m_nodes[C->parent].child1 = iC;
+			}
+			else
+			{
+				b2Assert(m_nodes[C->parent].child2 == iA);
+				m_nodes[C->parent].child2 = iC;
+			}
+		}
+		else
+		{
+			m_root = iC;
+		}
+
+		// Rotate
+		if (F->height > G->height)
+		{
+			C->child2 = iF;
+			A->child2 = iG;
+			G->parent = iA;
+			A->aabb.Combine(B->aabb, G->aabb);
+			C->aabb.Combine(A->aabb, F->aabb);
+
+			A->height = 1 + b2Max(B->height, G->height);
+			C->height = 1 + b2Max(A->height, F->height);
+		}
+		else
+		{
+			C->child2 = iG;
+			A->child2 = iF;
+			F->parent = iA;
+			A->aabb.Combine(B->aabb, F->aabb);
+			C->aabb.Combine(A->aabb, G->aabb);
+
+			A->height = 1 + b2Max(B->height, F->height);
+			C->height = 1 + b2Max(A->height, G->height);
+		}
+
+		return iC;
+	}
+	
+	// Rotate B up
+	if (balance < -1)
+	{
+		int32 iD = B->child1;
+		int32 iE = B->child2;
+		b2TreeNode* D = m_nodes + iD;
+		b2TreeNode* E = m_nodes + iE;
+		b2Assert(0 <= iD && iD < m_nodeCapacity);
+		b2Assert(0 <= iE && iE < m_nodeCapacity);
+
+		// Swap A and B
+		B->child1 = iA;
+		B->parent = A->parent;
+		A->parent = iB;
+
+		// A's old parent should point to B
+		if (B->parent != b2_nullNode)
+		{
+			if (m_nodes[B->parent].child1 == iA)
+			{
+				m_nodes[B->parent].child1 = iB;
+			}
+			else
+			{
+				b2Assert(m_nodes[B->parent].child2 == iA);
+				m_nodes[B->parent].child2 = iB;
+			}
+		}
+		else
+		{
+			m_root = iB;
+		}
+
+		// Rotate
+		if (D->height > E->height)
+		{
+			B->child2 = iD;
+			A->child1 = iE;
+			E->parent = iA;
+			A->aabb.Combine(C->aabb, E->aabb);
+			B->aabb.Combine(A->aabb, D->aabb);
+
+			A->height = 1 + b2Max(C->height, E->height);
+			B->height = 1 + b2Max(A->height, D->height);
+		}
+		else
+		{
+			B->child2 = iE;
+			A->child1 = iD;
+			D->parent = iA;
+			A->aabb.Combine(C->aabb, D->aabb);
+			B->aabb.Combine(A->aabb, E->aabb);
+
+			A->height = 1 + b2Max(C->height, D->height);
+			B->height = 1 + b2Max(A->height, E->height);
+		}
+
+		return iB;
+	}
+
+	return iA;
+}
+
+int32 b2DynamicTree::GetHeight() const
+{
+	if (m_root == b2_nullNode)
+	{
+		return 0;
+	}
+
+	return m_nodes[m_root].height;
+}
+
+//
+float32 b2DynamicTree::GetAreaRatio() const
+{
+	if (m_root == b2_nullNode)
+	{
+		return 0.0f;
+	}
+
+	const b2TreeNode* root = m_nodes + m_root;
+	float32 rootArea = root->aabb.GetPerimeter();
+
+	float32 totalArea = 0.0f;
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		const b2TreeNode* node = m_nodes + i;
+		if (node->height < 0)
+		{
+			// Free node in pool
+			continue;
+		}
+
+		totalArea += node->aabb.GetPerimeter();
+	}
+
+	return totalArea / rootArea;
+}
+
+// Compute the height of a sub-tree.
+int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
+{
+	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+	b2TreeNode* node = m_nodes + nodeId;
+
+	if (node->IsLeaf())
+	{
+		return 0;
+	}
+
+	int32 height1 = ComputeHeight(node->child1);
+	int32 height2 = ComputeHeight(node->child2);
+	return 1 + b2Max(height1, height2);
+}
+
+int32 b2DynamicTree::ComputeHeight() const
+{
+	int32 height = ComputeHeight(m_root);
+	return height;
+}
+
+void b2DynamicTree::ValidateStructure(int32 index) const
+{
+	if (index == b2_nullNode)
+	{
+		return;
+	}
+
+	if (index == m_root)
+	{
+		b2Assert(m_nodes[index].parent == b2_nullNode);
+	}
+
+	const b2TreeNode* node = m_nodes + index;
+
+	int32 child1 = node->child1;
+	int32 child2 = node->child2;
+
+	if (node->IsLeaf())
+	{
+		b2Assert(child1 == b2_nullNode);
+		b2Assert(child2 == b2_nullNode);
+		b2Assert(node->height == 0);
+		return;
+	}
+
+	b2Assert(0 <= child1 && child1 < m_nodeCapacity);
+	b2Assert(0 <= child2 && child2 < m_nodeCapacity);
+
+	b2Assert(m_nodes[child1].parent == index);
+	b2Assert(m_nodes[child2].parent == index);
+
+	ValidateStructure(child1);
+	ValidateStructure(child2);
+}
+
+void b2DynamicTree::ValidateMetrics(int32 index) const
+{
+	if (index == b2_nullNode)
+	{
+		return;
+	}
+
+	const b2TreeNode* node = m_nodes + index;
+
+	int32 child1 = node->child1;
+	int32 child2 = node->child2;
+
+	if (node->IsLeaf())
+	{
+		b2Assert(child1 == b2_nullNode);
+		b2Assert(child2 == b2_nullNode);
+		b2Assert(node->height == 0);
+		return;
+	}
+
+	b2Assert(0 <= child1 && child1 < m_nodeCapacity);
+	b2Assert(0 <= child2 && child2 < m_nodeCapacity);
+
+	int32 height1 = m_nodes[child1].height;
+	int32 height2 = m_nodes[child2].height;
+	int32 height;
+	height = 1 + b2Max(height1, height2);
+	b2Assert(node->height == height);
+
+	b2AABB aabb;
+	aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+
+	b2Assert(aabb.lowerBound == node->aabb.lowerBound);
+	b2Assert(aabb.upperBound == node->aabb.upperBound);
+
+	ValidateMetrics(child1);
+	ValidateMetrics(child2);
+}
+
+void b2DynamicTree::Validate() const
+{
+#if defined(b2DEBUG)
+	ValidateStructure(m_root);
+	ValidateMetrics(m_root);
+
+	int32 freeCount = 0;
+	int32 freeIndex = m_freeList;
+	while (freeIndex != b2_nullNode)
+	{
+		b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
+		freeIndex = m_nodes[freeIndex].next;
+		++freeCount;
+	}
+
+	b2Assert(GetHeight() == ComputeHeight());
+
+	b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
+#endif
+}
+
+int32 b2DynamicTree::GetMaxBalance() const
+{
+	int32 maxBalance = 0;
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		const b2TreeNode* node = m_nodes + i;
+		if (node->height <= 1)
+		{
+			continue;
+		}
+
+		b2Assert(node->IsLeaf() == false);
+
+		int32 child1 = node->child1;
+		int32 child2 = node->child2;
+		int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
+		maxBalance = b2Max(maxBalance, balance);
+	}
+
+	return maxBalance;
+}
+
+void b2DynamicTree::RebuildBottomUp()
+{
+	int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
+	int32 count = 0;
+
+	// Build array of leaves. Free the rest.
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		if (m_nodes[i].height < 0)
+		{
+			// free node in pool
+			continue;
+		}
+
+		if (m_nodes[i].IsLeaf())
+		{
+			m_nodes[i].parent = b2_nullNode;
+			nodes[count] = i;
+			++count;
+		}
+		else
+		{
+			FreeNode(i);
+		}
+	}
+
+	while (count > 1)
+	{
+		float32 minCost = b2_maxFloat;
+		int32 iMin = -1, jMin = -1;
+		for (int32 i = 0; i < count; ++i)
+		{
+			b2AABB aabbi = m_nodes[nodes[i]].aabb;
+
+			for (int32 j = i + 1; j < count; ++j)
+			{
+				b2AABB aabbj = m_nodes[nodes[j]].aabb;
+				b2AABB b;
+				b.Combine(aabbi, aabbj);
+				float32 cost = b.GetPerimeter();
+				if (cost < minCost)
+				{
+					iMin = i;
+					jMin = j;
+					minCost = cost;
+				}
+			}
+		}
+
+		int32 index1 = nodes[iMin];
+		int32 index2 = nodes[jMin];
+		b2TreeNode* child1 = m_nodes + index1;
+		b2TreeNode* child2 = m_nodes + index2;
+
+		int32 parentIndex = AllocateNode();
+		b2TreeNode* parent = m_nodes + parentIndex;
+		parent->child1 = index1;
+		parent->child2 = index2;
+		parent->height = 1 + b2Max(child1->height, child2->height);
+		parent->aabb.Combine(child1->aabb, child2->aabb);
+		parent->parent = b2_nullNode;
+
+		child1->parent = parentIndex;
+		child2->parent = parentIndex;
+
+		nodes[jMin] = nodes[count-1];
+		nodes[iMin] = parentIndex;
+		--count;
+	}
+
+	m_root = nodes[0];
+	b2Free(nodes);
+
+	Validate();
+}
+
+void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin)
+{
+	// Build array of leaves. Free the rest.
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		m_nodes[i].aabb.lowerBound -= newOrigin;
+		m_nodes[i].aabb.upperBound -= newOrigin;
+	}
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2DynamicTree.h

+/*
+* Copyright (c) 2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_DYNAMIC_TREE_H
+#define B2_DYNAMIC_TREE_H
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Common/b2GrowableStack.h"
+
+#define b2_nullNode (-1)
+
+/// A node in the dynamic tree. The client does not interact with this directly.
+struct b2TreeNode
+{
+	bool IsLeaf() const
+	{
+		return child1 == b2_nullNode;
+	}
+
+	/// Enlarged AABB
+	b2AABB aabb;
+
+	void* userData;
+
+	union
+	{
+		int32 parent;
+		int32 next;
+	};
+
+	int32 child1;
+	int32 child2;
+
+	// leaf = 0, free node = -1
+	int32 height;
+};
+
+/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
+/// 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);
+
+	/// 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;
+
+	/// 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;
+
+	/// Validate this tree. For testing.
+	void Validate() const;
+
+	/// Compute the height of the binary tree in O(N) time. Should not be
+	/// called often.
+	int32 GetHeight() const;
+
+	/// Get the maximum balance of an node in the tree. The balance is the difference
+	/// in height of the two children of a node.
+	int32 GetMaxBalance() const;
+
+	/// Get the ratio of the sum of the node areas to the root area.
+	float32 GetAreaRatio() const;
+
+	/// Build an optimal tree. Very expensive. For testing.
+	void RebuildBottomUp();
+
+	/// Shift the world origin. Useful for large worlds.
+	/// The shift formula is: position -= newOrigin
+	/// @param newOrigin the new origin with respect to the old origin
+	void ShiftOrigin(const b2Vec2& newOrigin);
+
+private:
+
+	int32 AllocateNode();
+	void FreeNode(int32 node);
+
+	void InsertLeaf(int32 node);
+	void RemoveLeaf(int32 node);
+
+	int32 Balance(int32 index);
+
+	int32 ComputeHeight() const;
+	int32 ComputeHeight(int32 nodeId) const;
+
+	void ValidateStructure(int32 index) const;
+	void ValidateMetrics(int32 index) const;
+
+	int32 m_root;
+
+	b2TreeNode* m_nodes;
+	int32 m_nodeCount;
+	int32 m_nodeCapacity;
+
+	int32 m_freeList;
+
+	/// This is used to 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
+{
+	b2GrowableStack<int32, 256> stack;
+	stack.Push(m_root);
+
+	while (stack.GetCount() > 0)
+	{
+		int32 nodeId = stack.Pop();
+		if (nodeId == b2_nullNode)
+		{
+			continue;
+		}
+
+		const b2TreeNode* node = m_nodes + nodeId;
+
+		if (b2TestOverlap(node->aabb, aabb))
+		{
+			if (node->IsLeaf())
+			{
+				bool proceed = callback->QueryCallback(nodeId);
+				if (proceed == false)
+				{
+					return;
+				}
+			}
+			else
+			{
+				stack.Push(node->child1);
+				stack.Push(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);
+	}
+
+	b2GrowableStack<int32, 256> stack;
+	stack.Push(m_root);
+
+	while (stack.GetCount() > 0)
+	{
+		int32 nodeId = stack.Pop();
+		if (nodeId == b2_nullNode)
+		{
+			continue;
+		}
+
+		const b2TreeNode* 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
+		{
+			stack.Push(node->child1);
+			stack.Push(node->child2);
+		}
+	}
+}
+
+#endif

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2TimeOfImpact.cpp

+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/b2Distance.h"
+#include "Box2D/Collision/b2TimeOfImpact.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+#include "Box2D/Common/b2Timer.h"
+
+#include <stdio.h>
+
+float32 b2_toiTime, b2_toiMaxTime;
+int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
+int32 b2_toiRootIters, b2_toiMaxRootIters;
+
+//
+struct b2SeparationFunction
+{
+	enum Type
+	{
+		e_points,
+		e_faceA,
+		e_faceB
+	};
+
+	// TODO_ERIN might not need to return the separation
+
+	float32 Initialize(const b2SimplexCache* cache,
+		const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
+		const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
+		float32 t1)
+	{
+		m_proxyA = proxyA;
+		m_proxyB = proxyB;
+		int32 count = cache->count;
+		b2Assert(0 < count && count < 3);
+
+		m_sweepA = sweepA;
+		m_sweepB = sweepB;
+
+		b2Transform xfA, xfB;
+		m_sweepA.GetTransform(&xfA, t1);
+		m_sweepB.GetTransform(&xfB, t1);
+
+		if (count == 1)
+		{
+			m_type = e_points;
+			b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
+			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
+			b2Vec2 pointA = b2Mul(xfA, localPointA);
+			b2Vec2 pointB = b2Mul(xfB, localPointB);
+			m_axis = pointB - pointA;
+			float32 s = m_axis.Normalize();
+			return s;
+		}
+		else if (cache->indexA[0] == cache->indexA[1])
+		{
+			// Two points on B and one on A.
+			m_type = e_faceB;
+			b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
+			b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
+
+			m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
+			m_axis.Normalize();
+			b2Vec2 normal = b2Mul(xfB.q, m_axis);
+
+			m_localPoint = 0.5f * (localPointB1 + localPointB2);
+			b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+			b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
+			b2Vec2 pointA = b2Mul(xfA, localPointA);
+
+			float32 s = b2Dot(pointA - pointB, normal);
+			if (s < 0.0f)
+			{
+				m_axis = -m_axis;
+				s = -s;
+			}
+			return s;
+		}
+		else
+		{
+			// Two points on A and one or two points on B.
+			m_type = e_faceA;
+			b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
+			b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
+			
+			m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
+			m_axis.Normalize();
+			b2Vec2 normal = b2Mul(xfA.q, m_axis);
+
+			m_localPoint = 0.5f * (localPointA1 + localPointA2);
+			b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
+			b2Vec2 pointB = b2Mul(xfB, localPointB);
+
+			float32 s = b2Dot(pointB - pointA, normal);
+			if (s < 0.0f)
+			{
+				m_axis = -m_axis;
+				s = -s;
+			}
+			return s;
+		}
+	}
+
+	//
+	float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
+	{
+		b2Transform xfA, xfB;
+		m_sweepA.GetTransform(&xfA, t);
+		m_sweepB.GetTransform(&xfB, t);
+
+		switch (m_type)
+		{
+		case e_points:
+			{
+				b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
+				b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
+
+				*indexA = m_proxyA->GetSupport(axisA);
+				*indexB = m_proxyB->GetSupport(axisB);
+
+				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
+				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
+				
+				b2Vec2 pointA = b2Mul(xfA, localPointA);
+				b2Vec2 pointB = b2Mul(xfB, localPointB);
+
+				float32 separation = b2Dot(pointB - pointA, m_axis);
+				return separation;
+			}
+
+		case e_faceA:
+			{
+				b2Vec2 normal = b2Mul(xfA.q, m_axis);
+				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+				b2Vec2 axisB = b2MulT(xfB.q, -normal);
+				
+				*indexA = -1;
+				*indexB = m_proxyB->GetSupport(axisB);
+
+				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
+				b2Vec2 pointB = b2Mul(xfB, localPointB);
+
+				float32 separation = b2Dot(pointB - pointA, normal);
+				return separation;
+			}
+
+		case e_faceB:
+			{
+				b2Vec2 normal = b2Mul(xfB.q, m_axis);
+				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+				b2Vec2 axisA = b2MulT(xfA.q, -normal);
+
+				*indexB = -1;
+				*indexA = m_proxyA->GetSupport(axisA);
+
+				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
+				b2Vec2 pointA = b2Mul(xfA, localPointA);
+
+				float32 separation = b2Dot(pointA - pointB, normal);
+				return separation;
+			}
+
+		default:
+			b2Assert(false);
+			*indexA = -1;
+			*indexB = -1;
+			return 0.0f;
+		}
+	}
+
+	//
+	float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
+	{
+		b2Transform xfA, xfB;
+		m_sweepA.GetTransform(&xfA, t);
+		m_sweepB.GetTransform(&xfB, t);
+
+		switch (m_type)
+		{
+		case e_points:
+			{
+				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
+				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
+
+				b2Vec2 pointA = b2Mul(xfA, localPointA);
+				b2Vec2 pointB = b2Mul(xfB, localPointB);
+				float32 separation = b2Dot(pointB - pointA, m_axis);
+
+				return separation;
+			}
+
+		case e_faceA:
+			{
+				b2Vec2 normal = b2Mul(xfA.q, m_axis);
+				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
+				b2Vec2 pointB = b2Mul(xfB, localPointB);
+
+				float32 separation = b2Dot(pointB - pointA, normal);
+				return separation;
+			}
+
+		case e_faceB:
+			{
+				b2Vec2 normal = b2Mul(xfB.q, m_axis);
+				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+				b2Vec2 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)
+{
+	b2Timer timer;
+
+	++b2_toiCalls;
+
+	output->state = b2TOIOutput::e_unknown;
+	output->t = input->tMax;
+
+	const b2DistanceProxy* proxyA = &input->proxyA;
+	const b2DistanceProxy* proxyB = &input->proxyB;
+
+	b2Sweep sweepA = input->sweepA;
+	b2Sweep sweepB = input->sweepB;
+
+	// Large rotations can make the root finder fail, so we normalize the
+	// sweep angles.
+	sweepA.Normalize();
+	sweepB.Normalize();
+
+	float32 tMax = input->tMax;
+
+	float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
+	float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
+	float32 tolerance = 0.25f * b2_linearSlop;
+	b2Assert(target > tolerance);
+
+	float32 t1 = 0.0f;
+	const int32 k_maxIterations = 20;	// TODO_ERIN b2Settings
+	int32 iter = 0;
+
+	// Prepare input for distance query.
+	b2SimplexCache cache;
+	cache.count = 0;
+	b2DistanceInput distanceInput;
+	distanceInput.proxyA = input->proxyA;
+	distanceInput.proxyB = input->proxyB;
+	distanceInput.useRadii = false;
+
+	// The outer loop progressively attempts to compute new separating axes.
+	// This loop terminates when an axis is repeated (no progress is made).
+	for(;;)
+	{
+		b2Transform xfA, xfB;
+		sweepA.GetTransform(&xfA, t1);
+		sweepB.GetTransform(&xfB, t1);
+
+		// Get the distance between shapes. We can also use the results
+		// to get a separating axis.
+		distanceInput.transformA = xfA;
+		distanceInput.transformB = xfB;
+		b2DistanceOutput distanceOutput;
+		b2Distance(&distanceOutput, &cache, &distanceInput);
+
+		// If the shapes are overlapped, we give up on continuous collision.
+		if (distanceOutput.distance <= 0.0f)
+		{
+			// Failure!
+			output->state = b2TOIOutput::e_overlapped;
+			output->t = 0.0f;
+			break;
+		}
+
+		if (distanceOutput.distance < target + tolerance)
+		{
+			// Victory!
+			output->state = b2TOIOutput::e_touching;
+			output->t = t1;
+			break;
+		}
+
+		// Initialize the separating axis.
+		b2SeparationFunction fcn;
+		fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
+#if 0
+		// Dump the curve seen by the root finder
+		{
+			const int32 N = 100;
+			float32 dx = 1.0f / N;
+			float32 xs[N+1];
+			float32 fs[N+1];
+
+			float32 x = 0.0f;
+
+			for (int32 i = 0; i <= N; ++i)
+			{
+				sweepA.GetTransform(&xfA, x);
+				sweepB.GetTransform(&xfB, x);
+				float32 f = fcn.Evaluate(xfA, xfB) - target;
+
+				printf("%g %g\n", x, f);
+
+				xs[i] = x;
+				fs[i] = f;
+
+				x += dx;
+			}
+		}
+#endif
+
+		// Compute the TOI on the separating axis. We do this by successively
+		// resolving the deepest point. This loop is bounded by the number of vertices.
+		bool done = false;
+		float32 t2 = tMax;
+		int32 pushBackIter = 0;
+		for (;;)
+		{
+			// Find the deepest point at t2. Store the witness point indices.
+			int32 indexA, indexB;
+			float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
+
+			// Is the final configuration separated?
+			if (s2 > target + tolerance)
+			{
+				// Victory!
+				output->state = b2TOIOutput::e_separated;
+				output->t = tMax;
+				done = true;
+				break;
+			}
+
+			// Has the separation reached tolerance?
+			if (s2 > target - tolerance)
+			{
+				// Advance the sweeps
+				t1 = t2;
+				break;
+			}
+
+			// Compute the initial separation of the witness points.
+			float32 s1 = fcn.Evaluate(indexA, indexB, t1);
+
+			// Check for initial overlap. This might happen if the root finder
+			// runs out of iterations.
+			if (s1 < target - tolerance)
+			{
+				output->state = b2TOIOutput::e_failed;
+				output->t = t1;
+				done = true;
+				break;
+			}
+
+			// Check for touching
+			if (s1 <= target + tolerance)
+			{
+				// Victory! t1 should hold the TOI (could be 0.0).
+				output->state = b2TOIOutput::e_touching;
+				output->t = t1;
+				done = true;
+				break;
+			}
+
+			// Compute 1D root of: f(x) - target = 0
+			int32 rootIterCount = 0;
+			float32 a1 = t1, a2 = t2;
+			for (;;)
+			{
+				// Use a mix of the secant rule and bisection.
+				float32 t;
+				if (rootIterCount & 1)
+				{
+					// Secant rule to improve convergence.
+					t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
+				}
+				else
+				{
+					// Bisection to guarantee progress.
+					t = 0.5f * (a1 + a2);
+				}
+
+				++rootIterCount;
+				++b2_toiRootIters;
+
+				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;
+				}
+				
+				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);
+
+	float32 time = timer.GetMilliseconds();
+	b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
+	b2_toiTime += time;
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Collision/b2TimeOfImpact.h

+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_TIME_OF_IMPACT_H
+#define B2_TIME_OF_IMPACT_H
+
+#include "Box2D/Common/b2Math.h"
+#include "Box2D/Collision/b2Distance.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

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2BlockAllocator.cpp

+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Common/b2BlockAllocator.h"
+#include <limits.h>
+#include <string.h>
+#include <stddef.h>
+
+int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = 
+{
+	16,		// 0
+	32,		// 1
+	64,		// 2
+	96,		// 3
+	128,	// 4
+	160,	// 5
+	192,	// 6
+	224,	// 7
+	256,	// 8
+	320,	// 9
+	384,	// 10
+	448,	// 11
+	512,	// 12
+	640,	// 13
+};
+uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1];
+bool b2BlockAllocator::s_blockSizeLookupInitialized;
+
+struct b2Chunk
+{
+	int32 blockSize;
+	b2Block* blocks;
+};
+
+struct b2Block
+{
+	b2Block* next;
+};
+
+b2BlockAllocator::b2BlockAllocator()
+{
+	b2Assert(b2_blockSizes < UCHAR_MAX);
+
+	m_chunkSpace = b2_chunkArrayIncrement;
+	m_chunkCount = 0;
+	m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
+	
+	memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
+	memset(m_freeLists, 0, sizeof(m_freeLists));
+
+	if (s_blockSizeLookupInitialized == false)
+	{
+		int32 j = 0;
+		for (int32 i = 1; i <= b2_maxBlockSize; ++i)
+		{
+			b2Assert(j < b2_blockSizes);
+			if (i <= s_blockSizes[j])
+			{
+				s_blockSizeLookup[i] = (uint8)j;
+			}
+			else
+			{
+				++j;
+				s_blockSizeLookup[i] = (uint8)j;
+			}
+		}
+
+		s_blockSizeLookupInitialized = true;
+	}
+}
+
+b2BlockAllocator::~b2BlockAllocator()
+{
+	for (int32 i = 0; i < m_chunkCount; ++i)
+	{
+		b2Free(m_chunks[i].blocks);
+	}
+
+	b2Free(m_chunks);
+}
+
+void* b2BlockAllocator::Allocate(int32 size)
+{
+	if (size == 0)
+		return nullptr;
+
+	b2Assert(0 < size);
+
+	if (size > b2_maxBlockSize)
+	{
+		return b2Alloc(size);
+	}
+
+	int32 index = s_blockSizeLookup[size];
+	b2Assert(0 <= index && index < b2_blockSizes);
+
+	if (m_freeLists[index])
+	{
+		b2Block* block = m_freeLists[index];
+		m_freeLists[index] = block->next;
+		return block;
+	}
+	else
+	{
+		if (m_chunkCount == m_chunkSpace)
+		{
+			b2Chunk* oldChunks = m_chunks;
+			m_chunkSpace += b2_chunkArrayIncrement;
+			m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
+			memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));
+			memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));
+			b2Free(oldChunks);
+		}
+
+		b2Chunk* chunk = m_chunks + m_chunkCount;
+		chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);
+#if defined(_DEBUG)
+		memset(chunk->blocks, 0xcd, b2_chunkSize);
+#endif
+		int32 blockSize = s_blockSizes[index];
+		chunk->blockSize = blockSize;
+		int32 blockCount = b2_chunkSize / blockSize;
+		b2Assert(blockCount * blockSize <= b2_chunkSize);
+		for (int32 i = 0; i < blockCount - 1; ++i)
+		{
+			b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);
+			b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));
+			block->next = next;
+		}
+		b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
+		last->next = nullptr;
+
+		m_freeLists[index] = chunk->blocks->next;
+		++m_chunkCount;
+
+		return chunk->blocks;
+	}
+}
+
+void b2BlockAllocator::Free(void* p, int32 size)
+{
+	if (size == 0)
+	{
+		return;
+	}
+
+	b2Assert(0 < size);
+
+	if (size > b2_maxBlockSize)
+	{
+		b2Free(p);
+		return;
+	}
+
+	int32 index = s_blockSizeLookup[size];
+	b2Assert(0 <= index && index < b2_blockSizes);
+
+#ifdef _DEBUG
+	// Verify the memory address and size is valid.
+	int32 blockSize = s_blockSizes[index];
+	bool found = false;
+	for (int32 i = 0; i < m_chunkCount; ++i)
+	{
+		b2Chunk* chunk = m_chunks + i;
+		if (chunk->blockSize != blockSize)
+		{
+			b2Assert(	(int8*)p + blockSize <= (int8*)chunk->blocks ||
+						(int8*)chunk->blocks + b2_chunkSize <= (int8*)p);
+		}
+		else
+		{
+			if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize)
+			{
+				found = true;
+			}
+		}
+	}
+
+	b2Assert(found);
+
+	memset(p, 0xfd, blockSize);
+#endif
+
+	b2Block* block = (b2Block*)p;
+	block->next = m_freeLists[index];
+	m_freeLists[index] = block;
+}
+
+void b2BlockAllocator::Clear()
+{
+	for (int32 i = 0; i < m_chunkCount; ++i)
+	{
+		b2Free(m_chunks[i].blocks);
+	}
+
+	m_chunkCount = 0;
+	memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
+
+	memset(m_freeLists, 0, sizeof(m_freeLists));
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2BlockAllocator.h

+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_BLOCK_ALLOCATOR_H
+#define B2_BLOCK_ALLOCATOR_H
+
+#include "Box2D/Common/b2Settings.h"
+
+const int32 b2_chunkSize = 16 * 1024;
+const int32 b2_maxBlockSize = 640;
+const int32 b2_blockSizes = 14;
+const int32 b2_chunkArrayIncrement = 128;
+
+struct b2Block;
+struct b2Chunk;
+
+/// This is a small object allocator used for allocating small
+/// objects that persist for more than one time step.
+/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp
+class b2BlockAllocator
+{
+public:
+	b2BlockAllocator();
+	~b2BlockAllocator();
+
+	/// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.
+	void* Allocate(int32 size);
+
+	/// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize.
+	void Free(void* p, int32 size);
+
+	void Clear();
+
+private:
+
+	b2Chunk* m_chunks;
+	int32 m_chunkCount;
+	int32 m_chunkSpace;
+
+	b2Block* m_freeLists[b2_blockSizes];
+
+	static int32 s_blockSizes[b2_blockSizes];
+	static uint8 s_blockSizeLookup[b2_maxBlockSize + 1];
+	static bool s_blockSizeLookupInitialized;
+};
+
+#endif

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2Draw.cpp

+/*
+* Copyright (c) 2011 Erin Catto http://box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Common/b2Draw.h"
+
+b2Draw::b2Draw()
+{
+	m_drawFlags = 0;
+}
+
+void b2Draw::SetFlags(uint32 flags)
+{
+	m_drawFlags = flags;
+}
+
+uint32 b2Draw::GetFlags() const
+{
+	return m_drawFlags;
+}
+
+void b2Draw::AppendFlags(uint32 flags)
+{
+	m_drawFlags |= flags;
+}
+
+void b2Draw::ClearFlags(uint32 flags)
+{
+	m_drawFlags &= ~flags;
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2Draw.h

+/*
+* Copyright (c) 2011 Erin Catto http://box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_DRAW_H
+#define B2_DRAW_H
+
+#include "Box2D/Common/b2Math.h"
+
+/// Color for debug drawing. Each value has the range [0,1].
+struct b2Color
+{
+	b2Color() {}
+	b2Color(float32 rIn, float32 gIn, float32 bIn, float32 aIn = 1.0f)
+	{
+		r = rIn; g = gIn; b = bIn; a = aIn;
+	}
+
+	void Set(float32 rIn, float32 gIn, float32 bIn, float32 aIn = 1.0f)
+	{
+		r = rIn; g = gIn; b = bIn; a = aIn;
+	}
+
+	float32 r, g, b, a;
+};
+
+/// Implement and register this class with a b2World to provide debug drawing of physics
+/// entities in your game.
+class b2Draw
+{
+public:
+	b2Draw();
+
+	virtual ~b2Draw() {}
+
+	enum
+	{
+		e_shapeBit				= 0x0001,	///< draw shapes
+		e_jointBit				= 0x0002,	///< draw joint connections
+		e_aabbBit				= 0x0004,	///< draw axis aligned bounding boxes
+		e_pairBit				= 0x0008,	///< draw broad-phase pairs
+		e_centerOfMassBit		= 0x0010	///< draw center of mass frame
+	};
+
+	/// Set the drawing flags.
+	void SetFlags(uint32 flags);
+
+	/// Get the drawing flags.
+	uint32 GetFlags() const;
+	
+	/// Append flags to the current flags.
+	void AppendFlags(uint32 flags);
+
+	/// Clear flags from the current flags.
+	void ClearFlags(uint32 flags);
+
+	/// Draw a closed polygon provided in CCW order.
+	virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
+
+	/// Draw a solid closed polygon provided in CCW order.
+	virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
+
+	/// Draw a circle.
+	virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;
+	
+	/// Draw a solid circle.
+	virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
+	
+	/// Draw a line segment.
+	virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
+
+	/// Draw a transform. Choose your own length scale.
+	/// @param xf a transform.
+	virtual void DrawTransform(const b2Transform& xf) = 0;
+
+	/// Draw a point.
+	virtual void DrawPoint(const b2Vec2& p, float32 size, const b2Color& color) = 0;
+
+protected:
+	uint32 m_drawFlags;
+};
+
+#endif

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2GrowableStack.h

+/*
+* Copyright (c) 2010 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_GROWABLE_STACK_H
+#define B2_GROWABLE_STACK_H
+#include "Box2D/Common/b2Settings.h"
+#include <string.h>
+
+/// This is a growable LIFO stack with an initial capacity of N.
+/// If the stack size exceeds the initial capacity, the heap is used
+/// to increase the size of the stack.
+template <typename T, int32 N>
+class b2GrowableStack
+{
+public:
+	b2GrowableStack()
+	{
+		m_stack = m_array;
+		m_count = 0;
+		m_capacity = N;
+	}
+
+	~b2GrowableStack()
+	{
+		if (m_stack != m_array)
+		{
+			b2Free(m_stack);
+			m_stack = nullptr;
+		}
+	}
+
+	void Push(const T& element)
+	{
+		if (m_count == m_capacity)
+		{
+			T* old = m_stack;
+			m_capacity *= 2;
+			m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
+			memcpy(m_stack, old, m_count * sizeof(T));
+			if (old != m_array)
+			{
+				b2Free(old);
+			}
+		}
+
+		m_stack[m_count] = element;
+		++m_count;
+	}
+
+	T Pop()
+	{
+		b2Assert(m_count > 0);
+		--m_count;
+		return m_stack[m_count];
+	}
+
+	int32 GetCount()
+	{
+		return m_count;
+	}
+
+private:
+	T* m_stack;
+	T m_array[N];
+	int32 m_count;
+	int32 m_capacity;
+};
+
+
+#endif

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2Math.cpp

+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Common/b2Math.h"
+
+const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
+
+/// Solve A * x = b, where b is a column vector. This is more efficient
+/// than computing the inverse in one-shot cases.
+b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
+{
+	float32 det = b2Dot(ex, b2Cross(ey, ez));
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+	b2Vec3 x;
+	x.x = det * b2Dot(b, b2Cross(ey, ez));
+	x.y = det * b2Dot(ex, b2Cross(b, ez));
+	x.z = det * b2Dot(ex, b2Cross(ey, b));
+	return x;
+}
+
+/// Solve A * x = b, where b is a column vector. This is more efficient
+/// than computing the inverse in one-shot cases.
+b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
+{
+	float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
+	float32 det = a11 * a22 - a12 * a21;
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+	b2Vec2 x;
+	x.x = det * (a22 * b.x - a12 * b.y);
+	x.y = det * (a11 * b.y - a21 * b.x);
+	return x;
+}
+
+///
+void b2Mat33::GetInverse22(b2Mat33* M) const
+{
+	float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+	float32 det = a * d - b * c;
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	M->ex.x =  det * d;	M->ey.x = -det * b; M->ex.z = 0.0f;
+	M->ex.y = -det * c;	M->ey.y =  det * a; M->ey.z = 0.0f;
+	M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;
+}
+
+/// Returns the zero matrix if singular.
+void b2Mat33::GetSymInverse33(b2Mat33* M) const
+{
+	float32 det = b2Dot(ex, b2Cross(ey, ez));
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	float32 a11 = ex.x, a12 = ey.x, a13 = ez.x;
+	float32 a22 = ey.y, a23 = ez.y;
+	float32 a33 = ez.z;
+
+	M->ex.x = det * (a22 * a33 - a23 * a23);
+	M->ex.y = det * (a13 * a23 - a12 * a33);
+	M->ex.z = det * (a12 * a23 - a13 * a22);
+
+	M->ey.x = M->ex.y;
+	M->ey.y = det * (a11 * a33 - a13 * a13);
+	M->ey.z = det * (a13 * a12 - a11 * a23);
+
+	M->ez.x = M->ex.z;
+	M->ez.y = M->ey.z;
+	M->ez.z = det * (a11 * a22 - a12 * a12);
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2Math.h

+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_MATH_H
+#define B2_MATH_H
+
+#include "Box2D/Common/b2Settings.h"
+#include <math.h>
+
+/// This function is used to ensure that a floating point number is not a NaN or infinity.
+inline bool b2IsValid(float32 x)
+{
+	int32 ix = *reinterpret_cast<int32*>(&x);
+	return (ix & 0x7f800000) != 0x7f800000;
+}
+
+/// This is a approximate yet fast inverse square-root.
+inline float32 b2InvSqrt(float32 x)
+{
+	union
+	{
+		float32 x;
+		int32 i;
+	} convert;
+
+	convert.x = x;
+	float32 xhalf = 0.5f * x;
+	convert.i = 0x5f3759df - (convert.i >> 1);
+	x = convert.x;
+	x = x * (1.5f - xhalf * x * x);
+	return x;
+}
+
+#define	b2Sqrt(x)	sqrtf(x)
+#define	b2Atan2(y, x)	atan2f(y, x)
+
+/// A 2D column vector.
+struct b2Vec2
+{
+	/// Default constructor does nothing (for performance).
+	b2Vec2() {}
+
+	/// Construct using coordinates.
+	b2Vec2(float32 xIn, float32 yIn) : x(xIn), y(yIn) {}
+
+	/// Set this vector to all zeros.
+	void SetZero() { x = 0.0f; y = 0.0f; }
+
+	/// Set this vector to some specified coordinates.
+	void Set(float32 x_, float32 y_) { x = x_; y = y_; }
+
+	/// Negate this vector.
+	b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
+	
+	/// Read from and indexed element.
+	float32 operator () (int32 i) const
+	{
+		return (&x)[i];
+	}
+
+	/// Write to an indexed element.
+	float32& operator () (int32 i)
+	{
+		return (&x)[i];
+	}
+
+	/// Add a vector to this vector.
+	void operator += (const b2Vec2& v)
+	{
+		x += v.x; y += v.y;
+	}
+	
+	/// Subtract a vector from this vector.
+	void operator -= (const b2Vec2& v)
+	{
+		x -= v.x; y -= v.y;
+	}
+
+	/// Multiply this vector by a scalar.
+	void operator *= (float32 a)
+	{
+		x *= a; y *= a;
+	}
+
+	/// Get the length of this vector (the norm).
+	float32 Length() const
+	{
+		return b2Sqrt(x * x + y * y);
+	}
+
+	/// Get the length squared. For performance, use this instead of
+	/// b2Vec2::Length (if possible).
+	float32 LengthSquared() const
+	{
+		return x * x + y * y;
+	}
+
+	/// Convert this vector into a unit vector. Returns the length.
+	float32 Normalize()
+	{
+		float32 length = Length();
+		if (length < b2_epsilon)
+		{
+			return 0.0f;
+		}
+		float32 invLength = 1.0f / length;
+		x *= invLength;
+		y *= invLength;
+
+		return length;
+	}
+
+	/// Does this vector contain finite coordinates?
+	bool IsValid() const
+	{
+		return b2IsValid(x) && b2IsValid(y);
+	}
+
+	/// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
+	b2Vec2 Skew() const
+	{
+		return b2Vec2(-y, x);
+	}
+
+	float32 x, y;
+};
+
+/// A 2D column vector with 3 elements.
+struct b2Vec3
+{
+	/// Default constructor does nothing (for performance).
+	b2Vec3() {}
+
+	/// Construct using coordinates.
+	b2Vec3(float32 xIn, float32 yIn, float32 zIn) : x(xIn), y(yIn), z(zIn) {}
+
+	/// Set this vector to all zeros.
+	void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
+
+	/// Set this vector to some specified coordinates.
+	void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
+
+	/// Negate this vector.
+	b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
+
+	/// Add a vector to this vector.
+	void operator += (const b2Vec3& v)
+	{
+		x += v.x; y += v.y; z += v.z;
+	}
+
+	/// Subtract a vector from this vector.
+	void operator -= (const b2Vec3& v)
+	{
+		x -= v.x; y -= v.y; z -= v.z;
+	}
+
+	/// Multiply this vector by a scalar.
+	void operator *= (float32 s)
+	{
+		x *= s; y *= s; z *= s;
+	}
+
+	float32 x, y, z;
+};
+
+/// A 2-by-2 matrix. Stored in column-major order.
+struct b2Mat22
+{
+	/// The default constructor does nothing (for performance).
+	b2Mat22() {}
+
+	/// Construct this matrix using columns.
+	b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
+	{
+		ex = c1;
+		ey = c2;
+	}
+
+	/// Construct this matrix using scalars.
+	b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
+	{
+		ex.x = a11; ex.y = a21;
+		ey.x = a12; ey.y = a22;
+	}
+
+	/// Initialize this matrix using columns.
+	void Set(const b2Vec2& c1, const b2Vec2& c2)
+	{
+		ex = c1;
+		ey = c2;
+	}
+
+	/// Set this to the identity matrix.
+	void SetIdentity()
+	{
+		ex.x = 1.0f; ey.x = 0.0f;
+		ex.y = 0.0f; ey.y = 1.0f;
+	}
+
+	/// Set this matrix to all zeros.
+	void SetZero()
+	{
+		ex.x = 0.0f; ey.x = 0.0f;
+		ex.y = 0.0f; ey.y = 0.0f;
+	}
+
+	b2Mat22 GetInverse() const
+	{
+		float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+		b2Mat22 B;
+		float32 det = a * d - b * c;
+		if (det != 0.0f)
+		{
+			det = 1.0f / det;
+		}
+		B.ex.x =  det * d;	B.ey.x = -det * b;
+		B.ex.y = -det * c;	B.ey.y =  det * a;
+		return B;
+	}
+
+	/// Solve A * x = b, where b is a column vector. This is more efficient
+	/// than computing the inverse in one-shot cases.
+	b2Vec2 Solve(const b2Vec2& b) const
+	{
+		float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
+		float32 det = a11 * a22 - a12 * a21;
+		if (det != 0.0f)
+		{
+			det = 1.0f / det;
+		}
+		b2Vec2 x;
+		x.x = det * (a22 * b.x - a12 * b.y);
+		x.y = det * (a11 * b.y - a21 * b.x);
+		return x;
+	}
+
+	b2Vec2 ex, ey;
+};
+
+/// A 3-by-3 matrix. Stored in column-major order.
+struct b2Mat33
+{
+	/// The default constructor does nothing (for performance).
+	b2Mat33() {}
+
+	/// Construct this matrix using columns.
+	b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
+	{
+		ex = c1;
+		ey = c2;
+		ez = c3;
+	}
+
+	/// Set this matrix to all zeros.
+	void SetZero()
+	{
+		ex.SetZero();
+		ey.SetZero();
+		ez.SetZero();
+	}
+
+	/// Solve A * x = b, where b is a column vector. This is more efficient
+	/// than computing the inverse in one-shot cases.
+	b2Vec3 Solve33(const b2Vec3& b) const;
+
+	/// Solve A * x = b, where b is a column vector. This is more efficient
+	/// than computing the inverse in one-shot cases. Solve only the upper
+	/// 2-by-2 matrix equation.
+	b2Vec2 Solve22(const b2Vec2& b) const;
+
+	/// Get the inverse of this matrix as a 2-by-2.
+	/// Returns the zero matrix if singular.
+	void GetInverse22(b2Mat33* M) const;
+
+	/// Get the symmetric inverse of this matrix as a 3-by-3.
+	/// Returns the zero matrix if singular.
+	void GetSymInverse33(b2Mat33* M) const;
+
+	b2Vec3 ex, ey, ez;
+};
+
+/// Rotation
+struct b2Rot
+{
+	b2Rot() {}
+
+	/// Initialize from an angle in radians
+	explicit b2Rot(float32 angle)
+	{
+		/// TODO_ERIN optimize
+		s = sinf(angle);
+		c = cosf(angle);
+	}
+
+	/// Set using an angle in radians.
+	void Set(float32 angle)
+	{
+		/// TODO_ERIN optimize
+		s = sinf(angle);
+		c = cosf(angle);
+	}
+
+	/// Set to the identity rotation
+	void SetIdentity()
+	{
+		s = 0.0f;
+		c = 1.0f;
+	}
+
+	/// Get the angle in radians
+	float32 GetAngle() const
+	{
+		return b2Atan2(s, c);
+	}
+
+	/// Get the x-axis
+	b2Vec2 GetXAxis() const
+	{
+		return b2Vec2(c, s);
+	}
+
+	/// Get the u-axis
+	b2Vec2 GetYAxis() const
+	{
+		return b2Vec2(-s, c);
+	}
+
+	/// Sine and cosine
+	float32 s, c;
+};
+
+/// A transform contains translation and rotation. It is used to represent
+/// the position and orientation of rigid frames.
+struct b2Transform
+{
+	/// The default constructor does nothing.
+	b2Transform() {}
+
+	/// Initialize using a position vector and a rotation.
+	b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
+
+	/// Set this to the identity transform.
+	void SetIdentity()
+	{
+		p.SetZero();
+		q.SetIdentity();
+	}
+
+	/// Set this based on the position and angle.
+	void Set(const b2Vec2& position, float32 angle)
+	{
+		p = position;
+		q.Set(angle);
+	}
+
+	b2Vec2 p;
+	b2Rot q;
+};
+
+/// This describes the motion of a body/shape for TOI computation.
+/// Shapes are defined with respect to the body origin, which may
+/// no coincide with the center of mass. However, to support dynamics
+/// we must interpolate the center of mass position.
+struct b2Sweep
+{
+	/// Get the interpolated transform at a specific time.
+	/// @param beta is a factor in [0,1], where 0 indicates alpha0.
+	void GetTransform(b2Transform* xfb, float32 beta) const;
+
+	/// Advance the sweep forward, yielding a new initial state.
+	/// @param alpha the new initial time.
+	void Advance(float32 alpha);
+
+	/// Normalize the angles.
+	void Normalize();
+
+	b2Vec2 localCenter;	///< local center of mass position
+	b2Vec2 c0, c;		///< center world positions
+	float32 a0, a;		///< world angles
+
+	/// Fraction of the current time step in the range [0,1]
+	/// c0 and a0 are the positions at alpha0.
+	float32 alpha0;
+};
+
+/// Useful constant
+extern const b2Vec2 b2Vec2_zero;
+
+/// Perform the dot product on two vectors.
+inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
+{
+	return a.x * b.x + a.y * b.y;
+}
+
+/// Perform the cross product on two vectors. In 2D this produces a scalar.
+inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
+{
+	return a.x * b.y - a.y * b.x;
+}
+
+/// Perform the cross product on a vector and a scalar. In 2D this produces
+/// a vector.
+inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
+{
+	return b2Vec2(s * a.y, -s * a.x);
+}
+
+/// Perform the cross product on a scalar and a vector. In 2D this produces
+/// a vector.
+inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
+{
+	return b2Vec2(-s * a.y, s * a.x);
+}
+
+/// Multiply a matrix times a vector. If a rotation matrix is provided,
+/// then this transforms the vector from one frame to another.
+inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
+{
+	return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
+}
+
+/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
+/// then this transforms the vector from one frame to another (inverse transform).
+inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
+{
+	return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
+}
+
+/// Add two vectors component-wise.
+inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
+{
+	return b2Vec2(a.x + b.x, a.y + b.y);
+}
+
+/// Subtract two vectors component-wise.
+inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
+{
+	return b2Vec2(a.x - b.x, a.y - b.y);
+}
+
+inline b2Vec2 operator * (float32 s, const b2Vec2& a)
+{
+	return b2Vec2(s * a.x, s * a.y);
+}
+
+inline bool operator == (const b2Vec2& a, const b2Vec2& b)
+{
+	return a.x == b.x && a.y == b.y;
+}
+
+inline bool operator != (const b2Vec2& a, const b2Vec2& b)
+{
+	return a.x != b.x || a.y != b.y;
+}
+
+inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
+{
+	b2Vec2 c = a - b;
+	return c.Length();
+}
+
+inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
+{
+	b2Vec2 c = a - b;
+	return b2Dot(c, c);
+}
+
+inline b2Vec3 operator * (float32 s, const b2Vec3& a)
+{
+	return b2Vec3(s * a.x, s * a.y, s * a.z);
+}
+
+/// Add two vectors component-wise.
+inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
+{
+	return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+
+/// Subtract two vectors component-wise.
+inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
+{
+	return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+
+/// Perform the dot product on two vectors.
+inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
+{
+	return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+
+/// Perform the cross product on two vectors.
+inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
+{
+	return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
+}
+
+inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
+{
+	return b2Mat22(A.ex + B.ex, A.ey + B.ey);
+}
+
+// A * B
+inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
+{
+	return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
+}
+
+// A^T * B
+inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
+{
+	b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
+	b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
+	return b2Mat22(c1, c2);
+}
+
+/// Multiply a matrix times a vector.
+inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
+{
+	return v.x * A.ex + v.y * A.ey + v.z * A.ez;
+}
+
+/// Multiply a matrix times a vector.
+inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
+{
+	return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
+}
+
+/// Multiply two rotations: q * r
+inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
+{
+	// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
+	// [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
+	// s = qs * rc + qc * rs
+	// c = qc * rc - qs * rs
+	b2Rot qr;
+	qr.s = q.s * r.c + q.c * r.s;
+	qr.c = q.c * r.c - q.s * r.s;
+	return qr;
+}
+
+/// Transpose multiply two rotations: qT * r
+inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
+{
+	// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
+	// [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
+	// s = qc * rs - qs * rc
+	// c = qc * rc + qs * rs
+	b2Rot qr;
+	qr.s = q.c * r.s - q.s * r.c;
+	qr.c = q.c * r.c + q.s * r.s;
+	return qr;
+}
+
+/// Rotate a vector
+inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
+{
+	return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
+}
+
+/// Inverse rotate a vector
+inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
+{
+	return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
+}
+
+inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
+{
+	float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
+	float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
+
+	return b2Vec2(x, y);
+}
+
+inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
+{
+	float32 px = v.x - T.p.x;
+	float32 py = v.y - T.p.y;
+	float32 x = (T.q.c * px + T.q.s * py);
+	float32 y = (-T.q.s * px + T.q.c * py);
+
+	return b2Vec2(x, y);
+}
+
+// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
+//    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
+inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
+{
+	b2Transform C;
+	C.q = b2Mul(A.q, B.q);
+	C.p = b2Mul(A.q, B.p) + A.p;
+	return C;
+}
+
+// v2 = A.q' * (B.q * v1 + B.p - A.p)
+//    = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
+{
+	b2Transform C;
+	C.q = b2MulT(A.q, B.q);
+	C.p = b2MulT(A.q, B.p - A.p);
+	return C;
+}
+
+template <typename T>
+inline T b2Abs(T a)
+{
+	return a > T(0) ? a : -a;
+}
+
+inline b2Vec2 b2Abs(const b2Vec2& a)
+{
+	return b2Vec2(b2Abs(a.x), b2Abs(a.y));
+}
+
+inline b2Mat22 b2Abs(const b2Mat22& A)
+{
+	return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
+}
+
+template <typename T>
+inline T b2Min(T a, T b)
+{
+	return a < b ? a : b;
+}
+
+inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
+{
+	return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
+}
+
+template <typename T>
+inline T b2Max(T a, T b)
+{
+	return a > b ? a : b;
+}
+
+inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
+{
+	return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
+}
+
+template <typename T>
+inline T b2Clamp(T a, T low, T high)
+{
+	return b2Max(low, b2Min(a, high));
+}
+
+inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
+{
+	return b2Max(low, b2Min(a, high));
+}
+
+template<typename T> inline void b2Swap(T& a, T& b)
+{
+	T tmp = a;
+	a = b;
+	b = tmp;
+}
+
+/// "Next Largest Power of 2
+/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
+/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
+/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
+/// largest power of 2. For a 32-bit value:"
+inline uint32 b2NextPowerOfTwo(uint32 x)
+{
+	x |= (x >> 1);
+	x |= (x >> 2);
+	x |= (x >> 4);
+	x |= (x >> 8);
+	x |= (x >> 16);
+	return x + 1;
+}
+
+inline bool b2IsPowerOfTwo(uint32 x)
+{
+	bool result = x > 0 && (x & (x - 1)) == 0;
+	return result;
+}
+
+inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
+{
+	xf->p = (1.0f - beta) * c0 + beta * c;
+	float32 angle = (1.0f - beta) * a0 + beta * a;
+	xf->q.Set(angle);
+
+	// Shift to origin
+	xf->p -= b2Mul(xf->q, localCenter);
+}
+
+inline void b2Sweep::Advance(float32 alpha)
+{
+	b2Assert(alpha0 < 1.0f);
+	float32 beta = (alpha - alpha0) / (1.0f - alpha0);
+	c0 += beta * (c - c0);
+	a0 += beta * (a - a0);
+	alpha0 = alpha;
+}
+
+/// Normalize an angle in radians to be between -pi and pi
+inline void b2Sweep::Normalize()
+{
+	float32 twoPi = 2.0f * b2_pi;
+	float32 d =  twoPi * floorf(a0 / twoPi);
+	a0 -= d;
+	a -= d;
+}
+
+#endif

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2Settings.cpp

+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Common/b2Settings.h"
+#include <stdio.h>
+#include <stdarg.h>
+#include <stdlib.h>
+
+b2Version b2_version = {2, 3, 2};
+
+// Memory allocators. Modify these to use your own allocator.
+void* b2Alloc(int32 size)
+{
+	return malloc(size);
+}
+
+void b2Free(void* mem)
+{
+	free(mem);
+}
+
+// You can modify this to use your logging facility.
+void b2Log(const char* string, ...)
+{
+	va_list args;
+	va_start(args, string);
+	vprintf(string, args);
+	va_end(args);
+}

web/static/doc_controle/tp/Box2D/src/Box2D/Common/b2Settings.h

+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_SETTINGS_H
+#define B2_SETTINGS_H
+
+#include <stddef.h>
+#include <assert.h>
+#include <float.h>
+
+#if !defined(NDEBUG)
+	#define b2DEBUG
+#endif
+
+#define B2_NOT_USED(x) ((void)(x))
+#define b2Assert(A) assert(A)
+
+typedef signed char	int8;
+typedef signed short int16;
+typedef signed int int32;
+typedef unsigned char uint8;
+typedef unsigned short uint16;
+typedef unsigned int uint32;
+typedef float float32;
+typedef double float64;
+
+#define	b2_maxFloat		FLT_MAX
+#define	b2_epsilon		FLT_EPSILON
+#define b2_pi			3.14159265359f
+
+/// @file
+/// Global tuning constants based on meters-kilograms-seconds (MKS) units.
+///
+
+// Collision
+
+/// The maximum number of contact points between two convex shapes. Do
+/// not change this value.
+#define b2_maxManifoldPoints	2
+
+/// The maximum number of vertices on a convex polygon. You cannot increase
+/// this too much because b2BlockAllocator has a maximum object size.
+#define b2_maxPolygonVertices	8
+
+/// This is used to fatten AABBs in the dynamic tree. This allows proxies
+/// to move by a small amount without triggering a tree adjustment.
+/// This is in meters.
+#define b2_aabbExtension		0.1f
+
+/// This is used to fatten AABBs in the dynamic tree. This is used to predict
+/// the future position based on the current displacement.
+/// This is a dimensionless multiplier.
+#define b2_aabbMultiplier		2.0f
+
+/// A small length used as a collision and constraint tolerance. Usually it is
+/// chosen to be numerically significant, but visually insignificant.
+#define b2_linearSlop			0.005f
+
+/// A small angle used as a collision and constraint tolerance. Usually it is
+/// chosen to be numerically significant, but visually insignificant.
+#define b2_angularSlop			(2.0f / 180.0f * b2_pi)
+
+/// The radius of the polygon/edge shape skin. This should not be modified. Making
+/// this smaller means polygons will have an insufficient buffer for continuous collision.
+/// Making it larger may create artifacts for vertex collision.
+#define b2_polygonRadius		(2.0f * b2_linearSlop)
+
+/// Maximum number of sub-steps per contact in continuous physics simulation.
+#define b2_maxSubSteps			8
+
+
+// Dynamics
+
+/// Maximum number of contacts to be handled to solve a TOI impact.
+#define b2_maxTOIContacts			32
+
+/// A velocity threshold for elastic collisions. Any collision with a relative linear
+/// velocity below this threshold will be treated as inelastic.
+#define b2_velocityThreshold		1.0f
+
+/// The maximum linear position correction used when solving constraints. This helps to
+/// prevent overshoot.
+#define b2_maxLinearCorrection		0.2f
+
+/// The maximum angular position correction used when solving constraints. This helps to
+/// prevent overshoot.
+#define b2_maxAngularCorrection		(8.0f / 180.0f * b2_pi)
+
+/// The maximum linear velocity of a body. This limit is very large and is used
+/// to prevent numerical problems. You shouldn't need to adjust this.
+#define b2_maxTranslation			2.0f
+#define b2_maxTranslationSquared	(b2_maxTranslation * b2_maxTranslation)
+
+/// The maximum angular velocity of a body. This limit is very large and is used
+/// to prevent numerical problems. You shouldn't need to adjust this.
+#define b2_maxRotation				(0.5f * b2_pi)
+#define b2_maxRotationSquared		(b2_maxRotation * b2_maxRotation)
+
+/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
+/// that overlap is removed in one time step. However using values close to 1 often lead
+/// to overshoot.
+#define b2_baumgarte				0.2f
+#define b2_toiBaugarte				0.75f
+
+
+// Sleep
+
+/// The time that a body must be still before it will go to sleep.
+#define b2_timeToSleep				0.5f
+
+/// A body cannot sleep if its linear velocity is above this tolerance.
+#define b2_linearSleepTolerance		0.01f
+
+/// A body cannot sleep if its angular velocity is above this tolerance.
+#define b2_angularSleepTolerance	(2.0f / 180.0f * b2_pi)
+
+// Memory Allocation
+
+/// Implement this function to use your own memory allocator.
+void* b2Alloc(int32 size);
+
+/// If you implement b2Alloc, you should also implement this function.
+void b2Free(void* mem);
+
+/// Logging function.
+void b2Log(const char* string, ...);
+
+/// Version numbering scheme.
+/// See http://en.wikipedia.org/wiki/Software_versioning
+struct b2Version
+{
+	int32 major;		///< significant changes
+	int32 minor;		///< incremental changes
+	int32 revision;		///< bug fixes
+};
+
+/// Current version.
+extern b2Version b2_version;
+
+#endif