From 31dd3390c2d86d99b26fb0aebca63173c4236b1f Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Wed, 22 Mar 2023 20:41:40 +0800 Subject: [PATCH 1/3] Fixed consistency of simulating ghost bodies --- .../src/Physics/System/SHPhysicsSystem.cpp | 132 ++++++++++-------- .../src/Physics/System/SHPhysicsSystem.h | 2 + 2 files changed, 73 insertions(+), 61 deletions(-) diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp index 32d6f03e..4fef71a3 100644 --- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp +++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp @@ -239,21 +239,6 @@ namespace SHADE void SHPhysicsSystem::SimulateBody(SHGhostBody& ghostBody, SimulateBodyInfo& simInfo, SimulateBodyOutput& output) { - // Check for a valid rigidbody - const auto* rigidBody = SHComponentManager::GetComponent_s(simInfo.bodyEID); - if (!rigidBody) - { - SHLOG_WARNING("Entity {} does not have a rigid body to simulate! This body will collide with everything!", simInfo.bodyEID) - } - - // Prepare simulation info (I'm basically declaring an entire body here) - float invMass = 1.0f / ghostBody.mass; - SHVec3 worldInvInertia = SHVec3::One; - SHVec3 worldCentroid = SHVec3::One; - - // Asserts. Don't be an idiot. - SHASSERT(invMass > 0, "GhostBody's mass in invalid") - // Build raycast layer from colliders. If none exist....then this never stops simulating technically. // I'm too lazy to handle that case, so I'll just throw an error. uint16_t raycastLayers = 0; @@ -275,10 +260,22 @@ namespace SHADE raycastInfo.continuous = false; raycastInfo.layers = raycastLayers; + // Check for a valid rigidbody + const auto* rigidBody = SHComponentManager::GetComponent_s(simInfo.bodyEID); + if (!rigidBody) + { + SHLOG_WARNING("Entity {} does not have a rigid body to simulate! This body will collide with everything!", simInfo.bodyEID) + } + + double accumulatedTime = 0.0f; + bool terminate = true; int iterationCounter = simInfo.maxSteps; + do { + accumulatedTime += simInfo.timeStep; + raycastInfo.distance = ghostBody.linearVelocity.Length() * simInfo.timeStep; // Do not take the entire velocity's length as that is for an entire second. raycastInfo.ray.position = ghostBody.position; raycastInfo.ray.direction = SHVec3::Normalise(ghostBody.linearVelocity); @@ -287,52 +284,8 @@ namespace SHADE if (terminate) return; - // Compute world space data - const SHMatrix R = SHMatrix::Rotate(ghostBody.orientation); - const SHMatrix RT = SHMatrix::Transpose(R); - - SHMatrix localInertiaTensor = SHMatrix::Identity; - - // Set the diagonals - localInertiaTensor.m[0][0] = ghostBody.localInvInertia.x; - localInertiaTensor.m[1][1] = ghostBody.localInvInertia.y; - localInertiaTensor.m[2][2] = ghostBody.localInvInertia.z; - - localInertiaTensor *= RT; - const SHVec3 DIAGONALS { localInertiaTensor.m[0][0], localInertiaTensor.m[1][1], localInertiaTensor.m[2][2] }; - - worldInvInertia = R * DIAGONALS; - - // Compute world centroid - worldCentroid = (R * ghostBody.localCentroid) + ghostBody.position; - - // Apply forces - ghostBody.accumulatedForce += simInfo.force; - ghostBody.angularVelocity += worldInvInertia * SHVec3::Cross(ghostBody.position + simInfo.forceOffset, simInfo.force); - ghostBody.accumulatedTorque += simInfo.torque; - - // Integrate Velocities - // Integrate forces and gravity into linear velocity - const SHVec3 LINEAR_ACCELERATION = ghostBody.accumulatedForce * invMass; - const SHVec3 GRAVITATIONAL_ACCELERATION = ghostBody.gravityScale ? worldState.settings.gravity * ghostBody.gravityScale : SHVec3::Zero; - - ghostBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * simInfo.timeStep * ghostBody.linearLock; - ghostBody.angularVelocity += worldInvInertia * (ghostBody.accumulatedTorque * simInfo.timeStep); - - // Apply drag (exponentially applied) - ghostBody.linearVelocity *= 1.0f / (1.0f + simInfo.timeStep * ghostBody.drag); - ghostBody.angularVelocity *= 1.0f / (1.0f + simInfo.timeStep * ghostBody.angularDrag); - - // Integrate Positions & Orientations - const SHQuaternion QV = SHQuaternion{ ghostBody.angularVelocity.x * simInfo.timeStep, ghostBody.angularVelocity.y * simInfo.timeStep, ghostBody.angularVelocity.z * simInfo.timeStep, 0.0f } * 0.5f; - - ghostBody.position += ghostBody.linearVelocity * simInfo.timeStep; - ghostBody.orientation += ghostBody.orientation * QV * SHQuaternion::FromEuler(ghostBody.angularLock); - ghostBody.orientation = SHQuaternion::Normalise(ghostBody.orientation); - - // Clear forces - ghostBody.accumulatedForce = SHVec3::Zero; - ghostBody.accumulatedTorque = SHVec3::Zero; + while (accumulatedTime > fixedDT) + simulateBody(ghostBody, simInfo); if (!simInfo.continuousForce) { @@ -494,4 +447,61 @@ namespace SHADE return onComponentRemovedEvent.get()->handle; } + void SHPhysicsSystem::simulateBody(SHGhostBody& ghostBody, const SimulateBodyInfo& simInfo) noexcept + { + float invMass = 1.0f / ghostBody.mass; + SHVec3 worldInvInertia = SHVec3::One; + SHVec3 worldCentroid = SHVec3::One; + + SHASSERT(invMass > 0, "GhostBody's mass in invalid") + + // Compute world space data + const SHMatrix R = SHMatrix::Rotate(ghostBody.orientation); + const SHMatrix RT = SHMatrix::Transpose(R); + + SHMatrix localInertiaTensor = SHMatrix::Identity; + + // Set the diagonals + localInertiaTensor.m[0][0] = ghostBody.localInvInertia.x; + localInertiaTensor.m[1][1] = ghostBody.localInvInertia.y; + localInertiaTensor.m[2][2] = ghostBody.localInvInertia.z; + + localInertiaTensor *= RT; + const SHVec3 DIAGONALS { localInertiaTensor.m[0][0], localInertiaTensor.m[1][1], localInertiaTensor.m[2][2] }; + + worldInvInertia = R * DIAGONALS; + + // Compute world centroid + worldCentroid = (R * ghostBody.localCentroid) + ghostBody.position; + + // Apply forces + ghostBody.accumulatedForce += simInfo.force; + ghostBody.angularVelocity += worldInvInertia * SHVec3::Cross(ghostBody.position + simInfo.forceOffset, simInfo.force); + ghostBody.accumulatedTorque += simInfo.torque; + + // Integrate Velocities + // Integrate forces and gravity into linear velocity + const SHVec3 LINEAR_ACCELERATION = ghostBody.accumulatedForce * invMass; + const SHVec3 GRAVITATIONAL_ACCELERATION = ghostBody.gravityScale ? worldState.settings.gravity * ghostBody.gravityScale : SHVec3::Zero; + + ghostBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * fixedDT * ghostBody.linearLock; + ghostBody.angularVelocity += worldInvInertia * (ghostBody.accumulatedTorque * fixedDT); + + // Apply drag (exponentially applied) + ghostBody.linearVelocity *= 1.0f / (1.0f + fixedDT * ghostBody.drag); + ghostBody.angularVelocity *= 1.0f / (1.0f + fixedDT * ghostBody.angularDrag); + + // Integrate Positions & Orientations + const SHQuaternion QV = SHQuaternion{ ghostBody.angularVelocity.x, ghostBody.angularVelocity.y, ghostBody.angularVelocity.z, 0.0f } * fixedDT * 0.5f; + + ghostBody.position += ghostBody.linearVelocity * simInfo.timeStep; + ghostBody.orientation += ghostBody.orientation * QV * SHQuaternion::FromEuler(ghostBody.angularLock); + ghostBody.orientation = SHQuaternion::Normalise(ghostBody.orientation); + + // Clear forces + ghostBody.accumulatedForce = SHVec3::Zero; + ghostBody.accumulatedTorque = SHVec3::Zero; + } + + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h index c81bf3a9..7f1e18f3 100644 --- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h +++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h @@ -274,5 +274,7 @@ namespace SHADE SHEventHandle onComponentAdded (SHEventPtr onComponentAddedEvent); SHEventHandle onComponentRemoved (SHEventPtr onComponentRemovedEvent); + + void simulateBody (SHGhostBody& ghostBody, const SimulateBodyInfo& simInfo) noexcept; }; } // namespace SHADE \ No newline at end of file From e02e3f5faf537dbf032561ff962d81b630fcac09 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Wed, 22 Mar 2023 22:21:39 +0800 Subject: [PATCH 2/3] silly mistake --- SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp index 4fef71a3..5b737728 100644 --- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp +++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp @@ -285,7 +285,11 @@ namespace SHADE return; while (accumulatedTime > fixedDT) + { simulateBody(ghostBody, simInfo); + accumulatedTime -= fixedDT; + } + if (!simInfo.continuousForce) { From 2737113a8450dee0d13ccca1c3938944f3dba4ae Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Wed, 22 Mar 2023 22:25:11 +0800 Subject: [PATCH 3/3] forces are cleared after first iteration --- .../src/Physics/System/SHPhysicsSystem.cpp | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp index 5b737728..49097807 100644 --- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp +++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp @@ -269,9 +269,7 @@ namespace SHADE double accumulatedTime = 0.0f; - bool terminate = true; - int iterationCounter = simInfo.maxSteps; - + int iterationCounter = simInfo.maxSteps; do { accumulatedTime += simInfo.timeStep; @@ -280,21 +278,22 @@ namespace SHADE raycastInfo.ray.position = ghostBody.position; raycastInfo.ray.direction = SHVec3::Normalise(ghostBody.linearVelocity); - terminate = !Raycast(raycastInfo).empty() || iterationCounter == 0; - if (terminate) + if (!Raycast(raycastInfo).empty()) return; while (accumulatedTime > fixedDT) { simulateBody(ghostBody, simInfo); accumulatedTime -= fixedDT; - } - - if (!simInfo.continuousForce) - { - simInfo.force = SHVec3::Zero; - simInfo.torque = SHVec3::Zero; + if (!simInfo.continuousForce) + { + simInfo.force = SHVec3::Zero; + simInfo.torque = SHVec3::Zero; + } + + if (--iterationCounter == 0) + return; } if (output.positions) @@ -303,8 +302,6 @@ namespace SHADE if (output.orientations) output.orientations->emplace_back(ghostBody.orientation); - --iterationCounter; - } while (true); }