From dab109bc771b950548087178194c1180cf739671 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Mon, 16 Jan 2023 02:43:31 +0800 Subject: [PATCH] Fixed a fatal error with rigid body rotations. --- .../src/Physics/Dynamics/SHMotionState.cpp | 5 +- .../src/Physics/Dynamics/SHPhysicsWorld.cpp | 2 +- .../src/Physics/Dynamics/SHRigidBody.cpp | 13 +-- .../Routines/SHPhysicsPostUpdateRoutine.cpp | 82 ++++++++++--------- 4 files changed, 52 insertions(+), 50 deletions(-) diff --git a/SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp b/SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp index cc014050..89c8ad32 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp @@ -109,10 +109,7 @@ namespace SHADE prevOrientation = orientation; - SHQuaternion qv{ velocity.x * dt, velocity.y * dt, velocity.z * dt, 0.0f }; - qv *= orientation; - - orientation += qv * 0.5f; + orientation += orientation * SHQuaternion{ velocity.x, velocity.y, velocity.z, 0.0f } * dt * 0.5f; orientation = SHQuaternion::Normalise(orientation); } diff --git a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp index 3595ec99..ed00e80e 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp @@ -142,7 +142,7 @@ namespace SHADE rigidBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * dt; // Integrate torque into angular velocity - rigidBody.angularVelocity += rigidBody.worldInvInertia * (rigidBody.accumulatedTorque * dt); + rigidBody.angularVelocity += rigidBody.worldInvInertia * rigidBody.accumulatedTorque * dt; // Apply drag (exponentially applied) rigidBody.linearVelocity *= 1.0f / (1.0f + dt * rigidBody.linearDrag); diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp index d76b79b8..1de9e543 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp @@ -637,16 +637,17 @@ namespace SHADE void SHRigidBody::ComputeWorldData() noexcept { + const SHMatrix R = SHMatrix::Rotate(motionState.orientation); + const SHMatrix RT = SHMatrix::Transpose(R); + + // Compute world centroid + worldCentroid = (R * localCentroid) + motionState.position; + if (bodyType == Type::STATIC) return; - const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation); - // Compute world inertia - worldInvInertia = SHMatrix::Transpose(ROTATION) * localInvInertia * ROTATION; - - // Compute world centroid - worldCentroid = (ROTATION * localCentroid) + motionState.position; + worldInvInertia = R * (localInvInertia * RT); } void SHRigidBody::ComputeMassData() noexcept diff --git a/SHADE_Engine/src/Physics/System/Routines/SHPhysicsPostUpdateRoutine.cpp b/SHADE_Engine/src/Physics/System/Routines/SHPhysicsPostUpdateRoutine.cpp index 5b0e5563..a08b943b 100644 --- a/SHADE_Engine/src/Physics/System/Routines/SHPhysicsPostUpdateRoutine.cpp +++ b/SHADE_Engine/src/Physics/System/Routines/SHPhysicsPostUpdateRoutine.cpp @@ -19,7 +19,6 @@ #include "Scene/SHSceneManager.h" #include "Scripting/SHScriptEngine.h" - namespace SHADE { /*-----------------------------------------------------------------------------------*/ @@ -48,50 +47,55 @@ namespace SHADE // Interpolate transforms for rendering. // Only rigid bodies can move due to physics, so we run through the rigid body component dense set. - const auto& RIGIDBODY_DENSE = SHComponentManager::GetDense(); - for (auto& rigidBodyComponent : RIGIDBODY_DENSE) + if (physicsSystem->worldUpdated) { - const EntityID EID = rigidBodyComponent.GetEID(); - - // Skip missing transforms - auto* transformComponent = SHComponentManager::GetComponent_s(EID); - if (!transformComponent) - continue; - - // Skip invalid bodies (Should not occur) - if (!rigidBodyComponent.rigidBody) - continue; - - // Skip inactive bodies - const bool IS_ACTIVE = SHSceneManager::CheckNodeAndComponentsActive(EID); - if (!IS_ACTIVE) - continue; - - const SHMotionState& MOTION_STATE = rigidBodyComponent.rigidBody->GetMotionState(); - - // Skip objects that have not moved - if (!MOTION_STATE) - continue; - - if (rigidBodyComponent.IsInterpolating()) + const auto& RIGIDBODY_DENSE = SHComponentManager::GetDense(); + for (auto& rigidBodyComponent : RIGIDBODY_DENSE) { - const SHVec3 RENDER_POSITION = MOTION_STATE.InterpolatePositions(FACTOR); - const SHQuaternion RENDER_ORIENTATION = MOTION_STATE.InterpolateOrientations(FACTOR); + const EntityID EID = rigidBodyComponent.GetEID(); - transformComponent->SetWorldPosition(RENDER_POSITION); - transformComponent->SetWorldOrientation(RENDER_ORIENTATION); - } - else - { - transformComponent->SetWorldPosition(MOTION_STATE.position); - transformComponent->SetWorldOrientation(MOTION_STATE.orientation); - } + // Skip missing transforms + auto* transformComponent = SHComponentManager::GetComponent_s(EID); + if (!transformComponent) + continue; - /* - * TODO: Test if the scene graph transforms abides by setting world position. Collisions will ignore the scene graph hierarchy. - */ + // Skip invalid bodies (Should not occur) + if (!rigidBodyComponent.rigidBody) + continue; + + // Skip inactive bodies + const bool IS_ACTIVE = SHSceneManager::CheckNodeAndComponentsActive(EID); + if (!IS_ACTIVE) + continue; + + const SHMotionState& MOTION_STATE = rigidBodyComponent.rigidBody->GetMotionState(); + + // Skip objects that have not moved + if (!MOTION_STATE) + continue; + + if (rigidBodyComponent.IsInterpolating()) + { + const SHVec3 RENDER_POSITION = MOTION_STATE.InterpolatePositions(FACTOR); + const SHQuaternion RENDER_ORIENTATION = MOTION_STATE.InterpolateOrientations(FACTOR); + + transformComponent->SetWorldPosition(RENDER_POSITION); + transformComponent->SetWorldOrientation(RENDER_ORIENTATION); + } + else + { + transformComponent->SetWorldPosition(MOTION_STATE.position); + transformComponent->SetWorldOrientation(MOTION_STATE.orientation); + } + + /* + * TODO: Test if the scene graph transforms abides by setting world position. Collisions will ignore the scene graph hierarchy. + */ + } } + + // Collision & Trigger messages if (scriptingSystem != nullptr) scriptingSystem->ExecuteCollisionFunctions();