View difference between Paste ID: d6fn0mLd and RqNmfEVq

 /*
 * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
 #include <Box2D/Dynamics/b2World.h>
 #include <Box2D/Dynamics/b2Body.h>
 #include <Box2D/Dynamics/b2Fixture.h>
 #include <Box2D/Dynamics/b2Island.h>
 #include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
 #include <Box2D/Dynamics/Contacts/b2Contact.h>
 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
 #include <Box2D/Collision/b2Collision.h>
 #include <Box2D/Collision/b2BroadPhase.h>
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
 #include <Box2D/Collision/Shapes/b2ChainShape.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
 #include <Box2D/Collision/b2TimeOfImpact.h>
 #include <Box2D/Common/b2Draw.h>
 #include <Box2D/Common/b2Timer.h>
 #include <new>
 
 #include <string>
 #include <stdio.h>
 
 b2World::b2World(const b2Vec2& gravity)
 {
 	m_destructionListener = NULL;
 	g_debugDraw = NULL;
 
 	m_bodyList = NULL;
 	m_jointList = NULL;
 
 	m_bodyCount = 0;
 	m_jointCount = 0;
 
 	m_warmStarting = true;
 	m_continuousPhysics = true;
 	m_subStepping = false;
 
 	m_stepComplete = true;
 
 	m_allowSleep = true;
 	m_gravity = gravity;
 
 	m_flags = e_clearForces;
 
 	m_inv_dt0 = 0.0f;
 
 	m_contactManager.m_allocator = &m_blockAllocator;
 
 	memset(&m_profile, 0, sizeof(b2Profile));
 }
 
 b2World::~b2World()
 {
 	// Some shapes allocate using b2Alloc.
 	b2Body* b = m_bodyList;
 	while (b)
 	{
 		b2Body* bNext = b->m_next;
 
 		b2Fixture* f = b->m_fixtureList;
 		while (f)
 		{
 			b2Fixture* fNext = f->m_next;
 			f->m_proxyCount = 0;
 			f->Destroy(&m_blockAllocator);
 			f = fNext;
 		}
 
 		b = bNext;
 	}
 }
 
 void b2World::SetDestructionListener(b2DestructionListener* listener)
 {
 	m_destructionListener = listener;
 }
 
 void b2World::SetContactFilter(b2ContactFilter* filter)
 {
 	m_contactManager.m_contactFilter = filter;
 }
 
 void b2World::SetContactListener(b2ContactListener* listener)
 {
 	m_contactManager.m_contactListener = listener;
 }
 
 void b2World::SetDebugDraw(b2Draw* debugDraw)
 {
 	g_debugDraw = debugDraw;
 }
 
 b2Body* b2World::CreateBody(const b2BodyDef* def)
 {
 	b2Assert(IsLocked() == false);
 	if (IsLocked())
 	{
 		return NULL;
 	}
 
 	void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
 	b2Body* b = new (mem) 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;
 }
 
 void b2World::DestroyBody(b2Body* b)
 {
 	b2Assert(m_bodyCount > 0);
 	b2Assert(IsLocked() == false);
 	if (IsLocked())
 	{
 		return;
 	}
 
 	// Delete the attached joints.
 	b2JointEdge* je = b->m_jointList;
 	while (je)
 	{
 		b2JointEdge* je0 = je;
 		je = je->next;
 
 		if (m_destructionListener)
 		{
 			m_destructionListener->SayGoodbye(je0->joint);
 		}
 
 		DestroyJoint(je0->joint);
 
 		b->m_jointList = je;
 	}
 	b->m_jointList = NULL;
 
 	// Delete the attached contacts.
 	b2ContactEdge* ce = b->m_contactList;
 	while (ce)
 	{
 		b2ContactEdge* ce0 = ce;
 		ce = ce->next;
 		m_contactManager.Destroy(ce0->contact);
 	}
 	b->m_contactList = NULL;
 
 	// Delete the attached fixtures. This destroys broad-phase proxies.
 	b2Fixture* f = b->m_fixtureList;
 	while (f)
 	{
 		b2Fixture* f0 = f;
 		f = f->m_next;
 
 		if (m_destructionListener)
 		{
 			m_destructionListener->SayGoodbye(f0);
 		}
 
 		f0->DestroyProxies(&m_contactManager.m_broadPhase);
 		f0->Destroy(&m_blockAllocator);
 		f0->~b2Fixture();
 		m_blockAllocator.Free(f0, sizeof(b2Fixture));
 
 		b->m_fixtureList = f;
 		b->m_fixtureCount -= 1;
 	}
 	b->m_fixtureList = NULL;
 	b->m_fixtureCount = 0;
 
 	// 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));
 }
 
 b2Joint* b2World::CreateJoint(const b2JointDef* def)
 {
 	b2Assert(IsLocked() == false);
 	if (IsLocked())
 	{
 		return NULL;
 	}
 
 	b2Joint* j = 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_edgeA.joint = j;
 	j->m_edgeA.other = j->m_bodyB;
 	j->m_edgeA.prev = NULL;
 	j->m_edgeA.next = j->m_bodyA->m_jointList;
 	if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA;
 	j->m_bodyA->m_jointList = &j->m_edgeA;
 
 	j->m_edgeB.joint = j;
 	j->m_edgeB.other = j->m_bodyA;
 	j->m_edgeB.prev = NULL;
 	j->m_edgeB.next = j->m_bodyB->m_jointList;
 	if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB;
 	j->m_bodyB->m_jointList = &j->m_edgeB;
 
 	b2Body* bodyA = def->bodyA;
 	b2Body* bodyB = def->bodyB;
 
 	// If the joint prevents collisions, then flag any contacts for filtering.
 	if (def->collideConnected == false)
 	{
 		b2ContactEdge* edge = bodyB->GetContactList();
 		while (edge)
 		{
 			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;
 }
 
 void b2World::DestroyJoint(b2Joint* j)
 {
 	b2Assert(IsLocked() == false);
 	if (IsLocked())
 	{
 		return;
 	}
 
 	bool collideConnected = 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.
 	b2Body* bodyA = j->m_bodyA;
 	b2Body* bodyB = j->m_bodyB;
 
 	// Wake up connected bodies.
 	bodyA->SetAwake(true);
 	bodyB->SetAwake(true);
 
 	// Remove from body 1.
 	if (j->m_edgeA.prev)
 	{
 		j->m_edgeA.prev->next = j->m_edgeA.next;
 	}
 
 	if (j->m_edgeA.next)
 	{
 		j->m_edgeA.next->prev = j->m_edgeA.prev;
 	}
 
 	if (&j->m_edgeA == bodyA->m_jointList)
 	{
 		bodyA->m_jointList = j->m_edgeA.next;
 	}
 
 	j->m_edgeA.prev = NULL;
 	j->m_edgeA.next = NULL;
 
 	// Remove from body 2
 	if (j->m_edgeB.prev)
 	{
 		j->m_edgeB.prev->next = j->m_edgeB.next;
 	}
 
 	if (j->m_edgeB.next)
 	{
 		j->m_edgeB.next->prev = j->m_edgeB.prev;
 	}
 
 	if (&j->m_edgeB == bodyB->m_jointList)
 	{
 		bodyB->m_jointList = j->m_edgeB.next;
 	}
 
 	j->m_edgeB.prev = NULL;
 	j->m_edgeB.next = NULL;
 
 	b2Joint::Destroy(j, &m_blockAllocator);
 
 	b2Assert(m_jointCount > 0);
 	--m_jointCount;
 
 	// If the joint prevents collisions, then flag any contacts for filtering.
 	if (collideConnected == false)
 	{
 		b2ContactEdge* edge = bodyB->GetContactList();
 		while (edge)
 		{
 			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;
 		}
 	}
 }
 
 //
 void b2World::SetAllowSleeping(bool flag)
 {
 	if (flag == m_allowSleep)
 	{
 		return;
 	}
 
 	m_allowSleep = flag;
 	if (m_allowSleep == false)
 	{
 		for (b2Body* b = m_bodyList; b; b = b->m_next)
 		{
 			b->SetAwake(true);
 		}
 	}
 }
 
 // Find islands, integrate and solve constraints, solve position constraints
 void b2World::Solve(const b2TimeStep& step)
 {
 	m_profile.solveInit = 0.0f;
 	m_profile.solveVelocity = 0.0f;
 	m_profile.solvePosition = 0.0f;
 
 	// Size the island for the worst case.
 	b2Island island(m_bodyCount,
 					m_contactManager.m_contactCount,
 					m_jointCount,
 					&m_stackAllocator,
 					m_contactManager.m_contactListener);
 
 	// Clear all the island flags.
 	for (b2Body* b = m_bodyList; b; b = b->m_next)
 	{
 		b->m_flags &= ~b2Body::e_islandFlag;
 	}
 
 	for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
 	{
 		c->m_flags &= ~b2Contact::e_islandFlag;
 	}
 
 	for (b2Joint* j = m_jointList; j; j = j->m_next)
 	{
 		j->m_islandFlag = false;
 	}
 
 	// Build and simulate all awake islands.
 	int32 stackSize = m_bodyCount;
 	b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
 	for (b2Body* seed = m_bodyList; seed; seed = seed->m_next)
 	{
 		if (seed->m_flags & b2Body::e_islandFlag)
 		{
 			continue;
 		}
 
 		if (seed->IsAwake() == false || seed->IsActive() == false)
 		{
 			continue;
 		}
 
 		// The seed can be dynamic or kinematic.
 		if (seed->GetType() == b2_staticBody)
 		{
 			continue;
 		}
 
 		//printf("{N}");
 		//fflush(stdout);
 		// Reset island and stack.
 		island.Clear();
 		int32 stackCount = 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.
 			b2Body* b = stack[--stackCount];
 			b2Assert(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() == b2_staticBody)
 			{
 				continue;
 			}
 
 			// Search all contacts connected to this body.
 			for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)
 			{
 				b2Contact* contact = ce->contact;
 
 				// Has this contact already been added to an island?
 				if (contact->m_flags & b2Contact::e_islandFlag)
 				{
 					continue;
 				}
 
 				// Is this contact solid and touching?
 				if (contact->IsEnabled() == false ||
 					contact->IsTouching() == false)
 				{
 					continue;
 				}
 
 				// Skip sensors.
 				bool sensorA = contact->m_fixtureA->m_isSensor;
 				bool sensorB = contact->m_fixtureB->m_isSensor;
 				if (sensorA || sensorB)
 				{
 					continue;
 				}
 
 				island.Add(contact);
 				contact->m_flags |= b2Contact::e_islandFlag;
 
 				b2Body* other = ce->other;
 
 				// Was the other body already added to this island?
 				if (other->m_flags & b2Body::e_islandFlag)
 				{
 					continue;
 				}
 
 				b2Assert(stackCount < stackSize);
 				stack[stackCount++] = other;
 				other->m_flags |= b2Body::e_islandFlag;
 			}
 
 			// Search all joints connect to this body.
 			for (b2JointEdge* je = b->m_jointList; je; je = je->next)
 			{
 				if (je->joint->m_islandFlag == true)
 				{
 					continue;
 				}
 
 				b2Body* other = je->other;
 
 				// Don't simulate joints connected to inactive bodies.
 				if (other->IsActive() == false)
 				{
 					continue;
 				}
 
 				island.Add(je->joint);
 				je->joint->m_islandFlag = true;
 
 				if (other->m_flags & b2Body::e_islandFlag)
 				{
 					continue;
 				}
 
 				b2Assert(stackCount < stackSize);
 				stack[stackCount++] = other;
 				other->m_flags |= b2Body::e_islandFlag;
 			}
 		}
 
 		//printf("{M}");
 		//fflush(stdout);
 		b2Profile profile;
 		island.Solve(&profile, step, m_gravity, m_allowSleep);
 		m_profile.solveInit += profile.solveInit;
 		m_profile.solveVelocity += profile.solveVelocity;
 		m_profile.solvePosition += profile.solvePosition;
 
 		//printf("{;}");
 		//fflush(stdout);
 		// Post solve cleanup.
 		for (int32 i = 0; i < island.m_bodyCount; ++i)
 		{
 			// Allow static bodies to participate in other islands.
 			b2Body* b = island.m_bodies[i];
 			if (b->GetType() == b2_staticBody)
 			{
 				b->m_flags &= ~b2Body::e_islandFlag;
 			}
 		}
 	}
 
 	m_stackAllocator.Free(stack);
 
 	{
 		b2Timer timer;
 		// Synchronize fixtures, check for out of range bodies.
 		for (b2Body* b = m_bodyList; b; b = b->GetNext())
 		{
 			// If a body was not in an island then it did not move.
 			if ((b->m_flags & b2Body::e_islandFlag) == 0)
 			{
 				continue;
 			}
 
 			if (b->GetType() == b2_staticBody)
 			{
 				continue;
 			}
 
 			// Update fixtures (for broad-phase).
 			b->SynchronizeFixtures();
 		}
 
 		// Look for new contacts.
 		m_contactManager.FindNewContacts();
 		m_profile.broadphase = timer.GetMilliseconds();
 	}
 }
 
 // Find TOI contacts and solve them.
 void b2World::SolveTOI(const b2TimeStep& step)
 {
 
 	printf("\n");
 	printf("{A}");
 	fflush(stdout);
 
 	b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);
 	printf("{B}");
 	fflush(stdout);
 
 	if (m_stepComplete)
 	{
 		for (b2Body* b = m_bodyList; b; b = b->m_next)
 		{
 			b->m_flags &= ~b2Body::e_islandFlag;
 			b->m_sweep.alpha0 = 0.0f;
 		}
 		printf("{C}");
 		fflush(stdout);
 
 		for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
 		{
 			// Invalidate TOI
 			c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
 			c->m_toiCount = 0;
 			c->m_toi = 1.0f;
 		}
 	}
 	printf("{D}");
 	fflush(stdout);
 
 	// Find TOI events and solve them.
 	for (;;)
 	{
 		// Find the first TOI.
 		b2Contact* minContact = NULL;
 		float32 minAlpha = 1.0f;
-		int32 contactCount = 0;
+
-		int32 contactProcessedCount = 0;
+
 		{
 			// Is this contact disabled?
 			if (c->IsEnabled() == false)
-			contactCount++;
+
 				continue;
 			}
 
 			// Prevent excessive sub-stepping.
 			if (c->m_toiCount > b2_maxSubSteps)
 			{
 				continue;
 			}
 
 			float32 alpha = 1.0f;
 			if (c->m_flags & b2Contact::e_toiFlag)
 			{
 				// This contact has a valid cached TOI.
 				alpha = c->m_toi;
 			}
 			else
 			{
 				b2Fixture* fA = c->GetFixtureA();
 				b2Fixture* fB = c->GetFixtureB();
 
 				// Is there a sensor?
 				if (fA->IsSensor() || fB->IsSensor())
 				{
 					continue;
 				}
 
 				b2Body* bA = fA->GetBody();
 				b2Body* bB = fB->GetBody();
 
 				b2BodyType typeA = bA->m_type;
 				b2BodyType typeB = bB->m_type;
 				b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);
 
 				bool activeA = bA->IsAwake() && typeA != b2_staticBody;
 				bool activeB = bB->IsAwake() && typeB != b2_staticBody;
 
 				// Is at least one body active (awake and dynamic or kinematic)?
 				if (activeA == false && activeB == false)
 				{
 					continue;
 				}
 
 				bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;
 				bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;
 
 				// Are these two non-bullet dynamic bodies?
 				if (collideA == false && collideB == false)
 				{
 					continue;
 				}
 
 				// Compute the TOI for this contact.
 				// Put the sweeps onto the same time interval.
 				float32 alpha0 = bA->m_sweep.alpha0;
 
 				if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
 				{
 					alpha0 = bB->m_sweep.alpha0;
 					bA->m_sweep.Advance(alpha0);
 				}
 				else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0)
 				{
 					alpha0 = bA->m_sweep.alpha0;
 					bB->m_sweep.Advance(alpha0);
 				}
 
 				b2Assert(alpha0 < 1.0f);
 
 				int32 indexA = c->GetChildIndexA();
 				int32 indexB = c->GetChildIndexB();
 
 				// Compute the time of impact in interval [0, minTOI]
 				b2TOIInput input;
 				input.proxyA.Set(fA->GetShape(), indexA);
 				input.proxyB.Set(fB->GetShape(), indexB);
 				input.sweepA = bA->m_sweep;
 				input.sweepB = bB->m_sweep;
 				input.tMax = 1.0f;
 
 				b2TOIOutput output;
 				b2TimeOfImpact(&output, &input);
 
 				// Beta is the fraction of the remaining portion of the .
 				float32 beta = output.t;
 				if (output.state == b2TOIOutput::e_touching)
 				{
 					alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
 				}
 				else
 				{
 					alpha = 1.0f;
 				}
 
 				c->m_toi = alpha;
 				c->m_flags |= b2Contact::e_toiFlag;
 			}
 
 			if (alpha < minAlpha)
-				contactProcessedCount++;
+
 				// This is the minimum TOI found so far.
 				minContact = c;
 				minAlpha = alpha;
 			}
 		}
 
 		if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha)
 		{
 			printf("{E2}");
-		printf("{c");
+
-		printf("%d",contactProcessedCount);
+
-		printf("/");
+
-		printf("%d",contactCount);
+
-		printf("}");
+
 			printf("{E1}");
 			fflush(stdout);
 		}
 
 		// Advance the bodies to the TOI.
 		b2Fixture* fA = minContact->GetFixtureA();
 		b2Fixture* fB = minContact->GetFixtureB();
 		b2Body* bA = fA->GetBody();
 		b2Body* bB = fB->GetBody();
 
 		b2Sweep backup1 = bA->m_sweep;
 		b2Sweep backup2 = bB->m_sweep;
 
 		bA->Advance(minAlpha);
 		bB->Advance(minAlpha);
 		printf("{F}");
 		fflush(stdout);
 
 		// The TOI contact likely has some new contact points.
 		minContact->Update(m_contactManager.m_contactListener);
 		minContact->m_flags &= ~b2Contact::e_toiFlag;
 		++minContact->m_toiCount;
 
 		printf("{G}");
 		fflush(stdout);
 		// Is the contact solid?
 		if (minContact->IsEnabled() == false || minContact->IsTouching() == false)
 		{
 			// Restore the sweeps.
 			minContact->SetEnabled(false);
 			bA->m_sweep = backup1;
 			bB->m_sweep = backup2;
 			bA->SynchronizeTransform();
 			bB->SynchronizeTransform();
 			continue;
 		}
 
 		bA->SetAwake(true);
 		bB->SetAwake(true);
 
 		// Build the island
 		island.Clear();
 		island.Add(bA);
 		island.Add(bB);
 		island.Add(minContact);
 
 		bA->m_flags |= b2Body::e_islandFlag;
 		bB->m_flags |= b2Body::e_islandFlag;
 		minContact->m_flags |= b2Contact::e_islandFlag;
 
 		// Get contacts on bodyA and bodyB.
 		b2Body* bodies[2] = {bA, bB};
 		for (int32 i = 0; i < 2; ++i)
 		{
 			b2Body* body = bodies[i];
 			if (body->m_type == b2_dynamicBody)
 			{
 				for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
 				{
 					if (island.m_bodyCount == island.m_bodyCapacity)
 					{
 						break;
 					}
 
 					if (island.m_contactCount == island.m_contactCapacity)
 					{
 						break;
 					}
 
 					b2Contact* contact = ce->contact;
 
 					// Has this contact already been added to the island?
 					if (contact->m_flags & b2Contact::e_islandFlag)
 					{
 						continue;
 					}
 
 					// Only add static, kinematic, or bullet bodies.
 					b2Body* other = ce->other;
 					if (other->m_type == b2_dynamicBody &&
 						body->IsBullet() == false && other->IsBullet() == false)
 					{
 						continue;
 					}
 
 					// Skip sensors.
 					bool sensorA = contact->m_fixtureA->m_isSensor;
 					bool sensorB = contact->m_fixtureB->m_isSensor;
 					if (sensorA || sensorB)
 					{
 						continue;
 					}
 
 					// Tentatively advance the body to the TOI.
 					b2Sweep backup = other->m_sweep;
 					if ((other->m_flags & b2Body::e_islandFlag) == 0)
 					{
 						other->Advance(minAlpha);
 					}
 
 					// Update the contact points
 					contact->Update(m_contactManager.m_contactListener);
 
 					// Was the contact disabled by the user?
 					if (contact->IsEnabled() == false)
 					{
 						other->m_sweep = backup;
 						other->SynchronizeTransform();
 						continue;
 					}
 
 					// Are there contact points?
 					if (contact->IsTouching() == false)
 					{
 						other->m_sweep = backup;
 						other->SynchronizeTransform();
 						continue;
 					}
 
 					// Add the contact to the island
 					contact->m_flags |= b2Contact::e_islandFlag;
 					island.Add(contact);
 
 					// Has the other body already been added to the island?
 					if (other->m_flags & b2Body::e_islandFlag)
 					{
 						continue;
 					}
 
 					// Add the other body to the island.
 					other->m_flags |= b2Body::e_islandFlag;
 
 					if (other->m_type != b2_staticBody)
 					{
 						other->SetAwake(true);
 					}
 
 					island.Add(other);
 				}
 			}
 		}
 		printf("{H}");
 		fflush(stdout);
 
 		b2TimeStep subStep;
 		subStep.dt = (1.0f - minAlpha) * step.dt;
 		subStep.inv_dt = 1.0f / subStep.dt;
 		subStep.dtRatio = 1.0f;
 		subStep.positionIterations = 20;
 		subStep.velocityIterations = step.velocityIterations;
 		subStep.warmStarting = false;
 		island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);
 		printf("{I}");
 		fflush(stdout);
 
 		// Reset island flags and synchronize broad-phase proxies.
 		for (int32 i = 0; i < island.m_bodyCount; ++i)
 		{
 			b2Body* body = island.m_bodies[i];
 			body->m_flags &= ~b2Body::e_islandFlag;
 
 			if (body->m_type != b2_dynamicBody)
 			{
 				continue;
 			}
 
 			body->SynchronizeFixtures();
 
 			// Invalidate all contact TOIs on this displaced body.
 			for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
 			{
 				ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
 			}
 		}
 		printf("{J}");
 		fflush(stdout);
 
 		// Commit fixture proxy movements to the broad-phase so that new contacts are created.
 		// Also, some contacts can be destroyed.
 		m_contactManager.FindNewContacts();
 		printf("{K}");
 		fflush(stdout);
 
 		if (m_subStepping)
 		{
 			m_stepComplete = false;
 			break;
 		}
 	}
 }
 
 void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
 {
 	b2Timer stepTimer;
 
 	// If new fixtures were added, we need to find the new contacts.
 	if (m_flags & e_newFixture)
 	{
 		m_contactManager.FindNewContacts();
 		m_flags &= ~e_newFixture;
 	}
 
 	m_flags |= e_locked;
 
 	b2TimeStep 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 = m_inv_dt0 * dt;
 
 	step.warmStarting = m_warmStarting;
 
 	// Update contacts. This is where some contacts are destroyed.
 	{
 		b2Timer timer;
 		m_contactManager.Collide();
 		m_profile.collide = timer.GetMilliseconds();
 	}
 
 	// Integrate velocities, solve velocity constraints, and integrate positions.
 	if (m_stepComplete && step.dt > 0.0f)
 	{
 		b2Timer timer;
 		Solve(step);
 		m_profile.solve = timer.GetMilliseconds();
 	}
 
 	// Handle TOI events.
 	if (m_continuousPhysics && step.dt > 0.0f)
 	{
 		b2Timer timer;
 		SolveTOI(step);
 		m_profile.solveTOI = timer.GetMilliseconds();
 	}
 
 	if (step.dt > 0.0f)
 	{
 		m_inv_dt0 = step.inv_dt;
 	}
 
 	if (m_flags & e_clearForces)
 	{
 		ClearForces();
 	}
 
 	m_flags &= ~e_locked;
 
 	m_profile.step = stepTimer.GetMilliseconds();
 
 	printf("\n");
 	printf("{PR|");
 	printf("%.2f", m_profile.step);
 	printf("|");
 	printf("%.2f", m_profile.collide);
 	printf("|");
 	printf("%.2f", m_profile.solve);
 	printf("|");
 	printf("%.2f", m_profile.solveInit);
 	printf("|");
 	printf("%.2f", m_profile.solveVelocity);
 	printf("|");
 	printf("%.2f", m_profile.solvePosition);
 	printf("|");
 	printf("%.2f", m_profile.solveTOI);
 	printf("|");
 	printf("%.2f", m_profile.broadphase);
 	printf("}");
 	fflush(stdout);
 
 }
 
 void b2World::ClearForces()
 {
 	for (b2Body* body = m_bodyList; body; body = body->GetNext())
 	{
 		body->m_force.SetZero();
 		body->m_torque = 0.0f;
 	}
 }
 
 struct b2WorldQueryWrapper
 {
 	bool QueryCallback(int32 proxyId)
 	{
 		b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId);
 		return callback->ReportFixture(proxy->fixture);
 	}
 
 	const b2BroadPhase* broadPhase;
 	b2QueryCallback* callback;
 };
 
 void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const
 {
 	b2WorldQueryWrapper wrapper;
 	wrapper.broadPhase = &m_contactManager.m_broadPhase;
 	wrapper.callback = callback;
 	m_contactManager.m_broadPhase.Query(&wrapper, aabb);
 }
 
 struct b2WorldRayCastWrapper
 {
 	float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId)
 	{
 		void* userData = broadPhase->GetUserData(proxyId);
 		b2FixtureProxy* proxy = (b2FixtureProxy*)userData;
 		b2Fixture* fixture = proxy->fixture;
 		int32 index = proxy->childIndex;
 		b2RayCastOutput output;
 		bool hit = fixture->RayCast(&output, input, index);
 
 		if (hit)
 		{
 			float32 fraction = output.fraction;
 			b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
 			return callback->ReportFixture(fixture, point, output.normal, fraction);
 		}
 
 		return input.maxFraction;
 	}
 
 	const b2BroadPhase* broadPhase;
 	b2RayCastCallback* callback;
 };
 
 void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const
 {
 	b2WorldRayCastWrapper wrapper;
 	wrapper.broadPhase = &m_contactManager.m_broadPhase;
 	wrapper.callback = callback;
 	b2RayCastInput input;
 	input.maxFraction = 1.0f;
 	input.p1 = point1;
 	input.p2 = point2;
 	m_contactManager.m_broadPhase.RayCast(&wrapper, input);
 }
 
 void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color)
 {
 	switch (fixture->GetType())
 	{
 	case b2Shape::e_circle:
 		{
 			b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();
 
 			b2Vec2 center = b2Mul(xf, circle->m_p);
 			float32 radius = circle->m_radius;
 			b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));
 
 			g_debugDraw->DrawSolidCircle(center, radius, axis, color);
 		}
 		break;
 
 	case b2Shape::e_edge:
 		{
 			b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
 			b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
 			b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
 			g_debugDraw->DrawSegment(v1, v2, color);
 		}
 		break;
 
 	case b2Shape::e_chain:
 		{
 			b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
 			int32 count = chain->m_count;
 			const b2Vec2* vertices = chain->m_vertices;
 
 			b2Vec2 v1 = b2Mul(xf, vertices[0]);
 			for (int32 i = 1; i < count; ++i)
 			{
 				b2Vec2 v2 = b2Mul(xf, vertices[i]);
 				g_debugDraw->DrawSegment(v1, v2, color);
 				g_debugDraw->DrawCircle(v1, 0.05f, color);
 				v1 = v2;
 			}
 		}
 		break;
 
 	case b2Shape::e_polygon:
 		{
 			b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
 			int32 vertexCount = poly->m_count;
 			b2Assert(vertexCount <= b2_maxPolygonVertices);
 			b2Vec2 vertices[b2_maxPolygonVertices];
 
 			for (int32 i = 0; i < vertexCount; ++i)
 			{
 				vertices[i] = b2Mul(xf, poly->m_vertices[i]);
 			}
 
 			g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
 		}
 		break;
 
     default:
         break;
 	}
 }
 
 void b2World::DrawJoint(b2Joint* joint)
 {
 	b2Body* bodyA = joint->GetBodyA();
 	b2Body* bodyB = joint->GetBodyB();
 	const b2Transform& xf1 = bodyA->GetTransform();
 	const b2Transform& xf2 = bodyB->GetTransform();
 	b2Vec2 x1 = xf1.p;
 	b2Vec2 x2 = xf2.p;
 	b2Vec2 p1 = joint->GetAnchorA();
 	b2Vec2 p2 = joint->GetAnchorB();
 
 	b2Color color(0.5f, 0.8f, 0.8f);
 
 	switch (joint->GetType())
 	{
 	case e_distanceJoint:
 		g_debugDraw->DrawSegment(p1, p2, color);
 		break;
 
 	case e_pulleyJoint:
 		{
 			b2PulleyJoint* pulley = (b2PulleyJoint*)joint;
 			b2Vec2 s1 = pulley->GetGroundAnchorA();
 			b2Vec2 s2 = pulley->GetGroundAnchorB();
 			g_debugDraw->DrawSegment(s1, p1, color);
 			g_debugDraw->DrawSegment(s2, p2, color);
 			g_debugDraw->DrawSegment(s1, s2, color);
 		}
 		break;
 
 	case e_mouseJoint:
 		// don't draw this
 		break;
 
 	default:
 		g_debugDraw->DrawSegment(x1, p1, color);
 		g_debugDraw->DrawSegment(p1, p2, color);
 		g_debugDraw->DrawSegment(x2, p2, color);
 	}
 }
 
 void b2World::DrawDebugData()
 {
 	if (g_debugDraw == NULL)
 	{
 		return;
 	}
 
 	uint32 flags = g_debugDraw->GetFlags();
 
 	if (flags & b2Draw::e_shapeBit)
 	{
 		for (b2Body* b = m_bodyList; b; b = b->GetNext())
 		{
 			const b2Transform& xf = b->GetTransform();
 			for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
 			{
 				if (b->IsActive() == false)
 				{
 					DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f));
 				}
 				else if (b->GetType() == b2_staticBody)
 				{
 					DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f));
 				}
 				else if (b->GetType() == b2_kinematicBody)
 				{
 					DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f));
 				}
 				else if (b->IsAwake() == false)
 				{
 					DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f));
 				}
 				else
 				{
 					DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f));
 				}
 			}
 		}
 	}
 
 	if (flags & b2Draw::e_jointBit)
 	{
 		for (b2Joint* j = m_jointList; j; j = j->GetNext())
 		{
 			DrawJoint(j);
 		}
 	}
 
 	if (flags & b2Draw::e_pairBit)
 	{
 		b2Color color(0.3f, 0.9f, 0.9f);
 		for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())
 		{
 			//b2Fixture* fixtureA = c->GetFixtureA();
 			//b2Fixture* fixtureB = c->GetFixtureB();
 
 			//b2Vec2 cA = fixtureA->GetAABB().GetCenter();
 			//b2Vec2 cB = fixtureB->GetAABB().GetCenter();
 
 			//g_debugDraw->DrawSegment(cA, cB, color);
 		}
 	}
 
 	if (flags & b2Draw::e_aabbBit)
 	{
 		b2Color color(0.9f, 0.3f, 0.9f);
 		b2BroadPhase* bp = &m_contactManager.m_broadPhase;
 
 		for (b2Body* b = m_bodyList; b; b = b->GetNext())
 		{
 			if (b->IsActive() == false)
 			{
 				continue;
 			}
 
 			for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
 			{
 				for (int32 i = 0; i < f->m_proxyCount; ++i)
 				{
 					b2FixtureProxy* proxy = f->m_proxies + i;
 					b2AABB aabb = bp->GetFatAABB(proxy->proxyId);
 					b2Vec2 vs[4];
 					vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
 					vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
 					vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
 					vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
 
 					g_debugDraw->DrawPolygon(vs, 4, color);
 				}
 			}
 		}
 	}
 
 	if (flags & b2Draw::e_centerOfMassBit)
 	{
 		for (b2Body* b = m_bodyList; b; b = b->GetNext())
 		{
 			b2Transform xf = b->GetTransform();
 			xf.p = b->GetWorldCenter();
 			g_debugDraw->DrawTransform(xf);
 		}
 	}
 }
 
 int32 b2World::GetProxyCount() const
 {
 	return m_contactManager.m_broadPhase.GetProxyCount();
 }
 
 int32 b2World::GetTreeHeight() const
 {
 	return m_contactManager.m_broadPhase.GetTreeHeight();
 }
 
 int32 b2World::GetTreeBalance() const
 {
 	return m_contactManager.m_broadPhase.GetTreeBalance();
 }
 
 float32 b2World::GetTreeQuality() const
 {
 	return m_contactManager.m_broadPhase.GetTreeQuality();
 }
 
 void b2World::ShiftOrigin(const b2Vec2& newOrigin)
 {
 	b2Assert((m_flags & e_locked) == 0);
 	if ((m_flags & e_locked) == e_locked)
 	{
 		return;
 	}
 
 	for (b2Body* b = m_bodyList; b; b = b->m_next)
 	{
 		b->m_xf.p -= newOrigin;
 		b->m_sweep.c0 -= newOrigin;
 		b->m_sweep.c -= newOrigin;
 	}
 
 	for (b2Joint* j = m_jointList; j; j = j->m_next)
 	{
 		j->ShiftOrigin(newOrigin);
 	}
 
 	m_contactManager.m_broadPhase.ShiftOrigin(newOrigin);
 }
 
 void b2World::Dump()
 {
 	if ((m_flags & e_locked) == e_locked)
 	{
 		return;
 	}
 
 	b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y);
 	b2Log("m_world->SetGravity(g);\n");
 
 	b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);
 	b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);
 	int32 i = 0;
 	for (b2Body* b = m_bodyList; b; b = b->m_next)
 	{
 		b->m_islandIndex = i;
 		b->Dump();
 		++i;
 	}
 
 	i = 0;
 	for (b2Joint* j = m_jointList; j; j = j->m_next)
 	{
 		j->m_index = i;
 		++i;
 	}
 
 	// First pass on joints, skip gear joints.
 	for (b2Joint* j = m_jointList; j; j = j->m_next)
 	{
 		if (j->m_type == e_gearJoint)
 		{
 			continue;
 		}
 
 		b2Log("{\n");
 		j->Dump();
 		b2Log("}\n");
 	}
 
 	// Second pass on joints, only gear joints.
 	for (b2Joint* j = m_jointList; j; j = j->m_next)
 	{
 		if (j->m_type != e_gearJoint)
 		{
 			continue;
 		}
 
 		b2Log("{\n");
 		j->Dump();
 		b2Log("}\n");
 	}
 
 	b2Log("b2Free(joints);\n");
 	b2Log("b2Free(bodies);\n");
 	b2Log("joints = NULL;\n");
 	b2Log("bodies = NULL;\n");
 }