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