Merge branch 'SP3-2-Physics' into PlayerControllerWIthNewPhysics

Assets/Scenes/SS_Playground.shade

@@ -0,0 +1,84 @@
+- EID: 0
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Camera Component:
+      Position: {x: 1, y: 5, z: 5}
+      Pitch: 0
+      Yaw: 0
+      Roll: 0
+      Width: 1920
+      Near: 0.00999999978
+      Far: 10000
+      Perspective: true
+      FOV: 90
+      IsActive: true
+  Scripts: ~
+- EID: 1
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 0, z: 0}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 5, y: 0.999998808, z: 5}
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 6
+          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}
+      IsActive: true
+  Scripts: ~
+- EID: 3
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 5, z: 0}
+      Rotate: {x: -0, y: 0.785398006, z: -0}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      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: false
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      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}
+      IsActive: true
+  Scripts:
+    - Type: PhysicsTestObj
+      Enabled: true
+      forceAmount: 50
+      torqueAmount: 25

Assets/Scenes/SS_Playground.shade.shmeta

@@ -0,0 +1,3 @@
+Name: SS_Playground
+ID: 92914350
+Type: 5

Assets/Scripts/Gameplay/Player/SC_PickAndThrow.cs

@@ -1,4 +1,4 @@
-using SHADE;
+using SHADE;
 using SHADE_Scripting;
 using SHADE_Scripting.Audio;
 using System;
@@ -28,6 +28,11 @@ public class PickAndThrow : Script
   public bool throwItem = false;
   private Vector3 prevTargetOffSet;
 
+  public GameObject silhouettePlayer;
+  public Renderable silhouettePlayerRend;
+  public GameObject silhouetteBag;
+  public Renderable silhouetteBagRend;
+
   [Tooltip("Lenght of ray")]
   public float rayDistance = 1;
 

Assets/Scripts/Gameplay/Player/SC_PlayerController.cs

@@ -1,4 +1,4 @@
-using SHADE;
+using SHADE;
 using System;
 using System.Collections.Generic;  
 using static Item;

Assets/Scripts/Gameplay/Player/SC_ThirdPersonCamera.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Collections.Generic;
 using System.Linq;
 using System.Text;

Assets/Scripts/Gameplay/SC_GameManager.cs

@@ -1,4 +1,4 @@
-using SHADE;
+using SHADE;
 using SHADE_Scripting.Audio;
 using SHADE_Scripting.UI;
 using System;

Assets/Scripts/Gameplay/SC_PreviewLevel.cs

@@ -1,4 +1,4 @@
-using SHADE;
+using SHADE;
 using System;
 using System.Collections.Generic;
 using System.Linq;

Assets/Scripts/Tests/PhysicsTestObj.cs

@@ -65,8 +65,37 @@ public class PhysicsTestObj : Script
     };
 
     public float forceAmount = 50.0f;
-    public float torqueAmount = 500.0f;
+    public float torqueAmount = 25.0f;
 
+    protected override void onTriggerEnter(CollisionInfo info)
+    {
+        Debug.Log("Trigger Enter");
+    }
+
+    protected override void onTriggerStay(CollisionInfo info)
+    {
+        Debug.Log("Trigger Stay");
+    }
+
+    protected override void onTriggerExit(CollisionInfo info)
+    {
+        Debug.Log("Trigger Exit");
+    }
+
+    protected override void onCollisionEnter(CollisionInfo info)
+    {
+        Debug.Log("Collision Enter");
+    }
+
+    protected override void onCollisionStay(CollisionInfo info)
+    {
+        Debug.Log("Collision Stay");
+    }
+
+    protected override void onCollisionExit(CollisionInfo info)
+    {
+        Debug.Log("Collision Exit");
+    }
     protected override void awake()
     {
         tf = GetComponent<Transform>();

Assets/Scripts/UI/EasingHelper.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Collections.Generic;
 using System.Linq;
 using System.Text;

Assets/Scripts/UI/SC_Cutscene.cs

@@ -0,0 +1,582 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using SHADE;
+
+
+public class Cutscene : Script
+{
+
+  public float duration = 3.0f;
+  private float oldDuration = 0.0f;
+
+  private Renderable pic1aRenderable;
+  private Renderable pic1bRenderable;
+  private Renderable pic1cRenderable;
+  private Renderable pic2aRenderable;
+  private Renderable pic2bRenderable;
+  private Renderable pic2cRenderable;
+  private Renderable pic3aRenderable;
+  private Renderable pic3bRenderable;
+  private Renderable pic3cRenderable;
+  private Renderable pic3dRenderable;
+  private Renderable pic3eRenderable;
+
+  private Transform pic1aTran;
+  private Transform pic1bTran;
+  private Transform pic1cTran;
+  private Transform pic2aTran;
+  private Transform pic2bTran;
+  private Transform pic2cTran;
+  private Transform pic3aTran;
+  private Transform pic3bTran;
+  private Transform pic3cTran;
+  private Transform pic3dTran;
+  private Transform pic3eTran;
+
+  private float alphaIn = 0.0f;
+  private float time = 0.0f;
+  private bool showPic1a = true;
+  private bool showPic1b = false;
+  private bool showPic1c = false;
+  private bool showPic2a = true;
+  private bool showPic2b = false;
+  private bool showPic2c = false;
+  private bool showPic3a = true;
+  private bool showPic3b = false;
+  private bool showPic3c = false;
+  private bool showPic3e = false;
+  private bool showPic3d = false;
+
+  private bool skip = false;
+
+  public GameObject cutscene1Points;
+  private List<Transform> listOfCutscene1Points;
+
+  public GameObject cutscene1Pics;
+  private List<Renderable> listOfCutscene1Pics;
+
+  public GameObject cutscene2Points;
+  private List<Transform> listOfCutscene2Points;
+
+  public GameObject cutscene2Pics;
+  private List<Renderable> listOfCutscene2Pics;
+
+  public GameObject cutscene3Points;
+  private List<Transform> listOfCutscene3Points;
+
+  public GameObject cutscene3Pics;
+  private List<Renderable> listOfCutscene3Pics;
+
+  private TextRenderable text1;
+  private TextRenderable text2;
+  private TextRenderable text3;
+
+  public GameObject canvas1;
+  public GameObject canvas2;
+  public GameObject canvas3;
+
+  private bool cutscene1Done = false;
+  private bool cutscene2Done = false;
+  private bool cutscene3Done = false;
+
+  protected override void awake()
+  {
+    initCutscene1();
+    initCutscene2();
+    initCutscene3();
+  }
+
+  protected override void update()
+  {
+    Canvas1();
+    Canvas2();
+    Canvas3();
+
+    if (Input.GetKeyDown(Input.KeyCode.Space) && !skip && (!cutscene1Done || !cutscene2Done || !cutscene3Done))
+    {
+      skip = true;
+      oldDuration = duration;
+      duration = 0.1f;
+    }
+
+    if (Input.GetKeyDown(Input.KeyCode.Space) && cutscene1Done && canvas1.IsActiveSelf)
+    {
+      canvas1.SetActive(false);
+      canvas2.SetActive(true);
+      duration = oldDuration;
+      skip = false;
+    }
+
+    if (Input.GetKeyDown(Input.KeyCode.Space) && cutscene2Done && canvas2.IsActiveSelf)
+    {
+      canvas2.SetActive(false);
+      canvas3.SetActive(true);
+      duration = oldDuration;
+      skip = false;
+    }
+
+    if (Input.GetKeyDown(Input.KeyCode.Space) && cutscene3Done && canvas3.IsActiveSelf)
+    {
+      //change scene
+    }
+  }
+
+  private void Canvas1()
+  {
+    if (canvas1.IsActiveSelf)
+    {
+      if (showPic1a)
+      {
+        if (time < duration)
+        {
+          pic1aTran.LocalPosition = Vector3.Lerp(pic1aTran.LocalPosition, listOfCutscene1Points[0].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic1aTran.LocalPosition = listOfCutscene1Points[0].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic1aRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic1a = false;
+          showPic1b = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+
+      if (showPic1b)
+      {
+        if (time < duration)
+        {
+          pic1bTran.LocalPosition = Vector3.Lerp(pic1bTran.LocalPosition, listOfCutscene1Points[1].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic1bTran.LocalPosition = listOfCutscene1Points[1].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic1bRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic1b = false;
+          showPic1c = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+
+      if (showPic1c)
+      {
+        if (time < duration)
+        {
+          pic1cTran.LocalPosition = Vector3.Lerp(pic1cTran.LocalPosition, listOfCutscene1Points[2].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic1cTran.LocalPosition = listOfCutscene1Points[2].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic1cRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic1c = false;
+          cutscene1Done = true;
+          text1.Enabled = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+    }
+  }
+
+  private void Canvas2()
+  {
+    if (canvas2.IsActiveSelf)
+    {
+      if (showPic2a)
+      {
+        if (time < duration)
+        {
+          pic2aTran.LocalPosition = Vector3.Lerp(pic2aTran.LocalPosition, listOfCutscene2Points[0].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic2aTran.LocalPosition = listOfCutscene2Points[0].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic2aRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic2a = false;
+          showPic2b = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+
+      if (showPic2b)
+      {
+        if (time < duration)
+        {
+          pic2bTran.LocalPosition = Vector3.Lerp(pic2bTran.LocalPosition, listOfCutscene2Points[1].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic2bTran.LocalPosition = listOfCutscene2Points[1].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic2bRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic2b = false;
+          showPic2c = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+
+      if (showPic2c)
+      {
+        if (time < duration)
+        {
+          pic2cTran.LocalPosition = Vector3.Lerp(pic2cTran.LocalPosition, listOfCutscene2Points[2].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic2cTran.LocalPosition = listOfCutscene2Points[2].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic2cRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic2c = false;
+          cutscene2Done = true;
+          text2.Enabled = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+    }
+  }
+
+  private void Canvas3()
+  {
+    if (canvas3.IsActiveSelf)
+    {
+      if (showPic3a)
+      {
+        if (time < duration)
+        {
+          pic3aTran.LocalPosition = Vector3.Lerp(pic3aTran.LocalPosition, listOfCutscene3Points[0].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic3aTran.LocalPosition = listOfCutscene3Points[0].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic3aRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic3a = false;
+          showPic3b = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+
+      if (showPic3b)
+      {
+        if (time < duration)
+        {
+          pic3bTran.LocalPosition = Vector3.Lerp(pic3bTran.LocalPosition, listOfCutscene3Points[1].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic3bTran.LocalPosition = listOfCutscene3Points[1].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic3bRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic3b = false;
+          showPic3c = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+
+      if (showPic3c)
+      {
+        if (time < duration)
+        {
+          pic3cTran.LocalPosition = Vector3.Lerp(pic3cTran.LocalPosition, listOfCutscene3Points[2].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic3cTran.LocalPosition = listOfCutscene3Points[2].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic3cRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic3c = false;
+          showPic3d = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+
+      if (showPic3d)
+      {
+        if (time < duration)
+        {
+          pic3dTran.LocalPosition = Vector3.Lerp(pic3dTran.LocalPosition, listOfCutscene3Points[3].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic3dTran.LocalPosition = listOfCutscene3Points[3].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic3dRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic3d = false;
+          showPic3e = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+
+      if (showPic3e)
+      {
+        if (time < duration)
+        {
+          pic3eTran.LocalPosition = Vector3.Lerp(pic3eTran.LocalPosition, listOfCutscene3Points[4].LocalPosition, time / duration);
+          alphaIn = SHADE.Math.Lerp(0.0f, 1.0f, time / duration);
+          time += Time.DeltaTimeF;
+        }
+        else
+        {
+          pic3eTran.LocalPosition = listOfCutscene3Points[4].LocalPosition;
+          alphaIn = 1.0f;
+        }
+
+        pic3eRenderable.Material.SetProperty<float>("data.alpha", alphaIn);
+        if (alphaIn >= 1.0f)
+        {
+          showPic3e = false;
+          cutscene3Done = true;
+          text3.Enabled = true;
+          time = 0;
+          alphaIn = 0;
+        }
+      }
+    }
+  }
+
+  private void initCutscene1()
+  {
+    if(cutscene1Points)
+      listOfCutscene1Points = cutscene1Points.GetComponentsInChildren<Transform>().ToList();
+    else
+      Debug.LogError("Cutscene1Points Missing");
+
+    if (listOfCutscene1Points.Count == 0)
+      Debug.LogError("Cutscene1Points Empty");
+
+    listOfCutscene1Pics = cutscene1Pics.GetComponentsInChildren<Renderable>().ToList();
+    if (listOfCutscene1Pics.Count == 0)
+      Debug.LogError("Cutscene1Pics Empty");
+
+    if (listOfCutscene1Pics[0])
+    {
+      pic1aRenderable = listOfCutscene1Pics[0].GetComponent<Renderable>();
+      pic1aTran = listOfCutscene1Pics[0].GetComponent<Transform>();
+      pic1aRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 1 PIC1 MISSING");
+
+    if (listOfCutscene1Pics[1])
+    {
+      pic1bRenderable = listOfCutscene1Pics[1].GetComponent<Renderable>();
+      pic1bTran = listOfCutscene1Pics[1].GetComponent<Transform>();
+      pic1bRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 1 PIC2 MISSING");
+
+    if (listOfCutscene1Pics[2])
+    {
+      pic1cRenderable = listOfCutscene1Pics[2].GetComponent<Renderable>();
+      pic1cTran = listOfCutscene1Pics[2].GetComponent<Transform>();
+      pic1cRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 1 PIC3 MISSING");
+
+    if (canvas1)
+    {
+      text1 = canvas1.GetComponentInChildren<TextRenderable>();
+      text1.Enabled = false;
+    }
+    else
+      Debug.LogError("Canvas 1 missing");
+  }
+
+  private void initCutscene2()
+  {
+    if(cutscene2Points)
+      listOfCutscene2Points = cutscene2Points.GetComponentsInChildren<Transform>().ToList();
+    else
+      Debug.LogError("cutscene2Points Missing");
+
+    if (listOfCutscene2Points.Count == 0)
+      Debug.LogError("Cutscene2Points Empty");
+
+    listOfCutscene2Pics = cutscene2Pics.GetComponentsInChildren<Renderable>().ToList();
+    if (listOfCutscene2Pics.Count == 0)
+      Debug.LogError("Cutscene2Pics Empty");
+
+    if (listOfCutscene2Pics[0])
+    {
+      pic2aRenderable = listOfCutscene2Pics[0].GetComponent<Renderable>();
+      pic2aTran = listOfCutscene2Pics[0].GetComponent<Transform>();
+      pic2aRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 2 PIC1 MISSING");
+
+    if (listOfCutscene2Pics[1])
+    {
+      pic2bRenderable = listOfCutscene2Pics[1].GetComponent<Renderable>();
+      pic2bTran = listOfCutscene2Pics[1].GetComponent<Transform>();
+      pic2bRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 2 PIC2 MISSING");
+
+    if (listOfCutscene2Pics[2])
+    {
+      pic2cRenderable = listOfCutscene2Pics[2].GetComponent<Renderable>();
+      pic2cTran = listOfCutscene2Pics[2].GetComponent<Transform>();
+      pic2cRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 2 PIC3 MISSING");
+
+    if (canvas2)
+    {
+      text2 = canvas2.GetComponentInChildren<TextRenderable>();
+      text2.Enabled = false;
+      canvas2.SetActive(false);
+    }
+    else
+      Debug.LogError("Canvas 2 missing");
+
+  }
+
+  private void initCutscene3()
+  {
+    if(cutscene3Points)
+      listOfCutscene3Points = cutscene3Points.GetComponentsInChildren<Transform>().ToList();
+    else
+      Debug.LogError("cutscene3Points Missing");
+
+    if (listOfCutscene3Points.Count == 0)
+      Debug.LogError("Cutscene3Points Empty");
+
+    listOfCutscene3Pics = cutscene3Pics.GetComponentsInChildren<Renderable>().ToList();
+    if (listOfCutscene3Pics.Count == 0)
+      Debug.LogError("Cutscene3Pics Empty");
+
+    if (listOfCutscene3Pics[0])
+    {
+      pic3aRenderable = listOfCutscene3Pics[0].GetComponent<Renderable>();
+      pic3aTran = listOfCutscene3Pics[0].GetComponent<Transform>();
+      pic3aRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 3 PIC1 MISSING");
+
+    if (listOfCutscene3Pics[1])
+    {
+      pic3bRenderable = listOfCutscene3Pics[1].GetComponent<Renderable>();
+      pic3bTran = listOfCutscene3Pics[1].GetComponent<Transform>();
+      pic3bRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 3 PIC2 MISSING");
+
+    if (listOfCutscene3Pics[2])
+    {
+      pic3cRenderable = listOfCutscene3Pics[2].GetComponent<Renderable>();
+      pic3cTran = listOfCutscene3Pics[2].GetComponent<Transform>();
+      pic3cRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 3 PIC3 MISSING");
+
+    if (listOfCutscene3Pics[3])
+    {
+      pic3dRenderable = listOfCutscene3Pics[3].GetComponent<Renderable>();
+      pic3dTran = listOfCutscene3Pics[3].GetComponent<Transform>();
+      pic3dRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 3 PIC4 MISSING");
+
+    if (listOfCutscene3Pics[4])
+    {
+      pic3eRenderable = listOfCutscene3Pics[4].GetComponent<Renderable>();
+      pic3eTran = listOfCutscene3Pics[4].GetComponent<Transform>();
+      pic3eRenderable.Material.SetProperty<float>("data.alpha", 0.0f);
+    }
+    else
+      Debug.LogError("SCENE 2 PIC5 MISSING");
+
+    if (canvas3)
+    {
+      text3 = canvas3.GetComponentInChildren<TextRenderable>();
+      text3.Enabled = false;
+      canvas3.SetActive(false);
+    }
+    else
+      Debug.LogError("Canvas 3 missing");
+
+  }
+}
+

Assets/Scripts/UI/SC_Cutscene.cs.shmeta

@@ -0,0 +1,3 @@
+Name: SC_Cutscene
+ID: 157565851
+Type: 9

Assets/Scripts/UI/SC_PauseMenu.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using SHADE;
 using SHADE_Scripting.Audio;
 

Assets/Scripts/UI/SC_SceneFadeInOut.cs

@@ -1,4 +1,4 @@
-using SHADE;
+using SHADE;
 using System;
 using System.Collections.Generic;
 using System.Linq;

Assets/Scripts/UI/SC_StealFoodPopUp.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using SHADE;
 
 namespace SHADE_Scripting.UI

SHADE_Application/src/Application/SBApplication.cpp

@@ -40,8 +40,6 @@
 #include "Graphics/MiddleEnd/Interface/SHRenderable.h"
 #include "Math/Transform/SHTransformComponent.h"
 
-#include "Scenes/SBTestScene.h"
-
 #include "Assets/SHAssetManager.h"
 #include "Scenes/SBMainScene.h"
 #include "Serialization/Configurations/SHConfigurationManager.h"

SHADE_Application/src/Scenes/SBTestScene.cpp

@@ -1,249 +0,0 @@
- #include "SBpch.h"
-#include "SBTestScene.h"
-
-#include "ECS_Base/Managers/SHSystemManager.h"
-#include "Graphics/MiddleEnd/Meshes/SHPrimitiveGenerator.h"
-#include "ECS_Base/Managers/SHEntityManager.h"
-#include "Graphics/MiddleEnd/Interface/SHRenderable.h"
-#include "Scene/SHSceneManager.h"
-#include "Graphics/MiddleEnd/Interface/SHGraphicsSystem.h"
-#include "Scripting/SHScriptEngine.h"
-#include "Math/Transform/SHTransformComponent.h"
-#include "Graphics/MiddleEnd/Interface/SHMaterialInstance.h"
-#include "Physics/Interface/SHRigidBodyComponent.h"
-#include "Physics/Interface/SHColliderComponent.h"
-#include "Graphics/MiddleEnd/Lights/SHLightComponent.h"
-
-#include "Assets/SHAssetManager.h"
-#include "Camera/SHCameraComponent.h"
-#include "Math/SHColour.h"
-#include "Resource/SHResourceManager.h"
-#include "Graphics/MiddleEnd/Interface/SHDebugDrawSystem.h"
-#include "Tools/SHDebugDraw.h"
-
-using namespace SHADE;
-
-namespace Sandbox
-{
-
-  void SBTestScene::WindowFocusFunc([[maybe_unused]] void* window, int focused)
-  {
-    if (focused)
-    {
-    }
-    else
-    {
-    }
-  }
-
-  void SBTestScene::Load()
-  {
-  }
-  void SBTestScene::Init()
-  {
-    SHADE::SHGraphicsSystem* graphicsSystem = static_cast<SHADE::SHGraphicsSystem*>(SHADE::SHSystemManager::GetSystem<SHADE::SHGraphicsSystem>());
-    // Create temp meshes
-    const auto CUBE_MESH = SHADE::SHPrimitiveGenerator::Cube(*graphicsSystem);
-
-    //Test Racoon mesh
-    std::vector<Handle<SHMesh>> handles;
-    std::vector<Handle<SHTexture>> texHandles;
-    for (const auto& asset : SHAssetManager::GetAllAssets())
-    {
-      switch (asset.type)
-      {
-        case AssetType::MESH:
-          if (asset.name == "Raccoon")
-          handles.emplace_back(SHResourceManager::LoadOrGet<SHMesh>(asset.id));
-          break;
-        case AssetType::TEXTURE:
-          if (asset.name == "RaccoonPreTexturedVer1_Base9")
-          texHandles.emplace_back(SHResourceManager::LoadOrGet<SHTexture>(asset.id));
-          break;
-      }
-    }
-    SHResourceManager::FinaliseChanges();
-
-    // Create Materials
-    auto baseRaccoonMat = graphicsSystem->AddOrGetBaseMaterialInstance();
-    auto baseRaccoonMatInstant = graphicsSystem->AddMaterialInstanceCopy(baseRaccoonMat);
-    baseRaccoonMatInstant->SetProperty("data.color", SHVec4(1.0f, 1.0f, 1.0f, 1.0f));
-    baseRaccoonMatInstant->SetProperty("data.textureIndex", 0);
-    baseRaccoonMatInstant->SetProperty("data.alpha", 0.1f);
-
-    auto baseFloorMatInstant = graphicsSystem->AddMaterialInstanceCopy(baseRaccoonMat);
-    baseFloorMatInstant->SetProperty("data.color", SHVec4(1.0f, 1.0f, 1.0f, 1.0f));
-    baseFloorMatInstant->SetProperty("data.textureIndex", 0);
-    baseFloorMatInstant->SetProperty("data.alpha", 0.1f);
-
-    auto dummy = SHEntityManager::CreateEntity<>();
-
-    auto floor = SHEntityManager::CreateEntity<SHTransformComponent,SHRenderable, SHRigidBodyComponent, SHColliderComponent>();
-    auto& floorRenderable = *SHComponentManager::GetComponent_s<SHRenderable>(floor);
-    auto& floorTransform = *SHComponentManager::GetComponent_s<SHTransformComponent>(floor);
-    auto& floorRigidBody = *SHComponentManager::GetComponent_s<SHRigidBodyComponent>(floor);
-    auto& floorCollider = *SHComponentManager::GetComponent_s<SHColliderComponent>(floor);
-
-    floorRenderable.SetMesh(CUBE_MESH);
-    floorRenderable.SetMaterial(graphicsSystem->GetDefaultMaterialInstance());
-
-    floorTransform.SetWorldScale({ 17.5f, 0.5f, 17.5f });
-    floorTransform.SetWorldPosition({ 0.0f, -3.0f, -5.0f });
-
-    floorRigidBody.SetType(SHRigidBodyComponent::Type::STATIC);
-
-    //floorCollider.AddBoundingBox();
-
-    // Create blank entity with a script
-    //testObj = SHADE::SHEntityManager::CreateEntity<SHRenderable, SHTransformComponent>();
-    //auto& testObjRenderable = *SHComponentManager::GetComponent<SHRenderable>(testObj);
-    //testObjRenderable.Mesh = CUBE_MESH;
-    //testObjRenderable.SetMaterial(matInst);
-
-    //raccoon =======================================================================================================================
-    auto racoon = SHEntityManager::CreateEntity<SHRenderable, SHTransformComponent, SHRigidBodyComponent, SHColliderComponent>(MAX_EID, "Player");
-    auto& racoonRenderable = *SHComponentManager::GetComponent_s<SHRenderable>(racoon);
-    auto& racoonTransform = *SHComponentManager::GetComponent_s<SHTransformComponent>(racoon);
-    auto& racoonRigidBody = *SHComponentManager::GetComponent_s<SHRigidBodyComponent>(racoon);
-    auto& racoonCollider = *SHComponentManager::GetComponent_s<SHColliderComponent>(racoon);
-
-    racoonRenderable.SetMesh(handles.front());
-    racoonRenderable.SetMaterial(baseRaccoonMatInstant);
-
-    racoonTransform.SetWorldScale({ 2.0f, 2.0f, 2.0f });
-    racoonTransform.SetWorldPosition({ -3.0f, -2.0f, -5.0f });
-
-    //racoonCollider.AddBoundingBox();
-    racoonCollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f,0.5f,0.0f));
-    //racoonCollider.GetCollisionShape(0).SetBoundingBox(SHVec3(0.5f, 0.5f, 0.5f));
-
-    auto racoonItemLocation = SHEntityManager::CreateEntity<SHTransformComponent>();
-    auto& racoonItemLocationTransform = *SHComponentManager::GetComponent_s<SHTransformComponent>(racoonItemLocation);
-    SHSceneManager::GetCurrentSceneGraph().SetParent(racoonItemLocation, racoon);
-
-    auto racoonCamera = SHEntityManager::CreateEntity<SHTransformComponent>();
-    SHSceneManager::GetCurrentSceneGraph().SetParent(racoonCamera, racoon);
-    //================================================================================================================================
-
-    //item ===========================================================================================================================
-    auto item = SHEntityManager::CreateEntity<SHRenderable, SHTransformComponent, SHRigidBodyComponent, SHColliderComponent>(MAX_EID, "item");
-    auto& itemRenderable = *SHComponentManager::GetComponent_s<SHRenderable>(item);
-    auto& itemTransform = *SHComponentManager::GetComponent_s<SHTransformComponent>(item);
-    auto& itemRigidBody = *SHComponentManager::GetComponent_s<SHRigidBodyComponent>(item);
-    auto& itemCollider = *SHComponentManager::GetComponent_s<SHColliderComponent>(item);
-
-    itemRenderable.SetMesh(handles.front());
-    itemRenderable.SetMaterial(baseRaccoonMatInstant);
-
-    itemTransform.SetWorldScale({ 2.0f, 2.0f, 2.0f });
-    itemTransform.SetWorldPosition({ 0.0f, -2.0f, -5.0f });
-
-    //itemCollider.AddBoundingBox();
-    //itemCollider.AddBoundingBox(SHVec3(2.0f,2.0f,2.0f));
-    itemCollider.GetCollisionShape(1).SetIsTrigger(true);
-
-    itemCollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f, 0.5f, 0.0f));
-    //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).SetBoundingBox(SHVec3(1.0f, 1.0f, 1.0f));
-
-    itemRigidBody.SetInterpolate(false);
-    itemRigidBody.SetFreezeRotationX(true);
-    itemRigidBody.SetFreezeRotationY(true);
-    itemRigidBody.SetFreezeRotationZ(true);
-    //================================================================================================================================
-
-    //AI =============================================================================================================================
-    auto AI = SHEntityManager::CreateEntity<SHRenderable, SHTransformComponent, SHRigidBodyComponent, SHColliderComponent>(MAX_EID, "AI");
-    auto& AIRenderable = *SHComponentManager::GetComponent_s<SHRenderable>(AI);
-    auto& AITransform = *SHComponentManager::GetComponent_s<SHTransformComponent>(AI);
-    auto& AIRigidBody = *SHComponentManager::GetComponent_s<SHRigidBodyComponent>(AI);
-    auto& AICollider = *SHComponentManager::GetComponent_s<SHColliderComponent>(AI);
-
-    AIRenderable.SetMesh(handles.front());
-    AIRenderable.SetMaterial(baseRaccoonMatInstant);
-
-    AITransform.SetWorldScale({ 2.0f, 2.0f, 2.0f });
-    AITransform.SetWorldPosition({ -8.0f, -2.0f, 2.5f });
-
-    //AICollider.AddBoundingBox();
-    AICollider.GetCollisionShape(0).SetPositionOffset(SHVec3(0.0f, 0.5f, 0.0f));
-    //AICollider.GetCollisionShape(0).SetBoundingBox(SHVec3(0.5f, 0.5f, 0.5f));
-
-    AIRigidBody.SetInterpolate(false);
-    AIRigidBody.SetFreezeRotationX(true);
-    AIRigidBody.SetFreezeRotationY(true);
-    AIRigidBody.SetFreezeRotationZ(true);
-    //================================================================================================================================
-
-    SHADE::SHScriptEngine* scriptEngine = static_cast<SHADE::SHScriptEngine*>(SHADE::SHSystemManager::GetSystem<SHADE::SHScriptEngine>());
-    scriptEngine->AddScript(racoon, "PlayerController");
-    scriptEngine->AddScript(racoon, "PickAndThrow");
-    scriptEngine->AddScript(racoonCamera, "ThirdPersonCamera");
-    scriptEngine->AddScript(AI, "AIPrototype");
-    scriptEngine->AddScript(item, "Item");
-
-    auto raccoonShowcase = SHEntityManager::CreateEntity<SHRenderable, SHTransformComponent>();
-    auto& renderableShowcase = *SHComponentManager::GetComponent_s<SHRenderable>(raccoonShowcase);
-    auto& transformShowcase = *SHComponentManager::GetComponent_s<SHTransformComponent>(raccoonShowcase);
-
-    renderableShowcase.SetMesh(handles.front());
-    renderableShowcase.SetMaterial(baseRaccoonMatInstant);
-    renderableShowcase.GetModifiableMaterial()->SetProperty("data.color", SHVec4(1.0f, 1.0f, 1.0f, 1.0f));
-    renderableShowcase.GetModifiableMaterial()->SetProperty("data.alpha", 1.0f);
-    renderableShowcase.GetModifiableMaterial()->SetProperty("data.textureIndex", 0);
-
-    transformShowcase.SetWorldPosition({ 3.0f, -1.0f, -1.0f });
-    transformShowcase.SetLocalScale({ 5.0f, 5.0f, 5.0f });
-    scriptEngine->AddScript(raccoonShowcase, "RaccoonShowcase");
-
-    SHComponentManager::AddComponent<SHCameraComponent>(0);
-    SHComponentManager::AddComponent<SHLightComponent>(0);
-    SHComponentManager::RemoveComponent <SHRigidBodyComponent>(0);
-    SHComponentManager::RemoveComponent <SHColliderComponent>(0);
-
-    auto ambientLight = SHEntityManager::CreateEntity<SHLightComponent>();
-    SHComponentManager::GetComponent<SHLightComponent>(ambientLight)->SetColor(SHColour::WHITE);
-    SHComponentManager::GetComponent<SHLightComponent>(ambientLight)->SetStrength(0.25f);
-    SHComponentManager::GetComponent<SHLightComponent>(ambientLight)->SetType(SH_LIGHT_TYPE::AMBIENT);
-  }
-
-  void SBTestScene::Update(float dt)
-  {
-    static float rotation = 0.0f;
-    SHVec3 direction{0.0f, 0.0f, 1.0f};
-    direction = SHVec3::RotateY(direction, rotation);
-
-    auto* lightComp =SHComponentManager::GetComponent<SHLightComponent>(0);
-    lightComp->SetDirection (direction);
-    rotation += 0.005f;
-    //auto& transform = *SHADE::SHComponentManager::GetComponent_s<SHADE::SHTransformComponent>(testObj);
-
-    //transform.SetWorldPosition({1.0f, 1.0f, -1.0f});
-    //transform.SetWorldRotation(0.0f, 0.0f + rotation, 0.0f);
-    //rotation += dt * 0.2f;
-
-    // Destroy entity if space is pressed
-    if (GetKeyState(VK_SPACE) & 0x8000)
-    {
-      rotation = 0.0f;
-      SHADE::SHScriptEngine* scriptEngine = static_cast<SHADE::SHScriptEngine*>(SHADE::SHSystemManager::GetSystem<SHADE::SHScriptEngine>());
-      scriptEngine->RemoveAllScripts(testObj);
-    }
-  }
-
-  void SBTestScene::Render()
-  {
-
-  }
-
-  void SBTestScene::Unload()
-  {
-  }
-
-  void SBTestScene::Free()
-  {
-    //SHSerialization::SerializeScene("resources/scenes/Scene01.SHADE");
-  }
-}

SHADE_Application/src/Scenes/SBTestScene.h

@@ -1,30 +0,0 @@
-#pragma once
-
-#include "Scene/SHScene.h"
-#include "Scene/SHSceneManager.h"
-
-namespace Sandbox
-{
-  class SBTestScene : public SHADE::SHScene
-  {
-  private:
-    EntityID camera;
-    EntityID testObj;
-    std::vector<EntityID> stressTestObjects;
-
-  public:
-    virtual void Load();
-    virtual void Init();
-    virtual void Update(float dt);
-    virtual void Render();
-    virtual void Free();
-    virtual void Unload();
-
-    //TODO: Change to new window DO IT IN CPP TOO
-    void WindowFocusFunc(void* window, int focused);
-
-    SBTestScene(void) = default;
-  };
-
-}
-

SHADE_Engine/src/Camera/SHCameraSystem.cpp

@@ -13,7 +13,7 @@
 #include "Math/SHRay.h"
 #include "Physics/System/SHPhysicsSystem.h"
 #include "Graphics/Events/SHGraphicsEvents.h"
-
+#include "Physics/Collision/CollisionTags/SHCollisionTagMatrix.h"
 
 namespace SHADE
 {
@@ -217,7 +217,7 @@ namespace SHADE
       SHVec3 cameraTarget = camera->position;
 
       
-      SHRaycaster::RaycastInfo info;
+      SHCollisionSpace::RaycastInfo info;
       
       info.layers = (uint16_t)(SHCollisionTagMatrix::GetTag("Camera")->GetMask());
       info.distance = pivot.armLength;
@@ -230,6 +230,7 @@ namespace SHADE
       {
         if (hitResult.distance < pivot.armLength)
         {
+          SHLOG_INFO("WHAT CAMERA HIT: {}", hitResult.entityHit);
 
           SHVec3 newOffset{ 0.0f,0.0f, 1.0f };
           newOffset = SHVec3::RotateX(newOffset, -(SHMath::DegreesToRadians(pivot.GetPitch())));

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

@@ -15,6 +15,7 @@
 #include "Editor/SHEditorWidgets.hpp"
 #include "Graphics/MiddleEnd/Interface/SHRenderable.h"
 #include "Graphics/MiddleEnd/Lights/SHLightComponent.h"
+#include "Physics/Interface/SHRigidBodyComponent.h"
 #include "Physics/Interface/SHColliderComponent.h"
 #include "Reflection/SHReflectionMetadata.h"
 #include "Resource/SHResourceManager.h"
@@ -262,31 +263,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::DragFloat("Mass", [component] {return component->GetMass(); }, [component](float const& value) {component->SetMass(value); }, "Mass");
+        SHEditorWidgets::CheckBox("Use Gravity", [component]{return component->IsGravityEnabled();}, [component](bool const& value){component->SetIsGravityEnabled(value);}, "Whether Gravity is enabled for this body");
+        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("Angular Drag", [component] {return component->GetAngularDrag(); }, [component](float const& value) {component->SetAngularDrag(value); }, "Angular 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", 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("Y", [component] {return component->GetFreezePositionY(); }, [component](bool const& value) {component->SetFreezePositionY(value); }, "Freeze Position - Y"); ImGui::SameLine();
-        SHEditorWidgets::CheckBox("Z", [component] {return component->GetFreezePositionZ(); }, [component](bool const& value) {component->SetFreezePositionZ(value); }, "Freeze Position - Z");
+        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); }, "Stops any displacement along the Y-axis."); ImGui::SameLine();
+        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("Y", [component] {return component->GetFreezeRotationY(); }, [component](bool const& value) {component->SetFreezeRotationY(value); }, "Freeze Rotation - Y"); ImGui::SameLine();
-        SHEditorWidgets::CheckBox("Z", [component] {return component->GetFreezeRotationZ(); }, [component](bool const& value) {component->SetFreezeRotationZ(value); }, "Freeze Rotation - Z");
+        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); }, "Stops any rotation about the Y-axis."); ImGui::SameLine();
+        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();
@@ -297,7 +301,14 @@ namespace SHADE
       {
         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);
@@ -341,64 +352,65 @@ namespace SHADE
 
       SHEditorWidgets::CheckBox("Draw Colliders", [component] { return component->GetDebugDrawState(); }, [component](bool value) { component->SetDebugDrawState(value); });
 
-      auto& colliders = component->GetCollisionShapes();
-      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* collisionShape = &component->GetCollisionShape(i);
+        SHCollisionShape* shape = component->GetCollisionShape(i);
         auto cursorPos = ImGui::GetCursorPos();
 
         
-        if (collisionShape->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 });
           
-          auto* boxShape = dynamic_cast<SHBox*>(collisionShape);
+          auto* box = reinterpret_cast<SHBox*>(shape);
           SHEditorWidgets::DragVec3
           (
             "Half Extents", { "X", "Y", "Z" },
-            [boxShape] { return boxShape->GetRelativeExtents(); },
-            [boxShape](SHVec3 const& vec) { boxShape->SetRelativeExtents(vec); });
+            [box] { return box->GetRelativeExtents(); },
+            [box](SHVec3 const& vec) { box->SetRelativeExtents(vec); });
         }
-        else if (collisionShape->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 });
-          auto* sphereShape = dynamic_cast<SHSphere*>(collisionShape);
+          auto* sphere = reinterpret_cast<SHSphere*>(shape);
           SHEditorWidgets::DragFloat
           (
             "Radius",
-            [sphereShape] { return sphereShape->GetRelativeRadius(); },
-            [sphereShape](float const& value) { sphereShape->SetRelativeRadius(value); });
+            [sphere] { return sphere->GetRelativeRadius(); },
+            [sphere](float const& value) { sphere->SetRelativeRadius(value); });
         }
-        //else if (collisionShape->GetType() == SHCollisionShape::Type::CAPSULE)
-        //{
+        else if (shape->GetType() == SHCollisionShape::Type::CAPSULE)
+        {
 
-        //}
+        }
 
         {
-          SHEditorWidgets::CheckBox("Is Trigger", [collisionShape] { return collisionShape->IsTrigger(); }, [collisionShape](bool value) { collisionShape->SetIsTrigger(value); });
-          SHEditorWidgets::ComboBox("Tag", collisionTagNames, [collisionShape]{return SHCollisionTagMatrix::GetTagIndex(collisionShape->GetCollisionTag().GetName());}, [collisionShape](int const& value){collisionShape->SetCollisionTag(SHCollisionTagMatrix::GetTag(value));});
+          SHEditorWidgets::CheckBox("Is Trigger", [shape] { return shape->IsTrigger(); }, [shape](bool value) { shape->SetIsTrigger(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", [collisionShape] { return collisionShape->GetFriction(); }, [collisionShape](float value) { collisionShape->SetFriction(value); }, "Friction", 0.05f, 0.0f, 1.0f);
-            SHEditorWidgets::DragFloat("Bounciness", [collisionShape] { return collisionShape->GetBounciness(); }, [collisionShape](float value) { collisionShape->SetBounciness(value); }, "Bounciness", 0.05f, 0.0f, 1.0f);
-            SHEditorWidgets::DragFloat("Mass Density", [collisionShape] { return collisionShape->GetDensity(); }, [collisionShape](float value) { collisionShape->SetDensity(value); }, "Mass Density", 0.1f, 0.0f);
+            SHEditorWidgets::DragFloat("Friction", [shape] { return shape->GetFriction(); }, [shape](float value) { shape->SetFriction(value); }, "Friction", 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", [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" }, [collisionShape] {return collisionShape->GetPositionOffset(); }, [collisionShape](SHVec3 const& vec) {collisionShape->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" }, 
-          [collisionShape]
+          [&shape]
           {
-            auto offset = collisionShape->GetRotationOffset();
+            auto offset = shape->GetRotationOffset();
             return offset;
           }, 
-          [collisionShape](SHVec3 const& vec)
+          [&shape](SHVec3 const& vec)
           {
-            collisionShape->SetRotationOffset(vec);
+            shape->SetRotationOffset(vec);
           }, true);
           SHEditorWidgets::EndPanel();
         }
@@ -413,21 +425,24 @@ namespace SHADE
       }
       if (colliderToDelete.has_value())
       {
-        component->RemoveCollisionShape(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->AddBoxCollisionShape(SHVec3::One);
+          auto* compositeCollider = dynamic_cast<SHCompositeCollider* const>(component->GetCollider());
+          compositeCollider->AddBoxCollisionShape(SHVec3::One);
         }
         if (ImGui::Selectable("Sphere Collider"))
         {
-          newColl = component->AddSphereCollisionShape(1.0f);
+          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

@@ -372,9 +372,14 @@ namespace SHADE
           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/Math/Geometry/SHAABB.cpp

@@ -0,0 +1,219 @@
+/****************************************************************************************
+ * \file      SHAABB.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation 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.
+****************************************************************************************/
+
+#include <SHpch.h>
+
+// Primary Header
+#include "SHAABB.h"
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Math/SHRay.h"
+
+using namespace DirectX;
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHAABB::SHAABB() noexcept
+  {
+    Extents = SHVec3::One * 0.5f;
+  }
+
+  SHAABB::SHAABB(const SHVec3& c, const SHVec3& hE) noexcept
+  {
+    Center  = c;
+    Extents = hE;
+  }
+
+
+  SHAABB::SHAABB(const SHAABB& rhs) noexcept
+  {
+    if (this == &rhs)
+      return;
+
+    Center  = rhs.Center;
+    Extents = rhs.Extents;
+  }
+
+  SHAABB::SHAABB(SHAABB&& rhs) noexcept
+  {
+    Center  = rhs.Center;
+    Extents = rhs.Extents;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHAABB& SHAABB::operator=(const SHAABB& rhs) noexcept
+  {
+    if (this != &rhs)
+    {
+      Center  = rhs.Center;
+      Extents = rhs.Extents;
+    }
+
+    return *this;
+  }
+
+  SHAABB& SHAABB::operator=(SHAABB&& rhs) noexcept
+  {
+    Center  = rhs.Center;
+    Extents = rhs.Extents;
+
+    return *this;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHVec3 SHAABB::GetCenter() const noexcept
+  {
+    return Center;
+  }
+
+  SHVec3 SHAABB::GetExtents() const noexcept
+  {
+    return Extents;
+  }
+
+  SHVec3 SHAABB::GetMin() const noexcept
+  {
+    return SHVec3{ Center.x - Extents.x, Center.y - Extents.y, Center.z - Extents.z };
+  }
+
+  SHVec3 SHAABB::GetMax() const noexcept
+  {
+    return SHVec3{ Center.x + Extents.x, Center.y + Extents.y, Center.z + Extents.z };
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Setter Function Definitions                                                       */
+  /*-----------------------------------------------------------------------------------*/
+
+  void SHAABB::SetCenter(const SHVec3& newCenter) noexcept
+  {
+    Center = newCenter;
+  }
+
+  void SHAABB::SetExtents(const SHVec3& newHalfExtents) noexcept
+  {
+    Extents = newHalfExtents;
+  }
+
+  void SHAABB::SetMin(const SHVec3& min) noexcept
+  {
+    const SHVec3 MAX = GetMax();
+
+    Center  = SHVec3::Lerp(min, MAX, 0.5f);
+    Extents = SHVec3::Abs((MAX - min) * 0.5f);
+  }
+
+  void SHAABB::SetMax(const SHVec3& max) noexcept
+  {
+    const SHVec3 MIN = GetMin();
+
+    Center  = SHVec3::Lerp(MIN, max, 0.5f);
+    Extents = SHVec3::Abs((max - MIN) * 0.5f);
+  }
+
+  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> SHAABB::GetVertices() const noexcept
+  {
+    std::vector<SHVec3> vertices{ 8 };
+    GetCorners(vertices.data());
+    return vertices;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Function Member Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHAABB::TestPoint(const SHVec3& point) const noexcept
+  {
+    return BoundingBox::Contains(point);
+  }
+
+  SHRaycastResult SHAABB::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
+    }
+
+    return result;
+  }
+
+  bool SHAABB::Contains(const SHAABB& rhs) const noexcept
+  {
+    return BoundingBox::Contains(rhs) == CONTAINS;
+  }
+
+  float SHAABB::Volume() const noexcept
+  {
+    return 8.0f * (Extents.x * Extents.y * Extents.z);
+  }
+
+  float SHAABB::SurfaceArea() const noexcept
+  {
+    return 8.0f * ((Extents.x * Extents.y) 
+      + (Extents.x * Extents.z) 
+      + (Extents.y * Extents.z));
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Static Function Member Definitions                                                */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHAABB SHAABB::Combine(const SHAABB& lhs, const SHAABB& rhs) noexcept
+  {
+    SHAABB result;
+    CreateMerged(result, lhs, rhs);
+    return result;
+  }
+
+  bool SHAABB::Intersect(const SHAABB& lhs, const SHAABB& rhs) noexcept
+  {
+    return lhs.Intersects(rhs);
+  }
+
+  SHAABB SHAABB::BuildFromBoxes(const SHAABB* boxes, size_t numBoxes) noexcept
+  {
+    SHAABB result;
+
+    for (size_t i = 1; i < numBoxes; ++i)
+      CreateMerged(result, boxes[i - 1], boxes[i]);
+
+    return result;
+  }
+
+  SHAABB SHAABB::BuildFromVertices(const SHVec3* vertices, size_t numVertices, size_t stride) noexcept
+  {
+    SHAABB result;
+    CreateFromPoints(result, numVertices, vertices, stride);
+    return result;
+  }
+
+} // namespace SHADE

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/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/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

@@ -46,7 +46,7 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     /** Standard Epsilon value for comparing Single-Precision Floating-Point values. */
-    static constexpr float EPSILON    = 0.001f;
+    static constexpr float EPSILON        = 0.0001f;
     
     /** Single-Precision Floating-Point value of infinity */ 
     static constexpr float INF            = std::numeric_limits<float>::infinity();
@@ -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
-  : XMFLOAT4( _x, _y, _z, _w )
+  SHQuaternion::SHQuaternion(const XMFLOAT4& xmfloat4) noexcept
+  : XMFLOAT4( xmfloat4 )
   {}
 
-  SHQuaternion::SHQuaternion(const reactphysics3d::Vector3& rp3dEuler) noexcept
-  : XMFLOAT4( 0.0f, 0.0f, 0.0f, 1.0f )
-  {
-    XMStoreFloat4(this, XMQuaternionRotationRollPitchYawFromVector(SHVec3 { rp3dEuler }));
-  }
-
-  SHQuaternion::SHQuaternion(const reactphysics3d::Quaternion& rp3dQuat) noexcept
-  : XMFLOAT4( rp3dQuat.x, rp3dQuat.y, rp3dQuat.z, rp3dQuat.w )
+  SHQuaternion::SHQuaternion(float _x, float _y, float _z, float _w) noexcept
+  : XMFLOAT4( _x, _y, _z, _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

@@ -10,8 +10,6 @@
 
 #include <SHpch.h>
 
-#include <reactphysics3d/mathematics/Ray.h>
-
 // Primary Header
 #include "SHRay.h"
 
@@ -29,14 +27,12 @@ namespace SHADE
 
   SHRay::SHRay(const SHVec3& pos, const SHVec3& dir) noexcept
   : position  { pos }
-  , direction { dir }
-  {}
-
-  SHRay::SHRay(const reactphysics3d::Ray& rp3dRay) noexcept
-  : position  { rp3dRay.point1 }
-  , direction { SHVec3::Normalise(rp3dRay.point2 - rp3dRay.point1) }
-  {}
-
+  {
+    if (dir.LengthSquared() > 1.0f)
+      direction = SHVec3::Normalise(dir);
+    else
+      direction = dir;
+  }
 
   /*-----------------------------------------------------------------------------------*/
   /* Operator Overload Definitions                                                     */
@@ -64,12 +60,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,19 +10,11 @@
 
 #pragma once
 
-#include <reactphysics3d/mathematics/Ray.h>
-
 // Project Headers
-#include "SH_API.h"
 #include "Vector/SHVec3.h"
 
-/*-------------------------------------------------------------------------------------*/
-/* Forward Declarations                                                                */
-/*-------------------------------------------------------------------------------------*/
-
 namespace SHADE
 {
-
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
@@ -45,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;
@@ -60,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/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,647 @@
+/****************************************************************************************
+ * \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& newAABB)
+  {
+    // Get node index
+    const int32_t INDEX_TO_UPDATE = nodeMap[id];
+
+    Node& nodeToUpdate = nodes[INDEX_TO_UPDATE];
+
+    // Update the AABB directly
+    const SHAABB OLD_AABB = nodeToUpdate.AABB;
+    nodeToUpdate.AABB     = newAABB;
+
+    // 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);
+
+    // If new AABB has not moved enough, skip.
+    // We only modify the position &/ size, but the AABB remains within this space.
+    if (OLD_AABB.Contains(nodeToUpdate.AABB))
+      return;
+
+    // Re-insert the node to find it's new neighbour
+    removeLeaf(INDEX_TO_UPDATE);
+    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/SHCollisionTags.h

@@ -117,5 +117,3 @@ namespace SHADE
 } // namespace SHADE
 
 SHADE::SHCollisionTag::Layer SH_API operator|(SHADE::SHCollisionTag::Layer lhs, SHADE::SHCollisionTag::Layer rhs) noexcept;
-
-

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 = SHCollisionState::INVALID;
+  };
+
+  /**
+   * @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 = SHCollisionState::INVALID;
+    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

@@ -0,0 +1,117 @@
+/****************************************************************************************
+ * \file      SHCollisionID.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 "Physics/Interface/SHColliderComponent.h"
+#include "Physics/Interface/SHRigidBodyComponent.h"
+
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Forward Declarations                                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  struct SHCollisionKeyHash;
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates the information when two collision shapes intersect.
+  */
+  class SH_API SHCollisionKey
+  {
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Friends                                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    friend struct SHCollisionKeyHash;
+
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionKey   ()                          noexcept;
+    SHCollisionKey   (const SHCollisionKey& rhs)  noexcept;
+    SHCollisionKey   (SHCollisionKey&& rhs)       noexcept;
+
+    ~SHCollisionKey  () noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionKey& operator= (const SHCollisionKey& rhs) noexcept;
+    SHCollisionKey& operator= (SHCollisionKey&& rhs)      noexcept;
+
+    bool operator==(const SHCollisionKey& rhs) const;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    [[nodiscard]] EntityID                    GetEntityA          ()  const noexcept;
+    [[nodiscard]] EntityID                    GetEntityB          ()  const noexcept;
+    [[nodiscard]] uint32_t                    GetShapeIndexA      ()  const noexcept;
+    [[nodiscard]] uint32_t                    GetShapeIndexB      ()  const noexcept;
+    [[nodiscard]] const SHRigidBodyComponent* GetRigidBodyA       ()  const noexcept;
+    [[nodiscard]] const SHRigidBodyComponent* GetRigidBodyB       ()  const noexcept;
+    [[nodiscard]] const SHCollisionShape*     GetCollisionShapeA  ()  const noexcept;
+    [[nodiscard]] const SHCollisionShape*     GetCollisionShapeB  ()  const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void SetEntityA         (EntityID entityID)     noexcept;
+    void SetEntityB         (EntityID entityID)     noexcept;
+    void SetCollisionShapeA (uint32_t shapeIndexA)  noexcept;
+    void SetCollisionShapeB (uint32_t shapeIndexB)  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                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    union
+    {
+      uint64_t value[2];  // EntityValue, ShapeIndexValue
+      uint32_t ids  [4];  // EntityA, EntityB, ShapeIndexA, ShapeIndexB
+    };
+  };
+
+  /**
+   * @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,74 @@
+/****************************************************************************************
+ * \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:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    struct
+    {
+      uint8_t inI;  // Incoming Incident Edge
+      uint8_t outI; // Outgoing Incident Edge
+      uint8_t inR;  // Incoming Reference Edge
+      uint8_t outR; // Outgoing Reference Edge
+    };
+
+    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
+
+    // We store points locally for each contact
+    SHVec3            localPointA;
+    SHVec3            localPointB;
+    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

@@ -0,0 +1,73 @@
+/****************************************************************************************
+ * \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/Narrowphase/SHSATInfo.h"
+#include "Physics/Collision/Shapes/SHCollisionShape.h"
+#include "SHContact.h"
+#include "SHCollisionEvents.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  struct SH_API SHManifold
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHManifold  (SHCollisionShape* a, SHCollisionShape* b)  noexcept;
+    SHManifold  (const SHManifold& rhs)                     noexcept;
+    SHManifold  (SHManifold&& rhs)                          noexcept;
+
+    ~SHManifold () noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHManifold& operator=(const SHManifold& rhs)  noexcept;
+    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;
+
+    SHSATInfo         cachedSATInfo;
+
+    SHVec3            normal;
+    SHVec3            tangents[SHContact::NUM_TANGENTS];
+    SHContact         contacts[MAX_NUM_CONTACTS];
+    
+  };
+  
+} // namespace SHADE
+
+#include "SHManifold.hpp"
+

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

@@ -0,0 +1,110 @@
+/****************************************************************************************
+ * \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 }
+  , cachedSATInfo { rhs.cachedSATInfo }
+  , 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 }
+  , cachedSATInfo { rhs.cachedSATInfo }
+  , 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;
+    cachedSATInfo = rhs.cachedSATInfo;
+    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;
+    cachedSATInfo = rhs.cachedSATInfo;
+    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

@@ -0,0 +1,35 @@
+/****************************************************************************************
+ * \file      SHCapsuleVsCapsule.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \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
+ *            of DigiPen Institute of Technology is prohibited.
+****************************************************************************************/
+
+#include <SHpch.h>
+
+// Primary Header
+#include "SHCollision.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHCollision::CapsuleVsCapsule(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    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,76 @@
+/****************************************************************************************
+ * \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"
+
+
+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:
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    static bool cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
+  };
+} // namespace SHADE

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

@@ -0,0 +1,65 @@
+/****************************************************************************************
+ * \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
+  };
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  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

@@ -1,7 +1,7 @@
 /****************************************************************************************
- * \file      SHPhysicsWorld.h
+ * \file      SHCollisionDispatch.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a Physics World.
+ * \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
@@ -10,10 +10,9 @@
 
 #pragma once
 
-#include <reactphysics3d/reactphysics3d.h>
-
 // Project Headers
-#include "Math/Vector/SHVec3.h"
+#include "Physics/Collision/Contacts/SHManifold.h"
+#include "Physics/Collision/Contacts/SHCollisionKey.h"
 
 namespace SHADE
 {
@@ -21,52 +20,36 @@ namespace SHADE
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  struct SH_API SHPhysicsWorldState
+  /**
+   * @brief
+   * Encapsulates static methods for running narrow-phase collision detection.
+  */
+  class SH_API SHCollisionDispatcher
   {
   public:
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
 
+    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                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    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;
-    };
+    using ManifoldCollide = bool(*)(SHManifold&, const SHCollisionShape& A, const SHCollisionShape& B);
+    using TriggerCollide  = bool(*)(const SHCollisionShape& A, const SHCollisionShape& B);
 
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    rp3d::PhysicsWorld* world;
-    WorldSettings       settings;
+    static constexpr int NUM_SHAPES   = static_cast<int>(SHCollisionShape::Type::COUNT);
 
-    /*---------------------------------------------------------------------------------*/
-    /* Constructors & Destructor                                                       */
-    /*---------------------------------------------------------------------------------*/
-
-    SHPhysicsWorldState() noexcept;
-
-    /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    void CreateWorld    (rp3d::PhysicsCommon& factory);
-    void DestroyWorld   (rp3d::PhysicsCommon& factory);
-
-    /**
-     * @brief Applies the current settings to the physics world. The world must be created
-     *        before this is called.
-    */
-    void UpdateSettings () const noexcept;
+    static const ManifoldCollide  manifoldCollide [NUM_SHAPES][NUM_SHAPES];
+    static const TriggerCollide   triggerCollide  [NUM_SHAPES][NUM_SHAPES];
   };
 
 

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

@@ -0,0 +1,38 @@
+/****************************************************************************************
+ * \file      SHCollisionUtils.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for some objects to assist with 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>
+
+// Primary Header
+#include "SHCollisionUtils.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHVec3 SHCollisionUtils::ShapeTransform::operator*(const SHVec3& rhs) const noexcept
+  {
+    return SHVec3::Rotate(rhs, orientation) + position;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionUtils::ShapeTransform SHCollisionUtils::ShapeTransform::GetInverse() const noexcept
+  {
+    const SHQuaternion INV_ORIENTATION = SHQuaternion::Inverse(orientation);
+    return ShapeTransform { SHVec3::Rotate(-position, INV_ORIENTATION), INV_ORIENTATION };
+  }
+
+
+} // namespace SHADE

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

@@ -0,0 +1,108 @@
+/****************************************************************************************
+ * \file      SHCollisionUtils.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for some objects to assist with 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
+
+// Project Headers
+#include "Math/Vector/SHVec3.h"
+#include "Physics/Collision/Contacts/SHContact.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Defines a bunch of helper objects for collision detection.
+  */
+  class SHCollisionUtils
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct ShapeTransform
+    {
+    public:
+      /*-------------------------------------------------------- ----------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------- ----------------------*/
+
+      SHVec3        position;
+      SHQuaternion  orientation;
+
+      /*-------------------------------------------------------- ----------------------*/
+      /* Operator Overloads                                                            */
+      /*-------------------------------------------------------- ----------------------*/
+
+      SHVec3 operator* (const SHVec3& rhs) const noexcept;
+
+      /*-------------------------------------------------------- ----------------------*/
+      /* Member Functions                                                              */
+      /*-------------------------------------------------------- ----------------------*/
+
+      ShapeTransform GetInverse() const noexcept;
+    };
+
+    struct FaceQuery
+    {
+    public:
+      /*-------------------------------------------------------- ----------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------- ----------------------*/
+
+      bool    colliding     = false;  // Allows for early out
+      int32_t closestFace   = -1;
+      float   bestDistance  = std::numeric_limits<float>::lowest();
+    };
+
+    struct EdgeQuery
+    {
+    public:
+      /*-------------------------------------------------------- ----------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------- ----------------------*/
+
+      int32_t halfEdgeA     = -1;
+      int32_t halfEdgeB     = -1;
+      int32_t axis          = -1;
+      float   bestDistance  = std::numeric_limits<float>::lowest();
+    };
+
+    struct EdgeContacts
+    {
+    public:
+      /*-------------------------------------------------------- ----------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------- ----------------------*/
+
+      SHVec3  closestPointA;
+      SHVec3  closestPointB;
+    };
+
+    struct ClipVertex
+    {
+    public:
+      /*-------------------------------------------------------- ----------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------- ----------------------*/
+
+      SHVec3            position;
+      SHContactFeatures featurePair;
+    };
+
+    using ClipVertices  = std::vector<ClipVertex>;
+  };
+
+
+} // namespace SHAD

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

@@ -0,0 +1,887 @@
+/****************************************************************************************
+ * \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 "SHCollisionUtils.h"
+#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
+{
+
+  /*
+   * ! References
+   *  https://ia801303.us.archive.org/30/items/GDC2013Gregorius/GDC2013-Gregorius.pdf
+   *  https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp
+   */
+
+  SHCollisionUtils::FaceQuery     queryFaceDirections           (const SHConvexPolyhedron&, const SHConvexPolyhedron&) noexcept;
+  SHCollisionUtils::EdgeQuery     queryEdgeDirections           (const SHConvexPolyhedron&, const SHConvexPolyhedron&) noexcept;
+  bool                            buildMinkowskiFace            (const SHConvexPolyhedron&, const SHConvexPolyhedron&, int32_t edgeA, int32_t edgeB) noexcept;
+  bool                            isMinkowskiFace               (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept;
+  float                           distanceBetweenEdges          (const SHConvexPolyhedron&, const SHConvexPolyhedron&, int32_t edgeA, int32_t edgeB) noexcept;
+  SHCollisionUtils::EdgeContacts  findClosestPointBetweenEdges  (const SHConvexPolyhedron&, const SHConvexPolyhedron&, int32_t edgeA, int32_t edgeB) noexcept;
+  int32_t                         findIncidentFace              (const SHConvexPolyhedron&, const SHVec3& normal) noexcept;
+  bool                            findFaceContacts              (SHManifold&, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept;
+  SHCollisionUtils::ClipVertices  clipPolygonWithPlane          (const SHCollisionUtils::ClipVertices& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept;
+  std::vector<int32_t>            reduceContacts                (const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept;
+
+  /*-----------------------------------------------------------------------------------*/
+  /* 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 SHCollisionUtils::SHCollisionUtils::FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
+    if (FACE_QUERY_A.bestDistance > 0.0f)
+      return false;
+
+    const SHCollisionUtils::SHCollisionUtils::FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A);
+    if (FACE_QUERY_B.bestDistance > 0.0f)
+      return false;
+
+    const SHCollisionUtils::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 ABSOLUTE_TOLERANCE = 0.01f;
+    static constexpr float RELATIVE_TOLERANCE = 0.95f;
+
+    const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
+    const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
+
+    const SHCollisionUtils::ShapeTransform TF_A { POLY_A.GetWorldCentroid(), POLY_A.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform TF_B { POLY_B.GetWorldCentroid(), POLY_B.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse();
+    const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse();
+
+    if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID)
+      return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B);
+
+    SHSATInfo cachedInfo;
+
+    const SHCollisionUtils::FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
+    if (FACE_QUERY_A.bestDistance > 0.0f)
+    {
+      // cache the info
+      cachedInfo.type         = SHSATInfo::Type::FACE;
+      cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
+      cachedInfo.info.refFace = FACE_QUERY_A.closestFace;
+
+      manifold.cachedSATInfo  = cachedInfo;
+
+      return false;
+    }
+      
+
+    const SHCollisionUtils::FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A);
+    if (FACE_QUERY_B.bestDistance > 0.0f)
+    {
+      // cache the info
+      cachedInfo.type         = SHSATInfo::Type::FACE;
+      cachedInfo.info.refPoly = SHSATInfo::ShapeID::B;
+      cachedInfo.info.refFace = FACE_QUERY_B.closestFace;
+
+      manifold.cachedSATInfo  = cachedInfo;
+
+      return false;
+    }
+
+    const SHCollisionUtils::EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B);
+    if (EDGE_QUERY.bestDistance > 0.0f)
+    {
+      // cache the info
+      cachedInfo.type       = SHSATInfo::Type::EDGE;
+      cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
+      cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
+
+      manifold.cachedSATInfo  = cachedInfo;
+
+      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;
+
+    SHCollisionUtils::FaceQuery minFaceQuery;
+    if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE)
+    {
+      minFaceQuery  = FACE_QUERY_A;
+      referencePoly = &POLY_A;
+      incidentPoly  = &POLY_B;
+      flipNormal    = false;
+
+      cachedInfo.type         = SHSATInfo::Type::FACE;
+      cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
+      cachedInfo.info.refFace = minFaceQuery.closestFace;
+    }
+    else
+    {
+      minFaceQuery  = FACE_QUERY_B;
+      referencePoly = &POLY_B;
+      incidentPoly  = &POLY_A;
+      flipNormal    = true;
+
+      cachedInfo.type         = SHSATInfo::Type::FACE;
+      cachedInfo.info.refPoly = SHSATInfo::ShapeID::B;
+      cachedInfo.info.refFace = minFaceQuery.closestFace;
+    }
+
+    minFaceQuery.bestDistance = std::max(FACE_QUERY_A.bestDistance, FACE_QUERY_B.bestDistance);
+
+    // 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 * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_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, TAIL_A - A.GetWorldCentroid()) < 0.0f)
+        manifold.normal = -manifold.normal;
+
+      // In this scenario, we only have one contact
+      uint32_t numContacts = 0;
+
+      const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(POLY_A, POLY_B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
+
+      SHContact contact;
+      contact.featurePair.key = EDGE_QUERY.axis;
+      contact.penetration     = -EDGE_QUERY.bestDistance;
+
+      contact.localPointA     = INV_TF_A * CONTACTS.closestPointB;
+      contact.localPointB     = INV_TF_B * CONTACTS.closestPointA;
+
+      manifold.contacts[numContacts++] = contact;
+      manifold.numContacts = numContacts;
+
+      // Cache the info
+      cachedInfo.type       = SHSATInfo::Type::EDGE;
+      cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
+      cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
+
+      manifold.cachedSATInfo = cachedInfo;
+
+      return true;
+    }
+
+    const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
+    const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
+
+    manifold.cachedSATInfo = cachedInfo;
+    return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionUtils::FaceQuery queryFaceDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
+  {
+    SHCollisionUtils::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;
+  }
+
+  SHCollisionUtils::EdgeQuery queryEdgeDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
+  {
+    SHCollisionUtils::EdgeQuery edgeQuery;
+
+    const int32_t EDGE_COUNT_A = A.GetHalfEdgeCount();
+    const int32_t EDGE_COUNT_B = B.GetHalfEdgeCount();
+
+    int32_t axis = -1;
+    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;
+
+        ++axis;
+
+        const float SEPARATION = distanceBetweenEdges(A, B, i, j);
+        if (SEPARATION > edgeQuery.bestDistance)
+        {
+          edgeQuery.bestDistance = SEPARATION;
+          edgeQuery.halfEdgeA = i;
+          edgeQuery.halfEdgeB = j;
+          edgeQuery.axis      = axis;
+        }
+      }
+    }
+
+    return edgeQuery;
+  }
+
+  bool 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 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);
+
+    // Short circuit may be faster without storing the booleans
+    return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f;
+  }
+
+  float 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 (cross product length close to 0)
+    if (SHMath::CompareFloat(SHVec3::Cross(DIR_A, DIR_B).LengthSquared(), 0.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, TAIL_A - A.GetWorldCentroid()) < 0.0f)
+      normal = -normal;
+
+    return SHVec3::Dot(normal, TAIL_B - TAIL_A);
+  }
+
+  SHCollisionUtils::EdgeContacts findClosestPointBetweenEdges(const SHConvexPolyhedron& polyA, const SHConvexPolyhedron& polyB, int32_t edgeA, int32_t edgeB) noexcept
+  {
+    /*
+     * A more robust solution can be found in Real-Time Collision Detection by
+     * Christer Ericson, page 148-149.
+     */
+
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = polyA.GetHalfEdge(edgeA);
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = polyB.GetHalfEdge(edgeB);
+
+    const SHVec3 HEAD_A = polyA.GetVertex(HALF_EDGE_A.headVertexIndex);
+    const SHVec3 TAIL_A = polyA.GetVertex(HALF_EDGE_A.tailVertexIndex);
+    const SHVec3 HEAD_B = polyB.GetVertex(HALF_EDGE_B.headVertexIndex);
+    const SHVec3 TAIL_B = polyB.GetVertex(HALF_EDGE_B.tailVertexIndex);
+
+
+    const SHVec3 D1 = HEAD_A - TAIL_A;
+    const SHVec3 D2 = HEAD_B - TAIL_B;
+    const SHVec3 R  = TAIL_A - TAIL_B;
+
+    const float A  = D1.LengthSquared();    // a
+    const float E  = D2.LengthSquared();    // e
+
+    const float B  = SHVec3::Dot(D1, D2);   // b
+    const float C  = SHVec3::Dot(D1, R);    // c
+    const float F  = SHVec3::Dot(D2, R);    // f
+
+    float s = 0.0f;
+    float t = 0.0f;
+
+    SHCollisionUtils::EdgeContacts result
+    {
+      .closestPointA = TAIL_A,
+      .closestPointB = TAIL_B
+    };
+
+    // If both segment degenerates into points
+    if (A <= SHMath::EPSILON && E <= SHMath::EPSILON)
+      return result;
+      
+
+    // Segment A degenerates into a point
+    if (A <= SHMath::EPSILON)
+      t = std::clamp(F / E, 0.0f, 1.0f);
+    else
+    {
+      // Segment B degenerates into a point
+      if (E <= SHMath::EPSILON)
+        s = std::clamp(-C / A, 0.0f, 1.0f);
+      else
+      {
+        const float DENOMINATOR = A * E - B * B;
+        if (!SHMath::CompareFloat(DENOMINATOR, 0.0f))
+          s = std::clamp((B * F - C * E) / DENOMINATOR, 0.0f, 1.0f);
+        else
+          s = 0.0f;
+
+        // Clamp closest point to segment B
+        t = (B * s + F) / E;
+        if (t < 0.0f)
+        {
+          t = 0.0f;
+          s = std::clamp(-C / A, 0.0f, 1.0f);
+        }
+        if (t > 1.0)
+        {
+          t = 1.0f;
+          s = std::clamp((B - C) / A, 0.0f, 1.0f);
+        }
+      }
+    }
+
+    result.closestPointA = TAIL_A + D1 * s;
+    result.closestPointB = TAIL_B + D2 * t;
+    return result;
+  }
+
+  int32_t 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;
+  }
+
+  bool findFaceContacts(SHManifold& manifold, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept
+  {
+    const SHCollisionUtils::ShapeTransform TF_INC { incPoly.GetWorldCentroid(), incPoly.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform TF_REF { refPoly.GetWorldCentroid(), refPoly.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform INV_TF_INC = TF_INC.GetInverse();
+    const SHCollisionUtils::ShapeTransform INV_TF_REF = TF_REF.GetInverse();
+
+
+    const SHHalfEdgeStructure::Face& INCIDENT_FACE  = incPoly.GetFace(incFace);
+    const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace);
+
+    const int32_t NUM_INCIDENT_VERTICES   = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
+    const int32_t NUM_REFERENCE_VERTICES  = static_cast<int32_t>(REFERENCE_FACE.vertexIndices.size());
+
+    const SHVec3 REF_NORMAL       = refPoly.GetNormal(refFace);
+    const SHVec3 LOCAL_REF_NORMAL = SHVec3::Rotate(REF_NORMAL, INV_TF_REF.orientation);
+
+    // Build incoming vertices to clip
+    // But it in the reference local-space
+    SHCollisionUtils::ClipVertices clipIn;
+    clipIn.resize(NUM_INCIDENT_VERTICES * 2, SHCollisionUtils::ClipVertex{});
+
+    int32_t numClipIn = 0;
+    for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i)
+    {
+      const int32_t PREV_I  = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1;
+      const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index;
+
+      // The incoming id is the previous edge
+      // The outgoing id is the current edge (where this vertex is the tail of)
+
+      SHCollisionUtils::ClipVertex v;
+      v.position          = INV_TF_REF * incPoly.GetVertex(V_INDEX);
+      v.featurePair.inI   = INCIDENT_FACE.vertexIndices[PREV_I].edgeIndex;
+      v.featurePair.outI  = INCIDENT_FACE.vertexIndices[i].edgeIndex;
+      v.featurePair.inR   = 0;
+      v.featurePair.outR  = 0;
+
+      clipIn[numClipIn++] = v;
+    }
+
+    // Clip the vertices against the reference face side planes.
+    // Number of side planes == number of edges == number of vertices
+    for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i)
+    {
+      // Side plane can be built with the vertex on the edge and the plane's normal
+      // Plane normal = faceNormal X tangent (v2 - v1)
+
+      const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index;
+      const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index;
+
+      // Get reference vertices in reference local space
+      const SHVec3  V1 = INV_TF_REF * refPoly.GetVertex(V1_INDEX);
+      const SHVec3  V2 = INV_TF_REF * refPoly.GetVertex(V2_INDEX);
+
+      const SHVec3  TANGENT       = SHVec3::Normalise(V2 - V1);
+      const SHVec3  PLANE_NORMAL  = SHVec3::Cross(LOCAL_REF_NORMAL, TANGENT);
+      const SHPlane CLIP_PLANE { V1, PLANE_NORMAL };
+
+      SHCollisionUtils::ClipVertices clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex);
+      if (clipOut.empty())
+        return false;
+
+      // Replace the clip container's contents with the clipped points for the next clipping pass
+      const int32_t NUM_CLIPPED = static_cast<int32_t>(clipOut.size());
+      for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex)
+      {
+        clipIn[clippedIndex].position         = clipOut[clippedIndex].position;
+        clipIn[clippedIndex].featurePair.key  = clipOut[clippedIndex].featurePair.key;
+      }
+      numClipIn = NUM_CLIPPED;
+    }
+
+    // From the final set of clipped points, only keep the points that are below the reference plane.
+    const SHVec3  LOCAL_REF_POINT = INV_TF_REF * refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index);
+    const SHPlane REFERENCE_PLANE{ LOCAL_REF_POINT, LOCAL_REF_NORMAL };
+
+    uint32_t numContacts = 0;
+
+    std::vector<SHContact> contacts;
+    for (int32_t i = 0; i < numClipIn; ++i)
+    {
+      const SHVec3 POS = clipIn[i].position;
+      const float DIST = REFERENCE_PLANE.SignedDistance(POS);
+      if (DIST <= 0.0f)
+      {
+        SHContact contact;
+        contact.penetration = -DIST;
+
+        // A: Reference, B: Incident
+        // Project the clipped point onto the reference place for localPointA
+        contact.localPointA = POS + LOCAL_REF_NORMAL * SHVec3::Dot(LOCAL_REF_NORMAL, POS - LOCAL_REF_POINT);
+        contact.localPointB = INV_TF_INC * (TF_REF * POS);
+
+        if (flip)
+        {
+          contact.featurePair.inI = clipIn[i].featurePair.inR;
+          contact.featurePair.inR = clipIn[i].featurePair.inI;
+          contact.featurePair.outI = clipIn[i].featurePair.outR;
+          contact.featurePair.outR = clipIn[i].featurePair.outI;
+
+          std::swap(contact.localPointA, contact.localPointB);
+        }
+        else
+        {
+          contact.featurePair.key = clipIn[i].featurePair.key;
+        }
+
+        contacts.emplace_back(contact);
+        ++numContacts;
+      }
+    }
+
+    // Reduce contact manifold if more than 4 points
+    if (numContacts > SHPHYSICS_MAX_MANIFOLD_CONTACTS)
+    {
+      const auto INDICES_TO_KEEP = reduceContacts(contacts, LOCAL_REF_NORMAL);
+      std::vector<SHContact> reducedContacts;
+
+      for (int32_t i = 0; i < SHPHYSICS_MAX_MANIFOLD_CONTACTS; ++i)
+        reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]);
+
+      contacts.clear();
+      // Copy contacts to main container
+      for (auto& contact : reducedContacts)
+        contacts.emplace_back(contact);
+    }
+
+    // Remove potential duplicate contact points
+    // No way about this being an n^2 loop
+    static constexpr float THRESHOLD = SHPHYSICS_SAME_CONTACT_DISTANCE * SHPHYSICS_SAME_CONTACT_DISTANCE;
+    for (auto i = contacts.begin(); i != contacts.end(); ++i)
+    {
+      for (auto j = i + 1; j != contacts.end();)
+      {
+        const float D2 = SHVec3::DistanceSquared(i->localPointA, j->localPointA);
+        if (D2 < THRESHOLD)
+          j = contacts.erase(j);
+        else
+          ++j;
+      }
+    }
+
+    // Copy final contacts into the manifold
+    numContacts = static_cast<int32_t>(contacts.size());
+    for (int32_t i = 0; i < numContacts; ++i)
+      manifold.contacts[i] = contacts[i];
+
+    manifold.numContacts  = numContacts;
+    manifold.normal       = REF_NORMAL;
+    if (flip)
+      manifold.normal = -manifold.normal;
+
+    return true;
+  }
+
+
+  SHCollisionUtils::ClipVertices clipPolygonWithPlane(const SHCollisionUtils::ClipVertices& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept
+  {
+   SHCollisionUtils::ClipVertices out;
+
+    int32_t v1Index = numIn - 1;
+    int32_t v2Index = 0;
+
+    float v1Distance = plane.SignedDistance(in[v1Index].position);
+    float v2Distance = 0.0f;
+
+    for (int32_t i = 0; i < numIn; ++i)
+    {
+      v2Index = i;
+
+      const SHVec3 v1Pos = in[v1Index].position;
+      const SHVec3 v2Pos = in[v2Index].position;
+
+      v2Distance = plane.SignedDistance(v2Pos);
+
+      // v1 in front, v2 behind
+      // keep the intersection point
+      if (v1Distance >= 0.0f && v2Distance < 0.0f)
+      {
+        SHCollisionUtils::ClipVertex intersection;
+
+        const SHVec3  PLANE_NORMAL  = -plane.GetNormal(); 
+
+        // In case the edge is parallel, the intersection is just the start point
+        const float   DOT           = SHVec3::Dot(v2Pos - v1Pos, PLANE_NORMAL);
+        const bool    IS_PARALLEL   = SHMath::CompareFloat(DOT, 0.0f);
+        const float   ALPHA         = IS_PARALLEL ? 0.0f : (plane.GetDistance() - SHVec3::Dot(v1Pos, PLANE_NORMAL)) / DOT;
+
+        intersection.position         = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA);
+        intersection.featurePair.inI  = in[v1Index].featurePair.outI;
+        intersection.featurePair.outI = 0;
+        intersection.featurePair.inR  = 0;
+        intersection.featurePair.outR = planeIdx;
+
+        out.emplace_back(intersection);
+      }
+
+      // v1 behind, v2 in front
+      // keep intersection point & v2
+      if(v1Distance < 0.0f && v2Distance >= 0.0f)
+      {
+        SHCollisionUtils::ClipVertex intersection;
+
+        const SHVec3  PLANE_NORMAL  = plane.GetNormal(); 
+
+        // In case the edge is parallel, the intersection is just the end point
+        const float   DOT           = SHVec3::Dot(v2Pos - v1Pos, PLANE_NORMAL);
+        const bool    IS_PARALLEL   = SHMath::CompareFloat(DOT, 0.0f);
+        const float   ALPHA         = IS_PARALLEL ? 0.0f : (-plane.GetDistance() - SHVec3::Dot(v1Pos, PLANE_NORMAL)) / DOT;
+
+        intersection.position         = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA);
+        intersection.featurePair.inI  = 0;
+        intersection.featurePair.outI = in[v2Index].featurePair.inI;
+        intersection.featurePair.inR  = planeIdx;
+        intersection.featurePair.outR = 0;
+
+        out.emplace_back(intersection);
+        out.emplace_back(in[v2Index]);
+      }
+
+      // both in front, keep v2
+      if (v1Distance >= 0.0f && v2Distance >= 0.0f)
+      {
+        out.emplace_back(in[v2Index]);
+      }
+
+      // v2 is the next v1
+      v1Index    = v2Index;
+      v1Distance = v2Distance;
+    }
+
+    return out;
+  }
+
+  std::vector<int32_t> reduceContacts(const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept
+  {
+    std::vector<int32_t> indicesToKeep;
+
+    // Use a map to temporarily store and track the contacts we want
+    std::unordered_map<int, const SHContact*> contactMap;
+
+    const int32_t NUM_CONTACTS = static_cast<int32_t>(in.size());
+    for (int32_t i = 0; i < NUM_CONTACTS; ++i)
+      contactMap.emplace(i, &in[i]);
+
+    // Find the furthest point in a given direction
+    int32_t indexToKeep = 0;
+    float bestDistance  = std::numeric_limits<float>::lowest();
+
+    for (const auto& [index, contact] : contactMap)
+    {
+      const float DIST = SHVec3::Dot(contact->localPointA, SHVec3::One);
+      if (DIST > bestDistance)
+      {
+        bestDistance  = DIST;
+        indexToKeep   = index;
+      }
+    }
+
+    indicesToKeep.emplace_back(indexToKeep);
+    contactMap.erase(indexToKeep);
+
+    indexToKeep   = 0;
+    bestDistance  = std::numeric_limits<float>::lowest();
+
+    // Find point furthest away from the first index
+    const SHVec3& FIRST_POS = in[indicesToKeep.back()].localPointA;
+    for (const auto& [index, contact] : contactMap)
+    {
+      const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->localPointA);
+      if (DIST_SQUARED > bestDistance)
+      {
+        bestDistance  = DIST_SQUARED;
+        indexToKeep   = index;
+      }
+    }
+
+    indicesToKeep.emplace_back(indexToKeep);
+    contactMap.erase(indexToKeep);
+
+    indexToKeep = 0;
+
+    // We compute the triangle with the largest area.
+    // The area can be positive or negative depending on the winding order.
+
+    float maxArea = std::numeric_limits<float>::lowest();
+    float minArea = std::numeric_limits<float>::max();
+
+    int32_t maxAreaIndex = -1;
+    int32_t minAreaIndex = -1;
+
+    const SHVec3& SECOND_POS = in[indicesToKeep.back()].localPointA;
+    for (const auto& [index, contact] : contactMap)
+    {
+      const SHVec3& POS   = contact->localPointA;
+      const SHVec3 TO_P1  = FIRST_POS - POS;
+      const SHVec3 TO_P2  = SECOND_POS - POS;
+
+      const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f;
+
+      if (AREA > maxArea)
+      {
+        maxArea = AREA;
+        maxAreaIndex = index;
+      }
+
+      if (AREA < minArea)
+      {
+        minArea = AREA;
+        minAreaIndex = index;
+      }
+    }
+
+    // Compare which triangle creates the largest area
+    bool isAreaPositive = false;
+    if (maxArea > std::fabs(minArea))
+    {
+      isAreaPositive  = true;
+      indexToKeep     = maxAreaIndex;
+    }
+    else
+    {
+      isAreaPositive  = false;
+      indexToKeep     = minAreaIndex;
+    }
+
+    indicesToKeep.emplace_back(indexToKeep);
+    contactMap.erase(indexToKeep);
+
+    indexToKeep = 0;
+
+    // For the last point, we want the point which forms the largest area that is winded opposite to the first triangle
+    // The areas should be inverted: If area was -ve, we want a +ve. Otherwise, vice versa.
+    float bestArea = 0.0f;
+
+    const SHVec3& THIRD_POS = in[indicesToKeep.back()].localPointA;
+    const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS };
+
+    for (const auto& [index, contact] : contactMap)
+    {
+      const SHVec3& Q = contact->localPointA;
+
+      for (int i = 0; i < 3; ++i)
+      {
+        const int P1 = i;
+        const int P2 = (i + 1) % 3;
+
+        const SHVec3 TO_P1 = ABC[P1] - Q;
+        const SHVec3 TO_P2 = ABC[P2] - Q;
+
+        const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f;
+
+        if (isAreaPositive && AREA < bestArea)
+        {
+          bestArea    = AREA;
+          indexToKeep = index;
+        }
+
+        if (!isAreaPositive && AREA > bestArea)
+        {
+          bestArea    = AREA;
+          indexToKeep = index;
+        }
+      }
+    }
+
+    indicesToKeep.emplace_back(indexToKeep);
+    return indicesToKeep;
+  }
+
+  bool SHCollision::cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
+  {
+    if (cachedInfo.type == SHSATInfo::Type::FACE)
+    {
+      SHASSERT(cachedInfo.info.refPoly != SHSATInfo::ShapeID::INVALID, "Attempted to perform cached SAT with an invalid cached collision!")
+
+      // Assume the reference poly was A
+      const SHConvexPolyhedron* incPoly = &B;
+      const SHConvexPolyhedron* refPoly = &A;
+      bool flip = false;
+
+      // Swap if it was B
+      if (cachedInfo.info.refPoly == SHSATInfo::ShapeID::B)
+      {
+        refPoly = &B;
+        incPoly = &A;
+        flip    = true;
+      }
+
+      const SHHalfEdgeStructure::Face&  REF_FACE    = refPoly->GetFace(cachedInfo.info.refFace);
+      const SHVec3                      REF_NORMAL  = refPoly->GetNormal(cachedInfo.info.refFace);
+
+      const SHVec3  SUPPORT_POINT = incPoly->FindSupportPoint(-REF_NORMAL);
+
+      const SHPlane REF_FACE_PLANE { refPoly->GetVertex(REF_FACE.vertexIndices[0].index), REF_NORMAL };
+      const float   DISTANCE = REF_FACE_PLANE.SignedDistance(SUPPORT_POINT);
+
+      if (DISTANCE > 0.0f)
+        return false;
+
+      // Find the incident face
+      const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL);
+
+      return findFaceContacts(manifold, *incPoly, INCIDENT_FACE_INDEX, *refPoly, cachedInfo.info.refFace, flip);
+    }
+
+    if (cachedInfo.type == SHSATInfo::Type::EDGE)
+    {
+      const float DISTANCE = distanceBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
+      if (DISTANCE > 0.0f)
+        return false;
+
+      const SHCollisionUtils::ShapeTransform TF_A { A.GetWorldCentroid(), A.GetWorldOrientation() };
+      const SHCollisionUtils::ShapeTransform TF_B { B.GetWorldCentroid(), B.GetWorldOrientation() };
+      const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse();
+      const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse();
+
+      const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA);
+      const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.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);
+
+      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
+      uint32_t numContacts = 0;
+
+      const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
+
+      SHContact contact;
+      // Take feature pair key from previous manifold
+      contact.featurePair.key = manifold.contacts[0].featurePair.key;
+      contact.penetration     = -DISTANCE;
+
+      contact.localPointA     = INV_TF_A * CONTACTS.closestPointB;
+      contact.localPointB     = INV_TF_B * CONTACTS.closestPointA;
+
+      manifold.contacts[numContacts++] = contact;
+      manifold.numContacts = numContacts;
+
+      return true;
+    }
+
+    // Should never reach this.
+    return false;
+  }
+} // namespace SHADE

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

@@ -0,0 +1,87 @@
+/****************************************************************************************
+ * \file      SHSATInfo.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for storing information of collision detection between two
+ *            convex shapes using SAT.
+ *
+ * \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 <numeric>    // int32_t
+
+namespace SHADE
+{
+  /**
+   * @brief
+   * Primarily used to cached collision information between two convex polyhedrons for
+   * temporal coherence.
+  */
+  struct SHSATInfo
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    enum class ShapeID : int32_t
+    {
+      A       = 0
+    , B       = 1
+
+    , INVALID = -1
+    };
+
+    enum class Type
+    {
+      EDGE
+    , FACE
+
+    , INVALID = -1
+    };
+
+    union ContactInfo
+    {
+      struct // FaceContact
+      {
+        ShapeID refPoly;
+        int32_t refFace;
+      };
+
+      struct // Edge Contact
+      {
+        int32_t edgeA;
+        int32_t edgeB;
+      };
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    Type        type;
+    ContactInfo info;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHSATInfo   ()                      noexcept;
+    SHSATInfo   (const SHSATInfo& rhs)  noexcept;
+    SHSATInfo   (SHSATInfo&& rhs)       noexcept;
+    ~SHSATInfo  () noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHSATInfo& operator=  (const SHSATInfo& rhs)  noexcept;
+    SHSATInfo& operator=  (SHSATInfo&& rhs)       noexcept;
+  };
+  
+} // namespace SHADE
+
+#include "SHSATInfo.hpp"

SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.hpp

@@ -0,0 +1,71 @@
+/****************************************************************************************
+ * \file      SHSATInfo.hpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation of inlined methods for cached SAT 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.
+****************************************************************************************/
+
+#pragma once
+
+// Primary Header
+#include "SHSATInfo.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor                                                         */
+  /*-----------------------------------------------------------------------------------*/
+
+  inline SHSATInfo::SHSATInfo() noexcept
+  : type  { Type::INVALID }
+  , info  {}
+  {
+    info.refPoly = ShapeID::INVALID;
+    info.refFace = -1;
+  }
+
+  inline SHSATInfo::SHSATInfo(const SHSATInfo& rhs) noexcept
+  : type  { rhs.type }
+  , info  {}
+  {
+    info.refPoly = rhs.info.refPoly;
+    info.refFace = rhs.info.refFace;
+    
+  }
+
+  inline SHSATInfo::SHSATInfo(SHSATInfo&& rhs) noexcept
+  : type  { rhs.type }
+  , info  {}
+  {
+    info.refPoly = rhs.info.refPoly;
+    info.refFace = rhs.info.refFace;
+  }
+
+  /*---------------------------------------------------------------------------------- */
+  /* Operator Overloads                                                                */
+  /*---------------------------------------------------------------------------------- */
+
+  inline SHSATInfo& SHSATInfo::operator=(const SHSATInfo& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    type          = rhs.type;
+    info.refPoly  = rhs.info.refPoly;
+    info.refFace  = rhs.info.refFace;
+
+    return *this;
+  }
+
+  inline SHSATInfo& SHSATInfo::operator=(SHSATInfo&& rhs) noexcept
+  {
+    type          = rhs.type;
+    info.refPoly  = rhs.info.refPoly;
+    info.refFace  = rhs.info.refFace;
+
+    return *this;
+  }
+}

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,370 @@
+/****************************************************************************************
+ * \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 "SHCollisionUtils.h"
+#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
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Local Functions Declarations                                                      */
+  /*-----------------------------------------------------------------------------------*/
+  
+  SHCollisionUtils::FaceQuery findClosestFace   (const SHSphere&, const SHConvexPolyhedron&) noexcept;
+  int32_t                     findClosestPoint  (const SHSphere&, const SHConvexPolyhedron&, int32_t faceIndex) noexcept;
+  int32_t                     findVoronoiRegion (const SHSphere&, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& t1, const SHVec3& t2) noexcept;
+
+
+  /*-----------------------------------------------------------------------------------*/
+  /* 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 SHCollisionUtils::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& POLY    = dynamic_cast<const SHConvexPolyhedron&>(B);
+
+    const SHVec3  CENTER  = SPHERE.Center;
+    const float   RADIUS  = SPHERE.GetWorldRadius();
+
+    const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLY);
+    if (!FACE_QUERY.colliding)
+      return false;
+
+    const SHCollisionUtils::ShapeTransform TF_A = { SPHERE.GetWorldCentroid(), SPHERE.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform TF_B = { POLY.GetWorldCentroid(), POLY.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse();
+    const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse();
+
+    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 = -POLY.GetNormal(FACE_QUERY.closestFace);
+
+      contact.penetration = PENETRATION;
+
+      // A is the sphere, B is the polyhedron
+      contact.localPointA = SHVec3::Rotate(manifold.normal * RADIUS, INV_TF_A.orientation);
+      contact.localPointB = (INV_TF_B * CENTER) + manifold.normal * (PENETRATION - RADIUS);
+
+      manifold.contacts[numContacts++] = contact;
+      manifold.numContacts = numContacts;
+
+      return true;
+    }
+
+    // Find closest face of polygon to circle
+    const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLY, FACE_QUERY.closestFace);
+
+    const auto&   FACE          = POLY.GetFace(FACE_QUERY.closestFace);
+    const SHVec3& FACE_NORMAL   = POLY.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 = POLY.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
+    const SHVec3 P2 = POLY.GetVertex(FACE.vertexIndices[P2_INDEX].index);
+    const SHVec3 P3 = POLY.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);
+
+        break;
+      }
+      case 3: // Region C
+      {
+        manifold.normal     = -SHVec3::Normalise(P1_TO_CENTER);
+        contact.penetration = RADIUS - P1_TO_CENTER.Length();
+
+        break;
+      }
+      case 4: // Region D
+      {
+        manifold.normal     = -FACE_NORMAL;
+        contact.penetration = PENETRATION;
+
+        break;
+      }
+      default: return false;  // Should never happen
+    }
+
+    contact.localPointA = SHVec3::Rotate(manifold.normal * RADIUS, INV_TF_A.orientation);
+    contact.localPointB = (INV_TF_B * CENTER) + manifold.normal * (RADIUS - contact.penetration);
+
+    manifold.contacts[numContacts++] = contact;
+    manifold.numContacts = numContacts;
+
+    return true;
+  }
+
+  bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    if (SphereVsConvex(manifold, B, A))
+    {
+      manifold.normal = -manifold.normal;
+
+      // There can only be one contact in this scenario, so we swap the first
+      std::swap(manifold.contacts[0].localPointA, manifold.contacts[0].localPointB);
+
+      return true;
+    };
+
+    return false;
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionUtils::FaceQuery findClosestFace
+  (
+    const SHSphere&           sphere
+  , const SHConvexPolyhedron& polyhedron
+  ) noexcept
+  {
+    SHCollisionUtils::FaceQuery faceQuery;
+
+    const SHVec3  CENTER  = sphere.GetWorldCentroid();
+    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 findClosestPoint
+  (
+    const SHSphere&           sphere
+  , const SHConvexPolyhedron& polyhedron
+  , int32_t                   faceIndex
+  ) noexcept
+  {
+    // Find closest point on face
+    int32_t closestPointIndex = -1;
+
+    const SHVec3  CENTER  = sphere.GetWorldCentroid();
+    
+    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 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.GetWorldCentroid();
+    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)
+    {
+      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,106 @@
+/****************************************************************************************
+ * \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 "SHCollisionUtils.h"
+#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;
+
+    const SHCollisionUtils::ShapeTransform TF_A = { SPHERE_A.GetWorldCentroid(), SPHERE_A.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform TF_B = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse();
+    const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse();
+
+    // Only populate the manifold if there is a collision
+
+    uint32_t numContacts = 0;
+
+    SHContact contact;
+    contact.featurePair.key = 0;
+    contact.penetration     = COMBINED_RADIUS - A_TO_B.Length();
+
+    // Degenerate case
+    if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f))
+    {
+      manifold.normal     = SHVec3::UnitY;
+      contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, INV_TF_A.orientation);
+      contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, INV_TF_B.orientation);
+
+      manifold.contacts[numContacts++] = contact;
+    }
+    else
+    {
+      manifold.normal     = SHVec3::Normalise(A_TO_B);
+      contact.localPointA = RADIUS_A * SHVec3::Normalise(INV_TF_A * CENTER_B);
+      contact.localPointB = RADIUS_B * SHVec3::Normalise(INV_TF_B * CENTER_A);
+      
+
+      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/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

@@ -1,7 +1,10 @@
 /****************************************************************************************
- * \file      SHPhysicsRaycaster.h
+ * \file      SHCollisionSpace.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a Physics Raycaster.
+ * \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
@@ -10,42 +13,39 @@
 
 #pragma once
 
-#include <vector>
-
-#include <reactphysics3d/reactphysics3d.h>
+#include <optional>
 
 // Project Headers
-#include "Math/SHRay.h"
-#include "Physics/Interface/PhysicsObject/SHPhysicsObjectManager.h"
-#include "Physics/Collision/CollisionTags/SHCollisionTagMatrix.h"
-#include "Physics/Collision/SHPhysicsRaycastResult.h"
+#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
 {
-  /*-----------------------------------------------------------------------------------*/
-  /* Forward Declarations                                                              */
-  /*-----------------------------------------------------------------------------------*/
-
-  class SHPhysicsSystem;
-
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  class SH_API SHRaycaster : public reactphysics3d::RaycastCallback
+  /**
+   * @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
   {
-  private:
-    /*---------------------------------------------------------------------------------*/
-    /* Type Definitions                                                                */
-    /*---------------------------------------------------------------------------------*/
-
-    using RaycastResults = std::vector<SHPhysicsRaycastResult>;
-
   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:
@@ -53,7 +53,7 @@ namespace SHADE
       /* Friends                                                                       */
       /*-------------------------------------------------------------------------------*/
 
-      friend class SHRaycaster;
+      friend class SHCollisionSpace;
 
     public:
       /*-------------------------------------------------------------------------------*/
@@ -86,24 +86,66 @@ namespace SHADE
       std::optional<EntityID> colliderEntityID;
     };
 
+    using RaycastResults = std::vector<SHPhysicsRaycastResult>;
+
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHRaycaster() noexcept;
+    SHCollisionSpace  () noexcept = default;
+    ~SHCollisionSpace () noexcept = default;
 
     /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
+    /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    void BindToSystem (SHPhysicsSystem* physicsSystem)    noexcept;
-    void BindToWorld  (rp3d::PhysicsWorld* physicsWorld)  noexcept;
+    const SHAABBTree::AABBs&  GetBroadphaseAABBs  ()  const noexcept;
 
-    rp3d::decimal notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) override;
+    /*---------------------------------------------------------------------------------*/
+    /* Setter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void SetContactManager(SHContactManager* contactManager) noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
 
     /**
      * @brief
-     * Casts a ray into the physics world.
+     * 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
@@ -113,21 +155,42 @@ namespace SHADE
     [[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                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    SHPhysicsSystem*            system;
-    rp3d::PhysicsWorld*         world;  
-    RaycastResults              results;      // Holds the temporary result after casting into the world
-    std::optional<RaycastInfo>  currentInfo;  // Hold the state of the current raycast.
+    SHContactManager* contactManager = nullptr;
+
+    Colliders         colliders;
+    NarrowphaseBatch  narrowphaseBatch;
+
+    SHAABBTree        broadphase;
 
     /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
+    /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    static int findColliderIndex (const rp3d::CollisionBody* rp3dBody, rp3d::Entity rp3dColliderEID) noexcept;
+    // 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/Shapes/SHBox.cpp

@@ -10,14 +10,13 @@
 
 #include <SHpch.h>
 
-#include <reactphysics3d/reactphysics3d.h>
-
 // Primary Header
 #include "SHBox.h"
 
 // Project Headers
+#include "Math/SHMathHelpers.h"
 #include "Math/SHMatrix.h"
-#include "Physics/Interface/SHColliderComponent.h"
+#include "Physics/Collision/SHCollider.h"
 
 namespace SHADE
 {
@@ -25,29 +24,88 @@ namespace SHADE
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/
 
-  SHBox::SHBox() noexcept
-  : SHCollisionShape  (Type::BOX)
+  SHBox::SHBox(SHCollisionShapeID id) noexcept
+  : SHConvexPolyhedron  (id, Type::BOX)
   , relativeExtents     { SHVec3::One }
   , scale               { SHVec3::One }
   {
-    if (rp3dCollider)
-      dynamic_cast<rp3d::BoxShape*>(rp3dCollider->getCollisionShape())->setHalfExtents(SHVec3::One * 0.5f);
+    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
   {
-    if (rp3dCollider)
-      return SHVec3{ dynamic_cast<rp3d::BoxShape*>(rp3dCollider->getCollisionShape())->getHalfExtents() };
-
-    return relativeExtents * scale * 0.5f;
+    return Extents;
   }
 
   SHVec3 SHBox::GetRelativeExtents() const noexcept
@@ -55,11 +113,87 @@ namespace SHADE
     return relativeExtents;
   }
 
+  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
   {
-    const SHQuaternion  ROTATION  = collider->GetTransform().orientation * SHQuaternion::FromEuler(rotationOffset);
-    const SHMatrix      TRS       = SHMatrix::Rotate(ROTATION) * SHMatrix::Translate(collider->GetTransform().position);
-    return SHVec3::Transform(positionOffset, TRS);
+    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;
   }
 
   /*-----------------------------------------------------------------------------------*/
@@ -68,11 +202,14 @@ namespace SHADE
 
   void SHBox::SetWorldExtents(const SHVec3& newWorldExtents) noexcept
   {
-    if (rp3dCollider)
-      dynamic_cast<rp3d::BoxShape*>(rp3dCollider->getCollisionShape())->setHalfExtents(newWorldExtents);
+    Extents = newWorldExtents;
 
     // Recompute Relative radius
-    relativeExtents = 2.0f * newWorldExtents / scale;
+    relativeExtents = 2.0f * Extents / scale;
+
+    // Hack: Indicate that the collider needs to update the broadphase proxy
+    if (collider)
+      collider->SetScale(collider->GetScale());
   }
 
   void SHBox::SetRelativeExtents(const SHVec3& newRelativeExtents) noexcept
@@ -80,8 +217,11 @@ namespace SHADE
     relativeExtents = newRelativeExtents;
 
     // Recompute world radius
-    if (rp3dCollider)
-      dynamic_cast<rp3d::BoxShape*>(rp3dCollider->getCollisionShape())->setHalfExtents(relativeExtents * scale * 0.5f);
+    Extents = relativeExtents * scale * 0.5f;
+
+    // Hack: Indicate that the collider needs to update the broadphase proxy
+    if (collider)
+      collider->SetScale(collider->GetScale());
   }
 
   void SHBox::SetScale(const SHVec3& newScale) noexcept
@@ -89,8 +229,11 @@ namespace SHADE
     scale = SHVec3::Abs(newScale);
 
     // Recompute world radius
-    if (rp3dCollider)
-      dynamic_cast<rp3d::BoxShape*>(rp3dCollider->getCollisionShape())->setHalfExtents(relativeExtents * scale * 0.5f);
+    Extents = relativeExtents * scale * 0.5f;
+
+    // Hack: Indicate that the collider needs to update the broadphase proxy
+    if (collider)
+      collider->SetScale(collider->GetScale());
   }
 
   /*-----------------------------------------------------------------------------------*/
@@ -100,21 +243,89 @@ namespace SHADE
   void SHBox::Update() noexcept
   {
     const SHTransform& PARENT_TRANSFORM = collider->GetTransform();
+
     SetScale(PARENT_TRANSFORM.scale);
 
-    SHCollisionShape::Update();
+    // 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->GetTransform().orientation * SHQuaternion::FromEuler(rotationOffset);
-    const SHVec3        SCALE     = GetWorldExtents() * 2.0f;
+    const SHQuaternion  ROTATION  = collider ? collider->GetTransform().orientation * transform.orientation : Orientation;
+    const SHVec3        SCALE     = SHVec3{ Extents } *2.0f;
 
-    const SHMatrix      TRS       = SHMatrix::Rotate(ROTATION) * SHMatrix::Translate(collider->GetTransform().position);
-    const SHVec3        POSITION  = SHVec3::Transform(positionOffset, TRS);
-
-    return SHMatrix::Transform(POSITION, ROTATION, SCALE);
+    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 };
+  }
+
+  SHVec3 SHBox::FindSupportPoint(const SHVec3& direction) const noexcept
+  {
+    float bestDistance = std::numeric_limits<float>::lowest();
+    
+
+    SHVec3 vertices[NUM_VERTICES];
+    GetCorners(vertices);
+
+    // No reason to put the center really..
+    SHVec3 bestPoint = Center;
+    for (auto& vertex : vertices)
+    {
+      const float PROJECTION = SHVec3::Dot(vertex, direction);
+
+      if (PROJECTION > bestDistance)
+      {
+        bestDistance = PROJECTION;
+        bestPoint = vertex;
+      }
+    }
+
+    return bestPoint;
+  }
+
+
 } // namespace SHADE

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

@@ -10,8 +10,10 @@
 
 #pragma once
 
+#include <DirectXCollision.h>
+
 // Project Headers
-#include "SHCollisionShape.h"
+#include "SHConvexPolyhedron.h"
 
 namespace SHADE
 {
@@ -19,31 +21,61 @@ 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 SHCollisionShape
+  class SH_API SHBox final  : public SHConvexPolyhedron
+                            , private DirectX::BoundingOrientedBox
   {
   private:
     /*---------------------------------------------------------------------------------*/
     /* Friends                                                                         */
     /*---------------------------------------------------------------------------------*/
 
-    friend class SHColliderComponent;
+    friend class SHCollider;
+    friend class SHCompositeCollider;
+    friend class SHCollision;
+    friend class SHCollisionShapeLibrary;
 
   public:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    static constexpr int NUM_VERTICES = 8;
+
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHBox  () noexcept;
+    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                                                                */
     /*---------------------------------------------------------------------------------*/
@@ -51,7 +83,19 @@ namespace SHADE
     [[nodiscard]] SHVec3        GetWorldExtents         ()              const noexcept;
     [[nodiscard]] SHVec3        GetRelativeExtents      ()              const noexcept;
 
+    // Overriden Methods
+
+    [[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                                                                */
@@ -66,7 +110,11 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     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;
+    [[nodiscard]] SHVec3          FindSupportPoint  (const SHVec3& direction) const noexcept override;
 
   private:
     /*---------------------------------------------------------------------------------*/
@@ -76,4 +124,6 @@ namespace SHADE
     SHVec3  relativeExtents;
     SHVec3  scale;
   };
+
+
 } // namespace SHADE

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

@@ -10,14 +10,11 @@
 
 #include <SHpch.h>
 
-
-#include <reactphysics3d/reactphysics3d.h>
-
 // Primary Header
 #include "SHCollisionShape.h"
 
 // Project Headers
-#include "Physics/Interface/SHColliderComponent.h"
+#include "Physics/Collision/SHCollider.h"
 #include "Physics/Collision/CollisionTags/SHCollisionTagMatrix.h"
 #include "Reflection/SHReflectionMetadata.h"
 #include "Tools/Utilities/SHUtilities.h"
@@ -28,11 +25,11 @@ namespace SHADE
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/
 
-  SHCollisionShape::SHCollisionShape(Type colliderType)
-  : rp3dCollider    { nullptr }
+  SHCollisionShape::SHCollisionShape(SHCollisionShapeID id, Type colliderType)
+  : id              { id }
+  , flags           { 0 }
   , collider        { nullptr }
   , collisionTag    { SHCollisionTagMatrix::GetTag(0) }
-  , flags           { 0 }
   {
     flags |= 1U << SHUtilities::ConvertEnum(colliderType);
   }
@@ -41,6 +38,16 @@ namespace SHADE
   /* 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();
@@ -63,7 +70,7 @@ namespace SHADE
 
   const SHVec3& SHCollisionShape::GetPositionOffset() const noexcept
   {
-    return positionOffset;
+    return transform.position;
   }
 
   const SHVec3& SHCollisionShape::GetRotationOffset() const noexcept
@@ -85,14 +92,6 @@ namespace SHADE
   }
 
   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;
@@ -105,12 +104,6 @@ namespace SHADE
     return *collisionTag;
   }
 
-  SHVec3 SHCollisionShape::GetWorldCentroid() const noexcept
-  {
-    return collider->GetTransform().position;
-  }
-
-
   /*-----------------------------------------------------------------------------------*/
   /* Setter Function Definitions                                                       */
   /*-----------------------------------------------------------------------------------*/
@@ -118,114 +111,49 @@ namespace SHADE
   void SHCollisionShape::SetCollisionTag(SHCollisionTag* newCollisionTag) noexcept
   {
     collisionTag = newCollisionTag;
-
-    if (rp3dCollider)
-    {
-      rp3dCollider->setCollisionCategoryBits(collisionTag->GetMask());
-      rp3dCollider->setCollideWithMaskBits(collisionTag->GetMask());
-    }
   }
 
   void SHCollisionShape::SetFriction(float friction) noexcept
   {
     material.SetFriction(friction);
-
-    if (rp3dCollider)
-      rp3dCollider->getMaterial().setFrictionCoefficient(material.GetFriction());
   }
 
   void SHCollisionShape::SetBounciness(float bounciness) noexcept
   {
     material.SetBounciness(bounciness);
-
-    if (rp3dCollider)
-      rp3dCollider->getMaterial().setBounciness(material.GetBounciness());
   }
 
   void SHCollisionShape::SetDensity(float density) noexcept
   {
     material.SetDensity(density);
-
-    if (rp3dCollider)
-    {
-      rp3dCollider->getMaterial().setMassDensity(material.GetDensity());
-
-      // Recompute the bodies' mass
-      dynamic_cast<rp3d::RigidBody*>(collider->collisionBody)->updateMassPropertiesFromColliders();
-    }
   }
 
   void SHCollisionShape::SetMaterial(const SHPhysicsMaterial& newMaterial) noexcept
   {
     material = newMaterial;
-
-    if (rp3dCollider)
-    {
-      auto& rp3dMaterial = rp3dCollider->getMaterial();
-      rp3dMaterial.setFrictionCoefficient(material.GetFriction());
-      rp3dMaterial.setBounciness(material.GetBounciness());
-      rp3dMaterial.setMassDensity(material.GetDensity());
-
-      // Recompute the bodies' mass
-      dynamic_cast<rp3d::RigidBody*>(collider->collisionBody)->updateMassPropertiesFromColliders();
-    }
   }
 
   void SHCollisionShape::SetPositionOffset(const SHVec3& posOffset) noexcept
   {
-    positionOffset = posOffset;
+    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 int FLAG_POS = 4;
     static constexpr uint8_t FLAG_VALUE = 1U << FLAG_POS;
 
     isTrigger ? flags |= FLAG_VALUE : flags &= ~FLAG_VALUE;
-
-    if (rp3dCollider)
-    {
-      rp3dCollider->setIsTrigger(isTrigger);
-
-      // Recompute the bodies' mass
-      dynamic_cast<rp3d::RigidBody*>(collider->collisionBody)->updateMassPropertiesFromColliders();
-    }
-      
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Public Member Function Definitions                                                */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHCollisionShape::UpdateCollisionTags() noexcept
-  {
-    if (rp3dCollider && collisionTag->IsDirty())
-    {
-      rp3dCollider->setCollisionCategoryBits(collisionTag->GetMask());
-      rp3dCollider->setCollideWithMaskBits(collisionTag->GetMask());
-    }
-  }
-
-
-  void SHCollisionShape::Update() noexcept
-  {
-    if (rp3dCollider)
-    {
-      const rp3d::Transform OFFSETS{ positionOffset, SHQuaternion::FromEuler(rotationOffset) };
-      rp3dCollider->setLocalToBodyTransform(OFFSETS);
-    }
-  }
-
-  SHMatrix SHCollisionShape::GetTRS() const noexcept
-  {
-    return SHMatrix::Identity;
   }
 
 } // namespace SHADE

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

@@ -11,12 +11,13 @@
 #pragma once
 
 #include <rttr/registration>
-#include <reactphysics3d/collision/Collider.h>
 
 // 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"
 
@@ -26,7 +27,7 @@ namespace SHADE
   /* Forward Declarations                                                              */
   /*-----------------------------------------------------------------------------------*/
 
-  class SHColliderComponent;
+  class SHRigidBody;
 
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
@@ -40,9 +41,12 @@ namespace SHADE
     /* Friends                                                                         */
     /*---------------------------------------------------------------------------------*/
 
-    friend class  SHPhysicsSystem;
-    friend class  SHPhysicsObjectManager;
+    friend class  SHCollider;
+    friend class  SHCompositeCollider;
     friend class  SHColliderComponent;
+    friend class  SHCollisionShapeLibrary;
+    friend class  SHCollisionSpace;
+    friend struct SHManifold;
 
   public:
     /*---------------------------------------------------------------------------------*/
@@ -53,6 +57,8 @@ namespace SHADE
     {
       SPHERE
     , BOX
+    , CAPSULE
+    , CONVEX_HULL
 
     , COUNT
     , INVALID = -1
@@ -62,7 +68,7 @@ namespace SHADE
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHCollisionShape(Type colliderType = Type::INVALID);
+    SHCollisionShape          (SHCollisionShapeID id, Type colliderType = Type::SPHERE);
 
     SHCollisionShape          (const SHCollisionShape& rhs) noexcept = default;
     SHCollisionShape          (SHCollisionShape&& rhs)      noexcept = default;
@@ -79,6 +85,9 @@ namespace SHADE
     /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
+    [[nodiscard]] EntityID                  GetEntityID             ()            const noexcept;
+    [[nodiscard]] uint32_t                  GetIndex                ()            const noexcept;
+
     // Material Properties
     // TODO: Remove individual setters once instanced materials are supported
 
@@ -96,11 +105,19 @@ namespace SHADE
 
     [[nodiscard]] Type                      GetType                 ()            const noexcept;
     [[nodiscard]] bool                      IsTrigger               ()            const noexcept;
-    [[nodiscard]] bool                      IsColliding()           const noexcept;
 
     [[nodiscard]] const SHCollisionTag&     GetCollisionTag         ()            const noexcept;
 
-    [[nodiscard]] virtual SHVec3            GetWorldCentroid()      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                                                                */
@@ -116,6 +133,8 @@ namespace SHADE
     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;
 
@@ -125,15 +144,29 @@ namespace SHADE
 
     /**
      * @brief
-     * Re-sets the collision tags if it is dirty. 
+     * Computes the transform of the shape.
     */
-    void UpdateCollisionTags() noexcept;
+    virtual void Update () noexcept = 0;
 
     /**
      * @brief
-     * Computes the transform of the shape.
+     * 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.
      */
-    virtual void Update() noexcept;
+    [[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
@@ -141,24 +174,36 @@ namespace SHADE
      * @return
      * The model-to-world matrix for rendering the shape.
      */
-    [[nodiscard]] virtual SHMatrix GetTRS() const noexcept;
+    [[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                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    reactphysics3d::Collider* rp3dCollider;
-    SHColliderComponent*      collider;     // The collider it belongs to.
+    SHCollisionShapeID  id;
+
+    SHCollider*         collider;     // The collider it belongs to.
     SHCollisionTag*     collisionTag;
     SHPhysicsMaterial   material;     // TODO: Change to pointer once instancing is supported
 
-    SHVec3                    positionOffset;
+    SHTransform         transform;    // Stores the local position and rotation.
+
+    // Needed for conversion to euler angles
     SHVec3              rotationOffset;
 
-    uint8_t                   flags;        // 0 0 0 trigger 0 capsule sphere box
+    uint8_t             flags;        // 0 0 0 trigger convexHull capsule box sphere
     
 
     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 "Physics/Interface/SHRigidBodyComponent.h"
-
+#include "ECS_Base/Entity/SHEntity.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;
-    SHCollisionInfo   (EntityID entityA, EntityID entityB)  noexcept;
+    SHCollisionShapeID  (EntityID eid, uint32_t shapeID)  noexcept;
+    SHCollisionShapeID  (const SHCollisionShapeID& rhs)   noexcept;
+    SHCollisionShapeID  (SHCollisionShapeID&& rhs)        noexcept;
 
-
-    SHCollisionInfo   (const SHCollisionInfo& rhs)  = default;
-    SHCollisionInfo   (SHCollisionInfo&& rhs)       = default;
-    ~SHCollisionInfo  ()                             = default;
+    ~SHCollisionShapeID () noexcept = default;
 
     /*---------------------------------------------------------------------------------*/
     /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
 
-    bool operator== (const SHCollisionInfo& rhs)      const noexcept;
-    bool operator!= (const SHCollisionInfo& rhs)      const noexcept;
+    SHCollisionShapeID& operator=(const SHCollisionShapeID& rhs)  noexcept;
+    SHCollisionShapeID& operator=(SHCollisionShapeID&& rhs)       noexcept;
 
-    SHCollisionInfo& operator= (const SHCollisionInfo& rhs) = default;
-    SHCollisionInfo& operator= (SHCollisionInfo&& rhs)      = default;
+    bool operator==(const SHCollisionShapeID& rhs) const;
 
     /*---------------------------------------------------------------------------------*/
     /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    [[nodiscard]] EntityID                    GetEntityA        ()  const noexcept;
-    [[nodiscard]] EntityID                    GetEntityB        ()  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;
+    [[nodiscard]] EntityID  GetEntityID   ()  const noexcept;
+    [[nodiscard]] uint32_t  GetShapeIndex ()  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[2];  // EntityValue, ColliderIndexValue
-      uint32_t ids  [4];  // EntityA, EntityB, ColliderIndexA, ColliderIndexB
+    uint64_t  value;
+    IDs       ids;
   };
 
-    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,180 @@
+/****************************************************************************************
+ * \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:
+      {
+        SHBox* box = boxes.find(shape->id)->second;
+        boxes.erase(shape->id);
+
+        delete box;
+        box = nullptr;
+
+        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 (1,5,6,2) Normal:  X
+     * Back:    2 (5,4,7,6) Normal:  Z
+     * Left:    3 (4,0,3,7) Normal: -X
+     * Bottom:  4 (0,4,5,1) Normal: -Y
+     * Top:     5 (2,6,7,3) 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 }
+    , { 1, 5, 6, 2 }
+    , { 4, 7, 6, 5 }
+    , { 4, 0, 3, 7 }
+    , { 0, 4, 5, 1 }
+    , { 2, 6, 7, 3 }
+    };
+
+    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

@@ -0,0 +1,108 @@
+/****************************************************************************************
+ * \file      SHCollisionShapeLibrary.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \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
+ *            of DigiPen Institute of Technology is prohibited.
+****************************************************************************************/
+
+#pragma once
+
+#include <unordered_map>
+
+// Project Header
+#include "SHSphere.h"
+#include "SHBox.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @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                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionShapeLibrary   () noexcept;
+    ~SHCollisionShapeLibrary  () noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Function Members                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Creates a sphere collision shape.
+     * @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);
+
+    /**
+     * @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                                                                    */
+    /*---------------------------------------------------------------------------------*/
+    
+    SHHalfEdgeStructure   boxHalfEdgeStructure;
+
+    Spheres               spheres;
+    Boxes                 boxes;
+    // TODO: Add capsules and hulls
+
+    /*---------------------------------------------------------------------------------*/
+    /* Function Members                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void createBoxPolyhedron() noexcept;
+  };
+
+} // namespace SHADE

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

@@ -0,0 +1,96 @@
+/****************************************************************************************
+ * \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);
+  }
+
+
+} // namespace SHADE

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

@@ -0,0 +1,93 @@
+/****************************************************************************************
+ * \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 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]] virtual SHVec3 FindSupportPoint  (const SHVec3& direction) const noexcept = 0;
+
+  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

@@ -10,15 +10,13 @@
 
 #include <SHpch.h>
 
-#include <reactphysics3d/reactphysics3d.h>
-
 // Primary Header
 #include "SHSphere.h"
 
 // Project Headers
 #include "Math/SHMathHelpers.h"
 #include "Math/SHMatrix.h"
-#include "Physics/Interface/SHColliderComponent.h"
+#include "Physics/Collision/SHCollider.h"
 
 namespace SHADE
 {
@@ -26,29 +24,80 @@ namespace SHADE
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/
 
-  SHSphere::SHSphere() noexcept
-  : SHCollisionShape  (Type::SPHERE)
+  SHSphere::SHSphere(SHCollisionShapeID id) noexcept
+  : SHCollisionShape  (id, Type::SPHERE)
   , relativeRadius    { 1.0f }
   , scale             { 1.0f }
   {
-    if (rp3dCollider)
-      dynamic_cast<rp3d::SphereShape*>(rp3dCollider->getCollisionShape())->setRadius(0.5f);
+    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
   {
-    if (rp3dCollider)
-      return dynamic_cast<rp3d::SphereShape*>(rp3dCollider->getCollisionShape())->getRadius();
-
-    return relativeRadius * scale * 0.5f;
+    return Radius;
   }
 
   float SHSphere::GetRelativeRadius() const noexcept
@@ -58,9 +107,54 @@ namespace SHADE
 
   SHVec3 SHSphere::GetWorldCentroid() const noexcept
   {
-    const SHQuaternion  ROTATION  = collider->GetTransform().orientation * SHQuaternion::FromEuler(rotationOffset);
-    const SHMatrix      TRS       = SHMatrix::Rotate(ROTATION) * SHMatrix::Translate(collider->GetTransform().position);
-    return SHVec3::Transform(positionOffset, TRS);
+    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;
   }
 
   /*-----------------------------------------------------------------------------------*/
@@ -69,11 +163,14 @@ namespace SHADE
 
   void SHSphere::SetWorldRadius(float newWorldRadius) noexcept
   {
-    if (rp3dCollider)
-      dynamic_cast<rp3d::SphereShape*>(rp3dCollider->getCollisionShape())->setRadius(newWorldRadius);
+    Radius = newWorldRadius;
 
     // Recompute Relative radius
-    relativeRadius = 2.0f * newWorldRadius / scale;
+    relativeRadius = 2.0f * Radius / scale;
+
+    // Hack: Indicate that the collider needs to update the broadphase proxy
+    if (collider)
+      collider->SetScale(collider->GetScale());
   }
 
   void SHSphere::SetRelativeRadius(float newRelativeRadius) noexcept
@@ -81,8 +178,11 @@ namespace SHADE
     relativeRadius = newRelativeRadius;
 
     // Recompute world radius
-    if (rp3dCollider)
-      dynamic_cast<rp3d::SphereShape*>(rp3dCollider->getCollisionShape())->setRadius(relativeRadius * scale * 0.5f);
+    Radius = relativeRadius * scale * 0.5f;
+
+    // Hack: Indicate that the collider needs to update the broadphase proxy
+    if (collider)
+      collider->SetScale(collider->GetScale());
   }
 
   void SHSphere::SetScale(float maxScale) noexcept
@@ -90,8 +190,11 @@ namespace SHADE
     scale = std::fabs(maxScale);
 
     // Recompute world radius
-    if (rp3dCollider)
-      dynamic_cast<rp3d::SphereShape*>(rp3dCollider->getCollisionShape())->setRadius(relativeRadius * scale * 0.5f);
+    Radius = relativeRadius * scale * 0.5f;
+
+    // Hack: Indicate that the collider needs to update the broadphase proxy
+    if (collider)
+      collider->SetScale(collider->GetScale());
   }
 
   /*-----------------------------------------------------------------------------------*/
@@ -105,18 +208,45 @@ namespace SHADE
     const float SPHERE_SCALE = std::fabs(SHMath::Max({ PARENT_TRANSFORM.scale.x, PARENT_TRANSFORM.scale.y, PARENT_TRANSFORM.scale.z }));
     SetScale(SPHERE_SCALE);
 
-    SHCollisionShape::Update();
+    // 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->GetTransform().orientation * SHQuaternion::FromEuler(rotationOffset);
-    const SHVec3        SCALE     = GetWorldRadius();
+    const SHQuaternion  ROTATION  = collider ? collider->GetTransform().orientation * transform.orientation : transform.orientation;
+    const SHVec3        SCALE     { Radius };
 
-    const SHMatrix      TRS       = SHMatrix::Rotate(ROTATION) * SHMatrix::Translate(collider->GetTransform().position);
-    const SHVec3        POSITION  = SHVec3::Transform(positionOffset, TRS);
-
-    return SHMatrix::Transform(POSITION, ROTATION, SCALE);
+    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

@@ -10,6 +10,8 @@
 
 #pragma once
 
+#include <DirectXCollision.h>
+
 // Project Headers
 #include "SHCollisionShape.h"
 
@@ -19,31 +21,54 @@ namespace SHADE
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
+  /**
+   * @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 SHColliderComponent;
+    friend class SHCollider;
+    friend class SHCompositeCollider;
+    friend class SHCollision;
+    friend class SHCollisionShapeLibrary;
 
   public:
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHSphere  ()  noexcept;
+    SHSphere  (SHCollisionShapeID id)             noexcept;
+    SHSphere  (const SHSphere& rhs) noexcept;
+    SHSphere  (SHSphere&& rhs)      noexcept;
+
     ~SHSphere () override = default;
 
     /*---------------------------------------------------------------------------------*/
     /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
 
+    SHSphere& operator=  (const SHSphere& rhs)  noexcept;
+    SHSphere& operator=  (SHSphere&& rhs)       noexcept;
+
     /*---------------------------------------------------------------------------------*/
     /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
@@ -52,6 +77,13 @@ namespace SHADE
     [[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                                                                */
@@ -66,7 +98,10 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     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:
     /*---------------------------------------------------------------------------------*/
@@ -76,4 +111,5 @@ namespace SHADE
     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;
+    SHMatrix  invInertiaA;
+    SHMatrix  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,63 @@
+/****************************************************************************************
+ * \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    { SHVec3::Zero }
+    , AngularVelocity   { SHVec3::Zero }
+    , LinearLockFactor  { SHVec3::Zero }
+    , AngularLockFactor { SHVec3::Zero }
+    {
+      if (rigidBody)
+      {
+        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,308 @@
+/****************************************************************************************
+ * \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"
+#include "Physics/Collision/Narrowphase/SHCollisionUtils.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
+      };
+
+      const auto* SHAPE_A = manifold.shapeA;
+      const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
+
+      for (uint32_t i = 0; i < manifold.numContacts; ++i)
+        collisionEvent.contactPoints[i] = TF_A * manifold.contacts[i].localPointA;
+
+      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;
+
+      const auto* SHAPE_A = manifold.shapeA;
+      const auto* SHAPE_B = manifold.shapeB;
+
+      const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
+
+      for (uint32_t i = 0; i < manifold.numContacts; ++i)
+      {
+        // Contact position will be world pont
+        const SHVec3 WORLD_POINT_A = TF_A * manifold.contacts[i].localPointA;
+
+        const ContactInfo INFO
+        {
+          .position = WORLD_POINT_A
+        , .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&  trigger = triggerPair->second;
+
+      const bool IS_COLLIDING = SHCollisionDispatcher::Collide(*trigger.A, *trigger.B);
+
+      auto& collisionState = trigger.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;
+
+      if (bodyA)
+      {
+        bodyA->SetLinearVelocity(STATE_A.LinearVelocity);
+        bodyA->SetAngularVelocity(STATE_A.AngularVelocity);
+      }
+
+      if (bodyB)
+      {
+        bodyB->SetLinearVelocity(STATE_B.LinearVelocity);
+        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_THIRD = 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_THIRD)
+      tangent0 = SHVec3{ NORMAL.y, -NORMAL.x, 0.0f };
+    else
+      tangent0 = SHVec3{ 0.0f, NORMAL.z, -NORMAL.y };
+
+    tangent0 = SHVec3::Normalise(tangent0);
+    tangent1 = SHVec3::Cross(NORMAL, tangent0);
+
+    // 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];
+
+        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,338 @@
+/****************************************************************************************
+ * \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"
+#include "Physics/Collision/Narrowphase/SHCollisionUtils.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
+
+    // Account for implicity-static bodies
+    if (BODY_A)
+    {
+      newConstraint.invMassA      = BODY_A->invMass;
+      newConstraint.centerOfMassA = BODY_A->worldCentroid;
+      newConstraint.invInertiaA   = BODY_A->worldInvInertia;
+    }
+    else
+    {
+      newConstraint.invMassA      = 0.0f;
+      newConstraint.centerOfMassA = SHVec3::Zero;
+
+      // Matrix needs to maintain its homogenous structure
+      newConstraint.invInertiaA = SHMatrix::Zero;
+      newConstraint.invInertiaA.m[3][3] = 1.0f; 
+    }
+
+    if (BODY_B)
+    {
+      newConstraint.invMassB      = BODY_B->invMass;
+      newConstraint.centerOfMassB = BODY_B->worldCentroid;
+      newConstraint.invInertiaB   = BODY_B->worldInvInertia;
+    }
+    else
+    {
+      newConstraint.invMassB      = 0.0f;
+      newConstraint.centerOfMassB = SHVec3::Zero;
+
+      // Matrix needs to maintain its homogenous structure
+      newConstraint.invInertiaB = SHMatrix::Zero;
+      newConstraint.invInertiaB.m[3][3] = 1.0f;
+    }
+
+    // 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);
+
+    const SHCollisionUtils::ShapeTransform TF_A{ SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform TF_B{ SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() };
+
+    for (uint32_t i = 0; i < newConstraint.numContacts; ++i)
+    {
+      newConstraint.contacts[i].rA = (TF_A * newConstraint.contacts[i].localPointA) - newConstraint.centerOfMassA;
+      newConstraint.contacts[i].rB = (TF_B * newConstraint.contacts[i].localPointB) - 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
+         * | ix * x |[ u v w ]
+         * | iy * y |          = [ ix * x * w + 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(contact.rA, constraint.tangents[j]);
+          const SHVec3 RB_CROSS_T = SHVec3::Cross(contact.rB, constraint.tangents[j]);
+
+          contact.tangentMass[j] = INV_MASS_SUM;
+          contact.tangentMass[j] += SHVec3::Dot(RA_CROSS_T, constraint.invInertiaA * RA_CROSS_T);
+          contact.tangentMass[j] += SHVec3::Dot(RB_CROSS_T, constraint.invInertiaB * 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)
+         */
+
+        float errorBias = 0.0f;
+        if (std::fabs(contact.penetration) > SHPHYSICS_LINEAR_SLOP)
+          errorBias = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(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 = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f;
+
+        contact.bias = errorBias + 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;
+
+        // 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;
+
+        // Solve normal impulse
+        // Re-compute relative velocity
+        velocityA = vA + SHVec3::Cross(wA, contact.rA);
+        velocityB = vB + SHVec3::Cross(wB, contact.rB);
+        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;
+        }
+      }
+
+      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      SHCollisionListener.h
+ * \file      SHContactSolver.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
- * \brief     Interface for a Collision Listener.
+ * \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
@@ -10,71 +11,83 @@
 
 #pragma once
 
-// External Dependencies
-#include <reactphysics3d/reactphysics3d.h>
-
 // Project Headers
-#include "SH_API.h"
-#include "Physics/Collision/SHCollisionInfo.h"
+#include "Constraints/SHContactConstraint.h"
+#include "Constraints/SHVelocityState.h"
 
 namespace SHADE
 {
-  /*-----------------------------------------------------------------------------------*/
-  /* Forward Declarations                                                              */
-  /*-----------------------------------------------------------------------------------*/
-
-  class SHPhysicsSystem;
-
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
-  class SH_API SHCollisionListener final : public rp3d::EventListener
+  /**
+   * @brief
+   * Encapsulates an object that builds contact constraints and solves them.
+  */
+  class SH_API SHContactSolver
   {
   public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    using VelocityStates      = std::unordered_map<EntityID, SHVelocityState>; 
+    using ContactConstraints  = std::unordered_map<SHCollisionKey, SHContactConstraint, SHCollisionKeyHash>;
+
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
 
-    SHCollisionListener() noexcept;
+    SHContactSolver   () noexcept = default;
+    ~SHContactSolver  () noexcept = default;
 
     /*---------------------------------------------------------------------------------*/
     /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    [[nodiscard]] const std::vector<SHCollisionInfo>& GetCollisionInfoContainer () const noexcept;
-    [[nodiscard]] const std::vector<SHCollisionInfo>& GetTriggerInfoContainer   () const noexcept;
+    [[nodiscard]] const VelocityStates&     GetVelocities         () const noexcept;
+    [[nodiscard]] const ContactConstraints& GetContantConstraints () const noexcept;
 
     /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
+    /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    void  BindToSystem    (SHPhysicsSystem* physicsSystem)  noexcept;
-    void  BindToWorld     (rp3d::PhysicsWorld* world)       noexcept;
-    void  CleanContainers ()  noexcept;
-    void  ClearContainers ()  noexcept;
+    /**
+     * @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;
 
-    void  onContact (const rp3d::CollisionCallback::CallbackData& callbackData) override;
-    void  onTrigger (const rp3d::OverlapCallback::CallbackData& callbackData) override;
+    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                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    SHPhysicsSystem*              system;
-    std::vector<SHCollisionInfo>  collisionInfoContainer;
-    std::vector<SHCollisionInfo>  triggerInfoContainer;
+    VelocityStates      velocityStates;
+    ContactConstraints  contactConstraints;
 
     /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
+    /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    static void     updateInfoContainers  (const SHCollisionInfo& collisionEvent, std::vector<SHCollisionInfo>& container) noexcept;
+    void preSolve   (float dt)  noexcept;
+    void solve      ()          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/Dynamics/SHMotionState.cpp

@@ -0,0 +1,128 @@
+/****************************************************************************************
+ * \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;
+
+    orientation += orientation * SHQuaternion{ velocity.x, velocity.y, velocity.z, 0.0f } * dt * 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,145 @@
+/****************************************************************************************
+ * \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 deterministic
+     * behaviour.
+    */
+    void Step             (float dt);
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    // EntityIDs are used to map resolved constraints 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,765 @@
+/****************************************************************************************
+ * \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 SHMatrix& SHRigidBody::GetLocalInvInertia() const noexcept
+  {
+    return localInvInertia;
+  }
+
+  const SHMatrix& 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.m[0][0] = localInvInertia.m[1][1] = localInvInertia.m[2][2] = 0.0f;
+      worldInvInertia.m[0][0] = worldInvInertia.m[1][1] = worldInvInertia.m[2][2] = 0.0f;
+    }
+    else
+    {
+      invMass         = 1.0f;
+      localInvInertia = SHMatrix::Identity;
+    }
+  }
+
+  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.m[0][0] = 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.m[1][1] = 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.m[2][2] = 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;
+    accumulatedTorque += 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
+  {
+    const SHMatrix R  = SHMatrix::Rotate(motionState.orientation);
+    const SHMatrix RT = SHMatrix::Transpose(R);
+
+    // Compute world centroid
+    worldCentroid = (R * localCentroid) + motionState.position;
+
+    if (bodyType != Type::DYNAMIC)
+      return;
+
+    // Compute world inertia
+    worldInvInertia = R * (localInvInertia * RT);
+  }
+
+  void SHRigidBody::ComputeMassData() noexcept
+  {
+    // Reset centroid
+    localCentroid   = SHVec3::Zero;
+    localInvInertia = SHMatrix::Identity;
+
+    // In the instance the body is a particle
+    if (!collider || collider->GetCollisionShapes().empty())
+    {
+      localInvInertia.m[0][0] = localInvInertia.m[1][1] = localInvInertia.m[2][2] = 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.m[0][0] = J.m[0][0];
+    localInvInertia.m[1][1] = J.m[1][1];
+    localInvInertia.m[2][2] = J.m[2][2];
+
+    localInvInertia = SHMatrix::Inverse(localInvInertia);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* 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 SHMatrix&     GetLocalInvInertia  ()  const noexcept;
+    [[nodiscard]] const SHMatrix&     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;
+
+    SHMatrix      localInvInertia;
+    SHMatrix      worldInvInertia;
+
+    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

@@ -13,9 +13,8 @@
 // Primary Header
 #include "SHPhysicsObject.h"
 
-// Project Headers
-#include "ECS_Base/Managers/SHSystemManager.h"
-
+#include "Physics/Collision/SHCollider.h"
+#include "Physics/Collision/SHCompositeCollider.h"
 
 namespace SHADE
 {
@@ -27,9 +26,138 @@ namespace SHADE
   : 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;
-    body = nullptr;
+
+    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

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

@@ -10,10 +10,9 @@
 
 #pragma once
 
-#include <reactphysics3d/reactphysics3d.h>
-
 // Project Headers
-#include "ECS_Base/Entity/SHEntity.h"
+#include "Physics/Collision/SHCollider.h"
+#include "Physics/Dynamics/SHRigidBody.h"
 
 namespace SHADE
 {
@@ -32,17 +31,79 @@ namespace SHADE
     /* Data Members                                                                      */
     /*-----------------------------------------------------------------------------------*/
 
-    // We use a rigid body all the time. Colliders without rigid bodies have a static body.
-
     EntityID      entityID  = MAX_EID;
-    rp3d::RigidBody*    body      = nullptr;
+    SHRigidBody*  rigidBody = nullptr;
+    SHCollider*   collider  = nullptr;
 
     /*-----------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                         */
     /*-----------------------------------------------------------------------------------*/
 
     SHPhysicsObject   (EntityID eid)                noexcept;
+    SHPhysicsObject   (const SHPhysicsObject& rhs)  noexcept;
+    SHPhysicsObject   (SHPhysicsObject&& rhs)       noexcept;
     ~SHPhysicsObject  ()                            noexcept;
 
+    /*-----------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                                */
+    /*-----------------------------------------------------------------------------------*/
+
+    SHPhysicsObject& operator=(const SHPhysicsObject& rhs)  noexcept;
+    SHPhysicsObject& operator=(SHPhysicsObject&& rhs)       noexcept;
+
+    /*-----------------------------------------------------------------------------------*/
+    /* Member Functions                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Checks if the physics object has a rigid body or a collider.
+     * @return
+     * True if the physics object has neither a rigid body nor a collider.
+     */
+    [[nodiscard]] bool  IsEmpty () const noexcept;
+
+    /**
+     * @brief
+     * Creates a rigid body for this physics object.
+     * @param bodyType
+     * The rigid body's type. Can be modified after creation.
+     * @return
+     * Pointer to the rigid body that was created. The memory of this rigid body is managed
+     * by the physics object itself.
+    */
+    SHRigidBody* CreateRigidBody (SHRigidBody::Type bodyType);
+
+    /**
+     * @brief
+     * Destroys the rigid body of this physics object and frees the memory.
+    */
+    void DestroyRigidBody () noexcept;
+
+    /**
+     * @brief
+     * Creates a collider for this physics object.
+     * @param colliderType
+     * The collider's type. Should not be modified after creation.
+     * @param transform.
+     * The world transform of the collider. Defaults to the identity transform.
+     * @return
+     * Pointer to the collider that was created. The memory of this collider is managed
+     * by the physics object itself.
+    */
+    SHCollider* CreateCompositeCollider (const SHTransform& transform = SHTransform::Identity);
+
+    /**
+     * @brief
+     * Destroys the collider of this physics object and frees the memory.
+    */
+    void DestroyCollider () noexcept;
+
+  private:
+    /*-----------------------------------------------------------------------------------*/
+    /* Member Functions                                                                  */
+    /*-----------------------------------------------------------------------------------*/
+
+    void deepCopyComponents(const SHRigidBody* rhsRigidBody, const SHCollider* rhsCollider);
   };
 } // namespace SHADE

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

@@ -15,9 +15,6 @@
 
 // Project Headers
 #include "Math/Transform/SHTransformComponent.h"
-#include "Physics/Collision/CollisionTags/SHCollisionTagMatrix.h"
-#include "Physics/Collision/Shapes/SHSphere.h"
-#include "Physics/Collision/Shapes/SHBox.h"
 #include "Physics/Interface/SHColliderComponent.h"
 #include "Physics/Interface/SHRigidBodyComponent.h"
 #include "Tools/Utilities/SHUtilities.h"
@@ -28,13 +25,6 @@ namespace SHADE
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/
 
-  SHPhysicsObjectManager::SHPhysicsObjectManager() noexcept
-  : factory       { nullptr }
-  , physicsWorld  { nullptr }
-  {}
-
-  
-
   SHPhysicsObjectManager::~SHPhysicsObjectManager() noexcept
   {
     RemoveAllObjects();
@@ -61,118 +51,80 @@ namespace SHADE
     return &PHYSICS_OBJECT_ITERATOR->second;
   }
 
-  /*-----------------------------------------------------------------------------------*/
-  /* Setter Function Definitions                                                       */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHPhysicsObjectManager::SetFactory(rp3d::PhysicsCommon* physicsFactory) noexcept
-  {
-    factory = physicsFactory;
-  }
-
-  void SHPhysicsObjectManager::SetPhysicsWorld(rp3d::PhysicsWorld* world) noexcept
-  {
-    physicsWorld = world;
-  }
-
   /*-----------------------------------------------------------------------------------*/
   /* Public Member Function Definitions                                                */
   /*-----------------------------------------------------------------------------------*/
 
   void SHPhysicsObjectManager::AddRigidBody(EntityID entityID) noexcept
   {
-    SHASSERT(physicsWorld, "Physics World Missing ffrom Physics Object Manager!")
-
     SHPhysicsObject* physicsObject = ensurePhysicsObject(entityID);
 
-    // Get the rigidbody and transform components
+    // Get the component
     auto* rigidBodyComponent = SHComponentManager::GetComponent<SHRigidBodyComponent>(entityID);
-    auto* transformComponent = SHComponentManager::GetComponent_s<SHTransformComponent>(entityID);
-
-    
-
-    if (!physicsObject->body)
-    {
-      if (!transformComponent)
-      {
-        SHLOG_ERROR("Unable to create a rigid body for Entity {} with missing transform!", entityID)
-        return;
-      }
 
     // Create a new rigidbody in the physics object
-      const rp3d::Transform RP3D_TRANSFORM{ transformComponent->GetWorldPosition(), transformComponent->GetWorldOrientation() };
-      physicsObject->body = physicsWorld->createRigidBody(RP3D_TRANSFORM);
+    const auto RIGID_BODY_TYPE = static_cast<SHRigidBody::Type>(rigidBodyComponent->GetType());
+    auto* rigidBody = physicsObject->CreateRigidBody(RIGID_BODY_TYPE);
+
+    SHVec3 worldPos = SHVec3::Zero;
+    SHQuaternion worldRot = SHQuaternion::Identity;
+
+    if (const auto* TRANSFORM_COMPONENT = SHComponentManager::GetComponent_s<SHTransformComponent>(entityID); TRANSFORM_COMPONENT)
+    {
+      worldPos = TRANSFORM_COMPONENT->GetWorldPosition();
+      worldRot = TRANSFORM_COMPONENT->GetWorldOrientation();
     }
 
+    SHMotionState& motionState = rigidBody->GetMotionState();
+    motionState.ForcePosition(worldPos);
+    motionState.ForceOrientation(worldRot);
+
     // Link with the component
-    rigidBodyComponent->SetRigidBody(physicsObject->body);
-
-    // Reset the type
-    const auto RIGID_BODY_TYPE = rigidBodyComponent->GetType();
-    switch (RIGID_BODY_TYPE)
-    {
-      case SHRigidBodyComponent::Type::STATIC:
-        physicsObject->body->setType(rp3d::BodyType::STATIC);
-        break;
-      case SHRigidBodyComponent::Type::KINEMATIC:
-        physicsObject->body->setType(rp3d::BodyType::KINEMATIC);
-        break;
-      case SHRigidBodyComponent::Type::DYNAMIC:
-        physicsObject->body->setType(rp3d::BodyType::DYNAMIC);
-        break;
-      default:
-        break;
-    }
+    rigidBodyComponent->SetRigidBody(rigidBody);
   }
 
   void SHPhysicsObjectManager::RemoveRigidBody(EntityID entityID) noexcept
   {
-    SHASSERT(physicsWorld, "Physics World Missing from Physics Object Manager!")
-
     const auto PHYSICS_OBJECT_ITERATOR = physicsObjects.find(entityID);
     if (PHYSICS_OBJECT_ITERATOR == physicsObjects.end())
       return;
 
     SHPhysicsObject* physicsObject = &PHYSICS_OBJECT_ITERATOR->second;
 
-    // If a collider component exists, we just set the body to static
-    if (SHComponentManager::GetComponent_s<SHColliderComponent>(entityID))
-      physicsObject->body->setType(rp3d::BodyType::STATIC);
-    else
-    {
-      physicsWorld->destroyRigidBody(physicsObject->body);
+    physicsObject->DestroyRigidBody();
+
+    // Destroy empty physics objects
+    if (physicsObject->IsEmpty())
       destroyPhysicsObject(entityID);
   }
-  }
 
-  void SHPhysicsObjectManager::AddCollider(EntityID entityID) noexcept
+  void SHPhysicsObjectManager::AddCollider(EntityID entityID, SHCollider::Type type) noexcept
   {
-    SHASSERT(physicsWorld, "Physics World Missing from Physics Object Manager!")
-
     SHPhysicsObject* physicsObject = ensurePhysicsObject(entityID);
 
-    // Get the collider & transform component
+    // Get the component
     auto* colliderComponent = SHComponentManager::GetComponent<SHColliderComponent>(entityID);
-    auto* transformComponent = SHComponentManager::GetComponent_s<SHTransformComponent>(entityID);
 
-    // Check if a body already exists. If it does, link it with the component
-    if (!physicsObject->body)
+    SHTransform worldTransform = SHTransform::Identity;
+    if (const auto* TRANSFORM_COMPONENT = SHComponentManager::GetComponent_s<SHTransformComponent>(entityID); TRANSFORM_COMPONENT)
     {
-      if (!transformComponent)
-      {
-        SHLOG_ERROR("Unable to create a collider for Entity {} with missing transform!", entityID)
-        return;
+      worldTransform.position     = TRANSFORM_COMPONENT->GetWorldPosition();
+      worldTransform.orientation  = TRANSFORM_COMPONENT->GetWorldOrientation();
+      worldTransform.scale        = TRANSFORM_COMPONENT->GetWorldScale();
+
+      worldTransform.ComputeTRS();
     }
 
-      // Create a static body
-      const rp3d::Transform RP3D_TRANSFORM{ transformComponent->GetWorldPosition(), transformComponent->GetWorldOrientation() };
-      physicsObject->body = physicsWorld->createRigidBody(RP3D_TRANSFORM);
-      physicsObject->body->setType(rp3d::BodyType::STATIC);
-    }
+    // Create a new composite collider in the physics object
+    if (type == SHCollider::Type::COMPOSITE)
+      physicsObject->CreateCompositeCollider(worldTransform);
 
-    // Link with component
-    colliderComponent->SetFactory(factory);
-    colliderComponent->SetCollisionBody(physicsObject->body);
+    // TODO: Hull collider
+
+    physicsObject->collider->SetLibrary(&shapeLibrary);
+
+    // Link with the component
+    colliderComponent->SetCollider(dynamic_cast<SHCompositeCollider*>(physicsObject->collider));
   }
 
   void SHPhysicsObjectManager::RemoveCollider(EntityID entityID) noexcept
@@ -182,16 +134,10 @@ namespace SHADE
     {
       SHPhysicsObject* physicsObject = &PHYSICS_OBJECT_ITERATOR->second;
 
-      // Remove all collision shapes from the body
-      int32_t numShapes = static_cast<int32_t>(physicsObject->body->getNbColliders());
-      while (--numShapes >= 0)
-      {
-        auto* rp3dCollider = physicsObject->body->getCollider(numShapes);
-        physicsObject->body->removeCollider(rp3dCollider);
-      }
+      physicsObject->DestroyCollider();
 
-      // Destroy if no rigidbody component
-      if (!SHComponentManager::GetComponent_s<SHRigidBodyComponent>(entityID))
+      // Destroy empty physics objects
+      if (physicsObject->IsEmpty())
         destroyPhysicsObject(entityID);
     }
   }
@@ -202,186 +148,6 @@ namespace SHADE
     physicsObjects.clear();
   }
   
-  void SHPhysicsObjectManager::AddRigidBodyDef(EntityID entityID) noexcept
-  {
-    auto* rigidBody = SHComponentManager::GetComponent<SHRigidBodyComponent>(entityID);
-
-    const SHRigidBodyDef RIGID_BODY_DEF
-    {
-      .entityID     = rigidBody->GetEID()
-    , .bodyType     = rigidBody->type
-    };
-
-    rigidBodyQueue.push(RIGID_BODY_DEF);
-  }
-
-  void SHPhysicsObjectManager::AddColliderDef(EntityID entityID) noexcept
-  {
-     auto* collider = SHComponentManager::GetComponent<SHColliderComponent>(entityID);
-
-    SHColliderDef colliderDef
-    {
-      .entityID = collider->GetEID()
-    };
-
-    for (const auto* shape : collider->shapes)
-    {
-      SHColliderDef::ShapeDef shapeDef
-      {
-        .type       = shape->GetType()
-      };
-
-      colliderDef.shapes.emplace_back(shapeDef);
-    }
-
-    colliderQueue.push(colliderDef);
-  }
-
-  void SHPhysicsObjectManager::FlushDefinitions() noexcept
-  {
-    SHASSERT(physicsWorld, "Physics World Missing ffrom Physics Object Manager!")
-
-    // Flush all rigid bodies
-    while (!rigidBodyQueue.empty())
-    {
-      const SHRigidBodyDef& DEF = rigidBodyQueue.front();
-
-      SHPhysicsObject* physicsObject = ensurePhysicsObject(DEF.entityID);
-
-      // Get transform component
-      auto* transformComponent = SHComponentManager::GetComponent_s<SHTransformComponent>(DEF.entityID);
-      if (!transformComponent)
-      {
-        SHLOG_ERROR("Unable to create a rigid body for Entity {} with missing transform!", DEF.entityID)
-
-        rigidBodyQueue.pop();
-        continue;
-      }
-
-      // Create a new rigidbody in the physics object
-      const rp3d::Transform RP3D_TRANSFORM{ transformComponent->GetWorldPosition(), transformComponent->GetWorldOrientation() };
-      physicsObject->body = physicsWorld->createRigidBody(RP3D_TRANSFORM);
-
-      // Get rigidBody component
-      auto* rigidBodyComponent = SHComponentManager::GetComponent<SHRigidBodyComponent>(DEF.entityID);
-      rigidBodyComponent->SetRigidBody(physicsObject->body);
-      
-      // Set type
-      rigidBodyComponent->type = DEF.bodyType;
-      switch (DEF.bodyType)
-      {
-        case SHRigidBodyComponent::Type::STATIC:
-          physicsObject->body->setType(rp3d::BodyType::STATIC);
-          break;
-        case SHRigidBodyComponent::Type::KINEMATIC:
-          physicsObject->body->setType(rp3d::BodyType::KINEMATIC);
-          break;
-        case SHRigidBodyComponent::Type::DYNAMIC:
-          physicsObject->body->setType(rp3d::BodyType::DYNAMIC);
-          break;
-        default:
-          break;
-      }
-      
-
-      // Re-set properties
-      rigidBodyComponent->SetGravityEnabled   (rigidBodyComponent->IsGravityEnabled());
-      rigidBodyComponent->SetIsAllowedToSleep (rigidBodyComponent->IsAllowedToSleep());
-      rigidBodyComponent->SetFreezePositionX  (rigidBodyComponent->GetFreezePositionX());
-      rigidBodyComponent->SetFreezePositionY  (rigidBodyComponent->GetFreezePositionY());
-      rigidBodyComponent->SetFreezePositionZ  (rigidBodyComponent->GetFreezePositionZ());
-      rigidBodyComponent->SetFreezeRotationX  (rigidBodyComponent->GetFreezeRotationX());
-      rigidBodyComponent->SetFreezeRotationY  (rigidBodyComponent->GetFreezeRotationY());
-      rigidBodyComponent->SetFreezeRotationZ  (rigidBodyComponent->GetFreezeRotationZ());
-
-      rigidBodyComponent->SetInterpolate      (rigidBodyComponent->IsInterpolating());
-      rigidBodyComponent->SetDrag             (rigidBodyComponent->GetDrag());
-      rigidBodyComponent->SetAngularDrag      (rigidBodyComponent->GetAngularDrag());
-
-      rigidBodyQueue.pop();
-    }
-
-    // Flush all colliders
-    while (!colliderQueue.empty())
-    {
-      const SHColliderDef& DEF = colliderQueue.front();
-
-      SHPhysicsObject* physicsObject = ensurePhysicsObject(DEF.entityID);
-
-      // Get transform component
-      auto* transformComponent = SHComponentManager::GetComponent_s<SHTransformComponent>(DEF.entityID);
-      if (!transformComponent)
-      {
-        SHLOG_ERROR("Unable to create a collider for Entity {} with missing transform!", DEF.entityID)
-
-        colliderQueue.pop();
-        continue;
-      }
-
-      if (!physicsObject->body)
-      {
-        // Create a static body
-        const rp3d::Transform RP3D_TRANSFORM{ transformComponent->GetWorldPosition(), transformComponent->GetWorldOrientation() };
-        physicsObject->body = physicsWorld->createRigidBody(RP3D_TRANSFORM);
-        physicsObject->body->setType(rp3d::BodyType::STATIC);
-      }
-
-      // Get rigidBody component
-      auto* colliderComponent = SHComponentManager::GetComponent<SHColliderComponent>(DEF.entityID);
-
-      colliderComponent->SetFactory(factory);
-      colliderComponent->SetCollisionBody(physicsObject->body);
-
-      // Add all shapes
-      for (size_t i = 0; i < colliderComponent->shapes.size(); ++i)
-      {
-        // Get the currrent shape
-        auto& collisionShape = colliderComponent->shapes[i];
-
-        // Add the rp3d collider to the shade collision shape
-        switch (collisionShape->GetType())
-        {
-          case SHCollisionShape::Type::SPHERE:
-          {
-            auto* sphereShape = dynamic_cast<SHSphere*>(collisionShape);
-
-            const float SPHERE_SCALE = std::fabs(SHMath::Max({ colliderComponent->transform.scale.x, colliderComponent->transform.scale.y, colliderComponent->transform.scale.z }));
-
-            rp3d::SphereShape* rp3dSphere = factory->createSphereShape(sphereShape->GetRelativeRadius() * SPHERE_SCALE * 0.5f);
-
-            const rp3d::Transform OFFSETS{ sphereShape->GetPositionOffset(), SHQuaternion::FromEuler(sphereShape->GetRotationOffset()) };
-            sphereShape->rp3dCollider = physicsObject->body->addCollider(rp3dSphere, OFFSETS);
-            sphereShape->rp3dCollider->setIsTrigger(sphereShape->IsTrigger());
-
-            break;
-          }
-          case SHCollisionShape::Type::BOX:
-          {
-            auto* boxShape = dynamic_cast<SHBox*>(collisionShape);
-
-            rp3d::BoxShape* rp3dBox = factory->createBoxShape(boxShape->GetRelativeExtents() * colliderComponent->transform.scale * 0.5f);
-
-            const rp3d::Transform OFFSETS{ boxShape->GetPositionOffset(), SHQuaternion::FromEuler(boxShape->GetRotationOffset()) };
-            boxShape->rp3dCollider = physicsObject->body->addCollider(rp3dBox, OFFSETS);
-            boxShape->rp3dCollider->setIsTrigger(boxShape->IsTrigger());
-
-            break;
-          }
-          default: break;
-        }
-
-        collisionShape->SetMaterial(collisionShape->GetMaterial());
-
-        const auto& COLLISION_TAG = collisionShape->GetCollisionTag();
-        collisionShape->SetCollisionTag(SHCollisionTagMatrix::GetTag(COLLISION_TAG.GetName()));
-      }
-
-      physicsObject->body->updateMassPropertiesFromColliders();
-
-      colliderQueue.pop();
-    }
-  }
-
   /*-----------------------------------------------------------------------------------*/
   /* Private Member Function Definitions                                               */
   /*-----------------------------------------------------------------------------------*/

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

@@ -10,14 +10,11 @@
 
 #pragma once
 
-#include <vector>
-#include <queue>
 #include <unordered_map>
 
 // Project Headers
 #include "SHPhysicsObject.h"
-#include "Physics/Interface/SHRigidBodyComponent.h"
-#include "Physics/Interface/SHColliderComponent.h"
+#include "Physics/Collision/Shapes/SHCollisionShapeLibrary.h"
 
 namespace SHADE
 {
@@ -36,35 +33,15 @@ namespace SHADE
     /*-----------------------------------------------------------------------------------*/
     /* Type Definitions                                                                  */
     /*-----------------------------------------------------------------------------------*/
+
     using EntityObjectMap = std::unordered_map<EntityID, SHPhysicsObject>;
 
   public:
-    /*-----------------------------------------------------------------------------------*/
-    /* Type Definitions                                                                  */
-    /*-----------------------------------------------------------------------------------*/
-
-    struct SHRigidBodyDef
-    {
-      EntityID                    entityID    = MAX_EID;
-      SHRigidBodyComponent::Type  bodyType    = SHRigidBodyComponent::Type::STATIC;
-    };
-
-    struct SHColliderDef
-    {
-      struct ShapeDef
-      {
-        SHCollisionShape::Type  type        = SHCollisionShape::Type::SPHERE;
-      };
-
-      EntityID                  entityID    = MAX_EID;
-      std::vector<ShapeDef>     shapes;
-    };
-
     /*-----------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                         */
     /*-----------------------------------------------------------------------------------*/
 
-    SHPhysicsObjectManager  () noexcept;
+    SHPhysicsObjectManager  () noexcept = default;
     ~SHPhysicsObjectManager () noexcept;
 
     /*-----------------------------------------------------------------------------------*/
@@ -74,13 +51,6 @@ namespace SHADE
     [[nodiscard]] EntityObjectMap&        GetPhysicsObjects ()                  noexcept;
     [[nodiscard]] const SHPhysicsObject*  GetPhysicsObject  (EntityID entityID) noexcept;
 
-    /*-----------------------------------------------------------------------------------*/
-    /* Setter Functions                                                                  */
-    /*-----------------------------------------------------------------------------------*/
-
-    void SetFactory     (rp3d::PhysicsCommon* physicsFactory) noexcept;
-    void SetPhysicsWorld(rp3d::PhysicsWorld* world)           noexcept;
-
     /*-----------------------------------------------------------------------------------*/
     /* Member Functions                                                                  */
     /*-----------------------------------------------------------------------------------*/
@@ -107,7 +77,7 @@ namespace SHADE
      * @param entityID
      * The entity to link the new collider to.
     */
-    void AddCollider      (EntityID entityID) noexcept;
+    void AddCollider      (EntityID entityID, SHCollider::Type type) noexcept;
 
     /**
      * @brief
@@ -124,43 +94,13 @@ namespace SHADE
     */
     void RemoveAllObjects ()                  noexcept;
 
-    /**
-     * @brief Adds a rigidbody definition to add to the physics world.
-     * @param rigidBody
-     * The rigidbody component
-    */
-    void AddRigidBodyDef  (EntityID entityID) noexcept;
-
-    /**
-     * @brief Adds a collider definition to add to the physics world.
-     * @param collider
-     * The collider component
-    */
-    void AddColliderDef   (EntityID entityID) noexcept;
-
-    /**
-     * @brief Adds all loaded definitions into the physics world.
-    */
-    void FlushDefinitions () noexcept;
-
   private:
-    /*-----------------------------------------------------------------------------------*/
-    /* Type Definitions                                                                  */
-    /*-----------------------------------------------------------------------------------*/
-
-    using RigidBodyQueue    = std::queue<SHRigidBodyDef>;
-    using ColliderQueue     = std::queue<SHColliderDef>;
-
     /*-----------------------------------------------------------------------------------*/
     /* Data Members                                                                      */
     /*-----------------------------------------------------------------------------------*/
 
-    rp3d::PhysicsCommon*    factory;
-    rp3d::PhysicsWorld*     physicsWorld;
     EntityObjectMap         physicsObjects;
-
-    RigidBodyQueue          rigidBodyQueue;
-    ColliderQueue           colliderQueue;
+    SHCollisionShapeLibrary shapeLibrary;
 
     /*-----------------------------------------------------------------------------------*/
     /* Member Functions                                                                  */

SHADE_Engine/src/Physics/Interface/SHColliderComponent.cpp

@@ -9,18 +9,10 @@
 ****************************************************************************************/
 
 #include <SHpch.h>
-#include <reactphysics3d/reactphysics3d.h>
 
 // Primary Header
 #include "SHColliderComponent.h"
 
-// Project Headers
-#include "ECS_Base/Managers/SHSystemManager.h"
-#include "Math/SHMathHelpers.h"
-#include "Physics/SHPhysicsEvents.h"
-#include "Physics/Collision/Shapes/SHSphere.h"
-#include "Physics/Collision/Shapes/SHBox.h"
-
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
@@ -28,257 +20,58 @@ namespace SHADE
   /*-----------------------------------------------------------------------------------*/
 
   SHColliderComponent::SHColliderComponent() noexcept
-  : flags         { ACTIVE_FLAG | MOVED_FLAG }
-  , factory       { nullptr }
-  , collisionBody { nullptr }
+  : collider { nullptr }
   {}
 
   /*-----------------------------------------------------------------------------------*/
   /* Getter Function Definitions                                                       */
   /*-----------------------------------------------------------------------------------*/
 
+  SHCompositeCollider* const SHColliderComponent::GetCollider() const noexcept
+  {
+    return collider;
+  }
+
+  const SHCollider::CollisionShapes* const SHColliderComponent::GetCollisionShapes() const noexcept
+  {
+    if (!collider)
+      return nullptr;
+
+    return &collider->GetCollisionShapes();
+  }
+
+
+  SHCollisionShape* const SHColliderComponent::GetCollisionShape(int index) const
+  {
+    if (!collider)
+      return nullptr;
+
+    return collider->GetCollisionShape(index);
+  }
+
   bool SHColliderComponent::GetDebugDrawState() const noexcept
   {
-    return flags & DRAW_FLAG;
-  }
+    if (!collider)
+      return false;
 
-   const SHTransform& SHColliderComponent::GetTransform() const noexcept
-  {
-    return transform;
-  }
-
-  const SHVec3& SHColliderComponent::GetPosition() const noexcept
-  {
-    return transform.position;
-  }
-
-  const SHQuaternion& SHColliderComponent::GetOrientation() const noexcept
-  {
-    return transform.orientation;
-  }
-
-  const SHVec3& SHColliderComponent::GetScale() const noexcept
-  {
-    return transform.scale;
-  }
-
-  const SHColliderComponent::CollisionShapes& SHColliderComponent::GetCollisionShapes() const noexcept
-  {
-    return shapes;
-  }
-
-  SHCollisionShape& SHColliderComponent::GetCollisionShape(int index)
-  {
-    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];
+    return collider->GetDebugDrawState();
   }
   
   /*-----------------------------------------------------------------------------------*/
   /* Setter Function Definitions                                                       */
   /*-----------------------------------------------------------------------------------*/
 
+  void SHColliderComponent::SetCollider(SHCompositeCollider* c) noexcept
+  {
+    collider = c;
+  }
+
   void SHColliderComponent::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       = GetEID()
-    , .debugDrawState = state 
-    };
-
-    SHEventManager::BroadcastEvent<SHColliderOnDebugDrawEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_DRAW_EVENT);
-
-  #endif
+    if (collider)
+      collider->SetDebugDrawState(state);
   }
 
-  void SHColliderComponent::SetFactory(rp3d::PhysicsCommon* physicsCommon) noexcept
-  {
-    factory = physicsCommon;
-  }
-
-  void SHColliderComponent::SetCollisionBody(rp3d::CollisionBody* body) noexcept
-  {
-    collisionBody = body;
-  }
-
-  void SHColliderComponent::SetTransform(const SHTransform& newTransform) noexcept
-  {
-    flags |= MOVED_FLAG;
-    transform = newTransform;
-  }
-
-  void SHColliderComponent::SetPosition(const SHVec3& newPosition) noexcept
-  {
-    flags |= MOVED_FLAG;
-    transform.position = newPosition;
-  }
-
-  void SHColliderComponent::SetOrientation(const SHQuaternion& newOrientation) noexcept
-  {
-    flags |= MOVED_FLAG;
-    transform.orientation = newOrientation;
-  }
-
-  void SHColliderComponent::SetScale(const SHVec3& newScale) noexcept
-  {
-    flags |= MOVED_FLAG;
-    transform.scale = newScale;
-  }
-
-  /*-----------------------------------------------------------------------------------*/
-  /* Public Function Member Definitions                                                */
-  /*-----------------------------------------------------------------------------------*/
-
-  void SHColliderComponent::OnDestroy()
-  {
-    int32_t numShapes = static_cast<int32_t>(shapes.size());
-    while (--numShapes >= 0)
-    {
-      delete shapes[numShapes];
-      shapes[numShapes] = nullptr;
-    }
-
-    shapes.clear();
-  }
-
-  const SHMatrix& SHColliderComponent::ComputeTRS() noexcept
-  {
-    return transform.ComputeTRS();
-  }
-
-  int SHColliderComponent::AddSphereCollisionShape(float relativeRadius, const SHVec3& posOffset, const SHVec3& rotOffset)
-  {
-    const float SPHERE_SCALE = std::fabs(SHMath::Max({ transform.scale.x, transform.scale.y, transform.scale.z }));
-
-    const uint32_t NEW_INDEX = static_cast<uint32_t>(shapes.size());
-
-    // Create collision shape
-    shapes.emplace_back(new SHSphere{});
-    auto* newSphere = dynamic_cast<SHSphere*>(shapes.back());
-
-    newSphere->collider        = this;
-    newSphere->positionOffset  = posOffset;
-    newSphere->rotationOffset  = rotOffset;
-    newSphere->relativeRadius  = relativeRadius;
-    newSphere->scale           = SPHERE_SCALE;
-
-    // Broadcast Event for adding a shape
-    const SHPhysicsColliderAddedEvent EVENT_DATA
-    {
-      .entityID       = GetEID()
-    , .colliderType   = SHCollisionShape::Type::BOX
-    , .colliderIndex  = static_cast<int>(NEW_INDEX)
-    };
-
-    SHEventManager::BroadcastEvent<SHPhysicsColliderAddedEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_ADDED_EVENT);
-
-    // Only link with react if a factory is present.
-    // Otherwise, it will be linked through the physics object manager once the definitions are flushed.
-    if (factory)
-    {
-      rp3d::SphereShape* rp3dSphere = factory->createSphereShape(relativeRadius * SPHERE_SCALE * 0.5f);
-
-      const rp3d::Transform OFFSETS{ posOffset, SHQuaternion::FromEuler(rotOffset) };
-      newSphere->rp3dCollider = collisionBody->addCollider(rp3dSphere, OFFSETS);
-
-      dynamic_cast<rp3d::RigidBody*>(collisionBody)->updateMassPropertiesFromColliders();
-    }
-
-    return static_cast<int>(NEW_INDEX);
-  }
-
-  int SHColliderComponent::AddBoxCollisionShape(const SHVec3& relativeExtents, const SHVec3& posOffset, const SHVec3& rotOffset)
-  {
-    const uint32_t NEW_INDEX = static_cast<uint32_t>(shapes.size());
-
-    // Create collision shape
-    shapes.emplace_back(new SHBox{});
-    auto* newBox = dynamic_cast<SHBox*>(shapes.back());
-
-    newBox->collider         = this;
-    newBox->positionOffset   = posOffset;
-    newBox->rotationOffset   = rotOffset;
-    newBox->relativeExtents  = relativeExtents;
-    newBox->scale            = SHVec3::Abs(transform.scale);
-
-    // Broadcast Event for adding a shape
-    const SHPhysicsColliderAddedEvent EVENT_DATA
-    {
-      .entityID       = GetEID()
-    , .colliderType   = SHCollisionShape::Type::BOX
-    , .colliderIndex  = static_cast<int>(NEW_INDEX)
-    };
-
-    SHEventManager::BroadcastEvent<SHPhysicsColliderAddedEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_ADDED_EVENT);
-
-    if (factory)
-    {
-      rp3d::BoxShape* rp3dBox = factory->createBoxShape(relativeExtents * newBox->scale * 0.5f);
-
-      const rp3d::Transform OFFSETS{ posOffset, SHQuaternion::FromEuler(rotOffset) };
-      newBox->rp3dCollider = collisionBody->addCollider(rp3dBox, OFFSETS);
-
-      dynamic_cast<rp3d::RigidBody*>(collisionBody)->updateMassPropertiesFromColliders();
-    }
-
-    return static_cast<int>(NEW_INDEX);
-  }
-
-  void SHColliderComponent::RemoveCollisionShape(int index)
-  {
-    const int NUM_SHAPES = static_cast<int>(shapes.size());
-
-    if (index < 0 || index >= NUM_SHAPES)
-      throw std::invalid_argument("Out-of-range index!");
-
-    int i = 0;
-    for (auto shapeIter = shapes.begin(); shapeIter != shapes.end(); ++i, ++shapeIter)
-    {
-      if (i == index)
-      {
-        collisionBody->removeCollider((*shapeIter)->rp3dCollider);
-        dynamic_cast<rp3d::RigidBody*>(collisionBody)->updateMassPropertiesFromColliders();
-
-        const SHPhysicsColliderRemovedEvent EVENT_DATA
-        {
-          .entityID       = GetEID()
-        , .colliderType   = (*shapeIter)->GetType()
-        , .colliderIndex  = index
-        };
-
-        // Broadcast Event for removing a shape
-        SHEventManager::BroadcastEvent<SHPhysicsColliderRemovedEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_REMOVED_EVENT);
-
-        SHLOG_INFO_D("Removing Collision Shape {} from Entity {}", index, GetEID())
-
-        delete *shapeIter;
-        shapeIter = shapes.erase(shapeIter);
-        return;
-      }
-    }
-  }
-
-  void SHColliderComponent::Update() noexcept
-  {
-    for (auto& shape : shapes)
-      shape->Update();
-  }
-
-  void SHColliderComponent::UpdateCollisionTags() noexcept
-  {
-    for (auto& shape : shapes)
-      shape->UpdateCollisionTags();
-  }
-
-
 
 } // namespace SHADE
 
@@ -287,5 +80,6 @@ RTTR_REGISTRATION
   using namespace rttr;
   using namespace SHADE;
 
-  registration::class_<SHColliderComponent>("Collider Component");
+  registration::class_<SHColliderComponent>("Collider Component")
+    .property("Is Debug Drawing", &SHColliderComponent::GetDebugDrawState, &SHColliderComponent::SetDebugDrawState);
 }