diff --git a/Code/OysterPhysics3D/Inertia.cpp b/Code/OysterPhysics3D/Inertia.cpp new file mode 100644 index 00000000..69df0e01 --- /dev/null +++ b/Code/OysterPhysics3D/Inertia.cpp @@ -0,0 +1,51 @@ +/******************************************************************** + * Created by Dan Andersson 2014 + ********************************************************************/ + +#include "Inertia.h" + +using namespace ::Oyster::Math3D; +using namespace ::Oyster::Physics3D; + +MomentOfInertia::MomentOfInertia() +{ + this->rotation = Quaternion::identity; + this->magnitude = Float3( 1.0f ); +} + +MomentOfInertia::MomentOfInertia( const Quaternion &r, const Float3 &m ) +{ + this->rotation = r; + this->magnitude = m; +} + +MomentOfInertia & MomentOfInertia::operator = ( const MomentOfInertia &i ) +{ + this->rotation = i.rotation; + this->magnitude = i.magnitude; + return *this; +} + +Float4 MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float4 &h ) const +{ + return this->CalculateAngularVelocity( externR, h, Float4() ); +} + +Float4 & MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float4 &h, Float4 &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; + 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 +{ + return this->CalculateAngularMomentum( externR, w, Float4() ); +} + +Float4 & MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float4 &w, Float4 &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; + 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 new file mode 100644 index 00000000..c7ffc49f --- /dev/null +++ b/Code/OysterPhysics3D/Inertia.h @@ -0,0 +1,119 @@ +/******************************************************************** + * Created by Dan Andersson & Robin Engman 2014 + ********************************************************************/ + +#ifndef OYSTER_PHYSICS_3D_INERTIA_H +#define OYSTER_PHYSICS_3D_INERTIA_H + +#include "OysterMath.h" + +namespace Oyster { namespace Physics3D +{ + struct MomentOfInertia + { + ::Oyster::Math::Quaternion rotation; + ::Oyster::Math::Float3 magnitude; + + MomentOfInertia(); + MomentOfInertia( const ::Oyster::Math::Quaternion &r, const ::Oyster::Math::Float3 &m ); + + 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::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; + + 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 ); + static ::Oyster::Math::Float CalculateCuboidX( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float depth ); + static ::Oyster::Math::Float CalculateCuboidY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth ); + static ::Oyster::Math::Float CalculateCuboidZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float height ); + static ::Oyster::Math::Float CalculateRodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length ); + static ::Oyster::Math::Float CalculateCylinderXY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius ); + static ::Oyster::Math::Float CalculateCylinderZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); + + static MomentOfInertia Sphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); + static MomentOfInertia HollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); + static MomentOfInertia Cuboid( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth ); + static MomentOfInertia RodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length ); + static MomentOfInertia Cylinder( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius ); + }; + + inline ::Oyster::Math::Float MomentOfInertia::CalculateSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return (2.0f / 5.0f) * mass * radius * radius; + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateHollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return (2.0f / 3.0f) * mass * radius * radius; + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCuboidX( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float depth ) + { + return (1.0f / 12.0f) * mass * (height * height + depth * depth); + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCuboidY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth ) + { + return (1.0f / 12.0f) * mass * (width * width + depth * depth); + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCuboidZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float height ) + { + return (1.0f / 12.0f) * mass * (height * height + width * width); + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateRodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length ) + { + return (1.0f / 12.0f) * mass * length * length; + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCylinderXY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius ) + { + return (1.0f / 12.0f) * mass * (3.0f * radius * radius + height * height); + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCylinderZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return 0.5f * mass * radius * radius; + } + + inline MomentOfInertia MomentOfInertia::Sphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(MomentOfInertia::CalculateSphere(mass, radius)) ); + } + + inline MomentOfInertia MomentOfInertia::HollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(MomentOfInertia::CalculateHollowSphere(mass, radius)) ); + } + + inline MomentOfInertia MomentOfInertia::Cuboid( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth ) + { + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(MomentOfInertia::CalculateCuboidX(mass, height, depth), + MomentOfInertia::CalculateCuboidY(mass, width, depth), + MomentOfInertia::CalculateCuboidZ(mass, height, width)) ); + } + + inline MomentOfInertia MomentOfInertia::RodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length ) + { + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(MomentOfInertia::CalculateRodCenter(mass , length)) ); + } + + inline MomentOfInertia MomentOfInertia::Cylinder( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius ) + { + ::Oyster::Math::Float cylinderXY = MomentOfInertia::CalculateCylinderXY( mass , height, radius ); + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(cylinderXY, cylinderXY, + MomentOfInertia::CalculateCylinderZ(mass, radius)) ); + } +} } + +#endif \ No newline at end of file diff --git a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj index 186ea535..dffeca4f 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj +++ b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj @@ -155,6 +155,7 @@ + @@ -174,6 +175,7 @@ + diff --git a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters index 02919d28..da336ce5 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters +++ b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters @@ -78,6 +78,9 @@ Header Files\Physics + + Header Files\Physics + @@ -125,5 +128,8 @@ Source Files\Physics + + Source Files\Physics + \ No newline at end of file