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
   {

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;

SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp

@@ -236,6 +236,135 @@ namespace SHADE
     return IS_COLLIDING;
   }
 
+  void SHPhysicsSystem::SimulateBody(std::vector<SHVec3>& positions, std::vector<SHQuaternion>& orientations, const SimulateBodyInfo& simInfo)
+  {
+    // Check for a valid rigidbody
+    const auto* rigidBody = SHComponentManager::GetComponent_s<SHRigidBodyComponent>(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<SHColliderComponent>(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                                               */
   /*-----------------------------------------------------------------------------------*/

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<float>(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<uint16_t>(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<SHVec3>& positions, std::vector<SHQuaternion>& orientations, const SimulateBodyInfo& simInfo);
+
     /*---------------------------------------------------------------------------------*/
     /* System Routines                                                                 */
     /*---------------------------------------------------------------------------------*/