RigidBody DONE

Needs only testing for confirmations
This commit is contained in:
Dander7BD 2013-11-21 11:39:11 +01:00
parent 5aa8e07b0d
commit 4362b88a74
6 changed files with 79 additions and 101 deletions

View File

@ -455,6 +455,10 @@ namespace LinearAlgebra3D
template<typename ScalarType> template<typename ScalarType>
inline ::LinearAlgebra::Vector3<ScalarType> VectorProjection( const ::LinearAlgebra::Vector3<ScalarType> &vector, const ::LinearAlgebra::Vector3<ScalarType> &axis ) inline ::LinearAlgebra::Vector3<ScalarType> VectorProjection( const ::LinearAlgebra::Vector3<ScalarType> &vector, const ::LinearAlgebra::Vector3<ScalarType> &axis )
{ return axis * ( vector.Dot(axis) / axis.Dot(axis) ); } { return axis * ( vector.Dot(axis) / axis.Dot(axis) ); }
template<typename ScalarType>
inline ::LinearAlgebra::Vector3<ScalarType> NormalProjection( const ::LinearAlgebra::Vector3<ScalarType> &vector, const ::LinearAlgebra::Vector3<ScalarType> &normalizedAxis )
{ return axis * ( vector.Dot(axis) ); }
} }
#include "Utilities.h" #include "Utilities.h"

View File

@ -160,4 +160,9 @@ namespace Oyster { namespace Math3D
{ {
return ::LinearAlgebra3D::VectorProjection( vector, axis ); return ::LinearAlgebra3D::VectorProjection( vector, axis );
} }
Float3 NormalProjection( const Float3 &vector, const Float3 &normalizedAxis )
{
return ::LinearAlgebra3D::NormalProjection( vector, normalizedAxis );
}
} } } }

View File

@ -244,6 +244,9 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized
/// returns the component vector of vector that is parallell with axis /// returns the component vector of vector that is parallell with axis
Float3 VectorProjection( const Float3 &vector, const Float3 &axis ); Float3 VectorProjection( const Float3 &vector, const Float3 &axis );
/// returns the component vector of vector that is parallell with axis. Faster than VectorProjection.
Float3 NormalProjection( const Float3 &vector, const Float3 &normalizedAxis );
/// Helper inline function that sets and then returns targetMem = projection * view /// Helper inline function that sets and then returns targetMem = projection * view
inline Float4x4 & ViewProjectionMatrix( const Float4x4 &view, const Float4x4 &projection, Float4x4 &targetMem = Float4x4() ) inline Float4x4 & ViewProjectionMatrix( const Float4x4 &view, const Float4x4 &projection, Float4x4 &targetMem = Float4x4() )
{ return targetMem = projection * view; } { return targetMem = projection * view; }

View File

@ -108,7 +108,7 @@ namespace Oyster { namespace Physics3D
******************************************************************/ ******************************************************************/
inline ::Oyster::Math::Float3 AngularImpulseAcceleration( const ::Oyster::Math::Float3 &linearImpulseAcceleration, const ::Oyster::Math::Float3 &worldOffset ) inline ::Oyster::Math::Float3 AngularImpulseAcceleration( const ::Oyster::Math::Float3 &linearImpulseAcceleration, const ::Oyster::Math::Float3 &worldOffset )
{ {
return offset.Cross( linearImpulseAcceleration ); return worldOffset.Cross( linearImpulseAcceleration );
} }
/****************************************************************** /******************************************************************
@ -129,6 +129,15 @@ namespace Oyster { namespace Physics3D
return ( momentOfInertiaInversed * ::Oyster::Math::Float4( angularMomentum, 0.0f ) ).xyz; return ( momentOfInertiaInversed * ::Oyster::Math::Float4( angularMomentum, 0.0f ) ).xyz;
} }
/******************************************************************
* Returns the world angular velocity of a mass in rotation.
* @todo TODO: improve doc
******************************************************************/
inline ::Oyster::Math::Float3 AngularVelocity( const ::Oyster::Math::Float3 &linearVelocity, const ::Oyster::Math::Float3 &worldOffset )
{
return worldOffset.Cross( linearVelocity );
}
/****************************************************************** /******************************************************************
* Returns the world tangential velocity at worldPos, of a mass in rotation. * Returns the world tangential velocity at worldPos, of a mass in rotation.
* @todo TODO: improve doc * @todo TODO: improve doc
@ -183,6 +192,12 @@ namespace Oyster { namespace Physics3D
return ( momentOfInertia * ::Oyster::Math::Float4(angularImpulseAcceleration, 0.0f) ).xyz; return ( momentOfInertia * ::Oyster::Math::Float4(angularImpulseAcceleration, 0.0f) ).xyz;
} }
inline ::Oyster::Math::Float3 Impulse( )
{
//! @todo TODO: implement calculation for impulse. Hijack Mattias Eriksson
return ::Oyster::Math::Float3::null;
}
namespace MomentOfInertia namespace MomentOfInertia
{ /// Library of Formulas to calculate moment of inerta for simple shapes { /// Library of Formulas to calculate moment of inerta for simple shapes
/** @todo TODO: add MomentOfInertia tensor formulas */ /** @todo TODO: add MomentOfInertia tensor formulas */

View File

@ -40,22 +40,20 @@ void RigidBody::Update_LeapFrog( Float deltaTime )
{ // by Dan Andersson: Euler leap frog update when Runga Kutta is not needed { // by Dan Andersson: Euler leap frog update when Runga Kutta is not needed
// 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. // 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( this->box.rotation, this->momentOfInertiaTensor ); Float4x4 wMomentOfInertiaTensor = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ); // RI
// updating the linear // updating the linear
// dv = dt * a = dt * F / m // dG = F * dt
// ds = dt * avg_v // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G
Float3 deltaLinearVelocity = this->impulseForceSum; Float3 linearImpulse = this->impulseForceSum * deltaTime; // aka deltaLinearMomentum
deltaLinearVelocity *= (deltaTime / this->mass); Float3 deltaPos = ( deltaTime / this->mass ) * ::Utility::Value::AverageWithDelta( this->linearMomentum, linearImpulse );
Float3 deltaPos = deltaTime * ::Utility::Value::AverageWithDelta( Formula::LinearVelocity(this->mass, this->linearMomentum), deltaLinearVelocity );
// updating the angular // updating the angular
// dw = dt * a = dt * ( I^-1 * T ) // dH = T * dt
// rotation = dt * avg_w // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H
Float4x4 inversedMomentOfInertiaTensor = wMomentOfInertiaTensor.GetInverse(); Float3 angularImpulse = this->impulseTorqueSum * deltaTime; // aka deltaAngularMomentum
Float3 deltaAngularVelocity = Formula::AngularImpulseAcceleration( inversedMomentOfInertiaTensor, this->impulseTorqueSum ); // I^-1 * T Float3 rotationAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(),
deltaAngularVelocity *= deltaTime; ::Utility::Value::AverageWithDelta(this->angularMomentum, angularImpulse) );
Float3 rotationAxis = ::Utility::Value::AverageWithDelta( Formula::AngularVelocity(inversedMomentOfInertiaTensor,this->angularMomentum), deltaAngularVelocity );
Float deltaRadian = rotationAxis.Dot( rotationAxis ); Float deltaRadian = rotationAxis.Dot( rotationAxis );
if( deltaRadian != 0.0f ) if( deltaRadian != 0.0f )
@ -72,10 +70,10 @@ void RigidBody::Update_LeapFrog( Float deltaTime )
this->box.center += deltaPos; this->box.center += deltaPos;
} }
// update movements and clear impulses // update movements and clear impulseForceSum and impulseTorqueSum
this->linearMomentum += Formula::LinearMomentum( this->mass, deltaLinearVelocity ); this->linearMomentum += linearImpulse;
this->impulseForceSum = Float3::null; this->impulseForceSum = Float3::null;
this->angularMomentum += Formula::AngularMomentum( wMomentOfInertiaTensor, deltaAngularVelocity ); this->angularMomentum += angularImpulse;
this->impulseTorqueSum = Float3::null; this->impulseTorqueSum = Float3::null;
} }
@ -226,67 +224,21 @@ Float3 RigidBody::GetLinearVelocity() const
return Formula::LinearVelocity( this->mass, this->linearMomentum ); return Formula::LinearVelocity( this->mass, this->linearMomentum );
} }
Float3 RigidBody::GetTangentialImpulseForceAt( const Float3 &worldPos ) const void RigidBody::GetMomentumAt( const Float3 &worldPos, const Float3 &surfaceNormal, Float3 &normalMomentum, Float3 &tangentialMomentum ) const
{ // by Dan Andersson {
Float3 worldOffset = worldPos - this->box.center; Float3 worldOffset = worldPos - this->box.center;
return Formula::TangentialImpulseForce( this->impulseTorqueSum, worldOffset ); Float3 momentum = Formula::TangentialLinearMomentum( this->angularMomentum, worldOffset );
} momentum += this->linearMomentum;
Float3 RigidBody::GetTangentialLinearMomentumAt( const Float3 &worldPos ) const normalMomentum = NormalProjection( momentum, surfaceNormal );
{ // by Dan Andersson tangentialMomentum = momentum - normalMomentum;
return Formula::TangentialLinearMomentum( this->angularMomentum, worldPos );
}
Float3 RigidBody::GetTangentialImpulseAccelerationAt( const Float3 &worldPos ) const
{ // by Dan Andersson
Float4x4 invWorldMomentOfInertia = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ).GetInverse();
Float3 worldOffset = worldPos - this->box.center;
return Formula::TangentialImpulseAcceleration( invWorldMomentOfInertia, this->impulseTorqueSum, worldOffset );
}
Float3 RigidBody::GetTangentialLinearVelocityAt( const Float3 &worldPos ) const
{ // by Dan Andersson
Float4x4 invWorldMomentOfInertia = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ).GetInverse();
Float3 worldOffset = worldPos - this->box.center;
return Formula::TangentialLinearVelocity( invWorldMomentOfInertia, this->angularMomentum, worldOffset );
}
Float3 RigidBody::GetImpulseForceAt( const Float3 &worldPos ) const
{ // by Dan Andersson
return Float3::null; //! @todo TODO: surface normal needed as well. Same goes for those below.
}
Float3 RigidBody::GetLinearMomentumAt( const Float3 &worldPos ) const
{ // by Dan Andersson
// Reminder! Momentum is a world value.
Float4 localMomentum = Float4( this->GetLinearMomentumAt_Local((this->GetView() * Float4(worldPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f
return (this->box.orientation * localMomentum).xyz; // should not be any disform thus result.w = 0.0f
// TODO: angularMomentum is a local value!!
return this->linearMomentum + Formula::TangentialLinearMomentum( this->angularMomentum, worldPos );
}
Float3 RigidBody::GetImpulseAccelerationAt( const Float3 &worldPos ) const
{ // by Dan Andersson
// Reminder! Acceleration is a world value.
return Formula::LinearImpulseAcceleration( this->mass, this->impulseForceSum )
+ Formula::TangentialImpulseAcceleration( this->momentOfInertiaTensor.GetInverse(), this->impulseTorqueSum, worldPos );
}
Float3 RigidBody::GetLinearVelocityAt( const Float3 &worldPos ) const
{ // by Dan Andersson
// Reminder! Velocity is a world value.
return Formula::LinearVelocity( this->mass, this->linearMomentum )
+ Formula::TangentialLinearVelocity( this->momentOfInertiaTensor.GetInverse(), this->angularMomentum, worldPos );
} }
void RigidBody::SetMomentOfInertia( const Float4x4 &localI ) void RigidBody::SetMomentOfInertia( const Float4x4 &localI )
{ // by Dan Andersson { // by Dan Andersson
if( i.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable if( localI.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable
{ {
this->momentOfInertiaTensor = i; this->momentOfInertiaTensor = localI;
} }
} }
@ -310,7 +262,8 @@ void RigidBody::SetMass_KeepMomentum( const Float &m )
void RigidBody::SetOrientation( const Float4x4 &o ) void RigidBody::SetOrientation( const Float4x4 &o )
{ // by Dan Andersson { // by Dan Andersson
this->box.orientation = o; ExtractRotationMatrix( o, this->box.rotation );
this->box.center = o.v[3].xyz;
} }
void RigidBody::SetSize( const Float3 &widthHeight ) void RigidBody::SetSize( const Float3 &widthHeight )
@ -325,64 +278,70 @@ void RigidBody::SetCenter( const Float3 &worldPos )
void RigidBody::SetImpulseTorque( const Float3 &worldT ) void RigidBody::SetImpulseTorque( const Float3 &worldT )
{ // by Dan Andersson { // by Dan Andersson
this->impulseTorqueSum = t; this->impulseTorqueSum = worldT;
} }
void RigidBody::SetAngularMomentum( const Float3 &worldH ) void RigidBody::SetAngularMomentum( const Float3 &worldH )
{ // by Dan Andersson { // by Dan Andersson
this->angularMomentum = h; this->angularMomentum = worldH;
} }
void RigidBody::SetAngularImpulseAcceleration( const Float3 &worldA ) void RigidBody::SetAngularImpulseAcceleration( const Float3 &worldA )
{ // by Dan Andersson { // by Dan Andersson
this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, a ); this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA );
} }
void RigidBody::SetAngularVelocity( const Float3 &worldW ) void RigidBody::SetAngularVelocity( const Float3 &worldW )
{ // by Dan Andersson { // by Dan Andersson
this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, w ); this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW );
} }
void RigidBody::SetImpulseForce( const Float3 &worldF ) void RigidBody::SetImpulseForce( const Float3 &worldF )
{ // by Dan Andersson { // by Dan Andersson
this->impulseForceSum = f; this->impulseForceSum = worldF;
} }
void RigidBody::SetLinearMomentum( const Float3 &worldG ) void RigidBody::SetLinearMomentum( const Float3 &worldG )
{ // by Dan Andersson { // by Dan Andersson
this->linearMomentum = g; this->linearMomentum = worldG;
} }
void RigidBody::SetLinearImpulseAcceleration( const Float3 &worldA ) void RigidBody::SetLinearImpulseAcceleration( const Float3 &worldA )
{ // by Dan Andersson { // by Dan Andersson
this->impulseForceSum = Formula::ImpulseForce( this->mass, a ); this->impulseForceSum = Formula::ImpulseForce( this->mass, worldA );
} }
void RigidBody::SetLinearVelocity( const Float3 &worldV ) void RigidBody::SetLinearVelocity( const Float3 &worldV )
{ // by Dan Andersson { // by Dan Andersson
this->linearMomentum = Formula::LinearMomentum( this->mass, v ); this->linearMomentum = Formula::LinearMomentum( this->mass, worldV );
} }
void RigidBody::SetImpulseForceAt( const Float3 &worldF, const Float3 &worldPos ) void RigidBody::SetImpulseForceAt( const Float3 &worldForce, const Float3 &worldPos )
{ // by Dan Andersson { // by Dan Andersson
// Reminder! Impulse force and torque is world values. Float3 worldOffset = worldPos - this->box.center;
this->impulseForceSum = VectorProjection( worldForce, worldPos ); this->impulseForceSum = VectorProjection( worldForce, worldOffset );
this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldPos ); this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldOffset );
} }
void RigidBody::SetLinearMomentumAt( const Float3 &worldG, const Float3 &worldPos ) void RigidBody::SetLinearMomentumAt( const Float3 &worldG, const Float3 &worldPos )
{ // by Dan Andersson { // by Dan Andersson
// Reminder! Linear and angular momentum is world values. Float3 worldOffset = worldPos - this->box.center;
this->linearMomentum = VectorProjection( worldG, worldPos ); this->linearMomentum = VectorProjection( worldG, worldOffset );
this->angularMomentum = Formula::AngularMomentum( worldG, worldPos ); this->angularMomentum = Formula::AngularMomentum( worldG, worldOffset );
} }
void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &pos ) void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos )
{ // by { // by Dan Andersson
Float3 worldOffset = worldPos - this->box.center;
this->impulseForceSum = Formula::ImpulseForce( this->mass, VectorProjection(worldA, worldOffset) );
this->impulseTorqueSum = Formula::ImpulseTorque( this->box.rotation * this->momentOfInertiaTensor,
Formula::AngularImpulseAcceleration(worldA, worldOffset) );
} }
void RigidBody::SetLinearVelocityAt( const Float3 &worldV, const Float3 &pos ) void RigidBody::SetLinearVelocityAt( const Float3 &worldV, const Float3 &worldPos )
{ // by { // by Dan Andersson
Float3 worldOffset = worldPos - this->box.center;
this->linearMomentum = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) );
this->angularMomentum = Formula::AngularMomentum( this->box.rotation * this->momentOfInertiaTensor,
Formula::AngularVelocity(worldV, worldOffset) );
} }

View File

@ -64,15 +64,7 @@ namespace Oyster { namespace Physics3D
::Oyster::Math::Float3 GetLinearImpulseAcceleration() const; ::Oyster::Math::Float3 GetLinearImpulseAcceleration() const;
::Oyster::Math::Float3 GetLinearVelocity() const; ::Oyster::Math::Float3 GetLinearVelocity() const;
::Oyster::Math::Float3 GetTangentialImpulseForceAt( const ::Oyster::Math::Float3 &worldPos ) const; void GetMomentumAt( const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &surfaceNormal, ::Oyster::Math::Float3 &normalMomentum, ::Oyster::Math::Float3 &tangentialMomentum ) const;
::Oyster::Math::Float3 GetTangentialLinearMomentumAt( const ::Oyster::Math::Float3 &worldPos ) const;
::Oyster::Math::Float3 GetTangentialImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldPos ) const;
::Oyster::Math::Float3 GetTangentialLinearVelocityAt( const ::Oyster::Math::Float3 &worldPos ) const;
::Oyster::Math::Float3 GetImpulseForceAt( const ::Oyster::Math::Float3 &worldPos ) const;
::Oyster::Math::Float3 GetLinearMomentumAt( const ::Oyster::Math::Float3 &worldPos ) const;
::Oyster::Math::Float3 GetImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldPos ) const;
::Oyster::Math::Float3 GetLinearVelocityAt( const ::Oyster::Math::Float3 &worldPos ) const;
// SET METHODS //////////////////////////////// // SET METHODS ////////////////////////////////