diff --git a/Code/OysterPhysics3D/OysterPhysics3D.h b/Code/OysterPhysics3D/OysterPhysics3D.h index 592361ef..3666b917 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.h +++ b/Code/OysterPhysics3D/OysterPhysics3D.h @@ -49,7 +49,7 @@ namespace Oyster { namespace Physics3D } /****************************************************************** - * Returns the local angular momentum of a mass in rotation. + * Returns the world angular momentum of a mass in rotation. * @todo TODO: improve doc ******************************************************************/ inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float4x4 &momentOfInertia, const ::Oyster::Math::Float3 &angularVelocity ) @@ -58,39 +58,39 @@ namespace Oyster { namespace Physics3D } /****************************************************************** - * Returns the local angular momentum of a mass in rotation. + * Returns the world angular momentum of a mass in rotation. * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float3 linearMomentum, const ::Oyster::Math::Float3 &offset ) + inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float3 linearMomentum, const ::Oyster::Math::Float3 &worldOffset ) { return offset.Cross( linearMomentum ); } /****************************************************************** - * Returns the local tangential momentum at localPos, of a mass in rotation. + * Returns the world tangential momentum at worldPos, of a mass in rotation. * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 TangentialLinearMomentum( const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &localOffset ) + inline ::Oyster::Math::Float3 TangentialLinearMomentum( const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &worldOffset ) { - return angularMomentum.Cross( localOffset ); + return angularMomentum.Cross( worldOffset ); } /****************************************************************** - * Returns the local tangential momentum at localPos, of a mass in rotation. + * Returns the world tangential momentum at worldPos, of a mass in rotation. * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 TangentialLinearMomentum( const ::Oyster::Math::Float4x4 &momentOfInertia, const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &localOffset ) + inline ::Oyster::Math::Float3 TangentialLinearMomentum( const ::Oyster::Math::Float4x4 &momentOfInertia, const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &worldOffset ) { - return TangentialLinearMomentum( AngularMomentum(momentOfInertia, angularVelocity), localOffset ); + return TangentialLinearMomentum( AngularMomentum(momentOfInertia, angularVelocity), worldOffset ); } /****************************************************************** - * Returns the local impulse force at localPos, of a mass in angular acceleration. + * Returns the world impulse force at worldPos, of a mass in angular acceleration. * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 TangentialImpulseForce( const ::Oyster::Math::Float3 &impulseTorque, const ::Oyster::Math::Float3 &localOffset ) + inline ::Oyster::Math::Float3 TangentialImpulseForce( const ::Oyster::Math::Float3 &impulseTorque, const ::Oyster::Math::Float3 &worldOffset ) { - return impulseTorque.Cross( localOffset ); + return impulseTorque.Cross( worldOffset ); } /****************************************************************** @@ -106,7 +106,7 @@ namespace Oyster { namespace Physics3D * * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 AngularImpulseAcceleration( const ::Oyster::Math::Float3 &linearImpulseAcceleration, const ::Oyster::Math::Float3 &offset ) + inline ::Oyster::Math::Float3 AngularImpulseAcceleration( const ::Oyster::Math::Float3 &linearImpulseAcceleration, const ::Oyster::Math::Float3 &worldOffset ) { return offset.Cross( linearImpulseAcceleration ); } @@ -121,7 +121,7 @@ namespace Oyster { namespace Physics3D } /****************************************************************** - * Returns the local angular velocity of a mass in rotation. + * Returns the world angular velocity of a mass in rotation. * @todo TODO: improve doc ******************************************************************/ inline ::Oyster::Math::Float3 AngularVelocity( const ::Oyster::Math::Float4x4 &momentOfInertiaInversed, const ::Oyster::Math::Float3 &angularMomentum ) @@ -130,19 +130,19 @@ namespace Oyster { namespace Physics3D } /****************************************************************** - * Returns the local tangential velocity at localPos, of a mass in rotation. + * Returns the world tangential velocity at worldPos, of a mass in rotation. * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &offset ) + inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &worldOffset ) { return angularVelocity.Cross( offset ); } /****************************************************************** - * Returns the local tangential velocity at localPos, of a mass in rotation. + * Returns the world tangential velocity at worldPos, of a mass in rotation. * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float4x4 &momentOfInertiaInversed, const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &offset ) + inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float4x4 &momentOfInertiaInversed, const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &worldOffset ) { return TangentialLinearVelocity( AngularVelocity(momentOfInertiaInversed, angularMomentum), offset ); } @@ -169,7 +169,7 @@ namespace Oyster { namespace Physics3D * * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 ImpulseTorque( const ::Oyster::Math::Float3 & impulseForce, const ::Oyster::Math::Float3 &offset ) + inline ::Oyster::Math::Float3 ImpulseTorque( const ::Oyster::Math::Float3 & impulseForce, const ::Oyster::Math::Float3 &worldOffset ) { return offset.Cross( impulseForce ); } diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 5f36ce15..6ed9da25 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -131,16 +131,6 @@ void RigidBody::ApplyAngularImpulseAcceleration( const Float3 &worldA ) this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA ); } -Float4x4 & RigidBody::AccessOrientation() -{ // by Dan Andersson - return this->box.orientation; -} - -const Float4x4 & RigidBody::AccessOrientation() const -{ // by Dan Andersson - return this->box.orientation; -} - Float3 & RigidBody::AccessBoundingReach() { // by Dan Andersson return this->box.boundingOffset; @@ -171,14 +161,14 @@ const Float & RigidBody::GetMass() const return this->mass; } -const Float4x4 & RigidBody::GetOrientation() const +const Float4x4 RigidBody::GetOrientation() const { // by Dan Andersson - return this->box.orientation; + return OrientationMatrix( this->box.rotation, this->box.center ); } Float4x4 RigidBody::GetView() const { // by Dan Andersson - return InverseOrientationMatrix( this->box.orientation ); + return InverseOrientationMatrix( this->GetOrientation() ); } const Float3 & RigidBody::GetBoundingReach() const @@ -249,18 +239,23 @@ Float3 RigidBody::GetTangentialLinearMomentumAt( const Float3 &worldPos ) const Float3 RigidBody::GetTangentialImpulseAccelerationAt( const Float3 &worldPos ) const { // by Dan Andersson - return this->GetTangentialImpulseAccelerationAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f + Float4x4 invWorldMomentOfInertia = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ).GetInverse(); + Float3 worldOffset = worldPos - this->box.center; + + return Formula::TangentialImpulseAcceleration( invWorldMomentOfInertia, this->impulseTorqueSum, worldOffset ); } Float3 RigidBody::GetTangentialLinearVelocityAt( const Float3 &worldPos ) const { // by Dan Andersson - return this->GetTangentialLinearVelocityAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f + Float4x4 invWorldMomentOfInertia = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ).GetInverse(); + Float3 worldOffset = worldPos - this->box.center; + + return Formula::TangentialLinearVelocity( invWorldMomentOfInertia, this->angularMomentum, worldOffset ); } Float3 RigidBody::GetImpulseForceAt( const Float3 &worldPos ) const { // by Dan Andersson - Float4 localForce = Float4( this->GetImpulseForceAt_Local((this->GetView() * Float4(worldPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f - return (this->box.orientation * localForce).xyz; // should not be any disform thus result.w = 0.0f + return Float3::null; //! @todo TODO: surface normal needed as well. Same goes for those below. } Float3 RigidBody::GetLinearMomentumAt( const Float3 &worldPos ) const