diff --git a/Code/OysterPhysics3D/Inertia.cpp b/Code/OysterPhysics3D/Inertia.cpp index 69df0e01..2e0e436b 100644 --- a/Code/OysterPhysics3D/Inertia.cpp +++ b/Code/OysterPhysics3D/Inertia.cpp @@ -26,26 +26,26 @@ MomentOfInertia & MomentOfInertia::operator = ( const MomentOfInertia &i ) return *this; } -Float4 MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float4 &h ) const +Float3 MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float3 &h ) const { - return this->CalculateAngularVelocity( externR, h, Float4() ); + return this->CalculateAngularVelocity( externR, h, Float3() ); } -Float4 & MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float4 &h, Float4 &targetMem ) const +Float3 & MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float3 &h, Float3 &targetMem ) const { // w = (R * I_R) * I_M^-1 * (R * I_R)^-1 * h Float4x4 rotation = RotationMatrix( externR ) * RotationMatrix( this->rotation ); - Float4 w = rotation.GetInverse() * h; + Float4 w = rotation.GetInverse() * Float4( h, 0.0f ); return targetMem = rotation * w.PiecewiseMultiplicationAdd( Float4(1.0f / this->magnitude.x, 1.0f / this->magnitude.y, 1.0f / this->magnitude.z, 0.0f) ); } -Float4 MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float4 &w ) const +Float3 MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float3 &w ) const { - return this->CalculateAngularMomentum( externR, w, Float4() ); + return this->CalculateAngularMomentum( externR, w, Float3() ); } -Float4 & MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float4 &w, Float4 &targetMem ) const +Float3 & MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float3 &w, Float3 &targetMem ) const { // h = (R * I_R) * I_M * (R * I_R)^-1 * w Float4x4 rotation = RotationMatrix( externR ) * RotationMatrix( this->rotation ); - Float4 h = rotation.GetInverse() * w; + Float4 h = rotation.GetInverse() * Float4( w, 0.0f ); return targetMem = rotation * h.PiecewiseMultiplicationAdd( Float4(this->magnitude.x, this->magnitude.y, this->magnitude.z, 0.0f) ); } \ No newline at end of file diff --git a/Code/OysterPhysics3D/Inertia.h b/Code/OysterPhysics3D/Inertia.h index c7ffc49f..502074a6 100644 --- a/Code/OysterPhysics3D/Inertia.h +++ b/Code/OysterPhysics3D/Inertia.h @@ -19,11 +19,11 @@ namespace Oyster { namespace Physics3D MomentOfInertia & operator = ( const MomentOfInertia &i ); - ::Oyster::Math::Float4 CalculateAngularVelocity( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float4 &angularMomentum ) const; - ::Oyster::Math::Float4 & CalculateAngularVelocity( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float4 &angularMomentum, ::Oyster::Math::Float4 &targetMem ) const; + ::Oyster::Math::Float3 CalculateAngularVelocity( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float3 &angularMomentum ) const; + ::Oyster::Math::Float3 & CalculateAngularVelocity( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float3 &angularMomentum, ::Oyster::Math::Float3 &targetMem ) const; - ::Oyster::Math::Float4 CalculateAngularMomentum( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float4 &angularVelocity ) const; - ::Oyster::Math::Float4 & CalculateAngularMomentum( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float4 &angularVelocity, ::Oyster::Math::Float4 &targetMem ) const; + ::Oyster::Math::Float3 CalculateAngularMomentum( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float3 &angularVelocity ) const; + ::Oyster::Math::Float3 & CalculateAngularMomentum( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float3 &angularVelocity, ::Oyster::Math::Float3 &targetMem ) const; static ::Oyster::Math::Float CalculateSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); static ::Oyster::Math::Float CalculateHollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index d4da0d00..8e5f5f2b 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -72,7 +72,7 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength ) this->impulse_Angular = Float4::null; } -void RigidBody::Predict_LeapFrog( Float4 &outDeltaPos, Float4 &outDeltaAxis, const Float4 &actingLinearImpulse, const Float4 &actingAngularImpulse, Float deltaTime ) +void RigidBody::Predict_LeapFrog( Float3 &outDeltaPos, Float3 &outDeltaAxis, const Float3 &actingLinearImpulse, const Float3 &actingAngularImpulse, Float deltaTime ) { // updating the linear // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G @@ -87,17 +87,17 @@ void RigidBody::Predict_LeapFrog( Float4 &outDeltaPos, Float4 &outDeltaAxis, con outDeltaAxis = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); } -void RigidBody::Move( const Float4 &deltaPos, const Float4 &deltaAxis ) +void RigidBody::Move( const Float3 &deltaPos, const Float3 &deltaAxis ) { this->centerPos += deltaPos; this->axis += deltaAxis; this->rotation = Rotation( this->axis ); } -void RigidBody::ApplyImpulse( const Float4 &worldJ, const Float4 &atWorldPos ) +void RigidBody::ApplyImpulse( const Float3 &worldJ, const Float3 &atWorldPos ) { // by Dan Andersson - Float4 worldOffset = atWorldPos - this->centerPos; - if( worldOffset != Float4::null ) + Float3 worldOffset = atWorldPos - this->centerPos; + if( worldOffset != Float3::null ) { this->impulse_Linear += VectorProjection( worldJ, atWorldPos ); this->impulse_Angular += Formula::ImpulseTorque( worldJ, atWorldPos ); @@ -118,7 +118,7 @@ Float RigidBody::GetMass() const return this->mass; } -const Quaternion & RigidBody::GetRotation() const +const Quaternion & RigidBody::GetRotationQuaternion() const { // by Dan Andersson return this->rotation; } @@ -138,24 +138,24 @@ Float4x4 RigidBody::GetView() const return ViewMatrix( this->rotation, this->centerPos ); } -Float4 RigidBody::GetVelocity_Linear() const +Float3 RigidBody::GetVelocity_Linear() const { // by Dan Andersson return Formula::LinearVelocity( this->mass, this->momentum_Linear ); } -Float4 RigidBody::GetVelocity_Angular() const +Float3 RigidBody::GetVelocity_Angular() const { // by Dan Andersson return this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular ); } -Float4 RigidBody::GetLinearMomentum( const Float4 &atWorldPos ) const +Float3 RigidBody::GetLinearMomentum( const Float3 &atWorldPos ) const { // by Dan Andersson return this->momentum_Linear + Formula::TangentialLinearMomentum( this->momentum_Angular, atWorldPos - this->centerPos ); } void RigidBody::SetMomentOfInertia_KeepVelocity( const MomentOfInertia &localTensorI ) { // by Dan Andersson - Float4 w = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular ); + Float3 w = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular ); this->momentOfInertiaTensor = localTensorI; this->momentum_Angular = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, w ); } @@ -169,7 +169,7 @@ 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 ); + Float3 v = Formula::LinearVelocity( this->mass, this->momentum_Linear ); this->mass = m; this->momentum_Linear = Formula::LinearMomentum( this->mass, v ); } @@ -183,46 +183,46 @@ void RigidBody::SetMass_KeepMomentum( const Float &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::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::SetRotation( const Float4x4 &r ) +void RigidBody::SetMomentum_Linear( const Float3 &worldG, const Float3 &atWorldPos ) { // 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; + Float3 worldOffset = atWorldPos - this->centerPos; this->momentum_Linear = VectorProjection( worldG, worldOffset ); this->momentum_Angular = Formula::AngularMomentum( worldG, worldOffset ); } -void RigidBody::SetVelocity_Linear( const Float4 &worldV ) +void RigidBody::SetVelocity_Linear( const Float3 &worldV ) { // by Dan Andersson this->momentum_Linear = Formula::LinearMomentum( this->mass, worldV ); } -void RigidBody::SetVelocity_Linear( const Float4 &worldV, const Float4 &atWorldPos ) +void RigidBody::SetVelocity_Linear( const Float3 &worldV, const Float3 &atWorldPos ) { // by Dan Andersson - Float4 worldOffset = atWorldPos - this->centerPos; + Float3 worldOffset = atWorldPos - this->centerPos; 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 ) +void RigidBody::SetVelocity_Angular( const Float3 &worldW ) { // by Dan Andersson this->momentum_Angular = this->momentOfInertiaTensor.CalculateAngularMomentum( this->rotation, worldW ); } -void RigidBody::SetImpulse_Linear( const Float4 &worldJ, const Float4 &atWorldPos ) +void RigidBody::SetImpulse_Linear( const Float3 &worldJ, const Float3 &atWorldPos ) { // by Dan Andersson - Float4 worldOffset = atWorldPos - this->centerPos; + Float3 worldOffset = atWorldPos - this->centerPos; this->impulse_Linear = VectorProjection( worldJ, worldOffset ); this->impulse_Angular = Formula::ImpulseTorque( worldJ, worldOffset ); } \ No newline at end of file diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index ad619180..c666662a 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -15,7 +15,7 @@ namespace Oyster { namespace Physics3D struct RigidBody { //! A struct of a simple rigid body. public: - ::Oyster::Math::Float4 centerPos, //!< Location of the body's center in the world. + ::Oyster::Math::Float3 centerPos, //!< Location of the body's center in the world. axis, //!< Euler rotationAxis of the body. boundingReach, //!< momentum_Linear, //!< The linear momentum G (kg*m/s). @@ -32,29 +32,29 @@ namespace Oyster { namespace Physics3D RigidBody & operator = ( const RigidBody &body ); void Update_LeapFrog( ::Oyster::Math::Float updateFrameLength ); - void Predict_LeapFrog( ::Oyster::Math::Float4 &outDeltaPos, ::Oyster::Math::Float4 &outDeltaAxis, const ::Oyster::Math::Float4 &actingLinearImpulse, const ::Oyster::Math::Float4 &actingAngularImpulse, ::Oyster::Math::Float deltaTime ); + void Predict_LeapFrog( ::Oyster::Math::Float3 &outDeltaPos, ::Oyster::Math::Float3 &outDeltaAxis, const ::Oyster::Math::Float3 &actingLinearImpulse, const ::Oyster::Math::Float3 &actingAngularImpulse, ::Oyster::Math::Float deltaTime ); - void Move( const ::Oyster::Math::Float4 &deltaPos, const ::Oyster::Math::Float4 &deltaAxis ); - void ApplyImpulse( const ::Oyster::Math::Float4 &worldJ, const ::Oyster::Math::Float4 &atWorldPos ); - void ApplyImpulse_Linear( const ::Oyster::Math::Float4 &worldJ ); - void ApplyImpulse_Angular( const ::Oyster::Math::Float4 &worldJ ); - void ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength ); - void ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos ); + void Move( const ::Oyster::Math::Float3 &deltaPos, const ::Oyster::Math::Float3 &deltaAxis ); + void ApplyImpulse( const ::Oyster::Math::Float3 &worldJ, const ::Oyster::Math::Float3 &atWorldPos ); + void ApplyImpulse_Linear( const ::Oyster::Math::Float3 &worldJ ); + void ApplyImpulse_Angular( const ::Oyster::Math::Float3 &worldJ ); + void ApplyForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength ); + void ApplyForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float3 &atWorldPos ); // GET METHODS //////////////////////////////// const ::Oyster::Physics3D::MomentOfInertia & GetMomentOfInertia() const; ::Oyster::Math::Float GetMass() const; - const ::Oyster::Math::Quaternion & GetRotation() const; + const ::Oyster::Math::Quaternion & GetRotationQuaternion() 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; + ::Oyster::Math::Float3 GetSize() const; + ::Oyster::Math::Float3 GetVelocity_Linear() const; + ::Oyster::Math::Float3 GetVelocity_Angular() const; + ::Oyster::Math::Float3 GetLinearMomentum( const ::Oyster::Math::Float3 &atWorldPos ) const; // SET METHODS //////////////////////////////// @@ -63,19 +63,19 @@ namespace Oyster { namespace Physics3D void SetMass_KeepVelocity( const ::Oyster::Math::Float &m ); void SetMass_KeepMomentum( const ::Oyster::Math::Float &m ); - void SetOrientation( const ::Oyster::Math::Float4x4 &o ); - void SetRotation( const ::Oyster::Math::Float4x4 &r ); - void SetSize( const ::Oyster::Math::Float4 &widthHeight ); + //void SetOrientation( const ::Oyster::Math::Float4x4 &o ); + //void SetRotation( const ::Oyster::Math::Float4x4 &r ); + void SetSize( const ::Oyster::Math::Float3 &widthHeight ); - void SetMomentum_Linear( const ::Oyster::Math::Float4 &worldG, const ::Oyster::Math::Float4 &atWorldPos ); + void SetMomentum_Linear( const ::Oyster::Math::Float3 &worldG, const ::Oyster::Math::Float3 &atWorldPos ); - void SetVelocity_Linear( const ::Oyster::Math::Float4 &worldV ); - void SetVelocity_Linear( const ::Oyster::Math::Float4 &worldV, const ::Oyster::Math::Float4 &atWorldPos ); - void SetVelocity_Angular( const ::Oyster::Math::Float4 &worldW ); + void SetVelocity_Linear( const ::Oyster::Math::Float3 &worldV ); + void SetVelocity_Linear( const ::Oyster::Math::Float3 &worldV, const ::Oyster::Math::Float3 &atWorldPos ); + void SetVelocity_Angular( const ::Oyster::Math::Float3 &worldW ); - void SetImpulse_Linear( const ::Oyster::Math::Float4 &worldJ, const ::Oyster::Math::Float4 &atWorldPos ); - void SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength ); - void SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos ); + void SetImpulse_Linear( const ::Oyster::Math::Float3 &worldJ, const ::Oyster::Math::Float3 &atWorldPos ); + //void SetForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength ); + //void SetForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float3 &atWorldPos ); private: ::Oyster::Math::Float mass; //!< m (kg) diff --git a/Code/OysterPhysics3D/RigidBody_Inline.h b/Code/OysterPhysics3D/RigidBody_Inline.h index 980442dd..795ec5e4 100644 --- a/Code/OysterPhysics3D/RigidBody_Inline.h +++ b/Code/OysterPhysics3D/RigidBody_Inline.h @@ -10,22 +10,22 @@ namespace Oyster { namespace Physics3D { - inline void RigidBody::ApplyImpulse_Linear( const ::Oyster::Math::Float4 &worldJ ) + inline void RigidBody::ApplyImpulse_Linear( const ::Oyster::Math::Float3 &worldJ ) { this->impulse_Linear += worldJ; } - inline void RigidBody::ApplyImpulse_Angular( const ::Oyster::Math::Float4 &worldJ ) + inline void RigidBody::ApplyImpulse_Angular( const ::Oyster::Math::Float3 &worldJ ) { this->impulse_Angular += worldJ; } - inline void RigidBody::ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength ) + inline void RigidBody::ApplyForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength ) { this->impulse_Linear += worldF * updateFrameLength; } - inline void RigidBody::ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos ) + inline void RigidBody::ApplyForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float3 &atWorldPos ) { this->ApplyImpulse( worldF * updateFrameLength, atWorldPos ); } @@ -40,26 +40,25 @@ namespace Oyster { namespace Physics3D return this->GetView(); } - inline ::Oyster::Math::Float4 RigidBody::GetSize() const + inline ::Oyster::Math::Float3 RigidBody::GetSize() const { return 2.0f * this->boundingReach; } - inline void RigidBody::SetSize( const ::Oyster::Math::Float4 &widthHeight ) + inline void RigidBody::SetSize( const ::Oyster::Math::Float3 &widthHeight ) { this->boundingReach = ::Utility::Value::Abs( 0.5f * widthHeight ); } - inline void RigidBody::SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength ) - { - this->impulse_Linear = worldF * updateFrameLength; - } - - inline void RigidBody::SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos ) - { - this->SetImpulse_Linear( worldF * updateFrameLength, atWorldPos ); - } + //inline void RigidBody::SetForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength ) + //{ + // this->impulse_Linear = worldF * updateFrameLength; + //} + //inline void RigidBody::SetForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float3 &atWorldPos ) + //{ + // this->SetImpulse_Linear( worldF * updateFrameLength, atWorldPos ); + //} } } #endif \ No newline at end of file