/*
	Trampoline Sim.
	Copyright (c) 2009, Darragh Coy.
	All rights reserved.
	Original work(s), source code & author(s) details available at: http://www.darraghcoy.com

	Redistribution and use in source and binary forms, with or without modification, are permitted 
	provided that the following conditions are met:

		* 	Redistributions of source code must retain the above copyright notice, this list of 
			conditions and the following disclaimer.
		* 	Redistributions in binary form must reproduce the above copyright notice, this list of 
			conditions and the following disclaimer in the documentation.
		* 	Neither the name of the author nor the names of contributors may be used to endorse or 
			promote products derived from this software without specific prior written permission.
		* 	The use of any third party libraries, modules or data files provided with the source 
			form of the program must follow and respect the individual licensing terms of these 
			components. You do not have permission to modify or circumvent the licensing terms for 
			provided third party components unless the third party licenses specifically allow this. 
			If you do not agree to the terms of provided third party components then you do not have 
			permission to use them in your derived work(s).
		* 	The terms of this license do not apply to separate (but related) distributions if supplied 
			by the author(s), only to the contents of this source distribution, derived works, and the 
			binaries produced by either modified or unmodified versions of the source code. The terms 
			of this license do not also apply to any third party libraries or components provided with 
			the source distribution. Such third party elements are licensed under their own separate 
			agreements.

	THIS SOFTWARE IS PROVIDED BY DARRAGH COY ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 
	BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
	ARE DISCLAIMED. IN NO EVENT SHALL DARRAGH COY BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
	EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 
	OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY 
	OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
	IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

//=====================================================================================================================
/// @brief		This class is a base class for rigid and soft bodies. It contains all properties common to both,
///				though some properties may be ignored in the case of soft bodies.
//=====================================================================================================================

#include "Precompiled.h"
#include "Physics/Body.h"

using namespace Game;

//=====================================================================================================================
// Statics
//=====================================================================================================================

/// This is the amount of gravitational acceleration appplied to all bodies

Vec3f Body::s_gravity = Vec3f( 0 , - 9.81f * 5.0f, 0 );

//=====================================================================================================================
/// @brief		Creates the rigid body
/// @param		shape - shape used by the rigid body
/// @param		mass - mass of the rigid body
/// @param		inertia_tensor - inertia tensor for the rigid body
/// @param		restitution - coefficient of restitution which affects collision response
/// @param		restitution_cutoff - when resolving impulses at contact points, this is the velocity of the contact point 
///				under which we begin to scale back (linearly) on the coefficient of restitution. It is used to help
///				stabalise micro-collisions at contact points and reduce the apparent vibration caused by such collisions.
/// @param		static_friction - this is the coefficient of static friction. This affects how much force is needed to
///				move the body from rest.
/// @param		dynamic_friction - dynamic friction coefficient. This affects how much dynamic friction is applied during
///				motion to the rigid body
/// @pre		Given shape cannot be null
/// @remarks	The rigid body takes ownership of the shape and will delete it when deleted
//=====================================================================================================================

Body::Body
( 
	Shape *				shape				,
	bool				moveable			,
	float				mass				,
	const Matrix3f &	inertia_tensor		,
	float				restitution			,
	float				restitution_cutoff	,
	float				static_friction		,
	float				dynamic_friction	,
	float				linear_dampening	, 
	float				angular_dampening
)
{
	ENSURE( shape != 0 , "Given shape cannot be null" )
	
	// Save shape:
	
	m_shape = shape;
	
	// Initialise these:
	
	ResetAccelerations();
	ResetVelocities();
	
	// Set all the rest of the variables

	SetMoveable(moveable);
	SetMass(mass);
	SetRestitution(restitution);
	SetRestitutionCutoff(restitution_cutoff);
	SetStaticFriction(static_friction);
	SetDynamicFriction(dynamic_friction);
	SetDampening(linear_dampening);
	SetAngularDampening(angular_dampening);
	SetInertiaTensor(inertia_tensor);
	SetWorldPosition(shape->GetPosition());
	SetWorldOrientation(shape->GetOrientation());
}

//=====================================================================================================================
/// @brief		Destroys the body
//=====================================================================================================================

Body::~Body(){ delete m_shape; }

//=====================================================================================================================
/// @brief		Performs collision detection and response between two dynamic objects.
/// @param		obj1 - first object in the collision detection. 
/// @param		obj2 - second object in the collision detection.
/// @param		time_delta - amount of time in between frames
/// @pre		Given objects cannot be null
/// @pre		Obj1 must be moveable
/// @pre		Obj2 must be moveable
/// @pre		time delta must be > 0
//=====================================================================================================================

void Body::DynamicDynamicCollision( Body & obj1 , Body & obj2 , float time_delta )
{
	//-----------------------------------------------------------------------------------------------------------------
	// Make sure ok:
	//-----------------------------------------------------------------------------------------------------------------
	
	ENSURE( &obj1						, "Given objects cannot be null"	)
	ENSURE( &obj2						, "Given objects cannot be null"	)
	ENSURE( obj1.IsMoveable() == true	, "Obj1 must be moveable"			)
	ENSURE( obj2.IsMoveable() == true	, "Obj2 must be moveable"			)
	ENSURE( time_delta > 0				, "time delta must be > 0"			)
	
	//-----------------------------------------------------------------------------------------------------------------
	// Collision detection
	//-----------------------------------------------------------------------------------------------------------------
	
	// Get a list of contact points with the other shape:

	std::vector<Contact> contacts; obj1.GetShape().GenerateContacts(obj2.GetShape(),contacts);
	
	// Sort the contacts based on penetration depth:
	
	std::sort(contacts.begin(),contacts.end());
	
	// Save the original linear and angular velocity of the objects
	
	Vec3f velocity_original1			= obj1.GetWorldVelocity();
	Vec3f velocity_original2			= obj2.GetWorldVelocity();
	Vec3f angular_velocity_original1	= obj1.GetWorldAngularVelocity();
	Vec3f angular_velocity_original2	= obj2.GetWorldAngularVelocity();
	
	//-----------------------------------------------------------------------------------------------------------------
	// Apply impulses
	//-----------------------------------------------------------------------------------------------------------------
	
	// Other object is static: run through all of the contact points:

	for ( unsigned i = 0 ; i < contacts.size() ; ++i )
	{
		// Get this contact

		Contact & c = contacts[i];
		
		// Get a matrix to transform to contact space
		
		Matrix3f to_contact_space = c.GetContactSpaceOrientationMatrix();
		
		// Get a matrix to transform from contact space
		
		Matrix3f from_contact_space = to_contact_space.Transpose();
		
		// Get the position of the contact relative to the bodies center of mass
		
		Vec3f relative_contact_pos1 = c.m_position - obj1.GetWorldPosition();
		Vec3f relative_contact_pos2 = c.m_position - obj2.GetWorldPosition();
		
		// Get the amount of velocity produced per unit impulse in contact space
		
		Matrix3f velocity_per_unit_impulse_contact1 = GetContactImpulseToVelocityMatrix
		(
			c					,
			obj1				,
			from_contact_space	,
			to_contact_space
		);
		
		Matrix3f velocity_per_unit_impulse_contact2 = GetContactImpulseToVelocityMatrix
		(
			c					,
			obj2				,
			from_contact_space	,
			to_contact_space
		);
		
		// Get the total velocity produced across the two bodies per unit impulse:
		
		Matrix3f velocity_per_unit_impulse_contact_combined;
		
		for ( unsigned r = 0 ; r < 3 ; ++r )
		for ( unsigned c = 0 ; c < 3 ; ++c )
		{
			velocity_per_unit_impulse_contact_combined.m_components[r][c] = 
				
				velocity_per_unit_impulse_contact1.m_components[r][c] +
				velocity_per_unit_impulse_contact2.m_components[r][c];
		}
		
		// Get the inverse of this matrix: we will need it later
		
		Matrix3f velocity_per_unit_impulse_contact_inverse = velocity_per_unit_impulse_contact_combined.Inverse();
		
		// Get the closing velocity in contact coordinates. This is the velocity at which we are approaching the object;
		
		Vec3f closing_velocity_contact;
		
		{
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Get the velocity of the object, minus the velocity produced by the last frames acceleration. The 
			// reason why the last frame's acceleration is ignored is to try and prevent micro-collisions when 
			// objects are resting on the ground. We ignore minor velocity buildup due to gravity because it is 
			// not physically possible to accelerate through the ground, and such velocity will never get the 
			// chance to buildup in the first place.. This helps to stabalise resting contacts. 
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			/*Vec3f lv = 
				
				( velocity_original1 - ( obj1.GetPreviousWorldAcceleration() * obj1.GetInverseMass() ) )
				-
				( velocity_original2 - ( obj2.GetPreviousWorldAcceleration() * obj2.GetInverseMass() ) );
			*/

			// THIS METHOD ACTUALLY APPEARS MORE STABLE FOR TWO DYNAMIC BODIES:

			Vec3f lv = velocity_original1 - velocity_original2;
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Now calculate the closing velocity of the contact point in contact coordinates, taking into account 
			// both velocity produced linearly and by rotation.. 
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			closing_velocity_contact = to_contact_space.Transform
			(
				lv +
				angular_velocity_original1.Cross(relative_contact_pos1) -
				angular_velocity_original2.Cross(relative_contact_pos2)
			);
		}
		
		// Only do the rest if we are actually closing in on the point:
		
		if ( closing_velocity_contact.x < 0 )
		{	
			// Calculate the coefficient of restitution we will use
			
			float restitution = ( obj2.GetRestitution() + obj1.GetRestitution() ) * 0.5f;
			
			// Calculate the value we will use for restitution cutoff
			
			float restitution_cutoff = ( obj1.GetRestitutionCutoff() + obj2.GetRestitutionCutoff() ) * 0.5f;

			// Calculate the level of static friction:
			
			float static_friction = ( obj1.GetStaticFriction() + obj2.GetStaticFriction() ) * 0.5f;
			
			// Calculate the level of dynamic friction:
			
			float dynamic_friction = ( obj1.GetDynamicFriction() + obj2.GetDynamicFriction() ) * 0.5f;
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if we are under the cutoff velocity where we begin to scale back the coefficient of 
			// restitution. This is done to stablise small scale collisions:
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			if ( - closing_velocity_contact.x < restitution_cutoff )
			{
				// Begin to scale back on restitution:
				
				restitution *= - closing_velocity_contact.x / restitution_cutoff;
			}
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''	
			// Calculate the required change in velocity along the normal: N.B if the coefficient of restitution 
			// is zero then all velocity will be removed
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			Vec3f contact_velocity_change = Vec3f
			( 
				- closing_velocity_contact.x * ( 1.0f + restitution )	,
				0														,
				0
			);
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if we are to apply static or dynamic friction from the reaction force acting on the contact 
			// normal. The reaction force is approximated by using the total impulse required to remove all 
			// velocity along the normal.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			// Get the reaction force at the normal:
			
			float reaction_force = - closing_velocity_contact.x * ( obj1.GetMass() + obj2.GetMass() ) / time_delta;
			
			// Calculate the force that is being applied to create movement along the contact point's collision plane:
			
			float planar_force = Float::Sqrt
			( 
				closing_velocity_contact.y * closing_velocity_contact.y +
				closing_velocity_contact.z * closing_velocity_contact.z 
			);
			
			planar_force *= ( obj1.GetMass() + obj2.GetMass() ) / time_delta;
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if static or dynamic friction applies:
			// Static friction applies if the planar force being applied to the body is greater than a certain
			// amount of the of the reaction force at the contact normal; in other forces
			// 
			// pf <= u * rf
			//
			// Where 
			// 
			// pf = the force being applied along the contact plane
			// rf = the reaction force applied at the contact point along the contact normal
			// u  = the coefficient of static friction
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			if ( planar_force <= reaction_force * static_friction )
			{
				// Apply static friction to remove all velocity along the plane directions:
				
				contact_velocity_change.y = - closing_velocity_contact.y;
				contact_velocity_change.z = - closing_velocity_contact.z;
			}
			else
			{
				// Calculate the direction in which we will apply static friction along the plane
				
				Vec2f friction_direction = Vec2f
				(
					- closing_velocity_contact.y ,
					- closing_velocity_contact.z
				);
				
				friction_direction.Normalize();
				
				//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
				// The amount of dynamic friction is calculated as follows:
				// 
				// ff = pd * ( u * rf )
				// 
				// Where
				//
				// ff = the friction force applied
				// pd = the direction of movement along the plane
				// u  = the coefficient of dynamic friction
				// rf = the reaction force at the plane normal.
				//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
				
				Vec2f friction_force = friction_direction * ( reaction_force * dynamic_friction );
				
				// Figure out the change in velocity we want to produce by first converting to an impulse, then scaling by inverse mass
				
				Vec2f friction_velocity_change = ( friction_force * time_delta ) * ( obj1.GetInverseMass() + obj2.GetInverseMass() );
				
				// Add to the velocity change we want:
				
				contact_velocity_change.y = friction_velocity_change.x;
				contact_velocity_change.z = friction_velocity_change.y;
			}
			
			// Calculate the required impulse to produce the required velocity change in contact space:
			
			Vec3f impulse_contact = velocity_per_unit_impulse_contact_inverse.Transform(contact_velocity_change);
			
			// Transform out of contact space again into world coordinates
			
			Vec3f impulse_world = from_contact_space.Transform(impulse_contact);
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Apply the impulse at the point of contact: scale back with the number of contact points in attempt
			// to approximate a fully correct contact solver that simulatenously resolves all forces at all of the 
			// contact points.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			obj1.ApplyWorldImpulse( + impulse_world  / float( contacts.size() ) , c.m_position );
			obj2.ApplyWorldImpulse( - impulse_world  / float( contacts.size() ) , c.m_position );
		}
	}
	
	//-----------------------------------------------------------------------------------------------------------------
	// Resolve collisions
	//-----------------------------------------------------------------------------------------------------------------
	
	// Take a number of iterations to solve all the penetrations
	
	unsigned iteraions = contacts.size();
	
	// Get the total inverse mass of both objects:
	
	float total_inverse_mass = 1.0f / ( obj1.GetMass() + obj2.GetMass() );
	
	// Now resolve all collisions

	for ( unsigned i = 0 ; i < iteraions && contacts.size() > 0 ; ++i )
	{
		// Get this contact

		Contact & c = contacts[0];

		// Resolve this collision: move both objects back, but weight by the mass
	
		obj1.SetWorldPosition( obj1.GetWorldPosition() + c.m_normal * c.m_penetration * ( obj1.GetMass() * total_inverse_mass ) );
		obj2.SetWorldPosition( obj2.GetWorldPosition() - c.m_normal * c.m_penetration * ( obj2.GetMass() * total_inverse_mass ) );
				
		// Clear the list of contacts:
		
		contacts.clear();
		
		// Regenerate contacts: objects have moved
		
		obj1.GetShape().GenerateContacts(obj2.GetShape(),contacts);
		
		// Sort the contacts based on penetration depth:
		
		std::sort(contacts.begin(),contacts.end());		
	}
}

//=====================================================================================================================
/// @brief		Performs collision detection and response between a static and dynamic object. Obj1 is static.
/// @param		obj1 - first object in the collision detection. 
/// @param		obj2 - second object in the collision detection.
/// @param		time_delta - amount of time in between frames
/// @pre		Given objects cannot be null
/// @pre		Obj1 must not be moveable
/// @pre		Obj2 must be moveable
/// @pre		time delta must be > 0
//=====================================================================================================================

void Body::StaticDynamicCollision( Body & obj1 , Body & obj2 , float time_delta )
{
	//-----------------------------------------------------------------------------------------------------------------
	// Make sure ok:
	//-----------------------------------------------------------------------------------------------------------------
	
	ENSURE( &obj1						, "Given objects cannot be null"	)
	ENSURE( &obj2						, "Given objects cannot be null"	)
	ENSURE( obj1.IsMoveable() == false	, "Obj1 must not be moveable"		)
	ENSURE( obj2.IsMoveable() == true	, "Obj2 must be moveable"			)
	ENSURE( time_delta > 0				, "time delta must be > 0"			)
	
	//-----------------------------------------------------------------------------------------------------------------
	// Collision detection
	//-----------------------------------------------------------------------------------------------------------------
	
	// Get a list of contact points with the other shape:

	std::vector<Contact> contacts; obj1.GetShape().GenerateContacts(obj2.GetShape(),contacts);
	
	// Sort the contacts based on penetration depth:
	
	std::sort(contacts.begin(),contacts.end());
	
	// Save the original linear and angular velocity of the object
	
	Vec3f velocity_original			= obj2.GetWorldVelocity();
	Vec3f angular_velocity_original = obj2.GetWorldAngularVelocity();
	
	//-----------------------------------------------------------------------------------------------------------------
	// Apply impulses
	//-----------------------------------------------------------------------------------------------------------------
	
	// Other object is static: run through all of the contact points:

	for ( unsigned i = 0 ; i < contacts.size() ; ++i )
	{
		// Get this contact

		Contact & c = contacts[i];
		
		// Get a matrix to transform to contact space
		
		Matrix3f to_contact_space = c.GetContactSpaceOrientationMatrix();
		
		// Get a matrix to transform from contact space
		
		Matrix3f from_contact_space = to_contact_space.Transpose();
		
		// Get the position of the contact relative to the body's center of mass
		
		Vec3f relative_contact_pos = c.m_position - obj2.GetWorldPosition();		
		
		// Get the amount of velocity produced per unit impulse in contact space
		
		Matrix3f velocity_per_unit_impulse_contact = GetContactImpulseToVelocityMatrix
		(
			c					,
			obj2				,
			from_contact_space	,
			to_contact_space
		);

		// Get mass:

		float mass = obj2.GetMass();

		// If dealing with a soft body also then account for it's mass:

		if ( obj1.IsSoftBody() )
		{
			float inv_mass = obj1.GetInverseMass();

			velocity_per_unit_impulse_contact.m_components[0][0] += inv_mass;
			velocity_per_unit_impulse_contact.m_components[1][1] += inv_mass;
			velocity_per_unit_impulse_contact.m_components[2][2] += inv_mass;

			mass += obj1.GetMass();
		}

		// Get inverse mass:

		float inv_mass = 1.0f / mass;
		
		// Get the inverse of this matrix: we will need it later
		
		Matrix3f velocity_per_unit_impulse_contact_inverse = velocity_per_unit_impulse_contact.Inverse();
		
		// Get the closing velocity in contact coordinates. This is the velocity at which we are approaching the object;
		
		Vec3f closing_velocity_contact;
		
		{
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Get the velocity of the object, minus the velocity produced by the last frames acceleration. The 
			// reason why the last frame's acceleration is ignored is to try and prevent micro-collisions when 
			// objects are resting on the ground. We ignore minor velocity buildup due to gravity because it is 
			// not physically possible to accelerate through the ground, and such velocity will never get the 
			// chance to buildup in the first place.. This helps to stabalise resting contacts. 
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			Vec3f lv = velocity_original - ( obj2.GetPreviousWorldAcceleration() * inv_mass );

			// Add the deformation velocity of the vertex to the veloctity: this is for soft bodies. 

			if ( obj1.IsSoftBody() ) lv -= c.m_deformation_velocity;
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Now calculate the closing velocity of the contact point in contact coordinates, taking into account 
			// both velocity produced linearly and by rotation.. 
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			closing_velocity_contact = to_contact_space.Transform
			(
				lv + angular_velocity_original.Cross(relative_contact_pos)
			);
		}
		
		// Only do the rest if we are actually closing in on the point:
		
		if ( closing_velocity_contact.x < 0 )
		{	
			// Calculate the coefficient of restitution we will use
			
			float restitution = obj2.GetRestitution();

			// N.B: If the second body is a soft body then use a very low restitution:

			if ( obj1.IsSoftBody() ) restitution *= 0.01f;
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if we are under the cutoff velocity where we begin to scale back the coefficient of 
			// restitution. This is done to stablise small scale collisions:
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			if ( - closing_velocity_contact.x < obj2.GetRestitutionCutoff() )
			{
				// Begin to scale back on restitution:
				
				restitution *= - closing_velocity_contact.x / obj2.GetRestitutionCutoff();
			}
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''	
			// Calculate the required change in velocity along the normal: N.B if the coefficient of restitution 
			// is zero then all velocity will be removed
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			Vec3f contact_velocity_change = Vec3f
			( 
				- closing_velocity_contact.x * ( 1.0f + restitution )	,
				0														,
				0
			);
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if we are to apply static or dynamic friction from the reaction force acting on the contact 
			// normal. The reaction force is approximated by using the total impulse required to remove all 
			// velocity along the normal.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			// Get the reaction force at the normal: see if dealing with a soft body though
			
			float reaction_force = - closing_velocity_contact.x * mass / time_delta;
			
			// Calculate the force that is being applied to create movement along the contact point's collision plane:
			
			float planar_force = Float::Sqrt
			( 
				closing_velocity_contact.y * closing_velocity_contact.y +
				closing_velocity_contact.z * closing_velocity_contact.z 
			);
			
			planar_force *= mass / time_delta;
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if static or dynamic friction applies:
			// Static friction applies if the planar force being applied to the body is greater than a certain
			// amount of the of the reaction force at the contact normal; in other forces
			// 
			// pf <= u * rf
			//
			// Where 
			// 
			// pf = the force being applied along the contact plane
			// rf = the reaction force applied at the contact point along the contact normal
			// u  = the coefficient of static friction
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			if ( planar_force <= reaction_force * obj2.GetStaticFriction() )
			{
				// Apply static friction to remove all velocity along the plane directions:
				
				contact_velocity_change.y = - closing_velocity_contact.y;
				contact_velocity_change.z = - closing_velocity_contact.z;
			}
			else
			{
				// Calculate the direction in which we will apply static friction along the plane
				
				Vec2f friction_direction = Vec2f
				(
					- closing_velocity_contact.y ,
					- closing_velocity_contact.z
				);
				
				friction_direction.Normalize();
				
				//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
				// The amount of dynamic friction is calculated as follows:
				// 
				// ff = pd * ( u * rf )
				// 
				// Where
				//
				// ff = the friction force applied
				// pd = the direction of movement along the plane
				// u  = the coefficient of dynamic friction
				// rf = the reaction force at the plane normal.
				//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
				
				Vec2f friction_force = friction_direction * ( reaction_force * obj2.GetDynamicFriction() );
				
				// Figure out the change in velocity we want to produce by first converting to an impulse, then scaling by inverse mass
				
				Vec2f friction_velocity_change = ( friction_force * time_delta ) * inv_mass;
				
				// Add to the velocity change we want:
				
				contact_velocity_change.y = friction_velocity_change.x;
				contact_velocity_change.z = friction_velocity_change.y;
			}
			
			// Calculate the required impulse to produce the required velocity change in contact space:
			
			Vec3f impulse_contact = velocity_per_unit_impulse_contact_inverse.Transform(contact_velocity_change);
			
			// Transform out of contact space again into world coordinates
			
			Vec3f impulse_world = from_contact_space.Transform(impulse_contact);
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Apply the impulse at the point of contact: scale back with the number of contact points in attempt
			// to approximate a fully correct contact solver that simulatenously resolves all forces at all of the 
			// contact points.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			obj2.ApplyWorldImpulse( impulse_world  / float( contacts.size() ) , c.m_position );

			// If the 1st object is a soft body then apply an impulse also:

			if ( obj1.IsSoftBody() )
			{
				obj1.ApplyWorldImpulse( - impulse_world  / float( contacts.size() ) , c.m_position );
			}
		}
	}
	
	//=============================================================================================================
	// Resolve collisions
	//=============================================================================================================
	
	// Take a number of iterations to solve all the penetrations
	
	unsigned iteraions = contacts.size();
	
	// Now resolve all collisions

	for ( unsigned i = 0 ; i < iteraions && contacts.size() > 0 ; ++i )
	{
		// Get this contact

		Contact & c = contacts[0];
	
		// Resolve this collision:
	
		obj2.SetWorldPosition( obj2.GetWorldPosition() + c.m_normal * c.m_penetration );
				
		// Clear the list of contacts:
		
		contacts.clear();
		
		// Regenerate contacts: object has moved
		
		obj1.GetShape().GenerateContacts(obj2.GetShape(),contacts);
		
		// Sort the contacts based on penetration depth:
		
		std::sort(contacts.begin(),contacts.end());		
	}
}

/*
void Body::StaticDynamicCollision( Body & obj1 , Body & obj2 , float time_delta )
{
	//-----------------------------------------------------------------------------------------------------------------
	// Make sure ok:
	//-----------------------------------------------------------------------------------------------------------------
	
	ENSURE( &obj1						, "Given objects cannot be null"	)
	ENSURE( &obj2						, "Given objects cannot be null"	)
	ENSURE( obj1.IsMoveable() == false	, "Obj1 must not be moveable"		)
	ENSURE( obj2.IsMoveable() == true	, "Obj2 must not moveable"			)
	ENSURE( time_delta > 0				, "time delta must be > 0"			)
	
	//-----------------------------------------------------------------------------------------------------------------
	// Collision detection
	//-----------------------------------------------------------------------------------------------------------------
	
	// Get a list of contact points with the other shape:

	std::vector<Contact> contacts; obj1.GetShape().GenerateContacts(obj2.GetShape(),contacts);
	
	// Sort the contacts based on penetration depth:
	
	std::sort(contacts.begin(),contacts.end());
	
	// Save the original linear and angular velocity of the object
	
	Vec3f velocity_original			= obj2.GetWorldVelocity();
	Vec3f angular_velocity_original = obj2.GetWorldAngularVelocity();
	
	//-----------------------------------------------------------------------------------------------------------------
	// Apply impulses
	//-----------------------------------------------------------------------------------------------------------------
	
	// Other object is static: run through all of the contact points:

	for ( unsigned i = 0 ; i < contacts.size() ; ++i )
	{
		// Get this contact

		Contact & c = contacts[i];
		
		// Get a matrix to transform to contact space
		
		Matrix3f to_contact_space = c.GetContactSpaceOrientationMatrix();
		
		// Get a matrix to transform from contact space
		
		Matrix3f from_contact_space = to_contact_space.Transpose();
		
		// Get the position of the contact relative to the body's center of mass
		
		Vec3f relative_contact_pos = c.m_position - obj2.GetWorldPosition();
		
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		// Create a skew symmetric matrix that is the equivalent of a cross product with the relative_contact_pos
		// variable. This is used to create a matrix that will transform a given vector in contact space into units
		// of torque.
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		
		Matrix3f cross_relative_contact_pos = Matrix3f::SkewSymmetric
		(
			relative_contact_pos.x,
			relative_contact_pos.y,
			relative_contact_pos.z
		);
		
		// We need the reverse of this matrix also
		
		Matrix3f reverse_cross_relative_contact_pos = Matrix3f::ReverseSkewSymmetric
		(
			relative_contact_pos.x,
			relative_contact_pos.y,
			relative_contact_pos.z
		);	
		
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		// Create a matrix that gives units of torque in world space, given a vector representing impulse. This 
		// vector can be in any direction in world space, not just along the contact normal. We use the previous 
		// skew-symetric matrix to generate this matrix, which performs the matrix equivalent of the cross product 
		// with the relative contact position. The to_contact_space matrix contains the three axis vectors of 
		// contact space in each column of the matrix.
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		
		Matrix3f torque_per_unit_impulse = cross_relative_contact_pos * to_contact_space;
		
		// Calculate the change in angular velocity per unit impulse along each of the three contact space axes
		
		Matrix3f delta_angular_velocity_per_unit_impulse = obj2.GetInverseInertiaTensor() * torque_per_unit_impulse;
		
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		// Calculate the amount of rotational induced velocity generated produced per unit of impulse along each of 
		// the three contact space axis vectors:
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		
		Matrix3f velocity_per_unit_impulse = reverse_cross_relative_contact_pos * delta_angular_velocity_per_unit_impulse;
		
		// Transform this velocity into contact space:
		
		Matrix3f velocity_per_unit_impulse_contact = from_contact_space * velocity_per_unit_impulse;
		
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		// The previous matix only takes into account velocity produced by rotation, we need to also take into
		// account ordinary linear velocity also. Since linear velocity scales equally in all the contact space 
		// directions, we only need to add the inverse mass to the x,y,z coords of the three column vectors of
		// this matrix in order to take account of linear velocity. Remember:
		//
		// force = mass * acceleration
		//
		// So:
		//
		// acceleration_per_unit_force = 1.0f / mass
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		
		velocity_per_unit_impulse_contact.m_components[0][0] += obj2.GetInverseMass();
		velocity_per_unit_impulse_contact.m_components[1][1] += obj2.GetInverseMass();
		velocity_per_unit_impulse_contact.m_components[2][2] += obj2.GetInverseMass();
		
		// Get the inverse of this matrix: we will need it later
		
		Matrix3f velocity_per_unit_impulse_contact_inverse = velocity_per_unit_impulse_contact.Inverse();
		
		// Get the closing velocity in contact coordinates. This is the velocity at which we are approaching the object;
		
		Vec3f closing_velocity_contact;
		
		{
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Get the velocity of the object, minus the velocity produced by the last frames acceleration. The 
			// reason why the last frame's acceleration is ignored is to try and prevent micro-collisions when 
			// objects are resting on the ground. We ignore minor velocity buildup due to gravity because it is 
			// not physically possible to accelerate through the ground, and such velocity will never get the 
			// chance to buildup in the first place.. This helps to stabalise resting contacts. 
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			Vec3f lv = velocity_original - ( obj2.GetPreviousWorldAcceleration() * obj2.GetInverseMass() );
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Now calculate the closing velocity of the contact point in contact coordinates, taking into account 
			// both velocity produced linearly and by rotation.. 
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			closing_velocity_contact = to_contact_space.Transform
			(
				lv + angular_velocity_original.Cross(relative_contact_pos)
			);
		}
		
		// Only do the rest if we are actually closing in on the point:
		
		if ( closing_velocity_contact.x < 0 )
		{	
			// Calculate the coefficient of restitution we will use
			
			float restitution = obj2.GetRestitution();
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if we are under the cutoff velocity where we begin to scale back the coefficient of 
			// restitution. This is done to stablise small scale collisions:
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			if ( - closing_velocity_contact.x < obj2.GetRestitutionCutoff() )
			{
				// Begin to scale back on restitution:
				
				restitution *= - closing_velocity_contact.x / obj2.GetRestitutionCutoff();
			}
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''	
			// Calculate the required change in velocity along the normal: N.B if the coefficient of restitution 
			// is zero then all velocity will be removed
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			Vec3f contact_velocity_change = Vec3f
			( 
				- closing_velocity_contact.x * ( 1.0f + restitution )	,
				0														,
				0
			);
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if we are to apply static or dynamic friction from the reaction force acting on the contact 
			// normal. The reaction force is approximated by using the total impulse required to remove all 
			// velocity along the normal.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			// Get the reaction force at the normal:
			
			float reaction_force = - closing_velocity_contact.x * obj2.GetMass() / time_delta;
			
			// Calculate the force that is being applied to create movement along the contact point's collision plane:
			
			float planar_force = Float::Sqrt
			( 
				closing_velocity_contact.y * closing_velocity_contact.y +
				closing_velocity_contact.z * closing_velocity_contact.z 
			);
			
			planar_force *= obj2.GetMass() / time_delta;
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// See if static or dynamic friction applies:
			// Static friction applies if the planar force being applied to the body is greater than a certain
			// amount of the of the reaction force at the contact normal; in other forces
			// 
			// pf <= u * rf
			//
			// Where 
			// 
			// pf = the force being applied along the contact plane
			// rf = the reaction force applied at the contact point along the contact normal
			// u  = the coefficient of static friction
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			if ( planar_force <= reaction_force * obj2.GetStaticFriction() )
			{
				// Apply static friction to remove all velocity along the plane directions:
				
				contact_velocity_change.y = - closing_velocity_contact.y;
				contact_velocity_change.z = - closing_velocity_contact.z;
			}
			else
			{
				// Calculate the direction in which we will apply static friction along the plane
				
				Vec2f friction_direction = Vec2f
				(
					- closing_velocity_contact.y ,
					- closing_velocity_contact.z
				);
				
				friction_direction.Normalize();
				
				//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
				// The amount of dynamic friction is calculated as follows:
				// 
				// ff = pd * ( u * rf )
				// 
				// Where
				//
				// ff = the friction force applied
				// pd = the direction of movement along the plane
				// u  = the coefficient of dynamic friction
				// rf = the reaction force at the plane normal.
				//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
				
				Vec2f friction_force = friction_direction * ( reaction_force * obj2.GetDynamicFriction() );
				
				// Figure out the change in velocity we want to produce by first converting to an impulse, then scaling by inverse mass
				
				Vec2f friction_velocity_change = ( friction_force * time_delta ) * obj2.GetInverseMass();
				
				// Add to the velocity change we want:
				
				contact_velocity_change.y = friction_velocity_change.x;
				contact_velocity_change.z = friction_velocity_change.y;
			}
			
			// Calculate the required impulse to produce the required velocity change in contact space:
			
			Vec3f impulse_contact = velocity_per_unit_impulse_contact_inverse.Transform(contact_velocity_change);
			
			// Transform out of contact space again into world coordinates
			
			Vec3f impulse_world = from_contact_space.Transform(impulse_contact);
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Apply the impulse at the point of contact: scale back with the number of contact points in attempt
			// to approximate a fully correct contact solver that simulatenously resolves all forces at all of the 
			// contact points.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			obj2.ApplyWorldImpulse( impulse_world  / float( contacts.size() ) , c.m_position );
		}
	}
	
	//=============================================================================================================
	// Resolve collisions
	//=============================================================================================================
	
	// Take a number of iterations to solve all the penetrations
	
	unsigned iteraions = contacts.size();
	
	// Now resolve all collisions

	for ( unsigned i = 0 ; i < iteraions && contacts.size() > 0 ; ++i )
	{
		// Get this contact

		Contact & c = contacts[0];
	
		// Resolve this collision:
	
		obj2.SetWorldPosition( obj2.GetWorldPosition() + c.m_normal * c.m_penetration );
				
		// Clear the list of contacts:
		
		contacts.clear();
		
		// Regenerate contacts: object has moved
		
		obj1.GetShape().GenerateContacts(obj2.GetShape(),contacts);
		
		// Sort the contacts based on penetration depth:
		
		std::sort(contacts.begin(),contacts.end());		
	}
}
*/

//=====================================================================================================================
/// @brief		Calculates the a matrix converting from impulse to velocity in contact space for the given object and contact
/// @param		c - contact involved in the collision
/// @param		b - body involved in the collision
/// @param		from_contact_space - matrix converting from contact space into world space
/// @param		to_contact_space - matrix converting to contact space from world space.
//=====================================================================================================================

Matrix3f Body::GetContactImpulseToVelocityMatrix
( 
	const Contact	& c						,	
	const Body		& b						,
	const Matrix3f	& from_contact_space	,
	const Matrix3f  & to_contact_space
)
{
	// Get the position of the contact relative to the body's center of mass
	
	Vec3f rel_contact_pos = c.m_position - b.GetWorldPosition();
	
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	// Create a skew symmetric matrix that is the equivalent of a cross product with the relative_contact_pos
	// variable. This is used to create a matrix that will transform a given vector in contact space into units
	// of torque.
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	
	Matrix3f cross_relative_contact_pos = Matrix3f::SkewSymmetric
	(
		rel_contact_pos.x,
		rel_contact_pos.y,
		rel_contact_pos.z
	);
	
	// We need the reverse of this matrix also
	
	Matrix3f reverse_cross_relative_contact_pos = Matrix3f::ReverseSkewSymmetric
	(
		rel_contact_pos.x,
		rel_contact_pos.y,
		rel_contact_pos.z
	);	
	
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	// Create a matrix that gives units of torque in world space, given a vector representing impulse. This 
	// vector can be in any direction in world space, not just along the contact normal. We use the previous 
	// skew-symetric matrix to generate this matrix, which performs the matrix equivalent of the cross product 
	// with the relative contact position. The to_contact_space matrix contains the three axis vectors of 
	// contact space in each column of the matrix.
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	
	Matrix3f torque_per_unit_impulse = cross_relative_contact_pos * to_contact_space;
	
	// Calculate the change in angular velocity per unit impulse along each of the three contact space axes
	
	Matrix3f delta_angular_velocity_per_unit_impulse = b.GetInverseInertiaTensor() * torque_per_unit_impulse;
	
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	// Calculate the amount of rotational induced velocity generated produced per unit of impulse along each of 
	// the three contact space axis vectors:
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	
	Matrix3f velocity_per_unit_impulse = reverse_cross_relative_contact_pos * delta_angular_velocity_per_unit_impulse;
	
	// Transform this velocity into contact space:
	
	Matrix3f velocity_per_unit_impulse_contact = from_contact_space * velocity_per_unit_impulse;
	
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	// The previous matix only takes into account velocity produced by rotation, we need to also take into
	// account ordinary linear velocity also. Since linear velocity scales equally in all the contact space 
	// directions, we only need to add the inverse mass to the x,y,z coords of the three column vectors of
	// this matrix in order to take account of linear velocity. Remember:
	//
	// force = mass * acceleration
	//
	// So:
	//
	// acceleration_per_unit_force = 1.0f / mass
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	
	velocity_per_unit_impulse_contact.m_components[0][0] += b.GetInverseMass();
	velocity_per_unit_impulse_contact.m_components[1][1] += b.GetInverseMass();
	velocity_per_unit_impulse_contact.m_components[2][2] += b.GetInverseMass();
	
	// Return the matrix:
	
	return velocity_per_unit_impulse_contact;
}
