Fixed a potential infinite loop in AABB Tree

This behaviour should've thrown an exception wtf???

Assets/Application.SHConfig

@@ -1,4 +1,4 @@
 Start in Fullscreen: false
-Starting Scene ID: 97158628
+Starting Scene ID: 97402985
 Window Size: {x: 1920, y: 1080}
 Window Title: SHADE Engine

Assets/CollisionTags.SHConfig

@@ -1,16 +1,16 @@
-0 1
+0 1 3
-1 2
+1 2 3
-2 3
+2 3 65535
-3 4
+3 4 65535
-4 5
+4 5 65535
-5 6
+5 6 65535
-6 7
+6 7 65535
-7 8
+7 8 65535
-8 9
+8 9 65535
-9 10
+9 10 65535
-10 11
+10 11 65535
-11 12
+11 12 65535
-12 13
+12 13 65535
-13 14
+13 14 65535
-14 15
+14 15 65535
-15 16
+15 16 65535

Assets/Scenes/PhysicsSandbox.shade

@@ -0,0 +1,358 @@
+- EID: 0
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 2.72256827, y: 0.501797795, z: -0.0273017883}
+      Rotate: {x: 0, y: 0, z: 0.436332315}
+      Scale: {x: 4.61070776, y: 0.99999392, z: 0.999996722}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 1
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Camera Component:
+      Position: {x: -0.5, y: 10, z: 3}
+      Pitch: 0
+      Yaw: 0
+      Roll: 0
+      Width: 1920
+      Height: 1080
+      Near: 0.00999999978
+      Far: 10000
+      Perspective: true
+      IsActive: true
+  Scripts: ~
+- EID: 3
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -1.45715916, y: 7.37748241, z: 0.227711335}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 10
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: true
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Sphere
+          Radius: 1
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+        - Is Trigger: true
+          Collision Tag: 1
+          Type: Sphere
+          Radius: 0.5
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0.75, y: 0.5, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts:
+    - Type: PhysicsTestObj
+      Enabled: true
+      forceAmount: 50
+      torqueAmount: 500
+- EID: 2
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 4.09544182, z: 0}
+      Rotate: {x: -0, y: 0, z: -0.436332315}
+      Scale: {x: 4.61071014, y: 0.999995887, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: true
+      Freeze Position Y: true
+      Freeze Position Z: true
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 2
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 4
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: -1.74209237, z: 0}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 10, y: 0.5, z: 10}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 5
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -4.80025721, y: 3, z: 0}
+      Rotate: {x: -0, y: 0, z: 1.57079601}
+      Scale: {x: 9.99975109, y: 0.499992192, z: 10}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 6
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 4.80000019, y: 3, z: 0}
+      Rotate: {x: -0, y: 0, z: 1.57079601}
+      Scale: {x: 9.99975109, y: 0.499992192, z: 10}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 7
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0.400000006, y: 13.5, z: 0}
+      Rotate: {x: -0, y: 0, z: 0}
+      Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 0.5
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: false
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 8
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 10, z: 0}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~

Assets/Scenes/PhysicsSandbox.shade.shmeta

@@ -0,0 +1,3 @@
+Name: PhysicsSandbox
+ID: 97402985
+Type: 5

Assets/Scripts/Gameplay/AIBehaviour/Implemented/LeafNodes/LeafSearch.cs

@@ -159,7 +159,7 @@ public partial class LeafSearch : BehaviourTreeNode
       //Since transform position is often the raccoon's base and the ray needs to hit somewhere higher to be more reliable
       Vector3 rayDestination = plrT.GlobalPosition + plrT.GlobalScale * playerCollider.PositionOffset;
       Ray sightRay = new Ray(eyePosition, rayDestination - eyePosition);
-      RaycastHit sightRayHit = Physics.Raycast(sightRay);
+      RaycastHit sightRayHit = Physics.Raycast(sightRay, false)[0];
       //As of November 2022, RaycastHit contains only the FIRST object hit by
       //the ray in the Other GameObject data member
       //Diren may likely add ALL objects hit by the ray over December

Assets/Scripts/Gameplay/Player/SC_PickAndThrow.cs

@@ -1,6 +1,7 @@
 using SHADE;
 using SHADE_Scripting;
 using System;
+using System.Collections.Generic;
 using static PlayerController;
 using static Item;
 
@@ -203,9 +204,13 @@ public class PickAndThrow : Script
       Vector3 playerRayPos = pc.tranform.GlobalPosition;
       playerRayPos.y += 0.05f;
       dirNor.Normalise();
-      RaycastHit ray1 = Physics.Raycast(new Ray(playerRayPos, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(22.5f))), rayDistance);
+      List<RaycastHit> rayList1 = Physics.Raycast(new Ray(playerRayPos, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(22.5f))), rayDistance, false);
-      RaycastHit ray2 = Physics.Raycast(new Ray(playerRayPos, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(-22.5f))), rayDistance);
+      List<RaycastHit> rayList2 = Physics.Raycast(new Ray(playerRayPos, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(-22.5f))), rayDistance, false);
-      RaycastHit ray3 = Physics.Raycast(new Ray(playerRayPos, dirNor), rayDistance * 0.75f);
+      List<RaycastHit> rayList3 = Physics.Raycast(new Ray(playerRayPos, dirNor), rayDistance * 0.75f, false);
+
+      RaycastHit ray1 = rayList1[0];
+      RaycastHit ray2 = rayList2[0];
+      RaycastHit ray3 = rayList3[0];
       inRange = CheckForItem(ray1) || CheckForItem(ray2) || CheckForItem(ray3);
     }
   }

Assets/Scripts/Tests/PhysicsTestObj.cs

@@ -0,0 +1,119 @@
+using SHADE;
+using System;
+using System.Collections.Generic;
+using static Item;
+
+
+public class PhysicsTestObj : Script
+{
+    public RigidBody body { get; set; }
+    public Collider collider { get; set; }
+
+    // Movement input booleans
+    public enum Direction
+    {
+        UP,
+        DOWN,
+        FORWARD,
+        BACK,
+        LEFT,
+        RIGHT
+    };
+
+    internal bool[] move   = new bool[6];
+    internal bool[] rotate = new bool[6];
+
+    internal Vector3[] moveVec = new Vector3[6]
+    {
+        Vector3.Up, 
+        Vector3.Down, 
+        Vector3.Back, 
+        Vector3.Forward, 
+        Vector3.Left, 
+        Vector3.Right
+    };
+
+    internal Vector3[] rotateVec = new Vector3[6]
+    {
+        Vector3.Right,
+        Vector3.Left,
+        Vector3.Forward,
+        Vector3.Down,
+        Vector3.Up,
+        Vector3.Down
+    };
+
+    internal Input.KeyCode[] moveInputKeys = new Input.KeyCode[6]
+    {
+        Input.KeyCode.Space,
+        Input.KeyCode.LeftControl,
+        Input.KeyCode.W,
+        Input.KeyCode.S,
+        Input.KeyCode.A,
+        Input.KeyCode.D
+    };
+
+    internal Input.KeyCode[] rotateInputKeys = new Input.KeyCode[6]
+    {
+        Input.KeyCode.I,
+        Input.KeyCode.K,
+        Input.KeyCode.U,
+        Input.KeyCode.O,
+        Input.KeyCode.J,
+        Input.KeyCode.L
+    };
+
+    public float forceAmount = 50.0f;
+    public float torqueAmount = 500.0f;
+
+    protected override void awake()
+    {
+        body = GetComponent<RigidBody>();
+        collider = GetComponent<Collider>();
+
+        for (int i = 0; i < 6; ++i)
+        {
+            move[i] = false;
+            rotate[i] = false;
+        }
+    }
+
+    protected override void update()
+    {
+        Ray colliderRay = new Ray();
+        colliderRay.Direction = Vector3.Right;
+        Physics.ColliderRaycast(collider.Owner, colliderRay, false);
+
+        for (int i = 0; i < 6; ++i)
+        {
+            if (Input.GetKeyDown(moveInputKeys[i]))
+                move[i] = true;
+
+            if (Input.GetKeyDown(rotateInputKeys[i]))
+                rotate[i] = true;
+        }
+    }
+
+    protected override void fixedUpdate()
+    {
+        for (int i = 0; i < 6; ++i)
+        {
+            bool shouldMove = move[i];
+            bool shouldRotate = rotate[i];
+
+            if (shouldMove)
+            {
+                //Vector3 offset = new Vector3(0.25f, 0.0f, 0.0f);
+                //rb.AddForceAtLocalPos(moveVec[i] * forceAmount, offset);
+                body.AddForce(moveVec[i] * forceAmount);
+                move[i] = false;
+            }
+
+            if (shouldRotate)
+            {
+                body.AddTorque(rotateVec[i] * torqueAmount);
+                rotate[i] = false;
+            }
+        }
+    }
+}

Assets/Scripts/Tests/PhysicsTestObj.cs.shmeta

@@ -0,0 +1,3 @@
+Name: PhysicsTestObj
+ID: 159293012
+Type: 9

SHADE_Application/src/Application/SBApplication.cpp

@@ -77,9 +77,6 @@ namespace Sandbox
     SHSystemManager::CreateSystem<SHScriptEngine>();
     SHSystemManager::CreateSystem<SHTransformSystem>();
     SHSystemManager::CreateSystem<SHPhysicsSystem>();
-#ifndef _PUBLISH
-    SHSystemManager::CreateSystem<SHPhysicsDebugDrawSystem>();
-#endif
 
     SHSystemManager::CreateSystem<SHAudioSystem>();
     SHSystemManager::CreateSystem<SHCameraSystem>();
@@ -90,7 +87,6 @@ namespace Sandbox
 
     SHSystemManager::CreateSystem<SHGraphicsSystem>();
     SHGraphicsSystem* graphicsSystem = static_cast<SHGraphicsSystem*>(SHSystemManager::GetSystem<SHGraphicsSystem>());
-    SHPhysicsSystem* physicsSystem = SHSystemManager::GetSystem<SHPhysicsSystem>();
 
     // Link up SHDebugDraw
     SHSystemManager::CreateSystem<SHDebugDrawSystem>();
@@ -105,6 +101,8 @@ namespace Sandbox
       editor->SetSDLWindow(sdlWindow);
       editor->SetSHWindow(&window);
     }
+
+    SHSystemManager::CreateSystem<SHPhysicsDebugDrawSystem>();
 #endif
     
     // Create Routines
@@ -116,11 +114,11 @@ namespace Sandbox
     SHSystemManager::RegisterRoutine<SHTransformSystem, SHTransformSystem::TransformPostLogicUpdate>();
 
     SHSystemManager::RegisterRoutine<SHPhysicsSystem, SHPhysicsSystem::PhysicsPreUpdate>();
-    SHSystemManager::RegisterRoutine<SHPhysicsSystem, SHPhysicsSystem::PhysicsFixedUpdate>();
+    SHSystemManager::RegisterRoutine<SHPhysicsSystem, SHPhysicsSystem::PhysicsUpdate>();
     SHSystemManager::RegisterRoutine<SHPhysicsSystem, SHPhysicsSystem::PhysicsPostUpdate>();
 
-#ifndef _PUBLISH
+#ifdef SHEDITOR
-    SHSystemManager::RegisterRoutine<SHPhysicsDebugDrawSystem, SHPhysicsDebugDrawSystem::PhysicsDebugDrawRoutine>();
+    SHSystemManager::RegisterRoutine<SHPhysicsDebugDrawSystem, SHPhysicsDebugDrawSystem::PhysicsDebugDraw>();
 #endif
 
     SHSystemManager::RegisterRoutine<SHTransformSystem, SHTransformSystem::TransformPostPhysicsUpdate>();
@@ -193,32 +191,12 @@ namespace Sandbox
 #endif
       SHSceneManager::SceneUpdate(0.016f);
 #ifdef SHEDITOR
+
       SHSystemManager::RunRoutines(editor->editorState != SHEditor::State::PLAY, SHFrameRateController::GetRawDeltaTime());
       editor->PollPicking();
 #else
       SHSystemManager::RunRoutines(false, SHFrameRateController::GetRawDeltaTime());
 #endif
-      // TODO: Move into an Editor menu
-      static bool drawContacts = false;
-      if (SHInputManager::GetKeyDown(SHInputManager::SH_KEYCODE::F9))
-      {
-        drawContacts = !drawContacts;
-        SHSystemManager::GetSystem<SHPhysicsDebugDrawSystem>()->SetDebugDrawFlag(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACT_POINTS, drawContacts);
-        SHSystemManager::GetSystem<SHPhysicsDebugDrawSystem>()->SetDebugDrawFlag(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACT_NORMALS, drawContacts);
-      }
-      static bool drawColliders = false;
-      if (SHInputManager::GetKeyDown(SHInputManager::SH_KEYCODE::F10))
-      {
-        drawColliders = !drawColliders;
-        SHSystemManager::GetSystem<SHPhysicsDebugDrawSystem>()->SetDebugDrawFlag(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDER, drawColliders);
-      }
-      static bool drawRays = false;
-      if (SHInputManager::GetKeyDown(SHInputManager::SH_KEYCODE::F11))
-      {
-        drawRays = !drawRays;
-        SHSystemManager::GetSystem<SHPhysicsDebugDrawSystem>()->SetDebugDrawFlag(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS, drawRays);
-      }
-      
     }
     // Finish all graphics jobs first
     graphicsSystem->AwaitGraphicsExecution();

SHADE_Application/src/Scenes/SBMainScene.cpp

@@ -44,23 +44,6 @@ namespace Sandbox
   {
     sceneName = SHSerialization::DeserializeSceneFromFile(sceneAssetID);
 
-    auto* physicsSystem = SHSystemManager::GetSystem<SHPhysicsSystem>();
-    if (!physicsSystem)
-    {
-      SHLOGV_CRITICAL("Failed to get the physics system for building the scene!")
-      return;
-    }
-
-  #ifdef SHEDITOR
-
-    physicsSystem->ForceBuild(SHSceneManager::GetCurrentSceneGraph());
-
-  #else
-
-    physicsSystem->BuildScene(SHSceneManager::GetCurrentSceneGraph());
-
-  #endif
-
     /*-----------------------------------------------------------------------*/
     /* TESTING CODE                                                          */
     /*-----------------------------------------------------------------------*/

SHADE_Application/src/Scenes/SBTestScene.cpp

@@ -92,7 +92,7 @@ namespace Sandbox
 
     floorRigidBody.SetType(SHRigidBodyComponent::Type::STATIC);
 
-    floorCollider.AddBoundingBox();
+    //floorCollider.AddBoundingBox();
 
     // Create blank entity with a script
     //testObj = SHADE::SHEntityManager::CreateEntity<SHRenderable, SHTransformComponent>();
@@ -113,9 +113,9 @@ namespace Sandbox
     racoonTransform.SetWorldScale({ 2.0f, 2.0f, 2.0f });
     racoonTransform.SetWorldPosition({ -3.0f, -2.0f, -5.0f });
 
-    racoonCollider.AddBoundingBox();
+    //racoonCollider.AddBoundingBox();
-    racoonCollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f,0.5f,0.0f));
+    //racoonCollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f,0.5f,0.0f));
-    racoonCollider.GetCollisionShape(0).SetBoundingBox(SHVec3(0.5f, 0.5f, 0.5f));
+    //racoonCollider.GetCollisionShape(0).SetBoundingBox(SHVec3(0.5f, 0.5f, 0.5f));
 
     auto racoonItemLocation = SHEntityManager::CreateEntity<SHTransformComponent>();
     auto& racoonItemLocationTransform = *SHComponentManager::GetComponent_s<SHTransformComponent>(racoonItemLocation);
@@ -138,15 +138,15 @@ namespace Sandbox
     itemTransform.SetWorldScale({ 2.0f, 2.0f, 2.0f });
     itemTransform.SetWorldPosition({ 0.0f, -2.0f, -5.0f });
 
-    itemCollider.AddBoundingBox();
+    //itemCollider.AddBoundingBox();
-    itemCollider.AddBoundingBox(SHVec3(2.0f,2.0f,2.0f));
+    //itemCollider.AddBoundingBox(SHVec3(2.0f,2.0f,2.0f));
-    itemCollider.GetCollisionShape(1).SetIsTrigger(true);
+    //itemCollider.GetCollisionShape(1).SetIsTrigger(true);
 
-    itemCollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f, 0.5f, 0.0f));
+    //itemCollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f, 0.5f, 0.0f));
-    itemCollider.GetCollisionShape(0).SetBoundingBox(SHVec3(0.5f, 0.5f, 0.5f));
+    //itemCollider.GetCollisionShape(0).SetBoundingBox(SHVec3(0.5f, 0.5f, 0.5f));
 
-    itemCollider.GetCollisionShape(1).SetPositionOffset(SHVec3(0.0f, 0.5f, 0.0f));
+    //itemCollider.GetCollisionShape(1).SetPositionOffset(SHVec3(0.0f, 0.5f, 0.0f));
-    itemCollider.GetCollisionShape(1).SetBoundingBox(SHVec3(1.0f, 1.0f, 1.0f));
+    //itemCollider.GetCollisionShape(1).SetBoundingBox(SHVec3(1.0f, 1.0f, 1.0f));
 
     itemRigidBody.SetInterpolate(false);
     itemRigidBody.SetFreezeRotationX(true);
@@ -167,9 +167,9 @@ namespace Sandbox
     AITransform.SetWorldScale({ 2.0f, 2.0f, 2.0f });
     AITransform.SetWorldPosition({ -8.0f, -2.0f, 2.5f });
 
-    AICollider.AddBoundingBox();
+    //AICollider.AddBoundingBox();
-    AICollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f, 0.5f, 0.0f));
+    //AICollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f, 0.5f, 0.0f));
-    AICollider.GetCollisionShape(0).SetBoundingBox(SHVec3(0.5f, 0.5f, 0.5f));
+    //AICollider.GetCollisionShape(0).SetBoundingBox(SHVec3(0.5f, 0.5f, 0.5f));
 
     AIRigidBody.SetInterpolate(false);
     AIRigidBody.SetFreezeRotationX(true);

SHADE_Engine/src/Camera/SHCameraArmComponent.h

@@ -10,7 +10,7 @@
 namespace SHADE
 {
 
-  class SHBox;
+  class SHAABB;
   class SHRay;
 
   class SH_API SHCameraArmComponent final: public SHComponent

SHADE_Engine/src/Camera/SHCameraSystem.cpp

@@ -10,7 +10,7 @@
 #include "Scene/SHSceneManager.h"
 #include "ECS_Base/Managers/SHSystemManager.h"
 #include "Editor/SHEditor.h"
-#include "Math/Geometry/SHBox.h"
+#include "Math/Geometry/SHAABB.h"
 #include "Math/SHRay.h"
 #include "Physics/System/SHPhysicsSystem.h"
 
@@ -186,20 +186,20 @@ namespace SHADE
       
      //SHLOG_INFO("Ray position: {},{},{}  direction:{},{},{}",pivot.ray.position.x, pivot.ray.position.y, pivot.ray.position.z,pivot.ray.direction.x, pivot.ray.direction.y, pivot.ray.direction.z)
 
-      auto result = physicsSystem->Raycast(pivot.ray );
+      //auto result = physicsSystem->Raycast(pivot.ray );
-      if (result && result.distance < pivot.GetArmLength())
+      //if (result && result.distance < pivot.GetArmLength())
-      {
+      //{
-        
+      //  
-        SHVec3 newOffset = SHVec3{ 0.0f,0.0f, result.distance * 0.8f };
+      //  SHVec3 newOffset = SHVec3{ 0.0f,0.0f, result.distance * 0.8f };
-        newOffset = SHVec3::RotateX(newOffset, -(SHMath::DegreesToRadians(pivot.GetPitch())));
+      //  newOffset = SHVec3::RotateX(newOffset, -(SHMath::DegreesToRadians(pivot.GetPitch())));
-        newOffset = SHVec3::RotateY(newOffset, (SHMath::DegreesToRadians(pivot.GetYaw())));
+      //  newOffset = SHVec3::RotateY(newOffset, (SHMath::DegreesToRadians(pivot.GetYaw())));
-        pivot.offset = newOffset;
+      //  pivot.offset = newOffset;
-        //SHLOG_INFO("CAMERA COLLISION HIT, {}", result.distance);
+      //  //SHLOG_INFO("CAMERA COLLISION HIT, {}", result.distance);
-      }
+      //}
-      else
+      //else
-      {
+      //{
-        //SHLOG_INFO("CAMERA COLLISION CANT HIT CAMERA");
+      //  //SHLOG_INFO("CAMERA COLLISION CANT HIT CAMERA");
-      }
+      //}
 
       
       

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/ColliderTagPanel/SHColliderTagPanel.cpp

@@ -1,7 +1,7 @@
 #include "SHpch.h"
 #include "SHColliderTagPanel.h"
 #include "ECS_Base/Managers/SHSystemManager.h"
-#include "Physics/Collision/SHCollisionTagMatrix.h"
+#include "Physics/Collision/CollisionTags/SHCollisionTagMatrix.h"
 #include "Editor/SHEditorWidgets.hpp"
 
 namespace SHADE
@@ -15,7 +15,7 @@ namespace SHADE
       ImGui::TableNextRow();
       ImGui::PushID("CollisionTagNames");
 
-      for (int i = SHCollisionTag::NUM_LAYERS; i >= 0; --i)
+      for (int i = SHCollisionTag::NUM_LAYERS; i >= 1; --i)
       {
         ImGui::TableNextColumn();
         if(i == SHCollisionTag::NUM_LAYERS) continue;
@@ -29,7 +29,7 @@ namespace SHADE
         ImGui::PopID();
       }
       ImGui::PopID();
-      for (int i = 0; i < SHCollisionTag::NUM_LAYERS; ++i)
+      for (int i = 0; i < SHCollisionTag::NUM_LAYERS - 1; ++i)
       {
         std::string tagName = SHCollisionTagMatrix::GetTagName(i);
         auto tag = SHCollisionTagMatrix::GetTag(i);
@@ -53,8 +53,8 @@ namespace SHADE
             tagName2 = std::to_string(idx);
 
           ImGui::TableNextColumn();
-          //if(i == idx)
+          if(i == idx)
-          //  continue;
+            continue;
           std::string label = std::format("##{} vs {}", tagName, tagName2);
           SHEditorWidgets::CheckBox(label, [tag, &idx]{return tag->GetLayerState(idx);}, [tag, i, idx](bool const& value){tag->SetLayerState(idx, value); SHCollisionTagMatrix::GetTag(idx)->SetLayerState(i, value);}, label.substr(2));
         }

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

@@ -18,10 +18,11 @@
 #include "Physics/Interface/SHColliderComponent.h"
 #include "Reflection/SHReflectionMetadata.h"
 #include "Resource/SHResourceManager.h"
-#include "Physics/Collision/SHCollisionTagMatrix.h"
 #include "Serialization/SHSerializationHelper.hpp"
 #include "Tools/Utilities/SHClipboardUtilities.h"
 #include "SHInspectorCommands.h"
+#include "Physics/Collision/SHCompositeCollider.h"
+#include "Physics/Collision/CollisionTags/SHCollisionTagMatrix.h"
 namespace SHADE
 {
   template<typename T>
@@ -255,31 +256,34 @@ namespace SHADE
       
       if(rbType == SHRigidBodyComponent::Type::DYNAMIC) //Dynamic only fields
       {
-        SHEditorWidgets::CheckBox("Use Gravity", [component]{return component->IsGravityEnabled();}, [component](bool const& value){component->SetGravityEnabled(value);}, "Gravity");
+        SHEditorWidgets::CheckBox("Use Gravity", [component]{return component->IsGravityEnabled();}, [component](bool const& value){component->SetIsGravityEnabled(value);}, "Whether Gravity is enabled for this body");
-        //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); }, "Per-object Gravity Scale", 0.1f, 0.0f);
+
+        SHEditorWidgets::CheckBox("Auto Mass", [component]{return component->GetAutoMass();}, [component](bool const& value){component->SetAutoMass(value);}, "If mass should be automatically computed. Setting mass will turn this off.");
+        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");
+        SHEditorWidgets::CheckBox("Interpolate", [component] {return component->IsInterpolating(); }, [component](bool const& value) {component->SetInterpolate(value); }, "If the position between frames should be interpolated.");
 
         SHEditorWidgets::BeginPanel(std::format("{} Constraints", ICON_FA_LOCK).data(), { ImGui::GetContentRegionAvail().x, ImGui::GetContentRegionAvail().y });
 
         SHEditorWidgets::TextLabel("Freeze Position");
         ImGui::PushID("FreezePos");
-        SHEditorWidgets::CheckBox("X", [component] {return component->GetFreezePositionX(); }, [component](bool const& value) {component->SetFreezePositionX(value); }, "Freeze Position - X"); ImGui::SameLine();
+        SHEditorWidgets::CheckBox("X", [component] {return component->GetFreezePositionX(); }, [component](bool const& value) {component->SetFreezePositionX(value); }, "Stops any displacement along the X-axis."); ImGui::SameLine();
-        SHEditorWidgets::CheckBox("Y", [component] {return component->GetFreezePositionY(); }, [component](bool const& value) {component->SetFreezePositionY(value); }, "Freeze Position - Y"); ImGui::SameLine();
+        SHEditorWidgets::CheckBox("Y", [component] {return component->GetFreezePositionY(); }, [component](bool const& value) {component->SetFreezePositionY(value); }, "Stops any displacement along the Y-axis."); ImGui::SameLine();
-        SHEditorWidgets::CheckBox("Z", [component] {return component->GetFreezePositionZ(); }, [component](bool const& value) {component->SetFreezePositionZ(value); }, "Freeze Position - Z");
+        SHEditorWidgets::CheckBox("Z", [component] {return component->GetFreezePositionZ(); }, [component](bool const& value) {component->SetFreezePositionZ(value); }, "Stops any displacement along the Z-axis.");
         ImGui::PopID();
 
 
         SHEditorWidgets::TextLabel("Freeze Rotation");
         ImGui::PushID("FreezeRot");
-        SHEditorWidgets::CheckBox("X", [component] {return component->GetFreezeRotationX(); }, [component](bool const& value) {component->SetFreezeRotationX(value); }, "Freeze Rotation - X"); ImGui::SameLine();
+        SHEditorWidgets::CheckBox("X", [component] {return component->GetFreezeRotationX(); }, [component](bool const& value) {component->SetFreezeRotationX(value); }, "Stops any rotation about the X-axis."); ImGui::SameLine();
-        SHEditorWidgets::CheckBox("Y", [component] {return component->GetFreezeRotationY(); }, [component](bool const& value) {component->SetFreezeRotationY(value); }, "Freeze Rotation - Y"); ImGui::SameLine();
+        SHEditorWidgets::CheckBox("Y", [component] {return component->GetFreezeRotationY(); }, [component](bool const& value) {component->SetFreezeRotationY(value); }, "Stops any rotation about the Y-axis."); ImGui::SameLine();
-        SHEditorWidgets::CheckBox("Z", [component] {return component->GetFreezeRotationZ(); }, [component](bool const& value) {component->SetFreezeRotationZ(value); }, "Freeze Rotation - Z");
+        SHEditorWidgets::CheckBox("Z", [component] {return component->GetFreezeRotationZ(); }, [component](bool const& value) {component->SetFreezeRotationZ(value); }, "Stops any rotation about the Z-axis.");
         ImGui::PopID();
 
         SHEditorWidgets::EndPanel();
@@ -288,9 +292,15 @@ 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("Rotation", { "X", "Y", "Z" }, [component] {return component->GetRotation(); }, [](SHVec3 const& value) {}, false, "Rotation", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
+        SHEditorWidgets::DragVec3("Rotation", { "X", "Y", "Z" }, [component]
+        {
+          // Convert it to degrees...
+          auto rot = component->GetRotation();
+          for (size_t i = 0; i < SHVec3::SIZE; ++i)
+            rot[i] = SHMath::RadiansToDegrees(rot[i]);
+          return rot;
+        }, [](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
         {
           SHEditorWidgets::DragVec3("Velocity", { "X", "Y", "Z" }, [component] {return component->GetLinearVelocity(); }, [](SHVec3 const& value) {}, false, "Linear Velocity", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
@@ -300,7 +310,7 @@ namespace SHADE
         {
           SHEditorWidgets::DragVec3("Force", { "X", "Y", "Z" }, [component] {return component->GetForce(); }, [](SHVec3 const& value) {}, false, "Force", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
           SHEditorWidgets::DragVec3("Torque", { "X", "Y", "Z" }, [component] {return component->GetTorque(); }, [](SHVec3 const& value) {}, false, "Torque", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
-          SHEditorWidgets::CheckBox("Is Asleep", [component] {return component->GetIsSleeping(); }, [](bool value) {}, "If the Rigid Body is asleep");
+          SHEditorWidgets::CheckBox("Is Asleep", [component] {return component->IsSleeping(); }, [](bool value) {}, "If the Rigid Body is asleep");
         } 
       }
 
@@ -332,64 +342,67 @@ namespace SHADE
     {
       DrawContextMenu(component);
 
-      auto& colliders = component->GetCollisionShapes();
+      SHEditorWidgets::CheckBox("Draw Colliders", [component] { return component->GetDebugDrawState(); }, [component](bool value) { component->SetDebugDrawState(value); });
-      int const size = static_cast<int>(colliders.size());
+
-      ImGui::BeginChild("Collision Shapes", { 0.0f,  colliders.empty() ? 1.0f : 250.0f }, true);
+      auto* collisionShapes = component->GetCollisionShapes();
+      int const size = collisionShapes ? static_cast<int>(collisionShapes->size()) : 0;
+      ImGui::BeginChild("Collision Shapes", { 0.0f,  collisionShapes->empty() ? 1.0f : 250.0f }, true);
       std::optional<int> colliderToDelete{ std::nullopt };
       for (int i{}; i < size; ++i)
       {
         ImGui::PushID(i);
-        SHCollisionShape* collider = &component->GetCollisionShape(i);
+        SHCollisionShape* shape = component->GetCollisionShape(i);
         auto cursorPos = ImGui::GetCursorPos();
 
-        
+        //collider->IsTrigger
-        if (collider->GetType() == SHCollisionShape::Type::BOX)
+        if (shape->GetType() == SHCollisionShape::Type::BOX)
         {
           SHEditorWidgets::BeginPanel(std::format("{} Box #{}", ICON_FA_CUBE, i).data(), { ImGui::GetContentRegionAvail().x, ImGui::GetContentRegionAvail().y });
           
-          const auto* BOX = reinterpret_cast<const SHBox*>(collider->GetShape());
+          auto* box = reinterpret_cast<SHBox*>(shape);
           SHEditorWidgets::DragVec3
           (
             "Half Extents", { "X", "Y", "Z" },
-            [BOX] { return BOX->GetRelativeExtents(); },
+            [box] { return box->GetRelativeExtents(); },
-            [collider](SHVec3 const& vec) { collider->SetBoundingBox(vec); });
+            [box](SHVec3 const& vec) { box->SetRelativeExtents(vec); });
         }
-        else if (collider->GetType() == SHCollisionShape::Type::SPHERE)
+        else if (shape->GetType() == SHCollisionShape::Type::SPHERE)
         {
           SHEditorWidgets::BeginPanel(std::format("{} Sphere #{}", ICON_MD_CIRCLE, i).data(), { ImGui::GetContentRegionAvail().x, ImGui::GetContentRegionAvail().y });
-          const auto* SPHERE = reinterpret_cast<const SHSphere*>(collider->GetShape());
+          auto* sphere = reinterpret_cast<SHSphere*>(shape);
           SHEditorWidgets::DragFloat
           (
             "Radius",
-            [SPHERE] { return SPHERE->GetRelativeRadius(); },
+            [sphere] { return sphere->GetRelativeRadius(); },
-            [collider](float const& value) { collider->SetBoundingSphere(value); });
+            [sphere](float const& value) { sphere->SetRelativeRadius(value); });
         }
-        else if (collider->GetType() == SHCollisionShape::Type::CAPSULE)
+        else if (shape->GetType() == SHCollisionShape::Type::CAPSULE)
         {
 
         }
 
         {
-          SHEditorWidgets::CheckBox("Is Trigger", [collider] { return collider->IsTrigger(); }, [collider](bool value) { collider->SetIsTrigger(value); });
+          SHEditorWidgets::CheckBox("Is Trigger", [shape] { return shape->IsTrigger(); }, [shape](bool value) { shape->SetIsTrigger(value); });
-          SHEditorWidgets::ComboBox("Tag", collisionTagNames, [collider]{return SHCollisionTagMatrix::GetTagIndex(collider->GetCollisionTag().GetName());}, [collider](int const& value){collider->SetCollisionTag(SHCollisionTagMatrix::GetTag(value));});
+          SHEditorWidgets::ComboBox("Tag", collisionTagNames, [shape]{return SHCollisionTagMatrix::GetTagIndex(shape->GetCollisionTag().GetName());}, [shape](int const& value){shape->SetCollisionTag(SHCollisionTagMatrix::GetTag(value));});
+
           if(ImGui::CollapsingHeader("Physics Material"))
           {
-            SHEditorWidgets::DragFloat("Friction", [collider] { return collider->GetFriction(); }, [collider](float value) { collider->SetFriction(value); }, "Friction", 0.05f, 0.0f, 1.0f);
+            SHEditorWidgets::DragFloat("Friction", [shape] { return shape->GetFriction(); }, [shape](float value) { shape->SetFriction(value); }, "Friction", 0.05f, 0.0f, 1.0f);
-            SHEditorWidgets::DragFloat("Bounciness", [collider] { return collider->GetBounciness(); }, [collider](float value) { collider->SetBounciness(value); }, "Bounciness", 0.05f, 0.0f, 1.0f);
+            SHEditorWidgets::DragFloat("Bounciness", [shape] { return shape->GetBounciness(); }, [shape](float value) { shape->SetBounciness(value); }, "Bounciness", 0.05f, 0.0f, 1.0f);
-            SHEditorWidgets::DragFloat("Mass Density", [collider] { return collider->GetDensity(); }, [collider](float value) { collider->SetDensity(value); }, "Mass Density", 0.1f, 0.0f);
+            SHEditorWidgets::DragFloat("Mass Density", [shape] { return shape->GetDensity(); }, [shape](float value) { shape->SetDensity(value); }, "Mass Density", 0.1f, 0.0f);
           }
 
           SHEditorWidgets::BeginPanel("Offsets",{ ImGui::GetContentRegionAvail().x, 30.0f });
-          SHEditorWidgets::DragVec3("Position", { "X", "Y", "Z" }, [&collider] {return collider->GetPositionOffset(); }, [&collider](SHVec3 const& vec) {collider->SetPositionOffset(vec); });
+          SHEditorWidgets::DragVec3("Position", { "X", "Y", "Z" }, [&shape] {return shape->GetPositionOffset(); }, [&shape](SHVec3 const& vec) {shape->SetPositionOffset(vec); });
           SHEditorWidgets::DragVec3("Rotation", { "X", "Y", "Z" }, 
-          [&collider]
+          [&shape]
           {
-            auto offset = collider->GetRotationOffset();
+            auto offset = shape->GetRotationOffset();
             return offset;
           }, 
-          [&collider](SHVec3 const& vec)
+          [&shape](SHVec3 const& vec)
           {
-            collider->SetRotationOffset(vec);
+            shape->SetRotationOffset(vec);
           }, true);
           SHEditorWidgets::EndPanel();
         }
@@ -404,21 +417,24 @@ namespace SHADE
       }
       if (colliderToDelete.has_value())
       {
-        component->RemoveCollider(colliderToDelete.value());
+        component->GetCollider()->RemoveCollisionShape(colliderToDelete.value());
       }
       ImGui::EndChild();
 
+      // TODO: Handle differences between composite & hull collider
       if (ImGui::BeginMenu("Add Collider"))
       {
         int newColl = -1;
 
         if (ImGui::Selectable("Box Collider"))
         {
-          newColl = component->AddBoundingBox();
+          auto* compositeCollider = dynamic_cast<SHCompositeCollider* const>(component->GetCollider());
+          compositeCollider->AddBoxCollisionShape(SHVec3::One);
         }
         if (ImGui::Selectable("Sphere Collider"))
         {
-          newColl = component->AddBoundingSphere();
+          auto* compositeCollider = dynamic_cast<SHCompositeCollider* const>(component->GetCollider());
+          compositeCollider->AddSphereCollisionShape(1.0f);
         }
 
         //No idea why this doesn't work

SHADE_Engine/src/Editor/EditorWindow/MenuBar/SHEditorMenuBar.cpp

@@ -25,6 +25,7 @@
 #include "Serialization/SHSerialization.h"
 #include "Serialization/Configurations/SHConfigurationManager.h"
 #include "Editor/EditorWindow/SHEditorWindowManager.h"
+#include "Physics/System/SHPhysicsDebugDrawSystem.h"
 
 const std::string LAYOUT_FOLDER_PATH{ std::string(ASSET_ROOT) + "/Editor/Layouts" };
 
@@ -88,6 +89,7 @@ namespace SHADE
         DrawThemeMenu();
         DrawLayoutMenu();
         DrawApplicationConfig();
+        DrawPhysicsSettings();
 
         std::string const sceneName{std::format("Current Scene: {}",SHSceneManager::GetSceneName().data())};
         auto const size = ImGui::CalcTextSize(sceneName.data());
@@ -303,4 +305,32 @@ namespace SHADE
       ImGui::EndMenu();
     }
   }
+
+  void SHEditorMenuBar::DrawPhysicsSettings() noexcept
+  {
+    if (ImGui::BeginMenu("Physics Settings"))
+    {
+      if (auto* physicsDebugDraw = SHSystemManager::GetSystem<SHPhysicsDebugDrawSystem>())
+      {
+        bool drawColliders = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS);
+        if (ImGui::Checkbox("Draw Colliders", &drawColliders))
+          physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS, drawColliders);
+
+        bool drawContactPoints = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS);
+        if (ImGui::Checkbox("Draw Contact Points", &drawContactPoints))
+          physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS, drawContactPoints);
+
+          bool drawRays = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS);
+        if (ImGui::Checkbox("Draw Rays", &drawRays))
+          physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS, drawRays);
+
+        bool drawBroadphase = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE);
+        if (ImGui::Checkbox("Draw Broadphase", &drawBroadphase))
+          physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE, drawBroadphase);
+      }
+
+      ImGui::EndMenu();
+    }
+  }
+
 }//namespace SHADE

SHADE_Engine/src/Editor/EditorWindow/MenuBar/SHEditorMenuBar.h

@@ -25,6 +25,7 @@ namespace SHADE
     void DrawThemeMenu() noexcept;
     void DrawLayoutMenu() noexcept;
     void DrawApplicationConfig() noexcept;
+    void DrawPhysicsSettings() noexcept;
 
     float menuBarHeight = 20.0f;
     std::vector<std::filesystem::path> layoutPaths;

SHADE_Engine/src/Events/SHEventDefines.h

@@ -23,4 +23,6 @@ constexpr SHEventIdentifier SH_SCENE_INIT_POST                    { 14 };
 constexpr SHEventIdentifier SH_SCENE_EXIT_PRE                     { 15 }; 
 constexpr SHEventIdentifier SH_SCENE_EXIT_POST                    { 16 }; 
 constexpr SHEventIdentifier SH_GRAPHICS_LIGHT_ENABLE_SHADOW_EVENT { 17 };
+constexpr SHEventIdentifier SH_PHYSICS_COLLIDER_DRAW_EVENT        { 18 };
+
 

SHADE_Engine/src/Math/Geometry/SHAABB.cpp

@@ -1,5 +1,5 @@
 /****************************************************************************************
- * \file      SHBox.cpp
+ * \file      SHAABB.cpp
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
  * \brief     Implementation for a 3-Dimensional Axis Aligned Bounding Box
  *
@@ -11,7 +11,7 @@
 #include <SHpch.h>
 
 // Primary Header
-#include "SHBox.h"
+#include "SHAABB.h"
 // Project Headers
 #include "Math/SHMathHelpers.h"
 #include "Math/SHRay.h"
@@ -24,75 +24,52 @@ namespace SHADE
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/
 
-  SHBox::SHBox() noexcept
+  SHAABB::SHAABB() noexcept
-  : RelativeExtents { SHVec3::One }
   {
-    type = Type::BOX;
+    Extents = SHVec3::One * 0.5f;
   }
 
-  SHBox::SHBox(const SHVec3& c, const SHVec3& hE) noexcept
+  SHAABB::SHAABB(const SHVec3& c, const SHVec3& hE) noexcept
-  : RelativeExtents { SHVec3::One }
   {
-    type = Type::BOX;
-
     Center  = c;
     Extents = hE;
   }
 
 
-  SHBox::SHBox(const SHBox& rhs) noexcept
+  SHAABB::SHAABB(const SHAABB& rhs) noexcept
   {
     if (this == &rhs)
       return;
 
-    type = Type::BOX;
-
     Center  = rhs.Center;
     Extents = rhs.Extents;
-    RelativeExtents = rhs.RelativeExtents;
   }
 
-  SHBox::SHBox(SHBox&& rhs) noexcept
+  SHAABB::SHAABB(SHAABB&& rhs) noexcept
   {
-    type = Type::BOX;
-
     Center  = rhs.Center;
     Extents = rhs.Extents;
-    RelativeExtents = rhs.RelativeExtents;
   }
 
   /*-----------------------------------------------------------------------------------*/
   /* Operator Overload Definitions                                                     */
   /*-----------------------------------------------------------------------------------*/
 
-  SHBox& SHBox::operator=(const SHBox& rhs) noexcept
+  SHAABB& SHAABB::operator=(const SHAABB& rhs) noexcept
   {
-    if (rhs.type != Type::BOX)
+    if (this != &rhs)
-    {
-      SHLOG_WARNING("Cannot assign a non-bounding box to a bounding box!")
-    }
-    else if (this != &rhs)
     {
       Center  = rhs.Center;
       Extents = rhs.Extents;
-      RelativeExtents = rhs.RelativeExtents;
     }
 
     return *this;
   }
 
-  SHBox& SHBox::operator=(SHBox&& rhs) noexcept
+  SHAABB& SHAABB::operator=(SHAABB&& rhs) noexcept
-  {
-    if (rhs.type != Type::BOX)
-    {
-      SHLOG_WARNING("Cannot assign a non-bounding box to a bounding box!")
-    }
-    else
   {
     Center  = rhs.Center;
     Extents = rhs.Extents;
-      RelativeExtents = rhs.RelativeExtents;
-    }
 
     return *this;
   }
@@ -101,27 +78,22 @@ namespace SHADE
   /* Getter Function Definitions                                                       */
   /*-----------------------------------------------------------------------------------*/
 
-  SHVec3 SHBox::GetCenter() const noexcept
+  SHVec3 SHAABB::GetCenter() const noexcept
   {
     return Center;
   }
 
-  SHVec3 SHBox::GetWorldExtents() const noexcept
+  SHVec3 SHAABB::GetExtents() const noexcept
   {
     return Extents;
   }
 
-  const SHVec3& SHBox::GetRelativeExtents() const noexcept
+  SHVec3 SHAABB::GetMin() const noexcept
-  {
-    return RelativeExtents;
-  }
-
-  SHVec3 SHBox::GetMin() const noexcept
   {
     return SHVec3{ Center.x - Extents.x, Center.y - Extents.y, Center.z - Extents.z };
   }
 
-  SHVec3 SHBox::GetMax() const noexcept
+  SHVec3 SHAABB::GetMax() const noexcept
   {
     return SHVec3{ Center.x + Extents.x, Center.y + Extents.y, Center.z + Extents.z };
   }
@@ -130,22 +102,17 @@ namespace SHADE
   /* Setter Function Definitions                                                       */
   /*-----------------------------------------------------------------------------------*/
 
-  void SHBox::SetCenter(const SHVec3& newCenter) noexcept
+  void SHAABB::SetCenter(const SHVec3& newCenter) noexcept
   {
     Center = newCenter;
   }
 
-  void SHBox::SetWorldExtents(const SHVec3& newWorldExtents) noexcept
+  void SHAABB::SetExtents(const SHVec3& newHalfExtents) noexcept
   {
-    Extents = newWorldExtents;
+    Extents = newHalfExtents;
   }
 
-  void SHBox::SetRelativeExtents(const SHVec3& newRelativeExtents) noexcept
+  void SHAABB::SetMin(const SHVec3& min) noexcept
-  {
-    RelativeExtents = newRelativeExtents;
-  }
-
-  void SHBox::SetMin(const SHVec3& min) noexcept
   {
     const SHVec3 MAX = GetMax();
 
@@ -153,7 +120,7 @@ namespace SHADE
     Extents = SHVec3::Abs((MAX - min) * 0.5f);
   }
 
-  void SHBox::SetMax(const SHVec3& max) noexcept
+  void SHAABB::SetMax(const SHVec3& max) noexcept
   {
     const SHVec3 MIN = GetMin();
 
@@ -161,13 +128,13 @@ namespace SHADE
     Extents = SHVec3::Abs((max - MIN) * 0.5f);
   }
 
-  void SHBox::SetMinMax(const SHVec3& min, const SHVec3& max) noexcept
+  void SHAABB::SetMinMax(const SHVec3& min, const SHVec3& max) noexcept
   {
     Center  = SHVec3::Lerp(min, max, 0.5f);
     Extents = SHVec3::Abs((max - min) * 0.5f);
   }
 
-  std::vector<SHVec3> SHBox::GetVertices() const noexcept
+  std::vector<SHVec3> SHAABB::GetVertices() const noexcept
   {
     std::vector<SHVec3> vertices{ 8 };
     GetCorners(vertices.data());
@@ -178,12 +145,12 @@ namespace SHADE
   /* Public Function Member Definitions                                                */
   /*-----------------------------------------------------------------------------------*/
 
-  bool SHBox::TestPoint(const SHVec3& point) const noexcept
+  bool SHAABB::TestPoint(const SHVec3& point) const noexcept
   {
     return BoundingBox::Contains(point);
   }
 
-  SHRaycastResult SHBox::Raycast(const SHRay& ray) const noexcept
+  SHRaycastResult SHAABB::Raycast(const SHRay& ray) const noexcept
   {
     SHRaycastResult result;
 
@@ -192,22 +159,24 @@ namespace SHADE
     {
       result.position = ray.position + ray.direction * result.distance;
       result.angle    = SHVec3::Angle(ray.position, result.position);
+
+      // TODO: Compute normal
     }
 
     return result;
   }
 
-  bool SHBox::Contains(const SHBox& rhs) const noexcept
+  bool SHAABB::Contains(const SHAABB& rhs) const noexcept
   {
-    return BoundingBox::Contains(rhs);
+    return BoundingBox::Contains(rhs) == CONTAINS;
   }
 
-  float SHBox::Volume() const noexcept
+  float SHAABB::Volume() const noexcept
   {
     return 8.0f * (Extents.x * Extents.y * Extents.z);
   }
 
-  float SHBox::SurfaceArea() const noexcept
+  float SHAABB::SurfaceArea() const noexcept
   {
     return 8.0f * ((Extents.x * Extents.y) 
       + (Extents.x * Extents.z) 
@@ -218,21 +187,21 @@ namespace SHADE
   /* Static Function Member Definitions                                                */
   /*-----------------------------------------------------------------------------------*/
 
-  SHBox SHBox::Combine(const SHBox& lhs, const SHBox& rhs) noexcept
+  SHAABB SHAABB::Combine(const SHAABB& lhs, const SHAABB& rhs) noexcept
   {
-    SHBox result;
+    SHAABB result;
     CreateMerged(result, lhs, rhs);
     return result;
   }
 
-  bool SHBox::Intersect(const SHBox& lhs, const SHBox& rhs) noexcept
+  bool SHAABB::Intersect(const SHAABB& lhs, const SHAABB& rhs) noexcept
   {
     return lhs.Intersects(rhs);
   }
 
-  SHBox SHBox::BuildFromBoxes(const SHBox* boxes, size_t numBoxes) noexcept
+  SHAABB SHAABB::BuildFromBoxes(const SHAABB* boxes, size_t numBoxes) noexcept
   {
-    SHBox result;
+    SHAABB result;
 
     for (size_t i = 1; i < numBoxes; ++i)
       CreateMerged(result, boxes[i - 1], boxes[i]);
@@ -240,9 +209,9 @@ namespace SHADE
     return result;
   }
 
-  SHBox SHBox::BuildFromVertices(const SHVec3* vertices, size_t numVertices, size_t stride) noexcept
+  SHAABB SHAABB::BuildFromVertices(const SHVec3* vertices, size_t numVertices, size_t stride) noexcept
   {
-    SHBox result;
+    SHAABB result;
     CreateFromPoints(result, numVertices, vertices, stride);
     return result;
   }

SHADE_Engine/src/Math/Geometry/SHAABB.h

@@ -0,0 +1,173 @@
+/****************************************************************************************
+ * \file      SHAABB.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a 3-Dimensional Axis Aligned Bounding Box
+ *
+ * \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
+
+#include <DirectXCollision.h>
+
+// Project Headers
+#include "Math/SHRay.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates a 3D Axis-Aligned Bounding Box.
+  */
+  class SH_API SHAABB : private DirectX::BoundingBox
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Static Data Members                                                             */
+    /*---------------------------------------------------------------------------------*/
+
+    static constexpr size_t NUM_VERTICES = 8;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    ~SHAABB  () noexcept = default;
+
+    SHAABB   ()                                                 noexcept;
+    SHAABB   (const SHVec3& center, const SHVec3& halfExtents)  noexcept;
+    SHAABB   (const SHAABB& rhs)                                noexcept;
+    SHAABB   (SHAABB&& rhs)                                     noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHAABB& operator=  (const SHAABB& rhs)  noexcept;
+    SHAABB& operator=  (SHAABB&& rhs)       noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] SHVec3              GetCenter     ()  const noexcept;
+    [[nodiscard]] SHVec3              GetExtents    ()  const noexcept;
+    [[nodiscard]] SHVec3              GetMin        ()  const noexcept;
+    [[nodiscard]] SHVec3              GetMax        ()  const noexcept;
+    [[nodiscard]] std::vector<SHVec3> GetVertices   ()  const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void  SetCenter   (const SHVec3& newCenter)               noexcept;
+    void  SetExtents  (const SHVec3& newHalfExtents)          noexcept;
+    void  SetMin      (const SHVec3& min)                     noexcept;
+    void  SetMax      (const SHVec3& max)                     noexcept;
+    void  SetMinMax   (const SHVec3& min, const SHVec3& max)  noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Checks if a point is inside the aabb.
+     * @param point
+     * The point to check.
+     * @return
+     * True if the point is inside the aabb.
+    */
+    [[nodiscard]] bool TestPoint (const SHVec3& point) const noexcept;
+
+    /**
+     * @brief
+     * Casts a ray against the aabb.
+     * @param ray
+     * The ray to cast.
+     * @return
+     * The result of the raycast. <br/>
+     * See the corresponding header for the contents of the raycast result object.
+     */
+    [[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept;
+
+    /**
+     * @brief
+     * Checks if an entire other aabb is contained by this aabb.
+     * @param rhs
+     * The aabb to check.
+     * @return
+     * True if the other sphere is completely contained by this aabb.
+     */
+    [[nodiscard]] bool Contains (const SHAABB& rhs) const noexcept;
+
+    /**
+     * @brief
+     * Calculates the volume of the aabb.
+     */
+    [[nodiscard]] float Volume () const noexcept;
+
+    /**
+     * @brief
+     * Calculates the surface area of the aabb.
+     */
+    [[nodiscard]] float SurfaceArea () const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Static Member Functions                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Combines two aabbs to form a larger aabb.
+     * If one aabb is completely contained by the other, the result is the larger aabb.
+     * @return
+     * The combined aabb.
+     */
+    [[nodiscard]] static SHAABB Combine (const SHAABB& lhs, const SHAABB& rhs)  noexcept;
+
+    /**
+     * @brief
+     * Checks if two aabbs are intersecting.
+     * @return
+     * True if they are intersecting.
+     */
+    [[nodiscard]] static bool Intersect (const SHAABB& lhs, const SHAABB& rhs)  noexcept;
+
+    /**
+     * @brief
+     * Builds a single aabb from multiple aabbs.
+     * @param spheres
+     * The set of aabbs to build from.
+     * @param numSpheres
+     * The number of aabbs in the set to build from.
+     * @return
+     * An aabb that contains all the spheres in the set.
+     */
+    [[nodiscard]] static SHAABB BuildFromBoxes (const SHAABB* boxes, size_t numBoxes)  noexcept;
+
+    /**
+     * @brief
+     * Builds a aabb from a set of vertices.
+     * @param vertices
+     * The vertices to build a aabb from.
+     * @param numVertices
+     * The number of vertices in the set to build from.
+     * @param stride
+     * The stride between each vertex, in the instance there is data in between each
+     * vertex that does not define the geometry of the object.
+     * @return
+     * An aabb that contains all the vertices in the set.
+     */
+    [[nodiscard]] static SHAABB BuildFromVertices (const SHVec3* vertices, size_t numVertices, size_t stride = 0) noexcept;
+  };
+
+
+} // namespace SHADE
+

SHADE_Engine/src/Math/Geometry/SHBox.h

@@ -1,105 +0,0 @@
-/****************************************************************************************
- * \file      SHBox.h
- * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a 3-Dimensional Axis Aligned Bounding Box
- *
- * \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
-
-#include <DirectXCollision.h>
-
-// Project Headers
-#include "SHShape.h"
-#include "SH_API.h"
-
-namespace SHADE
-{
-  /*-----------------------------------------------------------------------------------*/
-  /* Type Definitions                                                                  */
-  /*-----------------------------------------------------------------------------------*/
-
-  class SH_API SHBox :  public SHShape,
-                        private DirectX::BoundingBox
-  {
-  public:
-    /*---------------------------------------------------------------------------------*/
-    /* Static Data Members                                                             */
-    /*---------------------------------------------------------------------------------*/
-
-    static constexpr size_t NUM_VERTICES = 8;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Constructors & Destructor                                                       */
-    /*---------------------------------------------------------------------------------*/
-
-    ~SHBox  () override = default;
-
-    SHBox   ()                                                noexcept;
-    SHBox   (const SHVec3& center, const SHVec3& halfExtents) noexcept;
-    SHBox   (const SHBox& rhs)                        noexcept;
-    SHBox   (SHBox&& rhs)                             noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Operator Overloads                                                              */
-    /*---------------------------------------------------------------------------------*/
-
-    SHBox& operator=  (const SHBox& rhs)  noexcept;
-    SHBox& operator=  (SHBox&& rhs)       noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Getter Functions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    [[nodiscard]] SHVec3              GetCenter           ()  const noexcept;
-    [[nodiscard]] SHVec3              GetWorldExtents     ()  const noexcept;
-    [[nodiscard]] const SHVec3&       GetRelativeExtents  ()  const noexcept;
-    [[nodiscard]] SHVec3              GetMin              ()  const noexcept;
-    [[nodiscard]] SHVec3              GetMax              ()  const noexcept;
-    [[nodiscard]] std::vector<SHVec3> GetVertices         ()  const noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Setter Functions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    void  SetCenter           (const SHVec3& newCenter)               noexcept;
-    void  SetWorldExtents     (const SHVec3& newWorldExtents)         noexcept;
-    void  SetRelativeExtents  (const SHVec3& newRelativeExtents)      noexcept;
-    void  SetMin              (const SHVec3& min)                     noexcept;
-    void  SetMax              (const SHVec3& max)                     noexcept;
-    void  SetMinMax           (const SHVec3& min, const SHVec3& max)  noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    [[nodiscard]] bool            TestPoint   (const SHVec3& point) const noexcept override;
-    [[nodiscard]] SHRaycastResult Raycast(const SHRay& ray) const noexcept override;
-
-    [[nodiscard]] bool    Contains    (const SHBox& rhs)  const noexcept;
-    [[nodiscard]] float   Volume      ()  const noexcept;
-    [[nodiscard]] float   SurfaceArea ()  const noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Static Function Members                                                         */
-    /*---------------------------------------------------------------------------------*/
-
-    [[nodiscard]] static SHBox  Combine           (const SHBox& lhs, const SHBox& rhs)    noexcept;
-    [[nodiscard]] static bool   Intersect         (const SHBox& lhs, const SHBox& rhs)    noexcept;
-    [[nodiscard]] static SHBox  BuildFromBoxes    (const SHBox* boxes, size_t numBoxes)   noexcept;
-    [[nodiscard]] static SHBox  BuildFromVertices (const SHVec3* vertices, size_t numVertices, size_t stride = 0) noexcept;
-
-  private:
-    /*---------------------------------------------------------------------------------*/
-    /* Data Members                                                                    */
-    /*---------------------------------------------------------------------------------*/
-
-    SHVec3 RelativeExtents;
-  };
-
-
-} // namespace SHADE
-

SHADE_Engine/src/Math/Geometry/SHPlane.cpp

@@ -0,0 +1,141 @@
+/****************************************************************************************
+ * \file      SHPlane.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a plane.
+ *
+ * \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>
+
+#include <DirectXMath.h>
+
+// Primary Header
+#include "SHPlane.h"
+
+#include "Input/SHInputManager.h"
+#include "Math/SHMathHelpers.h"
+
+using namespace DirectX;
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHPlane::SHPlane() noexcept
+  : planeEquation { SHVec3::One }
+  {
+    planeEquation.w = 0.0f;
+  }
+
+  SHPlane::SHPlane(const SHVec3& point, const SHVec3& normal) noexcept
+  {
+    XMStoreFloat4(&planeEquation, XMPlaneFromPointNormal(point, normal));
+  }
+
+  SHPlane::SHPlane(float a, float b, float c, float d) noexcept
+  : planeEquation { a, b, c, d }
+  {}
+
+  SHPlane::SHPlane(const SHVec3& normal, float distance) noexcept
+  : planeEquation { normal }
+  {
+    planeEquation.w = distance;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHPlane::operator==(const SHPlane& rhs) const noexcept
+  {
+    return XMPlaneEqual(planeEquation, rhs.planeEquation);
+  }
+
+  bool SHPlane::operator!=(const SHPlane& rhs) const noexcept
+  {
+    return XMPlaneNotEqual(planeEquation, rhs.planeEquation);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHVec3 SHPlane::GetNormal() const noexcept
+  {
+    return SHVec3{ planeEquation.x, planeEquation.y, planeEquation.z };
+  }
+
+  float SHPlane::GetDistance() const noexcept
+  {
+    return planeEquation.w;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHPlane::SetNormal(const SHVec3& normal) noexcept
+  {
+    planeEquation.x = normal.x;
+    planeEquation.y = normal.y;
+    planeEquation.z = normal.z;
+  }
+
+  void SHPlane::SetDistance(float distance) noexcept
+  {
+    planeEquation.w = distance;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Function Member Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHPlane::TestPoint(const SHVec3& point) const noexcept
+  {
+    const float DISTANCE = SignedDistance(point);
+    return SHMath::CompareFloat(DISTANCE, 0.0f);
+  }
+
+  SHRaycastResult SHPlane::Raycast(const SHRay& ray) const noexcept
+  {
+    SHRaycastResult result;
+
+    const SHVec3 N = GetNormal();
+
+    // Check if ray is parallel to plane
+    const float N_DOT_D = N.Dot(ray.direction);
+    if (SHMath::CompareFloat(N_DOT_D, 0.0f))
+    {
+      result.hit = false;
+      return result;
+    }
+
+    const float DIST = (planeEquation.w - N.Dot(ray.position)) / N_DOT_D;
+    if (DIST < 0.0f || !SHMath::CompareFloat(DIST, 0.0f))
+    {
+      result.hit = false;
+      return result;
+    }
+
+    result.hit      = true;
+    result.distance = DIST;
+    result.position = ray.position + ray.direction * DIST;
+    result.angle    = SHVec3::Angle(ray.position, result.position);
+
+    // The normal is either the plane's normal or the reverse if the ray came from below
+    result.normal   = N_DOT_D < 0.0f ? -N : N;
+
+    return result;
+  }
+
+  float SHPlane::SignedDistance(const SHVec3& point) const noexcept
+  {
+    return XMVectorGetX(XMPlaneDotCoord(planeEquation, point));
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Math/Geometry/SHPlane.h

@@ -0,0 +1,121 @@
+/****************************************************************************************
+ * \file      SHPlane.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a plane.
+ *
+ * \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/SHRay.h"
+#include "Math/Vector/SHVec4.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates a 3D plane in point-normal form.
+  */
+  class SH_API SHPlane
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    ~SHPlane  ()                    noexcept = default;
+    SHPlane   (const SHPlane& rhs)  noexcept = default;
+    SHPlane   (SHPlane&& rhs)       noexcept = default;
+
+    SHPlane   ()                                          noexcept;
+    SHPlane   (const SHVec3& point, const SHVec3& normal) noexcept;
+    SHPlane   (float a, float b, float c, float d)        noexcept;
+    SHPlane   (const SHVec3& normal, float distance)      noexcept;
+    
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHPlane& operator=  (const SHPlane& rhs)  noexcept = default;
+    SHPlane& operator=  (SHPlane&& rhs)       noexcept = default;
+
+    bool operator==     (const SHPlane& rhs)  const noexcept;
+    bool operator!=     (const SHPlane& rhs)  const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] SHVec3  GetNormal   ()  const noexcept;
+    [[nodiscard]] float   GetDistance ()  const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void SetNormal    (const SHVec3& normal)  noexcept;
+    void SetDistance  (float distance)        noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Checks if a point is on the plane.
+     * @param point
+     * The point to check.
+     * @return
+     * True if the point is on the plane.
+    */
+    [[nodiscard]] bool TestPoint (const SHVec3& point) const noexcept;
+
+    /**
+     * @brief
+     * Casts a ray against the plane.
+     * @param ray
+     * The ray to cast.
+     * @return
+     * The result of the raycast. <br/>
+     * See the corresponding header for the contents of the raycast result object.
+     */
+    [[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept;
+
+    /**
+     * @brief
+     * Gets the signed distance from a point to the plane.
+     * @param point
+     * The point to check.
+     * @return
+     * The signed distance of the point to a plane. <br/>
+     * If the signed distance is negative, the point is behind the plane. <br/>
+     * If the signed distance is zero, the point is on the plane. <br/>
+     * If the signed distance is positive, the point is in front of the plane. <br/>
+    */
+    [[nodiscard]] float SignedDistance (const SHVec3& point) const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Static Member Functions                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    /*
+     * TODO:
+     * Transform plane
+     * Intersection Tests
+     */
+
+  private:
+
+    SHVec4 planeEquation;
+  }; 
+
+} // namespace SHADE

SHADE_Engine/src/Math/Geometry/SHSphere.cpp

@@ -1,208 +0,0 @@
-/****************************************************************************************
- * \file      SHBoundingSphere.cpp
- * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Implementation for a Bounding Sphere
- *
- * \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 "SHSphere.h"
-// Project Headers
-#include "Math/SHMathHelpers.h"
-#include "Math/SHRay.h"
-
-using namespace DirectX;
-
-namespace SHADE
-{
-  /*-----------------------------------------------------------------------------------*/
-  /* Constructors & Destructor Definitions                                             */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHSphere::SHSphere() noexcept
-  : RelativeRadius { 1.0f }
-  {
-    type = Type::SPHERE;
-  }
-
-  SHSphere::SHSphere(const SHVec3& center, float radius) noexcept
-  : RelativeRadius { 1.0f }
-  {
-    type = Type::SPHERE;
-
-    Center = center;
-    Radius = radius;
-  }
-
-  SHSphere::SHSphere(const SHSphere& rhs) noexcept
-  {
-    if (this == &rhs)
-      return;
-
-    type = Type::SPHERE;
-
-    Center          = rhs.Center;
-    Radius          = rhs.Radius;
-    RelativeRadius  = rhs.RelativeRadius;
-  }
-
-  SHSphere::SHSphere(SHSphere&& rhs) noexcept
-  {
-    type = Type::SPHERE;
-
-    Center          = rhs.Center;
-    Radius          = rhs.Radius;
-    RelativeRadius  = rhs.RelativeRadius;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Operator Overload Definitions                                                     */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHSphere& SHSphere::operator=(const SHSphere& rhs) noexcept
-  {
-    if (rhs.type != Type::SPHERE)
-    {
-      SHLOG_WARNING("Cannot assign a non-sphere to a sphere!")
-    }
-    else if (this != &rhs)
-    {
-      Center          = rhs.Center;
-      Radius          = rhs.Radius;
-      RelativeRadius  = rhs.RelativeRadius;
-    }
-
-    return *this;
-  }
-
-  SHSphere& SHSphere::operator=(SHSphere&& rhs) noexcept
-  {
-    if (rhs.type != Type::SPHERE)
-    {
-      SHLOG_WARNING("Cannot assign a non-sphere to a sphere!")
-    }
-    else
-    {
-      Center          = rhs.Center;
-      Radius          = rhs.Radius;
-      RelativeRadius  = rhs.RelativeRadius;
-    }
-
-    return *this;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Getter Function Definitions                                                       */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHVec3 SHSphere::GetCenter() const noexcept
-  {
-    return Center;
-  }
-
-  float SHSphere::GetWorldRadius() const noexcept
-  {
-    return Radius;
-  }
-
-  float SHSphere::GetRelativeRadius() const noexcept
-  {
-    return RelativeRadius;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Setter Function Definitions                                                       */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHSphere::SetCenter(const SHVec3& center) noexcept
-  {
-    Center = center;
-  }
-
-  void SHSphere::SetWorldRadius(float newWorldRadius) noexcept
-  {
-    Radius = newWorldRadius;
-  }
-
-  void SHSphere::SetRelativeRadius(float newRelativeRadius) noexcept
-  {
-    RelativeRadius = newRelativeRadius;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Public Function Member Definitions                                                */
-  /*-----------------------------------------------------------------------------------*/
-
-  bool SHSphere::TestPoint(const SHVec3& point) const noexcept
-  {
-    return BoundingSphere::Contains(point);
-  }
-
-  SHRaycastResult SHSphere::Raycast(const SHRay& ray) const noexcept
-  {
-    SHRaycastResult result;
-
-    result.hit = Intersects(ray.position, ray.direction, result.distance);
-    if (result.hit)
-    {
-      result.position = ray.position + ray.direction * result.distance;
-      result.angle    = SHVec3::Angle(ray.position, result.position);
-    }
-
-    return result;
-  }
-
-  bool SHSphere::Contains(const SHSphere& rhs) const noexcept
-  {
-    return BoundingSphere::Contains(rhs);
-  }
-
-  float SHSphere::Volume() const noexcept
-  {
-    return (4.0f / 3.0f) * SHMath::PI * (Radius * Radius * Radius);
-  }
-
-  float SHSphere::SurfaceArea() const noexcept
-  {
-    return 4.0f * SHMath::PI * (Radius * Radius);
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Static Function Member Definitions                                                */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHSphere SHSphere::Combine(const SHSphere& lhs, const SHSphere& rhs) noexcept
-  {
-    SHSphere result;
-    CreateMerged(result, lhs, rhs);
-    return result;
-  }
-
-  bool SHSphere::Intersect(const SHSphere& lhs, const SHSphere& rhs) noexcept
-  {
-    return lhs.Intersects(rhs);
-  }
-
-  SHSphere SHSphere::BuildFromSpheres(const SHSphere* spheres, size_t numSpheres) noexcept
-  {
-    SHSphere result;
-
-    for (size_t i = 1; i < numSpheres; ++i)
-      CreateMerged(result, spheres[i - 1], spheres[i]);
-
-    return result;
-  }
-
-  SHSphere SHSphere::BuildFromVertices(const SHVec3* vertices, size_t numVertices, size_t stride) noexcept
-  {
-    SHSphere result;
-    CreateFromPoints(result, numVertices, vertices, stride);
-    return result;
-  }
-
-} // namespace SHADE

SHADE_Engine/src/Math/SHColour.cpp

@@ -260,6 +260,8 @@ namespace SHADE
       case 1:   return y;
       case 2:   return z;
       case 3:   return w;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -274,6 +276,8 @@ namespace SHADE
       case 1:   return y;
       case 2:   return z;
       case 3:   return w;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -288,6 +292,8 @@ namespace SHADE
       case 1:   return y;
       case 2:   return z;
       case 3:   return w;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -302,6 +308,8 @@ namespace SHADE
       case 1:   return y;
       case 2:   return z;
       case 3:   return w;
+      // This will never hit
+      default:  return x;
     }
   }
 

SHADE_Engine/src/Math/SHMathHelpers.h

@@ -55,6 +55,8 @@ namespace SHADE
     static constexpr float HALF_PI        = PI * 0.5f;
     static constexpr float TWO_PI         = 2.0f * PI;
 
+    static constexpr float EULER_CONSTANT = std::numbers::egamma_v<float>;
+
     /*---------------------------------------------------------------------------------*/
     /* Static Function Members                                                         */
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Math/SHMatrix.cpp

@@ -34,6 +34,14 @@ namespace SHADE
     0.0f, 0.0f, 0.0f, 1.0f
   };
 
+  const SHMatrix SHMatrix::Zero
+  {
+    SHVec4::Zero
+  , SHVec4::Zero
+  , SHVec4::Zero
+  , SHVec4::Zero
+  };
+
   /*-----------------------------------------------------------------------------------*/
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/

SHADE_Engine/src/Math/SHMatrix.h

@@ -45,6 +45,7 @@ namespace SHADE
     static constexpr  size_t  NUM_COLS  = 4U;
 
     static const SHMatrix     Identity;
+    static const SHMatrix     Zero;
 
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */

SHADE_Engine/src/Math/SHQuaternion.cpp

@@ -40,18 +40,12 @@ namespace SHADE
   : XMFLOAT4( vec4.x, vec4.y, vec4.z, vec4.w )
   {}
 
-  SHQuaternion::SHQuaternion(float _x, float _y, float _z, float _w) noexcept
+  SHQuaternion::SHQuaternion(const XMFLOAT4& xmfloat4) noexcept
-  : XMFLOAT4( _x, _y, _z, _w )
+  : XMFLOAT4( xmfloat4 )
   {}
 
-  SHQuaternion::SHQuaternion(const reactphysics3d::Vector3& rp3dEuler) noexcept
+  SHQuaternion::SHQuaternion(float _x, float _y, float _z, float _w) noexcept
-  : XMFLOAT4( 0.0f, 0.0f, 0.0f, 1.0f )
+  : XMFLOAT4( _x, _y, _z, _w )
-  {
-    XMStoreFloat4(this, XMQuaternionRotationRollPitchYawFromVector(SHVec3 { rp3dEuler }));
-  }
-
-  SHQuaternion::SHQuaternion(const reactphysics3d::Quaternion& rp3dQuat) noexcept
-  : XMFLOAT4( rp3dQuat.x, rp3dQuat.y, rp3dQuat.z, rp3dQuat.w )
   {}
 
   /*-----------------------------------------------------------------------------------*/
@@ -141,16 +135,6 @@ namespace SHADE
     return XMQuaternionNotEqual(*this, rhs);
   }
 
-  SHQuaternion::operator reactphysics3d::Quaternion() const noexcept
-  {
-    return reactphysics3d::Quaternion{ x, y, z, w };
-  }
-
-  SHQuaternion::operator reactphysics3d::Vector3() const noexcept
-  {
-    return reactphysics3d::Vector3{ ToEuler() };
-  }
-
   SHQuaternion::operator XMVECTOR() const noexcept
   {
     return XMLoadFloat4(this);

SHADE_Engine/src/Math/SHQuaternion.h

@@ -11,7 +11,6 @@
 #pragma once
 
 #include <DirectXMath.h>
-#include <reactphysics3d/mathematics/Quaternion.h>
 
 #include <string>
 
@@ -50,13 +49,9 @@ namespace SHADE
 
     SHQuaternion  ()                                    noexcept;
     SHQuaternion  (const SHVec4& vec4)                  noexcept;
+    SHQuaternion  (const XMFLOAT4& xmfloat4)            noexcept;
     SHQuaternion  (float x, float y, float z, float w)  noexcept;
 
-    // Conversion from other math types
-
-    SHQuaternion  (const reactphysics3d::Vector3& rp3dEuler)    noexcept;
-    SHQuaternion  (const reactphysics3d::Quaternion& rp3dQuat)  noexcept;
-
     /*---------------------------------------------------------------------------------*/
     /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
@@ -82,8 +77,6 @@ namespace SHADE
 
     // Conversion to other math types used by SHADE
 
-    operator reactphysics3d::Quaternion       ()                        const noexcept;
-    operator reactphysics3d::Vector3          ()                        const noexcept;
     operator DirectX::XMVECTOR                ()                        const noexcept;
 
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Math/SHRay.cpp

@@ -30,12 +30,6 @@ namespace SHADE
   , direction { dir }
   {}
 
-  SHRay::SHRay(const reactphysics3d::Ray rp3dRay) noexcept
-  : position  { rp3dRay.point1 }
-  , direction { SHVec3::Normalise(rp3dRay.point2 - rp3dRay.point1) }
-  {}
-
-
   /*-----------------------------------------------------------------------------------*/
   /* Operator Overload Definitions                                                     */
   /*-----------------------------------------------------------------------------------*/
@@ -62,12 +56,6 @@ namespace SHADE
     return XMVector3NotEqual(LHS_POS, RHS_POS) || XMVector3NotEqual(LHS_DIR, RHS_DIR);
   }
 
-  SHRay::operator reactphysics3d::Ray() const noexcept
-  {
-    // We use 2km. Temp solution.
-    return reactphysics3d::Ray{ position, position + (direction * MAX_RAYCAST_DIST) };
-  }
-
   SHRaycastResult::operator bool() const noexcept
   {
     return hit;

SHADE_Engine/src/Math/SHRay.h

@@ -10,10 +10,7 @@
 
 #pragma once
 
-#include <reactphysics3d/mathematics/Ray.h>
-
 // Project Headers
-#include "SH_API.h"
 #include "Vector/SHVec3.h"
 
 namespace SHADE
@@ -40,7 +37,6 @@ namespace SHADE
 
     SHRay ()                                     noexcept;
     SHRay (const SHVec3& pos, const SHVec3& dir) noexcept;
-    SHRay (const reactphysics3d::Ray rp3dRay)    noexcept;
 
     SHRay (const SHRay&)  noexcept = default;
     SHRay (SHRay&&     )  noexcept = default;
@@ -55,8 +51,6 @@ namespace SHADE
 
     [[nodiscard]] bool operator==(const SHRay& rhs) const noexcept;
     [[nodiscard]] bool operator!=(const SHRay& rhs) const noexcept;
-
-    operator reactphysics3d::Ray()  const noexcept;
   };
 
   struct SH_API SHRaycastResult

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/Math/Vector/SHVec2.cpp

@@ -50,10 +50,6 @@ namespace SHADE
   : XMFLOAT2( _x, _y )
   {}
 
-  SHVec2::SHVec2(const reactphysics3d::Vector2& rp3dVec2) noexcept
-  : XMFLOAT2( rp3dVec2.x, rp3dVec2.y )
-  {}
-
   /*-----------------------------------------------------------------------------------*/
   /* Operator Overload Definitions                                                     */
   /*-----------------------------------------------------------------------------------*/
@@ -165,6 +161,8 @@ namespace SHADE
     {
       case 0:   return x;
       case 1:   return y;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -177,6 +175,8 @@ namespace SHADE
     {
       case 0:   return x;
       case 1:   return y;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -189,6 +189,8 @@ namespace SHADE
     {
       case 0:   return x;
       case 1:   return y;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -201,14 +203,11 @@ namespace SHADE
     {
       case 0:   return x;
       case 1:   return y;
+      // This will never hit
+      default:  return x;
     }
   }
 
-  SHVec2::operator reactphysics3d::Vector2() const noexcept
-  {
-    return reactphysics3d::Vector2{ x, y };
-  }
-
   SHVec2 operator* (float lhs, const SHVec2& rhs) noexcept
   {
     SHVec2 result;

SHADE_Engine/src/Math/Vector/SHVec2.h

@@ -11,7 +11,6 @@
 #pragma once
 
 #include <DirectXMath.h>
-#include <reactphysics3d/mathematics/Vector2.h>
 
 #include <string>
 #include <initializer_list>
@@ -59,10 +58,6 @@ namespace SHADE
     SHVec2  (float n)                   noexcept;
     SHVec2  (float x, float y)          noexcept;
 
-    // Conversion from other math types to SHADE
-
-    SHVec2  (const reactphysics3d::Vector2& rp3dVec2) noexcept;
-
     /*---------------------------------------------------------------------------------*/
     /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
@@ -73,7 +68,6 @@ namespace SHADE
     // Conversion to other math types used by SHADE
 
     operator DirectX::XMVECTOR        ()                  const noexcept;
-    operator reactphysics3d::Vector2  ()                  const noexcept;
 
     SHVec2&               operator+=  (const SHVec2& rhs)       noexcept;
     SHVec2&               operator-=  (const SHVec2& rhs)       noexcept;

SHADE_Engine/src/Math/Vector/SHVec3.cpp

@@ -57,14 +57,6 @@ namespace SHADE
   : XMFLOAT3( _x, _y, _z )
   {}
 
-  SHVec3::SHVec3(const reactphysics3d::Vector3& rp3dVec3) noexcept
-  : XMFLOAT3( rp3dVec3.x, rp3dVec3.y, rp3dVec3.z )
-  {}
-
-  SHVec3::SHVec3(const reactphysics3d::Quaternion& rp3dVec3) noexcept
-  : XMFLOAT3( SHQuaternion{rp3dVec3}.ToEuler() )
-  {}
-
   /*-----------------------------------------------------------------------------------*/
   /* Operator Overload Definitions                                                     */
   /*-----------------------------------------------------------------------------------*/
@@ -179,6 +171,8 @@ namespace SHADE
       case 0:   return x;
       case 1:   return y;
       case 2:   return z;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -192,6 +186,8 @@ namespace SHADE
       case 0:   return x;
       case 1:   return y;
       case 2:   return z;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -205,6 +201,8 @@ namespace SHADE
       case 0:   return x;
       case 1:   return y;
       case 2:   return z;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -218,19 +216,11 @@ namespace SHADE
       case 0:   return x;
       case 1:   return y;
       case 2:   return z;
+      // This will never hit
+      default:  return x;
     }
   }
 
-  SHVec3::operator reactphysics3d::Vector3() const noexcept 
-  {
-    return reactphysics3d::Vector3{ x, y , z };
-  }
-
-  SHVec3::operator reactphysics3d::Quaternion() const noexcept
-  {
-    return reactphysics3d::Quaternion::fromEulerAngles(x, y, z);
-  }
-
   SHVec3 operator* (float lhs, const SHVec3& rhs) noexcept
   {
     SHVec3 result;
@@ -382,6 +372,30 @@ namespace SHADE
     return lhs.Cross(rhs);
   }
 
+  SHMatrix SHVec3::OuterProduct(const SHVec3& lhs, const SHVec3& rhs) noexcept
+  {
+    /*
+     *  Outer product is a matrix multiplication u * vT
+     *  3x1 * 1x3 = 3x3
+     *
+     *  | u1 | | v1 v2 v3 |   | u1v1 u1v2 u1v3 |
+     *  | u2 |              = | u2v1 u2v2 u2v3 |
+     *  | u3 |                | u3v1 u3v2 u3v3 |
+     */
+
+    SHMatrix u  = SHMatrix::Zero;
+    SHMatrix vT = SHMatrix::Zero;
+
+    for (int i = 0; i < SIZE; ++i)
+    {
+      u.m[0][i]   = lhs[i];
+      vT.m[i][0]  = rhs[i];
+    }
+
+    return u * vT;
+  }
+
+
   SHVec3 SHVec3::Project(const SHVec3& v, const SHVec3& u) noexcept
   {
     SHVec3 result;

SHADE_Engine/src/Math/Vector/SHVec3.h

@@ -11,8 +11,6 @@
 #pragma once
 
 #include <DirectXMath.h>
-#include <reactphysics3d/mathematics/Vector3.h>
-#include <reactphysics3d/mathematics/Quaternion.h>
 
 #include <string>
 #include <initializer_list>
@@ -69,9 +67,6 @@ namespace SHADE
 
     // Conversion from other math types to SHADE
 
-    SHVec3  (const reactphysics3d::Vector3&     rp3dVec3) noexcept;
-    SHVec3  (const reactphysics3d::Quaternion&  rp3dVec3) noexcept;
-
     /*---------------------------------------------------------------------------------*/
     /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
@@ -81,8 +76,6 @@ namespace SHADE
 
     // Conversion to other math types used by SHADE
 
-    operator reactphysics3d::Vector3    ()                  const noexcept;
-    operator reactphysics3d::Quaternion ()                  const noexcept;
     operator DirectX::XMVECTOR          ()                  const noexcept;
 
     SHVec3&               operator+=    (const SHVec3& rhs)       noexcept;
@@ -135,6 +128,7 @@ namespace SHADE
     [[nodiscard]] static float    Angle           (const SHVec3& lhs, const SHVec3& rhs)                                            noexcept;
     [[nodiscard]] static float    Dot             (const SHVec3& lhs, const SHVec3& rhs)                                            noexcept;
     [[nodiscard]] static SHVec3   Cross           (const SHVec3& lhs, const SHVec3& rhs)                                            noexcept;
+    [[nodiscard]] static SHMatrix OuterProduct    (const SHVec3& lhs, const SHVec3& rhs)                                            noexcept;
     [[nodiscard]] static SHVec3   Project         (const SHVec3& v, const SHVec3& u)                                                noexcept;
     [[nodiscard]] static SHVec3   Reflect         (const SHVec3& v, const SHVec3& normal)                                           noexcept;
     [[nodiscard]] static SHVec3   Rotate          (const SHVec3& v, const SHVec3& axis, float angleInRad)                           noexcept;

SHADE_Engine/src/Math/Vector/SHVec4.cpp

@@ -164,6 +164,8 @@ namespace SHADE
       case 1:   return y;
       case 2:   return z;
       case 3:   return w;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -178,6 +180,8 @@ namespace SHADE
       case 1:   return y;
       case 2:   return z;
       case 3:   return w;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -192,6 +196,8 @@ namespace SHADE
       case 1:   return y;
       case 2:   return z;
       case 3:   return w;
+      // This will never hit
+      default:  return x;
     }
   }
 
@@ -206,6 +212,8 @@ namespace SHADE
       case 1:   return y;
       case 2:   return z;
       case 3:   return w;
+      // This will never hit
+      default:  return x;
     }
   }
 

SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp

@@ -0,0 +1,644 @@
+/****************************************************************************************
+ * \file      SHDynamicAABBTree.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Dynamic AABB Tree for broadphase collision detection.
+ *
+ * \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>
+
+#include <stack>
+
+// Primary Header
+#include "SHDynamicAABBTree.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHAABBTree::SHAABBTree() noexcept
+  : root      { NULL_NODE }
+  , nodes     { nullptr }
+  , nodeCount { 0 }
+  , capacity  { 1024 }
+  , freeList  { NULL_NODE }
+  {
+    // Build initial tree
+    nodes = new Node[1024];
+
+    addToFreeList(0);
+  }
+
+  SHAABBTree::~SHAABBTree() noexcept
+  {
+    delete[] nodes;
+  }
+
+  SHAABBTree::Node::Node() noexcept
+  : id      { MAX_EID, std::numeric_limits<uint32_t>::max() }
+  , parent  { NULL_NODE }
+  , left    { NULL_NODE }
+  , right   { NULL_NODE }
+  , height  { NULL_NODE }
+  {}
+
+  SHAABBTree::Node::Node(const Node& rhs) noexcept
+  : AABB    { rhs.AABB }
+  , id      { rhs.id }
+  , next    { rhs.next }
+  , left    { rhs.left }
+  , right   { rhs.right }
+  , height  { rhs.height }
+  {}
+
+  SHAABBTree::Node::Node(Node&& rhs) noexcept
+  : AABB    { rhs.AABB }
+  , id      { rhs.id }
+  , next    { rhs.next }
+  , left    { rhs.left }
+  , right   { rhs.right }
+  , height  { rhs.height }
+  {}
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHAABBTree::Node& SHAABBTree::Node::operator=(const Node& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    AABB   = rhs.AABB;
+    id     = rhs.id;
+    parent = rhs.parent;
+    next   = rhs.next;
+    left   = rhs.left;
+    right  = rhs.right;
+    height = rhs.height;
+
+    return *this;
+  }
+
+  SHAABBTree::Node& SHAABBTree::Node::operator=(Node&& rhs) noexcept
+  {
+    AABB   = std::move(rhs.AABB);
+    id     = std::move(rhs.id);
+    parent = rhs.parent;
+    next   = rhs.next;
+    left   = rhs.left;
+    right  = rhs.right;
+    height = rhs.height;
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  const std::vector<SHAABB>& SHAABBTree::GetAABBs() const noexcept
+  {
+    static AABBs                aabbs;
+    static std::stack<int32_t>  nodeIndices;
+
+    aabbs.clear();
+
+    nodeIndices.push(root);
+    while (!nodeIndices.empty())
+    {
+      // Pop the top node
+      const int INDEX = nodeIndices.top();
+      nodeIndices.pop();
+
+      // Skip null nodes
+      if (INDEX == NULL_NODE)
+        continue;
+
+      const Node& CURRENT_NODE = nodes[INDEX];
+
+      aabbs.emplace_back(CURRENT_NODE.AABB);
+
+      if (!isLeaf(INDEX))
+      {
+        nodeIndices.push(CURRENT_NODE.left);
+        nodeIndices.push(CURRENT_NODE.right);
+      }
+    }
+
+    return aabbs;
+  }
+
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHAABBTree::Insert(SHCollisionShapeID id, const SHAABB& AABB)
+  {
+    const int32_t NEW_INDEX = allocateNode();
+
+    if (!nodeMap.emplace(id, NEW_INDEX).second)
+    {
+      // Attempted to add a duplicate node
+      freeNode(NEW_INDEX);
+      return;
+    }
+
+    Node& newNode = nodes[NEW_INDEX];
+    newNode.AABB    = AABB;
+    newNode.id      = id;
+    newNode.height  = 0;
+
+    // Fatten the AABB
+    const SHVec3 EXTENSION{ AABB_EXTENSION };
+
+    const SHVec3 newMin = newNode.AABB.GetMin() - EXTENSION;
+    const SHVec3 newMax = newNode.AABB.GetMax() + EXTENSION;
+
+    newNode.AABB.SetMin(newMin);
+    newNode.AABB.SetMax(newMax);
+
+    insertLeaf(NEW_INDEX);
+  }
+
+  void SHAABBTree::Update(SHCollisionShapeID id, const SHAABB& AABB)
+  {
+    // Get node index
+    const int32_t INDEX_TO_UPDATE = nodeMap[id];
+
+    Node& nodeToUpdate = nodes[INDEX_TO_UPDATE];
+
+    // If new AABB has not moved enough, skip.
+    if (nodeToUpdate.AABB.Contains(AABB))
+      return;
+
+    removeLeaf(INDEX_TO_UPDATE);
+
+    nodeToUpdate.AABB = AABB;
+
+    // Fatten the AABB
+    const SHVec3 EXTENSION{ AABB_EXTENSION };
+
+    const SHVec3 newMin = nodeToUpdate.AABB.GetMin() - EXTENSION;
+    const SHVec3 newMax = nodeToUpdate.AABB.GetMax() + EXTENSION;
+
+    nodeToUpdate.AABB.SetMin(newMin);
+    nodeToUpdate.AABB.SetMax(newMax);
+
+    insertLeaf(INDEX_TO_UPDATE);
+  }
+
+  void SHAABBTree::Remove(SHCollisionShapeID id) noexcept
+  {
+    // Get node index
+    const int32_t INDEX_TO_REMOVE = nodeMap[id];
+
+    removeLeaf(INDEX_TO_REMOVE);
+    freeNode(INDEX_TO_REMOVE);
+
+    nodeMap.erase(id);
+  }
+
+  const std::vector<SHCollisionShapeID>& SHAABBTree::Query(SHCollisionShapeID id, const SHAABB& AABB) const noexcept
+  {
+    static std::vector<SHCollisionShapeID> potentialCollisions;
+    static std::stack<int32_t> nodeIndices;
+
+    potentialCollisions.clear();
+
+    // We use this to ignore shapes on the same entity
+    const EntityID EID = id.GetEntityID();
+
+    nodeIndices.push(root);
+    while (!nodeIndices.empty())
+    {
+      const int32_t INDEX = nodeIndices.top();
+      nodeIndices.pop();
+
+      if (INDEX == NULL_NODE)
+        continue;
+
+      const Node& NODE = nodes[INDEX];
+      if (!SHAABB::Intersect(AABB, NODE.AABB))
+        continue;
+
+      // Avoid checking against shapes of the same composite collider (and itself)
+      if (isLeaf(INDEX) && NODE.id.GetEntityID() != EID)
+      {
+        potentialCollisions.emplace_back(NODE.id);
+      }
+      else
+      {
+        nodeIndices.push(NODE.left);
+        nodeIndices.push(NODE.right);
+      }
+    }
+
+    return potentialCollisions;
+  }
+
+  const std::vector<SHCollisionShapeID>& SHAABBTree::Query(const SHRay& ray, float distance) const noexcept
+  {
+    static std::vector<SHCollisionShapeID> potentialHits;
+    static std::stack<int32_t> nodeIndices;
+
+    potentialHits.clear();
+
+    nodeIndices.push(root);
+    while (!nodeIndices.empty())
+    {
+      const int32_t INDEX = nodeIndices.top();
+      nodeIndices.pop();
+
+      if (INDEX == NULL_NODE)
+        continue;
+
+      const Node& NODE = nodes[INDEX];
+
+      const auto& RESULT = NODE.AABB.Raycast(ray);
+      if (!RESULT || RESULT.distance > distance)
+        continue;
+
+      if (isLeaf(INDEX))
+      {
+        potentialHits.emplace_back(NODE.id);
+      }
+      else
+      {
+        // Non-leaf nodes need to be traversed further
+        nodeIndices.push(NODE.left);
+        nodeIndices.push(NODE.right);
+      }
+    }
+
+    return potentialHits;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHAABBTree::isLeaf(int32_t index) const noexcept
+  {
+    const Node& NODE = nodes[index];
+    return NODE.left == NULL_NODE && NODE.right == NULL_NODE;
+  }
+
+  int32_t SHAABBTree::allocateNode()
+  {
+    if (freeList == NULL_NODE)
+    {
+      // No more free nodes available, so we need to resize the tree for more nodes
+      capacity *= 2;
+
+      Node* newNodes = new Node[capacity];
+
+      // Copy all the nodes manually. I do this instead of memcpy to guarantee it copies properly.
+      for (int32_t i = 0; i < nodeCount; ++i)
+      {
+        newNodes[i].AABB    = nodes[i].AABB;
+        newNodes[i].id      = nodes[i].id;
+        newNodes[i].parent  = nodes[i].parent;
+        newNodes[i].left    = nodes[i].left;
+        newNodes[i].right   = nodes[i].right;
+        newNodes[i].height  = nodes[i].height;
+      }
+
+      delete[] nodes;
+      nodes = newNodes;
+
+      addToFreeList(nodeCount);
+    }
+
+    const int32_t FREE_NODE = freeList;
+    freeList = nodes[FREE_NODE].next;
+
+    // Set node to default
+    Node& newNode = nodes[FREE_NODE];
+    newNode.parent  = NULL_NODE;
+    newNode.left    = NULL_NODE;
+    newNode.right   = NULL_NODE;
+    newNode.height  = NULL_NODE;
+
+    ++nodeCount;
+    return FREE_NODE;
+  }
+
+  void SHAABBTree::freeNode(int32_t index) noexcept
+  {
+    SHASSERT(index >= 0 && index < capacity, "Trying to free an invalid AABB Tree node!")
+
+    nodes[index].next     = freeList;
+    nodes[index].height   = NULL_NODE;
+
+    // Put it back on the free list
+    freeList = index;
+
+    --nodeCount;
+  }
+
+  void SHAABBTree::insertLeaf(int32_t index)
+  {
+    // If there is no root, the first insert must make the root
+    if (root == NULL_NODE)
+    {
+      root = index;
+      nodes[root].parent = NULL_NODE;
+      return;
+    }
+
+    // Find best sibling for new leaf
+    // Utilise Surface Area Heuristic
+    const SHAABB& LEAF_AABB  = nodes[index].AABB;
+
+    uint32_t searchIndex = root;
+    while (!isLeaf(searchIndex))
+    {
+      const SHAABB COMBINED_AABB = SHAABB::Combine(LEAF_AABB, nodes[searchIndex].AABB);
+      const float COMBINED_AREA = COMBINED_AABB.SurfaceArea();
+
+      const float INHERITED_COST  = 2.0f * (COMBINED_AREA - nodes[searchIndex].AABB.SurfaceArea());
+
+      const int32_t LEFT_INDEX  = nodes[searchIndex].left;
+      const int32_t RIGHT_INDEX = nodes[searchIndex].right;
+
+      float leftCost  = 0.0f;
+      float rightCost = 0.0f;
+
+      const float LEFT_COMBINED_AREA  = SHAABB::Combine(LEAF_AABB, nodes[LEFT_INDEX].AABB).SurfaceArea();
+      const float RIGHT_COMBINED_AREA = SHAABB::Combine(LEAF_AABB, nodes[RIGHT_INDEX].AABB).SurfaceArea();
+
+      // Compute cost for descending into the left
+      if (isLeaf(LEFT_INDEX))
+        leftCost = LEFT_COMBINED_AREA + INHERITED_COST;
+      else
+        leftCost = LEFT_COMBINED_AREA - nodes[LEFT_INDEX].AABB.SurfaceArea() + INHERITED_COST;
+
+      // Compute cost for descending into the right
+      if (isLeaf(RIGHT_INDEX))
+        rightCost = RIGHT_COMBINED_AREA + INHERITED_COST;
+      else
+        rightCost = RIGHT_COMBINED_AREA - nodes[RIGHT_INDEX].AABB.SurfaceArea() + INHERITED_COST;
+
+      // Early out
+      const float BRANCH_COST = 2.0f * COMBINED_AREA;
+      if (BRANCH_COST < leftCost && BRANCH_COST < rightCost)
+        break;
+
+      // Traverse
+      searchIndex = leftCost < rightCost ? LEFT_INDEX : RIGHT_INDEX;
+    }
+
+    const int32_t BEST_SIBLING = searchIndex;
+
+    // Create a new parent for the leaf
+    const int32_t OLD_PARENT = nodes[BEST_SIBLING].parent;
+    const int32_t NEW_PARENT = allocateNode();
+
+    Node& newParent = nodes[NEW_PARENT];
+    newParent.parent  = OLD_PARENT;
+    newParent.id      = SHCollisionShapeID{ MAX_EID, std::numeric_limits<uint32_t>::max() };
+    newParent.AABB    = SHAABB::Combine(LEAF_AABB, nodes[BEST_SIBLING].AABB);
+    newParent.height  = nodes[BEST_SIBLING].height + 1;
+
+    newParent.left  = BEST_SIBLING;
+      newParent.right = index;
+
+      nodes[BEST_SIBLING].parent  = NEW_PARENT;
+      nodes[index].parent         = NEW_PARENT;
+
+    // If sibling was the root
+    if (OLD_PARENT == NULL_NODE)
+      root = NEW_PARENT;
+    else
+      (nodes[OLD_PARENT].left == BEST_SIBLING ? nodes[OLD_PARENT].left : nodes[OLD_PARENT].right) = NEW_PARENT;
+
+    syncHierarchy(NEW_PARENT);
+  }
+
+  void SHAABBTree::removeLeaf(int32_t index)
+  {
+    if (index == root)
+    {
+      root = NULL_NODE;
+      return;
+    }
+
+    const int32_t PARENT      = nodes[index].parent;
+
+    if (PARENT == NULL_NODE)
+    {
+      freeNode(index);
+      return;
+    }
+
+    const int32_t GRANDPARENT = nodes[PARENT].parent;
+    const int32_t SIBLING     = nodes[PARENT].left == index ? nodes[PARENT].right : nodes[PARENT].left;
+
+    if (GRANDPARENT != NULL_NODE)
+    {
+      // Replace parent with sibling
+      (nodes[GRANDPARENT].left == PARENT ? nodes[GRANDPARENT].left : nodes[GRANDPARENT].right) = SIBLING;
+      nodes[SIBLING].parent = GRANDPARENT;
+    }
+    else
+    {
+      // Parent was root
+      root = SIBLING;
+      nodes[SIBLING].parent = NULL_NODE;
+    }
+
+    freeNode(PARENT);
+    syncHierarchy(GRANDPARENT);
+  }
+
+  void SHAABBTree::syncHierarchy(int32_t index)
+  {
+    while (index != NULL_NODE)
+    {
+      index = balance(index);
+
+      const int32_t LEFT_INDEX  = nodes[index].left;
+      const Node&   LEFT_NODE   = nodes[LEFT_INDEX];
+
+      const int32_t RIGHT_INDEX = nodes[index].right;
+      const Node&   RIGHT_NODE  = nodes[RIGHT_INDEX];
+
+      nodes[index].height = 1 + SHMath::Max(LEFT_NODE.height, RIGHT_NODE.height);
+      nodes[index].AABB   = SHAABB::Combine(LEFT_NODE.AABB, RIGHT_NODE.AABB);
+
+      // Sync up to the root
+      index = nodes[index].parent;
+    }
+  }
+
+  int32_t SHAABBTree::balance(int32_t index)
+  {
+    if (isLeaf(index) || nodes[index].height == 1)
+      return index;
+
+    Node& nodeA = nodes[index];
+
+    const int32_t LEFT  = nodeA.left;
+    const int32_t RIGHT = nodeA.right;
+
+    const int32_t DIFF = nodes[RIGHT].height - nodes[LEFT].height;
+
+    if (DIFF > 1)
+      return rotateLeft(index);
+
+    if (DIFF < -1)
+      return rotateRight(index);
+
+    return index;
+  }
+
+  int32_t SHAABBTree::rotateLeft(int32_t index)
+  {
+    /****************************
+          A             C
+         / \           / \
+        B   C   -->   A  F/G
+           / \       / \
+          F   G     B  G/F
+    ****************************/
+
+    // Promote C
+
+    Node& nodeA = nodes[index];
+
+    const int32_t B = nodeA.left;
+    const int32_t C = nodeA.right;
+
+    Node& nodeB = nodes[B];
+    Node& nodeC = nodes[C];
+
+    const int32_t F = nodeC.left;
+    const int32_t G = nodeC.right;
+
+    Node& nodeF = nodes[F];
+    Node& nodeG = nodes[G];
+
+    if (nodeA.parent != NULL_NODE)
+      (nodes[nodeA.parent].left == index ? nodes[nodeA.parent].left : nodes[nodeA.parent].right) = C;
+    else
+      root = C;
+
+    nodeC.left    = index;
+    nodeC.parent  = nodeA.parent;
+    nodeA.parent  = C;
+
+    if (nodeF.height > nodeG.height)
+    {
+      nodeC.right   = F;
+      nodeA.right   = G;
+      nodeG.parent  = index;
+
+      nodeA.AABB    = SHAABB::Combine(nodeB.AABB, nodeG.AABB);
+      nodeC.AABB    = SHAABB::Combine(nodeA.AABB, nodeF.AABB);
+
+      nodeA.height  = 1 + SHMath::Max(nodeB.height, nodeG.height);
+      nodeC.height  = 1 + SHMath::Max(nodeA.height, nodeF.height);
+    }
+    else
+    {
+      nodeC.right   = G;
+      nodeA.right   = F;
+      nodeF.parent  = index;
+
+      nodeA.AABB    = SHAABB::Combine(nodeB.AABB, nodeF.AABB);
+      nodeC.AABB    = SHAABB::Combine(nodeA.AABB, nodeG.AABB);
+
+      nodeA.height  = 1 + SHMath::Max(nodeB.height, nodeF.height);
+      nodeC.height  = 1 + SHMath::Max(nodeA.height, nodeG.height);
+    }
+
+    return C;
+  }
+
+  int32_t SHAABBTree::rotateRight(int32_t index)
+  {
+    /*************************
+          A            B
+         / \          / \
+        B   C  -->  D/E  A
+       / \              / \
+      D   E           E/D  C
+    *************************/
+
+    // Promote B
+
+    Node& nodeA = nodes[index];
+
+    const int32_t B = nodeA.left;
+    const int32_t C = nodeA.right;
+
+    Node& nodeB = nodes[B];
+    Node& nodeC = nodes[C];
+
+    const int32_t D = nodeB.left;
+    const int32_t E = nodeB.right;
+
+    Node& nodeD = nodes[D];
+    Node& nodeE = nodes[E];
+
+    if (nodeA.parent != NULL_NODE)
+      (nodes[nodeA.parent].left == index ? nodes[nodeA.parent].left : nodes[nodeA.parent].right) = B;
+    else
+      root = B;
+
+    nodeB.right   = index;
+    nodeB.parent  = nodeA.parent;
+    nodeA.parent  = B;
+
+    if (nodeD.height > nodeE.height)
+    {
+      nodeB.left    = D;
+      nodeA.left    = E;
+      nodeE.parent  = index;
+
+      nodeA.AABB    = SHAABB::Combine(nodeC.AABB, nodeE.AABB);
+      nodeB.AABB    = SHAABB::Combine(nodeA.AABB, nodeD.AABB);
+
+      nodeA.height  = 1 + SHMath::Max(nodeC.height, nodeE.height);
+      nodeB.height  = 1 + SHMath::Max(nodeA.height, nodeD.height);
+    }
+    else
+    {
+      nodeB.left    = E;
+      nodeA.left    = D;
+      nodeD.parent  = index;
+
+      nodeA.AABB    = SHAABB::Combine(nodeC.AABB, nodeD.AABB);
+      nodeB.AABB    = SHAABB::Combine(nodeA.AABB, nodeE.AABB);
+
+      nodeA.height  = 1 + SHMath::Max(nodeC.height, nodeD.height);
+      nodeB.height  = 1 + SHMath::Max(nodeA.height, nodeE.height);
+    }
+
+    return B;
+  }
+
+  void SHAABBTree::addToFreeList(int32_t index) noexcept
+  {
+    for (int32_t i = index; i < capacity; ++i)
+    {
+      nodes[i].next   = i + 1;
+      nodes[i].height = NULL_NODE;
+    }
+
+    nodes[capacity - 1].next    = NULL_NODE;
+    nodes[capacity - 1].height  = NULL_NODE;
+
+    freeList = index;
+  }
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.h

@@ -0,0 +1,156 @@
+/****************************************************************************************
+ * \file      SHDynamicAABBTree.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Dynamic AABB Tree for broadphase collision detection.
+ *
+ * \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
+
+#include <unordered_map>
+
+// Project Headers
+#include "Physics/Collision/Shapes/SHCollisionShape.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates a dynamic AABB Tree for collision detection.
+  */
+  class SH_API SHAABBTree
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    using AABBs = std::vector<SHAABB>;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    static constexpr int NULL_NODE = -1;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHAABBTree  () noexcept;
+    ~SHAABBTree () noexcept;
+
+    SHAABBTree(const SHAABBTree& other)                = delete;
+    SHAABBTree(SHAABBTree&& other) noexcept            = delete;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHAABBTree& operator=(const SHAABBTree& other)     = delete;
+    SHAABBTree& operator=(SHAABBTree&& other) noexcept = delete;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] const AABBs& GetAABBs  () const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void Insert  (SHCollisionShapeID id, const SHAABB& AABB);
+    void Update  (SHCollisionShapeID id, const SHAABB& AABB);
+    void Remove  (SHCollisionShapeID id) noexcept;
+
+    [[nodiscard]] const std::vector<SHCollisionShapeID>& Query(SHCollisionShapeID id, const SHAABB& AABB)  const noexcept;
+    [[nodiscard]] const std::vector<SHCollisionShapeID>& Query(const SHRay& ray, float distance)          const noexcept;
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct Node
+    {
+    public:
+      /*-------------------------------------------------------------------------------*/
+      /* Constructors & Destructor                                                     */
+      /*-------------------------------------------------------------------------------*/
+
+      Node  ()                noexcept;
+      Node  (const Node& rhs) noexcept;
+      Node  (Node&& rhs)      noexcept;
+
+      ~Node () noexcept = default;
+
+      /*-------------------------------------------------------------------------------*/
+      /* Operator Overloads                                                            */
+      /*-------------------------------------------------------------------------------*/
+
+      Node& operator=(const Node& rhs)  noexcept;
+      Node& operator=(Node&& rhs)       noexcept;
+
+      /*-------------------------------------------------------------------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      SHAABB               AABB;
+      SHCollisionShapeID  id;   // Used to lookup the collision shape & entity for culling against itself
+
+      union
+      {
+        int32_t parent;
+        int32_t next;
+      };
+
+
+      int32_t left;
+      int32_t right;
+      int32_t height;  // Leaves have a height of 0. Free nodes have a height of -1
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    static constexpr float AABB_EXTENSION   = 0.2f;
+
+    // For quick access
+    std::unordered_map<SHCollisionShapeID, int32_t, SHCollisionShapeIDHash> nodeMap;
+
+    int32_t root;
+    Node*   nodes;      // Dynamically allocated array of nodes. I use dynamic allocation as in the past, using a vector causes weird issues.
+    int32_t nodeCount;
+    int32_t capacity;   // Used for resizing the tree.
+    int32_t freeList;   // Stores the next available node on the free list.
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    bool    isLeaf        (int32_t index) const noexcept;
+
+    int32_t allocateNode  ();
+    void    freeNode      (int32_t index) noexcept;
+
+    void    insertLeaf    (int32_t index);
+    void    removeLeaf    (int32_t index);
+    void    syncHierarchy (int32_t index);
+    int32_t balance       (int32_t index);
+    int32_t rotateLeft    (int32_t index);
+    int32_t rotateRight   (int32_t index);
+
+    void    addToFreeList (int32_t index) noexcept;
+  };
+
+
+}

SHADE_Engine/src/Physics/Collision/CollisionTags/SHCollisionTagMatrix.cpp

@@ -14,6 +14,9 @@
 // Primary Header
 #include "SHCollisionTagMatrix.h"
 
+// Project Headers
+#include "Tools/Utilities/SHUtilities.h"
+
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
@@ -145,8 +148,9 @@ namespace SHADE
     /**
       * I HATE FILE IO
       *
-      * Each line in the file should be "index<space>tag name".
+      * Each line in the file should be "index<space>tag name<space>mask".
-      * If the line fails to follow this format, use the default tag name (index + 1)
+      * If the line fails to follow this format, use the default tag name (index + 1) and default mask.
+      * If no mask was read, use a default mask.
       */
 
     // Populate tag names with default
@@ -187,18 +191,40 @@ namespace SHADE
       {
         SHLOG_ERROR
         (
-          "Collision tag file line {} does not match the required format of 'index<space>tag name'. Default tag used for index {}"
+          "Collision tag file line {} does not match the required format of 'index<space>tag name<space>mask'. Default tag used for index {}"
         , linesRead + 1
         , tagIndex
         )
 
         // Use default
         collisionTags[tagIndex].SetName(std::to_string(tagIndex + 1));
+        collisionTags[tagIndex].SetMask(SHUtilities::ConvertEnum(SHCollisionTag::Layer::ALL));
         continue;
       }
 
       collisionTags[tagIndex].SetName(tagName);
 
+      // Next element is the mask value
+      std::string maskString;
+      ss >> maskString;
+
+      uint16_t mask = std::numeric_limits<uint16_t>::max();
+      if (maskString.empty())
+      {
+        SHLOG_ERROR
+        (
+          "Collision tag file line {} does not match the required format of 'index<space>tag name<space>mask'. Default mask used for index {}"
+        , linesRead + 1
+        , tagIndex
+        )
+      }
+      else
+      {
+        mask = static_cast<uint16_t>(std::stoi(maskString));
+      }
+
+      collisionTags[tagIndex].SetMask(mask);
+
       ss.clear();
     }
 
@@ -215,8 +241,9 @@ namespace SHADE
       return;
     }
 
+    // Index Name Mask
     for (int i = 0; i < SHCollisionTag::NUM_LAYERS; ++i)
-      collisionTagNamesFile << i << " " << collisionTags[i].GetName() << std::endl;
+      collisionTagNamesFile << i << " " << collisionTags[i].GetName() << " " << collisionTags[i].GetMask() << std::endl;
 
     collisionTagNamesFile.close();
   }

SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h

@@ -0,0 +1,60 @@
+/****************************************************************************************
+ * \file      SHCollisionKey.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for Collision Information for Collision & Triggers.
+ *
+ * \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 "SHCollisionKey.h"
+
+namespace SHADE
+{ 
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  enum class SHCollisionState
+  {
+    ENTER
+  , STAY
+  , EXIT
+
+  , TOTAL
+  , INVALID = -1
+  };
+
+  /**
+   * @brief
+   * Encapsulates the event for an intersection between two collision shapes that do not
+   * have physical resolution.
+  */
+  struct SH_API SHTriggerEvent
+  {
+  public:
+    SHCollisionKey     info;
+    SHCollisionState  state;
+  };
+
+  /**
+   * @brief
+   * Encapsulates the event for an intersection between two collision shapes that do
+   * have physical resolution.
+  */
+  struct SH_API SHCollisionEvent
+  {
+  public:
+    static constexpr int MAX_NUM_CONTACTS = 4;
+
+    SHCollisionKey    info;
+    SHCollisionState  state;
+    SHVec3            normal;
+    SHVec3            contactPoints[MAX_NUM_CONTACTS];
+  };
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionKey.cpp

@@ -0,0 +1,166 @@
+/****************************************************************************************
+ * \file      SHCollisionInfo.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for Collision Info.
+ *
+ * \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 "SHCollisionKey.h"
+
+// Project Headers
+#include "Physics/Collision/SHCollider.h"
+#include "Physics/Interface/SHColliderComponent.h"
+
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionKey::SHCollisionKey() noexcept
+  {
+    ids[ENTITY_A]       = MAX_EID;
+    ids[ENTITY_B]       = MAX_EID;
+    ids[SHAPE_INDEX_A]  = std::numeric_limits<uint32_t>::max();
+    ids[SHAPE_INDEX_B]  = std::numeric_limits<uint32_t>::max();
+  }
+
+  SHCollisionKey::SHCollisionKey(const SHCollisionKey& rhs) noexcept
+  {
+    value[0] = rhs.value[0];
+    value[1] = rhs.value[1];
+  }
+
+  SHCollisionKey::SHCollisionKey(SHCollisionKey&& rhs) noexcept
+  {
+    value[0] = rhs.value[0];
+    value[1] = rhs.value[1];
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionKey& SHCollisionKey::operator=(const SHCollisionKey& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    value[0] = rhs.value[0];
+    value[1] = rhs.value[1];
+
+    return *this;
+  }
+
+  SHCollisionKey& SHCollisionKey::operator=(SHCollisionKey&& rhs) noexcept
+  {
+    value[0] = rhs.value[0];
+    value[1] = rhs.value[1];
+
+    return *this;
+  }
+
+  bool SHCollisionKey::operator==(const SHCollisionKey& rhs) const
+  {
+    // When checking for equal, check both ways.
+    // Exact Match (A, idxA, B, idxB)
+    const bool EXACT_MATCH    = value[0] == rhs.value[0] && value[1] == rhs.value[1];
+
+    // Flipped Match: (B, idxB, A, idxA)
+    const bool FLIPPED_MATCH  = value[0] == rhs.value[1] && value[1] == rhs.value[0];
+
+    return EXACT_MATCH || FLIPPED_MATCH;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  EntityID SHCollisionKey::GetEntityA() const noexcept
+  {
+    return ids[ENTITY_A];
+  }
+
+  EntityID SHCollisionKey::GetEntityB() const noexcept
+  {
+    return ids[ENTITY_B];
+  }
+
+  uint32_t SHCollisionKey::GetShapeIndexA() const noexcept
+  {
+    return ids[SHAPE_INDEX_A];
+  }
+
+  uint32_t SHCollisionKey::GetShapeIndexB() const noexcept
+  {
+    return ids[SHAPE_INDEX_B];
+  }
+
+  const SHRigidBodyComponent* SHCollisionKey::GetRigidBodyA() const noexcept
+  {
+    return SHComponentManager::GetComponent_s<SHRigidBodyComponent>(ids[ENTITY_A]);
+  }
+
+  const SHRigidBodyComponent* SHCollisionKey::GetRigidBodyB() const noexcept
+  {
+    return SHComponentManager::GetComponent_s<SHRigidBodyComponent>(ids[ENTITY_B]);
+  }
+
+  const SHCollisionShape* SHCollisionKey::GetCollisionShapeA() const noexcept
+  {
+    const auto* COLLIDER_COMPONENT = SHComponentManager::GetComponent<SHColliderComponent>(ids[ENTITY_A]);
+    return COLLIDER_COMPONENT->GetCollider()->GetCollisionShape(ids[SHAPE_INDEX_A]);
+  }
+
+  const SHCollisionShape* SHCollisionKey::GetCollisionShapeB() const noexcept
+  {
+    const auto* COLLIDER_COMPONENT = SHComponentManager::GetComponent<SHColliderComponent>(ids[ENTITY_B]);
+    return COLLIDER_COMPONENT->GetCollider()->GetCollisionShape(ids[SHAPE_INDEX_B]);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHCollisionKey::SetEntityA(EntityID entityID) noexcept
+  {
+    ids[ENTITY_A] = entityID;
+  }
+
+  void SHCollisionKey::SetEntityB(EntityID entityID) noexcept
+  {
+    ids[ENTITY_B] = entityID;
+  }
+
+  void SHCollisionKey::SetCollisionShapeA(uint32_t shapeIndexA) noexcept
+  {
+    ids[SHAPE_INDEX_A] = shapeIndexA;
+  }
+
+  void SHCollisionKey::SetCollisionShapeB(uint32_t shapeIndexB) noexcept
+  {
+    ids[SHAPE_INDEX_B] = shapeIndexB;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Function Member Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  std::size_t SHCollisionKeyHash::operator()(const SHCollisionKey& id) const
+  {
+    static constexpr int NUM_IDS = ARRAYSIZE(id.ids);
+
+    // Hashable is not a word. Sue me.
+    auto hashablePtr = reinterpret_cast<std::basic_string_view<char32_t>::const_pointer>(id.ids);
+    return std::hash<std::u32string_view>{}(std::u32string_view(hashablePtr, NUM_IDS));
+  }
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionKey.h

@@ -1,7 +1,7 @@
 /****************************************************************************************
- * \file      SHPhysicsObject.h
+ * \file      SHCollisionID.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a Physics Object.
+ * \brief     Interface for Collision Information for Collision & Triggers.
  *
  * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
  *            disclosure of this file or its contents without the prior written consent
@@ -10,102 +10,108 @@
 
 #pragma once
 
-#include <reactphysics3d/reactphysics3d.h>
-
 // Project Headers
-#include "Math/Transform/SHTransformComponent.h"
-#include "Physics/Interface/SHRigidBodyComponent.h"
 #include "Physics/Interface/SHColliderComponent.h"
+#include "Physics/Interface/SHRigidBodyComponent.h"
+
 
 namespace SHADE
 {
+  /*-----------------------------------------------------------------------------------*/
+  /* Forward Declarations                                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  struct SHCollisionKeyHash;
+
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  class SH_API SHPhysicsObject
+  /**
+   * @brief
+   * Encapsulates the information when two collision shapes intersect.
+  */
+  class SH_API SHCollisionKey
   {
   private:
     /*---------------------------------------------------------------------------------*/
     /* Friends                                                                         */
     /*---------------------------------------------------------------------------------*/
 
-    friend class SHPhysicsSystem;
+    friend struct SHCollisionKeyHash;
-    friend class SHPhysicsObjectManager;
 
   public:
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHPhysicsObject           (EntityID eid, rp3d::PhysicsCommon* physicsFactory, rp3d::PhysicsWorld* physicsWorld) noexcept;
+    SHCollisionKey   ()                          noexcept;
-    SHPhysicsObject           (const SHPhysicsObject& rhs)  noexcept = default;
+    SHCollisionKey   (const SHCollisionKey& rhs)  noexcept;
-    SHPhysicsObject           (SHPhysicsObject&& rhs)       noexcept = default;
+    SHCollisionKey   (SHCollisionKey&& rhs)       noexcept;
-    virtual ~SHPhysicsObject  ()                            noexcept;
+
+    ~SHCollisionKey  () noexcept = default;
 
     /*---------------------------------------------------------------------------------*/
     /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
 
-    SHPhysicsObject& operator=(const SHPhysicsObject& rhs)  noexcept = default;
+    SHCollisionKey& operator= (const SHCollisionKey& rhs) noexcept;
-    SHPhysicsObject& operator=(SHPhysicsObject&& rhs)       noexcept = default;
+    SHCollisionKey& operator= (SHCollisionKey&& rhs)      noexcept;
+
+    bool operator==(const SHCollisionKey& rhs) const;
 
     /*---------------------------------------------------------------------------------*/
     /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    [[nodiscard]] SHVec3                GetPosition       ()  const noexcept;
+    [[nodiscard]] EntityID                    GetEntityA          ()  const noexcept;
-    [[nodiscard]] SHQuaternion          GetOrientation    ()  const noexcept;
+    [[nodiscard]] EntityID                    GetEntityB          ()  const noexcept;
-    [[nodiscard]] SHVec3                GetRotation       ()  const noexcept;
+    [[nodiscard]] uint32_t                    GetShapeIndexA      ()  const noexcept;
-
+    [[nodiscard]] uint32_t                    GetShapeIndexB      ()  const noexcept;
-    [[nodiscard]] rp3d::CollisionBody*  GetCollisionBody  ()  const noexcept;
+    [[nodiscard]] const SHRigidBodyComponent* GetRigidBodyA       ()  const noexcept;
-    [[nodiscard]] rp3d::RigidBody*      GetRigidBody      ()  const noexcept;
+    [[nodiscard]] const SHRigidBodyComponent* GetRigidBodyB       ()  const noexcept;
+    [[nodiscard]] const SHCollisionShape*     GetCollisionShapeA  ()  const noexcept;
+    [[nodiscard]] const SHCollisionShape*     GetCollisionShapeB  ()  const noexcept;
 
     /*---------------------------------------------------------------------------------*/
     /* Setter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    void  SetStaticBody () const noexcept;
+    void SetEntityA         (EntityID entityID)     noexcept;
-
+    void SetEntityB         (EntityID entityID)     noexcept;
-    /*---------------------------------------------------------------------------------*/
+    void SetCollisionShapeA (uint32_t shapeIndexA)  noexcept;
-    /* Function Members                                                                */
+    void SetCollisionShapeB (uint32_t shapeIndexB)  noexcept;
-    /*---------------------------------------------------------------------------------*/
-
-    int   AddCollisionShape         (int index);
-    void  RemoveCollisionShape      (int index);
-    void  RemoveAllCollisionShapes  () const noexcept;
-
-    void  SyncRigidBody             (SHRigidBodyComponent& component) const noexcept;
-    void  SyncColliders             (SHColliderComponent& component)  const noexcept;
 
   private:
+
+    static constexpr uint32_t ENTITY_A      = 0;
+    static constexpr uint32_t SHAPE_INDEX_A = 1;
+    static constexpr uint32_t ENTITY_B      = 2;
+    static constexpr uint32_t SHAPE_INDEX_B = 3;
+
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    EntityID              entityID;
+    union
-    bool                  collidersActive;  // Only used to sync with SHADE components
+    {
-
+      uint64_t value[2];  // EntityValue, ShapeIndexValue
-    rp3d::PhysicsCommon*  factory;
+      uint32_t ids  [4];  // EntityA, EntityB, ShapeIndexA, ShapeIndexB
-    rp3d::PhysicsWorld*   world;
-
-    rp3d::RigidBody*      rp3dBody;
-    rp3d::Transform       prevTransform;    // Cached transform for interpolation
-
-    /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    void syncColliderProperties (const SHCollisionShape& collisionShape, rp3d::Collider* rp3dCollider) const noexcept;
-
-    // Box Shapes
-
-    void addBoxShape            (const SHCollisionShape& boxShape) const noexcept;
-    void syncBoxShape           (int index, const SHCollisionShape& boxShape) const noexcept;
-
-    // Sphere Shapes
-
-    void addSphereShape         (const SHCollisionShape& sphereShape) const noexcept;
-    void syncSphereShape        (int index, const SHCollisionShape& sphereShape) const noexcept;
     };
+  };
+
+  /**
+   * @brief
+   * Encapsulates a functor to hash a CollisionKey
+  */
+  struct SHCollisionKeyHash
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    std::size_t operator()(const SHCollisionKey& id) const;
+  };
+
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h

@@ -0,0 +1,82 @@
+/****************************************************************************************
+ * \file      SHManifold.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Collision Manifold
+ *
+ * \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
+
+// Primary Header
+#include "Physics/Dynamics/SHRigidBody.h"
+#include "Physics/Collision/Shapes/SHCollisionShape.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates a value that represents the touching features of a contact.
+  */
+  union SHContactFeatures
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definit                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    enum class Type : uint8_t
+    {
+      VERTEX  = 0
+    , FACE    = 1
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    struct
+    {
+      uint8_t indexA;
+      uint8_t indexB;
+      uint8_t typeA;
+      uint8_t typeB;
+    };
+
+    uint32_t key = std::numeric_limits<uint32_t>::max();
+  };
+
+  /**
+   * @brief
+   * Encapsulates a physical collision contact.
+  */
+  struct SH_API SHContact
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    static constexpr int NUM_TANGENTS = 2;
+
+    float             penetration                   = 0.0f;
+    float             bias                          = 0.0f;     // Restitution + Baumguarte factor
+    float             normalImpulse                 = 0.0f;     // Accumulated normal impulse
+    float             normalMass                    = 0.0f;     // Effective mass along the normal
+    float             tangentImpulse[NUM_TANGENTS]  = { 0.0f }; // Accumulated tangent impulses
+    float             tangentMass[NUM_TANGENTS]     = { 0.0f }; // Effective masses along the tangents
+
+    SHVec3            position;
+    SHVec3            rA;                                       // Vector from COM of A to the contact
+    SHVec3            rB;                                       // Vector from COM of B to the contact
+    SHContactFeatures featurePair;
+  };
+}
+
+#pragma once

SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h

@@ -1,7 +1,7 @@
 /****************************************************************************************
- * \file      SHPhysicsWorld.h
+ * \file      SHManifold.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a Physics World.
+ * \brief     Interface for a Collision Manifold
  *
  * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
  *            disclosure of this file or its contents without the prior written consent
@@ -10,11 +10,11 @@
 
 #pragma once
 
-#include <reactphysics3d/reactphysics3d.h>
+// Primary Header
-
+#include "Physics/Dynamics/SHRigidBody.h"
-// Project Headers
+#include "Physics/Collision/Shapes/SHCollisionShape.h"
-#include "Math/SHMath.h"
+#include "SHContact.h"
-#include "SH_API.h"
+#include "SHCollisionEvents.h"
 
 namespace SHADE
 {
@@ -22,53 +22,49 @@ namespace SHADE
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  struct SH_API SHPhysicsWorldState
+  struct SH_API SHManifold
   {
   public:
-
-    /*---------------------------------------------------------------------------------*/
-    /* Type Definitions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    struct WorldSettings
-    {
-    public:
-      /*-------------------------------------------------------------------------------*/
-      /* Data Members                                                                  */
-      /*-------------------------------------------------------------------------------*/
-
-      SHVec3    gravity                     = SHVec3{ 0.0f, -9.81f, 0.0f };
-      uint16_t  numVelocitySolverIterations = 10;
-      uint16_t  numPositionSolverIterations = 5;
-      bool      sleepingEnabled             = true;
-    };
-
-    /*---------------------------------------------------------------------------------*/
-    /* Data Members                                                                    */
-    /*---------------------------------------------------------------------------------*/
-
-    rp3d::PhysicsWorld* world;
-    WorldSettings       settings;
-
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHPhysicsWorldState() noexcept;
+    SHManifold  (SHCollisionShape* a, SHCollisionShape* b)  noexcept;
+    SHManifold  (const SHManifold& rhs)                     noexcept;
+    SHManifold  (SHManifold&& rhs)                          noexcept;
+
+    ~SHManifold () noexcept = default;
 
     /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
+    /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
 
-    void CreateWorld    (rp3d::PhysicsCommon& factory);
+    SHManifold& operator=(const SHManifold& rhs)  noexcept;
-    void DestroyWorld   (rp3d::PhysicsCommon& factory);
+    SHManifold& operator=(SHManifold&& rhs)       noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    // We only need 4 contact points to build a stable manifold.
+    static constexpr int MAX_NUM_CONTACTS = 4;
+
+    SHCollisionShape* shapeA;
+    SHCollisionShape* shapeB;
+
+    SHRigidBody*      bodyA       = nullptr;
+    SHRigidBody*      bodyB       = nullptr;
+
+    uint32_t          numContacts = 0;
+    SHCollisionState  state       = SHCollisionState::INVALID;
+
+    SHVec3            normal;
+    SHVec3            tangents[SHContact::NUM_TANGENTS];
+    SHContact         contacts[MAX_NUM_CONTACTS];
     
-    /**
-     * @brief Applies the current settings to the physics world. The world must be created
-     *        before this is called.
-    */
-    void UpdateSettings () const noexcept;
   };
   
-
 } // namespace SHADE
+
+#include "SHManifold.hpp"
+

SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp

@@ -0,0 +1,106 @@
+/****************************************************************************************
+ * \file      SHManifold.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Collision Manifold
+ *
+ * \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
+
+// Primary Header
+#include "SHManifold.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  inline SHManifold::SHManifold(SHCollisionShape* a, SHCollisionShape* b) noexcept
+  : shapeA  { a }
+  , shapeB  { b }
+  {
+    bodyA = shapeA->collider->rigidBody;
+    bodyB = shapeB->collider->rigidBody;
+  }
+
+  inline SHManifold::SHManifold(const SHManifold& rhs) noexcept
+  : shapeA      { rhs.shapeA }
+  , shapeB      { rhs.shapeB }
+  , bodyA       { rhs.bodyA }
+  , bodyB       { rhs.bodyB }
+  , numContacts { rhs.numContacts }
+  , state       { rhs.state }
+  , normal      { rhs.normal }
+  {
+    static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
+    memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
+
+    for (int i = 0; i < SHContact::NUM_TANGENTS; ++i)
+      tangents[i] = rhs.tangents[i];
+  }
+
+  inline SHManifold::SHManifold(SHManifold&& rhs) noexcept
+  : shapeA      { rhs.shapeA }
+  , shapeB      { rhs.shapeB }
+  , bodyA       { rhs.bodyA }
+  , bodyB       { rhs.bodyB }
+  , numContacts { rhs.numContacts }
+  , state       { rhs.state }
+  , normal      { rhs.normal }
+  {
+    static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
+    memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
+
+    for (int i = 0; i < SHContact::NUM_TANGENTS; ++i)
+      tangents[i] = rhs.tangents[i];
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+  
+  inline SHManifold& SHManifold::operator=(const SHManifold& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    shapeA      = rhs.shapeA;
+    shapeB      = rhs.shapeB;
+    bodyA       = rhs.bodyA;
+    bodyB       = rhs.bodyB;
+    numContacts = rhs.numContacts;
+    state       = rhs.state;
+    normal      = rhs.normal;
+
+    static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
+    memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
+
+    for (int i = 0; i < SHContact::NUM_TANGENTS; ++i)
+      tangents[i] = rhs.tangents[i];
+
+    return *this;
+  }
+
+  inline SHManifold& SHManifold::operator=(SHManifold&& rhs) noexcept
+  {
+    shapeA      = rhs.shapeA;
+    shapeB      = rhs.shapeB;
+    bodyA       = rhs.bodyA;
+    bodyB       = rhs.bodyB;
+    numContacts = rhs.numContacts;
+    state       = rhs.state;
+    normal      = rhs.normal;
+
+    static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
+    memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
+
+    for (int i = 0; i < SHContact::NUM_TANGENTS; ++i)
+      tangents[i] = rhs.tangents[i];
+
+    return *this;
+  }
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHCapsuleVsCapsule.cpp

@@ -1,7 +1,7 @@
 /****************************************************************************************
- * \file      SHShape.cpp
+ * \file      SHCapsuleVsCapsule.cpp
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Implementation for a shape.
+ * \brief     Implementation for the Detecting Collisions between two capsules
  *
  * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
  *            disclosure of this file or its contents without the prior written consent
@@ -11,25 +11,25 @@
 #include <SHpch.h>
 
 // Primary Header
-#include "SHShape.h"
+#include "SHCollision.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
 
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
-  /* Constructors & Destructor Definitions                                             */
+  /* Public Member Functions Definitions                                               */
   /*-----------------------------------------------------------------------------------*/
 
-  SHShape::SHShape()
+  bool SHCollision::CapsuleVsCapsule(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
-  : type { Type::NONE }
-  {}
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Getter Function Definitions                                                       */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHShape::Type SHShape::GetType() const noexcept
   {
-    return type;
+    return false;
+  }
+
+  bool SHCollision::CapsuleVsCapsule(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return false;
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHCapsuleVsConvex.cpp

@@ -0,0 +1,49 @@
+/****************************************************************************************
+ * \file      SHCapsuleVsConvex.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for the Detecting Collisions between a capsule and a convex
+ *            polyhedron.
+ *
+ * \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 "SHCollision.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
+
+// TODO
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHCollision::CapsuleVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return false;
+  }
+
+  bool SHCollision::ConvexVsCapsule(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return CapsuleVsConvex(B, A);
+  }
+
+  bool SHCollision::CapsuleVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return false;
+  }
+
+  bool SHCollision::ConvexVsCapsule(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return CapsuleVsConvex(manifold, B, A);
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h

@@ -0,0 +1,124 @@
+/****************************************************************************************
+ * \file      SHCollision.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for the Detecting Collisions between two shapes
+ *
+ * \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/Geometry/SHPlane.h"
+#include "Physics/Collision/Contacts/SHManifold.h"
+#include "Physics/Collision/Contacts/SHCollisionKey.h"
+#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h"
+
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief 
+   * Encapsulates static methods for testing for collision between two shapes.
+  */
+  class SH_API SHCollision
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /* Spheres VS X */
+
+    [[nodiscard]] static bool SphereVsSphere    (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool SphereVsSphere    (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+    [[nodiscard]] static bool SphereVsCapsule   (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool SphereVsCapsule   (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+    [[nodiscard]] static bool SphereVsConvex    (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool SphereVsConvex    (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+    /* Capsule VS X */
+
+    [[nodiscard]] static bool CapsuleVsSphere   (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool CapsuleVsSphere   (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+    [[nodiscard]] static bool CapsuleVsCapsule  (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool CapsuleVsCapsule  (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+    [[nodiscard]] static bool CapsuleVsConvex   (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool CapsuleVsConvex   (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+    /* Polygon VS X */
+
+    [[nodiscard]] static bool ConvexVsSphere    (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool ConvexVsSphere    (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+    [[nodiscard]] static bool ConvexVsCapsule   (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool ConvexVsCapsule   (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+    [[nodiscard]] static bool ConvexVsConvex    (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+    [[nodiscard]] static bool ConvexVsConvex    (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct FaceQuery
+    {
+      bool    colliding     = false;  // Allows for early out
+      int32_t closestFace   = -1;
+      float   bestDistance  = std::numeric_limits<float>::lowest();
+    };
+
+    struct EdgeQuery
+    {
+      int32_t halfEdgeA     = -1;
+      int32_t halfEdgeB     = -1;
+      float   bestDistance  = std::numeric_limits<float>::lowest();
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    // Sphere VS Convex
+
+    static FaceQuery  findClosestFace   (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron) noexcept;
+    static int32_t    findClosestPoint  (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron, int32_t faceIndex) noexcept;
+    static int32_t    findVoronoiRegion (const SHSphere& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept;
+
+    // Capsule VS Convex
+
+    // TODO: Capsule VS Convex uses the same gauss map optimisation as convex vs convex
+
+    // Convex VS Convex
+
+    /*
+     * References
+     *  https://ia801303.us.archive.org/30/items/GDC2013Gregorius/GDC2013-Gregorius.pdf
+     *  https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp
+     */
+
+    static FaceQuery              queryFaceDirections           (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
+    static EdgeQuery              queryEdgeDirections           (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
+
+    static bool                   buildMinkowskiFace            (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
+    static bool                   isMinkowskiFace               (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept;
+    static float                  distanceBetweenEdges          (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
+    static SHVec3                 findClosestPointBetweenEdges  (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
+    static int32_t                findIncidentFace              (const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept;
+    static std::vector<SHContact> clipPolygonAgainstPlane       (const std::vector<SHContact>& in, const SHPlane& plane, int32_t refIdx) noexcept;
+    static std::vector<SHContact> reduceContacts                (const std::vector<SHContact>& in, const SHVec3& normal) noexcept;
+
+    // TODO: Reduce Manifold
+  };
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionDispatch.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************************
+ * \file      SHCollisionDispatch.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for the static Collision Dispatcher
+ *
+ * \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 "SHCollisionDispatch.h"
+
+// Project Header
+#include "SHCollision.h"
+#include "Tools/Utilities/SHUtilities.h"
+
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Static Data Member Definitions                                                    */
+  /*-----------------------------------------------------------------------------------*/
+
+  const SHCollisionDispatcher::ManifoldCollide SHCollisionDispatcher::manifoldCollide[NUM_SHAPES][NUM_SHAPES]
+  {
+    // <SHAPE> vs Sphere / Box / Capsule
+
+    { SHCollision::SphereVsSphere,  SHCollision::SphereVsConvex,  SHCollision::SphereVsCapsule  }  // Sphere
+  , { SHCollision::ConvexVsSphere,  SHCollision::ConvexVsConvex,  SHCollision::ConvexVsCapsule  }  // Box
+  , { SHCollision::CapsuleVsSphere, SHCollision::CapsuleVsConvex, SHCollision::CapsuleVsCapsule }  // Capsule
+  };
+
+  const SHCollisionDispatcher::TriggerCollide SHCollisionDispatcher::triggerCollide[NUM_SHAPES][NUM_SHAPES]
+  {
+    // <SHAPE> vs Sphere / Box / Capsule
+
+    { SHCollision::SphereVsSphere,  SHCollision::SphereVsConvex,  SHCollision::SphereVsCapsule  }  // Sphere
+  , { SHCollision::ConvexVsSphere,  SHCollision::ConvexVsConvex,  SHCollision::ConvexVsCapsule  }  // Box
+  , { SHCollision::CapsuleVsSphere, SHCollision::CapsuleVsConvex, SHCollision::CapsuleVsCapsule }  // Capsule
+  };
+
+  const bool SHCollisionDispatcher::collisionTable[NUM_TYPES][NUM_TYPES]
+  {
+    /*          S       ST      K       KT      D       DT    */
+    /* S  */  { false,  false,  false,  true,   true,   true }
+  , /* ST */  { false,  false,  true,   true,   true,   true }
+  , /* K  */  { false,  true,   false,  true,   true,   true }
+  , /* KT */  { true,   true,   true,   true,   true,   true }
+  , /* D  */  { true,   true,   true,   true,   true,   true }
+  , /* DT */  { true,   true,   true,   true,   true,   true }
+  };
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHCollisionDispatcher::ShouldCollide(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    // Filter through collision table
+    const int TYPE_A = SHUtilities::ConvertEnum(A.GetType()) + A.IsTrigger() ? TYPE_OFFSET : 0;
+    const int TYPE_B = SHUtilities::ConvertEnum(B.GetType()) + B.IsTrigger() ? TYPE_OFFSET : 0;
+
+    if (!collisionTable[TYPE_A][TYPE_B])
+      return false;
+  }
+
+  bool SHCollisionDispatcher::Collide(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const int TYPE_A = SHUtilities::ConvertEnum(A.GetType());
+    const int TYPE_B = SHUtilities::ConvertEnum(B.GetType());
+
+    return manifoldCollide[TYPE_A][TYPE_B](manifold, A, B);
+  }
+
+  bool SHCollisionDispatcher::Collide(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const int TYPE_A = SHUtilities::ConvertEnum(A.GetType());
+    const int TYPE_B = SHUtilities::ConvertEnum(B.GetType());
+
+    return triggerCollide[TYPE_A][TYPE_B](A, B);
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionDispatch.h

@@ -0,0 +1,87 @@
+/****************************************************************************************
+ * \file      SHCollisionDispatch.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for the static Collision Dispatcher
+ *
+ * \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/Collision/Contacts/SHManifold.h"
+#include "Physics/Collision/Contacts/SHCollisionKey.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates static methods for running narrow-phase collision detection.
+  */
+  class SH_API SHCollisionDispatcher
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Filters the collision through the collision table and layer matching.
+     * @param A
+     * A Collision Shape.
+     * @param B
+     * A Collision Shape.
+     * @return
+     * True if both shapes should be tested for collision.
+    */
+    static bool ShouldCollide (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+
+    static bool Collide       (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
+    static bool Collide       (const SHCollisionShape& A, const SHCollisionShape& B)                        noexcept;
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    using ManifoldCollide = bool(*)(SHManifold&, const SHCollisionShape& A, const SHCollisionShape& B);
+    using TriggerCollide  = bool(*)(const SHCollisionShape& A, const SHCollisionShape& B);
+
+    enum class Types
+    {
+      STATIC   
+    , KINEMATIC
+    , DYNAMIC
+    , STATIC_TRIGGER
+    , KINEMATIC_TRIGGER
+    , DYNAMIC_TRIGGER
+
+    , COUNT
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    // Read the Types enum class, then see where it's used and it'll make sense
+    static constexpr int TYPE_OFFSET  = 3;
+
+    static constexpr int NUM_SHAPES   = static_cast<int>(SHCollisionShape::Type::COUNT);
+    static constexpr int NUM_TYPES    = static_cast<int>(Types::COUNT);
+
+    static const ManifoldCollide  manifoldCollide [NUM_SHAPES][NUM_SHAPES];
+    static const TriggerCollide   triggerCollide  [NUM_SHAPES][NUM_SHAPES];
+
+    static const bool             collisionTable  [NUM_TYPES][NUM_TYPES];
+
+  };
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp

@@ -0,0 +1,618 @@
+/****************************************************************************************
+ * \file      SHConvexVsConvex.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for the Detecting Collisions between two convex polyhedrons.
+ *
+ * \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 "SHCollision.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Math/Geometry/SHPlane.h"
+#include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
+#include "Physics/SHPhysicsConstants.h"
+#include "Tools/Utilities/SHUtilities.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHCollision::ConvexVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
+    const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
+
+    const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
+    if (FACE_QUERY_A.bestDistance > 0.0f)
+      return false;
+
+    const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A);
+    if (FACE_QUERY_B.bestDistance > 0.0f)
+      return false;
+
+    const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B);
+    if (EDGE_QUERY.bestDistance > 0.0f)
+      return false;
+
+    return true;
+  }
+
+  bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    static constexpr float TOLERANCE  = 0.1f + SHPHYSICS_LINEAR_SLOP;
+
+    const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
+    const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
+
+    const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
+    if (FACE_QUERY_A.bestDistance > 0.0f)
+      return false;
+
+    const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A);
+    if (FACE_QUERY_B.bestDistance > 0.0f)
+      return false;
+
+    const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B);
+    if (EDGE_QUERY.bestDistance > 0.0f)
+      return false;
+
+    // Apply weight to improve frame coherence of normal directions.
+    // We want a normal in the direction from A -> B, so we flip the normal if needed.
+    bool                      flipNormal    = false;
+    const SHConvexPolyhedron* referencePoly = nullptr;
+    const SHConvexPolyhedron* incidentPoly  = nullptr;
+
+    FaceQuery minFaceQuery;
+    if (FACE_QUERY_A.bestDistance + TOLERANCE > FACE_QUERY_B.bestDistance)
+    {
+      minFaceQuery  = FACE_QUERY_A;
+      referencePoly = &POLY_A;
+      incidentPoly  = &POLY_B;
+    }
+    else
+    {
+      minFaceQuery  = FACE_QUERY_B;
+      referencePoly = &POLY_B;
+      incidentPoly  = &POLY_A;
+      flipNormal    = true;
+    }
+
+    uint32_t numContacts = 0;
+
+
+    // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
+    // each edge and use that as the contact point.
+    if (EDGE_QUERY.bestDistance > minFaceQuery.bestDistance + TOLERANCE)
+    {
+      const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
+      const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
+
+      const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex);
+      const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex);
+      const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex);
+      const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex);
+
+      const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
+      const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
+
+      manifold.normal = SHVec3::Cross(VB, VA);
+      // Flip normal if need to ( A -> B)
+      if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
+        manifold.normal = -manifold.normal;
+
+      // In this scenario, we only have one contact
+      SHContact contact;
+      contact.featurePair.typeA   = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+      contact.featurePair.indexA  = HALF_EDGE_A.tailVertexIndex;
+      contact.featurePair.typeB   = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+      contact.featurePair.indexB  = HALF_EDGE_B.tailVertexIndex;
+
+      contact.position    = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
+      contact.penetration = EDGE_QUERY.bestDistance;
+
+      manifold.contacts[numContacts++] = contact;
+      manifold.numContacts = numContacts;
+
+      return true;
+    }
+
+    const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
+    const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
+
+    // Create initial set of contacts as the entire incident face
+    const SHHalfEdgeStructure::Face& INCIDENT_FACE = incidentPoly->GetFace(INCIDENT_FACE_IDX);
+    const int32_t MAX_NUM_CONTACTS = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
+
+    std::vector<SHContact> incidentPolygon;
+    for (const int32_t i : std::views::iota(0, MAX_NUM_CONTACTS))
+    {
+      SHContact contact;
+
+      const int32_t VERTEX_INDEX = INCIDENT_FACE.vertexIndices[i].index;
+
+      contact.position = incidentPoly->GetVertex(VERTEX_INDEX);
+      contact.featurePair.indexA = minFaceQuery.closestFace;
+      contact.featurePair.indexB = VERTEX_INDEX;
+      contact.featurePair.typeA  = SHUtilities::ConvertEnum(SHContactFeatures::Type::FACE);
+      contact.featurePair.typeB  = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+
+      incidentPolygon.emplace_back(contact);
+    }
+
+    // Number of vertices == number of edges on face
+    // Clip against each edge of face
+
+    int32_t numClipped = 0;
+
+    const SHHalfEdgeStructure::Face& REFERENCE_FACE = referencePoly->GetFace(minFaceQuery.closestFace);
+    const int32_t NUM_EDGES = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
+
+    for (const int32_t i : std::views::iota(0, NUM_EDGES))
+    {
+      // Build a plane on the reference poly normal X tangent and the tail of the tangent
+      const int32_t V1_IDX = REFERENCE_FACE.vertexIndices[i].index;
+      const int32_t V2_IDX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_EDGES].index;
+
+      const SHVec3 V1 = referencePoly->GetVertex(V1_IDX);
+      const SHVec3 V2 = referencePoly->GetVertex(V2_IDX);
+
+      const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1);
+      const SHPlane PLANE{ V1, SHVec3::Cross(REFERENCE_NORMAL, TANGENT) };
+
+      auto clipped = clipPolygonAgainstPlane(incidentPolygon, PLANE, V1_IDX);
+      numClipped = static_cast<int32_t>(clipped.size());
+
+      // Resize the reused container to hold all the clipped points.
+      if (incidentPolygon.size() < clipped.size())
+      {
+        incidentPolygon.clear();
+        incidentPolygon.resize(numClipped);
+      }
+
+      memcpy_s(incidentPolygon.data(), sizeof(SHContact) * numClipped, clipped.data(), sizeof(SHContact) * numClipped);
+    }
+
+    // From clipped polygon, only keep points that are below the reference face
+    // The penetration is the signed distance to the plane formed by the reference face.
+    const SHPlane REFERENCE_PLANE{ referencePoly->GetVertex(REFERENCE_FACE.vertexIndices[0].index), REFERENCE_NORMAL };
+
+    std::vector<SHContact> finalContacts;
+
+    for (const auto& cp : incidentPolygon)
+    {
+      const float DEPTH = REFERENCE_PLANE.SignedDistance(cp.position);
+      if (DEPTH <= 0.0f)
+      {
+        SHContact contact;
+        contact.position        = cp.position;
+        contact.penetration     = -DEPTH;
+        contact.featurePair.key = cp.featurePair.key;
+
+        if (flipNormal)
+        {
+          contact.featurePair.indexA  = cp.featurePair.indexB;
+          contact.featurePair.indexB  = cp.featurePair.indexA;
+          contact.featurePair.typeA   = cp.featurePair.typeB;
+          contact.featurePair.typeB   = cp.featurePair.typeA;
+        }
+
+        finalContacts.emplace_back(contact);
+        ++numContacts;
+      }
+    }
+
+    if (numContacts > 4)
+    {
+      auto reducedSet = reduceContacts(finalContacts, REFERENCE_NORMAL);
+      numContacts = 4;
+
+      memcpy_s(manifold.contacts, sizeof(SHContact) * numContacts, reducedSet.data(), sizeof(SHContact) * numContacts);
+    }
+    else
+    {
+      memcpy_s(manifold.contacts, sizeof(SHContact) * numContacts, finalContacts.data(), sizeof(SHContact) * numContacts);
+    }
+
+    manifold.normal       = flipNormal ? -REFERENCE_NORMAL : REFERENCE_NORMAL;
+    manifold.numContacts  = numContacts;
+
+    return true;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollision::FaceQuery SHCollision::queryFaceDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
+  {
+    FaceQuery faceQuery;
+
+    const int32_t NUM_FACES = A.GetFaceCount();
+    for (const int32_t i : std::views::iota(0, NUM_FACES))
+    {
+      const SHHalfEdgeStructure::Face& FACE_A = A.GetFace(i);
+      const SHVec3 NORMAL_A = A.GetNormal(i);
+
+      // Smallest penetration is point closest to face normal
+      const SHVec3  SUPPORT_POINT = B.FindSupportPoint(-NORMAL_A);
+
+      const SHVec3  VERTEX_A = A.GetVertex(FACE_A.vertexIndices[0].index);
+
+      const SHPlane FACE_PLANE { VERTEX_A, NORMAL_A };
+      const float   DISTANCE = FACE_PLANE.SignedDistance(SUPPORT_POINT);
+
+      if (DISTANCE > faceQuery.bestDistance)
+      {
+        faceQuery.bestDistance = DISTANCE;
+        faceQuery.closestFace = i;
+      }
+    }
+
+    return faceQuery;
+  }
+
+  SHCollision::EdgeQuery SHCollision::queryEdgeDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
+  {
+    EdgeQuery edgeQuery;
+
+    const int32_t EDGE_COUNT_A = A.GetHalfEdgeCount();
+    const int32_t EDGE_COUNT_B = B.GetHalfEdgeCount();
+
+    for (int32_t i = 0; i < EDGE_COUNT_A; i += 2)
+    {
+      for (int32_t j = 0; j < EDGE_COUNT_B; j += 2)
+      {
+        const bool IS_MINKOWSKI_FACE = buildMinkowskiFace(A, B, i, j);
+        if (!IS_MINKOWSKI_FACE)
+          continue;
+
+        const float SEPARATION = distanceBetweenEdges(A, B, i, j);
+        if (SEPARATION > edgeQuery.bestDistance)
+        {
+          edgeQuery.bestDistance = SEPARATION;
+          edgeQuery.halfEdgeA = i;
+          edgeQuery.halfEdgeB = j;
+        }
+      }
+    }
+
+    return edgeQuery;
+  }
+
+  bool SHCollision::buildMinkowskiFace(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept
+  {
+    // Get Half Edge from both polygons
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
+    const SHHalfEdgeStructure::HalfEdge& TWIN_EDGE_A = A.GetHalfEdge(HALF_EDGE_A.twinEdgeIndex);
+
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB);
+    const SHHalfEdgeStructure::HalfEdge& TWIN_EDGE_B = B.GetHalfEdge(HALF_EDGE_B.twinEdgeIndex);
+
+
+
+    // Get normals from face and twin edge face
+    const SHVec3 NA = A.GetNormal(HALF_EDGE_A.faceIndex);
+    const SHVec3 NB = A.GetNormal(TWIN_EDGE_A.faceIndex);
+    const SHVec3 NC = B.GetNormal(HALF_EDGE_B.faceIndex);
+    const SHVec3 ND = B.GetNormal(TWIN_EDGE_B.faceIndex);
+
+    return isMinkowskiFace(NA, NB, -NC, -ND);
+  }
+
+  bool SHCollision::isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept
+  {
+    const SHVec3 BXA = SHVec3::Cross(b, a);
+    const SHVec3 DXC = SHVec3::Cross(d, c);
+
+    const float CBA = SHVec3::Dot(c, BXA);
+    const float DBA = SHVec3::Dot(d, BXA);
+    const float ADC = SHVec3::Dot(a, DXC);
+    const float BDC = SHVec3::Dot(b, DXC);
+
+    return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f;
+  }
+
+  float SHCollision::distanceBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept
+  {
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB);
+
+    const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex);
+    const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex);
+    const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex);
+    const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex);
+
+    const SHVec3 DIR_A = SHVec3::Normalise(HEAD_A - TAIL_A);
+    const SHVec3 DIR_B = SHVec3::Normalise(HEAD_B - TAIL_B);
+
+    // Check if the edges are parallel (abs dot product is 1)
+    const float DOT_BETWEEN_EDGES = std::fabs(SHVec3::Dot(DIR_A, DIR_B));
+    if (SHMath::CompareFloat(DOT_BETWEEN_EDGES, 1.0f))
+      return std::numeric_limits<float>::lowest();
+
+    SHVec3 normal = SHVec3::Cross(DIR_A, DIR_B);
+    // Flip normal if need to ( A -> B)
+    if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
+      normal = -normal;
+
+    return SHVec3::Dot(normal, HEAD_B - HEAD_A);
+  }
+
+  SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept
+  {
+    /*
+     * The two edges can be parameterised in the form p + tv
+     *
+     * LA(r) = A + rVA
+     * LB(s) = B + sVB
+     *
+     * The vector between the closest points is the cross product of VA and VB.
+     * Since the cross product is orthogonal to VA and VB, we can generalise this as:
+     * (LB(s) - LA(r)) /dot VA = 0
+     * (LB(s) - LA(r)) /dot VB = 0
+     *
+     * Where LB(s) - LA(r) is the same vector as VB X VA.
+     */
+
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB);
+
+    const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex);
+    const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex);
+    const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex);
+    const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex);
+
+    const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
+    const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
+
+    /*
+     * Find a1, a2, b1, b2, c1, c2 to solve the simultaneous equation
+     *
+     * C' = TAIL_B - TAIL_A
+     *
+     * a =  VB /dot U
+     * b = -VA /dot U
+     * c =  C' /dot U
+     *
+     * U is either VA or VB
+     */
+
+    const SHVec3 C = TAIL_B - TAIL_A;
+
+    const float VB_DOT_VA = SHVec3::Dot(VB, VA);  // a1
+    const float VB_DOT_VB = SHVec3::Dot(VB, VB);  // a2
+    const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1
+    const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2
+    const float C_DOT_VA  = SHVec3::Dot(C, VA);   // c1
+    const float C_DOT_VB  = SHVec3::Dot(C, VB);   // c2
+
+    /*
+     * We only need to solve for R
+     * R = (c1a2 / a1 - c2) / ( b2 - a2b1/a1 )
+     */
+
+    const float A2_OVER_A1  = VB_DOT_VB / VB_DOT_VA;
+    const float NUMERATOR   = C_DOT_VA * A2_OVER_A1 - C_DOT_VB;
+    const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1;
+
+    const float R = NUMERATOR / DENOMINATOR;
+
+    // Just take a point from A since it's A -> B
+    return TAIL_A + R * VA;
+  }
+
+  int32_t SHCollision::findIncidentFace(const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept
+  {
+    // Get the most anti-parallel face to the normal
+    int32_t bestFace        = 0;
+    float   bestProjection  = std::numeric_limits<float>::max();
+
+    const int32_t NUM_FACES = poly.GetFaceCount();
+    for (const int32_t i : std::views::iota(0, NUM_FACES))
+    {
+      const SHVec3  INC_NORMAL = poly.GetNormal(i);
+      const float   PROJECTION = SHVec3::Dot(INC_NORMAL, normal);
+
+      if (PROJECTION < bestProjection)
+      {
+        bestProjection  = PROJECTION;
+        bestFace        = i;
+      }
+    }
+
+    return bestFace;
+  }
+
+  std::vector<SHContact> SHCollision::clipPolygonAgainstPlane(const std::vector<SHContact>& in, const SHPlane& plane, int32_t refIdx) noexcept
+  {
+    static constexpr float THRESHOLD = -SHMath::EPSILON;
+
+    std::vector<SHContact> out;
+
+    // 
+    // If both are in front of the plane, keep the head.
+    // If both are behind the plane, ignore both.
+    // If they are on different sides, find the position in between that lies on the plane. Keep the head and interpolation position.
+
+    /*
+     *  For sutherland-hodgman clipping, we check where any two sets of points lie with respect to the plane
+     *  This variation is has 4 scenarios:
+     *
+     *  1. Both behind: ignore
+     *  2. Both in-front: Keep tail
+     *  3. Head behind, tail in-front: Keep interpolated position
+     *  4. Head in-front, tail behind: Keep head & interpolated position
+     */
+
+    const size_t NUM_POINTS = in.size();
+    for (size_t i = 0; i < NUM_POINTS; ++i)
+    {
+      const SHContact& TAIL = in[i];
+      const SHContact& HEAD = in[(i + 1) % NUM_POINTS];
+
+      const float TAIL_DISTANCE = plane.SignedDistance(TAIL.position);
+      const float HEAD_DISTANCE = plane.SignedDistance(HEAD.position);
+
+      // Scenario 1: Both Behind
+      if (HEAD_DISTANCE < THRESHOLD && TAIL_DISTANCE < THRESHOLD)
+        continue;
+
+      // Scenario 2: Both In-Front
+      if (HEAD_DISTANCE >= THRESHOLD && TAIL_DISTANCE >= THRESHOLD)
+        out.emplace_back(TAIL);
+
+      // Scenario 3: Head In-Front, Tail Behind
+      if (HEAD_DISTANCE >= THRESHOLD && TAIL_DISTANCE < THRESHOLD)
+      {
+        SHContact interpolated;
+
+        interpolated.featurePair.indexA = refIdx;
+        interpolated.featurePair.indexB = TAIL.featurePair.indexB;
+        interpolated.featurePair.typeA  = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+        interpolated.featurePair.typeB  = SHUtilities::ConvertEnum(SHContactFeatures::Type::FACE);
+
+        const SHVec3 DIR = SHVec3::Normalise(HEAD.position - TAIL.position);
+        interpolated.position = TAIL.position + DIR * std::fabs(TAIL_DISTANCE);
+
+        out.emplace_back(interpolated);
+      }
+
+      // Scenario 4: Head Behind, Tail In-Front
+      if (HEAD_DISTANCE < THRESHOLD && TAIL_DISTANCE >= THRESHOLD)
+      {
+        out.emplace_back(TAIL);
+
+        SHContact interpolated;
+
+        interpolated.featurePair.indexA = refIdx;
+        interpolated.featurePair.indexB = TAIL.featurePair.indexB;
+        interpolated.featurePair.typeA  = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+        interpolated.featurePair.typeB  = SHUtilities::ConvertEnum(SHContactFeatures::Type::FACE);
+
+        const SHVec3 DIR = SHVec3::Normalise(HEAD.position - TAIL.position);
+        interpolated.position = TAIL.position + DIR * std::fabs(TAIL_DISTANCE);
+
+        out.emplace_back(interpolated);
+      }
+    }
+
+    return out;
+  }
+
+  std::vector<SHContact> SHCollision::reduceContacts(const std::vector<SHContact>& in, const SHVec3& normal) noexcept
+  {
+    std::vector<SHContact> out;
+    std::unordered_map<int, SHVec3> contactMap;
+
+    const int NUM_IN = static_cast<int>(in.size());
+    for (const int i : std::views::iota(0, NUM_IN))
+      contactMap.emplace(i, in[i].position);
+
+    // We'll be reusing these in all 4 phases
+    float bestMetric  = std::numeric_limits<float>::lowest();
+    int   bestVertex  = -1;
+
+    // Find the furthest point alone the -Z-axis
+    for (const auto& [index, pos] : contactMap)
+    {
+      const float PROJECTION = SHVec3::Dot(pos, -SHVec3::UnitZ);
+
+      if (PROJECTION > bestMetric)
+      {
+        bestMetric = PROJECTION;
+        bestVertex = index;
+      }
+    }
+
+    const SHVec3& A = out.emplace_back(in[bestVertex]).position;
+    contactMap.erase(bestVertex);
+
+    // Reset
+    bestMetric  = std::numeric_limits<float>::lowest();
+    bestVertex  = -1;
+
+    // Find farthest point
+    for (const auto& [index, pos] : contactMap)
+    {
+      const float DISTANCE = SHVec3::DistanceSquared(A, pos);
+      if (DISTANCE > bestMetric)
+      {
+        bestMetric = DISTANCE;
+        bestVertex = index;
+      }
+    }
+
+    const SHVec3& B = out.emplace_back(in[bestVertex]).position;
+    contactMap.erase(bestVertex);
+
+    // Reset
+    bestMetric  = std::numeric_limits<float>::lowest();
+    bestVertex  = -1;
+
+    // Find next point with largest area
+    // Largest area can be found as 0.5 * crossProduct /dot normal
+    for (const auto& [index, pos] : contactMap)
+    {
+      const SHVec3 CA = A - pos;
+      const SHVec3 CB = B - pos;
+
+      const float AREA = SHVec3::Dot(0.5f * SHVec3::Cross(CA, CB), normal);
+
+      // We only want triangles with negative areas!
+      if (AREA > bestMetric)
+      {
+        bestMetric = AREA;
+        bestVertex = index;
+      }
+    }
+
+    const SHVec3& C = out.emplace_back(in[bestVertex]).position;
+    contactMap.erase(bestVertex);
+
+    const float CURRENT_AREA = bestMetric;
+
+    // Reset
+    bestMetric  = std::numeric_limits<float>::lowest();
+    bestVertex  = -1;
+
+    // Find the contact point which forms the largest polygon
+    // We need to construct 3 triangles and compare them
+    for (const auto& [index, pos] : contactMap)
+    {
+      const SHVec3 DA = A - pos;
+      const SHVec3 DB = B - pos;
+      const SHVec3 DC = C - pos;
+
+      float areas[3] =
+      {
+        SHVec3::Dot(0.5f * SHVec3::Cross(DA, DB), normal) // DAB
+      , SHVec3::Dot(0.5f * SHVec3::Cross(DB, DC), normal) // DBC
+      , SHVec3::Dot(0.5f * SHVec3::Cross(DC, DA), normal) // DCA
+      };
+
+      for (int i = 0; i < 3; ++i)
+      {
+        if (areas[i] < 0.0f && areas[i] + CURRENT_AREA > bestMetric)
+        {
+          bestMetric = areas[i] + CURRENT_AREA;
+          bestVertex = index;
+        }
+      }
+    }
+
+    out.emplace_back(in[bestVertex]);
+
+    return out;
+  }
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsCapsule.cpp

@@ -0,0 +1,48 @@
+/****************************************************************************************
+ * \file      SHSphereVsCapsule.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for the Detecting Collisions between a sphere and a capsule.
+ *
+ * \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 "SHCollision.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Physics/Collision/Shapes/SHSphere.h"
+
+// TODO
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHCollision::SphereVsCapsule(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return false;
+  }
+
+  bool SHCollision::CapsuleVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return SphereVsCapsule(B, A);
+  }
+
+  bool SHCollision::SphereVsCapsule(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return false;
+  }
+
+  bool SHCollision::CapsuleVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return SphereVsCapsule(manifold, B, A);
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp

@@ -0,0 +1,349 @@
+/****************************************************************************************
+ * \file      SHSphereVsConvex.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for the Detecting Collisions between a sphere and a convex
+ *            polyhedron.
+ *
+ * \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 "SHCollision.h"
+
+// Project Headers
+#include "Math/Geometry/SHPlane.h"
+#include "Math/SHMathHelpers.h"
+#include "Physics/Collision/Shapes/SHSphere.h"
+#include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
+#include "Physics/SHPhysicsConstants.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHCollision::SphereVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const SHSphere&           SPHERE      = dynamic_cast<const SHSphere&>(A);
+    const SHConvexPolyhedron& POLYHEDRON  = dynamic_cast<const SHConvexPolyhedron&>(B);
+
+    const SHVec3  CENTER  = SPHERE.Center;
+    const float   RADIUS  = SPHERE.GetWorldRadius();
+
+    // Find closest face
+    const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
+    if (!FACE_QUERY.colliding)
+      return false;
+
+    // If center of sphere is inside the polyhedron (below the face)
+    if (FACE_QUERY.bestDistance < SHMath::EPSILON)
+      return true;
+
+    // Find closest face of polygon to circle
+    const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace);
+
+    const auto&   FACE          = POLYHEDRON.GetFace(FACE_QUERY.closestFace);
+    const SHVec3& FACE_NORMAL   = POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
+    const int32_t NUM_VERTICES  = static_cast<int32_t>(FACE.vertexIndices.size());
+
+    // Get points and build tangents
+    const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
+    const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
+
+    const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
+    const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index);
+    const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index);
+
+    const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
+    const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
+
+    // Get the voronoi region it belongs in
+    const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
+
+    return REGION > 0;
+  }
+
+  bool SHCollision::ConvexVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return SphereVsConvex(B, A);
+  }
+
+  bool SHCollision::SphereVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    // Convert to underlying types
+    // For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual.
+    const SHSphere&           SPHERE      = dynamic_cast<const SHSphere&>(A);
+    const SHConvexPolyhedron& POLYHEDRON  = dynamic_cast<const SHConvexPolyhedron&>(B);
+
+    const SHVec3  CENTER  = SPHERE.Center;
+    const float   RADIUS  = SPHERE.GetWorldRadius();
+
+    const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
+    if (!FACE_QUERY.colliding)
+      return false;
+
+    uint32_t    numContacts   = 0;
+    const float PENETRATION   = RADIUS - FACE_QUERY.bestDistance;
+
+    SHContact contact;
+    contact.featurePair.key = 0;
+
+    // Check if center is inside polyhedron (below the face)
+    if (FACE_QUERY.bestDistance < SHMath::EPSILON)
+    {
+      manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
+
+      contact.penetration = PENETRATION;
+      contact.position    = CENTER;
+
+      manifold.contacts[numContacts++] = contact;
+      manifold.numContacts = numContacts;
+
+      return true;
+    }
+
+    // Find closest face of polygon to circle
+    const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace);
+
+    const auto&   FACE          = POLYHEDRON.GetFace(FACE_QUERY.closestFace);
+    const SHVec3& FACE_NORMAL   = POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
+    const int32_t NUM_VERTICES  = static_cast<int32_t>(FACE.vertexIndices.size());
+
+    // Get points and build tangents
+    const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
+    const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
+
+    const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
+    const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index);
+    const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index);
+
+    const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
+    const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
+
+    // Get the voronoi region it belongs in
+    const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
+    if (REGION == 0)
+      return false;
+
+    // Create contact information based on region
+
+    const SHVec3 P1_TO_CENTER = CENTER - P1;
+    switch (REGION)
+    {
+      case 1: // Region A
+      case 2: // Region B
+      {
+        // Find closest point
+        const SHVec3& TANGENT     = REGION == 1 ? TANGENT_1 : TANGENT_2;
+        const SHVec3 CP           = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT);
+        const SHVec3 CP_TO_CENTER = CENTER - CP;
+
+        manifold.normal     = -SHVec3::Normalise(CP_TO_CENTER);
+
+        contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
+        contact.position    = CP;
+
+        break;
+      }
+      case 3: // Region C
+      {
+        manifold.normal     = -SHVec3::Normalise(P1_TO_CENTER);
+
+        contact.penetration = RADIUS - P1_TO_CENTER.Length();
+        contact.position    = P1;
+
+        break;
+      }
+      case 4: // Region D
+      {
+        manifold.normal     = -FACE_NORMAL;
+
+        contact.penetration = PENETRATION;
+        contact.position    = CENTER - FACE_NORMAL * RADIUS;
+
+        break;
+      }
+      default: return false;  // Should never happen
+    }
+
+    manifold.contacts[numContacts++] = contact;
+    manifold.numContacts = numContacts;
+
+    return true;
+  }
+
+  bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const bool RESULT = SphereVsConvex(manifold, B, A);
+    if (RESULT)
+    {
+      // Flip the normal
+      manifold.normal = -manifold.normal;
+    }
+    return RESULT;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollision::FaceQuery SHCollision::findClosestFace
+  (
+    const SHSphere&           sphere
+  , const SHConvexPolyhedron& polyhedron
+  ) noexcept
+  {
+    FaceQuery faceQuery;
+
+    const SHVec3  CENTER  = sphere.Center;
+    const float   RADIUS  = sphere.GetWorldRadius();
+
+    /*
+     * Test against each face.
+     *
+     * 1. For each face, build a plane in point-normal form.
+     * 2. Find the signed distance from plane to center of sphere.
+     * 3. Save best distance and face.
+     */
+    for (int32_t i = 0; i < polyhedron.GetFaceCount(); ++i)
+    {
+      const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(i);
+
+      // Build plane equation
+      // Use first vertex to build the plane
+      const SHPlane FACE_PLANE { polyhedron.GetVertex(FACE.vertexIndices[0].index), polyhedron.GetNormal(i) };
+
+      // Find signed distance of center to plane
+      const float SIGNED_DIST = FACE_PLANE.SignedDistance(CENTER);
+
+      // Early out:
+      // If face is facing away from center, signed dist is negative.
+      // Therefore signed distance is only positive when sphere is in front of the face.
+      if (SIGNED_DIST > RADIUS)
+        return faceQuery;
+
+      if (SIGNED_DIST > faceQuery.bestDistance)
+      {
+        faceQuery.bestDistance = SIGNED_DIST;
+        faceQuery.closestFace = i;
+      }
+    }
+
+    faceQuery.colliding = true;
+    return faceQuery;
+  }
+
+  int32_t SHCollision::findClosestPoint
+  (
+    const SHSphere&           sphere
+  , const SHConvexPolyhedron& polyhedron
+  , int32_t                   faceIndex
+  ) noexcept
+  {
+    // Find closest point on face
+    int32_t closestPointIndex = -1;
+
+    const SHVec3  CENTER  = sphere.Center;
+    
+    const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(faceIndex);
+    const int32_t NUM_VERITICES = static_cast<int32_t>(FACE.vertexIndices.size());
+
+    float smallestDist = std::numeric_limits<float>::max();
+    for (int32_t i = 0; i < NUM_VERITICES; ++i)
+    {
+      const SHVec3 POINT = polyhedron.GetVertex(FACE.vertexIndices[i].index);
+      const float DIST = SHVec3::DistanceSquared(CENTER, POINT);
+
+      if (DIST < smallestDist)
+      {
+        smallestDist = DIST;
+        closestPointIndex = i;
+      }
+    }
+
+    return closestPointIndex;
+  }
+
+  int32_t SHCollision::findVoronoiRegion
+  (
+    const SHSphere& sphere
+  , const SHVec3&   faceVertex
+  , const SHVec3&   faceNormal
+  , const SHVec3&   tangent1
+  , const SHVec3&   tangent2
+  ) noexcept
+  {
+    static constexpr int NUM_TANGENTS = 2;
+
+    // Check against voronoi regions of the face to determine the type of the intersection test
+    // We have 3 voronoi regions to check: 1 -> 2, 1 -> 3 and 1 -> center
+    // If none of these are true, the sphere is above the face but not separating
+
+    /*
+     *                   | 2
+     *       _ _ _ _ _ _ | _ _ _
+     *      /           /
+     *   | / regionD | /   regionA
+     *   |/ _ _ _ _ _|/ _ _ _
+     *  3/  regionB  /1
+     *  /           /   regionC
+     *
+     */
+
+    const SHVec3  CENTER  = sphere.Center;
+    const float   RADIUS  = sphere.GetWorldRadius();
+
+    const SHVec3 TANGENTS         [NUM_TANGENTS] { tangent1, tangent2 };
+    const SHVec3 ADJACENT_NORMALS [NUM_TANGENTS] { SHVec3::Cross(tangent1, faceNormal), SHVec3::Cross(faceNormal, tangent2) };
+
+    const SHVec3 FACE_TO_CENTER = CENTER - faceVertex;
+
+    // To be inside either region A or B, 2 conditions must be satisfied
+    // 1. Same side as tangent
+    // 2. Same side as adjacent normal
+
+    // Check Region A & B
+    for (int i = 0; i < NUM_TANGENTS; ++i)
+    {
+      float projection = SHVec3::Dot(FACE_TO_CENTER, TANGENTS[i]);
+      if (projection >= 0.0f)
+      {
+        // Find closest point
+        const SHVec3 CLOSEST_POINT = faceVertex + projection * TANGENTS[i];
+
+        projection = SHVec3::Dot(FACE_TO_CENTER, ADJACENT_NORMALS[i]);
+        if (projection >= 0.0f)
+        {
+          if (projection > RADIUS)
+            return 0;
+
+          // Region 1 or 2 ( A or B)
+          return i + 1;
+        }
+      }
+    }
+
+    // Check Region C
+    // Face to vertex is in the opposite direction of any tangent.
+    const float PROJECTION = SHVec3::Dot(FACE_TO_CENTER, tangent1);
+    if (PROJECTION < 0.0f)
+    {
+      const float DISTANCE_SQUARED = SHVec3::DistanceSquared(faceVertex, CENTER);
+      if (DISTANCE_SQUARED > RADIUS * RADIUS)
+        return 0;
+
+      return 3;
+    }
+      
+
+    // Belongs in region D by default
+    return 4;
+  }
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp

@@ -0,0 +1,97 @@
+/****************************************************************************************
+ * \file      SHSphereVsSphere.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for the Detecting Collisions between two spheres
+ *
+ * \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 "SHCollision.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Physics/Collision/Shapes/SHSphere.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+   bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const SHSphere& SPHERE_A = dynamic_cast<const SHSphere&>(A);
+    const SHSphere& SPHERE_B = dynamic_cast<const SHSphere&>(B);
+
+    const SHVec3  CENTER_A = SPHERE_A.Center;
+    const float   RADIUS_A = SPHERE_A.Radius;
+    const SHVec3  CENTER_B = SPHERE_B.Center;
+    const float   RADIUS_B = SPHERE_B.Radius;
+
+
+    const SHVec3  A_TO_B                            = CENTER_B - CENTER_A;
+    const float   DISTANCE_BETWEEN_CENTERS_SQUARED  = A_TO_B.LengthSquared();
+
+    const float   COMBINED_RADIUS                   = RADIUS_B + RADIUS_A;
+    const float   COMBINED_RADIUS_SQUARED           = COMBINED_RADIUS * COMBINED_RADIUS;
+
+    if (DISTANCE_BETWEEN_CENTERS_SQUARED > COMBINED_RADIUS_SQUARED)
+      return false;
+  }
+
+  bool SHCollision::SphereVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    // Convert to underlying types
+    const SHSphere& SPHERE_A = dynamic_cast<const SHSphere&>(A);
+    const SHSphere& SPHERE_B = dynamic_cast<const SHSphere&>(B);
+
+    const SHVec3  CENTER_A = SPHERE_A.Center;
+    const float   RADIUS_A = SPHERE_A.Radius;
+    const SHVec3  CENTER_B = SPHERE_B.Center;
+    const float   RADIUS_B = SPHERE_B.Radius;
+
+
+    const SHVec3  A_TO_B                            = CENTER_B - CENTER_A;
+    const float   DISTANCE_BETWEEN_CENTERS_SQUARED  = A_TO_B.LengthSquared();
+
+    const float   COMBINED_RADIUS                   = RADIUS_B + RADIUS_A;
+    const float   COMBINED_RADIUS_SQUARED           = COMBINED_RADIUS * COMBINED_RADIUS;
+
+    if (DISTANCE_BETWEEN_CENTERS_SQUARED > COMBINED_RADIUS_SQUARED)
+      return false;
+
+    // Only populate the manifold if there is a collision
+
+    uint32_t numContacts = 0;
+
+    SHContact contact;
+    contact.featurePair.key = 0;
+
+    if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f))
+    {
+      manifold.normal     = SHVec3::UnitY;
+      contact.position    = CENTER_A;
+      contact.penetration = RADIUS_B;
+
+      manifold.contacts[numContacts++] = contact;
+    }
+    else
+    {
+      manifold.normal     = SHVec3::Normalise(A_TO_B);
+      contact.position    = CENTER_B - manifold.normal * RADIUS_B;
+      contact.penetration = COMBINED_RADIUS - A_TO_B.Length();
+
+      manifold.contacts[numContacts++] = contact;
+    }
+
+    manifold.numContacts = numContacts;
+    
+    return true;
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCollider.cpp

@@ -0,0 +1,393 @@
+/****************************************************************************************
+ * \file      SHCollider.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Base Collider Class.
+ *
+ * \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 "SHCollider.h"
+
+// Project Headers
+#include "Broadphase/SHDynamicAABBTree.h"
+#include "Events/SHEvent.h"
+#include "Math/SHMathHelpers.h"
+#include "Physics/SHPhysicsEvents.h"
+#include "Physics/Dynamics/SHRigidBody.h"
+
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollider::SHCollider(EntityID eid, const SHTransform& worldTransform) noexcept
+  : entityID      { eid }
+  , flags         { 0 }
+  , rigidBody     { nullptr }
+  , shapeLibrary  { nullptr }
+  , broadphase    { nullptr }
+  , transform     { worldTransform }
+  {
+    flags |= ACTIVE_FLAG;
+    flags |= MOVED_FLAG;
+  }
+
+  SHCollider::SHCollider(const SHCollider& rhs) noexcept
+  : entityID      { rhs.entityID }
+  , flags         { rhs.flags }
+  , rigidBody     { rhs.rigidBody }
+  , shapeLibrary  { rhs.shapeLibrary }
+  , broadphase    { rhs.broadphase }
+  , transform     { rhs.transform }
+  {}
+
+  SHCollider::SHCollider(SHCollider&& rhs) noexcept
+  : entityID      { rhs.entityID }
+  , flags         { rhs.flags }
+  , rigidBody     { rhs.rigidBody }
+  , shapeLibrary  { rhs.shapeLibrary }
+  , broadphase    { rhs.broadphase }
+  , transform     { rhs.transform }
+  {}
+
+  SHCollider::~SHCollider() noexcept
+  {
+    if (!shapeLibrary)
+    {
+      SHLOGV_ERROR("Shape factory is unlinked with Composite Collider {}. Unable to add destroy collider!", entityID)
+      return;
+    }
+
+    for (auto* shape : shapes)
+      shapeLibrary->DestroyShape(shape);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollider& SHCollider::operator=(const SHCollider& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    if (!shapeLibrary)
+    {
+      SHLOGV_ERROR("Shape factory is unlinked with Collider {}. Unable to add copy shapes!", entityID)
+      return *this;
+    }
+
+    entityID      = rhs.entityID;
+    flags         = rhs.flags;
+    rigidBody     = rhs.rigidBody;
+    shapeLibrary  = rhs.shapeLibrary;
+    broadphase    = rhs.broadphase;
+    transform     = rhs.transform;
+
+    copyShapes(rhs);
+
+    return *this;
+  }
+
+  SHCollider& SHCollider::operator=(SHCollider&& rhs) noexcept
+  {
+    if (!shapeLibrary)
+    {
+      SHLOGV_ERROR("Shape factory is unlinked with Collider {}. Unable to add copy shapes!", entityID)
+      return *this;
+    }
+
+    entityID      = rhs.entityID;
+    flags         = rhs.flags;
+    rigidBody     = rhs.rigidBody;
+    shapeLibrary  = rhs.shapeLibrary;
+    broadphase    = rhs.broadphase;
+    transform     = rhs.transform;
+
+    copyShapes(rhs);
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  EntityID SHCollider::GetEntityID() const noexcept
+  {
+    return entityID;
+  }
+
+  SHCollider::Type SHCollider::GetType() const noexcept
+  {
+    if (flags & COMPOSITE_FLAG)
+      return Type::COMPOSITE;
+
+    if (flags & HULL_FLAG)
+      return Type::HULL;
+
+    return Type::INVALID;
+  }
+
+  bool SHCollider::IsActive() const noexcept
+  {
+    return flags & ACTIVE_FLAG;
+  }
+
+  bool SHCollider::GetDebugDrawState() const noexcept
+  {
+    return flags & DRAW_FLAG;
+  }
+
+  const SHTransform& SHCollider::GetTransform() const noexcept
+  {
+    return transform;
+  }
+
+  const SHVec3& SHCollider::GetPosition() const noexcept
+  {
+    return transform.position;
+  }
+
+  const SHQuaternion& SHCollider::GetOrientation() const noexcept
+  {
+    return transform.orientation;
+  }
+
+  const SHVec3& SHCollider::GetScale() const noexcept
+  {
+    return transform.scale;
+  }
+
+  const SHCollider::CollisionShapes& SHCollider::GetCollisionShapes() const noexcept
+  {
+    return shapes;
+  }
+
+  SHCollisionShape* SHCollider::GetCollisionShape(int index) const
+  {
+    const int NUM_SHAPES = static_cast<int>(shapes.size());
+
+    if (index < 0 || index >= NUM_SHAPES)
+      throw std::invalid_argument("Out-of-range index!");
+
+    return shapes[index];
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHCollider::SetType(Type type) noexcept
+  {
+    if (type == Type::COMPOSITE)
+      flags |= COMPOSITE_FLAG;
+
+    if (type == Type::HULL)
+      flags |= HULL_FLAG;
+  }
+
+  void SHCollider::SetIsActive(bool state) noexcept
+  {
+    const bool PREV_STATE = flags & ACTIVE_FLAG;
+    state ? flags |= ACTIVE_FLAG : flags &= ~(ACTIVE_FLAG);
+
+    if (!broadphase)
+      return;
+
+    for (auto* shape : shapes)
+    {
+      if (PREV_STATE) // Previously inactive
+        broadphase->Insert(shape->id, shape->ComputeAABB());
+      else        // Previously active
+        broadphase->Remove(shape->id);
+    }
+  }
+
+  void SHCollider::SetDebugDrawState(bool state) noexcept
+  {
+    state ? flags |= DRAW_FLAG : flags &= ~(DRAW_FLAG);
+
+  #ifdef SHEDITOR
+    
+    // Broadcast event for the Debug Draw system to catch
+    const SHColliderOnDebugDrawEvent EVENT_DATA
+    {
+      .entityID       = entityID
+    , .debugDrawState = state 
+    };
+
+    SHEventManager::BroadcastEvent<SHColliderOnDebugDrawEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_DRAW_EVENT);
+
+  #endif
+  }
+
+  void SHCollider::SetRigidBody(SHRigidBody* rb) noexcept
+  {
+    rigidBody = rb;
+  }
+
+  void SHCollider::SetTransform(const SHTransform& newTransform) noexcept
+  {
+    flags |= MOVED_FLAG;
+    transform = newTransform;
+  }
+
+  void SHCollider::SetPosition(const SHVec3& newPosition) noexcept
+  {
+    flags |= MOVED_FLAG;
+    transform.position = newPosition;
+  }
+
+  void SHCollider::SetOrientation(const SHQuaternion& newOrientation) noexcept
+  {
+    flags |= MOVED_FLAG;
+    transform.orientation = newOrientation;
+  }
+
+  void SHCollider::SetScale(const SHVec3& newScale) noexcept
+  {
+    flags |= MOVED_FLAG;
+    transform.scale = newScale;
+  }
+
+  void SHCollider::SetLibrary(SHCollisionShapeLibrary* factory) noexcept
+  {
+    shapeLibrary = factory;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  const SHMatrix& SHCollider::ComputeTRS() noexcept
+  {
+    return transform.ComputeTRS();
+  }
+
+  void SHCollider::RemoveCollisionShape(int index)
+  {
+    if (!shapeLibrary)
+    {
+      SHLOGV_ERROR("Shape factory is unlinked with Collider {}. Unable to add remove shape!", entityID)
+      return;
+    }
+
+    const int NUM_SHAPES = static_cast<int>(shapes.size());
+
+    if (index < 0 || index >= NUM_SHAPES)
+      throw std::invalid_argument("Out-of-range index!");
+
+    auto shape = shapes.begin();
+    for (int i = 0; i < NUM_SHAPES; ++i, ++shape)
+    {
+      if (i == index)
+        break;
+    }
+
+    const SHPhysicsColliderRemovedEvent EVENT_DATA
+    {
+      .entityID       = entityID
+    , .colliderType   = (*shape)->GetType()
+    , .colliderIndex  = index
+    };
+
+     // Remove from broadphase
+    if (broadphase)
+      broadphase->Remove((*shape)->id);
+
+    shapeLibrary->DestroyShape(*shape);
+    *shape = nullptr;
+
+    // Remove the shape from the container to prevent accessing a nullptr
+    shape = shapes.erase(shape);
+
+   
+
+    // Broadcast Event for removing a shape
+    SHEventManager::BroadcastEvent<SHPhysicsColliderRemovedEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_REMOVED_EVENT);
+
+    if (rigidBody)
+      rigidBody->ComputeMassData();
+
+    SHLOG_INFO_D("Removing Collision Shape {} from Entity {}", index, entityID)
+  }
+
+  void SHCollider::Update() noexcept
+  {
+    for (auto* shape : shapes)
+      shape->Update();
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Function Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHCollider::copyShapes(const SHCollider& rhsCollider)
+  {
+    for (const auto* shape : rhsCollider.shapes)
+    {
+      switch (shape->GetType())
+      {
+        case SHCollisionShape::Type::BOX:
+        {
+          const SHBox* RHS_BOX = dynamic_cast<const SHBox*>(shape);
+
+          const SHBoxCreateInfo BOX_CREATE_INFO
+          {
+            .Center           = RHS_BOX->Center
+          , .Extents          = RHS_BOX->Extents
+          , .RelativeExtents  = RHS_BOX->relativeExtents
+          , .Orientation      = RHS_BOX->Orientation
+          , .Scale            = RHS_BOX->scale
+          };
+
+          const uint32_t NEW_INDEX = static_cast<uint32_t>(shapes.size());
+          const SHCollisionShapeID NEW_SHAPE_ID{ entityID, NEW_INDEX };
+
+          SHBox* box = shapeLibrary->CreateBox(NEW_SHAPE_ID, BOX_CREATE_INFO);
+          *box = *RHS_BOX;
+
+          shapes.emplace_back(box);
+
+          break;
+        }
+        case SHCollisionShape::Type::SPHERE:
+        {
+          const SHSphere* RHS_SPHERE = dynamic_cast<const SHSphere*>(shape);
+
+          const SHSphereCreateInfo SPHERE_CREATE_INFO
+          {
+            .Center         = RHS_SPHERE->Center
+          , .Radius         = RHS_SPHERE->Radius
+          , .RelativeRadius = RHS_SPHERE->relativeRadius
+          , .Scale          = RHS_SPHERE->scale
+          };
+
+          const uint32_t NEW_INDEX = static_cast<uint32_t>(shapes.size());
+          const SHCollisionShapeID NEW_SHAPE_ID{ entityID, NEW_INDEX };
+
+          SHSphere* sphere = shapeLibrary->CreateSphere(NEW_SHAPE_ID, SPHERE_CREATE_INFO);
+          *sphere = *RHS_SPHERE;
+
+          shapes.emplace_back(sphere);
+
+          break;
+        }
+        case SHCollisionShape::Type::CAPSULE:
+        {
+          break;
+        }
+        default: break;
+      }
+    }
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCollider.h

@@ -0,0 +1,178 @@
+/****************************************************************************************
+ * \file      SHCollider.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Base Collider Class.
+ *
+ * \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 "ECS_Base/Entity/SHEntity.h"
+#include "Math/Transform/SHTransform.h"
+#include "Physics/Collision/Shapes/SHCollisionShapeLibrary.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Forward Declarations                                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  class SHRigidBody;
+  class SHAABBTree;
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Base class for a collider.
+   * There are only two collider types supported by SHADE Engine: Composite & Hull
+  */
+  class SH_API SHCollider
+  {
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Friends                                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    friend class  SHCollisionSpace;
+    friend struct SHManifold;
+
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    enum class Type
+    {
+      COMPOSITE
+    , HULL
+
+    , TOTAL
+    , INVALID = -1
+    };
+
+    using CollisionShapes = std::vector<SHCollisionShape*>;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Constructor for a collider.
+     * @param eid
+     * The entity this collider belongs to.
+     * @param worldTransform
+     * The world transform for the collider. Defaults to the identity transform.
+     * This is particularly important for composite colliders for offsets & relative sizes.
+     * @return 
+    */
+    SHCollider  (EntityID eid, const SHTransform& worldTransform = SHTransform::Identity) noexcept;
+    SHCollider  (const SHCollider& rhs)                                                   noexcept;
+    SHCollider  (SHCollider&& rhs)                                                        noexcept;
+    virtual ~SHCollider ()                                                                noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+    
+    SHCollider& operator=(const SHCollider& rhs)  noexcept;
+    SHCollider& operator=(SHCollider&& rhs)       noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] EntityID                GetEntityID         ()  const noexcept;
+
+    [[nodiscard]] Type                    GetType             ()  const noexcept;
+    [[nodiscard]] bool                    IsActive            ()  const noexcept;
+    [[nodiscard]] bool                    GetDebugDrawState   ()  const noexcept;
+
+    [[nodiscard]] const SHTransform&      GetTransform        ()  const noexcept;
+    [[nodiscard]] const SHVec3&           GetPosition         ()  const noexcept;
+    [[nodiscard]] const SHQuaternion&     GetOrientation      ()  const noexcept;
+    [[nodiscard]] const SHVec3&           GetScale            ()  const noexcept;
+
+    [[nodiscard]] const CollisionShapes&  GetCollisionShapes  ()  const noexcept;
+    [[nodiscard]] SHCollisionShape*       GetCollisionShape   (int index)  const;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void SetType            (Type type)                           noexcept;
+    void SetIsActive        (bool state)                          noexcept;
+    void SetDebugDrawState  (bool state)                          noexcept;
+
+    void SetRigidBody       (SHRigidBody* rb)                     noexcept;
+
+    void SetTransform       (const SHTransform& newTransform)     noexcept;
+    void SetPosition        (const SHVec3& newPosition)           noexcept;
+    void SetOrientation     (const SHQuaternion& newOrientation)  noexcept;
+    void SetScale           (const SHVec3& newScale)              noexcept;
+
+    void SetLibrary         (SHCollisionShapeLibrary* factory)    noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Computes the TRS for the collider's transform
+     * @return
+     * The computed TRS.
+    */
+    const SHMatrix& ComputeTRS() noexcept;
+
+    /**
+     * @brief
+     * Removes a shape from the container. Removal reduces the size of the container.
+     * If removing all, perform removal from back to front.
+     * @param index
+     * The index of the shape to remove.
+     * @throws
+     * Invalid argument for out-of-range indices.
+    */
+    void RemoveCollisionShape (int index);
+
+    /**
+     * @brief
+     * Recomputes the transforms for all shapes in this composite collider.
+    */
+    void Update () noexcept;
+
+  protected:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    static constexpr uint8_t COMPOSITE_FLAG = 1U << static_cast<uint8_t>(Type::COMPOSITE);
+    static constexpr uint8_t HULL_FLAG      = 1U << static_cast<uint8_t>(Type::HULL);
+    static constexpr uint8_t ACTIVE_FLAG    = 1U << 2;
+    static constexpr uint8_t DRAW_FLAG      = 1U << 3;
+    static constexpr uint8_t MOVED_FLAG     = 1U << 4;
+
+    EntityID                  entityID;
+    uint8_t                   flags;    // 0 0 0 hasMoved debugDraw active hull composite 
+    SHRigidBody*              rigidBody;
+    SHCollisionShapeLibrary*  shapeLibrary;
+    SHAABBTree*               broadphase;
+    SHTransform               transform;
+    CollisionShapes           shapes;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void copyShapes       (const SHCollider& rhsCollider);
+  };
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCollisionInfo.cpp

@@ -1,93 +0,0 @@
-/****************************************************************************************
- * \file      SHCollisionInfo.cpp
- * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Implementation for Collision Info.
- *
- * \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 "SHCollisionInfo.h"
-
-namespace SHADE
-{
-  /*-----------------------------------------------------------------------------------*/
-  /* Constructors & Destructor Definitions                                             */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHCollisionInfo::SHCollisionInfo() noexcept
-  : collisionState  { State::INVALID }
-  {
-    ids[ENTITY_A]   = MAX_EID;
-    ids[ENTITY_B]   = MAX_EID;
-    ids[COLLIDER_A] = std::numeric_limits<uint32_t>::max();
-    ids[COLLIDER_B] = std::numeric_limits<uint32_t>::max();
-  }
-
-  SHCollisionInfo::SHCollisionInfo(EntityID entityA, EntityID entityB) noexcept
-  : collisionState  { State::INVALID }
-  {
-    ids[ENTITY_A]   = entityA;
-    ids[ENTITY_B]   = entityB;
-    ids[COLLIDER_A] = std::numeric_limits<uint32_t>::max();
-    ids[COLLIDER_B] = std::numeric_limits<uint32_t>::max();
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Operator Overload Definitions                                                     */
-  /*-----------------------------------------------------------------------------------*/
-
-  bool SHCollisionInfo::operator==(const SHCollisionInfo& rhs) const noexcept
-  {
-    return value[0] == rhs.value[0] && value[1] == rhs.value[1];
-  }
-
-  bool SHCollisionInfo::operator!=(const SHCollisionInfo& rhs) const noexcept
-  {
-    return  value[0] != rhs.value[0] || value[1] != rhs.value[1];
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Getter Function Definitions                                                       */
-  /*-----------------------------------------------------------------------------------*/
-
-  EntityID SHCollisionInfo::GetEntityA() const noexcept
-  {
-    return ids[ENTITY_A];
-  }
-
-  EntityID SHCollisionInfo::GetEntityB() const noexcept
-  {
-    return ids[ENTITY_B];
-  }
-
-  const SHRigidBodyComponent* SHCollisionInfo::GetRigidBodyA() const noexcept
-  {
-    return SHComponentManager::GetComponent_s<SHRigidBodyComponent>(ids[ENTITY_A]);
-  }
-
-  const SHRigidBodyComponent* SHCollisionInfo::GetRigidBodyB() const noexcept
-  {
-    return SHComponentManager::GetComponent_s<SHRigidBodyComponent>(ids[ENTITY_B]);
-  }
-
-  const SHCollisionShape* SHCollisionInfo::GetColliderA() const noexcept
-  {
-    return &SHComponentManager::GetComponent<SHColliderComponent>(ids[ENTITY_A])->GetCollisionShape(ids[COLLIDER_A]);
-  }
-
-  const SHCollisionShape* SHCollisionInfo::GetColliderB() const noexcept
-  {
-    return &SHComponentManager::GetComponent<SHColliderComponent>(ids[ENTITY_B])->GetCollisionShape(ids[COLLIDER_B]);
-  }
-
-  SHCollisionInfo::State SHCollisionInfo::GetCollisionState() const noexcept
-  {
-    return collisionState;
-  }
-
-} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCollisionListener.cpp

@@ -1,254 +0,0 @@
-/****************************************************************************************
- * \file      SHCollisionListener.cpp
- * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Implementation for a Collision Listener.
- *
- * \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 "SHCollisionListener.h"
-
-// Project Headers
-#include "ECS_Base/Managers/SHEntityManager.h"
-#include "Physics/PhysicsObject/SHPhysicsObject.h"
-#include "Physics/System/SHPhysicsSystem.h"
-#include "Scene/SHSceneManager.h"
-
-/*-------------------------------------------------------------------------------------*/
-/* Local Helper Functions                                                              */
-/*-------------------------------------------------------------------------------------*/
-
-uint32_t matchColliders(const SHADE::SHPhysicsObject&physicsObject, const rp3d::Entity colliderID)
-{
-  for (uint32_t i = 0; i < physicsObject.GetCollisionBody()->getNbColliders(); ++i)
-  {
-    const auto* collider = physicsObject.GetCollisionBody()->getCollider(i);
-    if (collider->getEntity() == colliderID)
-      return i;
-  }
-
-  return std::numeric_limits<uint32_t>::max();
-}
-
-namespace SHADE
-{
-  /*-----------------------------------------------------------------------------------*/
-  /* Constructors & Destructor Definitions                                             */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHCollisionListener::SHCollisionListener() noexcept
-  : system { nullptr }
-  {}
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Getter Function Definitions                                                       */
-  /*-----------------------------------------------------------------------------------*/
-
-  const std::vector<SHCollisionInfo>& SHCollisionListener::GetCollisionInfoContainer() const noexcept
-  {
-    return collisionInfoContainer;
-  }
-
-  const std::vector<SHCollisionInfo>& SHCollisionListener::GetTriggerInfoContainer() const noexcept
-  {
-    return triggerInfoContainer;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Public Function Member Definitions                                                */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHCollisionListener::BindToSystem(SHPhysicsSystem* physicsSystem) noexcept
-  {
-    system = physicsSystem;
-  }
-
-  void SHCollisionListener::BindToWorld(rp3d::PhysicsWorld* world) noexcept
-  {
-    if (!world)
-      return;
-
-    world->setEventListener(this);
-  }
-
-  void SHCollisionListener::CleanContainers() noexcept
-  {
-    static const auto CLEAR = [](std::vector<SHCollisionInfo>& container)
-    {
-      for (auto eventIter = container.begin(); eventIter != container.end();)
-      {
-        const SHCollisionInfo& C_INFO = *eventIter;
-
-        const bool INVALID_ENTITY   = !SHEntityManager::IsValidEID(C_INFO.GetEntityA()) || !SHEntityManager::IsValidEID(C_INFO.GetEntityB());
-        if (INVALID_ENTITY)
-        {
-          eventIter = container.erase(eventIter);
-          continue;
-        }
-        else
-        {
-          const bool CLEAR_EVENT      = C_INFO.GetCollisionState() == SHCollisionInfo::State::EXIT ||  C_INFO.GetCollisionState() == SHCollisionInfo::State::INVALID;
-
-          const bool INACTIVE_OBJECT  = !SHSceneManager::CheckNodeAndComponentsActive<SHColliderComponent>(C_INFO.GetEntityA())
-                                    ||  !SHSceneManager::CheckNodeAndComponentsActive<SHColliderComponent>(C_INFO.GetEntityB());
-
-          if (CLEAR_EVENT || INACTIVE_OBJECT)
-          {
-            eventIter = container.erase(eventIter);
-            continue;
-          }
-        }
-
-        ++eventIter;
-      }
-    };
-
-    CLEAR(collisionInfoContainer);
-    CLEAR(triggerInfoContainer);
-  }
-
-  void SHCollisionListener::ClearContainers() noexcept
-  {
-    collisionInfoContainer.clear();
-    triggerInfoContainer.clear();
-  }
-
-  void SHCollisionListener::onContact(const rp3d::CollisionCallback::CallbackData& callbackData)
-  {
-    for (uint32_t i = 0; i < callbackData.getNbContactPairs(); ++i)
-    {
-      const auto CONTACT_PAIR         = callbackData.getContactPair(i);
-      const SHCollisionInfo NEW_INFO  = generateCollisionInfo(CONTACT_PAIR);
-
-      updateInfoContainers(NEW_INFO, collisionInfoContainer);
-    }
-  }
-
-  void SHCollisionListener::onTrigger(const rp3d::OverlapCallback::CallbackData& callbackData)
-  {
-    for (uint32_t i = 0; i < callbackData.getNbOverlappingPairs(); ++i)
-    {
-      const auto OVERLAP_PAIR         = callbackData.getOverlappingPair(i);
-      const SHCollisionInfo NEW_INFO  = generateTriggerInfo(OVERLAP_PAIR);
-
-      updateInfoContainers(NEW_INFO, triggerInfoContainer);
-    }
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Private Function Member Definitions                                               */
-  /*-----------------------------------------------------------------------------------*/
-  
-  void SHCollisionListener::updateInfoContainers(const SHCollisionInfo& collisionEvent, std::vector<SHCollisionInfo>& container) noexcept
-  {
-    const auto IT = std::ranges::find_if(container.begin(), container.end(), [&](const SHCollisionInfo& info)
-    {
-      const bool ENTITY_MATCH     =   (info.ids[0] == collisionEvent.ids[0] && info.ids[1] == collisionEvent.ids[1])
-                                  ||  (info.ids[0] == collisionEvent.ids[1] && info.ids[1] == collisionEvent.ids[0]);
-      const bool COLLIDERS_MATCH  =   (info.ids[2] == collisionEvent.ids[2] && info.ids[3] == collisionEvent.ids[3])
-                                  ||  (info.ids[2] == collisionEvent.ids[3] && info.ids[3] == collisionEvent.ids[2]);
-      return ENTITY_MATCH && COLLIDERS_MATCH;
-    });
-
-    if (IT == container.end())
-      container.emplace_back(collisionEvent);
-    else
-      IT->collisionState = collisionEvent.collisionState;
-  }
-
-  SHCollisionInfo SHCollisionListener::generateCollisionInfo(const rp3d::CollisionCallback::ContactPair& cp) const noexcept
-  {
-    SHCollisionInfo cInfo;
-
-    // Update collision state
-    cInfo.collisionState = static_cast<SHCollisionInfo::State>(cp.getEventType());
-
-    // Match body and collider for collision event
-    const rp3d::Entity body1      = cp.getBody1()->getEntity();
-    const rp3d::Entity body2      = cp.getBody2()->getEntity();
-    const rp3d::Entity collider1  = cp.getCollider1()->getEntity();
-    const rp3d::Entity collider2  = cp.getCollider2()->getEntity();
-
-    // Find and match both ids
-    bool matched[2] = { false, false };
-
-    
-    for (auto& [entityID, physicsObject] : system->GetPhysicsObjects())
-    {
-      // Match body 1
-      if (matched[SHCollisionInfo::ENTITY_A] == false && physicsObject.GetCollisionBody()->getEntity() == body1)
-      {
-        cInfo.ids[SHCollisionInfo::ENTITY_A] = entityID;
-        cInfo.ids[SHCollisionInfo::COLLIDER_A] = matchColliders(physicsObject, collider1);
-
-        matched[SHCollisionInfo::ENTITY_A] = true;
-      }
-
-      // Match body 2
-      if (matched[SHCollisionInfo::ENTITY_B] == false && physicsObject.GetCollisionBody()->getEntity() == body2)
-      {
-        cInfo.ids[SHCollisionInfo::ENTITY_B] = entityID;
-        cInfo.ids[SHCollisionInfo::COLLIDER_B] = matchColliders(physicsObject, collider2);
-
-        matched[SHCollisionInfo::ENTITY_B] = true;
-      }
-
-      if (matched[SHCollisionInfo::ENTITY_A] == true && matched[SHCollisionInfo::ENTITY_B] == true)
-        return cInfo;
-    }
-
-    return cInfo;
-  }
-
-  SHCollisionInfo SHCollisionListener::generateTriggerInfo(const rp3d::OverlapCallback::OverlapPair& cp) const noexcept
-  {
-    SHCollisionInfo cInfo;
-
-    // Update collision state
-    cInfo.collisionState = static_cast<SHCollisionInfo::State>(cp.getEventType());
-
-    // Match body and collider for collision event
-    const rp3d::Entity body1      = cp.getBody1()->getEntity();
-    const rp3d::Entity body2      = cp.getBody2()->getEntity();
-    const rp3d::Entity collider1  = cp.getCollider1()->getEntity();
-    const rp3d::Entity collider2  = cp.getCollider2()->getEntity();
-
-    // Find and match both ids
-    bool matched[2] = { false, false };
-
-    
-    for (auto& [entityID, physicsObject] : system->GetPhysicsObjects())
-    {
-      // Match body 1
-      if (matched[SHCollisionInfo::ENTITY_A] == false && physicsObject.GetCollisionBody()->getEntity() == body1)
-      {
-        cInfo.ids[SHCollisionInfo::ENTITY_A] = entityID;
-        cInfo.ids[SHCollisionInfo::COLLIDER_A] = matchColliders(physicsObject, collider1);
-
-        matched[SHCollisionInfo::ENTITY_A] = true;
-      }
-
-      // Match body 2
-      if (matched[SHCollisionInfo::ENTITY_B] == false && physicsObject.GetCollisionBody()->getEntity() == body2)
-      {
-        cInfo.ids[SHCollisionInfo::ENTITY_B] = entityID;
-        cInfo.ids[SHCollisionInfo::COLLIDER_B] = matchColliders(physicsObject, collider2);
-
-        matched[SHCollisionInfo::ENTITY_B] = true;
-      }
-
-      if (matched[SHCollisionInfo::ENTITY_A] == true && matched[SHCollisionInfo::ENTITY_B] == true)
-        return cInfo;
-    }
-
-    return cInfo;
-  }
-
-
-
-} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCollisionSpace.cpp

@@ -0,0 +1,308 @@
+/****************************************************************************************
+ * \file      SHCollisionSpace.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Collision Space that handles collision detetction.
+ *
+ * \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 "SHCollisionSpace.h"
+
+#include "Narrowphase/SHCollisionDispatch.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  const SHAABBTree::AABBs& SHCollisionSpace::GetBroadphaseAABBs() const noexcept
+  {
+    return broadphase.GetAABBs();
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHCollisionSpace::SetContactManager(SHContactManager* _contactManager) noexcept
+  {
+    contactManager = _contactManager;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHCollisionSpace::AddCollider(SHCollider* collider) noexcept
+  {
+    const bool INSERTED = colliders.emplace(collider->entityID, collider).second;
+    if (!INSERTED)
+    {
+      SHLOG_WARNING_D("Attempting to add duplicate collider {} to the Physics World!", collider->entityID)
+      return;
+    }
+
+    collider->broadphase = &broadphase;
+
+    // Add all existing shapes to the broadphase
+    for (const auto* shape : collider->shapes)
+      broadphase.Insert(shape->id, shape->ComputeAABB());
+  }
+
+  void SHCollisionSpace::RemoveCollider(SHCollider* collider) noexcept
+  {
+    colliders.erase(collider->entityID);
+
+    const uint32_t NUM_SHAPES = static_cast<uint32_t>(collider->shapes.size());
+    if (NUM_SHAPES == 0)
+      return;
+
+    for (uint32_t i = 0; i < NUM_SHAPES; ++i)
+      broadphase.Remove(collider->shapes[i]->id);
+
+    if (contactManager)
+    {
+      contactManager->RemoveInvalidatedTrigger(collider->entityID);
+      contactManager->RemoveInvalidatedManifold(collider->entityID);
+    }
+
+    /*
+     * TODO:
+     * Get collider's rigid body
+     * Run through the rigid body's contact graph and wake all of its non-static bodies that are asleep
+     */
+  }
+
+  void SHCollisionSpace::UpdateBroadphase() noexcept
+  {
+    // Update any colliders that have moved
+    for (auto& collider : colliders | std::views::values)
+    {
+      const bool IS_ACTIVE  = collider->IsActive();
+      const bool HAS_MOVED  = collider->flags & SHCollider::MOVED_FLAG;
+
+      if (!IS_ACTIVE || !HAS_MOVED)
+        continue;
+      
+      // Clear hasMoved flag here
+      collider->flags &= ~SHCollider::MOVED_FLAG;
+
+      // Update moved shapes in broadphase
+      for (auto* shape : collider->shapes)
+        broadphase.Update(shape->id, shape->ComputeAABB());
+    }
+  }
+
+  void SHCollisionSpace::DetectCollisions() noexcept
+  {
+    // TODO: Profile broad-phase and narrow-phase
+
+    /*
+     * Broad-phase
+     */
+
+    // Broadphase Queries: Kinematic Triggers, Awake Dynamic Bodies & Dynamic Triggers
+    for (auto& collider : colliders | std::views::values)
+    {
+      // Colliders without bodies are considered to be static bodies
+      // This is specific to this engine because of Unity's stupid convention.
+      const bool IS_IMPLICIT_STATIC = !collider->rigidBody;
+      const bool IS_ACTIVE          = collider->IsActive();
+
+      // Skip inactive colliders
+      if (!IS_ACTIVE || IS_IMPLICIT_STATIC)
+        continue;
+
+      const bool IS_EXPLICIT_STATIC = collider->rigidBody->GetType() == SHRigidBody::Type::STATIC;
+      if (IS_EXPLICIT_STATIC)
+        continue;
+
+      // All remaining are kinematic or dynamic
+      // Iterate through shapes: if kinematic / dynamic trigger, else if dynamic & awake
+      // Results are loaded into the narrowphase batch
+      broadphaseQuery(collider->rigidBody->GetType(), collider);
+    }
+
+    /*
+     * Narrow-phase
+     */
+
+    // If no potential collisions, we can skip the entire narrow phase. No further updates necessary.
+    // All contact / trigger states persist in this step.
+    if (narrowphaseBatch.empty())
+      return;
+
+    // All narrowphase IDs are unique, there should be no duplicate collision checks.
+    // This applies both ways: A -> B and B -> A.
+    for (auto& [key, narrowphasePair] : narrowphaseBatch)
+    {
+      // Filter through tags before attempting narrow-phase
+      const uint16_t TAG_A = narrowphasePair.A->GetCollisionTag().GetMask();
+      const uint16_t TAG_B = narrowphasePair.B->GetCollisionTag().GetMask();
+
+      const bool MATCH_TAG = TAG_A & TAG_B;
+      if (!MATCH_TAG)
+        continue;
+
+      const bool IS_A_TRIGGER = narrowphasePair.A->IsTrigger();
+      const bool IS_B_TRIGGER = narrowphasePair.B->IsTrigger();
+
+      if (IS_A_TRIGGER || IS_B_TRIGGER)
+        collideTriggers(key, narrowphasePair);
+      else
+        collideManifolds(key, narrowphasePair);
+    }
+
+    // Clear every frame
+    narrowphaseBatch.clear();
+
+    // Test all collisions
+    if (contactManager)
+      contactManager->Update();
+  }
+
+  const SHCollisionSpace::RaycastResults& SHCollisionSpace::Raycast(const RaycastInfo& info) noexcept
+  {
+    static RaycastResults results;
+    results.clear();
+
+    const bool FILTER_COLLIDER = info.colliderEntityID.has_value();
+
+    // Cast ray into the broadphase scene
+    const auto& POTENTIAL_HITS = broadphase.Query(info.ray, info.distance);
+
+    if (POTENTIAL_HITS.empty())
+      return results;
+
+    // Iterate through all potential hits
+    const int NUM_HITS = static_cast<int>(POTENTIAL_HITS.size());
+    for (const int i : std::ranges::views::iota(0, NUM_HITS))
+    {
+      const auto HIT_ID = POTENTIAL_HITS[i];
+
+      const EntityID EID = HIT_ID.GetEntityID();
+
+      if (FILTER_COLLIDER && EID == info.colliderEntityID.value())
+        continue;
+
+      // Get shape
+      const uint32_t IDX = HIT_ID.GetShapeIndex();
+      const auto* SHAPE = colliders.find(EID)->second->GetCollisionShape(IDX);
+
+      // Filter the layers
+      const bool LAYER_MATCH = SHAPE->GetCollisionTag().GetMask() & info.layers;
+      if (!LAYER_MATCH)
+        continue;
+
+      // We cast to the underlying shape. THis is done because a convex hull will not have an inherited raycast method.
+      // Kinda awkward oversight...oops
+
+      SHRaycastResult baseResult;
+      switch (SHAPE->GetType())
+      {
+        case SHCollisionShape::Type::SPHERE:
+        {
+          baseResult = dynamic_cast<const SHSphere*>(SHAPE)->Raycast(info.ray);
+          break;
+        }
+        case SHCollisionShape::Type::BOX:
+        {
+          baseResult = dynamic_cast<const SHBox*>(SHAPE)->Raycast(info.ray);
+          break;
+        }
+        case SHCollisionShape::Type::CAPSULE:
+        {
+          // TODO
+          break;
+        }
+        default: continue;  // Redundant case
+      }
+
+      if (!baseResult || baseResult.distance > info.distance)
+        continue;
+
+      // Copy to a physics raycast result
+      SHPhysicsRaycastResult result;
+      memcpy_s(&result, sizeof(SHRaycastResult), &baseResult, sizeof(SHRaycastResult));
+
+      result.entityHit  = EID;
+      result.shapeIndex = IDX;
+
+      results.emplace_back(result);
+    }
+
+    return results;
+  }
+
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHCollisionSpace::broadphaseQuery(SHRigidBody::Type rigidBodyType, SHCollider* collider) noexcept
+  {
+    for (auto* shape : collider->shapes)
+    {
+      // For kinematic shapes, we only query triggers against everything else
+      if (rigidBodyType == SHRigidBody::Type::KINEMATIC && !shape->IsTrigger())
+        continue;
+
+      auto& potentialCollisions = broadphase.Query(shape->id, shape->ComputeAABB());
+
+      // Build narrow-phase pairs
+      auto* shapeA = shape;
+
+      const EntityID ID_A     = shape->id.GetEntityID();
+      const uint32_t INDEX_A  = shape->id.GetShapeIndex();
+
+      for (auto& id : potentialCollisions)
+      {
+        // Get corresponding shape
+        const EntityID ID_B     = id.GetEntityID();
+        const uint32_t INDEX_B  = id.GetShapeIndex();
+
+        auto* shapeB = colliders[ID_B]->GetCollisionShape(INDEX_B);
+
+        // Build collision ID
+        SHCollisionKey collisionKey;
+        collisionKey.SetEntityA(ID_A);
+        collisionKey.SetEntityB(ID_B);
+        collisionKey.SetCollisionShapeA(INDEX_A);
+        collisionKey.SetCollisionShapeB(INDEX_B);
+
+        // Check if it already exists. If it doesn't, put into batch.
+        // The overloaded equality operator ensures no duplicate collision tests are performed.
+        auto narrowphasePair = narrowphaseBatch.find(collisionKey);
+        if (narrowphasePair == narrowphaseBatch.end())
+          narrowphaseBatch.emplace(collisionKey, NarrowphasePair{ shapeA, shapeB });
+      }
+    }
+  }
+
+  void SHCollisionSpace::collideTriggers(const SHCollisionKey& key, NarrowphasePair& narrowphasePair) const noexcept
+  {
+    auto* A = narrowphasePair.A;
+    auto* B = narrowphasePair.B;
+
+    // Send to contact manager
+    if (contactManager)
+      contactManager->AddTrigger(key, A, B);
+  }
+
+  void SHCollisionSpace::collideManifolds(const SHCollisionKey& key, NarrowphasePair& narrowphasePair) const noexcept
+  {
+    auto* A = narrowphasePair.A;
+    auto* B = narrowphasePair.B;
+
+    // Send to contact manager
+    if (contactManager)
+      contactManager->AddManifold(key, A, B);
+  }
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCollisionSpace.h

@@ -0,0 +1,196 @@
+/****************************************************************************************
+ * \file      SHCollisionSpace.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Collision Space that handles collision detetction.
+ *            This is to separate the logic between dynamics and collision detection,
+ *            but the collision space does send information to the contact manager
+ *            for dynamic resolution and collision state reporting.
+ *
+ * \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
+
+#include <optional>
+
+// Project Headers
+#include "Broadphase/SHDynamicAABBTree.h"
+#include "Physics/Dynamics/SHContactManager.h"
+#include "SHCollider.h"
+#include "SHPhysicsRaycastResult.h"
+#include "CollisionTags/SHCollisionTags.h"
+#include "CollisionTags/SHCollisionTags.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Allows collision detection to be performed with the use of colliders & collision shapes.
+   * The space will generate manifold data for resolution when needed.
+  */
+  class SH_API SHCollisionSpace
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Contains information to cast a ray into the collision space.
+     * The collider entityID and shape index is optional. 
+    */
+    struct RaycastInfo
+    {
+    private:
+      /*-------------------------------------------------------------------------------*/
+      /* Friends                                                                       */
+      /*-------------------------------------------------------------------------------*/
+
+      friend class SHCollisionSpace;
+
+    public:
+      /*-------------------------------------------------------------------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      bool      continuous  = false;
+      uint16_t  layers      = static_cast<uint16_t>(SHCollisionTag::Layer::ALL);
+      float     distance    = std::numeric_limits<float>::infinity();
+      SHRay     ray;
+
+      /*-------------------------------------------------------------------------------*/
+      /* Setter Functions                                                              */
+      /*-------------------------------------------------------------------------------*/
+
+      /**
+       * @brief
+       * Sets the collider ID for the raycast. Setting this specifies that the ray
+       * should ignore this collider.
+       * @param eid
+       * The entity ID of the collider.
+      */
+      void SetColliderID(EntityID eid) noexcept { colliderEntityID = eid; }
+
+    private:
+      /*-------------------------------------------------------------------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      std::optional<EntityID> colliderEntityID;
+    };
+
+    using RaycastResults = std::vector<SHPhysicsRaycastResult>;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionSpace  () noexcept = default;
+    ~SHCollisionSpace () noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    const SHAABBTree::AABBs&  GetBroadphaseAABBs  ()  const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void SetContactManager(SHContactManager* contactManager) noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Adds a collider to the collision space for it to be tested for collision with
+     * other colliders.
+     * @param collider
+     * A collider to add. Duplicates will be ignored.
+    */
+    void AddCollider (SHCollider*  collider)   noexcept;
+
+    /**
+     * @brief
+     * Removes a collider from the collision space. This will prevent any collisions
+     * being detected between it and other colliders unless manually tested.
+     * @param collider
+     * A collider to remove. If a reference to it doesn't exist, it will be ignored.
+    */
+    void RemoveCollider (SHCollider*  collider)   noexcept;
+
+    /**
+     * @brief
+     * Invoke this method to update the broadphase of colliders that have been moved since
+     * the last frame.
+    */
+    void UpdateBroadphase () noexcept;
+
+    /**
+     * @brief 
+     * Detects collisions between all colliders. Results are sent to the attached contact
+     * manager for resolution.
+    */
+    void DetectCollisions () noexcept;
+
+    /**
+     * @brief
+     * Casts a ray into the collision space.
+     * @param info
+     * Contains the information for the raycast.
+     * @return
+     * A container of the objects hit by the ray. If nothing was hit, the container
+     * will be empty.
+     */
+    [[nodiscard]] const RaycastResults& Raycast(const RaycastInfo& info) noexcept;
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct NarrowphasePair
+    {
+      SHCollisionShape* A = nullptr;
+      SHCollisionShape* B = nullptr;
+    };
+
+    using Colliders         = std::unordered_map<EntityID, SHCollider*>;
+    using NarrowphaseBatch  = std::unordered_map<SHCollisionKey, NarrowphasePair, SHCollisionKeyHash>;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    SHContactManager* contactManager = nullptr;
+
+    Colliders         colliders;
+    NarrowphaseBatch  narrowphaseBatch;
+
+    SHAABBTree        broadphase;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    // Broadphase helpers
+
+    void                    broadphaseQuery   (SHRigidBody::Type rigidBodyType, SHCollider* collider) noexcept;
+
+    // Narrowphase helpers
+
+    void                    collideTriggers   (const SHCollisionKey& key, NarrowphasePair& narrowphasePair) const noexcept;
+    void                    collideManifolds  (const SHCollisionKey& key, NarrowphasePair& narrowphasePair) const noexcept;
+  };
+  
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCompositeCollider.cpp

@@ -0,0 +1,176 @@
+/****************************************************************************************
+ * \file      SHCompositeCollider.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Composite Collider Class.
+ *
+ * \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 "SHCompositeCollider.h"
+
+// Project Headers
+#include "Broadphase/SHDynamicAABBTree.h"
+#include "Math/SHMathHelpers.h"
+#include "Physics/SHPhysicsEvents.h"
+#include "Physics/Dynamics/SHRigidBody.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCompositeCollider::SHCompositeCollider(EntityID eid, const SHTransform& worldTransform) noexcept
+  : SHCollider( eid, worldTransform )
+  {
+    flags |= COMPOSITE_FLAG;
+  }
+
+  SHCompositeCollider::SHCompositeCollider(const SHCompositeCollider& rhs) noexcept
+  : SHCollider( rhs )
+  {}
+
+  SHCompositeCollider::SHCompositeCollider(SHCompositeCollider&& rhs) noexcept
+  : SHCollider( rhs )
+  {}
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCompositeCollider& SHCompositeCollider::operator=(const SHCompositeCollider& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    SHCollider::operator=(rhs);
+
+    return *this;
+  }
+
+  SHCompositeCollider& SHCompositeCollider::operator=(SHCompositeCollider&& rhs) noexcept
+  {
+    SHCollider::operator=(rhs);
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  int SHCompositeCollider::AddSphereCollisionShape(float relativeRadius, const SHVec3& posOffset, const SHVec3& rotOffset)
+  {
+    if (!shapeLibrary)
+    {
+      SHLOGV_ERROR("Shape factory is unlinked with Collider {}. Unable to add new shape!", entityID)
+      return -1;
+    }
+
+    // Compute world radius
+    const float SPHERE_SCALE = std::fabs(SHMath::Max({ transform.scale.x, transform.scale.y, transform.scale.z }));
+
+    // Compute center
+    const SHQuaternion FINAL_ROT  = transform.orientation * SHQuaternion::FromEuler(rotOffset);
+    const SHMatrix     TRS        = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(transform.position);
+
+    // Create Sphere
+    const SHSphereCreateInfo SPHERE_CREATE_INFO
+    {
+      .Center         = SHVec3::Transform(posOffset, TRS)
+    , .Radius         = relativeRadius * SPHERE_SCALE * 0.5f
+    , .RelativeRadius = relativeRadius
+    , .Scale          = SPHERE_SCALE
+    };
+
+    const uint32_t NEW_INDEX = static_cast<uint32_t>(shapes.size());
+    const SHCollisionShapeID NEW_SHAPE_ID{ entityID, NEW_INDEX };
+
+    SHSphere* sphere = shapeLibrary->CreateSphere(NEW_SHAPE_ID, SPHERE_CREATE_INFO);
+
+    // Set offsets
+    sphere->collider = this;
+    sphere->SetPositionOffset(posOffset);
+    sphere->SetRotationOffset(rotOffset);
+
+    shapes.emplace_back(sphere);
+
+    if (broadphase)
+      broadphase->Insert(NEW_SHAPE_ID, sphere->ComputeAABB());
+
+    // Broadcast Event for adding a shape
+    const SHPhysicsColliderAddedEvent EVENT_DATA
+    {
+      .entityID       = entityID
+    , .colliderType   = SHCollisionShape::Type::SPHERE
+    , .colliderIndex  = static_cast<int>(NEW_INDEX)
+    };
+
+    SHEventManager::BroadcastEvent<SHPhysicsColliderAddedEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_ADDED_EVENT);
+
+    if (rigidBody)
+      rigidBody->ComputeMassData();
+
+    return static_cast<int>(NEW_INDEX);
+  }
+
+  int SHCompositeCollider::AddBoxCollisionShape(const SHVec3& relativeExtents, const SHVec3& posOffset, const SHVec3& rotOffset)
+  {
+    if (!shapeLibrary)
+    {
+      SHLOGV_ERROR("Shape factory is unlinked with Collider {}. Unable to add new shape!", entityID)
+      return -1;
+    }
+
+    // Compute center
+    const SHQuaternion FINAL_ROT  = transform.orientation * SHQuaternion::FromEuler(rotOffset);
+    const SHMatrix     TRS        = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(transform.position);
+
+    // Create Sphere
+    const SHBoxCreateInfo BOX_CREATE_INFO
+    {
+      .Center           = SHVec3::Transform(posOffset, TRS)
+    , .Extents          = relativeExtents * SHVec3::Abs(transform.scale) * 0.5f
+    , .RelativeExtents  = relativeExtents
+    , .Orientation      = FINAL_ROT
+    , .Scale            = SHVec3::Abs(transform.scale)
+    };
+
+    const uint32_t NEW_INDEX = static_cast<uint32_t>(shapes.size());
+    const SHCollisionShapeID NEW_SHAPE_ID{ entityID, NEW_INDEX };
+
+    SHBox* box = shapeLibrary->CreateBox(NEW_SHAPE_ID, BOX_CREATE_INFO);
+
+    // Set offsets
+    box->collider = this;
+    box->SetPositionOffset(posOffset);
+    box->SetRotationOffset(rotOffset);
+
+    shapes.emplace_back(box);
+
+    if (broadphase)
+      broadphase->Insert(NEW_SHAPE_ID, box->ComputeAABB());
+
+    // Broadcast Event for adding a shape
+    const SHPhysicsColliderAddedEvent EVENT_DATA
+    {
+      .entityID       = entityID
+    , .colliderType   = SHCollisionShape::Type::BOX
+    , .colliderIndex  = static_cast<int>(NEW_INDEX)
+    };
+
+    SHEventManager::BroadcastEvent<SHPhysicsColliderAddedEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_ADDED_EVENT);
+
+    if (rigidBody)
+      rigidBody->ComputeMassData();
+
+    return static_cast<int>(NEW_INDEX);
+  }
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCompositeCollider.h

@@ -0,0 +1,80 @@
+/****************************************************************************************
+ * \file      SHCompositeCollider.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Composite Collider Class.
+ *
+ * \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 "SHCollider.h"
+
+namespace SHADE
+{
+  /**
+   * @brief
+   * Encapsulates the behaviour of a collider with composited shapes. <br/>
+   * Contains no data members but methods to add multiple shapes.
+  */
+  class SH_API SHCompositeCollider : public SHCollider
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCompositeCollider(EntityID eid, const SHTransform& worldTransform = SHTransform::Identity) noexcept;
+    SHCompositeCollider(const SHCompositeCollider& rhs) noexcept;
+    SHCompositeCollider(SHCompositeCollider&& rhs) noexcept;
+    ~SHCompositeCollider () noexcept override = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+    
+    SHCompositeCollider& operator=(const SHCompositeCollider& rhs)  noexcept;
+    SHCompositeCollider& operator=(SHCompositeCollider&& rhs)       noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Adds a sphere collision shape.
+     * @param relativeRadius
+     * The relative radius is constructed with respect to the world scale. <br/>
+     * Radius = max(scale.x, scale.y, scale.z) * 0.5 * relativeRadius
+     * @param posOffset
+     * The position offset of the sphere from the center of the collider. Defaults to a Zero Vector.
+     * @param rotOffset
+     * The rotation offset of the sphere from the rotation of the collider. Defaults to a Zero Vector.
+     * @return
+     * The index of the newly added shape.
+    */
+    int AddSphereCollisionShape (float relativeRadius, const SHVec3& posOffset = SHVec3::Zero, const SHVec3& rotOffset = SHVec3::Zero);
+
+    /**
+     * @brief
+     * Adds a box collision shape.
+     * @param relativeExtents
+     * The relative extents are constructed with respect to the world scale. <br/>
+     * Extents = scale * 0.5 * relativeExtents
+     * @param posOffset
+     * The position offset of the box from the center of the collider. Defaults to a Zero Vector.
+     * @param rotOffset
+     * The rotation offset of the box from the rotation of the collider. Defaults to a Zero Vector.
+     * @return
+     * The index of the newly added shape.
+    */
+    int AddBoxCollisionShape (const SHVec3& relativeExtents, const SHVec3& posOffset = SHVec3::Zero, const SHVec3& rotOffset = SHVec3::Zero);
+
+    // TODO: Add Capsule
+  };
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHPhysicsMaterial.h

@@ -19,6 +19,10 @@ namespace SHADE
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
+  /**
+   * @brief
+   * Encapsulates the data of a physics material for physics simulations.
+  */
   class SH_API SHPhysicsMaterial
   {
   public:

SHADE_Engine/src/Physics/Collision/SHPhysicsRaycaster.cpp

@@ -1,350 +0,0 @@
-/****************************************************************************************
- * \file      SHPhysicsRaycaster.cpp
- * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Implementation for a Physics Raycaster.
- *
- * \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 "SHPhysicsRaycaster.h"
-
-/*
- * TODO(DIREN):
- * Once the physics engine has been rebuilt, this whole implementation should change
- * and just call PhysicsWorld.Raycast etc.
- *
- * SHRaycastResult can be converted to a bool when necessary.
- */
-
-namespace SHADE
-{
-  /*-----------------------------------------------------------------------------------*/
-  /* Constructors & Destructor Definitions                                             */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHPhysicsRaycaster::SHPhysicsRaycaster() noexcept
-  : world { nullptr }
-  {}
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Getter Function Definitions                                                       */
-  /*-----------------------------------------------------------------------------------*/
-
-  const SHPhysicsRaycaster::RaycastPairs& SHPhysicsRaycaster::GetRaycasts() const noexcept
-  {
-    return raycasts;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Getter Function Definitions                                                       */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHPhysicsRaycaster::SetObjectManager(SHPhysicsObjectManager* physicsObjectManager) noexcept
-  {
-    objectManager = physicsObjectManager;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Public Function Member Definitions                                                */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHPhysicsRaycaster::BindToWorld(rp3d::PhysicsWorld* physicsWorld) noexcept
-  {
-    world = physicsWorld;
-  }
-
-  void SHPhysicsRaycaster::ClearFrame() noexcept
-  {
-    raycasts.clear();
-  }
-
-  SHPhysicsRaycastResult SHPhysicsRaycaster::Raycast(const SHRay& ray, float distance, const SHCollisionTag& collisionTag) noexcept
-  {
-    // Reset temp
-    temp = SHPhysicsRaycastResult{};
-    temp.distance = distance;
-
-    if (!world)
-    {
-      SHLOG_ERROR("Physics world missing for raycasting!")
-      return temp;
-    }
-
-    // If distance in infinity, cast to the default max distance of 2 km.
-    if (distance == std::numeric_limits<float>::infinity())
-    {
-      world->raycast(ray, this, collisionTag);
-    }
-    else
-    {
-      const SHVec3 END_POINT = ray.position + ray.direction * distance;
-      const rp3d::Ray RP3D_RAY{ ray.position, END_POINT };
-      world->raycast(RP3D_RAY, this, collisionTag);
-    }
-
-    // If a hit was found, populate temp info for return.
-    if (temp.hit)
-    {
-      temp.distance = SHVec3::Distance(ray.position, temp.position);
-      temp.angle    = SHVec3::Angle(ray.position, temp.position);
-    }
-
-    raycasts.emplace_back(ray, temp);
-    return temp;
-  }
-
-  SHPhysicsRaycastResult SHPhysicsRaycaster::Linecast(const SHVec3& start, const SHVec3& end, const SHCollisionTag& collisionTag) noexcept
-  {
-    temp = SHPhysicsRaycastResult{};
-    temp.distance = SHVec3::Distance(start, end);
-
-    if (!world)
-    {
-      SHLOG_ERROR("Physics world missing for raycasting!")
-      return temp;
-    }
-
-    const rp3d::Ray RP3D_RAY{ start, end };
-    world->raycast(RP3D_RAY, this, collisionTag);
-
-    if (temp.hit)
-    {
-      temp.distance = SHVec3::Distance(start, temp.position);
-      temp.angle    = SHVec3::Angle(start, temp.position);
-    }
-    
-    raycasts.emplace_back(RP3D_RAY, temp);
-    return temp;
-  }
-
-  SHPhysicsRaycastResult SHPhysicsRaycaster::ColliderRaycast(EntityID eid, const SHRay& ray, float distance) noexcept
-  {
-    SHPhysicsRaycastResult result;
-    result.distance = distance;
-
-    // Get a valid physics object with at least 1 collider.
-    const auto* PHYSICS_OBJECT = validateColliderRaycast(eid);
-    if (!PHYSICS_OBJECT)
-      return result;
-
-    auto* rp3dBody = PHYSICS_OBJECT->GetCollisionBody();
-
-    // Data to populate
-    rp3d::RaycastInfo rp3dRaycastInfo;
-    bool hit = false;
-
-    if (distance == std::numeric_limits<float>::infinity())
-    {
-      hit = rp3dBody->raycast(ray, rp3dRaycastInfo);
-    }
-    else
-    {
-      const SHVec3 END_POINT = ray.position + ray.direction * distance;
-      const rp3d::Ray RP3D_RAY{ ray.position, END_POINT };
-      hit = rp3dBody->raycast(RP3D_RAY, rp3dRaycastInfo);
-    }
-
-    if (hit)
-    {
-      result.hit        = true;
-      result.position   = rp3dRaycastInfo.worldPoint;
-      result.normal     = rp3dRaycastInfo.worldPoint;
-      result.distance   = SHVec3::Distance(ray.position, result.position);
-      result.angle      = SHVec3::Angle(ray.position, result.position);
-      result.entityHit  = eid;
-      result.shapeIndex = findColliderIndex(rp3dBody, rp3dRaycastInfo.collider->getEntity());
-    }
-
-    raycasts.emplace_back(ray, result);
-    return result;
-  }
-
-  SHPhysicsRaycastResult SHPhysicsRaycaster::ColliderRaycast(EntityID eid, int shapeIndex, const SHRay& ray, float distance) noexcept
-  {
-    SHPhysicsRaycastResult result;
-    result.distance = distance;
-
-    // Get a valid physics object with at least 1 collider.
-    const auto* PHYSICS_OBJECT = validateColliderRaycast(eid);
-    if (!PHYSICS_OBJECT)
-      return result;
-
-    // Boundary check for shape index
-    if (shapeIndex < 0 || shapeIndex >= static_cast<int>(PHYSICS_OBJECT->GetCollisionBody()->getNbColliders()))
-    {
-      SHLOGV_WARNING("Invalid collision shape index passed in")
-      return result;
-    }
-
-    auto* rp3dCollider = PHYSICS_OBJECT->GetCollisionBody()->getCollider(shapeIndex);
-
-    rp3d::RaycastInfo rp3dRaycastInfo;
-    bool hit = false;
-    if (distance == std::numeric_limits<float>::infinity())
-    {
-      hit = rp3dCollider->raycast(ray, rp3dRaycastInfo);
-    }
-    else
-    {
-      const SHVec3 END_POINT = ray.position + ray.direction * distance;
-      const rp3d::Ray RP3D_RAY{ ray.position, END_POINT };
-      hit = rp3dCollider->raycast(RP3D_RAY, rp3dRaycastInfo);
-    }
-
-    if (hit)
-    {
-      result.hit        = true;
-      result.position   = rp3dRaycastInfo.worldPoint;
-      result.normal     = rp3dRaycastInfo.worldPoint;
-      result.distance   = SHVec3::Distance(ray.position, result.position);
-      result.angle      = SHVec3::Angle(ray.position, result.position);
-      result.entityHit  = eid;
-      result.shapeIndex = shapeIndex;
-    }
-
-    raycasts.emplace_back(ray, result);
-    return result;
-  }
-
-   SHPhysicsRaycastResult SHPhysicsRaycaster::ColliderLinecast(EntityID eid, const SHVec3& start, const SHVec3& end) noexcept
-  {
-    SHPhysicsRaycastResult result;
-    result.distance = SHVec3::Distance(start, end);
-
-    const auto* PHYSICS_OBJECT = validateColliderRaycast(eid);
-    if (!PHYSICS_OBJECT)
-      return result;
-
-    auto* rp3dBody = PHYSICS_OBJECT->GetCollisionBody();
-
-    rp3d::RaycastInfo rp3dRaycastInfo;
-
-    const rp3d::Ray RP3D_RAY{ start, end };
-    if (rp3dBody->raycast(RP3D_RAY, rp3dRaycastInfo))
-    {
-      result.hit        = true;
-      result.position   = rp3dRaycastInfo.worldPoint;
-      result.normal     = rp3dRaycastInfo.worldPoint;
-      result.distance   = SHVec3::Distance(start, result.position);
-      result.angle      = SHVec3::Angle(end, result.position);
-      result.entityHit  = eid;
-      result.shapeIndex = findColliderIndex(rp3dBody, rp3dRaycastInfo.collider->getEntity());
-    }
-
-    raycasts.emplace_back(RP3D_RAY, result);
-    return result;
-  }
-
-  SHPhysicsRaycastResult SHPhysicsRaycaster::ColliderLinecast(EntityID eid, int shapeIndex, const SHVec3& start, const SHVec3& end) noexcept
-  {
-    SHPhysicsRaycastResult result;
-    result.distance = SHVec3::Distance(start, end);
-
-    const auto* PHYSICS_OBJECT = validateColliderRaycast(eid);
-    if (!PHYSICS_OBJECT)
-      return result;
-
-    if (shapeIndex < 0 || shapeIndex >= static_cast<int>(PHYSICS_OBJECT->GetCollisionBody()->getNbColliders()))
-    {
-      SHLOGV_WARNING("Invalid collision shape index passed in")
-      return result;
-    }
-
-    auto* rp3dCollider = PHYSICS_OBJECT->GetCollisionBody()->getCollider(shapeIndex);
-
-    rp3d::RaycastInfo rp3dRaycastInfo;
-
-    const rp3d::Ray RP3D_RAY{ start, end };
-    if (rp3dCollider->raycast(RP3D_RAY, rp3dRaycastInfo))
-    {
-      result.hit        = true;
-      result.position   = rp3dRaycastInfo.worldPoint;
-      result.normal     = rp3dRaycastInfo.worldPoint;
-      result.distance   = SHVec3::Distance(start, result.position);
-      result.angle      = SHVec3::Angle(end, result.position);
-      result.entityHit  = eid;
-      result.shapeIndex = shapeIndex;
-    }
-
-    raycasts.emplace_back(RP3D_RAY, result);
-    return result;
-  }
-
-  rp3d::decimal SHPhysicsRaycaster::notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo)
-  {
-    temp.hit      = true;
-    temp.position = raycastInfo.worldPoint;
-    temp.normal   = raycastInfo.worldNormal;
-
-    if (!objectManager)
-    {
-      SHLOGV_ERROR("No physics object manager linked with raycaster to match bodies")
-      return 0.0f;
-    }
-
-    // Compare body IDs to find the matching physics object
-    const auto HIT_BODY_EID = raycastInfo.body->getEntity();
-
-    for (const auto& [entityID, physicsObject] : objectManager->GetPhysicsObjects())
-    {
-      const auto RP3D_BODY = physicsObject.GetCollisionBody();
-
-      // Match rp3d bodies
-      if (RP3D_BODY->getEntity() != HIT_BODY_EID)
-        continue;
-
-      temp.entityHit = entityID;
-
-      // Find collider index
-      if (const int INDEX = findColliderIndex(RP3D_BODY, raycastInfo.collider->getEntity()); INDEX > -1)
-      {
-        temp.shapeIndex = INDEX;
-        break;
-      }
-    }
-
-    return 0.0f;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Private Function Member Definitions                                               */
-  /*-----------------------------------------------------------------------------------*/
-
-  SHPhysicsObject* SHPhysicsRaycaster::validateColliderRaycast(EntityID eid) noexcept
-  {
-    if (!objectManager)
-    {
-      SHLOGV_ERROR("No physics object manager linked with raycaster to match bodies")
-      return nullptr;
-    }
-
-    auto* physicsObject = objectManager->GetPhysicsObject(eid);
-    if (!physicsObject || physicsObject->GetCollisionBody()->getNbColliders() == 0)
-    {
-      SHLOGV_WARNING("Cannot cast ray at an entity without colliders!")
-      return nullptr;
-    }
-
-    return physicsObject;
-  }
-
-  int SHPhysicsRaycaster::findColliderIndex(const rp3d::CollisionBody* rp3dBody, rp3d::Entity rp3dColliderEID) noexcept
-  {
-    const int NUM_COLLISION_SHAPES = static_cast<int>(rp3dBody->getNbColliders());
-    for (int i = 0; i < NUM_COLLISION_SHAPES; ++i)
-    {
-      const auto COLLIDER_EID = rp3dBody->getCollider(i)->getEntity();
-      if (COLLIDER_EID == rp3dColliderEID)
-        return i;
-    }
-
-    return -1;
-  }
-
-
-} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHPhysicsRaycaster.h

@@ -1,134 +0,0 @@
-/****************************************************************************************
- * \file      SHPhysicsRaycaster.h
- * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a Physics Raycaster.
- *
- * \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
-
-#include <vector>
-
-#include <reactphysics3d/reactphysics3d.h>
-
-// Project Headers
-#include "Math/SHRay.h"
-#include "Physics/PhysicsObject/SHPhysicsObjectManager.h"
-#include "Physics/SHPhysicsWorld.h"
-#include "SH_API.h"
-#include "SHCollisionTags.h"
-#include "SHPhysicsRaycastResult.h"
-
-namespace SHADE
-{
-  /*-----------------------------------------------------------------------------------*/
-  /* Type Definitions                                                                  */
-  /*-----------------------------------------------------------------------------------*/
-
-  class SH_API SHPhysicsRaycaster : public reactphysics3d::RaycastCallback
-  {
-  private:
-    /*---------------------------------------------------------------------------------*/
-    /* Type Definitions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    using RaycastPair   = std::pair<SHRay, SHPhysicsRaycastResult>;
-    using RaycastPairs  = std::vector<RaycastPair>;
-
-  public:
-    /*---------------------------------------------------------------------------------*/
-    /* Constructors & Destructor                                                       */
-    /*---------------------------------------------------------------------------------*/
-
-    SHPhysicsRaycaster() noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Getter Functions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    [[nodiscard]] const RaycastPairs& GetRaycasts() const noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Setter Functions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    void SetObjectManager(SHPhysicsObjectManager* physicsObjectManager) noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    void BindToWorld  (rp3d::PhysicsWorld* physicsWorld)  noexcept;
-    void ClearFrame   ()                                  noexcept;
-
-    // TODO(Diren): Filtering, return all shades ray hits
-
-    SHPhysicsRaycastResult  Raycast
-    (
-      const SHRay&          ray
-    , float                 distance = std::numeric_limits<float>::infinity()
-    , const SHCollisionTag& collisionTag = SHCollisionTag{}
-    ) noexcept;
-
-    SHPhysicsRaycastResult  Linecast
-    (
-      const SHVec3&         start
-    , const SHVec3&         end
-    , const SHCollisionTag& collisionTag = SHCollisionTag{}
-    ) noexcept;
-
-    SHPhysicsRaycastResult  ColliderRaycast
-    (
-      EntityID              eid
-    , const SHRay&          ray
-    , float                 distance = std::numeric_limits<float>::infinity()
-    ) noexcept;
-
-    SHPhysicsRaycastResult  ColliderRaycast
-    (
-      EntityID              eid
-    , int                   shapeIndex
-    , const SHRay&          ray
-    , float                 distance = std::numeric_limits<float>::infinity()
-    ) noexcept;
-
-    SHPhysicsRaycastResult  ColliderLinecast
-    (
-      EntityID              eid
-    , const SHVec3&         start
-    , const SHVec3&         end
-    ) noexcept;
-
-    SHPhysicsRaycastResult  ColliderLinecast
-    (
-      EntityID              eid
-    , int                   shapeIndex
-    , const SHVec3&         start
-    , const SHVec3&         end
-    ) noexcept;
-
-    rp3d::decimal notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) override;
-
-  private:
-    /*---------------------------------------------------------------------------------*/
-    /* Data Members                                                                    */
-    /*---------------------------------------------------------------------------------*/
-
-    rp3d::PhysicsWorld*     world;  
-    SHPhysicsObjectManager* objectManager;  // For
-    SHPhysicsRaycastResult  temp;           // Holds the temporary result after casting into the world
-    RaycastPairs            raycasts;       // Used for debug drawing
-
-    /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    SHPhysicsObject*  validateColliderRaycast (EntityID eid) noexcept;
-    static int        findColliderIndex       (const rp3d::CollisionBody* rp3dBody, rp3d::Entity rp3dColliderEID) noexcept;
-  };
-
-  
-} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHBox.cpp

@@ -0,0 +1,298 @@
+/****************************************************************************************
+ * \file      SHBoxCollisionShape.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Box Collision Shape.
+ *
+ * \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 "SHBox.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Math/SHMatrix.h"
+#include "Physics/Collision/SHCollider.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHBox::SHBox(SHCollisionShapeID id) noexcept
+  : SHConvexPolyhedron  (id, Type::BOX)
+  , relativeExtents     { SHVec3::One }
+  , scale               { SHVec3::One }
+  {
+    Extents = SHVec3::One * 0.5f;
+  }
+
+  SHBox::SHBox(const SHBox& rhs) noexcept
+  : SHConvexPolyhedron  ( rhs )
+  , relativeExtents     { rhs.relativeExtents }
+  , scale               { rhs.scale }
+  {
+    Center  = rhs.Center;
+    Extents = rhs.Extents;
+    Orientation = rhs.Orientation;
+  }
+
+  SHBox::SHBox(SHBox&& rhs) noexcept
+  : SHConvexPolyhedron  ( rhs )
+  , relativeExtents     { rhs.relativeExtents }
+  , scale               { rhs.scale }
+  {
+    Center  = rhs.Center;
+    Extents = rhs.Extents;
+    Orientation = rhs.Orientation;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHBox& SHBox::operator=(const SHBox& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    // Collision Shape Properties
+
+    SHConvexPolyhedron::operator=(rhs);
+
+    // Box Properties
+
+    Center            = rhs.Center;
+    Extents           = rhs.Extents;
+    Orientation       = rhs.Orientation;
+
+    // Local Properties
+
+    relativeExtents   = rhs.relativeExtents;
+    scale             = rhs.scale;
+
+    return *this;
+  }
+
+  SHBox& SHBox::operator=(SHBox&& rhs) noexcept
+  {
+    // Collision Shape Properties
+
+    SHConvexPolyhedron::operator=(rhs);
+
+    // Box Properties
+
+    Center            = rhs.Center;
+    Extents           = rhs.Extents;
+    Orientation       = rhs.Orientation;
+
+    // Local Properties
+
+    relativeExtents   = rhs.relativeExtents;
+    scale             = rhs.scale;
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHVec3 SHBox::GetWorldExtents() const noexcept
+  {
+    return Extents;
+  }
+
+  SHVec3 SHBox::GetRelativeExtents() const noexcept
+  {
+    return relativeExtents;
+  }
+
+  int32_t SHBox::GetNumVertices() const noexcept
+  {
+    return NUM_VERTICES;
+  }
+
+  SHVec3 SHBox::GetVertex(int index) const
+  {
+    if (index < 0 || index >= NUM_VERTICES)
+      throw std::invalid_argument("Index out-of-range!");
+
+    SHVec3 vertices[NUM_VERTICES];
+    GetCorners(vertices);
+
+    return vertices[index];
+  }
+
+  SHVec3 SHBox::GetNormal(int faceIndex) const
+  {
+    // Get local normal
+    const SHVec3& LOCAL_NORMAL = halfEdgeStructure->GetFace(faceIndex).normal;
+
+    // Rotate normal into world space
+    return SHVec3::Rotate(LOCAL_NORMAL, Orientation);
+  }
+
+  SHVec3 SHBox::GetWorldCentroid() const noexcept
+  {
+    return Center;
+  }
+
+  SHVec3 SHBox::GetRelativeCentroid() const noexcept
+  {
+    if (collider)
+      return SHVec3{ Center }  - collider->GetPosition();
+
+    return Center;
+  }
+
+  SHVec3 SHBox::GetLocalCentroid() const noexcept
+  {
+    return SHVec3::Zero;
+  }
+
+  SHQuaternion SHBox::GetWorldOrientation() const noexcept
+  {
+    return Orientation;
+  }
+
+  SHQuaternion SHBox::GetRelativeOrientation() const noexcept
+  {
+    return transform.orientation;
+  }
+
+  float SHBox::GetVolume() const noexcept
+  {
+    return 8.0f * (Extents.x * Extents.y * Extents.z);
+  }
+
+  float SHBox::GetSurfaceArea() const noexcept
+  {
+    return 8.0f * (Extents.x * Extents.y
+      + Extents.x * Extents.z 
+      + Extents.y * Extents.z);
+  }
+
+  SHMatrix SHBox::GetInertiaTensor(float mass) const noexcept
+  {
+    static constexpr float ONE_OVER_TWELVE = (1.0f / 12.0f);
+
+    const float WIDTH   = 2.0f * Extents.x;
+    const float HEIGHT  = 2.0f * Extents.y;
+    const float DEPTH   = 2.0f * Extents.z;
+
+    const float WIDTH_SQUARED  = WIDTH * WIDTH;
+    const float HEIGHT_SQUARED = HEIGHT * HEIGHT;
+    const float DEPTH_SQUARED  = DEPTH * DEPTH;
+
+    const float H2_PLUS_D2 = HEIGHT_SQUARED + DEPTH_SQUARED;
+    const float W2_PLUS_H2 = WIDTH_SQUARED + HEIGHT_SQUARED;
+    const float W2_PLUS_D2 = WIDTH_SQUARED + DEPTH_SQUARED;
+
+    SHMatrix result;
+    result.m[0][0] = ONE_OVER_TWELVE * mass * H2_PLUS_D2;
+    result.m[1][1] = ONE_OVER_TWELVE * mass * W2_PLUS_H2;
+    result.m[2][2] = ONE_OVER_TWELVE * mass * W2_PLUS_D2;
+    return result;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHBox::SetWorldExtents(const SHVec3& newWorldExtents) noexcept
+  {
+    Extents = newWorldExtents;
+
+    // Recompute Relative radius
+    relativeExtents = 2.0f * Extents / scale;
+  }
+
+  void SHBox::SetRelativeExtents(const SHVec3& newRelativeExtents) noexcept
+  {
+    relativeExtents = newRelativeExtents;
+
+    // Recompute world radius
+    Extents = relativeExtents * scale * 0.5f;
+  }
+
+  void SHBox::SetScale(const SHVec3& newScale) noexcept
+  {
+    scale = SHVec3::Abs(newScale);
+
+    // Recompute world radius
+    Extents = relativeExtents * scale * 0.5f;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHBox::Update() noexcept
+  {
+    const SHTransform& PARENT_TRANSFORM = collider->GetTransform();
+
+    SetScale(PARENT_TRANSFORM.scale);
+
+    // Recompute center
+    const SHQuaternion  FINAL_ROT  = PARENT_TRANSFORM.orientation * transform.orientation;
+    const SHMatrix      TRS        = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(PARENT_TRANSFORM.position);
+
+    Orientation = FINAL_ROT;
+    Center      = SHVec3::Transform(transform.position, TRS);
+  }
+
+  bool SHBox::TestPoint(const SHVec3& point) const noexcept
+  {
+    return Contains(point);
+  }
+
+  SHRaycastResult SHBox::Raycast(const SHRay& ray) const noexcept
+  {
+    SHRaycastResult result;
+
+    result.hit = Intersects(ray.position, ray.direction, result.distance);
+    if (result.hit)
+    {
+      result.position = ray.position + ray.direction * result.distance;
+      result.angle    = SHVec3::Angle(ray.position, result.position);
+
+      // TODO: Compute Normal: Test which face the position belongs in. The normal is that face's normal.
+    }
+
+    return result;
+  }
+
+  SHMatrix SHBox::GetTRS() const noexcept
+  {
+    const SHQuaternion  ROTATION  = collider ? collider->GetTransform().orientation * transform.orientation : Orientation;
+    const SHVec3        SCALE     = SHVec3{ Extents } *2.0f;
+
+    return SHMatrix::Transform(Center, ROTATION, SCALE);
+  }
+
+  SHAABB SHBox::ComputeAABB() const noexcept
+  {
+    SHVec3 min{ std::numeric_limits<float>::max() };
+    SHVec3 max{ std::numeric_limits<float>::lowest() };
+
+    SHVec3 vertices[NUM_VERTICES];
+    GetCorners(vertices);
+
+    for (auto& vertex : vertices)
+    {
+      min = SHVec3::Min({ vertex, min });
+      max = SHVec3::Max({ vertex, max });
+    }
+
+    const SHVec3  HALF_EXTENTS  = (max - min) * 0.5f;
+    const SHVec3  CENTROID      = min + HALF_EXTENTS;
+
+    return SHAABB{ CENTROID, HALF_EXTENTS };
+  }
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHBox.h

@@ -0,0 +1,129 @@
+/****************************************************************************************
+ * \file      SHBox.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Box Collision Shape.
+ *
+ * \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
+
+#include <DirectXCollision.h>
+
+// Project Headers
+#include "SHConvexPolyhedron.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates the information to create a box.
+  */
+  struct SHBoxCreateInfo
+  {
+  public:
+    SHVec3        Center          = SHVec3::Zero;
+    SHVec3        Extents         = SHVec3::One * 0.5f;
+    SHVec3        RelativeExtents = SHVec3::One;
+    SHQuaternion  Orientation     = SHQuaternion::Identity;
+    SHVec3        Scale           = SHVec3::One;
+  };
+
+  /**
+   * @brief
+   * Encapsulate a Box Shape used for Physics Simulations.
+  */
+  class SH_API SHBox final  : public SHConvexPolyhedron
+                            , private DirectX::BoundingOrientedBox
+  {
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Friends                                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    friend class SHCollider;
+    friend class SHCompositeCollider;
+    friend class SHCollision;
+    friend class SHCollisionShapeLibrary;
+
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    static constexpr int NUM_VERTICES = 8;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHBox  (SHCollisionShapeID id)          noexcept;
+    SHBox  (const SHBox& rhs) noexcept;
+    SHBox  (SHBox&& rhs)      noexcept;
+
+    ~SHBox () override = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHBox& operator=  (const SHBox& rhs)  noexcept;
+    SHBox& operator=  (SHBox&& rhs)       noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] SHVec3        GetWorldExtents         ()              const noexcept;
+    [[nodiscard]] SHVec3        GetRelativeExtents      ()              const noexcept;
+
+    // Overriden Methods
+
+    [[nodiscard]] int32_t       GetNumVertices          ()              const noexcept override;
+    [[nodiscard]] SHVec3        GetVertex               (int index)     const override;
+    [[nodiscard]] SHVec3        GetNormal               (int faceIndex) const override;
+
+    [[nodiscard]] SHVec3        GetWorldCentroid        ()              const noexcept override;
+    [[nodiscard]] SHVec3        GetRelativeCentroid     ()              const noexcept override;
+    [[nodiscard]] SHVec3        GetLocalCentroid        ()              const noexcept override;
+    [[nodiscard]] SHQuaternion  GetWorldOrientation     ()              const noexcept override;
+    [[nodiscard]] SHQuaternion  GetRelativeOrientation  ()              const noexcept override;
+    [[nodiscard]] float         GetVolume               ()              const noexcept override;
+    [[nodiscard]] float         GetSurfaceArea          ()              const noexcept override;
+    [[nodiscard]] SHMatrix      GetInertiaTensor        (float mass)    const noexcept override;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void SetWorldExtents    (const SHVec3& newWorldExtents)         noexcept;
+    void SetRelativeExtents (const SHVec3& newRelativeExtents)      noexcept;
+    void SetScale           (const SHVec3& newScale)                noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Function Members                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void                          Update            ()                        noexcept override;
+    [[nodiscard]] bool            TestPoint         (const SHVec3& point)     const noexcept override;
+    [[nodiscard]] SHRaycastResult Raycast           (const SHRay& ray)        const noexcept override;
+    [[nodiscard]] SHMatrix        GetTRS            ()                        const noexcept override;
+    [[nodiscard]] SHAABB          ComputeAABB       ()                        const noexcept override;
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    SHVec3  relativeExtents;
+    SHVec3  scale;
+  };
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShape.cpp

@@ -0,0 +1,188 @@
+/****************************************************************************************
+ * \file      SHCollider.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Collider.
+ *
+ * \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 "SHCollisionShape.h"
+
+// Project Headers
+#include "Physics/Collision/SHCollider.h"
+#include "Physics/Collision/CollisionTags/SHCollisionTagMatrix.h"
+#include "Reflection/SHReflectionMetadata.h"
+#include "Tools/Utilities/SHUtilities.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionShape::SHCollisionShape(SHCollisionShapeID id, Type colliderType)
+  : id              { id }
+  , flags           { 0 }
+  , collider        { nullptr }
+  , collisionTag    { SHCollisionTagMatrix::GetTag(0) }
+  {
+    flags |= 1U << SHUtilities::ConvertEnum(colliderType);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  EntityID SHCollisionShape::GetEntityID() const noexcept
+  {
+    return id.GetEntityID();
+  }
+
+  uint32_t SHCollisionShape::GetIndex() const noexcept
+  {
+    return id.GetShapeIndex();
+  }
+
+  float SHCollisionShape::GetFriction() const noexcept
+  {
+    return material.GetFriction();
+  }
+
+  float SHCollisionShape::GetBounciness() const noexcept
+  {
+    return material.GetBounciness();
+  }
+
+  float SHCollisionShape::GetDensity() const noexcept
+  {
+    return material.GetDensity();
+  }
+
+  const SHPhysicsMaterial& SHCollisionShape::GetMaterial() const noexcept
+  {
+    return material;
+  }
+
+  const SHVec3& SHCollisionShape::GetPositionOffset() const noexcept
+  {
+    return transform.position;
+  }
+
+  const SHVec3& SHCollisionShape::GetRotationOffset() const noexcept
+  {
+    return rotationOffset;
+  }
+
+  SHCollisionShape::Type SHCollisionShape::GetType() const noexcept
+  {
+    for (int i = 0; i < SHUtilities::ConvertEnum(Type::COUNT); ++i)
+    {
+      const uint8_t FLAG_VALUE = 1U << SHUtilities::ConvertEnum(static_cast<Type>(i));
+
+      if (flags & FLAG_VALUE)
+        return static_cast<Type>(i);
+    }
+
+    return Type::INVALID;
+  }
+
+  bool SHCollisionShape::IsTrigger() const noexcept
+  {
+    static constexpr int FLAG_POS = 3;
+    static constexpr uint8_t FLAG_VALUE = 1U << FLAG_POS;
+
+    return flags & FLAG_VALUE;
+  }
+
+  bool SHCollisionShape::IsColliding() const noexcept
+  {
+    static constexpr int FLAG_POS = 4;
+    static constexpr uint8_t FLAG_VALUE = 1U << FLAG_POS;
+
+    return flags & FLAG_VALUE;
+  }
+
+  const SHCollisionTag& SHCollisionShape::GetCollisionTag() const noexcept
+  {
+    return *collisionTag;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHCollisionShape::SetCollisionTag(SHCollisionTag* newCollisionTag) noexcept
+  {
+    collisionTag = newCollisionTag;
+  }
+
+  void SHCollisionShape::SetFriction(float friction) noexcept
+  {
+    material.SetFriction(friction);
+  }
+
+  void SHCollisionShape::SetBounciness(float bounciness) noexcept
+  {
+    material.SetBounciness(bounciness);
+  }
+
+  void SHCollisionShape::SetDensity(float density) noexcept
+  {
+    material.SetDensity(density);
+  }
+
+  void SHCollisionShape::SetMaterial(const SHPhysicsMaterial& newMaterial) noexcept
+  {
+    material = newMaterial;
+  }
+
+  void SHCollisionShape::SetPositionOffset(const SHVec3& posOffset) noexcept
+  {
+    transform.position = posOffset;
+
+    Update();
+  }
+
+  void SHCollisionShape::SetRotationOffset(const SHVec3& rotOffset) noexcept
+  {
+    rotationOffset = rotOffset;
+    transform.orientation = SHQuaternion::FromEuler(rotationOffset);
+
+    Update();
+  }
+
+  void SHCollisionShape::SetIsTrigger(bool isTrigger) noexcept
+  {
+    static constexpr int FLAG_POS = 3;
+    static constexpr uint8_t FLAG_VALUE = 1U << FLAG_POS;
+
+    isTrigger ? flags |= FLAG_VALUE : flags &= ~FLAG_VALUE;
+  }
+
+} // namespace SHADE
+
+RTTR_REGISTRATION
+{
+  using namespace SHADE;
+  using namespace rttr;
+
+  registration::enumeration<SHCollisionShape::Type>("Collider Type")
+  (
+    value("Box",     SHCollisionShape::Type::BOX),
+    value("Sphere",  SHCollisionShape::Type::SPHERE)
+    // TODO(Diren): Add More Shapes
+  );
+
+  registration::class_<SHCollisionShape>("Collider")
+    .property("IsTrigger"       , &SHCollisionShape::IsTrigger        , &SHCollisionShape::SetIsTrigger     )
+    .property("Friction"        , &SHCollisionShape::GetFriction      , &SHCollisionShape::SetFriction      )
+    .property("Bounciness"      , &SHCollisionShape::GetBounciness    , &SHCollisionShape::SetBounciness    )
+    .property("Density"         , &SHCollisionShape::GetDensity       , &SHCollisionShape::SetDensity       )
+    .property("Position Offset" , &SHCollisionShape::GetPositionOffset, &SHCollisionShape::SetPositionOffset)
+    .property("Rotation Offset" , &SHCollisionShape::GetRotationOffset, &SHCollisionShape::SetRotationOffset) (metadata(META::angleInRad, true));
+}

SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShape.h

@@ -0,0 +1,210 @@
+/****************************************************************************************
+ * \file      SHCollisionShape.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Base CollisionShape Class.
+ *
+ * \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
+
+#include <rttr/registration>
+
+// Project Headers
+#include "ECS_Base/Entity/SHEntity.h"
+#include "Physics/Collision/CollisionTags/SHCollisionTags.h"
+#include "Physics/Collision/SHPhysicsMaterial.h"
+#include "SHCollisionShapeID.h"
+#include "Math/Geometry/SHAABB.h"
+#include "Math/Transform/SHTransform.h"
+#include "Physics/Collision/SHPhysicsRaycastResult.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Forward Declarations                                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  class SHRigidBody;
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  class SH_API SHCollisionShape
+  {
+  private:
+
+    /*---------------------------------------------------------------------------------*/
+    /* Friends                                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    friend class  SHCollider;
+    friend class  SHCompositeCollider;
+    friend class  SHColliderComponent;
+    friend class  SHCollisionShapeLibrary;
+    friend class  SHCollisionSpace;
+    friend struct SHManifold;
+
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    enum class Type
+    {
+      SPHERE
+    , BOX
+    , CAPSULE
+    , CONVEX_HULL
+
+    , COUNT
+    , INVALID = -1
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionShape          (SHCollisionShapeID id, Type colliderType = Type::SPHERE);
+
+    SHCollisionShape          (const SHCollisionShape& rhs) noexcept = default;
+    SHCollisionShape          (SHCollisionShape&& rhs)      noexcept = default;
+    virtual ~SHCollisionShape ()                            noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionShape& operator=(const SHCollisionShape& rhs)  noexcept = default;
+    SHCollisionShape& operator=(SHCollisionShape&& rhs)       noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] EntityID                  GetEntityID             ()            const noexcept;
+    [[nodiscard]] uint32_t                  GetIndex                ()            const noexcept;
+
+    // Material Properties
+    // TODO: Remove individual setters once instanced materials are supported
+
+    [[nodiscard]] float                     GetFriction             ()            const noexcept;
+    [[nodiscard]] float                     GetBounciness           ()            const noexcept;
+    [[nodiscard]] float                     GetDensity              ()            const noexcept;
+    [[nodiscard]] const SHPhysicsMaterial&  GetMaterial             ()            const noexcept;
+
+    // Offsets
+
+    [[nodiscard]] const SHVec3&             GetPositionOffset       ()            const noexcept;
+    [[nodiscard]] const SHVec3&             GetRotationOffset       ()            const noexcept;
+
+    // Flags
+
+    [[nodiscard]] Type                      GetType                 ()            const noexcept;
+    [[nodiscard]] bool                      IsTrigger               ()            const noexcept;
+    [[nodiscard]] bool                      IsColliding             ()            const noexcept;
+
+    [[nodiscard]] const SHCollisionTag&     GetCollisionTag         ()            const noexcept;
+
+    // Virtual methods
+
+    [[nodiscard]] virtual SHVec3            GetWorldCentroid        ()            const noexcept = 0;
+    [[nodiscard]] virtual SHVec3            GetRelativeCentroid     ()            const noexcept = 0;
+    [[nodiscard]] virtual SHVec3            GetLocalCentroid        ()            const noexcept = 0;
+    [[nodiscard]] virtual SHQuaternion      GetWorldOrientation     ()            const noexcept = 0;
+    [[nodiscard]] virtual SHQuaternion      GetRelativeOrientation  ()            const noexcept = 0;
+    [[nodiscard]] virtual float             GetVolume               ()            const noexcept = 0;
+    [[nodiscard]] virtual float             GetSurfaceArea          ()            const noexcept = 0;
+    [[nodiscard]] virtual SHMatrix          GetInertiaTensor        (float mass)  const noexcept = 0;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void  SetCollisionTag     (SHCollisionTag* newCollisionTag)       noexcept;
+
+    void  SetFriction         (float friction)                        noexcept;
+    void  SetBounciness       (float bounciness)                      noexcept;
+    void  SetDensity          (float density)                         noexcept;
+    void  SetMaterial         (const SHPhysicsMaterial& newMaterial)  noexcept;
+
+    void  SetPositionOffset   (const SHVec3& posOffset)               noexcept;
+    void  SetRotationOffset   (const SHVec3& rotOffset)               noexcept;
+
+    // Flags
+
+    // Forces rigidbody to recompute mass if one exists
+    void  SetIsTrigger        (bool isTrigger)                        noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Computes the transform of the shape.
+    */
+    virtual void Update () noexcept = 0;
+
+    /**
+     * @brief
+     * Tests if a point is inside this shape.
+     * @param point
+     * The point to test against the shape.
+     * @return
+     * True if the point is inside the shape. False otherwise.
+     */
+    [[nodiscard]] virtual bool TestPoint (const SHVec3& point) const noexcept = 0;
+
+    /**
+     * @brief
+     * Casts a ray at this shape.
+     * @param ray
+     * The ray to cast at the shape.
+     * @return
+     * The result of the ray cast. See the corresponding struct for it's contents.
+     */
+    [[nodiscard]] virtual SHRaycastResult Raycast (const SHRay& ray) const noexcept = 0;
+
+    /**
+     * @brief
+     * Computes the TRS matrix for rendering the shape.
+     * @return
+     * The model-to-world matrix for rendering the shape.
+     */
+    [[nodiscard]] virtual SHMatrix GetTRS () const noexcept = 0;
+
+    /**
+     * @brief
+     * Computes a tight-fitting AABB around this shape.
+     * @return
+     * An AABB.
+     */
+    [[nodiscard]] virtual SHAABB ComputeAABB () const noexcept = 0;
+
+  protected:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionShapeID  id;
+
+    SHCollider*         collider;     // The collider it belongs to.
+    SHCollisionTag*     collisionTag;
+    SHPhysicsMaterial   material;     // TODO: Change to pointer once instancing is supported
+
+    SHTransform         transform;    // Stores the local position and rotation.
+
+    // Needed for conversion to euler angles
+    SHVec3              rotationOffset;
+
+    uint8_t             flags;        // 0 0 0 trigger convexHull capsule sphere box
+    
+
+    RTTR_ENABLE()
+  };
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeID.h

@@ -1,7 +1,7 @@
 /****************************************************************************************
- * \file      SHCollisionInfo.h
+ * \file      SHCollisionShapeKey.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for Collision Information for Collision & Triggers.
+ * \brief     Interface for a Collison Shape ID Class and it's hashing function
  *
  * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
  *            disclosure of this file or its contents without the prior written consent
@@ -11,92 +11,95 @@
 #pragma once
 
 // Project Headers
-#include "Physics/Interface/SHColliderComponent.h"
+#include "ECS_Base/Entity/SHEntity.h"
-#include "Physics/Interface/SHRigidBodyComponent.h"
-
 
 namespace SHADE
 {
+  /*-----------------------------------------------------------------------------------*/
+  /* Forward Declarations                                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  struct SHCollisionShapeIDHash;
+
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  class SH_API SHCollisionInfo
+  /**
+   * @brief
+   * Encapsulates an identifier for a collision shape.
+  */
+  union SHCollisionShapeID
   {
   private:
     /*---------------------------------------------------------------------------------*/
     /* Friends                                                                         */
     /*---------------------------------------------------------------------------------*/
 
-    friend class SHCollisionListener;
+    friend struct SHCollisionShapeIDHash;
 
   public:
-    /*---------------------------------------------------------------------------------*/
-    /* Type Definitions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    enum class State
-    {
-      ENTER
-    , STAY
-    , EXIT
-
-    , TOTAL
-    , INVALID = -1
-    };
-
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHCollisionInfo   ()                                    noexcept;
+    SHCollisionShapeID  (EntityID eid, uint32_t shapeID)  noexcept;
-    SHCollisionInfo   (EntityID entityA, EntityID entityB)  noexcept;
+    SHCollisionShapeID  (const SHCollisionShapeID& rhs)   noexcept;
+    SHCollisionShapeID  (SHCollisionShapeID&& rhs)        noexcept;
 
-
+    ~SHCollisionShapeID () noexcept = default;
-    SHCollisionInfo   (const SHCollisionInfo& rhs)  = default;
-    SHCollisionInfo   (SHCollisionInfo&& rhs)       = default;
-    ~SHCollisionInfo  ()                             = default;
 
     /*---------------------------------------------------------------------------------*/
     /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
 
-    bool operator== (const SHCollisionInfo& rhs)      const noexcept;
+    SHCollisionShapeID& operator=(const SHCollisionShapeID& rhs)  noexcept;
-    bool operator!= (const SHCollisionInfo& rhs)      const noexcept;
+    SHCollisionShapeID& operator=(SHCollisionShapeID&& rhs)       noexcept;
 
-    SHCollisionInfo& operator= (const SHCollisionInfo& rhs) = default;
+    bool operator==(const SHCollisionShapeID& rhs) const;
-    SHCollisionInfo& operator= (SHCollisionInfo&& rhs)      = default;
 
     /*---------------------------------------------------------------------------------*/
     /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    [[nodiscard]] EntityID                    GetEntityA        ()  const noexcept;
+    [[nodiscard]] EntityID  GetEntityID   ()  const noexcept;
-    [[nodiscard]] EntityID                    GetEntityB        ()  const noexcept;
+    [[nodiscard]] uint32_t  GetShapeIndex ()  const noexcept;
-    [[nodiscard]] const SHRigidBodyComponent* GetRigidBodyA     ()  const noexcept;
-    [[nodiscard]] const SHRigidBodyComponent* GetRigidBodyB     ()  const noexcept;
-    [[nodiscard]] const SHCollisionShape*     GetColliderA      ()  const noexcept;
-    [[nodiscard]] const SHCollisionShape*     GetColliderB      ()  const noexcept;
-    [[nodiscard]] State                       GetCollisionState ()  const noexcept;
 
   private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct IDs
+    {
+    public:
+      EntityID  entityID    = MAX_EID;
+      uint32_t  shapeIndex  = std::numeric_limits<uint32_t>::max();
+    };
 
-    static constexpr uint32_t ENTITY_A    = 0;
-    static constexpr uint32_t ENTITY_B    = 1;
-    static constexpr uint32_t COLLIDER_A  = 2;
-    static constexpr uint32_t COLLIDER_B  = 3;
 
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    union
+    uint64_t  value;
-    {
+    IDs       ids;
-      uint64_t value[2];  // EntityValue, ColliderIndexValue
-      uint32_t ids  [4];  // EntityA, EntityB, ColliderIndexA, ColliderIndexB
   };
 
-    State     collisionState;
+  /**
+   * @brief
+   * Encapsulates a functor to hash a CollisionShapeID
+  */
+  struct SHCollisionShapeIDHash
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    std::size_t operator()(const SHCollisionShapeID& id) const;
   };
 
 } // namespace SHADE
+
+#include "SHCollisionShapeID.hpp"

SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeID.hpp

@@ -0,0 +1,80 @@
+/****************************************************************************************
+ * \file      SHCollisionShapeKey.hpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Inlined Implementations for a Collison Shape ID Class and
+ *            it's hashing function.
+ *
+ * \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
+
+// Primary Header
+#include "SHCollisionShapeID.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  inline SHCollisionShapeID::SHCollisionShapeID(EntityID eid, uint32_t shapeID) noexcept
+  : ids { eid, shapeID }
+  {}
+
+  inline SHCollisionShapeID::SHCollisionShapeID(const SHCollisionShapeID& rhs) noexcept
+  : ids { rhs.ids.entityID, rhs.ids.shapeIndex }
+  {}
+
+  inline SHCollisionShapeID::SHCollisionShapeID(SHCollisionShapeID&& rhs) noexcept
+  : ids { rhs.ids.entityID, rhs.ids.shapeIndex }
+  {}
+  
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  inline SHCollisionShapeID& SHCollisionShapeID::operator=(const SHCollisionShapeID& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    value = rhs.value;
+
+    return *this;
+  }
+
+  inline SHCollisionShapeID& SHCollisionShapeID::operator=(SHCollisionShapeID&& rhs) noexcept
+  {
+    value = rhs.value;
+
+    return *this;
+  }
+
+  inline bool SHCollisionShapeID::operator==(const SHCollisionShapeID& rhs) const
+  {
+    return value == rhs.value;
+  }
+
+  inline std::size_t SHCollisionShapeIDHash::operator()(const SHCollisionShapeID& id) const
+  {
+    return std::hash<uint64_t>{}(id.value);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  inline EntityID SHCollisionShapeID::GetEntityID() const noexcept
+  {
+    return ids.entityID;
+  }
+
+  inline uint32_t SHCollisionShapeID::GetShapeIndex() const noexcept
+  {
+    return ids.shapeIndex;
+  }
+}

SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp

@@ -0,0 +1,174 @@
+/****************************************************************************************
+ * \file      SHCollisionShapeLibrary.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Collison Shape Factory Class.
+ *
+ * \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 "SHCollisionShapeLibrary.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionShapeLibrary::SHCollisionShapeLibrary() noexcept
+  {
+    createBoxPolyhedron();
+  }
+
+  SHCollisionShapeLibrary::~SHCollisionShapeLibrary() noexcept
+  {
+    // Free all shapes in each container
+    for (auto* sphereCollisionShape : spheres | std::views::values)
+      DestroyShape(sphereCollisionShape);
+
+    // Free all shapes in each container
+    for (auto* boxCollisionShape : boxes | std::views::values)
+      DestroyShape(boxCollisionShape);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHSphere* SHCollisionShapeLibrary::CreateSphere(SHCollisionShapeID id, const SHSphereCreateInfo& createInfo)
+  {
+    const auto RESULT = spheres.emplace(id, new SHSphere{ id });
+    if (RESULT.second)
+    {
+      SHSphere* sphere = RESULT.first->second;
+
+      sphere->Center          = createInfo.Center;
+      sphere->Radius          = createInfo.Radius;
+      sphere->relativeRadius  = createInfo.RelativeRadius;
+      sphere->scale           = createInfo.Scale;
+
+      return sphere;
+    }
+
+    return spheres.find(id)->second;
+  }
+
+  SHBox* SHCollisionShapeLibrary::CreateBox(SHCollisionShapeID id, const SHBoxCreateInfo& createInfo)
+  {
+    const auto RESULT = boxes.emplace(id, new SHBox{ id });
+    if (RESULT.second)
+    {
+      SHBox* box = RESULT.first->second;
+
+      box->Center             = createInfo.Center;
+      box->Extents            = createInfo.Extents;
+      box->relativeExtents    = createInfo.RelativeExtents;
+      box->Orientation        = createInfo.Orientation;
+      box->scale              = createInfo.Scale;
+
+      // Set halfEdge data structure for the box
+      box->halfEdgeStructure  = &boxHalfEdgeStructure;
+
+      return box;
+    }
+
+    return boxes.find(id)->second;
+  }
+
+
+  void SHCollisionShapeLibrary::DestroyShape(SHCollisionShape* shape)
+  {
+    switch (shape->GetType())
+    {
+      case SHCollisionShape::Type::BOX:
+      {
+        break;
+      }
+      case SHCollisionShape::Type::SPHERE:
+      {
+        SHSphere* sphere = spheres.find(shape->id)->second;
+        spheres.erase(shape->id);
+
+        delete sphere;
+        sphere = nullptr;
+
+        break;
+      }
+      case SHCollisionShape::Type::CAPSULE:
+      {
+        break;
+      }
+      default: break;
+    }
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Function Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHCollisionShapeLibrary::createBoxPolyhedron() noexcept
+  {
+    static constexpr int NUM_VERTICES_PER_FACE  = 4;
+    static constexpr int NUM_FACES              = 6;
+
+    /*
+     * Vertices (Front/Back Face):
+     *
+     * 3/7 ----------  2/6
+     *     |        |
+     *     |        |
+     *     |        |
+     * 0/4 ----------  1/5
+     *
+     * Faces:
+     *
+     * Front:   0 (0,1,2,3) Normal: -Z
+     * Right:   1 (5,6,2,1) Normal:  X
+     * Back:    2 (5,4,7,6) Normal:  Z
+     * Left:    3 (0,3,7,4) Normal: -X
+     * Top:     4 (3,2,6,7) Normal:  Y
+     * Bottom:  5 (4,5,1,0) Normal: -Y
+     *
+     */
+
+    // Create face data
+
+    const SHVec3 FACE_NORMALS[NUM_FACES]
+    {
+      -SHVec3::UnitZ
+    ,  SHVec3::UnitX
+    ,  SHVec3::UnitZ
+    , -SHVec3::UnitX
+    ,  SHVec3::UnitY
+    , -SHVec3::UnitY
+    };
+
+    const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE]
+    {
+      { 0, 1, 2, 3 }
+    , { 5, 6, 2, 1 }
+    , { 5, 4, 7, 6 }
+    , { 0, 3, 7, 4 }
+    , { 2, 6, 7, 3 }
+    , { 5, 1, 0, 4 }
+    };
+
+    for (int i = 0; i < NUM_FACES; ++i)
+    {
+      SHHalfEdgeStructure::Face newFace;
+      newFace.normal = FACE_NORMALS[i];
+
+      for (int j = 0; j < NUM_VERTICES_PER_FACE; ++j)
+        newFace.vertexIndices.emplace_back(FACE_VERTICES[i][j]);
+
+      boxHalfEdgeStructure.AddFace(newFace);
+    }
+
+    boxHalfEdgeStructure.Build();
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.h

@@ -1,7 +1,7 @@
 /****************************************************************************************
- * \file      SHCollisionListener.h
+ * \file      SHCollisionShapeLibrary.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a Collision Listener.
+ * \brief     Interface for a Collison Shape Library.
  *
  * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
  *            disclosure of this file or its contents without the prior written consent
@@ -10,71 +10,99 @@
 
 #pragma once
 
-// External Dependencies
+#include <unordered_map>
-#include <reactphysics3d/reactphysics3d.h>
 
-// Project Headers
+// Project Header
-#include "SH_API.h"
+#include "SHSphere.h"
-#include "SHCollisionInfo.h"
+#include "SHBox.h"
 
 namespace SHADE
 {
-  /*-----------------------------------------------------------------------------------*/
-  /* Forward Declarations                                                              */
-  /*-----------------------------------------------------------------------------------*/
-
-  class SHPhysicsSystem;
-
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  class SH_API SHCollisionListener final : public rp3d::EventListener
+  /**
+   * @brief
+   * Encapsulates a class for Creating, Storing and Destroying Collision Shapes. <br/>
+   * All memory for collision shapes are stored in this factory class. <br/>
+  */
+  class SH_API SHCollisionShapeLibrary final
   {
   public:
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHCollisionListener() noexcept;
+    SHCollisionShapeLibrary   () noexcept;
-
+    ~SHCollisionShapeLibrary  () noexcept;
-    /*---------------------------------------------------------------------------------*/
-    /* Getter Functions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    [[nodiscard]] const std::vector<SHCollisionInfo>& GetCollisionInfoContainer () const noexcept;
-    [[nodiscard]] const std::vector<SHCollisionInfo>& GetTriggerInfoContainer   () const noexcept;
 
     /*---------------------------------------------------------------------------------*/
     /* Function Members                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    void  BindToSystem    (SHPhysicsSystem* physicsSystem)  noexcept;
+    /**
-    void  BindToWorld     (rp3d::PhysicsWorld* world)       noexcept;
+     * @brief
-    void  CleanContainers ()  noexcept;
+     * Creates a sphere collision shape.
-    void  ClearContainers ()  noexcept;
+     * @param id
+     * The ID of the shape.
+     * @param createInfo
+     * The info to create the sphere with.
+     * @return
+     * A new sphere collision shape.
+    */
+    SHSphere*  CreateSphere  (SHCollisionShapeID id, const SHSphereCreateInfo& createInfo);
 
-    void  onContact (const rp3d::CollisionCallback::CallbackData& callbackData) override;
+    /**
-    void  onTrigger (const rp3d::OverlapCallback::CallbackData& callbackData) override;
+     * @brief
+     * Creates a box collision shape.
+     * @param id
+     * The ID of the shape.
+     * @param createInfo
+     * The info to create the box with.
+     * @return
+     * A new box collision shape.
+    */
+    SHBox*  CreateBox  (SHCollisionShapeID id, const SHBoxCreateInfo& createInfo);
+
+    /**
+     * @brief
+     * Destroys a collision shape.
+     * * @param eid
+     * The entity the shape belongs to.
+     * @param shapeID
+     * The ID of the shape.
+     * @param shape
+     * The shape to destroy.
+    */
+    void DestroyShape  (SHCollisionShape* shape);
 
   private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    // We use unordered maps for fast lookup when deleting.
+    // Since we are not instancing shapes (yet?), I'd rather not iterate through an entire vector to find the shape.
+
+    using Spheres = std::unordered_map<SHCollisionShapeID, SHSphere*, SHCollisionShapeIDHash>;
+    using Boxes   = std::unordered_map<SHCollisionShapeID, SHBox*, SHCollisionShapeIDHash>;
 
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
     
-    SHPhysicsSystem*              system;
+    SHHalfEdgeStructure   boxHalfEdgeStructure;
-    std::vector<SHCollisionInfo>  collisionInfoContainer;
+
-    std::vector<SHCollisionInfo>  triggerInfoContainer;
+    Spheres               spheres;
+    Boxes                 boxes;
+    // TODO: Add capsules and hulls
 
     /*---------------------------------------------------------------------------------*/
     /* Function Members                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    static void     updateInfoContainers  (const SHCollisionInfo& collisionEvent, std::vector<SHCollisionInfo>& container) noexcept;
+    void createBoxPolyhedron() noexcept;
-
-    SHCollisionInfo generateCollisionInfo (const rp3d::CollisionCallback::ContactPair& cp)  const noexcept;
-    SHCollisionInfo generateTriggerInfo   (const rp3d::OverlapCallback::OverlapPair& cp)    const noexcept;
   };
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.cpp

@@ -0,0 +1,117 @@
+/****************************************************************************************
+ * \file      SHConvexPolyhedronCollisionShape.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a convex polyhedron collision shape.
+ *
+ * \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 "SHConvexPolyhedron.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHConvexPolyhedron::SHConvexPolyhedron(SHCollisionShapeID id,Type polyhedronType) noexcept
+  : SHCollisionShape  (id, polyhedronType)
+  , halfEdgeStructure { nullptr }
+  {}
+
+  SHConvexPolyhedron::SHConvexPolyhedron(const SHConvexPolyhedron& rhs) noexcept
+  : SHCollisionShape  ( rhs )
+  , halfEdgeStructure { rhs.halfEdgeStructure }
+  {}
+
+  SHConvexPolyhedron::SHConvexPolyhedron(SHConvexPolyhedron&& rhs) noexcept
+  : SHCollisionShape  ( rhs )
+  , halfEdgeStructure { rhs.halfEdgeStructure }
+  {}
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHConvexPolyhedron& SHConvexPolyhedron::operator=(const SHConvexPolyhedron& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    SHCollisionShape::operator=(rhs);
+
+    // Local Properties
+    halfEdgeStructure = rhs.halfEdgeStructure;
+
+    return *this;
+  }
+
+  SHConvexPolyhedron& SHConvexPolyhedron::operator=(SHConvexPolyhedron&& rhs) noexcept
+  {
+    SHCollisionShape::operator=(rhs);
+
+    // Local Properties
+    halfEdgeStructure = rhs.halfEdgeStructure;
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  const SHHalfEdgeStructure* SHConvexPolyhedron::GetHalfEdgeStructure() const noexcept
+  {
+    return halfEdgeStructure;
+  }
+
+  int32_t SHConvexPolyhedron::GetFaceCount() const noexcept
+  {
+    return halfEdgeStructure->GetFaceCount();
+  }
+
+  int32_t SHConvexPolyhedron::GetHalfEdgeCount() const noexcept
+  {
+    return halfEdgeStructure->GetHalfEdgeCount();
+  }
+
+  const SHHalfEdgeStructure::Face& SHConvexPolyhedron::GetFace(int index) const
+  {
+    // Assume it has already been initialised
+    return halfEdgeStructure->GetFace(index);
+  }
+
+  const SHHalfEdgeStructure::HalfEdge& SHConvexPolyhedron::GetHalfEdge(int index) const
+  {
+    // Assume it has already been initialised
+    return halfEdgeStructure->GetHalfEdge(index);
+  }
+
+  SHVec3 SHConvexPolyhedron::FindSupportPoint(const SHVec3& direction) const noexcept
+  {
+    float   bestDistance  = std::numeric_limits<float>::lowest();
+    SHVec3  bestPoint     = SHVec3::Zero;
+
+    for (const int32_t i : std::views::iota(0, GetNumVertices()))
+    {
+      const SHVec3 VERTEX = GetVertex(i);
+      const float PROJECTION = SHVec3::Dot(VERTEX, direction);
+
+      if (PROJECTION > bestDistance)
+      {
+        bestDistance = PROJECTION;
+        bestPoint = VERTEX;
+      }
+    }
+
+    return bestPoint;
+  }
+
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.h

@@ -0,0 +1,94 @@
+/****************************************************************************************
+ * \file      SHConvexPolyhedronCollisionShape.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a convex polyhedron collision shape.
+ *
+ * \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 "SHCollisionShape.h"
+#include "SHHalfEdgeStructure.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates a convex polyhedron shape used for Physics Simulations..
+  */
+  class SH_API SHConvexPolyhedron : public SHCollisionShape
+  {
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Friends                                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    friend class SHCollider;
+    friend class SHCollision;
+    friend class SHCollisionShapeLibrary;
+
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHConvexPolyhedron  (SHCollisionShapeID id, Type polyhedronType)  noexcept;
+    SHConvexPolyhedron  (const SHConvexPolyhedron& rhs) noexcept;
+    SHConvexPolyhedron  (SHConvexPolyhedron&& rhs)      noexcept;
+
+    ~SHConvexPolyhedron () override = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHConvexPolyhedron& operator=(const SHConvexPolyhedron& rhs)  noexcept;
+    SHConvexPolyhedron& operator=(SHConvexPolyhedron&& rhs)       noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] const SHHalfEdgeStructure*            GetHalfEdgeStructure  ()  const noexcept;
+    [[nodiscard]] int32_t                               GetFaceCount          ()  const noexcept;
+    [[nodiscard]] const SHHalfEdgeStructure::Face&      GetFace               (int index)  const;
+    [[nodiscard]] int32_t                               GetHalfEdgeCount      ()  const noexcept;
+    [[nodiscard]] const SHHalfEdgeStructure::HalfEdge&  GetHalfEdge           (int index)  const;
+
+    // Virtual Methods
+
+    [[nodiscard]] virtual int32_t GetNumVertices  ()              const noexcept = 0;
+    [[nodiscard]] virtual SHVec3  GetVertex       (int index)     const = 0;
+    [[nodiscard]] virtual SHVec3  GetNormal       (int faceIndex) const = 0;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Finds the most extreme point on the polygon in a given direction.
+     * @param direction
+     * The direction to find the support point in.
+     * @return
+     * The most extreme vertex in the given direction.
+     */
+    [[nodiscard]] SHVec3 FindSupportPoint  (const SHVec3& direction) const noexcept;
+
+  protected:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    SHHalfEdgeStructure* halfEdgeStructure; // Defines the polyhedron by it's half edges.
+  };
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHHalfEdgeStructure.cpp

@@ -0,0 +1,182 @@
+/****************************************************************************************
+ * \file      SHHalfEdgeStructure.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a half-edge data structure to represent polyhedra.
+ *
+ * \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 "SHHalfEdgeStructure.h"
+
+// Helper Macros
+
+#define BUILD_UINT64_FROM_UINT32S(a, b) (uint64_t)a << 32 | b
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHHalfEdgeStructure::Face::Face(const Face& rhs) noexcept
+  : normal { rhs.normal }
+  {
+    std::ranges::copy(rhs.vertexIndices.begin(), rhs.vertexIndices.end(), std::back_inserter(vertexIndices));
+  }
+
+  SHHalfEdgeStructure::Face::Face(Face&& rhs) noexcept
+  : normal { rhs.normal }
+  {
+    std::ranges::copy(rhs.vertexIndices.begin(), rhs.vertexIndices.end(), std::back_inserter(vertexIndices));
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHHalfEdgeStructure::Face& SHHalfEdgeStructure::Face::operator=(const Face& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    normal = rhs.normal;
+
+    vertexIndices.clear();
+    std::ranges::copy(rhs.vertexIndices.begin(), rhs.vertexIndices.end(), std::back_inserter(vertexIndices));
+
+    return *this;
+  }
+
+  SHHalfEdgeStructure::Face& SHHalfEdgeStructure::Face::operator=(Face&& rhs) noexcept
+  {
+    normal = rhs.normal;
+
+    vertexIndices.clear();
+    std::ranges::copy(rhs.vertexIndices.begin(), rhs.vertexIndices.end(), std::back_inserter(vertexIndices));
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  int32_t SHHalfEdgeStructure::GetFaceCount() const noexcept
+  {
+    return static_cast<int32_t>(faces.size());
+  }
+
+  int32_t SHHalfEdgeStructure::GetHalfEdgeCount() const noexcept
+  {
+    return static_cast<int32_t>(halfEdges.size());
+  }
+
+  const SHHalfEdgeStructure::Face& SHHalfEdgeStructure::GetFace(int32_t index) const
+  {
+    if (index < 0 || index >= static_cast<int32_t>(faces.size()))
+      throw std::invalid_argument("Index out-of-range!");
+
+    return faces[index];
+  }
+
+  const SHHalfEdgeStructure::HalfEdge& SHHalfEdgeStructure::GetHalfEdge(int32_t index) const
+  {
+    if (index < 0 || index >= static_cast<int32_t>(halfEdges.size()))
+      throw std::invalid_argument("Index out-of-range!");
+
+    return halfEdges[index];
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHHalfEdgeStructure::AddFace(const Face& face)
+  {
+    faces.emplace_back(face);
+  }
+
+  void SHHalfEdgeStructure::Build() noexcept
+  {
+    // We use the pair of vertex IDs on a half-edge to prevent duplicates
+    std::unordered_map<uint64_t, HalfEdge> edgeMap;
+    edgeMap.clear();
+
+    if (faces.empty())
+    {
+      SHLOGV_CRITICAL("Unable to build convex polyhedron, no faces have been added!")
+      return;
+    }
+
+    // For each face, build half edges
+    for (size_t i = 0; i < faces.size(); ++i)
+    {
+      Face& face = faces[i];
+
+      if (face.vertexIndices.empty())
+      {
+        SHLOGV_CRITICAL("Unable to build convex polyhedron, no vertices have been added to face {}!", i)
+        return;
+      }
+
+      // Iterate through vertices and build half-edges
+      for (size_t j = 0; j < face.vertexIndices.size(); ++j)
+      {
+        const int32_t TAIL = face.vertexIndices[j].index;
+        const int32_t HEAD = face.vertexIndices[(j + 1) % face.vertexIndices.size()].index;
+
+        const uint64_t NEW_EDGE_ID   = BUILD_UINT64_FROM_UINT32S(TAIL, HEAD);
+        const uint64_t TWIN_EDGE_ID  = BUILD_UINT64_FROM_UINT32S(HEAD, TAIL);
+
+        // Check if the half-edge has already been inserted
+        auto newEdgeIter = edgeMap.find(NEW_EDGE_ID);
+        if (newEdgeIter == edgeMap.end())
+        {
+          // Reuse the iterator for mapping with the twin
+          newEdgeIter = edgeMap.emplace(NEW_EDGE_ID, HalfEdge{}).first;
+
+          HalfEdge& newHalfEdge = newEdgeIter->second;
+          newHalfEdge.tailVertexIndex = TAIL;
+          newHalfEdge.headVertexIndex = HEAD;
+          newHalfEdge.faceIndex       = static_cast<int32_t>(i);
+
+          // Set edge index of the newly inserted edge as the size of the map - 1
+          // Since it is an unordered map, it will just be at the back
+          newHalfEdge.edgeIndex       = static_cast<int32_t>(edgeMap.size()) - 1;
+
+          // Map vertex to this edge index
+          face.vertexIndices[j].edgeIndex = newHalfEdge.edgeIndex;
+        }
+
+        // Find twin edge if one exists
+        auto twinEdgeIter = edgeMap.find(TWIN_EDGE_ID);
+        if (twinEdgeIter != edgeMap.end())
+        {
+          // Set the twin index of both the edges
+          HalfEdge& newHalfEdge   = newEdgeIter->second;
+          HalfEdge& twinHalfEdge  = twinEdgeIter->second;
+
+          newHalfEdge.twinEdgeIndex   = twinHalfEdge.edgeIndex;
+          twinHalfEdge.twinEdgeIndex  = newHalfEdge.edgeIndex;
+        }
+      }
+    }
+
+    // Copy all half edges into the vector
+    // At this point, no duplicates should be in the map and all edges should be linked.
+    for (auto& halfEdge : edgeMap | std::views::values)
+      halfEdges.emplace_back(halfEdge);
+
+    // Sort based on edge indices
+    std::ranges::sort(halfEdges.begin(), halfEdges.end(), [](const HalfEdge& lhs, const HalfEdge& rhs)
+    {
+      return lhs.edgeIndex < rhs.edgeIndex;
+    });
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHHalfEdgeStructure.h

@@ -0,0 +1,143 @@
+/****************************************************************************************
+ * \file      SHHalfEdgeStructure.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a half-edge data structure to represent polyhedra.
+ *
+ * \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
+
+#include <vector>
+
+// Project Headers
+#include "Math/Vector/SHVec3.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates data for a convex polyhedron's geometry represented as faces & half edges.
+  */
+  class SH_API SHHalfEdgeStructure
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Encapsulates the data half edge of a face on a polyhedron.
+    */
+    struct HalfEdge
+    {
+      /*-------------------------------------------------------------------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      //Head and tail forms the edge. 
+      //Head <----- Tail
+      int32_t tailVertexIndex = -1;
+
+      // Head is also tail of the next edge.
+      int32_t headVertexIndex = -1;
+
+      int32_t edgeIndex       = -1;
+      // Other half of the edge on a different face.
+      // Important for extrapolating face normals.
+      int32_t twinEdgeIndex   = -1;
+
+      // Adjacent face of this edge.
+      int32_t faceIndex       = -1;
+    };
+
+    struct Vertex
+    {
+    public:
+      /*-------------------------------------------------------------------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      int32_t index     = -1;
+      int32_t edgeIndex = -1;  // the half-edge that this vertex is a tail of.
+    };
+
+    /**
+     * @brief
+     * Encapsulates the data of a face on a polyhedron.
+    */
+    struct Face
+    {
+    public:
+      /*-------------------------------------------------------------------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      SHVec3                normal;
+      std::vector<Vertex>   vertexIndices;    // Must be in CCW order
+
+      /*-------------------------------------------------------------------------------*/
+      /* Constructors & Destructor                                                     */
+      /*-------------------------------------------------------------------------------*/
+
+      Face  ()                noexcept = default;
+      Face  (const Face& rhs) noexcept;
+      Face  (Face&& rhs)      noexcept;
+      ~Face ()                noexcept = default;
+
+      /*-------------------------------------------------------------------------------*/
+      /* Operator Overloads                                                            */
+      /*-------------------------------------------------------------------------------*/
+
+      Face& operator= (const Face& rhs) noexcept;
+      Face& operator= (Face&& rhs)      noexcept;
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] int32_t           GetFaceCount      () const noexcept;
+    [[nodiscard]] int32_t           GetHalfEdgeCount  () const noexcept;
+
+    [[nodiscard]] const Face&       GetFace           (int32_t index) const;
+    [[nodiscard]] const HalfEdge&   GetHalfEdge       (int32_t index) const;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Adds a face to the polyhedron. The face must be constructed outside the polyhedron.
+     * @param face
+     * The face to insert.
+    */
+    void AddFace (const Face& face);
+
+    /**
+     * @brief
+     * Builds the half-edges of the polyhedron using the faces. <br/>
+     * Before this method is invoked, there must be some faces.
+     * @return 
+    */
+    void Build() noexcept;
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    std::vector<Face>     faces;
+    std::vector<HalfEdge> halfEdges;
+
+  };
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHSphere.cpp

@@ -0,0 +1,240 @@
+/****************************************************************************************
+ * \file      SHSphereCollisionShape.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Sphere Collision Shape.
+ *
+ * \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 "SHSphere.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Math/SHMatrix.h"
+#include "Physics/Collision/SHCollider.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHSphere::SHSphere(SHCollisionShapeID id) noexcept
+  : SHCollisionShape  (id, Type::SPHERE)
+  , relativeRadius    { 1.0f }
+  , scale             { 1.0f }
+  {
+    Radius = 0.5f;
+  }
+
+  SHSphere::SHSphere(const SHSphere& rhs) noexcept
+  : SHCollisionShape  ( rhs )
+  , relativeRadius    { rhs.relativeRadius }
+  , scale             { rhs.scale }
+  {
+    Center = rhs.Center;
+    Radius = rhs.Radius;
+  }
+
+  SHSphere::SHSphere(SHSphere&& rhs) noexcept
+  : SHCollisionShape  ( rhs )
+  , relativeRadius    { rhs.relativeRadius }
+  , scale             { rhs.scale }
+  {
+    Center = rhs.Center;
+    Radius = rhs.Radius;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHSphere& SHSphere::operator=(const SHSphere& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    SHCollisionShape::operator=(rhs);
+
+    // Sphere Properties
+
+    Center          = rhs.Center;
+    Radius          = rhs.Radius;
+
+    // Local Properties
+
+    relativeRadius  = rhs.relativeRadius;
+    scale           = rhs.scale;
+
+    return *this;
+  }
+
+  SHSphere& SHSphere::operator=(SHSphere&& rhs) noexcept
+  {
+    SHCollisionShape::operator=(rhs);
+
+    // Sphere Properties
+
+    Center          = rhs.Center;
+    Radius          = rhs.Radius;
+
+    // Local Properties
+
+    relativeRadius  = rhs.relativeRadius;
+    scale           = rhs.scale;
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  float SHSphere::GetWorldRadius() const noexcept
+  {
+    return Radius;
+  }
+
+  float SHSphere::GetRelativeRadius() const noexcept
+  {
+    return relativeRadius;
+  }
+
+  SHVec3 SHSphere::GetWorldCentroid() const noexcept
+  {
+    return Center;
+  }
+
+  SHVec3 SHSphere::GetRelativeCentroid() const noexcept
+  {
+    if (collider)
+      return SHVec3{ Center }  - collider->GetPosition();
+
+    return Center;
+  }
+
+  SHVec3 SHSphere::GetLocalCentroid() const noexcept
+  {
+    return SHVec3::Zero;
+  }
+
+  SHQuaternion SHSphere::GetWorldOrientation() const noexcept
+  {
+    if (collider)
+      return collider->GetOrientation() * transform.orientation;
+
+    return transform.orientation;
+  }
+
+  SHQuaternion SHSphere::GetRelativeOrientation() const noexcept
+  {
+    return transform.orientation;
+  }
+
+  float SHSphere::GetVolume() const noexcept
+  {
+    return (4.0f / 3.0f) * SHMath::PI * (Radius * Radius * Radius);
+  }
+
+  float SHSphere::GetSurfaceArea() const noexcept
+  {
+    return 4.0f * SHMath::PI * (Radius * Radius);
+  }
+
+  SHMatrix SHSphere::GetInertiaTensor(float mass) const noexcept
+  {
+    static constexpr float TWO_OVER_FIVE = 2.0f / 5.0f;
+
+    const float DIAGONAL = TWO_OVER_FIVE * mass * (Radius * Radius);
+
+    SHMatrix result;
+    result.m[0][0] = result.m[1][1] = result.m[2][2] = DIAGONAL;
+    return result;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHSphere::SetWorldRadius(float newWorldRadius) noexcept
+  {
+    Radius = newWorldRadius;
+
+    // Recompute Relative radius
+    relativeRadius = 2.0f * Radius / scale;
+  }
+
+  void SHSphere::SetRelativeRadius(float newRelativeRadius) noexcept
+  {
+    relativeRadius = newRelativeRadius;
+
+    // Recompute world radius
+    Radius = relativeRadius * scale * 0.5f;
+  }
+
+  void SHSphere::SetScale(float maxScale) noexcept
+  {
+    scale = std::fabs(maxScale);
+
+    // Recompute world radius
+    Radius = relativeRadius * scale * 0.5f;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHSphere::Update() noexcept
+  {
+    const SHTransform& PARENT_TRANSFORM = collider->GetTransform();
+
+    const float SPHERE_SCALE = std::fabs(SHMath::Max({ PARENT_TRANSFORM.scale.x, PARENT_TRANSFORM.scale.y, PARENT_TRANSFORM.scale.z }));
+    SetScale(SPHERE_SCALE);
+
+    // Recompute center
+    const SHQuaternion  FINAL_ROT  = PARENT_TRANSFORM.orientation * transform.orientation;
+    const SHMatrix      TRS        = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(PARENT_TRANSFORM.position);
+
+    Center = SHVec3::Transform(transform.position, TRS);
+  }
+
+  bool SHSphere::TestPoint(const SHVec3& point) const noexcept
+  {
+    return Contains(point);
+  }
+
+  SHRaycastResult SHSphere::Raycast(const SHRay& ray) const noexcept
+  {
+    SHRaycastResult result;
+
+    result.hit = Intersects(ray.position, ray.direction, result.distance);
+    if (result.hit)
+    {
+      result.position = ray.position + ray.direction * result.distance;
+      result.angle    = SHVec3::Angle(ray.position, result.position);
+      result.normal   = SHVec3::Normalise(result.position - Center);
+    }
+
+    return result;
+  }
+
+  SHMatrix SHSphere::GetTRS() const noexcept
+  {
+    const SHQuaternion  ROTATION  = collider ? collider->GetTransform().orientation * transform.orientation : transform.orientation;
+    const SHVec3        SCALE     { Radius };
+
+    return SHMatrix::Transform(Center, ROTATION, SCALE);
+  }
+
+  SHAABB SHSphere::ComputeAABB() const noexcept
+  {
+    return SHAABB{ Center, SHVec3{ Radius } };
+  }
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHSphere.h

@@ -1,7 +1,7 @@
 /****************************************************************************************
- * \file      SHBoundingSphere.h
+ * \file      SHSphereCollisionShape.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a Bounding Sphere.
+ * \brief     Interface for a Sphere Collision Shape.
  *
  * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
  *            disclosure of this file or its contents without the prior written consent
@@ -13,8 +13,7 @@
 #include <DirectXCollision.h>
 
 // Project Headers
-#include "SHShape.h"
+#include "SHCollisionShape.h"
-#include "SH_API.h"
 
 namespace SHADE
 {
@@ -22,16 +21,42 @@ namespace SHADE
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  class SH_API SHSphere : public SHShape,
+  /**
-                          private DirectX::BoundingSphere
+   * @brief
+   * Encapsulates the information to create a sphere.
+  */
+  struct SHSphereCreateInfo
   {
+  public:
+    SHVec3  Center          = SHVec3::Zero;
+    float   Radius          = 1.0f;
+    float   RelativeRadius  = 1.0f;
+    float   Scale           = 1.0f;
+  };
+
+  /**
+   * @brief
+   * Encapsulate a Sphere Shape used for Physics Simulations.
+  */
+  class SH_API SHSphere final : public SHCollisionShape
+                              , private DirectX::BoundingSphere
+  {
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Friends                                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    friend class SHCollider;
+    friend class SHCompositeCollider;
+    friend class SHCollision;
+    friend class SHCollisionShapeLibrary;
+
   public:
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHSphere  ()                                    noexcept;
+    SHSphere  (SHCollisionShapeID id)             noexcept;
-    SHSphere  (const SHVec3& center, float radius)  noexcept;
     SHSphere  (const SHSphere& rhs) noexcept;
     SHSphere  (SHSphere&& rhs)      noexcept;
 
@@ -48,45 +73,43 @@ namespace SHADE
     /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    [[nodiscard]] SHVec3  GetCenter         ()  const noexcept;
     [[nodiscard]] float         GetWorldRadius          ()              const noexcept;
     [[nodiscard]] float         GetRelativeRadius       ()              const noexcept;
 
+    [[nodiscard]] SHVec3        GetWorldCentroid        ()              const noexcept override;
+    [[nodiscard]] SHVec3        GetRelativeCentroid     ()              const noexcept override;
+    [[nodiscard]] SHVec3        GetLocalCentroid        ()              const noexcept override;
+    [[nodiscard]] SHQuaternion  GetWorldOrientation     ()              const noexcept override;
+    [[nodiscard]] SHQuaternion  GetRelativeOrientation  ()              const noexcept override;
+    [[nodiscard]] float         GetVolume               ()              const noexcept override;
+    [[nodiscard]] float         GetSurfaceArea          ()              const noexcept override;
+    [[nodiscard]] SHMatrix      GetInertiaTensor        (float mass)    const noexcept override;
+
     /*---------------------------------------------------------------------------------*/
     /* Setter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    void SetCenter          (const SHVec3& center)    noexcept;
     void SetWorldRadius     (float newWorldRadius)    noexcept;
     void SetRelativeRadius  (float newRelativeRadius) noexcept;
+    void SetScale           (float maxScale)          noexcept;
 
     /*---------------------------------------------------------------------------------*/
     /* Function Members                                                                */
     /*---------------------------------------------------------------------------------*/
 
+    void                          Update      ()                    noexcept override;
     [[nodiscard]] bool            TestPoint   (const SHVec3& point) const noexcept override;
     [[nodiscard]] SHRaycastResult Raycast     (const SHRay& ray)    const noexcept override;
-
+    [[nodiscard]] SHMatrix        GetTRS      ()                    const noexcept override;
-    [[nodiscard]] bool    Contains    (const SHSphere& rhs) const noexcept;
+    [[nodiscard]] SHAABB          ComputeAABB ()                    const noexcept override;
-    [[nodiscard]] float   Volume      () const noexcept;
-    [[nodiscard]] float   SurfaceArea () const noexcept;
-
-
-    /*---------------------------------------------------------------------------------*/
-    /* Static Function Members                                                         */
-    /*---------------------------------------------------------------------------------*/
-
-    [[nodiscard]] static SHSphere Combine           (const SHSphere& lhs, const SHSphere& rhs)      noexcept;
-    [[nodiscard]] static bool             Intersect         (const SHSphere& lhs, const SHSphere& rhs)      noexcept;
-    [[nodiscard]] static SHSphere BuildFromSpheres  (const SHSphere* spheres, size_t numSpheres)            noexcept;
-    [[nodiscard]] static SHSphere BuildFromVertices (const SHVec3* vertices, size_t numVertices, size_t stride = 0) noexcept;
 
   private:
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    float RelativeRadius;
+    float relativeRadius;
-
+    float scale;
   };
+
 } // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h

@@ -0,0 +1,56 @@
+/****************************************************************************************
+ * \file      SHContactConstraint.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Contact Constraint.
+ *
+ * \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/Collision/Contacts/SHManifold.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  struct SH_API SHContactConstraint
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    // Use the entity IDs to map resolved constraints back to the bodies
+
+    SHCollisionKey key;
+
+    // Material Data
+
+    float     friction    = 0.0f;
+    float     restitution = 0.0f;
+
+    // Mass Data
+
+    float     invMassA    = 0.0f;
+    float     invMassB    = 0.0f;
+    SHVec3    invInertiaA;
+    SHVec3    invInertiaB;
+    SHVec3    centerOfMassA;
+    SHVec3    centerOfMassB;
+
+    // Collision Data
+
+    SHVec3    normal;
+    SHVec3    tangents[SHContact::NUM_TANGENTS];
+    SHContact contacts[SHManifold::MAX_NUM_CONTACTS];
+
+    uint32_t  numContacts = 0;
+  };
+} // namespace SHADE
+

SHADE_Engine/src/Physics/Dynamics/Constraints/SHVelocityState.h

@@ -0,0 +1,56 @@
+/****************************************************************************************
+ * \file      SHVelocityState.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Velocity State for constraint solving.
+ *
+ * \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/Dynamics/SHRigidBody.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  struct SH_API SHVelocityState
+  {
+  public:
+    SHVec3 LinearVelocity;
+    SHVec3 AngularVelocity;
+
+    SHVec3 LinearLockFactor;
+    SHVec3 AngularLockFactor;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHVelocityState (const SHRigidBody* rigidBody) noexcept
+    {
+      LinearVelocity    = rigidBody->GetLinearVelocity();
+      AngularVelocity   = rigidBody->GetAngularVelocity();
+
+      LinearLockFactor  = SHVec3
+      {
+        rigidBody->GetFreezePositionX() ? 0.0f :  1.0f
+      , rigidBody->GetFreezePositionY() ? 0.0f :  1.0f
+      , rigidBody->GetFreezePositionZ() ? 0.0f :  1.0f
+      };
+
+      AngularLockFactor = SHVec3
+      {
+        rigidBody->GetFreezeRotationX() ? 0.0f :  1.0f
+      , rigidBody->GetFreezeRotationY() ? 0.0f :  1.0f
+      , rigidBody->GetFreezeRotationZ() ? 0.0f :  1.0f
+      };
+    }
+  };
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp

@@ -0,0 +1,299 @@
+/****************************************************************************************
+ * \file      SHContactManager.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Contact Manager that stores collision information and
+ *            resolves contact constraints.
+ *
+ * \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 "SHContactManager.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Physics/Collision/Narrowphase/SHCollisionDispatch.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  const SHContactManager::TriggerEvents& SHContactManager::GetTriggerEvents() const noexcept
+  {
+    static TriggerEvents triggerEvents;
+
+    triggerEvents.clear();
+
+    for (auto& [id, trigger] : triggers)
+      triggerEvents.emplace_back(SHTriggerEvent{ id, trigger.state });
+
+    return triggerEvents;
+  }
+
+  const SHContactManager::CollisionEvents& SHContactManager::GetCollisionEvents() const noexcept
+  {
+    static CollisionEvents collisionEvents;
+
+    collisionEvents.clear();
+
+    for (auto& [id, manifold] : manifolds)
+    {
+      SHCollisionEvent collisionEvent
+      {
+        .info   = id
+      , .state  = manifold.state
+      , .normal = manifold.normal
+      };
+
+      for (uint32_t i = 0; i < manifold.numContacts; ++i)
+        collisionEvent.contactPoints[i] = manifold.contacts[i].position;
+
+      collisionEvents.emplace_back(collisionEvent);
+    }
+
+    return collisionEvents;
+  }
+
+  const SHContactManager::ContactPoints& SHContactManager::GetContactPoints() const noexcept
+  {
+    static ContactPoints contactPoints;
+    contactPoints.clear();
+
+    for (auto& manifold : manifolds | std::views::values)
+    {
+      // Skip exit state manifolds
+      if (manifold.state == SHCollisionState::EXIT)
+        continue;
+
+      for (uint32_t i = 0; i < manifold.numContacts; ++i)
+      {
+        const ContactInfo INFO
+        {
+          .position = manifold.contacts[i].position
+        , .normal   = manifold.normal 
+        };
+
+        contactPoints.emplace_back(INFO);
+      }
+    }
+
+    return contactPoints;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHContactManager::AddTrigger(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept
+  {
+    // If id not found, emplace new trigger.
+    auto trigger = triggers.find(key);
+    if (trigger == triggers.end())
+      triggers.emplace(key, Trigger{ A, B, SHCollisionState::INVALID });
+  }
+
+  void SHContactManager::AddManifold(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept
+  {
+    // If id not found, emplace new manifold
+    auto manifold = manifolds.find(key);
+    if (manifold == manifolds.end())
+      manifolds.emplace(key, SHManifold{ A, B });
+  }
+
+  void SHContactManager::RemoveInvalidatedTrigger(EntityID eid) noexcept
+  {
+    removeInvalidObject(triggers, eid);
+  }
+
+  void SHContactManager::RemoveInvalidatedTrigger(EntityID eid, uint32_t shapeIndex) noexcept
+  {
+    removeInvalidObject(triggers, eid, shapeIndex);
+  }
+
+  void SHContactManager::RemoveInvalidatedManifold(EntityID eid) noexcept
+  {
+    removeInvalidObject(manifolds, eid);
+  }
+
+  void SHContactManager::RemoveInvalidatedManifold(EntityID eid, uint32_t shapeIndex) noexcept
+  {
+    removeInvalidObject(manifolds, eid, shapeIndex);
+  }
+
+  void SHContactManager::Update() noexcept
+  {
+    // Clear expired or invalid collisions. If not, test collision.
+    for (auto manifoldPair = manifolds.begin(); manifoldPair != manifolds.end();)
+    {
+      // Test collision of every manifold.
+      SHManifold& manifold = manifoldPair->second;
+      SHManifold  oldManifold = manifold;
+
+      const bool IS_COLLIDING = SHCollisionDispatcher::Collide(manifold, *manifold.shapeA, *manifold.shapeB);
+
+      auto& collisionState = manifold.state;
+      updateCollisionState(IS_COLLIDING, collisionState);
+
+      const bool IS_INVALID = collisionState == SHCollisionState::INVALID;
+      if (IS_INVALID)
+      {
+        manifoldPair = manifolds.erase(manifoldPair);
+        continue;
+      }
+
+      updateManifold(manifold, oldManifold);
+      ++manifoldPair;
+    }
+
+    // Clear expired or invalid triggers, If not, test collision.
+    for (auto triggerPair = triggers.begin(); triggerPair != triggers.end();)
+    {
+      // Test collision of every trigger.
+      Trigger&  oldTrigger  = triggerPair->second;
+      Trigger   newTrigger  = oldTrigger;
+
+      const bool IS_COLLIDING = SHCollisionDispatcher::Collide(*newTrigger.A, *newTrigger.B);
+
+      auto& collisionState = newTrigger.state;
+      updateCollisionState(IS_COLLIDING, collisionState);
+
+      const bool IS_INVALID = collisionState == SHCollisionState::INVALID;
+      if (IS_INVALID)
+        triggerPair = triggers.erase(triggerPair);
+      else
+        ++triggerPair;
+    }
+  }
+
+  void SHContactManager::SolveCollisions(int numIterations, float dt) noexcept
+  {
+    static constexpr int SIZE_CONTACTS = sizeof(SHContact) * SHManifold::MAX_NUM_CONTACTS;
+
+    // Build constraints
+    for (auto& [key, manifold] : manifolds)
+      contactSolver.AddContact(key, manifold);
+
+    // Solve contacts
+    contactSolver.SolveContacts(numIterations, dt);
+
+    // Map impulses back to manifolds
+    const auto& CONTACT_CONSTRAINTS = contactSolver.GetContantConstraints();
+    const auto& VELOCITY_STATES     = contactSolver.GetVelocities();
+
+    for (auto& [key, contactConstraint] : CONTACT_CONSTRAINTS)
+    {
+      SHManifold& manifold = manifolds.find(key)->second;
+
+      for (uint32_t i = 0; i < contactConstraint.numContacts; ++i)
+        manifold.contacts[i] = contactConstraint.contacts[i];
+
+      // Assign velocities back to the bodies
+      SHRigidBody* bodyA = manifold.bodyA;
+      SHRigidBody* bodyB = manifold.bodyB;
+
+      const auto& STATE_A = VELOCITY_STATES.find(key.GetEntityA())->second;
+      const auto& STATE_B = VELOCITY_STATES.find(key.GetEntityB())->second;
+
+      bodyA->SetLinearVelocity(STATE_A.LinearVelocity);
+      bodyB->SetLinearVelocity(STATE_B.LinearVelocity);
+
+      bodyA->SetAngularVelocity(STATE_A.AngularVelocity);
+      bodyB->SetAngularVelocity(STATE_B.AngularVelocity);
+    }
+
+    contactSolver.Reset();
+  }
+
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHContactManager::updateCollisionState(bool isColliding, SHCollisionState& state) noexcept
+  {
+    if (isColliding)
+    {
+      // New states start at invalid. In the first frame of collision, move to enter.
+      // If it already in enter, move to stay
+      state = state == SHCollisionState::INVALID ? SHCollisionState::ENTER : SHCollisionState::STAY;
+    }
+    else
+    {
+      // If already exited and still not colliding, the collision has expired.
+      // Invalid states are removed in the next frame
+      if (state == SHCollisionState::EXIT)
+        state = SHCollisionState::INVALID;
+
+      // If previously colliding, move to exit.
+      if (state == SHCollisionState::ENTER || state == SHCollisionState::STAY)
+        state = SHCollisionState::EXIT;
+    }
+  }
+
+  void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept
+  {
+    static const float SQRT_ONE_OVER_THREE = std::sqrtf(1.0f / 3.0f);
+    // Early out since exiting a collision does not require an update beyond updating the state
+    if (manifold.state == SHCollisionState::EXIT)
+      return;
+
+    const SHVec3& NORMAL        = manifold.normal;
+    SHVec3& tangent0            = manifold.tangents[0];
+    SHVec3& tangent1            = manifold.tangents[1];
+
+    const SHVec3& OLD_TANGENT_0 = oldManifold.tangents[0];
+    const SHVec3& OLD_TANGENT_1 = oldManifold.tangents[1];
+
+    // Compute tangents
+    if (std::fabs(NORMAL.x) >= SQRT_ONE_OVER_THREE)
+      tangent0 = SHVec3{ NORMAL.y, -NORMAL.x, 0.0f };
+    else
+      tangent0 = SHVec3{ 0.0f, NORMAL.z, -NORMAL.y };
+
+    tangent0 = SHVec3::Normalise(tangent0);
+    tangent1 = SHVec3::Cross(tangent0, NORMAL);
+
+    // Accumulate impulses
+    for (uint32_t i = 0; i < manifold.numContacts; ++i)
+    {
+      SHContact& contact = manifold.contacts[i];
+
+      // Reset impulses
+      contact.normalImpulse = 0.0f;
+      contact.tangentImpulse[0] = contact.tangentImpulse[1] = 0.0f;
+
+      for (uint32_t j = 0; j < oldManifold.numContacts; ++j)
+      {
+        const SHContact& OLD_CONTACT = oldManifold.contacts[j];
+
+        SHLOGV_INFO_D
+        (
+          "Old vs New \n indexA: {} vs {} \n typeA: {} vs {} \n indexB: {} vs {} \n typeB: {} vs {}"
+        , OLD_CONTACT.featurePair.indexA, contact.featurePair.indexA
+        , OLD_CONTACT.featurePair.typeA, contact.featurePair.typeA
+        , OLD_CONTACT.featurePair.indexB, contact.featurePair.indexB
+        , OLD_CONTACT.featurePair.typeB, contact.featurePair.typeB
+        )
+
+        if (OLD_CONTACT.featurePair.key == contact.featurePair.key)
+        {
+          // If contact features persists, re-project old solution
+          contact.normalImpulse = OLD_CONTACT.normalImpulse;
+
+          const SHVec3 FRICTION_FORCE = OLD_TANGENT_0 * OLD_CONTACT.tangentImpulse[0] + OLD_TANGENT_1 * OLD_CONTACT.tangentImpulse[1];
+          contact.tangentImpulse[0]   = SHVec3::Dot(FRICTION_FORCE, tangent0); 
+          contact.tangentImpulse[1]   = SHVec3::Dot(FRICTION_FORCE, tangent1);
+
+          break;
+        }
+      }
+    }
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHContactManager.h

@@ -0,0 +1,140 @@
+/****************************************************************************************
+ * \file      SHContactManager.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Contact Manager that stores collision information.
+ *
+ * \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
+
+#include <unordered_map>
+
+// Project Headers
+#include "Physics/Collision/Contacts/SHCollisionEvents.h"
+#include "Physics/Collision/Contacts/SHCollisionKey.h"
+#include "Physics/Collision/Contacts/SHManifold.h"
+#include "SHContactSolver.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates a class that stores collision information.
+  */
+  class SH_API SHContactManager
+  {
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Friends                                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    friend class SHPhysicsWorld;
+
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct ContactInfo
+    {
+      SHVec3 position;
+      SHVec3 normal;
+    };
+
+    using TriggerEvents     = std::vector<SHTriggerEvent>;
+    using CollisionEvents   = std::vector<SHCollisionEvent>;
+    using ContactPoints     = std::vector<ContactInfo>;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHContactManager  ()  noexcept = default;
+    ~SHContactManager ()  noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    const TriggerEvents&    GetTriggerEvents      ()  const noexcept;
+    const CollisionEvents&  GetCollisionEvents    ()  const noexcept;
+    const ContactPoints&    GetContactPoints      ()  const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void AddTrigger                 (const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept;
+    void AddManifold                (const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept;
+
+    void RemoveInvalidatedTrigger   (EntityID eid)                      noexcept;
+    void RemoveInvalidatedTrigger   (EntityID eid, uint32_t shapeIndex) noexcept;
+    void RemoveInvalidatedManifold  (EntityID eid)                      noexcept;
+    void RemoveInvalidatedManifold  (EntityID eid, uint32_t shapeIndex) noexcept;
+
+    /**
+     * @brief
+     * Removes any invalidated contacts and triggers, then performs narrowphase collision
+     * detection on existing triggers and manifolds.
+    */
+    void Update                     () noexcept;
+
+    /**
+     * @brief
+     * Builds contact constraints and solves them. Results are stored in the corresponding
+     * manifolds abiding by the sequential impulse method.
+    */
+    void SolveCollisions            (int numIterations, float dt) noexcept;
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct Trigger
+    {
+      SHCollisionShape* A     = nullptr;
+      SHCollisionShape* B     = nullptr;
+
+      SHCollisionState state  = SHCollisionState::INVALID;
+    };
+
+    using Manifolds = std::unordered_map<SHCollisionKey, SHManifold, SHCollisionKeyHash>;
+    using Triggers  = std::unordered_map<SHCollisionKey, Trigger, SHCollisionKeyHash>;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    Manifolds       manifolds;
+    Triggers        triggers;
+
+    SHContactSolver contactSolver;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    static void updateCollisionState (bool isColliding, SHCollisionState& state)            noexcept;
+    static void updateManifold       (SHManifold& manifold, const SHManifold& oldManifold)  noexcept;
+
+    // Removal Helpers
+
+    template <typename T>
+    static void removeInvalidObject (std::unordered_map<SHCollisionKey, T, SHCollisionKeyHash>& container, EntityID eid) noexcept;
+    template <typename T>
+    static void removeInvalidObject  (std::unordered_map<SHCollisionKey, T, SHCollisionKeyHash>& container, EntityID eid, uint32_t shapeIndex) noexcept;
+
+  };
+
+  
+} // namespace SHADE
+
+#include "SHContactManager.hpp"

SHADE_Engine/src/Physics/Dynamics/SHContactManager.hpp

@@ -0,0 +1,61 @@
+/****************************************************************************************
+ * \file      SHContactManager.hpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for templated methods of the Contact Manager.
+ *
+ * \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
+
+// Primary Header
+#include "SHContactManager.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  template <typename T>
+  void SHContactManager::removeInvalidObject(std::unordered_map<SHCollisionKey, T, SHCollisionKeyHash>& container, EntityID eid) noexcept
+  {
+    if (container.empty())
+      return;
+
+    for (auto invalidated = container.begin(); invalidated != container.end();)
+    {
+      const auto& ID = invalidated->first;
+
+      const bool MATCHES_A = ID.GetEntityA() == eid;
+      const bool MATCHES_B = ID.GetEntityB() == eid;
+
+      if (MATCHES_A || MATCHES_B)
+        invalidated = container.erase(invalidated);
+      else
+        ++invalidated;
+    }
+  }
+
+  template <typename T>
+  void SHContactManager::removeInvalidObject(std::unordered_map<SHCollisionKey, T, SHCollisionKeyHash>& container, EntityID eid, uint32_t shapeIndex) noexcept
+  {
+    if (container.empty())
+      return;
+
+    for (auto invalidated = container.begin(); invalidated != container.end();)
+    {
+      const auto& ID = invalidated->first;
+
+      const bool MATCHES_A = ID.GetEntityA() == eid && ID.GetShapeIndexA() == shapeIndex;
+      const bool MATCHES_B = ID.GetEntityB() == eid && ID.GetShapeIndexB() == shapeIndex;
+
+      if (MATCHES_A || MATCHES_B)
+        invalidated = container.erase(invalidated);
+      else
+        ++invalidated;
+    }
+  }
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp

@@ -0,0 +1,306 @@
+/****************************************************************************************
+ * \file      SHContactSolver.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Contact Solver.
+ *
+ * \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 "SHContactSolver.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Physics/SHPhysicsConstants.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  const SHContactSolver::VelocityStates& SHContactSolver::GetVelocities() const noexcept
+  {
+    return velocityStates;
+  }
+
+  const SHContactSolver::ContactConstraints& SHContactSolver::GetContantConstraints() const noexcept
+  {
+    return contactConstraints;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHContactSolver::AddContact(const SHCollisionKey& key, const SHManifold& manifold) noexcept
+  {
+    SHContactConstraint& newConstraint = contactConstraints.emplace(key, SHContactConstraint{}).first->second;
+
+    const auto* SHAPE_A = manifold.shapeA;
+    const auto* SHAPE_B = manifold.shapeB;
+
+    const auto* BODY_A  = manifold.bodyA;
+    const auto* BODY_B  = manifold.bodyB;
+
+    // Add velocities if it doesn't already exist
+    velocityStates.emplace(key.GetEntityA(), SHVelocityState{ BODY_A });
+    velocityStates.emplace(key.GetEntityB(), SHVelocityState{ BODY_B });
+
+    // Mix friction & restitution
+    const float FRICTION_A    = SHAPE_A->GetFriction();
+    const float RESTITUTION_A = SHAPE_A->GetBounciness();
+
+    const float FRICTION_B    = SHAPE_B->GetFriction();
+    const float RESTITUTION_B = SHAPE_B->GetBounciness();
+
+    newConstraint.friction    = std::sqrtf(FRICTION_A * FRICTION_B);
+    newConstraint.restitution = std::max(RESTITUTION_A, RESTITUTION_B);
+
+    // Mass data
+
+    newConstraint.invMassA       = BODY_A->invMass;
+    newConstraint.invInertiaA    = BODY_A->worldInvInertia;
+    newConstraint.centerOfMassA  = BODY_A->worldCentroid;
+
+    newConstraint.invMassB       = BODY_B->invMass;
+    newConstraint.invInertiaB    = BODY_B->worldInvInertia;
+    newConstraint.centerOfMassB  = BODY_B->worldCentroid;
+
+    // Collision data
+
+    newConstraint.numContacts = manifold.numContacts;
+
+    newConstraint.normal = manifold.normal;
+
+    static constexpr size_t TANGENTS_SIZE = sizeof(SHVec3) * SHContact::NUM_TANGENTS;
+    static constexpr size_t CONTACTS_SIZE = sizeof(SHContact) * SHManifold::MAX_NUM_CONTACTS;
+
+    memcpy_s(newConstraint.tangents, TANGENTS_SIZE, manifold.tangents, TANGENTS_SIZE);
+    memcpy_s(newConstraint.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE);
+
+    // Compute rA & rB for contacts
+    for (uint32_t i = 0; i < newConstraint.numContacts; ++i)
+    {
+      newConstraint.contacts[i].rA = newConstraint.contacts[i].position - newConstraint.centerOfMassA;
+      newConstraint.contacts[i].rB = newConstraint.contacts[i].position - newConstraint.centerOfMassB;
+    }
+  }
+
+  void SHContactSolver::Reset() noexcept
+  {
+    velocityStates.clear();
+    contactConstraints.clear();
+  }
+
+  void SHContactSolver::SolveContacts(int numIterations, float dt) noexcept
+  {
+    preSolve(dt);
+
+    for (int i = 0; i < numIterations; ++i)
+      solve();
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHContactSolver::preSolve(float dt) noexcept
+  {
+    const float INV_DT = 1.0f / dt;
+
+    for (auto& [key, constraint] : contactConstraints)
+    {
+      const float INV_MASS_SUM = constraint.invMassA + constraint.invMassB;
+
+      SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second;
+      SHVelocityState& velocityStateB = velocityStates.find(key.GetEntityB())->second;
+
+      SHVec3 vA = velocityStateA.LinearVelocity;
+      SHVec3 wA = velocityStateA.AngularVelocity;
+      SHVec3 vB = velocityStateB.LinearVelocity;
+      SHVec3 wB = velocityStateB.AngularVelocity;
+
+      const SHVec3& LINEAR_LOCK_A   = velocityStateA.LinearLockFactor;
+      const SHVec3& ANGULAR_LOCK_A  = velocityStateA.AngularLockFactor;
+      const SHVec3& LINEAR_LOCK_B   = velocityStateB.LinearLockFactor;
+      const SHVec3& ANGULAR_LOCK_B  = velocityStateB.AngularLockFactor;
+
+      for (uint32_t i = 0; i < constraint.numContacts; ++i)
+      {
+        SHContact& contact = constraint.contacts[i];
+
+        // Calculate JM-1JT (Effective mass)
+        /*
+         * rXnT * I-1 * rXn:
+         *
+         * 1. 3x3 * 3x1 = 3x1
+         * 2. 1x3 * 3x1 = 1x1
+         *
+         * First do I-1 * rXn
+         * | ix  0   0  || x |   | ix * x |
+         * | 0   iy  0  || y | = | iy * y |
+         * | 0   0   iz || z |   | iz * z |
+         *
+         * Then dot product the result with rXnT
+         * [ u v w ]| ix * x |
+         *          | iy * y | = [ ix * x * u + iy * y * v + iz * z * w ]
+         *          | iz * z |
+         *
+         * Simplified:
+         *
+         * rXnT /dot (I-1 * rXn)
+         */
+
+        // Effective mass along Normal
+        const SHVec3 RA_CROSS_N = SHVec3::Cross(contact.rA, constraint.normal);
+        const SHVec3 RB_CROSS_N = SHVec3::Cross(contact.rB, constraint.normal);
+
+        contact.normalMass = INV_MASS_SUM;
+        contact.normalMass += SHVec3::Dot(RA_CROSS_N, constraint.invInertiaA * RA_CROSS_N);
+        contact.normalMass += SHVec3::Dot(RB_CROSS_N, constraint.invInertiaB * RB_CROSS_N);
+
+        // Invert the normal mass (we want the actual mass, not the inverse mass)
+        contact.normalMass = 1.0f / contact.normalMass;
+
+        // Effective mass along tangents (same steps as above)
+
+        for (int j = 0; j < SHContact::NUM_TANGENTS; ++j)
+        {
+          const SHVec3 RA_CROSS_T = SHVec3::Cross(constraint.tangents[j], contact.rA);
+          const SHVec3 RB_CROSS_T = SHVec3::Cross(constraint.tangents[j], contact.rB);
+
+          contact.tangentMass[j] = INV_MASS_SUM;
+          contact.tangentMass[j] += SHVec3::Dot(constraint.invInertiaA * RA_CROSS_T, RA_CROSS_T);
+          contact.tangentMass[j] += SHVec3::Dot(constraint.invInertiaB * RB_CROSS_T, RB_CROSS_T);
+
+          contact.tangentMass[j] = 1.0f / contact.tangentMass[j];
+        }
+
+        // Calculate bias per contact
+        /*
+         * error bias       = baumgarte factor / dt * penetration
+         * restituion bias  = restitution * (relative velocity /dot normal)
+         */
+
+        const float ERROR_BIAS = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP);
+        
+        // Warm starting
+        // Compute impulses
+        SHVec3 impulse = constraint.normal * contact.normalImpulse;
+        for (int j = 0; j < SHContact::NUM_TANGENTS; ++j)
+          impulse += constraint.tangents[j] * contact.tangentImpulse[j];
+
+        // Apply impulses onto velocities
+        vA -= impulse * constraint.invMassA * LINEAR_LOCK_A;
+        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, impulse) * ANGULAR_LOCK_A;
+
+        vB += impulse * constraint.invMassB * LINEAR_LOCK_B;
+        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, impulse) * ANGULAR_LOCK_B;
+
+        const SHVec3  RV_A = vA + SHVec3::Cross(wA, contact.rA);
+        const SHVec3  RV_B = vB + SHVec3::Cross(wB, contact.rB);
+        const float   RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal);
+
+        const float RESTITUTION_BIAS = RV_N > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f;
+        contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
+      }
+
+      velocityStateA.LinearVelocity   = vA;
+      velocityStateA.AngularVelocity  = wA;
+      velocityStateB.LinearVelocity   = vB;
+      velocityStateB.AngularVelocity  = wB;
+    }
+  }
+
+  void SHContactSolver::solve() noexcept
+  {
+    for (auto& [key, constraint] : contactConstraints)
+    {
+      SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second;
+      SHVelocityState& velocityStateB = velocityStates.find(key.GetEntityB())->second;
+
+      SHVec3 vA = velocityStateA.LinearVelocity;
+      SHVec3 wA = velocityStateA.AngularVelocity;
+      SHVec3 vB = velocityStateB.LinearVelocity;
+      SHVec3 wB = velocityStateB.AngularVelocity;
+
+      const SHVec3& LINEAR_LOCK_A   = velocityStateA.LinearLockFactor;
+      const SHVec3& ANGULAR_LOCK_A  = velocityStateA.AngularLockFactor;
+      const SHVec3& LINEAR_LOCK_B   = velocityStateB.LinearLockFactor;
+      const SHVec3& ANGULAR_LOCK_B  = velocityStateB.AngularLockFactor;
+
+      for (uint32_t i = 0; i < constraint.numContacts; ++i)
+      {
+        SHContact& contact = constraint.contacts[i];
+
+        // Compute relative velocity
+        SHVec3 velocityA = vA + SHVec3::Cross(wA, contact.rA);
+        SHVec3 velocityB = vB + SHVec3::Cross(wB, contact.rB);
+        SHVec3 relativeVelocity = velocityB - velocityA;
+
+        // Solve tangent impulse
+        for (int j = 0; j < SHContact::NUM_TANGENTS; ++j)
+        {
+          // Get scalar of relative velocity along tangent
+          const float VT = SHVec3::Dot(relativeVelocity, constraint.tangents[j]);
+
+          // Compute true tangent impulse
+          const float MAX_TANGENT_IMPULSE = constraint.friction * contact.normalImpulse;
+          const float OLD_TANGENT_IMPULSE = contact.tangentImpulse[j];
+
+          // We cannot exceed the maximum frictional force (coulumb's law)
+          // Compute true tangent impulse
+          float newTangentImpulse = -VT * contact.tangentMass[j];
+          contact.tangentImpulse[j] = std::clamp(OLD_TANGENT_IMPULSE + newTangentImpulse, -MAX_TANGENT_IMPULSE, MAX_TANGENT_IMPULSE);
+          newTangentImpulse = contact.tangentImpulse[j] - OLD_TANGENT_IMPULSE;
+
+          const SHVec3 TANGENT_IMPULSE = newTangentImpulse * constraint.tangents[j];
+
+          // Apply impulses
+          vA -= TANGENT_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
+          wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, TANGENT_IMPULSE) * ANGULAR_LOCK_A;
+
+          vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
+          wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B;
+        }
+
+        // Solve normal impulse
+        // Re-compute relative velocity
+        velocityA = vA + SHVec3::Cross(wA, contact.rA);
+        velocityB = vB + SHVec3::Cross(wB, contact.rB);
+        relativeVelocity = velocityB - velocityA;
+
+        // Get scalar of relative velocity along the normal
+        const float VN = SHVec3::Dot(relativeVelocity, constraint.normal);
+
+        // Compute true normal impulse
+        const float OLD_NORMAL_IMPULSE = contact.normalImpulse;
+
+        float newNormalImpulse = -(VN + contact.bias) * contact.normalMass;
+        contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f);
+        newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE;
+
+        const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
+
+        // Apply impulses
+        vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
+        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
+
+        vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
+        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
+      }
+
+      velocityStateA.LinearVelocity   = vA;
+      velocityStateA.AngularVelocity  = wA;
+      velocityStateB.LinearVelocity   = vB;
+      velocityStateB.AngularVelocity  = wB;
+    }
+  }
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h

@@ -1,7 +1,8 @@
 /****************************************************************************************
- * \file      SHShape.h
+ * \file      SHContactSolver.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a shape.
+ * \brief     Interface for a Contact Solver that builds contacct constraints and solves
+ *            them.
  *
  * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
  *            disclosure of this file or its contents without the prior written consent
@@ -11,9 +12,8 @@
 #pragma once
 
 // Project Headers
-#include "SH_API.h"
+#include "Constraints/SHContactConstraint.h"
-#include "Math/SHRay.h"
+#include "Constraints/SHVelocityState.h"
-
 
 namespace SHADE
 {
@@ -21,62 +21,73 @@ namespace SHADE
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  class SH_API SHShape
+  /**
+   * @brief
+   * Encapsulates an object that builds contact constraints and solves them.
+  */
+  class SH_API SHContactSolver
   {
   public:
     /*---------------------------------------------------------------------------------*/
     /* Type Definitions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    enum class Type
+    using VelocityStates      = std::unordered_map<EntityID, SHVelocityState>; 
-    {
+    using ContactConstraints  = std::unordered_map<SHCollisionKey, SHContactConstraint, SHCollisionKeyHash>;
-      BOX
-    , SPHERE
-    , CAPSULE
-    , CONVEX_HULL
-
-    , COUNT
-    , NONE    =  -1
-    };
-
-    /*---------------------------------------------------------------------------------*/
-    /* Data Members                                                                    */
-    /*---------------------------------------------------------------------------------*/
-
-    bool isIntersecting;
 
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    virtual ~SHShape  ()                = default;
+    SHContactSolver   () noexcept = default;
-
+    ~SHContactSolver  () noexcept = default;
-    SHShape           (const SHShape&)  = default;
-    SHShape           (SHShape&&)       = default;
-
-    SHShape& operator=(const SHShape&)  = default;
-    SHShape& operator=(SHShape&&)       = default;
-
-    SHShape();
 
     /*---------------------------------------------------------------------------------*/
     /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    [[nodiscard]] Type          GetType   () const noexcept;
+    [[nodiscard]] const VelocityStates&     GetVelocities         () const noexcept;
+    [[nodiscard]] const ContactConstraints& GetContantConstraints () const noexcept;
 
     /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
+    /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    [[nodiscard]] virtual bool            TestPoint (const SHVec3& point) const noexcept = 0;
+    /**
-    [[nodiscard]] virtual SHRaycastResult Raycast   (const SHRay& ray)    const noexcept = 0;
+     * @brief
+     * Build a contact constraint from a new manifold.
+     * @param manifold
+     * A manifold to build a contact constraint from.
+    */
+    void AddContact     (const SHCollisionKey& key, const SHManifold& manifold) noexcept;
 
-  protected:
+    void Reset          () noexcept;
+
+    /**
+     * @brief
+     * Solves all the contact constraints.
+     * @param numIterations
+     * The number of times to iterate over constraints when solving them.
+     * @param dt
+     * The delta time of the simulation step.
+    */
+    void SolveContacts  (int numIterations, float dt) noexcept;
+
+  private:
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    Type    type;
+    VelocityStates      velocityStates;
+    ContactConstraints  contactConstraints;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void preSolve   (float dt)  noexcept;
+    void solve      ()          noexcept;
+
   };
+
 } // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp

@@ -0,0 +1,131 @@
+/****************************************************************************************
+ * \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      = true;
+
+    prevPosition  = newPosition;
+    position      = newPosition;
+  }
+
+  void SHMotionState::ForceOrientation(const SHQuaternion& newOrientation) noexcept
+  {
+    hasMoved      = true;
+
+    prevOrientation  = newOrientation;
+    orientation      = newOrientation;
+  }
+
+
+  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;
+  }
+
+  void SHMotionState::IntegrateOrientation(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;
+
+    prevOrientation = orientation;
+
+    SHQuaternion qv{ velocity.x * dt, velocity.y * dt, velocity.z * dt, 0.0f };
+    qv *= orientation;
+
+    orientation += qv * 0.5f;
+    orientation = SHQuaternion::Normalise(orientation);
+  }
+
+
+  SHVec3 SHMotionState::InterpolatePositions(float factor) const noexcept
+  {
+    return SHVec3::ClampedLerp(prevPosition, position, factor);
+  }
+
+  SHQuaternion SHMotionState::InterpolateOrientations(float factor) const noexcept
+  {
+    return SHQuaternion::ClampedSlerp(prevOrientation, orientation, factor);
+  }
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHMotionState.h

@@ -0,0 +1,127 @@
+/****************************************************************************************
+ * \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/SHQuaternion.h"
+#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;
+
+    SHQuaternion  orientation;
+    SHQuaternion  prevOrientation;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* 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
+     * Forcefully sets the orientation. Meant to be used when transform overrides the rigid body
+     * orientations
+     * @param newOrientation 
+     * The new orientation to set.
+    */
+    void        ForceOrientation          (const SHQuaternion& newOrientation)  noexcept;
+
+    /**
+     * @brief
+     * Integrates the positions using linear velocity with respect to time.
+     * @param velocity
+     * The linear velocity to integrate.
+     * @param dt
+     * The delta time to integrate with respect to.
+    */
+    void        IntegratePosition         (const SHVec3& velocity, float dt)    noexcept;
+
+    /**
+     * @brief
+     * Integrates the orientation using angular velocity with respect to time.
+     * @param velocity
+     * The angular velocity to integrate.
+     * @param dt
+     * The delta time to integrate with respect to.
+    */
+    void        IntegrateOrientation      (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;
+
+    /**
+     * @brief
+     * Interpolates the orientation 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 orientation meant for rendering.
+    */
+    SHQuaternion  InterpolateOrientations (float factor)                  const noexcept;
+
+  };
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp

@@ -0,0 +1,190 @@
+/****************************************************************************************
+ * \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                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHPhysicsWorld::SHPhysicsWorld(const WorldSettings& worldSettings) noexcept
+  : settings        { worldSettings }
+  , collisionSpace  { nullptr }
+  {
+    SHLOG_INFO_D("Creating Physics World")
+  }
+
+  SHPhysicsWorld::~SHPhysicsWorld() noexcept
+  {
+    collisionSpace = nullptr;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  const SHContactManager::ContactPoints& SHPhysicsWorld::GetContactPoints() const noexcept
+  {
+    return contactManager.GetContactPoints();
+  }
+
+  const SHContactManager::TriggerEvents& SHPhysicsWorld::GetTriggerEvents() const noexcept
+  {
+    return contactManager.GetTriggerEvents();
+  }
+
+  const SHContactManager::CollisionEvents& SHPhysicsWorld::GetCollisionEvents() const noexcept
+  {
+    return contactManager.GetCollisionEvents();
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHPhysicsWorld::SetCollisionSpace(SHCollisionSpace* _collisionSpace) noexcept
+  {
+    collisionSpace = _collisionSpace;
+    _collisionSpace->SetContactManager(&contactManager);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* 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);
+
+    // Contact manager to remove invalidated contacts
+    contactManager.RemoveInvalidatedManifold(rigidBody->entityID);
+
+    // TODO: Run through the rigid body's contact graph and wake all of its non-static bodies that are asleep
+  }
+
+
+  void SHPhysicsWorld::Step(float dt)
+  {
+    /*
+     * Detect Collisions
+     */
+
+    if (collisionSpace)
+      collisionSpace->DetectCollisions();
+
+    /*
+     * Integrate Forces
+     */
+
+    for (auto* rigidBody : rigidBodies | std::views::values)
+    {
+      if (!rigidBody->IsActive())
+        continue;
+
+      rigidBody->ComputeWorldData();
+      integrateForces(*rigidBody, dt);
+    }
+
+
+    /*
+     * Resolve Contacts
+     */
+
+    contactManager.SolveCollisions(settings.numVelocitySolverIterations, dt);
+
+    /*
+     * Integrate Velocities
+     */
+
+    for (auto* rigidBody : rigidBodies | std::views::values)
+    {
+      if (!rigidBody->IsActive())
+        continue;
+
+      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;
+
+    // Integrate torque into angular velocity
+    rigidBody.angularVelocity += rigidBody.worldInvInertia * (rigidBody.accumulatedTorque * dt);
+
+    // Apply drag (exponentially applied)
+    rigidBody.linearVelocity  *= 1.0f / (1.0f + dt * rigidBody.linearDrag);
+    rigidBody.angularVelocity *= 1.0f / (1.0f + dt * rigidBody.angularDrag);
+
+    rigidBody.constrainLinearVelocities();
+    rigidBody.constrainAngularVelocities();
+  }
+
+  void SHPhysicsWorld::integrateVelocities(SHRigidBody& rigidBody, float dt) const noexcept
+  {
+    // 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;
+    }
+    // Dynamic & Kinematic bodies
+    // Both dynamic and kinematic can sleep when their velocities are under the thresholds.
+    else if (!rigidBody.IsSleeping())
+    {
+      rigidBody.constrainLinearVelocities();
+      rigidBody.constrainAngularVelocities();
+
+      rigidBody.motionState.IntegratePosition(rigidBody.linearVelocity, dt);
+      rigidBody.motionState.IntegrateOrientation(rigidBody.angularVelocity, dt);
+
+      // Sync with collider transforms if a collider is present
+      if (rigidBody.collider)
+      {
+        rigidBody.collider->SetPosition(rigidBody.motionState.position);
+        rigidBody.collider->SetOrientation(rigidBody.motionState.orientation);
+
+        rigidBody.collider->Update();
+      }
+    }
+
+    // Clear all forces
+    // We clear forces for static bodies as well for redundancy
+    rigidBody.ClearForces();
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h

@@ -0,0 +1,149 @@
+/****************************************************************************************
+ * \file      SHPhysicsWorld.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface 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.
+****************************************************************************************/
+
+#pragma once
+
+#include <unordered_map>
+
+// Project Headers
+#include "Physics/Collision/SHCollisionSpace.h"
+#include "SHContactManager.h"
+#include "SHContactSolver.h"
+#include "SHRigidBody.h"
+
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates the overall simulation of physics. The bulk of dynamics are handled here,
+   * with the collision detection handled by an attached collision space. <br/>
+   * A collision space must be created separately and attached with the world.
+  */
+  class SH_API SHPhysicsWorld
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct WorldSettings
+    {
+    public:
+      /*-------------------------------------------------------------------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      SHVec3    gravity                     = SHVec3{ 0.0f, -9.81f, 0.0f };
+      uint16_t  numVelocitySolverIterations = 10;
+      uint16_t  numPositionSolverIterations = 5;      // Unused until PGS is implemented
+      bool      sleepingEnabled             = true;
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    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;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    const SHContactManager::ContactPoints&    GetContactPoints    ()  const noexcept;
+    const SHContactManager::TriggerEvents&    GetTriggerEvents    ()  const noexcept;
+    const SHContactManager::CollisionEvents&  GetCollisionEvents  ()  const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void SetCollisionSpace(SHCollisionSpace* collisionSpace) noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Adds a rigid body to the world for it to be simulated using motion dynamics.
+     * @param rigidBody
+     * A rigid body to add. Duplicates will be ignored.
+    */
+    void AddRigidBody     (SHRigidBody* rigidBody)  noexcept;
+
+    /**
+     * @brief
+     * Removes a rigid body from the world. It's motion will not be affected unless
+     * explicitly modified.
+     * @param rigidBody
+     * A rigid body to add. Duplicates will be ignored.
+    */
+    void RemoveRigidBody  (SHRigidBody* rigidBody)  noexcept;
+
+    /**
+     * @brief
+     * Performs a single simulation step. <br/>
+     * Detect Collisions -> Integrate Forces -> Resolve Contacts -> Integrate Velocities
+     * @param dt
+     * A discrete time step for the simulation. This should be consistent for deteministic
+     * behaviour.
+    */
+    void Step             (float dt);
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    // EntityIDs are used to map resolved contraints back to bodies
+    using RigidBodies       = std::unordered_map<EntityID, SHRigidBody*>;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    WorldSettings     settings;
+    SHCollisionSpace* collisionSpace;
+
+    RigidBodies       rigidBodies;
+    SHContactManager  contactManager;
+    SHContactSolver   contactSolver;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Function Members                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    // TODO: Move to island when islands are set up
+    void integrateForces            (SHRigidBody& rigidBody, float dt) const noexcept;
+    void integrateVelocities        (SHRigidBody& rigidBody, float dt) const noexcept;
+
+    
+    
+
+  };
+
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp

@@ -0,0 +1,780 @@
+/****************************************************************************************
+ * \file      SHRigidBody.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Rigid Body.
+ *
+ * \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 "SHRigidBody.h"
+
+// Project Headers
+#include <complex.h>
+#include <numeric>
+
+#include "Physics/Collision/SHCollider.h"
+#include "Tools/Logger/SHLogger.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHRigidBody::SHRigidBody(EntityID eid, Type type) noexcept
+  : entityID      { eid }
+  , collider      { nullptr }
+  , bodyType      { type }
+  , gravityScale  { 1.0f }
+  , invMass       { type == Type::DYNAMIC ? 1.0f : 0.0f }
+  , linearDrag    { 0.01f }
+  , angularDrag   { 0.01f }
+  , flags         { 0U }
+  {
+    // Set default flags
+    flags |= 1U << 0;   // Body is active
+    flags |= 1U << 2;   // Sleeping is enabled
+    flags |= 1U << 3;   // Gravity is enabled
+
+    // TODO: Compute inertia if body is dynamic
+  }
+
+  SHRigidBody::SHRigidBody(const SHRigidBody& rhs) noexcept
+  : entityID        { rhs.entityID }
+  , collider        { nullptr }
+  , bodyType        { rhs.bodyType }
+  , gravityScale    { rhs.gravityScale }
+  , invMass         { rhs.invMass }
+  , linearDrag      { rhs.linearDrag }
+  , angularDrag     { rhs.angularDrag }
+  , localInvInertia { rhs.localInvInertia }
+  , worldInvInertia { rhs.worldInvInertia }
+  , localCentroid   { rhs.localCentroid }
+  , worldCentroid   { rhs.worldCentroid }
+  , flags           { rhs.flags }
+  , motionState     { rhs.motionState }
+  {
+    // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
+  }
+
+  SHRigidBody::SHRigidBody(SHRigidBody&& rhs) noexcept
+  : entityID        { rhs.entityID }
+  , collider        { nullptr }
+  , bodyType        { rhs.bodyType }
+  , gravityScale    { rhs.gravityScale }
+  , invMass         { rhs.invMass }
+  , linearDrag      { rhs.linearDrag }
+  , angularDrag     { rhs.angularDrag }
+  , localInvInertia { rhs.localInvInertia }
+  , worldInvInertia { rhs.worldInvInertia }
+  , localCentroid   { rhs.localCentroid }
+  , worldCentroid   { rhs.worldCentroid }
+  , flags           { rhs.flags }
+  , motionState     { std::move(rhs.motionState) }
+  {
+    // All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHRigidBody& SHRigidBody::operator=(const SHRigidBody& rhs) noexcept
+  {
+    // Handle self assignment
+    if (this == &rhs)
+      return *this;
+
+    entityID      = rhs.entityID;
+
+    // Deep copy the collider
+    *collider       = *rhs.collider;
+    bodyType        = rhs.bodyType;
+    gravityScale    = rhs.gravityScale;
+    invMass         = rhs.invMass;
+    linearDrag      = rhs.linearDrag;
+    angularDrag     = rhs.angularDrag;
+    localInvInertia = rhs.localInvInertia;
+    worldInvInertia = rhs.worldInvInertia;
+    localCentroid   = rhs.localCentroid;
+    worldCentroid   = rhs.worldCentroid;
+    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;
+    accumulatedTorque = SHVec3::Zero;
+    linearVelocity    = SHVec3::Zero;
+    angularVelocity   = SHVec3::Zero;
+
+    return *this;
+  }
+
+  SHRigidBody& SHRigidBody::operator=(SHRigidBody&& rhs) noexcept
+  {
+    entityID        = rhs.entityID;
+
+    // Deep copy the collider
+    *collider       = *rhs.collider;
+    bodyType        = rhs.bodyType;
+    gravityScale    = rhs.gravityScale;
+    invMass         = rhs.invMass;
+    linearDrag      = rhs.linearDrag;
+    angularDrag     = rhs.angularDrag;
+    localInvInertia = rhs.localInvInertia;
+    worldInvInertia = rhs.worldInvInertia;
+    localCentroid   = rhs.localCentroid;
+    worldCentroid   = rhs.worldCentroid;
+    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;
+    accumulatedTorque = SHVec3::Zero;
+    linearVelocity    = SHVec3::Zero;
+    angularVelocity   = SHVec3::Zero;
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHRigidBody::Type  SHRigidBody::GetType() const noexcept
+  {
+    return bodyType;
+  }
+
+  float SHRigidBody::GetGravityScale() const noexcept
+  {
+    return gravityScale;
+  }
+
+  float SHRigidBody::GetMass() const noexcept
+  {
+    return 1.0f/ invMass;
+  }
+
+  float SHRigidBody::GetLinearDrag() const noexcept
+  {
+    return linearDrag;
+  }
+
+  float SHRigidBody::GetAngularDrag() const noexcept
+  {
+    return angularDrag;
+  }
+
+  const SHVec3& SHRigidBody::GetLocalInvInertia() const noexcept
+  {
+    return localInvInertia;
+  }
+
+  const SHVec3& SHRigidBody::GetWorldInvInertia() const noexcept
+  {
+    return worldInvInertia;
+  }
+
+  const SHVec3& SHRigidBody::GetLocalCentroid() const noexcept
+  {
+    return localCentroid;
+  }
+
+  const SHVec3& SHRigidBody::GetWorldCentroid() const noexcept
+  {
+    return worldCentroid;
+  }
+
+  const SHVec3& SHRigidBody::GetForce() const noexcept
+  {
+    return accumulatedForce;
+  }
+
+  const SHVec3& SHRigidBody::GetTorque() const noexcept
+  {
+    return accumulatedTorque;
+  }
+
+  const SHVec3& SHRigidBody::GetLinearVelocity() const noexcept
+  {
+    // Check if linear velocity needs to be constrained
+
+    return linearVelocity;
+  }
+
+  const SHVec3& SHRigidBody::GetAngularVelocity() const noexcept
+  {
+    return angularVelocity;
+  }
+
+  // Flags
+
+  bool SHRigidBody::IsActive() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 0;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::IsSleeping() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 1;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::IsSleepingEnabled() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 2;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::IsGravityEnabled() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 3;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::IsAutoMassEnabled() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 4;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::IsTriggerInMassData() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 6;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::GetFreezePositionX() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 10;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::GetFreezePositionY() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 11;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::GetFreezePositionZ() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 12;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::GetFreezeRotationX() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 13;
+    return flags & (1U << FLAG_POS);
+  }
+
+  bool SHRigidBody::GetFreezeRotationY() const noexcept
+  {
+    static constexpr unsigned int FLAG_POS = 14;
+    return flags & (1U << FLAG_POS);
+  }
+
+  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::SetCollider(SHCollider* c) noexcept
+  {
+    collider = c;
+  }
+
+  void SHRigidBody::SetType(Type newType) noexcept
+  {
+    if (newType == bodyType)
+      return;
+
+    bodyType    = newType;
+
+    if (bodyType != Type::DYNAMIC)
+    {
+      invMass         = 0.0f;
+      localInvInertia = SHVec3::Zero;
+      worldInvInertia = SHVec3::Zero;
+    }
+    else
+    {
+      invMass         = 1.0f;
+      localInvInertia = SHVec3 { 1.0f };
+    }
+  }
+
+  void SHRigidBody::SetGravityScale(float newGravityScale) noexcept
+  {
+    gravityScale = newGravityScale;
+  }
+
+  void SHRigidBody::SetMass(float newMass) noexcept
+  {
+    if (bodyType != Type::DYNAMIC)
+    {
+      invMass = 0.0f;
+      SHLOG_WARNING("Cannot set mass of a non-Dynamic Body {}", entityID)
+      return;
+    }
+
+    if (newMass < 0.0f)
+    {
+      SHLOG_WARNING("Cannot set mass below 0. Object {}'s mass will remain unchanged.", entityID)
+      return;
+    }
+
+    invMass = 1.0f / newMass;
+
+    // Turn off automass
+    static constexpr unsigned int AUTO_MASS_FLAG = 4;
+    static constexpr uint16_t     VALUE = 1U << AUTO_MASS_FLAG;
+
+    flags &= ~(VALUE);
+    ComputeMassData();
+  }
+
+  void SHRigidBody::SetLinearDrag(float newLinearDrag) noexcept
+  {
+    if (bodyType == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set linear drag of a Static Body {}", entityID)
+      return;
+    }
+
+    if (newLinearDrag < 0.0f)
+    {
+      SHLOG_WARNING("Cannot set drag below 0. Object {}'s linear drag will remain unchanged.", entityID)
+      return;
+    }
+
+    linearDrag = newLinearDrag;
+  }
+
+  void SHRigidBody::SetAngularDrag(float newAngularDrag) noexcept
+  {
+    if (bodyType == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set angular drag of a Static Body {}", entityID)
+      return;
+    }
+
+    if (newAngularDrag < 0.0f)
+    {
+      SHLOG_WARNING("Cannot set drag below 0. Object {}'s angular drag will remain unchanged.", entityID)
+      return;
+    }
+
+    angularDrag = newAngularDrag;
+  }
+
+  void SHRigidBody::SetLinearVelocity(const SHVec3& newLinearVelocity) noexcept
+  {
+    if (bodyType == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set linear velocity of a Static Body {}", entityID)
+      return;
+    }
+
+    linearVelocity = newLinearVelocity;
+    constrainLinearVelocities();
+  }
+
+  void SHRigidBody::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept
+  {
+    if (bodyType == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set angular velocity of a Static Body {}", entityID)
+      return;
+    }
+
+    angularVelocity = newAngularVelocity;
+    constrainAngularVelocities();
+  }
+
+  void SHRigidBody::SetIsActive(bool isActive) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 0;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    isActive ? flags |= VALUE : flags &= ~VALUE;
+  }
+
+  void SHRigidBody::SetIsSleeping(bool isSleeping) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 1;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (isSleeping)
+    {
+      flags |= VALUE;
+
+      ClearForces();
+      linearVelocity  = SHVec3::Zero;
+      angularVelocity = SHVec3::Zero;
+    }
+    else
+    {
+      flags &= ~VALUE;
+    }
+  }
+
+  void SHRigidBody::SetSleepingEnabled(bool enableSleeping) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 2;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (enableSleeping)
+    {
+      flags |= VALUE;
+    }
+    else
+    {
+      flags &= ~VALUE;
+      // Wake the body
+      SetIsSleeping(false);
+    }
+  }
+
+  void SHRigidBody::SetGravityEnabled(bool enableGravity) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 3;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    enableGravity ? flags |= VALUE : flags &= ~VALUE;
+  }
+
+  void SHRigidBody::SetAutoMassEnabled(bool enableAutoMass) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 4;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (enableAutoMass)
+    {
+      flags |= VALUE;
+    }
+    else
+    {
+      flags &= ~VALUE;
+      // Use default mass of 1
+      invMass = 1.0f;
+    }
+
+    ComputeMassData();
+  }
+
+  void SHRigidBody::SetTriggerInMassData(bool triggerInMassData) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 6;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    triggerInMassData ? flags |= VALUE : flags &= ~VALUE;
+  }
+
+  void SHRigidBody::SetFreezePositionX(bool freezePositionX) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 10;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (freezePositionX)
+    {
+      flags |= VALUE;
+      // Reset linear velocity along X-axis
+      linearVelocity.x = 0.0f;
+    }
+    else
+    {
+      flags &= ~VALUE;
+    }
+  }
+
+  void SHRigidBody::SetFreezePositionY(bool freezePositionY) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 11;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (freezePositionY)
+    {
+      flags |= VALUE;
+      // Reset linear velocity along Y-axis
+      linearVelocity.y = 0.0f;
+    }
+    else
+    {
+      flags &= ~VALUE;
+    }
+  }
+
+  void SHRigidBody::SetFreezePositionZ(bool freezePositionZ) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 12;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (freezePositionZ)
+    {
+      flags |= VALUE;
+      // Reset linear velocity along Z-axis
+      linearVelocity.z = 0.0f;
+    }
+    else
+    {
+      flags &= ~VALUE;
+    }
+  }
+
+  void SHRigidBody::SetFreezeRotationX(bool freezeRotationX) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 13;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (freezeRotationX)
+    {
+      flags |= VALUE;
+      // Reset angular velocity along X-axis
+      angularVelocity.x = 0.0f;
+      // Set inertia tensor on the x-axis to 0
+      localInvInertia.x = 0.0f;
+    }
+    else
+    {
+      flags &= ~VALUE;
+      ComputeMassData();
+    }
+  }
+
+  void SHRigidBody::SetFreezeRotationY(bool freezeRotationY) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 14;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (freezeRotationY)
+    {
+      flags |= VALUE;
+      // Reset angular velocity along Y-axis
+      angularVelocity.y = 0.0f;
+      // Set inertia tensor on the y-axis to 0
+      localInvInertia.y = 0.0f;
+    }
+    else
+    {
+      flags &= ~VALUE;
+      ComputeMassData();
+    }
+  }
+
+  void SHRigidBody::SetFreezeRotationZ(bool freezeRotationZ) noexcept
+  {
+    static constexpr unsigned int FLAG_POS  = 15;
+    static constexpr uint16_t     VALUE     = 1U << FLAG_POS;
+
+    if (freezeRotationZ)
+    {
+      flags |= VALUE;
+      // Reset angular velocity along Z-axis
+      angularVelocity.z = 0.0f;
+      // Set inertia tensor on the z-axis to 0
+      localInvInertia.z = 0.0f;
+    }
+    else
+    {
+      flags &= ~VALUE;
+      ComputeMassData();
+    }
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definition                                                 */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHRigidBody::AddForce(const SHVec3& force, const SHVec3& pos) noexcept
+  {
+    if (bodyType != Type::DYNAMIC)
+      return;
+
+    accumulatedForce  += force;
+    angularVelocity += worldInvInertia * SHVec3::Cross(pos, force);
+  }
+
+  void SHRigidBody::AddImpulse(const SHVec3& impulse, const SHVec3& pos) noexcept
+  {
+    if (bodyType != Type::DYNAMIC)
+      return;
+
+    linearVelocity  += impulse * invMass;
+    angularVelocity += worldInvInertia * SHVec3::Cross(pos, impulse);
+  }
+
+  void SHRigidBody::AddTorque(const SHVec3& torque) noexcept
+  {
+    if (bodyType != Type::DYNAMIC)
+      return;
+
+    accumulatedTorque += torque;
+  }
+
+  void SHRigidBody::ClearForces() noexcept
+  {
+    accumulatedForce  = SHVec3::Zero;
+    accumulatedTorque = SHVec3::Zero;
+  }
+
+  void SHRigidBody::ComputeWorldData() noexcept
+  {
+    if (bodyType == Type::STATIC)
+      return;
+
+    const SHMatrix R  = SHMatrix::Rotate(motionState.orientation);
+    const SHMatrix RT = SHMatrix::Transpose(R);
+
+    /*
+     *  Compute world inertia as a Vector
+     *  
+     *  | a b c || x |   | ax + by + cz |
+     *  | d e f || y | = | dx + ey + fz |
+     *  | g h i || z |   | gx + hy + iz |
+     */
+    
+    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) + motionState.position;
+  }
+
+  void SHRigidBody::ComputeMassData() noexcept
+  {
+    // Reset centroid
+    localCentroid   = SHVec3::Zero;
+    localInvInertia = SHVec3{ 1.0f };
+
+    // In the instance the body is a particle
+    if (!collider || collider->GetCollisionShapes().empty())
+    {
+      localInvInertia = SHVec3{ invMass };
+      return;
+    }
+
+    const float CUSTOM_MASS       = 1.0f / invMass;
+    const bool  AUTO_MASS         = IsAutoMassEnabled();
+    const bool  INCLUDE_TRIGGERS  = IsTriggerInMassData();
+    
+
+    // Compute Total mass and store individual masses if custom mass is being used.
+    // Compute local centroid at the same time
+
+    // Zero matrix;
+    SHMatrix J = SHMatrix::Zero;
+
+    float totalMass = 0.0f;
+    std::vector<float> trueMass;  // We store the true masses here for calculating the ratio with custom masses.
+
+    const auto& SHAPES = collider->GetCollisionShapes();
+    for (auto* shape : SHAPES)
+    {
+      // We skip triggers by default
+      if (shape->IsTrigger() && !INCLUDE_TRIGGERS)
+        continue;
+
+      // p = m/v, therefore m = pv. This is the true mass of the shape.
+      const float MASS = shape->GetDensity() * shape->GetVolume();
+      totalMass += MASS;
+
+      trueMass.emplace_back(MASS);
+
+      // Weighted sum of masses contribute to the centroid's location using the collider's local position.
+      localCentroid += MASS * shape->GetRelativeCentroid();
+    }
+
+    if (totalMass > 0.0f)
+    {
+      localCentroid /= totalMass;
+
+      if (AUTO_MASS)
+        invMass = 1.0f / totalMass;
+    }
+
+    const SHMatrix R  = SHMatrix::Rotate(motionState.orientation);
+    const SHMatrix RT = SHMatrix::Transpose(R);
+
+    // We need the world centroid to compute the offset of the collider from the body's centroid
+    worldCentroid = (R * localCentroid) + motionState.position;
+
+    for (size_t i = 0; i < SHAPES.size(); ++i)
+    {
+      const auto* SHAPE = SHAPES[i];
+
+      // We skip triggers by default
+      if (SHAPE->IsTrigger() && !INCLUDE_TRIGGERS)
+        continue;
+
+      // If using custom mass, take the ratio of the mass
+      float actualMass = trueMass[i];
+      if (!AUTO_MASS)
+        actualMass *= CUSTOM_MASS / totalMass;
+
+      // Convert inertia tensor into local-space of the body
+      // R * I * RT = R * (I * RT)
+      SHMatrix I  = SHAPE->GetInertiaTensor( actualMass ) * RT;
+      I = R * I;
+
+      // Parallel Axis Theorem
+      // https://en.wikipedia.org/wiki/Parallel_axis_theorem
+      // J = I + m((R /dot R)E_3 - R /outerProduct R)
+      const SHVec3    R       = SHAPE->GetWorldCentroid() - worldCentroid;
+      const float     R_MAG2  = R.LengthSquared();
+      const SHMatrix  R_OX_R  = SHVec3::OuterProduct(R, R);
+
+      J += I + actualMass * (SHMatrix::Identity * R_MAG2 - R_OX_R);
+    }
+
+    // Set diagonals then invert
+    localInvInertia.x = totalMass == 0.0f ? 0.0f : 1.0f / J.m[0][0];
+    localInvInertia.y = totalMass == 0.0f ? 0.0f : 1.0f / J.m[1][1];
+    localInvInertia.z = totalMass == 0.0f ? 0.0f : 1.0f / J.m[2][2];
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Function Definition                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHRigidBody::constrainLinearVelocities() noexcept
+  {
+    linearVelocity.x = GetFreezePositionX() ? 0.0f : linearVelocity.x;
+    linearVelocity.y = GetFreezePositionY() ? 0.0f : linearVelocity.y;
+    linearVelocity.z = GetFreezePositionZ() ? 0.0f : linearVelocity.z;
+  }
+
+  void SHRigidBody::constrainAngularVelocities() noexcept
+  {
+    angularVelocity.x = GetFreezeRotationX() ? 0.0f : angularVelocity.x;
+    angularVelocity.y = GetFreezeRotationY() ? 0.0f : angularVelocity.y;
+    angularVelocity.z = GetFreezeRotationZ() ? 0.0f : angularVelocity.z;
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h

@@ -0,0 +1,268 @@
+/****************************************************************************************
+ * \file      SHRigidBody.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Rigid Body.
+ *
+ * \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 "ECS_Base/Entity/SHEntity.h"
+#include "Math/SHMatrix.h"
+#include "Math/Vector/SHVec3.h"
+#include "SHMotionState.h"
+
+namespace SHADE
+{
+  /*-------------------------------------------------------------------------------------*/
+  /* Forward Declarations                                                                */
+  /*-------------------------------------------------------------------------------------*/
+
+  class SHCollider;
+
+  /*-------------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                    */
+  /*-------------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates a Rigid Body used in Physics Simulations
+  */
+  class SH_API SHRigidBody
+  {
+  private:
+    /*-----------------------------------------------------------------------------------*/
+    /* Friends                                                                           */
+    /*-----------------------------------------------------------------------------------*/
+
+    friend class SHPhysicsWorld;
+    friend class SHContactSolver;
+
+  public:
+    /*-----------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    enum class Type
+    {
+      STATIC    // Immovable body with infinite mass
+    , KINEMATIC // Only movable by setting velocity, unaffected by forces. Has infinite mass.
+    , DYNAMIC   // Affected by forces.
+    };
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                         */
+    /*-----------------------------------------------------------------------------------*/
+
+    SHRigidBody (EntityID eid, Type type) noexcept;
+    SHRigidBody (const SHRigidBody& rhs)  noexcept;
+    SHRigidBody (SHRigidBody&& rhs)       noexcept;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                                */
+    /*-----------------------------------------------------------------------------------*/
+
+    SHRigidBody& operator=  (const SHRigidBody& rhs)  noexcept;
+    SHRigidBody& operator=  (SHRigidBody&& rhs)       noexcept;
+  
+    /*-----------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    [[nodiscard]] Type            GetType             ()  const noexcept;
+
+    [[nodiscard]] float           GetGravityScale     ()  const noexcept;
+
+    [[nodiscard]] float           GetMass             ()  const noexcept;
+    [[nodiscard]] float           GetLinearDrag       ()  const noexcept;
+    [[nodiscard]] float           GetAngularDrag      ()  const noexcept;
+
+    [[nodiscard]] const SHVec3&   GetLocalInvInertia  ()  const noexcept;
+    [[nodiscard]] const SHVec3&   GetWorldInvInertia  ()  const noexcept;
+
+    [[nodiscard]] const SHVec3&   GetLocalCentroid    ()  const noexcept;
+    [[nodiscard]] const SHVec3&   GetWorldCentroid    ()  const noexcept;
+
+    [[nodiscard]] const SHVec3&   GetForce            ()  const noexcept;
+    [[nodiscard]] const SHVec3&   GetTorque           ()  const noexcept;
+    [[nodiscard]] const SHVec3&   GetLinearVelocity   ()  const noexcept;
+    [[nodiscard]] const SHVec3&   GetAngularVelocity  ()  const noexcept;
+
+    // Flags
+
+    [[nodiscard]] bool            IsActive            ()  const noexcept;
+    [[nodiscard]] bool            IsSleeping          ()  const noexcept;
+    [[nodiscard]] bool            IsSleepingEnabled   ()  const noexcept;
+    [[nodiscard]] bool            IsGravityEnabled    ()  const noexcept;
+    [[nodiscard]] bool            IsAutoMassEnabled   ()  const noexcept;
+    [[nodiscard]] bool            IsTriggerInMassData ()  const noexcept;
+    [[nodiscard]] bool            GetFreezePositionX  ()  const noexcept;
+    [[nodiscard]] bool            GetFreezePositionY  ()  const noexcept;
+    [[nodiscard]] bool            GetFreezePositionZ  ()  const noexcept;
+    [[nodiscard]] bool            GetFreezeRotationX  ()  const noexcept;
+    [[nodiscard]] bool            GetFreezeRotationY  ()  const noexcept;
+    [[nodiscard]] bool            GetFreezeRotationZ  ()  const noexcept;
+
+    [[nodiscard]] SHMotionState&  GetMotionState      ()  noexcept;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    void SetCollider          (SHCollider* c)                   noexcept;
+
+    /**
+     * @brief
+     * Changing the type from non-Dynamic to Dynamic will set the default
+     * mass and drag values.
+    */
+    void SetType              (Type newType)                    noexcept;
+
+    void SetGravityScale      (float newGravityScale)           noexcept;
+
+    /**
+     * @brief
+     * Mass is only modifiable for Dynamic bodies.
+     * @param newMass
+     * The new mass to set. Values below 0 will be ignored.
+    */
+    void  SetMass             (float newMass)                   noexcept;
+
+    /**
+     * @brief
+     * Drag is only modifiable for non-Static bodies.
+     * @param newLinearDrag
+     * The new drag to set. Values below 0 will be ignored.
+    */
+    void  SetLinearDrag       (float newLinearDrag)             noexcept;
+
+    /**
+     * @brief
+     * Drag is only modifiable for non-Static bodies.
+     * @param newAngularDrag
+     * The new drag to set. Values below 0 will be ignored.
+    */
+    void  SetAngularDrag      (float newAngularDrag)            noexcept;
+
+    void  SetLinearVelocity   (const SHVec3& newLinearVelocity) noexcept;
+    void  SetAngularVelocity  (const SHVec3& newAngularVelocity)noexcept;
+
+    // Flags
+
+    void  SetIsActive         (bool isActive)                   noexcept;
+    void  SetIsSleeping       (bool isSleeping)                 noexcept;
+    void  SetSleepingEnabled  (bool enableSleeping)             noexcept;
+    void  SetGravityEnabled   (bool enableGravity)              noexcept;
+    void  SetAutoMassEnabled  (bool enableAutoMass)             noexcept;
+    void  SetTriggerInMassData(bool triggerInMassData)          noexcept;
+    void  SetFreezePositionX  (bool freezePositionX)            noexcept;
+    void  SetFreezePositionY  (bool freezePositionY)            noexcept;
+    void  SetFreezePositionZ  (bool freezePositionZ)            noexcept;
+    void  SetFreezeRotationX  (bool freezeRotationX)            noexcept;
+    void  SetFreezeRotationY  (bool freezeRotationY)            noexcept;
+    void  SetFreezeRotationZ  (bool freezeRotationZ)            noexcept;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Member Functions                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Adds a force to the body with an offset from it's center of mass. <br/>
+     * Non-dynamic bodies will be ignored.
+     * @param force
+     * The force to add to the body.
+     * @param pos
+     * The position from the center of mass to offset the force. Defaults to zero.
+    */
+    void AddForce         (const SHVec3& force, const SHVec3& pos = SHVec3::Zero) noexcept;
+
+    /**
+     * @brief
+     * Adds an impulse to the body with an offset from it's center of mass. <br/>
+     * Non-dynamic bodies will be ignored.
+     * @param impulse
+     * The impulse to add to the body.
+     * @param pos
+     * The position from the center of mass to offset the impulse. Defaults to zero.
+    */
+    void AddImpulse       (const SHVec3& impulse, const SHVec3& pos = SHVec3::Zero) noexcept;
+
+    /**
+     * @brief
+     * Adds torque to rotate the body about it's centroid. <br/>
+     * Non-dynamic bodies will be ignored.
+     * @param torque
+     * The torque to add to the body.
+    */
+    void AddTorque        (const SHVec3& torque) noexcept;
+
+    /**
+     * @brief
+     * Removes all the forces from the body.
+    */
+    void ClearForces      () noexcept;
+
+    /**
+     * @brief
+     * Computes the centroid and invInertia in world space.
+    */
+    void ComputeWorldData () noexcept;
+
+    /**
+     * @brief
+     * Computes the centroid and inertia of the object. <br/>
+     * If auto-mass is enabled, computes the mass. <br/>
+     * If auto-mass is disabled, the inertia is computed based on the ratio each shape's volume over the total volume.
+     * 
+    */
+    void ComputeMassData  () noexcept;
+
+  private:
+    /*-----------------------------------------------------------------------------------*/
+    /* Data Members                                                                      */
+    /*-----------------------------------------------------------------------------------*/
+
+    // The entityID here is only meant for linking with the actual component in the engine.
+    EntityID    entityID;
+    SHCollider* collider;
+
+    Type        bodyType;
+
+    float       gravityScale;
+
+    float       invMass;
+    float       linearDrag;
+    float       angularDrag;
+
+    SHVec3      localInvInertia;    // Only store the diagonals 
+    SHVec3      worldInvInertia;    // Only store the diagonals
+
+    SHVec3      localCentroid;
+    SHVec3      worldCentroid;
+      
+    SHVec3      accumulatedForce;
+    SHVec3      accumulatedTorque;
+
+    SHVec3      linearVelocity;
+    SHVec3      angularVelocity;
+
+    // aZ aY aX pZ pY pX 0 0 0 addTriggersToMassData inIsland autoMass enableGravity enableSleeping sleeping active
+    uint16_t      flags;
+
+    SHMotionState motionState;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Member Functions                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    void constrainLinearVelocities  ()  noexcept;
+    void constrainAngularVelocities ()  noexcept;
+    
+  };
+
+} // namespace SHADE

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

@@ -0,0 +1,163 @@
+/****************************************************************************************
+ * \file      SHPhysicsObject.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for a Physics Object.
+ *
+ * \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 "SHPhysicsObject.h"
+
+#include "Physics/Collision/SHCollider.h"
+#include "Physics/Collision/SHCompositeCollider.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructor & Destructor Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHPhysicsObject::SHPhysicsObject(EntityID eid) noexcept
+  : entityID  { eid }
+  {}
+
+  SHPhysicsObject::SHPhysicsObject(const SHPhysicsObject& rhs) noexcept
+  : entityID  { rhs.entityID }
+  {
+    deepCopyComponents(rhs.rigidBody, rhs.collider);
+  }
+
+  SHPhysicsObject::SHPhysicsObject(SHPhysicsObject&& rhs) noexcept
+  : entityID  { rhs.entityID }
+  {
+    deepCopyComponents(rhs.rigidBody, rhs.collider);
+  }
+
+  SHPhysicsObject::~SHPhysicsObject() noexcept
+  {
+    entityID = MAX_EID;
+
+    delete rigidBody;
+    rigidBody = nullptr;
+
+    delete collider;
+    collider = nullptr;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHPhysicsObject& SHPhysicsObject::operator=(const SHPhysicsObject& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    entityID = rhs.entityID;
+
+    deepCopyComponents(rhs.rigidBody, rhs.collider);
+
+    return *this;
+  }
+
+  SHPhysicsObject& SHPhysicsObject::operator=(SHPhysicsObject&& rhs) noexcept
+  {
+    entityID = rhs.entityID;
+
+    deepCopyComponents(rhs.rigidBody, rhs.collider);
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Function Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHPhysicsObject::IsEmpty() const noexcept
+  {
+    return rigidBody == nullptr && collider == nullptr;
+  }
+
+  SHRigidBody* SHPhysicsObject::CreateRigidBody(SHRigidBody::Type bodyType)
+  {
+    if (rigidBody)
+    {
+      SHLOG_INFO_D("Rigid body for Entity {} has already been created!", entityID)
+      return rigidBody;
+    }
+
+    rigidBody = new SHRigidBody{ entityID, bodyType };
+
+    // Link with collider if it exists
+    if (collider)
+    {
+      collider->SetRigidBody(rigidBody);
+      rigidBody->SetCollider(collider);
+    }
+
+    rigidBody->ComputeMassData();
+    return rigidBody;
+  }
+
+  void SHPhysicsObject::DestroyRigidBody() noexcept
+  {
+    delete rigidBody;
+    rigidBody = nullptr;
+
+    // Unlink with collider
+    if (collider)
+      collider->SetRigidBody(nullptr);
+  }
+
+  SHCollider* SHPhysicsObject::CreateCompositeCollider(const SHTransform& transform)
+  {
+    if (collider)
+    {
+      SHLOG_INFO_D("Collider for Entity {} has already been created!", entityID)
+      return collider;
+    }
+
+    collider = new SHCompositeCollider{ entityID, transform };
+
+    // Link with rigidBody if it exists
+    if (rigidBody)
+    {
+      rigidBody->SetCollider(collider);
+      collider->SetRigidBody(rigidBody);
+    }
+
+    return collider;
+  }
+
+  void SHPhysicsObject::DestroyCollider() noexcept
+  {
+    delete collider;
+    collider = nullptr;
+
+    // Unlink with rigid body
+    if (rigidBody)
+    {
+      rigidBody->SetCollider(nullptr);
+      rigidBody->ComputeMassData();
+    }
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Function Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHPhysicsObject::deepCopyComponents(const SHRigidBody* rhsRigidBody, const SHCollider* rhsCollider)
+  {
+    if (rhsRigidBody)
+      rigidBody = new SHRigidBody{ *rhsRigidBody };
+
+    if (rhsCollider)
+      collider = new SHCollider { *rhsCollider };
+  }
+
+} // namespace SHADE