xnabox2demo/Box2D.XNA/Dynamics/World.cs

/*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* 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.Diagnostics;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using System.Collections.Generic;

namespace Box2D.XNA
{
    [Flags]
    public enum WorldFlags
    {
        NewFixture  = (1 << 0),
        Locked      = (1 << 1),
        ClearForces = (1 << 2),
    };

    /// The world class manages all physics entities, dynamic simulation,
    /// and asynchronous queries. The world also contains efficient memory
    /// management facilities.
    public class World
    {
        /// ruct a world object.
        /// @param gravity the world gravity vector.
        /// @param doSleep improve performance by not simulating inactive bodies.
        public World(Vector2 gravity, bool doSleep)
        {
            WarmStarting = true;
            ContinuousPhysics = true;

            _allowSleep = doSleep;
            Gravity = gravity;

            _flags = WorldFlags.ClearForces;

            _queryAABBCallbackWrapper = QueryAABBCallbackWrapper;
            _rayCastCallbackWrapper = RayCastCallbackWrapper;
        }

	    /// Register a destruction listener.
	    public IDestructionListener DestructionListener { get; set; }

	    /// Register a contact filter to provide specific control over collision.
	    /// Otherwise the default filter is used (Settings.b2_defaultFilter).
	    public IContactFilter ContactFilter
        {
            get
            {
                return _contactManager.ContactFilter;
            }
            set
            {
                _contactManager.ContactFilter = value;
            }
        }

	    /// Register a contact event listener
	    public IContactListener ContactListener
        {
            get
            {
                return _contactManager.ContactListener;
            }
            set
            {
                _contactManager.ContactListener = value;
            }
        }

	    /// Register a routine for debug drawing. The debug draw functions are called
	    /// inside the World.Step method, so make sure your renderer is ready to
	    /// consume draw commands when you call Step().
	    public DebugDraw DebugDraw { get; set; }

	    /// Create a rigid body given a definition. No reference to the definition
	    /// is retained.
	    /// @warning This function is locked during callbacks.
	    public Body CreateBody(BodyDef def)
        {
            Debug.Assert(!IsLocked);
	        if (IsLocked)
	        {
		        return null;
	        }

	        var b = new Body(def, this);

	        // Add to world doubly linked list.
	        b._prev = null;
	        b._next = _bodyList;
	        if (_bodyList != null)
	        {
		        _bodyList._prev = b;
	        }
	        _bodyList = b;
	        ++_bodyCount;

	        return b;
        }

	    /// Destroy a rigid body given a definition. No reference to the definition
	    /// is retained. This function is locked during callbacks.
	    /// @warning This automatically deletes all associated shapes and joints.
	    /// @warning This function is locked during callbacks.
	    public void DestroyBody(Body b)
        {
            Debug.Assert(_bodyCount > 0);
	        Debug.Assert(!IsLocked);
	        if (IsLocked)
	        {
		        return;
	        }

	        // Delete the attached joints.
	        JointEdge je = b._jointList;
	        while (je != null)
	        {
		        JointEdge je0 = je;
		        je = je.Next;

		        if (DestructionListener != null)
		        {
			        DestructionListener.SayGoodbye(je0.Joint);
		        }

		        DestroyJoint(je0.Joint);
	        }
	        b._jointList = null;

	        // Delete the attached contacts.
	        ContactEdge ce = b._contactList;
	        while (ce != null)
	        {
		        ContactEdge ce0 = ce;
		        ce = ce.Next;
		        _contactManager.Destroy(ce0.Contact);
	        }
	        b._contactList = null;

	        // Delete the attached fixtures. This destroys broad-phase proxies.
	        Fixture f = b._fixtureList;
	        while (f != null)
	        {
		        Fixture f0 = f;
		        f = f._next;

		        if (DestructionListener != null)
		        {
			        DestructionListener.SayGoodbye(f0);
		        }

                f0.DestroyProxies(_contactManager._broadPhase);
		        f0.Destroy();
	        }
	        b._fixtureList = null;
	        b._fixtureCount = 0;

	        // Remove world body list.
	        if (b._prev != null)
	        {
		        b._prev._next = b._next;
	        }

            if (b._next != null)
	        {
		        b._next._prev = b._prev;
	        }

	        if (b == _bodyList)
	        {
		        _bodyList = b._next;
	        }

	        --_bodyCount;
        }

	    /// Create a joint to rain bodies together. No reference to the definition
	    /// is retained. This may cause the connected bodies to cease colliding.
	    /// @warning This function is locked during callbacks.
	    public Joint CreateJoint(JointDef def)
        {
	        Debug.Assert(!IsLocked);
	        if (IsLocked)
	        {
		        return null;
	        }

	        Joint j = Joint.Create(def);

	        // Connect to the world list.
	        j._prev = null;
	        j._next = _jointList;
	        if (_jointList != null)
	        {
		        _jointList._prev = j;
	        }
	        _jointList = j;
	        ++_jointCount;

	        // Connect to the bodies' doubly linked lists.
	        j._edgeA.Joint = j;
	        j._edgeA.Other = j._bodyB;
	        j._edgeA.Prev = null;
	        j._edgeA.Next = j._bodyA._jointList;

	        if (j._bodyA._jointList != null) 
                j._bodyA._jointList.Prev = j._edgeA;

	        j._bodyA._jointList = j._edgeA;

	        j._edgeB.Joint = j;
	        j._edgeB.Other = j._bodyA;
	        j._edgeB.Prev = null;
	        j._edgeB.Next = j._bodyB._jointList;

	        if (j._bodyB._jointList != null) 
                j._bodyB._jointList.Prev = j._edgeB;

	        j._bodyB._jointList = j._edgeB;

	        Body bodyA = def.bodyA;
	        Body bodyB = def.bodyB;

	        bool staticA = bodyA.GetType() == BodyType.Static;
	        bool staticB = bodyB.GetType() == BodyType.Static;

	        // If the joint prevents collisions, then flag any contacts for filtering.
	        if (def.collideConnected == false)
	        {
		        ContactEdge edge = bodyB.GetContactList();
		        while (edge != null)
		        {
			        if (edge.Other == bodyA)
			        {
				        // Flag the contact for filtering at the next time step (where either
				        // body is awake).
				        edge.Contact.FlagForFiltering();
			        }

			        edge = edge.Next;
		        }
	        }

	        // Note: creating a joint doesn't wake the bodies.

	        return j;
        }

	    /// Destroy a joint. This may cause the connected bodies to begin colliding.
	    /// @warning This function is locked during callbacks.
	    public void DestroyJoint(Joint j)
        {
	        Debug.Assert(!IsLocked);
	        if (IsLocked)
	        {
		        return;
	        }

	        bool collideConnected = j._collideConnected;

	        // Remove from the doubly linked list.
	        if (j._prev != null)
	        {
		        j._prev._next = j._next;
	        }

	        if (j._next != null)
	        {
		        j._next._prev = j._prev;
	        }

	        if (j == _jointList)
	        {
		        _jointList = j._next;
	        }

	        // Disconnect from island graph.
	        Body bodyA = j._bodyA;
	        Body bodyB = j._bodyB;

	        // Wake up connected bodies.
	        bodyA.SetAwake(true);
	        bodyB.SetAwake(true);

	        // Remove from body 1.
	        if (j._edgeA.Prev != null)
	        {
		        j._edgeA.Prev.Next = j._edgeA.Next;
	        }

	        if (j._edgeA.Next != null)
	        {
		        j._edgeA.Next.Prev = j._edgeA.Prev;
	        }

	        if (j._edgeA == bodyA._jointList)
	        {
		        bodyA._jointList = j._edgeA.Next;
	        }

	        j._edgeA.Prev = null;
	        j._edgeA.Next = null;

	        // Remove from body 2
	        if (j._edgeB.Prev != null)
	        {
		        j._edgeB.Prev.Next = j._edgeB.Next;
	        }

	        if (j._edgeB.Next != null)
	        {
		        j._edgeB.Next.Prev = j._edgeB.Prev;
	        }

	        if (j._edgeB == bodyB._jointList)
	        {
		        bodyB._jointList = j._edgeB.Next;
	        }

	        j._edgeB.Prev = null;
	        j._edgeB.Next = null;

	        Debug.Assert(_jointCount > 0);
	        --_jointCount;

	        // If the joint prevents collisions, then flag any contacts for filtering.
	        if (collideConnected == false)
	        {
		        ContactEdge edge = bodyB.GetContactList();
		        while (edge != null)
		        {
			        if (edge.Other == bodyA)
			        {
				        // Flag the contact for filtering at the next time step (where either
				        // body is awake).
				        edge.Contact.FlagForFiltering();
			        }

			        edge = edge.Next;
		        }
	        }
        }

	    /// Take a time step. This performs collision detection, integration,
	    /// and raint solution.
	    /// @param timeStep the amount of time to simulate, this should not vary.
	    /// @param velocityIterations for the velocity raint solver.
	    /// @param positionIterations for the position raint solver.
	    public void Step(float dt, int velocityIterations, int positionIterations)
        {
	        // If new fixtures were added, we need to find the new contacts.
	        if ((_flags & WorldFlags.NewFixture) == WorldFlags.NewFixture)
	        {
		        _contactManager.FindNewContacts();
		        _flags &= ~WorldFlags.NewFixture;
	        }

	        _flags |= WorldFlags.Locked;

	        TimeStep step;
	        step.dt = dt;
	        step.velocityIterations	= velocityIterations;
	        step.positionIterations = positionIterations;
	        if (dt > 0.0f)
	        {
		        step.inv_dt = 1.0f / dt;
	        }
	        else
	        {
		        step.inv_dt = 0.0f;
	        }

	        step.dtRatio = _inv_dt0 * dt;

	        step.warmStarting = WarmStarting;

	        // Update contacts. This is where some contacts are destroyed.
	        _contactManager.Collide();

	        // Integrate velocities, solve velocity raints, and integrate positions.
	        if (step.dt > 0.0f)
	        {
		        Solve(ref step);
	        }

	        // Handle TOI events.
	        if (ContinuousPhysics && step.dt > 0.0f)
	        {
		        SolveTOI();
	        }

	        if (step.dt > 0.0f)
	        {
		        _inv_dt0 = step.inv_dt;
	        }

            if ((_flags & WorldFlags.ClearForces) != 0)
            {
                ClearForces();
            }

	        _flags &= ~WorldFlags.Locked;
        }

        /// Call this after you are done with time steps to clear the forces. You normally
	    /// call this after each call to Step, unless you are performing sub-steps. By default,
	    /// forces will be automatically cleared, so you don't need to call this function.
	    /// @see SetAutoClearForces
        public void ClearForces()
        {
            for (Body body = _bodyList; body != null; body = body.GetNext())
            {
                body._force = Vector2.Zero;
                body._torque = 0.0f;
            }
        }

        /// Set flag to control automatic clearing of forces after each time step.
	    void SetAutoClearForces(bool flag)
        {
            if (flag)
            {
                _flags |= WorldFlags.ClearForces;
            }
            else
            {
                _flags &= ~WorldFlags.ClearForces;
            }
        }

	    /// Get the flag that controls automatic clearing of forces after each time step.
        bool GetAutoClearForces()
        {
            return (_flags & WorldFlags.ClearForces) == WorldFlags.ClearForces;
        }

        /// Get the contact manager for testing.
        public ContactManager GetContactManager()
        {
            return _contactManager;
        }

	    /// Call this to draw shapes and other debug draw data.
	    public void DrawDebugData()
        {
	        if (DebugDraw == null)
	        {
		        return;
	        }

	        DebugDrawFlags flags = DebugDraw.Flags;

	        if ((flags & DebugDrawFlags.Shape) == DebugDrawFlags.Shape)
	        {
		        for (Body b = _bodyList; b != null; b = b.GetNext())
		        {
                    Transform xf;
                    b.GetTransform(out xf);
			        for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
			        {
                        if (b.IsActive() == false)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.5f, 0.3f));
                        }
                        else if (b.GetType() == BodyType.Static)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
                        }
                        else if (b.GetType() == BodyType.Kinematic)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
                        }
				        else if (b.IsAwake() == false)
				        {
					        DrawShape(f, xf, new Color(0.6f, 0.6f, 0.6f));
				        }
				        else
				        {
					        DrawShape(f, xf, new Color(0.9f, 0.7f, 0.7f));
				        }
			        }
		        }
	        }

	        if ((flags & DebugDrawFlags.Joint) == DebugDrawFlags.Joint)
	        {
		        for (Joint j = _jointList; j != null; j = j.GetNext())
		        {
				    DrawJoint(j);
		        }
	        }

	        if ((flags & DebugDrawFlags.Pair) == DebugDrawFlags.Pair)
	        {
		        Color color = new Color(0.3f, 0.9f, 0.9f);
		        for (Contact c = _contactManager._contactList; c != null; c = c.GetNext())
		        {
			      /*  
                   Fixture fixtureA = c.GetFixtureA();
			       Fixture fixtureB = c.GetFixtureB();

                   AABB aabbA;
                   AABB aabbB;
                   fixtureA.GetAABB(out aabbA);
                   fixtureB.GetAABB(out aabbB);

                   Vector2 cA = aabbA.GetCenter();
                   Vector2 cB = aabbB.GetCenter();

                   DebugDraw.DrawSegment(cA, cB, color);
                  */
		        }
	        }

	        if ((flags & DebugDrawFlags.AABB) == DebugDrawFlags.AABB)
	        {
		        Color color = new Color(0.9f, 0.3f, 0.9f);
		        BroadPhase bp = _contactManager._broadPhase;

		        for (Body b = _bodyList; b != null; b = b.GetNext())
		        {
                    if (b.IsActive() == false)
			        {
				        continue;
			        }

			        for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
			        {
                        for (int i = 0; i < f._proxyCount; ++i)
				        {
					        FixtureProxy proxy = f._proxies[i];
					        AABB aabb;
                            bp.GetFatAABB(proxy.proxyId, out aabb);
                            FixedArray8<Vector2> vs = new FixedArray8<Vector2>();
					        vs[0] = new Vector2(aabb.lowerBound.X, aabb.lowerBound.Y);
					        vs[1] = new Vector2(aabb.upperBound.X, aabb.lowerBound.Y);
					        vs[2] = new Vector2(aabb.upperBound.X, aabb.upperBound.Y);
					        vs[3] = new Vector2(aabb.lowerBound.X, aabb.upperBound.Y);

					        DebugDraw.DrawPolygon(ref vs, 4, color);
				        }
			        }
		        }
	        }

	        if ((flags & DebugDrawFlags.CenterOfMass) == DebugDrawFlags.CenterOfMass)
	        {
		        for (Body b = _bodyList; b != null; b = b.GetNext())
		        {
                    Transform xf;
                    b.GetTransform(out xf);
			        xf.Position = b.GetWorldCenter();
			        DebugDraw.DrawTransform(ref xf);
		        }
	        }
        }

	    /// Query the world for all fixtures that potentially overlap the
	    /// provided AABB.
	    /// @param callback a user implemented callback class.
	    /// @param aabb the query box.
	    public void QueryAABB(Func<FixtureProxy, bool> callback, ref AABB aabb)
        {
            _queryAABBCallback = callback;
            _contactManager._broadPhase.Query(_queryAABBCallbackWrapper, ref aabb);
            _queryAABBCallback = null;
        }

        Func<FixtureProxy, bool> _queryAABBCallback;
        Func<int, bool> _queryAABBCallbackWrapper;

        bool QueryAABBCallbackWrapper(int proxyId)
        {
            FixtureProxy proxy = (FixtureProxy)_contactManager._broadPhase.GetUserData(proxyId);
            return _queryAABBCallback(proxy);
        }

        /// Ray-cast the world for all fixtures in the path of the ray. Your callback
	    /// controls whether you get the closest point, any point, or n-points.
	    /// The ray-cast ignores shapes that contain the starting point.
	    /// @param callback a user implemented callback class.
	    /// @param point1 the ray starting point
	    /// @param point2 the ray ending point
        public void RayCast(RayCastCallback callback, Vector2 point1, Vector2 point2)
        {
            RayCastInput input = new RayCastInput();
            input.maxFraction = 1.0f;
            input.p1 = point1;
            input.p2 = point2;

            _rayCastCallback = callback;
            _contactManager._broadPhase.RayCast(_rayCastCallbackWrapper, ref input);
            _rayCastCallback = null;
        }

        RayCastCallback _rayCastCallback;
        RayCastCallbackInternal _rayCastCallbackWrapper;

        float RayCastCallbackWrapper(ref RayCastInput input, int proxyId)
	    {
		    object userData = _contactManager._broadPhase.GetUserData(proxyId);
            FixtureProxy proxy = (FixtureProxy)userData;
            Fixture fixture = proxy.fixture;
            int index = proxy.childIndex;
		    RayCastOutput output;
		    bool hit = fixture.RayCast(out output, ref input, index);

		    if (hit)
		    {
			    float fraction = output.fraction;
			    Vector2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
                return _rayCastCallback(fixture, point, output.normal, fraction);
		    }

		    return input.maxFraction;
	    }

	    /// Get the world body list. With the returned body, use Body.GetNext to get
	    /// the next body in the world list. A null body indicates the end of the list.
	    /// @return the head of the world body list.
	    public Body GetBodyList()
        {
            return _bodyList;
        }

	    /// Get the world joint list. With the returned joint, use Joint.GetNext to get
	    /// the next joint in the world list. A null joint indicates the end of the list.
	    /// @return the head of the world joint list.
	    public Joint GetJointList()
        {
            return _jointList;
        }

	    /// Get the world contact list. With the returned contact, use Contact.GetNext to get
	    /// the next contact in the world list. A null contact indicates the end of the list.
	    /// @return the head of the world contact list.
	    /// @warning contacts are 
	    public Contact GetContactList()
        {
            return _contactManager._contactList;
        }

	    /// Enable/disable warm starting. For testing.
	    public bool WarmStarting { get; set; }

	    /// Enable/disable continuous physics. For testing.
	    public bool ContinuousPhysics { get; set; }

	    /// Get the number of broad-phase proxies.
	    public int ProxyCount 
        {  
            get
            {
                return _contactManager._broadPhase.ProxyCount;
            }
        }

	    /// Get the number of bodies.
	    public int BodyCount
        {
            get
            {
                return _bodyCount;
            }
        }

	    /// Get the number of joints.
	    public int JointCount
        {
            get
            {
                return _jointCount;
            }
        }
	    /// Get the number of contacts (each may have 0 or more contact points).
	    public int ContactCount
        {
            get
            {
                return _contactManager._contactCount;
            }
        }

	    /// Change the global gravity vector.
	    public Vector2 Gravity { get; set; }

	    /// Is the world locked (in the middle of a time step).
	    public bool IsLocked 
        { 
            get 
            { 
                return (_flags & WorldFlags.Locked) == WorldFlags.Locked; 
            }
            set 
            { 
                if (value)
                {
                    _flags |= WorldFlags.Locked;
                }
                else
                {
                    _flags &= ~WorldFlags.Locked;
                }
            }
        }

	    void Solve(ref TimeStep step)
        {
	        // Size the island for the worst case.
            _island.Reset(_bodyCount,
	                      _contactManager._contactCount,
	                      _jointCount,
	                      _contactManager.ContactListener);

	        // Clear all the island flags.
	        for (Body b = _bodyList; b != null; b = b._next)
	        {
		        b._flags &= ~BodyFlags.Island;
	        }
	        for (Contact c = _contactManager._contactList; c != null; c = c._next)
	        {
		        c._flags &= ~ContactFlags.Island;
	        }
	        for (Joint j = _jointList; j != null; j = j._next)
	        {
		        j._islandFlag = false;
	        }

	        // Build and simulate all awake islands.
            int stackSize = _bodyCount;
	        if (stackSize > stack.Length)
                stack = new Body[Math.Max(stack.Length * 2, stackSize)];

	        for (Body seed = _bodyList; seed != null; seed = seed._next)
	        {
		        if ((seed._flags & (BodyFlags.Island)) != BodyFlags.None)
		        {
			        continue;
		        }

		        if (seed.IsAwake() == false || seed.IsActive() == false)
		        {
			        continue;
		        }

                // The seed can be dynamic or kinematic.
                if (seed.GetType() == BodyType.Static)
                {
                    continue;
                }

		        // Reset island and stack.
		        _island.Clear();
		        int stackCount = 0;
		        stack[stackCount++] = seed;
		        seed._flags |= BodyFlags.Island;

		        // Perform a depth first search (DFS) on the raint graph.
		        while (stackCount > 0)
		        {
			        // Grab the next body off the stack and add it to the island.
			        Body b = stack[--stackCount];
                    Debug.Assert(b.IsActive() == true);
			        _island.Add(b);

			        // Make sure the body is awake.
                    b.SetAwake(true);
                    
			        // To keep islands as small as possible, we don't
			        // propagate islands across static bodies.
			        if (b.GetType() == BodyType.Static)
			        {
				        continue;
			        }

			        // Search all contacts connected to this body.
			        for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
			        {
                        Contact contact = ce.Contact;

				        // Has this contact already been added to an island?
                        if ((contact._flags & ContactFlags.Island) != ContactFlags.None)
				        {
					        continue;
				        }

                        // Is this contact solid and touching?
                        if (!ce.Contact.IsEnabled() || !ce.Contact.IsTouching())
				        {
					        continue;
				        }

                        // Skip sensors.
                        bool sensorA = contact._fixtureA._isSensor;
                        bool sensorB = contact._fixtureB._isSensor;
                        if (sensorA || sensorB)
                        {
                            continue;
                        }

                        _island.Add(contact);
        				contact._flags |= ContactFlags.Island;

				        Body other = ce.Other;

				        // Was the other body already added to this island?
				        if ((other._flags & BodyFlags.Island) != BodyFlags.None)
				        {
					        continue;
				        }

				        Debug.Assert(stackCount < stackSize);
				        stack[stackCount++] = other;
				        other._flags |= BodyFlags.Island;
			        }

			        // Search all joints connect to this body.
			        for (JointEdge je = b._jointList; je != null; je = je.Next)
			        {
				        if (je.Joint._islandFlag == true)
				        {
					        continue;
				        }                        

				        Body other = je.Other;

                        // Don't simulate joints connected to inactive bodies.
                        if (other.IsActive() == false)
                        {
                            continue;
                        }

                        _island.Add(je.Joint);
                        je.Joint._islandFlag = true;

				        if ((other._flags & BodyFlags.Island) != BodyFlags.None)
				        {
					        continue;
				        }

				        Debug.Assert(stackCount < stackSize);
				        stack[stackCount++] = other;
				        other._flags |= BodyFlags.Island;
			        }
		        }

		        _island.Solve(ref step, Gravity, _allowSleep);

		        // Post solve cleanup.
		        for (int i = 0; i < _island._bodyCount; ++i)
		        {
			        // Allow static bodies to participate in other islands.
			        Body b = _island._bodies[i];
			        if (b.GetType() == BodyType.Static)
			        {
				        b._flags &= ~BodyFlags.Island;
			        }
		        }
	        }

	        // Synchronize fixtures, check for out of range bodies.
	        for (Body b = _bodyList; b != null; b = b.GetNext())
	        {
		        // If a body was not in an island then it did not move.
                if ((b._flags & BodyFlags.Island) != BodyFlags.Island)
		        {
			        continue;
		        }

                if (b.GetType() == BodyType.Static)
		        {
			        continue;
		        }

		        // Update fixtures (for broad-phase).
		        b.SynchronizeFixtures();
	        }

	        // Look for new contacts.
	        _contactManager.FindNewContacts();
        }

        // Advance a dynamic body to its first time of contact
        // and adjust the position to ensure clearance.
        void SolveTOI(Body body)
        {
	        // Find the minimum contact.
	        Contact toiContact = null;
	        float toi = 1.0f;
            Body toiOther = null;
	        bool found;
	        int count;
	        int iter = 0;

	        bool bullet = body.IsBullet;

	        // Iterate until all contacts agree on the minimum TOI. We have
	        // to iterate because the TOI algorithm may skip some intermediate
	        // collisions when objects rotate through each other.
	        do
	        {
		        count = 0;
		        found = false;
		        for (ContactEdge ce = body._contactList; ce != null; ce = ce.Next)
		        {
                    if (ce.Contact == toiContact)
                    {
                        continue;
                    }

			        Body other = ce.Other;
			        BodyType type = other.GetType();

			        // Only bullets perform TOI with dynamic bodies.
			        if (bullet == true)
			        {
				        // Bullets only perform TOI with bodies that have their TOI resolved.
				        if ((other._flags & BodyFlags.Toi) == 0)
				        {
					        continue;
				        }

                        // No repeated hits on non-static bodies
				        if (type != BodyType.Static && (ce.Contact._flags & ContactFlags.BulletHit) != 0)
				        {
						    continue;
				        }
			        }
			        else if (type == BodyType.Dynamic)
			        {
				        continue;
			        }

			        // Check for a disabled contact.
			        Contact contact = ce.Contact;
			        if (contact.IsEnabled() == false)
			        {
				        continue;
			        }

			        // Prevent infinite looping.
			        if (contact._toiCount > 10)
			        {
				        continue;
			        }

			        Fixture fixtureA = contact._fixtureA;
			        Fixture fixtureB = contact._fixtureB;
                    int indexA = contact._indexA;
			        int indexB = contact._indexB;

			        // Cull sensors.
			        if (fixtureA.IsSensor() || fixtureB.IsSensor())
			        {
				        continue;
			        }

			        Body bodyA = fixtureA._body;
			        Body bodyB = fixtureB._body;

			        // Compute the time of impact in interval [0, minTOI]
			        TOIInput input = new TOIInput();
			        input.proxyA.Set(fixtureA.GetShape(), indexA);
			        input.proxyB.Set(fixtureB.GetShape(), indexB);
			        input.sweepA = bodyA._sweep;
			        input.sweepB = bodyB._sweep;
			        input.tMax = toi;

			        TOIOutput output;
			        TimeOfImpact.CalculateTimeOfImpact(out output, ref input);

			        if (output.State == TOIOutputState.Touching && output.t < toi)
			        {
				        toiContact = contact;
				        toi = output.t;
                        toiOther = other;
				        found = true;
			        }

			        ++count;
		        }

		        ++iter;
	        } while (found && count > 1 && iter < 50);

	        if (toiContact == null)
	        {
                body.Advance(1.0f);
		        return;
	        }

	        Sweep backup = body._sweep;
	        body.Advance(toi);
            toiContact.Update(_contactManager.ContactListener);
	        if (toiContact.IsEnabled() == false)
	        {
		        // Contact disabled. Backup and recurse.
		        body._sweep = backup;
		        SolveTOI(body);
	        }

	        ++toiContact._toiCount;

	        // Update all the valid contacts on this body and build a contact island.
	        count = 0;
	        for (ContactEdge ce = body._contactList; (ce != null) && (count < Settings.b2_maxTOIContacts); ce = ce.Next)
	        {
		        Body other = ce.Other;
		        BodyType type = other.GetType();

                // Only perform correction with static bodies, so the
                // body won't get pushed out of the world.
		        if (type == BodyType.Dynamic)
		        {
			        continue;
		        }

		        // Check for a disabled contact.
		        Contact contact = ce.Contact;
		        if (contact.IsEnabled() == false)
		        {
			        continue;
		        }

		        Fixture fixtureA = contact._fixtureA;
		        Fixture fixtureB = contact._fixtureB;

		        // Cull sensors.
		        if (fixtureA.IsSensor() || fixtureB.IsSensor())
		        {
			        continue;
		        }

		        // The contact likely has some new contact points. The listener
		        // gives the user a chance to disable the contact.
		        if (contact != toiContact)
		        {
			        contact.Update(_contactManager.ContactListener);
		        }

		        // Did the user disable the contact?
		        if (contact.IsEnabled() == false)
		        {
			        // Skip this contact.
			        continue;
		        }

		        if (contact.IsTouching() == false)
		        {
			        continue;
		        }

		        _toiContacts[count] = contact;
		        ++count;
	        }

	        // Reduce the TOI body's overlap with the contact island.
	        _toiSolver.Initialize(_toiContacts, count, body);

	        float k_toiBaumgarte = 0.75f;
	        //bool solved = false;
	        for (int i = 0; i < 20; ++i)
	        {
		        bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte);
		        if (contactsOkay)
		        {
			        //solved = true;
			        break;
		        }
	        }

            if (toiOther.GetType() != BodyType.Static)
	        {
		        toiContact._flags |= ContactFlags.BulletHit;
	        }
        }

        // Sequentially solve TOIs for each body. We bring each
        // body to the time of contact and perform some position correction.
        // Time is not conserved.
        void SolveTOI()
        {
	        // Prepare all contacts.
	        for (Contact c = _contactManager._contactList; c != null; c = c._next)
	        {
		        // Enable the contact
		        c._flags |= ContactFlags.Enabled;

		        // Set the number of TOI events for this contact to zero.
		        c._toiCount = 0;
	        }

	        // Initialize the TOI flag.
	        for (Body body = _bodyList; body != null; body = body._next)
	        {
		        // Kinematic, and static bodies will not be affected by the TOI event.
		        // If a body was not in an island then it did not move.
		        if ((body._flags & BodyFlags.Island) == 0 || body.GetType() == BodyType.Kinematic || body.GetType() == BodyType.Static)
		        {
			        body._flags |= BodyFlags.Toi;
		        }
		        else
		        {
			        body._flags &= ~BodyFlags.Toi;
		        }
	        }

	        // Collide non-bullets.
	        for (Body body = _bodyList; body != null; body = body._next)
	        {
		        if ((body._flags & BodyFlags.Toi) != BodyFlags.None)
		        {
			        continue;
		        }

		        if (body.IsBullet == true)
		        {
			        continue;
		        }

		        SolveTOI(body);

		        body._flags |= BodyFlags.Toi;
	        }

	        // Collide bullets.
	        for (Body body = _bodyList; body != null; body = body._next)
	        {
                if ((body._flags & BodyFlags.Toi) != BodyFlags.None)
		        {
			        continue;
		        }

		        if (body.IsBullet == false)
		        {
			        continue;
		        }

		        SolveTOI(body);

		        body._flags |= BodyFlags.Toi;
	        }
        }

	    void DrawJoint(Joint joint)
        {
	        Body b1 = joint.GetBodyA();
	        Body b2 = joint.GetBodyB();
            Transform xf1, xf2;
            b1.GetTransform(out xf1);
	        b2.GetTransform(out xf2);
	        Vector2 x1 = xf1.Position;
	        Vector2 x2 = xf2.Position;
	        Vector2 p1 = joint.GetAnchorA();
	        Vector2 p2 = joint.GetAnchorB();

	        Color color = new Color(0.5f, 0.8f, 0.8f);

	        switch (joint.JointType)
	        {
	        case JointType.Distance:
		        DebugDraw.DrawSegment(p1, p2, color);
		        break;

	        case JointType.Pulley:
		        {
			        PulleyJoint pulley = (PulleyJoint)joint;
			        Vector2 s1 = pulley.GetGroundAnchorA();
			        Vector2 s2 = pulley.GetGroundAnchorB();
			        DebugDraw.DrawSegment(s1, p1, color);
			        DebugDraw.DrawSegment(s2, p2, color);
			        DebugDraw.DrawSegment(s1, s2, color);
		        }
		        break;

	        case JointType.Mouse:
		        // don't draw this
		        break;

	        default:
		        DebugDraw.DrawSegment(x1, p1, color);
		        DebugDraw.DrawSegment(p1, p2, color);
		        DebugDraw.DrawSegment(x2, p2, color);
                break;
	        }
        }

	    void DrawShape(Fixture fixture, Transform xf, Color color)
        {
	        switch (fixture.ShapeType)
	        {
	        case ShapeType.Circle:
		        {
			        CircleShape circle = (CircleShape)fixture.GetShape();

			        Vector2 center = MathUtils.Multiply(ref xf, circle._p);
			        float radius = circle._radius;
			        Vector2 axis = xf.R.col1;

			        DebugDraw.DrawSolidCircle(center, radius, axis, color);
		        }
		        break;

	        case ShapeType.Polygon:
		        {
			        PolygonShape poly = (PolygonShape)fixture.GetShape();
			        int vertexCount = poly._vertexCount;
			        Debug.Assert(vertexCount <= Settings.b2_maxPolygonVertices);
			        FixedArray8<Vector2> vertices = new FixedArray8<Vector2>();

			        for (int i = 0; i < vertexCount; ++i)
			        {
				        vertices[i] = MathUtils.Multiply(ref xf, poly._vertices[i]);
			        }

			        DebugDraw.DrawSolidPolygon(ref vertices, vertexCount, color);
		        }
		        break;

            case ShapeType.Edge:
		        {
			        EdgeShape edge = (EdgeShape)fixture.GetShape();
			        Vector2 v1 = MathUtils.Multiply(ref xf, edge._vertex1);
			        Vector2 v2 = MathUtils.Multiply(ref xf, edge._vertex2);
			        DebugDraw.DrawSegment(v1, v2, color);
		        }
		        break;

	        case ShapeType.Loop:
		        {
			        LoopShape loop = (LoopShape)fixture.GetShape();
			        int count = loop._count;

			        Vector2 v1 = MathUtils.Multiply(ref xf, loop._vertices[count - 1]);
			        for (int i = 0; i < count; ++i)
			        {
				        Vector2 v2 = MathUtils.Multiply(ref xf, loop._vertices[i]);
				        DebugDraw.DrawSegment(v1, v2, color);
				        v1 = v2;
			        }
		        }
		        break;
	        }
        }

        TOISolver _toiSolver = new TOISolver();
        Contact[] _toiContacts = new Contact[Settings.b2_maxTOIContacts];
        internal Island _island = new Island();
        internal WorldFlags _flags;

	    internal ContactManager _contactManager = new ContactManager();
        internal Queue<Contact> _contactPool = new Queue<Contact>(256);

        internal Body _bodyList;
        internal Joint _jointList;

        internal int _bodyCount;
        internal int _jointCount;

        internal bool _allowSleep;

        internal Body _groundBody;

	    // This is used to compute the time step ratio to
	    // support a variable time step.
        internal float _inv_dt0;

        Body[] stack = new Body[64];
    }
}