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 void DrawSolidCircle(Vec2 center, float radius, Vec2 axis, Color color)
		{
			float k_segments = 16.0f;
			float k_increment = 2.0f * Box2DX.Common.Settings.Pi / k_segments;
			float theta = 0.0f;
			Gl.glEnable(Gl.GL_BLEND);
			Gl.glBlendFunc(Gl.GL_SRC_ALPHA, Gl.GL_ONE_MINUS_SRC_ALPHA);
			Gl.glColor4f(0.5f * color.R, 0.5f * color.G, 0.5f * color.B, 0.5f);
			Gl.glBegin(Gl.GL_TRIANGLE_FAN);
			for (int i = 0; i < k_segments; ++i)
			{
				Vec2 v = center + radius * new Vec2((float)SysMath.Cos(theta), (float)SysMath.Sin(theta));
				Gl.glVertex2f(v.X, v.Y);
				theta += k_increment;
			}
			Gl.glEnd();
			Gl.glDisable(Gl.GL_BLEND);

			theta = 0.0f;
			Gl.glColor4f(color.R, color.G, color.B, 1.0f);
			Gl.glBegin(Gl.GL_LINE_LOOP);
			for (int i = 0; i < k_segments; ++i)
			{
				Vec2 v = center + radius * new Vec2((float)SysMath.Cos(theta), (float)SysMath.Sin(theta));
				Gl.glVertex2f(v.X, v.Y);
				theta += k_increment;
			}
			Gl.glEnd();

			Vec2 p = center + radius * axis;
			Gl.glBegin(Gl.GL_LINES);
			Gl.glVertex2f(center.X, center.Y);
			Gl.glVertex2f(p.X, p.Y);
			Gl.glEnd();
		}

 public World(AABB worldAABB, Vec2 gravity, bool doSleep)
 {
     this._destructionListener = null;
     this._boundaryListener = null;
     this._contactFilter = WorldCallback.DefaultFilter;
     this._contactListener = null;
     this._debugDraw = null;
     this._bodyList = null;
     this._contactList = null;
     this._jointList = null;
     this._bodyCount = 0;
     this._contactCount = 0;
     this._jointCount = 0;
     this._warmStarting = true;
     this._continuousPhysics = true;
     this._allowSleep = doSleep;
     this._gravity = gravity;
     this._lock = false;
     this._inv_dt0 = 0f;
     this._contactManager = new ContactManager();
     this._contactManager._world = this;
     this._broadPhase = new BroadPhase(worldAABB, this._contactManager);
     BodyDef def = new BodyDef();
     this._groundBody = this.CreateBody(def);
 }

        public override void DrawSolidCircle(Vec2 center, float radius, Box2DX.Common.Vec2 axis, Color color)
        {
            float k_segments = 16.0f;
            float k_increment = 2.0f * (float)System.Math.PI / k_segments;
            float theta = 0.0f;
            GL.Color3(0.5f * color.R, 0.5f * color.G, 0.5f * color.B);
            GL.Disable(EnableCap.Texture2D);
            GL.Begin(BeginMode.TriangleFan);
            for (int i = 0; i < k_segments; ++i) {
                Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
                GL.Vertex3(v.X, v.Y, ZLayer);
                theta += k_increment;
            }
            GL.End();

            theta = 0.0f;
            GL.Color4(color.R, color.G, color.B, 1.0f);
            GL.Begin(BeginMode.LineLoop);
            for (int i = 0; i < k_segments; ++i) {
                Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
                GL.Vertex3(v.X, v.Y, ZLayer);
                theta += k_increment;
            }
            GL.End();

            Vec2 p = center + radius * axis;
            GL.Begin(BeginMode.Lines);
            GL.Vertex3(center.X, center.Y, ZLayer);
            GL.Vertex3(p.X, p.Y, 0);
            GL.End();
            GL.Enable(EnableCap.Texture2D);
        }

 public void Set(Vec2 x1, float a1, Vec2 x2, float a2)
 {
     this.Linear1 = x1;
     this.Angular1 = a1;
     this.Linear2 = x2;
     this.Angular2 = a2;
 }

 public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
 {
     lambda = 0f;
     normal = default(Vec2);
     Vec2 p = segment.P1;
     Vec2 a = segment.P2 - p;
     Vec2 a2 = this.P2 - this.P1;
     Vec2 vec = Vec2.Cross(a2, 1f);
     float num = 100f * Settings.FLT_EPSILON;
     float num2 = -Vec2.Dot(a, vec);
     bool result;
     if (num2 > num)
     {
         Vec2 a3 = p - this.P1;
         float num3 = Vec2.Dot(a3, vec);
         if (0f <= num3 && num3 <= maxLambda * num2)
         {
             float num4 = -a.X * a3.Y + a.Y * a3.X;
             if (-num * num2 <= num4 && num4 <= num2 * (1f + num))
             {
                 num3 /= num2;
                 vec.Normalize();
                 lambda = num3;
                 normal = vec;
                 result = true;
                 return result;
             }
         }
     }
     result = false;
     return result;
 }

 public override void Step(TimeStep 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 (Shape shape = body.GetShapeList(); shape != null; shape = shape.GetNext())
         {
             Vec2 sc;
             float sarea = shape.ComputeSubmergedArea(normal, offset, body.GetXForm(), 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;
         massc.X /= mass;
         massc.Y /= mass;
         if (area < Box2DX.Common.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 void Initialize(Body body1, Body body2, Vec2 anchor)
 {
     this.Body1 = body1;
     this.Body2 = body2;
     this.LocalAnchor1 = body1.GetLocalPoint(anchor);
     this.LocalAnchor2 = body2.GetLocalPoint(anchor);
     this.ReferenceAngle = body2.GetAngle() - body1.GetAngle();
 }

 public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis)
 {
     this.Body1 = body1;
     this.Body2 = body2;
     this.localAnchor1 = body1.GetLocalPoint(anchor);
     this.localAnchor2 = body2.GetLocalPoint(anchor);
     this.localAxis1 = body1.GetLocalVector(axis);
 }

        public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction)
        {
            _fixture = fixture;
            _point = point;
            _normal = normal;

            return fraction;
        }

 public void SetTarget(Vec2 target)
 {
     if (this._body2.IsSleeping())
     {
         this._body2.WakeUp();
     }
     this._target = target;
 }

 public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2)
 {
     this.Body1 = body1;
     this.Body2 = body2;
     this.LocalAnchor1 = body1.GetLocalPoint(anchor1);
     this.LocalAnchor2 = body2.GetLocalPoint(anchor2);
     this.Length = (anchor2 - anchor1).Length();
 }

 public static void DrawSegment(Vec2 p1, Vec2 p2, Color color, params object[] p)
 {
     Gl.Color3(color.R, color.G, color.B);
                 Gl.Begin(BeginMode.Lines);
                 Gl.Vertex2(p1.X, p1.Y);
                 Gl.Vertex2(p2.X, p2.Y);
                 Gl.End();
 }

		/// <summary>
		/// 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;
			Body2 = body2;
			LocalAnchor1 = body1.GetLocalPoint(anchor);
			LocalAnchor2 = body2.GetLocalPoint(anchor);
			LocalAxis1 = body1.GetLocalVector(axis);
			ReferenceAngle = body2.GetAngle() - body1.GetAngle();
		}

 public static void DrawPoint(Vec2 p, float size, Color color)
 {
     Gl.glPointSize(size);
     Gl.glBegin(Gl.GL_POINTS);
     Gl.glColor3f(color.R, color.G, color.B);
     Gl.glVertex2f(p.X, p.Y);
     Gl.glEnd();
     Gl.glPointSize(1.0f);
 }

 public static void DrawPoint(Vec2 p, float size, Color color)
 {
     Gl.PointSize(size);
                 Gl.Begin(BeginMode.Points);
                 Gl.Color3(color.R, color.G, color.B);
                 Gl.Vertex2(p.X, p.Y);
                 Gl.End();
                 Gl.PointSize(1.0f);
 }

		/// <summary>
		/// Initialize the bodies, anchors, and length using the world anchors.
		/// </summary>
		public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2)
		{
			Body1 = body1;
			Body2 = body2;
			LocalAnchor1 = body1.GetLocalPoint(anchor1);
			LocalAnchor2 = body2.GetLocalPoint(anchor2);
			Vec2 d = anchor2 - anchor1;
			Length = d.Length();
		}

		public override void DrawPolygon(Vec2[] vertices, int vertexCount, Color color)
		{
			Gl.glColor3f(color.R, color.G, color.B);
			Gl.glBegin(Gl.GL_LINE_LOOP);
			for (int i = 0; i < vertexCount; ++i)
			{
				Gl.glVertex2f(vertices[i].X, vertices[i].Y);
			}
			Gl.glEnd();			
		}

 public override void DrawPolygon(Vec2[] vertices, int vertexCount, Color color)
 {
     GL.Color3(color.R, color.G, color.B);
     GL.Disable(EnableCap.Texture2D);
     GL.Begin(BeginMode.LineLoop);
     for (int i = 0; i < vertexCount; ++i) {
         GL.Vertex3(vertices[i].X, vertices[i].Y, ZLayer);
     }
     GL.End();
     GL.Enable(EnableCap.Texture2D);
 }

 /// <summary>
 /// Solve A * x = b, where b is a column vector. This is more efficient
 /// than computing the inverse in one-shot cases. Solve only the upper
 /// 2-by-2 matrix equation.
 /// </summary>
 public Vec2 Solve22(Vec2 b)
 {
     float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y;
     float det = a11 * a22 - a12 * a21;
     Box2DXDebug.Assert(det != 0.0f);
     det = 1.0f / det;
     Vec2 x = new Vec2();
     x.X = det * (a22 * b.X - a12 * b.Y);
     x.Y = det * (a11 * b.Y - a21 * b.X);
     return x;
 }