From bbc489eac927e97ea1f4b3f6116219769cad9044 Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Thu, 19 Dec 2013 21:39:20 +0100 Subject: [PATCH] RigidBody improved and Gimbal lock proofed --- .../Implementation/SimpleRigidBody.cpp | 76 ++-- .../Implementation/SphericalRigidBody.cpp | 55 +-- Code/OysterMath/LinearMath.h | 108 +++++ Code/OysterMath/OysterMath.cpp | 55 +++ Code/OysterMath/OysterMath.h | 137 +++--- Code/OysterMath/Quaternion.h | 66 ++- Code/OysterPhysics3D/Box.cpp | 7 + Code/OysterPhysics3D/Box.h | 1 + Code/OysterPhysics3D/OysterPhysics3D.h | 27 ++ Code/OysterPhysics3D/OysterPhysics3D.vcxproj | 1 + .../OysterPhysics3D.vcxproj.filters | 3 + Code/OysterPhysics3D/RigidBody.cpp | 411 +++++------------- Code/OysterPhysics3D/RigidBody.h | 130 ++---- Code/OysterPhysics3D/RigidBody_Inline.h | 65 +++ 14 files changed, 615 insertions(+), 527 deletions(-) create mode 100644 Code/OysterPhysics3D/RigidBody_Inline.h diff --git a/Code/GamePhysics/Implementation/SimpleRigidBody.cpp b/Code/GamePhysics/Implementation/SimpleRigidBody.cpp index 5160b4eb..90df71a1 100644 --- a/Code/GamePhysics/Implementation/SimpleRigidBody.cpp +++ b/Code/GamePhysics/Implementation/SimpleRigidBody.cpp @@ -43,7 +43,8 @@ namespace Private SimpleRigidBody::SimpleRigidBody() { - this->rigid = RigidBody( Box(Float4x4::identity, Float3::null, Float3(1.0f)), 16.0f, Float4x4::identity ); + this->rigid = RigidBody(); + this->rigid.SetMass_KeepMomentum( 16.0f ); this->gravityNormal = Float3::null; this->collisionAction = Default::EventAction_Collision; this->ignoreGravity = false; @@ -52,9 +53,13 @@ SimpleRigidBody::SimpleRigidBody() SimpleRigidBody::SimpleRigidBody( const API::SimpleBodyDescription &desc ) { - this->rigid = RigidBody( Box( desc.rotation, desc.centerPosition.xyz, desc.size.xyz ), - desc.mass, - desc.inertiaTensor ); + this->rigid = RigidBody(); + this->rigid.SetRotation( desc.rotation ); + this->rigid.centerPos = desc.centerPosition; + this->rigid.SetSize( desc.size ); + this->rigid.SetMass_KeepMomentum( desc.mass ); + this->rigid.SetMomentOfInertia_KeepMomentum( desc.inertiaTensor ); + this->gravityNormal = Float3::null; if( desc.subscription ) @@ -81,31 +86,31 @@ SimpleRigidBody::State SimpleRigidBody::GetState() const { return State( this->rigid.GetMass(), this->rigid.restitutionCoeff, this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic, - this->rigid.GetMomentOfInertia(), this->rigid.box.boundingOffset, - this->rigid.box.center, AngularAxis(this->rigid.box.rotation), - Float4(this->rigid.linearMomentum, 0.0f), Float4(this->rigid.angularMomentum, 0.0f) ); + this->rigid.GetMomentOfInertia(), this->rigid.boundingReach, + this->rigid.centerPos, this->rigid.axis, + this->rigid.momentum_Linear, this->rigid.momentum_Angular ); } SimpleRigidBody::State & SimpleRigidBody::GetState( SimpleRigidBody::State &targetMem ) const { return targetMem = State( this->rigid.GetMass(), this->rigid.restitutionCoeff, this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic, - this->rigid.GetMomentOfInertia(), this->rigid.box.boundingOffset, - this->rigid.box.center, AngularAxis(this->rigid.box.rotation), - Float4(this->rigid.linearMomentum, 0.0f), Float4(this->rigid.angularMomentum, 0.0f) ); + this->rigid.GetMomentOfInertia(), this->rigid.boundingReach, + this->rigid.centerPos, this->rigid.axis, + this->rigid.momentum_Linear, this->rigid.momentum_Angular ); } void SimpleRigidBody::SetState( const SimpleRigidBody::State &state ) { - this->rigid.box.boundingOffset = state.GetReach(); - this->rigid.box.center = state.GetCenterPosition(); - this->rigid.box.rotation = state.GetRotation(); - this->rigid.angularMomentum = state.GetAngularMomentum().xyz; - this->rigid.linearMomentum = state.GetLinearMomentum().xyz; - this->rigid.impulseTorqueSum += state.GetAngularImpulse().xyz; - this->rigid.impulseForceSum += state.GetLinearImpulse().xyz; - this->rigid.restitutionCoeff = state.GetRestitutionCoeff(); - this->rigid.frictionCoeff_Static = state.GetFrictionCoeff_Static(); + this->rigid.centerPos = state.GetCenterPosition(); + this->rigid.SetRotation( state.GetRotation() ); + this->rigid.boundingReach = state.GetReach(); + this->rigid.momentum_Linear = state.GetLinearMomentum(); + this->rigid.momentum_Angular = state.GetAngularMomentum(); + this->rigid.impulse_Linear += state.GetLinearImpulse(); + this->rigid.impulse_Angular += state.GetAngularImpulse(); + this->rigid.restitutionCoeff = state.GetRestitutionCoeff(); + this->rigid.frictionCoeff_Static = state.GetFrictionCoeff_Static(); this->rigid.frictionCoeff_Kinetic = state.GetFrictionCoeff_Kinetic(); if( this->scene ) @@ -135,27 +140,27 @@ bool SimpleRigidBody::IsAffectedByGravity() const bool SimpleRigidBody::Intersects( const ICollideable &shape ) const { - return this->rigid.box.Intersects( shape ); + return Box( this->rigid.GetRotationMatrix(), this->rigid.centerPos, this->rigid.GetSize() ).Intersects( shape ); } bool SimpleRigidBody::Intersects( const ICollideable &shape, Float4 &worldPointOfContact ) const { - return this->rigid.box.Intersects( shape, worldPointOfContact ); + return Box( this->rigid.GetRotationMatrix(), this->rigid.centerPos, this->rigid.GetSize() ).Intersects( shape, worldPointOfContact ); } bool SimpleRigidBody::Intersects( const ICustomBody &object, Float4 &worldPointOfContact ) const { - return object.Intersects( this->rigid.box, worldPointOfContact ); + return object.Intersects( Box(this->rigid.GetRotationMatrix(), this->rigid.centerPos, this->rigid.GetSize()), worldPointOfContact ); } Sphere & SimpleRigidBody::GetBoundingSphere( Sphere &targetMem ) const { - return targetMem = Sphere( this->rigid.box.center, this->rigid.box.boundingOffset.GetMagnitude() ); + return targetMem = Sphere( this->rigid.centerPos, this->rigid.boundingReach.GetMagnitude() ); } Float4 & SimpleRigidBody::GetNormalAt( const Float4 &worldPos, Float4 &targetMem ) const { - Float4 offset = worldPos - this->rigid.box.center; + Float4 offset = worldPos - this->rigid.centerPos; Float distance = offset.Dot( offset ); Float3 normal = Float3::null; @@ -163,39 +168,40 @@ Float4 & SimpleRigidBody::GetNormalAt( const Float4 &worldPos, Float4 &targetMem { // sanity check Ray axis( Float4::standard_unit_w, offset / (Float)::std::sqrt(distance) ); Float minDistance = numeric_limits::max(); - - if( Private::Intersects(axis, Plane(this->rigid.box.xAxis, -this->rigid.box.boundingOffset.x), axis.collisionDistance) ) + Float4x4 rotationMatrix = this->rigid.GetRotationMatrix(); + + if( Private::Intersects(axis, Plane(rotationMatrix.v[0], -this->rigid.boundingReach.x), axis.collisionDistance) ) { // check along x-axis if( axis.collisionDistance < 0.0f ) - normal = -this->rigid.box.xAxis.xyz; + normal = -rotationMatrix.v[0].xyz; else - normal = this->rigid.box.xAxis.xyz; + normal = rotationMatrix.v[0].xyz; minDistance = Abs( axis.collisionDistance ); } - if( Private::Intersects(axis, Plane(this->rigid.box.yAxis, -this->rigid.box.boundingOffset.y), axis.collisionDistance) ) + if( Private::Intersects(axis, Plane(rotationMatrix.v[1], -this->rigid.boundingReach.y), axis.collisionDistance) ) { // check along y-axis distance = Abs( axis.collisionDistance ); // recycling memory if( minDistance > distance ) { if( axis.collisionDistance < 0.0f ) - normal = -this->rigid.box.yAxis.xyz; + normal = -rotationMatrix.v[1].xyz; else - normal = this->rigid.box.yAxis.xyz; + normal = rotationMatrix.v[1].xyz; minDistance = distance; } } - if( Private::Intersects(axis, Plane(this->rigid.box.zAxis, -this->rigid.box.boundingOffset.z), axis.collisionDistance) ) + if( Private::Intersects(axis, Plane(rotationMatrix.v[2], -this->rigid.boundingReach.z), axis.collisionDistance) ) { // check along z-axis if( minDistance > Abs( axis.collisionDistance ) ) { if( axis.collisionDistance < 0.0f ) - normal = -this->rigid.box.zAxis.xyz; + normal = -rotationMatrix.v[2].xyz; else - normal = this->rigid.box.zAxis.xyz; + normal = rotationMatrix.v[2].xyz; } } } @@ -211,7 +217,7 @@ Float3 & SimpleRigidBody::GetGravityNormal( Float3 &targetMem ) const //Float3 & SimpleRigidBody::GetCenter( Float3 &targetMem ) const //{ -// return targetMem = this->rigid.box.center; +// return targetMem = this->rigid.centerPos; //} // //Float4x4 & SimpleRigidBody::GetRotation( Float4x4 &targetMem ) const diff --git a/Code/GamePhysics/Implementation/SphericalRigidBody.cpp b/Code/GamePhysics/Implementation/SphericalRigidBody.cpp index e31fd0df..d0bebedf 100644 --- a/Code/GamePhysics/Implementation/SphericalRigidBody.cpp +++ b/Code/GamePhysics/Implementation/SphericalRigidBody.cpp @@ -10,7 +10,8 @@ using namespace ::Utility::Value; SphericalRigidBody::SphericalRigidBody() { - this->rigid = RigidBody( Box(Float4x4::identity, Float3::null, Float3(1.0f)), 10.0f, Float4x4::identity ); + this->rigid = RigidBody(); + this->rigid.SetMass_KeepMomentum( 10.0f ); this->gravityNormal = Float3::null; this->collisionAction = Default::EventAction_Collision; this->ignoreGravity = false; @@ -20,9 +21,13 @@ SphericalRigidBody::SphericalRigidBody() SphericalRigidBody::SphericalRigidBody( const API::SphericalBodyDescription &desc ) { - this->rigid = RigidBody( Box( desc.rotation, desc.centerPosition.xyz, Float3(2.0f * desc.radius) ), - desc.mass, - Formula::MomentOfInertia::CreateSphereMatrix( desc.mass, desc.radius ) ); + this->rigid = RigidBody(); + this->rigid.SetRotation( desc.rotation ); + 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->gravityNormal = Float3::null; if( desc.subscription ) @@ -50,31 +55,31 @@ SphericalRigidBody::State SphericalRigidBody::GetState() const { return State( this->rigid.GetMass(), this->rigid.restitutionCoeff, this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic, - this->rigid.GetMomentOfInertia(), this->rigid.box.boundingOffset, - this->rigid.box.center, AngularAxis(this->rigid.box.rotation), - Float4(this->rigid.linearMomentum, 0.0f), Float4(this->rigid.angularMomentum, 0.0f) ); + this->rigid.GetMomentOfInertia(), this->rigid.boundingReach, + this->rigid.centerPos, this->rigid.axis, + this->rigid.momentum_Linear, this->rigid.momentum_Angular ); } SphericalRigidBody::State & SphericalRigidBody::GetState( SphericalRigidBody::State &targetMem ) const { return targetMem = State( this->rigid.GetMass(), this->rigid.restitutionCoeff, this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic, - this->rigid.GetMomentOfInertia(), this->rigid.box.boundingOffset, - this->rigid.box.center, AngularAxis(this->rigid.box.rotation), - Float4(this->rigid.linearMomentum, 0.0f), Float4(this->rigid.angularMomentum, 0.0f) ); + this->rigid.GetMomentOfInertia(), this->rigid.boundingReach, + this->rigid.centerPos, this->rigid.axis, + this->rigid.momentum_Linear, this->rigid.momentum_Angular ); } void SphericalRigidBody::SetState( const SphericalRigidBody::State &state ) { - this->rigid.box.boundingOffset = state.GetReach(); - this->rigid.box.center = state.GetCenterPosition(); - this->rigid.box.rotation = state.GetRotation(); - this->rigid.angularMomentum = state.GetAngularMomentum().xyz; - this->rigid.linearMomentum = state.GetLinearMomentum().xyz; - this->rigid.impulseTorqueSum += state.GetAngularImpulse().xyz; - this->rigid.impulseForceSum += state.GetLinearImpulse().xyz; - this->rigid.restitutionCoeff = state.GetRestitutionCoeff(); - this->rigid.frictionCoeff_Static = state.GetFrictionCoeff_Static(); + this->rigid.centerPos = state.GetCenterPosition(); + this->rigid.SetRotation( state.GetRotation() ); + this->rigid.boundingReach = state.GetReach(); + this->rigid.momentum_Linear = state.GetLinearMomentum(); + this->rigid.momentum_Angular = state.GetAngularMomentum(); + this->rigid.impulse_Linear += state.GetLinearImpulse(); + this->rigid.impulse_Angular += state.GetAngularImpulse(); + this->rigid.restitutionCoeff = state.GetRestitutionCoeff(); + this->rigid.frictionCoeff_Static = state.GetFrictionCoeff_Static(); this->rigid.frictionCoeff_Kinetic = state.GetFrictionCoeff_Kinetic(); if( this->scene ) @@ -104,17 +109,17 @@ bool SphericalRigidBody::IsAffectedByGravity() const bool SphericalRigidBody::Intersects( const ICollideable &shape ) const { - return Sphere( this->rigid.box.center, this->rigid.box.boundingOffset.x ).Intersects( shape ); + return Sphere( this->rigid.centerPos, this->rigid.boundingReach.x ).Intersects( shape ); } bool SphericalRigidBody::Intersects( const ICollideable &shape, Float4 &worldPointOfContact ) const { - return Sphere( this->rigid.box.center, this->rigid.box.boundingOffset.x ).Intersects( shape, worldPointOfContact ); + return Sphere( this->rigid.centerPos, this->rigid.boundingReach.x ).Intersects( shape, worldPointOfContact ); } bool SphericalRigidBody::Intersects( const ICustomBody &object, Float4 &worldPointOfContact ) const { - return object.Intersects( Sphere(this->rigid.box.center, this->rigid.box.boundingOffset.x), worldPointOfContact ); + return object.Intersects( Sphere(this->rigid.centerPos, this->rigid.boundingReach.x), worldPointOfContact ); } Sphere & SphericalRigidBody::GetBoundingSphere( Sphere &targetMem ) const @@ -124,7 +129,7 @@ Sphere & SphericalRigidBody::GetBoundingSphere( Sphere &targetMem ) const Float4 & SphericalRigidBody::GetNormalAt( const Float4 &worldPos, Float4 &targetMem ) const { - targetMem = worldPos - this->rigid.box.center; + targetMem = worldPos - this->rigid.centerPos; Float magnitude = targetMem.GetMagnitude(); if( magnitude != 0.0f ) { // sanity check @@ -141,7 +146,7 @@ Float3 & SphericalRigidBody::GetGravityNormal( Float3 &targetMem ) const //Float3 & SphericalRigidBody::GetCenter( Float3 &targetMem ) const //{ -// return targetMem = this->rigid.box.center; +// return targetMem = this->rigid.centerPos; //} // //Float4x4 & SphericalRigidBody::GetRotation( Float4x4 &targetMem ) const @@ -167,7 +172,7 @@ Float3 SphericalRigidBody::GetRigidLinearVelocity() const UpdateState SphericalRigidBody::Update( Float timeStepLength ) { this->rigid.Update_LeapFrog( timeStepLength ); - this->body.center = this->rigid.GetCenter(); + this->body.center = this->rigid.centerPos; // compare previous and new state and return result //return this->current == this->previous ? UpdateState_resting : UpdateState_altered; diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h index f686ada4..0ebf94e7 100644 --- a/Code/OysterMath/LinearMath.h +++ b/Code/OysterMath/LinearMath.h @@ -281,6 +281,106 @@ namespace LinearAlgebra3D 0, 0, 0, 1 ); } + template + inline ::LinearAlgebra::Quaternion Rotation( const ScalarType &radian, const ::LinearAlgebra::Vector3 &normalizedAxis ) + { + ScalarType r = radian * 0.5f, + s = std::sin( r ), + c = std::cos( r ); + + return ::LinearAlgebra::Quaternion( normalizedAxis * s, c ); + } + + template + inline ::LinearAlgebra::Quaternion Rotation( const ScalarType &radian, const ::LinearAlgebra::Vector4 &normalizedAxis ) + { + ScalarType r = radian * 0.5f, + s = std::sin( r ), + c = std::cos( r ); + + return ::LinearAlgebra::Quaternion( (normalizedAxis * s).xyz, c ); + } + + template + inline ::LinearAlgebra::Quaternion Rotation( const ::LinearAlgebra::Vector3 &angularAxis ) + { + ScalarType radius = angularAxis.Dot( angularAxis ); + if( radius != 0 ) + { + radius = (ScalarType)::std::sqrt( radius ); + return Rotation( radius, angularAxis / radius ); + } + else + { + return ::LinearAlgebra::Quaternion::identity; + } + } + + template + inline ::LinearAlgebra::Quaternion Rotation( const ::LinearAlgebra::Vector4 &angularAxis ) + { + ScalarType radius = angularAxis.Dot( angularAxis ); + if( radius != 0 ) + { + radius = (ScalarType)::std::sqrt( radius ); + return Rotation( radius, angularAxis / radius ); + } + else + { + return ::LinearAlgebra::Quaternion::identity; + } + } + + template + inline ::LinearAlgebra::Matrix4x4 & RotationMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, ::LinearAlgebra::Matrix4x4 targetMem = ::LinearAlgebra::Matrix4x4() ) + { + ::LinearAlgebra::Quaternion conjugate = rotationQuaternion.GetConjugate(); + + targetMem.v[0] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(1,0,0)*conjugate).imaginary, 0 ); + targetMem.v[1] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,1,0)*conjugate).imaginary, 0 ); + targetMem.v[2] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,0,1)*conjugate).imaginary, 0 ); + targetMem.v[3] = ::LinearAlgebra::Vector4::standard_unit_w; + return targetMem; + } + + template + inline ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, const ::LinearAlgebra::Vector3 &translation, ::LinearAlgebra::Matrix4x4 targetMem = ::LinearAlgebra::Matrix4x4() ) + { + ::LinearAlgebra::Quaternion conjugate = rotationQuaternion.GetConjugate(); + + targetMem.v[0] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(1,0,0)*conjugate).imaginary, 0 ); + targetMem.v[1] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,1,0)*conjugate).imaginary, 0 ); + targetMem.v[2] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,0,1)*conjugate).imaginary, 0 ); + targetMem.v[3] = ::LinearAlgebra::Vector4( translation, 1 ); + return targetMem; + } + + template + inline ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, const ::LinearAlgebra::Vector4 &translation, ::LinearAlgebra::Matrix4x4 targetMem = ::LinearAlgebra::Matrix4x4() ) + { + ::LinearAlgebra::Quaternion conjugate = rotationQuaternion.GetConjugate(); + + targetMem.v[0] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(1,0,0)*conjugate).imaginary, 0 ); + targetMem.v[1] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,1,0)*conjugate).imaginary, 0 ); + targetMem.v[2] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,0,1)*conjugate).imaginary, 0 ); + targetMem.v[3] = translation; + return targetMem; + } + + template + inline ::LinearAlgebra::Matrix4x4 & ViewMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, const ::LinearAlgebra::Vector3 &translation, ::LinearAlgebra::Matrix4x4 targetMem = ::LinearAlgebra::Matrix4x4() ) + { + OrientationMatrix( rotationQuaternion, translation, targetMem ); + return InverseOrientationMatrix( targetMem, targetMem ); + } + + template + inline ::LinearAlgebra::Matrix4x4 & ViewMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, const ::LinearAlgebra::Vector4 &translation, ::LinearAlgebra::Matrix4x4 targetMem = ::LinearAlgebra::Matrix4x4() ) + { + OrientationMatrix( rotationQuaternion, translation, targetMem ); + return InverseOrientationMatrix( targetMem, targetMem ); + } + template inline ::LinearAlgebra::Matrix3x3 & RotationMatrix_AxisX( const ScalarType &radian, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { @@ -557,9 +657,17 @@ namespace LinearAlgebra3D inline ::LinearAlgebra::Vector3 VectorProjection( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Vector3 &axis ) { return axis * ( vector.Dot(axis) / axis.Dot(axis) ); } + template + inline ::LinearAlgebra::Vector4 VectorProjection( const ::LinearAlgebra::Vector4 &vector, const ::LinearAlgebra::Vector4 &axis ) + { return axis * ( vector.Dot(axis) / axis.Dot(axis) ); } + template inline ::LinearAlgebra::Vector3 NormalProjection( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Vector3 &normalizedAxis ) { return normalizedAxis * ( vector.Dot(normalizedAxis) ); } + + template + inline ::LinearAlgebra::Vector4 NormalProjection( const ::LinearAlgebra::Vector4 &vector, const ::LinearAlgebra::Vector4 &normalizedAxis ) + { return normalizedAxis * ( vector.Dot(normalizedAxis) ); } } #include "Utilities.h" diff --git a/Code/OysterMath/OysterMath.cpp b/Code/OysterMath/OysterMath.cpp index b72ad551..f44de9fd 100644 --- a/Code/OysterMath/OysterMath.cpp +++ b/Code/OysterMath/OysterMath.cpp @@ -101,6 +101,51 @@ namespace Oyster { namespace Math3D return ::LinearAlgebra3D::TranslationMatrix( position, targetMem ); } + Quaternion Rotation( Float radian, const Float3 &normalizedAxis ) + { + return ::LinearAlgebra3D::Rotation( radian, normalizedAxis ); + } + + Quaternion Rotation( Float radian, const Float4 &normalizedAxis ) + { + return ::LinearAlgebra3D::Rotation( radian, normalizedAxis ); + } + + Quaternion Rotation( const Float3 &angularAxis ) + { + return ::LinearAlgebra3D::Rotation( angularAxis ); + } + + Quaternion Rotation( const Float4 &angularAxis ) + { + return ::LinearAlgebra3D::Rotation( angularAxis ); + } + + Float4x4 & RotationMatrix( const Quaternion &rotationQuaternion, Float4x4 &targetMem ) + { + return ::LinearAlgebra3D::RotationMatrix( rotationQuaternion, targetMem ); + } + + Float4x4 & OrientationMatrix( const Quaternion &rotationQuaternion, const Float3 &translation, Float4x4 &targetMem ) + { + return ::LinearAlgebra3D::OrientationMatrix( rotationQuaternion, translation, targetMem ); + } + + Float4x4 & OrientationMatrix( const Quaternion &rotationQuaternion, const Float4 &translation, Float4x4 &targetMem ) + { + return ::LinearAlgebra3D::OrientationMatrix( rotationQuaternion, translation, targetMem ); + } + + Float4x4 & ViewMatrix( const Quaternion &rotationQuaternion, const Float3 &translation, Float4x4 &targetMem ) + { + return ::LinearAlgebra3D::ViewMatrix( rotationQuaternion, translation, targetMem ); + } + + Float4x4 & ViewMatrix( const Quaternion &rotationQuaternion, const Float4 &translation, Float4x4 &targetMem ) + { + return ::LinearAlgebra3D::ViewMatrix( rotationQuaternion, translation, targetMem ); + } + Float4x4 & RotationMatrix_AxisX( const Float &radian, Float4x4 &targetMem ) { return ::LinearAlgebra3D::RotationMatrix_AxisX( radian, targetMem ); @@ -217,8 +262,18 @@ namespace Oyster { namespace Math3D return ::LinearAlgebra3D::VectorProjection( vector, axis ); } + Float4 VectorProjection( const Float4 &vector, const Float4 &axis ) + { + return ::LinearAlgebra3D::VectorProjection( vector, axis ); + } + Float3 NormalProjection( const Float3 &vector, const Float3 &normalizedAxis ) { return ::LinearAlgebra3D::NormalProjection( vector, normalizedAxis ); } + + Float4 NormalProjection( const Float4 &vector, const Float4 &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 18a57458..2fe57718 100644 --- a/Code/OysterMath/OysterMath.h +++ b/Code/OysterMath/OysterMath.h @@ -9,17 +9,19 @@ #include "LinearMath.h" #include -namespace Oyster { namespace Math /// Oyster's native math library +namespace Oyster { namespace Math //! Oyster's native math library { - typedef float Float; /// Oyster's native scalar is float + typedef float Float; //!< Oyster's native scalar is float - typedef ::LinearAlgebra::Vector2 Float2; /// 2D Linear Vector for Oyster - typedef ::LinearAlgebra::Vector3 Float3; /// 3D Linear Vector for Oyster - typedef ::LinearAlgebra::Vector4 Float4; /// 4D Linear Vector for Oyster + typedef ::LinearAlgebra::Vector2 Float2; //!< 2D Linear Vector for Oyster + typedef ::LinearAlgebra::Vector3 Float3; //!< 3D Linear Vector for Oyster + typedef ::LinearAlgebra::Vector4 Float4; //!< 4D Linear Vector for Oyster - typedef ::LinearAlgebra::Matrix2x2 Float2x2; /// 2x2 Linear Matrix for Oyster - typedef ::LinearAlgebra::Matrix3x3 Float3x3; /// 3x3 Linear Matrix for Oyster - typedef ::LinearAlgebra::Matrix4x4 Float4x4; /// 4x4 Linear Matrix for Oyster + typedef ::LinearAlgebra::Matrix2x2 Float2x2; //!< 2x2 Linear Matrix for Oyster + typedef ::LinearAlgebra::Matrix3x3 Float3x3; //!< 3x3 Linear Matrix for Oyster + typedef ::LinearAlgebra::Matrix4x4 Float4x4; //!< 4x4 Linear Matrix for Oyster + + typedef ::LinearAlgebra::Quaternion Quaternion; //!< Quaternion for Oyster typedef Float4x4 Matrix; // by popular demand typedef Float2 Vector2; // by popular demand @@ -28,20 +30,20 @@ namespace Oyster { namespace Math /// Oyster's native math library const Float pi = 3.1415926535897932384626433832795f; - /// Function Highly recommended to check at start, just in case current version is using a feature that might be available. - /// @todo TODO: create a template UniquePointer to use here + //! Function Highly recommended to check at start, just in case current version is using a feature that might be available. + //! @todo TODO: create a template UniquePointer to use here bool IsSupported(); - /// Creates a solution matrix for 'outī= 'targetMem' * 'in'. - /// Returns false if there is no explicit solution. + //! Creates a solution matrix for 'outī= 'targetMem' * 'in'. + //! Returns false if there is no explicit solution. bool SuperpositionMatrix( const Float2x2 &in, const Float2x2 &out, Float2x2 &targetMem ); - /// Creates a solution matrix for 'outī= 'targetMem' * 'in'. - /// Returns false if there is no explicit solution. + //! Creates a solution matrix for 'outī= 'targetMem' * 'in'. + //! Returns false if there is no explicit solution. bool SuperpositionMatrix( const Float3x3 &in, const Float3x3 &out, Float3x3 &targetMem ); - /// Creates a solution matrix for 'outī= 'targetMem' * 'in'. - /// Returns false if there is no explicit solution. + //! Creates a solution matrix for 'outī= 'targetMem' * 'in'. + //! Returns false if there is no explicit solution. bool SuperpositionMatrix( const Float4x4 &in, const Float4x4 &out, Float4x4 &targetMem ); } } @@ -96,54 +98,54 @@ inline ::Oyster::Math::Float3x3 operator * ( const ::Oyster::Math::Float &left, inline ::Oyster::Math::Float4x4 operator * ( const ::Oyster::Math::Float &left, const ::Oyster::Math::Float4x4 &right ) { return ::Oyster::Math::Float4x4(right) *= left; } -namespace Oyster { namespace Math2D /// Oyster's native math library specialized for 2D +namespace Oyster { namespace Math2D //! Oyster's native math library specialized for 2D { using namespace ::Oyster::Math; // deliberate inheritance from ::Oyster::Math namespace - /// If there is an Y-axis on a 2D plane, then there is an explicit X-axis on and that is what is returned. - /// Recommended too make sure that yAxis is normalized. + //! If there is an Y-axis on a 2D plane, then there is an explicit X-axis on and that is what is returned. + //! Recommended too make sure that yAxis is normalized. Float2 X_AxisTo( const Float2 &yAxis ); - /// If there is an X-axis on a 2D plane, then there is an explicit Y-axis and that is what is returned. - /// Recommended too make sure that yAxis is normalized. + //! If there is an X-axis on a 2D plane, then there is an explicit Y-axis and that is what is returned. + //! Recommended too make sure that yAxis is normalized. Float2 Y_AxisTo( const Float2 &xAxis ); - /// Sets and returns targetMem to a translationMatrix with position as translation. + //! Sets and returns targetMem to a translationMatrix with position as translation. Float3x3 & TranslationMatrix( const Float2 &position, Float3x3 &targetMem = Float3x3() ); - /// Sets and returns targetMem as a counterclockwise rotationMatrix + //! Sets and returns targetMem as a counterclockwise rotationMatrix Float3x3 & RotationMatrix( const Float &radian, Float3x3 &targetMem = Float3x3() ); - /// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. + //! If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. Float2x2 & InverseRotationMatrix( const Float2x2 &rotation, Float2x2 &targetMem = Float2x2() ); - /// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. + //! If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem = Float3x3() ); - /// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector + //! Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector Float3x3 & OrientationMatrix( const Float2x2 &rotation, const Float2 &translation, Float3x3 &targetMem = Float3x3() ); - /// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector + //! Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector Float3x3 & OrientationMatrix( const Float3x3 &rotation, const Float2 &translation, Float3x3 &targetMem = Float3x3() ); - /// Sets and returns targetMem as an orientation Matrix with position as translation and radian rotation + //! Sets and returns targetMem as an orientation Matrix with position as translation and radian rotation Float3x3 & OrientationMatrix( const Float2 &position, const Float &radian, Float3x3 &targetMem = Float3x3() ); - /// Sets and returns targetMem as an orientation Matrix with position as translation and local y-axis directed at lookAt + //! Sets and returns targetMem as an orientation Matrix with position as translation and local y-axis directed at lookAt Float3x3 & OrientationMatrix( const Float2 &position, const Float2 &lookAt, Float3x3 &targetMem = Float3x3() ); - /// Sets and returns targetMem as an orientation Matrix that is rotated around localCenterOfRotation and then translated with position. - /// TODO: not tested + //! Sets and returns targetMem as an orientation Matrix that is rotated around localCenterOfRotation and then translated with position. + //! TODO: not tested Float3x3 & OrientationMatrix( const Float2 &position, Float radian, const Float2 &localCenterOfRotation, Float3x3 &targetMem = Float3x3() ); - /// If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method. + //! If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method. Float3x3 & InverseOrientationMatrix( const Float3x3 &orientationMatrix, Float3x3 &targetMem = Float3x3() ); - /// Returns targetmem after writing the rotation data from orientation, into it. + //! Returns targetmem after writing the rotation data from orientation, into it. Float3x3 & ExtractRotationMatrix( const Float3x3 &orientation, Float3x3 &targetMem = Float3x3() ); } } -namespace Oyster { namespace Math3D /// Oyster's native math library specialized for 3D +namespace Oyster { namespace Math3D //! Oyster's native math library specialized for 3D { using namespace ::Oyster::Math; // deliberate inheritance from ::Oyster::Math namespace @@ -156,35 +158,62 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized //! Extracts the angularAxis from orientationMatrix Float4 ExtractAngularAxis( const Float4x4 &orientationMatrix ); - /// Sets and returns targetMem to a translationMatrix with position as translation. + //! Sets and returns targetMem to a translationMatrix with position as translation. Float4x4 & TranslationMatrix( const Float3 &position, Float4x4 &targetMem = Float4x4() ); - /// Sets and returns targetMem as an counterclockwise rotation matrix around the global X-axis + /** @todo TODO: add doc */ + Quaternion Rotation( Float radian, const Float3 &normalizedAxis ); + + /** @todo TODO: add doc */ + Quaternion Rotation( Float radian, const Float3 &normalizedAxis ); + + /** @todo TODO: add doc */ + Quaternion Rotation( const Float3 &angularAxis ); + + /** @todo TODO: add doc */ + Quaternion Rotation( const Float4 &angularAxis ); + + /** @todo TODO: add doc */ + Float4x4 & RotationMatrix( const Quaternion &rotationQuaternion, Float4x4 &targetMem = Float4x4() ); + + /** @todo TODO: add doc */ + Float4x4 & OrientationMatrix( const Quaternion &rotationQuaternion, const Float3 &translation, Float4x4 &targetMem = Float4x4() ); + + /** @todo TODO: add doc */ + Float4x4 & OrientationMatrix( const Quaternion &rotationQuaternion, const Float4 &translation, Float4x4 &targetMem = Float4x4() ); + + /** @todo TODO: add doc */ + Float4x4 & ViewMatrix( const Quaternion &rotationQuaternion, const Float3 &translation, Float4x4 &targetMem = Float4x4() ); + + /** @todo TODO: add doc */ + Float4x4 & ViewMatrix( const Quaternion &rotationQuaternion, const Float4 &translation, Float4x4 &targetMem = Float4x4() ); + + //! Sets and returns targetMem as an counterclockwise rotation matrix around the global X-axis Float4x4 & RotationMatrix_AxisX( const Float &radian, Float4x4 &targetMem = Float4x4() ); - /// Sets and returns targetMem as an counterclockwise rotation matrix around the global Y-axis + //! Sets and returns targetMem as an counterclockwise rotation matrix around the global Y-axis Float4x4 & RotationMatrix_AxisY( const Float &radian, Float4x4 &targetMem = Float4x4() ); - /// Sets and returns targetMem as an counterclockwise rotation matrix around the global Z-axis + //! Sets and returns targetMem as an counterclockwise rotation matrix around the global Z-axis Float4x4 & RotationMatrix_AxisZ( const Float &radian, Float4x4 &targetMem = Float4x4() ); - /// Sets and returns targetMem as an counterclockwise rotation matrix around the angularAxis. + //! Sets and returns targetMem as an counterclockwise rotation matrix around the angularAxis. Float4x4 & RotationMatrix( const Float3 &angularAxis, Float4x4 &targetMem = Float4x4() ); - /// Sets and returns targetMem as an counterclockwise rotation matrix around the normalizedAxis. - /// Please make sure normalizedAxis is normalized. + //! Sets and returns targetMem as an counterclockwise rotation matrix around the normalizedAxis. + //! Please make sure normalizedAxis is normalized. Float4x4 & RotationMatrix( const Float &radian, const Float3 &normalizedAxis, Float4x4 &targetMem = Float4x4() ); - /// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. + //! If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem = Float3x3() ); - /// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. + //! If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. Float4x4 & InverseRotationMatrix( const Float4x4 &rotation, Float4x4 &targetMem = Float4x4() ); - /// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector + //! Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector Float4x4 & OrientationMatrix( const Float3x3 &rotation, const Float3 &translation, Float4x4 &targetMem = Float4x4() ); - /// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector + //! Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector Float4x4 & OrientationMatrix( const Float4x4 &rotation, const Float3 &translation, Float4x4 &targetMem = Float4x4() ); /******************************************************************* @@ -238,14 +267,14 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized //! @todo TODO: Add documentation and not tested Float4x4 & ViewMatrix_LookAtPos( const Float3 &worldLookAt, const Float3 &normalizedUpVector, const Float3 &worldPos, Float4x4 &targetMem = Float4x4() ); - /// If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method. + //! If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method. Float4x4 & InverseOrientationMatrix( const Float4x4 &orientationMatrix, Float4x4 &targetMem = Float4x4() ); // O0 = T0 * R0 // O1 = T1 * T0 * R1 * R0 Float4x4 & UpdateOrientationMatrix( const Float3 &deltaPosition, const Float4x4 &deltaRotationMatrix, Float4x4 &orientationMatrix ); - /// Returns targetmem after writing the rotation data from orientation, into it. + //! Returns targetmem after writing the rotation data from orientation, into it. Float4x4 & ExtractRotationMatrix( const Float4x4 &orientation, Float4x4 &targetMem = Float4x4() ); /******************************************************************* @@ -272,13 +301,19 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized *******************************************************************/ Float4x4 & ProjectionMatrix_Perspective( const Float &verticalFoV, const Float &aspectRatio, const Float &nearClip = ::std::numeric_limits::epsilon(), const Float &farClip = ::std::numeric_limits::max(), Float4x4 &targetMem = Float4x4() ); - /// 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 ); - /// returns the component vector of vector that is parallell with axis. Faster than VectorProjection. + //! returns the component vector of vector that is parallell with axis + Float4 VectorProjection( const Float4 &vector, const Float4 &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 + //! returns the component vector of vector that is parallell with axis. Faster than VectorProjection. + Float4 NormalProjection( const Float4 &vector, const Float4 &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; } @@ -290,7 +325,7 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized inline Float4x4 TransformMatrix( const Float4x4 &transformer, const Float4x4 &transformee ) { return transformer * transformee; } - /// Helper inline function that sets and then returns targetMem = transformer * transformee + //! Helper inline function that sets and then returns targetMem = transformer * transformee inline Float4 & TransformVector( const Float4x4 &transformer, const Float4 &transformee, Float4 &targetMem = Float4() ) { return targetMem = transformer * transformee; } } } diff --git a/Code/OysterMath/Quaternion.h b/Code/OysterMath/Quaternion.h index fd19100f..da790f75 100644 --- a/Code/OysterMath/Quaternion.h +++ b/Code/OysterMath/Quaternion.h @@ -22,8 +22,10 @@ namespace LinearAlgebra char byte[sizeof(ScalarType[2])]; }; + static const Quaternion null; + static const Quaternion identity; + Quaternion( ); - Quaternion( const Quaternion &quaternion ); Quaternion( const Vector3 &imaginary, const ScalarType &real ); operator ScalarType* ( ); @@ -53,40 +55,54 @@ namespace LinearAlgebra // Body /////////////////////////////////////////////////////////////////////////////////// + template const Quaternion Quaternion::null = Quaternion( Vector3((ScalarType)0), (ScalarType)0 ); + template const Quaternion Quaternion::identity = Quaternion( Vector3((ScalarType)0), (ScalarType)1 ); + template Quaternion::Quaternion( ) : imaginary(), real() {} template - Quaternion::Quaternion( const Quaternion &quaternion ) - { this->imaginary = quaternion.imaginary; this->real = quaternion.real; } - - template - Quaternion::Quaternion( const Vector3 &_imaginary, const ScalarType &_real ) - { this->imaginary = _imaginary; this->real = _real; } + Quaternion::Quaternion( const Vector3 &imaginary, const ScalarType &real ) + { + this->imaginary = imaginary; + this->real = real; + } template inline Quaternion::operator ScalarType* ( ) - { return this->element; } + { + return this->element; + } template inline Quaternion::operator const ScalarType* ( ) const - { return this->element; } + { + return this->element; + } template inline Quaternion::operator char* ( ) - { return this->byte; } + { + return this->byte; + } template inline Quaternion::operator const char* ( ) const - { return this->byte; } + { + return this->byte; + } template inline ScalarType & Quaternion::operator [] ( int i ) - { return this->element[i]; } + { + return this->element[i]; + } template inline const ScalarType & Quaternion::operator [] ( int i ) const - { return this->element[i]; } + { + return this->element[i]; + } template Quaternion & Quaternion::operator = ( const Quaternion &quaternion ) @@ -154,27 +170,39 @@ namespace LinearAlgebra template inline Quaternion Quaternion::operator * ( const ScalarType &scalar ) const - { return Quaternion(*this) *= scalar; } + { + return Quaternion(*this) *= scalar; + } template inline Quaternion Quaternion::operator / ( const ScalarType &scalar ) const - { return Quaternion(*this) /= scalar; } + { + return Quaternion(*this) /= scalar; + } template inline Quaternion Quaternion::operator + ( const Quaternion &quaternion ) const - { return Quaternion(*this) += quaternion; } + { + return Quaternion(*this) += quaternion; + } template inline Quaternion Quaternion::operator - ( const Quaternion &quaternion ) const - { return Quaternion(*this) -= quaternion; } + { + return Quaternion(*this) -= quaternion; + } template inline Quaternion Quaternion::operator - ( ) const - { return Quaternion(-this->imaginary, -this->real); } + { + return Quaternion(-this->imaginary, -this->real); + } template inline Quaternion Quaternion::GetConjugate( ) const - { return Quaternion(-this->imaginary, this->real ); } + { + return Quaternion(-this->imaginary, this->real ); + } } #endif \ No newline at end of file diff --git a/Code/OysterPhysics3D/Box.cpp b/Code/OysterPhysics3D/Box.cpp index 94f948a6..f55415b7 100644 --- a/Code/OysterPhysics3D/Box.cpp +++ b/Code/OysterPhysics3D/Box.cpp @@ -22,6 +22,13 @@ Box::Box( const Float4x4 &r, const Float3 &p, const Float3 &s ) : ICollideable(T this->boundingOffset = Float4( s * 0.5f , 0.0f); } +Box::Box( const Float4x4 &r, const Float4 &p, const Float4 &s ) : ICollideable(Type_box) +{ + this->rotation = r; + this->center = p; + this->boundingOffset = s * 0.5f; +} + Box::~Box( ) {} Box & Box::operator = ( const Box &box ) diff --git a/Code/OysterPhysics3D/Box.h b/Code/OysterPhysics3D/Box.h index 3259908f..f98ae362 100644 --- a/Code/OysterPhysics3D/Box.h +++ b/Code/OysterPhysics3D/Box.h @@ -28,6 +28,7 @@ namespace Oyster { namespace Collision3D Box( ); Box( const ::Oyster::Math::Float4x4 &rotation, const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &size ); + Box( const ::Oyster::Math::Float4x4 &rotation, const ::Oyster::Math::Float4 &worldPos, const ::Oyster::Math::Float4 &size ); virtual ~Box( ); Box & operator = ( const Box &box ); diff --git a/Code/OysterPhysics3D/OysterPhysics3D.h b/Code/OysterPhysics3D/OysterPhysics3D.h index 33d9a93e..f70d228f 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.h +++ b/Code/OysterPhysics3D/OysterPhysics3D.h @@ -192,6 +192,15 @@ namespace Oyster { namespace Physics3D return worldOffset.Cross( linearVelocity ); } + /****************************************************************** + * Returns the world angular velocity of a mass in rotation. + * @todo TODO: improve doc + ******************************************************************/ + inline ::Oyster::Math::Float4 AngularVelocity( const ::Oyster::Math::Float4 &linearVelocity, const ::Oyster::Math::Float4 &worldOffset ) + { + return ::Oyster::Math::Float4( worldOffset.xyz.Cross( linearVelocity.xyz ), 0.0f ); + } + /****************************************************************** * Returns the world tangential velocity at worldPos, of a mass in rotation. * @todo TODO: improve doc @@ -237,6 +246,15 @@ namespace Oyster { namespace Physics3D return worldOffset.Cross( impulseForce ); } + /****************************************************************** + * + * @todo TODO: improve doc + ******************************************************************/ + inline ::Oyster::Math::Float4 ImpulseTorque( const ::Oyster::Math::Float4 & impulseForce, const ::Oyster::Math::Float4 &worldOffset ) + { + return ::Oyster::Math::Float4( worldOffset.xyz.Cross(impulseForce.xyz), 0.0f ); + } + /****************************************************************** * T = I*a * @todo TODO: improve doc @@ -246,6 +264,15 @@ namespace Oyster { namespace Physics3D return ( momentOfInertia * ::Oyster::Math::Float4(angularImpulseAcceleration, 0.0f) ).xyz; } + /****************************************************************** + * T = I*a + * @todo TODO: improve doc + ******************************************************************/ + inline ::Oyster::Math::Float4 ImpulseTorque( const ::Oyster::Math::Float4x4 & momentOfInertia, const ::Oyster::Math::Float4 &angularImpulseAcceleration ) + { + return momentOfInertia * angularImpulseAcceleration; + } + namespace MomentOfInertia { /// Library of Formulas to calculate moment of inerta for simple shapes /** @todo TODO: add MomentOfInertia tensor formulas */ diff --git a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj index c36fded5..14eb0aaa 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj +++ b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj @@ -159,6 +159,7 @@ + diff --git a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters index e3a8a70d..02919d28 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters +++ b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters @@ -75,6 +75,9 @@ Header Files\Physics + + Header Files\Physics + diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index a71884f9..e774752b 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -9,418 +9,201 @@ using namespace ::Oyster::Collision3D; using namespace ::Oyster::Physics3D; using namespace ::Oyster::Math3D; -RigidBody::RigidBody( const Box &b, Float m, const Float4x4 &inertiaTensor ) +RigidBody::RigidBody( ) { // by Dan Andersson - this->box = b; - this->angularMomentum = Float3::null; - this->linearMomentum = Float3::null; - this->impulseTorqueSum = Float3::null; - this->impulseForceSum = Float3::null; + this->centerPos = Float4::standard_unit_w; + this->axis = Float4::null; + this->boundingReach = Float4( 0.5f, 0.5f, 0.5f, 0.0f ); + this->momentum_Linear = Float4::null; + this->momentum_Angular = Float4::null; + this->impulse_Linear = Float4::null; + this->impulse_Angular = Float4::null; this->restitutionCoeff = 1.0f; - this->frictionCoeff_Static = 1.0f; + this->frictionCoeff_Static = 0.5f; this->frictionCoeff_Kinetic = 1.0f; - - if( m != 0.0f ) - { - this->mass = m; - } - else - { - this->mass = ::Utility::Value::numeric_limits::epsilon(); - } - - if( inertiaTensor.GetDeterminant() != 0.0f ) - { - this->momentOfInertiaTensor = inertiaTensor; - } - else - { - this->momentOfInertiaTensor = Float4x4::identity; - } + this->mass = 10; + this->momentOfInertiaTensor = Float4x4::identity; + this->rotation = Quaternion::identity; } RigidBody & RigidBody::operator = ( const RigidBody &body ) { // by Dan Andersson - this->box = body.box; - this->angularMomentum = body.angularMomentum; - this->linearMomentum = body.linearMomentum; - this->impulseTorqueSum = body.impulseTorqueSum; - this->impulseForceSum = body.impulseForceSum; + this->centerPos = body.centerPos; + this->axis = body.axis; + this->boundingReach = body.boundingReach; + this->momentum_Linear = body.momentum_Linear; + this->momentum_Angular = body.momentum_Angular; + this->impulse_Linear = body.impulse_Linear; + this->impulse_Angular = body.impulse_Angular; this->restitutionCoeff = body.restitutionCoeff; this->frictionCoeff_Static = body.frictionCoeff_Static; this->frictionCoeff_Kinetic = body.frictionCoeff_Kinetic; this->mass = body.mass; this->momentOfInertiaTensor = body.momentOfInertiaTensor; + this->rotation = body.rotation; return *this; } -bool RigidBody::operator == ( const RigidBody &body ) -{ - if( this->box.center != body.box.center ) - { - return false; - } - - if( this->box.rotation != body.box.rotation ) - { - return false; - } - - if( this->box.boundingOffset != body.box.boundingOffset ) - { - return false; - } - - if( this->angularMomentum != body.angularMomentum ) - { - return false; - } - - if( this->linearMomentum != body.linearMomentum ) - { - return false; - } - - if( this->impulseTorqueSum != body.impulseTorqueSum ) - { - return false; - } - - if( this->impulseForceSum != body.impulseForceSum ) - { - return false; - } - - return true; -} - -bool RigidBody::operator != ( const RigidBody &body ) -{ - return !this->operator==( body ); -} - -void RigidBody::Update_LeapFrog( Float deltaTime ) +void RigidBody::Update_LeapFrog( Float updateFrameLength ) { // 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 ); // RI - // updating the linear - // dG = F * dt // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G - Float3 linearImpulse = this->impulseForceSum; // HACK: * deltaTime; // aka deltaLinearMomentum - Float3 deltaPos = ( deltaTime / this->mass ) * ::Utility::Value::AverageWithDelta( this->linearMomentum, linearImpulse ); + this->centerPos += ( updateFrameLength / this->mass ) * ::Utility::Value::AverageWithDelta( this->momentum_Linear, this->impulse_Linear ); // updating the angular - // dH = T * dt + 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 + // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H - Float3 angularImpulse = this->impulseTorqueSum; // HACK: * deltaTime; // aka deltaAngularMomentum - Float3 rotationAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), - ::Utility::Value::AverageWithDelta(this->angularMomentum, angularImpulse) ); - - Float deltaRadian = rotationAxis.Dot( rotationAxis ); - if( deltaRadian != 0.0f ) - { // branch depending if there is rotation - deltaRadian = ::std::sqrt( deltaRadian ); - rotationAxis /= deltaRadian; + this->axis += Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), ::Utility::Value::AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); + this->rotation = Rotation( this->axis ); - // using rotationAxis, deltaRadian and deltaPos to create a matrix to update the box - this->box.center.xyz += deltaPos; - TransformMatrix( RotationMatrix(deltaRadian, rotationAxis), this->box.rotation, this->box.rotation ); - } - else - { // no rotation, only use deltaPos to translate the RigidBody - this->box.center.xyz += deltaPos; - } - - // update momentums and clear impulseForceSum and impulseTorqueSum - this->linearMomentum += linearImpulse; - this->impulseForceSum = Float3::null; - this->angularMomentum += angularImpulse; - this->impulseTorqueSum = Float3::null; + // update momentums and clear impulse_Linear and impulse_Angular + this->momentum_Linear += this->impulse_Linear; + this->impulse_Linear = Float4::null; + this->momentum_Angular += this->impulse_Angular; + this->impulse_Angular = Float4::null; } -void RigidBody::ApplyImpulseForce( const Float3 &worldF ) +void RigidBody::ApplyImpulse( const Float4 &worldJ, const Float4 &atWorldPos ) { // by Dan Andersson - this->impulseForceSum += worldF; -} - -void RigidBody::ApplyImpulseForceAt( const Float3 &worldF, const Float3 &worldPos ) -{ // by Dan Andersson - Float3 worldOffset = worldPos - this->box.center.xyz; - if( worldOffset != Float3::null ) + Float4 worldOffset = atWorldPos - this->centerPos; + if( worldOffset != Float4::null ) { - this->impulseForceSum += VectorProjection( worldF, worldOffset ); - this->impulseTorqueSum += Formula::ImpulseTorque( worldF, worldOffset ); + this->impulse_Linear += VectorProjection( worldJ, atWorldPos ); + this->impulse_Angular += Formula::ImpulseTorque( worldJ, atWorldPos ); } else { - this->impulseForceSum += worldF; + this->impulse_Linear += worldJ; } } -void RigidBody::ApplyLinearImpulseAcceleration( const Float3 &worldA ) -{ // by Dan Andersson - this->impulseForceSum += Formula::ImpulseForce( this->mass, worldA ); -} - -void RigidBody::ApplyLinearImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos ) -{ // by Dan Andersson - Float3 worldOffset = worldPos - this->box.center.xyz; - if( worldOffset != Float3::null ) - { - this->impulseForceSum += Formula::ImpulseForce( this->mass, VectorProjection(worldA, worldOffset) ); - - // tanAcc = angularAcc x localPosition - // angularAcc = localPosition x tanAcc = localPosition x linearAcc - // T = I * angularAcc - this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, Formula::AngularImpulseAcceleration(worldA, worldOffset) ); - } - else - { - this->impulseForceSum += Formula::ImpulseForce( this->mass, worldA ); - } -} - -void RigidBody::ApplyImpulseTorque( const Float3 &worldT ) -{ // by Dan Andersson - this->impulseTorqueSum += worldT; -} - -void RigidBody::ApplyAngularImpulseAcceleration( const Float3 &worldA ) -{ // by Dan Andersson - this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA ); -} - -Float3 & RigidBody::AccessBoundingReach() -{ // by Dan Andersson - return this->box.boundingOffset.xyz; -} - -const Float3 & RigidBody::AccessBoundingReach() const -{ // by Dan Andersson - return this->box.boundingOffset.xyz; -} - -Float3 & RigidBody::AccessCenter() -{ // by Dan Andersson - return this->box.center.xyz; -} - -const Float3 & RigidBody::AccessCenter() const -{ // by Dan Andersson - return this->box.center.xyz; -} - const Float4x4 & RigidBody::GetMomentOfInertia() const { // by Dan Andersson return this->momentOfInertiaTensor; } -const Float & RigidBody::GetMass() const +Float RigidBody::GetMass() const { // by Dan Andersson return this->mass; } -const Float4x4 RigidBody::GetOrientation() const +const Quaternion & RigidBody::GetRotation() const { // by Dan Andersson - return OrientationMatrix( this->box.rotation, this->box.center.xyz ); + return this->rotation; +} + +Float4x4 RigidBody::GetRotationMatrix() const +{ // by Dan Andersson + return RotationMatrix( this->rotation ); +} + +Float4x4 RigidBody::GetOrientation() const +{ // by Dan Andersson + return ::Oyster::Math3D::OrientationMatrix( this->rotation, this->centerPos ); } Float4x4 RigidBody::GetView() const { // by Dan Andersson - return InverseOrientationMatrix( this->GetOrientation() ); + return ViewMatrix( this->rotation, this->centerPos ); } -const Float3 & RigidBody::GetBoundingReach() const +Float4 RigidBody::GetVelocity_Linear() const { // by Dan Andersson - return this->box.boundingOffset.xyz; + return Formula::LinearVelocity( this->mass, this->momentum_Linear ); } -Float3 RigidBody::GetSize() const +Float4 RigidBody::GetVelocity_Angular() const { // by Dan Andersson - return 2.0f * this->box.boundingOffset.xyz; + return Formula::AngularVelocity( this->momentOfInertiaTensor.GetInverse(), this->momentum_Angular ); } -const Float3 & RigidBody::GetCenter() const +Float4 RigidBody::GetLinearMomentum( const Float4 &atWorldPos ) const { // by Dan Andersson - return this->box.center.xyz; + return this->momentum_Linear + Formula::TangentialLinearMomentum( this->momentum_Angular, atWorldPos - this->centerPos ); } -const Float3 & RigidBody::GetImpulsTorque() const +void RigidBody::SetMomentOfInertia_KeepVelocity( const Float4x4 &localTensorI ) { // by Dan Andersson - return this->impulseTorqueSum; -} + if( localTensorI.GetDeterminant() != 0.0f ) + { // insanity check! MomentOfInertiaTensor must be invertable + Float4x4 rotationMatrix; RotationMatrix( this->rotation, rotationMatrix ); -const Float3 & RigidBody::GetAngularMomentum() const -{ // by Dan Andersson - return this->angularMomentum; -} - -Float3 RigidBody::GetAngularImpulseAcceleration() const -{ // by Dan Andersson - return Formula::AngularImpulseAcceleration( this->momentOfInertiaTensor.GetInverse(), this->impulseTorqueSum ); -} - -Float3 RigidBody::GetAngularVelocity() const -{ // by Dan Andersson - return Formula::AngularVelocity( this->momentOfInertiaTensor.GetInverse(), this->angularMomentum ); -} - -const Float3 & RigidBody::GetImpulseForce() const -{ // by Dan Andersson - return this->impulseForceSum; -} - -const Float3 & RigidBody::GetLinearMomentum() const -{ // by Dan Andersson - return this->linearMomentum; -} - -Float3 RigidBody::GetLinearImpulseAcceleration() const -{ // by Dan Andersson - return Formula::LinearImpulseAcceleration( this->mass, this->impulseForceSum ); -} - -Float3 RigidBody::GetLinearVelocity() const -{ // by Dan Andersson - return Formula::LinearVelocity( this->mass, this->linearMomentum ); -} - -void RigidBody::GetMomentumAt( const Float3 &worldPos, const Float3 &surfaceNormal, Float3 &normalMomentum, Float3 &tangentialMomentum ) const -{ // by Dan Andersson - Float3 worldOffset = worldPos - this->box.center.xyz; - Float3 momentum = Formula::TangentialLinearMomentum( this->angularMomentum, worldOffset ); - momentum += this->linearMomentum; - - normalMomentum = NormalProjection( momentum, surfaceNormal ); - tangentialMomentum = momentum - normalMomentum; -} - -void RigidBody::SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localI ) -{ // by Dan Andersson - if( localI.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable - { - Float3 w = Formula::AngularVelocity( (this->box.rotation * this->momentOfInertiaTensor).GetInverse(), - this->angularMomentum ); - this->momentOfInertiaTensor = localI; - this->angularMomentum = Formula::AngularMomentum( this->box.rotation*localI, w ); + Float4 w = Formula::AngularVelocity( (rotationMatrix * this->momentOfInertiaTensor).GetInverse(), this->momentum_Angular ); + this->momentOfInertiaTensor = localTensorI; + this->momentum_Angular = Formula::AngularMomentum( rotationMatrix * localTensorI, w ); } } -void RigidBody::SetMomentOfInertia_KeepMomentum( const Float4x4 &localI ) +void RigidBody::SetMomentOfInertia_KeepMomentum( const Float4x4 &localTensorI ) { // by Dan Andersson - if( localI.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable - { - this->momentOfInertiaTensor = localI; + if( localTensorI.GetDeterminant() != 0.0f ) + { // insanity check! MomentOfInertiaTensor must be invertable + this->momentOfInertiaTensor = localTensorI; } } void RigidBody::SetMass_KeepVelocity( const Float &m ) { // by Dan Andersson - if( m != 0.0f ) // insanitycheck! mass must be invertable - { - Float3 v = Formula::LinearVelocity( this->mass, this->linearMomentum ); + if( m != 0.0f ) + { // insanity check! Mass must be invertable + Float4 v = Formula::LinearVelocity( this->mass, this->momentum_Linear ); this->mass = m; - this->linearMomentum = Formula::LinearMomentum( this->mass, v ); + this->momentum_Linear = Formula::LinearMomentum( this->mass, v ); } } void RigidBody::SetMass_KeepMomentum( const Float &m ) { // by Dan Anderson - if( m != 0.0f ) // insanitycheck! mass must be invertable - { + if( m != 0.0f ) + { // insanity check! Mass must be invertable this->mass = m; } } void RigidBody::SetOrientation( const Float4x4 &o ) { // by Dan Andersson - ExtractRotationMatrix( o, this->box.rotation ); - this->box.center = o.v[3].xyz; -} - -void RigidBody::SetSize( const Float3 &widthHeight ) -{ // by Dan Andersson - this->box.boundingOffset = 0.5f * widthHeight; -} - -void RigidBody::SetCenter( const Float3 &worldPos ) -{ // by Dan Andersson - this->box.center = worldPos; + 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->box.rotation = r; + this->axis = ExtractAngularAxis( r ); + this->rotation = Rotation( this->axis ); } -void RigidBody::SetImpulseTorque( const Float3 &worldT ) +void RigidBody::SetMomentum_Linear( const Float4 &worldG, const Float4 &atWorldPos ) { // by Dan Andersson - this->impulseTorqueSum = worldT; + Float4 worldOffset = atWorldPos - this->centerPos; + this->momentum_Linear = VectorProjection( worldG, worldOffset ); + this->momentum_Angular = Formula::AngularMomentum( worldG, worldOffset ); } -void RigidBody::SetAngularMomentum( const Float3 &worldH ) +void RigidBody::SetVelocity_Linear( const Float4 &worldV ) { // by Dan Andersson - this->angularMomentum = worldH; + this->momentum_Linear = Formula::LinearMomentum( this->mass, worldV ); } -void RigidBody::SetAngularImpulseAcceleration( const Float3 &worldA ) +void RigidBody::SetVelocity_Linear( const Float4 &worldV, const Float4 &atWorldPos ) { // by Dan Andersson - this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA ); + 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) ); } -void RigidBody::SetAngularVelocity( const Float3 &worldW ) +void RigidBody::SetVelocity_Angular( const Float4 &worldW ) { // by Dan Andersson - this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW ); + this->momentum_Angular = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW ); } -void RigidBody::SetImpulseForce( const Float3 &worldF ) +void RigidBody::SetImpulse_Linear( const Float4 &worldJ, const Float4 &atWorldPos ) { // by Dan Andersson - this->impulseForceSum = worldF; -} - -void RigidBody::SetLinearMomentum( const Float3 &worldG ) -{ // by Dan Andersson - this->linearMomentum = worldG; -} - -void RigidBody::SetLinearImpulseAcceleration( const Float3 &worldA ) -{ // by Dan Andersson - this->impulseForceSum = Formula::ImpulseForce( this->mass, worldA ); -} - -void RigidBody::SetLinearVelocity( const Float3 &worldV ) -{ // by Dan Andersson - this->linearMomentum = Formula::LinearMomentum( this->mass, worldV ); -} - -void RigidBody::SetImpulseForceAt( const Float3 &worldForce, const Float3 &worldPos ) -{ // by Dan Andersson - Float3 worldOffset = worldPos - this->box.center.xyz; - this->impulseForceSum = VectorProjection( worldForce, worldOffset ); - this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldOffset ); -} - -void RigidBody::SetLinearMomentumAt( const Float3 &worldG, const Float3 &worldPos ) -{ // by Dan Andersson - Float3 worldOffset = worldPos - this->box.center.xyz; - this->linearMomentum = VectorProjection( worldG, worldOffset ); - this->angularMomentum = Formula::AngularMomentum( worldG, worldOffset ); -} - -void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos ) -{ // by Dan Andersson - Float3 worldOffset = worldPos - this->box.center.xyz; - 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 &worldPos ) -{ // by Dan Andersson - Float3 worldOffset = worldPos - this->box.center.xyz; - this->linearMomentum = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) ); - this->angularMomentum = Formula::AngularMomentum( this->box.rotation * this->momentOfInertiaTensor, - Formula::AngularVelocity(worldV, worldOffset) ); + Float4 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 fae7b842..c140982c 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -12,110 +12,74 @@ namespace Oyster { namespace Physics3D { struct RigidBody - { /// A struct of a simple rigid body. + { //! A struct of a simple rigid body. public: - ::Oyster::Collision3D::Box box; /** Contains data representing physical presence. (worldValue) */ - ::Oyster::Math::Float3 angularMomentum, /** The angular momentum H (Nm*s) around an parallell axis. (worldValue) */ - linearMomentum, /** The linear momentum G (kg*m/s). (worldValue) */ - impulseTorqueSum, /** The impulse torque T (Nm) that will be consumed each update. (worldValue) */ - impulseForceSum; /** The impulse force F (N) that will be consumed each update. (worldValue) */ - ::Oyster::Math::Float restitutionCoeff, frictionCoeff_Static, frictionCoeff_Kinetic; + ::Oyster::Math::Float4 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). + momentum_Angular, //!< The angular momentum H (Nm*s) around an parallell axis. + impulse_Linear, //!< The linear impulse sum Jl (kg*m/s) that will be consumed each update. + impulse_Angular; //!< The angular impulse sum Ja (kg*m^2/s) that will be consumed each update. + ::Oyster::Math::Float restitutionCoeff, //!< + frictionCoeff_Static, //!< + frictionCoeff_Kinetic; //!< - RigidBody( const ::Oyster::Collision3D::Box &box = ::Oyster::Collision3D::Box(), - ::Oyster::Math::Float mass = 12.0f, - const ::Oyster::Math::Float4x4 &inertiaTensor = ::Oyster::Math::Float4x4::identity ); + RigidBody(); RigidBody & operator = ( const RigidBody &body ); - bool operator == ( const RigidBody &body ); - bool operator != ( const RigidBody &body ); + void Update_LeapFrog( ::Oyster::Math::Float updateFrameLength ); - void Update_LeapFrog( ::Oyster::Math::Float deltaTime ); - void ApplyImpulseForce( const ::Oyster::Math::Float3 &worldF ); - void ApplyImpulseForceAt( const ::Oyster::Math::Float3 &worldF, const ::Oyster::Math::Float3 &worldPos ); - void ApplyLinearImpulseAcceleration( const ::Oyster::Math::Float3 &worldA ); - void ApplyLinearImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldA, const ::Oyster::Math::Float3 &worldPos ); - void ApplyImpulseTorque( const ::Oyster::Math::Float3 &worldT ); - void ApplyAngularImpulseAcceleration( const ::Oyster::Math::Float3 &worldA ); - - // ACCESS METHODS ///////////////////////////// - - ::Oyster::Math::Float3 & AccessBoundingReach(); - const ::Oyster::Math::Float3 & AccessBoundingReach() const; - ::Oyster::Math::Float3 & AccessCenter(); - const ::Oyster::Math::Float3 & AccessCenter() const; + 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 ); // GET METHODS //////////////////////////////// - const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const; - const ::Oyster::Math::Float & GetMass() const; - - const ::Oyster::Math::Float4x4 GetOrientation() const; - ::Oyster::Math::Float4x4 GetView() const; - const ::Oyster::Math::Float4x4 & GetToWorldMatrix() const; - ::Oyster::Math::Float4x4 GetToLocalMatrix() const; - - const ::Oyster::Math::Float3 & GetBoundingReach() const; - ::Oyster::Math::Float3 GetSize() const; - - const ::Oyster::Math::Float3 & GetCenter() const; - - const ::Oyster::Math::Float3 & GetImpulsTorque() const; - const ::Oyster::Math::Float3 & GetAngularMomentum() const; - ::Oyster::Math::Float3 GetAngularImpulseAcceleration() const; - ::Oyster::Math::Float3 GetAngularVelocity() const; - - const ::Oyster::Math::Float3 & GetImpulseForce() const; - const ::Oyster::Math::Float3 & GetLinearMomentum() const; - ::Oyster::Math::Float3 GetLinearImpulseAcceleration() const; - ::Oyster::Math::Float3 GetLinearVelocity() const; - - void GetMomentumAt( const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &surfaceNormal, ::Oyster::Math::Float3 &normalMomentum, ::Oyster::Math::Float3 &tangentialMomentum ) const; + 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; // SET METHODS //////////////////////////////// - void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localI ); - void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &localI ); + void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localTensorI ); + void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &localTensorI ); void SetMass_KeepVelocity( const ::Oyster::Math::Float &m ); void SetMass_KeepMomentum( const ::Oyster::Math::Float &m ); void SetOrientation( const ::Oyster::Math::Float4x4 &o ); - void SetSize( const ::Oyster::Math::Float3 &widthHeight ); - void SetCenter( const ::Oyster::Math::Float3 &worldPos ); void SetRotation( const ::Oyster::Math::Float4x4 &r ); - - void SetImpulseTorque( const ::Oyster::Math::Float3 &worldT ); - void SetAngularMomentum( const ::Oyster::Math::Float3 &worldH ); - void SetAngularImpulseAcceleration( const ::Oyster::Math::Float3 &worldA ); - void SetAngularVelocity( const ::Oyster::Math::Float3 &worldW ); - - void SetImpulseForce( const ::Oyster::Math::Float3 &worldF ); - void SetLinearMomentum( const ::Oyster::Math::Float3 &worldG ); - void SetLinearImpulseAcceleration( const ::Oyster::Math::Float3 &worldA ); - void SetLinearVelocity( const ::Oyster::Math::Float3 &worldV ); + void SetSize( const ::Oyster::Math::Float4 &widthHeight ); - void SetImpulseForceAt( const ::Oyster::Math::Float3 &worldF, const ::Oyster::Math::Float3 &worldPos ); - void SetLinearMomentumAt( const ::Oyster::Math::Float3 &worldG, const ::Oyster::Math::Float3 &worldPos ); - void SetImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldA, const ::Oyster::Math::Float3 &worldPos ); - void SetLinearVelocityAt( const ::Oyster::Math::Float3 &worldV, const ::Oyster::Math::Float3 &worldPos ); + void SetMomentum_Linear( const ::Oyster::Math::Float4 &worldG, const ::Oyster::Math::Float4 &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 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 ); 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::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::Quaternion rotation; //!< RotationAxis of the body. }; - - // INLINE IMPLEMENTATIONS /////////////////////////////////////// - - inline const ::Oyster::Math::Float4x4 & RigidBody::GetToWorldMatrix() const - { - return this->GetOrientation(); - } - - inline ::Oyster::Math::Float4x4 RigidBody::GetToLocalMatrix() const - { - return this->GetView(); - } - } } +#include "RigidBody_Inline.h" + #endif diff --git a/Code/OysterPhysics3D/RigidBody_Inline.h b/Code/OysterPhysics3D/RigidBody_Inline.h new file mode 100644 index 00000000..980442dd --- /dev/null +++ b/Code/OysterPhysics3D/RigidBody_Inline.h @@ -0,0 +1,65 @@ +///////////////////////////////////////////////////////////////////// +// INLINE IMPLEMENTATIONS +// Created by Dan Andersson 2013 +///////////////////////////////////////////////////////////////////// + +#ifndef OYSTER_PHYSICS_3D_RIGIDBODY_INLINE_H +#define OYSTER_PHYSICS_3D_RIGIDBODY_INLINE_H + +#include "RigidBody.h" + +namespace Oyster { namespace Physics3D +{ + inline void RigidBody::ApplyImpulse_Linear( const ::Oyster::Math::Float4 &worldJ ) + { + this->impulse_Linear += worldJ; + } + + inline void RigidBody::ApplyImpulse_Angular( const ::Oyster::Math::Float4 &worldJ ) + { + this->impulse_Angular += worldJ; + } + + inline void RigidBody::ApplyForce( const ::Oyster::Math::Float4 &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 ) + { + this->ApplyImpulse( worldF * updateFrameLength, atWorldPos ); + } + + inline ::Oyster::Math::Float4x4 RigidBody::GetToWorldMatrix() const + { + return this->GetOrientation(); + } + + inline ::Oyster::Math::Float4x4 RigidBody::GetToLocalMatrix() const + { + return this->GetView(); + } + + inline ::Oyster::Math::Float4 RigidBody::GetSize() const + { + return 2.0f * this->boundingReach; + } + + inline void RigidBody::SetSize( const ::Oyster::Math::Float4 &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 ); + } + +} } + +#endif \ No newline at end of file