From 11878d4cc2ba764ff0cf65e40cb7eb8b6024d1d9 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Mon, 20 Mar 2023 14:53:36 +0800 Subject: [PATCH] reworked simulate body --- SHADE_Engine/SHGhostBody.cpp | 51 ++++++++++ SHADE_Engine/SHGhostBody.h | 64 +++++++++++++ .../SHTrajectoryRenderingSubSystem.cpp | 28 ++++-- .../Interface/SHRigidBodyComponent.cpp | 10 ++ .../Physics/Interface/SHRigidBodyComponent.h | 3 + .../src/Physics/System/SHPhysicsSystem.cpp | 96 ++++++++----------- .../src/Physics/System/SHPhysicsSystem.h | 71 ++++++++------ 7 files changed, 230 insertions(+), 93 deletions(-) create mode 100644 SHADE_Engine/SHGhostBody.cpp create mode 100644 SHADE_Engine/SHGhostBody.h diff --git a/SHADE_Engine/SHGhostBody.cpp b/SHADE_Engine/SHGhostBody.cpp new file mode 100644 index 00000000..ef260521 --- /dev/null +++ b/SHADE_Engine/SHGhostBody.cpp @@ -0,0 +1,51 @@ +/**************************************************************************************** + * \file SHGhostBody.cpp + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Implementation for the Ghost Body meant for Simulation. + * + * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or + * disclosure of this file or its contents without the prior written consent + * of DigiPen Institute of Technology is prohibited. +****************************************************************************************/ + +#include + +// Primary Header +#include "SHGhostBody.h" + +namespace SHADE +{ + /*-----------------------------------------------------------------------------------*/ + /* Constructors & Destructor Definitions */ + /*-----------------------------------------------------------------------------------*/ + + SHGhostBody::SHGhostBody(const SHRigidBodyComponent& rigidBody) noexcept + : linearVelocity { rigidBody.GetLinearVelocity() } + , angularVelocity { rigidBody.GetAngularVelocity() } + , localCentroid { rigidBody.GetLocalCentroid() } + , accumulatedForce { rigidBody.GetForce() } + , accumulatedTorque{ rigidBody.GetTorque() } + , gravityScale { rigidBody.GetGravityScale() } + , mass { rigidBody.GetMass() } + , drag { rigidBody.GetDrag() } + , angularDrag { rigidBody.GetAngularDrag() } + , position { rigidBody.GetPosition() } + , orientation { SHQuaternion::FromEuler(rigidBody.GetRotation()) } + , useGravity { rigidBody.IsGravityEnabled() } + { + const SHVec3 LOCAL_INERTIA = rigidBody.GetLocalInertia(); + localInvInertia.x = 1.0f / LOCAL_INERTIA.x; + localInvInertia.y = 1.0f / LOCAL_INERTIA.y; + localInvInertia.z = 1.0f / LOCAL_INERTIA.z; + + linearLock.x = rigidBody.GetFreezePositionX() ? 1.0f : 0.0f; + linearLock.y = rigidBody.GetFreezePositionY() ? 1.0f : 0.0f; + linearLock.z = rigidBody.GetFreezePositionZ() ? 1.0f : 0.0f; + + angularLock.x = rigidBody.GetFreezeRotationX() ? 1.0f : 0.0f; + angularLock.y = rigidBody.GetFreezeRotationY() ? 1.0f : 0.0f; + angularLock.z = rigidBody.GetFreezeRotationZ() ? 1.0f : 0.0f; + } + + +} // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/SHGhostBody.h b/SHADE_Engine/SHGhostBody.h new file mode 100644 index 00000000..ebc2b0db --- /dev/null +++ b/SHADE_Engine/SHGhostBody.h @@ -0,0 +1,64 @@ +/**************************************************************************************** + * \file SHGhostBody.h + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Interface for the Ghost Body meant for Simulation. + * + * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or + * disclosure of this file or its contents without the prior written consent + * of DigiPen Institute of Technology is prohibited. +****************************************************************************************/ + +#pragma once + +// Project Headers +#include "Physics/Interface/SHRigidBodyComponent.h" + +namespace SHADE +{ + /*-----------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*-----------------------------------------------------------------------------------*/ + + /** + * @brief + * Encapsulates a rigid body that will be simulated in the world, but doesn't actually + * exist in the world. + */ + struct SHGhostBody + { + public: + /*---------------------------------------------------------------------------------*/ + /* Data Members */ + /*---------------------------------------------------------------------------------*/ + + SHVec3 linearVelocity = SHVec3::Zero; + SHVec3 angularVelocity = SHVec3::Zero; + + SHVec3 localInvInertia = SHVec3::One; + SHVec3 localCentroid = SHVec3::Zero; + SHVec3 accumulatedForce = SHVec3::Zero; + SHVec3 accumulatedTorque = SHVec3::Zero; + + SHVec3 linearLock = SHVec3::One; + SHVec3 angularLock = SHVec3::One; + + float gravityScale = 1.0f; + float mass = 1.0f; + float drag = 0.01f; + float angularDrag = 0.01f; + + SHVec3 position = SHVec3::Zero; + SHQuaternion orientation = SHQuaternion::Identity; + bool useGravity = true; + + /*---------------------------------------------------------------------------------*/ + /* Constructors & Destructor */ + /*---------------------------------------------------------------------------------*/ + + SHGhostBody () noexcept = default; + SHGhostBody (const SHRigidBodyComponent& rigidBody) noexcept; + + }; + + +} // namespace SHADE diff --git a/SHADE_Engine/src/Graphics/MiddleEnd/TrajectoryRendering/SHTrajectoryRenderingSubSystem.cpp b/SHADE_Engine/src/Graphics/MiddleEnd/TrajectoryRendering/SHTrajectoryRenderingSubSystem.cpp index f923add4..f2689209 100644 --- a/SHADE_Engine/src/Graphics/MiddleEnd/TrajectoryRendering/SHTrajectoryRenderingSubSystem.cpp +++ b/SHADE_Engine/src/Graphics/MiddleEnd/TrajectoryRendering/SHTrajectoryRenderingSubSystem.cpp @@ -1,5 +1,7 @@ #include "SHpch.h" #include "SHTrajectoryRenderingSubSystem.h" + +#include "../../../../SHGhostBody.h" #include "ECS_Base/Managers/SHComponentManager.h" #include "Graphics/MiddleEnd/TrajectoryRendering/SHTrajectoryRenderableComponent.h" #include "Graphics/Devices/SHVkLogicalDevice.h" @@ -85,17 +87,25 @@ namespace SHADE { std::vector positions{}; std::vector quats{}; - physicsSystem->SimulateBody - (positions, quats, - SHPhysicsSystem::SimulateBodyInfo - { - .bodyEID = entityToSimulate, - .force = comp.GetSimulationForce(), - .continuousForce = false, + + SHGhostBody defaultGhostBody{}; + + SHPhysicsSystem::SimulateBodyInfo simulateInfo + { + .bodyEID = entityToSimulate, + .force = comp.GetSimulationForce(), + .continuousForce = false, .timeStep = comp.GetSimulationTimestep(), .maxSteps = static_cast(comp.GetSimulationMaxSteps()), - } - ); + }; + + SHPhysicsSystem::SimulateBodyOutput output + { + .positions = &positions + , .orientations = &quats + }; + + physicsSystem->SimulateBody(defaultGhostBody, simulateInfo, output); comp.ResetSimulationInfo(); diff --git a/SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.cpp b/SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.cpp index b26f89d9..1017098f 100644 --- a/SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.cpp +++ b/SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.cpp @@ -150,6 +150,16 @@ namespace SHADE return rigidBody ? SHQuaternion{ rigidBody->getTransform().getOrientation() }.ToEuler() : SHVec3::Zero; } + SHVec3 SHRigidBodyComponent::GetLocalInertia() const noexcept + { + return rigidBody ? SHVec3{ rigidBody->getLocalInertiaTensor() } : SHVec3::Zero; + } + + SHVec3 SHRigidBodyComponent::GetLocalCentroid() const noexcept + { + return rigidBody ? SHVec3{ rigidBody->getLocalCenterOfMass() } : SHVec3::Zero; + } + /*-----------------------------------------------------------------------------------*/ /* Setter Function Definitions */ /*-----------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.h b/SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.h index a1cbe786..8f5fe3d4 100644 --- a/SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.h +++ b/SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.h @@ -103,6 +103,9 @@ namespace SHADE [[nodiscard]] SHVec3 GetPosition () const noexcept; [[nodiscard]] SHVec3 GetRotation () const noexcept; + [[nodiscard]] SHVec3 GetLocalInertia () const noexcept; + [[nodiscard]] SHVec3 GetLocalCentroid () const noexcept; + /*---------------------------------------------------------------------------------*/ /* Setter Functions */ /*---------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp index f19002e6..75ed50f4 100644 --- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp +++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp @@ -14,6 +14,7 @@ #include "SHPhysicsSystem.h" // Project Headers +#include "../../../SHGhostBody.h" #include "Assets/SHAssetMacros.h" #include "ECS_Base/Managers/SHComponentManager.h" #include "ECS_Base/Managers/SHEntityManager.h" @@ -236,44 +237,22 @@ namespace SHADE return IS_COLLIDING; } - void SHPhysicsSystem::SimulateBody(std::vector& positions, std::vector& orientations, const SimulateBodyInfo& simInfo) + 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!", simInfo.bodyEID) - return; - } - - // Ignore non-dynamic bodies - if (rigidBody->type != SHRigidBodyComponent::Type::DYNAMIC) - { - SHLOG_WARNING("Entity {} is not a dynamic body. We cannot simulate non-dynamic bodies!", simInfo.bodyEID) - return; + 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) - SHVec3 bodyPosition = rigidBody->GetPosition(); - SHQuaternion bodyOrientation = SHQuaternion::FromEuler(rigidBody->GetRotation()); - SHVec3 linearVelocity = rigidBody->GetLinearVelocity(); - SHVec3 angularVelocity = rigidBody->GetAngularVelocity(); - float invMass = 1.0f / rigidBody->GetMass(); - SHVec3 localInvInertia = rigidBody->rigidBody->getLocalInertiaTensor(); - SHVec3 worldInvInertia = SHVec3::One; - SHVec3 localCentroid = rigidBody->rigidBody->getLocalCenterOfMass(); - SHVec3 worldCentroid = SHVec3::One; - SHVec3 appliedForce = simInfo.force; - SHVec3 appliedTorque = simInfo.torque; - SHVec3 accumulatedForce = SHVec3::Zero; - SHVec3 accumulatedTorque = SHVec3::Zero; + float invMass = 1.0f / ghostBody.mass; + SHVec3 worldInvInertia = SHVec3::One; + SHVec3 worldCentroid = SHVec3::One; - const SHVec3& LINEAR_LOCK = rigidBody->rigidBody->getLinearLockAxisFactor(); - const SHVec3& ANGULAR_LOCK = rigidBody->rigidBody->getAngularLockAxisFactor(); - - // Invert the inertia - for (size_t i = 0; i < SHVec3::SIZE; ++i) - localInvInertia[i] = 1.0f / localInvInertia[i]; + // 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. @@ -300,24 +279,24 @@ namespace SHADE int iterationCounter = simInfo.maxSteps; do { - raycastInfo.distance = linearVelocity.Length() * simInfo.timeStep; // Do not take the entire velocity's length as that is for an entire second. - raycastInfo.ray.position = bodyPosition; - raycastInfo.ray.direction = SHVec3::Normalise(linearVelocity); + 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); terminate = !Raycast(raycastInfo).empty() || iterationCounter == 0; if (terminate) return; // Compute world space data - const SHMatrix R = SHMatrix::Rotate(bodyOrientation); + const SHMatrix R = SHMatrix::Rotate(ghostBody.orientation); const SHMatrix RT = SHMatrix::Transpose(R); SHMatrix localInertiaTensor = SHMatrix::Identity; // Set the diagonals - localInertiaTensor.m[0][0] = localInvInertia.x; - localInertiaTensor.m[1][1] = localInvInertia.y; - localInertiaTensor.m[2][2] = localInvInertia.z; + 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] }; @@ -325,42 +304,47 @@ namespace SHADE worldInvInertia = R * DIAGONALS; // Compute world centroid - worldCentroid = (R * localCentroid) + bodyPosition; + worldCentroid = (R * ghostBody.localCentroid) + ghostBody.position; // Apply forces - accumulatedForce += appliedForce; - angularVelocity += worldInvInertia * SHVec3::Cross(bodyPosition + simInfo.forceOffset, simInfo.force); - accumulatedTorque += appliedTorque; + 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 = accumulatedForce * invMass; - const SHVec3 GRAVITATIONAL_ACCELERATION = simInfo.simulateGravity ? worldState.settings.gravity * rigidBody->GetGravityScale() : SHVec3::Zero; + const SHVec3 LINEAR_ACCELERATION = ghostBody.accumulatedForce * invMass; + const SHVec3 GRAVITATIONAL_ACCELERATION = ghostBody.gravityScale ? worldState.settings.gravity * ghostBody.gravityScale : SHVec3::Zero; - linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * simInfo.timeStep * LINEAR_LOCK; - angularVelocity += worldInvInertia * (accumulatedTorque * simInfo.timeStep); + ghostBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * simInfo.timeStep * ghostBody.linearLock; + ghostBody.angularVelocity += worldInvInertia * (ghostBody.accumulatedTorque * simInfo.timeStep); // Apply drag (exponentially applied) - linearVelocity *= 1.0f / (1.0f + simInfo.timeStep * rigidBody->drag); - angularVelocity *= 1.0f / (1.0f + simInfo.timeStep * rigidBody->angularDrag); + 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{ angularVelocity.x * simInfo.timeStep, angularVelocity.y * simInfo.timeStep, angularVelocity.z * simInfo.timeStep, 0.0f } * 0.5f; + const SHQuaternion QV = SHQuaternion{ ghostBody.angularVelocity.x * simInfo.timeStep, ghostBody.angularVelocity.y * simInfo.timeStep, ghostBody.angularVelocity.z * simInfo.timeStep, 0.0f } * 0.5f; - bodyPosition += linearVelocity * simInfo.timeStep; - bodyOrientation += bodyOrientation * QV * SHQuaternion::FromEuler(ANGULAR_LOCK); - bodyOrientation = SHQuaternion::Normalise(bodyOrientation); + 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; - // Clear forces after the first frame if (!simInfo.continuousForce) { - accumulatedForce = SHVec3::Zero; - accumulatedTorque = SHVec3::Zero; - appliedForce = SHVec3::Zero; - appliedTorque = SHVec3::Zero; + simInfo.force = SHVec3::Zero; + simInfo.torque = SHVec3::Zero; } - positions.emplace_back(bodyPosition); + if (output.positions) + output.positions->emplace_back(ghostBody.position); + + if (output.orientations) + output.orientations->emplace_back(ghostBody.orientation); --iterationCounter; diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h index 0a1e2057..c81bf3a9 100644 --- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h +++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h @@ -28,6 +28,7 @@ namespace SHADE { + struct SHGhostBody; /*-----------------------------------------------------------------------------------*/ /* Type Definitions */ /*-----------------------------------------------------------------------------------*/ @@ -51,41 +52,54 @@ namespace SHADE /** * @brief * Used to simulate the motion of a rigid body, ignoring collision detection and response. - * @param bodyEID - * The EntityID of the Rigid Body to simulate. - * @param force - * The force applied onto the Rigid Body. - * @param forceOffset - * The position to apply the force onto the body relative to it's local Center of Mass. - * @param torque - * The torque to apply onto the Rigid Body. - * @param continuousForce - * If the force should be applied every step throughout the simulation. Defaults to false.
- * True : The force indicated is added to the body every step, therefore it has constant acceleration. - * False: The force is applied only in the first step, therefore it has constant speed. - * @param timeStep - * The timestep for each step of the simulation. Defaults to 0.016s (The default Fixed DT) - * @param maxSteps - * The number of steps to run the simulation for. Defaults to -1. - * < 0 : Runs until the object may hit something. Hit detection is done through raycasting. - * = 0 : Runs only the current step and checks if it may hit something. - * > 0 : Runs for the given number of steps or until it may hit something. */ struct SimulateBodyInfo { + public: + + // The EntityID of the Actual Rigid Body to simulate. If none is passed it, + // the Ghost Body will attempt to collide with everything. EntityID bodyEID = MAX_EID; + // The force applied onto the Ghost Body. SHVec3 force = SHVec3::Zero; + + // The position where the force is applied offset from the local centroid. SHVec3 forceOffset = SHVec3::Zero; + + // The torque to apply onto the Ghost Body. SHVec3 torque = SHVec3::Zero; - // Whether or not to clear the force after the first iteration + /* + If the force should be applied every step throughout the simulation. Defaults to false. + True : The force indicated is added to the body every step, therefore it has constant acceleration. + False: The force is applied only in the first step, therefore it has constant speed. + */ bool continuousForce = false; - bool simulateGravity = true; + + // The timestep for each step of the simulation. Defaults to 0.016s (The default Fixed DT) float timeStep = static_cast(SHPhysicsConstants::DEFAULT_FIXED_DT); + + /* + The number of steps to run the simulation for. Defaults to -1. + < 0 : Runs until the object may hit something. Hit detection is done through raycasting. + = 0 : Runs only the current step and checks if it may hit something. + > 0 : Runs for the given number of steps or until it may hit something. + */ int maxSteps = -1; }; + /** + * @brief + * Contains the output for the simulate body method. + */ + struct SimulateBodyOutput + { + public: + std::vector* positions = nullptr; + std::vector* orientations = nullptr; + }; + /*---------------------------------------------------------------------------------*/ /* Constructors & Destructor */ @@ -159,15 +173,16 @@ namespace SHADE /** * @brief - * Simulates the motion of a body until it collides with something. - * @param positions - * The output vector for the position of the body in each timestep. - * @param orientations - * The output vector for the orientations of the body in each timestep. + * Simulates a non-existent body in the physics world. + * The simulation will run based on the information passed in. + * @param ghostBody + * The definition of the body passed in. * @param simInfo - * The information for simulating the body. + * The information for how the simulation will run. + * @param output + * The transform results for position and orientations. */ - void SimulateBody(std::vector& positions, std::vector& orientations, const SimulateBodyInfo& simInfo); + void SimulateBody(SHGhostBody& ghostBody, SimulateBodyInfo& simInfo, SimulateBodyOutput& output); /*---------------------------------------------------------------------------------*/ /* System Routines */