游戏引擎

开发平台：
Visual C++

b2World.cpp：源码内容
							/*
* 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.
*/
#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/Dynamics/Contacts/b2TOISolver.h>
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/b2BroadPhase.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
#include <Box2D/Collision/b2TimeOfImpact.h>
#include <new>
b2World::b2World(const b2Vec2& gravity, bool doSleep)
{
	m_destructionListener = NULL;
	m_debugDraw = NULL;
	m_bodyList = NULL;
	m_jointList = NULL;
	m_bodyCount = 0;
	m_jointCount = 0;
	m_warmStarting = true;
	m_continuousPhysics = true;
	m_allowSleep = doSleep;
	m_gravity = gravity;
	m_flags = e_clearForces;
	m_inv_dt0 = 0.0f;
	m_contactManager.m_allocator = &m_blockAllocator;
}
b2World::~b2World()
{
}
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(b2DebugDraw* debugDraw)
{
	m_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 = 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->DestroyProxy(&m_contactManager.m_broadPhase);
		f0->Destroy(&m_blockAllocator);
		f0->~b2Fixture();
		m_blockAllocator.Free(f0, sizeof(b2Fixture));
	}
	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;
		}
	}
}
// Find islands, integrate and solve constraints, solve position constraints
void b2World::Solve(const b2TimeStep& step)
{
	// 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;
		}
		// 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;
			}
		}
		island.Solve(step, m_gravity, m_allowSleep);
		// 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);
	// 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();
}
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
void b2World::SolveTOI(b2Body* body)
{
	// Find the minimum contact.
	b2Contact* toiContact = NULL;
	float32 toi = 1.0f;
	b2Body* toiOther = NULL;
	bool found;
	int32 count;
	int32 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 (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
		{
			if (ce->contact == toiContact)
			{
				continue;
			}
			b2Body* other = ce->other;
			b2BodyType 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->m_flags & b2Body::e_toiFlag) == 0)
				{
					continue;
				}
				// No repeated hits on non-static bodies
				if (type != b2_staticBody && (ce->contact->m_flags & b2Contact::e_bulletHitFlag) != 0)
				{
						continue;
				}
			}
			else if (type == b2_dynamicBody)
			{
				continue;
			}
			// Check for a disabled contact.
			b2Contact* contact = ce->contact;
			if (contact->IsEnabled() == false)
			{
				continue;
			}
			// Prevent infinite looping.
			if (contact->m_toiCount > 10)
			{
				continue;
			}
			b2Fixture* fixtureA = contact->m_fixtureA;
			b2Fixture* fixtureB = contact->m_fixtureB;
			// Cull sensors.
			if (fixtureA->IsSensor() || fixtureB->IsSensor())
			{
				continue;
			}
			b2Body* bodyA = fixtureA->m_body;
			b2Body* bodyB = fixtureB->m_body;
			// Compute the time of impact in interval [0, minTOI]
			b2TOIInput input;
			input.proxyA.Set(fixtureA->GetShape());
			input.proxyB.Set(fixtureB->GetShape());
			input.sweepA = bodyA->m_sweep;
			input.sweepB = bodyB->m_sweep;
			input.tMax = toi;
			b2TOIOutput output;
			b2TimeOfImpact(&output, &input);
			if (output.state == b2TOIOutput::e_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;
	}
	b2Sweep backup = body->m_sweep;
	body->Advance(toi);
	toiContact->Update(m_contactManager.m_contactListener);
	if (toiContact->IsEnabled() == false)
	{
		// Contact disabled. Backup and recurse.
		body->m_sweep = backup;
		SolveTOI(body);
	}
	++toiContact->m_toiCount;
	// Update all the valid contacts on this body and build a contact island.
	b2Contact* contacts[b2_maxTOIContacts];
	count = 0;
	for (b2ContactEdge* ce = body->m_contactList; ce && count < b2_maxTOIContacts; ce = ce->next)
	{
		b2Body* other = ce->other;
		b2BodyType type = other->GetType();
		// Only perform correction with static bodies, so the
		// body won't get pushed out of the world.
		if (type == b2_dynamicBody)
		{
			continue;
		}
		// Check for a disabled contact.
		b2Contact* contact = ce->contact;
		if (contact->IsEnabled() == false)
		{
			continue;
		}
		b2Fixture* fixtureA = contact->m_fixtureA;
		b2Fixture* fixtureB = contact->m_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(m_contactManager.m_contactListener);
		}
		// Did the user disable the contact?
		if (contact->IsEnabled() == false)
		{
			// Skip this contact.
			continue;
		}
		if (contact->IsTouching() == false)
		{
			continue;
		}
		contacts[count] = contact;
		++count;
	}
	// Reduce the TOI body's overlap with the contact island.
	b2TOISolver solver(&m_stackAllocator);
	solver.Initialize(contacts, count, body);
	const float32 k_toiBaumgarte = 0.75f;
	bool solved = false;
	for (int32 i = 0; i < 20; ++i)
	{
		bool contactsOkay = solver.Solve(k_toiBaumgarte);
		if (contactsOkay)
		{
			solved = true;
			break;
		}
	}
	if (toiOther->GetType() != b2_staticBody)
	{
			toiContact->m_flags |= b2Contact::e_bulletHitFlag;
	}
}
// 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 b2World::SolveTOI()
{
	// Prepare all contacts.
	for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
	{
		// Enable the contact
		c->m_flags |= b2Contact::e_enabledFlag;
		// Set the number of TOI events for this contact to zero.
		c->m_toiCount = 0;
	}
	// Initialize the TOI flag.
	for (b2Body* body = m_bodyList; body; body = body->m_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->m_flags & b2Body::e_islandFlag) == 0 || body->GetType() == b2_kinematicBody || body->GetType() == b2_staticBody)
		{
			body->m_flags |= b2Body::e_toiFlag;
		}
		else
		{
			body->m_flags &= ~b2Body::e_toiFlag;
		}
	}
	// Collide non-bullets.
	for (b2Body* body = m_bodyList; body; body = body->m_next)
	{
		if (body->m_flags & b2Body::e_toiFlag)
		{
			continue;
		}
		if (body->IsBullet() == true)
		{
			continue;
		}
		SolveTOI(body);
		body->m_flags |= b2Body::e_toiFlag;
	}
	// Collide bullets.
	for (b2Body* body = m_bodyList; body; body = body->m_next)
	{
		if (body->m_flags & b2Body::e_toiFlag)
		{
			continue;
		}
		if (body->IsBullet() == false)
		{
			continue;
		}
		SolveTOI(body);
		body->m_flags |= b2Body::e_toiFlag;
	}
}
void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
{
	// 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.
	m_contactManager.Collide();
	// Integrate velocities, solve velocity constraints, and integrate positions.
	if (step.dt > 0.0f)
	{
		Solve(step);
	}
	// Handle TOI events.
	if (m_continuousPhysics && step.dt > 0.0f)
	{
		SolveTOI();
	}
	if (step.dt > 0.0f)
	{
		m_inv_dt0 = step.inv_dt;
	}
	if (m_flags & e_clearForces)
	{
		ClearForces();
	}
	m_flags &= ~e_locked;
}
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)
	{
		b2Fixture* fixture = (b2Fixture*)broadPhase->GetUserData(proxyId);
		return callback->ReportFixture(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);
		b2Fixture* fixture = (b2Fixture*)userData;
		b2RayCastOutput output;
		bool hit = fixture->RayCast(&output, input);
		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 = xf.R.col1;
			m_debugDraw->DrawSolidCircle(center, radius, axis, color);
		}
		break;
	case b2Shape::e_polygon:
		{
			b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
			int32 vertexCount = poly->m_vertexCount;
			b2Assert(vertexCount <= b2_maxPolygonVertices);
			b2Vec2 vertices[b2_maxPolygonVertices];
			for (int32 i = 0; i < vertexCount; ++i)
			{
				vertices[i] = b2Mul(xf, poly->m_vertices[i]);
			}
			m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
		}
		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.position;
	b2Vec2 x2 = xf2.position;
	b2Vec2 p1 = joint->GetAnchorA();
	b2Vec2 p2 = joint->GetAnchorB();
	b2Color color(0.5f, 0.8f, 0.8f);
	switch (joint->GetType())
	{
	case e_distanceJoint:
		m_debugDraw->DrawSegment(p1, p2, color);
		break;
	case e_pulleyJoint:
		{
			b2PulleyJoint* pulley = (b2PulleyJoint*)joint;
			b2Vec2 s1 = pulley->GetGroundAnchorA();
			b2Vec2 s2 = pulley->GetGroundAnchorB();
			m_debugDraw->DrawSegment(s1, p1, color);
			m_debugDraw->DrawSegment(s2, p2, color);
			m_debugDraw->DrawSegment(s1, s2, color);
		}
		break;
	case e_mouseJoint:
		// don't draw this
		break;
	default:
		m_debugDraw->DrawSegment(x1, p1, color);
		m_debugDraw->DrawSegment(p1, p2, color);
		m_debugDraw->DrawSegment(x2, p2, color);
	}
}
void b2World::DrawDebugData()
{
	if (m_debugDraw == NULL)
	{
		return;
	}
	uint32 flags = m_debugDraw->GetFlags();
	if (flags & b2DebugDraw::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 & b2DebugDraw::e_jointBit)
	{
		for (b2Joint* j = m_jointList; j; j = j->GetNext())
		{
			DrawJoint(j);
		}
	}
	if (flags & b2DebugDraw::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();
			m_debugDraw->DrawSegment(cA, cB, color);
		}
	}
	if (flags & b2DebugDraw::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())
			{
				b2AABB aabb = bp->GetFatAABB(f->m_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);
				m_debugDraw->DrawPolygon(vs, 4, color);
			}
		}
	}
	if (flags & b2DebugDraw::e_centerOfMassBit)
	{
		for (b2Body* b = m_bodyList; b; b = b->GetNext())
		{
			b2Transform xf = b->GetTransform();
			xf.position = b->GetWorldCenter();
			m_debugDraw->DrawTransform(xf);
		}
	}
}
int32 b2World::GetProxyCount() const
{
	return m_contactManager.m_broadPhase.GetProxyCount();
}

						