*update

60 files changed, 3379 insertions, 6373 deletions

diff --git a/Box2d/Assets/Program/Box2d/Box2DXDebug.cs b/Box2d/Assets/Program/Box2d/Box2DXDebug.cs
index 49013a6..50fa35c 100644
--- a/Box2d/Assets/Program/Box2d/Box2DXDebug.cs
+++ b/Box2d/Assets/Program/Box2d/Box2DXDebug.cs
@@ -31,30 +31,18 @@ namespace Box2DX
 		[Conditional("DEBUG")]
 		public static void Assert(bool condition)
 		{
-			if (!condition)
-			{
-				condition = condition;
-			}
 			Debug.Assert(condition);
 		}
 
 		[Conditional("DEBUG")]
 		public static void Assert(bool condition, string message)
 		{
-			if (!condition)
-			{
-				condition = condition;
-			}
 			Debug.Assert(condition, message);
 		}
 
 		[Conditional("DEBUG")]
 		public static void Assert(bool condition, string message, string detailMessage)
 		{
-			if (!condition)
-			{
-				condition = condition;
-			}
 			Debug.Assert(condition, message, detailMessage);
 		}
 
diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs
index fa10a9e..973ca6c 100644
--- a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideCircle.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,6 +19,10 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Common;
 
 namespace Box2DX.Collision
@@ -30,24 +34,45 @@ namespace Box2DX.Collision
 		{
 			manifold.PointCount = 0;
 
-			Vec2 p1 = Common.Math.Mul(xf1, circle1._position);
-			Vec2 p2 = Common.Math.Mul(xf2, circle2._position);
+			Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition());
+			Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition());
 
 			Vec2 d = p2 - p1;
 			float distSqr = Vec2.Dot(d, d);
-			float radius = circle1._radius + circle2._radius;
-			if (distSqr > radius * radius)
+			float r1 = circle1.GetRadius();
+			float r2 = circle2.GetRadius();
+			float radiusSum = r1 + r2;
+			if (distSqr > radiusSum * radiusSum)
 			{
 				return;
 			}
 
-			manifold.Type = ManifoldType.Circles;
-			manifold.LocalPoint = circle1._position;
-			manifold.LocalPlaneNormal.SetZero();
-			manifold.PointCount = 1;
+			float separation;
+			if (distSqr < Common.Settings.FLT_EPSILON)
+			{
+				separation = -radiusSum;
+				manifold.Normal.Set(0.0f, 1.0f);
+			}
+			else
+			{
+				float dist = Common.Math.Sqrt(distSqr);
+				separation = dist - radiusSum;
+				float a = 1.0f / dist;
+				manifold.Normal.X = a * d.X;
+				manifold.Normal.Y = a * d.Y;
+			}
 
-			manifold.Points[0].LocalPoint = circle2._position;
+			manifold.PointCount = 1;
 			manifold.Points[0].ID.Key = 0;
+			manifold.Points[0].Separation = separation;
+
+			p1 += r1 * manifold.Normal;
+			p2 -= r2 * manifold.Normal;
+
+			Vec2 p = 0.5f * (p1 + p2);
+
+			manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, p);
+			manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, p);
 		}
 
 		public static void CollidePolygonAndCircle(ref Manifold manifold,
@@ -56,16 +81,16 @@ namespace Box2DX.Collision
 			manifold.PointCount = 0;
 
 			// Compute circle position in the frame of the polygon.
-			Vec2 c = Common.Math.Mul(xf2, circle._position);
+			Vec2 c = Common.Math.Mul(xf2, circle.GetLocalPosition());
 			Vec2 cLocal = Common.Math.MulT(xf1, c);
 
 			// Find the min separating edge.
 			int normalIndex = 0;
 			float separation = -Settings.FLT_MAX;
-			float radius = polygon._radius + circle._radius;
-			int vertexCount = polygon._vertexCount;
-			Vec2[] vertices = polygon._vertices;
-			Vec2[] normals = polygon._normals;
+			float radius = circle.GetRadius();
+			int vertexCount = polygon.VertexCount;
+			Vec2[] vertices = polygon.GetVertices();
+			Vec2[] normals = polygon.Normals;
 
 			for (int i = 0; i < vertexCount; ++i)
 			{
@@ -83,73 +108,67 @@ namespace Box2DX.Collision
 				}
 			}
 
-			// Vertices that subtend the incident face.
-			int vertIndex1 = normalIndex;
-			int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
-			Vec2 v1 = vertices[vertIndex1];
-			Vec2 v2 = vertices[vertIndex2];
-
 			// If the center is inside the polygon ...
 			if (separation < Common.Settings.FLT_EPSILON)
 			{
 				manifold.PointCount = 1;
-				manifold.Type = ManifoldType.FaceA;
-				manifold.LocalPlaneNormal = normals[normalIndex];
-				manifold.LocalPoint = 0.5f * (v1 + v2);
-				manifold.Points[0].LocalPoint = circle._position;
-				manifold.Points[0].ID.Key = 0;
+				manifold.Normal = Common.Math.Mul(xf1.R, normals[normalIndex]);
+				manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
+				manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
+				manifold.Points[0].ID.Features.ReferenceEdge = 0;
+				manifold.Points[0].ID.Features.Flip = 0;
+				Vec2 position = c - radius * manifold.Normal;
+				manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position);
+				manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position);
+				manifold.Points[0].Separation = separation - radius;
 				return;
 			}
 
-			// Compute barycentric coordinates
-			float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
-			float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
-			if (u1 <= 0.0f)
-			{
-				if (Vec2.DistanceSquared(cLocal, v1) > radius * radius)
-				{
-					return;
-				}
+			// Project the circle center onto the edge segment.
+			int vertIndex1 = normalIndex;
+			int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
+			Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
 
-				manifold.PointCount = 1;
-				manifold.Type = ManifoldType.FaceA;
-				manifold.LocalPlaneNormal = cLocal - v1;
-				manifold.LocalPlaneNormal.Normalize();
-				manifold.LocalPoint = v1;
-				manifold.Points[0].LocalPoint = circle._position;
-				manifold.Points[0].ID.Key = 0;
+			float length = e.Normalize();
+			Box2DXDebug.Assert(length > Settings.FLT_EPSILON);
+
+			// Project the center onto the edge.
+			float u = Vec2.Dot(cLocal - vertices[vertIndex1], e);
+			Vec2 p;
+			if (u <= 0.0f)
+			{
+				p = vertices[vertIndex1];
+				manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
+				manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
 			}
-			else if (u2 <= 0.0f)
+			else if (u >= length)
 			{
-				if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
-				{
-					return;
-				}
-
-				manifold.PointCount = 1;
-				manifold.Type = ManifoldType.FaceA;
-				manifold.LocalPlaneNormal = cLocal - v2;
-				manifold.LocalPlaneNormal.Normalize();
-				manifold.LocalPoint = v2;
-				manifold.Points[0].LocalPoint = circle._position;
-				manifold.Points[0].ID.Key = 0;
+				p = vertices[vertIndex2];
+				manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
+				manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2;
 			}
 			else
 			{
-				Vec2 faceCenter = 0.5f * (v1 + v2);
-				float separation_ = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]);
-				if (separation_ > radius)
-				{
-					return;
-				}
+				p = vertices[vertIndex1] + u * e;
+				manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
+				manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
+			}
 
-				manifold.PointCount = 1;
-				manifold.Type = ManifoldType.FaceA;
-				manifold.LocalPlaneNormal = normals[vertIndex1];
-				manifold.LocalPoint = faceCenter;
-				manifold.Points[0].LocalPoint = circle._position;
-				manifold.Points[0].ID.Key = 0;
+			Vec2 d = cLocal - p;
+			float dist = d.Normalize();
+			if (dist > radius)
+			{
+				return;
 			}
+
+			manifold.PointCount = 1;
+			manifold.Normal = Common.Math.Mul(xf1.R, d);
+			Vec2 position_ = c - radius * manifold.Normal;
+			manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position_);
+			manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position_);
+			manifold.Points[0].Separation = dist - radius;
+			manifold.Points[0].ID.Features.ReferenceEdge = 0;
+			manifold.Points[0].ID.Features.Flip = 0;
 		}
 	}
 }
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs
deleted file mode 100644
index 8ea3821..0000000
--- a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
-
-  This software is provided 'as-is', without any express or implied
-  warranty.  In no event will the authors be held liable for any damages
-  arising from the use of this software.
-
-  Permission is granted to anyone to use this software for any purpose,
-  including commercial applications, and to alter it and redistribute it
-  freely, subject to the following restrictions:
-
-  1. The origin of this software must not be misrepresented; you must not
-     claim that you wrote the original software. If you use this software
-     in a product, an acknowledgment in the product documentation would be
-     appreciated but is not required.
-  2. Altered source versions must be plainly marked as such, and must not be
-     misrepresented as being the original software.
-  3. This notice may not be removed or altered from any source distribution.
-*/
-
-using Box2DX.Common;
-
-namespace Box2DX.Collision
-{
-	public partial class Collision
-	{
-		// This implements 2-sided edge vs circle collision.
-		public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, XForm transformA, CircleShape circle, XForm transformB)
-		{
-			manifold.PointCount = 0;
-			Vec2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
-			Vec2 normal = edge._normal;
-			Vec2 v1 = edge._v1;
-			Vec2 v2 = edge._v2;
-			float radius = edge._radius + circle._radius;
-
-			// Barycentric coordinates
-			float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
-			float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
-
-			if (u1 <= 0.0f)
-			{
-				// Behind v1
-				if (Vec2.DistanceSquared(cLocal, v1) > radius * radius)
-				{
-					return;
-				}
-
-				manifold.PointCount = 1;
-				manifold.Type = ManifoldType.FaceA;
-				manifold.LocalPlaneNormal = cLocal - v1;
-				manifold.LocalPlaneNormal.Normalize();
-				manifold.LocalPoint = v1;
-				manifold.Points[0].LocalPoint = circle._position;
-				manifold.Points[0].ID.Key = 0;
-			}
-			else if (u2 <= 0.0f)
-			{
-				// Ahead of v2
-				if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
-				{
-					return;
-				}
-
-				manifold.PointCount = 1;
-				manifold.Type = ManifoldType.FaceA;
-				manifold.LocalPlaneNormal = cLocal - v2;
-				manifold.LocalPlaneNormal.Normalize();
-				manifold.LocalPoint = v2;
-				manifold.Points[0].LocalPoint = circle._position;
-				manifold.Points[0].ID.Key = 0;
-			}
-			else
-			{
-				float separation = Vec2.Dot(cLocal - v1, normal);
-				if (separation < -radius || radius < separation)
-				{
-					return;
-				}
-				
-				manifold.PointCount = 1;
-				manifold.Type = ManifoldType.FaceA;
-				manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal;
-				manifold.LocalPoint = 0.5f * (v1 + v2);
-				manifold.Points[0].LocalPoint = circle._position;
-				manifold.Points[0].ID.Key = 0;
-			}
-		}
-
-		// Polygon versus 2-sided edge.
-		public static void CollidePolyAndEdge(ref Manifold manifold, PolygonShape polygon, XForm transformA, EdgeShape edge, XForm transformB)
-		{
-			PolygonShape polygonB = new PolygonShape();
-			polygonB.SetAsEdge(edge._v1, edge._v2);
-
-			CollidePolygons(ref manifold, polygon, transformA, polygonB, transformB);
-		}
-	}
-}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta
deleted file mode 100644
index b71db55..0000000
--- a/Box2d/Assets/Program/Box2d/Collision/Collision.CollideEdge.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: b683cd8505628a247bde0d512af54021
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs
index 65c48e4..87f64e6 100644
--- a/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/Collision.CollidePoly.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,23 +19,78 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Common;
 
 namespace Box2DX.Collision
 {
 	public partial class Collision
 	{
+		public struct ClipVertex
+		{
+			public Vec2 V;
+			public ContactID ID;
+		}
+
+		public static int ClipSegmentToLine(out ClipVertex[] vOut, ClipVertex[] vIn,
+			Vec2 normal, float offset)
+		{
+			if (vIn.Length != 2)
+				Box2DXDebug.ThrowBox2DXException("vIn should contain 2 element, but contains " + vIn.Length.ToString());
+			vOut = new ClipVertex[2];
+
+			// Start with no output points
+			int numOut = 0;
+
+			// Calculate the distance of end points to the line
+			float distance0 = Vec2.Dot(normal, vIn[0].V) - offset;
+			float distance1 = Vec2.Dot(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
+				float interp = distance0 / (distance0 - distance1);
+				vOut[numOut].V = vIn[0].V + interp * (vIn[1].V - vIn[0].V);
+				if (distance0 > 0.0f)
+				{
+					vOut[numOut].ID = vIn[0].ID;
+				}
+				else
+				{
+					vOut[numOut].ID = vIn[1].ID;
+				}
+				++numOut;
+			}
+
+			return numOut;
+		}
+
 		/// <summary>
 		/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
 		/// </summary>
-		public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
+		/// <param name="poly1"></param>
+		/// <param name="xf1"></param>
+		/// <param name="edge1"></param>
+		/// <param name="poly2"></param>
+		/// <param name="xf2"></param>
+		/// <returns></returns>
+		public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1,
+			PolygonShape poly2, XForm xf2)
 		{
-			int count1 = poly1._vertexCount;
-			Vec2[] vertices1 = poly1._vertices;
-			Vec2[] normals1 = poly1._normals;
+			int count1 = poly1.VertexCount;
+			Vec2[] vertices1 = poly1.GetVertices();
+			Vec2[] normals1 = poly1.Normals;
 
-			int count2 = poly2._vertexCount;
-			Vec2[] vertices2 = poly2._vertices;
+			int count2 = poly2.VertexCount;
+			Vec2[] vertices2 = poly2.GetVertices();
 
 			Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
 
@@ -65,13 +120,20 @@ namespace Box2DX.Collision
 		/// <summary>
 		/// Find the max separation between poly1 and poly2 using edge normals from poly1.
 		/// </summary>
-		public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2)
+		/// <param name="edgeIndex"></param>
+		/// <param name="poly1"></param>
+		/// <param name="xf1"></param>
+		/// <param name="poly2"></param>
+		/// <param name="xf2"></param>
+		/// <returns></returns>
+		public static float FindMaxSeparation(ref int edgeIndex,
+			PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2)
 		{
-			int count1 = poly1._vertexCount;
-			Vec2[] normals1 = poly1._normals;
+			int count1 = poly1.VertexCount;
+			Vec2[] normals1 = poly1.Normals;
 
 			// Vector pointing from the centroid of poly1 to the centroid of poly2.
-			Vec2 d = Common.Math.Mul(xf2, poly2._centroid) - Common.Math.Mul(xf1, poly1._centroid);
+			Vec2 d = Common.Math.Mul(xf2, poly2.GetCentroid()) - Common.Math.Mul(xf1, poly1.GetCentroid());
 			Vec2 dLocal1 = Common.Math.MulT(xf1.R, d);
 
 			// Find edge normal on poly1 that has the largest projection onto d.
@@ -89,14 +151,26 @@ namespace Box2DX.Collision
 
 			// Get the separation for the edge normal.
 			float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+			if (s > 0.0f)
+			{
+				return s;
+			}
 
 			// Check the separation for the previous edge normal.
 			int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
 			float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
+			if (sPrev > 0.0f)
+			{
+				return sPrev;
+			}
 
 			// Check the separation for the next edge normal.
 			int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
 			float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
+			if (sNext > 0.0f)
+			{
+				return sNext;
+			}
 
 			// Find the best edge and the search direction.
 			int bestEdge;
@@ -129,6 +203,10 @@ namespace Box2DX.Collision
 					edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
 
 				s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+				if (s > 0.0f)
+				{
+					return s;
+				}
 
 				if (s > bestSeparation)
 				{
@@ -148,12 +226,12 @@ namespace Box2DX.Collision
 		public static void FindIncidentEdge(out ClipVertex[] c,
 			PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
 		{
-			int count1 = poly1._vertexCount;
-			Vec2[] normals1 = poly1._normals;
+			int count1 = poly1.VertexCount;
+			Vec2[] normals1 = poly1.Normals;
 
-			int count2 = poly2._vertexCount;
-			Vec2[] vertices2 = poly2._vertices;
-			Vec2[] normals2 = poly2._normals;
+			int count2 = poly2.VertexCount;
+			Vec2[] vertices2 = poly2.GetVertices();
+			Vec2[] normals2 = poly2.Normals;
 
 			Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
 
@@ -200,16 +278,15 @@ namespace Box2DX.Collision
 			PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
 		{
 			manifold.PointCount = 0;
-			float totalRadius = polyA._radius + polyB._radius;
 
 			int edgeA = 0;
 			float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
-			if (separationA > totalRadius)
+			if (separationA > 0.0f)
 				return;
 
 			int edgeB = 0;
 			float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
-			if (separationB > totalRadius)
+			if (separationB > 0.0f)
 				return;
 
 			PolygonShape poly1;	// reference poly
@@ -217,9 +294,10 @@ namespace Box2DX.Collision
 			XForm xf1, xf2;
 			int edge1;		// reference edge
 			byte flip;
-			const float k_relativeTol = 0.98f;
-			const float k_absoluteTol = 0.001f;
+			float k_relativeTol = 0.98f;
+			float k_absoluteTol = 0.001f;
 
+			// TODO_ERIN use "radius" of poly for absolute tolerance.
 			if (separationB > k_relativeTol * separationA + k_absoluteTol)
 			{
 				poly1 = polyB;
@@ -227,7 +305,6 @@ namespace Box2DX.Collision
 				xf1 = xfB;
 				xf2 = xfA;
 				edge1 = edgeB;
-				manifold.Type = ManifoldType.FaceB;
 				flip = 1;
 			}
 			else
@@ -237,25 +314,19 @@ namespace Box2DX.Collision
 				xf1 = xfA;
 				xf2 = xfB;
 				edge1 = edgeA;
-				manifold.Type = ManifoldType.FaceA;
 				flip = 0;
 			}
 
 			ClipVertex[] incidentEdge;
 			Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
 
-			int count1 = poly1._vertexCount;
-			Vec2[] vertices1 = poly1._vertices;
+			int count1 = poly1.VertexCount;
+			Vec2[] vertices1 = poly1.GetVertices();
 
 			Vec2 v11 = vertices1[edge1];
 			Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
 
 			Vec2 dv = v12 - v11;
-
-			Vec2 localNormal = Vec2.Cross(dv, 1.0f);
-			localNormal.Normalize();
-			Vec2 planePoint = 0.5f * (v11 + v12);
-
 			Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11);
 			sideNormal.Normalize();
 			Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f);
@@ -285,18 +356,19 @@ namespace Box2DX.Collision
 				return;
 
 			// Now clipPoints2 contains the clipped points.
-			manifold.LocalPlaneNormal = localNormal;
-			manifold.LocalPoint = planePoint;
+			manifold.Normal = flip!=0 ? -frontNormal : frontNormal;
 
 			int pointCount = 0;
 			for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
 			{
 				float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
 
-				if (separation <= totalRadius)
+				if (separation <= 0.0f)
 				{
 					ManifoldPoint cp = manifold.Points[pointCount];
-					cp.LocalPoint = Common.Math.MulT(xf2, clipPoints2[i].V);
+					cp.Separation = separation;
+					cp.LocalPoint1 = Box2DX.Common.Math.MulT(xfA, clipPoints2[i].V);
+					cp.LocalPoint2 = Box2DX.Common.Math.MulT(xfB, clipPoints2[i].V);
 					cp.ID = clipPoints2[i].ID;
 					cp.ID.Features.Flip = flip;
 					++pointCount;
diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs
index d6abeb6..71b6d49 100644
--- a/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/Collision.Distance.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,612 +19,370 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
-#define DEBUG
-
 using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Common;
 
 namespace Box2DX.Collision
 {
-	/// <summary>
-	/// Used to warm start Distance.
-	/// Set count to zero on first call.
-	/// </summary>
-	public struct SimplexCache
+	public partial class Collision
 	{
-		/// <summary>
-		/// Length or area.
-		/// </summary>
-		public Single Metric;
-		public UInt16 Count;
-		/// <summary>
-		/// Vertices on shape A.
-		/// </summary>
-		//public Byte[/*3*/] IndexA;
-		public IndexArray IndexA;
-		/// <summary>
-		/// Vertices on shape B.
-		/// </summary>
-		//public Byte[/*3*/] IndexB;
-		public IndexArray IndexB;
-
-		//public SimplexCache(byte init)
-		//{
-		//	Metric = 0;
-		//	Count = 0;
-		//	IndexA = new Byte[3];
-		//	IndexB = new Byte[3];
-		//}
-	}
+		public static int GJKIterations = 0;
 
-	public struct IndexArray
-	{
-		private Byte I0, I1, I2;
+		// GJK using Voronoi regions (Christer Ericson) and region selection
+		// optimizations (Casey Muratori).
 
-		public Byte this[int index]
+		// The origin is either in the region of points[1] or in the edge region. The origin is
+		// not in region of points[0] because that is the old point.
+		public static int ProcessTwo(out Vec2 x1, out Vec2 x2, ref Vec2[] p1s, ref Vec2[] p2s,
+			ref Vec2[] points)
 		{
-			get
+			// If in point[1] region
+			Vec2 r = -points[1];
+			Vec2 d = points[0] - points[1];
+			float length = d.Normalize();
+			float lambda = Vec2.Dot(r, d);
+			if (lambda <= 0.0f || length < Common.Settings.FLT_EPSILON)
 			{
-#if DEBUG
-				Box2DXDebug.Assert(index >= 0 && index < 3);
-#endif
-				if (index == 0) return I0;
-				else if (index == 1) return I1;
-				else return I2;
+				// The simplex is reduced to a point.
+				x1 = p1s[1];
+				x2 = p2s[1];
+				p1s[0] = p1s[1];
+				p2s[0] = p2s[1];
+				points[0] = points[1];
+				return 1;
 			}
-			set
-			{
-#if DEBUG
-				Box2DXDebug.Assert(index >= 0 && index < 3);
-#endif
-				if (index == 0) I0 = value;
-				else if (index == 1) I1 = value;
-				else I2 = value;
-			}
-		}
-	}
-
-	/// <summary>
-	/// Input for Distance.
-	/// You have to option to use the shape radii
-	/// in the computation.
-	/// </summary>
-	public struct DistanceInput
-	{
-		public XForm TransformA;
-		public XForm TransformB;
-		public bool UseRadii;
-	}
-
-	/// <summary>
-	/// Output for Distance.
-	/// </summary>
-	public struct DistanceOutput
-	{
-		/// <summary>
-		/// Closest point on shapeA.
-		/// </summary>
-		public Vec2 PointA;
-		/// <summary>
-		/// Closest point on shapeB.
-		/// </summary>
-		public Vec2 PointB;
-		public float Distance;
-		/// <summary>
-		/// Number of GJK iterations used.
-		/// </summary>
-		public int Iterations;
-	}
-
-	// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
-
-	internal struct SimplexVertex
-	{
-		internal Vec2 wA;		// support point in shapeA
-		internal Vec2 wB;		// support point in shapeB
-		internal Vec2 w;		// wB - wA
-		internal float a;		// barycentric coordinate for closest point
-		internal int indexA;	// wA index
-		internal int indexB;	// wB index
-	}
-
-	internal struct Simplex
-	{
-		internal SimplexVertex _v1, _v2, _v3;
-		internal int _count;
-
-		internal unsafe void ReadCache(SimplexCache* cache, Shape shapeA, XForm transformA, Shape shapeB, XForm transformB)
-		{
-			Box2DXDebug.Assert(0 <= cache->Count && cache->Count <= 3);
-
-			// Copy data from cache.
-			_count = cache->Count;
-			SimplexVertex** vertices = stackalloc SimplexVertex*[3];
-			fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3)
-			{
-				vertices[0] = v1Ptr;
-				vertices[1] = v2Ptr;
-				vertices[2] = v3Ptr;
-				for (int i = 0; i < _count; ++i)
-				{
-					SimplexVertex* v = vertices[i];
-					v->indexA = cache->IndexA[i];
-					v->indexB = cache->IndexB[i];
-					Vec2 wALocal = shapeA.GetVertex(v->indexA);
-					Vec2 wBLocal = shapeB.GetVertex(v->indexB);
-					v->wA = Common.Math.Mul(transformA, wALocal);
-					v->wB = Common.Math.Mul(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 (_count > 1)
-				{
-					float metric1 = cache->Metric;
-					float metric2 = GetMetric();
-					if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON)
-					{
-						// Reset the simplex.
-						_count = 0;
-					}
-				}
-
-				// If the cache is empty or invalid ...
-				if (_count == 0)
-				{
-					SimplexVertex* v = vertices[0];
-					v->indexA = 0;
-					v->indexB = 0;
-					Vec2 wALocal = shapeA.GetVertex(0);
-					Vec2 wBLocal = shapeB.GetVertex(0);
-					v->wA = Common.Math.Mul(transformA, wALocal);
-					v->wB = Common.Math.Mul(transformB, wBLocal);
-					v->w = v->wB - v->wA;
-					_count = 1;
-				}
-			}
+			// Else in edge region
+			lambda /= length;
+			x1 = p1s[1] + lambda * (p1s[0] - p1s[1]);
+			x2 = p2s[1] + lambda * (p2s[0] - p2s[1]);
+			return 2;
 		}
 
-		internal unsafe void WriteCache(SimplexCache* cache)
+		// Possible regions:
+		// - points[2]
+		// - edge points[0]-points[2]
+		// - edge points[1]-points[2]
+		// - inside the triangle
+		public static int ProcessThree(out Vec2 x1, out Vec2 x2, ref Vec2[] p1s, ref Vec2[] p2s,
+			ref Vec2[] points)
 		{
-			cache->Metric = GetMetric();
-			cache->Count = (UInt16)_count;
-			SimplexVertex** vertices = stackalloc SimplexVertex*[3];
-			fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3)
-			{
-				vertices[0] = v1Ptr;
-				vertices[1] = v2Ptr;
-				vertices[2] = v3Ptr;
-				for (int i = 0; i < _count; ++i)
-				{
-					cache->IndexA[i] = (Byte)(vertices[i]->indexA);
-					cache->IndexB[i] = (Byte)(vertices[i]->indexB);
-				}
-			}
-		}
+			Vec2 a = points[0];
+			Vec2 b = points[1];
+			Vec2 c = points[2];
 
-		internal Vec2 GetClosestPoint()
-		{
-			switch (_count)
-			{
-				case 0:
-#if DEBUG
-					Box2DXDebug.Assert(false);
-#endif
-					return Vec2.Zero;
-				case 1:
-					return _v1.w;
-				case 2:
-					return _v1.a * _v1.w + _v2.a * _v2.w;
-				case 3:
-					return Vec2.Zero;
-				default:
-#if DEBUG
-					Box2DXDebug.Assert(false);
-#endif
-					return Vec2.Zero;
-			}
-		}
+			Vec2 ab = b - a;
+			Vec2 ac = c - a;
+			Vec2 bc = c - b;
 
-		internal unsafe void GetWitnessPoints(Vec2* pA, Vec2* pB)
-		{
-			switch (_count)
-			{
-				case 0:
-					Box2DXDebug.Assert(false);
-					break;
-
-				case 1:
-					*pA = _v1.wA;
-					*pB = _v1.wB;
-					break;
-
-				case 2:
-					*pA = _v1.a * _v1.wA + _v2.a * _v2.wA;
-					*pB = _v1.a * _v1.wB + _v2.a * _v2.wB;
-					break;
-
-				case 3:
-					*pA = _v1.a * _v1.wA + _v2.a * _v2.wA + _v3.a * _v3.wA;
-					*pB = *pA;
-					break;
-
-				default:
-					Box2DXDebug.Assert(false);
-					break;
-			}
-		}
+			float sn = -Vec2.Dot(a, ab), sd = Vec2.Dot(b, ab);
+			float tn = -Vec2.Dot(a, ac), td = Vec2.Dot(c, ac);
+			float un = -Vec2.Dot(b, bc), ud = Vec2.Dot(c, bc);
 
-		internal float GetMetric()
-		{
-			switch (_count)
+			// In vertex c region?
+			if (td <= 0.0f && ud <= 0.0f)
 			{
-				case 0:
-#if DEBUG
-					Box2DXDebug.Assert(false);
-#endif
-					return 0.0f;
+				// Single point
+				x1 = p1s[2];
+				x2 = p2s[2];
+				p1s[0] = p1s[2];
+				p2s[0] = p2s[2];
+				points[0] = points[2];
+				return 1;
+			}
 
-				case 1:
-					return 0.0f;
+			// Should not be in vertex a or b region.
 
-				case 2:
-					return Vec2.Distance(_v1.w, _v2.w);
+			//B2_NOT_USED(sd);
+			//B2_NOT_USED(sn);			
+			Box2DXDebug.Assert(sn > 0.0f || tn > 0.0f);
+			Box2DXDebug.Assert(sd > 0.0f || un > 0.0f);
 
-				case 3:
-					return Vec2.Cross(_v2.w - _v1.w, _v3.w - _v1.w);
+			float n = Vec2.Cross(ab, ac);
 
-				default:
-#if DEBUG
-					Box2DXDebug.Assert(false);
+#if TARGET_FLOAT32_IS_FIXED
+				n = (n < 0.0f) ? -1.0f : ((n > 0.0f) ? 1.0f : 0.0f);
 #endif
-					return 0.0f;
-			}
-		}
 
-		// 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
-		internal void Solve2()
-		{
-			Vec2 w1 = _v1.w;
-			Vec2 w2 = _v2.w;
-			Vec2 e12 = w2 - w1;
+			// Should not be in edge ab region.
+			float vc = n * Vec2.Cross(a, b);
+			Box2DXDebug.Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f);
 
-			// w1 region
-			float d12_2 = -Vec2.Dot(w1, e12);
-			if (d12_2 <= 0.0f)
+			// In edge bc region?
+			float va = n * Vec2.Cross(b, c);
+			if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un + ud) > 0.0f)
 			{
-				// a2 <= 0, so we clamp it to 0
-				_v1.a = 1.0f;
-				_count = 1;
-				return;
+				Box2DXDebug.Assert(un + ud > 0.0f);
+				float lambda = un / (un + ud);
+				x1 = p1s[1] + lambda * (p1s[2] - p1s[1]);
+				x2 = p2s[1] + lambda * (p2s[2] - p2s[1]);
+				p1s[0] = p1s[2];
+				p2s[0] = p2s[2];
+				points[0] = points[2];
+				return 2;
 			}
 
-			// w2 region
-			float d12_1 = Vec2.Dot(w2, e12);
-			if (d12_1 <= 0.0f)
+			// In edge ac region?
+			float vb = n * Vec2.Cross(c, a);
+			if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn + td) > 0.0f)
 			{
-				// a1 <= 0, so we clamp it to 0
-				_v2.a = 1.0f;
-				_count = 1;
-				_v1 = _v2;
-				return;
+				Box2DXDebug.Assert(tn + td > 0.0f);
+				float lambda = tn / (tn + td);
+				x1 = p1s[0] + lambda * (p1s[2] - p1s[0]);
+				x2 = p2s[0] + lambda * (p2s[2] - p2s[0]);
+				p1s[1] = p1s[2];
+				p2s[1] = p2s[2];
+				points[1] = points[2];
+				return 2;
 			}
 
-			// Must be in e12 region.
-			float inv_d12 = 1.0f / (d12_1 + d12_2);
-			_v1.a = d12_1 * inv_d12;
-			_v2.a = d12_2 * inv_d12;
-			_count = 2;
+			// Inside the triangle, compute barycentric coordinates
+			float denom = va + vb + vc;
+			Box2DXDebug.Assert(denom > 0.0f);
+			denom = 1.0f / denom;
+#if TARGET_FLOAT32_IS_FIXED
+			x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]);
+			x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]);
+#else
+			float u = va * denom;
+			float v = vb * denom;
+			float w = 1.0f - u - v;
+			x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
+			x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
+#endif
+			return 3;
 		}
 
-		// Possible regions:
-		// - points[2]
-		// - edge points[0]-points[2]
-		// - edge points[1]-points[2]
-		// - inside the triangle
-		internal void Solve3()
+		public static bool InPoints(Vec2 w, Vec2[] points, int pointCount)
 		{
-			Vec2 w1 = _v1.w;
-			Vec2 w2 = _v2.w;
-			Vec2 w3 = _v3.w;
-
-			// Edge12
-			// [1      1     ][a1] = [1]
-			// [w1.e12 w2.e12][a2] = [0]
-			// a3 = 0
-			Vec2 e12 = w2 - w1;
-			float w1e12 = Vec2.Dot(w1, e12);
-			float w2e12 = Vec2.Dot(w2, e12);
-			float d12_1 = w2e12;
-			float d12_2 = -w1e12;
-
-			// Edge13
-			// [1      1     ][a1] = [1]
-			// [w1.e13 w3.e13][a3] = [0]
-			// a2 = 0
-			Vec2 e13 = w3 - w1;
-			float w1e13 = Vec2.Dot(w1, e13);
-			float w3e13 = Vec2.Dot(w3, e13);
-			float d13_1 = w3e13;
-			float d13_2 = -w1e13;
-
-			// Edge23
-			// [1      1     ][a2] = [1]
-			// [w2.e23 w3.e23][a3] = [0]
-			// a1 = 0
-			Vec2 e23 = w3 - w2;
-			float w2e23 = Vec2.Dot(w2, e23);
-			float w3e23 = Vec2.Dot(w3, e23);
-			float d23_1 = w3e23;
-			float d23_2 = -w2e23;
-
-			// Triangle123
-			float n123 = Vec2.Cross(e12, e13);
-
-			float d123_1 = n123 * Vec2.Cross(w2, w3);
-			float d123_2 = n123 * Vec2.Cross(w3, w1);
-			float d123_3 = n123 * Vec2.Cross(w1, w2);
-
-			// w1 region
-			if (d12_2 <= 0.0f && d13_2 <= 0.0f)
-			{
-				_v1.a = 1.0f;
-				_count = 1;
-				return;
-			}
-
-			// e12
-			if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
+			float k_tolerance = 100.0f * Common.Settings.FLT_EPSILON;
+			for (int i = 0; i < pointCount; ++i)
 			{
-				float inv_d12 = 1.0f / (d12_1 + d12_2);
-				_v1.a = d12_1 * inv_d12;
-				_v2.a = d12_1 * inv_d12;
-				_count = 2;
-				return;
-			}
-
-			// e13
-			if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
-			{
-				float inv_d13 = 1.0f / (d13_1 + d13_2);
-				_v1.a = d13_1 * inv_d13;
-				_v3.a = d13_2 * inv_d13;
-				_count = 2;
-				_v2 = _v3;
-				return;
-			}
-
-			// w2 region
-			if (d12_1 <= 0.0f && d23_2 <= 0.0f)
-			{
-				_v2.a = 1.0f;
-				_count = 1;
-				_v1 = _v2;
-				return;
-			}
+				Vec2 d = Common.Math.Abs(w - points[i]);
+				Vec2 m = Common.Math.Max(Common.Math.Abs(w), Common.Math.Abs(points[i]));
 
-			// w3 region
-			if (d13_1 <= 0.0f && d23_1 <= 0.0f)
-			{
-				_v3.a = 1.0f;
-				_count = 1;
-				_v1 = _v3;
-				return;
+				if (d.X < k_tolerance * (m.X + 1.0f) &&
+					d.Y < k_tolerance * (m.Y + 1.0f))
+				{
+					return true;
+				}
 			}
 
-			// e23
-			if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
-			{
-				float inv_d23 = 1.0f / (d23_1 + d23_2);
-				_v2.a = d23_1 * inv_d23;
-				_v3.a = d23_2 * inv_d23;
-				_count = 2;
-				_v1 = _v3;
-				return;
-			}
+			return false;
+		}
 
-			// Must be in triangle123
-			float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
-			_v1.a = d123_1 * inv_d123;
-			_v2.a = d123_2 * inv_d123;
-			_v3.a = d123_3 * inv_d123;
-			_count = 3;
+		public interface IGenericShape
+		{
+			Vec2 Support(XForm xf, Vec2 v);
+			Vec2 GetFirstVertex(XForm xf);
 		}
-	}
 
-	public partial class Collision
-	{
-		/// <summary>
-		/// Compute the closest points between two shapes. Supports any combination of:
-		/// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output.
-		/// On the first call set SimplexCache.Count to zero.
-		/// </summary>		
-		public unsafe static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
+		public static float DistanceGeneric<T1, T2>(out Vec2 x1, out Vec2 x2,
+						   T1 shape1_, XForm xf1, T2 shape2_, XForm xf2)
 		{
-			output = new DistanceOutput();
+			IGenericShape shape1 = shape1_ as IGenericShape;
+			IGenericShape shape2 = shape2_ as IGenericShape;
 
-			XForm transformA = input.TransformA;
-			XForm transformB = input.TransformB;
+			if (shape1 == null || shape2 == null)
+				Box2DXDebug.Assert(false, "Can not cast some parameters to IGenericShape");
 
-			// Initialize the simplex.
-			Simplex simplex = new Simplex();
-			fixed (SimplexCache* sPtr = &cache)
-			{
-				simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
-			}	
+			Vec2[] p1s = new Vec2[3], p2s = new Vec2[3];
+			Vec2[] points = new Vec2[3];
+			int pointCount = 0;
 
-			// Get simplex vertices as an array.
-			SimplexVertex* vertices = &simplex._v1;
+			x1 = shape1.GetFirstVertex(xf1);
+			x2 = shape2.GetFirstVertex(xf2);
 
-			// These store the vertices of the last simplex so that we
-			// can check for duplicates and prevent cycling.
-			int* lastA = stackalloc int[4], lastB = stackalloc int[4];
-			int lastCount;
+			float vSqr = 0.0f;
+			int maxIterations = 20;
 
-			// Main iteration loop.
-			int iter = 0;
-			const int k_maxIterationCount = 20;
-			while (iter < k_maxIterationCount)
+			for (int iter = 0; iter < maxIterations; ++iter)
 			{
-				// Copy simplex so we can identify duplicates.
-				lastCount = simplex._count;
-				int i;
-				for (i = 0; i < lastCount; ++i)
+				Vec2 v = x2 - x1;
+				Vec2 w1 = shape1.Support(xf1, v);
+				Vec2 w2 = shape2.Support(xf2, -v);
+
+				vSqr = Vec2.Dot(v, v);
+				Vec2 w = w2 - w1;
+				float vw = Vec2.Dot(v, w);
+				if (vSqr - vw <= 0.01f * vSqr || Collision.InPoints(w, points, pointCount)) // or w in points
 				{
-					lastA[i] = vertices[i].indexA;
-					lastB[i] = vertices[i].indexB;
+					if (pointCount == 0)
+					{
+						x1 = w1;
+						x2 = w2;
+					}
+					Collision.GJKIterations = iter;
+					return Common.Math.Sqrt(vSqr);
 				}
 
-				switch (simplex._count)
+				switch (pointCount)
 				{
-					case 1:
-						break;
-
-					case 2:
-						simplex.Solve2();
+					case 0:
+						p1s[0] = w1;
+						p2s[0] = w2;
+						points[0] = w;
+						x1 = p1s[0];
+						x2 = p2s[0];
+						++pointCount;
 						break;
 
-					case 3:
-						simplex.Solve3();
+					case 1:
+						p1s[1] = w1;
+						p2s[1] = w2;
+						points[1] = w;
+						pointCount = Collision.ProcessTwo(out x1, out x2, ref p1s, ref p2s, ref points);
 						break;
 
-					default:
-#if DEBUG
-						Box2DXDebug.Assert(false);
-#endif
+					case 2:
+						p1s[2] = w1;
+						p2s[2] = w2;
+						points[2] = w;
+						pointCount = Collision.ProcessThree(out x1, out x2, ref p1s, ref p2s, ref points);
 						break;
 				}
 
-				// If we have 3 points, then the origin is in the corresponding triangle.
-				if (simplex._count == 3)
+				// If we have three points, then the origin is in the corresponding triangle.
+				if (pointCount == 3)
 				{
-					break;
+					Collision.GJKIterations = iter;
+					return 0.0f;
 				}
 
-				// Compute closest point.
-				Vec2 p = simplex.GetClosestPoint();
-				float distanceSqr = p.LengthSquared();
-
-				// Ensure the search direction is numerically fit.
-				if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
+				float maxSqr = -Common.Settings.FLT_MAX;
+				for (int i = 0; i < pointCount; ++i)
 				{
-					// 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;
+					maxSqr = Common.Math.Max(maxSqr, Vec2.Dot(points[i], points[i]));
 				}
 
-				// Compute a tentative new simplex vertex using support points.
-				SimplexVertex* vertex = vertices + simplex._count;
-				vertex->indexA = shapeA.GetSupport(Common.Math.MulT(transformA.R, p));
-				vertex->wA = Common.Math.Mul(transformA, shapeA.GetVertex(vertex->indexA));
-				//Vec2 wBLocal;
-				vertex->indexB = shapeB.GetSupport(Common.Math.MulT(transformB.R, -p));
-				vertex->wB = Common.Math.Mul(transformB, shapeB.GetVertex(vertex->indexB));
-				vertex->w = vertex->wB - vertex->wA;
-
-				// Iteration count is equated to the number of support point calls.
-				++iter;
-
-				// Check for convergence.
-				float lowerBound = Vec2.Dot(p, vertex->w);
-				float upperBound = distanceSqr;
-				const float k_relativeTolSqr = 0.01f * 0.01f;	// 1:100
-				if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
+#if TARGET_FLOAT32_IS_FIXED
+				if (pointCount == 3 || vSqr <= 5.0*Common.Settings.FLT_EPSILON * maxSqr)
+#else
+				if (vSqr <= 100.0f * Common.Settings.FLT_EPSILON * maxSqr)
+#endif
 				{
-					// Converged!
-					break;
-				}
+					Collision.GJKIterations = iter;
+					v = x2 - x1;
+					vSqr = Vec2.Dot(v, v);
 
-				// Check for duplicate support points.
-				bool duplicate = false;
-				for (i = 0; i < lastCount; ++i)
-				{
-					if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
-					{
-						duplicate = true;
-						break;
-					}
+					return Common.Math.Sqrt(vSqr);
 				}
+			}
 
-				// If we found a duplicate support point we must exit to avoid cycling.
-				if (duplicate)
-				{
-					break;
-				}
+			Collision.GJKIterations = maxIterations;
+			return Common.Math.Sqrt(vSqr);
+		}
 
-				// New vertex is ok and needed.
-				++simplex._count;
+		public static float DistanceCC(out Vec2 x1, out Vec2 x2,
+			CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
+		{
+			Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition());
+			Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition());
+
+			Vec2 d = p2 - p1;
+			float dSqr = Vec2.Dot(d, d);
+			float r1 = circle1.GetRadius() - Settings.ToiSlop;
+			float r2 = circle2.GetRadius() - Settings.ToiSlop;
+			float r = r1 + r2;
+			if (dSqr > r * r)
+			{
+				float dLen = d.Normalize();
+				float distance = dLen - r;
+				x1 = p1 + r1 * d;
+				x2 = p2 - r2 * d;
+				return distance;
+			}
+			else if (dSqr > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON)
+			{
+				d.Normalize();
+				x1 = p1 + r1 * d;
+				x2 = x1;
+				return 0.0f;
 			}
 
+			x1 = p1;
+			x2 = x1;
+			return 0.0f;
+		}
 
-			fixed (DistanceOutput* doPtr = &output)
+#warning "CAS"
+		// This is used for polygon-vs-circle distance.
+		public class Point : Collision.IGenericShape
+		{
+			public Vec2 p;
+
+			public Vec2 Support(XForm xf, Vec2 v)
 			{
-				// Prepare output.
-				simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
-				doPtr->Distance = Vec2.Distance(doPtr->PointA, doPtr->PointB);
-				doPtr->Iterations = iter;
+				return p;
 			}
 
-			fixed (SimplexCache* sPtr = &cache)
+			public Vec2 GetFirstVertex(XForm xf)
 			{
-				// Cache the simplex.
-				simplex.WriteCache(sPtr);
+				return p;
 			}
+		}
 
-			// Apply radii if requested.
-			if (input.UseRadii)
+		// GJK is more robust with polygon-vs-point than polygon-vs-circle.
+		// So we convert polygon-vs-circle to polygon-vs-point.
+		public static float DistancePC(out Vec2 x1, out Vec2 x2,
+			PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
+		{
+			Point point = new Point();
+			point.p = Common.Math.Mul(xf2, circle.GetLocalPosition());
+
+			float distance = DistanceGeneric<PolygonShape, Point>(out x1, out x2, polygon, xf1, point, XForm.Identity);
+
+			float r = circle.GetRadius() - Settings.ToiSlop;
+
+			if (distance > r)
 			{
-				float rA = shapeA._radius;
-				float rB = shapeB._radius;
+				distance -= r;
+				Vec2 d = x2 - x1;
+				d.Normalize();
+				x2 -= r * d;
+			}
+			else
+			{
+				distance = 0.0f;
+				x2 = x1;
+			}
 
-				if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON)
-				{
-					// Shapes are still no overlapped.
-					// Move the witness points to the outer surface.
-					output.Distance -= rA + rB;
-					Vec2 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.
-					Vec2 p = 0.5f * (output.PointA + output.PointB);
-					output.PointA = p;
-					output.PointB = p;
-					output.Distance = 0.0f;
-				}
+			return distance;
+		}
+
+		public static float Distance(out Vec2 x1, out Vec2 x2,
+			Shape shape1, XForm xf1, Shape shape2, XForm xf2)
+		{
+			x1 = new Vec2();
+			x2 = new Vec2();
+
+			ShapeType type1 = shape1.GetType();
+			ShapeType type2 = shape2.GetType();
+
+			if (type1 == ShapeType.CircleShape && type2 == ShapeType.CircleShape)
+			{
+				return DistanceCC(out x1, out x2, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2);
+			}
+
+			if (type1 == ShapeType.PolygonShape && type2 == ShapeType.CircleShape)
+			{
+				return DistancePC(out x1, out x2, (PolygonShape)shape1, xf1, (CircleShape)shape2, xf2);
+			}
+
+			if (type1 == ShapeType.CircleShape && type2 == ShapeType.PolygonShape)
+			{
+				return DistancePC(out x2, out x1, (PolygonShape)shape2, xf2, (CircleShape)shape1, xf1);
 			}
+
+			if (type1 == ShapeType.PolygonShape && type2 == ShapeType.PolygonShape)
+			{
+				return DistanceGeneric(out x1, out x2, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2);
+			}
+
+			return 0.0f;
 		}
 	}
 }
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs
index b59f19b..a28a74c 100644
--- a/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/Collision.TimeOfImpact.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,339 +19,106 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Common;
 
 namespace Box2DX.Collision
 {
-	/// <summary>
-	/// Inpute parameters for TimeOfImpact
-	/// </summary>
-	public struct TOIInput
-	{
-		public Sweep SweepA;
-		public Sweep SweepB;
-		public float SweepRadiusA;
-		public float SweepRadiusB;
-		public float Tolerance;
-	}
-
-	internal struct SeparationFunction
-	{
-		internal enum Type
-		{
-			Points,
-			FaceA,
-			FaceB
-		};
-
-		internal unsafe void Initialize(SimplexCache* cache,
-			Shape shapeA, XForm transformA,
-			Shape shapeB, XForm transformB)
-		{
-			ShapeA = shapeA;
-			ShapeB = shapeB;
-			int count = cache->Count;
-			Box2DXDebug.Assert(0 < count && count < 3);
-
-			if (count == 1)
-			{
-				FaceType = Type.Points;
-				Vec2 localPointA = ShapeA.GetVertex(cache->IndexA[0]);
-				Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
-				Vec2 pointA = Common.Math.Mul(transformA, localPointA);
-				Vec2 pointB = Common.Math.Mul(transformB, localPointB);
-				Axis = pointB - pointA;
-				Axis.Normalize();
-			}
-			else if (cache->IndexB[0] == cache->IndexB[1])
-			{
-				// Two points on A and one on B
-				FaceType = Type.FaceA;
-				Vec2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]);
-				Vec2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]);
-				Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
-				LocalPoint = 0.5f * (localPointA1 + localPointA2);
-				Axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
-				Axis.Normalize();
-
-				Vec2 normal = Common.Math.Mul(transformA.R, Axis);
-				Vec2 pointA = Common.Math.Mul(transformA, LocalPoint);
-				Vec2 pointB = Common.Math.Mul(transformB, localPointB);
-
-				float s = Vec2.Dot(pointB - pointA, normal);
-				if (s < 0.0f)
-				{
-					Axis = -Axis;
-				}
-			}
-			else
-			{
-				// Two points on B and one or two points on A.
-				// We ignore the second point on A.
-				FaceType = Type.FaceB;
-				Vec2 localPointA = shapeA.GetVertex(cache->IndexA[0]);
-				Vec2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]);
-				Vec2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]);
-				LocalPoint = 0.5f * (localPointB1 + localPointB2);
-				Axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
-				Axis.Normalize();
-
-				Vec2 normal = Common.Math.Mul(transformB.R, Axis);
-				Vec2 pointB = Common.Math.Mul(transformB, LocalPoint);
-				Vec2 pointA = Common.Math.Mul(transformA, localPointA);
-
-				float s = Vec2.Dot(pointA - pointB, normal);
-				if (s < 0.0f)
-				{
-					Axis = -Axis;
-				}
-			}
-		}
-
-		internal float Evaluate(XForm transformA, XForm transformB)
-		{
-			switch (FaceType)
-			{
-				case Type.Points:
-					{
-						Vec2 axisA = Common.Math.MulT(transformA.R, Axis);
-						Vec2 axisB = Common.Math.MulT(transformB.R, -Axis);
-						Vec2 localPointA = ShapeA.GetSupportVertex(axisA);
-						Vec2 localPointB = ShapeB.GetSupportVertex(axisB);
-						Vec2 pointA = Common.Math.Mul(transformA, localPointA);
-						Vec2 pointB = Common.Math.Mul(transformB, localPointB);
-						float separation = Vec2.Dot(pointB - pointA, Axis);
-						return separation;
-					}
-
-				case Type.FaceA:
-					{
-						Vec2 normal = Common.Math.Mul(transformA.R, Axis);
-						Vec2 pointA = Common.Math.Mul(transformA, LocalPoint);
-
-						Vec2 axisB = Common.Math.MulT(transformB.R, -normal);
-
-						Vec2 localPointB = ShapeB.GetSupportVertex(axisB);
-						Vec2 pointB = Common.Math.Mul(transformB, localPointB);
-
-						float separation = Vec2.Dot(pointB - pointA, normal);
-						return separation;
-					}
-
-				case Type.FaceB:
-					{
-						Vec2 normal = Common.Math.Mul(transformB.R, Axis);
-						Vec2 pointB = Common.Math.Mul(transformB, LocalPoint);
-
-						Vec2 axisA = Common.Math.MulT(transformA.R, -normal);
-
-						Vec2 localPointA = ShapeA.GetSupportVertex(axisA);
-						Vec2 pointA = Common.Math.Mul(transformA, localPointA);
-
-						float separation = Vec2.Dot(pointA - pointB, normal);
-						return separation;
-					}
-
-				default:
-					Box2DXDebug.Assert(false);
-					return 0.0f;
-			}
-		}
-
-		internal Shape ShapeA;
-		internal Shape ShapeB;
-		internal Type FaceType;
-		internal Vec2 LocalPoint;
-		internal Vec2 Axis;
-	}
-
 	public partial class Collision
-	{		
-		public static int MaxToiIters;
-		public static int MaxToiRootIters;
-
-		// CCD via the secant method.
+	{
+		// This algorithm uses conservative advancement to compute the time of
+		// impact (TOI) of two shapes.
+		// Refs: Bullet, Young Kim
 		/// <summary>
 		/// Compute the time when two shapes begin to touch or touch at a closer distance.
-		/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
-		/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
-		/// smaller than sum of the shape radii.
-		/// Warning the sweeps must have the same time interval.
+		/// warning the sweeps must have the same time interval.
 		/// </summary>
+		/// <param name="shape1"></param>
+		/// <param name="sweep1"></param>
+		/// <param name="shape2"></param>
+		/// <param name="sweep2"></param>
 		/// <returns>
 		/// The fraction between [0,1] in which the shapes first touch.
 		/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
 		/// </returns>
-		public static float TimeOfImpact(TOIInput input, Shape shapeA, Shape shapeB)
+#warning: "check params"
+		public static float TimeOfImpact(Shape shape1, Sweep sweep1, Shape shape2, Sweep sweep2)
 		{
-			Sweep sweepA = input.SweepA;
-			Sweep sweepB = input.SweepB;
+			float r1 = shape1.GetSweepRadius();
+			float r2 = shape2.GetSweepRadius();
 
-			Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
-			Box2DXDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON);
+			Box2DXDebug.Assert(sweep1.T0 == sweep2.T0);
+			Box2DXDebug.Assert(1.0f - sweep1.T0 > Common.Settings.FLT_EPSILON);
 
-			float radius = shapeA._radius + shapeB._radius;
-			float tolerance = input.Tolerance;
+			float t0 = sweep1.T0;
+			Vec2 v1 = sweep1.C - sweep1.C0;
+			Vec2 v2 = sweep2.C - sweep2.C0;
+			float omega1 = sweep1.A - sweep1.A0;
+			float omega2 = sweep2.A - sweep2.A0;
 
 			float alpha = 0.0f;
 
-			const int k_maxIterations = 1000;	// TODO_ERIN b2Settings
+			Vec2 p1, p2;
+			int k_maxIterations = 20;	// TODO_ERIN b2Settings
 			int iter = 0;
-			float target = 0.0f;
-
-			// Prepare input for distance query.
-			SimplexCache cache = new SimplexCache();
-			cache.Count = 0;
-			DistanceInput distanceInput;
-			distanceInput.UseRadii = false;
+			Vec2 normal = Vec2.Zero;
+			float distance = 0.0f;
+			float targetDistance = 0.0f;
 
 			for (; ; )
 			{
-				XForm xfA, xfB;
-				sweepA.GetTransform(out xfA, alpha);
-				sweepB.GetTransform(out xfB, alpha);
+				float t = (1.0f - alpha) * t0 + alpha;
+				XForm xf1, xf2;
+				sweep1.GetXForm(out xf1, t);
+				sweep2.GetXForm(out xf2, t);
 
 				// Get the distance between shapes.
-				distanceInput.TransformA = xfA;
-				distanceInput.TransformB = xfB;
-				DistanceOutput distanceOutput;
-				Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB);
-
-				if (distanceOutput.Distance <= 0.0f)
-				{
-					alpha = 1.0f;
-					break;
-				}
-
-				SeparationFunction fcn = new SeparationFunction();
-				unsafe
-				{
-					fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
-				}
-
-				float separation = fcn.Evaluate(xfA, xfB);
-				if (separation <= 0.0f)
-				{
-					alpha = 1.0f;
-					break;
-				}
+				distance = Collision.Distance(out p1, out p2, shape1, xf1, shape2, xf2);
 
 				if (iter == 0)
 				{
 					// Compute a reasonable target distance to give some breathing room
-					// for conservative advancement. We take advantage of the shape radii
-					// to create additional clearance.
-					if (separation > radius)
+					// for conservative advancement.
+					if (distance > 2.0f * Settings.ToiSlop)
 					{
-						target = Common.Math.Max(radius - tolerance, 0.75f * radius);
+						targetDistance = 1.5f * Settings.ToiSlop;
 					}
 					else
 					{
-						target = Common.Math.Max(separation - tolerance, 0.02f * radius);
+						targetDistance = Common.Math.Max(0.05f * Settings.ToiSlop, distance - 0.5f * Settings.ToiSlop);
 					}
 				}
 
-				if (separation - target < 0.5f * tolerance)
+				if (distance - targetDistance < 0.05f * Settings.ToiSlop || iter == k_maxIterations)
 				{
-					if (iter == 0)
-					{
-						alpha = 1.0f;
-						break;
-					}
-
 					break;
 				}
 
-#if _FALSE
-				// 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
+				normal = p2 - p1;
+				normal.Normalize();
 
-				// Compute 1D root of: f(x) - target = 0
-				float newAlpha = alpha;
+				// Compute upper bound on remaining movement.
+				float approachVelocityBound = Vec2.Dot(normal, v1 - v2) +
+					Common.Math.Abs(omega1) * r1 + Common.Math.Abs(omega2) * r2;
+				if (Common.Math.Abs(approachVelocityBound) < Common.Settings.FLT_EPSILON)
 				{
-					float x1 = alpha, x2 = 1.0f;
-
-					float f1 = separation;
-
-					sweepA.GetTransform(out xfA, x2);
-					sweepB.GetTransform(out xfB, x2);
-					float f2 = fcn.Evaluate(xfA, xfB);
-
-					// If intervals don't overlap at t2, then we are done.
-					if (f2 >= target)
-					{
-						alpha = 1.0f;
-						break;
-					}
-
-					// Determine when intervals intersect.
-					int rootIterCount = 0;
-					for (; ; )
-					{
-						// Use a mix of the secant rule and bisection.
-						float x;
-						if ((rootIterCount & 1) != 0)
-						{
-							// Secant rule to improve convergence.
-							x = x1 + (target - f1) * (x2 - x1) / (f2 - f1);
-						}
-						else
-						{
-							// Bisection to guarantee progress.
-							x = 0.5f * (x1 + x2);
-						}
-
-						sweepA.GetTransform(out xfA, x);
-						sweepB.GetTransform(out xfB, x);
-
-						float f = fcn.Evaluate(xfA, xfB);
-
-						if (Common.Math.Abs(f - target) < 0.025f * tolerance)
-						{
-							newAlpha = x;
-							break;
-						}
-
-						// Ensure we continue to bracket the root.
-						if (f > target)
-						{
-							x1 = x;
-							f1 = f;
-						}
-						else
-						{
-							x2 = x;
-							f2 = f;
-						}
-
-						++rootIterCount;
+					alpha = 1.0f;
+					break;
+				}
 
-						Box2DXDebug.Assert(rootIterCount < 50);
-					}
+				// Get the conservative time increment. Don't advance all the way.
+				float dAlpha = (distance - targetDistance) / approachVelocityBound;
+				//float dt = (distance - 0.5f * Settings.LinearSlop) / approachVelocityBound;
+				float newAlpha = alpha + dAlpha;
 
-					MaxToiRootIters = Common.Math.Max(MaxToiRootIters, rootIterCount);
+				// The shapes may be moving apart or a safe distance apart.
+				if (newAlpha < 0.0f || 1.0f < newAlpha)
+				{
+					alpha = 1.0f;
+					break;
 				}
 
 				// Ensure significant advancement.
@@ -363,15 +130,8 @@ namespace Box2DX.Collision
 				alpha = newAlpha;
 
 				++iter;
-
-				if (iter == k_maxIterations)
-				{
-					break;
-				}
 			}
 
-			MaxToiIters = Common.Math.Max(MaxToiIters, iter);
-
 			return alpha;
 		}
 	}
diff --git a/Box2d/Assets/Program/Box2d/Collision/Collision.cs b/Box2d/Assets/Program/Box2d/Collision/Collision.cs
index e77e23b..6a88ab2 100644
--- a/Box2d/Assets/Program/Box2d/Collision/Collision.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/Collision.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -20,6 +20,9 @@
 */
 
 using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Common;
 
 namespace Box2DX.Collision
@@ -45,90 +48,6 @@ namespace Box2DX.Collision
 
 			return true;
 		}
-
-		/// <summary>
-		/// 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.
-		/// </summary>
-		public static void GetPointStates(PointState[/*b2_maxManifoldPoints*/] state1, PointState[/*b2_maxManifoldPoints*/] state2,
-					  Manifold manifold1, Manifold manifold2)
-		{
-			for (int i = 0; i < Common.Settings.MaxManifoldPoints; ++i)
-			{
-				state1[i] = PointState.NullState;
-				state2[i] = PointState.NullState;
-			}
-
-			// Detect persists and removes.
-			for (int i = 0; i < manifold1.PointCount; ++i)
-			{
-				ContactID id = manifold1.Points[i].ID;
-
-				state1[i] = PointState.RemoveState;
-
-				for (int j = 0; j < manifold2.PointCount; ++j)
-				{
-					if (manifold2.Points[j].ID.Key == id.Key)
-					{
-						state1[i] = PointState.PersistState;
-						break;
-					}
-				}
-			}
-
-			// Detect persists and adds.
-			for (int i = 0; i < manifold2.PointCount; ++i)
-			{
-				ContactID id = manifold2.Points[i].ID;
-
-				state2[i] = PointState.AddState;
-
-				for (int j = 0; j < manifold1.PointCount; ++j)
-				{
-					if (manifold1.Points[j].ID.Key == id.Key)
-					{
-						state2[i] = PointState.PersistState;
-						break;
-					}
-				}
-			}
-		}
-
-		// Sutherland-Hodgman clipping.
-		public static int ClipSegmentToLine(out ClipVertex[/*2*/] vOut, ClipVertex[/*2*/] vIn, Vec2 normal, float offset)
-		{
-			vOut = new ClipVertex[2];
-
-			// Start with no output points
-			int numOut = 0;
-
-			// Calculate the distance of end points to the line
-			float distance0 = Vec2.Dot(normal, vIn[0].V) - offset;
-			float distance1 = Vec2.Dot(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
-				float interp = distance0 / (distance0 - distance1);
-				vOut[numOut].V = vIn[0].V + interp * (vIn[1].V - vIn[0].V);
-				if (distance0 > 0.0f)
-				{
-					vOut[numOut].ID = vIn[0].ID;
-				}
-				else
-				{
-					vOut[numOut].ID = vIn[1].ID;
-				}
-				++numOut;
-			}
-
-			return numOut;
-		}
 	}
 
 	/// <summary>
@@ -139,22 +58,22 @@ namespace Box2DX.Collision
 		/// <summary>
 		/// The edge that defines the outward contact normal.
 		/// </summary>
-		public Byte ReferenceEdge;
+		public byte ReferenceEdge;
 
 		/// <summary>
 		/// The edge most anti-parallel to the reference edge.
 		/// </summary>
-		public Byte IncidentEdge;
+		public byte IncidentEdge;
 
 		/// <summary>
 		/// The vertex (0 or 1) on the incident edge that was clipped.
 		/// </summary>
-		public Byte IncidentVertex;
+		public byte IncidentVertex;
 
 		/// <summary>
 		/// A value of 1 indicates that the reference edge is on shape2.
 		/// </summary>
-		public Byte Flip;
+		public byte Flip;
 	}
 
 	/// <summary>
@@ -170,27 +89,33 @@ namespace Box2DX.Collision
 		/// Used to quickly compare contact ids.
 		/// </summary>
 		[System.Runtime.InteropServices.FieldOffset(0)]
-		public UInt32 Key;
+		public uint Key;
 	}
 
+#warning "CAS"
 	/// <summary>
 	/// 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:
-	/// -Circles: the local center of circleB
-	/// -FaceA: the local center of cirlceB or the clip point of polygonB
-	/// -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.
+	/// The point is stored in local coordinates because CCD
+	/// requires sub-stepping in which the separation is stale.
 	/// </summary>
 	public class ManifoldPoint
 	{
 		/// <summary>
-		/// Usage depends on manifold type.
+		/// Local position of the contact point in body1.
 		/// </summary>
-		public Vec2 LocalPoint;
+		public Vec2 LocalPoint1;
+
+		/// <summary>
+		/// Local position of the contact point in body2.
+		/// </summary>
+		public Vec2 LocalPoint2;
+
+		/// <summary>
+		/// The separation of the shapes along the normal vector.
+		/// </summary>
+		public float Separation;
 
 		/// <summary>
 		/// The non-penetration impulse.
@@ -210,7 +135,9 @@ namespace Box2DX.Collision
 		public ManifoldPoint Clone()
 		{
 			ManifoldPoint newPoint = new ManifoldPoint();
-			newPoint.LocalPoint = this.LocalPoint;
+			newPoint.LocalPoint1 = this.LocalPoint1;
+			newPoint.LocalPoint2 = this.LocalPoint2;
+			newPoint.Separation = this.Separation;
 			newPoint.NormalImpulse = this.NormalImpulse;
 			newPoint.TangentImpulse = this.TangentImpulse;
 			newPoint.ID = this.ID;
@@ -218,13 +145,7 @@ namespace Box2DX.Collision
 		}
 	}
 
-	public enum ManifoldType
-	{
-		Circles,
-		FaceA,
-		FaceB
-	}
-
+#warning "CAS"
 	/// <summary>
 	/// A manifold for two touching convex shapes.
 	/// </summary>
@@ -235,14 +156,10 @@ namespace Box2DX.Collision
 		/// </summary>
 		public ManifoldPoint[/*Settings.MaxManifoldPoints*/] Points = new ManifoldPoint[Settings.MaxManifoldPoints];
 
-		public Vec2 LocalPlaneNormal;
-
 		/// <summary>
-		/// Usage depends on manifold type.
+		/// The shared unit normal vector.
 		/// </summary>
-		public Vec2 LocalPoint;
-
-		public ManifoldType Type;
+		public Vec2 Normal;
 
 		/// <summary>
 		/// The number of manifold points.
@@ -258,9 +175,7 @@ namespace Box2DX.Collision
 		public Manifold Clone()
 		{
 			Manifold newManifold = new Manifold();
-			newManifold.LocalPlaneNormal = this.LocalPlaneNormal;
-			newManifold.LocalPoint = this.LocalPoint;
-			newManifold.Type = this.Type;
+			newManifold.Normal = this.Normal;
 			newManifold.PointCount = this.PointCount;
 			int pointCount = this.Points.Length;
 			ManifoldPoint[] tmp = new ManifoldPoint[pointCount];
@@ -296,6 +211,11 @@ namespace Box2DX.Collision
 		/// <summary>
 		/// Ray cast against this segment with another segment.        
 		/// </summary>
+		/// <param name="lambda"></param>
+		/// <param name="normal"></param>
+		/// <param name="segment"></param>
+		/// <param name="maxLambda"></param>
+		/// <returns></returns>
 		public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
 		{
 			lambda = 0f;
@@ -351,16 +271,6 @@ namespace Box2DX.Collision
 	/// </summary>
 	public struct AABB
 	{
-		/// <summary>
-		/// The lower vertex.
-		/// </summary>
-		public Vec2 LowerBound;
-
-		/// <summary>
-		/// The upper vertex.
-		/// </summary>
-		public Vec2 UpperBound;
-
 		/// Verify that the bounds are sorted.
 		public bool IsValid
 		{
@@ -373,251 +283,36 @@ namespace Box2DX.Collision
 			}
 		}
 
-		/// Get the center of the AABB.
-		public Vec2 Center
-		{
-			get { return 0.5f * (LowerBound + UpperBound); }
-		}
-
-		/// Get the extents of the AABB (half-widths).
-		public Vec2 Extents
-		{
-			get { return 0.5f * (UpperBound - LowerBound); }
-		}
-
-		/// Combine two AABBs into this one.
-		public void Combine(AABB aabb1, AABB aabb2)
-		{
-			LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound);
-			UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound);
-		}
-
-		/// Does this aabb contain the provided AABB.
-		public bool Contains(AABB aabb)
-		{
-			bool 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;
-		}
 
 		/// <summary>
-		// From Real-time Collision Detection, p179.
+		/// The lower vertex.
 		/// </summary>
-		public void RayCast(out RayCastOutput output, RayCastInput input)
-		{
-			float tmin = -Common.Settings.FLT_MAX;
-			float tmax = Common.Settings.FLT_MAX;
-
-			output = new RayCastOutput();
-
-			output.Hit = false;
-
-			Vec2 p = input.P1;
-			Vec2 d = input.P2 - input.P1;
-			Vec2 absD = Common.Math.Abs(d);
-
-			Vec2 normal = new Vec2(0);
-
-			for (int i = 0; i < 2; ++i)
-			{
-				if (absD[i] < Common.Settings.FLT_EPSILON)
-				{
-					// Parallel.
-					if (p[i] < LowerBound[i] || UpperBound[i] < p[i])
-					{
-						return;
-					}
-				}
-				else
-				{
-					float inv_d = 1.0f / d[i];
-					float t1 = (LowerBound[i] - p[i]) * inv_d;
-					float t2 = (UpperBound[i] - p[i]) * inv_d;
-
-					// Sign of the normal vector.
-					float s = -1.0f;
-
-					if (t1 > t2)
-					{
-						Common.Math.Swap(ref t1, ref t2);
-						s = 1.0f;
-					}
-
-					// Push the min up
-					if (t1 > tmin)
-					{
-						normal.SetZero();
-						normal[i] = s;
-						tmin = t1;
-					}
-
-					// Pull the max down
-					tmax = Common.Math.Min(tmax, t2);
-
-					if (tmin > tmax)
-					{
-						return;
-					}
-				}
-			}
-
-			// Does the ray start inside the box?
-			// Does the ray intersect beyond the max fraction?
-			if (tmin < 0.0f || input.MaxFraction < tmin)
-			{
-				return;
-			}
-
-			// Intersection.
-			output.Fraction = tmin;
-			output.Normal = normal;
-			output.Hit = true;
-		}
-	}
+		public Vec2 LowerBound;
 
-	/// <summary>
-	/// This is used for determining the state of contact points.
-	/// </summary>
-	public enum PointState
-	{
 		/// <summary>
-		/// Point does not exist.
-		/// </summary>
-		NullState,
-		/// <summary>
-		/// Point was added in the update.
-		/// </summary>
-		AddState,
-		/// <summary>
-		/// Point persisted across the update.
-		/// </summary>
-		PersistState,
-		/// <summary>
-		///Point was removed in the update.
+		/// The upper vertex.
 		/// </summary>
-		RemoveState
+		public Vec2 UpperBound;
 	}
 
 	/// <summary>
-	/// This is used to compute the current state of a contact manifold.
+	/// An oriented bounding box.
 	/// </summary>
-	public class WorldManifold
+	public struct OBB
 	{
 		/// <summary>
-		/// World vector pointing from A to B.
+		/// The rotation matrix.
 		/// </summary>
-		public Vec2 Normal;
+		public Mat22 R;
+
 		/// <summary>
-		/// World contact point (point of intersection).
+		/// The local centroid.
 		/// </summary>
-		public Vec2[] Points = new Vec2[Common.Settings.MaxManifoldPoints];
+		public Vec2 Center;
 
-		public WorldManifold Clone()
-		{
-			WorldManifold newManifold = new WorldManifold();
-			newManifold.Normal = this.Normal;
-			this.Points.CopyTo(newManifold.Points, 0);
-			return newManifold;
-		}
-
-		/// 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.
-		public void Initialize(Manifold manifold, XForm xfA, float radiusA, XForm xfB, float radiusB)
-		{
-			if (manifold.PointCount == 0)
-			{
-				return;
-			}
-
-			switch (manifold.Type)
-			{
-				case ManifoldType.Circles:
-					{
-						Vec2 pointA = Common.Math.Mul(xfA, manifold.LocalPoint);
-						Vec2 pointB = Common.Math.Mul(xfB, manifold.Points[0].LocalPoint);
-						Vec2 normal = new Vec2(1.0f, 0.0f);
-						if (Vec2.DistanceSquared(pointA, pointB) > Common.Settings.FLT_EPSILON_SQUARED)
-						{
-							normal = pointB - pointA;
-							normal.Normalize();
-						}
-
-						Normal = normal;
-
-						Vec2 cA = pointA + radiusA * normal;
-						Vec2 cB = pointB - radiusB * normal;
-						Points[0] = 0.5f * (cA + cB);
-					}
-					break;
-
-				case ManifoldType.FaceA:
-					{
-						Vec2 normal = Common.Math.Mul(xfA.R, manifold.LocalPlaneNormal);
-						Vec2 planePoint = Common.Math.Mul(xfA, manifold.LocalPoint);
-
-						// Ensure normal points from A to B.
-						Normal = normal;
-
-						for (int i = 0; i < manifold.PointCount; ++i)
-						{
-							Vec2 clipPoint = Common.Math.Mul(xfB, manifold.Points[i].LocalPoint);
-							Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
-							Vec2 cB = clipPoint - radiusB * normal;
-							Points[i] = 0.5f * (cA + cB);
-						}
-					}
-					break;
-
-				case ManifoldType.FaceB:
-					{
-						Vec2 normal = Common.Math.Mul(xfB.R, manifold.LocalPlaneNormal);
-						Vec2 planePoint = Common.Math.Mul(xfB, manifold.LocalPoint);
-
-						// Ensure normal points from A to B.
-						Normal = -normal;
-
-						for (int i = 0; i < manifold.PointCount; ++i)
-						{
-							Vec2 clipPoint = Common.Math.Mul(xfA, manifold.Points[i].LocalPoint);
-							Vec2 cA = clipPoint - radiusA * normal;
-							Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
-							Points[i] = 0.5f * (cA + cB);
-						}
-					}
-					break;
-			}
-		}
-	}
-
-	/// <summary>
-	/// Used for computing contact manifolds.
-	/// </summary>
-	public struct ClipVertex
-	{
-		public Vec2 V;
-		public ContactID ID;
-	}
-
-	/// <summary>
-	/// Ray-cast input data.
-	/// </summary>
-	public struct RayCastInput
-	{
-		public Vec2 P1, P2;
-		public float MaxFraction;
-	}
-
-	/// <summary>
-	/// Ray-cast output data.
-	/// </summary>
-	public struct RayCastOutput
-	{
-		public Vec2 Normal;
-		public float Fraction;
-		public bool Hit;
+		/// <summary>
+		/// The half-widths.
+		/// </summary>
+		public Vec2 Extents;
 	}
 }
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Collision/PairManager.cs b/Box2d/Assets/Program/Box2d/Collision/PairManager.cs
index 0fcb8ab..361fee5 100644
--- a/Box2d/Assets/Program/Box2d/Collision/PairManager.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/PairManager.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -26,10 +26,15 @@
 #define DEBUG
 
 using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Common;
 
 namespace Box2DX.Collision
 {
+#warning "CAS"
 	public class Pair
 	{
 		[Flags]
@@ -104,12 +109,12 @@ namespace Box2DX.Collision
 			_freePair = 0;
 			for (int i = 0; i < Settings.MaxPairs; ++i)
 			{
-				_pairs[i] = new Pair();//todo: need some pool here
+				_pairs[i] = new Pair();
 				_pairs[i].ProxyId1 = PairManager.NullProxy;
 				_pairs[i].ProxyId2 = PairManager.NullProxy;
 				_pairs[i].UserData = null;
 				_pairs[i].Status = 0;
-				_pairs[i].Next = (ushort)(i + 1U);
+				_pairs[i].Next = (ushort)(i + (ushort)1);
 			}
 			_pairs[Settings.MaxPairs - 1].Next = PairManager.NullPair;
 			_pairCount = 0;
@@ -475,15 +480,15 @@ namespace Box2DX.Collision
 
 		public static int BufferedPairSortPredicate(BufferedPair pair1, BufferedPair pair2)
 		{
-			if (pair1.ProxyId1 < pair2.ProxyId1)
+			if (pair1.ProxyId1 > pair2.ProxyId1)
 				return 1;
-			else if (pair1.ProxyId1 > pair2.ProxyId1)
+			else if (pair1.ProxyId1 < pair2.ProxyId1)
 				return -1;
 			else
 			{
-				if (pair1.ProxyId2 < pair2.ProxyId2)
+				if (pair1.ProxyId2 > pair2.ProxyId2)
 					return 1;
-				else if (pair1.ProxyId2 > pair2.ProxyId2)
+				else if (pair1.ProxyId2 < pair2.ProxyId2)
 					return -1;
 			}
 
diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs
index 553accd..5e1fa30 100644
--- a/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/CircleShape.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,26 +19,62 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Common;
 
 namespace Box2DX.Collision
 {
 	/// <summary>
+	/// This structure is used to build circle shapes.
+	/// </summary>
+	public class CircleDef : ShapeDef
+	{
+		public Vec2 LocalPosition;
+		public float Radius;
+
+		public CircleDef()
+		{
+			Type = ShapeType.CircleShape;
+			LocalPosition = Vec2.Zero;
+			Radius = 1.0f;
+		}
+	}
+
+	/// <summary>
 	/// A circle shape.
 	/// </summary>
 	public class CircleShape : Shape
 	{
-		// Position
-		internal Vec2 _position;
+		// Local position in parent body
+		private Vec2 _localPosition;
+		// Radius of this circle.
+		private float _radius;
 
-		public CircleShape()			
+		internal CircleShape(ShapeDef def)
+			: base(def)
 		{
+			Box2DXDebug.Assert(def.Type == ShapeType.CircleShape);
+			CircleDef circleDef = (CircleDef)def;
+
 			_type = ShapeType.CircleShape;
+			_localPosition = circleDef.LocalPosition;
+			_radius = circleDef.Radius;
+		}
+
+		internal override void UpdateSweepRadius(Vec2 center)
+		{
+			// Update the sweep radius (maximum radius) as measured from
+			// a local center point.
+			Vec2 d = _localPosition - center;
+			_sweepRadius = d.Length() + _radius - Settings.ToiSlop;
 		}
 
 		public override bool TestPoint(XForm transform, Vec2 p)
 		{
-			Vec2 center = transform.Position + Common.Math.Mul(transform.R, _position);
+			Vec2 center = transform.Position + Common.Math.Mul(transform.R, _localPosition);
 			Vec2 d = p - center;
 			return Vec2.Dot(d, d) <= _radius * _radius;
 		}
@@ -52,7 +88,7 @@ namespace Box2DX.Collision
 			lambda = 0f;
 			normal = Vec2.Zero;
 
-			Vec2 position = transform.Position + Common.Math.Mul(transform.R, _position);
+			Vec2 position = transform.Position + Common.Math.Mul(transform.R, _localPosition);
 			Vec2 s = segment.P1 - position;
 			float b = Vec2.Dot(s, s) - _radius * _radius;
 
@@ -95,85 +131,51 @@ namespace Box2DX.Collision
 		{
 			aabb = new AABB();
 
-			Vec2 p = transform.Position + Common.Math.Mul(transform.R, _position);
+			Vec2 p = transform.Position + Common.Math.Mul(transform.R, _localPosition);
 			aabb.LowerBound.Set(p.X - _radius, p.Y - _radius);
 			aabb.UpperBound.Set(p.X + _radius, p.Y + _radius);
 		}
 
-		public override void ComputeMass(out MassData massData, float density)
+		public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2)
 		{
-			massData = new MassData();
+			aabb = new AABB();
 
-			massData.Mass = density * Settings.Pi * _radius * _radius;
-			massData.Center = _position;
+			Vec2 p1 = transform1.Position + Common.Math.Mul(transform1.R, _localPosition);
+			Vec2 p2 = transform2.Position + Common.Math.Mul(transform2.R, _localPosition);
+			Vec2 lower = Common.Math.Min(p1, p2);
+			Vec2 upper = Common.Math.Max(p1, p2);
 
-			// inertia about the local origin
-			massData.I = massData.Mass * (0.5f * _radius * _radius + Vec2.Dot(_position, _position));
-		}		
+			aabb.LowerBound.Set(lower.X - _radius, lower.Y - _radius);
+			aabb.UpperBound.Set(upper.X + _radius, upper.Y + _radius);
+		}
 
-		public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c)
+		public override void ComputeMass(out MassData massData)
 		{
-			Vec2 p = Box2DX.Common.Math.Mul(xf, _position);
-			float l = -(Vec2.Dot(normal, p) - offset);
-			if (l < -_radius + Box2DX.Common.Settings.FLT_EPSILON)
-			{
-				//Completely dry
-				c = new Vec2();
-				return 0;
-			}
-			if (l > _radius)
-			{
-				//Completely wet
-				c = p;
-				return Box2DX.Common.Settings.Pi * _radius * _radius;
-			}
-
-			//Magic
-			float r2 = _radius * _radius;
-			float l2 = l * l;
-			float area = r2 * ((float)System.Math.Asin(l / _radius) + Box2DX.Common.Settings.Pi / 2) +
-				l * Box2DX.Common.Math.Sqrt(r2 - l2);
-			float com = -2.0f / 3.0f * (float)System.Math.Pow(r2 - l2, 1.5f) / area;
-
-			c.X = p.X + normal.X * com;
-			c.Y = p.Y + normal.Y * com;
+			massData = new MassData();
 
-			return area;
-		}
+			massData.Mass = _density * Settings.Pi * _radius * _radius;
+			massData.Center = _localPosition;
 
-		/// <summary>
-		/// Get the supporting vertex index in the given direction.
-		/// </summary>
-		public override int GetSupport(Vec2 d)
-		{
-			return 0;
+			// inertia about the local origin
+			massData.I = massData.Mass * (0.5f * _radius * _radius + Vec2.Dot(_localPosition, _localPosition));
 		}
 
 		/// <summary>
-		/// Get the supporting vertex in the given direction.
+		/// Get the local position of this circle in its parent body.
 		/// </summary>
-		public override Vec2 GetSupportVertex(Vec2 d)
+		/// <returns></returns>
+		public Vec2 GetLocalPosition()
 		{
-			return _position;
+			return _localPosition;
 		}
 
 		/// <summary>
-		/// Get a vertex by index. Used by Distance.
+		/// Get the radius of this circle.
 		/// </summary>
-		public override Vec2 GetVertex(int index)
-		{
-			Box2DXDebug.Assert(index == 0);
-			return _position;
-		}
-
-		public override float ComputeSweepRadius(Vec2 pivot)
+		/// <returns></returns>
+		public float GetRadius()
 		{
-			return Vec2.Distance(_position, pivot);
+			return _radius;
 		}
-
-		/// <summary>
-		/// Get the vertex count.
-		/// </summary>
-		public int VertexCount { get { return 1; } }
 	}
 }
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs
deleted file mode 100644
index 348a293..0000000
--- a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs
+++ /dev/null
@@ -1,277 +0,0 @@
-/*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
-
-  This software is provided 'as-is', without any express or implied
-  warranty.  In no event will the authors be held liable for any damages
-  arising from the use of this software.
-
-  Permission is granted to anyone to use this software for any purpose,
-  including commercial applications, and to alter it and redistribute it
-  freely, subject to the following restrictions:
-
-  1. The origin of this software must not be misrepresented; you must not
-     claim that you wrote the original software. If you use this software
-     in a product, an acknowledgment in the product documentation would be
-     appreciated but is not required.
-  2. Altered source versions must be plainly marked as such, and must not be
-     misrepresented as being the original software.
-  3. This notice may not be removed or altered from any source distribution.
-*/
-
-using System;
-using System.Collections.Generic;
-using System.Text;
-
-using Box2DX.Common;
-
-namespace Box2DX.Collision
-{
-	public class EdgeShape : Shape
-	{
-		public Vec2 _v1;
-		public Vec2 _v2;
-
-		public float _length;
-
-		public Vec2 _normal;
-
-		public Vec2 _direction;
-
-		// Unit vector halfway between m_direction and m_prevEdge.m_direction:
-		public Vec2 _cornerDir1;
-
-		// Unit vector halfway between m_direction and m_nextEdge.m_direction:
-		public Vec2 _cornerDir2;
-
-		public bool _cornerConvex1;
-		public bool _cornerConvex2;
-
-		public EdgeShape _nextEdge;
-		public EdgeShape _prevEdge;
-
-		public EdgeShape()
-		{
-			_type = ShapeType.EdgeShape;
-			_radius = Settings.PolygonRadius;
-		}
-
-		public override void Dispose()
-		{
-			if (_prevEdge != null)
-			{
-				_prevEdge._nextEdge = null;
-			}
-
-			if (_nextEdge != null)
-			{
-				_nextEdge._prevEdge = null;
-			}
-		}
-
-		public void Set(Vec2 v1, Vec2 v2)
-		{
-			_v1 = v1;
-			_v2 = v2;
-
-			_direction = _v2 - _v1;
-			_length = _direction.Normalize();
-			_normal = Vec2.Cross(_direction, 1.0f);
-
-			_cornerDir1 = _normal;
-			_cornerDir2 = -1.0f * _normal;
-		}
-
-		public override bool TestPoint(XForm transform, Vec2 p)
-		{
-			return false;
-		}
-
-		public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
-		{
-			Vec2 r = segment.P2 - segment.P1;
-			Vec2 v1 = Common.Math.Mul(transform, _v1);
-			Vec2 d = Common.Math.Mul(transform, _v2) - v1;
-			Vec2 n = Vec2.Cross(d, 1.0f);
-
-			float k_slop = 100.0f * Common.Settings.FLT_EPSILON;
-			float denom = -Vec2.Dot(r, n);
-
-			// Cull back facing collision and ignore parallel segments.
-			if (denom > k_slop)
-			{
-				// Does the segment intersect the infinite line associated with this segment?
-				Vec2 b = segment.P1 - v1;
-				float a = Vec2.Dot(b, n);
-
-				if (0.0f <= a && a <= maxLambda * denom)
-				{
-					float mu2 = -r.X * b.Y + r.Y * b.X;
-
-					// Does the segment intersect this segment?
-					if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
-					{
-						a /= denom;
-						n.Normalize();
-						lambda = a;
-						normal = n;
-						return SegmentCollide.HitCollide;
-					}
-				}
-			}
-
-			lambda = 0;
-			normal = new Vec2();
-			return SegmentCollide.MissCollide;
-		}
-
-		public override void ComputeAABB(out AABB aabb, XForm transform)
-		{
-			Vec2 v1 = Common.Math.Mul(transform, _v1);
-			Vec2 v2 = Common.Math.Mul(transform, _v2);
-
-			Vec2 r = new Vec2(_radius, _radius);
-			aabb.LowerBound = Common.Math.Min(v1, v2) - r;
-			aabb.UpperBound = Common.Math.Max(v1, v2) + r;
-		}
-
-		public override void ComputeMass(out MassData massData, float density)
-		{
-			massData.Mass = 0.0f;
-			massData.Center = _v1;
-			massData.I = 0.0f;
-		}
-
-		public void SetPrevEdge(EdgeShape edge, Vec2 cornerDir, bool convex)
-		{
-			_prevEdge = edge;
-			_cornerDir1 = cornerDir;
-			_cornerConvex1 = convex;
-		}
-
-		public void SetNextEdge(EdgeShape edge, Vec2 cornerDir, bool convex)
-		{
-			_nextEdge = edge;
-			_cornerDir2 = cornerDir;
-			_cornerConvex2 = convex;
-		}
-
-		public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c)
-		{
-			//Note that v0 is independent of any details of the specific edge
-			//We are relying on v0 being consistent between multiple edges of the same body
-			Vec2 v0 = offset * normal;
-			//b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;
-
-			Vec2 v1 = Common.Math.Mul(xf, _v1);
-			Vec2 v2 = Common.Math.Mul(xf, _v2);
-
-			float d1 = Vec2.Dot(normal, v1) - offset;
-			float d2 = Vec2.Dot(normal, v2) - offset;
-
-			if (d1 > 0.0f)
-			{
-				if (d2 > 0.0f)
-				{
-					c = new Vec2();
-					return 0.0f;
-				}
-				else
-				{
-					v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
-				}
-			}
-			else
-			{
-				if (d2 > 0.0f)
-				{
-					v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
-				}
-				else
-				{
-					//Nothing
-				}
-			}
-
-			// v0,v1,v2 represents a fully submerged triangle
-			float k_inv3 = 1.0f / 3.0f;
-
-			// Area weighted centroid
-			c = k_inv3 * (v0 + v1 + v2);
-
-			Vec2 e1 = v1 - v0;
-			Vec2 e2 = v2 - v0;
-
-			return 0.5f * Vec2.Cross(e1, e2);
-		}
-
-		public float Length
-		{
-			get { return _length; }
-		}
-
-		public Vec2 Vertex1
-		{
-			get { return _v1; }
-		}
-
-		public Vec2 Vertex2
-		{
-			get { return _v2; }
-		}
-
-		public Vec2 NormalVector
-		{
-			get { return _normal; }
-		}
-
-		public Vec2 DirectionVector
-		{
-			get { return _direction; }
-		}
-
-		public Vec2 Corner1Vector
-		{
-			get { return _cornerDir1; }
-		}
-
-		public Vec2 Corner2Vector
-		{
-			get { return _cornerDir2; }
-		}
-
-		public override int GetSupport(Vec2 d)
-		{
-			return Vec2.Dot(_v1, d) > Vec2.Dot(_v2, d) ? 0 : 1;
-		}
-
-		public override Vec2 GetSupportVertex(Vec2 d)
-		{
-			return Vec2.Dot(_v1, d) > Vec2.Dot(_v2, d) ? _v1 : _v2;
-		}
-
-		public override Vec2 GetVertex(int index)
-		{
-			Box2DXDebug.Assert(0 <= index && index < 2);
-			if (index == 0) return _v1;
-			else return _v2;
-		}
-
-		public bool Corner1IsConvex
-		{
-			get { return _cornerConvex1; }
-		}
-
-		public bool Corner2IsConvex
-		{
-			get { return _cornerConvex2; }
-		}
-
-		public override float ComputeSweepRadius(Vec2 pivot)
-		{
-			float ds1 = Vec2.DistanceSquared(_v1, pivot);
-			float ds2 = Vec2.DistanceSquared(_v2, pivot);
-			return Common.Math.Sqrt(Common.Math.Max(ds1, ds2));
-		}
-	}
-}
diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta b/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta
deleted file mode 100644
index 8051fd4..0000000
--- a/Box2d/Assets/Program/Box2d/Collision/Shapes/EdgeShape.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: 1e86753e9f98b2d429a099076e61bf32
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs
index 6a655f7..e68294e 100644
--- a/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/PolygonShape.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -30,144 +30,263 @@ using Box2DX.Common;
 namespace Box2DX.Collision
 {
 	/// <summary>
-	/// A convex polygon. It is assumed that the interior of the polygon is to the left of each edge.
+	/// Convex polygon. The vertices must be in CCW order for a right-handed
+	/// coordinate system with the z-axis coming out of the screen.
 	/// </summary>
-	public class PolygonShape : Shape
+	public class PolygonDef : ShapeDef
 	{
-		internal Vec2 _centroid;
-		internal Vec2[] _vertices = new Vec2[Settings.MaxPolygonVertices];
-		internal Vec2[] _normals = new Vec2[Settings.MaxPolygonVertices];
+		/// <summary>
+		/// The number of polygon vertices.
+		/// </summary>
+		public int VertexCount;
 
-		internal int _vertexCount;
+		/// <summary>
+		/// The polygon vertices in local coordinates.
+		/// </summary>
+		public Vec2[] Vertices = new Vec2[Settings.MaxPolygonVertices];
 
-		public int VertexCount
+		public PolygonDef()
+		{
+			Type = ShapeType.PolygonShape;
+			VertexCount = 0;
+		}
+
+		/// <summary>
+		/// Build vertices to represent an axis-aligned box.
+		/// </summary>
+		/// <param name="hx">The half-width</param>
+		/// <param name="hy">The half-height.</param>
+		public void SetAsBox(float hx, float hy)
 		{
-			get { return _vertexCount; }
+			VertexCount = 4;
+			Vertices[0].Set(-hx, -hy);
+			Vertices[1].Set(hx, -hy);
+			Vertices[2].Set(hx, hy);
+			Vertices[3].Set(-hx, hy);
 		}
 
-		public Vec2[] Vertices
+
+		/// <summary>
+		/// Build vertices to represent an oriented box.
+		/// </summary>
+		/// <param name="hx">The half-width</param>
+		/// <param name="hy">The half-height.</param>
+		/// <param name="center">The center of the box in local coordinates.</param>
+		/// <param name="angle">The rotation of the box in local coordinates.</param>
+		public void SetAsBox(float hx, float hy, Vec2 center, float angle)
+		{
+			SetAsBox(hx, hy);
+			XForm xf = new XForm();
+			xf.Position = center;
+			xf.R.Set(angle);
+
+			for (int i = 0; i < VertexCount; ++i)
+			{
+				Vertices[i] = Common.Math.Mul(xf, Vertices[i]);
+			}
+		}
+	}
+
+	/// <summary>
+	/// A convex polygon.
+	/// </summary>
+	public class PolygonShape : Shape, Collision.IGenericShape
+	{
+		// Local position of the polygon centroid.
+		private Vec2 _centroid;
+		/// <summary>
+		/// Get local centroid relative to the parent body.
+		/// </summary>
+		/// <returns></returns>
+		//public Vector2 Centroid { get { return _centroid; } }
+		public Vec2 GetCentroid() { return _centroid; }
+
+		private OBB _obb;
+		/// <summary>
+		/// Get the oriented bounding box relative to the parent body.
+		/// </summary>
+		//public OBB OBB { get { return _obb; } }
+		public OBB GetOBB() { return _obb; }
+
+		private int _vertexCount;
+		/// <summary>
+		/// Get the vertex count.
+		/// </summary>
+		public int VertexCount { get { return _vertexCount; } }
+
+		private Vec2[] _vertices = new Vec2[Settings.MaxPolygonVertices];
+		/// <summary>
+		/// Get the vertices in local coordinates.
+		/// </summary>
+		//public Vector2[] Vertices { get { return _vertices; } }
+		public Vec2[] GetVertices() { return _vertices; }
+
+		private Vec2[] _coreVertices = new Vec2[Settings.MaxPolygonVertices];
+		/// <summary>
+		/// Get the core vertices in local coordinates. These vertices
+		/// represent a smaller polygon that is used for time of impact
+		/// computations.
+		/// </summary>
+		//public Vector2[] CoreVertices { get { return _coreVertices; } }
+		public Vec2[] GetCoreVertices() { return _coreVertices; }
+
+		private Vec2[] _normals = new Vec2[Settings.MaxPolygonVertices];
+		/// <summary>
+		/// Get the edge normal vectors. There is one for each vertex.
+		/// </summary>
+		public Vec2[] Normals { get { return _normals; } }
+
+		/// <summary>
+		/// Get the first vertex and apply the supplied transform.
+		/// </summary>
+		public Vec2 GetFirstVertex(XForm xf)
 		{
-			get { return _vertices; }
+			return Common.Math.Mul(xf, _coreVertices[0]);
 		}
 
 		/// <summary>
-		/// Copy vertices. This assumes the vertices define a convex polygon.
-		/// It is assumed that the exterior is the the right of each edge.
+		/// Get the centroid and apply the supplied transform.
+		/// </summary>
+		public Vec2 Centroid(XForm xf)
+		{
+			return Common.Math.Mul(xf, _centroid);
+		}		
+
+		/// <summary>
+		/// Get the support point in the given world direction.
+		/// Use the supplied transform.
 		/// </summary>
-		public void Set(Vec2[] vertices, int count)
+		public Vec2 Support(XForm xf, Vec2 d)
 		{
-			Box2DXDebug.Assert(3 <= count && count <= Settings.MaxPolygonVertices);
-			_vertexCount = count;
+			Vec2 dLocal = Common.Math.MulT(xf.R, d);
+
+			int bestIndex = 0;
+			float bestValue = Vec2.Dot(_coreVertices[0], dLocal);
+			for (int i = 1; i < _vertexCount; ++i)
+			{
+				float value = Vec2.Dot(_coreVertices[i], dLocal);
+				if (value > bestValue)
+				{
+					bestIndex = i;
+					bestValue = value;
+				}
+			}
+
+			return Common.Math.Mul(xf, _coreVertices[bestIndex]);
+		}
+
+		internal PolygonShape(ShapeDef def)
+			: base(def)
+		{
+			Box2DXDebug.Assert(def.Type == ShapeType.PolygonShape);
+			_type = ShapeType.PolygonShape;
+			PolygonDef poly = (PolygonDef)def;
+
+			// Get the vertices transformed into the body frame.
+			_vertexCount = poly.VertexCount;
+			Box2DXDebug.Assert(3 <= _vertexCount && _vertexCount <= Settings.MaxPolygonVertices);
 
-			int i;
 			// Copy vertices.
-			for (i = 0; i < _vertexCount; ++i)
+			for (int i = 0; i < _vertexCount; ++i)
 			{
-				_vertices[i] = vertices[i];
+				_vertices[i] = poly.Vertices[i];
 			}
 
 			// Compute normals. Ensure the edges have non-zero length.
-			for (i = 0; i < _vertexCount; ++i)
+			for (int i = 0; i < _vertexCount; ++i)
 			{
 				int i1 = i;
-				int i2 = i + 1 < count ? i + 1 : 0;
+				int i2 = i + 1 < _vertexCount ? i + 1 : 0;
 				Vec2 edge = _vertices[i2] - _vertices[i1];
-				Box2DXDebug.Assert(edge.LengthSquared() > Settings.FLT_EPSILON_SQUARED);
+				Box2DXDebug.Assert(edge.LengthSquared() > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON);
 				_normals[i] = Vec2.Cross(edge, 1.0f);
 				_normals[i].Normalize();
 			}
 
 #if DEBUG
-			// Ensure the polygon is convex and the interior
-			// is to the left of each edge.
-			for (i = 0; i < _vertexCount; ++i)
+			// Ensure the polygon is convex.
+			for (int i = 0; i < _vertexCount; ++i)
 			{
-				int i1 = i;
-				int i2 = i + 1 < count ? i + 1 : 0;
-				Vec2 edge = _vertices[i2] - _vertices[i1];
-
 				for (int j = 0; j < _vertexCount; ++j)
 				{
 					// Don't check vertices on the current edge.
-					if (j == i1 || j == i2)
+					if (j == i || j == (i + 1) % _vertexCount)
 					{
 						continue;
 					}
 
-					Vec2 r = _vertices[j] - _vertices[i1];
-
-					// Your polygon is non-convex (it has an indentation) or
-					// has colinear edges.
-					float s = Vec2.Cross(edge, r);
-					Box2DXDebug.Assert(s > 0.0f);
+					// Your polygon is non-convex (it has an indentation).
+					// Or your polygon is too skinny.
+					float s = Vec2.Dot(_normals[i], _vertices[j] - _vertices[i]);
+					Box2DXDebug.Assert(s < -Settings.LinearSlop);
 				}
 			}
-#endif
 
-			// Compute the polygon centroid.
-			_centroid = ComputeCentroid(_vertices, _vertexCount);
-		}
+			// Ensure the polygon is counter-clockwise.
+			for (int i = 1; i < _vertexCount; ++i)
+			{
+				float cross = Vec2.Cross(_normals[i - 1], _normals[i]);
 
-		/// <summary>
-		/// Build vertices to represent an axis-aligned box.
-		/// </summary>
-		/// <param name="hx">The half-width</param>
-		/// <param name="hy">The half-height.</param>
-		public void SetAsBox(float hx, float hy)
-		{
-			_vertexCount = 4;
-			_vertices[0].Set(-hx, -hy);
-			_vertices[1].Set(hx, -hy);
-			_vertices[2].Set(hx, hy);
-			_vertices[3].Set(-hx, hy);
-			_normals[0].Set(0.0f, -1.0f);
-			_normals[1].Set(1.0f, 0.0f);
-			_normals[2].Set(0.0f, 1.0f);
-			_normals[3].Set(-1.0f, 0.0f);
-			_centroid = new Vec2(0);
-		}
+				// Keep asinf happy.
+				cross = Common.Math.Clamp(cross, -1.0f, 1.0f);
 
+				// You have consecutive edges that are almost parallel on your polygon.
+				float angle = (float)System.Math.Asin(cross);
+				Box2DXDebug.Assert(angle > Settings.AngularSlop);
+			}
+#endif
 
-		/// <summary>
-		/// Build vertices to represent an oriented box.
-		/// </summary>
-		/// <param name="hx">The half-width</param>
-		/// <param name="hy">The half-height.</param>
-		/// <param name="center">The center of the box in local coordinates.</param>
-		/// <param name="angle">The rotation of the box in local coordinates.</param>
-		public void SetAsBox(float hx, float hy, Vec2 center, float angle)
-		{
-			SetAsBox(hx, hy);
+			// Compute the polygon centroid.
+			_centroid = ComputeCentroid(poly.Vertices, poly.VertexCount);
 
-			XForm xf = new XForm();
-			xf.Position = center;
-			xf.R.Set(angle);
+			// Compute the oriented bounding box.
+			ComputeOBB(out _obb, _vertices, _vertexCount);
 
-			// Transform vertices and normals.
+			// Create core polygon shape by shifting edges inward.
+			// Also compute the min/max radius for CCD.
 			for (int i = 0; i < _vertexCount; ++i)
 			{
-				_vertices[i] = Common.Math.Mul(xf, _vertices[i]);
-				_normals[i] = Common.Math.Mul(xf.R, _normals[i]);
+				int i1 = i - 1 >= 0 ? i - 1 : _vertexCount - 1;
+				int i2 = i;
+
+				Vec2 n1 = _normals[i1];
+				Vec2 n2 = _normals[i2];
+				Vec2 v = _vertices[i] - _centroid; ;
+
+				Vec2 d = new Vec2();
+				d.X = Vec2.Dot(n1, v) - Settings.ToiSlop;
+				d.Y = Vec2.Dot(n2, v) - Settings.ToiSlop;
+
+				// Shifting the edge inward by b2_toiSlop should
+				// not cause the plane to pass the centroid.
+
+				// Your shape has a radius/extent less than b2_toiSlop.
+				Box2DXDebug.Assert(d.X >= 0.0f);
+				Box2DXDebug.Assert(d.Y >= 0.0f);
+				Mat22 A = new Mat22();
+				A.Col1.X = n1.X; A.Col2.X = n1.Y;
+				A.Col1.Y = n2.X; A.Col2.Y = n2.Y;
+				_coreVertices[i] = A.Solve(d) + _centroid;
 			}
 		}
 
-		public void SetAsEdge(Vec2 v1, Vec2 v2)
+		internal override void UpdateSweepRadius(Vec2 center)
 		{
-			_vertexCount = 2;
-			_vertices[0] = v1;
-			_vertices[1] = v2;
-			_centroid = 0.5f * (v1 + v2);
-			_normals[0] = Vec2.Cross(v2 - v1, 1.0f);
-			_normals[0].Normalize();
-			_normals[1] = -_normals[0];
-		}
+			// Update the sweep radius (maximum radius) as measured from
+			// a local center point.
+			_sweepRadius = 0.0f;
+			for (int i = 0; i < _vertexCount; ++i)
+			{
+				Vec2 d = _coreVertices[i] - center;
+				_sweepRadius = Common.Math.Max(_sweepRadius, d.Length());
+			}
+		}						
 
 		public override bool TestPoint(XForm xf, Vec2 p)
 		{
 			Vec2 pLocal = Common.Math.MulT(xf.R, p - xf.Position);
 
-			int vc = _vertexCount;
-			for (int i = 0; i < vc; ++i)
+			for (int i = 0; i < _vertexCount; ++i)
 			{
 				float dot = Vec2.Dot(_normals[i], pLocal - _vertices[i]);
 				if (dot > 0.0f)
@@ -248,22 +367,24 @@ namespace Box2DX.Collision
 
 		public override void ComputeAABB(out AABB aabb, XForm xf)
 		{
-			Vec2 lower = Common.Math.Mul(xf, _vertices[0]);
-			Vec2 upper = lower;
-
-			for (int i = 1; i < _vertexCount; ++i)
-			{
-				Vec2 v = Common.Math.Mul(xf, _vertices[i]);
-				lower = Common.Math.Min(lower, v);
-				upper = Common.Math.Max(upper, v);
-			}
+			Mat22 R = Common.Math.Mul(xf.R, _obb.R);
+			Mat22 absR = Common.Math.Abs(R);
+			Vec2 h = Common.Math.Mul(absR, _obb.Extents);
+			Vec2 position = xf.Position + Common.Math.Mul(xf.R, _obb.Center);
+			aabb.LowerBound = position - h;
+			aabb.UpperBound = position + h;
+		}
 
-			Vec2 r = new Vec2(_radius);
-			aabb.LowerBound = lower - r;
-			aabb.UpperBound = upper + r;
+		public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2)
+		{
+			AABB aabb1, aabb2;
+			ComputeAABB(out aabb1, transform1);
+			ComputeAABB(out aabb2, transform2);
+			aabb.LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound);
+			aabb.UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound);
 		}
 
-		public override void ComputeMass(out MassData massData, float denstity)
+		public override void ComputeMass(out MassData massData)
 		{
 			// Polygon mass, centroid, and inertia.
 			// Let rho be the polygon density in mass per unit area.
@@ -291,24 +412,25 @@ namespace Box2DX.Collision
 
 			Box2DXDebug.Assert(_vertexCount >= 3);
 
-			Vec2 center = new Vec2(0);
+			Vec2 center = new Vec2();
+			center.Set(0.0f, 0.0f);
 			float area = 0.0f;
 			float I = 0.0f;
 
 			// pRef is the reference point for forming triangles.
 			// It's location doesn't change the result (except for rounding error).
-			Vec2 pRef = new Vec2(0);
+			Vec2 pRef = new Vec2(0.0f, 0.0f);
 
 #if O
 			// This code would put the reference point inside the polygon.
-			for (int i = 0; i < vCount; ++i)
+			for (int i = 0; i < _vertexCount; ++i)
 			{
 				pRef += _vertices[i];
 			}
 			pRef *= 1.0f / count;
 #endif
 
-			const float k_inv3 = 1.0f / 3.0f;
+			float k_inv3 = 1.0f / 3.0f;
 
 			for (int i = 0; i < _vertexCount; ++i)
 			{
@@ -339,7 +461,7 @@ namespace Box2DX.Collision
 			}
 
 			// Total mass
-			massData.Mass = denstity * area;
+			massData.Mass = _density * area;
 
 			// Center of mass
 			Box2DXDebug.Assert(area > Common.Settings.FLT_EPSILON);
@@ -347,197 +469,19 @@ namespace Box2DX.Collision
 			massData.Center = center;
 
 			// Inertia tensor relative to the local origin.
-			massData.I = denstity * I;
-		}
-
-		public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c)
-		{
-			//Transform plane into shape co-ordinates
-			Vec2 normalL = Box2DX.Common.Math.MulT(xf.R, normal);
-			float offsetL = offset - Vec2.Dot(normal, xf.Position);
-
-			float[] depths = new float[Common.Settings.MaxPolygonVertices];
-			int diveCount = 0;
-			int intoIndex = -1;
-			int outoIndex = -1;
-
-			bool lastSubmerged = false;
-			int i;
-			for (i = 0; i < _vertexCount; i++)
-			{
-				depths[i] = Vec2.Dot(normalL, _vertices[i]) - offsetL;
-				bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON;
-				if (i > 0)
-				{
-					if (isSubmerged)
-					{
-						if (!lastSubmerged)
-						{
-							intoIndex = i - 1;
-							diveCount++;
-						}
-					}
-					else
-					{
-						if (lastSubmerged)
-						{
-							outoIndex = i - 1;
-							diveCount++;
-						}
-					}
-				}
-				lastSubmerged = isSubmerged;
-			}
-			switch (diveCount)
-			{
-				case 0:
-					if (lastSubmerged)
-					{
-						//Completely submerged
-						MassData md;
-						ComputeMass(out md, 1f);
-						c = Common.Math.Mul(xf, md.Center);
-						return md.Mass;
-					}
-					else
-					{
-						//Completely dry
-						c = new Vec2();
-						return 0;
-					}
-					break;
-				case 1:
-					if (intoIndex == -1)
-					{
-						intoIndex = _vertexCount - 1;
-					}
-					else
-					{
-						outoIndex = _vertexCount - 1;
-					}
-					break;
-			}
-			int intoIndex2 = (intoIndex + 1) % _vertexCount;
-			int outoIndex2 = (outoIndex + 1) % _vertexCount;
-
-			float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
-			float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
-
-			Vec2 intoVec = new Vec2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda,
-							_vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda);
-			Vec2 outoVec = new Vec2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda,
-							_vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda);
-
-			//Initialize accumulator
-			float area = 0;
-			Vec2 center = new Vec2(0);
-			Vec2 p2 = _vertices[intoIndex2];
-			Vec2 p3;
-
-			const float k_inv3 = 1.0f / 3.0f;
-
-			//An awkward loop from intoIndex2+1 to outIndex2
-			i = intoIndex2;
-			while (i != outoIndex2)
-			{
-				i = (i + 1) % _vertexCount;
-				if (i == outoIndex2)
-					p3 = outoVec;
-				else
-					p3 = _vertices[i];
-				//Add the triangle formed by intoVec,p2,p3
-				{
-					Vec2 e1 = p2 - intoVec;
-					Vec2 e2 = p3 - intoVec;
-
-					float D = Vec2.Cross(e1, e2);
-
-					float triangleArea = 0.5f * D;
-
-					area += triangleArea;
-
-					// Area weighted centroid
-					center += triangleArea * k_inv3 * (intoVec + p2 + p3);
-
-				}
-				//
-				p2 = p3;
-			}
-
-			//Normalize and transform centroid
-			center *= 1.0f / area;
-
-			c = Common.Math.Mul(xf, center);
-
-			return area;
-		}
-
-		public override float ComputeSweepRadius(Vec2 pivot)
-		{
-			int vCount = _vertexCount;
-			Box2DXDebug.Assert(vCount > 0);
-			float sr = Vec2.DistanceSquared(_vertices[0], pivot);
-			for (int i = 1; i < vCount; ++i)
-			{
-				sr = Common.Math.Max(sr, Vec2.DistanceSquared(_vertices[i], pivot));
-			}
-
-			return Common.Math.Sqrt(sr);
-		}
-
-		/// <summary>
-		/// Get the supporting vertex index in the given direction.
-		/// </summary>
-		public override int GetSupport(Vec2 d)
-		{
-			int bestIndex = 0;
-			float bestValue = Vec2.Dot(_vertices[0], d);
-			for (int i = 1; i < _vertexCount; ++i)
-			{
-				float value = Vec2.Dot(_vertices[i], d);
-				if (value > bestValue)
-				{
-					bestIndex = i;
-					bestValue = value;
-				}
-			}
-
-			return bestIndex;
-		}
-
-		public override Vec2 GetSupportVertex(Vec2 d)
-		{
-			int bestIndex = 0;
-			float bestValue = Vec2.Dot(_vertices[0], d);
-			for (int i = 1; i < _vertexCount; ++i)
-			{
-				float value = Vec2.Dot(_vertices[i], d);
-				if (value > bestValue)
-				{
-					bestIndex = i;
-					bestValue = value;
-				}
-			}
-
-			return _vertices[bestIndex];
-		}
-
-		public override Vec2 GetVertex(int index)
-		{
-			Box2DXDebug.Assert(0 <= index && index < _vertexCount);
-			return _vertices[index];
+			massData.I = _density * I;
 		}
 
 		public static Vec2 ComputeCentroid(Vec2[] vs, int count)
 		{
 			Box2DXDebug.Assert(count >= 3);
 
-			Vec2 c = new Vec2(0f);
-			float area = 0f;
+			Vec2 c = new Vec2(); c.Set(0.0f, 0.0f);
+			float area = 0.0f;
 
 			// pRef is the reference point for forming triangles.
 			// It's location doesn't change the result (except for rounding error).
-			Vec2 pRef = new Vec2(0f);
+			Vec2 pRef = new Vec2(0.0f, 0.0f);
 #if O
 			// This code would put the reference point inside the polygon.
 			for (int i = 0; i < count; ++i)
@@ -547,7 +491,7 @@ namespace Box2DX.Collision
 			pRef *= 1.0f / count;
 #endif
 
-			const float inv3 = 1.0f / 3.0f;
+			float inv3 = 1.0f / 3.0f;
 
 			for (int i = 0; i < count; ++i)
 			{
@@ -574,104 +518,7 @@ namespace Box2DX.Collision
 			return c;
 		}
 
-		public PolygonShape()
-		{
-			_type = ShapeType.PolygonShape; 
-			_radius = Settings.PolygonRadius;
-
-			/*Box2DXDebug.Assert(def.Type == ShapeType.PolygonShape);
-			_type = ShapeType.PolygonShape;
-			PolygonDef poly = (PolygonDef)def;
-
-			// Get the vertices transformed into the body frame.
-			_vertexCount = poly.VertexCount;
-			Box2DXDebug.Assert(3 <= _vertexCount && _vertexCount <= Settings.MaxPolygonVertices);
-
-			// Copy vertices.
-			for (int i = 0; i < _vertexCount; ++i)
-			{
-				_vertices[i] = poly.Vertices[i];
-			}
-
-			// Compute normals. Ensure the edges have non-zero length.
-			for (int i = 0; i < _vertexCount; ++i)
-			{
-				int i1 = i;
-				int i2 = i + 1 < _vertexCount ? i + 1 : 0;
-				Vec2 edge = _vertices[i2] - _vertices[i1];
-				Box2DXDebug.Assert(edge.LengthSquared() > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON);
-				_normals[i] = Vec2.Cross(edge, 1.0f);
-				_normals[i].Normalize();
-			}
-
-#if DEBUG
-			// Ensure the polygon is convex.
-			for (int i = 0; i < _vertexCount; ++i)
-			{
-				for (int j = 0; j < _vertexCount; ++j)
-				{
-					// Don't check vertices on the current edge.
-					if (j == i || j == (i + 1) % _vertexCount)
-					{
-						continue;
-					}
-
-					// Your polygon is non-convex (it has an indentation).
-					// Or your polygon is too skinny.
-					float s = Vec2.Dot(_normals[i], _vertices[j] - _vertices[i]);
-					Box2DXDebug.Assert(s < -Settings.LinearSlop);
-				}
-			}
-
-			// Ensure the polygon is counter-clockwise.
-			for (int i = 1; i < _vertexCount; ++i)
-			{
-				float cross = Vec2.Cross(_normals[i - 1], _normals[i]);
-
-				// Keep asinf happy.
-				cross = Common.Math.Clamp(cross, -1.0f, 1.0f);
-
-				// You have consecutive edges that are almost parallel on your polygon.
-				float angle = (float)System.Math.Asin(cross);
-				Box2DXDebug.Assert(angle > Settings.AngularSlop);
-			}
-#endif
-
-			// Compute the polygon centroid.
-			_centroid = ComputeCentroid(poly.Vertices, poly.VertexCount);
-
-			// Compute the oriented bounding box.
-			ComputeOBB(out _obb, _vertices, _vertexCount);
-
-			// Create core polygon shape by shifting edges inward.
-			// Also compute the min/max radius for CCD.
-			for (int i = 0; i < _vertexCount; ++i)
-			{
-				int i1 = i - 1 >= 0 ? i - 1 : _vertexCount - 1;
-				int i2 = i;
-
-				Vec2 n1 = _normals[i1];
-				Vec2 n2 = _normals[i2];
-				Vec2 v = _vertices[i] - _centroid; ;
-
-				Vec2 d = new Vec2();
-				d.X = Vec2.Dot(n1, v) - Settings.ToiSlop;
-				d.Y = Vec2.Dot(n2, v) - Settings.ToiSlop;
-
-				// Shifting the edge inward by b2_toiSlop should
-				// not cause the plane to pass the centroid.
-
-				// Your shape has a radius/extent less than b2_toiSlop.
-				Box2DXDebug.Assert(d.X >= 0.0f);
-				Box2DXDebug.Assert(d.Y >= 0.0f);
-				Mat22 A = new Mat22();
-				A.Col1.X = n1.X; A.Col2.X = n1.Y;
-				A.Col1.Y = n2.X; A.Col2.Y = n2.Y;
-				_coreVertices[i] = A.Solve(d) + _centroid;
-			}*/
-		}
-
-		/*// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf
+		// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf
 		public static void ComputeOBB(out OBB obb, Vec2[] vs, int count)
 		{
 			obb = new OBB();
@@ -719,6 +566,6 @@ namespace Box2DX.Collision
 			}
 
 			Box2DXDebug.Assert(minArea < Common.Settings.FLT_MAX);
-		}*/
+		}
 	}
 }
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs
index 1afd116..94480ee 100644
--- a/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs
+++ b/Box2d/Assets/Program/Box2d/Collision/Shapes/Shape.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -20,7 +20,11 @@
 */
 
 using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Common;
+using Box2DX.Dynamics;
 
 namespace Box2DX.Collision
 {
@@ -46,6 +50,30 @@ namespace Box2DX.Collision
 	}
 
 	/// <summary>
+	/// This holds contact filtering data.
+	/// </summary>
+	public struct FilterData
+	{
+		/// <summary>
+		/// The collision category bits. Normally you would just set one bit.
+		/// </summary>
+		public ushort CategoryBits;
+
+		/// <summary>
+		/// The collision mask bits. This states the categories that this
+		/// shape would accept for collision.
+		/// </summary>
+		public ushort MaskBits;
+
+		/// <summary>
+		/// Collision groups allow a certain group of objects to never collide (negative)
+		/// or always collide (positive). Zero means no collision group. Non-zero group
+		/// filtering always wins against the mask bits.
+		/// </summary>
+		public short GroupIndex;
+	}
+
+	/// <summary>
 	/// The various collision shape types supported by Box2D.
 	/// </summary>
 	public enum ShapeType
@@ -53,7 +81,6 @@ namespace Box2DX.Collision
 		UnknownShape = -1,
 		CircleShape,
 		PolygonShape,
-		EdgeShape,
 		ShapeTypeCount,
 	}
 
@@ -67,20 +94,156 @@ namespace Box2DX.Collision
 		HitCollide = 1
 	}
 
+#warning "CAS"
+	/// <summary>
+	/// A shape definition is used to construct a shape. This class defines an
+	/// abstract shape definition. You can reuse shape definitions safely.
+	/// </summary>
+	public class ShapeDef
+	{
+		/// <summary>
+		/// Holds the shape type for down-casting.
+		/// </summary>
+		public ShapeType Type;
+
+		/// <summary>
+		/// Use this to store application specify shape data.
+		/// </summary>
+		public object UserData;
+
+		/// <summary>
+		/// The shape's friction coefficient, usually in the range [0,1].
+		/// </summary>
+		public float Friction;
+
+		/// <summary>
+		/// The shape's restitution (elasticity) usually in the range [0,1].
+		/// </summary>
+		public float Restitution;
+
+		/// <summary>
+		/// The shape's density, usually in kg/m^2.
+		/// </summary>
+		public float Density;
+
+		/// <summary>
+		/// A sensor shape collects contact information but never generates a collision
+		/// response.
+		/// </summary>
+		public bool IsSensor;
+
+		/// <summary>
+		/// Contact filtering data.
+		/// </summary>
+		public FilterData Filter;
+
+		/// <summary>
+		/// The constructor sets the default shape definition values.
+		/// </summary>
+		public ShapeDef()
+		{
+			Type = ShapeType.UnknownShape;
+			UserData = null;
+			Friction = 0.2f;
+			Restitution = 0.0f;
+			Density = 0.0f;
+			Filter.CategoryBits = 0x0001;
+			Filter.MaskBits = 0xFFFF;
+			Filter.GroupIndex = 0;
+			IsSensor = false;
+		}
+	}
+
 	/// <summary>
-	/// A shape is used for collision detection. You can create a shape however you like.
-	/// Shapes used for simulation in World are created automatically when a Fixture is created.
+	/// A shape is used for collision detection. Shapes are created in World.
+	/// You can use shape for collision detection before they are attached to the world.
+	/// Warning: you cannot reuse shapes.
 	/// </summary>
 	public abstract class Shape : IDisposable
 	{
-		#region Fields
+		#region Fields and Properties
 
-		protected ShapeType _type = ShapeType.UnknownShape;
-		internal float _radius;
+		protected ShapeType _type;
+		/// <summary>
+		/// Get the type of this shape. You can use this to down cast to the concrete shape.
+		/// </summary>
+		//public ShapeType Type { get { return _type; } }
+		public new ShapeType GetType() { return _type; }
 
-		#endregion Fields
+		internal Shape _next;
+		/// <summary>
+		/// Get the next shape in the parent body's shape list.
+		/// </summary>
+		//public Shape Next { get { return _next; } }
+		public Shape GetNext() { return _next; }
 
-		protected Shape() { }
+		internal Body _body;
+		/// <summary>
+		/// Get the parent body of this shape. This is NULL if the shape is not attached.
+		/// </summary>
+		//public Body Body { get { return _body; } }
+		public Body GetBody() { return _body; }
+
+		// Sweep radius relative to the parent body's center of mass.
+		protected float _sweepRadius;
+		/// <summary>
+		/// Get the maximum radius about the parent body's center of mass.
+		/// </summary>
+		public float GetSweepRadius() { return _sweepRadius; }
+
+		protected float _density;
+
+		protected float _friction;
+		public float Friction { get { return _friction; } set { _friction = value; } }
+
+		protected float _restitution;
+		public float Restitution { get { return _restitution; } set { _restitution = value; } }
+
+		protected ushort _proxyId;
+
+		protected bool _isSensor;
+		/// <summary>
+		/// Is this shape a sensor (non-solid)?
+		/// </summary>
+		public bool IsSensor { get { return _isSensor; } }
+
+		protected FilterData _filter;
+		/// <summary>
+		/// Get\Set the contact filtering data. You must call World.Refilter to correct
+		/// existing contacts/non-contacts.
+		/// </summary>
+		public FilterData FilterData
+		{
+			get { return _filter; }
+			set { _filter = value; }
+		}
+
+		protected object _userData;
+		/// <summary>
+		/// Get the user data that was assigned in the shape definition. Use this to
+		/// store your application specific data.
+		/// </summary>
+		public object UserData
+		{
+			get { return _userData; }
+			set { _userData = value; }
+		}
+
+		#endregion Fields and Properties
+
+		protected Shape(ShapeDef def)
+		{
+			_userData = def.UserData;
+			_friction = def.Friction;
+			_restitution = def.Restitution;
+			_density = def.Density;
+			_body = null;
+			_sweepRadius = 0.0f;
+			_next = null;
+			_proxyId = PairManager.NullProxy;
+			_filter = def.Filter;
+			_isSensor = def.IsSensor;
+		}
 
 		/// <summary>
 		/// Test a point for containment in this shape. This only works for convex shapes.
@@ -110,35 +273,144 @@ namespace Box2DX.Collision
 		public abstract void ComputeAABB(out AABB aabb, XForm xf);
 
 		/// <summary>
+		/// Given two transforms, compute the associated swept axis aligned bounding box for this shape.
+		/// </summary>
+		/// <param name="aabb">Returns the axis aligned box.</param>
+		/// <param name="xf1">The starting shape world transform.</param>
+		/// <param name="xf2">The ending shape world transform.</param>
+		public abstract void ComputeSweptAABB(out AABB aabb, XForm xf1, XForm xf2);
+
+		/// <summary>
 		/// Compute the mass properties of this shape using its dimensions and density.
 		/// The inertia tensor is computed about the local origin, not the centroid.
 		/// </summary>
 		/// <param name="massData">Returns the mass data for this shape</param>
-		public abstract void ComputeMass(out MassData massData, float density);
+		public abstract void ComputeMass(out MassData massData);
 
-		/// <summary>
-		/// Compute the volume and centroid of this shape intersected with a half plane.
-		/// </summary>
-		/// <param name="normal">Normal the surface normal.</param>
-		/// <param name="offset">Offset the surface offset along normal.</param>
-		/// <param name="xf">The shape transform.</param>
-		/// <param name="c">Returns the centroid.</param>
-		/// <returns>The total volume less than offset along normal.</returns>
-		public abstract float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c);
+		internal abstract void UpdateSweepRadius(Vec2 center);
 
-		/// <summary>
-		/// Compute the sweep radius. This is used for conservative advancement (continuous collision detection).
-		/// </summary>
-		/// <param name="pivot">Pivot is the pivot point for rotation.</param>
-		/// <returns>The distance of the furthest point from the pivot.</returns>
-		public abstract float ComputeSweepRadius(Vec2 pivot);
+		internal static Shape Create(ShapeDef def)
+		{
+			switch (def.Type)
+			{
+				case ShapeType.CircleShape:
+					{
+						return new CircleShape(def);
+					}
+
+				case ShapeType.PolygonShape:
+					{
+						return new PolygonShape(def);
+					}
+
+				default:
+					Box2DXDebug.Assert(false);
+					return null;
+			}
+		}
+
+		internal static void Destroy(Shape s)
+		{
+			switch (s.GetType())
+			{
+				case ShapeType.CircleShape:
+					if (s is IDisposable)
+						(s as IDisposable).Dispose();
+					s = null;
+					break;
+
+				case ShapeType.PolygonShape:
+					if (s is IDisposable)
+						(s as IDisposable).Dispose();
+					s = null;
+					break;
+
+				default:
+					Box2DXDebug.Assert(false);
+					break;
+			}
+		}
+
+		internal void CreateProxy(BroadPhase broadPhase, XForm transform)
+		{
+			Box2DXDebug.Assert(_proxyId == PairManager.NullProxy);
 
-		public abstract Vec2 GetVertex(int index);
+			AABB aabb;
+			ComputeAABB(out aabb, transform);
 
-		public abstract int GetSupport(Vec2 d);
+			bool inRange = broadPhase.InRange(aabb);
 
-		public abstract Vec2 GetSupportVertex(Vec2 d);
+			// You are creating a shape outside the world box.
+			Box2DXDebug.Assert(inRange);
 
-		public virtual void Dispose(){}
- 	}
+			if (inRange)
+			{
+				_proxyId = broadPhase.CreateProxy(aabb, this);
+			}
+			else
+			{
+				_proxyId = PairManager.NullProxy;
+			}
+		}
+
+		internal void DestroyProxy(BroadPhase broadPhase)
+		{
+			if (_proxyId != PairManager.NullProxy)
+			{
+				broadPhase.DestroyProxy(_proxyId);
+				_proxyId = PairManager.NullProxy;
+			}
+		}
+
+		internal bool Synchronize(BroadPhase broadPhase, XForm transform1, XForm transform2)
+		{
+			if (_proxyId == PairManager.NullProxy)
+			{
+				return false;
+			}
+
+			// Compute an AABB that covers the swept shape (may miss some rotation effect).
+			AABB aabb;
+			ComputeSweptAABB(out aabb, transform1, transform2);
+
+			if (broadPhase.InRange(aabb))
+			{
+				broadPhase.MoveProxy(_proxyId, aabb);
+				return true;
+			}
+			else
+			{
+				return false;
+			}
+		}
+
+		internal void RefilterProxy(BroadPhase broadPhase, XForm transform)
+		{
+			if (_proxyId == PairManager.NullProxy)
+			{
+				return;
+			}
+
+			broadPhase.DestroyProxy(_proxyId);
+
+			AABB aabb;
+			ComputeAABB(out aabb, transform);
+
+			bool inRange = broadPhase.InRange(aabb);
+
+			if (inRange)
+			{
+				_proxyId = broadPhase.CreateProxy(aabb, this);
+			}
+			else
+			{
+				_proxyId = PairManager.NullProxy;
+			}
+		}
+
+		public virtual void Dispose()
+		{
+			Box2DXDebug.Assert(_proxyId == PairManager.NullProxy);
+		}
+	}
 }
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Collision/TODO.txt b/Box2d/Assets/Program/Box2d/Collision/TODO.txt
deleted file mode 100644
index 312bdc2..0000000
--- a/Box2d/Assets/Program/Box2d/Collision/TODO.txt
+++ /dev/null
@@ -1 +0,0 @@
-check broadphase, port dynamictree
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta b/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta
deleted file mode 100644
index 3719ec6..0000000
--- a/Box2d/Assets/Program/Box2d/Collision/TODO.txt.meta
+++ /dev/null
@@ -1,7 +0,0 @@
-fileFormatVersion: 2
-guid: 18843330e42568c4bbff292ac904fd78
-TextScriptImporter:
-  externalObjects: {}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Common/Mat22.cs b/Box2d/Assets/Program/Box2d/Common/Mat22.cs
index ba70149..5550ab6 100644
--- a/Box2d/Assets/Program/Box2d/Common/Mat22.cs
+++ b/Box2d/Assets/Program/Box2d/Common/Mat22.cs
@@ -110,7 +110,7 @@ namespace Box2DX.Common
 		/// <summary>
 		/// Compute the inverse of this matrix, such that inv(A) * A = identity.
 		/// </summary>
-		public Mat22 GetInverse()
+		public Mat22 Invert()
 		{
 			float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y;
 			Mat22 B = new Mat22();
diff --git a/Box2d/Assets/Program/Box2d/Common/Math.cs b/Box2d/Assets/Program/Box2d/Common/Math.cs
index 8c35e3a..ea3b037 100644
--- a/Box2d/Assets/Program/Box2d/Common/Math.cs
+++ b/Box2d/Assets/Program/Box2d/Common/Math.cs
@@ -27,6 +27,10 @@ namespace Box2DX.Common
 {
 	public class Math
 	{
+		//public static float FLT_EPSILON = 1.192092896e-07f; //smallest such that 1.0f+FLT_EPSILON != 1.0f
+		public static readonly float FLOAT32_EPSILON = 1.192092896e-07f;
+		//public static float FLT_MAX = 3.402823466e+38F;
+		public static readonly float FLOAT32_MAX = 3.402823466e+38F;
 		public static readonly ushort USHRT_MAX = 0xffff;
 		public static readonly byte UCHAR_MAX = 0xff;
 		public static readonly int RAND_LIMIT = 32767;
@@ -256,10 +260,5 @@ namespace Box2DX.Common
 			Vec3 u = v.X * A.Col1 + v.Y * A.Col2 + v.Z * A.Col3;
 			return u;
 		}
-
-		public static float Atan2(float y, float x)
-		{
-			return (float)System.Math.Atan2(y, x);
-		}
 	}
 }
diff --git a/Box2d/Assets/Program/Box2d/Common/Settings.cs b/Box2d/Assets/Program/Box2d/Common/Settings.cs
index d9184ad..e3e971f 100644
--- a/Box2d/Assets/Program/Box2d/Common/Settings.cs
+++ b/Box2d/Assets/Program/Box2d/Common/Settings.cs
@@ -33,8 +33,7 @@ namespace Box2DX.Common
 		public static float	FORCE_SCALE2(x){ return x<<7;}
 		public static float FORCE_INV_SCALE2(x)	{return x>>7;}
 #else
-		public static readonly float FLT_EPSILON = 1.192092896e-07F;//smallest such that 1.0f+FLT_EPSILON != 1.0f
-		public static readonly float FLT_EPSILON_SQUARED = FLT_EPSILON * FLT_EPSILON;//smallest such that 1.0f+FLT_EPSILON != 1.0f
+		public static readonly float FLT_EPSILON = 1.192092896e-07F;
 		public static readonly float FLT_MAX = 3.402823466e+38F;
 		public static float FORCE_SCALE(float x) { return x; }
 		public static float FORCE_INV_SCALE(float x) { return x; }
@@ -65,13 +64,6 @@ namespace Box2DX.Common
 		public static readonly float AngularSlop = 2.0f / 180.0f * Pi; // 2 degrees
 
 		/// <summary>
-		/// The radius of the polygon/edge shape skin. This should not be modified. Making
-		/// this smaller means polygons will have and insufficient for continuous collision.
-		/// Making it larger may create artifacts for vertex collision.
-		/// </summary>
-		public static readonly float PolygonRadius = 2.0f * LinearSlop;
-
-		/// <summary>
 		/// Continuous collision detection (CCD) works with core, shrunken shapes. This is amount
 		/// by which shapes are automatically shrunk to work with CCD. 
 		/// This must be larger than LinearSlop.
@@ -124,21 +116,6 @@ namespace Box2DX.Common
 #if !TARGET_FLOAT32_IS_FIXED
 		public static readonly float MaxAngularVelocitySquared = MaxAngularVelocity * MaxAngularVelocity;
 #endif
-
-		/// <summary>
-		/// 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.
-		/// </summary>
-		public static readonly float MaxTranslation = 2.0f;
-		public static readonly float MaxTranslationSquared = (MaxTranslation * MaxTranslation);
-
-		/// <summary>
-		/// 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.
-		/// </summary>
-		public static readonly float MaxRotation = (0.5f * Pi);
-		public static readonly float MaxRotationSquared = (MaxRotation * MaxRotation);
-
 		/// <summary>
 		/// 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.
diff --git a/Box2d/Assets/Program/Box2d/Common/Sweep.cs b/Box2d/Assets/Program/Box2d/Common/Sweep.cs
index 09e9560..9d967e5 100644
--- a/Box2d/Assets/Program/Box2d/Common/Sweep.cs
+++ b/Box2d/Assets/Program/Box2d/Common/Sweep.cs
@@ -37,16 +37,27 @@ namespace Box2DX.Common
 		/// <summary>
 		/// Get the interpolated transform at a specific time.
 		/// </summary>
-		/// <param name="alpha">Alpha is a factor in [0,1], where 0 indicates t0.</param>
-		public void GetTransform(out XForm xf, float alpha)
+		/// <param name="t">The normalized time in [0,1].</param>
+		public void GetXForm(out XForm xf, float t)
 		{
 			xf = new XForm();
-			xf.Position = (1.0f - alpha) * C0 + alpha * C;
-			float angle = (1.0f - alpha) * A0 + alpha * A;
-			xf.R.Set(angle);
+
+			// center = p + R * LocalCenter
+			if (1.0f - T0 > Math.FLOAT32_EPSILON)
+			{
+				float alpha = (t - T0) / (1.0f - T0);
+				xf.Position = (1.0f - alpha) * C0 + alpha * C;
+				float angle = (1.0f - alpha) * A0 + alpha * A;
+				xf.R.Set(angle);
+			}
+			else
+			{
+				xf.Position = C;
+				xf.R.Set(A);
+			}
 
 			// Shift to origin
-			xf.Position -= Common.Math.Mul(xf.R, LocalCenter);
+			xf.Position -= Math.Mul(xf.R, LocalCenter);
 		}
 
 		/// <summary>
@@ -55,7 +66,7 @@ namespace Box2DX.Common
 		/// <param name="t">The new initial time.</param>
 		public void Advance(float t)
 		{
-			if (T0 < t && 1.0f - T0 > Settings.FLT_EPSILON)
+			if (T0 < t && 1.0f - T0 > Math.FLOAT32_EPSILON)
 			{
 				float alpha = (t - T0) / (1.0f - T0);
 				C0 = (1.0f - alpha) * C0 + alpha * C;
diff --git a/Box2d/Assets/Program/Box2d/Common/Vec2.cs b/Box2d/Assets/Program/Box2d/Common/Vec2.cs
index d614845..656e1be 100644
--- a/Box2d/Assets/Program/Box2d/Common/Vec2.cs
+++ b/Box2d/Assets/Program/Box2d/Common/Vec2.cs
@@ -32,38 +32,6 @@ namespace Box2DX.Common
 	{
 		public float X, Y;
 
-		public float this[int i]
-		{
-			get
-			{
-				if (i == 0) return X;
-				else if (i == 1) return Y;
-				else
-				{
-					Box2DXDebug.Assert(false, "Incorrect Vec2 element!");
-					return 0;
-				}
-			}
-			set
-			{
-				if (i == 0) X = value;
-				else if (i == 1) Y = value;
-				else
-				{
-					Box2DXDebug.Assert(false, "Incorrect Vec2 element!");					
-				}
-			}
-		}
-
-		/// <summary>
-		/// Construct using coordinates.
-		/// </summary>
-		public Vec2(float x)
-		{
-			X = x;
-			Y = x;
-		}
-
 		/// <summary>
 		/// Construct using coordinates.
 		/// </summary>
@@ -83,8 +51,6 @@ namespace Box2DX.Common
 		/// </summary>
 		public void Set(float x, float y) { X = x; Y = y; }
 
-		public void Set(float xy) { X = xy; Y = xy; }
-
 		/// <summary>
 		///  Get the length of this vector (the norm).
 		/// </summary>
@@ -108,7 +74,7 @@ namespace Box2DX.Common
 		public float Normalize()
 		{
 			float length = Length();
-			if (length < Settings.FLT_EPSILON)
+			if (length < Math.FLOAT32_EPSILON)
 			{
 				return 0.0f;
 			}
@@ -172,7 +138,7 @@ namespace Box2DX.Common
 
 		public static bool operator !=(Vec2 a, Vec2 b)
 		{
-			return a.X != b.X || a.Y != b.Y;
+			return a.X != b.X && a.Y != b.Y;
 		}
 
 		public static Vec2 Zero { get { return new Vec2(0, 0); } }
diff --git a/Box2d/Assets/Program/Box2d/Common/XForm.cs b/Box2d/Assets/Program/Box2d/Common/XForm.cs
index de507b5..2664047 100644
--- a/Box2d/Assets/Program/Box2d/Common/XForm.cs
+++ b/Box2d/Assets/Program/Box2d/Common/XForm.cs
@@ -54,19 +54,6 @@ namespace Box2DX.Common
 			R.SetIdentity();
 		}
 
-		/// Set this based on the position and angle.
-		public void Set(Vec2 p, float angle)
-		{
-			Position = p;
-			R.Set(angle);
-		}
-
-		/// Calculate the angle that the rotation matrix represents.
-		public float GetAngle()
-		{
-			return Math.Atan2(R.Col1.Y, R.Col1.X);
-		}
-
 		public static XForm Identity { get { return new XForm(Vec2.Zero, Mat22.Identity); } }
 	}
 }
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Body.cs b/Box2d/Assets/Program/Box2d/Dynamics/Body.cs
index 1daa9a5..f7dd434 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Body.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Body.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -20,6 +20,8 @@
 */
 
 using System;
+using System.Collections.Generic;
+using System.Text;
 
 using Box2DX.Common;
 using Box2DX.Collision;
@@ -30,12 +32,12 @@ namespace Box2DX.Dynamics
 	/// A body definition holds all the data needed to construct a rigid body.
 	/// You can safely re-use body definitions.
 	/// </summary>
-	public struct BodyDef
+	public class BodyDef
 	{
 		/// <summary>
 		/// This constructor sets the body definition default values.
 		/// </summary>
-		public BodyDef(byte init)
+		public BodyDef()
 		{
 			MassData = new MassData();
 			MassData.Center.SetZero();
@@ -45,8 +47,6 @@ namespace Box2DX.Dynamics
 			Position = new Vec2();
 			Position.Set(0.0f, 0.0f);
 			Angle = 0.0f;
-			LinearVelocity = new Vec2(0f, 0f);
-			AngularVelocity = 0.0f;
 			LinearDamping = 0.0f;
 			AngularDamping = 0.0f;
 			AllowSleep = true;
@@ -78,12 +78,6 @@ namespace Box2DX.Dynamics
 		/// </summary>
 		public float Angle;
 
-		/// The linear velocity of the body in world co-ordinates.
-		public Vec2 LinearVelocity;
-
-		// The angular velocity of the body.
-		public float AngularVelocity;
-
 		/// <summary>
 		/// Linear damping is use to reduce the linear velocity. The damping parameter
 		/// can be larger than 1.0f but the damping effect becomes sensitive to the
@@ -124,10 +118,11 @@ namespace Box2DX.Dynamics
 	}
 
 	/// <summary>
-	/// A rigid body. These are created via World.CreateBody.
+	/// A rigid body.
 	/// </summary>
 	public class Body : IDisposable
 	{
+
 		[Flags]
 		public enum BodyFlags
 		{
@@ -151,7 +146,7 @@ namespace Box2DX.Dynamics
 
 		internal int _islandIndex;
 
-		internal XForm _xf;		// the body origin transform
+		private XForm _xf;		// the body origin transform
 
 		internal Sweep _sweep;	// the swept motion for CCD
 
@@ -165,17 +160,15 @@ namespace Box2DX.Dynamics
 		internal Body _prev;
 		internal Body _next;
 
-		internal Fixture _fixtureList;
-		internal int _fixtureCount;
+		internal Shape _shapeList;
+		internal int _shapeCount;
 
 		internal JointEdge _jointList;
 		internal ContactEdge _contactList;
 
-		internal Controllers.ControllerEdge _controllerList;
-
 		internal float _mass;
 		internal float _invMass;
-		internal float _I;
+		private float _I;
 		internal float _invI;
 
 		internal float _linearDamping;
@@ -218,29 +211,25 @@ namespace Box2DX.Dynamics
 			_sweep.A0 = _sweep.A = bd.Angle;
 			_sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter);
 
-			//_jointList = null;
-			//_contactList = null;
-			//_controllerList = null;
-			//_prev = null;
-			//_next = null;
-
-			_linearVelocity = bd.LinearVelocity;
-			_angularVelocity = bd.AngularVelocity;
+			_jointList = null;
+			_contactList = null;
+			_prev = null;
+			_next = null;
 
 			_linearDamping = bd.LinearDamping;
 			_angularDamping = bd.AngularDamping;
 
-			//_force.Set(0.0f, 0.0f);
-			//_torque = 0.0f;
+			_force.Set(0.0f, 0.0f);
+			_torque = 0.0f;
 
-			//_linearVelocity.SetZero();
-			//_angularVelocity = 0.0f;
+			_linearVelocity.SetZero();
+			_angularVelocity = 0.0f;
 
-			//_sleepTime = 0.0f;
+			_sleepTime = 0.0f;
 
-			//_invMass = 0.0f;
-			//_I = 0.0f;
-			//_invI = 0.0f;
+			_invMass = 0.0f;
+			_I = 0.0f;
+			_invI = 0.0f;
 
 			_mass = bd.MassData.Mass;
 
@@ -249,9 +238,12 @@ namespace Box2DX.Dynamics
 				_invMass = 1.0f / _mass;
 			}
 
-			_I = bd.MassData.I;
+			if ((_flags & BodyFlags.FixedRotation) == 0)
+			{
+				_I = bd.MassData.I;
+			}
 
-			if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
+			if (_I > 0.0f)
 			{
 				_invI = 1.0f / _I;
 			}
@@ -267,8 +259,8 @@ namespace Box2DX.Dynamics
 
 			_userData = bd.UserData;
 
-			//_fixtureList = null;
-			//_fixtureCount = 0;
+			_shapeList = null;
+			_shapeCount = 0;
 		}
 
 		public void Dispose()
@@ -277,55 +269,13 @@ namespace Box2DX.Dynamics
 			// shapes and joints are destroyed in World.Destroy
 		}
 
-		internal bool SynchronizeFixtures()
-		{
-			XForm xf1 = new XForm();
-			xf1.R.Set(_sweep.A0);
-			xf1.Position = _sweep.C0 - Common.Math.Mul(xf1.R, _sweep.LocalCenter);
-
-			bool inRange = true;
-			for (Fixture f = _fixtureList; f != null; f = f.Next)
-			{
-				inRange = f.Synchronize(_world._broadPhase, xf1, _xf);
-				if (inRange == false)
-				{
-					break;
-				}
-			}
-
-			if (inRange == false)
-			{
-				_flags |= BodyFlags.Frozen;
-				_linearVelocity.SetZero();
-				_angularVelocity = 0.0f;
-
-				// Failure
-				return false;
-			}
-
-			// Success
-			return true;
-		}
-
-		// This is used to prevent connected bodies from colliding.
-		// It may lie, depending on the collideConnected flag.
-		internal bool IsConnected(Body other)
-		{
-			for (JointEdge jn = _jointList; jn != null; jn = jn.Next)
-			{
-				if (jn.Other == other)
-					return jn.Joint._collideConnected == false;
-			}
-
-			return false;
-		}
-
 		/// <summary>
-		/// Creates a fixture and attach it to this body.
+		/// Creates a shape and attach it to this body.
 		/// @warning This function is locked during callbacks.
 		/// </summary>
-		/// <param name="def">The fixture definition.</param>
-		public Fixture CreateFixture(FixtureDef def)
+		/// <param name="shapeDef">The shape definition.</param>
+		/// <returns></returns>
+		public Shape CreateShape(ShapeDef shapeDef)
 		{
 			Box2DXDebug.Assert(_world._lock == false);
 			if (_world._lock == true)
@@ -333,28 +283,31 @@ namespace Box2DX.Dynamics
 				return null;
 			}
 
-			BroadPhase broadPhase = _world._broadPhase;
+			Shape s = Shape.Create(shapeDef);
 
-			Fixture fixture = new Fixture();
-			fixture.Create(broadPhase, this, _xf, def);
+			s._next = _shapeList;
+			_shapeList = s;
+			++_shapeCount;
 
-			fixture._next = _fixtureList;
-			_fixtureList = fixture;
-			++_fixtureCount;
+			s._body = this;
 
-			fixture._body = this;
+			// Add the shape to the world's broad-phase.
+			s.CreateProxy(_world._broadPhase, _xf);
 
-			return fixture;
+			// Compute the sweep radius for CCD.
+			s.UpdateSweepRadius(_sweep.LocalCenter);
+
+			return s;
 		}
 
 		/// <summary>
-		/// Destroy a fixture. This removes the fixture from the broad-phase and
-		/// therefore destroys any contacts associated with this fixture. All fixtures
+		/// Destroy a shape. This removes the shape from the broad-phase and
+		/// therefore destroys any contacts associated with this shape. All shapes
 		/// attached to a body are implicitly destroyed when the body is destroyed.
 		/// @warning This function is locked during callbacks.
 		/// </summary>
-		/// <param name="fixture">The fixture to be removed.</param>
-		public void DestroyFixture(Fixture fixture)
+		/// <param name="shape">The shape to be removed.</param>
+		public void DestroyShape(Shape shape)
 		{
 			Box2DXDebug.Assert(_world._lock == false);
 			if (_world._lock == true)
@@ -362,35 +315,33 @@ namespace Box2DX.Dynamics
 				return;
 			}
 
-			Box2DXDebug.Assert(fixture.Body == this);
+			Box2DXDebug.Assert(shape.GetBody() == this);
+			shape.DestroyProxy(_world._broadPhase);
 
-			// Remove the fixture from this body's singly linked list.
-			Box2DXDebug.Assert(_fixtureCount > 0);
-			Fixture node = _fixtureList;
+			Box2DXDebug.Assert(_shapeCount > 0);
+			Shape node = _shapeList;
 			bool found = false;
 			while (node != null)
 			{
-				if (node == fixture)
+				if (node == shape)
 				{
-					//*node = fixture->m_next;
-					_fixtureList = fixture.Next;
+					_shapeList = shape._next;
 					found = true;
 					break;
 				}
 
-				node = node.Next;
+				node = node._next;
 			}
 
 			// You tried to remove a shape that is not attached to this body.
 			Box2DXDebug.Assert(found);
 
-			BroadPhase broadPhase = _world._broadPhase;
+			shape._body = null;
+			shape._next = null;
 
-			fixture.Destroy(broadPhase);
-			fixture._body = null;
-			fixture._next = null;
+			--_shapeCount;
 
-			--_fixtureCount;
+			Shape.Destroy(shape);
 		}
 
 		// TODO_ERIN adjust linear velocity and torque to account for movement of center.
@@ -398,8 +349,9 @@ namespace Box2DX.Dynamics
 		/// Set the mass properties. Note that this changes the center of mass position.
 		/// If you are not sure how to compute mass properties, use SetMassFromShapes.
 		/// The inertia tensor is assumed to be relative to the center of mass.
+		/// @param massData the mass properties.
 		/// </summary>
-		/// <param name="massData">The mass properties.</param>
+		/// <param name="massData"></param>
 		public void SetMass(MassData massData)
 		{
 			Box2DXDebug.Assert(_world._lock == false);
@@ -419,17 +371,26 @@ namespace Box2DX.Dynamics
 				_invMass = 1.0f / _mass;
 			}
 
-			_I = massData.I;
+			if ((_flags & BodyFlags.FixedRotation) == 0)
+			{
+				_I = massData.I;
+			}
 
-			if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
+			if (_I > 0.0f)
 			{
-				_invI = 1.0f / _I;
+				_invI = 1.0f /_I;
 			}
 
 			// Move center of mass.
 			_sweep.LocalCenter = massData.Center;
 			_sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter);
 
+			// Update the sweep radii of all child shapes.
+			for (Shape s = _shapeList; s != null; s = s._next)
+			{
+				s.UpdateSweepRadius(_sweep.LocalCenter);
+			}
+
 			BodyType oldType = _type;
 			if (_invMass == 0.0f && _invI == 0.0f)
 			{
@@ -443,9 +404,9 @@ namespace Box2DX.Dynamics
 			// If the body type changed, we need to refilter the broad-phase proxies.
 			if (oldType != _type)
 			{
-				for (Fixture f = _fixtureList; f != null; f = f.Next)
+				for (Shape s = _shapeList; s!=null; s = s._next)
 				{
-					f.RefilterProxy(_world._broadPhase, _xf);
+					s.RefilterProxy(_world._broadPhase, _xf);
 				}
 			}
 		}
@@ -471,10 +432,10 @@ namespace Box2DX.Dynamics
 			_invI = 0.0f;
 
 			Vec2 center = Vec2.Zero;
-			for (Fixture f = _fixtureList; f != null; f = f.Next)
+			for (Shape s = _shapeList; s!=null; s = s._next)
 			{
 				MassData massData;
-				f.ComputeMass(out massData);
+				s.ComputeMass(out massData);
 				_mass += massData.Mass;
 				center += massData.Mass * massData.Center;
 				_I += massData.I;
@@ -504,6 +465,12 @@ namespace Box2DX.Dynamics
 			_sweep.LocalCenter = center;
 			_sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter);
 
+			// Update the sweep radii of all child shapes.
+			for (Shape s = _shapeList; s != null; s = s._next)
+			{
+				s.UpdateSweepRadius(_sweep.LocalCenter);
+			}
+
 			BodyType oldType = _type;
 			if (_invMass == 0.0f && _invI == 0.0f)
 			{
@@ -517,9 +484,9 @@ namespace Box2DX.Dynamics
 			// If the body type changed, we need to refilter the broad-phase proxies.
 			if (oldType != _type)
 			{
-				for (Fixture f = _fixtureList; f != null; f = f.Next)
+				for (Shape s = _shapeList; s!=null; s = s._next)
 				{
-					f.RefilterProxy(_world._broadPhase, _xf);
+					s.RefilterProxy(_world._broadPhase, _xf);
 				}
 			}
 		}
@@ -553,9 +520,9 @@ namespace Box2DX.Dynamics
 			_sweep.A0 = _sweep.A = angle;
 
 			bool freeze = false;
-			for (Fixture f = _fixtureList; f != null; f = f.Next)
+			for (Shape s = _shapeList; s != null; s = s._next)
 			{
-				bool inRange = f.Synchronize(_world._broadPhase, _xf, _xf);
+				bool inRange = s.Synchronize(_world._broadPhase, _xf, _xf);
 
 				if (inRange == false)
 				{
@@ -569,6 +536,10 @@ namespace Box2DX.Dynamics
 				_flags |= BodyFlags.Frozen;
 				_linearVelocity.SetZero();
 				_angularVelocity = 0.0f;
+				for (Shape s = _shapeList; s != null; s = s._next)
+				{
+					s.DestroyProxy(_world._broadPhase);
+				}
 
 				// Failure
 				return false;
@@ -580,20 +551,6 @@ namespace Box2DX.Dynamics
 		}
 
 		/// <summary>
-		/// Set the position of the body's origin and rotation (radians).
-		/// This breaks any contacts and wakes the other bodies.
-		/// Note this is less efficient than the other overload - you should use that
-		/// if the angle is available.
-		/// </summary>
-		/// <param name="xf">The transform of position and angle to set the body to.</param>
-		/// <returns>False if the movement put a shape outside the world. In this case the
-		/// body is automatically frozen.</returns>
-		public bool SetXForm(XForm xf)
-		{
-			return SetXForm(xf.Position, xf.GetAngle());
-		}
-
-		/// <summary>
 		/// Get the body transform for the body's origin.
 		/// </summary>
 		/// <returns>Return the world transform of the body's origin.</returns>
@@ -603,24 +560,6 @@ namespace Box2DX.Dynamics
 		}
 
 		/// <summary>
-		/// Set the world body origin position.
-		/// </summary>
-		/// <param name="position">The new position of the body.</param>
-		public void SetPosition(Vec2 position)
-		{
-			SetXForm(position, GetAngle());
-		}
-
-		/// <summary>
-		/// Set the world body angle.
-		/// </summary>
-		/// <param name="angle">The new angle of the body.</param>
-		public void SetAngle(float angle)
-		{
-			SetXForm(GetPosition(), angle);
-		}
-
-		/// <summary>
 		/// Get the world body origin position.
 		/// </summary>
 		/// <returns>Return the world position of the body's origin.</returns>
@@ -678,9 +617,9 @@ namespace Box2DX.Dynamics
 		/// Set the angular velocity.
 		/// </summary>
 		/// <param name="omega">The new angular velocity in radians/second.</param>
-		public void SetAngularVelocity(float w)
+		public void SetAngularVelocity(float omega)
 		{
-			_angularVelocity = w;
+			_angularVelocity = omega;
 		}
 
 		/// <summary>
@@ -714,7 +653,7 @@ namespace Box2DX.Dynamics
 		/// without affecting the linear velocity of the center of mass.
 		/// This wakes up the body.
 		/// </summary>
-		/// <param name="torque">Torque about the z-axis (out of the screen), usually in N-m.</param>
+		/// <param name="torque">About the z-axis (out of the screen), usually in N-m.</param>
 		public void ApplyTorque(float torque)
 		{
 			if (IsSleeping())
@@ -760,19 +699,6 @@ namespace Box2DX.Dynamics
 		}
 
 		/// <summary>
-		/// Get the mass data of the body.
-		/// </summary>
-		/// <returns>A struct containing the mass, inertia and center of the body.</returns>
-		public MassData GetMassData()
-		{
-			MassData massData = new MassData();
-			massData.Mass = _mass;
-			massData.I = _I;
-			massData.Center = GetWorldCenter();
-			return massData;
-		}
-
-		/// <summary>
 		/// Get the world coordinates of a point given the local coordinates.
 		/// </summary>
 		/// <param name="localPoint">A point on the body measured relative the the body's origin.</param>
@@ -832,26 +758,6 @@ namespace Box2DX.Dynamics
 			return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
 		}
 
-		public float GetLinearDamping()
-		{
-			return _linearDamping;
-		}
-
-		public void SetLinearDamping(float linearDamping)
-		{
-			_linearDamping = linearDamping;
-		}
-
-		public float GetAngularDamping()
-		{
-			return _angularDamping;
-		}
-
-		public void SetAngularDamping(float angularDamping)
-		{
-			_angularDamping = angularDamping;
-		}
-
 		/// <summary>
 		/// Is this body treated like a bullet for continuous collision detection?
 		/// </summary>
@@ -877,31 +783,6 @@ namespace Box2DX.Dynamics
 			}
 		}
 
-		public bool IsFixedRotation()
-		{
-			return (_flags & BodyFlags.FixedRotation) == BodyFlags.FixedRotation;
-		}
-
-		public void SetFixedRotation(bool fixedr)
-		{
-			if (fixedr)
-			{
-				_angularVelocity = 0.0f;
-				_invI = 0.0f;
-				_flags |= BodyFlags.FixedRotation;
-			}
-			else
-			{
-				if (_I > 0.0f)
-				{
-					// Recover _invI from _I.
-					_invI = 1.0f / _I;
-					_flags &= BodyFlags.FixedRotation;
-				}
-				// TODO: Else what?
-			}
-		}
-
 		/// <summary>
 		/// Is this body static (immovable)?
 		/// </summary>
@@ -911,22 +792,6 @@ namespace Box2DX.Dynamics
 			return _type == BodyType.Static;
 		}
 
-		public void SetStatic()
-		{
-			if (_type == BodyType.Static)
-				return;
-			_mass = 0.0f;
-			_invMass = 0.0f;
-			_I = 0.0f;
-			_invI = 0.0f;
-			_type = BodyType.Static;
-
-			for (Fixture f = _fixtureList; f != null; f = f.Next)
-			{
-				f.RefilterProxy(_world._broadPhase, _xf);
-			}
-		}
-
 		/// <summary>
 		/// Is this body dynamic (movable)?
 		/// </summary>
@@ -954,11 +819,6 @@ namespace Box2DX.Dynamics
 			return (_flags & BodyFlags.Sleep) == BodyFlags.Sleep;
 		}
 
-		public bool IsAllowSleeping()
-		{
-			return (_flags & BodyFlags.AllowSleep) == BodyFlags.AllowSleep;
-		}
-
 		/// <summary>
 		/// You can disable sleeping on this body.
 		/// </summary>
@@ -1000,12 +860,12 @@ namespace Box2DX.Dynamics
 		}
 
 		/// <summary>
-		/// Get the list of all fixtures attached to this body.
+		/// Get the list of all shapes attached to this body.
 		/// </summary>
 		/// <returns></returns>
-		public Fixture GetFixtureList()
+		public Shape GetShapeList()
 		{
-			return _fixtureList;
+			return _shapeList;
 		}
 
 		/// <summary>
@@ -1017,10 +877,6 @@ namespace Box2DX.Dynamics
 			return _jointList;
 		}
 
-		public Controllers.ControllerEdge GetControllerList()
-		{
-			return _controllerList;
-		}
 
 		/// <summary>
 		/// Get the next body in the world's body list.
@@ -1052,12 +908,59 @@ namespace Box2DX.Dynamics
 		/// <returns></returns>
 		public World GetWorld() { return _world; }
 
+		internal bool SynchronizeShapes()
+		{
+			XForm xf1 = new XForm();
+			xf1.R.Set(_sweep.A0);
+			xf1.Position = _sweep.C0 - Common.Math.Mul(xf1.R, _sweep.LocalCenter);
+
+			bool inRange = true;
+			for (Shape s = _shapeList; s != null; s = s._next)
+			{
+				inRange = s.Synchronize(_world._broadPhase, xf1, _xf);
+				if (inRange == false)
+				{
+					break;
+				}
+			}
+
+			if (inRange == false)
+			{
+				_flags |= BodyFlags.Frozen;
+				_linearVelocity.SetZero();
+				_angularVelocity = 0.0f;
+				for (Shape s = _shapeList; s != null; s = s._next)
+				{
+					s.DestroyProxy(_world._broadPhase);
+				}
+
+				// Failure
+				return false;
+			}
+
+			// Success
+			return true;
+		}
+
 		internal void SynchronizeTransform()
 		{
 			_xf.R.Set(_sweep.A);
 			_xf.Position = _sweep.C - Common.Math.Mul(_xf.R, _sweep.LocalCenter);
 		}
 
+		// This is used to prevent connected bodies from colliding.
+		// It may lie, depending on the collideConnected flag.
+		internal bool IsConnected(Body other)
+		{
+			for (JointEdge jn = _jointList; jn != null; jn = jn.Next)
+			{
+				if (jn.Other == other)
+					return jn.Joint._collideConnected == false;
+			}
+
+			return false;
+		}
+
 		internal void Advance(float t)
 		{
 			// Advance to the new safe time.
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs b/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs
index 667c848..2b91ed1 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/ContactManager.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,6 +19,11 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using Box2DX.Common;
 using Box2DX.Collision;
 
 namespace Box2DX.Dynamics
@@ -36,40 +41,44 @@ namespace Box2DX.Dynamics
 
 		public bool _destroyImmediate;
 
-		public ContactManager() { }
+		public ContactManager()
+		{
+			_world = null;
+			_destroyImmediate = false;
+		}
 
 		// This is a callback from the broadphase when two AABB proxies begin
 		// to overlap. We create a Contact to manage the narrow phase.
-		public override object PairAdded(object proxyUserDataA, object proxyUserDataB)
+		public override object PairAdded(object proxyUserData1, object proxyUserData2)
 		{
-			Fixture fixtureA = proxyUserDataA as Fixture;
-			Fixture fixtureB = proxyUserDataB as Fixture;
+			Shape shape1 = proxyUserData1 as Shape;
+			Shape shape2 = proxyUserData2 as Shape;
 
-			Body bodyA = fixtureA.Body;
-			Body bodyB = fixtureB.Body;
+			Body body1 = shape1.GetBody();
+			Body body2 = shape2.GetBody();
 
-			if (bodyA.IsStatic() && bodyB.IsStatic())
+			if (body1.IsStatic() && body2.IsStatic())
 			{
 				return _nullContact;
 			}
 
-			if (fixtureA.Body == fixtureB.Body)
+			if (shape1.GetBody() == shape2.GetBody())
 			{
 				return _nullContact;
 			}
 
-			if (bodyB.IsConnected(bodyA))
+			if (body2.IsConnected(body1))
 			{
 				return _nullContact;
 			}
 
-			if (_world._contactFilter != null && _world._contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
+			if (_world._contactFilter != null && _world._contactFilter.ShouldCollide(shape1, shape2) == false)
 			{
 				return _nullContact;
 			}
 
 			// Call the factory.
-			Contact c = Contact.Create(fixtureA, fixtureB);
+			Contact c = Contact.Create(shape1, shape2);
 
 			if (c == null)
 			{
@@ -77,10 +86,10 @@ namespace Box2DX.Dynamics
 			}
 
 			// Contact creation may swap shapes.
-			fixtureA = c.FixtureA;
-			fixtureB = c.FixtureB;
-			bodyA = fixtureA.Body;
-			bodyB = fixtureB.Body;
+			shape1 = c.GetShape1();
+			shape2 = c.GetShape2();
+			body1 = shape1.GetBody();
+			body2 = shape2.GetBody();
 
 			// Insert into the world.
 			c._prev = null;
@@ -94,28 +103,28 @@ namespace Box2DX.Dynamics
 			// Connect to island graph.
 
 			// Connect to body 1
-			c._nodeA.Contact = c;
-			c._nodeA.Other = bodyB;
+			c._node1.Contact = c;
+			c._node1.Other = body2;
 
-			c._nodeA.Prev = null;
-			c._nodeA.Next = bodyA._contactList;
-			if (bodyA._contactList != null)
+			c._node1.Prev = null;
+			c._node1.Next = body1._contactList;
+			if (body1._contactList != null)
 			{
-				bodyA._contactList.Prev = c._nodeA;
+				body1._contactList.Prev = c._node1;
 			}
-			bodyA._contactList = c._nodeA;
+			body1._contactList = c._node1;
 
 			// Connect to body 2
-			c._nodeB.Contact = c;
-			c._nodeB.Other = bodyA;
+			c._node2.Contact = c;
+			c._node2.Other = body1;
 
-			c._nodeB.Prev = null;
-			c._nodeB.Next = bodyB._contactList;
-			if (bodyB._contactList != null)
+			c._node2.Prev = null;
+			c._node2.Next = body2._contactList;
+			if (body2._contactList != null)
 			{
-				bodyB._contactList.Prev = c._nodeB;
+				body2._contactList.Prev = c._node2;
 			}
-			bodyB._contactList = c._nodeB;
+			body2._contactList = c._node2;
 
 			++_world._contactCount;
 			return c;
@@ -146,15 +155,40 @@ namespace Box2DX.Dynamics
 
 		public void Destroy(Contact c)
 		{
-			Fixture fixtureA = c.FixtureA;
-			Fixture fixtureB = c.FixtureB;
-			Body bodyA = fixtureA.Body;
-			Body bodyB = fixtureB.Body;
-
-			if (c.Manifold.PointCount > 0)
+			Shape shape1 = c.GetShape1();
+			Shape shape2 = c.GetShape2();
+			Body body1 = shape1.GetBody();
+			Body body2 = shape2.GetBody();
+
+			ContactPoint cp = new ContactPoint();
+			cp.Shape1 = shape1;
+			cp.Shape2 = shape2;
+			cp.Friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
+			cp.Restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);
+
+			// Inform the user that this contact is ending.
+			int manifoldCount = c.GetManifoldCount();
+			if (manifoldCount > 0 && _world._contactListener!=null)
 			{
-				if(_world._contactListener!=null)
-					_world._contactListener.EndContact(c);
+				Manifold[] manifolds = c.GetManifolds();
+
+				for (int i = 0; i < manifoldCount; ++i)
+				{
+					Manifold manifold = manifolds[i];
+					cp.Normal = manifold.Normal;
+
+					for (int j = 0; j < manifold.PointCount; ++j)
+					{
+						ManifoldPoint mp = manifold.Points[j];
+						cp.Position = body1.GetWorldPoint(mp.LocalPoint1);
+						Vec2 v1 = body1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
+						Vec2 v2 = body2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
+						cp.Velocity = v2 - v1;
+						cp.Separation = mp.Separation;
+						cp.ID = mp.ID;
+						_world._contactListener.Remove(cp);
+					}
+				}
 			}
 
 			// Remove from the world.
@@ -174,39 +208,39 @@ namespace Box2DX.Dynamics
 			}
 
 			// Remove from body 1
-			if (c._nodeA.Prev != null)
+			if (c._node1.Prev != null)
 			{
-				c._nodeA.Prev.Next = c._nodeA.Next;
+				c._node1.Prev.Next = c._node1.Next;
 			}
 
-			if (c._nodeA.Next != null)
+			if (c._node1.Next != null)
 			{
-				c._nodeA.Next.Prev = c._nodeA.Prev;
+				c._node1.Next.Prev = c._node1.Prev;
 			}
 
-			if (c._nodeA == bodyA._contactList)
+			if (c._node1 == body1._contactList)
 			{
-				bodyA._contactList = c._nodeA.Next;
+				body1._contactList = c._node1.Next;
 			}
 
 			// Remove from body 2
-			if (c._nodeB.Prev != null)
+			if (c._node2.Prev != null)
 			{
-				c._nodeB.Prev.Next = c._nodeB.Next;
+				c._node2.Prev.Next = c._node2.Next;
 			}
 
-			if (c._nodeB.Next != null)
+			if (c._node2.Next != null)
 			{
-				c._nodeB.Next.Prev = c._nodeB.Prev;
+				c._node2.Next.Prev = c._node2.Prev;
 			}
 
-			if (c._nodeB == bodyB._contactList)
+			if (c._node2 == body2._contactList)
 			{
-				bodyB._contactList = c._nodeB.Next;
+				body2._contactList = c._node2.Next;
 			}
 
 			// Call the factory.
-			Contact.Destroy(ref c);
+			Contact.Destroy(c);
 			--_world._contactCount;
 		}
 
@@ -218,9 +252,9 @@ namespace Box2DX.Dynamics
 			// Update awake contacts.
 			for (Contact c = _world._contactList; c != null; c = c.GetNext())
 			{
-				Body bodyA = c._fixtureA.Body;
-				Body bodyB = c._fixtureB.Body;
-				if (bodyA.IsSleeping() && bodyB.IsSleeping())
+				Body body1 = c.GetShape1().GetBody();
+				Body body2 = c.GetShape2().GetBody();
+				if (body1.IsSleeping() && body2.IsSleeping())
 				{
 					continue;
 				}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs
index 98fe5fd..2d86681 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/CircleContact.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,6 +19,10 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Collision;
 using Box2DX.Common;
 
@@ -26,25 +30,105 @@ namespace Box2DX.Dynamics
 {
 	public class CircleContact : Contact
 	{
-		public CircleContact(Fixture fixtureA, Fixture fixtureB)
-			: base(fixtureA, fixtureB)
+		public Manifold _manifold = new Manifold();
+
+		public override Manifold[] GetManifolds()
+		{
+			return new Manifold[] { _manifold };
+		}
+
+		public CircleContact(Shape s1, Shape s2)
+			: base(s1, s2)
 		{
-			Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.CircleShape);
-			Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape);
-			CollideShapeFunction = CollideCircles;
+			Box2DXDebug.Assert(_shape1.GetType() == ShapeType.CircleShape);
+			Box2DXDebug.Assert(_shape2.GetType() == ShapeType.CircleShape);
+			_manifold.PointCount = 0;
+			_manifold.Points[0].NormalImpulse = 0.0f;
+			_manifold.Points[0].TangentImpulse = 0.0f;
 		}
 
-		private static void CollideCircles(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
+		public override void Evaluate(ContactListener listener)
 		{
-			Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2);
+			Body b1 = _shape1.GetBody();
+			Body b2 = _shape2.GetBody();
+#warning "needfix"
+			//memcpy(&m0, &m_manifold, sizeof(b2Manifold));
+			Manifold m0 = _manifold.Clone();
+
+			Collision.Collision.CollideCircles(ref _manifold, (CircleShape)_shape1, b1.GetXForm(), 
+				(CircleShape)_shape2, b2.GetXForm());
+
+			ContactPoint cp = new ContactPoint();
+			cp.Shape1 = _shape1;
+			cp.Shape2 = _shape2;
+			cp.Friction = Settings.MixFriction(_shape1.Friction, _shape2.Friction);
+			cp.Restitution = Settings.MixRestitution(_shape1.Restitution, _shape2.Restitution);
+
+			if (_manifold.PointCount > 0)
+			{
+				_manifoldCount = 1;
+				ManifoldPoint mp = _manifold.Points[0];
+
+				if (m0.PointCount == 0)
+				{
+					mp.NormalImpulse = 0.0f;
+					mp.TangentImpulse = 0.0f;
+
+					if (listener!=null)
+					{
+						cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
+						Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
+						Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
+						cp.Velocity = v2 - v1;
+						cp.Normal = _manifold.Normal;
+						cp.Separation = mp.Separation;
+						cp.ID = mp.ID;
+						listener.Add(cp);
+					}
+				}
+				else
+				{
+					ManifoldPoint mp0 = m0.Points[0];
+					mp.NormalImpulse = mp0.NormalImpulse;
+					mp.TangentImpulse = mp0.TangentImpulse;
+
+					if (listener!=null)
+					{
+						cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
+						Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
+						Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
+						cp.Velocity = v2 - v1;
+						cp.Normal = _manifold.Normal;
+						cp.Separation = mp.Separation;
+						cp.ID = mp.ID;
+						listener.Persist(cp);
+					}
+				}
+			}
+			else
+			{
+				_manifoldCount = 0;
+				if (m0.PointCount > 0 && listener!=null)
+				{
+					ManifoldPoint mp0 = m0.Points[0];
+					cp.Position = b1.GetWorldPoint(mp0.LocalPoint1);
+					Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1);
+					Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2);
+					cp.Velocity = v2 - v1;
+					cp.Normal = m0.Normal;
+					cp.Separation = mp0.Separation;
+					cp.ID = mp0.ID;
+					listener.Remove(cp);
+				}
+			}
 		}
 
-		new public static Contact Create(Fixture fixtureA, Fixture fixtureB)
+		new public static Contact Create(Shape shape1, Shape shape2)
 		{
-			return new CircleContact(fixtureA, fixtureB);
+			return new CircleContact(shape1, shape2);
 		}
 
-		new public static void Destroy(ref Contact contact)
+		new public static void Destroy(Contact contact)
 		{
 			contact = null;
 		}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs
index 7c23ca5..127e610 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/Contact.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -20,13 +20,16 @@
 */
 
 using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Collision;
 using Box2DX.Common;
 
 namespace Box2DX.Dynamics
 {
-	public delegate Contact ContactCreateFcn(Fixture fixtureA, Fixture fixtureB);
-	public delegate void ContactDestroyFcn(ref Contact contact);
+	public delegate Contact ContactCreateFcn(Shape shape1, Shape shape2);
+	public delegate void ContactDestroyFcn(Contact contact);
 
 	public struct ContactRegister
 	{
@@ -35,6 +38,7 @@ namespace Box2DX.Dynamics
 		public bool Primary;
 	}
 
+#warning "CAS"
 	/// <summary>
 	/// A contact edge is used to connect bodies and contacts together
 	/// in a contact graph where each body is a node and each contact
@@ -62,6 +66,86 @@ namespace Box2DX.Dynamics
 		public ContactEdge Next;
 	}
 
+#warning "CAS"
+	/// <summary>
+	/// This structure is used to report contact points.
+	/// </summary>
+	public class ContactPoint
+	{
+		/// <summary>
+		/// The first shape.
+		/// </summary>
+		public Shape Shape1;
+		/// <summary>
+		/// The second shape.
+		/// </summary>
+		public Shape Shape2;
+		/// <summary>
+		/// Position in world coordinates.
+		/// </summary>
+		public Vec2 Position;
+		/// <summary>
+		/// Velocity of point on body2 relative to point on body1 (pre-solver).
+		/// </summary>
+		public Vec2 Velocity;
+		/// <summary>
+		/// Points from shape1 to shape2.
+		/// </summary>
+		public Vec2 Normal;
+		/// <summary>
+		/// The separation is negative when shapes are touching.
+		/// </summary>
+		public float Separation;
+		/// <summary>
+		/// The combined friction coefficient.
+		/// </summary>
+		public float Friction;
+		/// <summary>
+		/// The combined restitution coefficient.
+		/// </summary>
+		public float Restitution;
+		/// <summary>
+		/// The contact id identifies the features in contact.
+		/// </summary>
+		public ContactID ID;
+	}
+
+#warning "CAS"
+	/// <summary>
+	/// This structure is used to report contact point results.
+	/// </summary>
+	public class ContactResult
+	{
+		/// <summary>
+		/// The first shape.
+		/// </summary>
+		public Shape Shape1;
+		/// <summary>
+		/// The second shape.
+		/// </summary>
+		public Shape Shape2;
+		/// <summary>
+		/// Position in world coordinates.
+		/// </summary>
+		public Vec2 Position;
+		/// <summary>
+		/// Points from shape1 to shape2.
+		/// </summary>
+		public Vec2 Normal;
+		/// <summary>
+		/// The normal impulse applied to body2.
+		/// </summary>
+		public float NormalImpulse;
+		/// <summary>
+		/// The tangent impulse applied to body2.
+		/// </summary>
+		public float TangentImpulse;
+		/// <summary>
+		/// The contact id identifies the features in contact.
+		/// </summary>
+		public ContactID ID;
+	}
+
 	/// <summary>
 	/// 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
@@ -75,56 +159,62 @@ namespace Box2DX.Dynamics
 			NonSolid = 0x0001,
 			Slow = 0x0002,
 			Island = 0x0004,
-			Toi = 0x0008,
-			Touch = 0x0010
+			Toi = 0x0008
 		}
 
 		public static ContactRegister[][] s_registers =
 			new ContactRegister[(int)ShapeType.ShapeTypeCount][/*(int)ShapeType.ShapeTypeCount*/];
-		public static bool s_initialized;
+		public static bool s_initialized = false;
 
 		public CollisionFlags _flags;
+		public int _manifoldCount;
 
 		// World pool and list pointers.
 		public Contact _prev;
 		public Contact _next;
 
 		// Nodes for connecting bodies.
-		public ContactEdge _nodeA;
-		public ContactEdge _nodeB;
+		public ContactEdge _node1;
+		public ContactEdge _node2;
 
-		public Fixture _fixtureA;
-		public Fixture _fixtureB;
-		
-		public Manifold _manifold = new Manifold();
+		public Shape _shape1;
+		public Shape _shape2;
 
 		public float _toi;
 
-		internal delegate void CollideShapeDelegate(
-			ref Manifold manifold, Shape circle1, XForm xf1, Shape circle2, XForm xf2);
-		internal CollideShapeDelegate CollideShapeFunction;
-
-		public Contact(){}
+		public Contact()
+		{
+			_shape1 = null; _shape2 = null;
+		}
 
-		public Contact(Fixture fA, Fixture fB)
+		public Contact(Shape s1, Shape s2)
 		{
 			_flags = 0;
 
-			if (fA.IsSensor || fB.IsSensor)
+			if (s1.IsSensor || s2.IsSensor)
 			{
 				_flags |= CollisionFlags.NonSolid;
 			}
 
-			_fixtureA = fA;
-			_fixtureB = fB;
+			_shape1 = s1;
+			_shape2 = s2;
 
-			_manifold.PointCount = 0;
+			_manifoldCount = 0;
 
 			_prev = null;
 			_next = null;
 
-			_nodeA = new ContactEdge();
-			_nodeB = new ContactEdge();
+			_node1 = new ContactEdge();
+			_node1.Contact = null;
+			_node1.Prev = null;
+			_node1.Next = null;
+			_node1.Other = null;
+
+			_node2 = new ContactEdge();
+			_node2.Contact = null;
+			_node2.Prev = null;
+			_node2.Next = null;
+			_node2.Other = null;
 		}
 
 		public static void AddType(ContactCreateFcn createFcn, ContactDestroyFcn destoryFcn,
@@ -142,6 +232,9 @@ namespace Box2DX.Dynamics
 
 			if (type1 != type2)
 			{
+				//if (_registers[(int)type2] == null)
+				//	_registers[(int)type2] = new ContactRegister[(int)ShapeType.ShapeTypeCount];
+
 				s_registers[(int)type2][(int)type1].CreateFcn = createFcn;
 				s_registers[(int)type2][(int)type1].DestroyFcn = destoryFcn;
 				s_registers[(int)type2][(int)type1].Primary = false;
@@ -153,12 +246,9 @@ namespace Box2DX.Dynamics
 			AddType(CircleContact.Create, CircleContact.Destroy, ShapeType.CircleShape, ShapeType.CircleShape);
 			AddType(PolyAndCircleContact.Create, PolyAndCircleContact.Destroy, ShapeType.PolygonShape, ShapeType.CircleShape);
 			AddType(PolygonContact.Create, PolygonContact.Destroy, ShapeType.PolygonShape, ShapeType.PolygonShape);
-
-			AddType(EdgeAndCircleContact.Create, EdgeAndCircleContact.Destroy, ShapeType.EdgeShape, ShapeType.CircleShape);
-			AddType(PolyAndEdgeContact.Create, PolyAndEdgeContact.Destroy, ShapeType.PolygonShape, ShapeType.EdgeShape);
 		}
 
-		public static Contact Create(Fixture fixtureA, Fixture fixtureB)
+		public static Contact Create(Shape shape1, Shape shape2)
 		{
 			if (s_initialized == false)
 			{
@@ -166,8 +256,8 @@ namespace Box2DX.Dynamics
 				s_initialized = true;
 			}
 
-			ShapeType type1 = fixtureA.ShapeType;
-			ShapeType type2 = fixtureB.ShapeType;
+			ShapeType type1 = shape1.GetType();
+			ShapeType type2 = shape2.GetType();
 
 			Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount);
 			Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount);
@@ -177,11 +267,17 @@ namespace Box2DX.Dynamics
 			{
 				if (s_registers[(int)type1][(int)type2].Primary)
 				{
-					return createFcn(fixtureA, fixtureB);
+					return createFcn(shape1, shape2);
 				}
 				else
 				{
-					return createFcn(fixtureB, fixtureA);
+					Contact c = createFcn(shape2, shape1);
+					for (int i = 0; i < c.GetManifoldCount(); ++i)
+					{
+						Manifold m = c.GetManifolds()[i];
+						m.Normal = -m.Normal;
+					}
+					return c;
 				}
 			}
 			else
@@ -190,187 +286,107 @@ namespace Box2DX.Dynamics
 			}
 		}
 
-		public static void Destroy(ref Contact contact)
+		public static void Destroy(Contact contact)
 		{
 			Box2DXDebug.Assert(s_initialized == true);
 
-			if (contact._manifold.PointCount > 0)
+			if (contact.GetManifoldCount() > 0)
 			{
-				contact.FixtureA.Body.WakeUp();
-				contact.FixtureB.Body.WakeUp();
+				contact.GetShape1().GetBody().WakeUp();
+				contact.GetShape2().GetBody().WakeUp();
 			}
 
-			ShapeType typeA = contact.FixtureA.ShapeType;
-			ShapeType typeB = contact.FixtureB.ShapeType;
-
-			Box2DXDebug.Assert(ShapeType.UnknownShape < typeA && typeA < ShapeType.ShapeTypeCount);
-			Box2DXDebug.Assert(ShapeType.UnknownShape < typeB && typeB < ShapeType.ShapeTypeCount);
+			ShapeType type1 = contact.GetShape1().GetType();
+			ShapeType type2 = contact.GetShape2().GetType();
 
-			ContactDestroyFcn destroyFcn = s_registers[(int)typeA][(int)typeB].DestroyFcn;
-			destroyFcn(ref contact);
-		}
-
-		public void Update(ContactListener listener)
-		{
-			Manifold oldManifold = _manifold.Clone();
-
-			Evaluate();
-
-			Body bodyA = _fixtureA.Body;
-			Body bodyB = _fixtureB.Body;
-
-			int oldCount = oldManifold.PointCount;
-			int newCount = _manifold.PointCount;
-
-			if (newCount == 0 && oldCount > 0)
-			{
-				bodyA.WakeUp();
-				bodyB.WakeUp();
-			}
-
-			// Slow contacts don't generate TOI events.
-			if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
-			{
-				_flags &= ~CollisionFlags.Slow;
-			}
-			else
-			{
-				_flags |= CollisionFlags.Slow;
-			}
-
-			// Match old contact ids to new contact ids and copy the
-			// stored impulses to warm start the solver.
-			for (int i = 0; i < _manifold.PointCount; ++i)
-			{
-				ManifoldPoint mp2 = _manifold.Points[i];
-				mp2.NormalImpulse = 0.0f;
-				mp2.TangentImpulse = 0.0f;
-				ContactID id2 = mp2.ID;
-
-				for (int j = 0; j < oldManifold.PointCount; ++j)
-				{
-					ManifoldPoint mp1 = oldManifold.Points[j];
-
-					if (mp1.ID.Key == id2.Key)
-					{
-						mp2.NormalImpulse = mp1.NormalImpulse;
-						mp2.TangentImpulse = mp1.TangentImpulse;
-						break;
-					}
-				}
-			}
-
-			if (oldCount == 0 && newCount > 0)
-			{
-				_flags |= CollisionFlags.Touch;
-				if(listener!=null)
-					listener.BeginContact(this);
-			}
-
-			if (oldCount > 0 && newCount == 0)
-			{
-				_flags &= ~CollisionFlags.Touch;
-				if (listener != null)
-				listener.EndContact(this);
-			}
-
-			if ((_flags & CollisionFlags.NonSolid) == 0)
-			{
-				if (listener != null)
-					listener.PreSolve(this, oldManifold);
-
-				// The user may have disabled contact.
-				if (_manifold.PointCount == 0)
-				{
-					_flags &= ~CollisionFlags.Touch;
-				}
-			}
-		}
-
-		public void Evaluate()
-		{
-			Body bodyA = _fixtureA.Body;
-			Body bodyB = _fixtureB.Body;
-
-			Box2DXDebug.Assert(CollideShapeFunction!=null);
-
-			CollideShapeFunction(ref _manifold, _fixtureA.Shape, bodyA.GetXForm(), _fixtureB.Shape, bodyB.GetXForm());
-		}
+			Box2DXDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount);
+			Box2DXDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount);
 
-		public float ComputeTOI(Sweep sweepA, Sweep sweepB)
-		{
-			TOIInput input = new TOIInput();
-			input.SweepA = sweepA;
-			input.SweepB = sweepB;
-			input.SweepRadiusA = _fixtureA.ComputeSweepRadius(sweepA.LocalCenter);
-			input.SweepRadiusB = _fixtureB.ComputeSweepRadius(sweepB.LocalCenter);
-			input.Tolerance = Common.Settings.LinearSlop;
-
-			return Collision.Collision.TimeOfImpact(input, _fixtureA.Shape, _fixtureB.Shape);
+			ContactDestroyFcn destroyFcn = s_registers[(int)type1][(int)type2].DestroyFcn;
+			destroyFcn(contact);
 		}
 
 		/// <summary>
-		/// Get the contact manifold.
+		/// Get the manifold array.
 		/// </summary>
-		public Manifold Manifold
-		{
-			get { return _manifold; }
-		}
+		/// <returns></returns>
+		public abstract Manifold[] GetManifolds();
 
 		/// <summary>
-		/// Get the world manifold.
-		/// </summary>		
-		public void GetWorldManifold(out WorldManifold worldManifold)
+		/// Get the number of manifolds. This is 0 or 1 between convex shapes.
+		/// This may be greater than 1 for convex-vs-concave shapes. Each
+		/// manifold holds up to two contact points with a shared contact normal.
+		/// </summary>
+		/// <returns></returns>
+		public int GetManifoldCount()
 		{
-			worldManifold = new WorldManifold();
-
-			Body bodyA = _fixtureA.Body;
-			Body bodyB = _fixtureB.Body;
-			Shape shapeA = _fixtureA.Shape;
-			Shape shapeB = _fixtureB.Shape;
-
-			worldManifold.Initialize(_manifold, bodyA.GetXForm(), shapeA._radius, bodyB.GetXForm(), shapeB._radius);
+			return _manifoldCount;
 		}
 
 		/// <summary>
 		/// Is this contact solid?
 		/// </summary>
 		/// <returns>True if this contact should generate a response.</returns>
-		public bool IsSolid
+		public bool IsSolid()
 		{
-			get { return (_flags & CollisionFlags.NonSolid) == 0; }
-		}
-
-		/// <summary>
-		/// Are fixtures touching?
-		/// </summary>
-		public bool AreTouching
-		{
-			get { return (_flags & CollisionFlags.Touch) == CollisionFlags.Touch; }
+			return (_flags & CollisionFlags.NonSolid) == 0;
 		}
 
 		/// <summary>
 		/// Get the next contact in the world's contact list.
 		/// </summary>
+		/// <returns></returns>
 		public Contact GetNext()
 		{
 			return _next;
 		}
 
 		/// <summary>
-		/// Get the first fixture in this contact.
+		/// Get the first shape in this contact.
 		/// </summary>
-		public Fixture FixtureA
+		/// <returns></returns>
+		public Shape GetShape1()
 		{
-			get { return _fixtureA; }
+			return _shape1;
 		}
 
 		/// <summary>
-		/// Get the second fixture in this contact.
+		/// Get the second shape in this contact.
 		/// </summary>
-		public Fixture FixtureB
+		/// <returns></returns>
+		public Shape GetShape2()
+		{
+			return _shape2;
+		}
+
+		public void Update(ContactListener listener)
 		{
-			get { return _fixtureB; }
-		}		
+			int oldCount = GetManifoldCount();
+
+			Evaluate(listener);
+
+			int newCount = GetManifoldCount();
+
+			Body body1 = _shape1.GetBody();
+			Body body2 = _shape2.GetBody();
+
+			if (newCount == 0 && oldCount > 0)
+			{
+				body1.WakeUp();
+				body2.WakeUp();
+			}
+
+			// Slow contacts don't generate TOI events.
+			if (body1.IsStatic() || body1.IsBullet() || body2.IsStatic() || body2.IsBullet())
+			{
+				_flags &= ~CollisionFlags.Slow;
+			}
+			else
+			{
+				_flags |= CollisionFlags.Slow;
+			}
+		}
+
+		public abstract void Evaluate(ContactListener listener);
 	}
 }
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs
index a8affb6..2578848 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/ContactSolver.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -20,48 +20,52 @@
 */
 
 //#define B2_DEBUG_SOLVER
+#define FIRST
 
 using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Collision;
 using Box2DX.Common;
 
 namespace Box2DX.Dynamics
 {
-	public struct ContactConstraintPoint
+#warning "CAS"
+	public class ContactConstraintPoint
 	{
-		public Vec2 LocalPoint;
-		public Vec2 RA;
-		public Vec2 RB;
+		public Vec2 LocalAnchor1;
+		public Vec2 LocalAnchor2;
+		public Vec2 R1;
+		public Vec2 R2;
 		public float NormalImpulse;
 		public float TangentImpulse;
 		public float NormalMass;
 		public float TangentMass;
 		public float EqualizedMass;
+		public float Separation;
 		public float VelocityBias;
 	}
 
+#warning "CAS"
 	public class ContactConstraint
 	{
 		public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxManifoldPoints];
-		public Vec2 LocalPlaneNormal;
-		public Vec2 LocalPoint;
 		public Vec2 Normal;
 		public Mat22 NormalMass;
 		public Mat22 K;
-		public Body BodyA;
-		public Body BodyB;
-		public ManifoldType Type;
-		public float Radius;
+		public Manifold Manifold;
+		public Body Body1;
+		public Body Body2;
 		public float Friction;
 		public float Restitution;
 		public int PointCount;
-		public Manifold Manifold;
 
-		//public ContactConstraint()
-		//{
-		//	for (int i = 0; i < Settings.MaxManifoldPoints; i++)
-		//		Points[i] = new ContactConstraintPoint();
-		//}
+		public ContactConstraint()
+		{
+			for (int i = 0; i < Settings.MaxManifoldPoints; i++)
+				Points[i] = new ContactConstraintPoint();
+		}
 	}
 
 	public class ContactSolver : IDisposable
@@ -73,147 +77,156 @@ namespace Box2DX.Dynamics
 		public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
 		{
 			_step = step;
-			_constraintCount = contactCount;
+
+			_constraintCount = 0;
+			for (int i = 0; i < contactCount; ++i)
+			{
+				Box2DXDebug.Assert(contacts[i].IsSolid());
+				_constraintCount += contacts[i].GetManifoldCount();
+			}
 
 			_constraints = new ContactConstraint[_constraintCount];
 			for (int i = 0; i < _constraintCount; i++)
 				_constraints[i] = new ContactConstraint();
 
 			int count = 0;
-			for (int i = 0; i < _constraintCount; ++i)
+			for (int i = 0; i < contactCount; ++i)
 			{
 				Contact contact = contacts[i];
 
-				Fixture fixtureA = contact._fixtureA;
-				Fixture fixtureB = contact._fixtureB;
-				Shape shapeA = fixtureA.Shape;
-				Shape shapeB = fixtureB.Shape;
-				float radiusA = shapeA._radius;
-				float radiusB = shapeB._radius;
-				Body bodyA = fixtureA.Body;
-				Body bodyB = fixtureB.Body;
-				Manifold manifold = contact.Manifold;
-
-				float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction);
-				float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution);
-
-				Vec2 vA = bodyA._linearVelocity;
-				Vec2 vB = bodyB._linearVelocity;
-				float wA = bodyA._angularVelocity;
-				float wB = bodyB._angularVelocity;
-
-				Box2DXDebug.Assert(manifold.PointCount > 0);
-
-				WorldManifold worldManifold = new WorldManifold();
-				worldManifold.Initialize(manifold, bodyA._xf, radiusA, bodyB._xf, radiusB);
-
-				ContactConstraint cc = _constraints[i];
-				cc.BodyA = bodyA;
-				cc.BodyB = bodyB;
-				cc.Manifold = manifold;
-				cc.Normal = worldManifold.Normal;
-				cc.PointCount = manifold.PointCount;
-				cc.Friction = friction;
-				cc.Restitution = restitution;
-
-				cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
-				cc.LocalPoint = manifold.LocalPoint;
-				cc.Radius = radiusA + radiusB;
-				cc.Type = manifold.Type;
-
-				unsafe
+				Shape shape1 = contact._shape1;
+				Shape shape2 = contact._shape2;
+				Body b1 = shape1.GetBody();
+				Body b2 = shape2.GetBody();
+				int manifoldCount = contact.GetManifoldCount();
+				Manifold[] manifolds = contact.GetManifolds();
+
+				float friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
+				float restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);
+
+				Vec2 v1 = b1._linearVelocity;
+				Vec2 v2 = b2._linearVelocity;
+				float w1 = b1._angularVelocity;
+				float w2 = b2._angularVelocity;
+
+				for (int j = 0; j < manifoldCount; ++j)
 				{
-					fixed (ContactConstraintPoint* ccPointsPtr = cc.Points)
-					{
-						for (int j = 0; j < cc.PointCount; ++j)
-						{
-							ManifoldPoint cp = manifold.Points[j];
-							ContactConstraintPoint* ccp = &ccPointsPtr[j];
+					Manifold manifold = manifolds[j];
 
-							ccp->NormalImpulse = cp.NormalImpulse;
-							ccp->TangentImpulse = cp.TangentImpulse;
+					Box2DXDebug.Assert(manifold.PointCount > 0);
+
+					Vec2 normal = manifold.Normal;
+
+					Box2DXDebug.Assert(count < _constraintCount);
+					ContactConstraint cc = _constraints[count];
+					cc.Body1 = b1;
+					cc.Body2 = b2;
+					cc.Manifold = manifold;
+					cc.Normal = normal;
+					cc.PointCount = manifold.PointCount;
+					cc.Friction = friction;
+					cc.Restitution = restitution;
+
+					for (int k = 0; k < cc.PointCount; ++k)
+					{
+						ManifoldPoint cp = manifold.Points[k];
+						ContactConstraintPoint ccp = cc.Points[k];
 
-							ccp->LocalPoint = cp.LocalPoint;
+						ccp.NormalImpulse = cp.NormalImpulse;
+						ccp.TangentImpulse = cp.TangentImpulse;
+						ccp.Separation = cp.Separation;
 
-							ccp->RA = worldManifold.Points[j] - bodyA._sweep.C;
-							ccp->RB = worldManifold.Points[j] - bodyB._sweep.C;
+						ccp.LocalAnchor1 = cp.LocalPoint1;
+						ccp.LocalAnchor2 = cp.LocalPoint2;
+						ccp.R1 = Common.Math.Mul(b1.GetXForm().R, cp.LocalPoint1 - b1.GetLocalCenter());
+						ccp.R2 = Common.Math.Mul(b2.GetXForm().R, cp.LocalPoint2 - b2.GetLocalCenter());
 
-							float rnA = Vec2.Cross(ccp->RA, cc.Normal);
-							float rnB = Vec2.Cross(ccp->RB, cc.Normal);
-							rnA *= rnA;
-							rnB *= rnB;
+						float rn1 = Vec2.Cross(ccp.R1, normal);
+						float rn2 = Vec2.Cross(ccp.R2, normal);
+						rn1 *= rn1;
+						rn2 *= rn2;
 
-							float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
+						float kNormal = b1._invMass + b2._invMass + b1._invI * rn1 + b2._invI * rn2;
 
-							Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
-							ccp->NormalMass = 1.0f / kNormal;
+						Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
+						ccp.NormalMass = 1.0f / kNormal;
 
-							float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
-							kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
+						float kEqualized = b1._mass * b1._invMass + b2._mass * b2._invMass;
+						kEqualized += b1._mass * b1._invI * rn1 + b2._mass * b2._invI * rn2;
 
-							Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
-							ccp->EqualizedMass = 1.0f / kEqualized;
+						Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
+						ccp.EqualizedMass = 1.0f / kEqualized;
 
-							Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);
+						Vec2 tangent = Vec2.Cross(normal, 1.0f);
 
-							float rtA = Vec2.Cross(ccp->RA, tangent);
-							float rtB = Vec2.Cross(ccp->RB, tangent);
-							rtA *= rtA;
-							rtB *= rtB;
+						float rt1 = Vec2.Cross(ccp.R1, tangent);
+						float rt2 = Vec2.Cross(ccp.R2, tangent);
+						rt1 *= rt1;
+						rt2 *= rt2;
 
-							float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
+						float kTangent = b1._invMass + b2._invMass + b1._invI * rt1 + b2._invI * rt2;
 
-							Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
-							ccp->TangentMass = 1.0f / kTangent;
+						Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
+						ccp.TangentMass = 1.0f / kTangent;
 
-							// Setup a velocity bias for restitution.
-							ccp->VelocityBias = 0.0f;
-							float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA));
-							if (vRel < -Common.Settings.VelocityThreshold)
+						// Setup a velocity bias for restitution.
+						ccp.VelocityBias = 0.0f;
+						if (ccp.Separation > 0.0f)
+						{
+							ccp.VelocityBias = -step.Inv_Dt * ccp.Separation; // TODO_ERIN b2TimeStep
+						}
+						else
+						{
+							float vRel = Vec2.Dot(cc.Normal, v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1));
+							if (vRel < -Settings.VelocityThreshold)
 							{
-								ccp->VelocityBias = -cc.Restitution * vRel;
+								ccp.VelocityBias = -cc.Restitution * vRel;
 							}
 						}
+					}
 
-						// If we have two points, then prepare the block solver.
-						if (cc.PointCount == 2)
+					// If we have two points, then prepare the block solver.
+					if (cc.PointCount == 2)
+					{
+						ContactConstraintPoint ccp1 = cc.Points[0];
+						ContactConstraintPoint ccp2 = cc.Points[1];
+
+						float invMass1 = b1._invMass;
+						float invI1 = b1._invI;
+						float invMass2 = b2._invMass;
+						float invI2 = b2._invI;
+
+						float rn11 = Vec2.Cross(ccp1.R1, normal);
+						float rn12 = Vec2.Cross(ccp1.R2, normal);
+						float rn21 = Vec2.Cross(ccp2.R1, normal);
+						float rn22 = Vec2.Cross(ccp2.R2, normal);
+
+						float k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12;
+						float k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22;
+						float k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22;
+
+						// Ensure a reasonable condition number.
+						const float k_maxConditionNumber = 100.0f;
+						if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
 						{
-							ContactConstraintPoint* ccp1 = &ccPointsPtr[0];
-							ContactConstraintPoint* ccp2 = &ccPointsPtr[1];
-
-							float invMassA = bodyA._invMass;
-							float invIA = bodyA._invI;
-							float invMassB = bodyB._invMass;
-							float invIB = bodyB._invI;
-
-							float rn1A = Vec2.Cross(ccp1->RA, cc.Normal);
-							float rn1B = Vec2.Cross(ccp1->RB, cc.Normal);
-							float rn2A = Vec2.Cross(ccp2->RA, cc.Normal);
-							float rn2B = Vec2.Cross(ccp2->RB, cc.Normal);
-
-							float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
-							float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
-							float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
-
-							// Ensure a reasonable condition number.
-							const float k_maxConditionNumber = 100.0f;
-							if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
-							{
-								// K is safe to invert.
-								cc.K.Col1.Set(k11, k12);
-								cc.K.Col2.Set(k12, k22);
-								cc.NormalMass = cc.K.GetInverse();
-							}
-							else
-							{
-								// The constraints are redundant, just use one.
-								// TODO_ERIN use deepest?
-								cc.PointCount = 1;
-							}
+							// K is safe to invert.
+							cc.K.Col1.Set(k11, k12);
+							cc.K.Col2.Set(k12, k22);
+							cc.NormalMass = cc.K.Invert();
+						}
+						else
+						{
+							// The constraints are redundant, just use one.
+							// TODO_ERIN use deepest?
+							cc.PointCount = 1;
 						}
 					}
+
+					++count;
 				}
 			}
+
+			Box2DXDebug.Assert(count == _constraintCount);
 		}
 
 		public void Dispose()
@@ -223,47 +236,41 @@ namespace Box2DX.Dynamics
 
 		public void InitVelocityConstraints(TimeStep step)
 		{
-			unsafe
+			// Warm start.
+			for (int i = 0; i < _constraintCount; ++i)
 			{
-				// Warm start.
-				for (int i = 0; i < _constraintCount; ++i)
+				ContactConstraint c = _constraints[i];
+
+				Body b1 = c.Body1;
+				Body b2 = c.Body2;
+				float invMass1 = b1._invMass;
+				float invI1 = b1._invI;
+				float invMass2 = b2._invMass;
+				float invI2 = b2._invI;
+				Vec2 normal = c.Normal;
+				Vec2 tangent = Vec2.Cross(normal, 1.0f);
+
+				if (step.WarmStarting)
 				{
-					ContactConstraint c = _constraints[i];
-
-					Body bodyA = c.BodyA;
-					Body bodyB = c.BodyB;
-					float invMassA = bodyA._invMass;
-					float invIA = bodyA._invI;
-					float invMassB = bodyB._invMass;
-					float invIB = bodyB._invI;
-					Vec2 normal = c.Normal;
-					Vec2 tangent = Vec2.Cross(normal, 1.0f);
-
-					fixed (ContactConstraintPoint* pointsPtr = c.Points)
+					for (int j = 0; j < c.PointCount; ++j)
 					{
-						if (step.WarmStarting)
-						{
-							for (int j = 0; j < c.PointCount; ++j)
-							{
-								ContactConstraintPoint* ccp = &pointsPtr[j];
-								ccp->NormalImpulse *= step.DtRatio;
-								ccp->TangentImpulse *= step.DtRatio;
-								Vec2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent;
-								bodyA._angularVelocity -= invIA * Vec2.Cross(ccp->RA, P);
-								bodyA._linearVelocity -= invMassA * P;
-								bodyB._angularVelocity += invIB * Vec2.Cross(ccp->RB, P);
-								bodyB._linearVelocity += invMassB * P;
-							}
-						}
-						else
-						{
-							for (int j = 0; j < c.PointCount; ++j)
-							{
-								ContactConstraintPoint* ccp = &pointsPtr[j];
-								ccp->NormalImpulse = 0.0f;
-								ccp->TangentImpulse = 0.0f;
-							}
-						}
+						ContactConstraintPoint ccp = c.Points[j];
+						ccp.NormalImpulse *= step.DtRatio;
+						ccp.TangentImpulse *= step.DtRatio;
+						Vec2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent;
+						b1._angularVelocity -= invI1 * Vec2.Cross(ccp.R1, P);
+						b1._linearVelocity -= invMass1 * P;
+						b2._angularVelocity += invI2 * Vec2.Cross(ccp.R2, P);
+						b2._linearVelocity += invMass2 * P;
+					}
+				}
+				else
+				{
+					for (int j = 0; j < c.PointCount; ++j)
+					{
+						ContactConstraintPoint ccp = c.Points[j];
+						ccp.NormalImpulse = 0.0f;
+						ccp.TangentImpulse = 0.0f;
 					}
 				}
 			}
@@ -274,303 +281,297 @@ namespace Box2DX.Dynamics
 			for (int i = 0; i < _constraintCount; ++i)
 			{
 				ContactConstraint c = _constraints[i];
-				Body bodyA = c.BodyA;
-				Body bodyB = c.BodyB;
-				float wA = bodyA._angularVelocity;
-				float wB = bodyB._angularVelocity;
-				Vec2 vA = bodyA._linearVelocity;
-				Vec2 vB = bodyB._linearVelocity;
-				float invMassA = bodyA._invMass;
-				float invIA = bodyA._invI;
-				float invMassB = bodyB._invMass;
-				float invIB = bodyB._invI;
+				Body b1 = c.Body1;
+				Body b2 = c.Body2;
+				float w1 = b1._angularVelocity;
+				float w2 = b2._angularVelocity;
+				Vec2 v1 = b1._linearVelocity;
+				Vec2 v2 = b2._linearVelocity;
+				float invMass1 = b1._invMass;
+				float invI1 = b1._invI;
+				float invMass2 = b2._invMass;
+				float invI2 = b2._invI;
 				Vec2 normal = c.Normal;
 				Vec2 tangent = Vec2.Cross(normal, 1.0f);
 				float friction = c.Friction;
 
 				Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2);
 
-				unsafe
+				// Solve normal constraints
+				if (c.PointCount == 1)
 				{
-					fixed (ContactConstraintPoint* pointsPtr = c.Points)
+					ContactConstraintPoint ccp = c.Points[0];
+
+					// Relative velocity at contact
+					Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);
+
+					// Compute normal impulse
+					float vn = Vec2.Dot(dv, normal);
+					float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);
+
+					// Clamp the accumulated impulse
+					float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
+					lambda = newImpulse - ccp.NormalImpulse;
+
+					// Apply contact impulse
+					Vec2 P = lambda * normal;
+
+					v1 -= invMass1 * P;
+					w1 -= invI1 * Vec2.Cross(ccp.R1, P);
+
+					v2 += invMass2 * P;
+					w2 += invI2 * Vec2.Cross(ccp.R2, P);
+
+					ccp.NormalImpulse = newImpulse;
+				}
+				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 = vn_0 - 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 = x' - a
+					// 
+					// Plug into above equation:
+					//
+					// vn = A * x + b
+					//    = A * (x' - a) + b
+					//    = A * x' + b - A * a
+					//    = A * x' + b'
+					// b' = b - A * a;
+
+					ContactConstraintPoint cp1 = c.Points[0];
+					ContactConstraintPoint cp2 = c.Points[1];
+
+					Vec2 a = new Vec2(cp1.NormalImpulse, cp2.NormalImpulse);
+					Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
+
+					// Relative velocity at contact
+					Vec2 dv1 = v2 + Vec2.Cross(w2, cp1.R2) - v1 - Vec2.Cross(w1, cp1.R1);
+					Vec2 dv2 = v2 + Vec2.Cross(w2, cp2.R2) - v1 - Vec2.Cross(w1, cp2.R1);
+
+					// Compute normal velocity
+					float vn1 = Vec2.Dot(dv1, normal);
+					float vn2 = Vec2.Dot(dv2, normal);
+
+					Vec2 b;
+					b.X = vn1 - cp1.VelocityBias;
+					b.Y = vn2 - cp2.VelocityBias;
+					b -= Common.Math.Mul(c.K, a);
+
+					const float k_errorTol = 1e-3f;
+					for (; ; )
 					{
-						// Solve tangent constraints
-						for (int j = 0; j < c.PointCount; ++j)
+						//
+						// Case 1: vn = 0
+						//
+						// 0 = A * x' + b'
+						//
+						// Solve for x':
+						//
+						// x' = - inv(A) * b'
+						//
+						Vec2 x = -Common.Math.Mul(c.NormalMass, b);
+
+						if (x.X >= 0.0f && x.Y >= 0.0f)
 						{
-							ContactConstraintPoint* ccp = &pointsPtr[j];
-
-							// Relative velocity at contact
-							Vec2 dv = vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA);
+							// Resubstitute for the incremental impulse
+							Vec2 d = x - a;
 
-							// Compute tangent force
-							float vt = Vec2.Dot(dv, tangent);
-							float lambda = ccp->TangentMass * (-vt);
+							// Apply incremental impulse
+							Vec2 P1 = d.X * normal;
+							Vec2 P2 = d.Y * normal;
+							v1 -= invMass1 * (P1 + P2);
+							w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
 
-							// b2Clamp the accumulated force
-							float maxFriction = friction * ccp->NormalImpulse;
-							float newImpulse = Common.Math.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction);
-							lambda = newImpulse - ccp->TangentImpulse;
+							v2 += invMass2 * (P1 + P2);
+							w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
 
-							// Apply contact impulse
-							Vec2 P = lambda * tangent;
+							// Accumulate
+							cp1.NormalImpulse = x.X;
+							cp2.NormalImpulse = x.Y;
 
-							vA -= invMassA * P;
-							wA -= invIA * Vec2.Cross(ccp->RA, P);
+#if B2_DEBUG_SOLVER
+							// Postconditions
+							dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);
+							dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);
 
-							vB += invMassB * P;
-							wB += invIB * Vec2.Cross(ccp->RB, P);
+							// Compute normal velocity
+							vn1 = Vector2.Dot(dv1, normal);
+							vn2 = Vector2.Dot(dv2, normal);
 
-							ccp->TangentImpulse = newImpulse;
+							Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
+							Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
+#endif
+							break;
 						}
 
-						// Solve normal constraints
-						if (c.PointCount == 1)
+						//
+						// 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 = c.K.Col1.Y * x.X + b.Y;
+
+						if (x.X >= 0.0f && vn2 >= 0.0f)
 						{
-							ContactConstraintPoint ccp = c.Points[0];
+							// Resubstitute for the incremental impulse
+							Vec2 d = x - a;
 
-							// Relative velocity at contact
-							Vec2 dv = vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA);
+							// Apply incremental impulse
+							Vec2 P1 = d.X * normal;
+							Vec2 P2 = d.Y * normal;
+							v1 -= invMass1 * (P1 + P2);
+							w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
 
-							// Compute normal impulse
-							float vn = Vec2.Dot(dv, normal);
-							float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);
+							v2 += invMass2 * (P1 + P2);
+							w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
 
-							// Clamp the accumulated impulse
-							float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
-							lambda = newImpulse - ccp.NormalImpulse;
+							// Accumulate
+							cp1.NormalImpulse = x.X;
+							cp2.NormalImpulse = x.Y;
 
-							// Apply contact impulse
-							Vec2 P = lambda * normal;
-							vA -= invMassA * P;
-							wA -= invIA * Vec2.Cross(ccp.RA, P);
+#if B2_DEBUG_SOLVER
+							// Postconditions
+							dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);
 
-							vB += invMassB * P;
-							wB += invIB * Vec2.Cross(ccp.RB, P);
-							ccp.NormalImpulse = newImpulse;
+							// Compute normal velocity
+							vn1 = Vector2.Dot(dv1, normal);
+
+							Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
+#endif
+							break;
 						}
-						else
+
+
+						//
+						// Case 3: w2 = 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 = c.K.Col2.X * x.Y + b.X;
+						vn2 = 0.0f;
+
+						if (x.Y >= 0.0f && vn1 >= 0.0f)
 						{
-							// 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 = vn_0 - 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 = x' - a
-							// 
-							// Plug into above equation:
-							//
-							// vn = A * x + b
-							//    = A * (x' - a) + b
-							//    = A * x' + b - A * a
-							//    = A * x' + b'
-							// b' = b - A * a;
-
-							ContactConstraintPoint* cp1 = &pointsPtr[0];
-							ContactConstraintPoint* cp2 = &pointsPtr[1];
-
-							Vec2 a = new Vec2(cp1->NormalImpulse, cp2->NormalImpulse);
-							Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
-
-							// Relative velocity at contact
-							Vec2 dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
-							Vec2 dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);
+							// Resubstitute for the incremental impulse
+							Vec2 d = x - a;
 
-							// Compute normal velocity
-							float vn1 = Vec2.Dot(dv1, normal);
-							float vn2 = Vec2.Dot(dv2, normal);
+							// Apply incremental impulse
+							Vec2 P1 = d.X * normal;
+							Vec2 P2 = d.Y * normal;
+							v1 -= invMass1 * (P1 + P2);
+							w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
 
-							Vec2 b;
-							b.X = vn1 - cp1->VelocityBias;
-							b.Y = vn2 - cp2->VelocityBias;
-							b -= Common.Math.Mul(c.K, a);
+							v2 += invMass2 * (P1 + P2);
+							w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
 
-							const float k_errorTol = 1e-3f;
-							//B2_NOT_USED(k_errorTol);
+							// Accumulate
+							cp1.NormalImpulse = x.X;
+							cp2.NormalImpulse = x.Y;
 
-							for (; ; )
-							{
-								//
-								// Case 1: vn = 0
-								//
-								// 0 = A * x' + b'
-								//
-								// Solve for x':
-								//
-								// x' = - inv(A) * b'
-								//
-								Vec2 x = -Common.Math.Mul(c.NormalMass, b);
-
-								if (x.X >= 0.0f && x.Y >= 0.0f)
-								{
-									// Resubstitute for the incremental impulse
-									Vec2 d = x - a;
-
-									// Apply incremental impulse
-									Vec2 P1 = d.X * normal;
-									Vec2 P2 = d.Y * normal;
-									vA -= invMassA * (P1 + P2);
-									wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2));
-
-									vB += invMassB * (P1 + P2);
-									wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2));
-
-									// Accumulate
-									cp1->NormalImpulse = x.X;
-									cp2->NormalImpulse = x.Y;
-
-#if DEBUG_SOLVER
-									// Postconditions
-									dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
-									dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);
-
-									// Compute normal velocity
-									vn1 = Vec2.Dot(dv1, normal);
-									vn2 = Vec2.Dot(dv2, normal);
-
-									Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
-									Box2DXDebug.Assert(Common.Math.Abs(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 = c.K.Col1.Y * x.X + b.Y;
-
-								if (x.X >= 0.0f && vn2 >= 0.0f)
-								{
-									// Resubstitute for the incremental impulse
-									Vec2 d = x - a;
-
-									// Apply incremental impulse
-									Vec2 P1 = d.X * normal;
-									Vec2 P2 = d.Y * normal;
-									vA -= invMassA * (P1 + P2);
-									wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2));
-
-									vB += invMassB * (P1 + P2);
-									wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2));
-
-									// Accumulate
-									cp1->NormalImpulse = x.X;
-									cp2->NormalImpulse = x.Y;
-
-#if DEBUG_SOLVER
-									// Postconditions
-									dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
-
-									// Compute normal velocity
-									vn1 = Vec2.Dot(dv1, normal);
-
-									Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
-#endif
-									break;
-								}
-
-
-								//
-								// Case 3: w2 = 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 = c.K.Col2.X * x.Y + b.X;
-								vn2 = 0.0f;
-
-								if (x.Y >= 0.0f && vn1 >= 0.0f)
-								{
-									// Resubstitute for the incremental impulse
-									Vec2 d = x - a;
-
-									// Apply incremental impulse
-									Vec2 P1 = d.X * normal;
-									Vec2 P2 = d.Y * normal;
-									vA -= invMassA * (P1 + P2);
-									wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2));
-
-									vB += invMassB * (P1 + P2);
-									wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2));
-
-									// Accumulate
-									cp1->NormalImpulse = x.X;
-									cp2->NormalImpulse = x.Y;
-
-#if DEBUG_SOLVER
-									// Postconditions
-									dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);
-
-									// Compute normal velocity
-									vn2 = Vec2.Dot(dv2, normal);
-
-									Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
+#if B2_DEBUG_SOLVER
+							// Postconditions
+							dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);
+
+							// Compute normal velocity
+							vn2 = Vector2.Dot(dv2, normal);
+
+							Box2DXDebug.Assert(Common.Math.Abs(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
-									Vec2 d = x - a;
-
-									// Apply incremental impulse
-									Vec2 P1 = d.X * normal;
-									Vec2 P2 = d.Y * normal;
-									vA -= invMassA * (P1 + P2);
-									wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2));
-
-									vB += invMassB * (P1 + P2);
-									wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(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;
-							}
+							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
+							Vec2 d = x - a;
+
+							// Apply incremental impulse
+							Vec2 P1 = d.X * normal;
+							Vec2 P2 = d.Y * normal;
+							v1 -= invMass1 * (P1 + P2);
+							w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
+
+							v2 += invMass2 * (P1 + P2);
+							w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
+
+							// Accumulate
+							cp1.NormalImpulse = x.X;
+							cp2.NormalImpulse = x.Y;
+
+							break;
 						}
 
-						bodyA._linearVelocity = vA;
-						bodyA._angularVelocity = wA;
-						bodyB._linearVelocity = vB;
-						bodyB._angularVelocity = wB;
+						// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
+						break;
 					}
 				}
+
+				// Solve tangent constraints
+				for (int j = 0; j < c.PointCount; ++j)
+				{
+					ContactConstraintPoint ccp = c.Points[j];
+
+					// Relative velocity at contact
+					Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);
+
+					// Compute tangent force
+					float vt = Vec2.Dot(dv, tangent);
+					float lambda = ccp.TangentMass * (-vt);
+
+					// Clamp the accumulated force
+					float maxFriction = friction * ccp.NormalImpulse;
+					float newImpulse = Common.Math.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction);
+					lambda = newImpulse - ccp.TangentImpulse;
+
+					// Apply contact impulse
+					Vec2 P = lambda * tangent;
+
+					v1 -= invMass1 * P;
+					w1 -= invI1 * Vec2.Cross(ccp.R1, P);
+
+					v2 += invMass2 * P;
+					w2 += invI2 * Vec2.Cross(ccp.R2, P);
+
+					ccp.TangentImpulse = newImpulse;
+				}
+
+				b1._linearVelocity = v1;
+				b1._angularVelocity = w1;
+				b2._linearVelocity = v2;
+				b2._angularVelocity = w2;
 			}
 		}
 
@@ -589,73 +590,7 @@ namespace Box2DX.Dynamics
 			}
 		}
 
-		internal class PositionSolverManifold
-		{
-			internal Vec2 Normal;
-			internal Vec2[] Points = new Vec2[Settings.MaxManifoldPoints];
-			internal float[] Separations = new float[Settings.MaxManifoldPoints];
-
-			internal void Initialize(ContactConstraint cc)
-			{
-				Box2DXDebug.Assert(cc.PointCount > 0);
-
-				switch (cc.Type)
-				{
-					case ManifoldType.Circles:
-						{
-							Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint);
-							Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint);
-							if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON_SQUARED)
-							{
-								Normal = pointB - pointA;
-								Normal.Normalize();
-							}
-							else
-							{
-								Normal.Set(1.0f, 0.0f);
-							}
-
-							Points[0] = 0.5f * (pointA + pointB);
-							Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius;
-						}
-						break;
-
-					case ManifoldType.FaceA:
-						{
-							Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal);
-							Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint);
-
-							for (int i = 0; i < cc.PointCount; ++i)
-							{
-								Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint);
-								Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
-								Points[i] = clipPoint;
-							}
-						}
-						break;
-
-					case ManifoldType.FaceB:
-						{
-							Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal);
-							Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint);
-
-							for (int i = 0; i < cc.PointCount; ++i)
-							{
-								Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint);
-								Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
-								Points[i] = clipPoint;
-							}
-
-							// Ensure normal points from A to B
-							Normal = -Normal;
-						}
-						break;
-				}
-			}
-		}
-
-		private static PositionSolverManifold s_PositionSolverManifold = new PositionSolverManifold();
-
+#if FIRST
 		public bool SolvePositionConstraints(float baumgarte)
 		{
 			float minSeparation = 0.0f;
@@ -663,25 +598,29 @@ namespace Box2DX.Dynamics
 			for (int i = 0; i < _constraintCount; ++i)
 			{
 				ContactConstraint c = _constraints[i];
-				Body bodyA = c.BodyA;
-				Body bodyB = c.BodyB;
-
-				float invMassA = bodyA._mass * bodyA._invMass;
-				float invIA = bodyA._mass * bodyA._invI;
-				float invMassB = bodyB._mass * bodyB._invMass;
-				float invIB = bodyB._mass * bodyB._invI;
+				Body b1 = c.Body1;
+				Body b2 = c.Body2;
+				float invMass1 = b1._mass * b1._invMass;
+				float invI1 = b1._mass * b1._invI;
+				float invMass2 = b2._mass * b2._invMass;
+				float invI2 = b2._mass * b2._invI;
 
-				s_PositionSolverManifold.Initialize(c);
-				Vec2 normal = s_PositionSolverManifold.Normal;
+				Vec2 normal = c.Normal;
 
 				// Solver normal constraints
 				for (int j = 0; j < c.PointCount; ++j)
 				{
-					Vec2 point = s_PositionSolverManifold.Points[j];
-					float separation = s_PositionSolverManifold.Separations[j];
+					ContactConstraintPoint ccp = c.Points[j];
+
+					Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, ccp.LocalAnchor1 - b1.GetLocalCenter());
+					Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, ccp.LocalAnchor2 - b2.GetLocalCenter());
 
-					Vec2 rA = point - bodyA._sweep.C;
-					Vec2 rB = point - bodyB._sweep.C;
+					Vec2 p1 = b1._sweep.C + r1;
+					Vec2 p2 = b2._sweep.C + r2;
+					Vec2 dp = p2 - p1;
+
+					// Approximate the current separation.
+					float separation = Vec2.Dot(dp, normal) + ccp.Separation;
 
 					// Track max constraint error.
 					minSeparation = Common.Math.Min(minSeparation, separation);
@@ -690,17 +629,17 @@ namespace Box2DX.Dynamics
 					float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
 
 					// Compute normal impulse
-					float impulse = -c.Points[j].EqualizedMass * C;
+					float impulse = -ccp.EqualizedMass * C;
 
 					Vec2 P = impulse * normal;
 
-					bodyA._sweep.C -= invMassA * P;
-					bodyA._sweep.A -= invIA * Vec2.Cross(rA, P);
-					bodyA.SynchronizeTransform();
+					b1._sweep.C -= invMass1 * P;
+					b1._sweep.A -= invI1 * Vec2.Cross(r1, P);
+					b1.SynchronizeTransform();
 
-					bodyB._sweep.C += invMassB * P;
-					bodyB._sweep.A += invIB * Vec2.Cross(rB, P);
-					bodyB.SynchronizeTransform();
+					b2._sweep.C += invMass2 * P;
+					b2._sweep.A += invI2 * Vec2.Cross(r2, P);
+					b2.SynchronizeTransform();
 				}
 			}
 
@@ -709,4 +648,6 @@ namespace Box2DX.Dynamics
 			return minSeparation >= -1.5f * Settings.LinearSlop;
 		}
 	}
+#else
+#endif
 }
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs
deleted file mode 100644
index 999154c..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
-
-  This software is provided 'as-is', without any express or implied
-  warranty.  In no event will the authors be held liable for any damages
-  arising from the use of this software.
-
-  Permission is granted to anyone to use this software for any purpose,
-  including commercial applications, and to alter it and redistribute it
-  freely, subject to the following restrictions:
-
-  1. The origin of this software must not be misrepresented; you must not
-     claim that you wrote the original software. If you use this software
-     in a product, an acknowledgment in the product documentation would be
-     appreciated but is not required.
-  2. Altered source versions must be plainly marked as such, and must not be
-     misrepresented as being the original software.
-  3. This notice may not be removed or altered from any source distribution.
-*/
-
-using Box2DX.Collision;
-using Box2DX.Common;
-
-namespace Box2DX.Dynamics
-{
-	public class EdgeAndCircleContact : Contact
-	{
-		public EdgeAndCircleContact(Fixture fixtureA, Fixture fixtureB)
-			: base(fixtureA, fixtureB)
-		{
-			Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.EdgeShape);
-			Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape);
-			_manifold.PointCount = 0;
-			_manifold.Points[0].NormalImpulse = 0.0f;
-			_manifold.Points[0].TangentImpulse = 0.0f;
-			CollideShapeFunction = CollideEdgeAndCircle;
-		}
-
-		private static void CollideEdgeAndCircle(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
-		{
-			Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)shape1, xf1, (CircleShape)shape2, xf2);
-		}
-
-		new public static Contact Create(Fixture fixtureA, Fixture fixtureB)
-		{
-			return new EdgeAndCircleContact(fixtureA, fixtureB);
-		}
-
-		new public static void Destroy(ref Contact contact)
-		{
-			contact = null;
-		}
-	}
-}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta
deleted file mode 100644
index 442c45d..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/EdgeAndCircleContact.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: 576caff6a1c4ee846aa596cfb70c8a29
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs
index 31c7bb1..2012997 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/NullContact.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,6 +19,10 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Collision;
 using Box2DX.Common;
 
@@ -26,10 +30,8 @@ namespace Box2DX.Dynamics
 {
 	public class NullContact : Contact
 	{
-		public NullContact()
-		{
-			CollideShapeFunction = Collide;
-		}
-		private static void Collide(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) { }
+		public NullContact() { }
+		public override void Evaluate(ContactListener listener) { }
+		public override Manifold[] GetManifolds() { return null; }
 	}
 }
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs
index 4a63bf8..4166623 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndCircleContact.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,6 +19,10 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Collision;
 using Box2DX.Common;
 
@@ -26,25 +30,139 @@ namespace Box2DX.Dynamics
 {
 	public class PolyAndCircleContact : Contact
 	{
-		public PolyAndCircleContact(Fixture fixtureA, Fixture fixtureB)
-			: base(fixtureA, fixtureB)
+		public Manifold _manifold = new Manifold();
+
+		public override Manifold[] GetManifolds()
+		{
+			return new Manifold[] { _manifold };
+		}
+
+		public PolyAndCircleContact(Shape s1, Shape s2)
+			: base(s1, s2)
 		{
-			Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape);
-			Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape);
-			CollideShapeFunction = CollidePolygonCircle;
+			Box2DXDebug.Assert(_shape1.GetType() == ShapeType.PolygonShape);
+			Box2DXDebug.Assert(_shape2.GetType() == ShapeType.CircleShape);
+			_manifold.PointCount = 0;
 		}
 
-		private static void CollidePolygonCircle(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
+		public override void Evaluate(ContactListener listener)
 		{
-			Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)shape1, xf1, (CircleShape)shape2, xf2);
+			Body b1 = _shape1.GetBody();
+			Body b2 = _shape2.GetBody();
+#warning "needfix"
+			//memcpy(&m0, &m_manifold, sizeof(b2Manifold));
+			Manifold m0 = _manifold.Clone();
+
+			Collision.Collision.CollidePolygonAndCircle(ref _manifold, (PolygonShape)_shape1, b1.GetXForm(),
+				(CircleShape)_shape2, b2.GetXForm());
+
+			bool[] persisted = new bool[] { false, false };
+
+			ContactPoint cp = new ContactPoint();
+			cp.Shape1 = _shape1;
+			cp.Shape2 = _shape2;
+			cp.Friction = Settings.MixFriction(_shape1.Friction, _shape2.Friction);
+			cp.Restitution = Settings.MixRestitution(_shape1.Restitution, _shape2.Restitution);
+
+			// Match contact ids to facilitate warm starting.
+			if (_manifold.PointCount > 0)
+			{
+				// Match old contact ids to new contact ids and copy the
+				// stored impulses to warm start the solver.
+				for (int i = 0; i < _manifold.PointCount; ++i)
+				{
+					ManifoldPoint mp = _manifold.Points[i];
+					mp.NormalImpulse = 0.0f;
+					mp.TangentImpulse = 0.0f;
+					bool found = false;
+					ContactID id = mp.ID;
+
+					for (int j = 0; j < m0.PointCount; ++j)
+					{
+						if (persisted[j] == true)
+						{
+							continue;
+						}
+
+						ManifoldPoint mp0 = m0.Points[j];
+
+						if (mp0.ID.Key == id.Key)
+						{
+							persisted[j] = true;
+							mp.NormalImpulse = mp0.NormalImpulse;
+							mp.TangentImpulse = mp0.TangentImpulse;
+
+							// A persistent point.
+							found = true;
+
+							// Report persistent point.
+							if (listener != null)
+							{
+								cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
+								Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
+								Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
+								cp.Velocity = v2 - v1;
+								cp.Normal = _manifold.Normal;
+								cp.Separation = mp.Separation;
+								cp.ID = id;
+								listener.Persist(cp);
+							}
+							break;
+						}
+					}
+
+					// Report added point.
+					if (found == false && listener != null)
+					{
+						cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
+						Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
+						Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
+						cp.Velocity = v2 - v1;
+						cp.Normal = _manifold.Normal;
+						cp.Separation = mp.Separation;
+						cp.ID = id;
+						listener.Add(cp);
+					}
+				}
+
+				_manifoldCount = 1;
+			}
+			else
+			{
+				_manifoldCount = 0;
+			}
+
+			if (listener == null)
+			{
+				return;
+			}
+
+			// Report removed points.
+			for (int i = 0; i < m0.PointCount; ++i)
+			{
+				if (persisted[i])
+				{
+					continue;
+				}
+
+				ManifoldPoint mp0 = m0.Points[i];
+				cp.Position = b1.GetWorldPoint(mp0.LocalPoint1);
+				Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1);
+				Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2);
+				cp.Velocity = v2 - v1;
+				cp.Normal = m0.Normal;
+				cp.Separation = mp0.Separation;
+				cp.ID = mp0.ID;
+				listener.Remove(cp);
+			}
 		}
 
-		new public static Contact Create(Fixture fixtureA, Fixture fixtureB)
+		new public static Contact Create(Shape shape1, Shape shape2)
 		{
-			return new PolyAndCircleContact(fixtureA, fixtureB);
+			return new PolyAndCircleContact(shape1, shape2);
 		}
 
-		new public static void Destroy(ref Contact contact)
+		new public static void Destroy(Contact contact)
 		{
 			contact = null;
 		}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs
deleted file mode 100644
index a50553f..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
-
-  This software is provided 'as-is', without any express or implied
-  warranty.  In no event will the authors be held liable for any damages
-  arising from the use of this software.
-
-  Permission is granted to anyone to use this software for any purpose,
-  including commercial applications, and to alter it and redistribute it
-  freely, subject to the following restrictions:
-
-  1. The origin of this software must not be misrepresented; you must not
-     claim that you wrote the original software. If you use this software
-     in a product, an acknowledgment in the product documentation would be
-     appreciated but is not required.
-  2. Altered source versions must be plainly marked as such, and must not be
-     misrepresented as being the original software.
-  3. This notice may not be removed or altered from any source distribution.
-*/
-
-using Box2DX.Collision;
-using Box2DX.Common;
-
-namespace Box2DX.Dynamics
-{
-	public class PolyAndEdgeContact : Contact
-	{
-		public PolyAndEdgeContact(Fixture fixtureA, Fixture fixtureB)
-			: base(fixtureA, fixtureB)
-		{
-			Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape);
-			Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.EdgeShape);
-			CollideShapeFunction = CollidePolyAndEdgeContact;
-		}
-
-		private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
-		{
-			Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2);
-		}
-
-		new public static Contact Create(Fixture fixtureA, Fixture fixtureB)
-		{
-			return new PolyAndEdgeContact(fixtureA, fixtureB);
-		}
-
-		new public static void Destroy(ref Contact contact)
-		{
-			contact = null;
-		}
-	}
-}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta
deleted file mode 100644
index 5f83e8a..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyAndEdgeContact.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: c42cbf67f18e2414192739161ff94e89
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs
index 5b101fb..ebf24cf 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Contacts/PolyContact.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -19,6 +19,10 @@
   3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
+using System.Collections.Generic;
+using System.Text;
+
 using Box2DX.Collision;
 using Box2DX.Common;
 
@@ -26,25 +30,139 @@ namespace Box2DX.Dynamics
 {
 	public class PolygonContact : Contact
 	{
-		public PolygonContact(Fixture fixtureA, Fixture fixtureB)
-			: base(fixtureA, fixtureB)
+		public Manifold _manifold = new Manifold();
+
+		public override Manifold[] GetManifolds()
+		{
+			return new Manifold[] { _manifold };
+		}
+
+		public PolygonContact(Shape s1, Shape s2)
+			: base(s1, s2)
 		{
-			Box2DXDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape);
-			Box2DXDebug.Assert(fixtureB.ShapeType == ShapeType.PolygonShape);
-			CollideShapeFunction = CollidePolygons;
+			Box2DXDebug.Assert(_shape1.GetType() == ShapeType.PolygonShape);
+			Box2DXDebug.Assert(_shape2.GetType() == ShapeType.PolygonShape);
+			_manifold.PointCount = 0;
 		}
 
-		private static void CollidePolygons(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
+		public override void Evaluate(ContactListener listener)
 		{
-			Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2);
+			Body b1 = _shape1.GetBody();
+			Body b2 = _shape2.GetBody();
+#warning "needfix"
+			//memcpy(&m0, &m_manifold, sizeof(b2Manifold));
+			Manifold m0 = _manifold.Clone();
+
+			Collision.Collision.CollidePolygons(ref _manifold, (PolygonShape)_shape1, b1.GetXForm(),
+				(PolygonShape)_shape2, b2.GetXForm());
+
+			bool[] persisted = new bool[] { false, false };
+
+			ContactPoint cp = new ContactPoint();
+			cp.Shape1 = _shape1;
+			cp.Shape2 = _shape2;
+			cp.Friction = Settings.MixFriction(_shape1.Friction, _shape2.Friction);
+			cp.Restitution = Settings.MixRestitution(_shape1.Restitution, _shape2.Restitution);
+
+			// Match contact ids to facilitate warm starting.
+			if (_manifold.PointCount > 0)
+			{
+				// Match old contact ids to new contact ids and copy the
+				// stored impulses to warm start the solver.
+				for (int i = 0; i < _manifold.PointCount; ++i)
+				{
+					ManifoldPoint mp = _manifold.Points[i];
+					mp.NormalImpulse = 0.0f;
+					mp.TangentImpulse = 0.0f;
+					bool found = false;
+					ContactID id = mp.ID;
+
+					for (int j = 0; j < m0.PointCount; ++j)
+					{
+						if (persisted[j] == true)
+						{
+							continue;
+						}
+
+						ManifoldPoint mp0 = m0.Points[j];
+
+						if (mp0.ID.Key == id.Key)
+						{
+							persisted[j] = true;
+							mp.NormalImpulse = mp0.NormalImpulse;
+							mp.TangentImpulse = mp0.TangentImpulse;
+
+							// A persistent point.
+							found = true;
+
+							// Report persistent point.
+							if (listener != null)
+							{
+								cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
+								Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
+								Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
+								cp.Velocity = v2 - v1;
+								cp.Normal = _manifold.Normal;
+								cp.Separation = mp.Separation;
+								cp.ID = id;
+								listener.Persist(cp);
+							}
+							break;
+						}
+					}
+
+					// Report added point.
+					if (found == false && listener != null)
+					{
+						cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
+						Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
+						Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
+						cp.Velocity = v2 - v1;
+						cp.Normal = _manifold.Normal;
+						cp.Separation = mp.Separation;
+						cp.ID = id;
+						listener.Add(cp);
+					}
+				}
+
+				_manifoldCount = 1;
+			}
+			else
+			{
+				_manifoldCount = 0;
+			}
+
+			if (listener == null)
+			{
+				return;
+			}
+
+			// Report removed points.
+			for (int i = 0; i < m0.PointCount; ++i)
+			{
+				if (persisted[i])
+				{
+					continue;
+				}
+
+				ManifoldPoint mp0 = m0.Points[i];
+				cp.Position = b1.GetWorldPoint(mp0.LocalPoint1);
+				Vec2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1);
+				Vec2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2);
+				cp.Velocity = v2 - v1;
+				cp.Normal = m0.Normal;
+				cp.Separation = mp0.Separation;
+				cp.ID = mp0.ID;
+				listener.Remove(cp);
+			}
 		}
 
-		new public static Contact Create(Fixture fixtureA, Fixture fixtureB)
+		new public static Contact Create(Shape shape1, Shape shape2)
 		{
-			return new PolygonContact(fixtureA, fixtureB);
+			return new PolygonContact(shape1, shape2);
 		}
 
-		new public static void Destroy(ref Contact contact)
+		new public static void Destroy(Contact contact)
 		{
 			contact = null;
 		}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta
deleted file mode 100644
index 446ed50..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers.meta
+++ /dev/null
@@ -1,8 +0,0 @@
-fileFormatVersion: 2
-guid: 960c2a03774a86d4986f403eb182daaa
-folderAsset: yes
-DefaultImporter:
-  externalObjects: {}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs
deleted file mode 100644
index dcdb244..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs
+++ /dev/null
@@ -1,156 +0,0 @@
-using Box2DX.Common;
-
-namespace Box2DX.Dynamics.Controllers
-{
-    /// <summary>
-    /// This class is used to build buoyancy controllers
-    /// </summary>
-    public class BuoyancyControllerDef
-    {
-        /// The outer surface normal
-        public Vec2 Normal;
-        /// The height of the fluid surface along the normal
-        public float Offset;
-        /// The fluid density
-        public float Density;
-        /// Fluid velocity, for drag calculations
-        public Vec2 Velocity;
-        /// Linear drag co-efficient
-        public float LinearDrag;
-        /// Linear drag co-efficient
-        public float AngularDrag;
-        /// If false, bodies are assumed to be uniformly dense, otherwise use the shapes densities
-        public bool UseDensity; //False by default to prevent a gotcha
-        /// If true, gravity is taken from the world instead of the gravity parameter.
-        public bool UseWorldGravity;
-        /// Gravity vector, if the world's gravity is not used
-        public Vec2 Gravity;
-
-        public BuoyancyControllerDef()
-        {
-            Normal = new Vec2(0, 1);
-            Offset = 0;
-            Density = 0;
-            Velocity = new Vec2(0, 0);
-            LinearDrag = 0;
-            AngularDrag = 0;
-            UseDensity = false;
-            UseWorldGravity = true;
-            Gravity = new Vec2(0, 0);
-        }
-    }
-
-    /// <summary>
-    /// Calculates buoyancy forces for fluids in the form of a half plane.
-    /// </summary>
-    public class BuoyancyController : Controller
-    {
-        /// The outer surface normal
-        public Vec2 Normal;
-        /// The height of the fluid surface along the normal
-        public float Offset;
-        /// The fluid density
-        public float Density;
-        /// Fluid velocity, for drag calculations
-        public Vec2 Velocity;
-        /// Linear drag co-efficient
-        public float LinearDrag;
-        /// Linear drag co-efficient
-        public float AngularDrag;
-        /// If false, bodies are assumed to be uniformly dense, otherwise use the shapes densities
-        public bool UseDensity; //False by default to prevent a gotcha
-        /// If true, gravity is taken from the world instead of the gravity parameter.
-        public bool UseWorldGravity;
-        /// Gravity vector, if the world's gravity is not used
-        public Vec2 Gravity;
-
-        public BuoyancyController(BuoyancyControllerDef buoyancyControllerDef)
-        {
-            Normal = buoyancyControllerDef.Normal;
-            Offset = buoyancyControllerDef.Offset;
-            Density = buoyancyControllerDef.Density;
-            Velocity = buoyancyControllerDef.Velocity;
-            LinearDrag = buoyancyControllerDef.LinearDrag;
-            AngularDrag = buoyancyControllerDef.AngularDrag;
-            UseDensity = buoyancyControllerDef.UseDensity;
-            UseWorldGravity = buoyancyControllerDef.UseWorldGravity;
-            Gravity = buoyancyControllerDef.Gravity;
-        }
-
-        public override void Step(TimeStep step)
-        {
-            //B2_NOT_USED(step);
-            if (_bodyList == null)
-                return;
-
-            if (UseWorldGravity)
-            {
-                Gravity = _world.Gravity;
-            }
-            for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
-            {
-                Body body = i.body;
-                if (body.IsSleeping())
-                {
-                    //Buoyancy force is just a function of position,
-                    //so unlike most forces, it is safe to ignore sleeping bodes
-                    continue;
-                }
-                Vec2 areac = new Vec2(0, 0);
-                Vec2 massc = new Vec2(0, 0);
-                float area = 0;
-                float mass = 0;
-                for (Fixture shape = body.GetFixtureList(); shape != null; shape = shape.Next)
-                {
-                    Vec2 sc;
-                    float sarea = shape.ComputeSubmergedArea(Normal, Offset, out sc);
-                    area += sarea;
-                    areac.X += sarea * sc.X;
-                    areac.Y += sarea * sc.Y;
-                    float shapeDensity = 0;
-                    if (UseDensity)
-                    {
-                        //TODO: Expose density publicly
-                        shapeDensity = shape.Density;
-                    }
-                    else
-                    {
-                        shapeDensity = 1;
-                    }
-                    mass += sarea * shapeDensity;
-                    massc.X += sarea * sc.X * shapeDensity;
-                    massc.Y += sarea * sc.Y * shapeDensity;
-                }
-                areac.X /= area;
-                areac.Y /= area;
-                //Vec2 localCentroid = Math.MulT(body.GetXForm(), areac);
-                massc.X /= mass;
-                massc.Y /= mass;
-                if (area < Settings.FLT_EPSILON)
-                    continue;
-                //Buoyancy
-                Vec2 buoyancyForce = -Density * area * Gravity;
-                body.ApplyForce(buoyancyForce, massc);
-                //Linear drag
-                Vec2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity;
-                dragForce *= -LinearDrag * area;
-                body.ApplyForce(dragForce, areac);
-                //Angular drag
-                //TODO: Something that makes more physical sense?
-                body.ApplyTorque(-body.GetInertia() / body.GetMass() * area * body.GetAngularVelocity() * AngularDrag);
-            
-            }
-        }
-
-        public override void Draw(DebugDraw debugDraw)
-        {
-            float r = 1000;
-            Vec2 p1 = Offset * Normal + Vec2.Cross(Normal, r);
-            Vec2 p2 = Offset * Normal - Vec2.Cross(Normal, r);
-
-            Color color = new Color(0, 0, 0.8f);
-
-            debugDraw.DrawSegment(p1, p2, color);
-        }
-    }
-}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta
deleted file mode 100644
index 7785241..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/BuoyancyController.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: 41543fe534c548f4b817e2ae3fab50e5
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs
deleted file mode 100644
index 421ca04..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-using Box2DX.Common;
-
-namespace Box2DX.Dynamics.Controllers
-{
-
-    /// This class is used to build constant acceleration controllers
-    public class ConstantAccelControllerDef
-    {
-        /// <summary>
-        /// The force to apply
-        /// </summary>
-        public Vec2 A;
-    }
-
-    public class ConstantAccelController : Controller
-    {
-        /// <summary>
-        /// The force to apply
-        /// </summary>
-        public Vec2 A;
-
-        public ConstantAccelController(ConstantAccelControllerDef def)
-        {
-            A = def.A;
-        }
-
-        public override void Step(TimeStep step)
-        {
-            for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
-            {
-                Body body = i.body;
-                if (body.IsSleeping())
-                    continue;
-                body.SetLinearVelocity(body.GetLinearVelocity() + step.Dt * A);
-            }
-        }
-    }
-}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta
deleted file mode 100644
index 80e33fc..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantAccelController.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: 9c5b0816bc173a74f8f8534af62524d4
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs
deleted file mode 100644
index 8ef9a57..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-using Box2DX.Common;
-
-namespace Box2DX.Dynamics.Controllers
-{
-
-    /// <summary>
-    /// This class is used to build constant force controllers
-    /// </summary>
-    public class ConstantForceControllerDef
-    {
-        /// The force to apply
-        public Vec2 F;
-    }
-
-    public class ConstantForceController : Controller
-    {
-        /// <summary>
-        /// The force to apply
-        /// </summary>
-        Vec2 F;
-
-        public ConstantForceController(ConstantForceControllerDef def)
-        {
-            F = def.F;
-        }
-
-        public override void Step(TimeStep step)
-        {
-            //B2_NOT_USED(step);
-            for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
-            {
-                Body body = i.body;
-                if (body.IsSleeping())
-                    continue;
-                body.ApplyForce(F, body.GetWorldCenter());
-            }
-        }
-    }
-}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta
deleted file mode 100644
index fb47c5a..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/ConstantForceController.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: aa5d8d9cf6eba0b46bd1484725fb9423
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs
deleted file mode 100644
index 26bfbdf..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs
+++ /dev/null
@@ -1,174 +0,0 @@
-using System;
-
-namespace Box2DX.Dynamics.Controllers
-{
-    /// <summary>
-    /// A controller edge is used to connect bodies and controllers together
-    /// in a bipartite graph.
-    /// </summary>
-    public class ControllerEdge
-    {
-        public Controller controller;		// provides quick access to other end of this edge.
-        public Body body;					// the body
-        public ControllerEdge prevBody;		// the previous controller edge in the controllers's joint list
-        public ControllerEdge nextBody;		// the next controller edge in the controllers's joint list
-        public ControllerEdge prevController;		// the previous controller edge in the body's joint list
-        public ControllerEdge nextController;		// the next controller edge in the body's joint list
-    }
-
-    /// <summary>
-    /// Base class for controllers. Controllers are a convience for encapsulating common
-    /// per-step functionality.
-    /// </summary>
-    public abstract class Controller : IDisposable
-    {
-        internal Controller _prev;
-        internal Controller _next;
-
-        internal World _world;
-        protected ControllerEdge _bodyList;
-        protected int _bodyCount;
-
-        public Controller()
-        {
-            _bodyList = null;
-            _bodyCount = 0;
-        }
-
-        public Controller(World world)
-        {
-            _bodyList = null;
-            _bodyCount = 0;
-
-            _world = world;
-        }
-
-        public void Dispose()
-        {
-            //Remove attached bodies
-
-            //Previus implementation:
-            //while (_bodyCount > 0)
-            //    RemoveBody(_bodyList.body);
-
-            Clear();
-        }
-
-        /// <summary>
-        /// Controllers override this to implement per-step functionality.
-        /// </summary>
-        public abstract void Step(TimeStep step);
-
-        /// <summary>
-        /// Controllers override this to provide debug drawing.
-        /// </summary>
-        public virtual void Draw(DebugDraw debugDraw) { }
-
-        /// <summary>
-        /// Adds a body to the controller list.
-        /// </summary>
-        public void AddBody(Body body)
-        {
-            ControllerEdge edge = new ControllerEdge();
-
-            edge.body = body;
-            edge.controller = this;
-
-            //Add edge to controller list
-            edge.nextBody = _bodyList;
-            edge.prevBody = null;
-            if (_bodyList != null)
-                _bodyList.prevBody = edge;
-            _bodyList = edge;
-            ++_bodyCount;
-
-            //Add edge to body list
-            edge.nextController = body._controllerList;
-            edge.prevController = null;
-            if (body._controllerList != null)
-                body._controllerList.prevController = edge;
-            body._controllerList = edge;
-        }
-
-        /// <summary>
-        /// Removes a body from the controller list.
-        /// </summary>
-        public void RemoveBody(Body body)
-        {
-            //Assert that the controller is not empty
-            Box2DXDebug.Assert(_bodyCount > 0);
-
-            //Find the corresponding edge
-            ControllerEdge edge = _bodyList;
-            while (edge != null && edge.body != body)
-                edge = edge.nextBody;
-
-            //Assert that we are removing a body that is currently attached to the controller
-            Box2DXDebug.Assert(edge != null);
-
-            //Remove edge from controller list
-            if (edge.prevBody != null)
-                edge.prevBody.nextBody = edge.nextBody;
-            if (edge.nextBody != null)
-                edge.nextBody.prevBody = edge.prevBody;
-            if (edge == _bodyList)
-                _bodyList = edge.nextBody;
-            --_bodyCount;
-
-            //Remove edge from body list
-            if (edge.prevController != null)
-                edge.prevController.nextController = edge.nextController;
-            if (edge.nextController != null)
-                edge.nextController.prevController = edge.prevController;
-            if (edge == body._controllerList)
-                body._controllerList = edge.nextController;
-
-            //Free the edge
-            edge = null;
-        }
-
-        /// <summary>
-        /// Removes all bodies from the controller list.
-        /// </summary>
-        public void Clear()
-        {
-#warning "Check this"
-            ControllerEdge current = _bodyList;
-            while (current != null)
-            {
-                ControllerEdge edge = current;
-
-                //Remove edge from controller list
-                _bodyList = edge.nextBody;
-
-                //Remove edge from body list
-                if (edge.prevController != null)
-                    edge.prevController.nextController = edge.nextController;
-                if (edge.nextController != null)
-                    edge.nextController.prevController = edge.prevController;
-                if (edge == edge.body._controllerList)
-                    edge.body._controllerList = edge.nextController;
-
-                //Free the edge
-                //m_world->m_blockAllocator.Free(edge, sizeof(b2ControllerEdge));
-            }
-
-            _bodyCount = 0;
-        }
-
-        /// <summary>
-        /// Get the next body in the world's body list.
-        /// </summary>
-        internal Controller GetNext() { return _next; }
-
-        /// <summary>
-        /// Get the parent world of this body.
-        /// </summary>
-        internal World GetWorld() { return _world; }
-
-        /// <summary>
-        /// Get the attached body list
-        /// </summary>
-        internal ControllerEdge GetBodyList() { return _bodyList; }
-    }
-}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta
deleted file mode 100644
index 197cb7f..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/Controller.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: 5cca52c5b9850aa469cd4033061b8049
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs
deleted file mode 100644
index 333e51c..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-using Box2DX.Common;
-using Math = Box2DX.Common.Math;
-
-namespace Box2DX.Dynamics.Controllers
-{
-    /// This class is used to build gravity controllers
-    public class GravityControllerDef
-    {
-        /// <summary>
-        /// Specifies the strength of the gravitiation force
-        /// </summary>
-        public float G;
-
-        /// <summary>
-        /// If true, gravity is proportional to r^-2, otherwise r^-1
-        /// </summary>
-        public bool InvSqr;
-    }
-
-    public class GravityController : Controller
-    {
-        /// <summary>
-        /// Specifies the strength of the gravitiation force
-        /// </summary>
-        public float G;
-
-        /// If true, gravity is proportional to r^-2, otherwise r^-1
-        public bool InvSqr;
-
-        public GravityController(GravityControllerDef def)
-        {
-            G = def.G;
-            InvSqr = def.InvSqr;
-        }
-
-        public override void Step(TimeStep step)
-        {
-            //B2_NOT_USED(step);
-            if (InvSqr)
-            {
-                for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
-                {
-                    Body body1 = i.body;
-                    for (ControllerEdge j = _bodyList; j != i; j = j.nextBody)
-                    {
-                        Body body2 = j.body;
-                        Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
-                        float r2 = d.LengthSquared();
-                        if (r2 < Settings.FLT_EPSILON)
-                            continue;
-
-                        Vec2 f = G / r2 / Math.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d;
-                        body1.ApplyForce(f, body1.GetWorldCenter());
-                        body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
-                    }
-                }
-            }
-            else
-            {
-                for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
-                {
-                    Body body1 = i.body;
-                    for (ControllerEdge j = _bodyList; j != i; j = j.nextBody)
-                    {
-                        Body body2 = j.body;
-                        Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
-                        float r2 = d.LengthSquared();
-                        if (r2 < Settings.FLT_EPSILON)
-                            continue;
-                        Vec2 f = G / r2 * body1.GetMass() * body2.GetMass() * d;
-                        body1.ApplyForce(f, body1.GetWorldCenter());
-                        body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
-                    }
-                }
-            }
-        }
-    }
-}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta
deleted file mode 100644
index b010f74..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/GravityController.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: d18e2e93089bd984d9a454597208b26e
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs
deleted file mode 100644
index 8fb5af3..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs
+++ /dev/null
@@ -1,89 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* This software is provided 'as-is', without any express or implied
-* warranty.  In no event will the authors be held liable for any damages
-* arising from the use of this software.
-* Permission is granted to anyone to use this software for any purpose,
-* including commercial applications, and to alter it and redistribute it
-* freely, subject to the following restrictions:
-* 1. The origin of this software must not be misrepresented; you must not
-* claim that you wrote the original software. If you use this software
-* in a product, an acknowledgment in the product documentation would be
-* appreciated but is not required.
-* 2. Altered source versions must be plainly marked as such, and must not be
-* misrepresented as being the original software.
-* 3. This notice may not be removed or altered from any source distribution.
-*/
-
-using Box2DX.Common;
-
-namespace Box2DX.Dynamics.Controllers
-{
-
-    /// <summary>
-    /// This class is used to build tensor damping controllers
-    /// </summary>
-    public class b2TensorDampingControllerDef
-    {
-        /// Tensor to use in damping model
-        Mat22 T;
-        /// Set this to a positive number to clamp the maximum amount of damping done.
-        float maxTimestep;
-    };
-
-    public class TensorDampingController : Controller
-    {
-
-        /// <summary>
-        /// Tensor to use in damping model
-        /// Some examples (matrixes in format (row1; row2) )
-        ///(-a 0;0 -a)		Standard isotropic damping with strength a
-        ///(0 a;-a 0)		Electron in fixed field - a force at right angles to velocity with proportional magnitude
-        ///(-a 0;0 -b)		Differing x and y damping. Useful e.g. for top-down wheels.
-        ///By the way, tensor in this case just means matrix, don't let the terminology get you down.
-        /// </summary>
-        Mat22 T;
-
-        /// <summary>
-        /// Set this to a positive number to clamp the maximum amount of damping done.
-        /// Typically one wants maxTimestep to be 1/(max eigenvalue of T), so that damping will never cause something to reverse direction
-        /// </summary>
-        float MaxTimestep;
-
-        /// Sets damping independantly along the x and y axes
-        public void SetAxisAligned(float xDamping, float yDamping)
-        {
-            T.Col1.X = -xDamping;
-            T.Col1.Y = 0;
-            T.Col2.X = 0;
-            T.Col2.Y = -yDamping;
-            if (xDamping > 0 || yDamping > 0)
-            {
-                MaxTimestep = 1 / Math.Max(xDamping, yDamping);
-            }
-            else
-            {
-                MaxTimestep = 0;
-            }
-        }
-
-        public override void Step(TimeStep step)
-        {
-            float timestep = step.Dt;
-            if (timestep <= Settings.FLT_EPSILON)
-                return;
-            if (timestep > MaxTimestep && MaxTimestep > 0)
-                timestep = MaxTimestep;
-            for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
-            {
-                Body body = i.body;
-                if (body.IsSleeping())
-                    continue;
-
-                Vec2 damping = body.GetWorldVector(Math.Mul(T, body.GetLocalVector(body.GetLinearVelocity())));
-                body.SetLinearVelocity(body.GetLinearVelocity() + timestep*damping);
-            }
-        }
-    }
-}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta
deleted file mode 100644
index a975c8f..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Controllers/TensorDampingController.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: c119a4aba6eaccf408daeb709363782c
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs b/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs
deleted file mode 100644
index eecfd1f..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs
+++ /dev/null
@@ -1,478 +0,0 @@
-/*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
-
-  This software is provided 'as-is', without any express or implied
-  warranty.  In no event will the authors be held liable for any damages
-  arising from the use of this software.
-
-  Permission is granted to anyone to use this software for any purpose,
-  including commercial applications, and to alter it and redistribute it
-  freely, subject to the following restrictions:
-
-  1. The origin of this software must not be misrepresented; you must not
-     claim that you wrote the original software. If you use this software
-     in a product, an acknowledgment in the product documentation would be
-     appreciated but is not required.
-  2. Altered source versions must be plainly marked as such, and must not be
-     misrepresented as being the original software.
-  3. This notice may not be removed or altered from any source distribution.
-*/
-
-using System;
-using System.Collections.Generic;
-using System.Text;
-using Box2DX.Collision;
-using Box2DX.Common;
-
-namespace Box2DX.Dynamics
-{
-	/// <summary>
-	/// This holds contact filtering data.
-	/// </summary>
-	public struct FilterData
-	{
-		/// <summary>
-		/// The collision category bits. Normally you would just set one bit.
-		/// </summary>
-		public ushort CategoryBits;
-
-		/// <summary>
-		/// The collision mask bits. This states the categories that this
-		/// shape would accept for collision.
-		/// </summary>
-		public ushort MaskBits;
-
-		/// <summary>
-		/// Collision groups allow a certain group of objects to never collide (negative)
-		/// or always collide (positive). Zero means no collision group. Non-zero group
-		/// filtering always wins against the mask bits.
-		/// </summary>
-		public short GroupIndex;
-	}
-
-	/// <summary>
-	/// A fixture definition is used to create a fixture. This class defines an
-	/// abstract fixture definition. You can reuse fixture definitions safely.
-	/// </summary>
-	public class FixtureDef
-	{
-		/// <summary>
-		/// The constructor sets the default fixture definition values.
-		/// </summary>	
-		public FixtureDef()
-		{
-			Type = ShapeType.UnknownShape;
-			UserData = null;
-			Friction = 0.2f;
-			Restitution = 0.0f;
-			Density = 0.0f;
-			Filter.CategoryBits = 0x0001;
-			Filter.MaskBits = 0xFFFF;
-			Filter.GroupIndex = 0;
-			IsSensor = false;
-		}
-
-		/// <summary>
-		/// Holds the shape type for down-casting.
-		/// </summary>
-		public ShapeType Type;
-
-		/// <summary>
-		/// Use this to store application specific fixture data.
-		/// </summary>
-		public object UserData;
-
-		/// <summary>
-		/// The friction coefficient, usually in the range [0,1].
-		/// </summary>
-		public float Friction;
-
-		/// <summary>
-		/// The restitution (elasticity) usually in the range [0,1].
-		/// </summary>
-		public float Restitution;
-
-		/// <summary>
-		/// The density, usually in kg/m^2.
-		/// </summary>
-		public float Density;
-
-		/// <summary>
-		/// A sensor shape collects contact information but never generates a collision response.
-		/// </summary>
-		public bool IsSensor;
-
-		/// <summary>
-		/// Contact filtering data.
-		/// </summary>
-		public FilterData Filter;
-	}
-
-	/// <summary>
-	/// This structure is used to build a fixture with a circle shape.
-	/// </summary>
-	public class CircleDef : FixtureDef
-	{
-		public Vec2 LocalPosition;
-		public float Radius;
-
-		public CircleDef()
-		{
-			Type = ShapeType.CircleShape;
-			LocalPosition = Vec2.Zero;
-			Radius = 1.0f;
-		}
-	}
-
-	/// <summary>
-	/// Convex polygon. The vertices must be ordered so that the outside of
-	/// the polygon is on the right side of the edges (looking along the edge
-	/// from start to end).
-	/// </summary>
-	public class PolygonDef : FixtureDef
-	{
-		/// <summary>
-		/// The number of polygon vertices.
-		/// </summary>
-		public int VertexCount;
-
-		/// <summary>
-		/// The polygon vertices in local coordinates.
-		/// </summary>
-		public Vec2[] Vertices = new Vec2[Settings.MaxPolygonVertices];
-
-		public PolygonDef()
-		{
-			Type = ShapeType.PolygonShape;
-			VertexCount = 0;
-		}
-
-		/// <summary>
-		/// Build vertices to represent an axis-aligned box.
-		/// </summary>
-		/// <param name="hx">The half-width</param>
-		/// <param name="hy">The half-height.</param>
-		public void SetAsBox(float hx, float hy)
-		{
-			VertexCount = 4;
-			Vertices[0].Set(-hx, -hy);
-			Vertices[1].Set(hx, -hy);
-			Vertices[2].Set(hx, hy);
-			Vertices[3].Set(-hx, hy);
-		}
-
-
-		/// <summary>
-		/// Build vertices to represent an oriented box.
-		/// </summary>
-		/// <param name="hx">The half-width</param>
-		/// <param name="hy">The half-height.</param>
-		/// <param name="center">The center of the box in local coordinates.</param>
-		/// <param name="angle">The rotation of the box in local coordinates.</param>
-		public void SetAsBox(float hx, float hy, Vec2 center, float angle)
-		{
-			SetAsBox(hx, hy);
-
-			XForm xf = new XForm();
-			xf.Position = center;
-			xf.R.Set(angle);
-
-			for (int i = 0; i < VertexCount; ++i)
-			{
-				Vertices[i] = Common.Math.Mul(xf, Vertices[i]);
-			}
-		}
-	}
-
-	/// <summary>
-	/// This structure is used to build a chain of edges.
-	/// </summary>
-	public class EdgeDef : FixtureDef
-	{
-		public EdgeDef()
-		{
-			Type = ShapeType.EdgeShape;
-		}
-
-		/// <summary>
-		/// The start vertex.
-		/// </summary>
-		public Vec2 Vertex1;
-
-		/// <summary>
-		/// The end vertex.
-		/// </summary>
-		public Vec2 Vertex2;
-	}
-
-	/// <summary>
-	/// A fixture is used to attach a shape to a body for collision detection. A fixture
-	/// inherits its transform from its parent. Fixtures hold additional non-geometric data
-	/// such as friction, collision filters, etc.
-	/// Fixtures are created via Body.CreateFixture.
-	/// @warning you cannot reuse fixtures.
-	/// </summary>
-	public class Fixture
-	{
-		protected ShapeType _type;
-		protected bool _isSensor;
-		protected UInt16 _proxyId;
-
-		internal Body _body;
-		protected Shape _shape;
-		internal Fixture _next;
-
-		/// <summary>
-		/// Contact filtering data. You must call b2World::Refilter to correct
-		/// existing contacts/non-contacts.
-		/// </summary>
-		public FilterData Filter;
-
-		/// <summary>
-		/// Is this fixture a sensor (non-solid)?
-		/// </summary>
-		public bool IsSensor { get { return _isSensor; } }
-
-		/// <summary>
-		/// Get the child shape. You can modify the child shape, however you should not change the
-		/// number of vertices because this will crash some collision caching mechanisms.
-		/// </summary>
-		public Shape Shape { get { return _shape; } }
-
-		/// <summary>
-		/// Get the type of this shape. You can use this to down cast to the concrete shape.
-		/// </summary>
-		public ShapeType ShapeType { get { return _type; } }
-
-		/// <summary>
-		/// Get the next fixture in the parent body's fixture list.
-		/// </summary>
-		public Fixture Next { get { return _next; } }
-
-		/// <summary>
-		/// Get the parent body of this fixture. This is NULL if the fixture is not attached.
-		/// </summary>
-		public Body Body { get { return _body; } }
-
-		/// <summary>
-		/// User data that was assigned in the fixture definition. Use this to
-		/// store your application specific data.
-		/// </summary>
-		public object UserData;
-
-		/// <summary>
-		/// Friction coefficient, usually in the range [0,1].
-		/// </summary>
-		public float Friction;
-
-		/// <summary>
-		/// Restitution (elasticity) usually in the range [0,1].
-		/// </summary>
-		public float Restitution;
-
-		/// <summary>
-		/// Density, usually in kg/m^2.
-		/// </summary>
-		public float Density;
-
-		public Fixture()
-		{
-			_proxyId = PairManager.NullProxy;
-		}
-
-		public void Create(BroadPhase broadPhase, Body body, XForm xf, FixtureDef def)
-		{
-			UserData = def.UserData;
-			Friction = def.Friction;
-			Restitution = def.Restitution;
-			Density = def.Density;
-
-			_body = body;
-			_next = null;
-
-			Filter = def.Filter;
-
-			_isSensor = def.IsSensor;
-
-			_type = def.Type;
-
-			// Allocate and initialize the child shape.
-			switch (_type)
-			{
-				case ShapeType.CircleShape:
-					{
-						CircleShape circle = new CircleShape();
-						CircleDef circleDef = (CircleDef)def;
-						circle._position = circleDef.LocalPosition;
-						circle._radius = circleDef.Radius;
-						_shape = circle;
-					}
-					break;
-
-				case ShapeType.PolygonShape:
-					{
-						PolygonShape polygon = new PolygonShape();
-						PolygonDef polygonDef = (PolygonDef)def;
-						polygon.Set(polygonDef.Vertices, polygonDef.VertexCount);
-						_shape = polygon;
-					}
-					break;
-
-				case ShapeType.EdgeShape:
-					{
-						EdgeShape edge = new EdgeShape();
-						EdgeDef edgeDef = (EdgeDef)def;
-						edge.Set(edgeDef.Vertex1, edgeDef.Vertex2);
-						_shape = edge;
-					}
-					break;
-
-				default:
-					Box2DXDebug.Assert(false);
-					break;
-			}
-
-			// Create proxy in the broad-phase.
-			AABB aabb;
-			_shape.ComputeAABB(out aabb, xf);
-
-			bool inRange = broadPhase.InRange(aabb);
-
-			// You are creating a shape outside the world box.
-			Box2DXDebug.Assert(inRange);
-
-			if (inRange)
-			{
-				_proxyId = broadPhase.CreateProxy(aabb, this);
-			}
-			else
-			{
-				_proxyId = PairManager.NullProxy;
-			}
-		}
-
-		public void Destroy(BroadPhase broadPhase)
-		{
-			// Remove proxy from the broad-phase.
-			if (_proxyId != PairManager.NullProxy)
-			{
-				broadPhase.DestroyProxy(_proxyId);
-				_proxyId = PairManager.NullProxy;
-			}
-
-			// Free the child shape.
-			_shape.Dispose();
-			_shape = null;
-		}
-
-		internal bool Synchronize(BroadPhase broadPhase, XForm transform1, XForm transform2)
-		{
-			if (_proxyId == PairManager.NullProxy)
-			{
-				return false;
-			}
-
-			// Compute an AABB that covers the swept shape (may miss some rotation effect).
-			AABB aabb1, aabb2;
-			_shape.ComputeAABB(out aabb1, transform1);
-			_shape.ComputeAABB(out aabb2, transform2);
-
-			AABB aabb = new AABB();
-			aabb.Combine(aabb1, aabb2);
-
-			if (broadPhase.InRange(aabb))
-			{
-				broadPhase.MoveProxy(_proxyId, aabb);
-				return true;
-			}
-			else
-			{
-				return false;
-			}
-		}
-
-		internal void RefilterProxy(BroadPhase broadPhase, XForm transform)
-		{
-			if (_proxyId == PairManager.NullProxy)
-			{
-				return;
-			}
-
-			broadPhase.DestroyProxy(_proxyId);
-
-			AABB aabb;
-			_shape.ComputeAABB(out aabb, transform);
-
-			bool inRange = broadPhase.InRange(aabb);
-
-			if (inRange)
-			{
-				_proxyId = broadPhase.CreateProxy(aabb, this);
-			}
-			else
-			{
-				_proxyId = PairManager.NullProxy;
-			}
-		}
-
-		public virtual void Dispose()
-		{
-			Box2DXDebug.Assert(_proxyId == PairManager.NullProxy);
-			Box2DXDebug.Assert(_shape == null);
-		}
-
-		/// <summary>
-		/// Compute the mass properties of this shape using its dimensions and density.
-		/// The inertia tensor is computed about the local origin, not the centroid.
-		/// </summary>
-		/// <param name="massData">Returns the mass data for this shape.</param>
-		public void ComputeMass(out MassData massData)
-		{
-			_shape.ComputeMass(out massData, Density);
-		}
-
-		/// <summary>
-		/// Compute the volume and centroid of this fixture intersected with a half plane.
-		/// </summary>
-		/// <param name="normal">Normal the surface normal.</param>
-		/// <param name="offset">Offset the surface offset along normal.</param>
-		/// <param name="c">Returns the centroid.</param>
-		/// <returns>The total volume less than offset along normal.</returns>
-		public float ComputeSubmergedArea(Vec2 normal, float offset, out Vec2 c)
-		{
-			return _shape.ComputeSubmergedArea(normal, offset, _body.GetXForm(), out c);
-		}
-
-		/// <summary>
-		/// Test a point for containment in this fixture. This only works for convex shapes.
-		/// </summary>
-		/// <param name="p">A point in world coordinates.</param>
-		public bool TestPoint(Vec2 p)
-		{
-			return _shape.TestPoint(_body.GetXForm(), p);
-		}
-
-		/// <summary>
-		/// Perform a ray cast against this shape.
-		/// </summary>
-		/// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point
-		/// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param>
-		/// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal
-		/// is not set.</param>
-		/// <param name="segment">Defines the begin and end point of the ray cast.</param>
-		/// <param name="maxLambda">A number typically in the range [0,1].</param>
-		public SegmentCollide TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
-		{
-			return _shape.TestSegment(_body.GetXForm(), out lambda, out normal, segment, maxLambda);
-		}
-
-		/// <summary>
-		/// Get the maximum radius about the parent body's center of mass.
-		/// </summary>
-		public float ComputeSweepRadius(Vec2 pivot)
-		{
-			return _shape.ComputeSweepRadius(pivot);
-		}
-	}
-}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta b/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta
deleted file mode 100644
index c5f3148..0000000
--- a/Box2d/Assets/Program/Box2d/Dynamics/Fixture.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: 121b855d9b0acd24c8362d3416dc5907
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Island.cs b/Box2d/Assets/Program/Box2d/Dynamics/Island.cs
index 0f24b8e..2fa4bd8 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Island.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Island.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -106,9 +106,9 @@ to increase the number of cache hits. Much of misses are due to random access
 to body data. The constraint structures are iterated over linearly, which leads
 to few cache misses.
 
-The _bodies are not accessed during iteration. Instead read only data, such as
+The bodies are not accessed during iteration. Instead read only data, such as
 the mass values are stored with the constraints. The mutable data are the constraint
-impulses and the _bodies velocities/positions. The impulses are held inside the
+impulses and the bodies velocities/positions. The impulses are held inside the
 constraint structures. The body velocities/positions are held in compact, temporary
 arrays to increase the number of cache hits. Linear and angular velocity are
 stored in a single array since multiple arrays lead to multiple misses.
@@ -184,9 +184,9 @@ namespace Box2DX.Dynamics
 			_bodyCapacity = bodyCapacity;
 			_contactCapacity = contactCapacity;
 			_jointCapacity = jointCapacity;
-			//__bodyCount = 0;
-			//_contactCount = 0;
-			//_jointCount = 0;
+			_bodyCount = 0;
+			_contactCount = 0;
+			_jointCount = 0;
 
 			_listener = listener;
 
@@ -242,6 +242,39 @@ namespace Box2DX.Dynamics
 				// v2 = (1.0f - c * dt) * v1
 				b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
 				b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
+
+				// Check for large velocities.
+#if TARGET_FLOAT32_IS_FIXED
+				// Fixed point code written this way to prevent
+				// overflows, float code is optimized for speed
+
+				float vMagnitude = b._linearVelocity.Length();
+				if(vMagnitude > Settings.MaxLinearVelocity)
+				{
+					b._linearVelocity *= Settings.MaxLinearVelocity/vMagnitude;
+				}
+				b._angularVelocity = Vector2.Clamp(b._angularVelocity, 
+					-Settings.MaxAngularVelocity, Settings.MaxAngularVelocity);
+
+#else
+				if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared)
+				{
+					b._linearVelocity.Normalize();
+					b._linearVelocity *= Settings.MaxLinearVelocity;
+				}
+
+				if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared)
+				{
+					if (b._angularVelocity < 0.0f)
+					{
+						b._angularVelocity = -Settings.MaxAngularVelocity;
+					}
+					else
+					{
+						b._angularVelocity = Settings.MaxAngularVelocity;
+					}
+				}
+#endif
 			}
 
 			ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);
@@ -275,27 +308,6 @@ namespace Box2DX.Dynamics
 				if (b.IsStatic())
 					continue;
 
-				// Check for large velocities.
-				Vec2 translation = step.Dt * b._linearVelocity;
-				if (Common.Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
-				{
-					translation.Normalize();
-					b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation;
-				}
-
-				float rotation = step.Dt * b._angularVelocity;
-				if (rotation * rotation > Settings.MaxRotationSquared)
-				{
-					if (rotation < 0.0)
-					{
-						b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
-					}
-					else
-					{
-						b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
-					}
-				}
-
 				// Store positions for continuous collision.
 				b._sweep.C0 = b._sweep.C;
 				b._sweep.A0 = b._sweep.A;
@@ -311,14 +323,13 @@ namespace Box2DX.Dynamics
 			}
 
 			// Iterate over constraints.
-			for (int i = 0; i < step.PositionIterations; ++i)
+			for (int ii = 0; ii < step.PositionIterations; ++ii)
 			{
 				bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
-
 				bool jointsOkay = true;
-				for (int j = 0; j < _jointCount; ++j)
+				for (int i = 0; i < _jointCount; ++i)
 				{
-					bool jointOkay = _joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
+					bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/);
 					jointsOkay = jointsOkay && jointOkay;
 				}
 
@@ -333,8 +344,7 @@ namespace Box2DX.Dynamics
 
 			if (allowSleep)
 			{
-				float minSleepTime = Settings.FLT_MAX;
-
+				float minSleepTime = Common.Settings.FLT_MAX;
 #if !TARGET_FLOAT32_IS_FIXED
 				float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
 				float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
@@ -360,7 +370,7 @@ namespace Box2DX.Dynamics
 						Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
 						Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
 #else
- b._angularVelocity * b._angularVelocity > angTolSqr ||
+						b._angularVelocity * b._angularVelocity > angTolSqr ||
 						Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
 #endif
 					{
@@ -391,24 +401,31 @@ namespace Box2DX.Dynamics
 		{
 			ContactSolver contactSolver = new ContactSolver(subStep, _contacts, _contactCount);
 
-			// No warm starting is needed for TOI events because warm
-			// starting impulses were applied in the discrete solver.
+			// No warm starting needed for TOI contact events.
+
+#if B2_TOI_JOINTS
+			// For joints, initialize with the last full step warm starting values
+			subStep.WarmStarting = true;
 
-			// Warm starting for joints is off for now, but we need to
-			// call this function to compute Jacobians.
 			for (int i = 0; i < _jointCount; ++i)
 			{
 				_joints[i].InitVelocityConstraints(subStep);
 			}
 
+			// ...but don't update the warm starting during TOI solve!
+			subStep.WarmStarting = false;
+#endif
+
 			// Solve velocity constraints.
 			for (int i = 0; i < subStep.VelocityIterations; ++i)
 			{
 				contactSolver.SolveVelocityConstraints();
-				for (int j = 0; j < _jointCount; ++j)
-				{
-					_joints[j].SolveVelocityConstraints(subStep);
-				}
+#if B2_TOI_JOINTS
+			for (int j = 0; j < _jointCount; ++j)
+			{
+				_joints[j].SolveVelocityConstraints(subStep);
+			}
+#endif
 			}
 
 			// Don't store the TOI contact forces for warm starting
@@ -422,27 +439,6 @@ namespace Box2DX.Dynamics
 				if (b.IsStatic())
 					continue;
 
-				// Check for large velocities.
-				Vec2 translation = subStep.Dt * b._linearVelocity;
-				if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
-				{
-					translation.Normalize();
-					b._linearVelocity = (Settings.MaxTranslation * subStep.Inv_Dt) * translation;
-				}
-
-				float rotation = subStep.Dt * b._angularVelocity;
-				if (rotation * rotation > Settings.MaxRotationSquared)
-				{
-					if (rotation < 0.0)
-					{
-						b._angularVelocity = -subStep.Inv_Dt * Settings.MaxRotation;
-					}
-					else
-					{
-						b._angularVelocity = subStep.Inv_Dt * Settings.MaxRotation;
-					}
-				}
-
 				// Store positions for continuous collision.
 				b._sweep.C0 = b._sweep.C;
 				b._sweep.A0 = b._sweep.A;
@@ -458,21 +454,28 @@ namespace Box2DX.Dynamics
 			}
 
 			// Solve position constraints.
-			const float k_toiBaumgarte = 0.75f;
+			float k_toiBaumgarte = 0.75f;
 			for (int i = 0; i < subStep.PositionIterations; ++i)
 			{
 				bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
+#if B2_TOI_JOINTS
 				bool jointsOkay = true;
 				for (int j = 0; j < _jointCount; ++j)
 				{
-					bool jointOkay = _joints[j].SolvePositionConstraints(k_toiBaumgarte);
+					bool jointOkay = _joints[j].SolvePositionConstraints();
 					jointsOkay = jointsOkay && jointOkay;
 				}
-
+				
 				if (contactsOkay && jointsOkay)
 				{
 					break;
 				}
+#else
+				if (contactsOkay)
+				{
+					break;
+				}
+#endif
 			}
 
 			Report(contactSolver._constraints);
@@ -508,14 +511,31 @@ namespace Box2DX.Dynamics
 			{
 				Contact c = _contacts[i];
 				ContactConstraint cc = constraints[i];
-				ContactImpulse impulse = new ContactImpulse();
-				for (int j = 0; j < cc.PointCount; ++j)
+				ContactResult cr = new ContactResult();
+				cr.Shape1 = c.GetShape1();
+				cr.Shape2 = c.GetShape2();
+				Body b1 = cr.Shape1.GetBody();
+				int manifoldCount = c.GetManifoldCount();
+				Manifold[] manifolds = c.GetManifolds();
+				for (int j = 0; j < manifoldCount; ++j)
 				{
-					impulse.normalImpulses[j] = cc.Points[j].NormalImpulse;
-					impulse.tangentImpulses[j] = cc.Points[j].TangentImpulse;
-				}
+					Manifold manifold = manifolds[j];
+					cr.Normal = manifold.Normal;
+					for (int k = 0; k < manifold.PointCount; ++k)
+					{
+						ManifoldPoint point = manifold.Points[k];
+						ContactConstraintPoint ccp = cc.Points[k];
+						cr.Position = b1.GetWorldPoint(point.LocalPoint1);
+
+						// TOI constraint results are not stored, so get
+						// the result from the constraint.
+						cr.NormalImpulse = ccp.NormalImpulse;
+						cr.TangentImpulse = ccp.TangentImpulse;
+						cr.ID = point.ID;
 
-				_listener.PostSolve(c, impulse);
+						_listener.Result(cr);
+					}
+				}
 			}
 		}
 	}
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs
index f00b664..6b104dc 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/DistanceJoint.cs
@@ -65,6 +65,10 @@ namespace Box2DX.Dynamics
 		/// <summary>
 		/// Initialize the bodies, anchors, and length using the world anchors.
 		/// </summary>
+		/// <param name="body1"></param>
+		/// <param name="body2"></param>
+		/// <param name="anchor1"></param>
+		/// <param name="anchor2"></param>
 		public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2)
 		{
 			Body1 = body1;
@@ -129,14 +133,14 @@ namespace Box2DX.Dynamics
 			get { return _body2.GetWorldPoint(_localAnchor2);}
 		}
 
-		public override Vec2 GetReactionForce(float inv_dt)
+		public override Vec2 ReactionForce
 		{
-			return (inv_dt * _impulse) * _u;
+			get { return (_inv_dt * _impulse) * _u; }
 		}
 
-		public override float GetReactionTorque(float inv_dt)
+		public override float ReactionTorque
 		{
-			return 0.0f;
+			get { return 0.0f; }
 		}
 
 		public DistanceJoint(DistanceJointDef def)
@@ -150,10 +154,13 @@ namespace Box2DX.Dynamics
 			_impulse = 0.0f;
 			_gamma = 0.0f;
 			_bias = 0.0f;
+			_inv_dt = 0.0f;
 		}
 
 		internal override void InitVelocityConstraints(TimeStep step)
 		{
+			_inv_dt = step.Inv_Dt;
+
 			Body b1 = _body1;
 			Body b2 = _body2;
 
@@ -201,7 +208,6 @@ namespace Box2DX.Dynamics
 
 			if (step.WarmStarting)
 			{
-				//Scale the inpulse to support a variable timestep.
 				_impulse *= step.DtRatio;
 				Vec2 P = _impulse * _u;
 				b1._linearVelocity -= b1._invMass * P;
@@ -215,11 +221,10 @@ namespace Box2DX.Dynamics
 			}
 		}
 
-		internal override bool SolvePositionConstraints(float baumgarte)
+		internal override bool SolvePositionConstraints()
 		{
 			if (_frequencyHz > 0.0f)
 			{
-				//There is no possition correction for soft distace constraint.
 				return true;
 			}
 
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs
index 7dbdf6a..ebacae3 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/GearJoint.cs
@@ -118,25 +118,31 @@ namespace Box2DX.Dynamics
 		public float _mass;
 
 		// Impulse for accumulation/warm starting.
-		public float _impulse;
+		public float _force;
 
 		public override Vec2 Anchor1 { get { return _body1.GetWorldPoint(_localAnchor1); } }
 		public override Vec2 Anchor2 { get { return _body2.GetWorldPoint(_localAnchor2); } }
 
-		public override Vec2 GetReactionForce(float inv_dt)
+		public override Vec2 ReactionForce
 		{
-			// TODO_ERIN not tested
-			Vec2 P = _impulse * _J.Linear2;
-			return inv_dt * P;
+			get
+			{
+				// TODO_ERIN not tested
+				Vec2 F = Settings.FORCE_SCALE(_force) * _J.Linear2;
+				return F;
+			}
 		}
 
-		public override float GetReactionTorque(float inv_dt)
+		public override float ReactionTorque
 		{
-			// TODO_ERIN not tested
-			Vec2 r = Common.Math.Mul(_body2.GetXForm().R, _localAnchor2 - _body2.GetLocalCenter());
-			Vec2 P = _impulse * _J.Linear2;
-			float L = _impulse * _J.Angular2 - Vec2.Cross(r, P);
-			return inv_dt * L;
+			get
+			{
+				// TODO_ERIN not tested
+				Vec2 r = Common.Math.Mul(_body2.GetXForm().R, _localAnchor2 - _body2.GetLocalCenter());
+				Vec2 F = _force * _J.Linear2;
+				float T = Settings.FORCE_SCALE(_force * _J.Angular2 - Vec2.Cross(r, F));
+				return T;
+			}
 		}
 
 		/// <summary>
@@ -200,7 +206,7 @@ namespace Box2DX.Dynamics
 
 			_constant = coordinate1 + _ratio * coordinate2;
 
-			_impulse = 0.0f;
+			_force = 0.0f;
 		}
 
 		internal override void InitVelocityConstraints(TimeStep step)
@@ -250,14 +256,15 @@ namespace Box2DX.Dynamics
 			if (step.WarmStarting)
 			{
 				// Warm starting.
-				b1._linearVelocity += b1._invMass * _impulse * _J.Linear1;
-				b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
-				b2._linearVelocity += b2._invMass * _impulse * _J.Linear2;
-				b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
+				float P = Settings.FORCE_SCALE(step.Dt) * _force;
+				b1._linearVelocity += b1._invMass * P * _J.Linear1;
+				b1._angularVelocity += b1._invI * P * _J.Angular1;
+				b2._linearVelocity += b2._invMass * P * _J.Linear2;
+				b2._angularVelocity += b2._invI * P * _J.Angular2;
 			}
 			else
 			{
-				_impulse = 0.0f;
+				_force = 0.0f;
 			}
 		}
 
@@ -266,18 +273,20 @@ namespace Box2DX.Dynamics
 			Body b1 = _body1;
 			Body b2 = _body2;
 
-			float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
+			float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity,
+										b2._linearVelocity, b2._angularVelocity);
 
-			float impulse = _mass * (-Cdot);
-			_impulse += impulse;
+			float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _mass * Cdot;
+			_force += force;
 
-			b1._linearVelocity += b1._invMass * impulse * _J.Linear1;
-			b1._angularVelocity += b1._invI * impulse * _J.Angular1;
-			b2._linearVelocity += b2._invMass * impulse * _J.Linear2;
-			b2._angularVelocity += b2._invI * impulse * _J.Angular2;
+			float P = Settings.FORCE_SCALE(step.Dt) * force;
+			b1._linearVelocity += b1._invMass * P * _J.Linear1;
+			b1._angularVelocity += b1._invI * P * _J.Angular1;
+			b2._linearVelocity += b2._invMass * P * _J.Linear2;
+			b2._angularVelocity += b2._invI * P * _J.Angular2;
 		}
 
-		internal override bool SolvePositionConstraints(float baumgarte)
+		internal override bool SolvePositionConstraints()
 		{
 			float linearError = 0.0f;
 
@@ -305,7 +314,7 @@ namespace Box2DX.Dynamics
 
 			float C = _constant - (coordinate1 + _ratio * coordinate2);
 
-			float impulse = _mass * (-C);
+			float impulse = -_mass * C;
 
 			b1._sweep.C += b1._invMass * impulse * _J.Linear1;
 			b1._sweep.A += b1._invI * impulse * _J.Angular1;
@@ -315,8 +324,7 @@ namespace Box2DX.Dynamics
 			b1.SynchronizeTransform();
 			b2.SynchronizeTransform();
 
-			//TODO_ERIN not implemented
 			return linearError < Settings.LinearSlop;
 		}
 	}
-}
+}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs
index 232f98b..f3265b4 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/Joint.cs
@@ -35,8 +35,7 @@ namespace Box2DX.Dynamics
 		DistanceJoint,
 		PulleyJoint,
 		MouseJoint,
-		GearJoint,
-		LineJoint
+		GearJoint
 	}
 
 	public enum LimitState
@@ -158,19 +157,17 @@ namespace Box2DX.Dynamics
 		internal Body _body1;
 		internal Body _body2;
 
+		protected float _inv_dt;
+
 		internal bool _islandFlag;
 		internal bool _collideConnected;
 
 		protected object _userData;
 
-		// Cache here per time step to reduce cache misses.
-		protected Vec2 _localCenter1, _localCenter2;
-		protected float _invMass1, _invI1;
-		protected float _invMass2, _invI2;
-
 		/// <summary>
 		/// Get the type of the concrete joint.
 		/// </summary>
+		/// <returns></returns>
 		public new JointType GetType()
 		{
 			return _type;
@@ -208,13 +205,15 @@ namespace Box2DX.Dynamics
 
 		/// <summary>
 		/// Get the reaction force on body2 at the joint anchor.
-		/// </summary>		
-		public abstract Vec2 GetReactionForce(float inv_dt);
+		/// </summary>
+		/// <returns></returns>
+		public abstract Vec2 ReactionForce { get; }
 
 		/// <summary>
 		/// Get the reaction torque on body2.
-		/// </summary>		
-		public abstract float GetReactionTorque(float inv_dt);
+		/// </summary>
+		/// <returns></returns>
+		public abstract float ReactionTorque { get; }
 
 		/// <summary>
 		/// Get the next joint the world joint list.
@@ -258,36 +257,37 @@ namespace Box2DX.Dynamics
 						joint = new DistanceJoint((DistanceJointDef)def);
 					}
 					break;
+
 				case JointType.MouseJoint:
 					{
 						joint = new MouseJoint((MouseJointDef)def);
 					}
 					break;
+
 				case JointType.PrismaticJoint:
 					{
 						joint = new PrismaticJoint((PrismaticJointDef)def);
 					}
 					break;
+
 				case JointType.RevoluteJoint:
 					{
 						joint = new RevoluteJoint((RevoluteJointDef)def);
 					}
 					break;
+
 				case JointType.PulleyJoint:
 					{
 						joint = new PulleyJoint((PulleyJointDef)def);
 					}
 					break;
+
 				case JointType.GearJoint:
 					{
 						joint = new GearJoint((GearJointDef)def);
 					}
 					break;
-				case JointType.LineJoint:
-					{
-						joint = new LineJoint((LineJointDef)def);
-					}
-					break;
+
 				default:
 					Box2DXDebug.Assert(false);
 					break;
@@ -305,12 +305,7 @@ namespace Box2DX.Dynamics
 		internal abstract void SolveVelocityConstraints(TimeStep step);
 
 		// This returns true if the position errors are within tolerance.
-		internal abstract bool SolvePositionConstraints(float baumgarte);
-
-		internal void ComputeXForm(ref XForm xf, Vec2 center, Vec2 localCenter, float angle)
-		{
-			xf.R.Set(angle);
-			xf.Position = center - Box2DX.Common.Math.Mul(xf.R, localCenter);
-		}
+		internal virtual void InitPositionConstraints() { }
+		internal abstract bool SolvePositionConstraints();
 	}
 }
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs
index 04b2103..5f28270 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/LineJoint.cs
@@ -1,710 +1 @@
-/*
-  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-
-  This software is provided 'as-is', without any express or implied
-  warranty.  In no event will the authors be held liable for any damages
-  arising from the use of this software.
-
-  Permission is granted to anyone to use this software for any purpose,
-  including commercial applications, and to alter it and redistribute it
-  freely, subject to the following restrictions:
-
-  1. The origin of this software must not be misrepresented; you must not
-     claim that you wrote the original software. If you use this software
-     in a product, an acknowledgment in the product documentation would be
-     appreciated but is not required.
-  2. Altered source versions must be plainly marked as such, and must not be
-     misrepresented as being the original software.
-  3. This notice may not be removed or altered from any source distribution.
-*/
-
-// Linear constraint (point-to-line)
-// d = p2 - p1 = x2 + r2 - x1 - r1
-// C = dot(perp, d)
-// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
-//      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
-// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
-//
-// K = J * invM * JT
-//
-// J = [-a -s1 a s2]
-// a = perp
-// s1 = cross(d + r1, a) = cross(p2 - x1, a)
-// s2 = cross(r2, a) = cross(p2 - x2, a)
-
-
-// Motor/Limit linear constraint
-// C = dot(ax1, d)
-// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
-// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
-
-// Block Solver
-// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
-// when the mass has poor distribution (leading to large torques about the joint anchor points).
-//
-// The Jacobian has 3 rows:
-// J = [-uT -s1 uT s2] // linear
-//     [-vT -a1 vT a2] // limit
-//
-// u = perp
-// v = axis
-// s1 = cross(d + r1, u), s2 = cross(r2, u)
-// a1 = cross(d + r1, v), a2 = cross(r2, v)
-
-// M * (v2 - v1) = JT * df
-// J * v2 = bias
-//
-// v2 = v1 + invM * JT * df
-// J * (v1 + invM * JT * df) = bias
-// K * df = bias - J * v1 = -Cdot
-// K = J * invM * JT
-// Cdot = J * v1 - bias
-//
-// Now solve for f2.
-// df = f2 - f1
-// K * (f2 - f1) = -Cdot
-// f2 = invK * (-Cdot) + f1
-//
-// Clamp accumulated limit impulse.
-// lower: f2(2) = max(f2(2), 0)
-// upper: f2(2) = min(f2(2), 0)
-//
-// Solve for correct f2(1)
-// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1
-//                = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2)
-// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1)
-// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
-//
-// Now compute impulse to be applied:
-// df = f2 - f1
-
-using System;
-using System.Collections.Generic;
-using System.Text;
-
-using Box2DX.Common;
-
-namespace Box2DX.Dynamics
-{
-	/// <summary>
-	/// Line joint definition. This requires defining a line of
-	/// motion using an axis and an anchor point. The definition uses local
-	/// anchor points and a local axis so that the initial configuration
-	/// can violate the constraint slightly. The joint translation is zero
-	/// when the local anchor points coincide in world space. Using local
-	/// anchors and a local axis helps when saving and loading a game.
-	/// </summary>
-	public class LineJointDef : JointDef
-	{
-		public LineJointDef()
-		{
-			Type = JointType.LineJoint;
-			localAnchor1.SetZero();
-			localAnchor2.SetZero();
-			localAxis1.Set(1.0f, 0.0f);
-			enableLimit = false;
-			lowerTranslation = 0.0f;
-			upperTranslation = 0.0f;
-			enableMotor = false;
-			maxMotorForce = 0.0f;
-			motorSpeed = 0.0f;
-		}
-
-		/// <summary>
-		/// Initialize the bodies, anchors, axis, and reference angle using the world
-		/// anchor and world axis.
-		/// </summary>
-		public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis)
-		{
-			Body1 = body1;
-			Body2 = body2;
-			localAnchor1 = body1.GetLocalPoint(anchor);
-			localAnchor2 = body2.GetLocalPoint(anchor);
-			localAxis1 = body1.GetLocalVector(axis);
-		}
-
-		/// <summary>
-		/// The local anchor point relative to body1's origin.
-		/// </summary>
-		public Vec2 localAnchor1;
-
-		/// <summary>
-		/// The local anchor point relative to body2's origin.
-		/// </summary>
-		public Vec2 localAnchor2;
-
-		/// <summary>
-		/// The local translation axis in body1.
-		/// </summary>
-		public Vec2 localAxis1;
-
-		/// <summary>
-		/// Enable/disable the joint limit.
-		/// </summary>
-		public bool enableLimit;
-
-		/// <summary>
-		/// The lower translation limit, usually in meters.
-		/// </summary>
-		public float lowerTranslation;
-
-		/// <summary>
-		/// The upper translation limit, usually in meters.
-		/// </summary>
-		public float upperTranslation;
-
-		/// <summary>
-		/// Enable/disable the joint motor.
-		/// </summary>
-		public bool enableMotor;
-
-		/// <summary>
-		/// The maximum motor torque, usually in N-m.
-		/// </summary>
-		public float maxMotorForce;
-
-		/// <summary>
-		/// The desired motor speed in radians per second.
-		/// </summary>
-		public float motorSpeed;
-	}
-
-	/// <summary>
-	/// A line joint. This joint provides one degree of freedom: translation
-	/// along an axis fixed in body1. You can use a joint limit to restrict
-	/// the range of motion and a joint motor to drive the motion or to
-	/// model joint friction.
-	/// </summary>
-	public class LineJoint : Joint
-	{
-		public Vec2 _localAnchor1;
-		public Vec2 _localAnchor2;
-		public Vec2 _localXAxis1;
-		public Vec2 _localYAxis1;
-
-		public Vec2 _axis, _perp;
-		public float _s1, _s2;
-		public float _a1, _a2;
-
-		public Mat22 _K;
-		public Vec2 _impulse;
-
-		public float _motorMass;			// effective mass for motor/limit translational constraint.
-		public float _motorImpulse;
-
-		public float _lowerTranslation;
-		public float _upperTranslation;
-		public float _maxMotorForce;
-		public float _motorSpeed;
-
-		public bool _enableLimit;
-		public bool _enableMotor;
-		public LimitState _limitState;
-
-		public LineJoint(LineJointDef def)
-			: base(def)
-		{
-			_localAnchor1 = def.localAnchor1;
-			_localAnchor2 = def.localAnchor2;
-			_localXAxis1 = def.localAxis1;
-			_localYAxis1 = Vec2.Cross(1.0f, _localXAxis1);
-
-			_impulse.SetZero();
-			_motorMass = 0.0f;
-			_motorImpulse = 0.0f;
-
-			_lowerTranslation = def.lowerTranslation;
-			_upperTranslation = def.upperTranslation;
-			_maxMotorForce = Settings.FORCE_INV_SCALE(def.maxMotorForce);
-			_motorSpeed = def.motorSpeed;
-			_enableLimit = def.enableLimit;
-			_enableMotor = def.enableMotor;
-			_limitState = LimitState.InactiveLimit;
-
-			_axis.SetZero();
-			_perp.SetZero();
-		}
-
-		public override Vec2 Anchor1
-		{
-			get { return _body1.GetWorldPoint(_localAnchor1); }
-		}
-
-		public override Vec2 Anchor2
-		{
-			get { return _body2.GetWorldPoint(_localAnchor2); }
-		}
-
-		public override Vec2 GetReactionForce(float inv_dt)
-		{
-			return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis);
-		}
-
-		public override float GetReactionTorque(float inv_dt)
-		{
-			return 0.0f;
-		}
-
-		/// <summary>
-		/// Get the current joint translation, usually in meters.
-		/// </summary>
-		public float GetJointTranslation()
-		{
-			Body b1 = _body1;
-			Body b2 = _body2;
-
-			Vec2 p1 = b1.GetWorldPoint(_localAnchor1);
-			Vec2 p2 = b2.GetWorldPoint(_localAnchor2);
-			Vec2 d = p2 - p1;
-			Vec2 axis = b1.GetWorldVector(_localXAxis1);
-
-			float translation = Vec2.Dot(d, axis);
-			return translation;
-		}
-
-		/// <summary>
-		/// Get the current joint translation speed, usually in meters per second.
-		/// </summary>
-		public float GetJointSpeed()
-		{
-			Body b1 = _body1;
-			Body b2 = _body2;
-
-			Vec2 r1 = Box2DX.Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
-			Vec2 r2 = Box2DX.Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
-			Vec2 p1 = b1._sweep.C + r1;
-			Vec2 p2 = b2._sweep.C + r2;
-			Vec2 d = p2 - p1;
-			Vec2 axis = b1.GetWorldVector(_localXAxis1);
-
-			Vec2 v1 = b1._linearVelocity;
-			Vec2 v2 = b2._linearVelocity;
-			float w1 = b1._angularVelocity;
-			float w2 = b2._angularVelocity;
-
-			float speed = Vec2.Dot(d, Vec2.Cross(w1, axis)) + Vec2.Dot(axis, v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1));
-			return speed;
-		}
-
-		/// <summary>
-		/// Is the joint limit enabled?
-		/// </summary>
-		public bool IsLimitEnabled()
-		{
-			return _enableLimit;
-		}
-
-		/// <summary>
-		/// Enable/disable the joint limit.
-		/// </summary>
-		public void EnableLimit(bool flag)
-		{
-			_body1.WakeUp();
-			_body2.WakeUp();
-			_enableLimit = flag;
-		}
-
-		/// <summary>
-		/// Get the lower joint limit, usually in meters.
-		/// </summary>
-		public float GetLowerLimit()
-		{
-			return _lowerTranslation;
-		}
-
-		/// <summary>
-		/// Get the upper joint limit, usually in meters.
-		/// </summary>
-		public float GetUpperLimit()
-		{
-			return _upperTranslation;
-		}
-
-		/// <summary>
-		/// Set the joint limits, usually in meters.
-		/// </summary>
-		public void SetLimits(float lower, float upper)
-		{
-			Box2DXDebug.Assert(lower <= upper);
-			_body1.WakeUp();
-			_body2.WakeUp();
-			_lowerTranslation = lower;
-			_upperTranslation = upper;
-		}
-
-		/// <summary>
-		/// Is the joint motor enabled?
-		/// </summary>
-		public bool IsMotorEnabled()
-		{
-			return _enableMotor;
-		}
-
-		/// <summary>
-		/// Enable/disable the joint motor.
-		/// </summary>
-		public void EnableMotor(bool flag)
-		{
-			_body1.WakeUp();
-			_body2.WakeUp();
-			_enableMotor = flag;
-		}
-
-		/// <summary>
-		/// Set the motor speed, usually in meters per second.
-		/// </summary>
-		public void SetMotorSpeed(float speed)
-		{
-			_body1.WakeUp();
-			_body2.WakeUp();
-			_motorSpeed = speed;
-		}
-
-		/// <summary>
-		/// Set the maximum motor force, usually in N.
-		/// </summary>
-		public void SetMaxMotorForce(float force)
-		{
-			_body1.WakeUp();
-			_body2.WakeUp();
-			_maxMotorForce = Settings.FORCE_SCALE(1.0f) * force;
-		}
-
-		/// <summary>
-		/// Get the current motor force, usually in N.
-		/// </summary>
-		public float GetMotorForce()
-		{
-			return _motorImpulse;
-		}
-
-		/// <summary>
-		/// Get the motor speed, usually in meters per second.
-		/// </summary>
-		public float GetMotorSpeed()
-		{
-			return _motorSpeed;
-		}
-
-		internal override void InitVelocityConstraints(TimeStep step)
-		{
-			Body b1 = _body1;
-			Body b2 = _body2;
-
-			_localCenter1 = b1.GetLocalCenter();
-			_localCenter2 = b2.GetLocalCenter();
-
-			XForm xf1 = b1.GetXForm();
-			XForm xf2 = b2.GetXForm();
-
-			// Compute the effective masses.
-			Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
-			Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
-			Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
-
-			_invMass1 = b1._invMass;
-			_invI1 = b1._invI;
-			_invMass2 = b2._invMass;
-			_invI2 = b2._invI;
-
-			// Compute motor Jacobian and effective mass.
-			{
-				_axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1);
-				_a1 = Vec2.Cross(d + r1, _axis);
-				_a2 = Vec2.Cross(r2, _axis);
-
-				_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
-				Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
-				_motorMass = 1.0f / _motorMass;
-			}
-
-			// Prismatic constraint.
-			{
-				_perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1);
-
-				_s1 = Vec2.Cross(d + r1, _perp);
-				_s2 = Vec2.Cross(r2, _perp);
-
-				float m1 = _invMass1, m2 = _invMass2;
-				float i1 = _invI1, i2 = _invI2;
-
-				float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-				float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
-				float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
-
-				_K.Col1.Set(k11, k12);
-				_K.Col2.Set(k12, k22);
-			}
-
-			// Compute motor and limit terms.
-			if (_enableLimit)
-			{
-				float jointTranslation = Vec2.Dot(_axis, d);
-				if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
-				{
-					_limitState = LimitState.EqualLimits;
-				}
-				else if (jointTranslation <= _lowerTranslation)
-				{
-					if (_limitState != LimitState.AtLowerLimit)
-					{
-						_limitState = LimitState.AtLowerLimit;
-						_impulse.Y = 0.0f;
-					}
-				}
-				else if (jointTranslation >= _upperTranslation)
-				{
-					if (_limitState != LimitState.AtUpperLimit)
-					{
-						_limitState = LimitState.AtUpperLimit;
-						_impulse.Y = 0.0f;
-					}
-				}
-				else
-				{
-					_limitState = LimitState.InactiveLimit;
-					_impulse.Y = 0.0f;
-				}
-			}
-			else
-			{
-				_limitState = LimitState.InactiveLimit;
-			}
-
-			if (_enableMotor == false)
-			{
-				_motorImpulse = 0.0f;
-			}
-
-			if (step.WarmStarting)
-			{
-				// Account for variable time step.
-				_impulse *= step.DtRatio;
-				_motorImpulse *= step.DtRatio;
-
-				Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
-				float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
-				float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;
-
-				b1._linearVelocity -= _invMass1 * P;
-				b1._angularVelocity -= _invI1 * L1;
-
-				b2._linearVelocity += _invMass2 * P;
-				b2._angularVelocity += _invI2 * L2;
-			}
-			else
-			{
-				_impulse.SetZero();
-				_motorImpulse = 0.0f;
-			}
-		}
-		
-		internal override void SolveVelocityConstraints(TimeStep step)
-		{
-			Body b1 = _body1;
-			Body b2 = _body2;
-
-			Vec2 v1 = b1._linearVelocity;
-			float w1 = b1._angularVelocity;
-			Vec2 v2 = b2._linearVelocity;
-			float w2 = b2._angularVelocity;
-
-			// Solve linear motor constraint.
-			if (_enableMotor && _limitState != LimitState.EqualLimits)
-			{
-				float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
-				float impulse = _motorMass * (_motorSpeed - Cdot);
-				float oldImpulse = _motorImpulse;
-				float maxImpulse = step.Dt * _maxMotorForce;
-				_motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
-				impulse = _motorImpulse - oldImpulse;
-
-				Vec2 P = impulse * _axis;
-				float L1 = impulse * _a1;
-				float L2 = impulse * _a2;
-
-				v1 -= _invMass1 * P;
-				w1 -= _invI1 * L1;
-
-				v2 += _invMass2 * P;
-				w2 += _invI2 * L2;
-			}
-
-			float Cdot1 = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
-
-			if (_enableLimit && _limitState != LimitState.InactiveLimit)
-			{
-				// Solve prismatic and limit constraint in block form.
-				float Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
-				Vec2 Cdot = new Vec2(Cdot1, Cdot2);
-
-				Vec2 f1 = _impulse;
-				Vec2 df =  _K.Solve(-Cdot);
-				_impulse += df;
-
-				if (_limitState == LimitState.AtLowerLimit)
-				{
-					_impulse.Y = Box2DX.Common.Math.Max(_impulse.Y, 0.0f);
-				}
-				else if (_limitState == LimitState.AtUpperLimit)
-				{
-					_impulse.Y = Box2DX.Common.Math.Min(_impulse.Y, 0.0f);
-				}
-
-				// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
-				float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.Col2.X;
-				float f2r = b / _K.Col1.X + f1.X;
-				_impulse.X = f2r;
-
-				df = _impulse - f1;
-
-				Vec2 P = df.X * _perp + df.Y * _axis;
-				float L1 = df.X * _s1 + df.Y * _a1;
-				float L2 = df.X * _s2 + df.Y * _a2;
-
-				v1 -= _invMass1 * P;
-				w1 -= _invI1 * L1;
-
-				v2 += _invMass2 * P;
-				w2 += _invI2 * L2;
-			}
-			else
-			{
-				// Limit is inactive, just solve the prismatic constraint in block form.
-				float df = (-Cdot1) / _K.Col1.X;
-				_impulse.X += df;
-
-				Vec2 P = df * _perp;
-				float L1 = df * _s1;
-				float L2 = df * _s2;
-
-				v1 -= _invMass1 * P;
-				w1 -= _invI1 * L1;
-
-				v2 += _invMass2 * P;
-				w2 += _invI2 * L2;
-			}
-
-			b1._linearVelocity = v1;
-			b1._angularVelocity = w1;
-			b2._linearVelocity = v2;
-			b2._angularVelocity = w2;
-		}
-
-		internal override bool SolvePositionConstraints(float baumgarte)
-		{
-			Body b1 = _body1;
-			Body b2 = _body2;
-
-			Vec2 c1 = b1._sweep.C;
-			float a1 = b1._sweep.A;
-
-			Vec2 c2 = b2._sweep.C;
-			float a2 = b2._sweep.A;
-
-			// Solve linear limit constraint.
-			float linearError = 0.0f, angularError = 0.0f;
-			bool active = false;
-			float C2 = 0.0f;
-
-			Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
-
-			Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
-			Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
-			Vec2 d = c2 + r2 - c1 - r1;
-
-			if (_enableLimit)
-			{
-				_axis = Box2DX.Common.Math.Mul(R1, _localXAxis1);
-
-				_a1 = Vec2.Cross(d + r1, _axis);
-				_a2 = Vec2.Cross(r2, _axis);
-
-				float translation = Vec2.Dot(_axis, d);
-				if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
-				{
-					// Prevent large angular corrections
-					C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
-					linearError = Box2DX.Common.Math.Abs(translation);
-					active = true;
-				}
-				else if (translation <= _lowerTranslation)
-				{
-					// Prevent large linear corrections and allow some slop.
-					C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
-					linearError = _lowerTranslation - translation;
-					active = true;
-				}
-				else if (translation >= _upperTranslation)
-				{
-					// Prevent large linear corrections and allow some slop.
-					C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
-					linearError = translation - _upperTranslation;
-					active = true;
-				}
-			}
-
-			_perp = Box2DX.Common.Math.Mul(R1, _localYAxis1);
-
-			_s1 = Vec2.Cross(d + r1, _perp);
-			_s2 = Vec2.Cross(r2, _perp);
-
-			Vec2 impulse;
-			float C1;
-			C1 = Vec2.Dot(_perp, d);
-
-			linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1));
-			angularError = 0.0f;
-
-			if (active)
-			{
-				float m1 = _invMass1, m2 = _invMass2;
-				float i1 = _invI1, i2 = _invI2;
-
-				float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-				float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
-				float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
-
-				_K.Col1.Set(k11, k12);
-				_K.Col2.Set(k12, k22);
-
-				Vec2 C = new Vec2();
-				C.X = C1;
-				C.Y = C2;
-
-				impulse = _K.Solve(-C);
-			}
-			else
-			{
-				float m1 = _invMass1, m2 = _invMass2;
-				float i1 = _invI1, i2 = _invI2;
-
-				float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-
-				float impulse1 = (-C1) / k11;
-				impulse.X = impulse1;
-				impulse.Y = 0.0f;
-			}
-
-			Vec2 P = impulse.X * _perp + impulse.Y * _axis;
-			float L1 = impulse.X * _s1 + impulse.Y * _a1;
-			float L2 = impulse.X * _s2 + impulse.Y * _a2;
-
-			c1 -= _invMass1 * P;
-			a1 -= _invI1 * L1;
-			c2 += _invMass2 * P;
-			a2 += _invI2 * L2;
-
-			// TODO_ERIN remove need for this.
-			b1._sweep.C = c1;
-			b1._sweep.A = a1;
-			b2._sweep.C = c2;
-			b2._sweep.A = a2;
-			b1.SynchronizeTransform();
-			b2.SynchronizeTransform();
-
-			return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
-		}
-	}
-}
\ No newline at end of file
+
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs
index 19eb83c..57a89a7 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/MouseJoint.cs
@@ -48,6 +48,7 @@ namespace Box2DX.Dynamics
 			MaxForce = 0.0f;
 			FrequencyHz = 5.0f;
 			DampingRatio = 0.7f;
+			TimeStep = 1.0f / 60.0f;
 		}
 
 		/// <summary>
@@ -72,6 +73,11 @@ namespace Box2DX.Dynamics
 		/// The damping ratio. 0 = no damping, 1 = critical damping.
 		/// </summary>
 		public float DampingRatio;
+
+		/// <summary>
+		/// The time step used in the simulation.
+		/// </summary>
+		public float TimeStep;
 	}
 
 	/// <summary>
@@ -89,10 +95,8 @@ namespace Box2DX.Dynamics
 		public Mat22 _mass;		// effective mass for point-to-point constraint.
 		public Vec2 _C;				// position error
 		public float _maxForce;
-		public float _frequencyHz;
-		public float _dampingRatio;
-		public float _beta;
-		public float _gamma;
+		public float _beta;			// bias factor
+		public float _gamma;		// softness
 
 		public override Vec2 Anchor1
 		{
@@ -104,19 +108,20 @@ namespace Box2DX.Dynamics
 			get { return _body2.GetWorldPoint(_localAnchor); }
 		}
 
-		public override Vec2 GetReactionForce(float inv_dt)
+		public override Vec2 ReactionForce
 		{
-			return inv_dt * _impulse;
+			get { return Settings.FORCE_SCALE(1.0f) * _impulse; }
 		}
 
-		public override float GetReactionTorque(float inv_dt)
+		public override float ReactionTorque
 		{
-			return inv_dt * 0.0f;
+			get { return 0.0f; }
 		}
 
 		/// <summary>
 		/// Use this to update the target point.
 		/// </summary>
+		/// <param name="target"></param>
 		public void SetTarget(Vec2 target)
 		{
 			if (_body2.IsSleeping())
@@ -132,37 +137,29 @@ namespace Box2DX.Dynamics
 			_target = def.Target;
 			_localAnchor = Common.Math.MulT(_body2.GetXForm(), _target);
 
-			_maxForce = def.MaxForce;
+			_maxForce = Settings.FORCE_INV_SCALE(def.MaxForce);
 			_impulse.SetZero();
 
-			_frequencyHz = def.FrequencyHz;
-			_dampingRatio = def.DampingRatio;
-
-			_beta = 0.0f;
-			_gamma = 0.0f;
-		}
-
-		internal override void InitVelocityConstraints(TimeStep step)
-		{
-			Body b = _body2;
-
-			float mass = b.GetMass();
+			float mass = _body2._mass;
 
 			// Frequency
-			float omega = 2.0f * Settings.Pi * _frequencyHz;
+			float omega = 2.0f * Settings.Pi * def.FrequencyHz;
 
 			// Damping coefficient
-			float d = 2.0f * mass * _dampingRatio * omega;
+			float d = 2.0f * mass * def.DampingRatio * omega;
 
 			// Spring stiffness
-			float k = mass * (omega * omega);
+			float k = (def.TimeStep * mass) * (omega * omega);
 
 			// magic formulas
-			// gamma has units of inverse mass.
-			// beta has units of inverse time.
-			Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON);
-			_gamma = 1.0f / (step.Dt * (d + step.Dt * k));
-			_beta = step.Dt * k * _gamma;
+			Box2DXDebug.Assert(d + k > Settings.FLT_EPSILON);
+			_gamma = 1.0f / (d + k);
+			_beta = k / (d + k);
+		}
+
+		internal override void InitVelocityConstraints(TimeStep step)
+		{
+			Body b = _body2;
 
 			// Compute the effective mass matrix.
 			Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());
@@ -185,7 +182,7 @@ namespace Box2DX.Dynamics
 			K.Col1.X += _gamma;
 			K.Col2.Y += _gamma;
 
-			_mass = K.GetInverse();
+			_mass = K.Invert();
 
 			_C = b._sweep.C + r - _target;
 
@@ -193,9 +190,9 @@ namespace Box2DX.Dynamics
 			b._angularVelocity *= 0.98f;
 
 			// Warm starting.
-			_impulse *= step.DtRatio;
-			b._linearVelocity += invMass * _impulse;
-			b._angularVelocity += invI * Vec2.Cross(r, _impulse);
+			Vec2 P = Settings.FORCE_SCALE(step.Dt) * _impulse;
+			b._linearVelocity += invMass * P;
+			b._angularVelocity += invI * Vec2.Cross(r, P);
 		}
 
 		internal override void SolveVelocityConstraints(TimeStep step)
@@ -206,24 +203,26 @@ namespace Box2DX.Dynamics
 
 			// Cdot = v + cross(w, r)
 			Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r);
-			Vec2 impulse = Box2DX.Common.Math.Mul(_mass, -(Cdot + _beta * _C + _gamma * _impulse));
+			Vec2 force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Common.Math.Mul(_mass, Cdot + 
+				(_beta * step.Inv_Dt) * _C + Settings.FORCE_SCALE(step.Dt) * (_gamma * _impulse));
 
-			Vec2 oldImpulse = _impulse;
-			_impulse += impulse;
-			float maxImpulse = step.Dt * _maxForce;
-			if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
+			Vec2 oldForce = _impulse;
+			_impulse += force;
+			float forceMagnitude = _impulse.Length();
+			if (forceMagnitude > _maxForce)
 			{
-				_impulse *= maxImpulse / _impulse.Length();
+				_impulse *= _maxForce / forceMagnitude;
 			}
-			impulse = _impulse - oldImpulse;
+			force = _impulse - oldForce;
 
-			b._linearVelocity += b._invMass * impulse;
-			b._angularVelocity += b._invI * Vec2.Cross(r, impulse);
+			Vec2 P = Settings.FORCE_SCALE(step.Dt) * force;
+			b._linearVelocity += b._invMass * P;
+			b._angularVelocity += b._invI * Vec2.Cross(r, P);
 		}
 
-		internal override bool SolvePositionConstraints(float baumgarte)
+		internal override bool SolvePositionConstraints()
 		{
 			return true;
 		}
 	}
-}
+}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs
index c6ee2cf..8234d31 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PrismaticJoint.cs
@@ -21,71 +21,21 @@
 
 // Linear constraint (point-to-line)
 // d = p2 - p1 = x2 + r2 - x1 - r1
-// C = dot(perp, d)
-// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
-//      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
-// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+// C = dot(ay1, d)
+// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//      = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
+// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
 //
 // Angular constraint
 // C = a2 - a1 + a_initial
 // Cdot = w2 - w1
 // J = [0 0 -1 0 0 1]
-//
-// K = J * invM * JT
-//
-// J = [-a -s1 a s2]
-//     [0  -1  0  1]
-// a = perp
-// s1 = cross(d + r1, a) = cross(p2 - x1, a)
-// s2 = cross(r2, a) = cross(p2 - x2, a)
-
 
 // Motor/Limit linear constraint
 // C = dot(ax1, d)
 // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
 // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
 
-// Block Solver
-// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
-// when the mass has poor distribution (leading to large torques about the joint anchor points).
-//
-// The Jacobian has 3 rows:
-// J = [-uT -s1 uT s2] // linear
-//     [0   -1   0  1] // angular
-//     [-vT -a1 vT a2] // limit
-//
-// u = perp
-// v = axis
-// s1 = cross(d + r1, u), s2 = cross(r2, u)
-// a1 = cross(d + r1, v), a2 = cross(r2, v)
-
-// M * (v2 - v1) = JT * df
-// J * v2 = bias
-//
-// v2 = v1 + invM * JT * df
-// J * (v1 + invM * JT * df) = bias
-// K * df = bias - J * v1 = -Cdot
-// K = J * invM * JT
-// Cdot = J * v1 - bias
-//
-// Now solve for f2.
-// df = f2 - f1
-// K * (f2 - f1) = -Cdot
-// f2 = invK * (-Cdot) + f1
-//
-// Clamp accumulated limit impulse.
-// lower: f2(3) = max(f2(3), 0)
-// upper: f2(3) = min(f2(3), 0)
-//
-// Solve for correct f2(1:2)
-// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
-//                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
-// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
-// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-//
-// Now compute impulse to be applied:
-// df = f2 - f1
-
 using System;
 using System.Collections.Generic;
 using System.Text;
@@ -126,6 +76,10 @@ namespace Box2DX.Dynamics
 		/// Initialize the bodies, anchors, axis, and reference angle using the world
 		/// anchor and world axis.
 		/// </summary>
+		/// <param name="body1"></param>
+		/// <param name="body2"></param>
+		/// <param name="anchor"></param>
+		/// <param name="axis"></param>
 		public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis)
 		{
 			Body1 = body1;
@@ -201,15 +155,18 @@ namespace Box2DX.Dynamics
 		public Vec2 _localYAxis1;
 		public float _refAngle;
 
-		public Vec2 _axis, _perp;
-		public float _s1, _s2;
-		public float _a1, _a2;
+		public Jacobian _linearJacobian;
+		public float _linearMass;				// effective mass for point-to-line constraint.
+		public float _force;
 
-		public Mat33 _K;
-		public Vec3 _impulse;
+		public float _angularMass;			// effective mass for angular constraint.
+		public float _torque;
 
+		public Jacobian _motorJacobian;
 		public float _motorMass;			// effective mass for motor/limit translational constraint.
-		public float _motorImpulse;
+		public float _motorForce;
+		public float _limitForce;
+		public float _limitPositionImpulse;
 
 		public float _lowerTranslation;
 		public float _upperTranslation;
@@ -230,14 +187,20 @@ namespace Box2DX.Dynamics
 			get { return _body2.GetWorldPoint(_localAnchor2); }
 		}
 
-		public override Vec2 GetReactionForce(float inv_dt)
+		public override Vec2 ReactionForce
 		{
-			return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis);
+			get
+			{
+				Vec2 ax1 = Common.Math.Mul(_body1.GetXForm().R, _localXAxis1);
+				Vec2 ay1 = Common.Math.Mul(_body1.GetXForm().R, _localYAxis1);
+
+				return Settings.FORCE_SCALE(1.0f) * (_limitForce * ax1 + _force * ay1);
+			}
 		}
 
-		public override float GetReactionTorque(float inv_dt)
+		public override float ReactionTorque
 		{
-			return inv_dt * _impulse.Y;
+			get { return Settings.FORCE_SCALE(_torque); }
 		}
 
 		/// <summary>
@@ -298,10 +261,9 @@ namespace Box2DX.Dynamics
 		/// <summary>
 		/// Enable/disable the joint limit.
 		/// </summary>
+		/// <param name="flag"></param>
 		public void EnableLimit(bool flag)
 		{
-			_body1.WakeUp();
-			_body2.WakeUp();
 			_enableLimit = flag;
 		}
 
@@ -324,11 +286,11 @@ namespace Box2DX.Dynamics
 		/// <summary>
 		/// Set the joint limits, usually in meters.
 		/// </summary>
+		/// <param name="lower"></param>
+		/// <param name="upper"></param>
 		public void SetLimits(float lower, float upper)
 		{
 			Box2DXDebug.Assert(lower <= upper);
-			_body1.WakeUp();
-			_body2.WakeUp();
 			_lowerTranslation = lower;
 			_upperTranslation = upper;
 		}
@@ -344,10 +306,9 @@ namespace Box2DX.Dynamics
 		/// <summary>
 		/// Enable/disable the joint motor.
 		/// </summary>
+		/// <param name="flag"></param>
 		public void EnableMotor(bool flag)
 		{
-			_body1.WakeUp();
-			_body2.WakeUp();
 			_enableMotor = flag;
 		}
 
@@ -357,21 +318,15 @@ namespace Box2DX.Dynamics
 		public float MotorSpeed
 		{
 			get { return _motorSpeed; }
-			set
-			{
-				_body1.WakeUp();
-				_body2.WakeUp();
-				_motorSpeed = value;
-			}
+			set { _motorSpeed = value; }
 		}
 
 		/// <summary>
 		/// Set the maximum motor force, usually in N.
 		/// </summary>
+		/// <param name="torque"></param>
 		public void SetMaxMotorForce(float force)
 		{
-			_body1.WakeUp();
-			_body2.WakeUp();
 			_maxMotorForce = Settings.FORCE_SCALE(1.0f) * force;
 		}
 
@@ -380,7 +335,7 @@ namespace Box2DX.Dynamics
 		/// </summary>
 		public float MotorForce
 		{
-			get { return _motorImpulse; }
+			get { return _motorForce; }
 		}
 
 		public PrismaticJoint(PrismaticJointDef def)
@@ -392,9 +347,18 @@ namespace Box2DX.Dynamics
 			_localYAxis1 = Vec2.Cross(1.0f, _localXAxis1);
 			_refAngle = def.ReferenceAngle;
 
-			_impulse.SetZero();
+			_linearJacobian.SetZero();
+			_linearMass = 0.0f;
+			_force = 0.0f;
+
+			_angularMass = 0.0f;
+			_torque = 0.0f;
+
+			_motorJacobian.SetZero();
 			_motorMass = 0.0f;
-			_motorImpulse = 0.0f;
+			_motorForce = 0.0f;
+			_limitForce = 0.0f;
+			_limitPositionImpulse = 0.0f;
 
 			_lowerTranslation = def.LowerTranslation;
 			_upperTranslation = def.UpperTranslation;
@@ -402,10 +366,6 @@ namespace Box2DX.Dynamics
 			_motorSpeed = def.MotorSpeed;
 			_enableLimit = def.EnableLimit;
 			_enableMotor = def.EnableMotor;
-			_limitState = LimitState.InactiveLimit;
-
-			_axis.SetZero();
-			_perp.SetZero();
 		}
 
 		internal override void InitVelocityConstraints(TimeStep step)
@@ -413,344 +373,286 @@ namespace Box2DX.Dynamics
 			Body b1 = _body1;
 			Body b2 = _body2;
 
-			// You cannot create a prismatic joint between bodies that
-			// both have fixed rotation.
-			Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
-
-			_localCenter1 = b1.GetLocalCenter();
-			_localCenter2 = b2.GetLocalCenter();
-
-			XForm xf1 = b1.GetXForm();
-			XForm xf2 = b2.GetXForm();
-
 			// Compute the effective masses.
-			Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
-			Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
-			Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
-
-			_invMass1 = b1._invMass;
-			_invI1 = b1._invI;
-			_invMass2 = b2._invMass;
-			_invI2 = b2._invI;
-
-			// Compute motor Jacobian and effective mass.
+			Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
+			Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
+
+			float invMass1 = b1._invMass, invMass2 = b2._invMass;
+			float invI1 = b1._invI, invI2 = b2._invI;
+
+			// Compute point to line constraint effective mass.
+			// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+			Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
+			Vec2 e = b2._sweep.C + r2 - b1._sweep.C;	// e = d + r1
+
+			_linearJacobian.Set(-ay1, -Vec2.Cross(e, ay1), ay1, Vec2.Cross(r2, ay1));
+			_linearMass = invMass1 + invI1 * _linearJacobian.Angular1 * _linearJacobian.Angular1 +
+							invMass2 + invI2 * _linearJacobian.Angular2 * _linearJacobian.Angular2;
+			Box2DXDebug.Assert(_linearMass > Settings.FLT_EPSILON);
+			_linearMass = 1.0f / _linearMass;
+
+			// Compute angular constraint effective mass.
+			_angularMass = invI1 + invI2;
+			if (_angularMass > Settings.FLT_EPSILON)
 			{
-				_axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1);
-				_a1 = Vec2.Cross(d + r1, _axis);
-				_a2 = Vec2.Cross(r2, _axis);
-
-				_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
-				Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
-				_motorMass = 1.0f / _motorMass;
-			}
-
-			// Prismatic constraint.
-			{
-				_perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1);
-
-				_s1 = Vec2.Cross(d + r1, _perp);
-				_s2 = Vec2.Cross(r2, _perp);
-
-				float m1 = _invMass1, m2 = _invMass2;
-				float i1 = _invI1, i2 = _invI2;
-
-				float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-				float k12 = i1 * _s1 + i2 * _s2;
-				float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
-				float k22 = i1 + i2;
-				float k23 = i1 * _a1 + i2 * _a2;
-				float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
-
-				_K.Col1.Set(k11, k12, k13);
-				_K.Col2.Set(k12, k22, k23);
-				_K.Col3.Set(k13, k23, k33);
+				_angularMass = 1.0f / _angularMass;
 			}
 
 			// Compute motor and limit terms.
-			if (_enableLimit)
+			if (_enableLimit || _enableMotor)
 			{
-				float jointTranslation = Vec2.Dot(_axis, d);
-				if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
-				{
-					_limitState = LimitState.EqualLimits;
-				}
-				else if (jointTranslation <= _lowerTranslation)
+				// The motor and limit share a Jacobian and effective mass.
+				Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
+				_motorJacobian.Set(-ax1, -Vec2.Cross(e, ax1), ax1, Vec2.Cross(r2, ax1));
+				_motorMass = invMass1 + invI1 * _motorJacobian.Angular1 * _motorJacobian.Angular1 +
+								invMass2 + invI2 * _motorJacobian.Angular2 * _motorJacobian.Angular2;
+				Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
+				_motorMass = 1.0f / _motorMass;
+
+				if (_enableLimit)
 				{
-					if (_limitState != LimitState.AtLowerLimit)
+					Vec2 d = e - r1;	// p2 - p1
+					float jointTranslation = Vec2.Dot(ax1, d);
+					if (Box2DXMath.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
+					{
+						_limitState = LimitState.EqualLimits;
+					}
+					else if (jointTranslation <= _lowerTranslation)
 					{
+						if (_limitState != LimitState.AtLowerLimit)
+						{
+							_limitForce = 0.0f;
+						}
 						_limitState = LimitState.AtLowerLimit;
-						_impulse.Z = 0.0f;
 					}
-				}
-				else if (jointTranslation >= _upperTranslation)
-				{
-					if (_limitState != LimitState.AtUpperLimit)
+					else if (jointTranslation >= _upperTranslation)
 					{
+						if (_limitState != LimitState.AtUpperLimit)
+						{
+							_limitForce = 0.0f;
+						}
 						_limitState = LimitState.AtUpperLimit;
-						_impulse.Z = 0.0f;
 					}
-				}
-				else
-				{
-					_limitState = LimitState.InactiveLimit;
-					_impulse.Z = 0.0f;
+					else
+					{
+						_limitState = LimitState.InactiveLimit;
+						_limitForce = 0.0f;
+					}
 				}
 			}
-			else
+
+			if (_enableMotor == false)
 			{
-				_limitState = LimitState.InactiveLimit;
+				_motorForce = 0.0f;
 			}
 
-			if (_enableMotor == false)
+			if (_enableLimit == false)
 			{
-				_motorImpulse = 0.0f;
+				_limitForce = 0.0f;
 			}
 
 			if (step.WarmStarting)
 			{
-				// Account for variable time step.
-				_impulse *= step.DtRatio;
-				_motorImpulse *= step.DtRatio;
+				Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear1 + (_motorForce + _limitForce) * _motorJacobian.Linear1);
+				Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear2 + (_motorForce + _limitForce) * _motorJacobian.Linear2);
+				float L1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular1 - _torque + (_motorForce + _limitForce) * _motorJacobian.Angular1);
+				float L2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular2 + _torque + (_motorForce + _limitForce) * _motorJacobian.Angular2);
 
-				Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
-				float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
-				float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
+				b1._linearVelocity += invMass1 * P1;
+				b1._angularVelocity += invI1 * L1;
 
-				b1._linearVelocity -= _invMass1 * P;
-				b1._angularVelocity -= _invI1 * L1;
-
-				b2._linearVelocity += _invMass2 * P;
-				b2._angularVelocity += _invI2 * L2;
+				b2._linearVelocity += invMass2 * P2;
+				b2._angularVelocity += invI2 * L2;
 			}
 			else
 			{
-				_impulse.SetZero();
-				_motorImpulse = 0.0f;
+				_force = 0.0f;
+				_torque = 0.0f;
+				_limitForce = 0.0f;
+				_motorForce = 0.0f;
 			}
+
+			_limitPositionImpulse = 0.0f;
 		}
 
 		internal override void SolveVelocityConstraints(TimeStep step)
 		{
 			Body b1 = _body1;
-			Body b2  = _body2;
+			Body b2 = _body2;
+
+			float invMass1 = b1._invMass, invMass2 = b2._invMass;
+			float invI1 = b1._invI, invI2 = b2._invI;
+
+			// Solve linear constraint.
+			float linearCdot = _linearJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
+			float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _linearMass * linearCdot;
+			_force += force;
 
-			Vec2 v1 = b1._linearVelocity;
-			float w1 = b1._angularVelocity;
-			Vec2 v2 = b2._linearVelocity;
-			float w2 = b2._angularVelocity;
+			float P = Settings.FORCE_SCALE(step.Dt) * force;
+			b1._linearVelocity += (invMass1 * P) * _linearJacobian.Linear1;
+			b1._angularVelocity += invI1 * P * _linearJacobian.Angular1;
+
+			b2._linearVelocity += (invMass2 * P) * _linearJacobian.Linear2;
+			b2._angularVelocity += invI2 * P * _linearJacobian.Angular2;
+
+			// Solve angular constraint.
+			float angularCdot = b2._angularVelocity - b1._angularVelocity;
+			float torque = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _angularMass * angularCdot;
+			_torque += torque;
+
+			float L = Settings.FORCE_SCALE(step.Dt) * torque;
+			b1._angularVelocity -= invI1 * L;
+			b2._angularVelocity += invI2 * L;
 
 			// Solve linear motor constraint.
 			if (_enableMotor && _limitState != LimitState.EqualLimits)
 			{
-				float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
-				float impulse = _motorMass * (_motorSpeed - Cdot);
-				float oldImpulse = _motorImpulse;
-				float maxImpulse = step.Dt * _maxMotorForce;
-				_motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
-				impulse = _motorImpulse - oldImpulse;
-
-				Vec2 P = impulse * _axis;
-				float L1 = impulse * _a1;
-				float L2 = impulse * _a2;
-
-				v1 -= _invMass1 * P;
-				w1 -= _invI1 * L1;
-
-				v2 += _invMass2 * P;
-				w2 += _invI2 * L2;
+				float motorCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity) - _motorSpeed;
+				float motorForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * motorCdot;
+				float oldMotorForce = _motorForce;
+				_motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorForce, _maxMotorForce);
+				motorForce = _motorForce - oldMotorForce;
+
+				float P_ = Settings.FORCE_SCALE(step.Dt) * motorForce;
+				b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1;
+				b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1;
+
+				b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2;
+				b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2;
 			}
 
-			Vec2 Cdot1;
-			Cdot1.X = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
-			Cdot1.Y = w2 - w1;
-
+			// Solve linear limit constraint.
 			if (_enableLimit && _limitState != LimitState.InactiveLimit)
 			{
-				// Solve prismatic and limit constraint in block form.
-				float Cdot2;
-				Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
-				Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);
-
-				Vec3 f1 = _impulse;
-				Vec3 df =  _K.Solve33(-Cdot);
-				_impulse += df;
+				float limitCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
+				float limitForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * limitCdot;
 
-				if (_limitState ==LimitState.AtLowerLimit)
+				if (_limitState == LimitState.EqualLimits)
 				{
-					_impulse.Z = Box2DX.Common.Math.Max(_impulse.Z, 0.0f);
+					_limitForce += limitForce;
+				}
+				else if (_limitState == LimitState.AtLowerLimit)
+				{
+					float oldLimitForce = _limitForce;
+					_limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f);
+					limitForce = _limitForce - oldLimitForce;
 				}
 				else if (_limitState == LimitState.AtUpperLimit)
 				{
-					_impulse.Z = Box2DX.Common.Math.Min(_impulse.Z, 0.0f);
+					float oldLimitForce = _limitForce;
+					_limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f);
+					limitForce = _limitForce - oldLimitForce;
 				}
 
-				// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-				Vec2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vec2(_K.Col3.X, _K.Col3.Y);
-				Vec2 f2r = _K.Solve22(b) + new Vec2(f1.X, f1.Y);
-				_impulse.X = f2r.X;
-				_impulse.Y = f2r.Y;
-
-				df = _impulse - f1;
-
-				Vec2 P = df.X * _perp + df.Z * _axis;
-				float L1 = df.X * _s1 + df.Y + df.Z * _a1;
-				float L2 = df.X * _s2 + df.Y + df.Z * _a2;
+				float P_ = Settings.FORCE_SCALE(step.Dt) * limitForce;
 
-				v1 -= _invMass1 * P;
-				w1 -= _invI1 * L1;
+				b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1;
+				b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1;
 
-				v2 += _invMass2 * P;
-				w2 += _invI2 * L2;
+				b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2;
+				b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2;
 			}
-			else
-			{
-				// Limit is inactive, just solve the prismatic constraint in block form.
-				Vec2 df = _K.Solve22(-Cdot1);
-				_impulse.X += df.X;
-				_impulse.Y += df.Y;
-
-				Vec2 P = df.X * _perp;
-				float L1 = df.X * _s1 + df.Y;
-				float L2 = df.X * _s2 + df.Y;
-
-				v1 -= _invMass1 * P;
-				w1 -= _invI1 * L1;
-
-				v2 += _invMass2 * P;
-				w2 += _invI2 * L2;
-			}
-
-			b1._linearVelocity = v1;
-			b1._angularVelocity = w1;
-			b2._linearVelocity = v2;
-			b2._angularVelocity = w2;
 		}
 
-		internal override bool SolvePositionConstraints(float baumgarte)
+		internal override bool SolvePositionConstraints()
 		{
 			Body b1 = _body1;
 			Body b2 = _body2;
 
-			Vec2 c1 = b1._sweep.C;
-			float a1 = b1._sweep.A;
+			float invMass1 = b1._invMass, invMass2 = b2._invMass;
+			float invI1 = b1._invI, invI2 = b2._invI;
 
-			Vec2 c2 = b2._sweep.C;
-			float a2 = b2._sweep.A;
+			Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
+			Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
+			Vec2 p1 = b1._sweep.C + r1;
+			Vec2 p2 = b2._sweep.C + r2;
+			Vec2 d = p2 - p1;
+			Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
 
-			// Solve linear limit constraint.
-			float linearError = 0.0f, angularError = 0.0f;
-			bool active = false;
-			float C2 = 0.0f;
+			// Solve linear (point-to-line) constraint.
+			float linearC = Vec2.Dot(ay1, d);
+			// Prevent overly large corrections.
+			linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
+			float linearImpulse = -_linearMass * linearC;
+
+			b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1;
+			b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1;
+			//b1->SynchronizeTransform(); // updated by angular constraint
+			b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2;
+			b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2;
+			//b2->SynchronizeTransform(); // updated by angular constraint
 
-			Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
+			float positionError = Box2DXMath.Abs(linearC);
 
-			Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
-			Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
-			Vec2 d = c2 + r2 - c1 - r1;
+			// Solve angular constraint.
+			float angularC = b2._sweep.A - b1._sweep.A - _refAngle;
+			// Prevent overly large corrections.
+			angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
+			float angularImpulse = -_angularMass * angularC;
 
-			if (_enableLimit)
+			b1._sweep.A -= b1._invI * angularImpulse;
+			b2._sweep.A += b2._invI * angularImpulse;
+
+			b1.SynchronizeTransform();
+			b2.SynchronizeTransform();
+
+			float angularError = Box2DXMath.Abs(angularC);
+
+			// Solve linear limit constraint.
+			if (_enableLimit && _limitState != LimitState.InactiveLimit)
 			{
-				_axis = Box2DX.Common.Math.Mul(R1, _localXAxis1);
+				Vec2 r1_ = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
+				Vec2 r2_ = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
+				Vec2 p1_ = b1._sweep.C + r1_;
+				Vec2 p2_ = b2._sweep.C + r2_;
+				Vec2 d_ = p2_ - p1_;
+				Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
 
-				_a1 = Vec2.Cross(d + r1, _axis);
-				_a2 = Vec2.Cross(r2, _axis);
+				float translation = Vec2.Dot(ax1, d_);
+				float limitImpulse = 0.0f;
 
-				float translation = Vec2.Dot(_axis, d);
-				if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
+				if (_limitState == LimitState.EqualLimits)
 				{
 					// Prevent large angular corrections
-					C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
-					linearError = Box2DX.Common.Math.Abs(translation);
-					active = true;
+					float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
+					limitImpulse = -_motorMass * limitC;
+					positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC));
 				}
-				else if (translation <= _lowerTranslation)
+				else if (_limitState == LimitState.AtLowerLimit)
 				{
+					float limitC = translation - _lowerTranslation;
+					positionError = Box2DXMath.Max(positionError, -limitC);
+
 					// Prevent large linear corrections and allow some slop.
-					C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
-					linearError = _lowerTranslation - translation;
-					active = true;
+					limitC = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
+					limitImpulse = -_motorMass * limitC;
+					float oldLimitImpulse = _limitPositionImpulse;
+					_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
+					limitImpulse = _limitPositionImpulse - oldLimitImpulse;
 				}
-				else if (translation >= _upperTranslation)
+				else if (_limitState == LimitState.AtUpperLimit)
 				{
+					float limitC = translation - _upperTranslation;
+					positionError = Box2DXMath.Max(positionError, limitC);
+
 					// Prevent large linear corrections and allow some slop.
-					C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
-					linearError = translation - _upperTranslation;
-					active = true;
+					limitC = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
+					limitImpulse = -_motorMass * limitC;
+					float oldLimitImpulse = _limitPositionImpulse;
+					_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
+					limitImpulse = _limitPositionImpulse - oldLimitImpulse;
 				}
-			}
-
-			_perp = Box2DX.Common.Math.Mul(R1, _localYAxis1);
-
-			_s1 = Vec2.Cross(d + r1, _perp);
-			_s2 = Vec2.Cross(r2, _perp);
 
-			Vec3 impulse;
-			Vec2 C1 = new Vec2();
-			C1.X = Vec2.Dot(_perp, d);
-			C1.Y = a2 - a1 - _refAngle;
+				b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1;
+				b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1;
+				b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2;
+				b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2;
 
-			linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.X));
-			angularError = Box2DX.Common.Math.Abs(C1.Y);
-
-			if (active)
-			{
-				float m1 = _invMass1, m2 = _invMass2;
-				float i1 = _invI1, i2 = _invI2;
-
-				float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-				float k12 = i1 * _s1 + i2 * _s2;
-				float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
-				float k22 = i1 + i2;
-				float k23 = i1 * _a1 + i2 * _a2;
-				float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
-
-				_K.Col1.Set(k11, k12, k13);
-				_K.Col2.Set(k12, k22, k23);
-				_K.Col3.Set(k13, k23, k33);
-
-				Vec3 C = new Vec3();
-				C.X = C1.X;
-				C.Y = C1.Y;
-				C.Z = C2;
-
-				impulse = _K.Solve33(-C);
-			}
-			else
-			{
-				float m1 = _invMass1, m2 = _invMass2;
-				float i1 = _invI1, i2 = _invI2;
-
-				float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
-				float k12 = i1 * _s1 + i2 * _s2;
-				float k22 = i1 + i2;
-
-				_K.Col1.Set(k11, k12, 0.0f);
-				_K.Col2.Set(k12, k22, 0.0f);
-
-				Vec2 impulse1 = _K.Solve22(-C1);
-				impulse.X = impulse1.X;
-				impulse.Y = impulse1.Y;
-				impulse.Z = 0.0f;
+				b1.SynchronizeTransform();
+				b2.SynchronizeTransform();
 			}
 
-			Vec2 P = impulse.X * _perp + impulse.Z * _axis;
-			float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
-			float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
-
-			c1 -= _invMass1 * P;
-			a1 -= _invI1 * L1;
-			c2 += _invMass2 * P;
-			a2 += _invI2 * L2;
-
-			// TODO_ERIN remove need for this.
-			b1._sweep.C = c1;
-			b1._sweep.A = a1;
-			b2._sweep.C = c2;
-			b2._sweep.A = a2;
-			b1.SynchronizeTransform();
-			b2.SynchronizeTransform();
-			
-			return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
+			return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
 		}
 	}
-}
+}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs
index ab92536..a7d22f9 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/PulleyJoint.cs
@@ -88,7 +88,7 @@ namespace Box2DX.Dynamics
 			Vec2 d2 = anchor2 - groundAnchor2;
 			Length2 = d2.Length();
 			Ratio = ratio;
-			Box2DXDebug.Assert(ratio > Settings.FLT_EPSILON);
+			Box2DXDebug.Assert(ratio > Common.Settings.FLT_EPSILON);
 			float C = Length1 + ratio * Length2;
 			MaxLength1 = C - ratio * PulleyJoint.MinPulleyLength;
 			MaxLength2 = (C - PulleyJoint.MinPulleyLength) / ratio;
@@ -173,9 +173,14 @@ namespace Box2DX.Dynamics
 		public float _limitMass2;
 
 		// Impulses for accumulation/warm starting.
-		public float _impulse;
-		public float _limitImpulse1;
-		public float _limitImpulse2;
+		public float _force;
+		public float _limitForce1;
+		public float _limitForce2;
+
+		// Position impulses for accumulation.
+		public float _positionImpulse;
+		public float _limitPositionImpulse1;
+		public float _limitPositionImpulse2;
 
 		public LimitState _state;
 		public LimitState _limitState1;
@@ -191,15 +196,18 @@ namespace Box2DX.Dynamics
 			get { return _body2.GetWorldPoint(_localAnchor2); }
 		}
 
-		public override Vec2 GetReactionForce(float inv_dt)
+		public override Vec2 ReactionForce
 		{
-			Vec2 P = _impulse * _u2;
-			return inv_dt * P;
+			get
+			{
+				Vec2 F = Settings.FORCE_SCALE(_force) * _u2;
+				return F;
+			}
 		}
 
-		public override float GetReactionTorque(float inv_dt)
+		public override float ReactionTorque
 		{
-			return 0.0f;
+			get { return 0.0f; }
 		}
 
 		/// <summary>
@@ -271,9 +279,9 @@ namespace Box2DX.Dynamics
 			_maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength);
 			_maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio);
 
-			_impulse = 0.0f;
-			_limitImpulse1 = 0.0f;
-			_limitImpulse2 = 0.0f;
+			_force = 0.0f;
+			_limitForce1 = 0.0f;
+			_limitForce2 = 0.0f;
 		}
 
 		internal override void InitVelocityConstraints(TimeStep step)
@@ -319,31 +327,34 @@ namespace Box2DX.Dynamics
 			if (C > 0.0f)
 			{
 				_state = LimitState.InactiveLimit;
-				_impulse = 0.0f;
+				_force = 0.0f;
 			}
 			else
 			{
 				_state = LimitState.AtUpperLimit;
+				_positionImpulse = 0.0f;
 			}
 
 			if (length1 < _maxLength1)
 			{
 				_limitState1 = LimitState.InactiveLimit;
-				_limitImpulse1 = 0.0f;
+				_limitForce1 = 0.0f;
 			}
 			else
 			{
 				_limitState1 = LimitState.AtUpperLimit;
+				_limitPositionImpulse1 = 0.0f;
 			}
 
 			if (length2 < _maxLength2)
 			{
 				_limitState2 = LimitState.InactiveLimit;
-				_limitImpulse2 = 0.0f;
+				_limitForce2 = 0.0f;
 			}
 			else
 			{
 				_limitState2 = LimitState.AtUpperLimit;
+				_limitPositionImpulse2 = 0.0f;
 			}
 
 			// Compute effective mass.
@@ -362,14 +373,9 @@ namespace Box2DX.Dynamics
 
 			if (step.WarmStarting)
 			{
-				// Scale impulses to support variable time steps.
-				_impulse *= step.DtRatio;
-				_limitImpulse1 *= step.DtRatio;
-				_limitImpulse2 *= step.DtRatio;
-
 				// Warm starting.
-				Vec2 P1 = -(_impulse + _limitImpulse1) * _u1;
-				Vec2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
+				Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (-_force - _limitForce1) * _u1;
+				Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (-_ratio * _force - _limitForce2) * _u2;
 				b1._linearVelocity += b1._invMass * P1;
 				b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
 				b2._linearVelocity += b2._invMass * P2;
@@ -377,9 +383,9 @@ namespace Box2DX.Dynamics
 			}
 			else
 			{
-				_impulse = 0.0f;
-				_limitImpulse1 = 0.0f;
-				_limitImpulse2 = 0.0f;
+				_force = 0.0f;
+				_limitForce1 = 0.0f;
+				_limitForce2 = 0.0f;
 			}
 		}
 
@@ -397,13 +403,13 @@ namespace Box2DX.Dynamics
 				Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);
 
 				float Cdot = -Vec2.Dot(_u1, v1) - _ratio * Vec2.Dot(_u2, v2);
-				float impulse = _pulleyMass * (-Cdot);
-				float oldImpulse = _impulse;
-				_impulse = Box2DX.Common.Math.Max(0.0f, _impulse + impulse);
-				impulse = _impulse - oldImpulse;
+				float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _pulleyMass * Cdot;
+				float oldForce = _force;
+				_force = Box2DXMath.Max(0.0f, _force + force);
+				force = _force - oldForce;
 
-				Vec2 P1 = -impulse * _u1;
-				Vec2 P2 = -_ratio * impulse * _u2;
+				Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1;
+				Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * _ratio * force * _u2;
 				b1._linearVelocity += b1._invMass * P1;
 				b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
 				b2._linearVelocity += b2._invMass * P2;
@@ -415,12 +421,12 @@ namespace Box2DX.Dynamics
 				Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);
 
 				float Cdot = -Vec2.Dot(_u1, v1);
-				float impulse = -_limitMass1 * Cdot;
-				float oldImpulse = _limitImpulse1;
-				_limitImpulse1 = Box2DX.Common.Math.Max(0.0f, _limitImpulse1 + impulse);
-				impulse = _limitImpulse1 - oldImpulse;
+				float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass1 * Cdot;
+				float oldForce = _limitForce1;
+				_limitForce1 = Box2DXMath.Max(0.0f, _limitForce1 + force);
+				force = _limitForce1 - oldForce;
 
-				Vec2 P1 = -impulse * _u1;
+				Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1;
 				b1._linearVelocity += b1._invMass * P1;
 				b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
 			}
@@ -430,18 +436,18 @@ namespace Box2DX.Dynamics
 				Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);
 
 				float Cdot = -Vec2.Dot(_u2, v2);
-				float impulse = -_limitMass2 * Cdot;
-				float oldImpulse = _limitImpulse2;
-				_limitImpulse2 = Box2DX.Common.Math.Max(0.0f, _limitImpulse2 + impulse);
-				impulse = _limitImpulse2 - oldImpulse;
+				float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass2 * Cdot;
+				float oldForce = _limitForce2;
+				_limitForce2 = Box2DXMath.Max(0.0f, _limitForce2 + force);
+				force = _limitForce2 - oldForce;
 
-				Vec2 P2 = -impulse * _u2;
+				Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * force * _u2;
 				b2._linearVelocity += b2._invMass * P2;
 				b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
 			}
 		}
 
-		internal override bool SolvePositionConstraints(float baumgarte)
+		internal override bool SolvePositionConstraints()
 		{
 			Body b1 = _body1;
 			Body b2 = _body2;
@@ -489,6 +495,9 @@ namespace Box2DX.Dynamics
 
 				C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
 				float impulse = -_pulleyMass * C;
+				float oldImpulse = _positionImpulse;
+				_positionImpulse = Box2DXMath.Max(0.0f, _positionImpulse + impulse);
+				impulse = _positionImpulse - oldImpulse;
 
 				Vec2 P1 = -impulse * _u1;
 				Vec2 P2 = -_ratio * impulse * _u2;
@@ -523,6 +532,9 @@ namespace Box2DX.Dynamics
 				linearError = Box2DXMath.Max(linearError, -C);
 				C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
 				float impulse = -_limitMass1 * C;
+				float oldLimitPositionImpulse = _limitPositionImpulse1;
+				_limitPositionImpulse1 = Box2DXMath.Max(0.0f, _limitPositionImpulse1 + impulse);
+				impulse = _limitPositionImpulse1 - oldLimitPositionImpulse;
 
 				Vec2 P1 = -impulse * _u1;
 				b1._sweep.C += b1._invMass * P1;
@@ -552,6 +564,9 @@ namespace Box2DX.Dynamics
 				linearError = Box2DXMath.Max(linearError, -C);
 				C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
 				float impulse = -_limitMass2 * C;
+				float oldLimitPositionImpulse = _limitPositionImpulse2;
+				_limitPositionImpulse2 = Box2DXMath.Max(0.0f, _limitPositionImpulse2 + impulse);
+				impulse = _limitPositionImpulse2 - oldLimitPositionImpulse;
 
 				Vec2 P2 = -impulse * _u2;
 				b2._sweep.C += b2._invMass * P2;
@@ -563,4 +578,4 @@ namespace Box2DX.Dynamics
 			return linearError < Settings.LinearSlop;
 		}
 	}
-}
+}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs b/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs
index e201e01..e337d5d 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/Joints/RevoluteJoint.cs
@@ -76,6 +76,9 @@ namespace Box2DX.Dynamics
 		/// Initialize the bodies, anchors, and reference angle using the world
 		/// anchor.
 		/// </summary>
+		/// <param name="body1"></param>
+		/// <param name="body2"></param>
+		/// <param name="anchor"></param>
 		public void Initialize(Body body1, Body body2, Vec2 anchor)
 		{
 			Body1 = body1;
@@ -144,9 +147,12 @@ namespace Box2DX.Dynamics
 	{
 		public Vec2 _localAnchor1;	// relative
 		public Vec2 _localAnchor2;
-		public Vec3 _impulse;
-		public float _motorImpulse;
-		public Mat33 _mass; //effective mass for p2p constraint.
+		public Vec2 _pivotForce;
+		public float _motorForce;
+		public float _limitForce;
+		public float _limitPositionImpulse;
+
+		public Mat22 _pivotMass;		// effective mass for point-to-point constraint.
 		public float _motorMass;	// effective mass for motor/limit angular constraint.
 
 		public bool _enableMotor;
@@ -159,6 +165,10 @@ namespace Box2DX.Dynamics
 		public float _upperAngle;
 		public LimitState _limitState;
 
+#if B2_TOI_JOINTS
+		publiv Vector2 _lastWarmStartingPivotForce;
+#endif
+
 		public override Vec2 Anchor1
 		{
 			get { return _body1.GetWorldPoint(_localAnchor1); }
@@ -169,15 +179,14 @@ namespace Box2DX.Dynamics
 			get { return _body2.GetWorldPoint(_localAnchor2); }
 		}
 
-		public override Vec2 GetReactionForce(float inv_dt)
+		public override Vec2 ReactionForce
 		{
-			Vec2 P = new Vec2(_impulse.X, _impulse.Y);
-			return inv_dt * P;
+			get { return Settings.FORCE_SCALE(1.0f) * _pivotForce; }
 		}
 
-		public override float GetReactionTorque(float inv_dt)
+		public override float ReactionTorque
 		{
-			return inv_dt * _impulse.Z;
+			get { return _limitForce; }
 		}
 
 		/// <summary>
@@ -218,10 +227,9 @@ namespace Box2DX.Dynamics
 		/// <summary>
 		/// Enable/disable the joint limit.
 		/// </summary>
+		/// <param name="flag"></param>
 		public void EnableLimit(bool flag)
 		{
-			_body1.WakeUp();
-			_body2.WakeUp();
 			_enableLimit = flag;
 		}
 
@@ -244,11 +252,11 @@ namespace Box2DX.Dynamics
 		/// <summary>
 		/// Set the joint limits in radians.
 		/// </summary>
+		/// <param name="lower"></param>
+		/// <param name="upper"></param>
 		public void SetLimits(float lower, float upper)
 		{
 			Box2DXDebug.Assert(lower <= upper);
-			_body1.WakeUp();
-			_body2.WakeUp();
 			_lowerAngle = lower;
 			_upperAngle = upper;
 		}
@@ -264,10 +272,9 @@ namespace Box2DX.Dynamics
 		/// <summary>
 		/// Enable/disable the joint motor.
 		/// </summary>
+		/// <param name="flag"></param>
 		public void EnableMotor(bool flag)
 		{
-			_body1.WakeUp();
-			_body2.WakeUp();
 			_enableMotor = flag;
 		}
 
@@ -277,30 +284,22 @@ namespace Box2DX.Dynamics
 		public float MotorSpeed
 		{
 			get { return _motorSpeed; }
-			set
-			{
-				_body1.WakeUp();
-				_body2.WakeUp();
-				_motorSpeed = value;
-			}
+			set { _motorSpeed = value; }
 		}
 
 		/// <summary>
 		/// Set the maximum motor torque, usually in N-m.
 		/// </summary>
+		/// <param name="torque"></param>
 		public void SetMaxMotorTorque(float torque)
 		{
-			_body1.WakeUp();
-			_body2.WakeUp();
 			_maxMotorTorque = torque;
 		}
 
-		/// <summary>
 		/// Get the current motor torque, usually in N-m.
-		/// </summary>
 		public float MotorTorque
 		{
-			get { return _motorImpulse; }
+			get { return _motorForce; }
 		}
 
 		public RevoluteJoint(RevoluteJointDef def)
@@ -310,8 +309,10 @@ namespace Box2DX.Dynamics
 			_localAnchor2 = def.LocalAnchor2;
 			_referenceAngle = def.ReferenceAngle;
 
-			_impulse = new Vec3();
-			_motorImpulse = 0.0f;
+			_pivotForce.Set(0.0f, 0.0f);
+			_motorForce = 0.0f;
+			_limitForce = 0.0f;
+			_limitPositionImpulse = 0.0f;
 
 			_lowerAngle = def.LowerAngle;
 			_upperAngle = def.UpperAngle;
@@ -319,7 +320,6 @@ namespace Box2DX.Dynamics
 			_motorSpeed = def.MotorSpeed;
 			_enableLimit = def.EnableLimit;
 			_enableMotor = def.EnableMotor;
-			_limitState = LimitState.InactiveLimit;
 		}
 
 		internal override void InitVelocityConstraints(TimeStep step)
@@ -327,44 +327,36 @@ namespace Box2DX.Dynamics
 			Body b1 = _body1;
 			Body b2 = _body2;
 
-			if (_enableMotor || _enableLimit)
-			{
-				// You cannot create a rotation limit between bodies that
-				// both have fixed rotation.
-				Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
-			}
-
 			// Compute the effective mass matrix.
 			Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
 			Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
 
-			// J = [-I -r1_skew I r2_skew]
-			//     [ 0       -1 0       1]
-			// r_skew = [-ry; rx]
+			// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+			//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+			//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+			float invMass1 = b1._invMass, invMass2 = b2._invMass;
+			float invI1 = b1._invI, invI2 = b2._invI;
 
-			// Matlab
-			// K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
-			//     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
-			//     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]
+			Mat22 K1 = new Mat22();
+			K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
+			K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
 
-			float m1 = b1._invMass, m2 = b2._invMass;
-			float i1 = b1._invI, i2 = b2._invI;
+			Mat22 K2 = new Mat22();
+			K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
+			K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
 
-			_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
-			_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
-			_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
-			_mass.Col1.Y = _mass.Col2.X;
-			_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
-			_mass.Col3.Y = r1.X * i1 + r2.X * i2;
-			_mass.Col1.Z = _mass.Col3.X;
-			_mass.Col2.Z = _mass.Col3.Y;
-			_mass.Col3.Z = i1 + i2;
+			Mat22 K3 = new Mat22();
+			K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
+			K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
 
-			_motorMass = 1.0f / (i1 + i2);
+			Mat22 K = K1 + K2 + K3;
+			_pivotMass = K.Invert();
+
+			_motorMass = 1.0f / (invI1 + invI2);
 
 			if (_enableMotor == false)
 			{
-				_motorImpulse = 0.0f;
+				_motorForce = 0.0f;
 			}
 
 			if (_enableLimit)
@@ -378,7 +370,7 @@ namespace Box2DX.Dynamics
 				{
 					if (_limitState != LimitState.AtLowerLimit)
 					{
-						_impulse.Z = 0.0f;
+						_limitForce = 0.0f;
 					}
 					_limitState = LimitState.AtLowerLimit;
 				}
@@ -386,40 +378,37 @@ namespace Box2DX.Dynamics
 				{
 					if (_limitState != LimitState.AtUpperLimit)
 					{
-						_impulse.Z = 0.0f;
+						_limitForce = 0.0f;
 					}
 					_limitState = LimitState.AtUpperLimit;
 				}
 				else
 				{
 					_limitState = LimitState.InactiveLimit;
-					_impulse.Z = 0.0f;
+					_limitForce = 0.0f;
 				}
 			}
 			else
 			{
-				_limitState = LimitState.InactiveLimit;
+				_limitForce = 0.0f;
 			}
 
 			if (step.WarmStarting)
 			{
-				// Scale impulses to support a variable time step.
-				_impulse *= step.DtRatio;
-				_motorImpulse *= step.DtRatio;
-
-				Vec2 P = new Vec2(_impulse.X, _impulse.Y);
-
-				b1._linearVelocity -= m1 * P;
-				b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z);
+				b1._linearVelocity -= Settings.FORCE_SCALE(step.Dt) * invMass1 * _pivotForce;
+				b1._angularVelocity -= Settings.FORCE_SCALE(step.Dt) * invI1 * (Vec2.Cross(r1, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));
 
-				b2._linearVelocity += m2 * P;
-				b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z);
+				b2._linearVelocity += Settings.FORCE_SCALE(step.Dt) * invMass2 * _pivotForce;
+				b2._angularVelocity += Settings.FORCE_SCALE(step.Dt) * invI2 * (Vec2.Cross(r2, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));
 			}
 			else
 			{
-				_impulse.SetZero();
-				_motorImpulse = 0.0f;
+				_pivotForce.SetZero();
+				_motorForce = 0.0f;
+				_limitForce = 0.0f;
 			}
+
+			_limitPositionImpulse = 0.0f;
 		}
 
 		internal override void SolveVelocityConstraints(TimeStep step)
@@ -427,119 +416,129 @@ namespace Box2DX.Dynamics
 			Body b1 = _body1;
 			Body b2 = _body2;
 
-			Vec2 v1 = b1._linearVelocity;
-			float w1 = b1._angularVelocity;
-			Vec2 v2 = b2._linearVelocity;
-			float w2 = b2._angularVelocity;
+			Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
+			Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
 
-			float m1 = b1._invMass, m2 = b2._invMass;
-			float i1 = b1._invI, i2 = b2._invI;
+			// Solve point-to-point constraint
+			Vec2 pivotCdot = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2) - b1._linearVelocity -
+				Vec2.Cross(b1._angularVelocity, r1);
+			Vec2 pivotForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Box2DXMath.Mul(_pivotMass, pivotCdot);
+
+#if B2_TOI_JOINTS
+			if (step.WarmStarting)
+			{
+					_pivotForce += pivotForce;
+					_lastWarmStartingPivotForce = _pivotForce;
+			}
+			else
+			{
+				_pivotForce = _lastWarmStartingPivotForce;
+				//Do not update warm starting value!
+			}
+#else
+			_pivotForce += pivotForce;
+#endif
+
+			Vec2 P = Settings.FORCE_SCALE(step.Dt) * pivotForce;
+			b1._linearVelocity -= b1._invMass * P;
+			b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
+
+			b2._linearVelocity += b2._invMass * P;
+			b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
 
-			//Solve motor constraint.
 			if (_enableMotor && _limitState != LimitState.EqualLimits)
 			{
-				float Cdot = w2 - w1 - _motorSpeed;
-				float impulse = _motorMass * (-Cdot);
-				float oldImpulse = _motorImpulse;
-				float maxImpulse = step.Dt * _maxMotorTorque;
-				_motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
-				impulse = _motorImpulse - oldImpulse;
-
-				w1 -= i1 * impulse;
-				w2 += i2 * impulse;
+				float motorCdot = b2._angularVelocity - b1._angularVelocity - _motorSpeed;
+				float motorForce = -step.Inv_Dt * _motorMass * motorCdot;
+				float oldMotorForce = _motorForce;
+				_motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorTorque, _maxMotorTorque);
+				motorForce = _motorForce - oldMotorForce;
+
+				float P_ = step.Dt * motorForce;
+				b1._angularVelocity -= b1._invI * P_;
+				b2._angularVelocity += b2._invI * P_;
 			}
 
-			//Solve limit constraint.
 			if (_enableLimit && _limitState != LimitState.InactiveLimit)
 			{
-				Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
-				Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
-
-				// Solve point-to-point constraint
-				Vec2 Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
-				float Cdot2 = w2 - w1;
-				Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);
-
-				Vec3 impulse = _mass.Solve33(-Cdot);
+				float limitCdot = b2._angularVelocity - b1._angularVelocity;
+				float limitForce = -step.Inv_Dt * _motorMass * limitCdot;
 
 				if (_limitState == LimitState.EqualLimits)
 				{
-					_impulse += impulse;
+					_limitForce += limitForce;
 				}
 				else if (_limitState == LimitState.AtLowerLimit)
 				{
-					float newImpulse = _impulse.Z + impulse.Z;
-					if (newImpulse < 0.0f)
-					{
-						Vec2 reduced = _mass.Solve22(-Cdot1);
-						impulse.X = reduced.X;
-						impulse.Y = reduced.Y;
-						impulse.Z = -_impulse.Z;
-						_impulse.X += reduced.X;
-						_impulse.Y += reduced.Y;
-						_impulse.Z = 0.0f;
-					}
+					float oldLimitForce = _limitForce;
+					_limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f);
+					limitForce = _limitForce - oldLimitForce;
 				}
 				else if (_limitState == LimitState.AtUpperLimit)
 				{
-					float newImpulse = _impulse.Z + impulse.Z;
-					if (newImpulse > 0.0f)
-					{
-						Vec2 reduced = _mass.Solve22(-Cdot1);
-						impulse.X = reduced.X;
-						impulse.Y = reduced.Y;
-						impulse.Z = -_impulse.Z;
-						_impulse.X += reduced.X;
-						_impulse.Y += reduced.Y;
-						_impulse.Z = 0.0f;
-					}
+					float oldLimitForce = _limitForce;
+					_limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f);
+					limitForce = _limitForce - oldLimitForce;
 				}
 
-				Vec2 P = new Vec2(impulse.X, impulse.Y);
+				float P_ = step.Dt * limitForce;
+				b1._angularVelocity -= b1._invI * P_;
+				b2._angularVelocity += b2._invI * P_;
+			}
+		}
+
+		internal override bool SolvePositionConstraints()
+		{
+			Body b1 = _body1;
+			Body b2 = _body2;
+
+			float positionError = 0.0f;
 
-				v1 -= m1 * P;
-				w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z);
+			// Solve point-to-point position error.
+			Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
+			Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
 
-				v2 += m2 * P;
-				w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z);
-			}
-			else
-			{
-				Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
-				Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
+			Vec2 p1 = b1._sweep.C + r1;
+			Vec2 p2 = b2._sweep.C + r2;
+			Vec2 ptpC = p2 - p1;
 
-				// Solve point-to-point constraint
-				Vec2 Cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
-				Vec2 impulse = _mass.Solve22(-Cdot);
+			positionError = ptpC.Length();
 
-				_impulse.X += impulse.X;
-				_impulse.Y += impulse.Y;
+			// Prevent overly large corrections.
+			//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
+			//ptpC = b2Clamp(ptpC, -dpMax, dpMax);
 
-				v1 -= m1 * impulse;
-				w1 -= i1 * Vec2.Cross(r1, impulse);
+			float invMass1 = b1._invMass, invMass2 = b2._invMass;
+			float invI1 = b1._invI, invI2 = b2._invI;
 
-				v2 += m2 * impulse;
-				w2 += i2 * Vec2.Cross(r2, impulse);
-			}
+			Mat22 K1 = new Mat22();
+			K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
+			K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
 
-			b1._linearVelocity = v1;
-			b1._angularVelocity = w1;
-			b2._linearVelocity = v2;
-			b2._angularVelocity = w2;
-		}
+			Mat22 K2 = new Mat22();
+			K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
+			K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
 
-		internal override bool SolvePositionConstraints(float baumgarte)
-		{
-			// TODO_ERIN block solve with limit.
+			Mat22 K3 = new Mat22();
+			K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
+			K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
 
-			Body b1 = _body1;
-			Body b2 = _body2;
+			Mat22 K = K1 + K2 + K3;
+			Vec2 impulse = K.Solve(-ptpC);
+
+			b1._sweep.C -= b1._invMass * impulse;
+			b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse);
 
+			b2._sweep.C += b2._invMass * impulse;
+			b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse);
+
+			b1.SynchronizeTransform();
+			b2.SynchronizeTransform();
+
+			// Handle limits.
 			float angularError = 0.0f;
-			float positionError = 0.0f;
 
-			// Solve angular limit constraint.
-			if (_enableLimit && _limitState !=  LimitState.InactiveLimit)
+			if (_enableLimit && _limitState != LimitState.InactiveLimit)
 			{
 				float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
 				float limitImpulse = 0.0f;
@@ -547,27 +546,33 @@ namespace Box2DX.Dynamics
 				if (_limitState == LimitState.EqualLimits)
 				{
 					// Prevent large angular corrections
-					float C = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
-					limitImpulse = -_motorMass * C;
-					angularError = Box2DXMath.Abs(C);
+					float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
+					limitImpulse = -_motorMass * limitC;
+					angularError = Box2DXMath.Abs(limitC);
 				}
 				else if (_limitState == LimitState.AtLowerLimit)
 				{
-					float C = angle - _lowerAngle;
-					angularError = -C;
+					float limitC = angle - _lowerAngle;
+					angularError = Box2DXMath.Max(0.0f, -limitC);
 
 					// Prevent large angular corrections and allow some slop.
-					C = Box2DXMath.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
-					limitImpulse = -_motorMass * C;
+					limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
+					limitImpulse = -_motorMass * limitC;
+					float oldLimitImpulse = _limitPositionImpulse;
+					_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
+					limitImpulse = _limitPositionImpulse - oldLimitImpulse;
 				}
 				else if (_limitState == LimitState.AtUpperLimit)
 				{
-					float C = angle - _upperAngle;
-					angularError = C;
+					float limitC = angle - _upperAngle;
+					angularError = Box2DXMath.Max(0.0f, limitC);
 
 					// Prevent large angular corrections and allow some slop.
-					C = Box2DXMath.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
-					limitImpulse = -_motorMass * C;
+					limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
+					limitImpulse = -_motorMass * limitC;
+					float oldLimitImpulse = _limitPositionImpulse;
+					_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
+					limitImpulse = _limitPositionImpulse - oldLimitImpulse;
 				}
 
 				b1._sweep.A -= b1._invI * limitImpulse;
@@ -577,60 +582,7 @@ namespace Box2DX.Dynamics
 				b2.SynchronizeTransform();
 			}
 
-			// Solve point-to-point constraint.
-			{
-				Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
-				Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
-
-				Vec2 C = b2._sweep.C + r2 - b1._sweep.C - r1;
-				positionError = C.Length();
-
-				float invMass1 = b1._invMass, invMass2 = b2._invMass;
-				float invI1 = b1._invI, invI2 = b2._invI;
-
-				// Handle large detachment.
-				float k_allowedStretch = 10.0f * Settings.LinearSlop;
-				if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
-				{
-					// Use a particle solution (no rotation).
-					Vec2 u = C; u.Normalize();
-					float k = invMass1 + invMass2;
-					Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
-					float m = 1.0f / k;
-					Vec2 impulse = m * (-C);
-					float k_beta = 0.5f;
-					b1._sweep.C -= k_beta * invMass1 * impulse;
-					b2._sweep.C += k_beta * invMass2 * impulse;
-
-					C = b2._sweep.C + r2 - b1._sweep.C - r1;
-				}
-
-				Mat22 K1 = new Mat22();
-				K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
-				K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
-
-				Mat22 K2 = new Mat22();
-				K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
-				K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
-
-				Mat22 K3 = new Mat22();
-				K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
-				K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
-
-				Mat22 K = K1 + K2 + K3;
-				Vec2 impulse_ = K.Solve(-C);
-
-				b1._sweep.C -= b1._invMass * impulse_;
-				b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_);
-
-				b2._sweep.C += b2._invMass * impulse_;
-				b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_);
-
-				b1.SynchronizeTransform();
-				b2.SynchronizeTransform();
-			}
-
 			return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
 		}
 	}
-}
+}
\ No newline at end of file
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/World.cs b/Box2d/Assets/Program/Box2d/Dynamics/World.cs
index 7ebc5eb..39f1308 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/World.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/World.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -20,6 +20,8 @@
 */
 
 using System;
+using System.Collections.Generic;
+using System.Text;
 
 using Box2DX.Common;
 using Box2DX.Collision;
@@ -33,7 +35,7 @@ namespace Box2DX.Dynamics
 		public float DtRatio;	// dt * inv_dt0
 		public int VelocityIterations;
 		public int PositionIterations;
-		public bool WarmStarting;
+		public bool WarmStarting;		
 	}
 
 	/// <summary>
@@ -49,7 +51,6 @@ namespace Box2DX.Dynamics
 
 		private Body _bodyList;
 		private Joint _jointList;
-		private Controllers.Controller _controllerList;
 
 		private Vec2 _raycastNormal;
 		private object _raycastUserData;
@@ -62,7 +63,6 @@ namespace Box2DX.Dynamics
 		private int _bodyCount;
 		internal int _contactCount;
 		private int _jointCount;
-		private int _controllerCount;
 
 		private Vec2 _gravity;
 		/// <summary>
@@ -100,7 +100,7 @@ namespace Box2DX.Dynamics
 		{
 			_destructionListener = null;
 			_boundaryListener = null;
-			_contactFilter = null;
+			_contactFilter = WorldCallback.DefaultFilter;
 			_contactListener = null;
 			_debugDraw = null;
 
@@ -252,30 +252,23 @@ namespace Box2DX.Dynamics
 				DestroyJoint(jn0.Joint);
 			}
 
-			//Detach controllers attached to this body
-			Controllers.ControllerEdge ce = b._controllerList;
-			while (ce != null)
-			{
-				Controllers.ControllerEdge ce0 = ce;
-				ce = ce.nextController;
-
-				ce0.controller.RemoveBody(b);
-			}
-
-			// Delete the attached fixtures. This destroys broad-phase
+			// Delete the attached shapes. This destroys broad-phase
 			// proxies and pairs, leading to the destruction of contacts.
-			Fixture f = b._fixtureList;
-			while (f != null)
+			Shape s = null;
+			if (b._shapeList != null)
+				s = b._shapeList;
+			while (s != null)
 			{
-				Fixture f0 = f;
-				f = f.Next;
+				Shape s0 = s;
+				s = s._next;
 
 				if (_destructionListener != null)
 				{
-					_destructionListener.SayGoodbye(f0);
+					_destructionListener.SayGoodbye(s0);
 				}
 
-				f0.Destroy(_broadPhase);
+				s0.DestroyProxy(_broadPhase);
+				Shape.Destroy(s0);
 			}
 
 			// Remove world body list.
@@ -344,10 +337,10 @@ namespace Box2DX.Dynamics
 			if (def.CollideConnected == false)
 			{
 				// Reset the proxies on the body with the minimum number of shapes.
-				Body b = def.Body1._fixtureCount < def.Body2._fixtureCount ? def.Body1 : def.Body2;
-				for (Fixture f = b._fixtureList; f != null; f = f.Next)
+				Body b = def.Body1._shapeCount < def.Body2._shapeCount ? def.Body1 : def.Body2;
+				for (Shape s = b._shapeList; s != null; s = s._next)
 				{
-					f.RefilterProxy(_broadPhase, b.GetXForm());
+					s.RefilterProxy(_broadPhase, b.GetXForm());
 				}
 			}
 
@@ -436,40 +429,14 @@ namespace Box2DX.Dynamics
 			if (collideConnected == false)
 			{
 				// Reset the proxies on the body with the minimum number of shapes.
-				Body b = body1._fixtureCount < body2._fixtureCount ? body1 : body2;
-				for (Fixture f = b._fixtureList; f != null; f = f.Next)
+				Body b = body1._shapeCount < body2._shapeCount ? body1 : body2;
+				for (Shape s = b._shapeList; s != null; s = s._next)
 				{
-					f.RefilterProxy(_broadPhase, b.GetXForm());
+					s.RefilterProxy(_broadPhase, b.GetXForm());
 				}
 			}
 		}
 
-		public Controllers.Controller AddController(Controllers.Controller def)
-		{
-			def._next = _controllerList;
-			def._prev = null;
-			if (_controllerList != null)
-				_controllerList._prev = def;
-			_controllerList = def;
-			++_controllerCount;
-
-			def._world = this;
-
-			return def;
-		}
-
-		public void RemoveController(Controllers.Controller controller)
-		{
-			Box2DXDebug.Assert(_controllerCount > 0);
-			if (controller._next != null)
-				controller._next._prev = controller._prev;
-			if (controller._prev != null)
-				controller._prev._next = controller._next;
-			if (controller == _controllerList)
-				_controllerList = controller._next;
-			--_controllerCount;
-		}
-
 		/// <summary>
 		/// The world provides a single static ground body with no collision shapes.
 		/// You can use this to simplify the creation of joints and static shapes.
@@ -500,23 +467,13 @@ namespace Box2DX.Dynamics
 			return _jointList;
 		}
 
-		public Controllers.Controller GetControllerList()
-		{
-			return _controllerList;
-		}
-
-		public int GetControllerCount()
-		{
-			return _controllerCount;
-		}
-
 		/// <summary>
-		/// Re-filter a fixture. This re-runs contact filtering on a fixture.
+		/// Re-filter a shape. This re-runs contact filtering on a shape.
 		/// </summary>		
-		public void Refilter(Fixture fixture)
+		public void Refilter(Shape shape)
 		{
 			Box2DXDebug.Assert(_lock == false);
-			fixture.RefilterProxy(_broadPhase, fixture.Body.GetXForm());
+			shape.RefilterProxy(_broadPhase, shape.GetBody().GetXForm());
 		}
 
 		/// <summary>
@@ -620,7 +577,7 @@ namespace Box2DX.Dynamics
 		/// @param shapes a user allocated shape pointer array of size maxCount (or greater).
 		/// @param maxCount the capacity of the shapes array.
 		/// @return the number of shapes found in aabb.
-		public int Query(AABB aabb, Fixture[] fixtures, int maxCount)
+		public int Query(AABB aabb, Shape[] shapes, int maxCount)
 		{
 			//using (object[] results = new object[maxCount])
 			{
@@ -630,7 +587,7 @@ namespace Box2DX.Dynamics
 
 				for (int i = 0; i < count; ++i)
 				{
-					fixtures[i] = (Fixture)results[i];
+					shapes[i] = (Shape)results[i];
 				}
 
 				results = null;
@@ -650,7 +607,7 @@ namespace Box2DX.Dynamics
 		/// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param>
 		/// <param name="userData">Passed through the worlds contact filter, with method RayCollide. This can be used to filter valid shapes.</param>
 		/// <returns>The number of shapes found</returns>
-		public int Raycast(Segment segment, out Fixture[] fixtures, int maxCount, bool solidShapes, object userData)
+		public int Raycast(Segment segment, Shape[] shapes, int maxCount, bool solidShapes, object userData)
 		{
 #warning "PTR"
 			_raycastSegment = segment;
@@ -658,14 +615,12 @@ namespace Box2DX.Dynamics
 			_raycastSolidShape = solidShapes;
 
 			object[] results = new object[maxCount];
-			fixtures = new Fixture[maxCount];
 			int count = _broadPhase.QuerySegment(segment, results, maxCount, RaycastSortKey);
-
 			for (int i = 0; i < count; ++i)
 			{
-				fixtures[i] = (Fixture)results[i];
+				shapes[i] = (Shape)results[i];
 			}
-
+			results = null;
 			return count;
 		}
 
@@ -680,14 +635,15 @@ namespace Box2DX.Dynamics
 		/// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param>
 		/// <param name="userData"></param>
 		/// <returns>Returns the colliding shape shape, or null if not found.</returns>
-		public Fixture RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData)
+		public Shape RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData)
 		{
+			lambda = 0;
+			normal = new Vec2(0,0);
+
 			int maxCount = 1;
-			Fixture[] fixture;
-			lambda = 0.0f;
-			normal = new Vec2();
+			Shape[] shape = new Shape[maxCount];
 
-			int count = Raycast(segment, out fixture, maxCount, solidShapes, userData);
+			int count = Raycast(segment, shape, maxCount, solidShapes, userData);
 
 			if (count == 0)
 				return null;
@@ -696,20 +652,15 @@ namespace Box2DX.Dynamics
 
 			//Redundantly do TestSegment a second time, as the previous one's results are inaccessible
 
-			fixture[0].TestSegment(out lambda, out normal, segment, 1);
+			XForm xf = shape[0].GetBody().GetXForm();
+			shape[0].TestSegment(xf, out lambda, out normal, segment, 1);
 			//We already know it returns true
-			return fixture[0];
+			return shape[0];
 		}
 
 		// Find islands, integrate and solve constraints, solve position constraints
 		private void Solve(TimeStep step)
 		{
-			// Step all controlls
-			for (Controllers.Controller controller = _controllerList; controller != null; controller = controller._next)
-			{
-				controller.Step(step);
-			}
-
 			// Size the island for the worst case.
 			Island island = new Island(_bodyCount, _contactCount, _jointCount, _contactListener);
 
@@ -777,7 +728,7 @@ namespace Box2DX.Dynamics
 							}
 
 							// Is this contact touching?
-							if ((cn.Contact._flags & Contact.CollisionFlags.Touch) == (Contact.CollisionFlags)0)
+							if (cn.Contact.GetManifoldCount() == 0)
 							{
 								continue;
 							}
@@ -854,7 +805,7 @@ namespace Box2DX.Dynamics
 				// Update shapes (for broad-phase). If the shapes go out of
 				// the world AABB then shapes and contacts may be destroyed,
 				// including contacts that are
-				bool inRange = b.SynchronizeFixtures();
+				bool inRange = b.SynchronizeShapes();
 
 				// Did the body's shapes leave the world?
 				if (inRange == false && _boundaryListener != null)
@@ -884,7 +835,6 @@ namespace Box2DX.Dynamics
 			//  --queueSize;
 			int queueCapacity = _bodyCount;
 			Body[] queue = new Body[queueCapacity];
-
 			for (Body b = _bodyList; b != null; b = b._next)
 			{
 				b._flags &= ~Body.BodyFlags.Island;
@@ -897,10 +847,12 @@ namespace Box2DX.Dynamics
 				c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island);
 			}
 
-			for (Joint j = _jointList; j != null; j = j._next)
+#if B2_TOI_JOINTS
+			for (Joint j = _jointList; j!=null; j = j._next)
 			{
-				j._islandFlag = false;
+					j._islandFlag = false;
 			}
+#endif
 
 			// Find TOI events and solve them.
 			for (; ; )
@@ -911,7 +863,7 @@ namespace Box2DX.Dynamics
 
 				for (Contact c = _contactList; c != null; c = c._next)
 				{
-					if ((int)(c._flags & (Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) == 1)
+					if ((c._flags & (Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0)
 					{
 						continue;
 					}
@@ -919,7 +871,7 @@ namespace Box2DX.Dynamics
 					// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
 
 					float toi = 1.0f;
-					if ((int)(c._flags & Contact.CollisionFlags.Toi) == 1)
+					if ((c._flags & Contact.CollisionFlags.Toi) != 0)
 					{
 						// This contact has a valid cached TOI.
 						toi = c._toi;
@@ -927,42 +879,38 @@ namespace Box2DX.Dynamics
 					else
 					{
 						// Compute the TOI for this contact.
-						Fixture s1 = c.FixtureA;
-						Fixture s2 = c.FixtureB;
-						Body b1 = s1.Body;
-						Body b2 = s2.Body;
+						Shape s1_ = c.GetShape1();
+						Shape s2_ = c.GetShape2();
+						Body b1_ = s1_.GetBody();
+						Body b2_ = s2_.GetBody();
 
-						if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping()))
+						if ((b1_.IsStatic() || b1_.IsSleeping()) && (b2_.IsStatic() || b2_.IsSleeping()))
 						{
 							continue;
 						}
 
 						// Put the sweeps onto the same time interval.
-						float t0 = b1._sweep.T0;
+						float t0 = b1_._sweep.T0;
 
-						if (b1._sweep.T0 < b2._sweep.T0)
+						if (b1_._sweep.T0 < b2_._sweep.T0)
 						{
-							t0 = b2._sweep.T0;
-							b1._sweep.Advance(t0);
+							t0 = b2_._sweep.T0;
+							b1_._sweep.Advance(t0);
 						}
-						else if (b2._sweep.T0 < b1._sweep.T0)
+						else if (b2_._sweep.T0 < b1_._sweep.T0)
 						{
-							t0 = b1._sweep.T0;
-							b2._sweep.Advance(t0);
+							t0 = b1_._sweep.T0;
+							b2_._sweep.Advance(t0);
 						}
 
 						Box2DXDebug.Assert(t0 < 1.0f);
 
 						// Compute the time of impact.
-						toi = c.ComputeTOI(b1._sweep, b2._sweep);
-						//b2TimeOfImpact(c->m_fixtureA->GetShape(), b1->m_sweep, c->m_fixtureB->GetShape(), b2->m_sweep);
-
+						toi = Collision.Collision.TimeOfImpact(c._shape1, b1_._sweep, c._shape2, b2_._sweep);
 						Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f);
 
-						// If the TOI is in range ...
-						if (0.0f < toi && toi < 1.0f)
+						if (toi > 0.0f && toi < 1.0f)
 						{
-							// Interpolate on the actual range.
 							toi = Common.Math.Min((1.0f - toi) * t0 + toi, 1.0f);
 						}
 
@@ -971,7 +919,7 @@ namespace Box2DX.Dynamics
 						c._flags |= Contact.CollisionFlags.Toi;
 					}
 
-					if (Settings.FLT_EPSILON < toi && toi < minTOI)
+					if (Common.Settings.FLT_EPSILON < toi && toi < minTOI)
 					{
 						// This is the minimum TOI found so far.
 						minContact = c;
@@ -979,25 +927,25 @@ namespace Box2DX.Dynamics
 					}
 				}
 
-				if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI)
+				if (minContact == null || 1.0f - 100.0f * Common.Settings.FLT_EPSILON < minTOI)
 				{
 					// No more TOI events. Done!
 					break;
 				}
 
 				// Advance the bodies to the TOI.
-				Fixture f1 = minContact.FixtureA;
-				Fixture f2 = minContact.FixtureB;
-				Body b3 = f1.Body;
-				Body b4 = f2.Body;
-				b3.Advance(minTOI);
-				b4.Advance(minTOI);
+				Shape s1 = minContact.GetShape1();
+				Shape s2 = minContact.GetShape2();
+				Body b1 = s1.GetBody();
+				Body b2 = s2.GetBody();
+				b1.Advance(minTOI);
+				b2.Advance(minTOI);
 
 				// The TOI contact likely has some new contact points.
 				minContact.Update(_contactListener);
 				minContact._flags &= ~Contact.CollisionFlags.Toi;
 
-				if ((minContact._flags & Contact.CollisionFlags.Touch) == 0)
+				if (minContact.GetManifoldCount() == 0)
 				{
 					// This shouldn't happen. Numerical error?
 					//b2Assert(false);
@@ -1005,17 +953,17 @@ namespace Box2DX.Dynamics
 				}
 
 				// Build the TOI island. We need a dynamic seed.
-				Body seed = b3;
+				Body seed = b1;
 				if (seed.IsStatic())
 				{
-					seed = b4;
+					seed = b2;
 				}
 
 				// Reset island and queue.
 				island.Clear();
 
-				int queueStart = 0; // starting index for queue
-				int queueSize = 0;  // elements in queue
+				int queueStart = 0; //starting index for queue
+				int queueSize = 0;  //elements in queue
 				queue[queueStart + queueSize++] = seed;
 				seed._flags |= Body.BodyFlags.Island;
 
@@ -1025,7 +973,6 @@ namespace Box2DX.Dynamics
 					// Grab the next body off the stack and add it to the island.
 					Body b = queue[queueStart++];
 					--queueSize;
-
 					island.Add(b);
 
 					// Make sure the body is awake.
@@ -1039,7 +986,7 @@ namespace Box2DX.Dynamics
 					}
 
 					// Search all contacts connected to this body.
-					for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next)
+					for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next)
 					{
 						// Does the TOI island still have space for contacts?
 						if (island._contactCount == island._contactCapacity)
@@ -1048,25 +995,25 @@ namespace Box2DX.Dynamics
 						}
 
 						// Has this contact already been added to an island? Skip slow or non-solid contacts.
-						if ((int)(cEdge.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0)
+						if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0)
 						{
 							continue;
 						}
 
 						// Is this contact touching? For performance we are not updating this contact.
-						if ((cEdge.Contact._flags & Contact.CollisionFlags.Touch) == 0)
+						if (cn.Contact.GetManifoldCount() == 0)
 						{
 							continue;
 						}
 
-						island.Add(cEdge.Contact);
-						cEdge.Contact._flags |= Contact.CollisionFlags.Island;
+						island.Add(cn.Contact);
+						cn.Contact._flags |= Contact.CollisionFlags.Island;
 
 						// Update other body.
-						Body other = cEdge.Other;
+						Body other = cn.Other;
 
 						// Was the other body already added to this island?
-						if ((int)(other._flags & Body.BodyFlags.Island) == 1)
+						if ((other._flags & Body.BodyFlags.Island) != 0)
 						{
 							continue;
 						}
@@ -1078,53 +1025,53 @@ namespace Box2DX.Dynamics
 							other.WakeUp();
 						}
 
-						//Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
-						queue[queueStart + queueSize] = other;
-						++queueSize;
+						Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
+						queue[queueStart + queueSize++] = other;
 						other._flags |= Body.BodyFlags.Island;
 					}
 
-					for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next)
+#if B2_TOI_JOINTS
+					for (JointEdge jn = b._jointList; jn!=null; jn = jn.Next)
 					{
 						if (island._jointCount == island._jointCapacity)
 						{
 							continue;
 						}
-
-						if (jEdge.Joint._islandFlag == true)
+						
+						if (jn.Joint._islandFlag == true)
 						{
 							continue;
 						}
-
-						island.Add(jEdge.Joint);
-
-						jEdge.Joint._islandFlag = true;
-
-						Body other = jEdge.Other;
-
-						if ((int)(other._flags & Body.BodyFlags.Island) == 1)
+						
+						island.Add(jn.Joint);
+						
+						jn.Joint._islandFlag = true;
+						
+						Body other = jn.Other;
+						
+						if (other._flags & Body.BodyFlags.Island)
 						{
 							continue;
 						}
-
+						
 						if (!other.IsStatic())
 						{
 							other.Advance(minTOI);
 							other.WakeUp();
 						}
-
-						//Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
-						queue[queueStart + queueSize] = other;
-						++queueSize;
-						other._flags |= Body.BodyFlags.Island;
+						
+						Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
+						queue[queueStart + queueSize++] = other;
+						other._flags |= Body.BodyFlags.Island; 
 					}
+#endif
 				}
 
-				TimeStep subStep;
+				TimeStep subStep = new TimeStep();
 				subStep.WarmStarting = false;
 				subStep.Dt = (1.0f - minTOI) * step.Dt;
+				Box2DXDebug.Assert(subStep.Dt > Common.Settings.FLT_EPSILON);
 				subStep.Inv_Dt = 1.0f / subStep.Dt;
-				subStep.DtRatio = 0.0f;
 				subStep.VelocityIterations = step.VelocityIterations;
 				subStep.PositionIterations = step.PositionIterations;
 
@@ -1137,7 +1084,7 @@ namespace Box2DX.Dynamics
 					Body b = island._bodies[i];
 					b._flags &= ~Body.BodyFlags.Island;
 
-					if ((int)(b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) == 1)
+					if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
 					{
 						continue;
 					}
@@ -1147,12 +1094,12 @@ namespace Box2DX.Dynamics
 						continue;
 					}
 
-					// Update fixtures (for broad-phase). If the fixtures go out of
-					// the world AABB then fixtures and contacts may be destroyed,
+					// Update shapes (for broad-phase). If the shapes go out of
+					// the world AABB then shapes and contacts may be destroyed,
 					// including contacts that are
-					bool inRange = b.SynchronizeFixtures();
+					bool inRange = b.SynchronizeShapes();
 
-					// Did the body's fixtures leave the world?
+					// Did the body's shapes leave the world?
 					if (inRange == false && _boundaryListener != null)
 					{
 						_boundaryListener.Violation(b);
@@ -1180,7 +1127,7 @@ namespace Box2DX.Dynamics
 					j._islandFlag = false;
 				}
 
-				// Commit fixture proxy movements to the broad-phase so that new contacts are created.
+				// Commit shape proxy movements to the broad-phase so that new contacts are created.
 				// Also, some contacts can be destroyed.
 				_broadPhase.Commit();
 			}
@@ -1230,30 +1177,34 @@ namespace Box2DX.Dynamics
 			}
 		}
 
-		private void DrawFixture(Fixture fixture, XForm xf, Color color, bool core)
+		private void DrawShape(Shape shape, XForm xf, Color color, bool core)
 		{
-#warning "the core argument is not used, the coreColor variable is also not used"
 			Color coreColor = new Color(0.9f, 0.6f, 0.6f);
 
-			switch (fixture.ShapeType)
+			switch (shape.GetType())
 			{
 				case ShapeType.CircleShape:
 					{
-						CircleShape circle = (CircleShape)fixture.Shape;
+						CircleShape circle = (CircleShape)shape;
 
-						Vec2 center = Common.Math.Mul(xf, circle._position);
-						float radius = circle._radius;
+						Vec2 center = Common.Math.Mul(xf, circle.GetLocalPosition());
+						float radius = circle.GetRadius();
 						Vec2 axis = xf.R.Col1;
 
 						_debugDraw.DrawSolidCircle(center, radius, axis, color);
+
+						if (core)
+						{
+							_debugDraw.DrawCircle(center, radius - Settings.ToiSlop, coreColor);
+						}
 					}
 					break;
 
 				case ShapeType.PolygonShape:
 					{
-						PolygonShape poly = (PolygonShape)fixture.Shape;
-						int vertexCount = poly._vertexCount;
-						Vec2[] localVertices = poly._vertices;
+						PolygonShape poly = (PolygonShape)shape;
+						int vertexCount = poly.VertexCount;
+						Vec2[] localVertices = poly.GetVertices();
 
 						Box2DXDebug.Assert(vertexCount <= Settings.MaxPolygonVertices);
 						Vec2[] vertices = new Vec2[Settings.MaxPolygonVertices];
@@ -1264,14 +1215,16 @@ namespace Box2DX.Dynamics
 						}
 
 						_debugDraw.DrawSolidPolygon(vertices, vertexCount, color);
-					}
-					break;
 
-				case ShapeType.EdgeShape:
-					{
-						EdgeShape edge = (EdgeShape)fixture.Shape;
-
-						_debugDraw.DrawSegment(Common.Math.Mul(xf, edge.Vertex1), Common.Math.Mul(xf, edge.Vertex2), color);
+						if (core)
+						{
+							Vec2[] localCoreVertices = poly.GetCoreVertices();
+							for (int i = 0; i < vertexCount; ++i)
+							{
+								vertices[i] = Common.Math.Mul(xf, localCoreVertices[i]);
+							}
+							_debugDraw.DrawPolygon(vertices, vertexCount, coreColor);
+						}
 					}
 					break;
 			}
@@ -1293,19 +1246,19 @@ namespace Box2DX.Dynamics
 				for (Body b = _bodyList; b != null; b = b.GetNext())
 				{
 					XForm xf = b.GetXForm();
-					for (Fixture f = b.GetFixtureList(); f != null; f = f.Next)
+					for (Shape s = b.GetShapeList(); s != null; s = s.GetNext())
 					{
 						if (b.IsStatic())
 						{
-							DrawFixture(f, xf, new Color(0.5f, 0.9f, 0.5f), core);
+							DrawShape(s, xf, new Color(0.5f, 0.9f, 0.5f), core);
 						}
 						else if (b.IsSleeping())
 						{
-							DrawFixture(f, xf, new Color(0.5f, 0.5f, 0.9f), core);
+							DrawShape(s, xf, new Color(0.5f, 0.5f, 0.9f), core);
 						}
 						else
 						{
-							DrawFixture(f, xf, new Color(0.9f, 0.9f, 0.9f), core);
+							DrawShape(s, xf, new Color(0.9f, 0.9f, 0.9f), core);
 						}
 					}
 				}
@@ -1322,14 +1275,6 @@ namespace Box2DX.Dynamics
 				}
 			}
 
-			if ((flags & DebugDraw.DrawFlags.Controller) != 0)
-			{
-				for (Controllers.Controller c = _controllerList; c != null; c = c.GetNext())
-				{
-					c.Draw(_debugDraw);
-				}
-			}
-
 			if ((flags & DebugDraw.DrawFlags.Pair) != 0)
 			{
 				BroadPhase bp = _broadPhase;
@@ -1406,6 +1351,40 @@ namespace Box2DX.Dynamics
 				_debugDraw.DrawPolygon(vs, 4, new Color(0.3f, 0.9f, 0.9f));
 			}
 
+			if ((flags & DebugDraw.DrawFlags.Obb) != 0)
+			{
+				Color color = new Color(0.5f, 0.3f, 0.5f);
+
+				for (Body b = _bodyList; b != null; b = b.GetNext())
+				{
+					XForm xf = b.GetXForm();
+					for (Shape s = b.GetShapeList(); s != null; s = s.GetNext())
+					{
+						if (s.GetType() != ShapeType.PolygonShape)
+						{
+							continue;
+						}
+
+						PolygonShape poly = (PolygonShape)s;
+						OBB obb = poly.GetOBB();
+						Vec2 h = obb.Extents;
+						Vec2[] vs = new Vec2[4];
+						vs[0].Set(-h.X, -h.Y);
+						vs[1].Set(h.X, -h.Y);
+						vs[2].Set(h.X, h.Y);
+						vs[3].Set(-h.X, h.Y);
+
+						for (int i = 0; i < 4; ++i)
+						{
+							vs[i] = obb.Center + Common.Math.Mul(obb.R, vs[i]);
+							vs[i] = Common.Math.Mul(xf, vs[i]);
+						}
+
+						_debugDraw.DrawPolygon(vs, 4, color);
+					}
+				}
+			}
+
 			if ((flags & DebugDraw.DrawFlags.CenterOfMass) != 0)
 			{
 				for (Body b = _bodyList; b != null; b = b.GetNext())
@@ -1420,17 +1399,17 @@ namespace Box2DX.Dynamics
 		//Is it safe to pass private static function pointers?
 		private static float RaycastSortKey(object data)
 		{
-			Fixture fixture = data as Fixture;
-			Box2DXDebug.Assert(fixture != null);
-			Body body = fixture.Body;
+			Shape shape = data as Shape;
+			Box2DXDebug.Assert(shape != null);
+			Body body = shape.GetBody();
 			World world = body.GetWorld();
+			XForm xf = body.GetXForm();
 
-			if (world._contactFilter != null && !world._contactFilter.RayCollide(world._raycastUserData, fixture))
+			if (world._contactFilter!=null && !world._contactFilter.RayCollide(world._raycastUserData, shape))
 				return -1;
 
 			float lambda;
-
-			SegmentCollide collide = fixture.TestSegment(out lambda, out world._raycastNormal, world._raycastSegment, 1);
+			SegmentCollide collide = shape.TestSegment(xf, out lambda, out world._raycastNormal, world._raycastSegment, 1);
 
 			if (world._raycastSolidShape && collide == SegmentCollide.MissCollide)
 				return -1;
diff --git a/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs b/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs
index 4f7909b..6bd7f35 100644
--- a/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs
+++ b/Box2d/Assets/Program/Box2d/Dynamics/WorldCallbacks.cs
@@ -1,6 +1,6 @@
 /*
-  Box2DX Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx
-  Box2D original C++ version Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
+  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 
   This software is provided 'as-is', without any express or implied
   warranty.  In no event will the authors be held liable for any damages
@@ -20,6 +20,8 @@
 */
 
 using System;
+using System.Collections.Generic;
+using System.Text;
 
 using Box2DX.Common;
 using Box2DX.Collision;
@@ -43,7 +45,7 @@ namespace Box2DX.Dynamics
 		/// Called when any shape is about to be destroyed due
 		/// to the destruction of its parent body.
 		/// </summary>
-		public abstract void SayGoodbye(Fixture fixture);
+		public abstract void SayGoodbye(Shape shape);
 	}
 
 	/// <summary>
@@ -69,45 +71,42 @@ namespace Box2DX.Dynamics
 		/// If you implement your own collision filter you may want to build from this implementation.
 		/// @warning for performance reasons this is only called when the AABBs begin to overlap.
 		/// </summary>
-		public virtual bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
+		public virtual bool ShouldCollide(Shape shape1, Shape shape2)
 		{
-			FilterData filterA = fixtureA.Filter;
-			FilterData filterB = fixtureB.Filter;
+			FilterData filter1 = shape1.FilterData;
+			FilterData filter2 = shape2.FilterData;
 
-			if (filterA.GroupIndex == filterB.GroupIndex && filterA.GroupIndex != 0)
+			if (filter1.GroupIndex == filter2.GroupIndex && filter1.GroupIndex != 0)
 			{
-				return filterA.GroupIndex > 0;
+				return filter1.GroupIndex > 0;
 			}
 
-			bool collide = (filterA.MaskBits & filterB.CategoryBits) != 0 && (filterA.CategoryBits & filterB.MaskBits) != 0;
+			bool collide = (filter1.MaskBits & filter2.CategoryBits) != 0 && (filter1.CategoryBits & filter2.MaskBits) != 0;
 			return collide;
 		}
 
 		/// <summary>
 		/// Return true if the given shape should be considered for ray intersection.
 		/// </summary>
-		public bool RayCollide(object userData, Fixture fixture)
+		public bool RayCollide(object userData, Shape shape)
 		{
 			//By default, cast userData as a shape, and then collide if the shapes would collide
 			if (userData == null)
-			{
 				return true;
-			}
-
-			return ShouldCollide((Fixture)userData, fixture);
+			return ShouldCollide((Shape)userData, shape);
 		}
 	}
 
-	/// Contact impulses for reporting. Impulses are used instead of forces because
-	/// sub-step forces may approach infinity for rigid body collisions. These
-	/// match up one-to-one with the contact points in b2Manifold.
-	public class ContactImpulse
+	public class WorldCallback
 	{
-		public float[] normalImpulses = new float[Settings.MaxManifoldPoints];
-		public float[] tangentImpulses = new float[Settings.MaxManifoldPoints];
+		/// <summary>
+		/// The default contact filter.
+		/// </summary>
+		public static ContactFilter DefaultFilter = new ContactFilter();
 	}
 
-	/// Implement this class to get contact information. You can use these results for
+	/// <summary>
+	/// Implement this class to get collision results. You can use these results for
 	/// things like sounds and game logic. You can also get contact results by
 	/// traversing the contact lists after the time step. However, you might miss
 	/// some contacts because continuous physics leads to sub-stepping.
@@ -115,34 +114,33 @@ namespace Box2DX.Dynamics
 	/// single time step.
 	/// You should strive to make your callbacks efficient because there may be
 	/// many callbacks per time step.
-	/// @warning You cannot create/destroy Box2DX entities inside these callbacks.
-	public interface ContactListener
+	/// @warning The contact separation is the last computed value.
+	/// @warning You cannot create/destroy Box2D entities inside these callbacks.
+	/// </summary>
+	public abstract class ContactListener
 	{
-		/// Called when two fixtures begin to touch.
-		void BeginContact(Contact contact);
-
-		/// Called when two fixtures cease to touch.
-		void EndContact(Contact contact);
-
-		/// This is called after a contact is updated. This allows you to inspect a
-		/// contact before it goes to the solver. If you are careful, you can modify the
-		/// contact manifold (e.g. disable contact).
-		/// A copy of the old manifold is provided so that you can detect changes.
-		/// Note: this is called only for awake bodies.
-		/// Note: this is called even when the number of contact points is zero.
-		/// Note: this is not called for sensors.
-		/// Note: if you set the number of contact points to zero, you will not
-		/// get an EndContact callback. However, you may get a BeginContact callback
-		/// the next step.
-		void PreSolve(Contact contact, Manifold oldManifold);
-
-		/// This lets you inspect a contact after the solver is finished. This is useful
-		/// for inspecting impulses.
-		/// Note: the contact manifold does not include time of impact impulses, which can be
-		/// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly
-		/// in a separate data structure.
-		/// Note: this is only called for contacts that are touching, solid, and awake.
-		void PostSolve(Contact contact, ContactImpulse impulse);
+		/// <summary>
+		/// Called when a contact point is added. This includes the geometry
+		/// and the forces.
+		/// </summary>
+		public virtual void Add(ContactPoint point) { return; }
+
+		/// <summary>
+		/// Called when a contact point persists. This includes the geometry
+		/// and the forces.
+		/// </summary>
+		public virtual void Persist(ContactPoint point) { return; }
+
+		/// <summary>
+		/// Called when a contact point is removed. This includes the last
+		/// computed geometry and forces.
+		/// </summary>
+		public virtual void Remove(ContactPoint point) { return; }
+
+		/// <summary>
+		/// Called after a contact point is solved.
+		/// </summary>
+		public virtual void Result(ContactResult point) { return; }
 	}
 
 	/// <summary>
@@ -156,10 +154,6 @@ namespace Box2DX.Dynamics
 		{
 			R = r; G = g; B = b;
 		}
-		public void Set(float r, float g, float b)
-		{
-			R = r; G = g; B = b;
-		}
 	}
 
 	/// <summary>
@@ -173,13 +167,12 @@ namespace Box2DX.Dynamics
 		{
 			Shape = 0x0001, // draw shapes
 			Joint = 0x0002, // draw joint connections
-			CoreShape = 0x0004, // draw core (TOI) shapes       // should be removed in this revision?
+			CoreShape = 0x0004, // draw core (TOI) shapes
 			Aabb = 0x0008, // draw axis aligned bounding boxes
-			Obb = 0x0010, // draw oriented bounding boxes       // should be removed in this revision?
+			Obb = 0x0010, // draw oriented bounding boxes
 			Pair = 0x0020, // draw broad-phase pairs
-			CenterOfMass = 0x0040, // draw center of mass frame
-			Controller = 0x0080 // draw center of mass frame
-		};
+			CenterOfMass = 0x0040 // draw center of mass frame
+		}
 
 		protected DrawFlags _drawFlags;