From b8d0a106b30319b40f51ec958aa35f43679110ed Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Wed, 20 Nov 2013 11:09:27 +0100 Subject: [PATCH 01/11] Physics branch disaster Patch critical changes in Sprint1 branch caused major problems for the physics patch. Files had to be backed up, Physics branch destroyed along with it's log data. So that a new Physics branch could be made from the new Sprint1. This patch is those copied files put back into the new physics branch. .. logdata and 3hours lost (1h/person) --- Code/GamePhysics/GamePhysics.vcxproj | 6 +- Code/GamePhysics/GamePhysics.vcxproj.filters | 13 ++- .../Implementation/PhysicsAPI_Impl.cpp | 63 ++++++++++++++ .../Implementation/PhysicsAPI_Impl.h | 34 ++++++++ Code/GamePhysics/PhysicsAPI.h | 58 +++++++++++++ Code/OysterMath/OysterMath.h | 8 +- Code/OysterPhysics3D/RigidBody.cpp | 7 +- Code/OysterPhysics3D/RigidBody.h | 84 ++++++++----------- 8 files changed, 215 insertions(+), 58 deletions(-) create mode 100644 Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp create mode 100644 Code/GamePhysics/Implementation/PhysicsAPI_Impl.h create mode 100644 Code/GamePhysics/PhysicsAPI.h diff --git a/Code/GamePhysics/GamePhysics.vcxproj b/Code/GamePhysics/GamePhysics.vcxproj index 797fdb2c..f3a87a2a 100644 --- a/Code/GamePhysics/GamePhysics.vcxproj +++ b/Code/GamePhysics/GamePhysics.vcxproj @@ -145,7 +145,11 @@ - + + + + + diff --git a/Code/GamePhysics/GamePhysics.vcxproj.filters b/Code/GamePhysics/GamePhysics.vcxproj.filters index b8f7f1a0..8c1b9f8b 100644 --- a/Code/GamePhysics/GamePhysics.vcxproj.filters +++ b/Code/GamePhysics/GamePhysics.vcxproj.filters @@ -16,16 +16,21 @@ {f2cb55b8-47a0-45c7-8dce-5b93f945a57b} - - {cac9a78f-f09b-4850-b1aa-ea87e8368678} - {792daa4b-b2f7-4664-9529-71a929365274} - + Header Files\Include + + Header Files\Implementation + + + + + Header Files\Implementation + \ No newline at end of file diff --git a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp new file mode 100644 index 00000000..09d0b1b5 --- /dev/null +++ b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp @@ -0,0 +1,63 @@ +#include "PhysicsAPI_Impl.h" + +using namespace Oyster; +using namespace Physics; + +API_Impl instance; + +API & Instance() +{ + return instance; +} + +API_Impl::API_Impl() +{ + /** @todo TODO: Fix this constructor.*/ +} + +API_Impl::~API_Impl() +{ + /** @todo TODO: Fix this destructor.*/ +} + +void API_Impl::SetDeltaTime( float deltaTime ) +{ + /** @todo TODO: Fix this function.*/ +} +void API_Impl::SetGravityConstant( float g ) +{ + /** @todo TODO: Fix this function.*/ +} +void API_Impl::SetAction( EventAction_Collision functionPointer ) +{ + /** @todo TODO: Fix this function.*/ +} +void API_Impl::SetAction( EventAction_Destruction functionPointer ) +{ + /** @todo TODO: Fix this function.*/ +} + +void API_Impl::Update() +{ + /** @todo TODO: Fix this function.*/ +} + +void API_Impl::MoveToLimbo( unsigned int objRef ) +{ + /** @todo TODO: Fix this function.*/ +} +void API_Impl::ReleaseFromLimbo( unsigned int objRef ) +{ + /** @todo TODO: Fix this function.*/ +} + +unsigned int API_Impl::AddObject( ::Utility::DynamicMemory::UniquePointer handle ) +{ + /** @todo TODO: Fix this function.*/ + + return 0; +} +void API_Impl::DestroyObject( unsigned int objRef ) +{ + /** @todo TODO: Fix this function.*/ +} \ No newline at end of file diff --git a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.h b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.h new file mode 100644 index 00000000..445080b5 --- /dev/null +++ b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.h @@ -0,0 +1,34 @@ +#ifndef PHYSICS_API_IMPL_H +#define PHYSICS_API_IMPL_H + +#include "../PhysicsAPI.h" + +namespace Oyster +{ + namespace Physics + { + class API_Impl : public API + { + public: + API_Impl(); + virtual ~API_Impl(); + + void SetDeltaTime( float deltaTime ); + void SetGravityConstant( float g ); + void SetAction( EventAction_Collision functionPointer ); + void SetAction( EventAction_Destruction functionPointer ); + + void Update(); + + void MoveToLimbo( unsigned int objRef ); + void ReleaseFromLimbo( unsigned int objRef ); + + unsigned int AddObject( ::Utility::DynamicMemory::UniquePointer handle ); + void DestroyObject( unsigned int objRef ); + }; + + } + +} + +#endif \ No newline at end of file diff --git a/Code/GamePhysics/PhysicsAPI.h b/Code/GamePhysics/PhysicsAPI.h new file mode 100644 index 00000000..5711b277 --- /dev/null +++ b/Code/GamePhysics/PhysicsAPI.h @@ -0,0 +1,58 @@ +#ifndef PHYSICS_API_H +#define PHYSICS_API_H + +#include "OysterMath.h" +#include "Utilities.h" + +namespace Oyster +{ + namespace Physics + { + class API; + class IRigidBody; + class IParticle; + + namespace Constant + { + const float gravity_constant = (const float)6.67284e-11; // The _big_G_! ( N(m/kg)^2 ) Used in real gravityforcefields. + } + + class API + { + public: + typedef void (*EventAction_Collision)( unsigned int, unsigned int ); + typedef void (*EventAction_Destruction)( unsigned int, ::Utility::DynamicMemory::UniquePointer ); + + static API & Instance(); + + virtual void SetDeltaTime( float deltaTime ) = 0; + virtual void SetGravityConstant( float g ) = 0; + virtual void SetAction( EventAction_Collision functionPointer ) = 0; + virtual void SetAction( EventAction_Destruction functionPointer ) = 0; + + virtual void Update() = 0; + + virtual void MoveToLimbo( unsigned int objRef ); + virtual void ReleaseFromLimbo( unsigned int objRef ); + + virtual unsigned int AddObject( ::Utility::DynamicMemory::UniquePointer handle ); + virtual void DestroyObject( unsigned int objRef ); + }; + + class IRigidBody + { + public: + + }; + + class IParticle + { + public: + + }; + } + + namespace Collision + {} +} +#endif \ No newline at end of file diff --git a/Code/OysterMath/OysterMath.h b/Code/OysterMath/OysterMath.h index 07ae50d3..25381140 100644 --- a/Code/OysterMath/OysterMath.h +++ b/Code/OysterMath/OysterMath.h @@ -218,10 +218,14 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized inline Float4x4 & ViewProjectionMatrix( const Float4x4 &view, const Float4x4 &projection, Float4x4 &targetMem = Float4x4() ) { return targetMem = projection * view; } - /// Helper inline function that sets and then returns targetMem = transformer * transformee - inline Float4x4 & TransformMatrix( const Float4x4 &transformer, const Float4x4 &transformee, Float4x4 &targetMem = Float4x4() ) + /** Helper inline function that sets and then returns targetMem = transformer * transformee */ + inline Float4x4 & TransformMatrix( const Float4x4 &transformer, const Float4x4 &transformee, Float4x4 &targetMem ) { return targetMem = transformer * transformee; } + /** Helper inline function that sets and then returns transformer * transformee */ + inline Float4x4 TransformMatrix( const Float4x4 &transformer, const Float4x4 &transformee ) + { return 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/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index fff6b994..319d4da4 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -39,6 +39,9 @@ RigidBody & RigidBody::operator = ( const RigidBody &body ) void RigidBody::Update_LeapFrog( Float deltaTime ) { // 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.orientation, this->momentOfInertiaTensor ); + // updating the linear // dv = dt * a = dt * F / m // ds = dt * avg_v @@ -49,7 +52,7 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) // updating the angular // dw = dt * a = dt * ( I^-1 * T ) // rotation = dt * avg_w - Float4x4 inversedMomentOfInertiaTensor = this->momentOfInertiaTensor.GetInverse(); + Float4x4 inversedMomentOfInertiaTensor = wMomentOfInertiaTensor.GetInverse(); Float3 deltaAngularVelocity = Formula::AngularImpulseAcceleration( inversedMomentOfInertiaTensor, this->impulseTorqueSum ); // I^-1 * T deltaAngularVelocity *= deltaTime; Float3 rotationAxis = ::Utility::Value::AverageWithDelta( Formula::AngularVelocity(inversedMomentOfInertiaTensor,this->angularMomentum), deltaAngularVelocity ); @@ -73,7 +76,7 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) // update movements and clear impulses this->linearMomentum += Formula::LinearMomentum( this->mass, deltaLinearVelocity ); this->impulseForceSum = Float3::null; - this->angularMomentum += Formula::AngularMomentum( this->momentOfInertiaTensor, deltaAngularVelocity ); + this->angularMomentum += Formula::AngularMomentum( wMomentOfInertiaTensor, deltaAngularVelocity ); this->impulseTorqueSum = Float3::null; } diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index a2d32f95..4184ca2e 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -14,25 +14,23 @@ namespace Oyster { namespace Physics3D struct RigidBody { /// A struct of a simple rigid body. public: - ::Oyster::Collision3D::Box box; /// Contains data representing physical presence. - ::Oyster::Math::Float3 angularMomentum, /// The angular momentum H (Nm*s) around an parallell axis. - linearMomentum, /// The linear momentum G (kg*m/s). - impulseTorqueSum, /// The impulse torque T (Nm) that will be consumed each update. - impulseForceSum; /// The impulse force F (N) that will be consumed each update. + ::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) */ RigidBody( const ::Oyster::Collision3D::Box &box = ::Oyster::Collision3D::Box(), ::Oyster::Math::Float mass = 1.0f ); RigidBody & operator = ( const RigidBody &body ); void Update_LeapFrog( ::Oyster::Math::Float deltaTime ); - void ApplyImpulseForce( const ::Oyster::Math::Float3 &f ); - void ApplyImpulseForceAt_Local( const ::Oyster::Math::Float3 &f, const ::Oyster::Math::Float3 &pos ); - void ApplyImpulseForceAt_World( const ::Oyster::Math::Float3 &f, const ::Oyster::Math::Float3 &pos ); /// ApplyImpulseForce_LocalPos is preferred - void ApplyLinearImpulseAcceleration( const ::Oyster::Math::Float3 &a ); - void ApplyLinearImpulseAccelerationAt_Local( const ::Oyster::Math::Float3 &a, const ::Oyster::Math::Float3 &pos ); - void ApplyLinearImpulseAccelerationAt_World( const ::Oyster::Math::Float3 &a, const ::Oyster::Math::Float3 &pos ); /// ApplyLinearImpulseAcceleration_LocalPos is preferred - void ApplyImpulseTorque( const ::Oyster::Math::Float3 &t ); - void ApplyAngularImpulseAcceleration( const ::Oyster::Math::Float3 &a ); + 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 ///////////////////////////// @@ -68,56 +66,44 @@ namespace Oyster { namespace Physics3D ::Oyster::Math::Float3 GetLinearImpulseAcceleration() const; ::Oyster::Math::Float3 GetLinearVelocity() const; - ::Oyster::Math::Float3 GetTangentialImpulseForceAt_Local( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetTangentialImpulseForceAt_World( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetTangentialLinearMomentumAt_Local( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetTangentialLinearMomentumAt_World( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetTangentialImpulseAccelerationAt_Local( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetTangentialImpulseAccelerationAt_World( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetTangentialLinearVelocityAt_Local( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetTangentialLinearVelocityAt_World( const ::Oyster::Math::Float3 &pos ) const; + ::Oyster::Math::Float3 GetTangentialImpulseForceAt( const ::Oyster::Math::Float3 &worldPos ) const; + ::Oyster::Math::Float3 GetTangentialLinearMomentumAt( const ::Oyster::Math::Float3 &worldPos ) const; + ::Oyster::Math::Float3 GetTangentialImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldPos ) const; + ::Oyster::Math::Float3 GetTangentialLinearVelocityAt( const ::Oyster::Math::Float3 &worldPos ) const; - ::Oyster::Math::Float3 GetImpulseForceAt_Local( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetImpulseForceAt_World( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetLinearMomentumAt_Local( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetLinearMomentumAt_World( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetImpulseAccelerationAt_Local( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetImpulseAccelerationAt_World( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetLinearVelocityAt_Local( const ::Oyster::Math::Float3 &pos ) const; - ::Oyster::Math::Float3 GetLinearVelocityAt_World( const ::Oyster::Math::Float3 &pos ) const; + ::Oyster::Math::Float3 GetImpulseForceAt( const ::Oyster::Math::Float3 &worldPos ) const; + ::Oyster::Math::Float3 GetLinearMomentumAt( const ::Oyster::Math::Float3 &worldPos ) const; + ::Oyster::Math::Float3 GetImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldPos ) const; + ::Oyster::Math::Float3 GetLinearVelocityAt( const ::Oyster::Math::Float3 &worldPos ) const; // SET METHODS //////////////////////////////// - void SetMomentOfInertia( const ::Oyster::Math::Float4x4 &i ); + void SetMomentOfInertia( const ::Oyster::Math::Float4x4 &localI ); 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 &p ); + void SetCenter( const ::Oyster::Math::Float3 &worldPos ); - void SetImpulseTorque( const ::Oyster::Math::Float3 &t ); - void SetAngularMomentum( const ::Oyster::Math::Float3 &h ); - void SetAngularImpulseAcceleration( const ::Oyster::Math::Float3 &a ); - void SetAngularVelocity( const ::Oyster::Math::Float3 &w ); + 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 &f ); - void SetLinearMomentum( const ::Oyster::Math::Float3 &g ); - void SetLinearImpulseAcceleration( const ::Oyster::Math::Float3 &a ); - void SetLinearVelocity( const ::Oyster::Math::Float3 &v ); + 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 SetImpulseForceAt_Local( const ::Oyster::Math::Float3 &f, const ::Oyster::Math::Float3 &pos ); - void SetImpulseForceAt_World( const ::Oyster::Math::Float3 &f, const ::Oyster::Math::Float3 &pos ); - void SetLinearMomentumAt_Local( const ::Oyster::Math::Float3 &g, const ::Oyster::Math::Float3 &pos ); - void SetLinearMomentumAt_World( const ::Oyster::Math::Float3 &g, const ::Oyster::Math::Float3 &pos ); - void SetImpulseAccelerationAt_Local( const ::Oyster::Math::Float3 &a, const ::Oyster::Math::Float3 &pos ); - void SetImpulseAccelerationAt_World( const ::Oyster::Math::Float3 &a, const ::Oyster::Math::Float3 &pos ); - void SetLinearVelocityAt_Local( const ::Oyster::Math::Float3 &v, const ::Oyster::Math::Float3 &pos ); - void SetLinearVelocityAt_World( const ::Oyster::Math::Float3 &v, const ::Oyster::Math::Float3 &pos ); + 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 ); 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 ) + ::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) */ }; // INLINE IMPLEMENTATIONS /////////////////////////////////////// From e41d16cc0312cf964d47c528522a81ba4d9b1234 Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Wed, 20 Nov 2013 11:36:17 +0100 Subject: [PATCH 02/11] Task assignment in RigidBody.cpp --- Code/OysterPhysics3D/RigidBody.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 319d4da4..32395d8d 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -259,6 +259,10 @@ Float3 RigidBody::GetTangentialImpulseForceAt_World( const Float3 &worldPos ) co return this->GetTangentialImpulseForceAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f } +// Dan +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Robin + Float3 RigidBody::GetTangentialLinearMomentumAt_Local( const Float3 &localPos ) const { // by Dan Andersson return Formula::TangentialLinearMomentum( this->angularMomentum, localPos ); From 2149ca8ab5ab23bd4cedffb81b1e9f09f0bc7357 Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Wed, 20 Nov 2013 11:44:54 +0100 Subject: [PATCH 03/11] Dans fix part of RigidBody.cpp DONE --- Code/OysterPhysics3D/RigidBody.cpp | 62 +++++++++++------------------- 1 file changed, 23 insertions(+), 39 deletions(-) diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 32395d8d..7af77118 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -80,68 +80,56 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) this->impulseTorqueSum = Float3::null; } -void RigidBody::ApplyImpulseForce( const Float3 &f ) +void RigidBody::ApplyImpulseForce( const Float3 &worldF ) { // by Dan Andersson - this->impulseForceSum += f; + this->impulseForceSum += worldF; } -void RigidBody::ApplyImpulseForceAt_Local( const Float3 &localForce, const Float3 &localOffset ) +void RigidBody::ApplyImpulseForceAt( const Float3 &worldF, const Float3 &worldPos ) { // by Dan Andersson - if( localOffset != Float3::null ) + Float3 worldOffset = worldPos - this->box.center; + if( worldOffset != Float3::null ) { - this->impulseForceSum += VectorProjection( localForce, localOffset ); - this->impulseTorqueSum += Formula::ImpulseTorque( localForce, localOffset ); + this->impulseForceSum += VectorProjection( worldF, worldOffset ); + this->impulseTorqueSum += Formula::ImpulseTorque( worldF, worldOffset ); } else { - this->impulseForceSum += localForce; + this->impulseForceSum += worldF; } } -void RigidBody::ApplyImpulseForceAt_World( const Float3 &worldForce, const Float3 &worldPos ) +void RigidBody::ApplyLinearImpulseAcceleration( const Float3 &worldA ) { // by Dan Andersson - Float4x4 view = this->GetView(); - this->ApplyImpulseForceAt_Local( (view * Float4(worldForce, 0.0f)).xyz, - (view * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f + this->impulseForceSum += Formula::ImpulseForce( this->mass, worldA ); } -void RigidBody::ApplyLinearImpulseAcceleration( const Float3 &a ) +void RigidBody::ApplyLinearImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos ) { // by Dan Andersson - this->impulseForceSum += Formula::ImpulseForce( this->mass, a ); -} - -void RigidBody::ApplyLinearImpulseAccelerationAt_Local( const Float3 &localImpulseLinearAcc, const Float3 &localOffset ) -{ // by Dan Andersson - if( localOffset != Float3::null ) + Float3 worldOffset = worldPos - this->box.center; + if( worldOffset != Float3::null ) { - this->impulseForceSum += Formula::ImpulseForce( this->mass, VectorProjection(localImpulseLinearAcc, localOffset) ); + 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(localImpulseLinearAcc, localOffset) ); + this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, Formula::AngularImpulseAcceleration(worldA, worldOffset) ); } else { - this->impulseForceSum += Formula::ImpulseForce( this->mass, localImpulseLinearAcc ); + this->impulseForceSum += Formula::ImpulseForce( this->mass, worldA ); } } -void RigidBody::ApplyLinearImpulseAccelerationAt_World( const Float3 &worldImpulseLinearAcc, const Float3 &worldPos ) +void RigidBody::ApplyImpulseTorque( const Float3 &worldT ) { // by Dan Andersson - Float4x4 view = this->GetView(); - this->ApplyLinearImpulseAccelerationAt_Local( (view * Float4(worldImpulseLinearAcc, 0.0f)).xyz, - (view * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f + this->impulseTorqueSum += worldT; } -void RigidBody::ApplyImpulseTorque( const Float3 &t ) +void RigidBody::ApplyAngularImpulseAcceleration( const Float3 &worldA ) { // by Dan Andersson - this->impulseTorqueSum += t; -} - -void RigidBody::ApplyAngularImpulseAcceleration( const Float3 &a ) -{ // by Dan Andersson - this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, a ); + this->impulseTorqueSum += Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA ); } Float4x4 & RigidBody::AccessOrientation() @@ -249,14 +237,10 @@ Float3 RigidBody::GetLinearVelocity() const return Formula::LinearVelocity( this->mass, this->linearMomentum ); } -Float3 RigidBody::GetTangentialImpulseForceAt_Local( const Float3 &localPos ) const +Float3 RigidBody::GetTangentialImpulseForceAt( const Float3 &worldPos ) const { // by Dan Andersson - return Formula::TangentialImpulseForce( this->impulseTorqueSum, localPos ); -} - -Float3 RigidBody::GetTangentialImpulseForceAt_World( const Float3 &worldPos ) const -{ // by Dan Andersson - return this->GetTangentialImpulseForceAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f + Float3 worldOffset = worldPos - this->box.center; + return Formula::TangentialImpulseForce( this->impulseTorqueSum, worldOffset ); } // Dan From 80301ae02ebd2af921bee1c57ba479b7a9bf4ec3 Mon Sep 17 00:00:00 2001 From: Robin Engman Date: Wed, 20 Nov 2013 11:48:39 +0100 Subject: [PATCH 04/11] Robin's fix part of RigidBody.cpp DONE --- Code/OysterPhysics3D/RigidBody.cpp | 115 ++++++----------------------- 1 file changed, 22 insertions(+), 93 deletions(-) diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 7af77118..f6d0695d 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -243,58 +243,28 @@ Float3 RigidBody::GetTangentialImpulseForceAt( const Float3 &worldPos ) const return Formula::TangentialImpulseForce( this->impulseTorqueSum, worldOffset ); } -// Dan -////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// Robin - -Float3 RigidBody::GetTangentialLinearMomentumAt_Local( const Float3 &localPos ) const +Float3 RigidBody::GetTangentialLinearMomentumAt( const Float3 &worldPos ) const { // by Dan Andersson - return Formula::TangentialLinearMomentum( this->angularMomentum, localPos ); + return Formula::TangentialLinearMomentum( this->angularMomentum, worldPos ); } -Float3 RigidBody::GetTangentialLinearMomentumAt_World( const Float3 &worldPos ) const -{ // by Dan Andersson - return this->GetTangentialLinearMomentumAt_Local( (this->GetView() * Float4(worldPos, 1.0f)).xyz ); // should not be any disform thus result.w = 1.0f -} - -Float3 RigidBody::GetTangentialImpulseAccelerationAt_Local( const Float3 &localPos ) const -{ // by Dan Andersson - return Formula::TangentialImpulseAcceleration( this->momentOfInertiaTensor.GetInverse(), this->impulseTorqueSum, localPos ); -} - -Float3 RigidBody::GetTangentialImpulseAccelerationAt_World( 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 } -Float3 RigidBody::GetTangentialLinearVelocityAt_Local( const Float3 &localPos ) const -{ // by Dan Andersson - return Formula::TangentialLinearVelocity( this->momentOfInertiaTensor.GetInverse(), this->angularMomentum, localPos ); -} - -Float3 RigidBody::GetTangentialLinearVelocityAt_World( const Float3 &worldPos ) const +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 } -Float3 RigidBody::GetImpulseForceAt_Local( const Float3 &localPos ) const -{ // by Dan Andersson - return this->impulseForceSum + Formula::TangentialImpulseForce( this->impulseForceSum, localPos ); -} - -Float3 RigidBody::GetImpulseForceAt_World( const Float3 &worldPos ) const +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 } -Float3 RigidBody::GetLinearMomentumAt_Local( const Float3 &localPos ) const -{ // by Dan Andersson - // Reminder! Momentum is a world value. - return Float3::null; // TODO: -} - -Float3 RigidBody::GetLinearMomentumAt_World( const Float3 &worldPos ) const +Float3 RigidBody::GetLinearMomentumAt( const Float3 &worldPos ) const { // by Dan Andersson // Reminder! Momentum is a world value. Float4 localMomentum = Float4( this->GetLinearMomentumAt_Local((this->GetView() * Float4(worldPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f @@ -304,35 +274,21 @@ Float3 RigidBody::GetLinearMomentumAt_World( const Float3 &worldPos ) const return this->linearMomentum + Formula::TangentialLinearMomentum( this->angularMomentum, worldPos ); } -Float3 RigidBody::GetImpulseAccelerationAt_Local( const Float3 &localPos ) const -{ // by Dan Andersson - // Reminder! Acceleration is a world value. - Float4 worldAccel = Float4( this->GetImpulseAccelerationAt_Local((this->box.orientation * Float4(localPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f - return (this->GetView() * worldAccel).xyz; // should not be any disform thus result.w = 0.0f -} - -Float3 RigidBody::GetImpulseAccelerationAt_World( const Float3 &worldPos ) const +Float3 RigidBody::GetImpulseAccelerationAt( const Float3 &worldPos ) const { // by Dan Andersson // Reminder! Acceleration is a world value. return Formula::LinearImpulseAcceleration( this->mass, this->impulseForceSum ) + Formula::TangentialImpulseAcceleration( this->momentOfInertiaTensor.GetInverse(), this->impulseTorqueSum, worldPos ); } -Float3 RigidBody::GetLinearVelocityAt_Local( const Float3 &localPos ) const -{ // by Dan Andersson - // Reminder! Velocity is a world value. - Float4 worldV = Float4( this->GetLinearVelocityAt_Local((this->box.orientation * Float4(localPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f - return (this->GetView() * worldV).xyz; // should not be any disform thus result.w = 0.0f -} - -Float3 RigidBody::GetLinearVelocityAt_World( const Float3 &worldPos ) const +Float3 RigidBody::GetLinearVelocityAt( const Float3 &worldPos ) const { // by Dan Andersson // Reminder! Velocity is a world value. return Formula::LinearVelocity( this->mass, this->linearMomentum ) + Formula::TangentialLinearVelocity( this->momentOfInertiaTensor.GetInverse(), this->angularMomentum, worldPos ); } -void RigidBody::SetMomentOfInertia( const Float4x4 &i ) +void RigidBody::SetMomentOfInertia( const Float4x4 &localI ) { // by Dan Andersson if( i.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable { @@ -368,98 +324,71 @@ void RigidBody::SetSize( const Float3 &widthHeight ) this->box.boundingOffset = 0.5f * widthHeight; } -void RigidBody::SetCenter( const Float3 &p ) +void RigidBody::SetCenter( const Float3 &worldPos ) { // by Dan Andersson this->box.center = p; } -void RigidBody::SetImpulseTorque( const Float3 &t ) +void RigidBody::SetImpulseTorque( const Float3 &worldT ) { // by Dan Andersson this->impulseTorqueSum = t; } -void RigidBody::SetAngularMomentum( const Float3 &h ) +void RigidBody::SetAngularMomentum( const Float3 &worldH ) { // by Dan Andersson this->angularMomentum = h; } -void RigidBody::SetAngularImpulseAcceleration( const Float3 &a ) +void RigidBody::SetAngularImpulseAcceleration( const Float3 &worldA ) { // by Dan Andersson this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, a ); } -void RigidBody::SetAngularVelocity( const Float3 &w ) +void RigidBody::SetAngularVelocity( const Float3 &worldW ) { // by Dan Andersson this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, w ); } -void RigidBody::SetImpulseForce( const Float3 &f ) +void RigidBody::SetImpulseForce( const Float3 &worldF ) { // by Dan Andersson this->impulseForceSum = f; } -void RigidBody::SetLinearMomentum( const Float3 &g ) +void RigidBody::SetLinearMomentum( const Float3 &worldG ) { // by Dan Andersson this->linearMomentum = g; } -void RigidBody::SetLinearImpulseAcceleration( const Float3 &a ) +void RigidBody::SetLinearImpulseAcceleration( const Float3 &worldA ) { // by Dan Andersson this->impulseForceSum = Formula::ImpulseForce( this->mass, a ); } -void RigidBody::SetLinearVelocity( const Float3 &v ) +void RigidBody::SetLinearVelocity( const Float3 &worldV ) { // by Dan Andersson this->linearMomentum = Formula::LinearMomentum( this->mass, v ); } -void RigidBody::SetImpulseForceAt_Local( const Float3 &localForce, const Float3 &localPos ) -{ // by Dan Andersson - // Reminder! Impulse force and torque is world values. - Float3 worldForce = ( this->box.orientation * Float4(localForce, 0.0f) ).xyz, - worldPos = ( this->box.orientation * Float4(localPos, 1.0f) ).xyz; - this->SetImpulseForceAt_World( worldForce, worldPos ); - -} - -void RigidBody::SetImpulseForceAt_World( const Float3 &worldForce, const Float3 &worldPos ) +void RigidBody::SetImpulseForceAt( const Float3 &worldF, const Float3 &worldPos ) { // by Dan Andersson // Reminder! Impulse force and torque is world values. this->impulseForceSum = VectorProjection( worldForce, worldPos ); this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldPos ); } -void RigidBody::SetLinearMomentumAt_Local( const Float3 &localG, const Float3 &localPos ) -{ // by Dan Andersson - // Reminder! Linear and angular momentum is world values. - Float3 worldG = ( this->box.orientation * Float4(localG, 0.0f) ).xyz, - worldPos = ( this->box.orientation * Float4(localPos, 1.0f) ).xyz; - this->SetLinearMomentumAt_World( worldG, worldPos ); -} - -void RigidBody::SetLinearMomentumAt_World( const Float3 &worldG, const Float3 &worldPos ) +void RigidBody::SetLinearMomentumAt( const Float3 &worldG, const Float3 &worldPos ) { // by Dan Andersson // Reminder! Linear and angular momentum is world values. this->linearMomentum = VectorProjection( worldG, worldPos ); this->angularMomentum = Formula::AngularMomentum( worldG, worldPos ); } -void RigidBody::SetImpulseAccelerationAt_Local( const Float3 &a, const Float3 &pos ) -{ // by Dan Andersson - -} - -void RigidBody::SetImpulseAccelerationAt_World( const Float3 &a, const Float3 &pos ) +void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &pos ) { // by } -void RigidBody::SetLinearVelocityAt_Local( const Float3 &v, const Float3 &pos ) -{ // by - -} - -void RigidBody::SetLinearVelocityAt_World( const Float3 &v, const Float3 &pos ) +void RigidBody::SetLinearVelocityAt( const Float3 &worldV, const Float3 &pos ) { // by } \ No newline at end of file From 687fb6835a39ad9c40b715731c85b64c8bdcdbe3 Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Wed, 20 Nov 2013 11:58:27 +0100 Subject: [PATCH 05/11] TangentialImpulseAcceleration fixed selfexplanatory documentation --- Code/OysterPhysics3D/OysterPhysics3D.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Code/OysterPhysics3D/OysterPhysics3D.h b/Code/OysterPhysics3D/OysterPhysics3D.h index a5d58c0b..ad049907 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.h +++ b/Code/OysterPhysics3D/OysterPhysics3D.h @@ -112,12 +112,12 @@ namespace Oyster { namespace Physics3D } /****************************************************************** - * Returns the local impulse acceleration at localPos, of a mass in angular acceleration. + * Returns the world impulse acceleration at ( worldOffset = worldPos - body's center of gravity ), of a mass in angular acceleration. * @todo TODO: improve doc ******************************************************************/ - inline ::Oyster::Math::Float3 TangentialImpulseAcceleration( const ::Oyster::Math::Float4x4 &momentOfInertiaInversed, const ::Oyster::Math::Float3 &impulseTorque, const ::Oyster::Math::Float3 &localOffset ) + inline ::Oyster::Math::Float3 TangentialImpulseAcceleration( const ::Oyster::Math::Float4x4 &worldMomentOfInertiaInversed, const ::Oyster::Math::Float3 &worldImpulseTorque, const ::Oyster::Math::Float3 &worldOffset ) { - return AngularImpulseAcceleration( momentOfInertiaInversed, impulseTorque ).Cross( localOffset ); + return AngularImpulseAcceleration( worldMomentOfInertiaInversed, worldImpulseTorque ).Cross( worldOffset ); } /****************************************************************** From da6eda2a8c78959b0afe6de4fd95f952c1b99c6c Mon Sep 17 00:00:00 2001 From: Robin Engman Date: Wed, 20 Nov 2013 13:48:54 +0100 Subject: [PATCH 06/11] Commented non-functional features. --- Code/OysterPhysics3D/Box.cpp | 14 +++++++------- Code/OysterPhysics3D/BoxAxisAligned.cpp | 18 +++++++++--------- Code/OysterPhysics3D/Frustrum.cpp | 20 ++++++++++---------- Code/OysterPhysics3D/ICollideable.h | 2 +- 4 files changed, 27 insertions(+), 27 deletions(-) diff --git a/Code/OysterPhysics3D/Box.cpp b/Code/OysterPhysics3D/Box.cpp index a20b8195..ed42ba76 100644 --- a/Code/OysterPhysics3D/Box.cpp +++ b/Code/OysterPhysics3D/Box.cpp @@ -31,10 +31,10 @@ bool Box::Intersects( const ICollideable *target ) const case Type_ray: return Utility::Intersect( *this, *(Ray*)target, ((Ray*)target)->collisionDistance ); case Type_sphere: return Utility::Intersect( *this, *(Sphere*)target ); case Type_plane: return Utility::Intersect( *this, *(Plane*)target ); - case Type_triangle: return false; // TODO: : + // case Type_triangle: return false; // TODO: : case Type_box_axis_aligned: return Utility::Intersect( *this, *(BoxAxisAligned*)target ); case Type_box: return Utility::Intersect( *this, *(Box*)target ); - case Type_frustrum: return false; // TODO: : + // case Type_frustrum: return false; // TODO: : default: return false; } } @@ -44,11 +44,11 @@ bool Box::Contains( const ICollideable *target ) const switch( target->type ) { case Type_point: return Utility::Intersect( *this, *(Point*)target ); - case Type_sphere: return false; // TODO: - case Type_triangle: return false; // TODO: - case Type_box_axis_aligned: return false; // TODO: - case Type_box: return false; // TODO: - case Type_frustrum: return false; // TODO: + // case Type_sphere: return false; // TODO: + // case Type_triangle: return false; // TODO: + // case Type_box_axis_aligned: return false; // TODO: + // case Type_box: return false; // TODO: + // case Type_frustrum: return false; // TODO: default: return false; } } \ No newline at end of file diff --git a/Code/OysterPhysics3D/BoxAxisAligned.cpp b/Code/OysterPhysics3D/BoxAxisAligned.cpp index c782ed25..04bc53f1 100644 --- a/Code/OysterPhysics3D/BoxAxisAligned.cpp +++ b/Code/OysterPhysics3D/BoxAxisAligned.cpp @@ -33,10 +33,10 @@ bool BoxAxisAligned::Intersects( const ICollideable *target ) const case Type_ray: return Utility::Intersect( *this, *(Ray*)target, ((Ray*)target)->collisionDistance ); case Type_sphere: return Utility::Intersect( *this, *(Sphere*)target ); case Type_plane: return Utility::Intersect( *this, *(Plane*)target ); - case Type_triangle: return false; // TODO: + // case Type_triangle: return false; // TODO: case Type_box_axis_aligned: return Utility::Intersect( *this, *(BoxAxisAligned*)target ); - case Type_box: return false; // TODO: - case Type_frustrum: return false; // TODO: + // case Type_box: return false; // TODO: + // case Type_frustrum: return false; // TODO: default: return false; } } @@ -45,12 +45,12 @@ bool BoxAxisAligned::Contains( const ICollideable *target ) const { switch( target->type ) { - case Type_point: return false; // TODO: - case Type_sphere: return false; // TODO: - case Type_triangle: return false; // TODO: - case Type_box_axis_aligned: return false; // TODO: - case Type_box: return false; // TODO: - case Type_frustrum: return false; // TODO: + // case Type_point: return false; // TODO: + // case Type_sphere: return false; // TODO: + // case Type_triangle: return false; // TODO: + // case Type_box_axis_aligned: return false; // TODO: + // case Type_box: return false; // TODO: + // case Type_frustrum: return false; // TODO: default: return false; } } \ No newline at end of file diff --git a/Code/OysterPhysics3D/Frustrum.cpp b/Code/OysterPhysics3D/Frustrum.cpp index e4da0ce9..9e45f579 100644 --- a/Code/OysterPhysics3D/Frustrum.cpp +++ b/Code/OysterPhysics3D/Frustrum.cpp @@ -200,11 +200,11 @@ bool Frustrum::Intersects( const ICollideable *target ) const case Type_ray: return Utility::Intersect( *this, *(Ray*)target, ((Ray*)target)->collisionDistance ); case Type_sphere: return Utility::Intersect( *this, *(Sphere*)target ); case Type_plane: return Utility::Intersect( *this, *(Plane*)target ); - case Type_triangle: return false; // TODO: + // case Type_triangle: return false; // TODO: case Type_box_axis_aligned: return Utility::Intersect( *this, *(BoxAxisAligned*)target ); case Type_box: return Utility::Intersect( *this, *(Box*)target ); case Type_frustrum: return Utility::Intersect( *this, *(Frustrum*)target ); - case Type_line: return false; // TODO: + // case Type_line: return false; // TODO: default: return false; } } @@ -214,14 +214,14 @@ bool Frustrum::Contains( const ICollideable *target ) const switch( target->type ) { case Type_point: return Utility::Intersect( *this, *(Point*)target ); - case Type_ray: return false; // TODO: - case Type_sphere: return false; // TODO: - case Type_plane: return false; // TODO: - case Type_triangle: return false; // TODO: - case Type_box_axis_aligned: return false; // TODO: - case Type_box: return false; // TODO: - case Type_frustrum: return false; // TODO: - case Type_line: return false; // TODO: + // case Type_ray: return false; // TODO: + // case Type_sphere: return false; // TODO: + // case Type_plane: return false; // TODO: + // case Type_triangle: return false; // TODO: + // case Type_box_axis_aligned: return false; // TODO: + // case Type_box: return false; // TODO: + // case Type_frustrum: return false; // TODO: + // case Type_line: return false; // TODO: default: return false; } } \ No newline at end of file diff --git a/Code/OysterPhysics3D/ICollideable.h b/Code/OysterPhysics3D/ICollideable.h index aa6c7891..4b620506 100644 --- a/Code/OysterPhysics3D/ICollideable.h +++ b/Code/OysterPhysics3D/ICollideable.h @@ -21,7 +21,7 @@ namespace Oyster { namespace Collision3D /// Contains a collection of 3D shapes Type_ray, Type_sphere, Type_plane, - Type_triangle, + // Type_triangle, Type_box_axis_aligned, Type_box, Type_frustrum, From baec4b0392ad997d93ae566a0f25bd17b2cbc027 Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Wed, 20 Nov 2013 16:58:56 +0100 Subject: [PATCH 07/11] Box updated orientation Matrix have been split into a rotation Matrix and a translation Vector --- Code/OysterMath/LinearMath.h | 65 +++++ Code/OysterMath/OysterMath.cpp | 94 +++++++- Code/OysterMath/OysterMath.h | 30 +++ Code/OysterPhysics3D/Box.cpp | 17 +- Code/OysterPhysics3D/Box.h | 7 +- Code/OysterPhysics3D/OysterCollision3D.cpp | 262 ++++++++++++--------- Code/OysterPhysics3D/RigidBody.cpp | 9 +- Code/OysterPhysics3D/RigidBody.h | 4 +- 8 files changed, 350 insertions(+), 138 deletions(-) diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h index 4a036e99..c09caf32 100644 --- a/Code/OysterMath/LinearMath.h +++ b/Code/OysterMath/LinearMath.h @@ -161,6 +161,32 @@ namespace LinearAlgebra2D return targetMem = ::LinearAlgebra::Matrix3x3( c,-s, 0, s, c, 0, 0, 0, 1 ); } + template + inline ::LinearAlgebra::Matrix2x2 & InverseRotationMatrix( const ::LinearAlgebra::Matrix2x2 &rotationMatrix ) + { + return rotationMatrix.GetTranspose(); + } + + template + inline ::LinearAlgebra::Matrix3x3 & InverseRotationMatrix( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) + { + return rotationMatrix.GetTranspose(); + } + + template + inline ::LinearAlgebra::Matrix3x3 OrientationMatrix( const ::LinearAlgebra::Matrix2x2 &rotation, const ::LinearAlgebra::Vector2 &translation ) + { + return ::LinearAlgebra::Matrix3x3( ::LinearAlgebra::Vector3(rotation.v[0], 0), + ::LinearAlgebra::Vector3(rotation.v[1], 0), + ::LinearAlgebra::Vector3(translation, 1) ); + } + + template + inline ::LinearAlgebra::Matrix3x3 OrientationMatrix( const ::LinearAlgebra::Matrix3x3 &rotation, const ::LinearAlgebra::Vector2 &translation ) + { + return ::LinearAlgebra::Matrix3x3( rotation.v[0], rotation.v[1], ::LinearAlgebra::Vector3(translation, 1) ); + } + template inline ::LinearAlgebra::Matrix3x3 & OrientationMatrix( const ScalarType &radian, const ::LinearAlgebra::Vector2 &position, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { @@ -197,6 +223,12 @@ namespace LinearAlgebra2D orientationMatrix.m12, orientationMatrix.m22, -orientationMatrix.v[1].xy.Dot(orientationMatrix.v[2].xy), 0, 0, 1 ); } + + template + inline ::LinearAlgebra::Matrix3x3 ExtractRotationMatrix( const ::LinearAlgebra::Matrix3x3 &orientationMatrix ) + { + return ::LinearAlgebra::Matrix3x3( orientationMatrix.v[0], orientationMatrix.v[1], ::LinearAlgebra::Vector3::standard_unit_z ); + } } namespace LinearAlgebra3D @@ -283,6 +315,33 @@ namespace LinearAlgebra3D return targetMem; } + template + inline ::LinearAlgebra::Matrix3x3 & InverseRotationMatrix( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) + { + return rotationMatrix.GetTranspose(); + } + + template + inline ::LinearAlgebra::Matrix4x4 & InverseRotationMatrix( const ::LinearAlgebra::Matrix4x4 &rotationMatrix ) + { + return rotationMatrix.GetTranspose(); + } + + template + inline ::LinearAlgebra::Matrix4x4 OrientationMatrix( const ::LinearAlgebra::Matrix3x3 &rotation, const ::LinearAlgebra::Vector3 &translation ) + { + return ::LinearAlgebra::Matrix4x4( ::LinearAlgebra::Vector4(rotation.v[0], 0), + ::LinearAlgebra::Vector4(rotation.v[1], 0), + ::LinearAlgebra::Vector4(rotation.v[2], 0), + ::LinearAlgebra::Vector4(translation, 1) ); + } + + template + inline ::LinearAlgebra::Matrix4x4 OrientationMatrix( const ::LinearAlgebra::Matrix4x4 &rotation, const ::LinearAlgebra::Vector3 &translation ) + { + return ::LinearAlgebra::Matrix4x4( rotation.v[0], rotation.v[1], rotation.v[2], ::LinearAlgebra::Vector4(translation, 1) ); + } + template ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Vector3 &normalizedAxis, const ScalarType &deltaRadian, const ::LinearAlgebra::Vector3 &sumTranslation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { /** @todo TODO: not tested */ @@ -349,6 +408,12 @@ namespace LinearAlgebra3D return orientationMatrix; } + template + inline ::LinearAlgebra::Matrix4x4 ExtractRotationMatrix( const ::LinearAlgebra::Matrix4x4 &orientationMatrix ) + { + return ::LinearAlgebra::Matrix4x4( orientationMatrix.v[0], orientationMatrix.v[1], orientationMatrix.v[2], ::LinearAlgebra::Vector4::standard_unit_w ); + } + /* Creates an orthographic projection matrix designed for DirectX enviroment. width; of the projection sample volume. height; of the projection sample volume. diff --git a/Code/OysterMath/OysterMath.cpp b/Code/OysterMath/OysterMath.cpp index 84b46b7b..fb2486fc 100644 --- a/Code/OysterMath/OysterMath.cpp +++ b/Code/OysterMath/OysterMath.cpp @@ -32,18 +32,51 @@ namespace Oyster { namespace Math2D Float3x3 & RotationMatrix( const Float &radian, Float3x3 &targetMem ) { return ::LinearAlgebra2D::RotationMatrix( radian, targetMem ); } + + Float2x2 & InverseRotationMatrix( const Float2x2 &rotation, Float2x2 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::InverseRotationMatrix( rotation ); + } + + Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::InverseRotationMatrix( rotation ); + } + + Float3x3 & OrientationMatrix( const Float2x2 &rotation, const Float2 &translation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::OrientationMatrix( rotation, translation ); + } + + Float3x3 & OrientationMatrix( const Float3x3 &rotation, const Float2 &translation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::OrientationMatrix( rotation, translation ); + } Float3x3 & OrientationMatrix( const Float2 &position, const Float &radian, Float3x3 &targetMem ) - { return ::LinearAlgebra2D::OrientationMatrix( radian, position, targetMem ); } + { + return ::LinearAlgebra2D::OrientationMatrix( radian, position, targetMem ); + } Float3x3 & OrientationMatrix( const Float2 &position, const Float2 &lookAt, Float3x3 &targetMem ) - { return ::LinearAlgebra2D::OrientationMatrix( lookAt, position, targetMem ); } + { + return ::LinearAlgebra2D::OrientationMatrix( lookAt, position, targetMem ); + } Float3x3 & OrientationMatrix( const Float2 &position, Float radian, const Float2 &localCenterOfRotation, Float3x3 &targetMem ) - { return ::LinearAlgebra2D::OrientationMatrix( radian, position, localCenterOfRotation, targetMem ); } + { + return ::LinearAlgebra2D::OrientationMatrix( radian, position, localCenterOfRotation, targetMem ); + } Float3x3 & InverseOrientationMatrix( const Float3x3 &orientationMatrix, Float3x3 &targetMem ) - { return ::LinearAlgebra2D::InverseOrientationMatrix( orientationMatrix, targetMem ); } + { + return ::LinearAlgebra2D::InverseOrientationMatrix( orientationMatrix, targetMem ); + } + + Float3x3 & ExtractRotationMatrix( const Float3x3 &orientation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::ExtractRotationMatrix( orientation ); + } } } namespace Oyster { namespace Math3D @@ -62,27 +95,66 @@ namespace Oyster { namespace Math3D Float4x4 & RotationMatrix( const Float &radian, const Float3 &normalizedAxis, Float4x4 &targetMem ) { return ::LinearAlgebra3D::RotationMatrix( normalizedAxis, radian, targetMem ); } + + Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::InverseRotationMatrix( rotation ); + } + + Float4x4 & InverseRotationMatrix( const Float4x4 &rotation, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::InverseRotationMatrix( rotation ); + } + + Float4x4 & OrientationMatrix( const Float3x3 &rotation, const Float3 &translation, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::OrientationMatrix( rotation, translation ); + } + + Float4x4 & OrientationMatrix( const Float4x4 &rotation, const Float3 &translation, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::OrientationMatrix( rotation, translation ); + } Float4x4 & OrientationMatrix( const Float3 &normalizedAxis, const Float & deltaRadian, const Float3 &sumTranslation, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::OrientationMatrix( normalizedAxis, deltaRadian, sumTranslation, targetMem ); } + { + return ::LinearAlgebra3D::OrientationMatrix( normalizedAxis, deltaRadian, sumTranslation, targetMem ); + } Float4x4 & OrientationMatrix( const Float3 &sumDeltaAngularAxis, const Float3 &sumTranslation, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, targetMem ); } + { + return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, targetMem ); + } Float4x4 & OrientationMatrix( const Float3 &sumDeltaAngularAxis, const Float3 &sumTranslation, const Float3 ¢erOfMass, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, centerOfMass, targetMem ); } + { + return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, centerOfMass, targetMem ); + } Float4x4 & InverseOrientationMatrix( const Float4x4 &orientationMatrix, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::InverseOrientationMatrix( orientationMatrix, targetMem ); } + { + return ::LinearAlgebra3D::InverseOrientationMatrix( orientationMatrix, targetMem ); + } Float4x4 & UpdateOrientationMatrix( const Float3 &deltaPosition, const Float4x4 &deltaRotationMatrix, Float4x4 &orientationMatrix ) - { return ::LinearAlgebra3D::UpdateOrientationMatrix( deltaPosition, deltaRotationMatrix, orientationMatrix ); } + { + return ::LinearAlgebra3D::UpdateOrientationMatrix( deltaPosition, deltaRotationMatrix, orientationMatrix ); + } + + Float4x4 & ExtractRotationMatrix( const Float4x4 &orientation, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::ExtractRotationMatrix( orientation ); + } Float4x4 & ProjectionMatrix_Orthographic( const Float &width, const Float &height, const Float &nearClip, const Float &farClip, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::ProjectionMatrix_Orthographic( width, height, nearClip, farClip, targetMem ); } + { + return ::LinearAlgebra3D::ProjectionMatrix_Orthographic( width, height, nearClip, farClip, targetMem ); + } Float4x4 & ProjectionMatrix_Perspective( const Float &verticalFoV, const Float &aspectRatio, const Float &nearClip, const Float &farClip, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::ProjectionMatrix_Perspective( verticalFoV, aspectRatio, nearClip, farClip, targetMem ); } + { + return ::LinearAlgebra3D::ProjectionMatrix_Perspective( verticalFoV, aspectRatio, nearClip, farClip, targetMem ); + } Float3 VectorProjection( const Float3 &vector, const Float3 &axis ) { diff --git a/Code/OysterMath/OysterMath.h b/Code/OysterMath/OysterMath.h index 25381140..fd116b04 100644 --- a/Code/OysterMath/OysterMath.h +++ b/Code/OysterMath/OysterMath.h @@ -114,6 +114,18 @@ namespace Oyster { namespace Math2D /// Oyster's native math library specialized /// 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. + 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. + Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem = Float3x3() ); + + /// 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 + 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 Float3x3 & OrientationMatrix( const Float2 &position, const Float &radian, Float3x3 &targetMem = Float3x3() ); @@ -126,6 +138,9 @@ namespace Oyster { namespace Math2D /// Oyster's native math library specialized /// 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. + Float3x3 & ExtractRotationMatrix( const Float3x3 &orientation, Float3x3 &targetMem = Float3x3() ); } } namespace Oyster { namespace Math3D /// Oyster's native math library specialized for 3D @@ -148,6 +163,18 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized /// 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. + 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. + Float4x4 & InverseRotationMatrix( const Float4x4 &rotation, Float4x4 &targetMem = Float4x4() ); + + /// 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 + Float4x4 & OrientationMatrix( const Float4x4 &rotation, const Float3 &translation, Float4x4 &targetMem = Float4x4() ); + /******************************************************************* * Sets and returns targetMem as an orientation Matrix * @param normalizedAxis: The normalized vector parallell with the rotationAxis. @@ -187,6 +214,9 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized // 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. + Float4x4 & ExtractRotationMatrix( const Float4x4 &orientation, Float4x4 &targetMem = Float4x4() ); + /******************************************************************* * Creates an orthographic projection matrix designed for DirectX enviroment. * @param width; of the projection sample volume. diff --git a/Code/OysterPhysics3D/Box.cpp b/Code/OysterPhysics3D/Box.cpp index ed42ba76..ba97c5c5 100644 --- a/Code/OysterPhysics3D/Box.cpp +++ b/Code/OysterPhysics3D/Box.cpp @@ -8,19 +8,28 @@ using namespace ::Oyster::Collision3D; using namespace ::Oyster::Math3D; -Box::Box( ) : ICollideable(Type_box), orientation(Float4x4::identity), boundingOffset() {} -Box::Box( const Float4x4 &o, const Float3 &s ) : ICollideable(Type_box), orientation(o), boundingOffset(s*0.5) {} +Box::Box( ) + : ICollideable(Type_box), rotation(Float4x4::identity), center(0.0f), boundingOffset(0.5f) +{} + +Box::Box( const Float4x4 &r, const Float3 &p, const Float3 &s ) + : ICollideable(Type_box), rotation(r), center(p), boundingOffset(s*0.5) +{} + Box::~Box( ) {} Box & Box::operator = ( const Box &box ) { - this->orientation = box.orientation; + this->rotation = box.rotation; + this->center = box.center; this->boundingOffset = box.boundingOffset; return *this; } ::Utility::DynamicMemory::UniquePointer Box::Clone( ) const -{ return ::Utility::DynamicMemory::UniquePointer( new Box(*this) ); } +{ + return ::Utility::DynamicMemory::UniquePointer( new Box(*this) ); +} bool Box::Intersects( const ICollideable *target ) const { diff --git a/Code/OysterPhysics3D/Box.h b/Code/OysterPhysics3D/Box.h index 034639d0..ee46d6b6 100644 --- a/Code/OysterPhysics3D/Box.h +++ b/Code/OysterPhysics3D/Box.h @@ -16,19 +16,18 @@ namespace Oyster { namespace Collision3D public: union { - struct{ ::Oyster::Math::Float4x4 orientation; ::Oyster::Math::Float3 boundingOffset; }; + struct{ ::Oyster::Math::Float4x4 rotation; ::Oyster::Math::Float3 center, boundingOffset; }; struct { ::Oyster::Math::Float3 xAxis; ::Oyster::Math::Float paddingA; ::Oyster::Math::Float3 yAxis; ::Oyster::Math::Float paddingB; ::Oyster::Math::Float3 zAxis; ::Oyster::Math::Float paddingC; - ::Oyster::Math::Float3 center; }; - char byte[sizeof(::Oyster::Math::Float4x4) + sizeof(::Oyster::Math::Float3)]; + char byte[sizeof(::Oyster::Math::Float4x4) + 2*sizeof(::Oyster::Math::Float3)]; }; Box( ); - Box( const ::Oyster::Math::Float4x4 &orientation, const ::Oyster::Math::Float3 &size ); + Box( const ::Oyster::Math::Float4x4 &rotation, const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &size ); virtual ~Box( ); Box & operator = ( const Box &box ); diff --git a/Code/OysterPhysics3D/OysterCollision3D.cpp b/Code/OysterPhysics3D/OysterCollision3D.cpp index 5cb5a1bc..131cad62 100644 --- a/Code/OysterPhysics3D/OysterCollision3D.cpp +++ b/Code/OysterPhysics3D/OysterCollision3D.cpp @@ -6,7 +6,8 @@ #include "Utilities.h" #include -using namespace Oyster::Math3D; +using namespace ::Oyster::Math3D; +using namespace ::Utility::Value; namespace Oyster { namespace Collision3D { namespace Utility { @@ -19,13 +20,13 @@ namespace Oyster { namespace Collision3D { namespace Utility // Float calculations can suffer roundingerrors. Which is where epsilon = 1e-20 comes into the picture inline bool EqualsZero( const Float &value ) { // by Dan Andersson - return ::Utility::Value::Abs( value ) < epsilon; + return Abs( value ) < epsilon; } // Float calculations can suffer roundingerrors. Which is where epsilon = 1e-20 comes into the picture inline bool NotEqualsZero( const Float &value ) { // by Dan Andersson - return ::Utility::Value::Abs( value ) > epsilon; + return Abs( value ) > epsilon; } // returns true if miss/reject @@ -39,8 +40,8 @@ namespace Oyster { namespace Collision3D { namespace Utility t2 = e - boundingOffset; t1 /= f; t2 /= f; if( t1 > t2 ) ::Utility::Element::Swap( t1, t2 ); - tMin = ::Utility::Value::Max( tMin, t1 ); - tMax = ::Utility::Value::Min( tMax, t2 ); + tMin = Max( tMin, t1 ); + tMax = Min( tMax, t2 ); if( tMin > tMax ) return true; if( tMax < 0.0f ) return true; } @@ -65,101 +66,140 @@ namespace Oyster { namespace Collision3D { namespace Utility { // by Dan Andersson Float3 c = (box.maxVertex + box.minVertex) * 0.5f, // box.Center h = (box.maxVertex - box.minVertex) * 0.5f; // box.halfSize - boxExtend = h.x * ::Utility::Value::Abs(plane.normal.x); // Box max extending towards plane - boxExtend += h.y * ::Utility::Value::Abs(plane.normal.y); - boxExtend += h.z * ::Utility::Value::Abs(plane.normal.z); + boxExtend = h.x * Abs(plane.normal.x); // Box max extending towards plane + boxExtend += h.y * Abs(plane.normal.y); + boxExtend += h.z * Abs(plane.normal.z); centerDistance = c.Dot(plane.normal) + plane.phasing; // distance between box center and plane } void Compare( Float &boxExtend, Float ¢erDistance, const Plane &plane, const Box &box ) { // by Dan Andersson - boxExtend = box.boundingOffset.x * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[0].xyz)); // Box max extending towards plane - boxExtend += box.boundingOffset.y * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[1].xyz)); - boxExtend += box.boundingOffset.z * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[2].xyz)); + boxExtend = box.boundingOffset.x * Abs(plane.normal.Dot(box.xAxis)); // Box max extending towards plane + boxExtend += box.boundingOffset.y * Abs(plane.normal.Dot(box.yAxis)); + boxExtend += box.boundingOffset.z * Abs(plane.normal.Dot(box.zAxis)); - centerDistance = box.orientation.v[3].xyz.Dot(plane.normal) + plane.phasing; // distance between box center and plane + centerDistance = box.center.Dot(plane.normal) + plane.phasing; // distance between box center and plane } - bool FifteenAxisVsAlignedAxisOverlappingChecks( const Float3 &boundingOffsetA, const Float3 &boundingOffsetB, const Float4x4 &orientationB ) - { // by Dan Andersson - Float4x4 absOrientationB; - { - Float4x4 tO = orientationB.GetTranspose(); - absOrientationB.v[0] = ::Utility::Value::Abs(tO.v[0]); - if( absOrientationB.v[0].w > boundingOffsetA.x + boundingOffsetB.Dot(absOrientationB.v[0].xyz) ) return false; - absOrientationB.v[1] = ::Utility::Value::Abs(tO.v[1]); - if( absOrientationB.v[1].w > boundingOffsetA.y + boundingOffsetB.Dot(absOrientationB.v[1].xyz) ) return false; - absOrientationB.v[2] = ::Utility::Value::Abs(tO.v[2]); - if( absOrientationB.v[2].w > boundingOffsetA.z + boundingOffsetB.Dot(absOrientationB.v[2].xyz) ) return false; + bool SeperatingAxisTest_AxisAlignedVsTransformedBox( const Float3 &boundingOffsetA, const Float3 &boundingOffsetB, const Float4x4 &rotationB, const Float3 &worldOffset ) + { // by Dan Andersson + + /***************************************************************** + * Uses the Seperating Axis Theorem + * if( |t dot s| > hA dot |s * RA| + hB dot |s * RB| ) then not intersecting + * |t dot s| > hA dot |s| + hB dot |s * RB| .. as RA = I + * + * t: objectB's offset from objectA [worldOffset] + * s: current comparison axis + * hA: boundingReach vector of objectA. Only absolute values is assumed. [boundingOffsetA] + * hB: boundingReach vector of objectB. Only absolute values is assumed. [boundingOffsetB] + * RA: rotation matrix of objectA. Is identity matrix here, thus omitted. + * RB: rotation matrix of objectB. Is transformed into objectA's view at this point. [rotationB] + * + * Note: s * RB = (RB^T * s)^T = (RB^-1 * s)^T .... vector == vector^T + *****************************************************************/ + + Float4x4 absRotationB = Abs(rotationB); + Float3 absWorldOffset = Abs(worldOffset); // |t|: [absWorldOffset] + + // s = { 1, 0, 0 } [ RA.v[0] ] + if( absWorldOffset.x > boundingOffsetA.x + boundingOffsetB.Dot(Float3(absRotationB.v[0].x, absRotationB.v[1].x, absRotationB.v[2].x)) ) + { // |t dot s| > hA dot |s| + hB dot |s * RB| -->> t.x > hA.x + hB dot |{RB.v[0].x, RB.v[1].x, RB.v[2].x}| + return false; } - absOrientationB.Transpose(); - if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[0])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[0].xyz) ) return false; - if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[1])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[1].xyz) ) return false; - if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[2])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[2].xyz) ) return false; + // s = { 0, 1, 0 } [ RA.v[1] ] + if( absWorldOffset.y > boundingOffsetA.y + boundingOffsetB.Dot(Float3(absRotationB.v[0].y, absRotationB.v[1].y, absRotationB.v[2].y)) ) + { // t.y > hA.y + hB dot |{RB.v[0].y, RB.v[1].y, RB.v[2].y}| + return false; + } + + // s = { 0, 0, 1 } [ RA.v[2] ] + if( absWorldOffset.z > boundingOffsetA.z + boundingOffsetB.Dot(Float3(absRotationB.v[0].z, absRotationB.v[1].z, absRotationB.v[2].z)) ) + { // t.z > hA.z + hB dot |{RB.v[0].z, RB.v[1].z, RB.v[2].z}| + return false; + } + + // s = RB.v[0].xyz + if( Abs(worldOffset.Dot(rotationB.v[0].xyz)) > boundingOffsetA.Dot(absRotationB.v[0].xyz) + boundingOffsetB.x ) + { // |t dot s| > hA dot |s| + hB dot |s * RB| -->> |t dot s| > hA dot |s| + hB dot |{1, 0, 0}| -->> |t dot s| > hA dot |s| + hB.x + return false; + } + + // s = RB.v[1].xyz + if( Abs(worldOffset.Dot(rotationB.v[1].xyz)) > boundingOffsetA.Dot(absRotationB.v[1].xyz) + boundingOffsetB.y ) + { // |t dot s| > hA dot |s| + hB.y + return false; + } + + // s = RB.v[2].xyz + if( Abs(worldOffset.Dot(rotationB.v[2].xyz)) > boundingOffsetA.Dot(absRotationB.v[2].xyz) + boundingOffsetB.z ) + { // |t dot s| > hA dot |s| + hB.z + return false; + } + + // s = ( 1,0,0 ) x rotationB.v[0].xyz: + Float d = boundingOffsetA.y * absRotationB.v[0].z; + d += boundingOffsetA.z * absRotationB.v[0].y; + d += boundingOffsetB.y * absRotationB.v[2].x; + d += boundingOffsetB.z * absRotationB.v[1].x; + if( Abs(worldOffset.z*rotationB.v[0].y - worldOffset.y*rotationB.v[0].z) > d ) return false; + + // s = ( 1,0,0 ) x rotationB.v[1].xyz: + d = boundingOffsetA.y * absRotationB.v[1].z; + d += boundingOffsetA.z * absRotationB.v[1].y; + d += boundingOffsetB.x * absRotationB.v[2].x; + d += boundingOffsetB.z * absRotationB.v[0].x; + if( Abs(worldOffset.z*rotationB.v[1].y - worldOffset.y*rotationB.v[1].z) > d ) return false; + + // s = ( 1,0,0 ) x rotationB.v[2].xyz: + d = boundingOffsetA.y * absRotationB.v[2].z; + d += boundingOffsetA.z * absRotationB.v[2].y; + d += boundingOffsetB.x * absRotationB.v[1].x; + d += boundingOffsetB.y * absRotationB.v[0].x; + if( Abs(worldOffset.z*rotationB.v[2].y - worldOffset.y*rotationB.v[2].z) > d ) return false; + + // s = ( 0,1,0 ) x rotationB.v[0].xyz: + d = boundingOffsetA.x * absRotationB.v[0].z; + d += boundingOffsetA.z * absRotationB.v[0].x; + d += boundingOffsetB.y * absRotationB.v[2].y; + d += boundingOffsetB.z * absRotationB.v[1].y; + if( Abs(worldOffset.x*rotationB.v[0].z - worldOffset.z*rotationB.v[0].x) > d ) return false; - // ( 1,0,0 ) x orientationB.v[0].xyz: - Float d = boundingOffsetA.y * absOrientationB.v[0].z; - d += boundingOffsetA.z * absOrientationB.v[0].y; - d += boundingOffsetB.y * absOrientationB.v[2].x; - d += boundingOffsetB.z * absOrientationB.v[1].x; - if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[0].y - orientationB.v[3].y*orientationB.v[0].z) > d ) return false; + // s = ( 0,1,0 ) x rotationB.v[1].xyz: + d = boundingOffsetA.x * absRotationB.v[1].z; + d += boundingOffsetA.z * absRotationB.v[1].x; + d += boundingOffsetB.x * absRotationB.v[2].y; + d += boundingOffsetB.z * absRotationB.v[0].y; + if( Abs(worldOffset.x*rotationB.v[1].z - worldOffset.z*rotationB.v[1].x) > d ) return false; - // ( 1,0,0 ) x orientationB.v[1].xyz: - d = boundingOffsetA.y * absOrientationB.v[1].z; - d += boundingOffsetA.z * absOrientationB.v[1].y; - d += boundingOffsetB.x * absOrientationB.v[2].x; - d += boundingOffsetB.z * absOrientationB.v[0].x; - if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[1].y - orientationB.v[3].y*orientationB.v[1].z) > d ) return false; + // s = ( 0,1,0 ) x rotationB.v[2].xyz: + d = boundingOffsetA.x * absRotationB.v[2].z; + d += boundingOffsetA.z * absRotationB.v[2].x; + d += boundingOffsetB.x * absRotationB.v[1].y; + d += boundingOffsetB.y * absRotationB.v[0].y; + if( Abs(worldOffset.x*rotationB.v[2].z - worldOffset.z*rotationB.v[2].x) > d ) return false; - // ( 1,0,0 ) x orientationB.v[2].xyz: - d = boundingOffsetA.y * absOrientationB.v[2].z; - d += boundingOffsetA.z * absOrientationB.v[2].y; - d += boundingOffsetB.x * absOrientationB.v[1].x; - d += boundingOffsetB.y * absOrientationB.v[0].x; - if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[2].y - orientationB.v[3].y*orientationB.v[2].z) > d ) return false; + // s = ( 0,0,1 ) x rotationB.v[0].xyz: + d = boundingOffsetA.x * absRotationB.v[0].y; + d += boundingOffsetA.y * absRotationB.v[0].x; + d += boundingOffsetB.y * absRotationB.v[2].z; + d += boundingOffsetB.z * absRotationB.v[1].z; + if( Abs(worldOffset.y*rotationB.v[0].x - worldOffset.x*rotationB.v[0].y) > d ) return false; - // ( 0,1,0 ) x orientationB.v[0].xyz: - d = boundingOffsetA.x * absOrientationB.v[0].z; - d += boundingOffsetA.z * absOrientationB.v[0].x; - d += boundingOffsetB.y * absOrientationB.v[2].y; - d += boundingOffsetB.z * absOrientationB.v[1].y; - if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[0].z - orientationB.v[3].z*orientationB.v[0].x) > d ) return false; - - // ( 0,1,0 ) x orientationB.v[1].xyz: - d = boundingOffsetA.x * absOrientationB.v[1].z; - d += boundingOffsetA.z * absOrientationB.v[1].x; - d += boundingOffsetB.x * absOrientationB.v[2].y; - d += boundingOffsetB.z * absOrientationB.v[0].y; - if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[1].z - orientationB.v[3].z*orientationB.v[1].x) > d ) return false; + // s = ( 0,0,1 ) x rotationB.v[1].xyz: + d = boundingOffsetA.x * absRotationB.v[1].y; + d += boundingOffsetA.y * absRotationB.v[1].x; + d += boundingOffsetB.x * absRotationB.v[2].z; + d += boundingOffsetB.z * absRotationB.v[0].z; + if( Abs(worldOffset.y*rotationB.v[1].x - worldOffset.x*rotationB.v[1].y) > d ) return false; - // ( 0,1,0 ) x orientationB.v[2].xyz: - d = boundingOffsetA.x * absOrientationB.v[2].z; - d += boundingOffsetA.z * absOrientationB.v[2].x; - d += boundingOffsetB.x * absOrientationB.v[1].y; - d += boundingOffsetB.y * absOrientationB.v[0].y; - if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[2].z - orientationB.v[3].z*orientationB.v[2].x) > d ) return false; - - // ( 0,0,1 ) x orientationB.v[0].xyz: - d = boundingOffsetA.x * absOrientationB.v[0].y; - d += boundingOffsetA.y * absOrientationB.v[0].x; - d += boundingOffsetB.y * absOrientationB.v[2].z; - d += boundingOffsetB.z * absOrientationB.v[1].z; - if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[0].x - orientationB.v[3].x*orientationB.v[0].y) > d ) return false; - - // ( 0,0,1 ) x orientationB.v[1].xyz: - d = boundingOffsetA.x * absOrientationB.v[1].y; - d += boundingOffsetA.y * absOrientationB.v[1].x; - d += boundingOffsetB.x * absOrientationB.v[2].z; - d += boundingOffsetB.z * absOrientationB.v[0].z; - if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[1].x - orientationB.v[3].x*orientationB.v[1].y) > d ) return false; - - // ( 0,0,1 ) x orientationB.v[2].xyz: - d = boundingOffsetA.x * absOrientationB.v[2].y; - d += boundingOffsetA.y * absOrientationB.v[2].x; - d += boundingOffsetB.x * absOrientationB.v[1].z; - d += boundingOffsetB.y * absOrientationB.v[0].z; - if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[2].x - orientationB.v[3].x*orientationB.v[2].y) > d ) return false; + // s = ( 0,0,1 ) x rotationB.v[2].xyz: + d = boundingOffsetA.x * absRotationB.v[2].y; + d += boundingOffsetA.y * absRotationB.v[2].x; + d += boundingOffsetB.x * absRotationB.v[1].z; + d += boundingOffsetB.y * absRotationB.v[0].z; + if( Abs(worldOffset.y*rotationB.v[2].x - worldOffset.x*rotationB.v[2].y) > d ) return false; return true; } @@ -353,8 +393,8 @@ namespace Oyster { namespace Collision3D { namespace Utility bool Intersect( const BoxAxisAligned &box, const Sphere &sphere ) { // by Dan Andersson - Float3 e = ::Utility::Value::Max( box.minVertex - sphere.center, Float3::null ); - e += ::Utility::Value::Max( sphere.center - box.maxVertex, Float3::null ); + Float3 e = Max( box.minVertex - sphere.center, Float3::null ); + e += Max( sphere.center - box.maxVertex, Float3::null ); if( e.Dot(e) > (sphere.radius * sphere.radius) ) return false; return true; @@ -385,17 +425,17 @@ namespace Oyster { namespace Collision3D { namespace Utility bool Intersect( const Box &box, const Point &point ) { // by Dan Andersson - Float3 dPos = point.center - box.orientation.v[3].xyz; + Float3 dPos = point.center - box.center; - Float coordinate = dPos.Dot( box.orientation.v[0].xyz ); + Float coordinate = dPos.Dot( box.xAxis ); if( coordinate > box.boundingOffset.x ) return false; if( coordinate < -box.boundingOffset.x ) return false; - coordinate = dPos.Dot( box.orientation.v[1].xyz ); + coordinate = dPos.Dot( box.yAxis ); if( coordinate > box.boundingOffset.y ) return false; if( coordinate < -box.boundingOffset.y ) return false; - coordinate = dPos.Dot( box.orientation.v[2].xyz ); + coordinate = dPos.Dot( box.zAxis ); if( coordinate > box.boundingOffset.z ) return false; if( coordinate < -box.boundingOffset.z ) return false; @@ -419,11 +459,11 @@ namespace Oyster { namespace Collision3D { namespace Utility bool Intersect( const Box &box, const Sphere &sphere ) { // by Dan Andersson - Float3 e = sphere.center - box.orientation.v[3].xyz, - centerL = Float3( e.Dot(box.orientation.v[0].xyz), e.Dot(box.orientation.v[1].xyz), e.Dot(box.orientation.v[2].xyz) ); + Float3 e = sphere.center - box.center, + centerL = Float3( e.Dot(box.xAxis), e.Dot(box.yAxis), e.Dot(box.zAxis) ); - e = ::Utility::Value::Max( (box.boundingOffset + centerL)*=-1.0f, Float3::null ); - e += ::Utility::Value::Max( centerL - box.boundingOffset, Float3::null ); + e = Max( (box.boundingOffset + centerL)*=-1.0f, Float3::null ); + e += Max( centerL - box.boundingOffset, Float3::null ); if( e.Dot(e) > (sphere.radius * sphere.radius) ) return false; return true; @@ -440,20 +480,20 @@ namespace Oyster { namespace Collision3D { namespace Utility bool Intersect( const Box &boxA, const BoxAxisAligned &boxB ) { // by Dan Andersson - Float3 alignedOffsetBoundaries = boxB.maxVertex - boxB.minVertex; - Float4x4 translated = boxA.orientation; - translated.v[3].xyz -= boxB.minVertex; - translated.v[3].xyz += alignedOffsetBoundaries * 0.5f; - alignedOffsetBoundaries = ::Utility::Value::Abs(alignedOffsetBoundaries); - return Private::FifteenAxisVsAlignedAxisOverlappingChecks( alignedOffsetBoundaries, boxA.boundingOffset, translated ); + Float3 alignedOffsetBoundaries = (boxB.maxVertex - boxB.minVertex) * 0.5f, + offset = boxA.center - Average( boxB.minVertex, boxB.maxVertex ); + return Private::SeperatingAxisTest_AxisAlignedVsTransformedBox( alignedOffsetBoundaries, boxA.boundingOffset, boxA.rotation, offset ); } bool Intersect( const Box &boxA, const Box &boxB ) { // by Dan Andersson - Float4x4 M; - InverseOrientationMatrix( boxA.orientation, M ); - TransformMatrix( M, boxB.orientation, M ); /// TODO: not verified - return Private::FifteenAxisVsAlignedAxisOverlappingChecks( boxA.boundingOffset, boxB.boundingOffset, M ); + Float4x4 orientationA = OrientationMatrix(boxA.rotation, boxA.center), + orientationB = OrientationMatrix(boxB.rotation, boxB.center), + invOrientationA = InverseOrientationMatrix( orientationA ); + + orientationB = TransformMatrix( invOrientationA, orientationB ); + + return Private::SeperatingAxisTest_AxisAlignedVsTransformedBox( boxA.boundingOffset, boxB.boundingOffset, ExtractRotationMatrix(orientationB), orientationB.v[3].xyz ); } bool Intersect( const Frustrum &frustrum, const Point &point ) @@ -490,37 +530,37 @@ namespace Oyster { namespace Collision3D { namespace Utility if( Intersect(frustrum.leftPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.rightPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.bottomPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.topPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.nearPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.farPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( intersected ) return true; diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 319d4da4..9f031979 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -40,7 +40,7 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) { // 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.orientation, this->momentOfInertiaTensor ); + Float4x4 wMomentOfInertiaTensor = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ); // updating the linear // dv = dt * a = dt * F / m @@ -63,10 +63,9 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) deltaRadian = ::std::sqrt( deltaRadian ); rotationAxis /= deltaRadian; - // using rotationAxis, deltaRadian and deltaPos to create a matrix to update the orientation matrix - UpdateOrientationMatrix( deltaPos, RotationMatrix(deltaRadian, rotationAxis), this->box.orientation ); - - /** @todo TODO: ISSUE! how is momentOfInertiaTensor related to the orientation of the RigidBody? */ + // using rotationAxis, deltaRadian and deltaPos to create a matrix to update the box + this->box.center += deltaPos; + TransformMatrix( RotationMatrix(deltaRadian, rotationAxis), this->box.rotation, this->box.rotation ); } else { // no rotation, only use deltaPos to translate the RigidBody diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index 4184ca2e..4b7f65c0 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -34,8 +34,6 @@ namespace Oyster { namespace Physics3D // ACCESS METHODS ///////////////////////////// - ::Oyster::Math::Float4x4 & AccessOrientation(); - const ::Oyster::Math::Float4x4 & AccessOrientation() const; ::Oyster::Math::Float3 & AccessBoundingReach(); const ::Oyster::Math::Float3 & AccessBoundingReach() const; ::Oyster::Math::Float3 & AccessCenter(); @@ -46,7 +44,7 @@ namespace Oyster { namespace Physics3D const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const; const ::Oyster::Math::Float & GetMass() const; - const ::Oyster::Math::Float4x4 & GetOrientation() const; + const ::Oyster::Math::Float4x4 GetOrientation() const; ::Oyster::Math::Float4x4 GetView() const; const ::Oyster::Math::Float4x4 & GetToWorldMatrix() const; ::Oyster::Math::Float4x4 GetToLocalMatrix() const; From d90074d9097f203c3f37e01df72d95ea312b675d Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Wed, 20 Nov 2013 17:40:12 +0100 Subject: [PATCH 08/11] Minor changes Some argument renaming and changed a few RigidBody functions, in respect to the change in the Box struct --- Code/OysterPhysics3D/OysterPhysics3D.h | 38 +++++++++++++------------- Code/OysterPhysics3D/RigidBody.cpp | 29 ++++++++------------ 2 files changed, 31 insertions(+), 36 deletions(-) diff --git a/Code/OysterPhysics3D/OysterPhysics3D.h b/Code/OysterPhysics3D/OysterPhysics3D.h index ad049907..86e29307 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 From f51d2312dd34804a13533933c24c4a4fbb818eb9 Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Thu, 21 Nov 2013 11:39:11 +0100 Subject: [PATCH 09/11] RigidBody DONE Needs only testing for confirmations --- Code/OysterMath/LinearMath.h | 4 + Code/OysterMath/OysterMath.cpp | 5 + Code/OysterMath/OysterMath.h | 3 + Code/OysterPhysics3D/OysterPhysics3D.h | 17 ++- Code/OysterPhysics3D/RigidBody.cpp | 141 +++++++++---------------- Code/OysterPhysics3D/RigidBody.h | 10 +- 6 files changed, 79 insertions(+), 101 deletions(-) diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h index c09caf32..4c954cd6 100644 --- a/Code/OysterMath/LinearMath.h +++ b/Code/OysterMath/LinearMath.h @@ -455,6 +455,10 @@ namespace LinearAlgebra3D template inline ::LinearAlgebra::Vector3 VectorProjection( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Vector3 &axis ) { return axis * ( vector.Dot(axis) / axis.Dot(axis) ); } + + template + inline ::LinearAlgebra::Vector3 NormalProjection( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Vector3 &normalizedAxis ) + { return axis * ( vector.Dot(axis) ); } } #include "Utilities.h" diff --git a/Code/OysterMath/OysterMath.cpp b/Code/OysterMath/OysterMath.cpp index fb2486fc..ab318e92 100644 --- a/Code/OysterMath/OysterMath.cpp +++ b/Code/OysterMath/OysterMath.cpp @@ -160,4 +160,9 @@ namespace Oyster { namespace Math3D { return ::LinearAlgebra3D::VectorProjection( vector, axis ); } + + Float3 NormalProjection( const Float3 &vector, const Float3 &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 fd116b04..4a1eeefe 100644 --- a/Code/OysterMath/OysterMath.h +++ b/Code/OysterMath/OysterMath.h @@ -244,6 +244,9 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized /// 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. + Float3 NormalProjection( const Float3 &vector, const Float3 &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; } diff --git a/Code/OysterPhysics3D/OysterPhysics3D.h b/Code/OysterPhysics3D/OysterPhysics3D.h index 86e29307..2eb9db9a 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.h +++ b/Code/OysterPhysics3D/OysterPhysics3D.h @@ -108,7 +108,7 @@ namespace Oyster { namespace Physics3D ******************************************************************/ inline ::Oyster::Math::Float3 AngularImpulseAcceleration( const ::Oyster::Math::Float3 &linearImpulseAcceleration, const ::Oyster::Math::Float3 &worldOffset ) { - return offset.Cross( linearImpulseAcceleration ); + return worldOffset.Cross( linearImpulseAcceleration ); } /****************************************************************** @@ -129,6 +129,15 @@ namespace Oyster { namespace Physics3D return ( momentOfInertiaInversed * ::Oyster::Math::Float4( angularMomentum, 0.0f ) ).xyz; } + /****************************************************************** + * Returns the world angular velocity of a mass in rotation. + * @todo TODO: improve doc + ******************************************************************/ + inline ::Oyster::Math::Float3 AngularVelocity( const ::Oyster::Math::Float3 &linearVelocity, const ::Oyster::Math::Float3 &worldOffset ) + { + return worldOffset.Cross( linearVelocity ); + } + /****************************************************************** * Returns the world tangential velocity at worldPos, of a mass in rotation. * @todo TODO: improve doc @@ -183,6 +192,12 @@ namespace Oyster { namespace Physics3D return ( momentOfInertia * ::Oyster::Math::Float4(angularImpulseAcceleration, 0.0f) ).xyz; } + inline ::Oyster::Math::Float3 Impulse( ) + { + //! @todo TODO: implement calculation for impulse. Hijack Mattias Eriksson + return ::Oyster::Math::Float3::null; + } + namespace MomentOfInertia { /// Library of Formulas to calculate moment of inerta for simple shapes /** @todo TODO: add MomentOfInertia tensor formulas */ diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 6ed9da25..c65b040f 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -40,22 +40,20 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) { // 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 ); + Float4x4 wMomentOfInertiaTensor = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ); // RI // updating the linear - // dv = dt * a = dt * F / m - // ds = dt * avg_v - Float3 deltaLinearVelocity = this->impulseForceSum; - deltaLinearVelocity *= (deltaTime / this->mass); - Float3 deltaPos = deltaTime * ::Utility::Value::AverageWithDelta( Formula::LinearVelocity(this->mass, this->linearMomentum), deltaLinearVelocity ); + // dG = F * dt + // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G + Float3 linearImpulse = this->impulseForceSum * deltaTime; // aka deltaLinearMomentum + Float3 deltaPos = ( deltaTime / this->mass ) * ::Utility::Value::AverageWithDelta( this->linearMomentum, linearImpulse ); // updating the angular - // dw = dt * a = dt * ( I^-1 * T ) - // rotation = dt * avg_w - Float4x4 inversedMomentOfInertiaTensor = wMomentOfInertiaTensor.GetInverse(); - Float3 deltaAngularVelocity = Formula::AngularImpulseAcceleration( inversedMomentOfInertiaTensor, this->impulseTorqueSum ); // I^-1 * T - deltaAngularVelocity *= deltaTime; - Float3 rotationAxis = ::Utility::Value::AverageWithDelta( Formula::AngularVelocity(inversedMomentOfInertiaTensor,this->angularMomentum), deltaAngularVelocity ); + // dH = T * dt + // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H + Float3 angularImpulse = this->impulseTorqueSum * deltaTime; // aka deltaAngularMomentum + Float3 rotationAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), + ::Utility::Value::AverageWithDelta(this->angularMomentum, angularImpulse) ); Float deltaRadian = rotationAxis.Dot( rotationAxis ); if( deltaRadian != 0.0f ) @@ -72,10 +70,10 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) this->box.center += deltaPos; } - // update movements and clear impulses - this->linearMomentum += Formula::LinearMomentum( this->mass, deltaLinearVelocity ); + // update movements and clear impulseForceSum and impulseTorqueSum + this->linearMomentum += linearImpulse; this->impulseForceSum = Float3::null; - this->angularMomentum += Formula::AngularMomentum( wMomentOfInertiaTensor, deltaAngularVelocity ); + this->angularMomentum += angularImpulse; this->impulseTorqueSum = Float3::null; } @@ -226,67 +224,21 @@ Float3 RigidBody::GetLinearVelocity() const return Formula::LinearVelocity( this->mass, this->linearMomentum ); } -Float3 RigidBody::GetTangentialImpulseForceAt( const Float3 &worldPos ) const -{ // by Dan Andersson +void RigidBody::GetMomentumAt( const Float3 &worldPos, const Float3 &surfaceNormal, Float3 &normalMomentum, Float3 &tangentialMomentum ) const +{ Float3 worldOffset = worldPos - this->box.center; - return Formula::TangentialImpulseForce( this->impulseTorqueSum, worldOffset ); -} + Float3 momentum = Formula::TangentialLinearMomentum( this->angularMomentum, worldOffset ); + momentum += this->linearMomentum; -Float3 RigidBody::GetTangentialLinearMomentumAt( const Float3 &worldPos ) const -{ // by Dan Andersson - return Formula::TangentialLinearMomentum( this->angularMomentum, worldPos ); -} - -Float3 RigidBody::GetTangentialImpulseAccelerationAt( const Float3 &worldPos ) const -{ // by Dan Andersson - 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 - 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 - return Float3::null; //! @todo TODO: surface normal needed as well. Same goes for those below. -} - -Float3 RigidBody::GetLinearMomentumAt( const Float3 &worldPos ) const -{ // by Dan Andersson - // Reminder! Momentum is a world value. - Float4 localMomentum = Float4( this->GetLinearMomentumAt_Local((this->GetView() * Float4(worldPos, 1.0f)).xyz), 0.0f ); // should not be any disform thus result.w = 1.0f - return (this->box.orientation * localMomentum).xyz; // should not be any disform thus result.w = 0.0f - - // TODO: angularMomentum is a local value!! - return this->linearMomentum + Formula::TangentialLinearMomentum( this->angularMomentum, worldPos ); -} - -Float3 RigidBody::GetImpulseAccelerationAt( const Float3 &worldPos ) const -{ // by Dan Andersson - // Reminder! Acceleration is a world value. - return Formula::LinearImpulseAcceleration( this->mass, this->impulseForceSum ) - + Formula::TangentialImpulseAcceleration( this->momentOfInertiaTensor.GetInverse(), this->impulseTorqueSum, worldPos ); -} - -Float3 RigidBody::GetLinearVelocityAt( const Float3 &worldPos ) const -{ // by Dan Andersson - // Reminder! Velocity is a world value. - return Formula::LinearVelocity( this->mass, this->linearMomentum ) - + Formula::TangentialLinearVelocity( this->momentOfInertiaTensor.GetInverse(), this->angularMomentum, worldPos ); + normalMomentum = NormalProjection( momentum, surfaceNormal ); + tangentialMomentum = momentum - normalMomentum; } void RigidBody::SetMomentOfInertia( const Float4x4 &localI ) { // by Dan Andersson - if( i.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable + if( localI.GetDeterminant() != 0.0f ) // insanitycheck! momentOfInertiaTensor must be invertable { - this->momentOfInertiaTensor = i; + this->momentOfInertiaTensor = localI; } } @@ -310,7 +262,8 @@ void RigidBody::SetMass_KeepMomentum( const Float &m ) void RigidBody::SetOrientation( const Float4x4 &o ) { // by Dan Andersson - this->box.orientation = o; + ExtractRotationMatrix( o, this->box.rotation ); + this->box.center = o.v[3].xyz; } void RigidBody::SetSize( const Float3 &widthHeight ) @@ -325,64 +278,70 @@ void RigidBody::SetCenter( const Float3 &worldPos ) void RigidBody::SetImpulseTorque( const Float3 &worldT ) { // by Dan Andersson - this->impulseTorqueSum = t; + this->impulseTorqueSum = worldT; } void RigidBody::SetAngularMomentum( const Float3 &worldH ) { // by Dan Andersson - this->angularMomentum = h; + this->angularMomentum = worldH; } void RigidBody::SetAngularImpulseAcceleration( const Float3 &worldA ) { // by Dan Andersson - this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, a ); + this->impulseTorqueSum = Formula::ImpulseTorque( this->momentOfInertiaTensor, worldA ); } void RigidBody::SetAngularVelocity( const Float3 &worldW ) { // by Dan Andersson - this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, w ); + this->angularMomentum = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW ); } void RigidBody::SetImpulseForce( const Float3 &worldF ) { // by Dan Andersson - this->impulseForceSum = f; + this->impulseForceSum = worldF; } void RigidBody::SetLinearMomentum( const Float3 &worldG ) { // by Dan Andersson - this->linearMomentum = g; + this->linearMomentum = worldG; } void RigidBody::SetLinearImpulseAcceleration( const Float3 &worldA ) { // by Dan Andersson - this->impulseForceSum = Formula::ImpulseForce( this->mass, a ); + this->impulseForceSum = Formula::ImpulseForce( this->mass, worldA ); } void RigidBody::SetLinearVelocity( const Float3 &worldV ) { // by Dan Andersson - this->linearMomentum = Formula::LinearMomentum( this->mass, v ); + this->linearMomentum = Formula::LinearMomentum( this->mass, worldV ); } -void RigidBody::SetImpulseForceAt( const Float3 &worldF, const Float3 &worldPos ) +void RigidBody::SetImpulseForceAt( const Float3 &worldForce, const Float3 &worldPos ) { // by Dan Andersson - // Reminder! Impulse force and torque is world values. - this->impulseForceSum = VectorProjection( worldForce, worldPos ); - this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldPos ); + Float3 worldOffset = worldPos - this->box.center; + this->impulseForceSum = VectorProjection( worldForce, worldOffset ); + this->impulseTorqueSum = Formula::ImpulseTorque( worldForce, worldOffset ); } void RigidBody::SetLinearMomentumAt( const Float3 &worldG, const Float3 &worldPos ) { // by Dan Andersson - // Reminder! Linear and angular momentum is world values. - this->linearMomentum = VectorProjection( worldG, worldPos ); - this->angularMomentum = Formula::AngularMomentum( worldG, worldPos ); + Float3 worldOffset = worldPos - this->box.center; + this->linearMomentum = VectorProjection( worldG, worldOffset ); + this->angularMomentum = Formula::AngularMomentum( worldG, worldOffset ); } -void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &pos ) -{ // by - +void RigidBody::SetImpulseAccelerationAt( const Float3 &worldA, const Float3 &worldPos ) +{ // by Dan Andersson + Float3 worldOffset = worldPos - this->box.center; + 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 &pos ) -{ // by - +void RigidBody::SetLinearVelocityAt( const Float3 &worldV, const Float3 &worldPos ) +{ // by Dan Andersson + Float3 worldOffset = worldPos - this->box.center; + this->linearMomentum = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) ); + this->angularMomentum = Formula::AngularMomentum( this->box.rotation * this->momentOfInertiaTensor, + Formula::AngularVelocity(worldV, worldOffset) ); } \ No newline at end of file diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index 4b7f65c0..19ce6bb2 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -64,15 +64,7 @@ namespace Oyster { namespace Physics3D ::Oyster::Math::Float3 GetLinearImpulseAcceleration() const; ::Oyster::Math::Float3 GetLinearVelocity() const; - ::Oyster::Math::Float3 GetTangentialImpulseForceAt( const ::Oyster::Math::Float3 &worldPos ) const; - ::Oyster::Math::Float3 GetTangentialLinearMomentumAt( const ::Oyster::Math::Float3 &worldPos ) const; - ::Oyster::Math::Float3 GetTangentialImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldPos ) const; - ::Oyster::Math::Float3 GetTangentialLinearVelocityAt( const ::Oyster::Math::Float3 &worldPos ) const; - - ::Oyster::Math::Float3 GetImpulseForceAt( const ::Oyster::Math::Float3 &worldPos ) const; - ::Oyster::Math::Float3 GetLinearMomentumAt( const ::Oyster::Math::Float3 &worldPos ) const; - ::Oyster::Math::Float3 GetImpulseAccelerationAt( const ::Oyster::Math::Float3 &worldPos ) const; - ::Oyster::Math::Float3 GetLinearVelocityAt( const ::Oyster::Math::Float3 &worldPos ) const; + void GetMomentumAt( const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &surfaceNormal, ::Oyster::Math::Float3 &normalMomentum, ::Oyster::Math::Float3 &tangentialMomentum ) const; // SET METHODS //////////////////////////////// From 938826b84e9ae02b4667a0ab31225927566ea8dc Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Thu, 21 Nov 2013 11:47:20 +0100 Subject: [PATCH 10/11] Forgot to test compile RigidBody Compile Errors is fixed --- Code/OysterPhysics3D/OysterPhysics3D.h | 8 ++++---- Code/OysterPhysics3D/RigidBody.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Code/OysterPhysics3D/OysterPhysics3D.h b/Code/OysterPhysics3D/OysterPhysics3D.h index 2eb9db9a..ca9948a8 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.h +++ b/Code/OysterPhysics3D/OysterPhysics3D.h @@ -63,7 +63,7 @@ namespace Oyster { namespace Physics3D ******************************************************************/ inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float3 linearMomentum, const ::Oyster::Math::Float3 &worldOffset ) { - return offset.Cross( linearMomentum ); + return worldOffset.Cross( linearMomentum ); } /****************************************************************** @@ -144,7 +144,7 @@ namespace Oyster { namespace Physics3D ******************************************************************/ inline ::Oyster::Math::Float3 TangentialLinearVelocity( const ::Oyster::Math::Float3 &angularVelocity, const ::Oyster::Math::Float3 &worldOffset ) { - return angularVelocity.Cross( offset ); + return angularVelocity.Cross( worldOffset ); } /****************************************************************** @@ -153,7 +153,7 @@ namespace Oyster { namespace Physics3D ******************************************************************/ 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 ); + return TangentialLinearVelocity( AngularVelocity(momentOfInertiaInversed, angularMomentum), worldOffset ); } /****************************************************************** @@ -180,7 +180,7 @@ namespace Oyster { namespace Physics3D ******************************************************************/ inline ::Oyster::Math::Float3 ImpulseTorque( const ::Oyster::Math::Float3 & impulseForce, const ::Oyster::Math::Float3 &worldOffset ) { - return offset.Cross( impulseForce ); + return worldOffset.Cross( impulseForce ); } /****************************************************************** diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index c65b040f..fb663cf4 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -273,7 +273,7 @@ void RigidBody::SetSize( const Float3 &widthHeight ) void RigidBody::SetCenter( const Float3 &worldPos ) { // by Dan Andersson - this->box.center = p; + this->box.center = worldPos; } void RigidBody::SetImpulseTorque( const Float3 &worldT ) From dc9f4597e24c0b5344ce6e467d53feb9c3aef326 Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Thu, 21 Nov 2013 15:19:32 +0100 Subject: [PATCH 11/11] Added OrientationMatrix build methods + other stuff --- Code/GamePhysics/GamePhysics.vcxproj.filters | 2 +- Code/GamePhysics/PhysicsAPI.h | 26 +++++++++++++++++--- Code/OysterMath/LinearMath.h | 19 +++++++++++++- Code/OysterMath/OysterMath.cpp | 10 ++++++++ Code/OysterMath/OysterMath.h | 6 +++++ Code/OysterPhysics3D/RigidBody.cpp | 2 +- 6 files changed, 58 insertions(+), 7 deletions(-) diff --git a/Code/GamePhysics/GamePhysics.vcxproj.filters b/Code/GamePhysics/GamePhysics.vcxproj.filters index 8c1b9f8b..07661556 100644 --- a/Code/GamePhysics/GamePhysics.vcxproj.filters +++ b/Code/GamePhysics/GamePhysics.vcxproj.filters @@ -30,7 +30,7 @@ - Header Files\Implementation + Source Files \ No newline at end of file diff --git a/Code/GamePhysics/PhysicsAPI.h b/Code/GamePhysics/PhysicsAPI.h index 5711b277..d9ae0a59 100644 --- a/Code/GamePhysics/PhysicsAPI.h +++ b/Code/GamePhysics/PhysicsAPI.h @@ -1,6 +1,7 @@ #ifndef PHYSICS_API_H #define PHYSICS_API_H +#include "OysterCollision3D.h" #include "OysterMath.h" #include "Utilities.h" @@ -12,6 +13,12 @@ namespace Oyster class IRigidBody; class IParticle; + enum UpdateState + { + resting, + altered + }; + namespace Constant { const float gravity_constant = (const float)6.67284e-11; // The _big_G_! ( N(m/kg)^2 ) Used in real gravityforcefields. @@ -32,17 +39,28 @@ namespace Oyster virtual void Update() = 0; - virtual void MoveToLimbo( unsigned int objRef ); - virtual void ReleaseFromLimbo( unsigned int objRef ); + virtual bool IsInLimbo( unsigned int objRef ) = 0; + virtual void MoveToLimbo( unsigned int objRef ) = 0; + virtual void ReleaseFromLimbo( unsigned int objRef ) = 0; - virtual unsigned int AddObject( ::Utility::DynamicMemory::UniquePointer handle ); - virtual void DestroyObject( unsigned int objRef ); + virtual unsigned int AddObject( ::Utility::DynamicMemory::UniquePointer handle ) = 0; + virtual ::Utility::DynamicMemory::UniquePointer ExtractObject( unsigned int objRef ) = 0; + virtual void DestroyObject( unsigned int objRef ) = 0; }; class IRigidBody { public: + virtual ~IRigidBody() {}; + virtual UpdateState Update( ::Oyster::Math::Float timeStepLength ) = 0; + + virtual bool IsSubscribingCollisions() const = 0; + virtual bool IsIntersecting( const IRigidBody &object, ::Oyster::Math::Float &deltaWhen, ::Oyster::Math::Float3 &worldPointOfContact ) = 0; + + virtual unsigned int GetReference() const = 0; + virtual ::Oyster::Collision3D::Sphere & GetBoundingSphere( ::Oyster::Collision3D::Sphere &targetMem = ::Oyster::Collision3D::Sphere() ) const = 0; + virtual ::Oyster::Math::Float3 & GetNormalAt( const ::Oyster::Math::Float3 &worldPos, ::Oyster::Math::Float3 &targetMem = ::Oyster::Math::Float3() ) const = 0; }; class IParticle diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h index 4c954cd6..ba427bb6 100644 --- a/Code/OysterMath/LinearMath.h +++ b/Code/OysterMath/LinearMath.h @@ -386,6 +386,23 @@ namespace LinearAlgebra3D return targetMem; } + template + inline ::LinearAlgebra::Matrix4x4 OrientationMatrix_LookAtDirection( const ::LinearAlgebra::Vector3 &normalizedDirection, const ::LinearAlgebra::Vector3 &normalizedUpVector, const ::LinearAlgebra::Vector3 &worldPos ) + { // Righthanded system! Forward is considered to be along negative z axis. Up is considered along positive y axis. + ::LinearAlgebra::Vector3 right = normalizedDirection.Cross( normalizedUpVector ).GetNormalized(); + return ::LinearAlgebra::Matrix4x4( ::LinearAlgebra::Vector4( right, 0.0f ), + ::LinearAlgebra::Vector4( right.Cross( normalizedDirection ), 0.0f ), + ::LinearAlgebra::Vector4( -normalizedDirection, 0.0f ), + ::LinearAlgebra::Vector4( worldPos, 1.0f ) ); + } + + template + inline ::LinearAlgebra::Matrix4x4 OrientationMatrix_LookAtPos( const ::LinearAlgebra::Vector3 &worldLookAt, const ::LinearAlgebra::Vector3 &normalizedUpVector, const ::LinearAlgebra::Vector3 &worldPos ) + { // Righthanded system! Forward is considered to be along negative z axis. Up is considered along positive y axis. + ::LinearAlgebra::Vector3 direction = ( worldLookAt - worldPos ).GetNormalized(); + return OrientationMatrix_LookAtDirection( direction, normalizedUpVector, worldPos ); + } + template inline ::LinearAlgebra::Matrix4x4 & InverseOrientationMatrix( const ::LinearAlgebra::Matrix4x4 &orientationMatrix, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { @@ -458,7 +475,7 @@ namespace LinearAlgebra3D template inline ::LinearAlgebra::Vector3 NormalProjection( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Vector3 &normalizedAxis ) - { return axis * ( vector.Dot(axis) ); } + { return normalizedAxis * ( vector.Dot(normalizedAxis) ); } } #include "Utilities.h" diff --git a/Code/OysterMath/OysterMath.cpp b/Code/OysterMath/OysterMath.cpp index ab318e92..35df5975 100644 --- a/Code/OysterMath/OysterMath.cpp +++ b/Code/OysterMath/OysterMath.cpp @@ -131,6 +131,16 @@ namespace Oyster { namespace Math3D return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, centerOfMass, targetMem ); } + Float4x4 & OrientationMatrix_LookAtDirection( const Float3 &normalizedDirection, const Float3 &normalizedUpVector, const Float3 &worldPos, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::OrientationMatrix_LookAtDirection( normalizedDirection, normalizedUpVector, worldPos ); + } + + Float4x4 & OrientationMatrix_LookAtPos( const Float3 &worldLookAt, const Float3 &normalizedUpVector, const Float3 &worldPos, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::OrientationMatrix_LookAtPos( worldLookAt, normalizedUpVector, worldPos ); + } + Float4x4 & InverseOrientationMatrix( const Float4x4 &orientationMatrix, Float4x4 &targetMem ) { return ::LinearAlgebra3D::InverseOrientationMatrix( orientationMatrix, targetMem ); diff --git a/Code/OysterMath/OysterMath.h b/Code/OysterMath/OysterMath.h index 4a1eeefe..125fd29d 100644 --- a/Code/OysterMath/OysterMath.h +++ b/Code/OysterMath/OysterMath.h @@ -207,6 +207,12 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized *******************************************************************/ Float4x4 & OrientationMatrix( const Float3 &sumDeltaAngularAxis, const Float3 &sumTranslation, const Float3 ¢erOfMass, Float4x4 &targetMem = Float4x4() ); + //! @todo TODO: Add documentation and not tested + Float4x4 & OrientationMatrix_LookAtDirection( const Float3 &normalizedDirection, const Float3 &normalizedUpVector, const Float3 &worldPos, Float4x4 &targetMem = Float4x4() ); + + //! @todo TODO: Add documentation and not tested + Float4x4 & OrientationMatrix_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. Float4x4 & InverseOrientationMatrix( const Float4x4 &orientationMatrix, Float4x4 &targetMem = Float4x4() ); diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index fb663cf4..214a1aee 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -70,7 +70,7 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) this->box.center += deltaPos; } - // update movements and clear impulseForceSum and impulseTorqueSum + // update momentums and clear impulseForceSum and impulseTorqueSum this->linearMomentum += linearImpulse; this->impulseForceSum = Float3::null; this->angularMomentum += angularImpulse;