reflexion 0.1.12 → 0.1.17

data/src/physics/Box2D/Dynamics/b2Fixture.h

@@ -1,345 +0,0 @@
-/*
-* Copyright (c) 2006-2009 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.
-*/
-
-#ifndef B2_FIXTURE_H
-#define B2_FIXTURE_H
-
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Collision/Shapes/b2Shape.h>
-
-class b2BlockAllocator;
-class b2Body;
-class b2BroadPhase;
-class b2Fixture;
-
-/// This holds contact filtering data.
-struct b2Filter
-{
-	b2Filter()
-	{
-		categoryBits = 0x0001;
-		maskBits = 0xFFFF;
-		groupIndex = 0;
-	}
-
-	/// The collision category bits. Normally you would just set one bit.
-	uint16 categoryBits;
-
-	/// The collision mask bits. This states the categories that this
-	/// shape would accept for collision.
-	uint16 maskBits;
-
-	/// Collision groups allow a certain group of objects to never collide (negative)
-	/// or always collide (positive). Zero means no collision group. Non-zero group
-	/// filtering always wins against the mask bits.
-	int16 groupIndex;
-};
-
-/// A fixture definition is used to create a fixture. This class defines an
-/// abstract fixture definition. You can reuse fixture definitions safely.
-struct b2FixtureDef
-{
-	/// The constructor sets the default fixture definition values.
-	b2FixtureDef()
-	{
-		shape = NULL;
-		userData = NULL;
-		friction = 0.2f;
-		restitution = 0.0f;
-		density = 0.0f;
-		isSensor = false;
-	}
-
-	/// The shape, this must be set. The shape will be cloned, so you
-	/// can create the shape on the stack.
-	const b2Shape* shape;
-
-	/// Use this to store application specific fixture data.
-	void* userData;
-
-	/// The friction coefficient, usually in the range [0,1].
-	float32 friction;
-
-	/// The restitution (elasticity) usually in the range [0,1].
-	float32 restitution;
-
-	/// The density, usually in kg/m^2.
-	float32 density;
-
-	/// A sensor shape collects contact information but never generates a collision
-	/// response.
-	bool isSensor;
-
-	/// Contact filtering data.
-	b2Filter filter;
-};
-
-/// This proxy is used internally to connect fixtures to the broad-phase.
-struct b2FixtureProxy
-{
-	b2AABB aabb;
-	b2Fixture* fixture;
-	int32 childIndex;
-	int32 proxyId;
-};
-
-/// A fixture is used to attach a shape to a body for collision detection. A fixture
-/// inherits its transform from its parent. Fixtures hold additional non-geometric data
-/// such as friction, collision filters, etc.
-/// Fixtures are created via b2Body::CreateFixture.
-/// @warning you cannot reuse fixtures.
-class b2Fixture
-{
-public:
-	/// Get the type of the child shape. You can use this to down cast to the concrete shape.
-	/// @return the shape type.
-	b2Shape::Type GetType() const;
-
-	/// Get the child shape. You can modify the child shape, however you should not change the
-	/// number of vertices because this will crash some collision caching mechanisms.
-	/// Manipulating the shape may lead to non-physical behavior.
-	b2Shape* GetShape();
-	const b2Shape* GetShape() const;
-
-	/// Set if this fixture is a sensor.
-	void SetSensor(bool sensor);
-
-	/// Is this fixture a sensor (non-solid)?
-	/// @return the true if the shape is a sensor.
-	bool IsSensor() const;
-
-	/// Set the contact filtering data. This will not update contacts until the next time
-	/// step when either parent body is active and awake.
-	/// This automatically calls Refilter.
-	void SetFilterData(const b2Filter& filter);
-
-	/// Get the contact filtering data.
-	const b2Filter& GetFilterData() const;
-
-	/// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide.
-	void Refilter();
-
-	/// Get the parent body of this fixture. This is NULL if the fixture is not attached.
-	/// @return the parent body.
-	b2Body* GetBody();
-	const b2Body* GetBody() const;
-
-	/// Get the next fixture in the parent body's fixture list.
-	/// @return the next shape.
-	b2Fixture* GetNext();
-	const b2Fixture* GetNext() const;
-
-	/// Get the user data that was assigned in the fixture definition. Use this to
-	/// store your application specific data.
-	void* GetUserData() const;
-
-	/// Set the user data. Use this to store your application specific data.
-	void SetUserData(void* data);
-
-	/// Test a point for containment in this fixture.
-	/// @param p a point in world coordinates.
-	bool TestPoint(const b2Vec2& p) const;
-
-	/// Cast a ray against this shape.
-	/// @param output the ray-cast results.
-	/// @param input the ray-cast input parameters.
-	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const;
-
-	/// Get the mass data for this fixture. The mass data is based on the density and
-	/// the shape. The rotational inertia is about the shape's origin. This operation
-	/// may be expensive.
-	void GetMassData(b2MassData* massData) const;
-
-	/// Set the density of this fixture. This will _not_ automatically adjust the mass
-	/// of the body. You must call b2Body::ResetMassData to update the body's mass.
-	void SetDensity(float32 density);
-
-	/// Get the density of this fixture.
-	float32 GetDensity() const;
-
-	/// Get the coefficient of friction.
-	float32 GetFriction() const;
-
-	/// Set the coefficient of friction. This will _not_ change the friction of
-	/// existing contacts.
-	void SetFriction(float32 friction);
-
-	/// Get the coefficient of restitution.
-	float32 GetRestitution() const;
-
-	/// Set the coefficient of restitution. This will _not_ change the restitution of
-	/// existing contacts.
-	void SetRestitution(float32 restitution);
-
-	/// Get the fixture's AABB. This AABB may be enlarge and/or stale.
-	/// If you need a more accurate AABB, compute it using the shape and
-	/// the body transform.
-	const b2AABB& GetAABB(int32 childIndex) const;
-
-	/// Dump this fixture to the log file.
-	void Dump(int32 bodyIndex);
-
-protected:
-
-	friend class b2Body;
-	friend class b2World;
-	friend class b2Contact;
-	friend class b2ContactManager;
-
-	b2Fixture();
-
-	// We need separation create/destroy functions from the constructor/destructor because
-	// the destructor cannot access the allocator (no destructor arguments allowed by C++).
-	void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def);
-	void Destroy(b2BlockAllocator* allocator);
-
-	// These support body activation/deactivation.
-	void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf);
-	void DestroyProxies(b2BroadPhase* broadPhase);
-
-	void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2);
-
-	float32 m_density;
-
-	b2Fixture* m_next;
-	b2Body* m_body;
-
-	b2Shape* m_shape;
-
-	float32 m_friction;
-	float32 m_restitution;
-
-	b2FixtureProxy* m_proxies;
-	int32 m_proxyCount;
-
-	b2Filter m_filter;
-
-	bool m_isSensor;
-
-	void* m_userData;
-};
-
-inline b2Shape::Type b2Fixture::GetType() const
-{
-	return m_shape->GetType();
-}
-
-inline b2Shape* b2Fixture::GetShape()
-{
-	return m_shape;
-}
-
-inline const b2Shape* b2Fixture::GetShape() const
-{
-	return m_shape;
-}
-
-inline bool b2Fixture::IsSensor() const
-{
-	return m_isSensor;
-}
-
-inline const b2Filter& b2Fixture::GetFilterData() const
-{
-	return m_filter;
-}
-
-inline void* b2Fixture::GetUserData() const
-{
-	return m_userData;
-}
-
-inline void b2Fixture::SetUserData(void* data)
-{
-	m_userData = data;
-}
-
-inline b2Body* b2Fixture::GetBody()
-{
-	return m_body;
-}
-
-inline const b2Body* b2Fixture::GetBody() const
-{
-	return m_body;
-}
-
-inline b2Fixture* b2Fixture::GetNext()
-{
-	return m_next;
-}
-
-inline const b2Fixture* b2Fixture::GetNext() const
-{
-	return m_next;
-}
-
-inline void b2Fixture::SetDensity(float32 density)
-{
-	b2Assert(b2IsValid(density) && density >= 0.0f);
-	m_density = density;
-}
-
-inline float32 b2Fixture::GetDensity() const
-{
-	return m_density;
-}
-
-inline float32 b2Fixture::GetFriction() const
-{
-	return m_friction;
-}
-
-inline void b2Fixture::SetFriction(float32 friction)
-{
-	m_friction = friction;
-}
-
-inline float32 b2Fixture::GetRestitution() const
-{
-	return m_restitution;
-}
-
-inline void b2Fixture::SetRestitution(float32 restitution)
-{
-	m_restitution = restitution;
-}
-
-inline bool b2Fixture::TestPoint(const b2Vec2& p) const
-{
-	return m_shape->TestPoint(m_body->GetTransform(), p);
-}
-
-inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const
-{
-	return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex);
-}
-
-inline void b2Fixture::GetMassData(b2MassData* massData) const
-{
-	m_shape->ComputeMass(massData, m_density);
-}
-
-inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const
-{
-	b2Assert(0 <= childIndex && childIndex < m_proxyCount);
-	return m_proxies[childIndex].aabb;
-}
-
-#endif

data/src/physics/Box2D/Dynamics/b2Island.cpp

@@ -1,539 +0,0 @@
-/*
-* 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/Collision/b2Distance.h>
-#include <Box2D/Dynamics/b2Island.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2World.h>
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
-#include <Box2D/Dynamics/Joints/b2Joint.h>
-#include <Box2D/Common/b2StackAllocator.h>
-#include <Box2D/Common/b2Timer.h>
-
-/*
-Position Correction Notes
-=========================
-I tried the several algorithms for position correction of the 2D revolute joint.
-I looked at these systems:
-- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
-- suspension bridge with 30 1m long planks of length 1m.
-- multi-link chain with 30 1m long links.
-
-Here are the algorithms:
-
-Baumgarte - A fraction of the position error is added to the velocity error. There is no
-separate position solver.
-
-Pseudo Velocities - After the velocity solver and position integration,
-the position error, Jacobian, and effective mass are recomputed. Then
-the velocity constraints are solved with pseudo velocities and a fraction
-of the position error is added to the pseudo velocity error. The pseudo
-velocities are initialized to zero and there is no warm-starting. After
-the position solver, the pseudo velocities are added to the positions.
-This is also called the First Order World method or the Position LCP method.
-
-Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
-position error is re-computed for each constraint and the positions are updated
-after the constraint is solved. The radius vectors (aka Jacobians) are
-re-computed too (otherwise the algorithm has horrible instability). The pseudo
-velocity states are not needed because they are effectively zero at the beginning
-of each iteration. Since we have the current position error, we allow the
-iterations to terminate early if the error becomes smaller than b2_linearSlop.
-
-Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
-each time a constraint is solved.
-
-Here are the results:
-Baumgarte - this is the cheapest algorithm but it has some stability problems,
-especially with the bridge. The chain links separate easily close to the root
-and they jitter as they struggle to pull together. This is one of the most common
-methods in the field. The big drawback is that the position correction artificially
-affects the momentum, thus leading to instabilities and false bounce. I used a
-bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
-factor makes joints and contacts more spongy.
-
-Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
-stable. However, joints still separate with large angular velocities. Drag the
-simple pendulum in a circle quickly and the joint will separate. The chain separates
-easily and does not recover. I used a bias factor of 0.2. A larger value lead to
-the bridge collapsing when a heavy cube drops on it.
-
-Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
-Velocities, but in other ways it is worse. The bridge and chain are much more
-stable, but the simple pendulum goes unstable at high angular velocities.
-
-Full NGS - stable in all tests. The joints display good stiffness. The bridge
-still sags, but this is better than infinite forces.
-
-Recommendations
-Pseudo Velocities are not really worthwhile because the bridge and chain cannot
-recover from joint separation. In other cases the benefit over Baumgarte is small.
-
-Modified NGS is not a robust method for the revolute joint due to the violent
-instability seen in the simple pendulum. Perhaps it is viable with other constraint
-types, especially scalar constraints where the effective mass is a scalar.
-
-This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
-and is very fast. I don't think we can escape Baumgarte, especially in highly
-demanding cases where high constraint fidelity is not needed.
-
-Full NGS is robust and easy on the eyes. I recommend this as an option for
-higher fidelity simulation and certainly for suspension bridges and long chains.
-Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
-joint separation can be problematic. The number of NGS iterations can be reduced
-for better performance without harming robustness much.
-
-Each joint in a can be handled differently in the position solver. So I recommend
-a system where the user can select the algorithm on a per joint basis. I would
-probably default to the slower Full NGS and let the user select the faster
-Baumgarte method in performance critical scenarios.
-*/
-
-/*
-Cache Performance
-
-The Box2D solvers are dominated by cache misses. Data structures are designed
-to increase the number of cache hits. Much of misses are due to random access
-to body data. The constraint structures are iterated over linearly, which leads
-to few cache misses.
-
-The bodies are not accessed during iteration. Instead read only data, such as
-the mass values are stored with the constraints. The mutable data are the constraint
-impulses and the bodies velocities/positions. The impulses are held inside the
-constraint structures. The body velocities/positions are held in compact, temporary
-arrays to increase the number of cache hits. Linear and angular velocity are
-stored in a single array since multiple arrays lead to multiple misses.
-*/
-
-/*
-2D Rotation
-
-R = [cos(theta) -sin(theta)]
-    [sin(theta) cos(theta) ]
-
-thetaDot = omega
-
-Let q1 = cos(theta), q2 = sin(theta).
-R = [q1 -q2]
-    [q2  q1]
-
-q1Dot = -thetaDot * q2
-q2Dot = thetaDot * q1
-
-q1_new = q1_old - dt * w * q2
-q2_new = q2_old + dt * w * q1
-then normalize.
-
-This might be faster than computing sin+cos.
-However, we can compute sin+cos of the same angle fast.
-*/
-
-b2Island::b2Island(
-	int32 bodyCapacity,
-	int32 contactCapacity,
-	int32 jointCapacity,
-	b2StackAllocator* allocator,
-	b2ContactListener* listener)
-{
-	m_bodyCapacity = bodyCapacity;
-	m_contactCapacity = contactCapacity;
-	m_jointCapacity	 = jointCapacity;
-	m_bodyCount = 0;
-	m_contactCount = 0;
-	m_jointCount = 0;
-
-	m_allocator = allocator;
-	m_listener = listener;
-
-	m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
-	m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity	 * sizeof(b2Contact*));
-	m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
-
-	m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
-	m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
-}
-
-b2Island::~b2Island()
-{
-	// Warning: the order should reverse the constructor order.
-	m_allocator->Free(m_positions);
-	m_allocator->Free(m_velocities);
-	m_allocator->Free(m_joints);
-	m_allocator->Free(m_contacts);
-	m_allocator->Free(m_bodies);
-}
-
-void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
-{
-	b2Timer timer;
-
-	float32 h = step.dt;
-
-	// Integrate velocities and apply damping. Initialize the body state.
-	for (int32 i = 0; i < m_bodyCount; ++i)
-	{
-		b2Body* b = m_bodies[i];
-
-		b2Vec2 c = b->m_sweep.c;
-		float32 a = b->m_sweep.a;
-		b2Vec2 v = b->m_linearVelocity;
-		float32 w = b->m_angularVelocity;
-
-		// Store positions for continuous collision.
-		b->m_sweep.c0 = b->m_sweep.c;
-		b->m_sweep.a0 = b->m_sweep.a;
-
-		if (b->m_type == b2_dynamicBody)
-		{
-			// Integrate velocities.
-			v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
-			w += h * b->m_invI * b->m_torque;
-
-			// Apply damping.
-			// ODE: dv/dt + c * v = 0
-			// Solution: v(t) = v0 * exp(-c * t)
-			// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
-			// v2 = exp(-c * dt) * v1
-			// Pade approximation:
-			// v2 = v1 * 1 / (1 + c * dt)
-			v *= 1.0f / (1.0f + h * b->m_linearDamping);
-			w *= 1.0f / (1.0f + h * b->m_angularDamping);
-		}
-
-		m_positions[i].c = c;
-		m_positions[i].a = a;
-		m_velocities[i].v = v;
-		m_velocities[i].w = w;
-	}
-
-	timer.Reset();
-
-	// Solver data
-	b2SolverData solverData;
-	solverData.step = step;
-	solverData.positions = m_positions;
-	solverData.velocities = m_velocities;
-
-	// Initialize velocity constraints.
-	b2ContactSolverDef contactSolverDef;
-	contactSolverDef.step = step;
-	contactSolverDef.contacts = m_contacts;
-	contactSolverDef.count = m_contactCount;
-	contactSolverDef.positions = m_positions;
-	contactSolverDef.velocities = m_velocities;
-	contactSolverDef.allocator = m_allocator;
-
-	b2ContactSolver contactSolver(&contactSolverDef);
-	contactSolver.InitializeVelocityConstraints();
-
-	if (step.warmStarting)
-	{
-		contactSolver.WarmStart();
-	}
-	
-	for (int32 i = 0; i < m_jointCount; ++i)
-	{
-		m_joints[i]->InitVelocityConstraints(solverData);
-	}
-
-	profile->solveInit = timer.GetMilliseconds();
-
-	// Solve velocity constraints
-	timer.Reset();
-	for (int32 i = 0; i < step.velocityIterations; ++i)
-	{
-		for (int32 j = 0; j < m_jointCount; ++j)
-		{
-			m_joints[j]->SolveVelocityConstraints(solverData);
-		}
-
-		contactSolver.SolveVelocityConstraints();
-	}
-
-	// Store impulses for warm starting
-	contactSolver.StoreImpulses();
-	profile->solveVelocity = timer.GetMilliseconds();
-
-	// Integrate positions
-	for (int32 i = 0; i < m_bodyCount; ++i)
-	{
-		b2Vec2 c = m_positions[i].c;
-		float32 a = m_positions[i].a;
-		b2Vec2 v = m_velocities[i].v;
-		float32 w = m_velocities[i].w;
-
-		// Check for large velocities
-		b2Vec2 translation = h * v;
-		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
-		{
-			float32 ratio = b2_maxTranslation / translation.Length();
-			v *= ratio;
-		}
-
-		float32 rotation = h * w;
-		if (rotation * rotation > b2_maxRotationSquared)
-		{
-			float32 ratio = b2_maxRotation / b2Abs(rotation);
-			w *= ratio;
-		}
-
-		// Integrate
-		c += h * v;
-		a += h * w;
-
-		m_positions[i].c = c;
-		m_positions[i].a = a;
-		m_velocities[i].v = v;
-		m_velocities[i].w = w;
-	}
-
-	// Solve position constraints
-	timer.Reset();
-	bool positionSolved = false;
-	for (int32 i = 0; i < step.positionIterations; ++i)
-	{
-		bool contactsOkay = contactSolver.SolvePositionConstraints();
-
-		bool jointsOkay = true;
-		for (int32 i = 0; i < m_jointCount; ++i)
-		{
-			bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
-			jointsOkay = jointsOkay && jointOkay;
-		}
-
-		if (contactsOkay && jointsOkay)
-		{
-			// Exit early if the position errors are small.
-			positionSolved = true;
-			break;
-		}
-	}
-
-	// Copy state buffers back to the bodies
-	for (int32 i = 0; i < m_bodyCount; ++i)
-	{
-		b2Body* body = m_bodies[i];
-		body->m_sweep.c = m_positions[i].c;
-		body->m_sweep.a = m_positions[i].a;
-		body->m_linearVelocity = m_velocities[i].v;
-		body->m_angularVelocity = m_velocities[i].w;
-		body->SynchronizeTransform();
-	}
-
-	profile->solvePosition = timer.GetMilliseconds();
-
-	Report(contactSolver.m_velocityConstraints);
-
-	if (allowSleep)
-	{
-		float32 minSleepTime = b2_maxFloat;
-
-		const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
-		const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
-
-		for (int32 i = 0; i < m_bodyCount; ++i)
-		{
-			b2Body* b = m_bodies[i];
-			if (b->GetType() == b2_staticBody)
-			{
-				continue;
-			}
-
-			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
-				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
-				b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
-			{
-				b->m_sleepTime = 0.0f;
-				minSleepTime = 0.0f;
-			}
-			else
-			{
-				b->m_sleepTime += h;
-				minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
-			}
-		}
-
-		if (minSleepTime >= b2_timeToSleep && positionSolved)
-		{
-			for (int32 i = 0; i < m_bodyCount; ++i)
-			{
-				b2Body* b = m_bodies[i];
-				b->SetAwake(false);
-			}
-		}
-	}
-}
-
-void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
-{
-	b2Assert(toiIndexA < m_bodyCount);
-	b2Assert(toiIndexB < m_bodyCount);
-
-	// Initialize the body state.
-	for (int32 i = 0; i < m_bodyCount; ++i)
-	{
-		b2Body* b = m_bodies[i];
-		m_positions[i].c = b->m_sweep.c;
-		m_positions[i].a = b->m_sweep.a;
-		m_velocities[i].v = b->m_linearVelocity;
-		m_velocities[i].w = b->m_angularVelocity;
-	}
-
-	b2ContactSolverDef contactSolverDef;
-	contactSolverDef.contacts = m_contacts;
-	contactSolverDef.count = m_contactCount;
-	contactSolverDef.allocator = m_allocator;
-	contactSolverDef.step = subStep;
-	contactSolverDef.positions = m_positions;
-	contactSolverDef.velocities = m_velocities;
-	b2ContactSolver contactSolver(&contactSolverDef);
-
-	// Solve position constraints.
-	for (int32 i = 0; i < subStep.positionIterations; ++i)
-	{
-		bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
-		if (contactsOkay)
-		{
-			break;
-		}
-	}
-
-#if 0
-	// Is the new position really safe?
-	for (int32 i = 0; i < m_contactCount; ++i)
-	{
-		b2Contact* c = m_contacts[i];
-		b2Fixture* fA = c->GetFixtureA();
-		b2Fixture* fB = c->GetFixtureB();
-
-		b2Body* bA = fA->GetBody();
-		b2Body* bB = fB->GetBody();
-
-		int32 indexA = c->GetChildIndexA();
-		int32 indexB = c->GetChildIndexB();
-
-		b2DistanceInput input;
-		input.proxyA.Set(fA->GetShape(), indexA);
-		input.proxyB.Set(fB->GetShape(), indexB);
-		input.transformA = bA->GetTransform();
-		input.transformB = bB->GetTransform();
-		input.useRadii = false;
-
-		b2DistanceOutput output;
-		b2SimplexCache cache;
-		cache.count = 0;
-		b2Distance(&output, &cache, &input);
-
-		if (output.distance == 0 || cache.count == 3)
-		{
-			cache.count += 0;
-		}
-	}
-#endif
-
-	// Leap of faith to new safe state.
-	m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
-	m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
-	m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
-	m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
-
-	// No warm starting is needed for TOI events because warm
-	// starting impulses were applied in the discrete solver.
-	contactSolver.InitializeVelocityConstraints();
-
-	// Solve velocity constraints.
-	for (int32 i = 0; i < subStep.velocityIterations; ++i)
-	{
-		contactSolver.SolveVelocityConstraints();
-	}
-
-	// Don't store the TOI contact forces for warm starting
-	// because they can be quite large.
-
-	float32 h = subStep.dt;
-
-	// Integrate positions
-	for (int32 i = 0; i < m_bodyCount; ++i)
-	{
-		b2Vec2 c = m_positions[i].c;
-		float32 a = m_positions[i].a;
-		b2Vec2 v = m_velocities[i].v;
-		float32 w = m_velocities[i].w;
-
-		// Check for large velocities
-		b2Vec2 translation = h * v;
-		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
-		{
-			float32 ratio = b2_maxTranslation / translation.Length();
-			v *= ratio;
-		}
-
-		float32 rotation = h * w;
-		if (rotation * rotation > b2_maxRotationSquared)
-		{
-			float32 ratio = b2_maxRotation / b2Abs(rotation);
-			w *= ratio;
-		}
-
-		// Integrate
-		c += h * v;
-		a += h * w;
-
-		m_positions[i].c = c;
-		m_positions[i].a = a;
-		m_velocities[i].v = v;
-		m_velocities[i].w = w;
-
-		// Sync bodies
-		b2Body* body = m_bodies[i];
-		body->m_sweep.c = c;
-		body->m_sweep.a = a;
-		body->m_linearVelocity = v;
-		body->m_angularVelocity = w;
-		body->SynchronizeTransform();
-	}
-
-	Report(contactSolver.m_velocityConstraints);
-}
-
-void b2Island::Report(const b2ContactVelocityConstraint* constraints)
-{
-	if (m_listener == NULL)
-	{
-		return;
-	}
-
-	for (int32 i = 0; i < m_contactCount; ++i)
-	{
-		b2Contact* c = m_contacts[i];
-
-		const b2ContactVelocityConstraint* vc = constraints + i;
-		
-		b2ContactImpulse impulse;
-		impulse.count = vc->pointCount;
-		for (int32 j = 0; j < vc->pointCount; ++j)
-		{
-			impulse.normalImpulses[j] = vc->points[j].normalImpulse;
-			impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;
-		}
-
-		m_listener->PostSolve(c, &impulse);
-	}
-}