From 4362b88a7479b2db00145043468f153ef95dce67 Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Thu, 21 Nov 2013 11:39:11 +0100 Subject: [PATCH] RigidBody DONE Needs only testing for confirmations --- Code/OysterMath/LinearMath.h | 4 + Code/OysterMath/OysterMath.cpp | 5 + Code/OysterMath/OysterMath.h | 3 + Code/OysterPhysics3D/OysterPhysics3D.h | 17 ++- Code/OysterPhysics3D/RigidBody.cpp | 141 +++++++++---------------- Code/OysterPhysics3D/RigidBody.h | 10 +- 6 files changed, 79 insertions(+), 101 deletions(-) diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h index c09caf32..4c954cd6 100644 --- a/Code/OysterMath/LinearMath.h +++ b/Code/OysterMath/LinearMath.h @@ -455,6 +455,10 @@ namespace LinearAlgebra3D template inline ::LinearAlgebra::Vector3 VectorProjection( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Vector3 &axis ) { return axis * ( vector.Dot(axis) / axis.Dot(axis) ); } + + template + inline ::LinearAlgebra::Vector3 NormalProjection( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Vector3 &normalizedAxis ) + { return axis * ( vector.Dot(axis) ); } } #include "Utilities.h" diff --git a/Code/OysterMath/OysterMath.cpp b/Code/OysterMath/OysterMath.cpp index fb2486fc..ab318e92 100644 --- a/Code/OysterMath/OysterMath.cpp +++ b/Code/OysterMath/OysterMath.cpp @@ -160,4 +160,9 @@ namespace Oyster { namespace Math3D { return ::LinearAlgebra3D::VectorProjection( vector, axis ); } + + Float3 NormalProjection( const Float3 &vector, const Float3 &normalizedAxis ) + { + return ::LinearAlgebra3D::NormalProjection( vector, normalizedAxis ); + } } } \ No newline at end of file diff --git a/Code/OysterMath/OysterMath.h b/Code/OysterMath/OysterMath.h index fd116b04..4a1eeefe 100644 --- a/Code/OysterMath/OysterMath.h +++ b/Code/OysterMath/OysterMath.h @@ -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 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 inline Float4x4 & ViewProjectionMatrix( const Float4x4 &view, const Float4x4 &projection, Float4x4 &targetMem = Float4x4() ) { return targetMem = projection * view; } diff --git a/Code/OysterPhysics3D/OysterPhysics3D.h b/Code/OysterPhysics3D/OysterPhysics3D.h index 3666b917..ea229c24 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.h +++ b/Code/OysterPhysics3D/OysterPhysics3D.h @@ -108,7 +108,7 @@ namespace Oyster { namespace Physics3D ******************************************************************/ 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; } + /****************************************************************** + * 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. * @todo TODO: improve doc @@ -183,6 +192,12 @@ namespace Oyster { namespace Physics3D 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 { /// Library of Formulas to calculate moment of inerta for simple shapes /** @todo TODO: add MomentOfInertia tensor formulas */ diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 6ed9da25..c65b040f 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -40,22 +40,20 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) { // 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. - Float4x4 wMomentOfInertiaTensor = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ); + Float4x4 wMomentOfInertiaTensor = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ); // RI // updating the linear - // dv = dt * a = dt * F / m - // ds = dt * avg_v - Float3 deltaLinearVelocity = this->impulseForceSum; - deltaLinearVelocity *= (deltaTime / this->mass); - Float3 deltaPos = deltaTime * ::Utility::Value::AverageWithDelta( Formula::LinearVelocity(this->mass, this->linearMomentum), deltaLinearVelocity ); + // dG = F * dt + // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G + Float3 linearImpulse = this->impulseForceSum * deltaTime; // aka deltaLinearMomentum + Float3 deltaPos = ( deltaTime / this->mass ) * ::Utility::Value::AverageWithDelta( this->linearMomentum, linearImpulse ); // updating the angular - // dw = dt * a = dt * ( I^-1 * T ) - // rotation = dt * avg_w - Float4x4 inversedMomentOfInertiaTensor = wMomentOfInertiaTensor.GetInverse(); - Float3 deltaAngularVelocity = Formula::AngularImpulseAcceleration( inversedMomentOfInertiaTensor, this->impulseTorqueSum ); // I^-1 * T - deltaAngularVelocity *= deltaTime; - Float3 rotationAxis = ::Utility::Value::AverageWithDelta( Formula::AngularVelocity(inversedMomentOfInertiaTensor,this->angularMomentum), deltaAngularVelocity ); + // dH = T * dt + // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H + Float3 angularImpulse = this->impulseTorqueSum * deltaTime; // aka deltaAngularMomentum + Float3 rotationAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), + ::Utility::Value::AverageWithDelta(this->angularMomentum, angularImpulse) ); Float deltaRadian = rotationAxis.Dot( rotationAxis ); if( deltaRadian != 0.0f ) @@ -72,10 +70,10 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) this->box.center += deltaPos; } - // update movements and clear impulses - this->linearMomentum += Formula::LinearMomentum( this->mass, deltaLinearVelocity ); + // update movements and clear impulseForceSum and impulseTorqueSum + this->linearMomentum += linearImpulse; this->impulseForceSum = Float3::null; - this->angularMomentum += Formula::AngularMomentum( wMomentOfInertiaTensor, deltaAngularVelocity ); + this->angularMomentum += angularImpulse; this->impulseTorqueSum = Float3::null; } @@ -226,67 +224,21 @@ Float3 RigidBody::GetLinearVelocity() const return Formula::LinearVelocity( this->mass, this->linearMomentum ); } -Float3 RigidBody::GetTangentialImpulseForceAt( const Float3 &worldPos ) const -{ // by Dan Andersson +void RigidBody::GetMomentumAt( const Float3 &worldPos, const Float3 &surfaceNormal, Float3 &normalMomentum, Float3 &tangentialMomentum ) const +{ 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 -{ // by Dan Andersson - 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 ); + normalMomentum = NormalProjection( momentum, surfaceNormal ); + tangentialMomentum = momentum - normalMomentum; } void RigidBody::SetMomentOfInertia( const Float4x4 &localI ) { // 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 ) { // 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 ) @@ -325,64 +278,70 @@ void RigidBody::SetCenter( const Float3 &worldPos ) void RigidBody::SetImpulseTorque( const Float3 &worldT ) { // by Dan Andersson - this->impulseTorqueSum = t; + this->impulseTorqueSum = worldT; } void RigidBody::SetAngularMomentum( const Float3 &worldH ) { // by Dan Andersson - this->angularMomentum = h; + this->angularMomentum = worldH; } void RigidBody::SetAngularImpulseAcceleration( const Float3 &worldA ) { // by Dan Andersson - this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, a ); + this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA ); } void RigidBody::SetAngularVelocity( const Float3 &worldW ) { // by Dan Andersson - this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, w ); + this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW ); } void RigidBody::SetImpulseForce( const Float3 &worldF ) { // by Dan Andersson - this->impulseForceSum = f; + this->impulseForceSum = worldF; } void RigidBody::SetLinearMomentum( const Float3 &worldG ) { // by Dan Andersson - this->linearMomentum = g; + this->linearMomentum = worldG; } void RigidBody::SetLinearImpulseAcceleration( const Float3 &worldA ) { // by Dan Andersson - this->impulseForceSum = Formula::ImpulseForce( this->mass, a ); + this->impulseForceSum = Formula::ImpulseForce( this->mass, worldA ); } void RigidBody::SetLinearVelocity( const Float3 &worldV ) { // 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 - // Reminder! Impulse force and torque is world values. - this->impulseForceSum = VectorProjection( worldForce, worldPos ); - this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldPos ); + Float3 worldOffset = worldPos - this->box.center; + this->impulseForceSum = VectorProjection( worldForce, worldOffset ); + this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldOffset ); } void RigidBody::SetLinearMomentumAt( const Float3 &worldG, const Float3 &worldPos ) { // by Dan Andersson - // Reminder! Linear and angular momentum is world values. - this->linearMomentum = VectorProjection( worldG, worldPos ); - this->angularMomentum = Formula::AngularMomentum( worldG, worldPos ); + Float3 worldOffset = worldPos - this->box.center; + this->linearMomentum = VectorProjection( worldG, worldOffset ); + this->angularMomentum = Formula::AngularMomentum( worldG, worldOffset ); } -void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &pos ) -{ // by - +void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos ) +{ // 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 ) -{ // by - +void RigidBody::SetLinearVelocityAt( const Float3 &worldV, const Float3 &worldPos ) +{ // 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) ); } \ No newline at end of file diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index 4b7f65c0..19ce6bb2 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -64,15 +64,7 @@ namespace Oyster { namespace Physics3D ::Oyster::Math::Float3 GetLinearImpulseAcceleration() const; ::Oyster::Math::Float3 GetLinearVelocity() const; - ::Oyster::Math::Float3 GetTangentialImpulseForceAt( const ::Oyster::Math::Float3 &worldPos ) 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; + void GetMomentumAt( const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &surfaceNormal, ::Oyster::Math::Float3 &normalMomentum, ::Oyster::Math::Float3 &tangentialMomentum ) const; // SET METHODS ////////////////////////////////