/*
	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 represents a rigid body. It holds information such as position, mass, inertia and 
///				velocity as well as a shape. It also allows for forces to be applied to the body and for collision
///				with another body.
//=====================================================================================================================

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

using namespace Game;

//=====================================================================================================================
/// @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
//=====================================================================================================================

RigidBody::RigidBody
( 
	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
)
: 
Body
(
	shape				,
	moveable			,
	mass				,
	inertia_tensor		,
	restitution			,
	restitution_cutoff	,
	static_friction		,
	dynamic_friction	,
	linear_dampening	,
	angular_dampening
)
{
	// Nothing to do here ... 
}

//=====================================================================================================================
/// @brief		Updates all the dynamics and forces acting on the rigid body
/// @param		time_delta - The timestep to advance the simulation by
//=====================================================================================================================

void RigidBody::Integrate( float time_delta )
{
	// If not moveable then do nothing:
	
	if ( IsMoveable() == false ) return;
	
	// Apply gravity to the body:
	
	ApplyWorldAcceleration( GetWorldGravity() );

	// Add to both linear and angular velocities according to the acceleration forces acting on the body: v = v + at
	
	SetWorldVelocity		( GetWorldVelocity()		+ GetWorldAcceleration()		* time_delta );
	SetWorldAngularVelocity	( GetWorldAngularVelocity() + GetWorldAngularAcceleration() * time_delta );
	
	// Apply dampening:
	
	SetWorldVelocity		( GetWorldVelocity()		* Float::Pow( GetDampening()		, time_delta ) );
	SetWorldAngularVelocity	( GetWorldAngularVelocity()	* Float::Pow( GetAngularDampening()	, time_delta ) );
	
	// Update the change in position: p = vt + att * 0.5
	
	SetWorldPosition
	( 
		GetWorldPosition() + ( GetWorldVelocity() * time_delta ) + ( GetWorldAcceleration() * time_delta * time_delta * 0.5f ) 
	);	
	
	// Update orientation 
	
	{
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
		/// FORMULA FOR UPDATING A QUATERNION USING A CHANGE REPRESENTED IN ANGULAR VELOCITY FORMAT
		///
		/// ON = OO + ( OW * OO ) * 0.5
		///
		/// WHERE:
		///
		/// OO = equals the original orientation
		/// OW = equals the change in orientation as it was in angular velocity format, put into
		///		 a new quaternion format whereby the components are as follows:
		///
		///		 x = angular_velocity.x * time_delta
		///		 y = angular_velocity.y * time_delta
		///		 z = angular_velocity.z * time_delta
		///		 s = 0
		///
		/// ON = equals the resulting quaternion. This must be normalised afterwards to represent a 
		///		 pure rotation.
		///
		/// EXERT: 'Game Engine Physics Development' by Ian Millington.
		//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

		// Get the original orientation

		Quaternionf oo = GetWorldOrientation();

		// Convert the angular velocity into the required quaternion format:

		Quaternionf ow = Quaternionf
		(
			GetWorldAngularVelocity().x * time_delta + GetWorldAngularAcceleration().x * time_delta * time_delta * 0.5f,
			GetWorldAngularVelocity().y * time_delta + GetWorldAngularAcceleration().y * time_delta * time_delta * 0.5f,
			GetWorldAngularVelocity().z * time_delta + GetWorldAngularAcceleration().z * time_delta * time_delta * 0.5f,
			0
		);

		// Calculate the result

		Quaternionf on = oo + ( ( ow * oo ) * 0.5f );

		// Normalise and save:

		on.Normalize(); SetWorldOrientation(on);
	}
} 

//=====================================================================================================================
/// @brief		Collides this rigid body with the given rigid body and does collision response:
/// @param		time_delta - amount of time over which we do an update
/// @param		other - other rigid body to collide with
//=====================================================================================================================

void RigidBody::Collide( float time_delta , Body & other )
{	
	// See if we are moveable:
	
	if ( IsMoveable() )
	{
		if ( other.IsMoveable() )
		{
			DynamicDynamicCollision(*this,other,time_delta);
		}
		else
		{
			StaticDynamicCollision(other,*this,time_delta);
		}
	}
	else
	{
		if ( other.IsMoveable() )
		{
			StaticDynamicCollision(*this,other,time_delta);
		}
	}
}

//=====================================================================================================================
/// @brief		Called after the end of each update to the object: after Integrate() and Collide()
//=====================================================================================================================

void RigidBody::EndUpdate()
{
	// Save the previous linear acceleration for later stabilisation of resting contacts:
	
	SetPreviousWorldAcceleration( GetWorldAcceleration() );
	
	// Reset all forces acting on the rigid body
	
	ResetAccelerations();
}

#pragma region(APPLY_X_FUNCTIONS)

//=====================================================================================================================
/// @brief		Applies a given linear acceleration to the body. Acceleration is given in world coordinates.
/// @param		acceleration - acceleration to apply
/// @remarks	All accelerations that are added are continously reset after each integration, thus one must continously
///				call this function to apply a continous acceleration.
//=====================================================================================================================

void RigidBody::ApplyWorldAcceleration( const Vec3f & acceleration )
{ 
	SetWorldAcceleration( GetWorldAcceleration() + acceleration );
}

//=====================================================================================================================
/// @brief		Applies a given linear acceleration to the body at the given position in world coordinates.
///				Acceleration is in world coords also.
/// @param		acceleration - acceleration to apply
/// @param		position - position to apply the acceleration at
/// @remarks	All accelerations that are added are continously reset after each integration, thus one must continously
///				call this function to apply a continous acceleration.
//=====================================================================================================================

void RigidBody::ApplyWorldAcceleration( const Vec3f & acceleration , const Vec3f & position )
{
	// Apply the force that would be required to produce this acceleration:
	
	Vec3f force = acceleration * GetMass();
	
	// Apply the force:
	
	ApplyWorldForce(force,position);
}

//=====================================================================================================================
/// @brief		Applies a given angular acceleration to the body in world coordinates
/// @param		angular_acceleration - the amount of angular acceleration to apply
/// @remarks	All accelerations that are added are continously reset after each integration, thus one must continously
///				call this function to apply a continous acceleration.
//=====================================================================================================================

void RigidBody::ApplyWorldAngularAcceleration( const Vec3f & angular_acceleration )
{	
	SetWorldAngularAcceleration( GetWorldAngularAcceleration() + angular_acceleration );
}

//=====================================================================================================================
/// @brief		Applies a given force through the center of mass of the body. Force is given in world coordinates.
///				Because the force is through the center of mass it produces NO torque.
/// @param		force -	strength of the force to apply
/// @remarks	All forces that are added are continously reset after each integration, thus one must continously
///				call this function to apply a continous force.
//=====================================================================================================================

void RigidBody::ApplyWorldForce( const Vec3f & force )
{
	// Increase acceleration by the force: force = mass * acceleration
	
	SetWorldAcceleration( GetWorldAcceleration() + force * GetInverseMass() );
}

//=====================================================================================================================
/// @brief		Applies a given force from a given point in world coordinates. Force is given in world coordinates also.
/// @param		force - force to apply
/// @param		position - position of the force
/// @remarks	All forces that are added are continously reset after each integration, thus one must continously
///				call this function to apply a continous force.
//=====================================================================================================================

void RigidBody::ApplyWorldForce( const Vec3f & force , const Vec3f & position )
{
	// Apply normal linear force

	SetWorldAcceleration( GetWorldAcceleration() + force * GetInverseMass() );
	
	// Get a vector from the center of mass to the action point:
	
	Vec3f relative = position - GetWorldPosition();
	
	// Torque is the cross product of this vector with the force: T = Pf x F
	
	Vec3f torque = force.Cross(relative);
	
	// Apply the torque
	
	ApplyWorldTorque(torque);
}

//=====================================================================================================================
/// @brief		Applies torque to the body. Torque given is in world coordinates.
/// @param		torque - the amount of torque to apply to the body
/// @remarks	All torques that are added are continously reset after each integration, thus one must continously
///				call this function to apply a continous torque.
//=====================================================================================================================

void RigidBody::ApplyWorldTorque( const Vec3f & torque )
{
	// Transform by the inverse inertia tensor:
	
	Vec3f angular_acceleration = GetInverseInertiaTensor().Transform(torque);
	
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	// Now add to the angular acceleration: torque = angular_acceleration * interia
	// So: angular_accceleration = torque * inverse_interia
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	
	SetWorldAngularAcceleration( GetWorldAngularAcceleration() + angular_acceleration );
}

//=====================================================================================================================
/// @brief		Applies an impulse through the center of the body in world coordinates
/// @param		impulse - impulse to apply to the body
//=====================================================================================================================

void RigidBody::ApplyWorldImpulse( const Vec3f & impulse )
{
	// Caculate the change in velocity from the change in momentum:
	
	Vec3f velocity_change = impulse * GetInverseMass();
	
	// Apply the change in velocity:
	
	SetWorldVelocity( GetWorldVelocity() + velocity_change );
}

//=====================================================================================================================
/// @brief		Applies an impulse to the body at the given world position. Impulse is also in world coordinates.
/// @param		impulse - impulse to apply to the body
/// @param		position - position to apply the impulse at
//=====================================================================================================================

void RigidBody::ApplyWorldImpulse( const Vec3f & impulse , const Vec3f & position )
{
	// Caculate the change in velocity from the change in momentum:
	
	Vec3f velocity_change = impulse * GetInverseMass();
	
	// Apply the change in velocity:
	
	SetWorldVelocity( GetWorldVelocity() + velocity_change );
	
	// Calculate the change in angular momentum and thus the change in angular velocity
	
	Vec3f angular_impulse = (position - GetWorldPosition()).Cross(impulse);
	
	// Calculate the change in angular velocity
	
	Vec3f angular_velocity_change = GetInverseInertiaTensor().Transform(angular_impulse);
	
	// Add to the angular velocity
	
	SetWorldAngularVelocity( GetWorldAngularVelocity() + angular_velocity_change );
}

#pragma endregion
