From c36345c60cc1c1c0d0db1141134e9b56d5f691a5 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Tue, 7 Mar 2023 18:48:06 +0800 Subject: [PATCH] Added a simulate body method Raycast effectiveness is untested, but simulation logic matches standard 3D rigid body simulation methods --- .../src/Camera/SHCameraArmComponent.h | 4 +- SHADE_Engine/src/Physics/SHPhysicsConstants.h | 4 +- .../src/Physics/System/SHPhysicsSystem.cpp | 129 ++++++++++++++++++ .../src/Physics/System/SHPhysicsSystem.h | 36 ++++- 4 files changed, 165 insertions(+), 8 deletions(-) diff --git a/SHADE_Engine/src/Camera/SHCameraArmComponent.h b/SHADE_Engine/src/Camera/SHCameraArmComponent.h index 9d8ec853..76f27360 100644 --- a/SHADE_Engine/src/Camera/SHCameraArmComponent.h +++ b/SHADE_Engine/src/Camera/SHCameraArmComponent.h @@ -9,9 +9,7 @@ namespace SHADE { - - class SHBox; - class SHRay; + struct SHRay; class SH_API SHCameraArmComponent final: public SHComponent { diff --git a/SHADE_Engine/src/Physics/SHPhysicsConstants.h b/SHADE_Engine/src/Physics/SHPhysicsConstants.h index fe3b9ee5..072c7755 100644 --- a/SHADE_Engine/src/Physics/SHPhysicsConstants.h +++ b/SHADE_Engine/src/Physics/SHPhysicsConstants.h @@ -10,14 +10,12 @@ #pragma once -// Project Headers -#include "Math/Vector/SHVec3.h" - namespace SHADE { struct SHPhysicsConstants { static constexpr double DEFAULT_FIXED_DT = 1.0 / 60.0; + static constexpr double DEFAULT_FIXED_UPDATE_RATE = 60.0; static constexpr int DEFAULT_VELOCITY_ITERATIONS = 10; static constexpr int DEFAULT_POSITION_ITERATIONS = 5; diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp index e51fdab9..787a9f78 100644 --- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp +++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp @@ -236,6 +236,135 @@ namespace SHADE return IS_COLLIDING; } + void SHPhysicsSystem::SimulateBody(std::vector& positions, std::vector& orientations, const SimulateBodyInfo& simInfo) + { + // 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; + } + + // 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; + + 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]; + + + // 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; + if (const auto* colliders = SHComponentManager::GetComponent_s(simInfo.bodyEID); colliders) + { + const auto& shapes = colliders->GetCollisionShapes(); + for (auto& shape : shapes) + raycastLayers |= shape->GetCollisionTag().GetMask(); + } + else + { + SHLOG_WARNING("Unable to simulate body {} without a collider (it will simulate forever!)", simInfo.bodyEID) + return; + } + + // Raycast direction is always in the direction of the linear velocity + // Raycast distance is the length of the velocity + SHRaycaster::RaycastInfo raycastInfo; + raycastInfo.continuous = false; + raycastInfo.layers = raycastLayers; + + bool terminate = true; + do + { + raycastInfo.distance = linearVelocity.Length(); + raycastInfo.ray.position = bodyPosition; + raycastInfo.ray.direction = SHVec3::Normalise(linearVelocity); + + terminate = !Raycast(raycastInfo).empty(); + if (terminate) + break; + + // Compute world space data + const SHMatrix R = SHMatrix::Rotate(bodyOrientation); + 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 *= RT; + const SHVec3 DIAGONALS { localInertiaTensor.m[0][0], localInertiaTensor.m[1][1], localInertiaTensor.m[2][2] }; + + worldInvInertia = R * DIAGONALS; + + // Compute world centroid + worldCentroid = (R * localCentroid) + bodyPosition; + + // Apply forces + accumulatedForce += appliedForce; + angularVelocity += worldInvInertia * SHVec3::Cross(bodyPosition + simInfo.forceOffset, simInfo.force); + accumulatedTorque += appliedTorque; + + // Integrate Velocities + // Integrate forces and gravity into linear velocity + const SHVec3 LINEAR_ACCELERATION = accumulatedForce * invMass; + const SHVec3 GRAVITATIONAL_ACCELERATION = rigidBody->IsGravityEnabled() ? worldState.settings.gravity * rigidBody->GetGravityScale() : SHVec3::Zero; + + linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * simInfo.timeStep * LINEAR_LOCK; + angularVelocity += worldInvInertia * (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); + + // Integrate Positions & Orientations + const SHQuaternion QV = SHQuaternion{ angularVelocity.x * simInfo.timeStep, angularVelocity.y * simInfo.timeStep, angularVelocity.z * simInfo.timeStep, 0.0f } * 0.5f; + + bodyPosition += linearVelocity * simInfo.timeStep; + bodyOrientation += bodyOrientation * QV; + bodyOrientation = SHQuaternion::Normalise(bodyOrientation); + + // Clear forces after the first frame + if (!simInfo.continuousForce) + { + accumulatedForce = SHVec3::Zero; + accumulatedTorque = SHVec3::Zero; + appliedForce = SHVec3::Zero; + appliedTorque = SHVec3::Zero; + } + + positions.emplace_back(bodyPosition); + + } while (true); + } + /*-----------------------------------------------------------------------------------*/ /* Private Function Member Definitions */ /*-----------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h index fe1e9c5c..d95203b9 100644 --- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h +++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.h @@ -44,14 +44,34 @@ namespace SHADE friend class SHRaycaster; public: - + /*---------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*---------------------------------------------------------------------------------*/ + + /** + * @brief + * Used to simulate the motion of a rigid body until it hits something. + */ + struct SimulateBodyInfo + { + EntityID bodyEID = MAX_EID; + + SHVec3 force = SHVec3::Zero; + SHVec3 forceOffset = SHVec3::Zero; + SHVec3 torque = SHVec3::Zero; + + // Whether or not to clear the force after the first iteration + bool continuousForce = false; + float timeStep = static_cast(SHPhysicsConstants::DEFAULT_FIXED_DT); + }; + /*---------------------------------------------------------------------------------*/ /* Constructors & Destructor */ /*---------------------------------------------------------------------------------*/ SHPhysicsSystem () noexcept; - ~SHPhysicsSystem() noexcept; + ~SHPhysicsSystem() noexcept override; /*---------------------------------------------------------------------------------*/ /* Getter Functions */ @@ -116,6 +136,18 @@ namespace SHADE */ [[nodiscard]] bool TestAABBOverlap(const SHAABB& aabb, uint16_t layers = static_cast(SHCollisionTag::Layer::ALL)); + /** + * @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. + * @param simInfo + * The information for simulating the body. + */ + void SimulateBody(std::vector& positions, std::vector& orientations, const SimulateBodyInfo& simInfo); + /*---------------------------------------------------------------------------------*/ /* System Routines */ /*---------------------------------------------------------------------------------*/