Added physics world and tested applied gravity for linear movement

Assets/Scenes/PhysicsSandbox.shade

@@ -9,10 +9,10 @@
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
     RigidBody Component:
-      Type: Static
+      Type: Dynamic
       Auto Mass: false
       Mass: 1
-      Drag: 0
+      Drag: 1
       Angular Drag: 0
       Use Gravity: true
       Gravity Scale: 1
@@ -26,3 +26,20 @@
       Freeze Rotation Z: false
       IsActive: true
   Scripts: ~
+- EID: 1
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Camera Component:
+      Position: {x: 0, y: 2, z: 5}
+      Pitch: 0
+      Yaw: 0
+      Roll: 0
+      Width: 1920
+      Height: 1080
+      Near: 0.00999999978
+      Far: 10000
+      Perspective: true
+      IsActive: true
+  Scripts: ~

SHADE_Engine/src/ECS_Base/Managers/SHEntityManager.cpp

@@ -162,7 +162,7 @@ namespace SHADE
 
 			//SHSceneNode* parentNode = entityVec[eIndex]->GetSceneNode()->GetParent();
 
-			//SHSceneGraph::RemoveChild(parentNode,entityVec[eIndex].get());
+			//SHSceneGraph::removeChild(parentNode,entityVec[eIndex].get());
 
 			//TODO remove from parent and recursively delete child.
 

SHADE_Engine/src/Editor/EditorWindow/Inspector/SHEditorComponentView.hpp

@@ -252,12 +252,13 @@ namespace SHADE
       if(rbType == SHRigidBodyComponent::Type::DYNAMIC) //Dynamic only fields
       {
         SHEditorWidgets::CheckBox("Use Gravity", [component]{return component->IsGravityEnabled();}, [component](bool const& value){component->SetIsGravityEnabled(value);}, "Gravity");
-        //SHEditorWidgets::DragFloat("Mass", [component] {return component->GetMass(); }, [component](float const& value) {component->SetMass(value); }, "Mass");
+        SHEditorWidgets::DragFloat("Gravity Scale", [component] { return component->GetGravityScale(); }, [component](float const& value) { component->SetGravityScale(value); }, "Gravity Scale", 0.1f, 0.0f);
+        SHEditorWidgets::DragFloat("Mass", [component] {return component->GetMass(); }, [component](float const& value) {component->SetMass(value); }, "Mass");
       }
       if (rbType == SHRigidBodyComponent::Type::DYNAMIC || rbType == SHRigidBodyComponent::Type::KINEMATIC) //Dynamic or Kinematic only fields
       {
-        SHEditorWidgets::DragFloat("Drag", [component] {return component->GetDrag(); }, [component](float const& value) {component->SetDrag(value); }, "Drag");
+        SHEditorWidgets::DragFloat("Drag", [component] {return component->GetDrag(); }, [component](float const& value) {component->SetDrag(value); }, "Drag", 0.1f, 0.0f);
-        SHEditorWidgets::DragFloat("Angular Drag", [component] {return component->GetAngularDrag(); }, [component](float const& value) {component->SetAngularDrag(value); }, "Angular Drag");
+        SHEditorWidgets::DragFloat("Angular Drag", [component] {return component->GetAngularDrag(); }, [component](float const& value) {component->SetAngularDrag(value); }, "Angular Drag", 0.1f, 0.0f);
 
         SHEditorWidgets::CheckBox("Interpolate", [component] {return component->IsInterpolating(); }, [component](bool const& value) {component->SetInterpolate(value); }, "Interpolate");
 
@@ -284,8 +285,7 @@ namespace SHADE
       //Debug Info (Read-Only)
       if(ImGui::CollapsingHeader("Debug Information", ImGuiTreeNodeFlags_DefaultOpen))//Dynamic or Kinematic only fields
       {
-        SHEditorWidgets::DragFloat("Mass", [component] { return component->GetMass(); }, [](float value){}, "Mass", 0.1f, 0.0f, std::numeric_limits<float>::infinity(), "%.3f", ImGuiSliderFlags_ReadOnly);
+        SHEditorWidgets::DragVec3("Position", { "X", "Y", "Z" }, [component] {return component->GetPosition(); }, [](SHVec3 const& value) {}, false, "Position", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
-        //SHEditorWidgets::DragVec3("Position", { "X", "Y", "Z" }, [component] {return component->GetPosition(); }, [](SHVec3 const& value) {}, false, "Position", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
         //SHEditorWidgets::DragVec3("Rotation", { "X", "Y", "Z" }, [component] {return component->GetRotation(); }, [](SHVec3 const& value) {}, false, "Rotation", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
         if (rbType == SHRigidBodyComponent::Type::DYNAMIC || rbType == SHRigidBodyComponent::Type::KINEMATIC) //Dynamic or Kinematic only fields
         {

SHADE_Engine/src/Math/Transform/SHTransformSystem.cpp

@@ -246,8 +246,6 @@ namespace SHADE
     tf.world.position = SHVec3::Transform(tf.local.position, localToWorld);
     tf.world.scale    = tf.local.scale * (parent ? parent->GetLocalScale() : SHVec3::One);
 
-    
-
     if (convertRotation)
     {
       tf.worldRotation  = tf.localRotation + (parent ? parent->GetLocalRotation() : SHVec3::Zero);

SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp

@@ -0,0 +1,100 @@
+/****************************************************************************************
+ * \file      SHMotionState.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Motion State.
+ *
+ * \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 <SHpch.h>
+
+// Primary Header
+#include "SHMotionState.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHMotionState::SHMotionState() noexcept
+  : hasMoved { false }
+  {}
+
+  SHMotionState::SHMotionState(const SHMotionState& rhs) noexcept
+  : hasMoved      { rhs.hasMoved }
+  , position      { rhs.position }
+  , prevPosition  { rhs.prevPosition }
+  {}
+
+  SHMotionState::SHMotionState(SHMotionState&& rhs) noexcept
+  : hasMoved      { rhs.hasMoved }
+  , position      { rhs.position }
+  , prevPosition  { rhs.prevPosition }
+  {}
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHMotionState& SHMotionState::operator=(const SHMotionState& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    hasMoved      = rhs.hasMoved;
+    position      = rhs.position;
+    prevPosition  = rhs.prevPosition;
+
+    return *this;
+  }
+
+  SHMotionState& SHMotionState::operator=(SHMotionState&& rhs) noexcept
+  {
+    hasMoved      = rhs.hasMoved;
+    position      = rhs.position;
+    prevPosition  = rhs.prevPosition;
+
+    return *this;
+  }
+
+  SHMotionState::operator bool() const noexcept
+  {
+    return hasMoved;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definition                                                 */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHMotionState::ForcePosition(const SHVec3& newPosition) noexcept
+  {
+    hasMoved      = false;
+
+    prevPosition  = newPosition;
+    position      = newPosition;
+  }
+
+  void SHMotionState::IntegratePosition(const SHVec3& velocity, float dt) noexcept
+  {
+    // Velocities are 0 when objects are static or sleeping. We do not want to integrate them here.
+    // This call should never reach here.
+
+    hasMoved = true;
+
+    prevPosition = position;
+    position += velocity * dt;
+  }
+
+  SHVec3 SHMotionState::InterpolatePositions(float factor) const noexcept
+  {
+    return SHVec3::ClampedLerp(prevPosition, position, factor);
+  }
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHMotionState.h

@@ -0,0 +1,93 @@
+/****************************************************************************************
+ * \file      SHMotionState.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Motion State.
+ *
+ * \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 "Math/Vector/SHVec3.h"
+
+namespace SHADE
+{
+  /*-------------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                    */
+  /*-------------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates the motion state of a rigid body in physics.
+  */
+  struct SH_API SHMotionState
+  {
+  public:
+    /*-----------------------------------------------------------------------------------*/
+    /* Data Members                                                                      */
+    /*-----------------------------------------------------------------------------------*/
+
+    bool    hasMoved;
+
+    SHVec3  position;
+    SHVec3  prevPosition;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                         */
+    /*-----------------------------------------------------------------------------------*/
+
+    SHMotionState ()                          noexcept;
+    SHMotionState (const SHMotionState& rhs)  noexcept;
+    SHMotionState (SHMotionState&& rhs)       noexcept;
+
+    ~SHMotionState() = default;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                                */
+    /*-----------------------------------------------------------------------------------*/
+
+    SHMotionState&  operator=     (const SHMotionState& rhs)  noexcept;
+    SHMotionState&  operator=     (SHMotionState&& rhs)       noexcept;
+
+                    operator bool ()                    const noexcept;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Function Members                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Forcefully sets the position. Meant to be used when transform overrides the rigid body
+     * positions.
+     * @param newPosition
+     * The new position to set.
+    */
+    void ForcePosition          (const SHVec3& newPosition)         noexcept;
+
+    /**
+     * @brief
+     * Integrates the positions using velocity with respect to time.
+     * @param velocity
+     * The velocity to integrate.
+     * @param dt
+     * The delta time to integrate with respect to.
+    */
+    void IntegratePosition      (const SHVec3& velocity, float dt)  noexcept;
+
+    /**
+     * @brief
+     * Interpolates the position between the previous and the last using a given factor.
+     * @param factor
+     * The factor to interpolate by. Should be between 0 & 1.
+     * @returns
+     * The interpolated position meant for rendering.
+    */
+    SHVec3 InterpolatePositions (float factor)                const noexcept;
+
+  };
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp

@@ -0,0 +1,122 @@
+/****************************************************************************************
+ * \file      SHPhysicsWorld.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Physics World.
+ *
+ * \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 <SHpch.h>
+
+// Primary Header
+#include "SHPhysicsWorld.h"
+
+// Project Headers
+
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHPhysicsWorld::SHPhysicsWorld(const WorldSettings& worldSettings) noexcept
+  : settings    { worldSettings }
+  {
+    rigidBodies.clear();
+    SHLOG_INFO_D("Creating Physics World")
+  }
+
+  SHPhysicsWorld::~SHPhysicsWorld() noexcept
+  {
+    rigidBodies.clear();
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHPhysicsWorld::AddRigidBody(SHRigidBody* rigidBody) noexcept
+  {
+    const bool INSERTED = rigidBodies.emplace(rigidBody->entityID, rigidBody).second;
+    if (!INSERTED)
+    {
+      SHLOG_WARNING_D("Attempting to add duplicate rigid body {} to the Physics World!", rigidBody->entityID)
+    }
+  }
+
+  void SHPhysicsWorld::RemoveRigidBody(SHRigidBody* rigidBody) noexcept
+  {
+    rigidBodies.erase(rigidBody->entityID);
+  }
+
+  void SHPhysicsWorld::Step(float dt)
+  {
+    for (auto* rigidBody : rigidBodies | std::views::values)
+      integrateForces(*rigidBody, dt);
+
+    for (auto* rigidBody : rigidBodies | std::views::values)
+      integrateVelocities(*rigidBody, dt);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHPhysicsWorld::integrateForces(SHRigidBody& rigidBody, float dt) const noexcept
+  {
+    if (rigidBody.bodyType != SHRigidBody::Type::DYNAMIC)
+      return;
+
+    // Integrate forces and gravity into linear velocity
+    const SHVec3 LINEAR_ACCELERATION        = rigidBody.accumulatedForce * rigidBody.invMass;
+    const SHVec3 GRAVITATIONAL_ACCELERATION = rigidBody.IsGravityEnabled() ? settings.gravity * rigidBody.gravityScale : SHVec3::Zero;
+
+    rigidBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * dt;
+
+    // Apply drag (exponentially applied)
+    rigidBody.linearVelocity *= 1.0f / (1.0f + dt * rigidBody.linearDrag);
+  }
+
+  void SHPhysicsWorld::integrateVelocities(SHRigidBody& rigidBody, float dt) const noexcept
+  {
+    static const auto ENFORCE_CONSTRAINED_VELOCITIES = [](SHRigidBody& rigidBody)
+    {
+      // Enforce linear constraints
+      rigidBody.linearVelocity = SHVec3
+      {
+        rigidBody.GetFreezePositionX() ? 0.0f : rigidBody.linearVelocity.x
+      , rigidBody.GetFreezePositionY() ? 0.0f : rigidBody.linearVelocity.y
+      , rigidBody.GetFreezePositionZ() ? 0.0f : rigidBody.linearVelocity.z
+      };
+
+      // TODO: Enforce angular constraints
+    };
+
+    // Always reset movement flag
+    rigidBody.motionState.hasMoved = false;
+
+    // Set all velocities of static bodies to 0
+    if (rigidBody.bodyType == SHRigidBody::Type::STATIC)
+    {
+      rigidBody.linearVelocity = SHVec3::Zero;
+    }
+    else // Dynamic & Kinematic bodies
+    {
+      // Both dynamic and kinematic can sleep when their velocities are under the thresholds.
+      if (!rigidBody.IsSleeping())
+      {
+        ENFORCE_CONSTRAINED_VELOCITIES(rigidBody);
+
+        rigidBody.motionState.IntegratePosition(rigidBody.linearVelocity, dt);
+        // TODO: Integrate orientations
+      }
+    }
+
+    // Clear all forces
+    // We clear forces for static bodies as well for redundancy
+    rigidBody.ClearForces();
+  }
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h

@@ -10,7 +10,10 @@
 
 #pragma once
 
+#include <unordered_map>
+
 // Project Headers
+#include "SHRigidBody.h"
 #include "Math/SHMath.h"
 #include "SH_API.h"
 
@@ -20,7 +23,7 @@ namespace SHADE
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  struct SH_API SHPhysicsWorldState
+  class SH_API SHPhysicsWorld
   {
   public:
 
@@ -41,22 +44,55 @@ namespace SHADE
       bool      sleepingEnabled             = true;
     };
 
-    /*---------------------------------------------------------------------------------*/
-    /* Data Members                                                                    */
-    /*---------------------------------------------------------------------------------*/
-
-    WorldSettings       settings;
-
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHPhysicsWorldState() noexcept;
+    SHPhysicsWorld  (const WorldSettings& worldSettings = WorldSettings{})  noexcept;
+    ~SHPhysicsWorld ()                                                      noexcept;
+    
+    SHPhysicsWorld  (const SHPhysicsWorld&) = delete;
+    SHPhysicsWorld  (SHPhysicsWorld&&)      = delete;
+
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHPhysicsWorld& operator=(const SHPhysicsWorld&)  = delete;
+    SHPhysicsWorld& operator=(SHPhysicsWorld&&)       = delete;
 
     /*---------------------------------------------------------------------------------*/
     /* Function Members                                                                */
     /*---------------------------------------------------------------------------------*/
 
+    void AddRigidBody     (SHRigidBody* rigidBody) noexcept;
+    void RemoveRigidBody  (SHRigidBody* rigidBody) noexcept;
+
+    void Step             (float dt);
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    using RigidBodies = std::unordered_map<EntityID, SHRigidBody*>;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    WorldSettings settings;
+
+    RigidBodies   rigidBodies;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Function Members                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void integrateForces      (SHRigidBody& rigidBody, float dt) const noexcept;
+    void integrateVelocities  (SHRigidBody& rigidBody, float dt) const noexcept;
+
   };
 
 

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp

@@ -26,7 +26,7 @@ namespace SHADE
   : entityID      { eid }
   , bodyType      { type }
   , invMass       { type == Type::DYNAMIC ? 1.0f : 0.0f }
-  , linearDrag    { type != Type::STATIC ? 0.01f : 0.0f }
+  , linearDrag    { 0.01f }
   , gravityScale  { 1.0f }
   , flags         { 0U }
   {
@@ -45,6 +45,7 @@ namespace SHADE
   , linearDrag    { rhs.linearDrag }
   , gravityScale  { rhs.gravityScale }
   , flags         { rhs.flags }
+  , motionState   { rhs.motionState }
   {
     // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
   }
@@ -56,6 +57,7 @@ namespace SHADE
   , linearDrag    { rhs.linearDrag }
   , gravityScale  { rhs.gravityScale }
   , flags         { rhs.flags }
+  , motionState   { std::move(rhs.motionState) }
   {
     // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
   }
@@ -70,28 +72,36 @@ namespace SHADE
     if (this == &rhs)
       return *this;
 
-    entityID     = rhs.entityID;
+    entityID      = rhs.entityID;
-    bodyType     = rhs.bodyType;
+    bodyType      = rhs.bodyType;
-    invMass      = rhs.invMass;
+    invMass       = rhs.invMass;
-    linearDrag   = rhs.linearDrag;
+    linearDrag    = rhs.linearDrag;
-    gravityScale = rhs.gravityScale;
+    gravityScale  = rhs.gravityScale;
-    flags        = rhs.flags;
+    flags         = rhs.flags;
+    motionState   = rhs.motionState;
 
     // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
+    accumulatedForce  = SHVec3::Zero;
+    linearVelocity    = SHVec3::Zero;
+    angularVelocity   = SHVec3::Zero;
 
     return *this;
   }
 
   SHRigidBody& SHRigidBody::operator=(SHRigidBody&& rhs) noexcept
   {
-    entityID     = rhs.entityID;
+    entityID      = rhs.entityID;
-    bodyType     = rhs.bodyType;
+    bodyType      = rhs.bodyType;
-    invMass      = rhs.invMass;
+    invMass       = rhs.invMass;
-    linearDrag   = rhs.linearDrag;
+    linearDrag    = rhs.linearDrag;
-    gravityScale = rhs.gravityScale;
+    gravityScale  = rhs.gravityScale;
-    flags        = rhs.flags;
+    flags         = rhs.flags;
+    motionState   = std::move(rhs.motionState);
 
     // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
+    accumulatedForce  = SHVec3::Zero;
+    linearVelocity    = SHVec3::Zero;
+    angularVelocity   = SHVec3::Zero;
 
     return *this;
   }
@@ -105,118 +115,124 @@ namespace SHADE
     return bodyType;
   }
 
-  float              SHRigidBody::GetMass() const noexcept
+  float SHRigidBody::GetMass() const noexcept
   {
     return 1.0f/ invMass;
   }
 
-  float              SHRigidBody::GetLinearDrag() const noexcept
+  float SHRigidBody::GetLinearDrag() const noexcept
   {
     return linearDrag;
   }
 
-  float              SHRigidBody::GetGravityScale() const noexcept
+  float SHRigidBody::GetGravityScale() const noexcept
   {
     return gravityScale;
   }
 
-  const SHVec3&      SHRigidBody::GetAccumulatedForce() const noexcept
+  const SHVec3& SHRigidBody::GetAccumulatedForce() const noexcept
   {
     return accumulatedForce;
   }
 
-  const SHVec3&      SHRigidBody::GetLinearVelocity() const noexcept
+  const SHVec3& SHRigidBody::GetLinearVelocity() const noexcept
   {
     return linearVelocity;
   }
 
-  const SHVec3&      SHRigidBody::GetAngularVelocity() const noexcept
+  const SHVec3& SHRigidBody::GetAngularVelocity() const noexcept
   {
     return angularVelocity;
   }
 
   // Flags
 
-  bool               SHRigidBody::IsActive() const noexcept
+  bool SHRigidBody::IsActive() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 0;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::IsSleeping() const noexcept
+  bool SHRigidBody::IsSleeping() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 1;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::IsSleepingEnabled() const noexcept
+  bool SHRigidBody::IsSleepingEnabled() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 2;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::IsGravityEnabled() const noexcept
+  bool SHRigidBody::IsGravityEnabled() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 3;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::IsAutoMassEnabled() const noexcept
+  bool SHRigidBody::IsAutoMassEnabled() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 4;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::GetFreezePositionX() const noexcept
+  bool SHRigidBody::GetFreezePositionX() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 10;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::GetFreezePositionY() const noexcept
+  bool SHRigidBody::GetFreezePositionY() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 11;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::GetFreezePositionZ() const noexcept
+  bool SHRigidBody::GetFreezePositionZ() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 12;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::GetFreezeRotationX() const noexcept
+  bool SHRigidBody::GetFreezeRotationX() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 13;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::GetFreezeRotationY() const noexcept
+  bool SHRigidBody::GetFreezeRotationY() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 14;
     return flags & (1U << FLAG_POS);
   }
 
-  bool               SHRigidBody::GetFreezeRotationZ() const noexcept
+  bool SHRigidBody::GetFreezeRotationZ() const noexcept
   {
     static constexpr unsigned int FLAG_POS = 15;
     return flags & (1U << FLAG_POS);
   }
 
+  SHMotionState& SHRigidBody::GetMotionState() noexcept
+  {
+    return motionState;
+  }
+
+
   /*-----------------------------------------------------------------------------------*/
   /* Setter Functions Definitions                                                      */
   /*-----------------------------------------------------------------------------------*/
 
-  void               SHRigidBody::SetType(Type newType) noexcept
+  void SHRigidBody::SetType(Type newType) noexcept
   {
     if (newType == bodyType)
       return;
 
-    bodyType  = newType;
+    bodyType    = newType;
-    invMass   = newType == Type::DYNAMIC ? 1.0f : 0.0f;
+    invMass     = newType == Type::DYNAMIC ? 1.0f : 0.0f;
   }
 
-  void               SHRigidBody::SetMass(float newMass) noexcept
+  void SHRigidBody::SetMass(float newMass) noexcept
   {
     if (bodyType != Type::DYNAMIC)
     {
@@ -235,7 +251,7 @@ namespace SHADE
     // TODO: Recompute inertia tensor
   }
 
-  void               SHRigidBody::SetLinearDrag(float newLinearDrag) noexcept
+  void SHRigidBody::SetLinearDrag(float newLinearDrag) noexcept
   {
     if (bodyType == Type::STATIC)
     {
@@ -252,17 +268,17 @@ namespace SHADE
     linearDrag = newLinearDrag;
   }
 
-  void               SHRigidBody::SetGravityScale(float newGravityScale) noexcept
+  void SHRigidBody::SetGravityScale(float newGravityScale) noexcept
   {
     gravityScale = newGravityScale;
   }
 
-  void               SHRigidBody::SetLinearVelocity(const SHVec3& newLinearVelocity) noexcept
+  void SHRigidBody::SetLinearVelocity(const SHVec3& newLinearVelocity) noexcept
   {
     linearVelocity = newLinearVelocity;
   }
 
-  void               SHRigidBody::SetIsActive(bool isActive) noexcept
+  void SHRigidBody::SetIsActive(bool isActive) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 0;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -270,7 +286,7 @@ namespace SHADE
     isActive ? flags |= VALUE : flags &= ~VALUE;
   }
 
-  void               SHRigidBody::SetIsSleeping(bool isSleeping) noexcept
+  void SHRigidBody::SetIsSleeping(bool isSleeping) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 1;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -278,7 +294,7 @@ namespace SHADE
     isSleeping ? flags |= VALUE : flags &= ~VALUE;
   }
 
-  void               SHRigidBody::SetSleepingEnabled(bool enableSleeping) noexcept
+  void SHRigidBody::SetSleepingEnabled(bool enableSleeping) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 2;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -295,7 +311,7 @@ namespace SHADE
     }
   }
 
-  void               SHRigidBody::SetGravityEnabled(bool enableGravity) noexcept
+  void SHRigidBody::SetGravityEnabled(bool enableGravity) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 3;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -303,7 +319,7 @@ namespace SHADE
     enableGravity ? flags |= VALUE : flags &= ~VALUE;
   }
 
-  void               SHRigidBody::SetAutoMassEnabled(bool enableAutoMass) noexcept
+  void SHRigidBody::SetAutoMassEnabled(bool enableAutoMass) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 4;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -321,7 +337,7 @@ namespace SHADE
     }
   }
 
-  void               SHRigidBody::SetFreezePositionX(bool freezePositionX) noexcept
+  void SHRigidBody::SetFreezePositionX(bool freezePositionX) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 10;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -338,7 +354,7 @@ namespace SHADE
     }
   }
 
-  void               SHRigidBody::SetFreezePositionY(bool freezePositionY) noexcept
+  void SHRigidBody::SetFreezePositionY(bool freezePositionY) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 11;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -355,7 +371,7 @@ namespace SHADE
     }
   }
 
-  void               SHRigidBody::SetFreezePositionZ(bool freezePositionZ) noexcept
+  void SHRigidBody::SetFreezePositionZ(bool freezePositionZ) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 12;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -372,7 +388,7 @@ namespace SHADE
     }
   }
 
-  void               SHRigidBody::SetFreezeRotationX(bool freezeRotationX) noexcept
+  void SHRigidBody::SetFreezeRotationX(bool freezeRotationX) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 13;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -389,7 +405,7 @@ namespace SHADE
     }
   }
 
-  void               SHRigidBody::SetFreezeRotationY(bool freezeRotationY) noexcept
+  void SHRigidBody::SetFreezeRotationY(bool freezeRotationY) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 14;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -406,7 +422,7 @@ namespace SHADE
     }
   }
 
-  void               SHRigidBody::SetFreezeRotationZ(bool freezeRotationZ) noexcept
+  void SHRigidBody::SetFreezeRotationZ(bool freezeRotationZ) noexcept
   {
     static constexpr unsigned int FLAG_POS  = 15;
     static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
@@ -424,10 +440,10 @@ namespace SHADE
   }
 
   /*-----------------------------------------------------------------------------------*/
-  /* Member Function Definitions                                                       */
+  /* Public Member Function Definition                                                 */
   /*-----------------------------------------------------------------------------------*/
 
-  void               SHRigidBody::AddForce(const SHVec3& force, const SHVec3& pos) noexcept
+  void SHRigidBody::AddForce(const SHVec3& force, const SHVec3& pos) noexcept
   {
     if (bodyType != Type::DYNAMIC)
       return;
@@ -436,7 +452,7 @@ namespace SHADE
     // Compute torque when force is offset
   }
 
-  void               SHRigidBody::AddImpulse(const SHVec3& impulse, const SHVec3& pos) noexcept
+  void SHRigidBody::AddImpulse(const SHVec3& impulse, const SHVec3& pos) noexcept
   {
     if (bodyType != Type::DYNAMIC)
     return;
@@ -444,7 +460,7 @@ namespace SHADE
     linearVelocity += impulse * invMass; 
   }
 
-  void               SHRigidBody::ClearForces() noexcept
+  void SHRigidBody::ClearForces() noexcept
   {
     accumulatedForce = SHVec3::Zero;
   }

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h

@@ -13,7 +13,7 @@
 // Project Headers
 #include "ECS_Base/SHECSMacros.h"
 #include "Math/Vector/SHVec3.h"
-#include "SH_API.h"
+#include "SHMotionState.h"
 
 namespace SHADE
 {
@@ -27,6 +27,13 @@ namespace SHADE
   */
   class SH_API SHRigidBody
   {
+  private:
+    /*-----------------------------------------------------------------------------------*/
+    /* Friends                                                                           */
+    /*-----------------------------------------------------------------------------------*/
+
+    friend class SHPhysicsWorld;
+
   public:
     /*-----------------------------------------------------------------------------------*/
     /* Type Definitions                                                                  */
@@ -83,13 +90,16 @@ namespace SHADE
     [[nodiscard]] bool                GetFreezeRotationY  ()  const noexcept;
     [[nodiscard]] bool                GetFreezeRotationZ  ()  const noexcept;
 
+    [[nodiscard]] SHMotionState&      GetMotionState      ()  noexcept;
+
     /*-----------------------------------------------------------------------------------*/
     /* Setter Functions                                                                  */
     /*-----------------------------------------------------------------------------------*/
 
     /**
      * @brief
-     * Changing the type from non-Dynamic to Dynamic will set the default mass.
+     * Changing the type from non-Dynamic to Dynamic will set the default
+     * mass and drag values.
     */
     void SetType              (Type newType)                    noexcept;
 
@@ -111,7 +121,6 @@ namespace SHADE
 
 
     void  SetGravityScale     (float newGravityScale)           noexcept;
-
     void  SetLinearVelocity   (const SHVec3& newLinearVelocity) noexcept;
 
     // Flags
@@ -164,22 +173,24 @@ namespace SHADE
     /*-----------------------------------------------------------------------------------*/
 
     // The entityID here is only meant for linking with the actual component in the engine.
-    EntityID  entityID;
+    EntityID      entityID;
 
-    Type      bodyType;
+    Type          bodyType;
 
-    float     invMass;
+    float         invMass;
-    float     linearDrag;
+    float         linearDrag;
 
-    float     gravityScale;
+    float         gravityScale;
       
-    SHVec3    accumulatedForce;
+    SHVec3        accumulatedForce;
 
-    SHVec3    linearVelocity;
+    SHVec3        linearVelocity;
-    SHVec3    angularVelocity;
+    SHVec3        angularVelocity;
 
     // aZ aY aX pZ pY pX 0 0 0 0 0 autoMass enableGravity enableSleeping sleeping active
-    uint16_t  flags;
+    uint16_t      flags;
+
+    SHMotionState motionState;
   };
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Interface/PhysicsObject/SHPhysicsObject.cpp

@@ -44,9 +44,8 @@ namespace SHADE
     delete rigidBody;
   }
 
-
   /*-----------------------------------------------------------------------------------*/
-  /* Operator Overload Definitions                                                      */
+  /* Operator Overload Definitions                                                     */
   /*-----------------------------------------------------------------------------------*/
 
   SHPhysicsObject& SHPhysicsObject::operator=(const SHPhysicsObject& rhs) noexcept
@@ -71,4 +70,14 @@ namespace SHADE
 
     return *this;
   }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHPhysicsObject::IsEmpty() const noexcept
+  {
+    return rigidBody == nullptr;
+  }
+
 } // namespace SHADE

SHADE_Engine/src/Physics/Interface/PhysicsObject/SHPhysicsObject.h

@@ -48,5 +48,11 @@ namespace SHADE
 
     SHPhysicsObject& operator=(const SHPhysicsObject& rhs)  noexcept;
     SHPhysicsObject& operator=(SHPhysicsObject&& rhs)       noexcept;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Member Functions                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    bool IsEmpty() const noexcept;
   };
 } // namespace SHADE

SHADE_Engine/src/Physics/Interface/PhysicsObject/SHPhysicsObjectManager.cpp

@@ -14,6 +14,7 @@
 #include "SHPhysicsObjectManager.h"
 
 // Project Headers
+#include "Math/Transform/SHTransformComponent.h"
 #include "Physics/Interface/SHColliderComponent.h"
 #include "Physics/Interface/SHCollisionShape.h"
 #include "Physics/Interface/RigidBodyComponent/SHRigidBodyComponent.h"
@@ -21,16 +22,25 @@
 
 namespace SHADE
 {
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHPhysicsObjectManager::~SHPhysicsObjectManager() noexcept
+  {
+    RemoveAllObjects();
+  }
+
   /*-----------------------------------------------------------------------------------*/
   /* Getter Function Definitions                                                       */
   /*-----------------------------------------------------------------------------------*/
 
-  SHPhysicsObjectManager::EntityObjectMap&  SHPhysicsObjectManager::GetPhysicsObjects() noexcept
+  SHPhysicsObjectManager::EntityObjectMap& SHPhysicsObjectManager::GetPhysicsObjects() noexcept
   {
     return physicsObjects;
   }
 
-  const SHPhysicsObject*                    SHPhysicsObjectManager::GetPhysicsObject(EntityID entityID) noexcept
+  const SHPhysicsObject* SHPhysicsObjectManager::GetPhysicsObject(EntityID entityID) noexcept
   {
     const auto PHYSICS_OBJECT_ITERATOR = physicsObjects.find(entityID);
     if (PHYSICS_OBJECT_ITERATOR == physicsObjects.end())
@@ -68,6 +78,17 @@ namespace SHADE
     , static_cast<SHRigidBody::Type>(rigidBodyComponent->GetType())
     };
 
+    SHMotionState& motionState = physicsObject->rigidBody->GetMotionState();
+
+    if (const auto* TRANSFORM_COMPONENT = SHComponentManager::GetComponent<SHTransformComponent>(entityID); TRANSFORM_COMPONENT)
+    {
+      motionState.ForcePosition(TRANSFORM_COMPONENT->GetWorldPosition());
+    }
+    else
+    {
+      motionState.ForcePosition(SHVec3::Zero);
+    }
+
     // Link with the component
     rigidBodyComponent->SetRigidBody(physicsObject->rigidBody);
 
@@ -85,7 +106,7 @@ namespace SHADE
       physicsObject->rigidBody = nullptr;
 
       // Destroy empty physics objects
-      if (physicsObject->rigidBody == nullptr)
+      if (physicsObject->IsEmpty())
         destroyPhysicsObject(entityID);
     }
 
@@ -101,7 +122,15 @@ namespace SHADE
 
   void SHPhysicsObjectManager::RemoveCollider(EntityID entityID) noexcept
   {
-    // Unlink with the component
+    const auto PHYSICS_OBJECT_ITERATOR = physicsObjects.find(entityID);
+    if (PHYSICS_OBJECT_ITERATOR != physicsObjects.end())
+    {
+      SHPhysicsObject* physicsObject = &PHYSICS_OBJECT_ITERATOR->second;
+
+      // Destroy empty physics objects
+      if (physicsObject->IsEmpty())
+        destroyPhysicsObject(entityID);
+    }
 
     // TODO: Broadcast event
   }
@@ -120,6 +149,11 @@ namespace SHADE
     // TODO: Broadcast event
   }
 
+  void SHPhysicsObjectManager::RemoveAllObjects() noexcept
+  {
+    physicsObjects.clear();
+  }
+  
   /*-----------------------------------------------------------------------------------*/
   /* Private Member Function Definitions                                               */
   /*-----------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/Interface/PhysicsObject/SHPhysicsObjectManager.h

@@ -40,7 +40,8 @@ namespace SHADE
     /* Constructors & Destructor                                                         */
     /*-----------------------------------------------------------------------------------*/
 
-    SHPhysicsObjectManager() noexcept = default;
+    SHPhysicsObjectManager  () noexcept = default;
+    ~SHPhysicsObjectManager () noexcept;
 
     /*-----------------------------------------------------------------------------------*/
     /* Getter Functions                                                                  */
@@ -107,6 +108,13 @@ namespace SHADE
     */
     void RemoveCollisionShape (EntityID entityID, uint8_t shapeID)  noexcept;
 
+    /**
+     * @brief
+     * Removes all physics object in the manager. This is only meant to be called when
+     * the world is being destroyed or the scene is being changed.
+    */
+    void RemoveAllObjects     ()                                    noexcept;
+
   private:
     /*-----------------------------------------------------------------------------------*/
     /* Data Members                                                                      */

SHADE_Engine/src/Physics/Interface/RigidBodyComponent/SHRigidBodyComponent.cpp

@@ -182,6 +182,15 @@ namespace SHADE
     return SHVec3::Zero;
   }
 
+  SHVec3 SHRigidBodyComponent::GetPosition() const noexcept
+  {
+    if (rigidBody)
+      return rigidBody->GetMotionState().position;
+
+    return SHVec3::Zero;
+  }
+
+
   /*-----------------------------------------------------------------------------------*/
   /* Setter Functions Definitions                                                      */
   /*-----------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/Interface/RigidBodyComponent/SHRigidBodyComponent.h

@@ -97,6 +97,8 @@ namespace SHADE
     [[nodiscard]] SHVec3              GetLinearVelocity   ()  const noexcept;
     [[nodiscard]] SHVec3              GetAngularVelocity  ()  const noexcept;
 
+    [[nodiscard]] SHVec3              GetPosition         ()  const noexcept;
+
     /*---------------------------------------------------------------------------------*/
     /* Setter Functions                                                                */
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/SHPhysicsWorld.cpp

@@ -1,27 +0,0 @@
-/****************************************************************************************
- * \file      SHPhysicsWorld.cpp
- * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Implementation for a Physics World.
- *
- * \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 <SHpch.h>
-
-// Primary Header
-#include "SHPhysicsWorld.h"
-
-namespace SHADE
-{
-  /*-----------------------------------------------------------------------------------*/
-  /* Constructors & Destructor Definitions                                             */
-  /*-----------------------------------------------------------------------------------*/
-
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Public Function Members Definitions                                               */
-  /*-----------------------------------------------------------------------------------*/
-
-} // namespace SHADE

SHADE_Engine/src/Physics/System/Routines/SHPhysicsPostUpdateRoutine.cpp

@@ -15,6 +15,7 @@
 
 // Project Headers
 #include "ECS_Base/Managers/SHSystemManager.h"
+#include "Math/Transform/SHTransformComponent.h"
 #include "Scripting/SHScriptEngine.h"
 
 
@@ -39,24 +40,40 @@ namespace SHADE
 
     if (scriptingSystem == nullptr)
     {
-        SHLOGV_ERROR("Unable to invoke collision and trigger script events due to missing SHScriptEngine!");
+      SHLOGV_ERROR("Unable to invoke collision and trigger script events due to missing SHScriptEngine!");
     }
 
-    // Interpolate transforms for rendering
+    const float FACTOR = static_cast<float>(physicsSystem->interpolationFactor);
+
+    // Interpolate transforms for rendering.
+    // Only rigid bodies can move due to physics, so we run through the rigid body component dense set.
+    const auto& RIGIDBODY_DENSE = SHComponentManager::GetDense<SHRigidBodyComponent>();
+    for (auto& rigidBodyComponent : RIGIDBODY_DENSE)
+    {
+      auto* transformComponent = SHComponentManager::GetComponent_s<SHTransformComponent>(rigidBodyComponent.GetEID());
+      if (!transformComponent)
+        continue;
+
+      if (!rigidBodyComponent.rigidBody)
+        continue;
+
+      if (const SHMotionState& MOTION_STATE = rigidBodyComponent.rigidBody->GetMotionState(); MOTION_STATE)
+      {
+        SHVec3 renderPosition = rigidBodyComponent.IsInterpolating()
+                              ? MOTION_STATE.InterpolatePositions(FACTOR)
+                              : MOTION_STATE.position;
+
+        /*
+         * TODO: Test if the scene graph transforms abides by setting world position. Collisions will ignore the scene graph hierarchy.
+         */
+
+        transformComponent->SetWorldPosition(renderPosition);
+        // TODO: SetOrientation
+      }
+    }
 
     // Collision & Trigger messages
     if (scriptingSystem != nullptr)
       scriptingSystem->ExecuteCollisionFunctions();
-
   }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Private Function Member Definitions                                               */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHPhysicsSystem::PhysicsPostUpdate::syncTransforms(SHRigidBodyComponent* rbComponent) const noexcept
-  {
-    
-  }
-
 }

SHADE_Engine/src/Physics/System/Routines/SHPhysicsPreUpdateRoutine.cpp

@@ -36,27 +36,27 @@ namespace SHADE
     auto* physicsSystem = reinterpret_cast<SHPhysicsSystem*>(GetSystem());
 
     // Get all physics objects & sync transforms
-    for (auto& physicsObject : physicsSystem->physicsObjectManager.GetPhysicsObjects() | std::views::values)
+    for (auto& [entityID, physicsObject] : physicsSystem->physicsObjectManager.GetPhysicsObjects())
-      syncTransforms(&physicsObject);
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Private Function Member Definitions                                                */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHPhysicsSystem::PhysicsPreUpdate::syncTransforms(SHPhysicsObject* physicsObject) const noexcept
-  {
-    const EntityID EID = physicsObject->entityID;
-
-    // Get relevant components: Transform, Rigidbody & Collider
-    const auto* TRANSFORM_COMPONENT = SHComponentManager::GetComponent_s<SHTransformComponent>(EID);
-
-    auto* rigidBodyComponent  = SHComponentManager::GetComponent_s<SHRigidBodyComponent>(EID);
-    auto* colliderComponent   = SHComponentManager::GetComponent_s<SHColliderComponent>(EID);
-
-    if (TRANSFORM_COMPONENT && TRANSFORM_COMPONENT->HasChanged())
     {
-      // Sync the objects transforms
+      const auto* TRANSFORM_COMPONENT = SHComponentManager::GetComponent_s<SHTransformComponent>(entityID);
+
+      // We assume that all engine components and physics object components have been successfully linked
+
+      if (TRANSFORM_COMPONENT && TRANSFORM_COMPONENT->HasChanged())
+      {
+        // Sync the objects transforms
+        const SHVec3 WORLD_POS = TRANSFORM_COMPONENT->GetWorldPosition();
+
+        if (physicsObject.rigidBody)
+        {
+          SHMotionState& motionState = physicsObject.rigidBody->GetMotionState();
+
+          motionState.ForcePosition(TRANSFORM_COMPONENT->GetWorldPosition());
+          // TODO: Force Orientation
+        }
+      }
+
+        // TODO: Sync Collider Transform with World Transform
     }
   }
 }

SHADE_Engine/src/Physics/System/Routines/SHPhysicsUpdateRoutine.cpp

@@ -47,7 +47,11 @@ namespace SHADE
     int count = 0;
     while (accumulatedTime > FIXED_DT)
     {
+      if (scriptEngine)
+        scriptEngine->ExecuteFixedUpdates();
 
+      if (physicsSystem->physicsWorld)
+        physicsSystem->physicsWorld->Step(static_cast<float>(FIXED_DT));
 
       accumulatedTime -= FIXED_DT;
       ++count;

SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp

@@ -28,10 +28,11 @@ namespace SHADE
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/
 
-  SHPhysicsSystem::SHPhysicsSystem()
+  SHPhysicsSystem::SHPhysicsSystem() noexcept
   : worldUpdated        { false }
   , interpolationFactor { 0.0 }
   , fixedDT             { DEFAULT_FIXED_STEP }
+  , physicsWorld        { nullptr }
   {
     // Add more events here to register them
 
@@ -45,6 +46,11 @@ namespace SHADE
   #endif
   }
 
+  SHPhysicsSystem::~SHPhysicsSystem() noexcept
+  {
+    delete physicsWorld;
+  }
+
   /*-----------------------------------------------------------------------------------*/
   /* Getter Function Definitions                                                       */
   /*-----------------------------------------------------------------------------------*/
@@ -141,24 +147,66 @@ namespace SHADE
   SHEventHandle SHPhysicsSystem::onSceneInit(SHEventPtr onSceneInitEvent)
   {
     /*
-     * TODO:
      * If a world already exists, destroy it
-     * Recreate the world
+     * Recreate the world.
      *
      * If there is an editor and editor mode is already playing, link all entities with the world immediately.
      */
 
+    // Destroy an existing world if there is one.
+    //! This should almost never happen.
+    if (physicsWorld)
+    {
+      // Remove all references of physics objects from the world
+      for (const auto& PHYSICS_OBJECT : physicsObjectManager.GetPhysicsObjects() | std::views::values)
+      {
+        if (PHYSICS_OBJECT.rigidBody)
+          physicsWorld->RemoveRigidBody(PHYSICS_OBJECT.rigidBody);
+      }
+
+      delete physicsWorld;
+      physicsWorld = nullptr;
+    }
+
+    // Create the physics world
+    physicsWorld = new SHPhysicsWorld;
+
+  #ifdef SHEDITOR
+
+    // Link all entities with the world if editor is already playing.
+    // This is for handling scene changes while the editor is active.
+    
+    const auto* EDITOR = SHSystemManager::GetSystem<SHEditor>();
+    if (EDITOR && EDITOR->editorState == SHEditor::State::PLAY)
+    {
+      for (const auto& PHYSICS_OBJECT : physicsObjectManager.GetPhysicsObjects() | std::views::values)
+      {
+        if (PHYSICS_OBJECT.rigidBody)
+          physicsWorld->AddRigidBody(PHYSICS_OBJECT.rigidBody);
+      }
+    }
+
+  #endif
+
     return onSceneInitEvent.get()->handle;
   }
 
   SHEventHandle SHPhysicsSystem::onSceneExit(SHEventPtr onSceneExitEvent)
   {
     /*
-     * TODO:
      * Destroy the physics world.
      * Destroy all physics objects.
      */
 
+    for (const auto& PHYSICS_OBJECT : physicsObjectManager.GetPhysicsObjects() | std::views::values)
+    {
+      if (PHYSICS_OBJECT.rigidBody)
+        physicsWorld->RemoveRigidBody(PHYSICS_OBJECT.rigidBody);
+    }
+
+    delete physicsWorld;
+    physicsWorld = nullptr;
+
     return onSceneExitEvent.get()->handle;
   }
 
@@ -181,8 +229,17 @@ namespace SHADE
 
       // Link engine components with physics object component
       if (IS_RIGID_BODY)
+      {
         physicsObjectManager.AddRigidBody(EID);
 
+        if (physicsWorld)
+        {
+          auto* rigidBody = physicsObjectManager.GetPhysicsObject(EID)->rigidBody;
+          physicsWorld->AddRigidBody(rigidBody);
+        }
+      }
+        
+
       if (IS_COLLIDER)
         physicsObjectManager.AddCollider(EID);
     }
@@ -209,7 +266,16 @@ namespace SHADE
 
       // Link engine components with physics object component
       if (IS_RIGID_BODY)
+      {
+        if (physicsWorld)
+        {
+          auto* rigidBody = physicsObjectManager.GetPhysicsObject(EID)->rigidBody;
+          physicsWorld->RemoveRigidBody(rigidBody);
+        }
+
         physicsObjectManager.RemoveRigidBody(EID);
+      }
+        
 
       if (IS_COLLIDER)
         physicsObjectManager.RemoveCollider(EID);
@@ -222,10 +288,15 @@ namespace SHADE
 
   SHEventHandle SHPhysicsSystem::onEditorPlay(SHEventPtr onEditorPlayEvent)
   {
-    /*
+    // Add all physics components to the physics world
-      * TODO:
+    for (const auto& PHYSICS_OBJECT : physicsObjectManager.GetPhysicsObjects() | std::views::values)
-      * Link all entities with the world.
+    {
-      */
+      // Add rigid body if it exists
+      if (PHYSICS_OBJECT.rigidBody)
+        physicsWorld->AddRigidBody(PHYSICS_OBJECT.rigidBody);
+
+      // TODO: Add Collider if it exists
+    }
 
     return onEditorPlayEvent.get()->handle;
   }

SHADE_Engine/src/Physics/System/SHPhysicsSystem.h

@@ -16,6 +16,7 @@
 #include "ECS_Base/System/SHFixedSystemRoutine.h"
 #include "Events/SHEvent.h"
 
+#include "Physics/Dynamics/SHPhysicsWorld.h"
 #include "Physics/Interface/PhysicsObject/SHPhysicsObjectManager.h"
 #include "Physics/Interface/RigidBodyComponent/SHRigidBodyComponent.h"
 #include "Physics/Interface/SHColliderComponent.h"
@@ -34,7 +35,8 @@ namespace SHADE
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHPhysicsSystem();
+    SHPhysicsSystem () noexcept;
+    ~SHPhysicsSystem() noexcept;
 
     /*---------------------------------------------------------------------------------*/
     /* Getter Functions                                                                */
@@ -75,9 +77,6 @@ namespace SHADE
     public:
       PhysicsPreUpdate();
       void Execute(double dt) noexcept override;
-
-    private:
-      void syncTransforms(SHPhysicsObject* physicsObject) const noexcept;
     };
 
     /**
@@ -103,9 +102,6 @@ namespace SHADE
     public:
       PhysicsPostUpdate();
       void Execute(double dt) noexcept override;
-
-    private:
-      void syncTransforms(SHRigidBodyComponent* rbComponent) const noexcept;
     };
 
   private:
@@ -136,6 +132,8 @@ namespace SHADE
 
     // Sub-systems / managers
 
+    SHPhysicsWorld*         physicsWorld;
+
     SHPhysicsObjectManager  physicsObjectManager;
 
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/System/SHPhysicsSystemInterface.h

@@ -49,8 +49,9 @@ namespace SHADE
     /* Static Usage Functions                                                          */
     /*---------------------------------------------------------------------------------*/
 
-    [[nodiscard]] static const std::vector<SHCollisionInfo>&  GetCollisionInfo() noexcept;
+    [[nodiscard]] static const std::vector<SHCollisionInfo>&  GetCollisionInfo    ()  noexcept;
-    [[nodiscard]] static const std::vector<SHCollisionInfo>&  GetTriggerInfo  () noexcept;
+    [[nodiscard]] static const std::vector<SHCollisionInfo>&  GetTriggerInfo      ()  noexcept;
-    [[nodiscard]] static double                               GetFixedDT      () noexcept;
+    [[nodiscard]] static double                               GetFixedDT          ()  noexcept;
+    [[nodiscard]] static int                                  GetFixedUpdateRate  ()  noexcept;
   };
 }