diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index e774752b..e33308e2 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -8,6 +8,7 @@ using namespace ::Oyster::Collision3D; using namespace ::Oyster::Physics3D; using namespace ::Oyster::Math3D; +using namespace ::Utility::Value; RigidBody::RigidBody( ) { // by Dan Andersson @@ -49,7 +50,7 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength ) // updating the linear // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G - this->centerPos += ( updateFrameLength / this->mass ) * ::Utility::Value::AverageWithDelta( this->momentum_Linear, this->impulse_Linear ); + this->centerPos += ( updateFrameLength / this->mass ) * AverageWithDelta( this->momentum_Linear, this->impulse_Linear ); // updating the angular Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix ); @@ -57,7 +58,7 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength ) Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H - this->axis += Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), ::Utility::Value::AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); + this->axis += Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); this->rotation = Rotation( this->axis ); // update momentums and clear impulse_Linear and impulse_Angular @@ -67,6 +68,20 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength ) this->impulse_Angular = Float4::null; } +void RigidBody::Predict_LeapFrog( Float4 &outDeltaPos, Float4 &outDeltaAxis, const Float4 &actingLinearImpulse, const Float4 &actingAngularImpulse, Float deltaTime ) +{ + // updating the linear + // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G + outDeltaPos = ( deltaTime / this->mass ) * AverageWithDelta( this->momentum_Linear, actingLinearImpulse ); + + // updating the angular + Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix ); + Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI + + // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H + outDeltaAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, actingAngularImpulse) ); +} + void RigidBody::ApplyImpulse( const Float4 &worldJ, const Float4 &atWorldPos ) { // by Dan Andersson Float4 worldOffset = atWorldPos - this->centerPos; diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index c140982c..1b32c57b 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -30,6 +30,7 @@ namespace Oyster { namespace Physics3D RigidBody & operator = ( const RigidBody &body ); void Update_LeapFrog( ::Oyster::Math::Float updateFrameLength ); + void Predict_LeapFrog( ::Oyster::Math::Float4 &outDeltaPos, ::Oyster::Math::Float4 &outDeltaAxis, const ::Oyster::Math::Float4 &actingLinearImpulse, const ::Oyster::Math::Float4 &actingAngularImpulse, ::Oyster::Math::Float deltaTime ); void ApplyImpulse( const ::Oyster::Math::Float4 &worldJ, const ::Oyster::Math::Float4 &atWorldPos ); void ApplyImpulse_Linear( const ::Oyster::Math::Float4 &worldJ );