Danbias/Code/OysterPhysics3D/RigidBody.cpp

/////////////////////////////////////////////////////////////////////
// Created by Dan Andersson & Robin Engman 2013
/////////////////////////////////////////////////////////////////////

#include "RigidBody.h"
#include "Utilities.h"

using namespace ::Oyster::Collision3D;
using namespace ::Oyster::Physics3D;
using namespace ::Oyster::Math3D;
using namespace ::Utility::Value;

RigidBody::RigidBody( )
{ // by Dan Andersson
	this->centerPos = Float4::standard_unit_w;
	this->axis = Float4::null;
	this->boundingReach = Float4( 0.5f, 0.5f, 0.5f, 0.0f );
	this->momentum_Linear = Float4::null;
	this->momentum_Angular = Float4::null;
	this->impulse_Linear = Float4::null;
	this->impulse_Angular = Float4::null;
	this->restitutionCoeff = 1.0f;
	this->frictionCoeff_Static = 0.5f;
	this->frictionCoeff_Kinetic = 1.0f;
	this->mass = 10;
	this->momentOfInertiaTensor = Float4x4::identity;
	this->rotation = Quaternion::identity;
}

RigidBody & RigidBody::operator = ( const RigidBody &body )
{ // by Dan Andersson
	this->centerPos = body.centerPos;
	this->axis = body.axis;
	this->boundingReach = body.boundingReach;
	this->momentum_Linear = body.momentum_Linear;
	this->momentum_Angular = body.momentum_Angular;
	this->impulse_Linear = body.impulse_Linear;
	this->impulse_Angular = body.impulse_Angular;
	this->restitutionCoeff = body.restitutionCoeff;
	this->frictionCoeff_Static = body.frictionCoeff_Static;
	this->frictionCoeff_Kinetic = body.frictionCoeff_Kinetic;
	this->mass = body.mass;
	this->momentOfInertiaTensor = body.momentOfInertiaTensor;
	this->rotation = body.rotation;
	return *this;
}

void RigidBody::Update_LeapFrog( Float updateFrameLength )
{ // by Dan Andersson: Euler leap frog update when Runga Kutta is not needed
	
	// updating the linear
	// ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G
	this->centerPos += ( updateFrameLength / this->mass ) * AverageWithDelta( this->momentum_Linear, this->impulse_Linear );

	// updating the angular
	Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix );
	// Important! The member data is all world data except the Inertia tensor. Thus a new InertiaTensor needs to be created to be compatible with the rest of the world data.
	Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI

	// dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H
	this->axis += Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, this->impulse_Angular) );
	this->rotation = Rotation( this->axis );

	// update momentums and clear impulse_Linear and impulse_Angular
	this->momentum_Linear += this->impulse_Linear;
	this->impulse_Linear = Float4::null;
	this->momentum_Angular += this->impulse_Angular;
	this->impulse_Angular = Float4::null;
}

void RigidBody::Predict_LeapFrog( Float4 &outDeltaPos, Float4 &outDeltaAxis, const Float4 &actingLinearImpulse, const Float4 &actingAngularImpulse, Float deltaTime )
{
	// updating the linear
	// ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G
	outDeltaPos = ( deltaTime / this->mass ) * AverageWithDelta( this->momentum_Linear, actingLinearImpulse );

	// updating the angular
	Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix );
	Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI

	// dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H
	outDeltaAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, actingAngularImpulse) );
}

void RigidBody::ApplyImpulse( const Float4 &worldJ, const Float4 &atWorldPos )
{ // by Dan Andersson
	Float4 worldOffset = atWorldPos - this->centerPos;
	if( worldOffset != Float4::null )
	{
		this->impulse_Linear += VectorProjection( worldJ, atWorldPos );
		this->impulse_Angular += Formula::ImpulseTorque( worldJ, atWorldPos );
	}
	else
	{
		this->impulse_Linear += worldJ;
	}
}

const Float4x4 & RigidBody::GetMomentOfInertia() const
{ // by Dan Andersson
	return this->momentOfInertiaTensor;
}

Float RigidBody::GetMass() const
{ // by Dan Andersson
	return this->mass;
}

const Quaternion & RigidBody::GetRotation() const
{ // by Dan Andersson
	return this->rotation;
}

Float4x4 RigidBody::GetRotationMatrix() const
{ // by Dan Andersson
	return RotationMatrix( this->rotation );
}

Float4x4 RigidBody::GetOrientation() const
{ // by Dan Andersson
	return ::Oyster::Math3D::OrientationMatrix( this->rotation, this->centerPos );
}

Float4x4 RigidBody::GetView() const
{ // by Dan Andersson
	return ViewMatrix( this->rotation, this->centerPos );
}

Float4 RigidBody::GetVelocity_Linear() const
{ // by Dan Andersson
	return Formula::LinearVelocity( this->mass, this->momentum_Linear );
}

Float4 RigidBody::GetVelocity_Angular() const
{ // by Dan Andersson
	return Formula::AngularVelocity( this->momentOfInertiaTensor.GetInverse(), this->momentum_Angular );
}

Float4 RigidBody::GetLinearMomentum( const Float4 &atWorldPos ) const
{ // by Dan Andersson
	return this->momentum_Linear + Formula::TangentialLinearMomentum( this->momentum_Angular, atWorldPos - this->centerPos );
}

void RigidBody::SetMomentOfInertia_KeepVelocity( const Float4x4 &localTensorI )
{ // by Dan Andersson
	if( localTensorI.GetDeterminant() != 0.0f )
	{ // insanity check! MomentOfInertiaTensor must be invertable
		Float4x4 rotationMatrix; RotationMatrix( this->rotation, rotationMatrix );

		Float4 w = Formula::AngularVelocity( (rotationMatrix * this->momentOfInertiaTensor).GetInverse(), this->momentum_Angular );
		this->momentOfInertiaTensor = localTensorI;
		this->momentum_Angular = Formula::AngularMomentum( rotationMatrix * localTensorI, w );
	}
}

void RigidBody::SetMomentOfInertia_KeepMomentum( const Float4x4 &localTensorI )
{ // by Dan Andersson
	if( localTensorI.GetDeterminant() != 0.0f )
	{ // insanity check! MomentOfInertiaTensor must be invertable
		this->momentOfInertiaTensor = localTensorI;
	}
}

void RigidBody::SetMass_KeepVelocity( const Float &m )
{ // by Dan Andersson
	if( m != 0.0f )
	{ // insanity check! Mass must be invertable
		Float4 v = Formula::LinearVelocity( this->mass, this->momentum_Linear );
		this->mass = m;
		this->momentum_Linear = Formula::LinearMomentum( this->mass, v );
	}
}

void RigidBody::SetMass_KeepMomentum( const Float &m )
{ // by Dan Anderson
	if( m != 0.0f )
	{ // insanity check! Mass must be invertable
		this->mass = m;
	}
}

void RigidBody::SetOrientation( const Float4x4 &o )
{ // by Dan Andersson
	this->axis = ExtractAngularAxis( o );
	this->rotation = Rotation( this->axis );
	this->centerPos = o.v[3].xyz;
}

void RigidBody::SetRotation( const Float4x4 &r )
{ // by Dan Andersson
	this->axis = ExtractAngularAxis( r );
	this->rotation = Rotation( this->axis );
}

void RigidBody::SetMomentum_Linear( const Float4 &worldG, const Float4 &atWorldPos )
{ // by Dan Andersson
	Float4 worldOffset = atWorldPos - this->centerPos;
	this->momentum_Linear = VectorProjection( worldG, worldOffset );
	this->momentum_Angular = Formula::AngularMomentum( worldG, worldOffset );
}

void RigidBody::SetVelocity_Linear( const Float4 &worldV )
{ // by Dan Andersson
	this->momentum_Linear = Formula::LinearMomentum( this->mass, worldV );
}

void RigidBody::SetVelocity_Linear( const Float4 &worldV, const Float4 &atWorldPos )
{ // by Dan Andersson
	Float4 worldOffset = atWorldPos - this->centerPos;
	this->momentum_Linear = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) );
	this->momentum_Angular = Formula::AngularMomentum( RotationMatrix(this->rotation) * this->momentOfInertiaTensor, Formula::AngularVelocity(worldV, worldOffset) );
}

void RigidBody::SetVelocity_Angular( const Float4 &worldW )
{ // by Dan Andersson
	this->momentum_Angular = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW );
}

void RigidBody::SetImpulse_Linear( const Float4 &worldJ, const Float4 &atWorldPos )
{ // by Dan Andersson
	Float4 worldOffset = atWorldPos - this->centerPos;
	this->impulse_Linear = VectorProjection( worldJ, worldOffset );
	this->impulse_Angular = Formula::ImpulseTorque( worldJ, worldOffset );
}