/*
* 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.
*/

package Box2D.Dynamics{

import Box2D.Common.Math.*;
import Box2D.Common.*;
import Box2D.Collision.*;
import Box2D.Collision.Shapes.*;
import Box2D.Dynamics.*;
import Box2D.Dynamics.Contacts.*;
import Box2D.Dynamics.Joints.*;


public class b2World
{
	
	// Construct a world object.
	/// @param worldAABB a bounding box that completely encompasses all your shapes.
	/// @param gravity the world gravity vector.
	/// @param doSleep improve performance by not simulating inactive bodies.
	public function b2World(worldAABB:b2AABB, gravity:b2Vec2, doSleep:Boolean){
		
		m_destructionListener = null;
		m_boundaryListener = null;
		m_contactFilter = b2ContactFilter.b2_defaultFilter;
		m_contactListener = null;
		m_debugDraw = null;
		
		m_bodyList = null;
		m_contactList = null;
		m_jointList = null;
		
		m_bodyCount = 0;
		m_contactCount = 0;
		m_jointCount = 0;
		
		m_positionCorrection = true;
		m_warmStarting = true;
		m_continuousPhysics = true;
		
		m_allowSleep = doSleep;
		m_gravity = gravity;
		
		m_lock = false;
		
		m_inv_dt0 = 0.0;
		
		m_contactManager.m_world = this;
		//void* mem = b2Alloc(sizeof(b2BroadPhase));
		m_broadPhase = new b2BroadPhase(worldAABB, m_contactManager);
		
		var bd:b2BodyDef = new b2BodyDef();
		m_groundBody = CreateBody(bd);
		
		var b2Dis:b2Distance		=new b2Distance();
		b2Distance.InitializeRegisters();
	}

	/// Destruct the world. All physics entities are destroyed and all heap memory is released.
	//~b2World();

	/// Register a destruction listener.
	public function SetDestructionListener(listener:b2DestructionListener) : void{
		m_destructionListener = listener;
	}

	/// Register a broad-phase boundary listener.
	public function SetBoundaryListener(listener:b2BoundaryListener) : void{
		m_boundaryListener = listener;
	}

	/// Register a contact filter to provide specific control over collision.
	/// Otherwise the default filter is used (b2_defaultFilter).
	public function SetContactFilter(filter:b2ContactFilter) : void{
		m_contactFilter = filter;
	}

	/// Register a contact event listener
	public function SetContactListener(listener:b2ContactListener) : void{
		m_contactListener = listener;
	}

	/// Register a routine for debug drawing. The debug draw functions are called
	/// inside the b2World::Step method, so make sure your renderer is ready to
	/// consume draw commands when you call Step().
	public function SetDebugDraw(debugDraw:b2DebugDraw) : void{
		m_debugDraw = debugDraw;
	}
	
	/// Perform validation of internal data structures.
	public function Validate() : void
	{
		m_broadPhase.Validate();
	}
	
	/// Get the number of broad-phase proxies.
	public function GetProxyCount() : int
	{
		return m_broadPhase.m_proxyCount;
	}
	
	/// Get the number of broad-phase pairs.
	public function GetPairCount() : int
	{
		return m_broadPhase.m_pairManager.m_pairCount;
	}
	
	/// Create a rigid body given a definition. No reference to the definition
	/// is retained.
	/// @warning This function is locked during callbacks.
	public function CreateBody(def:b2BodyDef) : b2Body{
		
		//b2Settings.b2Assert(m_lock == false);
		if (m_lock == true)
		{
			return null;
		}
		
		//void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
		var b:b2Body = new b2Body(def, this);
		
		// Add to world doubly linked list.
		b.m_prev = null;
		b.m_next = m_bodyList;
		if (m_bodyList)
		{
			m_bodyList.m_prev = b;
		}
		m_bodyList = b;
		++m_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 function DestroyBody(b:b2Body) : void{
		
		//b2Settings.b2Assert(m_bodyCount > 0);
		//b2Settings.b2Assert(m_lock == false);
		if (m_lock == true)
		{
			return;
		}
		
		// Delete the attached joints.
		var jn:b2JointEdge = b.m_jointList;
		while (jn)
		{
			var jn0:b2JointEdge = jn;
			jn = jn.next;
			
			if (m_destructionListener)
			{
				m_destructionListener.SayGoodbyeJoint(jn0.joint);
			}
			
			DestroyJoint(jn0.joint);
		}
		
		// Delete the attached shapes. This destroys broad-phase
		// proxies and pairs, leading to the destruction of contacts.
		var s:b2Shape = b.m_shapeList;
		while (s)
		{
			var s0:b2Shape = s;
			s = s.m_next;
			
			if (m_destructionListener)
			{
				m_destructionListener.SayGoodbyeShape(s0);
			}
			
			s0.DestroyProxy(m_broadPhase);
			b2Shape.Destroy(s0, m_blockAllocator);
		}
		
		// Remove world body list.
		if (b.m_prev)
		{
			b.m_prev.m_next = b.m_next;
		}
		
		if (b.m_next)
		{
			b.m_next.m_prev = b.m_prev;
		}
		
		if (b == m_bodyList)
		{
			m_bodyList = b.m_next;
		}
		
		--m_bodyCount;
		//b->~b2Body();
		//m_blockAllocator.Free(b, sizeof(b2Body));
		
	}

	/// Create a joint to constrain 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 function CreateJoint(def:b2JointDef) : b2Joint{
		
		//b2Settings.b2Assert(m_lock == false);
		
		var j:b2Joint = b2Joint.Create(def, m_blockAllocator);
		
		// Connect to the world list.
		j.m_prev = null;
		j.m_next = m_jointList;
		if (m_jointList)
		{
			m_jointList.m_prev = j;
		}
		m_jointList = j;
		++m_jointCount;
		
		// Connect to the bodies' doubly linked lists.
		j.m_node1.joint = j;
		j.m_node1.other = j.m_body2;
		j.m_node1.prev = null;
		j.m_node1.next = j.m_body1.m_jointList;
		if (j.m_body1.m_jointList) j.m_body1.m_jointList.prev = j.m_node1;
		j.m_body1.m_jointList = j.m_node1;
		
		j.m_node2.joint = j;
		j.m_node2.other = j.m_body1;
		j.m_node2.prev = null;
		j.m_node2.next = j.m_body2.m_jointList;
		if (j.m_body2.m_jointList) j.m_body2.m_jointList.prev = j.m_node2;
		j.m_body2.m_jointList = j.m_node2;
		
		// If the joint prevents collisions, then reset collision filtering.
		if (def.collideConnected == false)
		{
			// Reset the proxies on the body with the minimum number of shapes.
			var b:b2Body = def.body1.m_shapeCount < def.body2.m_shapeCount ? def.body1 : def.body2;
			for (var s:b2Shape = b.m_shapeList; s; s = s.m_next)
			{
				s.RefilterProxy(m_broadPhase, b.m_xf);
			}
		}
		
		return j;
		
	}

	/// Destroy a joint. This may cause the connected bodies to begin colliding.
	/// @warning This function is locked during callbacks.
	public function DestroyJoint(j:b2Joint) : void{
		
		//b2Settings.b2Assert(m_lock == false);
		
		var collideConnected:Boolean = j.m_collideConnected;
		
		// Remove from the doubly linked list.
		if (j.m_prev)
		{
			j.m_prev.m_next = j.m_next;
		}
		
		if (j.m_next)
		{
			j.m_next.m_prev = j.m_prev;
		}
		
		if (j == m_jointList)
		{
			m_jointList = j.m_next;
		}
		
		// Disconnect from island graph.
		var body1:b2Body = j.m_body1;
		var body2:b2Body = j.m_body2;
		
		// Wake up connected bodies.
		body1.WakeUp();
		body2.WakeUp();
		
		// Remove from body 1.
		if (j.m_node1.prev)
		{
			j.m_node1.prev.next = j.m_node1.next;
		}
		
		if (j.m_node1.next)
		{
			j.m_node1.next.prev = j.m_node1.prev;
		}
		
		if (j.m_node1 == body1.m_jointList)
		{
			body1.m_jointList = j.m_node1.next;
		}
		
		j.m_node1.prev = null;
		j.m_node1.next = null;
		
		// Remove from body 2
		if (j.m_node2.prev)
		{
			j.m_node2.prev.next = j.m_node2.next;
		}
		
		if (j.m_node2.next)
		{
			j.m_node2.next.prev = j.m_node2.prev;
		}
		
		if (j.m_node2 == body2.m_jointList)
		{
			body2.m_jointList = j.m_node2.next;
		}
		
		j.m_node2.prev = null;
		j.m_node2.next = null;
		
		b2Joint.Destroy(j, m_blockAllocator);
		
		//b2Settings.b2Assert(m_jointCount > 0);
		--m_jointCount;
		
		// If the joint prevents collisions, then reset collision filtering.
		if (collideConnected == false)
		{
			// Reset the proxies on the body with the minimum number of shapes.
			var b:b2Body = body1.m_shapeCount < body2.m_shapeCount ? body1 : body2;
			for (var s:b2Shape = b.m_shapeList; s; s = s.m_next)
			{
				s.RefilterProxy(m_broadPhase, b.m_xf);
			}
		}
		
	}
	
	public function CreateGroundShape(def:b2ShapeDef) : * {
		//b2Settings.b2Assert(m_lock == false);
		if (m_lock == true)
		{
			return null;
		}
		
		switch (def.type)
		{
		case b2Shape.e_staticEdgeShape:
			return new b2StaticEdgeChain(def, this);
		
		default:
			return m_groundBody.CreateShape(def);
		}
	}

	/// Re-filter a shape. This re-runs contact filtering on a shape.
	public function Refilter(shape:b2Shape) : void
	{
		shape.RefilterProxy(m_broadPhase, shape.m_body.m_xf);
	}
	
	/// Enable/disable warm starting. For testing.
	public function SetWarmStarting(flag: Boolean) : void { m_warmStarting = flag; }

	/// Enable/disable position correction. For testing.
	public function SetPositionCorrection(flag: Boolean) : void { m_positionCorrection = flag; }

	/// Enable/disable continuous physics. For testing.
	public function SetContinuousPhysics(flag: Boolean) : void { m_continuousPhysics = flag; }
	
	/// Get the number of bodies.
	public function GetBodyCount() : int
	{
		return m_bodyCount;
	}
	
	/// Get the number joints.
	public function GetJointCount() : int
	{
		return m_jointCount;
	}
	
	/// Get the number of contacts (each may have 0 or more contact points).
	public function GetContactCount() : int
	{
		return m_contactCount;
	}
	
	/// Change the global gravity vector.
	public function SetGravity(gravity: b2Vec2): void
	{
		m_gravity = gravity;
	}
	
	/// 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.
	public function GetGroundBody() : b2Body{
		return m_groundBody;
	}

	/// Take a time step. This performs collision detection, integration,
	/// and constraint solution.
	/// @param timeStep the amount of time to simulate, this should not vary.
	/// @param iterations the number of iterations to be used by the constraint solver.
	public function Step(dt:Number, iterations:int) : void{
		
		m_lock = true;
		
		var step:b2TimeStep = new b2TimeStep();
		step.dt = dt;
		step.maxIterations	= iterations;
		if (dt > 0.0)
		{
			step.inv_dt = 1.0 / dt;
		}
		else
		{
			step.inv_dt = 0.0;
		}
		
		step.dtRatio = m_inv_dt0 * dt;
		
		step.positionCorrection = m_positionCorrection;
		step.warmStarting = m_warmStarting;
		
		// Update contacts.
		m_contactManager.Collide();
		
		// Integrate velocities, solve velocity constraints, and integrate positions.
		if (step.dt > 0.0)
		{
			Solve(step);
		}
		
		// Handle TOI events.
		if (m_continuousPhysics && step.dt > 0.0)
		{
			SolveTOI(step);
		}
		
		// Draw debug information.
		DrawDebugData();
		
		m_inv_dt0 = step.inv_dt;
		m_lock = false;
	}

	/// Query the world for all shapes that potentially overlap the
	/// provided AABB. You provide a shape pointer buffer of specified
	/// size. The number of shapes found is returned.
	/// @param aabb the query box.
	/// @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 function Query(aabb:b2AABB, shapes:Array, maxCount:int) : int{
		
		//void** results = (void**)m_stackAllocator.Allocate(maxCount * sizeof(void*));
		var results:Array = new Array(maxCount);
		
		var count:int = m_broadPhase.QueryAABB(aabb, results, maxCount);
		
		for (var i:int = 0; i < count; ++i)
		{
			shapes[i] = results[i];
		}
		
		//m_stackAllocator.Free(results);
		return count;
		
	}

	/// Get the world body list. With the returned body, use b2Body::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 function GetBodyList() : b2Body{
		return m_bodyList;
	}

	/// Get the world joint list. With the returned joint, use b2Joint::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 function GetJointList() : b2Joint{
		return m_jointList;
	}


	//--------------- Internals Below -------------------
	// Internal yet public to make life easier.

	// Find islands, integrate and solve constraints, solve position constraints
	public function Solve(step:b2TimeStep) : void{
		
		var b:b2Body;
		
		m_positionIterationCount = 0;
		
		// Size the island for the worst case.
		var island:b2Island = new b2Island(m_bodyCount, m_contactCount, m_jointCount, m_stackAllocator, m_contactListener);
		
		// Clear all the island flags.
		for (b = m_bodyList; b; b = b.m_next)
		{
			b.m_flags &= ~b2Body.e_islandFlag;
		}
		for (var c:b2Contact = m_contactList; c; c = c.m_next)
		{
			c.m_flags &= ~b2Contact.e_islandFlag;
		}
		for (var j:b2Joint = m_jointList; j; j = j.m_next)
		{
			j.m_islandFlag = false;
		}
		
		// Build and simulate all awake islands.
		var stackSize:int = m_bodyCount;
		//b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
		var stack:Array = new Array(stackSize);
		for (var seed:b2Body = m_bodyList; seed; seed = seed.m_next)
		{
			if (seed.m_flags & (b2Body.e_islandFlag | b2Body.e_sleepFlag | b2Body.e_frozenFlag))
			{
				continue;
			}
			
			if (seed.IsStatic())
			{
				continue;
			}
			
			// Reset island and stack.
			island.Clear();
			var stackCount:int = 0;
			stack[stackCount++] = seed;
			seed.m_flags |= b2Body.e_islandFlag;
			
			// Perform a depth first search (DFS) on the constraint graph.
			while (stackCount > 0)
			{
				// Grab the next body off the stack and add it to the island.
				b = stack[--stackCount];
				island.AddBody(b);
				
				// Make sure the body is awake.
				b.m_flags &= ~b2Body.e_sleepFlag;
				
				// To keep islands as small as possible, we don't
				// propagate islands across static bodies.
				if (b.IsStatic())
				{
					continue;
				}
				
				var other:b2Body;
				// Search all contacts connected to this body.
				for (var cn:b2ContactEdge = b.m_contactList; cn; cn = cn.next)
				{
					// Has this contact already been added to an island?
					if (cn.contact.m_flags & (b2Contact.e_islandFlag | b2Contact.e_nonSolidFlag))
					{
						continue;
					}
					
					// Is this contact touching?
					if (cn.contact.m_manifoldCount == 0)
					{
						continue;
					}
					
					island.AddContact(cn.contact);
					cn.contact.m_flags |= b2Contact.e_islandFlag;
					
					//var other:b2Body = cn.other;
					other = cn.other;
					
					// Was the other body already added to this island?
					if (other.m_flags & b2Body.e_islandFlag)
					{
						continue;
					}
					
					//b2Settings.b2Assert(stackCount < stackSize);
					stack[stackCount++] = other;
					other.m_flags |= b2Body.e_islandFlag;
				}
				
				// Search all joints connect to this body.
				for (var jn:b2JointEdge = b.m_jointList; jn; jn = jn.next)
				{
					if (jn.joint.m_islandFlag == true)
					{
						continue;
					}
					
					island.AddJoint(jn.joint);
					jn.joint.m_islandFlag = true;
					
					//var other:b2Body = jn.other;
					other = jn.other;
					if (other.m_flags & b2Body.e_islandFlag)
					{
						continue;
					}
					
					//b2Settings.b2Assert(stackCount < stackSize);
					stack[stackCount++] = other;
					other.m_flags |= b2Body.e_islandFlag;
				}
			}
			
			island.Solve(step, m_gravity, m_positionCorrection, m_allowSleep);
			//m_positionIterationCount = Math.max(m_positionIterationCount, island.m_positionIterationCount);
			if (island.m_positionIterationCount > m_positionIterationCount) {
				m_positionIterationCount = island.m_positionIterationCount;
			}
			
			// Post solve cleanup.
			for (var i:int = 0; i < island.m_bodyCount; ++i)
			{
				// Allow static bodies to participate in other islands.
				b = island.m_bodies[i];
				if (b.IsStatic())
				{
					b.m_flags &= ~b2Body.e_islandFlag;
				}
			}
		}
		
		//m_stackAllocator.Free(stack);
		
		// Synchronize shapes, check for out of range bodies.
		for (b = m_bodyList; b; b = b.m_next)
		{
			if (b.m_flags & (b2Body.e_sleepFlag | b2Body.e_frozenFlag))
			{
				continue;
			}
			
			if (b.IsStatic())
			{
				continue;
			}
			
			// 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
			var inRange:Boolean = b.SynchronizeShapes();
			
			// Did the body's shapes leave the world?
			if (inRange == false && m_boundaryListener != null)
			{
				m_boundaryListener.Violation(b);
			}
		}
		
		// Commit shape proxy movements to the broad-phase so that new contacts are created.
		// Also, some contacts can be destroyed.
		m_broadPhase.Commit();
		
	}
	
	// Find TOI contacts and solve them.
	public function SolveTOI(step:b2TimeStep) : void{
		
		var b:b2Body;
		var s1:b2Shape;
		var s2:b2Shape;
		var b1:b2Body;
		var b2:b2Body;
		var cn:b2ContactEdge;
		
		// Reserve an island and a stack for TOI island solution.
		var island:b2Island = new b2Island(m_bodyCount, b2Settings.b2_maxTOIContactsPerIsland, 0, m_stackAllocator, m_contactListener);
		var stackSize:int = m_bodyCount;
		
		//b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
		var stack:Array = new Array(stackSize);
		
		for (b = m_bodyList; b; b = b.m_next)
		{
			b.m_flags &= ~b2Body.e_islandFlag;
			b.m_sweep.t0 = 0.0;
		}
		
		var c:b2Contact;
		for (c = m_contactList; c; c = c.m_next)
		{
			// Invalidate TOI
			c.m_flags &= ~(b2Contact.e_toiFlag | b2Contact.e_islandFlag);
		}
		
		// Find TOI events and solve them.
		for (;;)
		{
			// Find the first TOI.
			var minContact:b2Contact = null;
			var minTOI:Number = 1.0;
			
			for (c = m_contactList; c; c = c.m_next)
			{
				if (c.m_flags & (b2Contact.e_slowFlag | b2Contact.e_nonSolidFlag))
				{
					continue;
				}
				
				// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
				
				var toi:Number = 1.0;
				if (c.m_flags & b2Contact.e_toiFlag)
				{
					// This contact has a valid cached TOI.
					toi = c.m_toi;
				}
				else
				{
					// Compute the TOI for this contact.
					s1 = c.m_shape1;
					s2 = c.m_shape2;
					b1 = s1.m_body;
					b2 = s2.m_body;
					
					if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping()))
					{
						continue;
					}
					
					// Put the sweeps onto the same time interval.
					var t0:Number = b1.m_sweep.t0;
					
					if (b1.m_sweep.t0 < b2.m_sweep.t0)
					{
						t0 = b2.m_sweep.t0;
						b1.m_sweep.Advance(t0);
					}
					else if (b2.m_sweep.t0 < b1.m_sweep.t0)
					{
						t0 = b1.m_sweep.t0;
						b2.m_sweep.Advance(t0);
					}
					
					//b2Settings.b2Assert(t0 < 1.0f);
					
					// Compute the time of impact.
					toi = b2TimeOfImpact.TimeOfImpact(c.m_shape1, b1.m_sweep, c.m_shape2, b2.m_sweep);
					//b2Settings.b2Assert(0.0 <= toi && toi <= 1.0);
					
					if (toi > 0.0 && toi < 1.0)
					{
						//toi = Math.min((1.0 - toi) * t0 + toi, 1.0);
						toi = (1.0 - toi) * t0 + toi;
						if (toi > 1) toi = 1;
					}
					
					
					c.m_toi = toi;
					c.m_flags |= b2Contact.e_toiFlag;
				}
				
				if (Number.MIN_VALUE < toi && toi < minTOI)
				{
					// This is the minimum TOI found so far.
					minContact = c;
					minTOI = toi;
				}
			}
			
			if (minContact == null || 1.0 - 100.0 * Number.MIN_VALUE < minTOI)
			{
				// No more TOI events. Done!
				break;
			}
			
			// Advance the bodies to the TOI.
			s1 = minContact.m_shape1;
			s2 = minContact.m_shape2;
			b1 = s1.m_body;
			b2 = s2.m_body;
			b1.Advance(minTOI);
			b2.Advance(minTOI);
			
			// The TOI contact likely has some new contact points.
			minContact.Update(m_contactListener);
			minContact.m_flags &= ~b2Contact.e_toiFlag;
			
			if (minContact.m_manifoldCount == 0)
			{
				// This shouldn't happen. Numerical error?
				//b2Assert(false);
				continue;
			}
			
			// Build the TOI island. We need a dynamic seed.
			var seed:b2Body = b1;
			if (seed.IsStatic())
			{
				seed = b2;
			}
			
			// Reset island and stack.
			island.Clear();
			var stackCount:int = 0;
			stack[stackCount++] = seed;
			seed.m_flags |= b2Body.e_islandFlag;
			
			// Perform a depth first search (DFS) on the contact graph.
			while (stackCount > 0)
			{
				// Grab the next body off the stack and add it to the island.
				b = stack[--stackCount];
				island.AddBody(b);
				
				// Make sure the body is awake.
				b.m_flags &= ~b2Body.e_sleepFlag;
				
				// To keep islands as small as possible, we don't
				// propagate islands across static bodies.
				if (b.IsStatic())
				{
					continue;
				}
				
				// Search all contacts connected to this body.
				for (cn = b.m_contactList; cn; cn = cn.next)
				{
					// Does the TOI island still have space for contacts?
					if (island.m_contactCount == island.m_contactCapacity)
					{
						continue;
					}
					
					// Has this contact already been added to an island? Skip slow or non-solid contacts.
					if (cn.contact.m_flags & (b2Contact.e_islandFlag | b2Contact.e_slowFlag | b2Contact.e_nonSolidFlag))
					{
						continue;
					}
					
					// Is this contact touching? For performance we are not updating this contact.
					if (cn.contact.m_manifoldCount == 0)
					{
						continue;
					}
					
					island.AddContact(cn.contact);
					cn.contact.m_flags |= b2Contact.e_islandFlag;
					
					// Update other body.
					var other:b2Body = cn.other;
					
					// Was the other body already added to this island?
					if (other.m_flags & b2Body.e_islandFlag)
					{
						continue;
					}
					
					// March forward, this can do no harm since this is the min TOI.
					if (other.IsStatic() == false)
					{
						other.Advance(minTOI);
						other.WakeUp();
					}
					
					//b2Settings.b2Assert(stackCount < stackSize);
					stack[stackCount++] = other;
					other.m_flags |= b2Body.e_islandFlag;
				}
			}
			
			var subStep:b2TimeStep = new b2TimeStep();
			subStep.dt = (1.0 - minTOI) * step.dt;
			//b2Settings.b2Assert(subStep.dt > Number.MIN_VALUE);
			subStep.inv_dt = 1.0 / subStep.dt;
			subStep.maxIterations = step.maxIterations;
			
			island.SolveTOI(subStep);
			
			var i:int;
			// Post solve cleanup.
			for (i = 0; i < island.m_bodyCount; ++i)
			{
				// Allow bodies to participate in future TOI islands.
				b = island.m_bodies[i];
				b.m_flags &= ~b2Body.e_islandFlag;
				
				if (b.m_flags & (b2Body.e_sleepFlag | b2Body.e_frozenFlag))
				{
					continue;
				}
				
				if (b.IsStatic())
				{
					continue;
				}
				
				// 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
				var inRange:Boolean = b.SynchronizeShapes();
				
				// Did the body's shapes leave the world?
				if (inRange == false && m_boundaryListener != null)
				{
					m_boundaryListener.Violation(b);
				}
				
				// Invalidate all contact TOIs associated with this body. Some of these
				// may not be in the island because they were not touching.
				for (cn = b.m_contactList; cn; cn = cn.next)
				{
					cn.contact.m_flags &= ~b2Contact.e_toiFlag;
				}
			}
			
			for (i = 0; i < island.m_contactCount; ++i)
			{
				// Allow contacts to participate in future TOI islands.
				c = island.m_contacts[i];
				c.m_flags &= ~(b2Contact.e_toiFlag | b2Contact.e_islandFlag);
			}
			
			// Commit shape proxy movements to the broad-phase so that new contacts are created.
			// Also, some contacts can be destroyed.
			m_broadPhase.Commit();
		}
		
		//m_stackAllocator.Free(stack);
		
	}
	
	static private var s_jointColor:b2Color = new b2Color(0.5, 0.8, 0.8);
	//
	public function DrawJoint(joint:b2Joint) : void{
		
		var b1:b2Body = joint.m_body1;
		var b2:b2Body = joint.m_body2;
		var xf1:b2XForm = b1.m_xf;
		var xf2:b2XForm = b2.m_xf;
		var x1:b2Vec2 = xf1.position;
		var x2:b2Vec2 = xf2.position;
		var p1:b2Vec2 = joint.GetAnchor1();
		var p2:b2Vec2 = joint.GetAnchor2();
		
		//b2Color color(0.5f, 0.8f, 0.8f);
		var color:b2Color = s_jointColor;
		
		switch (joint.m_type)
		{
		case b2Joint.e_distanceJoint:
			m_debugDraw.DrawSegment(p1, p2, color);
			break;
		
		case b2Joint.e_pulleyJoint:
			{
				var pulley:b2PulleyJoint = (joint as b2PulleyJoint);
				var s1:b2Vec2 = pulley.GetGroundAnchor1();
				var s2:b2Vec2 = pulley.GetGroundAnchor2();
				m_debugDraw.DrawSegment(s1, p1, color);
				m_debugDraw.DrawSegment(s2, p2, color);
				m_debugDraw.DrawSegment(s1, s2, color);
			}
			break;
		
		case b2Joint.e_mouseJoint:
			m_debugDraw.DrawSegment(p1, p2, color);
			break;
		
		default:
			if (b1 != m_groundBody)
				m_debugDraw.DrawSegment(x1, p1, color);
			m_debugDraw.DrawSegment(p1, p2, color);
			if (b2 != m_groundBody)
				m_debugDraw.DrawSegment(x2, p2, color);
		}
	}
	
	static private var s_coreColor:b2Color = new b2Color(0.9, 0.6, 0.6);
	public function DrawShape(shape:b2Shape, xf:b2XForm, color:b2Color, core:Boolean) : void{
		
		var coreColor:b2Color = s_coreColor;
		
		switch (shape.m_type)
		{
		case b2Shape.e_circleShape:
			{
				var circle:b2CircleShape = (shape as b2CircleShape);
				
				var center:b2Vec2 = b2Math.b2MulX(xf, circle.m_localPosition);
				var radius:Number = circle.m_radius;
				var axis:b2Vec2 = xf.R.col1;
				
				m_debugDraw.DrawSolidCircle(center, radius, axis, color);
				
				if (core)
				{
					m_debugDraw.DrawCircle(center, radius - b2Settings.b2_toiSlop, coreColor);
				}
			}
			break;
		
		case b2Shape.e_polygonShape:
			{
				var i:int;
				var poly:b2PolygonShape = (shape as b2PolygonShape);
				var vertexCount:int = poly.GetVertexCount();
				var localVertices:Array = poly.GetVertices();
				
				//b2Assert(vertexCount <= b2_maxPolygonVertices);
				var vertices:Array = new Array(b2Settings.b2_maxPolygonVertices);
				
				for (i = 0; i < vertexCount; ++i)
				{
					vertices[i] = b2Math.b2MulX(xf, localVertices[i]);
				}
				
				m_debugDraw.DrawSolidPolygon(vertices, vertexCount, color);
				
				if (core)
				{
					var localCoreVertices:Array = poly.GetCoreVertices();
					for (i = 0; i < vertexCount; ++i)
					{
						vertices[i] = b2Math.b2MulX(xf, localCoreVertices[i]);
					}
					m_debugDraw.DrawPolygon(vertices, vertexCount, coreColor);
				}
			}
			break;
		
		case b2Shape.e_concaveArcShape:
			{
				var arc:b2ConcaveArcShape = (shape as b2ConcaveArcShape);
				vertexCount = arc.GetVertexCount();
				localVertices = arc.GetVertices();
				
				center = b2Math.b2MulX(xf, arc.m_arcCenter);
				
				//b2Assert(vertexCount <= b2_maxPolygonVertices);
				vertices = new Array(b2Settings.b2_maxPolygonVertices);
				
				for (i = 0; i < vertexCount; ++i)
				{
					vertices[i] = b2Math.b2MulX(xf, localVertices[i]);
				}
				
				m_debugDraw.DrawSolidConcaveArc(vertices, vertexCount, center, color);
				
				if (core)
				{
					localCoreVertices = arc.GetCoreVertices();
					for (i = 0; i < vertexCount; ++i)
					{
						vertices[i] = b2Math.b2MulX(xf, localCoreVertices[i]);
					}
					m_debugDraw.DrawConcaveArc(vertices, vertexCount, center, coreColor);
				}
			}
			break;
			
		case b2Shape.e_staticEdgeShape:
			{
				var edge:b2StaticEdgeShape = (shape as b2StaticEdgeShape);
				
				m_debugDraw.DrawSegment(edge.m_v1, edge.m_v2, color);
				
				if (core)
				{
					m_debugDraw.DrawSegment(edge.m_coreV1, edge.m_coreV2, coreColor);
				}
			}
			break;
		
		}
	}
	
	
	static private var s_xf:b2XForm = new b2XForm();
	public function DrawDebugData() : void{
		
		if (m_debugDraw == null)
		{
			return;
		}
		
		m_debugDraw.m_sprite.graphics.clear();
		
		var flags:uint = m_debugDraw.GetFlags();
		
		var i:int;
		var b:b2Body;
		var s:b2Shape;
		var j:b2Joint;
		var bp:b2BroadPhase;
		var invQ:b2Vec2 = new b2Vec2;
		var x1:b2Vec2 = new b2Vec2;
		var x2:b2Vec2 = new b2Vec2;
		var color:b2Color = new b2Color(0,0,0);
		var xf:b2XForm;
		var b1:b2AABB = new b2AABB();
		var b2:b2AABB = new b2AABB();
		var vs:Array = [new b2Vec2(), new b2Vec2(), new b2Vec2(), new b2Vec2()];
		
		if (flags & b2DebugDraw.e_shapeBit)
		{
			var core:Boolean = (flags & b2DebugDraw.e_coreShapeBit) == b2DebugDraw.e_coreShapeBit;
			
			for (b = m_bodyList; b; b = b.m_next)
			{
				xf = b.m_xf;
				for (s = b.GetShapeList(); s; s = s.m_next)
				{
					if (b.IsStatic())
					{
						DrawShape(s, xf, new b2Color(0.5, 0.9, 0.5), core);
					}
					else if (b.IsSleeping())
					{
						DrawShape(s, xf, new b2Color(0.5, 0.5, 0.9), core);
					}
					else
					{
						DrawShape(s, xf, new b2Color(0.9, 0.9, 0.9), core);
					}
				}
			}
		}
		
		if (flags & b2DebugDraw.e_jointBit)
		{
			for (j = m_jointList; j; j = j.m_next)
			{
				//if (j.m_type != b2Joint.e_mouseJoint)
				//{
					DrawJoint(j);
				//}
			}
		}
		
		if (flags & b2DebugDraw.e_pairBit)
		{
			bp = m_broadPhase;
			//b2Vec2 invQ;
			invQ.Set(1.0 / bp.m_quantizationFactor.x, 1.0 / bp.m_quantizationFactor.y);
			//b2Color color(0.9f, 0.9f, 0.3f);
			color.Set(0.9, 0.9, 0.3);
			
			for (i = 0; i < b2Pair.b2_tableCapacity; ++i)
			{
				var index:uint = bp.m_pairManager.m_hashTable[i];
				while (index != b2Pair.b2_nullPair)
				{
					var pair:b2Pair = bp.m_pairManager.m_pairs[ index ];
					var p1:b2Proxy = bp.m_proxyPool[ pair.proxyId1 ];
					var p2:b2Proxy = bp.m_proxyPool[ pair.proxyId2 ];
					
					//b2AABB b1, b2;
					b1.lowerBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p1.lowerBounds[0]].value;
					b1.lowerBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p1.lowerBounds[1]].value;
					b1.upperBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p1.upperBounds[0]].value;
					b1.upperBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p1.upperBounds[1]].value;
					b2.lowerBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p2.lowerBounds[0]].value;
					b2.lowerBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p2.lowerBounds[1]].value;
					b2.upperBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p2.upperBounds[0]].value;
					b2.upperBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p2.upperBounds[1]].value;
					
					//b2Vec2 x1 = 0.5f * (b1.lowerBound + b1.upperBound);
					x1.x = 0.5 * (b1.lowerBound.x + b1.upperBound.x);
					x1.y = 0.5 * (b1.lowerBound.y + b1.upperBound.y);
					//b2Vec2 x2 = 0.5f * (b2.lowerBound + b2.upperBound);
					x2.x = 0.5 * (b2.lowerBound.x + b2.upperBound.x);
					x2.y = 0.5 * (b2.lowerBound.y + b2.upperBound.y);
					
					m_debugDraw.DrawSegment(x1, x2, color);
					
					index = pair.next;
				}
			}
		}
		
		if (flags & b2DebugDraw.e_aabbBit)
		{
			bp = m_broadPhase;
			var worldLower:b2Vec2 = bp.m_worldAABB.lowerBound;
			var worldUpper:b2Vec2 = bp.m_worldAABB.upperBound;
			
			//b2Vec2 invQ;
			invQ.Set(1.0 / bp.m_quantizationFactor.x, 1.0 / bp.m_quantizationFactor.y);
			//b2Color color(0.9f, 0.3f, 0.9f);
			color.Set(0.9, 0.3, 0.9);
			for (i = 0; i < b2Settings.b2_maxProxies; ++i)
			{
				var p:b2Proxy = bp.m_proxyPool[ i];
				if (p.IsValid() == false)
				{
					continue;
				}
				
				//b2AABB b1;
				b1.lowerBound.x = worldLower.x + invQ.x * bp.m_bounds[0][p.lowerBounds[0]].value;
				b1.lowerBound.y = worldLower.y + invQ.y * bp.m_bounds[1][p.lowerBounds[1]].value;
				b1.upperBound.x = worldLower.x + invQ.x * bp.m_bounds[0][p.upperBounds[0]].value;
				b1.upperBound.y = worldLower.y + invQ.y * bp.m_bounds[1][p.upperBounds[1]].value;
				
				//b2Vec2 vs[4];
				vs[0].Set(b1.lowerBound.x, b1.lowerBound.y);
				vs[1].Set(b1.upperBound.x, b1.lowerBound.y);
				vs[2].Set(b1.upperBound.x, b1.upperBound.y);
				vs[3].Set(b1.lowerBound.x, b1.upperBound.y);
				
				m_debugDraw.DrawPolygon(vs, 4, color);
			}
			
			//b2Vec2 vs[4];
			vs[0].Set(worldLower.x, worldLower.y);
			vs[1].Set(worldUpper.x, worldLower.y);
			vs[2].Set(worldUpper.x, worldUpper.y);
			vs[3].Set(worldLower.x, worldUpper.y);
			m_debugDraw.DrawPolygon(vs, 4, new b2Color(0.3, 0.9, 0.9));
		}
		
		if (flags & b2DebugDraw.e_obbBit)
		{
			//b2Color color(0.5f, 0.3f, 0.5f);
			color.Set(0.5, 0.3, 0.5);
			
			for (b = m_bodyList; b; b = b.m_next)
			{
				xf = b.m_xf;
				for (s = b.GetShapeList(); s; s = s.m_next)
				{
					if (s.m_type != b2Shape.e_polygonShape)
					{
						continue;
					}
					
					var poly:b2PolygonShape = (s as b2PolygonShape);
					var obb:b2OBB = poly.GetOBB();
					var h:b2Vec2 = obb.extents;
					//b2Vec2 vs[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 (i = 0; i < 4; ++i)
					{
						//vs[i] = obb.center + b2Mul(obb.R, vs[i]);
						var tMat:b2Mat22 = obb.R;
						var tVec:b2Vec2 = vs[i];
						var tX:Number;
						tX      = obb.center.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
						vs[i].y = obb.center.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
						vs[i].x = tX;
						//vs[i] = b2Mul(xf, vs[i]);
						tMat = xf.R;
						tX      = xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
						vs[i].y = xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
						vs[i].x = tX;
					}
					
					m_debugDraw.DrawPolygon(vs, 4, color);
				}
			}
		}
		
		if (flags & b2DebugDraw.e_centerOfMassBit)
		{
			for (b = m_bodyList; b; b = b.m_next)
			{
				xf = s_xf;
				xf.R = b.m_xf.R;
				xf.position = b.GetWorldCenter();
				m_debugDraw.DrawXForm(xf);
			}
		}
	}

	public var m_blockAllocator:*;
	public var m_stackAllocator:*;

	public var m_lock:Boolean;

	public var m_broadPhase:b2BroadPhase;
	public var m_contactManager:b2ContactManager = new b2ContactManager();

	public var m_bodyList:b2Body;
	public var m_jointList:b2Joint;

	// Do not access
	public var m_contactList:b2Contact;

	public var m_bodyCount:int;
	public var m_contactCount:int;
	public var m_jointCount:int;

	public var m_gravity:b2Vec2;
	public var m_allowSleep:Boolean;

	public var m_groundBody:b2Body;

	public var m_destructionListener:b2DestructionListener;
	public var m_boundaryListener:b2BoundaryListener;
	public var m_contactFilter:b2ContactFilter;
	public var m_contactListener:b2ContactListener;
	public var m_debugDraw:b2DebugDraw;

	public var m_inv_dt0:Number;

	public var m_positionIterationCount:int;

	// This is for debugging the solver.
	static public var m_positionCorrection:Boolean;

	// This is for debugging the solver.
	static public var m_warmStarting:Boolean;

	// This is for debugging the solver.
	static public var m_continuousPhysics:Boolean;
	
};



}