diff --git a/Code/GamePhysics/Implementation/SphericalRigidBody.cpp b/Code/GamePhysics/Implementation/SphericalRigidBody.cpp index 9fd8d219..bb497851 100644 --- a/Code/GamePhysics/Implementation/SphericalRigidBody.cpp +++ b/Code/GamePhysics/Implementation/SphericalRigidBody.cpp @@ -28,7 +28,7 @@ SphericalRigidBody::SphericalRigidBody( const API::SphericalBodyDescription &des this->rigid.centerPos = desc.centerPosition; this->rigid.boundingReach = Float4( desc.radius, desc.radius, desc.radius, 0.0f ); this->rigid.SetMass_KeepMomentum( desc.mass ); - this->rigid.SetMomentOfInertia_KeepMomentum( Formula::MomentOfInertia::CreateSphereMatrix( desc.mass, desc.radius ) ); + this->rigid.SetMomentOfInertia_KeepMomentum( MomentOfInertia::Sphere(desc.mass, desc.radius) ); this->deltaPos = Float4::null; this->deltaAxis = Float4::null; diff --git a/Code/GamePhysics/PhysicsStructs-Impl.h b/Code/GamePhysics/PhysicsStructs-Impl.h index 46de90cc..8918ab98 100644 --- a/Code/GamePhysics/PhysicsStructs-Impl.h +++ b/Code/GamePhysics/PhysicsStructs-Impl.h @@ -19,7 +19,7 @@ namespace Oyster this->restitutionCoeff = 1.0f; this->frictionCoeff_Dynamic = 0.5f; this->frictionCoeff_Static = 0.5f; - this->inertiaTensor = ::Oyster::Math::Float4x4::identity; + this->inertiaTensor = ::Oyster::Physics3D::MomentOfInertia(); this->subscription_onCollision = NULL; this->subscription_onCollisionResponse = NULL; this->subscription_onMovement = NULL; @@ -41,7 +41,7 @@ namespace Oyster this->ignoreGravity = false; } - inline CustomBodyState::CustomBodyState( ::Oyster::Math::Float mass, ::Oyster::Math::Float restitutionCoeff, ::Oyster::Math::Float staticFrictionCoeff, ::Oyster::Math::Float kineticFrictionCoeff, const ::Oyster::Math::Float4x4 &inertiaTensor, const ::Oyster::Math::Float4 &reach, const ::Oyster::Math::Float4 ¢erPos, const ::Oyster::Math::Float4 &rotation, const ::Oyster::Math::Float4 &linearMomentum, const ::Oyster::Math::Float4 &angularMomentum, const ::Oyster::Math::Float4 &gravityNormal ) + inline CustomBodyState::CustomBodyState( ::Oyster::Math::Float mass, ::Oyster::Math::Float restitutionCoeff, ::Oyster::Math::Float staticFrictionCoeff, ::Oyster::Math::Float kineticFrictionCoeff, const ::Oyster::Physics3D::MomentOfInertia &inertiaTensor, const ::Oyster::Math::Float4 &reach, const ::Oyster::Math::Float4 ¢erPos, const ::Oyster::Math::Float4 &rotation, const ::Oyster::Math::Float4 &linearMomentum, const ::Oyster::Math::Float4 &angularMomentum, const ::Oyster::Math::Float4 &gravityNormal ) { this->mass = mass; this->restitutionCoeff = restitutionCoeff; @@ -102,7 +102,7 @@ namespace Oyster return this->kineticFrictionCoeff; } - inline const ::Oyster::Math::Float4x4 & CustomBodyState::GetMomentOfInertia() const + inline const ::Oyster::Physics3D::MomentOfInertia & CustomBodyState::GetMomentOfInertia() const { return this->inertiaTensor; } @@ -219,20 +219,17 @@ namespace Oyster this->kineticFrictionCoeff = kineticU; } - inline void CustomBodyState::SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &tensor ) + inline void CustomBodyState::SetMomentOfInertia_KeepMomentum( const ::Oyster::Physics3D::MomentOfInertia &tensor ) { this->inertiaTensor = tensor; } - inline void CustomBodyState::SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &tensor ) + inline void CustomBodyState::SetMomentOfInertia_KeepVelocity( const ::Oyster::Physics3D::MomentOfInertia &tensor ) { - if( tensor.GetDeterminant() != 0.0f ) - { // sanity block! - ::Oyster::Math::Float4x4 rotation = ::Oyster::Math3D::RotationMatrix(this->angularAxis.xyz); - ::Oyster::Math::Float4 w = ::Oyster::Physics3D::Formula::AngularVelocity( (rotation * this->inertiaTensor).GetInverse(), this->angularMomentum ); - this->inertiaTensor = tensor; - this->angularMomentum = ::Oyster::Physics3D::Formula::AngularMomentum( rotation * tensor, w ); - } + ::Oyster::Math::Quaternion rotation = ::Oyster::Math3D::Rotation(this->angularAxis); + ::Oyster::Math::Float4 w = this->inertiaTensor.CalculateAngularVelocity( rotation, this->angularMomentum ); + this->inertiaTensor = tensor; + this->angularMomentum = this->inertiaTensor.CalculateAngularMomentum( rotation, w ); } inline void CustomBodyState::SetSize( const ::Oyster::Math::Float4 &size ) diff --git a/Code/GamePhysics/PhysicsStructs.h b/Code/GamePhysics/PhysicsStructs.h index 1bc1736f..4eaaf46d 100644 --- a/Code/GamePhysics/PhysicsStructs.h +++ b/Code/GamePhysics/PhysicsStructs.h @@ -3,6 +3,7 @@ #include "OysterMath.h" #include "PhysicsAPI.h" +#include "Inertia.h" namespace Oyster { namespace Physics { @@ -17,7 +18,7 @@ namespace Oyster { namespace Physics ::Oyster::Math::Float restitutionCoeff; ::Oyster::Math::Float frictionCoeff_Static; ::Oyster::Math::Float frictionCoeff_Dynamic; - ::Oyster::Math::Float4x4 inertiaTensor; + ::Oyster::Physics3D::MomentOfInertia inertiaTensor; ::Oyster::Physics::ICustomBody::EventAction_Collision subscription_onCollision; ::Oyster::Physics::ICustomBody::EventAction_CollisionResponse subscription_onCollisionResponse; ::Oyster::Physics::ICustomBody::EventAction_Move subscription_onMovement; @@ -50,7 +51,7 @@ namespace Oyster { namespace Physics ::Oyster::Math::Float restitutionCoeff = 1.0f, ::Oyster::Math::Float staticFrictionCoeff = 1.0f, ::Oyster::Math::Float kineticFrictionCoeff = 1.0f, - const ::Oyster::Math::Float4x4 &inertiaTensor = ::Oyster::Math::Float4x4::identity, + const ::Oyster::Physics3D::MomentOfInertia &inertiaTensor = ::Oyster::Physics3D::MomentOfInertia(), const ::Oyster::Math::Float4 &reach = ::Oyster::Math::Float4::null, const ::Oyster::Math::Float4 ¢erPos = ::Oyster::Math::Float4::standard_unit_w, const ::Oyster::Math::Float4 &rotation = ::Oyster::Math::Float4::null, @@ -64,7 +65,7 @@ namespace Oyster { namespace Physics const ::Oyster::Math::Float GetRestitutionCoeff() const; const ::Oyster::Math::Float GetFrictionCoeff_Static() const; const ::Oyster::Math::Float GetFrictionCoeff_Kinetic() const; - const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const; + const ::Oyster::Physics3D::MomentOfInertia & GetMomentOfInertia() const; const ::Oyster::Math::Float4 & GetReach() const; ::Oyster::Math::Float4 GetSize() const; const ::Oyster::Math::Float4 & GetCenterPosition() const; @@ -87,8 +88,8 @@ namespace Oyster { namespace Physics void SetMass_KeepVelocity( ::Oyster::Math::Float m ); void SetRestitutionCoeff( ::Oyster::Math::Float e ); void SetFrictionCoeff( ::Oyster::Math::Float staticU, ::Oyster::Math::Float kineticU ); - void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &tensor ); - void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &tensor ); + void SetMomentOfInertia_KeepMomentum( const ::Oyster::Physics3D::MomentOfInertia &tensor ); + void SetMomentOfInertia_KeepVelocity( const ::Oyster::Physics3D::MomentOfInertia &tensor ); void SetSize( const ::Oyster::Math::Float4 &size ); void SetReach( const ::Oyster::Math::Float4 &halfSize ); void SetCenterPosition( const ::Oyster::Math::Float4 ¢erPos ); @@ -115,7 +116,7 @@ namespace Oyster { namespace Physics private: ::Oyster::Math::Float mass, restitutionCoeff, staticFrictionCoeff, kineticFrictionCoeff; - ::Oyster::Math::Float4x4 inertiaTensor; + ::Oyster::Physics3D::MomentOfInertia inertiaTensor; ::Oyster::Math::Float4 reach, centerPos, angularAxis; ::Oyster::Math::Float4 linearMomentum, angularMomentum; ::Oyster::Math::Float4 linearImpulse, angularImpulse; diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 80c800a6..d4da0d00 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -23,7 +23,7 @@ RigidBody::RigidBody( ) this->frictionCoeff_Static = 0.5f; this->frictionCoeff_Kinetic = 1.0f; this->mass = 10; - this->momentOfInertiaTensor = Float4x4::identity; + this->momentOfInertiaTensor = MomentOfInertia(); this->rotation = Quaternion::identity; } @@ -51,17 +51,18 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength ) // 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 ); + //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 + //Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H //! HACK: @todo Rotation temporary disabled //this->axis += Radian( Formula::AngularVelocity(wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, this->impulse_Angular)) ); - //this->rotation = Rotation( this->axis ); + this->axis += this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, 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; @@ -78,11 +79,12 @@ void RigidBody::Predict_LeapFrog( Float4 &outDeltaPos, Float4 &outDeltaAxis, con 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 + //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) ); + //outDeltaAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, actingAngularImpulse) ); + outDeltaAxis = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); } void RigidBody::Move( const Float4 &deltaPos, const Float4 &deltaAxis ) @@ -106,7 +108,7 @@ void RigidBody::ApplyImpulse( const Float4 &worldJ, const Float4 &atWorldPos ) } } -const Float4x4 & RigidBody::GetMomentOfInertia() const +const MomentOfInertia & RigidBody::GetMomentOfInertia() const { // by Dan Andersson return this->momentOfInertiaTensor; } @@ -143,7 +145,7 @@ Float4 RigidBody::GetVelocity_Linear() const Float4 RigidBody::GetVelocity_Angular() const { // by Dan Andersson - return Formula::AngularVelocity( this->momentOfInertiaTensor.GetInverse(), this->momentum_Angular ); + return this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular ); } Float4 RigidBody::GetLinearMomentum( const Float4 &atWorldPos ) const @@ -151,24 +153,16 @@ Float4 RigidBody::GetLinearMomentum( const Float4 &atWorldPos ) const return this->momentum_Linear + Formula::TangentialLinearMomentum( this->momentum_Angular, atWorldPos - this->centerPos ); } -void RigidBody::SetMomentOfInertia_KeepVelocity( const Float4x4 &localTensorI ) +void RigidBody::SetMomentOfInertia_KeepVelocity( const MomentOfInertia &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 ); - } + Float4 w = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular ); + this->momentOfInertiaTensor = localTensorI; + this->momentum_Angular = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, w ); } -void RigidBody::SetMomentOfInertia_KeepMomentum( const Float4x4 &localTensorI ) +void RigidBody::SetMomentOfInertia_KeepMomentum( const MomentOfInertia &localTensorI ) { // by Dan Andersson - if( localTensorI.GetDeterminant() != 0.0f ) - { // insanity check! MomentOfInertiaTensor must be invertable - this->momentOfInertiaTensor = localTensorI; - } + this->momentOfInertiaTensor = localTensorI; } void RigidBody::SetMass_KeepVelocity( const Float &m ) @@ -217,13 +211,13 @@ void RigidBody::SetVelocity_Linear( const Float4 &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) ); + this->momentum_Linear = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) ); + this->momentum_Angular = this->momentOfInertiaTensor.CalculateAngularMomentum( this->rotation, Formula::AngularVelocity(worldV, worldOffset) ); } void RigidBody::SetVelocity_Angular( const Float4 &worldW ) { // by Dan Andersson - this->momentum_Angular = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW ); + this->momentum_Angular = this->momentOfInertiaTensor.CalculateAngularMomentum( this->rotation, worldW ); } void RigidBody::SetImpulse_Linear( const Float4 &worldJ, const Float4 &atWorldPos ) diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index 51c5d2d8..ad619180 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -8,6 +8,7 @@ #include "OysterMath.h" #include "OysterCollision3D.h" #include "OysterPhysics3D.h" +#include "Inertia.h" namespace Oyster { namespace Physics3D { @@ -42,23 +43,23 @@ namespace Oyster { namespace Physics3D // GET METHODS //////////////////////////////// - const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const; - ::Oyster::Math::Float GetMass() const; - const ::Oyster::Math::Quaternion & GetRotation() const; - ::Oyster::Math::Float4x4 GetRotationMatrix() const; - ::Oyster::Math::Float4x4 GetOrientation() const; - ::Oyster::Math::Float4x4 GetView() const; - ::Oyster::Math::Float4x4 GetToWorldMatrix() const; - ::Oyster::Math::Float4x4 GetToLocalMatrix() const; - ::Oyster::Math::Float4 GetSize() const; - ::Oyster::Math::Float4 GetVelocity_Linear() const; - ::Oyster::Math::Float4 GetVelocity_Angular() const; - ::Oyster::Math::Float4 GetLinearMomentum( const ::Oyster::Math::Float4 &atWorldPos ) const; + const ::Oyster::Physics3D::MomentOfInertia & GetMomentOfInertia() const; + ::Oyster::Math::Float GetMass() const; + const ::Oyster::Math::Quaternion & GetRotation() const; + ::Oyster::Math::Float4x4 GetRotationMatrix() const; + ::Oyster::Math::Float4x4 GetOrientation() const; + ::Oyster::Math::Float4x4 GetView() const; + ::Oyster::Math::Float4x4 GetToWorldMatrix() const; + ::Oyster::Math::Float4x4 GetToLocalMatrix() const; + ::Oyster::Math::Float4 GetSize() const; + ::Oyster::Math::Float4 GetVelocity_Linear() const; + ::Oyster::Math::Float4 GetVelocity_Angular() const; + ::Oyster::Math::Float4 GetLinearMomentum( const ::Oyster::Math::Float4 &atWorldPos ) const; // SET METHODS //////////////////////////////// - void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localTensorI ); - void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &localTensorI ); + void SetMomentOfInertia_KeepVelocity( const ::Oyster::Physics3D::MomentOfInertia &localTensorI ); + void SetMomentOfInertia_KeepMomentum( const ::Oyster::Physics3D::MomentOfInertia &localTensorI ); void SetMass_KeepVelocity( const ::Oyster::Math::Float &m ); void SetMass_KeepMomentum( const ::Oyster::Math::Float &m ); @@ -78,7 +79,8 @@ namespace Oyster { namespace Physics3D private: ::Oyster::Math::Float mass; //!< m (kg) - ::Oyster::Math::Float4x4 momentOfInertiaTensor; //!< I (Nm*s) Tensor matrix ( only need to be 3x3 matrix, but is 4x4 for future hardware acceleration ) (localValue) + //::Oyster::Math::Float4x4 momentOfInertiaTensor; //!< I (Nm*s) Tensor matrix ( only need to be 3x3 matrix, but is 4x4 for future hardware acceleration ) (localValue) + ::Oyster::Physics3D::MomentOfInertia momentOfInertiaTensor; ::Oyster::Math::Quaternion rotation; //!< RotationAxis of the body. }; } }