diff --git a/.gitignore b/.gitignore
index ee4109a6..c004e4dc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,11 +1,28 @@
*.ilk
*.pdb
-*.obj
*.log
*.idb
+*.exp
+*.iobj
+*.ipdb
*.tlog
*.tlog/
*.lastbuildstate
*.sdf
-*.user
\ No newline at end of file
+*.user
+*.suo
+
+Intermediates/
+ipch/
+
+.vs/
+
+# Intermediates
+obj/
+
+# Binaries
+bin/*
+!Web/*
+!*.js
+*.filters
diff --git a/Dependencies/Box2D/Box2D.vcxproj b/Dependencies/Box2D/Box2D.vcxproj
new file mode 100644
index 00000000..716d73d9
--- /dev/null
+++ b/Dependencies/Box2D/Box2D.vcxproj
@@ -0,0 +1,184 @@
+
+
+
+
+ Debug
+ Win32
+
+
+ Release
+ Win32
+
+
+
+ {BCE9869C-CDE3-4935-99AB-4522493CE0B3}
+ Box2D
+ 10.0.17763.0
+
+
+
+ StaticLibrary
+ true
+ v141
+ MultiByte
+
+
+ StaticLibrary
+ false
+ v141
+ true
+ MultiByte
+
+
+
+
+
+
+
+
+
+
+
+
+ $(SolutionDir)bin\$(Configuration)\
+ $(SolutionDir)bin\$(Configuration)\Intermediates\$(ProjectName)\
+
+
+ $(SolutionDir)bin\$(Configuration)\
+ $(SolutionDir)bin\$(Configuration)\Intermediates\$(ProjectName)\
+
+
+
+ Level3
+ Disabled
+ true
+ src;
+ true
+
+
+ true
+
+
+ true
+
+
+
+
+ Level3
+ Full
+ true
+ true
+ true
+ AnySuitable
+ Speed
+ StreamingSIMDExtensions2
+ Fast
+ src;
+
+
+ true
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Dependencies/Box2D/src/Box2D.h b/Dependencies/Box2D/src/Box2D.h
new file mode 100644
index 00000000..ce95bacd
--- /dev/null
+++ b/Dependencies/Box2D/src/Box2D.h
@@ -0,0 +1,68 @@
+/*
+* 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 BOX2D_H
+#define BOX2D_H
+
+/**
+\mainpage Box2D API Documentation
+
+\section intro_sec Getting Started
+
+For documentation please see http://box2d.org/documentation.html
+
+For discussion please visit http://box2d.org/forum
+*/
+
+// These include files constitute the main Box2D API
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#endif
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2ChainShape.cpp b/Dependencies/Box2D/src/Collision/Shapes/b2ChainShape.cpp
new file mode 100644
index 00000000..fd133d38
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2ChainShape.cpp
@@ -0,0 +1,193 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#include
+#include
+#include
+#include
+
+b2ChainShape::~b2ChainShape()
+{
+ Clear();
+}
+
+void b2ChainShape::Clear()
+{
+ b2Free(m_vertices);
+ m_vertices = NULL;
+ m_count = 0;
+}
+
+void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
+{
+ b2Assert(m_vertices == NULL && m_count == 0);
+ b2Assert(count >= 3);
+ for (int32 i = 1; i < count; ++i)
+ {
+ b2Vec2 v1 = vertices[i-1];
+ b2Vec2 v2 = vertices[i];
+ // If the code crashes here, it means your vertices are too close together.
+ b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
+ }
+
+ m_count = count + 1;
+ m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
+ memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
+ m_vertices[count] = m_vertices[0];
+ m_prevVertex = m_vertices[m_count - 2];
+ m_nextVertex = m_vertices[1];
+ m_hasPrevVertex = true;
+ m_hasNextVertex = true;
+}
+
+void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
+{
+ b2Assert(m_vertices == NULL && m_count == 0);
+ b2Assert(count >= 2);
+ for (int32 i = 1; i < count; ++i)
+ {
+ // If the code crashes here, it means your vertices are too close together.
+ b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop);
+ }
+
+ m_count = count;
+ m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
+ memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
+
+ m_hasPrevVertex = false;
+ m_hasNextVertex = false;
+
+ m_prevVertex.SetZero();
+ m_nextVertex.SetZero();
+}
+
+void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex)
+{
+ m_prevVertex = prevVertex;
+ m_hasPrevVertex = true;
+}
+
+void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex)
+{
+ m_nextVertex = nextVertex;
+ m_hasNextVertex = true;
+}
+
+b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const
+{
+ void* mem = allocator->Allocate(sizeof(b2ChainShape));
+ b2ChainShape* clone = new (mem) b2ChainShape;
+ clone->CreateChain(m_vertices, m_count);
+ clone->m_prevVertex = m_prevVertex;
+ clone->m_nextVertex = m_nextVertex;
+ clone->m_hasPrevVertex = m_hasPrevVertex;
+ clone->m_hasNextVertex = m_hasNextVertex;
+ return clone;
+}
+
+int32 b2ChainShape::GetChildCount() const
+{
+ // edge count = vertex count - 1
+ return m_count - 1;
+}
+
+void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const
+{
+ b2Assert(0 <= index && index < m_count - 1);
+ edge->m_type = b2Shape::e_edge;
+ edge->m_radius = m_radius;
+
+ edge->m_vertex1 = m_vertices[index + 0];
+ edge->m_vertex2 = m_vertices[index + 1];
+
+ if (index > 0)
+ {
+ edge->m_vertex0 = m_vertices[index - 1];
+ edge->m_hasVertex0 = true;
+ }
+ else
+ {
+ edge->m_vertex0 = m_prevVertex;
+ edge->m_hasVertex0 = m_hasPrevVertex;
+ }
+
+ if (index < m_count - 2)
+ {
+ edge->m_vertex3 = m_vertices[index + 2];
+ edge->m_hasVertex3 = true;
+ }
+ else
+ {
+ edge->m_vertex3 = m_nextVertex;
+ edge->m_hasVertex3 = m_hasNextVertex;
+ }
+}
+
+bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+ B2_NOT_USED(xf);
+ B2_NOT_USED(p);
+ return false;
+}
+
+bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& xf, int32 childIndex) const
+{
+ b2Assert(childIndex < m_count);
+
+ b2EdgeShape edgeShape;
+
+ int32 i1 = childIndex;
+ int32 i2 = childIndex + 1;
+ if (i2 == m_count)
+ {
+ i2 = 0;
+ }
+
+ edgeShape.m_vertex1 = m_vertices[i1];
+ edgeShape.m_vertex2 = m_vertices[i2];
+
+ return edgeShape.RayCast(output, input, xf, 0);
+}
+
+void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
+{
+ b2Assert(childIndex < m_count);
+
+ int32 i1 = childIndex;
+ int32 i2 = childIndex + 1;
+ if (i2 == m_count)
+ {
+ i2 = 0;
+ }
+
+ b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);
+ b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);
+
+ aabb->lowerBound = b2Min(v1, v2);
+ aabb->upperBound = b2Max(v1, v2);
+}
+
+void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+ B2_NOT_USED(density);
+
+ massData->mass = 0.0f;
+ massData->center.SetZero();
+ massData->I = 0.0f;
+}
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2ChainShape.h b/Dependencies/Box2D/src/Collision/Shapes/b2ChainShape.h
new file mode 100644
index 00000000..24a5234b
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2ChainShape.h
@@ -0,0 +1,105 @@
+/*
+* Copyright (c) 2006-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_CHAIN_SHAPE_H
+#define B2_CHAIN_SHAPE_H
+
+#include
+
+class b2EdgeShape;
+
+/// A chain shape is a free form sequence of line segments.
+/// The chain has two-sided collision, so you can use inside and outside collision.
+/// Therefore, you may use any winding order.
+/// Since there may be many vertices, they are allocated using b2Alloc.
+/// Connectivity information is used to create smooth collisions.
+/// WARNING: The chain will not collide properly if there are self-intersections.
+class b2ChainShape : public b2Shape
+{
+public:
+ b2ChainShape();
+
+ /// The destructor frees the vertices using b2Free.
+ ~b2ChainShape();
+
+ /// Clear all data.
+ void Clear();
+
+ /// Create a loop. This automatically adjusts connectivity.
+ /// @param vertices an array of vertices, these are copied
+ /// @param count the vertex count
+ void CreateLoop(const b2Vec2* vertices, int32 count);
+
+ /// Create a chain with isolated end vertices.
+ /// @param vertices an array of vertices, these are copied
+ /// @param count the vertex count
+ void CreateChain(const b2Vec2* vertices, int32 count);
+
+ /// Establish connectivity to a vertex that precedes the first vertex.
+ /// Don't call this for loops.
+ void SetPrevVertex(const b2Vec2& prevVertex);
+
+ /// Establish connectivity to a vertex that follows the last vertex.
+ /// Don't call this for loops.
+ void SetNextVertex(const b2Vec2& nextVertex);
+
+ /// Implement b2Shape. Vertices are cloned using b2Alloc.
+ b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+ /// @see b2Shape::GetChildCount
+ int32 GetChildCount() const;
+
+ /// Get a child edge.
+ void GetChildEdge(b2EdgeShape* edge, int32 index) const;
+
+ /// This always return false.
+ /// @see b2Shape::TestPoint
+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+ /// Implement b2Shape.
+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& transform, int32 childIndex) const;
+
+ /// @see b2Shape::ComputeAABB
+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
+
+ /// Chains have zero mass.
+ /// @see b2Shape::ComputeMass
+ void ComputeMass(b2MassData* massData, float32 density) const;
+
+ /// The vertices. Owned by this class.
+ b2Vec2* m_vertices;
+
+ /// The vertex count.
+ int32 m_count;
+
+ b2Vec2 m_prevVertex, m_nextVertex;
+ bool m_hasPrevVertex, m_hasNextVertex;
+};
+
+inline b2ChainShape::b2ChainShape()
+{
+ m_type = e_chain;
+ m_radius = b2_polygonRadius;
+ m_vertices = NULL;
+ m_count = 0;
+ m_hasPrevVertex = false;
+ m_hasNextVertex = false;
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2CircleShape.cpp b/Dependencies/Box2D/src/Collision/Shapes/b2CircleShape.cpp
new file mode 100644
index 00000000..d04373a8
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2CircleShape.cpp
@@ -0,0 +1,99 @@
+/*
+* 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
+#include
+
+b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
+{
+ void* mem = allocator->Allocate(sizeof(b2CircleShape));
+ b2CircleShape* clone = new (mem) b2CircleShape;
+ *clone = *this;
+ return clone;
+}
+
+int32 b2CircleShape::GetChildCount() const
+{
+ return 1;
+}
+
+bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
+{
+ b2Vec2 center = transform.p + b2Mul(transform.q, m_p);
+ b2Vec2 d = p - center;
+ return b2Dot(d, d) <= m_radius * m_radius;
+}
+
+// Collision Detection in Interactive 3D Environments by Gino van den Bergen
+// From Section 3.1.2
+// x = s + a * r
+// norm(x) = radius
+bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& transform, int32 childIndex) const
+{
+ B2_NOT_USED(childIndex);
+
+ b2Vec2 position = transform.p + b2Mul(transform.q, m_p);
+ b2Vec2 s = input.p1 - position;
+ float32 b = b2Dot(s, s) - m_radius * m_radius;
+
+ // Solve quadratic equation.
+ b2Vec2 r = input.p2 - input.p1;
+ float32 c = b2Dot(s, r);
+ float32 rr = b2Dot(r, r);
+ float32 sigma = c * c - rr * b;
+
+ // Check for negative discriminant and short segment.
+ if (sigma < 0.0f || rr < b2_epsilon)
+ {
+ return false;
+ }
+
+ // Find the point of intersection of the line with the circle.
+ float32 a = -(c + b2Sqrt(sigma));
+
+ // Is the intersection point on the segment?
+ if (0.0f <= a && a <= input.maxFraction * rr)
+ {
+ a /= rr;
+ output->fraction = a;
+ output->normal = s + a * r;
+ output->normal.Normalize();
+ return true;
+ }
+
+ return false;
+}
+
+void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const
+{
+ B2_NOT_USED(childIndex);
+
+ b2Vec2 p = transform.p + b2Mul(transform.q, m_p);
+ aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
+ aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
+}
+
+void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+ massData->mass = density * b2_pi * m_radius * m_radius;
+ massData->center = m_p;
+
+ // inertia about the local origin
+ massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p));
+}
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2CircleShape.h b/Dependencies/Box2D/src/Collision/Shapes/b2CircleShape.h
new file mode 100644
index 00000000..dbbd2134
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2CircleShape.h
@@ -0,0 +1,91 @@
+/*
+* 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_CIRCLE_SHAPE_H
+#define B2_CIRCLE_SHAPE_H
+
+#include
+
+/// A circle shape.
+class b2CircleShape : public b2Shape
+{
+public:
+ b2CircleShape();
+
+ /// Implement b2Shape.
+ b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+ /// @see b2Shape::GetChildCount
+ int32 GetChildCount() const;
+
+ /// Implement b2Shape.
+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+ /// Implement b2Shape.
+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& transform, int32 childIndex) const;
+
+ /// @see b2Shape::ComputeAABB
+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
+
+ /// @see b2Shape::ComputeMass
+ void ComputeMass(b2MassData* massData, float32 density) const;
+
+ /// Get the supporting vertex index in the given direction.
+ int32 GetSupport(const b2Vec2& d) const;
+
+ /// Get the supporting vertex in the given direction.
+ const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
+
+ /// Get the vertex count.
+ int32 GetVertexCount() const { return 1; }
+
+ /// Get a vertex by index. Used by b2Distance.
+ const b2Vec2& GetVertex(int32 index) const;
+
+ /// Position
+ b2Vec2 m_p;
+};
+
+inline b2CircleShape::b2CircleShape()
+{
+ m_type = e_circle;
+ m_radius = 0.0f;
+ m_p.SetZero();
+}
+
+inline int32 b2CircleShape::GetSupport(const b2Vec2 &d) const
+{
+ B2_NOT_USED(d);
+ return 0;
+}
+
+inline const b2Vec2& b2CircleShape::GetSupportVertex(const b2Vec2 &d) const
+{
+ B2_NOT_USED(d);
+ return m_p;
+}
+
+inline const b2Vec2& b2CircleShape::GetVertex(int32 index) const
+{
+ B2_NOT_USED(index);
+ b2Assert(index == 0);
+ return m_p;
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2EdgeShape.cpp b/Dependencies/Box2D/src/Collision/Shapes/b2EdgeShape.cpp
new file mode 100644
index 00000000..66e5b530
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2EdgeShape.cpp
@@ -0,0 +1,138 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#include
+#include
+
+void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)
+{
+ m_vertex1 = v1;
+ m_vertex2 = v2;
+ m_hasVertex0 = false;
+ m_hasVertex3 = false;
+}
+
+b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const
+{
+ void* mem = allocator->Allocate(sizeof(b2EdgeShape));
+ b2EdgeShape* clone = new (mem) b2EdgeShape;
+ *clone = *this;
+ return clone;
+}
+
+int32 b2EdgeShape::GetChildCount() const
+{
+ return 1;
+}
+
+bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+ B2_NOT_USED(xf);
+ B2_NOT_USED(p);
+ return false;
+}
+
+// p = p1 + t * d
+// v = v1 + s * e
+// p1 + t * d = v1 + s * e
+// s * e - t * d = p1 - v1
+bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& xf, int32 childIndex) const
+{
+ B2_NOT_USED(childIndex);
+
+ // Put the ray into the edge's frame of reference.
+ b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
+ b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
+ b2Vec2 d = p2 - p1;
+
+ b2Vec2 v1 = m_vertex1;
+ b2Vec2 v2 = m_vertex2;
+ b2Vec2 e = v2 - v1;
+ b2Vec2 normal(e.y, -e.x);
+ normal.Normalize();
+
+ // q = p1 + t * d
+ // dot(normal, q - v1) = 0
+ // dot(normal, p1 - v1) + t * dot(normal, d) = 0
+ float32 numerator = b2Dot(normal, v1 - p1);
+ float32 denominator = b2Dot(normal, d);
+
+ if (denominator == 0.0f)
+ {
+ return false;
+ }
+
+ float32 t = numerator / denominator;
+ if (t < 0.0f || input.maxFraction < t)
+ {
+ return false;
+ }
+
+ b2Vec2 q = p1 + t * d;
+
+ // q = v1 + s * r
+ // s = dot(q - v1, r) / dot(r, r)
+ b2Vec2 r = v2 - v1;
+ float32 rr = b2Dot(r, r);
+ if (rr == 0.0f)
+ {
+ return false;
+ }
+
+ float32 s = b2Dot(q - v1, r) / rr;
+ if (s < 0.0f || 1.0f < s)
+ {
+ return false;
+ }
+
+ output->fraction = t;
+ if (numerator > 0.0f)
+ {
+ output->normal = -b2Mul(xf.q, normal);
+ }
+ else
+ {
+ output->normal = b2Mul(xf.q, normal);
+ }
+ return true;
+}
+
+void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
+{
+ B2_NOT_USED(childIndex);
+
+ b2Vec2 v1 = b2Mul(xf, m_vertex1);
+ b2Vec2 v2 = b2Mul(xf, m_vertex2);
+
+ b2Vec2 lower = b2Min(v1, v2);
+ b2Vec2 upper = b2Max(v1, v2);
+
+ b2Vec2 r(m_radius, m_radius);
+ aabb->lowerBound = lower - r;
+ aabb->upperBound = upper + r;
+}
+
+void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+ B2_NOT_USED(density);
+
+ massData->mass = 0.0f;
+ massData->center = 0.5f * (m_vertex1 + m_vertex2);
+ massData->I = 0.0f;
+}
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2EdgeShape.h b/Dependencies/Box2D/src/Collision/Shapes/b2EdgeShape.h
new file mode 100644
index 00000000..f2038dd8
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2EdgeShape.h
@@ -0,0 +1,74 @@
+/*
+* Copyright (c) 2006-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_EDGE_SHAPE_H
+#define B2_EDGE_SHAPE_H
+
+#include
+
+/// A line segment (edge) shape. These can be connected in chains or loops
+/// to other edge shapes. The connectivity information is used to ensure
+/// correct contact normals.
+class b2EdgeShape : public b2Shape
+{
+public:
+ b2EdgeShape();
+
+ /// Set this as an isolated edge.
+ void Set(const b2Vec2& v1, const b2Vec2& v2);
+
+ /// Implement b2Shape.
+ b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+ /// @see b2Shape::GetChildCount
+ int32 GetChildCount() const;
+
+ /// @see b2Shape::TestPoint
+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+ /// Implement b2Shape.
+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& transform, int32 childIndex) const;
+
+ /// @see b2Shape::ComputeAABB
+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
+
+ /// @see b2Shape::ComputeMass
+ void ComputeMass(b2MassData* massData, float32 density) const;
+
+ /// These are the edge vertices
+ b2Vec2 m_vertex1, m_vertex2;
+
+ /// Optional adjacent vertices. These are used for smooth collision.
+ b2Vec2 m_vertex0, m_vertex3;
+ bool m_hasVertex0, m_hasVertex3;
+};
+
+inline b2EdgeShape::b2EdgeShape()
+{
+ m_type = e_edge;
+ m_radius = b2_polygonRadius;
+ m_vertex0.x = 0.0f;
+ m_vertex0.y = 0.0f;
+ m_vertex3.x = 0.0f;
+ m_vertex3.y = 0.0f;
+ m_hasVertex0 = false;
+ m_hasVertex3 = false;
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2PolygonShape.cpp b/Dependencies/Box2D/src/Collision/Shapes/b2PolygonShape.cpp
new file mode 100644
index 00000000..452a59c7
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2PolygonShape.cpp
@@ -0,0 +1,467 @@
+/*
+* 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
+#include
+
+b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
+{
+ void* mem = allocator->Allocate(sizeof(b2PolygonShape));
+ b2PolygonShape* clone = new (mem) b2PolygonShape;
+ *clone = *this;
+ return clone;
+}
+
+void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
+{
+ m_count = 4;
+ m_vertices[0].Set(-hx, -hy);
+ m_vertices[1].Set( hx, -hy);
+ m_vertices[2].Set( hx, hy);
+ m_vertices[3].Set(-hx, hy);
+ m_normals[0].Set(0.0f, -1.0f);
+ m_normals[1].Set(1.0f, 0.0f);
+ m_normals[2].Set(0.0f, 1.0f);
+ m_normals[3].Set(-1.0f, 0.0f);
+ m_centroid.SetZero();
+}
+
+void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
+{
+ m_count = 4;
+ m_vertices[0].Set(-hx, -hy);
+ m_vertices[1].Set( hx, -hy);
+ m_vertices[2].Set( hx, hy);
+ m_vertices[3].Set(-hx, hy);
+ m_normals[0].Set(0.0f, -1.0f);
+ m_normals[1].Set(1.0f, 0.0f);
+ m_normals[2].Set(0.0f, 1.0f);
+ m_normals[3].Set(-1.0f, 0.0f);
+ m_centroid = center;
+
+ b2Transform xf;
+ xf.p = center;
+ xf.q.Set(angle);
+
+ // Transform vertices and normals.
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ m_vertices[i] = b2Mul(xf, m_vertices[i]);
+ m_normals[i] = b2Mul(xf.q, m_normals[i]);
+ }
+}
+
+int32 b2PolygonShape::GetChildCount() const
+{
+ return 1;
+}
+
+static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
+{
+ b2Assert(count >= 3);
+
+ b2Vec2 c; c.Set(0.0f, 0.0f);
+ float32 area = 0.0f;
+
+ // pRef is the reference point for forming triangles.
+ // It's location doesn't change the result (except for rounding error).
+ b2Vec2 pRef(0.0f, 0.0f);
+#if 0
+ // This code would put the reference point inside the polygon.
+ for (int32 i = 0; i < count; ++i)
+ {
+ pRef += vs[i];
+ }
+ pRef *= 1.0f / count;
+#endif
+
+ const float32 inv3 = 1.0f / 3.0f;
+
+ for (int32 i = 0; i < count; ++i)
+ {
+ // Triangle vertices.
+ b2Vec2 p1 = pRef;
+ b2Vec2 p2 = vs[i];
+ b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
+
+ b2Vec2 e1 = p2 - p1;
+ b2Vec2 e2 = p3 - p1;
+
+ float32 D = b2Cross(e1, e2);
+
+ float32 triangleArea = 0.5f * D;
+ area += triangleArea;
+
+ // Area weighted centroid
+ c += triangleArea * inv3 * (p1 + p2 + p3);
+ }
+
+ // Centroid
+ b2Assert(area > b2_epsilon);
+ c *= 1.0f / area;
+ return c;
+}
+
+void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
+{
+ b2Assert(3 <= count && count <= b2_maxPolygonVertices);
+ if (count < 3)
+ {
+ SetAsBox(1.0f, 1.0f);
+ return;
+ }
+
+ int32 n = b2Min(count, b2_maxPolygonVertices);
+
+ // Perform welding and copy vertices into local buffer.
+ b2Vec2 ps[b2_maxPolygonVertices];
+ int32 tempCount = 0;
+ for (int32 i = 0; i < n; ++i)
+ {
+ b2Vec2 v = vertices[i];
+
+ bool unique = true;
+ for (int32 j = 0; j < tempCount; ++j)
+ {
+ if (b2DistanceSquared(v, ps[j]) < ((0.5f * b2_linearSlop) * (0.5f * b2_linearSlop)))
+ {
+ unique = false;
+ break;
+ }
+ }
+
+ if (unique)
+ {
+ ps[tempCount++] = v;
+ }
+ }
+
+ n = tempCount;
+ if (n < 3)
+ {
+ // Polygon is degenerate.
+ b2Assert(false);
+ SetAsBox(1.0f, 1.0f);
+ return;
+ }
+
+ // Create the convex hull using the Gift wrapping algorithm
+ // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
+
+ // Find the right most point on the hull
+ int32 i0 = 0;
+ float32 x0 = ps[0].x;
+ for (int32 i = 1; i < n; ++i)
+ {
+ float32 x = ps[i].x;
+ if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
+ {
+ i0 = i;
+ x0 = x;
+ }
+ }
+
+ int32 hull[b2_maxPolygonVertices];
+ int32 m = 0;
+ int32 ih = i0;
+
+ for (;;)
+ {
+ hull[m] = ih;
+
+ int32 ie = 0;
+ for (int32 j = 1; j < n; ++j)
+ {
+ if (ie == ih)
+ {
+ ie = j;
+ continue;
+ }
+
+ b2Vec2 r = ps[ie] - ps[hull[m]];
+ b2Vec2 v = ps[j] - ps[hull[m]];
+ float32 c = b2Cross(r, v);
+ if (c < 0.0f)
+ {
+ ie = j;
+ }
+
+ // Collinearity check
+ if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
+ {
+ ie = j;
+ }
+ }
+
+ ++m;
+ ih = ie;
+
+ if (ie == i0)
+ {
+ break;
+ }
+ }
+
+ if (m < 3)
+ {
+ // Polygon is degenerate.
+ b2Assert(false);
+ SetAsBox(1.0f, 1.0f);
+ return;
+ }
+
+ m_count = m;
+
+ // Copy vertices.
+ for (int32 i = 0; i < m; ++i)
+ {
+ m_vertices[i] = ps[hull[i]];
+ }
+
+ // Compute normals. Ensure the edges have non-zero length.
+ for (int32 i = 0; i < m; ++i)
+ {
+ int32 i1 = i;
+ int32 i2 = i + 1 < m ? i + 1 : 0;
+ b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
+ b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
+ m_normals[i] = b2Cross(edge, 1.0f);
+ m_normals[i].Normalize();
+ }
+
+ // Compute the polygon centroid.
+ m_centroid = ComputeCentroid(m_vertices, m);
+}
+
+bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+ b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
+
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
+ if (dot > 0.0f)
+ {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& xf, int32 childIndex) const
+{
+ B2_NOT_USED(childIndex);
+
+ // Put the ray into the polygon's frame of reference.
+ b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
+ b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
+ b2Vec2 d = p2 - p1;
+
+ float32 lower = 0.0f, upper = input.maxFraction;
+
+ int32 index = -1;
+
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ // p = p1 + a * d
+ // dot(normal, p - v) = 0
+ // dot(normal, p1 - v) + a * dot(normal, d) = 0
+ float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
+ float32 denominator = b2Dot(m_normals[i], d);
+
+ if (denominator == 0.0f)
+ {
+ if (numerator < 0.0f)
+ {
+ return false;
+ }
+ }
+ else
+ {
+ // Note: we want this predicate without division:
+ // lower < numerator / denominator, where denominator < 0
+ // Since denominator < 0, we have to flip the inequality:
+ // lower < numerator / denominator <==> denominator * lower > numerator.
+ if (denominator < 0.0f && numerator < lower * denominator)
+ {
+ // Increase lower.
+ // The segment enters this half-space.
+ lower = numerator / denominator;
+ index = i;
+ }
+ else if (denominator > 0.0f && numerator < upper * denominator)
+ {
+ // Decrease upper.
+ // The segment exits this half-space.
+ upper = numerator / denominator;
+ }
+ }
+
+ // The use of epsilon here causes the assert on lower to trip
+ // in some cases. Apparently the use of epsilon was to make edge
+ // shapes work, but now those are handled separately.
+ //if (upper < lower - b2_epsilon)
+ if (upper < lower)
+ {
+ return false;
+ }
+ }
+
+ b2Assert(0.0f <= lower && lower <= input.maxFraction);
+
+ if (index >= 0)
+ {
+ output->fraction = lower;
+ output->normal = b2Mul(xf.q, m_normals[index]);
+ return true;
+ }
+
+ return false;
+}
+
+void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
+{
+ B2_NOT_USED(childIndex);
+
+ b2Vec2 lower = b2Mul(xf, m_vertices[0]);
+ b2Vec2 upper = lower;
+
+ for (int32 i = 1; i < m_count; ++i)
+ {
+ b2Vec2 v = b2Mul(xf, m_vertices[i]);
+ lower = b2Min(lower, v);
+ upper = b2Max(upper, v);
+ }
+
+ b2Vec2 r(m_radius, m_radius);
+ aabb->lowerBound = lower - r;
+ aabb->upperBound = upper + r;
+}
+
+void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+ // Polygon mass, centroid, and inertia.
+ // Let rho be the polygon density in mass per unit area.
+ // Then:
+ // mass = rho * int(dA)
+ // centroid.x = (1/mass) * rho * int(x * dA)
+ // centroid.y = (1/mass) * rho * int(y * dA)
+ // I = rho * int((x*x + y*y) * dA)
+ //
+ // We can compute these integrals by summing all the integrals
+ // for each triangle of the polygon. To evaluate the integral
+ // for a single triangle, we make a change of variables to
+ // the (u,v) coordinates of the triangle:
+ // x = x0 + e1x * u + e2x * v
+ // y = y0 + e1y * u + e2y * v
+ // where 0 <= u && 0 <= v && u + v <= 1.
+ //
+ // We integrate u from [0,1-v] and then v from [0,1].
+ // We also need to use the Jacobian of the transformation:
+ // D = cross(e1, e2)
+ //
+ // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
+ //
+ // The rest of the derivation is handled by computer algebra.
+
+ b2Assert(m_count >= 3);
+
+ b2Vec2 center; center.Set(0.0f, 0.0f);
+ float32 area = 0.0f;
+ float32 I = 0.0f;
+
+ // s is the reference point for forming triangles.
+ // It's location doesn't change the result (except for rounding error).
+ b2Vec2 s(0.0f, 0.0f);
+
+ // This code would put the reference point inside the polygon.
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ s += m_vertices[i];
+ }
+ s *= 1.0f / m_count;
+
+ const float32 k_inv3 = 1.0f / 3.0f;
+
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ // Triangle vertices.
+ b2Vec2 e1 = m_vertices[i] - s;
+ b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
+
+ float32 D = b2Cross(e1, e2);
+
+ float32 triangleArea = 0.5f * D;
+ area += triangleArea;
+
+ // Area weighted centroid
+ center += triangleArea * k_inv3 * (e1 + e2);
+
+ float32 ex1 = e1.x, ey1 = e1.y;
+ float32 ex2 = e2.x, ey2 = e2.y;
+
+ float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
+ float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
+
+ I += (0.25f * k_inv3 * D) * (intx2 + inty2);
+ }
+
+ // Total mass
+ massData->mass = density * area;
+
+ // Center of mass
+ b2Assert(area > b2_epsilon);
+ center *= 1.0f / area;
+ massData->center = center + s;
+
+ // Inertia tensor relative to the local origin (point s).
+ massData->I = density * I;
+
+ // Shift to center of mass then to original body origin.
+ massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
+}
+
+bool b2PolygonShape::Validate() const
+{
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ int32 i1 = i;
+ int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
+ b2Vec2 p = m_vertices[i1];
+ b2Vec2 e = m_vertices[i2] - p;
+
+ for (int32 j = 0; j < m_count; ++j)
+ {
+ if (j == i1 || j == i2)
+ {
+ continue;
+ }
+
+ b2Vec2 v = m_vertices[j] - p;
+ float32 c = b2Cross(e, v);
+ if (c < 0.0f)
+ {
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2PolygonShape.h b/Dependencies/Box2D/src/Collision/Shapes/b2PolygonShape.h
new file mode 100644
index 00000000..38050bc7
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2PolygonShape.h
@@ -0,0 +1,101 @@
+/*
+* 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_POLYGON_SHAPE_H
+#define B2_POLYGON_SHAPE_H
+
+#include
+
+/// A convex polygon. It is assumed that the interior of the polygon is to
+/// the left of each edge.
+/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.
+/// In most cases you should not need many vertices for a convex polygon.
+class b2PolygonShape : public b2Shape
+{
+public:
+ b2PolygonShape();
+
+ /// Implement b2Shape.
+ b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+ /// @see b2Shape::GetChildCount
+ int32 GetChildCount() const;
+
+ /// Create a convex hull from the given array of local points.
+ /// The count must be in the range [3, b2_maxPolygonVertices].
+ /// @warning the points may be re-ordered, even if they form a convex polygon
+ /// @warning collinear points are handled but not removed. Collinear points
+ /// may lead to poor stacking behavior.
+ void Set(const b2Vec2* points, int32 count);
+
+ /// Build vertices to represent an axis-aligned box centered on the local origin.
+ /// @param hx the half-width.
+ /// @param hy the half-height.
+ void SetAsBox(float32 hx, float32 hy);
+
+ /// Build vertices to represent an oriented box.
+ /// @param hx the half-width.
+ /// @param hy the half-height.
+ /// @param center the center of the box in local coordinates.
+ /// @param angle the rotation of the box in local coordinates.
+ void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);
+
+ /// @see b2Shape::TestPoint
+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+ /// Implement b2Shape.
+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& transform, int32 childIndex) const;
+
+ /// @see b2Shape::ComputeAABB
+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
+
+ /// @see b2Shape::ComputeMass
+ void ComputeMass(b2MassData* massData, float32 density) const;
+
+ /// Get the vertex count.
+ int32 GetVertexCount() const { return m_count; }
+
+ /// Get a vertex by index.
+ const b2Vec2& GetVertex(int32 index) const;
+
+ /// Validate convexity. This is a very time consuming operation.
+ /// @returns true if valid
+ bool Validate() const;
+
+ b2Vec2 m_centroid;
+ b2Vec2 m_vertices[b2_maxPolygonVertices];
+ b2Vec2 m_normals[b2_maxPolygonVertices];
+ int32 m_count;
+};
+
+inline b2PolygonShape::b2PolygonShape()
+{
+ m_type = e_polygon;
+ m_radius = b2_polygonRadius;
+ m_count = 0;
+ m_centroid.SetZero();
+}
+
+inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const
+{
+ b2Assert(0 <= index && index < m_count);
+ return m_vertices[index];
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Collision/Shapes/b2Shape.h b/Dependencies/Box2D/src/Collision/Shapes/b2Shape.h
new file mode 100644
index 00000000..8a665bc5
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/Shapes/b2Shape.h
@@ -0,0 +1,101 @@
+/*
+* 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_SHAPE_H
+#define B2_SHAPE_H
+
+#include
+#include
+#include
+
+/// This holds the mass data computed for a shape.
+struct b2MassData
+{
+ /// The mass of the shape, usually in kilograms.
+ float32 mass;
+
+ /// The position of the shape's centroid relative to the shape's origin.
+ b2Vec2 center;
+
+ /// The rotational inertia of the shape about the local origin.
+ float32 I;
+};
+
+/// A shape is used for collision detection. You can create a shape however you like.
+/// Shapes used for simulation in b2World are created automatically when a b2Fixture
+/// is created. Shapes may encapsulate a one or more child shapes.
+class b2Shape
+{
+public:
+
+ enum Type
+ {
+ e_circle = 0,
+ e_edge = 1,
+ e_polygon = 2,
+ e_chain = 3,
+ e_typeCount = 4
+ };
+
+ virtual ~b2Shape() {}
+
+ /// Clone the concrete shape using the provided allocator.
+ virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0;
+
+ /// Get the type of this shape. You can use this to down cast to the concrete shape.
+ /// @return the shape type.
+ Type GetType() const;
+
+ /// Get the number of child primitives.
+ virtual int32 GetChildCount() const = 0;
+
+ /// Test a point for containment in this shape. This only works for convex shapes.
+ /// @param xf the shape world transform.
+ /// @param p a point in world coordinates.
+ virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;
+
+ /// Cast a ray against a child shape.
+ /// @param output the ray-cast results.
+ /// @param input the ray-cast input parameters.
+ /// @param transform the transform to be applied to the shape.
+ /// @param childIndex the child shape index
+ virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+ const b2Transform& transform, int32 childIndex) const = 0;
+
+ /// Given a transform, compute the associated axis aligned bounding box for a child shape.
+ /// @param aabb returns the axis aligned box.
+ /// @param xf the world transform of the shape.
+ /// @param childIndex the child shape
+ virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0;
+
+ /// Compute the mass properties of this shape using its dimensions and density.
+ /// The inertia tensor is computed about the local origin.
+ /// @param massData returns the mass data for this shape.
+ /// @param density the density in kilograms per meter squared.
+ virtual void ComputeMass(b2MassData* massData, float32 density) const = 0;
+
+ Type m_type;
+ float32 m_radius;
+};
+
+inline b2Shape::Type b2Shape::GetType() const
+{
+ return m_type;
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Collision/b2BroadPhase.cpp b/Dependencies/Box2D/src/Collision/b2BroadPhase.cpp
new file mode 100644
index 00000000..1940b24b
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2BroadPhase.cpp
@@ -0,0 +1,119 @@
+/*
+* 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
+
+b2BroadPhase::b2BroadPhase()
+{
+ m_proxyCount = 0;
+
+ m_pairCapacity = 16;
+ m_pairCount = 0;
+ m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
+
+ m_moveCapacity = 16;
+ m_moveCount = 0;
+ m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
+}
+
+b2BroadPhase::~b2BroadPhase()
+{
+ b2Free(m_moveBuffer);
+ b2Free(m_pairBuffer);
+}
+
+int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
+{
+ int32 proxyId = m_tree.CreateProxy(aabb, userData);
+ ++m_proxyCount;
+ BufferMove(proxyId);
+ return proxyId;
+}
+
+void b2BroadPhase::DestroyProxy(int32 proxyId)
+{
+ UnBufferMove(proxyId);
+ --m_proxyCount;
+ m_tree.DestroyProxy(proxyId);
+}
+
+void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
+{
+ bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement);
+ if (buffer)
+ {
+ BufferMove(proxyId);
+ }
+}
+
+void b2BroadPhase::TouchProxy(int32 proxyId)
+{
+ BufferMove(proxyId);
+}
+
+void b2BroadPhase::BufferMove(int32 proxyId)
+{
+ if (m_moveCount == m_moveCapacity)
+ {
+ int32* oldBuffer = m_moveBuffer;
+ m_moveCapacity *= 2;
+ m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
+ memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32));
+ b2Free(oldBuffer);
+ }
+
+ m_moveBuffer[m_moveCount] = proxyId;
+ ++m_moveCount;
+}
+
+void b2BroadPhase::UnBufferMove(int32 proxyId)
+{
+ for (int32 i = 0; i < m_moveCount; ++i)
+ {
+ if (m_moveBuffer[i] == proxyId)
+ {
+ m_moveBuffer[i] = e_nullProxy;
+ }
+ }
+}
+
+// This is called from b2DynamicTree::Query when we are gathering pairs.
+bool b2BroadPhase::QueryCallback(int32 proxyId)
+{
+ // A proxy cannot form a pair with itself.
+ if (proxyId == m_queryProxyId)
+ {
+ return true;
+ }
+
+ // Grow the pair buffer as needed.
+ if (m_pairCount == m_pairCapacity)
+ {
+ b2Pair* oldBuffer = m_pairBuffer;
+ m_pairCapacity *= 2;
+ m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
+ memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
+ b2Free(oldBuffer);
+ }
+
+ m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId);
+ m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId);
+ ++m_pairCount;
+
+ return true;
+}
diff --git a/Dependencies/Box2D/src/Collision/b2BroadPhase.h b/Dependencies/Box2D/src/Collision/b2BroadPhase.h
new file mode 100644
index 00000000..4764fdd6
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2BroadPhase.h
@@ -0,0 +1,257 @@
+/*
+* 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_BROAD_PHASE_H
+#define B2_BROAD_PHASE_H
+
+#include
+#include
+#include
+#include
+
+struct b2Pair
+{
+ int32 proxyIdA;
+ int32 proxyIdB;
+};
+
+/// The broad-phase is used for computing pairs and performing volume queries and ray casts.
+/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
+/// It is up to the client to consume the new pairs and to track subsequent overlap.
+class b2BroadPhase
+{
+public:
+
+ enum
+ {
+ e_nullProxy = -1
+ };
+
+ b2BroadPhase();
+ ~b2BroadPhase();
+
+ /// Create a proxy with an initial AABB. Pairs are not reported until
+ /// UpdatePairs is called.
+ int32 CreateProxy(const b2AABB& aabb, void* userData);
+
+ /// Destroy a proxy. It is up to the client to remove any pairs.
+ void DestroyProxy(int32 proxyId);
+
+ /// Call MoveProxy as many times as you like, then when you are done
+ /// call UpdatePairs to finalized the proxy pairs (for your time step).
+ void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);
+
+ /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
+ void TouchProxy(int32 proxyId);
+
+ /// Get the fat AABB for a proxy.
+ const b2AABB& GetFatAABB(int32 proxyId) const;
+
+ /// Get user data from a proxy. Returns NULL if the id is invalid.
+ void* GetUserData(int32 proxyId) const;
+
+ /// Test overlap of fat AABBs.
+ bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const;
+
+ /// Get the number of proxies.
+ int32 GetProxyCount() const;
+
+ /// Update the pairs. This results in pair callbacks. This can only add pairs.
+ template
+ void UpdatePairs(T* callback);
+
+ /// Query an AABB for overlapping proxies. The callback class
+ /// is called for each proxy that overlaps the supplied AABB.
+ template
+ 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
+ void RayCast(T* callback, const b2RayCastInput& input) const;
+
+ /// Get the height of the embedded tree.
+ int32 GetTreeHeight() const;
+
+ /// Get the balance of the embedded tree.
+ int32 GetTreeBalance() const;
+
+ /// Get the quality metric of the embedded tree.
+ float32 GetTreeQuality() const;
+
+ /// 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:
+
+ friend class b2DynamicTree;
+
+ void BufferMove(int32 proxyId);
+ void UnBufferMove(int32 proxyId);
+
+ bool QueryCallback(int32 proxyId);
+
+ b2DynamicTree m_tree;
+
+ int32 m_proxyCount;
+
+ int32* m_moveBuffer;
+ int32 m_moveCapacity;
+ int32 m_moveCount;
+
+ b2Pair* m_pairBuffer;
+ int32 m_pairCapacity;
+ int32 m_pairCount;
+
+ int32 m_queryProxyId;
+};
+
+/// This is used to sort pairs.
+inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2)
+{
+ if (pair1.proxyIdA < pair2.proxyIdA)
+ {
+ return true;
+ }
+
+ if (pair1.proxyIdA == pair2.proxyIdA)
+ {
+ return pair1.proxyIdB < pair2.proxyIdB;
+ }
+
+ return false;
+}
+
+inline void* b2BroadPhase::GetUserData(int32 proxyId) const
+{
+ return m_tree.GetUserData(proxyId);
+}
+
+inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const
+{
+ const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA);
+ const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB);
+ return b2TestOverlap(aabbA, aabbB);
+}
+
+inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const
+{
+ return m_tree.GetFatAABB(proxyId);
+}
+
+inline int32 b2BroadPhase::GetProxyCount() const
+{
+ return m_proxyCount;
+}
+
+inline int32 b2BroadPhase::GetTreeHeight() const
+{
+ return m_tree.GetHeight();
+}
+
+inline int32 b2BroadPhase::GetTreeBalance() const
+{
+ return m_tree.GetMaxBalance();
+}
+
+inline float32 b2BroadPhase::GetTreeQuality() const
+{
+ return m_tree.GetAreaRatio();
+}
+
+template
+void b2BroadPhase::UpdatePairs(T* callback)
+{
+ // Reset pair buffer
+ m_pairCount = 0;
+
+ // Perform tree queries for all moving proxies.
+ for (int32 i = 0; i < m_moveCount; ++i)
+ {
+ m_queryProxyId = m_moveBuffer[i];
+ if (m_queryProxyId == e_nullProxy)
+ {
+ continue;
+ }
+
+ // We have to query the tree with the fat AABB so that
+ // we don't fail to create a pair that may touch later.
+ const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId);
+
+ // Query tree, create pairs and add them pair buffer.
+ m_tree.Query(this, fatAABB);
+ }
+
+ // Reset move buffer
+ m_moveCount = 0;
+
+ // Sort the pair buffer to expose duplicates.
+ std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan);
+
+ // Send the pairs back to the client.
+ int32 i = 0;
+ while (i < m_pairCount)
+ {
+ b2Pair* primaryPair = m_pairBuffer + i;
+ void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA);
+ void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB);
+
+ callback->AddPair(userDataA, userDataB);
+ ++i;
+
+ // Skip any duplicate pairs.
+ while (i < m_pairCount)
+ {
+ b2Pair* pair = m_pairBuffer + i;
+ if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB)
+ {
+ break;
+ }
+ ++i;
+ }
+ }
+
+ // Try to keep the tree balanced.
+ //m_tree.Rebalance(4);
+}
+
+template
+inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const
+{
+ m_tree.Query(callback, aabb);
+}
+
+template
+inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const
+{
+ m_tree.RayCast(callback, input);
+}
+
+inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin)
+{
+ m_tree.ShiftOrigin(newOrigin);
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Collision/b2CollideCircle.cpp b/Dependencies/Box2D/src/Collision/b2CollideCircle.cpp
new file mode 100644
index 00000000..573c8ba9
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2CollideCircle.cpp
@@ -0,0 +1,154 @@
+/*
+* 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
+#include
+#include
+
+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 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
+ if (separation > radius)
+ {
+ return;
+ }
+
+ manifold->pointCount = 1;
+ manifold->type = b2Manifold::e_faceA;
+ manifold->localNormal = normals[vertIndex1];
+ manifold->localPoint = faceCenter;
+ manifold->points[0].localPoint = circleB->m_p;
+ manifold->points[0].id.key = 0;
+ }
+}
diff --git a/Dependencies/Box2D/src/Collision/b2CollideEdge.cpp b/Dependencies/Box2D/src/Collision/b2CollideEdge.cpp
new file mode 100644
index 00000000..7d2ec478
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2CollideEdge.cpp
@@ -0,0 +1,698 @@
+/*
+ * 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
+#include
+#include
+#include
+
+
+// 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 = 2.0f * b2_polygonRadius;
+
+ 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(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(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(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(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);
+}
diff --git a/Dependencies/Box2D/src/Collision/b2CollidePolygon.cpp b/Dependencies/Box2D/src/Collision/b2CollidePolygon.cpp
new file mode 100644
index 00000000..08d81eff
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2CollidePolygon.cpp
@@ -0,0 +1,239 @@
+/*
+* 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
+#include
+
+// 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;
+}
diff --git a/Dependencies/Box2D/src/Collision/b2Collision.cpp b/Dependencies/Box2D/src/Collision/b2Collision.cpp
new file mode 100644
index 00000000..48aa2360
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2Collision.cpp
@@ -0,0 +1,252 @@
+/*
+* 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
+#include
+
+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(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;
+}
diff --git a/Dependencies/Box2D/src/Collision/b2Collision.h b/Dependencies/Box2D/src/Collision/b2Collision.h
new file mode 100644
index 00000000..0fbf780f
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2Collision.h
@@ -0,0 +1,277 @@
+/*
+* 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
+#include
+
+/// @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
diff --git a/Dependencies/Box2D/src/Collision/b2Distance.cpp b/Dependencies/Box2D/src/Collision/b2Distance.cpp
new file mode 100644
index 00000000..c4a7d961
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2Distance.cpp
@@ -0,0 +1,603 @@
+/*
+* 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
+#include
+#include
+#include
+#include
+
+// 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(shape);
+ m_vertices = &circle->m_p;
+ m_count = 1;
+ m_radius = circle->m_radius;
+ }
+ break;
+
+ case b2Shape::e_polygon:
+ {
+ const b2PolygonShape* polygon = static_cast(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(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(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;
+
+ float32 distanceSqr1 = b2_maxFloat;
+ float32 distanceSqr2 = distanceSqr1;
+
+ // Main iteration loop.
+ int32 iter = 0;
+ while (iter < k_maxIters)
+ {
+ // Copy simplex so we can identify duplicates.
+ saveCount = simplex.m_count;
+ for (int32 i = 0; i < saveCount; ++i)
+ {
+ saveA[i] = vertices[i].indexA;
+ saveB[i] = vertices[i].indexB;
+ }
+
+ switch (simplex.m_count)
+ {
+ case 1:
+ break;
+
+ case 2:
+ simplex.Solve2();
+ break;
+
+ case 3:
+ simplex.Solve3();
+ break;
+
+ default:
+ b2Assert(false);
+ }
+
+ // If we have 3 points, then the origin is in the corresponding triangle.
+ if (simplex.m_count == 3)
+ {
+ break;
+ }
+
+ // Compute closest point.
+ b2Vec2 p = simplex.GetClosestPoint();
+ distanceSqr2 = p.LengthSquared();
+
+ // Ensure progress
+ if (distanceSqr2 >= distanceSqr1)
+ {
+ //break;
+ }
+ distanceSqr1 = distanceSqr2;
+
+ // Get search direction.
+ b2Vec2 d = simplex.GetSearchDirection();
+
+ // Ensure the search direction is numerically fit.
+ if (d.LengthSquared() < b2_epsilon * b2_epsilon)
+ {
+ // The origin is probably contained by a line segment
+ // or triangle. Thus the shapes are overlapped.
+
+ // We can't return zero here even though there may be overlap.
+ // In case the simplex is a point, segment, or triangle it is difficult
+ // to determine if the origin is contained in the CSO or very close to it.
+ break;
+ }
+
+ // Compute a tentative new simplex vertex using support points.
+ b2SimplexVertex* vertex = vertices + simplex.m_count;
+ vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
+ vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
+ b2Vec2 wBLocal;
+ vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
+ vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
+ vertex->w = vertex->wB - vertex->wA;
+
+ // Iteration count is equated to the number of support point calls.
+ ++iter;
+ ++b2_gjkIters;
+
+ // Check for duplicate support points. This is the main termination criteria.
+ bool duplicate = false;
+ for (int32 i = 0; i < saveCount; ++i)
+ {
+ if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
+ {
+ duplicate = true;
+ break;
+ }
+ }
+
+ // If we found a duplicate support point we must exit to avoid cycling.
+ if (duplicate)
+ {
+ break;
+ }
+
+ // New vertex is ok and needed.
+ ++simplex.m_count;
+ }
+
+ b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
+
+ // Prepare output.
+ simplex.GetWitnessPoints(&output->pointA, &output->pointB);
+ output->distance = b2Distance(output->pointA, output->pointB);
+ output->iterations = iter;
+
+ // Cache the simplex.
+ simplex.WriteCache(cache);
+
+ // Apply radii if requested.
+ if (input->useRadii)
+ {
+ float32 rA = proxyA->m_radius;
+ float32 rB = proxyB->m_radius;
+
+ if (output->distance > rA + rB && output->distance > b2_epsilon)
+ {
+ // Shapes are still no overlapped.
+ // Move the witness points to the outer surface.
+ output->distance -= rA + rB;
+ b2Vec2 normal = output->pointB - output->pointA;
+ normal.Normalize();
+ output->pointA += rA * normal;
+ output->pointB -= rB * normal;
+ }
+ else
+ {
+ // Shapes are overlapped when radii are considered.
+ // Move the witness points to the middle.
+ b2Vec2 p = 0.5f * (output->pointA + output->pointB);
+ output->pointA = p;
+ output->pointB = p;
+ output->distance = 0.0f;
+ }
+ }
+}
diff --git a/Dependencies/Box2D/src/Collision/b2Distance.h b/Dependencies/Box2D/src/Collision/b2Distance.h
new file mode 100644
index 00000000..e380cacb
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2Distance.h
@@ -0,0 +1,141 @@
+
+/*
+* 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
+
+class b2Shape;
+
+/// A distance proxy is used by the GJK algorithm.
+/// It encapsulates any shape.
+struct b2DistanceProxy
+{
+ b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {}
+
+ /// Initialize the proxy using the given shape. The shape
+ /// must remain in scope while the proxy is in use.
+ void Set(const b2Shape* shape, 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
diff --git a/Dependencies/Box2D/src/Collision/b2DynamicTree.cpp b/Dependencies/Box2D/src/Collision/b2DynamicTree.cpp
new file mode 100644
index 00000000..1b60efa4
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2DynamicTree.cpp
@@ -0,0 +1,778 @@
+/*
+* 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
+#include
+
+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 = NULL;
+ ++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 = NULL;
+ 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
+{
+ 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);
+}
+
+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;
+ }
+}
diff --git a/Dependencies/Box2D/src/Collision/b2DynamicTree.h b/Dependencies/Box2D/src/Collision/b2DynamicTree.h
new file mode 100644
index 00000000..702e76da
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2DynamicTree.h
@@ -0,0 +1,289 @@
+/*
+* 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
+#include
+
+#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
+ 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
+ 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
+inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
+{
+ b2GrowableStack 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
+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 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
diff --git a/Dependencies/Box2D/src/Collision/b2TimeOfImpact.cpp b/Dependencies/Box2D/src/Collision/b2TimeOfImpact.cpp
new file mode 100644
index 00000000..9c3fba6c
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2TimeOfImpact.cpp
@@ -0,0 +1,486 @@
+/*
+* 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
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+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;
+}
diff --git a/Dependencies/Box2D/src/Collision/b2TimeOfImpact.h b/Dependencies/Box2D/src/Collision/b2TimeOfImpact.h
new file mode 100644
index 00000000..6373c31f
--- /dev/null
+++ b/Dependencies/Box2D/src/Collision/b2TimeOfImpact.h
@@ -0,0 +1,58 @@
+/*
+* 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
+#include
+
+/// 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
diff --git a/Dependencies/Box2D/src/Common/b2BlockAllocator.cpp b/Dependencies/Box2D/src/Common/b2BlockAllocator.cpp
new file mode 100644
index 00000000..419f315f
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2BlockAllocator.cpp
@@ -0,0 +1,215 @@
+/*
+* 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
+#include
+#include
+#include
+
+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 NULL;
+
+ 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 = NULL;
+
+ 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));
+}
diff --git a/Dependencies/Box2D/src/Common/b2BlockAllocator.h b/Dependencies/Box2D/src/Common/b2BlockAllocator.h
new file mode 100644
index 00000000..618cc2d1
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2BlockAllocator.h
@@ -0,0 +1,62 @@
+/*
+* 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
+
+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
diff --git a/Dependencies/Box2D/src/Common/b2Draw.cpp b/Dependencies/Box2D/src/Common/b2Draw.cpp
new file mode 100644
index 00000000..49134950
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2Draw.cpp
@@ -0,0 +1,44 @@
+/*
+* 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
+
+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;
+}
diff --git a/Dependencies/Box2D/src/Common/b2Draw.h b/Dependencies/Box2D/src/Common/b2Draw.h
new file mode 100644
index 00000000..02d7fcb7
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2Draw.h
@@ -0,0 +1,86 @@
+/*
+* 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
+
+/// Color for debug drawing. Each value has the range [0,1].
+struct b2Color
+{
+ b2Color() {}
+ b2Color(float32 r, float32 g, float32 b, float32 a = 1.0f) : r(r), g(g), b(b), a(a) {}
+ void Set(float32 ri, float32 gi, float32 bi, float32 ai = 1.0f) { r = ri; g = gi; b = bi; a = ai; }
+ 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;
+
+protected:
+ uint32 m_drawFlags;
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Common/b2GrowableStack.h b/Dependencies/Box2D/src/Common/b2GrowableStack.h
new file mode 100644
index 00000000..27aa27be
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2GrowableStack.h
@@ -0,0 +1,85 @@
+/*
+* 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
+#include
+
+/// 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
+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 = NULL;
+ }
+ }
+
+ 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
diff --git a/Dependencies/Box2D/src/Common/b2Math.cpp b/Dependencies/Box2D/src/Common/b2Math.cpp
new file mode 100644
index 00000000..5e820106
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2Math.cpp
@@ -0,0 +1,94 @@
+/*
+* 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
+
+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);
+}
diff --git a/Dependencies/Box2D/src/Common/b2Math.h b/Dependencies/Box2D/src/Common/b2Math.h
new file mode 100644
index 00000000..65ec28f1
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2Math.h
@@ -0,0 +1,720 @@
+/*
+* 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
+#include
+
+/// 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(&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 x, float32 y) : x(x), y(y) {}
+
+ /// 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 x, float32 y, float32 z) : x(x), y(y), z(z) {}
+
+ /// 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 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
+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
+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
+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
+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 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
diff --git a/Dependencies/Box2D/src/Common/b2Settings.cpp b/Dependencies/Box2D/src/Common/b2Settings.cpp
new file mode 100644
index 00000000..d7c81bac
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2Settings.cpp
@@ -0,0 +1,44 @@
+/*
+* 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
+#include
+#include
+#include
+
+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);
+}
diff --git a/Dependencies/Box2D/src/Common/b2Settings.h b/Dependencies/Box2D/src/Common/b2Settings.h
new file mode 100644
index 00000000..86f76bd2
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2Settings.h
@@ -0,0 +1,151 @@
+/*
+* 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
+#include
+#include
+
+#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
diff --git a/Dependencies/Box2D/src/Common/b2StackAllocator.cpp b/Dependencies/Box2D/src/Common/b2StackAllocator.cpp
new file mode 100644
index 00000000..e76e0bd6
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2StackAllocator.cpp
@@ -0,0 +1,83 @@
+/*
+* 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
+#include
+
+b2StackAllocator::b2StackAllocator()
+{
+ m_index = 0;
+ m_allocation = 0;
+ m_maxAllocation = 0;
+ m_entryCount = 0;
+}
+
+b2StackAllocator::~b2StackAllocator()
+{
+ b2Assert(m_index == 0);
+ b2Assert(m_entryCount == 0);
+}
+
+void* b2StackAllocator::Allocate(int32 size)
+{
+ b2Assert(m_entryCount < b2_maxStackEntries);
+
+ b2StackEntry* entry = m_entries + m_entryCount;
+ entry->size = size;
+ if (m_index + size > b2_stackSize)
+ {
+ entry->data = (char*)b2Alloc(size);
+ entry->usedMalloc = true;
+ }
+ else
+ {
+ entry->data = m_data + m_index;
+ entry->usedMalloc = false;
+ m_index += size;
+ }
+
+ m_allocation += size;
+ m_maxAllocation = b2Max(m_maxAllocation, m_allocation);
+ ++m_entryCount;
+
+ return entry->data;
+}
+
+void b2StackAllocator::Free(void* p)
+{
+ b2Assert(m_entryCount > 0);
+ b2StackEntry* entry = m_entries + m_entryCount - 1;
+ b2Assert(p == entry->data);
+ if (entry->usedMalloc)
+ {
+ b2Free(p);
+ }
+ else
+ {
+ m_index -= entry->size;
+ }
+ m_allocation -= entry->size;
+ --m_entryCount;
+
+ p = NULL;
+}
+
+int32 b2StackAllocator::GetMaxAllocation() const
+{
+ return m_maxAllocation;
+}
diff --git a/Dependencies/Box2D/src/Common/b2StackAllocator.h b/Dependencies/Box2D/src/Common/b2StackAllocator.h
new file mode 100644
index 00000000..6e0a0287
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2StackAllocator.h
@@ -0,0 +1,60 @@
+/*
+* 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_STACK_ALLOCATOR_H
+#define B2_STACK_ALLOCATOR_H
+
+#include
+
+const int32 b2_stackSize = 100 * 1024; // 100k
+const int32 b2_maxStackEntries = 32;
+
+struct b2StackEntry
+{
+ char* data;
+ int32 size;
+ bool usedMalloc;
+};
+
+// This is a stack allocator used for fast per step allocations.
+// You must nest allocate/free pairs. The code will assert
+// if you try to interleave multiple allocate/free pairs.
+class b2StackAllocator
+{
+public:
+ b2StackAllocator();
+ ~b2StackAllocator();
+
+ void* Allocate(int32 size);
+ void Free(void* p);
+
+ int32 GetMaxAllocation() const;
+
+private:
+
+ char m_data[b2_stackSize];
+ int32 m_index;
+
+ int32 m_allocation;
+ int32 m_maxAllocation;
+
+ b2StackEntry m_entries[b2_maxStackEntries];
+ int32 m_entryCount;
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Common/b2Timer.cpp b/Dependencies/Box2D/src/Common/b2Timer.cpp
new file mode 100644
index 00000000..4aabf07a
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2Timer.cpp
@@ -0,0 +1,101 @@
+/*
+* 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
+
+#if defined(_WIN32)
+
+float64 b2Timer::s_invFrequency = 0.0f;
+
+#define WIN32_LEAN_AND_MEAN
+#include
+
+b2Timer::b2Timer()
+{
+ LARGE_INTEGER largeInteger;
+
+ if (s_invFrequency == 0.0f)
+ {
+ QueryPerformanceFrequency(&largeInteger);
+ s_invFrequency = float64(largeInteger.QuadPart);
+ if (s_invFrequency > 0.0f)
+ {
+ s_invFrequency = 1000.0f / s_invFrequency;
+ }
+ }
+
+ QueryPerformanceCounter(&largeInteger);
+ m_start = float64(largeInteger.QuadPart);
+}
+
+void b2Timer::Reset()
+{
+ LARGE_INTEGER largeInteger;
+ QueryPerformanceCounter(&largeInteger);
+ m_start = float64(largeInteger.QuadPart);
+}
+
+float32 b2Timer::GetMilliseconds() const
+{
+ LARGE_INTEGER largeInteger;
+ QueryPerformanceCounter(&largeInteger);
+ float64 count = float64(largeInteger.QuadPart);
+ float32 ms = float32(s_invFrequency * (count - m_start));
+ return ms;
+}
+
+#elif defined(__linux__) || defined (__APPLE__)
+
+#include
+
+b2Timer::b2Timer()
+{
+ Reset();
+}
+
+void b2Timer::Reset()
+{
+ timeval t;
+ gettimeofday(&t, 0);
+ m_start_sec = t.tv_sec;
+ m_start_usec = t.tv_usec;
+}
+
+float32 b2Timer::GetMilliseconds() const
+{
+ timeval t;
+ gettimeofday(&t, 0);
+ return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec);
+}
+
+#else
+
+b2Timer::b2Timer()
+{
+}
+
+void b2Timer::Reset()
+{
+}
+
+float32 b2Timer::GetMilliseconds() const
+{
+ return 0.0f;
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Common/b2Timer.h b/Dependencies/Box2D/src/Common/b2Timer.h
new file mode 100644
index 00000000..5d029fad
--- /dev/null
+++ b/Dependencies/Box2D/src/Common/b2Timer.h
@@ -0,0 +1,50 @@
+/*
+* 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_TIMER_H
+#define B2_TIMER_H
+
+#include
+
+/// Timer for profiling. This has platform specific code and may
+/// not work on every platform.
+class b2Timer
+{
+public:
+
+ /// Constructor
+ b2Timer();
+
+ /// Reset the timer.
+ void Reset();
+
+ /// Get the time since construction or the last reset.
+ float32 GetMilliseconds() const;
+
+private:
+
+#if defined(_WIN32)
+ float64 m_start;
+ static float64 s_invFrequency;
+#elif defined(__linux__) || defined (__APPLE__)
+ unsigned long m_start_sec;
+ unsigned long m_start_usec;
+#endif
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp
new file mode 100644
index 00000000..f01673ac
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp
@@ -0,0 +1,53 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact));
+ return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB);
+}
+
+void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();
+ allocator->Free(contact, sizeof(b2ChainAndCircleContact));
+}
+
+b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
+: b2Contact(fixtureA, indexA, fixtureB, indexB)
+{
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+ b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
+ b2EdgeShape edge;
+ chain->GetChildEdge(&edge, m_indexA);
+ b2CollideEdgeAndCircle( manifold, &edge, xfA,
+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndCircleContact.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndCircleContact.h
new file mode 100644
index 00000000..40894dc6
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndCircleContact.h
@@ -0,0 +1,39 @@
+/*
+* 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_CHAIN_AND_CIRCLE_CONTACT_H
+#define B2_CHAIN_AND_CIRCLE_CONTACT_H
+
+#include
+
+class b2BlockAllocator;
+
+class b2ChainAndCircleContact : public b2Contact
+{
+public:
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+ ~b2ChainAndCircleContact() {}
+
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp
new file mode 100644
index 00000000..89915969
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp
@@ -0,0 +1,53 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact));
+ return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB);
+}
+
+void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact();
+ allocator->Free(contact, sizeof(b2ChainAndPolygonContact));
+}
+
+b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
+: b2Contact(fixtureA, indexA, fixtureB, indexB)
+{
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+ b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
+ b2EdgeShape edge;
+ chain->GetChildEdge(&edge, m_indexA);
+ b2CollideEdgeAndPolygon( manifold, &edge, xfA,
+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndPolygonContact.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndPolygonContact.h
new file mode 100644
index 00000000..4a15a6f6
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2ChainAndPolygonContact.h
@@ -0,0 +1,39 @@
+/*
+* 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_CHAIN_AND_POLYGON_CONTACT_H
+#define B2_CHAIN_AND_POLYGON_CONTACT_H
+
+#include
+
+class b2BlockAllocator;
+
+class b2ChainAndPolygonContact : public b2Contact
+{
+public:
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+ ~b2ChainAndPolygonContact() {}
+
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2CircleContact.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2CircleContact.cpp
new file mode 100644
index 00000000..c234bece
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2CircleContact.cpp
@@ -0,0 +1,52 @@
+/*
+* 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
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2CircleContact));
+ return new (mem) b2CircleContact(fixtureA, fixtureB);
+}
+
+void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2CircleContact*)contact)->~b2CircleContact();
+ allocator->Free(contact, sizeof(b2CircleContact));
+}
+
+b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+ : b2Contact(fixtureA, 0, fixtureB, 0)
+{
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+ b2CollideCircles(manifold,
+ (b2CircleShape*)m_fixtureA->GetShape(), xfA,
+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2CircleContact.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2CircleContact.h
new file mode 100644
index 00000000..8cec3167
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2CircleContact.h
@@ -0,0 +1,39 @@
+/*
+* 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_CIRCLE_CONTACT_H
+#define B2_CIRCLE_CONTACT_H
+
+#include
+
+class b2BlockAllocator;
+
+class b2CircleContact : public b2Contact
+{
+public:
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+ ~b2CircleContact() {}
+
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2Contact.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2Contact.cpp
new file mode 100644
index 00000000..5c8cc6ff
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2Contact.cpp
@@ -0,0 +1,247 @@
+/*
+* 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
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
+bool b2Contact::s_initialized = false;
+
+void b2Contact::InitializeRegisters()
+{
+ AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
+ AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
+ AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
+ AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle);
+ AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon);
+ AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle);
+ AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon);
+}
+
+void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
+ b2Shape::Type type1, b2Shape::Type type2)
+{
+ b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
+ b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
+
+ s_registers[type1][type2].createFcn = createFcn;
+ s_registers[type1][type2].destroyFcn = destoryFcn;
+ s_registers[type1][type2].primary = true;
+
+ if (type1 != type2)
+ {
+ s_registers[type2][type1].createFcn = createFcn;
+ s_registers[type2][type1].destroyFcn = destoryFcn;
+ s_registers[type2][type1].primary = false;
+ }
+}
+
+b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+{
+ if (s_initialized == false)
+ {
+ InitializeRegisters();
+ s_initialized = true;
+ }
+
+ b2Shape::Type type1 = fixtureA->GetType();
+ b2Shape::Type type2 = fixtureB->GetType();
+
+ b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
+ b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
+
+ b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
+ if (createFcn)
+ {
+ if (s_registers[type1][type2].primary)
+ {
+ return createFcn(fixtureA, indexA, fixtureB, indexB, allocator);
+ }
+ else
+ {
+ return createFcn(fixtureB, indexB, fixtureA, indexA, allocator);
+ }
+ }
+ else
+ {
+ return NULL;
+ }
+}
+
+void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ b2Assert(s_initialized == true);
+
+ b2Fixture* fixtureA = contact->m_fixtureA;
+ b2Fixture* fixtureB = contact->m_fixtureB;
+
+ if (contact->m_manifold.pointCount > 0 &&
+ fixtureA->IsSensor() == false &&
+ fixtureB->IsSensor() == false)
+ {
+ fixtureA->GetBody()->SetAwake(true);
+ fixtureB->GetBody()->SetAwake(true);
+ }
+
+ b2Shape::Type typeA = fixtureA->GetType();
+ b2Shape::Type typeB = fixtureB->GetType();
+
+ b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
+ b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
+
+ b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
+ destroyFcn(contact, allocator);
+}
+
+b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
+{
+ m_flags = e_enabledFlag;
+
+ m_fixtureA = fA;
+ m_fixtureB = fB;
+
+ m_indexA = indexA;
+ m_indexB = indexB;
+
+ m_manifold.pointCount = 0;
+
+ m_prev = NULL;
+ m_next = NULL;
+
+ m_nodeA.contact = NULL;
+ m_nodeA.prev = NULL;
+ m_nodeA.next = NULL;
+ m_nodeA.other = NULL;
+
+ m_nodeB.contact = NULL;
+ m_nodeB.prev = NULL;
+ m_nodeB.next = NULL;
+ m_nodeB.other = NULL;
+
+ m_toiCount = 0;
+
+ m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
+ m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
+
+ m_tangentSpeed = 0.0f;
+}
+
+// Update the contact manifold and touching status.
+// Note: do not assume the fixture AABBs are overlapping or are valid.
+void b2Contact::Update(b2ContactListener* listener)
+{
+ b2Manifold oldManifold = m_manifold;
+
+ // Re-enable this contact.
+ m_flags |= e_enabledFlag;
+
+ bool touching = false;
+ bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
+
+ bool sensorA = m_fixtureA->IsSensor();
+ bool sensorB = m_fixtureB->IsSensor();
+ bool sensor = sensorA || sensorB;
+
+ b2Body* bodyA = m_fixtureA->GetBody();
+ b2Body* bodyB = m_fixtureB->GetBody();
+ const b2Transform& xfA = bodyA->GetTransform();
+ const b2Transform& xfB = bodyB->GetTransform();
+
+ // Is this contact a sensor?
+ if (sensor)
+ {
+ const b2Shape* shapeA = m_fixtureA->GetShape();
+ const b2Shape* shapeB = m_fixtureB->GetShape();
+ touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
+
+ // Sensors don't generate manifolds.
+ m_manifold.pointCount = 0;
+ }
+ else
+ {
+ Evaluate(&m_manifold, xfA, xfB);
+ touching = m_manifold.pointCount > 0;
+
+ // Match old contact ids to new contact ids and copy the
+ // stored impulses to warm start the solver.
+ for (int32 i = 0; i < m_manifold.pointCount; ++i)
+ {
+ b2ManifoldPoint* mp2 = m_manifold.points + i;
+ mp2->normalImpulse = 0.0f;
+ mp2->tangentImpulse = 0.0f;
+ b2ContactID id2 = mp2->id;
+
+ for (int32 j = 0; j < oldManifold.pointCount; ++j)
+ {
+ b2ManifoldPoint* mp1 = oldManifold.points + j;
+
+ if (mp1->id.key == id2.key)
+ {
+ mp2->normalImpulse = mp1->normalImpulse;
+ mp2->tangentImpulse = mp1->tangentImpulse;
+ break;
+ }
+ }
+ }
+
+ if (touching != wasTouching)
+ {
+ bodyA->SetAwake(true);
+ bodyB->SetAwake(true);
+ }
+ }
+
+ if (touching)
+ {
+ m_flags |= e_touchingFlag;
+ }
+ else
+ {
+ m_flags &= ~e_touchingFlag;
+ }
+
+ if (wasTouching == false && touching == true && listener)
+ {
+ listener->BeginContact(this);
+ }
+
+ if (wasTouching == true && touching == false && listener)
+ {
+ listener->EndContact(this);
+ }
+
+ if (sensor == false && touching && listener)
+ {
+ listener->PreSolve(this, &oldManifold);
+ }
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2Contact.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2Contact.h
new file mode 100644
index 00000000..dae998cb
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2Contact.h
@@ -0,0 +1,349 @@
+/*
+* 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_CONTACT_H
+#define B2_CONTACT_H
+
+#include
+#include
+#include
+#include
+
+class b2Body;
+class b2Contact;
+class b2Fixture;
+class b2World;
+class b2BlockAllocator;
+class b2StackAllocator;
+class b2ContactListener;
+
+/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero.
+/// For example, anything slides on ice.
+inline float32 b2MixFriction(float32 friction1, float32 friction2)
+{
+ return b2Sqrt(friction1 * friction2);
+}
+
+/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
+/// For example, a superball bounces on anything.
+inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)
+{
+ return restitution1 > restitution2 ? restitution1 : restitution2;
+}
+
+typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA,
+ b2Fixture* fixtureB, int32 indexB,
+ b2BlockAllocator* allocator);
+typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);
+
+struct b2ContactRegister
+{
+ b2ContactCreateFcn* createFcn;
+ b2ContactDestroyFcn* destroyFcn;
+ bool primary;
+};
+
+/// A contact edge is used to connect bodies and contacts together
+/// in a contact graph where each body is a node and each contact
+/// is an edge. A contact edge belongs to a doubly linked list
+/// maintained in each attached body. Each contact has two contact
+/// nodes, one for each attached body.
+struct b2ContactEdge
+{
+ b2Body* other; ///< provides quick access to the other body attached.
+ b2Contact* contact; ///< the contact
+ b2ContactEdge* prev; ///< the previous contact edge in the body's contact list
+ b2ContactEdge* next; ///< the next contact edge in the body's contact list
+};
+
+/// The class manages contact between two shapes. A contact exists for each overlapping
+/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
+/// that has no contact points.
+class b2Contact
+{
+public:
+
+ /// Get the contact manifold. Do not modify the manifold unless you understand the
+ /// internals of Box2D.
+ b2Manifold* GetManifold();
+ const b2Manifold* GetManifold() const;
+
+ /// Get the world manifold.
+ void GetWorldManifold(b2WorldManifold* worldManifold) const;
+
+ /// Is this contact touching?
+ bool IsTouching() const;
+
+ /// Enable/disable this contact. This can be used inside the pre-solve
+ /// contact listener. The contact is only disabled for the current
+ /// time step (or sub-step in continuous collisions).
+ void SetEnabled(bool flag);
+
+ /// Has this contact been disabled?
+ bool IsEnabled() const;
+
+ /// Get the next contact in the world's contact list.
+ b2Contact* GetNext();
+ const b2Contact* GetNext() const;
+
+ /// Get fixture A in this contact.
+ b2Fixture* GetFixtureA();
+ const b2Fixture* GetFixtureA() const;
+
+ /// Get the child primitive index for fixture A.
+ int32 GetChildIndexA() const;
+
+ /// Get fixture B in this contact.
+ b2Fixture* GetFixtureB();
+ const b2Fixture* GetFixtureB() const;
+
+ /// Get the child primitive index for fixture B.
+ int32 GetChildIndexB() const;
+
+ /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.
+ /// This value persists until set or reset.
+ void SetFriction(float32 friction);
+
+ /// Get the friction.
+ float32 GetFriction() const;
+
+ /// Reset the friction mixture to the default value.
+ void ResetFriction();
+
+ /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.
+ /// The value persists until you set or reset.
+ void SetRestitution(float32 restitution);
+
+ /// Get the restitution.
+ float32 GetRestitution() const;
+
+ /// Reset the restitution to the default value.
+ void ResetRestitution();
+
+ /// Set the desired tangent speed for a conveyor belt behavior. In meters per second.
+ void SetTangentSpeed(float32 speed);
+
+ /// Get the desired tangent speed. In meters per second.
+ float32 GetTangentSpeed() const;
+
+ /// Evaluate this contact with your own manifold and transforms.
+ virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;
+
+protected:
+ friend class b2ContactManager;
+ friend class b2World;
+ friend class b2ContactSolver;
+ friend class b2Body;
+ friend class b2Fixture;
+
+ // Flags stored in m_flags
+ enum
+ {
+ // Used when crawling contact graph when forming islands.
+ e_islandFlag = 0x0001,
+
+ // Set when the shapes are touching.
+ e_touchingFlag = 0x0002,
+
+ // This contact can be disabled (by user)
+ e_enabledFlag = 0x0004,
+
+ // This contact needs filtering because a fixture filter was changed.
+ e_filterFlag = 0x0008,
+
+ // This bullet contact had a TOI event
+ e_bulletHitFlag = 0x0010,
+
+ // This contact has a valid TOI in m_toi
+ e_toiFlag = 0x0020
+ };
+
+ /// Flag this contact for filtering. Filtering will occur the next time step.
+ void FlagForFiltering();
+
+ static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,
+ b2Shape::Type typeA, b2Shape::Type typeB);
+ static void InitializeRegisters();
+ static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {}
+ b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+ virtual ~b2Contact() {}
+
+ void Update(b2ContactListener* listener);
+
+ static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
+ static bool s_initialized;
+
+ uint32 m_flags;
+
+ // World pool and list pointers.
+ b2Contact* m_prev;
+ b2Contact* m_next;
+
+ // Nodes for connecting bodies.
+ b2ContactEdge m_nodeA;
+ b2ContactEdge m_nodeB;
+
+ b2Fixture* m_fixtureA;
+ b2Fixture* m_fixtureB;
+
+ int32 m_indexA;
+ int32 m_indexB;
+
+ b2Manifold m_manifold;
+
+ int32 m_toiCount;
+ float32 m_toi;
+
+ float32 m_friction;
+ float32 m_restitution;
+
+ float32 m_tangentSpeed;
+};
+
+inline b2Manifold* b2Contact::GetManifold()
+{
+ return &m_manifold;
+}
+
+inline const b2Manifold* b2Contact::GetManifold() const
+{
+ return &m_manifold;
+}
+
+inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const
+{
+ const b2Body* bodyA = m_fixtureA->GetBody();
+ const b2Body* bodyB = m_fixtureB->GetBody();
+ const b2Shape* shapeA = m_fixtureA->GetShape();
+ const b2Shape* shapeB = m_fixtureB->GetShape();
+
+ worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);
+}
+
+inline void b2Contact::SetEnabled(bool flag)
+{
+ if (flag)
+ {
+ m_flags |= e_enabledFlag;
+ }
+ else
+ {
+ m_flags &= ~e_enabledFlag;
+ }
+}
+
+inline bool b2Contact::IsEnabled() const
+{
+ return (m_flags & e_enabledFlag) == e_enabledFlag;
+}
+
+inline bool b2Contact::IsTouching() const
+{
+ return (m_flags & e_touchingFlag) == e_touchingFlag;
+}
+
+inline b2Contact* b2Contact::GetNext()
+{
+ return m_next;
+}
+
+inline const b2Contact* b2Contact::GetNext() const
+{
+ return m_next;
+}
+
+inline b2Fixture* b2Contact::GetFixtureA()
+{
+ return m_fixtureA;
+}
+
+inline const b2Fixture* b2Contact::GetFixtureA() const
+{
+ return m_fixtureA;
+}
+
+inline b2Fixture* b2Contact::GetFixtureB()
+{
+ return m_fixtureB;
+}
+
+inline int32 b2Contact::GetChildIndexA() const
+{
+ return m_indexA;
+}
+
+inline const b2Fixture* b2Contact::GetFixtureB() const
+{
+ return m_fixtureB;
+}
+
+inline int32 b2Contact::GetChildIndexB() const
+{
+ return m_indexB;
+}
+
+inline void b2Contact::FlagForFiltering()
+{
+ m_flags |= e_filterFlag;
+}
+
+inline void b2Contact::SetFriction(float32 friction)
+{
+ m_friction = friction;
+}
+
+inline float32 b2Contact::GetFriction() const
+{
+ return m_friction;
+}
+
+inline void b2Contact::ResetFriction()
+{
+ m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
+}
+
+inline void b2Contact::SetRestitution(float32 restitution)
+{
+ m_restitution = restitution;
+}
+
+inline float32 b2Contact::GetRestitution() const
+{
+ return m_restitution;
+}
+
+inline void b2Contact::ResetRestitution()
+{
+ m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
+}
+
+inline void b2Contact::SetTangentSpeed(float32 speed)
+{
+ m_tangentSpeed = speed;
+}
+
+inline float32 b2Contact::GetTangentSpeed() const
+{
+ return m_tangentSpeed;
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2ContactSolver.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2ContactSolver.cpp
new file mode 100644
index 00000000..fc41583e
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2ContactSolver.cpp
@@ -0,0 +1,838 @@
+/*
+* Copyright (c) 2006-2011 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
+
+#include
+#include
+#include
+#include
+#include
+
+#define B2_DEBUG_SOLVER 0
+
+bool g_blockSolve = true;
+
+struct b2ContactPositionConstraint
+{
+ b2Vec2 localPoints[b2_maxManifoldPoints];
+ b2Vec2 localNormal;
+ b2Vec2 localPoint;
+ int32 indexA;
+ int32 indexB;
+ float32 invMassA, invMassB;
+ b2Vec2 localCenterA, localCenterB;
+ float32 invIA, invIB;
+ b2Manifold::Type type;
+ float32 radiusA, radiusB;
+ int32 pointCount;
+};
+
+b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
+{
+ m_step = def->step;
+ m_allocator = def->allocator;
+ m_count = def->count;
+ m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint));
+ m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint));
+ m_positions = def->positions;
+ m_velocities = def->velocities;
+ m_contacts = def->contacts;
+
+ // Initialize position independent portions of the constraints.
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ b2Contact* contact = m_contacts[i];
+
+ b2Fixture* fixtureA = contact->m_fixtureA;
+ b2Fixture* fixtureB = contact->m_fixtureB;
+ b2Shape* shapeA = fixtureA->GetShape();
+ b2Shape* shapeB = fixtureB->GetShape();
+ float32 radiusA = shapeA->m_radius;
+ float32 radiusB = shapeB->m_radius;
+ b2Body* bodyA = fixtureA->GetBody();
+ b2Body* bodyB = fixtureB->GetBody();
+ b2Manifold* manifold = contact->GetManifold();
+
+ int32 pointCount = manifold->pointCount;
+ b2Assert(pointCount > 0);
+
+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+ vc->friction = contact->m_friction;
+ vc->restitution = contact->m_restitution;
+ vc->tangentSpeed = contact->m_tangentSpeed;
+ vc->indexA = bodyA->m_islandIndex;
+ vc->indexB = bodyB->m_islandIndex;
+ vc->invMassA = bodyA->m_invMass;
+ vc->invMassB = bodyB->m_invMass;
+ vc->invIA = bodyA->m_invI;
+ vc->invIB = bodyB->m_invI;
+ vc->contactIndex = i;
+ vc->pointCount = pointCount;
+ vc->K.SetZero();
+ vc->normalMass.SetZero();
+
+ b2ContactPositionConstraint* pc = m_positionConstraints + i;
+ pc->indexA = bodyA->m_islandIndex;
+ pc->indexB = bodyB->m_islandIndex;
+ pc->invMassA = bodyA->m_invMass;
+ pc->invMassB = bodyB->m_invMass;
+ pc->localCenterA = bodyA->m_sweep.localCenter;
+ pc->localCenterB = bodyB->m_sweep.localCenter;
+ pc->invIA = bodyA->m_invI;
+ pc->invIB = bodyB->m_invI;
+ pc->localNormal = manifold->localNormal;
+ pc->localPoint = manifold->localPoint;
+ pc->pointCount = pointCount;
+ pc->radiusA = radiusA;
+ pc->radiusB = radiusB;
+ pc->type = manifold->type;
+
+ for (int32 j = 0; j < pointCount; ++j)
+ {
+ b2ManifoldPoint* cp = manifold->points + j;
+ b2VelocityConstraintPoint* vcp = vc->points + j;
+
+ if (m_step.warmStarting)
+ {
+ vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
+ vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
+ }
+ else
+ {
+ vcp->normalImpulse = 0.0f;
+ vcp->tangentImpulse = 0.0f;
+ }
+
+ vcp->rA.SetZero();
+ vcp->rB.SetZero();
+ vcp->normalMass = 0.0f;
+ vcp->tangentMass = 0.0f;
+ vcp->velocityBias = 0.0f;
+
+ pc->localPoints[j] = cp->localPoint;
+ }
+ }
+}
+
+b2ContactSolver::~b2ContactSolver()
+{
+ m_allocator->Free(m_velocityConstraints);
+ m_allocator->Free(m_positionConstraints);
+}
+
+// Initialize position dependent portions of the velocity constraints.
+void b2ContactSolver::InitializeVelocityConstraints()
+{
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+ b2ContactPositionConstraint* pc = m_positionConstraints + i;
+
+ float32 radiusA = pc->radiusA;
+ float32 radiusB = pc->radiusB;
+ b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
+
+ int32 indexA = vc->indexA;
+ int32 indexB = vc->indexB;
+
+ float32 mA = vc->invMassA;
+ float32 mB = vc->invMassB;
+ float32 iA = vc->invIA;
+ float32 iB = vc->invIB;
+ b2Vec2 localCenterA = pc->localCenterA;
+ b2Vec2 localCenterB = pc->localCenterB;
+
+ b2Vec2 cA = m_positions[indexA].c;
+ float32 aA = m_positions[indexA].a;
+ b2Vec2 vA = m_velocities[indexA].v;
+ float32 wA = m_velocities[indexA].w;
+
+ b2Vec2 cB = m_positions[indexB].c;
+ float32 aB = m_positions[indexB].a;
+ b2Vec2 vB = m_velocities[indexB].v;
+ float32 wB = m_velocities[indexB].w;
+
+ b2Assert(manifold->pointCount > 0);
+
+ b2Transform xfA, xfB;
+ xfA.q.Set(aA);
+ xfB.q.Set(aB);
+ xfA.p = cA - b2Mul(xfA.q, localCenterA);
+ xfB.p = cB - b2Mul(xfB.q, localCenterB);
+
+ b2WorldManifold worldManifold;
+ worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
+
+ vc->normal = worldManifold.normal;
+
+ int32 pointCount = vc->pointCount;
+ for (int32 j = 0; j < pointCount; ++j)
+ {
+ b2VelocityConstraintPoint* vcp = vc->points + j;
+
+ vcp->rA = worldManifold.points[j] - cA;
+ vcp->rB = worldManifold.points[j] - cB;
+
+ float32 rnA = b2Cross(vcp->rA, vc->normal);
+ float32 rnB = b2Cross(vcp->rB, vc->normal);
+
+ float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+ vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
+
+ b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
+
+ float32 rtA = b2Cross(vcp->rA, tangent);
+ float32 rtB = b2Cross(vcp->rB, tangent);
+
+ float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
+
+ vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
+
+ // Setup a velocity bias for restitution.
+ vcp->velocityBias = 0.0f;
+ float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
+ if (vRel < -b2_velocityThreshold)
+ {
+ vcp->velocityBias = -vc->restitution * vRel;
+ }
+ }
+
+ // If we have two points, then prepare the block solver.
+ if (vc->pointCount == 2 && g_blockSolve)
+ {
+ b2VelocityConstraintPoint* vcp1 = vc->points + 0;
+ b2VelocityConstraintPoint* vcp2 = vc->points + 1;
+
+ float32 rn1A = b2Cross(vcp1->rA, vc->normal);
+ float32 rn1B = b2Cross(vcp1->rB, vc->normal);
+ float32 rn2A = b2Cross(vcp2->rA, vc->normal);
+ float32 rn2B = b2Cross(vcp2->rB, vc->normal);
+
+ float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
+ float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
+ float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
+
+ // Ensure a reasonable condition number.
+ const float32 k_maxConditionNumber = 1000.0f;
+ if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
+ {
+ // K is safe to invert.
+ vc->K.ex.Set(k11, k12);
+ vc->K.ey.Set(k12, k22);
+ vc->normalMass = vc->K.GetInverse();
+ }
+ else
+ {
+ // The constraints are redundant, just use one.
+ // TODO_ERIN use deepest?
+ vc->pointCount = 1;
+ }
+ }
+ }
+}
+
+void b2ContactSolver::WarmStart()
+{
+ // Warm start.
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+
+ int32 indexA = vc->indexA;
+ int32 indexB = vc->indexB;
+ float32 mA = vc->invMassA;
+ float32 iA = vc->invIA;
+ float32 mB = vc->invMassB;
+ float32 iB = vc->invIB;
+ int32 pointCount = vc->pointCount;
+
+ b2Vec2 vA = m_velocities[indexA].v;
+ float32 wA = m_velocities[indexA].w;
+ b2Vec2 vB = m_velocities[indexB].v;
+ float32 wB = m_velocities[indexB].w;
+
+ b2Vec2 normal = vc->normal;
+ b2Vec2 tangent = b2Cross(normal, 1.0f);
+
+ for (int32 j = 0; j < pointCount; ++j)
+ {
+ b2VelocityConstraintPoint* vcp = vc->points + j;
+ b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent;
+ wA -= iA * b2Cross(vcp->rA, P);
+ vA -= mA * P;
+ wB += iB * b2Cross(vcp->rB, P);
+ vB += mB * P;
+ }
+
+ m_velocities[indexA].v = vA;
+ m_velocities[indexA].w = wA;
+ m_velocities[indexB].v = vB;
+ m_velocities[indexB].w = wB;
+ }
+}
+
+void b2ContactSolver::SolveVelocityConstraints()
+{
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+
+ int32 indexA = vc->indexA;
+ int32 indexB = vc->indexB;
+ float32 mA = vc->invMassA;
+ float32 iA = vc->invIA;
+ float32 mB = vc->invMassB;
+ float32 iB = vc->invIB;
+ int32 pointCount = vc->pointCount;
+
+ b2Vec2 vA = m_velocities[indexA].v;
+ float32 wA = m_velocities[indexA].w;
+ b2Vec2 vB = m_velocities[indexB].v;
+ float32 wB = m_velocities[indexB].w;
+
+ b2Vec2 normal = vc->normal;
+ b2Vec2 tangent = b2Cross(normal, 1.0f);
+ float32 friction = vc->friction;
+
+ b2Assert(pointCount == 1 || pointCount == 2);
+
+ // Solve tangent constraints first because non-penetration is more important
+ // than friction.
+ for (int32 j = 0; j < pointCount; ++j)
+ {
+ b2VelocityConstraintPoint* vcp = vc->points + j;
+
+ // Relative velocity at contact
+ b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
+
+ // Compute tangent force
+ float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
+ float32 lambda = vcp->tangentMass * (-vt);
+
+ // b2Clamp the accumulated force
+ float32 maxFriction = friction * vcp->normalImpulse;
+ float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
+ lambda = newImpulse - vcp->tangentImpulse;
+ vcp->tangentImpulse = newImpulse;
+
+ // Apply contact impulse
+ b2Vec2 P = lambda * tangent;
+
+ vA -= mA * P;
+ wA -= iA * b2Cross(vcp->rA, P);
+
+ vB += mB * P;
+ wB += iB * b2Cross(vcp->rB, P);
+ }
+
+ // Solve normal constraints
+ if (pointCount == 1 || g_blockSolve == false)
+ {
+ for (int32 i = 0; i < pointCount; ++i)
+ {
+ b2VelocityConstraintPoint* vcp = vc->points + i;
+
+ // Relative velocity at contact
+ b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
+
+ // Compute normal impulse
+ float32 vn = b2Dot(dv, normal);
+ float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
+
+ // b2Clamp the accumulated impulse
+ float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
+ lambda = newImpulse - vcp->normalImpulse;
+ vcp->normalImpulse = newImpulse;
+
+ // Apply contact impulse
+ b2Vec2 P = lambda * normal;
+ vA -= mA * P;
+ wA -= iA * b2Cross(vcp->rA, P);
+
+ vB += mB * P;
+ wB += iB * b2Cross(vcp->rB, P);
+ }
+ }
+ else
+ {
+ // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
+ // Build the mini LCP for this contact patch
+ //
+ // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
+ //
+ // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
+ // b = vn0 - velocityBias
+ //
+ // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
+ // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
+ // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
+ // solution that satisfies the problem is chosen.
+ //
+ // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
+ // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
+ //
+ // Substitute:
+ //
+ // x = a + d
+ //
+ // a := old total impulse
+ // x := new total impulse
+ // d := incremental impulse
+ //
+ // For the current iteration we extend the formula for the incremental impulse
+ // to compute the new total impulse:
+ //
+ // vn = A * d + b
+ // = A * (x - a) + b
+ // = A * x + b - A * a
+ // = A * x + b'
+ // b' = b - A * a;
+
+ b2VelocityConstraintPoint* cp1 = vc->points + 0;
+ b2VelocityConstraintPoint* cp2 = vc->points + 1;
+
+ b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
+ b2Assert(a.x >= 0.0f && a.y >= 0.0f);
+
+ // Relative velocity at contact
+ b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
+ b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
+
+ // Compute normal velocity
+ float32 vn1 = b2Dot(dv1, normal);
+ float32 vn2 = b2Dot(dv2, normal);
+
+ b2Vec2 b;
+ b.x = vn1 - cp1->velocityBias;
+ b.y = vn2 - cp2->velocityBias;
+
+ // Compute b'
+ b -= b2Mul(vc->K, a);
+
+ const float32 k_errorTol = 1e-3f;
+ B2_NOT_USED(k_errorTol);
+
+ for (;;)
+ {
+ //
+ // Case 1: vn = 0
+ //
+ // 0 = A * x + b'
+ //
+ // Solve for x:
+ //
+ // x = - inv(A) * b'
+ //
+ b2Vec2 x = - b2Mul(vc->normalMass, b);
+
+ if (x.x >= 0.0f && x.y >= 0.0f)
+ {
+ // Get the incremental impulse
+ b2Vec2 d = x - a;
+
+ // Apply incremental impulse
+ b2Vec2 P1 = d.x * normal;
+ b2Vec2 P2 = d.y * normal;
+ vA -= mA * (P1 + P2);
+ wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+
+ vB += mB * (P1 + P2);
+ wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+
+ // Accumulate
+ cp1->normalImpulse = x.x;
+ cp2->normalImpulse = x.y;
+
+#if B2_DEBUG_SOLVER == 1
+ // Postconditions
+ dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
+ dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
+
+ // Compute normal velocity
+ vn1 = b2Dot(dv1, normal);
+ vn2 = b2Dot(dv2, normal);
+
+ b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
+ b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
+#endif
+ break;
+ }
+
+ //
+ // Case 2: vn1 = 0 and x2 = 0
+ //
+ // 0 = a11 * x1 + a12 * 0 + b1'
+ // vn2 = a21 * x1 + a22 * 0 + b2'
+ //
+ x.x = - cp1->normalMass * b.x;
+ x.y = 0.0f;
+ vn1 = 0.0f;
+ vn2 = vc->K.ex.y * x.x + b.y;
+
+ if (x.x >= 0.0f && vn2 >= 0.0f)
+ {
+ // Get the incremental impulse
+ b2Vec2 d = x - a;
+
+ // Apply incremental impulse
+ b2Vec2 P1 = d.x * normal;
+ b2Vec2 P2 = d.y * normal;
+ vA -= mA * (P1 + P2);
+ wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+
+ vB += mB * (P1 + P2);
+ wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+
+ // Accumulate
+ cp1->normalImpulse = x.x;
+ cp2->normalImpulse = x.y;
+
+#if B2_DEBUG_SOLVER == 1
+ // Postconditions
+ dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
+
+ // Compute normal velocity
+ vn1 = b2Dot(dv1, normal);
+
+ b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
+#endif
+ break;
+ }
+
+
+ //
+ // Case 3: vn2 = 0 and x1 = 0
+ //
+ // vn1 = a11 * 0 + a12 * x2 + b1'
+ // 0 = a21 * 0 + a22 * x2 + b2'
+ //
+ x.x = 0.0f;
+ x.y = - cp2->normalMass * b.y;
+ vn1 = vc->K.ey.x * x.y + b.x;
+ vn2 = 0.0f;
+
+ if (x.y >= 0.0f && vn1 >= 0.0f)
+ {
+ // Resubstitute for the incremental impulse
+ b2Vec2 d = x - a;
+
+ // Apply incremental impulse
+ b2Vec2 P1 = d.x * normal;
+ b2Vec2 P2 = d.y * normal;
+ vA -= mA * (P1 + P2);
+ wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+
+ vB += mB * (P1 + P2);
+ wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+
+ // Accumulate
+ cp1->normalImpulse = x.x;
+ cp2->normalImpulse = x.y;
+
+#if B2_DEBUG_SOLVER == 1
+ // Postconditions
+ dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
+
+ // Compute normal velocity
+ vn2 = b2Dot(dv2, normal);
+
+ b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
+#endif
+ break;
+ }
+
+ //
+ // Case 4: x1 = 0 and x2 = 0
+ //
+ // vn1 = b1
+ // vn2 = b2;
+ x.x = 0.0f;
+ x.y = 0.0f;
+ vn1 = b.x;
+ vn2 = b.y;
+
+ if (vn1 >= 0.0f && vn2 >= 0.0f )
+ {
+ // Resubstitute for the incremental impulse
+ b2Vec2 d = x - a;
+
+ // Apply incremental impulse
+ b2Vec2 P1 = d.x * normal;
+ b2Vec2 P2 = d.y * normal;
+ vA -= mA * (P1 + P2);
+ wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+
+ vB += mB * (P1 + P2);
+ wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+
+ // Accumulate
+ cp1->normalImpulse = x.x;
+ cp2->normalImpulse = x.y;
+
+ break;
+ }
+
+ // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
+ break;
+ }
+ }
+
+ m_velocities[indexA].v = vA;
+ m_velocities[indexA].w = wA;
+ m_velocities[indexB].v = vB;
+ m_velocities[indexB].w = wB;
+ }
+}
+
+void b2ContactSolver::StoreImpulses()
+{
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+ b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
+
+ for (int32 j = 0; j < vc->pointCount; ++j)
+ {
+ manifold->points[j].normalImpulse = vc->points[j].normalImpulse;
+ manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse;
+ }
+ }
+}
+
+struct b2PositionSolverManifold
+{
+ void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index)
+ {
+ b2Assert(pc->pointCount > 0);
+
+ switch (pc->type)
+ {
+ case b2Manifold::e_circles:
+ {
+ b2Vec2 pointA = b2Mul(xfA, pc->localPoint);
+ b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]);
+ normal = pointB - pointA;
+ normal.Normalize();
+ point = 0.5f * (pointA + pointB);
+ separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB;
+ }
+ break;
+
+ case b2Manifold::e_faceA:
+ {
+ normal = b2Mul(xfA.q, pc->localNormal);
+ b2Vec2 planePoint = b2Mul(xfA, pc->localPoint);
+
+ b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]);
+ separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
+ point = clipPoint;
+ }
+ break;
+
+ case b2Manifold::e_faceB:
+ {
+ normal = b2Mul(xfB.q, pc->localNormal);
+ b2Vec2 planePoint = b2Mul(xfB, pc->localPoint);
+
+ b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]);
+ separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
+ point = clipPoint;
+
+ // Ensure normal points from A to B
+ normal = -normal;
+ }
+ break;
+ }
+ }
+
+ b2Vec2 normal;
+ b2Vec2 point;
+ float32 separation;
+};
+
+// Sequential solver.
+bool b2ContactSolver::SolvePositionConstraints()
+{
+ float32 minSeparation = 0.0f;
+
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ b2ContactPositionConstraint* pc = m_positionConstraints + i;
+
+ int32 indexA = pc->indexA;
+ int32 indexB = pc->indexB;
+ b2Vec2 localCenterA = pc->localCenterA;
+ float32 mA = pc->invMassA;
+ float32 iA = pc->invIA;
+ b2Vec2 localCenterB = pc->localCenterB;
+ float32 mB = pc->invMassB;
+ float32 iB = pc->invIB;
+ int32 pointCount = pc->pointCount;
+
+ b2Vec2 cA = m_positions[indexA].c;
+ float32 aA = m_positions[indexA].a;
+
+ b2Vec2 cB = m_positions[indexB].c;
+ float32 aB = m_positions[indexB].a;
+
+ // Solve normal constraints
+ for (int32 j = 0; j < pointCount; ++j)
+ {
+ b2Transform xfA, xfB;
+ xfA.q.Set(aA);
+ xfB.q.Set(aB);
+ xfA.p = cA - b2Mul(xfA.q, localCenterA);
+ xfB.p = cB - b2Mul(xfB.q, localCenterB);
+
+ b2PositionSolverManifold psm;
+ psm.Initialize(pc, xfA, xfB, j);
+ b2Vec2 normal = psm.normal;
+
+ b2Vec2 point = psm.point;
+ float32 separation = psm.separation;
+
+ b2Vec2 rA = point - cA;
+ b2Vec2 rB = point - cB;
+
+ // Track max constraint error.
+ minSeparation = b2Min(minSeparation, separation);
+
+ // Prevent large corrections and allow slop.
+ float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+
+ // Compute the effective mass.
+ float32 rnA = b2Cross(rA, normal);
+ float32 rnB = b2Cross(rB, normal);
+ float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+ // Compute normal impulse
+ float32 impulse = K > 0.0f ? - C / K : 0.0f;
+
+ b2Vec2 P = impulse * normal;
+
+ cA -= mA * P;
+ aA -= iA * b2Cross(rA, P);
+
+ cB += mB * P;
+ aB += iB * b2Cross(rB, P);
+ }
+
+ m_positions[indexA].c = cA;
+ m_positions[indexA].a = aA;
+
+ m_positions[indexB].c = cB;
+ m_positions[indexB].a = aB;
+ }
+
+ // We can't expect minSpeparation >= -b2_linearSlop because we don't
+ // push the separation above -b2_linearSlop.
+ return minSeparation >= -3.0f * b2_linearSlop;
+}
+
+// Sequential position solver for position constraints.
+bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
+{
+ float32 minSeparation = 0.0f;
+
+ for (int32 i = 0; i < m_count; ++i)
+ {
+ b2ContactPositionConstraint* pc = m_positionConstraints + i;
+
+ int32 indexA = pc->indexA;
+ int32 indexB = pc->indexB;
+ b2Vec2 localCenterA = pc->localCenterA;
+ b2Vec2 localCenterB = pc->localCenterB;
+ int32 pointCount = pc->pointCount;
+
+ float32 mA = 0.0f;
+ float32 iA = 0.0f;
+ if (indexA == toiIndexA || indexA == toiIndexB)
+ {
+ mA = pc->invMassA;
+ iA = pc->invIA;
+ }
+
+ float32 mB = 0.0f;
+ float32 iB = 0.;
+ if (indexB == toiIndexA || indexB == toiIndexB)
+ {
+ mB = pc->invMassB;
+ iB = pc->invIB;
+ }
+
+ b2Vec2 cA = m_positions[indexA].c;
+ float32 aA = m_positions[indexA].a;
+
+ b2Vec2 cB = m_positions[indexB].c;
+ float32 aB = m_positions[indexB].a;
+
+ // Solve normal constraints
+ for (int32 j = 0; j < pointCount; ++j)
+ {
+ b2Transform xfA, xfB;
+ xfA.q.Set(aA);
+ xfB.q.Set(aB);
+ xfA.p = cA - b2Mul(xfA.q, localCenterA);
+ xfB.p = cB - b2Mul(xfB.q, localCenterB);
+
+ b2PositionSolverManifold psm;
+ psm.Initialize(pc, xfA, xfB, j);
+ b2Vec2 normal = psm.normal;
+
+ b2Vec2 point = psm.point;
+ float32 separation = psm.separation;
+
+ b2Vec2 rA = point - cA;
+ b2Vec2 rB = point - cB;
+
+ // Track max constraint error.
+ minSeparation = b2Min(minSeparation, separation);
+
+ // Prevent large corrections and allow slop.
+ float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+
+ // Compute the effective mass.
+ float32 rnA = b2Cross(rA, normal);
+ float32 rnB = b2Cross(rB, normal);
+ float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+ // Compute normal impulse
+ float32 impulse = K > 0.0f ? - C / K : 0.0f;
+
+ b2Vec2 P = impulse * normal;
+
+ cA -= mA * P;
+ aA -= iA * b2Cross(rA, P);
+
+ cB += mB * P;
+ aB += iB * b2Cross(rB, P);
+ }
+
+ m_positions[indexA].c = cA;
+ m_positions[indexA].a = aA;
+
+ m_positions[indexB].c = cB;
+ m_positions[indexB].a = aB;
+ }
+
+ // We can't expect minSpeparation >= -b2_linearSlop because we don't
+ // push the separation above -b2_linearSlop.
+ return minSeparation >= -1.5f * b2_linearSlop;
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2ContactSolver.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2ContactSolver.h
new file mode 100644
index 00000000..c7e0426b
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2ContactSolver.h
@@ -0,0 +1,95 @@
+/*
+* 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_CONTACT_SOLVER_H
+#define B2_CONTACT_SOLVER_H
+
+#include
+#include
+#include
+
+class b2Contact;
+class b2Body;
+class b2StackAllocator;
+struct b2ContactPositionConstraint;
+
+struct b2VelocityConstraintPoint
+{
+ b2Vec2 rA;
+ b2Vec2 rB;
+ float32 normalImpulse;
+ float32 tangentImpulse;
+ float32 normalMass;
+ float32 tangentMass;
+ float32 velocityBias;
+};
+
+struct b2ContactVelocityConstraint
+{
+ b2VelocityConstraintPoint points[b2_maxManifoldPoints];
+ b2Vec2 normal;
+ b2Mat22 normalMass;
+ b2Mat22 K;
+ int32 indexA;
+ int32 indexB;
+ float32 invMassA, invMassB;
+ float32 invIA, invIB;
+ float32 friction;
+ float32 restitution;
+ float32 tangentSpeed;
+ int32 pointCount;
+ int32 contactIndex;
+};
+
+struct b2ContactSolverDef
+{
+ b2TimeStep step;
+ b2Contact** contacts;
+ int32 count;
+ b2Position* positions;
+ b2Velocity* velocities;
+ b2StackAllocator* allocator;
+};
+
+class b2ContactSolver
+{
+public:
+ b2ContactSolver(b2ContactSolverDef* def);
+ ~b2ContactSolver();
+
+ void InitializeVelocityConstraints();
+
+ void WarmStart();
+ void SolveVelocityConstraints();
+ void StoreImpulses();
+
+ bool SolvePositionConstraints();
+ bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);
+
+ b2TimeStep m_step;
+ b2Position* m_positions;
+ b2Velocity* m_velocities;
+ b2StackAllocator* m_allocator;
+ b2ContactPositionConstraint* m_positionConstraints;
+ b2ContactVelocityConstraint* m_velocityConstraints;
+ b2Contact** m_contacts;
+ int m_count;
+};
+
+#endif
+
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp
new file mode 100644
index 00000000..b2be085a
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp
@@ -0,0 +1,49 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#include
+#include
+#include
+
+#include
+
+b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));
+ return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);
+}
+
+void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();
+ allocator->Free(contact, sizeof(b2EdgeAndCircleContact));
+}
+
+b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+ b2CollideEdgeAndCircle( manifold,
+ (b2EdgeShape*)m_fixtureA->GetShape(), xfA,
+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndCircleContact.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndCircleContact.h
new file mode 100644
index 00000000..0373e09e
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndCircleContact.h
@@ -0,0 +1,39 @@
+/*
+* 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_EDGE_AND_CIRCLE_CONTACT_H
+#define B2_EDGE_AND_CIRCLE_CONTACT_H
+
+#include
+
+class b2BlockAllocator;
+
+class b2EdgeAndCircleContact : public b2Contact
+{
+public:
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+ ~b2EdgeAndCircleContact() {}
+
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp
new file mode 100644
index 00000000..75415291
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp
@@ -0,0 +1,49 @@
+/*
+* Copyright (c) 2006-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.
+*/
+
+#include
+#include
+#include
+
+#include
+
+b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));
+ return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);
+}
+
+void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();
+ allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));
+}
+
+b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+ b2CollideEdgeAndPolygon( manifold,
+ (b2EdgeShape*)m_fixtureA->GetShape(), xfA,
+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndPolygonContact.h
new file mode 100644
index 00000000..ccffb942
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2EdgeAndPolygonContact.h
@@ -0,0 +1,39 @@
+/*
+* 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_EDGE_AND_POLYGON_CONTACT_H
+#define B2_EDGE_AND_POLYGON_CONTACT_H
+
+#include
+
+class b2BlockAllocator;
+
+class b2EdgeAndPolygonContact : public b2Contact
+{
+public:
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+ ~b2EdgeAndPolygonContact() {}
+
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp
new file mode 100644
index 00000000..860e2ca0
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp
@@ -0,0 +1,49 @@
+/*
+* 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
+#include
+#include
+
+#include
+
+b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
+ return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
+}
+
+void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();
+ allocator->Free(contact, sizeof(b2PolygonAndCircleContact));
+}
+
+b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+ b2CollidePolygonAndCircle( manifold,
+ (b2PolygonShape*)m_fixtureA->GetShape(), xfA,
+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonAndCircleContact.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonAndCircleContact.h
new file mode 100644
index 00000000..4902cee2
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonAndCircleContact.h
@@ -0,0 +1,38 @@
+/*
+* 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_POLYGON_AND_CIRCLE_CONTACT_H
+#define B2_POLYGON_AND_CIRCLE_CONTACT_H
+
+#include
+
+class b2BlockAllocator;
+
+class b2PolygonAndCircleContact : public b2Contact
+{
+public:
+ static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+ ~b2PolygonAndCircleContact() {}
+
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonContact.cpp b/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonContact.cpp
new file mode 100644
index 00000000..deed8fda
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonContact.cpp
@@ -0,0 +1,52 @@
+/*
+* 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
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2PolygonContact));
+ return new (mem) b2PolygonContact(fixtureA, fixtureB);
+}
+
+void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2PolygonContact*)contact)->~b2PolygonContact();
+ allocator->Free(contact, sizeof(b2PolygonContact));
+}
+
+b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+ : b2Contact(fixtureA, 0, fixtureB, 0)
+{
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+ b2CollidePolygons( manifold,
+ (b2PolygonShape*)m_fixtureA->GetShape(), xfA,
+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonContact.h b/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonContact.h
new file mode 100644
index 00000000..9c9d3c6f
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Contacts/b2PolygonContact.h
@@ -0,0 +1,39 @@
+/*
+* 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_POLYGON_CONTACT_H
+#define B2_POLYGON_CONTACT_H
+
+#include
+
+class b2BlockAllocator;
+
+class b2PolygonContact : public b2Contact
+{
+public:
+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+ b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+ ~b2PolygonContact() {}
+
+ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2DistanceJoint.cpp b/Dependencies/Box2D/src/Dynamics/Joints/b2DistanceJoint.cpp
new file mode 100644
index 00000000..8abb0588
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2DistanceJoint.cpp
@@ -0,0 +1,260 @@
+/*
+* Copyright (c) 2006-2011 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
+#include
+#include
+
+// 1-D constrained system
+// m (v2 - v1) = lambda
+// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
+// x2 = x1 + h * v2
+
+// 1-D mass-damper-spring system
+// m (v2 - v1) + h * d * v2 + h * k *
+
+// C = norm(p2 - p1) - L
+// u = (p2 - p1) / norm(p2 - p1)
+// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+// J = [-u -cross(r1, u) u cross(r2, u)]
+// K = J * invM * JT
+// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
+
+void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
+ const b2Vec2& anchor1, const b2Vec2& anchor2)
+{
+ bodyA = b1;
+ bodyB = b2;
+ localAnchorA = bodyA->GetLocalPoint(anchor1);
+ localAnchorB = bodyB->GetLocalPoint(anchor2);
+ b2Vec2 d = anchor2 - anchor1;
+ length = d.Length();
+}
+
+b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
+: b2Joint(def)
+{
+ m_localAnchorA = def->localAnchorA;
+ m_localAnchorB = def->localAnchorB;
+ m_length = def->length;
+ m_frequencyHz = def->frequencyHz;
+ m_dampingRatio = def->dampingRatio;
+ m_impulse = 0.0f;
+ m_gamma = 0.0f;
+ m_bias = 0.0f;
+}
+
+void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
+{
+ m_indexA = m_bodyA->m_islandIndex;
+ m_indexB = m_bodyB->m_islandIndex;
+ m_localCenterA = m_bodyA->m_sweep.localCenter;
+ m_localCenterB = m_bodyB->m_sweep.localCenter;
+ m_invMassA = m_bodyA->m_invMass;
+ m_invMassB = m_bodyB->m_invMass;
+ m_invIA = m_bodyA->m_invI;
+ m_invIB = m_bodyB->m_invI;
+
+ b2Vec2 cA = data.positions[m_indexA].c;
+ float32 aA = data.positions[m_indexA].a;
+ b2Vec2 vA = data.velocities[m_indexA].v;
+ float32 wA = data.velocities[m_indexA].w;
+
+ b2Vec2 cB = data.positions[m_indexB].c;
+ float32 aB = data.positions[m_indexB].a;
+ b2Vec2 vB = data.velocities[m_indexB].v;
+ float32 wB = data.velocities[m_indexB].w;
+
+ b2Rot qA(aA), qB(aB);
+
+ m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+ m_u = cB + m_rB - cA - m_rA;
+
+ // Handle singularity.
+ float32 length = m_u.Length();
+ if (length > b2_linearSlop)
+ {
+ m_u *= 1.0f / length;
+ }
+ else
+ {
+ m_u.Set(0.0f, 0.0f);
+ }
+
+ float32 crAu = b2Cross(m_rA, m_u);
+ float32 crBu = b2Cross(m_rB, m_u);
+ float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
+
+ // Compute the effective mass matrix.
+ m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+
+ if (m_frequencyHz > 0.0f)
+ {
+ float32 C = length - m_length;
+
+ // Frequency
+ float32 omega = 2.0f * b2_pi * m_frequencyHz;
+
+ // Damping coefficient
+ float32 d = 2.0f * m_mass * m_dampingRatio * omega;
+
+ // Spring stiffness
+ float32 k = m_mass * omega * omega;
+
+ // magic formulas
+ float32 h = data.step.dt;
+ m_gamma = h * (d + h * k);
+ m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
+ m_bias = C * h * k * m_gamma;
+
+ invMass += m_gamma;
+ m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+ }
+ else
+ {
+ m_gamma = 0.0f;
+ m_bias = 0.0f;
+ }
+
+ if (data.step.warmStarting)
+ {
+ // Scale the impulse to support a variable time step.
+ m_impulse *= data.step.dtRatio;
+
+ b2Vec2 P = m_impulse * m_u;
+ vA -= m_invMassA * P;
+ wA -= m_invIA * b2Cross(m_rA, P);
+ vB += m_invMassB * P;
+ wB += m_invIB * b2Cross(m_rB, P);
+ }
+ else
+ {
+ m_impulse = 0.0f;
+ }
+
+ data.velocities[m_indexA].v = vA;
+ data.velocities[m_indexA].w = wA;
+ data.velocities[m_indexB].v = vB;
+ data.velocities[m_indexB].w = wB;
+}
+
+void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data)
+{
+ b2Vec2 vA = data.velocities[m_indexA].v;
+ float32 wA = data.velocities[m_indexA].w;
+ b2Vec2 vB = data.velocities[m_indexB].v;
+ float32 wB = data.velocities[m_indexB].w;
+
+ // Cdot = dot(u, v + cross(w, r))
+ b2Vec2 vpA = vA + b2Cross(wA, m_rA);
+ b2Vec2 vpB = vB + b2Cross(wB, m_rB);
+ float32 Cdot = b2Dot(m_u, vpB - vpA);
+
+ float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
+ m_impulse += impulse;
+
+ b2Vec2 P = impulse * m_u;
+ vA -= m_invMassA * P;
+ wA -= m_invIA * b2Cross(m_rA, P);
+ vB += m_invMassB * P;
+ wB += m_invIB * b2Cross(m_rB, P);
+
+ data.velocities[m_indexA].v = vA;
+ data.velocities[m_indexA].w = wA;
+ data.velocities[m_indexB].v = vB;
+ data.velocities[m_indexB].w = wB;
+}
+
+bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+ if (m_frequencyHz > 0.0f)
+ {
+ // There is no position correction for soft distance constraints.
+ return true;
+ }
+
+ b2Vec2 cA = data.positions[m_indexA].c;
+ float32 aA = data.positions[m_indexA].a;
+ b2Vec2 cB = data.positions[m_indexB].c;
+ float32 aB = data.positions[m_indexB].a;
+
+ b2Rot qA(aA), qB(aB);
+
+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+ b2Vec2 u = cB + rB - cA - rA;
+
+ float32 length = u.Normalize();
+ float32 C = length - m_length;
+ C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+
+ float32 impulse = -m_mass * C;
+ b2Vec2 P = impulse * u;
+
+ cA -= m_invMassA * P;
+ aA -= m_invIA * b2Cross(rA, P);
+ cB += m_invMassB * P;
+ aB += m_invIB * b2Cross(rB, P);
+
+ data.positions[m_indexA].c = cA;
+ data.positions[m_indexA].a = aA;
+ data.positions[m_indexB].c = cB;
+ data.positions[m_indexB].a = aB;
+
+ return b2Abs(C) < b2_linearSlop;
+}
+
+b2Vec2 b2DistanceJoint::GetAnchorA() const
+{
+ return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2DistanceJoint::GetAnchorB() const
+{
+ return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const
+{
+ b2Vec2 F = (inv_dt * m_impulse) * m_u;
+ return F;
+}
+
+float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
+{
+ B2_NOT_USED(inv_dt);
+ return 0.0f;
+}
+
+void b2DistanceJoint::Dump()
+{
+ int32 indexA = m_bodyA->m_islandIndex;
+ int32 indexB = m_bodyB->m_islandIndex;
+
+ b2Log(" b2DistanceJointDef jd;\n");
+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);
+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);
+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+ b2Log(" jd.length = %.15lef;\n", m_length);
+ b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);
+ b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);
+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2DistanceJoint.h b/Dependencies/Box2D/src/Dynamics/Joints/b2DistanceJoint.h
new file mode 100644
index 00000000..6237a9da
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2DistanceJoint.h
@@ -0,0 +1,169 @@
+/*
+* Copyright (c) 2006-2007 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_JOINT_H
+#define B2_DISTANCE_JOINT_H
+
+#include
+
+/// Distance joint definition. This requires defining an
+/// anchor point on both bodies and the non-zero length of the
+/// distance joint. The definition uses local anchor points
+/// so that the initial configuration can violate the constraint
+/// slightly. This helps when saving and loading a game.
+/// @warning Do not use a zero or short length.
+struct b2DistanceJointDef : public b2JointDef
+{
+ b2DistanceJointDef()
+ {
+ type = e_distanceJoint;
+ localAnchorA.Set(0.0f, 0.0f);
+ localAnchorB.Set(0.0f, 0.0f);
+ length = 1.0f;
+ frequencyHz = 0.0f;
+ dampingRatio = 0.0f;
+ }
+
+ /// Initialize the bodies, anchors, and length using the world
+ /// anchors.
+ void Initialize(b2Body* bodyA, b2Body* bodyB,
+ const b2Vec2& anchorA, const b2Vec2& anchorB);
+
+ /// The local anchor point relative to bodyA's origin.
+ b2Vec2 localAnchorA;
+
+ /// The local anchor point relative to bodyB's origin.
+ b2Vec2 localAnchorB;
+
+ /// The natural length between the anchor points.
+ float32 length;
+
+ /// The mass-spring-damper frequency in Hertz. A value of 0
+ /// disables softness.
+ float32 frequencyHz;
+
+ /// The damping ratio. 0 = no damping, 1 = critical damping.
+ float32 dampingRatio;
+};
+
+/// A distance joint constrains two points on two bodies
+/// to remain at a fixed distance from each other. You can view
+/// this as a massless, rigid rod.
+class b2DistanceJoint : public b2Joint
+{
+public:
+
+ b2Vec2 GetAnchorA() const;
+ b2Vec2 GetAnchorB() const;
+
+ /// Get the reaction force given the inverse time step.
+ /// Unit is N.
+ b2Vec2 GetReactionForce(float32 inv_dt) const;
+
+ /// Get the reaction torque given the inverse time step.
+ /// Unit is N*m. This is always zero for a distance joint.
+ float32 GetReactionTorque(float32 inv_dt) const;
+
+ /// The local anchor point relative to bodyA's origin.
+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+ /// The local anchor point relative to bodyB's origin.
+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
+
+ /// Set/get the natural length.
+ /// Manipulating the length can lead to non-physical behavior when the frequency is zero.
+ void SetLength(float32 length);
+ float32 GetLength() const;
+
+ /// Set/get frequency in Hz.
+ void SetFrequency(float32 hz);
+ float32 GetFrequency() const;
+
+ /// Set/get damping ratio.
+ void SetDampingRatio(float32 ratio);
+ float32 GetDampingRatio() const;
+
+ /// Dump joint to dmLog
+ void Dump();
+
+protected:
+
+ friend class b2Joint;
+ b2DistanceJoint(const b2DistanceJointDef* data);
+
+ void InitVelocityConstraints(const b2SolverData& data);
+ void SolveVelocityConstraints(const b2SolverData& data);
+ bool SolvePositionConstraints(const b2SolverData& data);
+
+ float32 m_frequencyHz;
+ float32 m_dampingRatio;
+ float32 m_bias;
+
+ // Solver shared
+ b2Vec2 m_localAnchorA;
+ b2Vec2 m_localAnchorB;
+ float32 m_gamma;
+ float32 m_impulse;
+ float32 m_length;
+
+ // Solver temp
+ int32 m_indexA;
+ int32 m_indexB;
+ b2Vec2 m_u;
+ b2Vec2 m_rA;
+ b2Vec2 m_rB;
+ b2Vec2 m_localCenterA;
+ b2Vec2 m_localCenterB;
+ float32 m_invMassA;
+ float32 m_invMassB;
+ float32 m_invIA;
+ float32 m_invIB;
+ float32 m_mass;
+};
+
+inline void b2DistanceJoint::SetLength(float32 length)
+{
+ m_length = length;
+}
+
+inline float32 b2DistanceJoint::GetLength() const
+{
+ return m_length;
+}
+
+inline void b2DistanceJoint::SetFrequency(float32 hz)
+{
+ m_frequencyHz = hz;
+}
+
+inline float32 b2DistanceJoint::GetFrequency() const
+{
+ return m_frequencyHz;
+}
+
+inline void b2DistanceJoint::SetDampingRatio(float32 ratio)
+{
+ m_dampingRatio = ratio;
+}
+
+inline float32 b2DistanceJoint::GetDampingRatio() const
+{
+ return m_dampingRatio;
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2FrictionJoint.cpp b/Dependencies/Box2D/src/Dynamics/Joints/b2FrictionJoint.cpp
new file mode 100644
index 00000000..229dc1e6
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2FrictionJoint.cpp
@@ -0,0 +1,251 @@
+/*
+* Copyright (c) 2006-2011 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
+#include
+#include
+
+// Point-to-point constraint
+// Cdot = v2 - v1
+// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
+{
+ bodyA = bA;
+ bodyB = bB;
+ localAnchorA = bodyA->GetLocalPoint(anchor);
+ localAnchorB = bodyB->GetLocalPoint(anchor);
+}
+
+b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
+: b2Joint(def)
+{
+ m_localAnchorA = def->localAnchorA;
+ m_localAnchorB = def->localAnchorB;
+
+ m_linearImpulse.SetZero();
+ m_angularImpulse = 0.0f;
+
+ m_maxForce = def->maxForce;
+ m_maxTorque = def->maxTorque;
+}
+
+void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
+{
+ m_indexA = m_bodyA->m_islandIndex;
+ m_indexB = m_bodyB->m_islandIndex;
+ m_localCenterA = m_bodyA->m_sweep.localCenter;
+ m_localCenterB = m_bodyB->m_sweep.localCenter;
+ m_invMassA = m_bodyA->m_invMass;
+ m_invMassB = m_bodyB->m_invMass;
+ m_invIA = m_bodyA->m_invI;
+ m_invIB = m_bodyB->m_invI;
+
+ float32 aA = data.positions[m_indexA].a;
+ b2Vec2 vA = data.velocities[m_indexA].v;
+ float32 wA = data.velocities[m_indexA].w;
+
+ float32 aB = data.positions[m_indexB].a;
+ b2Vec2 vB = data.velocities[m_indexB].v;
+ float32 wB = data.velocities[m_indexB].w;
+
+ b2Rot qA(aA), qB(aB);
+
+ // Compute the effective mass matrix.
+ m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+
+ // J = [-I -r1_skew I r2_skew]
+ // [ 0 -1 0 1]
+ // r_skew = [-ry; rx]
+
+ // Matlab
+ // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
+ // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
+ // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
+
+ float32 mA = m_invMassA, mB = m_invMassB;
+ float32 iA = m_invIA, iB = m_invIB;
+
+ b2Mat22 K;
+ K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
+ K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
+ K.ey.x = K.ex.y;
+ K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
+
+ m_linearMass = K.GetInverse();
+
+ m_angularMass = iA + iB;
+ if (m_angularMass > 0.0f)
+ {
+ m_angularMass = 1.0f / m_angularMass;
+ }
+
+ if (data.step.warmStarting)
+ {
+ // Scale impulses to support a variable time step.
+ m_linearImpulse *= data.step.dtRatio;
+ m_angularImpulse *= data.step.dtRatio;
+
+ b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
+ vA -= mA * P;
+ wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
+ vB += mB * P;
+ wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
+ }
+ else
+ {
+ m_linearImpulse.SetZero();
+ m_angularImpulse = 0.0f;
+ }
+
+ data.velocities[m_indexA].v = vA;
+ data.velocities[m_indexA].w = wA;
+ data.velocities[m_indexB].v = vB;
+ data.velocities[m_indexB].w = wB;
+}
+
+void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
+{
+ b2Vec2 vA = data.velocities[m_indexA].v;
+ float32 wA = data.velocities[m_indexA].w;
+ b2Vec2 vB = data.velocities[m_indexB].v;
+ float32 wB = data.velocities[m_indexB].w;
+
+ float32 mA = m_invMassA, mB = m_invMassB;
+ float32 iA = m_invIA, iB = m_invIB;
+
+ float32 h = data.step.dt;
+
+ // Solve angular friction
+ {
+ float32 Cdot = wB - wA;
+ float32 impulse = -m_angularMass * Cdot;
+
+ float32 oldImpulse = m_angularImpulse;
+ float32 maxImpulse = h * m_maxTorque;
+ m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
+ impulse = m_angularImpulse - oldImpulse;
+
+ wA -= iA * impulse;
+ wB += iB * impulse;
+ }
+
+ // Solve linear friction
+ {
+ b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
+
+ b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
+ b2Vec2 oldImpulse = m_linearImpulse;
+ m_linearImpulse += impulse;
+
+ float32 maxImpulse = h * m_maxForce;
+
+ if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
+ {
+ m_linearImpulse.Normalize();
+ m_linearImpulse *= maxImpulse;
+ }
+
+ impulse = m_linearImpulse - oldImpulse;
+
+ vA -= mA * impulse;
+ wA -= iA * b2Cross(m_rA, impulse);
+
+ vB += mB * impulse;
+ wB += iB * b2Cross(m_rB, impulse);
+ }
+
+ data.velocities[m_indexA].v = vA;
+ data.velocities[m_indexA].w = wA;
+ data.velocities[m_indexB].v = vB;
+ data.velocities[m_indexB].w = wB;
+}
+
+bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+ B2_NOT_USED(data);
+
+ return true;
+}
+
+b2Vec2 b2FrictionJoint::GetAnchorA() const
+{
+ return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2FrictionJoint::GetAnchorB() const
+{
+ return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
+{
+ return inv_dt * m_linearImpulse;
+}
+
+float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
+{
+ return inv_dt * m_angularImpulse;
+}
+
+void b2FrictionJoint::SetMaxForce(float32 force)
+{
+ b2Assert(b2IsValid(force) && force >= 0.0f);
+ m_maxForce = force;
+}
+
+float32 b2FrictionJoint::GetMaxForce() const
+{
+ return m_maxForce;
+}
+
+void b2FrictionJoint::SetMaxTorque(float32 torque)
+{
+ b2Assert(b2IsValid(torque) && torque >= 0.0f);
+ m_maxTorque = torque;
+}
+
+float32 b2FrictionJoint::GetMaxTorque() const
+{
+ return m_maxTorque;
+}
+
+void b2FrictionJoint::Dump()
+{
+ int32 indexA = m_bodyA->m_islandIndex;
+ int32 indexB = m_bodyB->m_islandIndex;
+
+ b2Log(" b2FrictionJointDef jd;\n");
+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);
+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);
+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+ b2Log(" jd.maxForce = %.15lef;\n", m_maxForce);
+ b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque);
+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2FrictionJoint.h b/Dependencies/Box2D/src/Dynamics/Joints/b2FrictionJoint.h
new file mode 100644
index 00000000..7cd15de2
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2FrictionJoint.h
@@ -0,0 +1,119 @@
+/*
+* Copyright (c) 2006-2007 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_FRICTION_JOINT_H
+#define B2_FRICTION_JOINT_H
+
+#include
+
+/// Friction joint definition.
+struct b2FrictionJointDef : public b2JointDef
+{
+ b2FrictionJointDef()
+ {
+ type = e_frictionJoint;
+ localAnchorA.SetZero();
+ localAnchorB.SetZero();
+ maxForce = 0.0f;
+ maxTorque = 0.0f;
+ }
+
+ /// Initialize the bodies, anchors, axis, and reference angle using the world
+ /// anchor and world axis.
+ void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
+
+ /// The local anchor point relative to bodyA's origin.
+ b2Vec2 localAnchorA;
+
+ /// The local anchor point relative to bodyB's origin.
+ b2Vec2 localAnchorB;
+
+ /// The maximum friction force in N.
+ float32 maxForce;
+
+ /// The maximum friction torque in N-m.
+ float32 maxTorque;
+};
+
+/// Friction joint. This is used for top-down friction.
+/// It provides 2D translational friction and angular friction.
+class b2FrictionJoint : public b2Joint
+{
+public:
+ b2Vec2 GetAnchorA() const;
+ b2Vec2 GetAnchorB() const;
+
+ b2Vec2 GetReactionForce(float32 inv_dt) const;
+ float32 GetReactionTorque(float32 inv_dt) const;
+
+ /// The local anchor point relative to bodyA's origin.
+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+ /// The local anchor point relative to bodyB's origin.
+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
+
+ /// Set the maximum friction force in N.
+ void SetMaxForce(float32 force);
+
+ /// Get the maximum friction force in N.
+ float32 GetMaxForce() const;
+
+ /// Set the maximum friction torque in N*m.
+ void SetMaxTorque(float32 torque);
+
+ /// Get the maximum friction torque in N*m.
+ float32 GetMaxTorque() const;
+
+ /// Dump joint to dmLog
+ void Dump();
+
+protected:
+
+ friend class b2Joint;
+
+ b2FrictionJoint(const b2FrictionJointDef* def);
+
+ void InitVelocityConstraints(const b2SolverData& data);
+ void SolveVelocityConstraints(const b2SolverData& data);
+ bool SolvePositionConstraints(const b2SolverData& data);
+
+ b2Vec2 m_localAnchorA;
+ b2Vec2 m_localAnchorB;
+
+ // Solver shared
+ b2Vec2 m_linearImpulse;
+ float32 m_angularImpulse;
+ float32 m_maxForce;
+ float32 m_maxTorque;
+
+ // Solver temp
+ int32 m_indexA;
+ int32 m_indexB;
+ b2Vec2 m_rA;
+ b2Vec2 m_rB;
+ b2Vec2 m_localCenterA;
+ b2Vec2 m_localCenterB;
+ float32 m_invMassA;
+ float32 m_invMassB;
+ float32 m_invIA;
+ float32 m_invIB;
+ b2Mat22 m_linearMass;
+ float32 m_angularMass;
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2GearJoint.cpp b/Dependencies/Box2D/src/Dynamics/Joints/b2GearJoint.cpp
new file mode 100644
index 00000000..9a750200
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2GearJoint.cpp
@@ -0,0 +1,419 @@
+/*
+* Copyright (c) 2007-2011 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
+#include
+#include
+#include
+#include
+
+// Gear Joint:
+// C0 = (coordinate1 + ratio * coordinate2)_initial
+// C = (coordinate1 + ratio * coordinate2) - C0 = 0
+// J = [J1 ratio * J2]
+// K = J * invM * JT
+// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
+//
+// Revolute:
+// coordinate = rotation
+// Cdot = angularVelocity
+// J = [0 0 1]
+// K = J * invM * JT = invI
+//
+// Prismatic:
+// coordinate = dot(p - pg, ug)
+// Cdot = dot(v + cross(w, r), ug)
+// J = [ug cross(r, ug)]
+// K = J * invM * JT = invMass + invI * cross(r, ug)^2
+
+b2GearJoint::b2GearJoint(const b2GearJointDef* def)
+: b2Joint(def)
+{
+ m_joint1 = def->joint1;
+ m_joint2 = def->joint2;
+
+ m_typeA = m_joint1->GetType();
+ m_typeB = m_joint2->GetType();
+
+ b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
+ b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
+
+ float32 coordinateA, coordinateB;
+
+ // TODO_ERIN there might be some problem with the joint edges in b2Joint.
+
+ m_bodyC = m_joint1->GetBodyA();
+ m_bodyA = m_joint1->GetBodyB();
+
+ // Get geometry of joint1
+ b2Transform xfA = m_bodyA->m_xf;
+ float32 aA = m_bodyA->m_sweep.a;
+ b2Transform xfC = m_bodyC->m_xf;
+ float32 aC = m_bodyC->m_sweep.a;
+
+ if (m_typeA == e_revoluteJoint)
+ {
+ b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
+ m_localAnchorC = revolute->m_localAnchorA;
+ m_localAnchorA = revolute->m_localAnchorB;
+ m_referenceAngleA = revolute->m_referenceAngle;
+ m_localAxisC.SetZero();
+
+ coordinateA = aA - aC - m_referenceAngleA;
+ }
+ else
+ {
+ b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
+ m_localAnchorC = prismatic->m_localAnchorA;
+ m_localAnchorA = prismatic->m_localAnchorB;
+ m_referenceAngleA = prismatic->m_referenceAngle;
+ m_localAxisC = prismatic->m_localXAxisA;
+
+ b2Vec2 pC = m_localAnchorC;
+ b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
+ coordinateA = b2Dot(pA - pC, m_localAxisC);
+ }
+
+ m_bodyD = m_joint2->GetBodyA();
+ m_bodyB = m_joint2->GetBodyB();
+
+ // Get geometry of joint2
+ b2Transform xfB = m_bodyB->m_xf;
+ float32 aB = m_bodyB->m_sweep.a;
+ b2Transform xfD = m_bodyD->m_xf;
+ float32 aD = m_bodyD->m_sweep.a;
+
+ if (m_typeB == e_revoluteJoint)
+ {
+ b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
+ m_localAnchorD = revolute->m_localAnchorA;
+ m_localAnchorB = revolute->m_localAnchorB;
+ m_referenceAngleB = revolute->m_referenceAngle;
+ m_localAxisD.SetZero();
+
+ coordinateB = aB - aD - m_referenceAngleB;
+ }
+ else
+ {
+ b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
+ m_localAnchorD = prismatic->m_localAnchorA;
+ m_localAnchorB = prismatic->m_localAnchorB;
+ m_referenceAngleB = prismatic->m_referenceAngle;
+ m_localAxisD = prismatic->m_localXAxisA;
+
+ b2Vec2 pD = m_localAnchorD;
+ b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
+ coordinateB = b2Dot(pB - pD, m_localAxisD);
+ }
+
+ m_ratio = def->ratio;
+
+ m_constant = coordinateA + m_ratio * coordinateB;
+
+ m_impulse = 0.0f;
+}
+
+void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
+{
+ m_indexA = m_bodyA->m_islandIndex;
+ m_indexB = m_bodyB->m_islandIndex;
+ m_indexC = m_bodyC->m_islandIndex;
+ m_indexD = m_bodyD->m_islandIndex;
+ m_lcA = m_bodyA->m_sweep.localCenter;
+ m_lcB = m_bodyB->m_sweep.localCenter;
+ m_lcC = m_bodyC->m_sweep.localCenter;
+ m_lcD = m_bodyD->m_sweep.localCenter;
+ m_mA = m_bodyA->m_invMass;
+ m_mB = m_bodyB->m_invMass;
+ m_mC = m_bodyC->m_invMass;
+ m_mD = m_bodyD->m_invMass;
+ m_iA = m_bodyA->m_invI;
+ m_iB = m_bodyB->m_invI;
+ m_iC = m_bodyC->m_invI;
+ m_iD = m_bodyD->m_invI;
+
+ float32 aA = data.positions[m_indexA].a;
+ b2Vec2 vA = data.velocities[m_indexA].v;
+ float32 wA = data.velocities[m_indexA].w;
+
+ float32 aB = data.positions[m_indexB].a;
+ b2Vec2 vB = data.velocities[m_indexB].v;
+ float32 wB = data.velocities[m_indexB].w;
+
+ float32 aC = data.positions[m_indexC].a;
+ b2Vec2 vC = data.velocities[m_indexC].v;
+ float32 wC = data.velocities[m_indexC].w;
+
+ float32 aD = data.positions[m_indexD].a;
+ b2Vec2 vD = data.velocities[m_indexD].v;
+ float32 wD = data.velocities[m_indexD].w;
+
+ b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
+
+ m_mass = 0.0f;
+
+ if (m_typeA == e_revoluteJoint)
+ {
+ m_JvAC.SetZero();
+ m_JwA = 1.0f;
+ m_JwC = 1.0f;
+ m_mass += m_iA + m_iC;
+ }
+ else
+ {
+ b2Vec2 u = b2Mul(qC, m_localAxisC);
+ b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
+ m_JvAC = u;
+ m_JwC = b2Cross(rC, u);
+ m_JwA = b2Cross(rA, u);
+ m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
+ }
+
+ if (m_typeB == e_revoluteJoint)
+ {
+ m_JvBD.SetZero();
+ m_JwB = m_ratio;
+ m_JwD = m_ratio;
+ m_mass += m_ratio * m_ratio * (m_iB + m_iD);
+ }
+ else
+ {
+ b2Vec2 u = b2Mul(qD, m_localAxisD);
+ b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
+ m_JvBD = m_ratio * u;
+ m_JwD = m_ratio * b2Cross(rD, u);
+ m_JwB = m_ratio * b2Cross(rB, u);
+ m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
+ }
+
+ // Compute effective mass.
+ m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
+
+ if (data.step.warmStarting)
+ {
+ vA += (m_mA * m_impulse) * m_JvAC;
+ wA += m_iA * m_impulse * m_JwA;
+ vB += (m_mB * m_impulse) * m_JvBD;
+ wB += m_iB * m_impulse * m_JwB;
+ vC -= (m_mC * m_impulse) * m_JvAC;
+ wC -= m_iC * m_impulse * m_JwC;
+ vD -= (m_mD * m_impulse) * m_JvBD;
+ wD -= m_iD * m_impulse * m_JwD;
+ }
+ else
+ {
+ m_impulse = 0.0f;
+ }
+
+ data.velocities[m_indexA].v = vA;
+ data.velocities[m_indexA].w = wA;
+ data.velocities[m_indexB].v = vB;
+ data.velocities[m_indexB].w = wB;
+ data.velocities[m_indexC].v = vC;
+ data.velocities[m_indexC].w = wC;
+ data.velocities[m_indexD].v = vD;
+ data.velocities[m_indexD].w = wD;
+}
+
+void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data)
+{
+ b2Vec2 vA = data.velocities[m_indexA].v;
+ float32 wA = data.velocities[m_indexA].w;
+ b2Vec2 vB = data.velocities[m_indexB].v;
+ float32 wB = data.velocities[m_indexB].w;
+ b2Vec2 vC = data.velocities[m_indexC].v;
+ float32 wC = data.velocities[m_indexC].w;
+ b2Vec2 vD = data.velocities[m_indexD].v;
+ float32 wD = data.velocities[m_indexD].w;
+
+ float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
+ Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
+
+ float32 impulse = -m_mass * Cdot;
+ m_impulse += impulse;
+
+ vA += (m_mA * impulse) * m_JvAC;
+ wA += m_iA * impulse * m_JwA;
+ vB += (m_mB * impulse) * m_JvBD;
+ wB += m_iB * impulse * m_JwB;
+ vC -= (m_mC * impulse) * m_JvAC;
+ wC -= m_iC * impulse * m_JwC;
+ vD -= (m_mD * impulse) * m_JvBD;
+ wD -= m_iD * impulse * m_JwD;
+
+ data.velocities[m_indexA].v = vA;
+ data.velocities[m_indexA].w = wA;
+ data.velocities[m_indexB].v = vB;
+ data.velocities[m_indexB].w = wB;
+ data.velocities[m_indexC].v = vC;
+ data.velocities[m_indexC].w = wC;
+ data.velocities[m_indexD].v = vD;
+ data.velocities[m_indexD].w = wD;
+}
+
+bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+ b2Vec2 cA = data.positions[m_indexA].c;
+ float32 aA = data.positions[m_indexA].a;
+ b2Vec2 cB = data.positions[m_indexB].c;
+ float32 aB = data.positions[m_indexB].a;
+ b2Vec2 cC = data.positions[m_indexC].c;
+ float32 aC = data.positions[m_indexC].a;
+ b2Vec2 cD = data.positions[m_indexD].c;
+ float32 aD = data.positions[m_indexD].a;
+
+ b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
+
+ float32 linearError = 0.0f;
+
+ float32 coordinateA, coordinateB;
+
+ b2Vec2 JvAC, JvBD;
+ float32 JwA, JwB, JwC, JwD;
+ float32 mass = 0.0f;
+
+ if (m_typeA == e_revoluteJoint)
+ {
+ JvAC.SetZero();
+ JwA = 1.0f;
+ JwC = 1.0f;
+ mass += m_iA + m_iC;
+
+ coordinateA = aA - aC - m_referenceAngleA;
+ }
+ else
+ {
+ b2Vec2 u = b2Mul(qC, m_localAxisC);
+ b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
+ JvAC = u;
+ JwC = b2Cross(rC, u);
+ JwA = b2Cross(rA, u);
+ mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
+
+ b2Vec2 pC = m_localAnchorC - m_lcC;
+ b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
+ coordinateA = b2Dot(pA - pC, m_localAxisC);
+ }
+
+ if (m_typeB == e_revoluteJoint)
+ {
+ JvBD.SetZero();
+ JwB = m_ratio;
+ JwD = m_ratio;
+ mass += m_ratio * m_ratio * (m_iB + m_iD);
+
+ coordinateB = aB - aD - m_referenceAngleB;
+ }
+ else
+ {
+ b2Vec2 u = b2Mul(qD, m_localAxisD);
+ b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
+ JvBD = m_ratio * u;
+ JwD = m_ratio * b2Cross(rD, u);
+ JwB = m_ratio * b2Cross(rB, u);
+ mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
+
+ b2Vec2 pD = m_localAnchorD - m_lcD;
+ b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
+ coordinateB = b2Dot(pB - pD, m_localAxisD);
+ }
+
+ float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
+
+ float32 impulse = 0.0f;
+ if (mass > 0.0f)
+ {
+ impulse = -C / mass;
+ }
+
+ cA += m_mA * impulse * JvAC;
+ aA += m_iA * impulse * JwA;
+ cB += m_mB * impulse * JvBD;
+ aB += m_iB * impulse * JwB;
+ cC -= m_mC * impulse * JvAC;
+ aC -= m_iC * impulse * JwC;
+ cD -= m_mD * impulse * JvBD;
+ aD -= m_iD * impulse * JwD;
+
+ data.positions[m_indexA].c = cA;
+ data.positions[m_indexA].a = aA;
+ data.positions[m_indexB].c = cB;
+ data.positions[m_indexB].a = aB;
+ data.positions[m_indexC].c = cC;
+ data.positions[m_indexC].a = aC;
+ data.positions[m_indexD].c = cD;
+ data.positions[m_indexD].a = aD;
+
+ // TODO_ERIN not implemented
+ return linearError < b2_linearSlop;
+}
+
+b2Vec2 b2GearJoint::GetAnchorA() const
+{
+ return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2GearJoint::GetAnchorB() const
+{
+ return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
+{
+ b2Vec2 P = m_impulse * m_JvAC;
+ return inv_dt * P;
+}
+
+float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
+{
+ float32 L = m_impulse * m_JwA;
+ return inv_dt * L;
+}
+
+void b2GearJoint::SetRatio(float32 ratio)
+{
+ b2Assert(b2IsValid(ratio));
+ m_ratio = ratio;
+}
+
+float32 b2GearJoint::GetRatio() const
+{
+ return m_ratio;
+}
+
+void b2GearJoint::Dump()
+{
+ int32 indexA = m_bodyA->m_islandIndex;
+ int32 indexB = m_bodyB->m_islandIndex;
+
+ int32 index1 = m_joint1->m_index;
+ int32 index2 = m_joint2->m_index;
+
+ b2Log(" b2GearJointDef jd;\n");
+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);
+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);
+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
+ b2Log(" jd.joint1 = joints[%d];\n", index1);
+ b2Log(" jd.joint2 = joints[%d];\n", index2);
+ b2Log(" jd.ratio = %.15lef;\n", m_ratio);
+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2GearJoint.h b/Dependencies/Box2D/src/Dynamics/Joints/b2GearJoint.h
new file mode 100644
index 00000000..81897720
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2GearJoint.h
@@ -0,0 +1,125 @@
+/*
+* Copyright (c) 2006-2011 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_GEAR_JOINT_H
+#define B2_GEAR_JOINT_H
+
+#include
+
+/// Gear joint definition. This definition requires two existing
+/// revolute or prismatic joints (any combination will work).
+struct b2GearJointDef : public b2JointDef
+{
+ b2GearJointDef()
+ {
+ type = e_gearJoint;
+ joint1 = NULL;
+ joint2 = NULL;
+ ratio = 1.0f;
+ }
+
+ /// The first revolute/prismatic joint attached to the gear joint.
+ b2Joint* joint1;
+
+ /// The second revolute/prismatic joint attached to the gear joint.
+ b2Joint* joint2;
+
+ /// The gear ratio.
+ /// @see b2GearJoint for explanation.
+ float32 ratio;
+};
+
+/// A gear joint is used to connect two joints together. Either joint
+/// can be a revolute or prismatic joint. You specify a gear ratio
+/// to bind the motions together:
+/// coordinate1 + ratio * coordinate2 = constant
+/// The ratio can be negative or positive. If one joint is a revolute joint
+/// and the other joint is a prismatic joint, then the ratio will have units
+/// of length or units of 1/length.
+/// @warning You have to manually destroy the gear joint if joint1 or joint2
+/// is destroyed.
+class b2GearJoint : public b2Joint
+{
+public:
+ b2Vec2 GetAnchorA() const;
+ b2Vec2 GetAnchorB() const;
+
+ b2Vec2 GetReactionForce(float32 inv_dt) const;
+ float32 GetReactionTorque(float32 inv_dt) const;
+
+ /// Get the first joint.
+ b2Joint* GetJoint1() { return m_joint1; }
+
+ /// Get the second joint.
+ b2Joint* GetJoint2() { return m_joint2; }
+
+ /// Set/Get the gear ratio.
+ void SetRatio(float32 ratio);
+ float32 GetRatio() const;
+
+ /// Dump joint to dmLog
+ void Dump();
+
+protected:
+
+ friend class b2Joint;
+ b2GearJoint(const b2GearJointDef* data);
+
+ void InitVelocityConstraints(const b2SolverData& data);
+ void SolveVelocityConstraints(const b2SolverData& data);
+ bool SolvePositionConstraints(const b2SolverData& data);
+
+ b2Joint* m_joint1;
+ b2Joint* m_joint2;
+
+ b2JointType m_typeA;
+ b2JointType m_typeB;
+
+ // Body A is connected to body C
+ // Body B is connected to body D
+ b2Body* m_bodyC;
+ b2Body* m_bodyD;
+
+ // Solver shared
+ b2Vec2 m_localAnchorA;
+ b2Vec2 m_localAnchorB;
+ b2Vec2 m_localAnchorC;
+ b2Vec2 m_localAnchorD;
+
+ b2Vec2 m_localAxisC;
+ b2Vec2 m_localAxisD;
+
+ float32 m_referenceAngleA;
+ float32 m_referenceAngleB;
+
+ float32 m_constant;
+ float32 m_ratio;
+
+ float32 m_impulse;
+
+ // Solver temp
+ int32 m_indexA, m_indexB, m_indexC, m_indexD;
+ b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD;
+ float32 m_mA, m_mB, m_mC, m_mD;
+ float32 m_iA, m_iB, m_iC, m_iD;
+ b2Vec2 m_JvAC, m_JvBD;
+ float32 m_JwA, m_JwB, m_JwC, m_JwD;
+ float32 m_mass;
+};
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2Joint.cpp b/Dependencies/Box2D/src/Dynamics/Joints/b2Joint.cpp
new file mode 100644
index 00000000..c0b55952
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2Joint.cpp
@@ -0,0 +1,211 @@
+/*
+* Copyright (c) 2006-2007 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
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
+{
+ b2Joint* joint = NULL;
+
+ switch (def->type)
+ {
+ case e_distanceJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
+ joint = new (mem) b2DistanceJoint(static_cast(def));
+ }
+ break;
+
+ case e_mouseJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2MouseJoint));
+ joint = new (mem) b2MouseJoint(static_cast(def));
+ }
+ break;
+
+ case e_prismaticJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
+ joint = new (mem) b2PrismaticJoint(static_cast(def));
+ }
+ break;
+
+ case e_revoluteJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
+ joint = new (mem) b2RevoluteJoint(static_cast(def));
+ }
+ break;
+
+ case e_pulleyJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
+ joint = new (mem) b2PulleyJoint(static_cast(def));
+ }
+ break;
+
+ case e_gearJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2GearJoint));
+ joint = new (mem) b2GearJoint(static_cast(def));
+ }
+ break;
+
+ case e_wheelJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2WheelJoint));
+ joint = new (mem) b2WheelJoint(static_cast(def));
+ }
+ break;
+
+ case e_weldJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2WeldJoint));
+ joint = new (mem) b2WeldJoint(static_cast(def));
+ }
+ break;
+
+ case e_frictionJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
+ joint = new (mem) b2FrictionJoint(static_cast(def));
+ }
+ break;
+
+ case e_ropeJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2RopeJoint));
+ joint = new (mem) b2RopeJoint(static_cast(def));
+ }
+ break;
+
+ case e_motorJoint:
+ {
+ void* mem = allocator->Allocate(sizeof(b2MotorJoint));
+ joint = new (mem) b2MotorJoint(static_cast(def));
+ }
+ break;
+
+ default:
+ b2Assert(false);
+ break;
+ }
+
+ return joint;
+}
+
+void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
+{
+ joint->~b2Joint();
+ switch (joint->m_type)
+ {
+ case e_distanceJoint:
+ allocator->Free(joint, sizeof(b2DistanceJoint));
+ break;
+
+ case e_mouseJoint:
+ allocator->Free(joint, sizeof(b2MouseJoint));
+ break;
+
+ case e_prismaticJoint:
+ allocator->Free(joint, sizeof(b2PrismaticJoint));
+ break;
+
+ case e_revoluteJoint:
+ allocator->Free(joint, sizeof(b2RevoluteJoint));
+ break;
+
+ case e_pulleyJoint:
+ allocator->Free(joint, sizeof(b2PulleyJoint));
+ break;
+
+ case e_gearJoint:
+ allocator->Free(joint, sizeof(b2GearJoint));
+ break;
+
+ case e_wheelJoint:
+ allocator->Free(joint, sizeof(b2WheelJoint));
+ break;
+
+ case e_weldJoint:
+ allocator->Free(joint, sizeof(b2WeldJoint));
+ break;
+
+ case e_frictionJoint:
+ allocator->Free(joint, sizeof(b2FrictionJoint));
+ break;
+
+ case e_ropeJoint:
+ allocator->Free(joint, sizeof(b2RopeJoint));
+ break;
+
+ case e_motorJoint:
+ allocator->Free(joint, sizeof(b2MotorJoint));
+ break;
+
+ default:
+ b2Assert(false);
+ break;
+ }
+}
+
+b2Joint::b2Joint(const b2JointDef* def)
+{
+ b2Assert(def->bodyA != def->bodyB);
+
+ m_type = def->type;
+ m_prev = NULL;
+ m_next = NULL;
+ m_bodyA = def->bodyA;
+ m_bodyB = def->bodyB;
+ m_index = 0;
+ m_collideConnected = def->collideConnected;
+ m_islandFlag = false;
+ m_userData = def->userData;
+
+ m_edgeA.joint = NULL;
+ m_edgeA.other = NULL;
+ m_edgeA.prev = NULL;
+ m_edgeA.next = NULL;
+
+ m_edgeB.joint = NULL;
+ m_edgeB.other = NULL;
+ m_edgeB.prev = NULL;
+ m_edgeB.next = NULL;
+}
+
+bool b2Joint::IsActive() const
+{
+ return m_bodyA->IsActive() && m_bodyB->IsActive();
+}
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2Joint.h b/Dependencies/Box2D/src/Dynamics/Joints/b2Joint.h
new file mode 100644
index 00000000..0f04c702
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2Joint.h
@@ -0,0 +1,226 @@
+/*
+* Copyright (c) 2006-2007 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_JOINT_H
+#define B2_JOINT_H
+
+#include
+
+class b2Body;
+class b2Joint;
+struct b2SolverData;
+class b2BlockAllocator;
+
+enum b2JointType
+{
+ e_unknownJoint,
+ e_revoluteJoint,
+ e_prismaticJoint,
+ e_distanceJoint,
+ e_pulleyJoint,
+ e_mouseJoint,
+ e_gearJoint,
+ e_wheelJoint,
+ e_weldJoint,
+ e_frictionJoint,
+ e_ropeJoint,
+ e_motorJoint
+};
+
+enum b2LimitState
+{
+ e_inactiveLimit,
+ e_atLowerLimit,
+ e_atUpperLimit,
+ e_equalLimits
+};
+
+struct b2Jacobian
+{
+ b2Vec2 linear;
+ float32 angularA;
+ float32 angularB;
+};
+
+/// A joint edge is used to connect bodies and joints together
+/// in a joint graph where each body is a node and each joint
+/// is an edge. A joint edge belongs to a doubly linked list
+/// maintained in each attached body. Each joint has two joint
+/// nodes, one for each attached body.
+struct b2JointEdge
+{
+ b2Body* other; ///< provides quick access to the other body attached.
+ b2Joint* joint; ///< the joint
+ b2JointEdge* prev; ///< the previous joint edge in the body's joint list
+ b2JointEdge* next; ///< the next joint edge in the body's joint list
+};
+
+/// Joint definitions are used to construct joints.
+struct b2JointDef
+{
+ b2JointDef()
+ {
+ type = e_unknownJoint;
+ userData = NULL;
+ bodyA = NULL;
+ bodyB = NULL;
+ collideConnected = false;
+ }
+
+ /// The joint type is set automatically for concrete joint types.
+ b2JointType type;
+
+ /// Use this to attach application specific data to your joints.
+ void* userData;
+
+ /// The first attached body.
+ b2Body* bodyA;
+
+ /// The second attached body.
+ b2Body* bodyB;
+
+ /// Set this flag to true if the attached bodies should collide.
+ bool collideConnected;
+};
+
+/// The base joint class. Joints are used to constraint two bodies together in
+/// various fashions. Some joints also feature limits and motors.
+class b2Joint
+{
+public:
+
+ /// Get the type of the concrete joint.
+ b2JointType GetType() const;
+
+ /// Get the first body attached to this joint.
+ b2Body* GetBodyA();
+
+ /// Get the second body attached to this joint.
+ b2Body* GetBodyB();
+
+ /// Get the anchor point on bodyA in world coordinates.
+ virtual b2Vec2 GetAnchorA() const = 0;
+
+ /// Get the anchor point on bodyB in world coordinates.
+ virtual b2Vec2 GetAnchorB() const = 0;
+
+ /// Get the reaction force on bodyB at the joint anchor in Newtons.
+ virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0;
+
+ /// Get the reaction torque on bodyB in N*m.
+ virtual float32 GetReactionTorque(float32 inv_dt) const = 0;
+
+ /// Get the next joint the world joint list.
+ b2Joint* GetNext();
+ const b2Joint* GetNext() const;
+
+ /// Get the user data pointer.
+ void* GetUserData() const;
+
+ /// Set the user data pointer.
+ void SetUserData(void* data);
+
+ /// Short-cut function to determine if either body is inactive.
+ bool IsActive() const;
+
+ /// Get collide connected.
+ /// Note: modifying the collide connect flag won't work correctly because
+ /// the flag is only checked when fixture AABBs begin to overlap.
+ bool GetCollideConnected() const;
+
+ /// Dump this joint to the log file.
+ virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); }
+
+ /// Shift the origin for any points stored in world coordinates.
+ virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); }
+
+protected:
+ friend class b2World;
+ friend class b2Body;
+ friend class b2Island;
+ friend class b2GearJoint;
+
+ static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);
+ static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);
+
+ b2Joint(const b2JointDef* def);
+ virtual ~b2Joint() {}
+
+ virtual void InitVelocityConstraints(const b2SolverData& data) = 0;
+ virtual void SolveVelocityConstraints(const b2SolverData& data) = 0;
+
+ // This returns true if the position errors are within tolerance.
+ virtual bool SolvePositionConstraints(const b2SolverData& data) = 0;
+
+ b2JointType m_type;
+ b2Joint* m_prev;
+ b2Joint* m_next;
+ b2JointEdge m_edgeA;
+ b2JointEdge m_edgeB;
+ b2Body* m_bodyA;
+ b2Body* m_bodyB;
+
+ int32 m_index;
+
+ bool m_islandFlag;
+ bool m_collideConnected;
+
+ void* m_userData;
+};
+
+inline b2JointType b2Joint::GetType() const
+{
+ return m_type;
+}
+
+inline b2Body* b2Joint::GetBodyA()
+{
+ return m_bodyA;
+}
+
+inline b2Body* b2Joint::GetBodyB()
+{
+ return m_bodyB;
+}
+
+inline b2Joint* b2Joint::GetNext()
+{
+ return m_next;
+}
+
+inline const b2Joint* b2Joint::GetNext() const
+{
+ return m_next;
+}
+
+inline void* b2Joint::GetUserData() const
+{
+ return m_userData;
+}
+
+inline void b2Joint::SetUserData(void* data)
+{
+ m_userData = data;
+}
+
+inline bool b2Joint::GetCollideConnected() const
+{
+ return m_collideConnected;
+}
+
+#endif
diff --git a/Dependencies/Box2D/src/Dynamics/Joints/b2MotorJoint.cpp b/Dependencies/Box2D/src/Dynamics/Joints/b2MotorJoint.cpp
new file mode 100644
index 00000000..7f95ef62
--- /dev/null
+++ b/Dependencies/Box2D/src/Dynamics/Joints/b2MotorJoint.cpp
@@ -0,0 +1,304 @@
+/*
+* Copyright (c) 2006-2012 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
+#include
+#include