From 88e67ff8762c440808e891e154fa173809bd810d Mon Sep 17 00:00:00 2001 From: Dander7BD Date: Wed, 29 Jan 2014 14:57:39 +0100 Subject: [PATCH] Inertia big patch It's ali-..WORKS! It works! --- Code/GamePhysics/PhysicsStructs-Impl.h | 21 +++--------------- Code/OysterPhysics3D/Inertia.cpp | 30 +++++++++++++++++++------- Code/OysterPhysics3D/OysterPhysics3D.h | 2 +- Code/OysterPhysics3D/RigidBody.cpp | 26 +++------------------- 4 files changed, 29 insertions(+), 50 deletions(-) diff --git a/Code/GamePhysics/PhysicsStructs-Impl.h b/Code/GamePhysics/PhysicsStructs-Impl.h index fa5533fa..05cba2da 100644 --- a/Code/GamePhysics/PhysicsStructs-Impl.h +++ b/Code/GamePhysics/PhysicsStructs-Impl.h @@ -15,7 +15,7 @@ namespace Oyster this->rotation = ::Oyster::Math::Float4x4::identity; this->centerPosition = ::Oyster::Math::Float3::null; this->size = ::Oyster::Math::Float3( 1.0f ); - this->mass = 12.0f; + this->mass = 6.0f; this->restitutionCoeff = 1.0f; this->frictionCoeff_Dynamic = 0.5f; this->frictionCoeff_Static = 0.5f; @@ -124,7 +124,7 @@ namespace Oyster inline const ::Oyster::Math::Float3 & CustomBodyState::GetAngularAxis() const { - return this->angularAxis; + return ::Utility::Value::Radian(this->angularAxis); } inline ::Oyster::Math::Float4x4 CustomBodyState::GetRotation() const @@ -201,8 +201,6 @@ namespace Oyster { if( m != 0.0f ) { // sanity block! - // Formula::LinearMomentum( m, Formula::LinearVelocity(this->mass, this->linearMomentum) ) - // is the same as (this->linearMomentum / this->mass) * m = (m / this->mass) * this->linearMomentum this->linearMomentum *= (m / this->mass); this->mass = m; } @@ -263,19 +261,6 @@ namespace Oyster this->isSpatiallyAltered = this->isDisturbed = true; } - /*inline void CustomBodyState::SetRotation( const ::Oyster::Math::Float4x4 &rotation ) - { - this->SetRotation( ::Oyster::Math3D::AngularAxis(rotation) ); - } - - inline void CustomBodyState::SetOrientation( const ::Oyster::Math::Float4x4 &orientation ) - { - this->SetRotation( ::Oyster::Math3D::ExtractAngularAxis(orientation) ); - this->SetCenterPosition( orientation.v[3] ); - }*/ - - - inline void CustomBodyState::SetLinearMomentum( const ::Oyster::Math::Float3 &g ) { this->linearMomentum = g; @@ -334,8 +319,8 @@ namespace Oyster ::Oyster::Math::Float3 offset = at - this->centerPos; ::Oyster::Math::Float3 deltaAngularImpulse = ::Oyster::Physics3D::Formula::AngularMomentum( j, offset ); this->linearImpulse += j - ::Oyster::Physics3D::Formula::TangentialLinearMomentum( deltaAngularImpulse, offset ); - this->angularImpulse += deltaAngularImpulse; + this->angularImpulse += deltaAngularImpulse; this->isDisturbed = true; } diff --git a/Code/OysterPhysics3D/Inertia.cpp b/Code/OysterPhysics3D/Inertia.cpp index 2e0e436b..45551e2c 100644 --- a/Code/OysterPhysics3D/Inertia.cpp +++ b/Code/OysterPhysics3D/Inertia.cpp @@ -32,10 +32,17 @@ Float3 MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, con } Float3 & MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float3 &h, Float3 &targetMem ) const -{ // w = (R * I_R) * I_M^-1 * (R * I_R)^-1 * h - Float4x4 rotation = RotationMatrix( externR ) * RotationMatrix( this->rotation ); - Float4 w = rotation.GetInverse() * Float4( h, 0.0f ); - return targetMem = rotation * w.PiecewiseMultiplicationAdd( Float4(1.0f / this->magnitude.x, 1.0f / this->magnitude.y, 1.0f / this->magnitude.z, 0.0f) ); +{ // w = h * | (2/3) * I_M^-1 (R I_R)^-1 h | / |h| + Float hMagnitudeSquared = h.Dot( h ); + if( hMagnitudeSquared > 0.0f ) + { + Float4x4 rotationInverse = (RotationMatrix( externR ) * RotationMatrix( this->rotation )).Transpose(); + Float4 v = rotationInverse * Float4( h, 0.0f ); + v.PiecewiseMultiplicationAdd( Float4((2.0f/3.0f) / this->magnitude.x, (2.0f/3.0f) / this->magnitude.y, (2.0f/3.0f) / this->magnitude.z, 0.0f) ); + return targetMem = (Float4( h, 0.0f ) * ( v.GetMagnitude() / ( (Float)::std::sqrt(hMagnitudeSquared)) ) ).xyz; + } + else + return targetMem = Float3::null; } Float3 MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float3 &w ) const @@ -44,8 +51,15 @@ Float3 MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, con } Float3 & MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float3 &w, Float3 &targetMem ) const -{ // h = (R * I_R) * I_M * (R * I_R)^-1 * w - Float4x4 rotation = RotationMatrix( externR ) * RotationMatrix( this->rotation ); - Float4 h = rotation.GetInverse() * Float4( w, 0.0f ); - return targetMem = rotation * h.PiecewiseMultiplicationAdd( Float4(this->magnitude.x, this->magnitude.y, this->magnitude.z, 0.0f) ); +{ // h = w * | (3/2) * I_M (R I_R)^-1 w | / |w| + Float wMagnitudeSquared = w.Dot( w ); + if( wMagnitudeSquared > 0.0f ) + { + Float4x4 rotationInverse = (RotationMatrix( externR ) * RotationMatrix( this->rotation )).Transpose(); + Float4 v = rotationInverse * Float4( w, 0.0f ); + v.PiecewiseMultiplicationAdd( Float4((3.0f/2.0f) * this->magnitude.x, (3.0f/2.0f) * this->magnitude.y, (3.0f/2.0f) * this->magnitude.z, 0.0f) ); + return targetMem = (Float4( w, 0.0f ) * ( v.GetMagnitude() / (Float)::std::sqrt(wMagnitudeSquared) ) ).xyz; + } + else + return targetMem = Float3::null; } \ No newline at end of file diff --git a/Code/OysterPhysics3D/OysterPhysics3D.h b/Code/OysterPhysics3D/OysterPhysics3D.h index f814ff46..e1eb94e3 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.h +++ b/Code/OysterPhysics3D/OysterPhysics3D.h @@ -113,6 +113,7 @@ namespace Oyster { namespace Physics3D /****************************************************************** * Returns the world angular momentum of a mass in rotation. + * H = r x G * @todo TODO: improve doc ******************************************************************/ inline ::Oyster::Math::Float3 AngularMomentum( const ::Oyster::Math::Float3 linearMomentum, const ::Oyster::Math::Float3 &worldOffset ) @@ -381,7 +382,6 @@ namespace Oyster { namespace Physics3D return inertia; } - } } } } diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 8e5f5f2b..0c60c597 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -53,22 +53,15 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength ) this->centerPos += ( updateFrameLength / this->mass ) * AverageWithDelta( this->momentum_Linear, this->impulse_Linear ); // updating the angular - //Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix ); - // Important! The member data is all world data except the Inertia tensor. Thus a new InertiaTensor needs to be created to be compatible with the rest of the world data. - //Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI - - // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H - - //! HACK: @todo Rotation temporary disabled - //this->axis += Radian( Formula::AngularVelocity(wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, this->impulse_Angular)) ); - this->axis += this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); + // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H + this->axis += updateFrameLength * this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); this->rotation = Rotation( this->axis ); // update momentums and clear impulse_Linear and impulse_Angular this->momentum_Linear += this->impulse_Linear; this->impulse_Linear = Float4::null; - //this->momentum_Angular += this->impulse_Angular; //! HACK: @todo Rotation temporary disabled + this->momentum_Angular += this->impulse_Angular; //! HACK: @todo Rotation temporary disabled this->impulse_Angular = Float4::null; } @@ -183,19 +176,6 @@ void RigidBody::SetMass_KeepMomentum( const Float &m ) } } -//void RigidBody::SetOrientation( const Float4x4 &o ) -//{ // by Dan Andersson -// this->axis = ExtractAngularAxis( o ); -// this->rotation = Rotation( this->axis ); -// this->centerPos = o.v[3].xyz; -//} -// -//void RigidBody::SetRotation( const Float4x4 &r ) -//{ // by Dan Andersson -// this->axis = ExtractAngularAxis( r ); -// this->rotation = Rotation( this->axis ); -//} - void RigidBody::SetMomentum_Linear( const Float3 &worldG, const Float3 &atWorldPos ) { // by Dan Andersson Float3 worldOffset = atWorldPos - this->centerPos;