Danbias/Code/OysterPhysics3D/RigidBody.cpp

231 lines
8.3 KiB
C++

/////////////////////////////////////////////////////////////////////
// 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::Move( const Float4 &deltaPos, const Float4 &deltaAxis )
{
this->centerPos += deltaPos;
this->axis += deltaAxis;
this->rotation = Rotation( this->axis );
}
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 );
}