Implemented backbone for collision detection with broadphase

Assets/Scenes/PhysicsSandbox.shade

@@ -4,7 +4,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0, y: 1.77475965, z: 0}
+      Translate: {x: 0, y: 3, z: 0}
       Rotate: {x: 0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
@@ -59,3 +59,43 @@
       Perspective: true
       IsActive: true
   Scripts: ~
+- EID: 2
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 1, z: 0}
+      Rotate: {x: 0, y: 0, z: 0}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: false
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: true
+      Colliders:
+        - Is Trigger: false
+          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: ~

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

@@ -0,0 +1,607 @@
+/****************************************************************************************
+ * \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<SHBox>& SHAABBTree::GetAABBs() const noexcept
+  {
+    static std::vector<SHBox>   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 SHBox& 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 SHBox& AABB)
+  {
+    // Get node index
+    const int32_t INDEX_TO_UPDATE = nodeMap[id];
+
+    Node& nodeToUpdate = nodes[INDEX_TO_UPDATE];
+
+    // If new AABB has not moved enough, skip.
+    if (nodeToUpdate.AABB.Contains(AABB))
+      return;
+
+    removeLeaf(INDEX_TO_UPDATE);
+
+    nodeToUpdate.AABB = AABB;
+
+    // Fatten the AABB
+    const SHVec3 EXTENSION{ AABB_EXTENSION };
+
+    const SHVec3 newMin = nodeToUpdate.AABB.GetMin() - EXTENSION;
+    const SHVec3 newMax = nodeToUpdate.AABB.GetMax() + EXTENSION;
+
+    nodeToUpdate.AABB.SetMin(newMin);
+    nodeToUpdate.AABB.SetMax(newMax);
+
+    insertLeaf(INDEX_TO_UPDATE);
+  }
+
+  void SHAABBTree::Remove(SHCollisionShapeID id) noexcept
+  {
+    // Get node index
+    const int32_t INDEX_TO_REMOVE = nodeMap[id];
+
+    removeLeaf(INDEX_TO_REMOVE);
+    freeNode(INDEX_TO_REMOVE);
+  }
+
+  const std::vector<SHCollisionShapeID>& SHAABBTree::Query(SHCollisionShapeID id, const SHBox& 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 (!SHBox::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;
+
+    potentialHits.clear();
+
+    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     = NULL_NODE;
+    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 SHBox& LEAF_AABB  = nodes[index].AABB;
+
+    uint32_t searchIndex = root;
+    while (!isLeaf(searchIndex))
+    {
+      const SHBox COMBINED_AABB = SHBox::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  = SHBox::Combine(LEAF_AABB, nodes[LEFT_INDEX].AABB).SurfaceArea();
+      const float RIGHT_COMBINED_AREA = SHBox::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    = SHBox::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;
+    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   = SHBox::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    = SHBox::Combine(nodeB.AABB, nodeG.AABB);
+      nodeC.AABB    = SHBox::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    = SHBox::Combine(nodeB.AABB, nodeF.AABB);
+      nodeC.AABB    = SHBox::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    = SHBox::Combine(nodeC.AABB, nodeE.AABB);
+      nodeB.AABB    = SHBox::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    = SHBox::Combine(nodeC.AABB, nodeD.AABB);
+      nodeB.AABB    = SHBox::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,150 @@
+/****************************************************************************************
+ * \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/CollisionShapes/SHCollisionShape.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates a dynamic AABB Tree for collision detection.
+  */
+  class SH_API SHAABBTree
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* 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 std::vector<SHBox>& GetAABBs  () const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    void Insert  (SHCollisionShapeID id, const SHBox& AABB);
+    void Update  (SHCollisionShapeID id, const SHBox& AABB);
+    void Remove  (SHCollisionShapeID id) noexcept;
+
+    [[nodiscard]] const std::vector<SHCollisionShapeID>& Query(SHCollisionShapeID id, const SHBox& 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                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      SHBox               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/CollisionShapes/SHCollisionShape.h

@@ -17,6 +17,7 @@
 #include "Physics/Collision/CollisionTags/SHCollisionTags.h"
 #include "Physics/Collision/SHPhysicsMaterial.h"
 #include "SHCollisionShapeID.h"
+#include "Math/Geometry/SHBox.h"
 #include "Math/Transform/SHTransform.h"
 
 namespace SHADE
@@ -33,8 +34,10 @@ namespace SHADE
     /* Friends                                                                         */
     /*---------------------------------------------------------------------------------*/
 
+    friend class SHCollider;
     friend class SHColliderComponent;
     friend class SHCollisionShapeFactory;
+    friend class SHPhysicsWorld;
 
   public:
     /*---------------------------------------------------------------------------------*/
@@ -120,6 +123,7 @@ namespace SHADE
     virtual void                    ComputeTransforms     ()            noexcept = 0;
     [[nodiscard]] virtual SHMatrix  GetInertiaTensor      (float mass)  const noexcept = 0;
     [[nodiscard]] virtual SHMatrix  ComputeWorldTransform ()            const noexcept = 0;
+    [[nodiscard]] virtual SHBox     ComputeAABB           ()            const noexcept = 0;
 
   protected:
     /*---------------------------------------------------------------------------------*/
@@ -136,7 +140,7 @@ namespace SHADE
     // Needed for conversion to euler angles
     SHVec3              rotationOffset;
 
-    uint8_t             flags;          // 0 0 wasColliding isColliding trigger capsule sphere box
+    uint8_t             flags;          // 0 0 0 isColliding trigger capsule sphere box
     SHCollisionTag*     collisionTag;
 
     RTTR_ENABLE()

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

@@ -10,6 +10,7 @@
 
 #pragma once
 
+// Project Headers
 #include "ECS_Base/Entity/SHEntity.h"
 
 namespace SHADE
@@ -55,11 +56,14 @@ namespace SHADE
     SHCollisionShapeID& operator=(const SHCollisionShapeID& rhs)  noexcept;
     SHCollisionShapeID& operator=(SHCollisionShapeID&& rhs)       noexcept;
 
+    bool operator==(const SHCollisionShapeID& rhs) const;
+
     /*---------------------------------------------------------------------------------*/
-    /* Member Functions                                                                */
+    /* Getter Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    bool operator==(const SHCollisionShapeID& rhs) const;
+    [[nodiscard]] EntityID  GetEntityID   ()  const noexcept;
+    [[nodiscard]] uint32_t  GetShapeIndex ()  const noexcept;
 
   private:
     /*---------------------------------------------------------------------------------*/
@@ -70,7 +74,7 @@ namespace SHADE
     {
     public:
       EntityID  entityID    = MAX_EID;
-      uint32_t  shapeID   = std::numeric_limits<uint32_t>::max();
+      uint32_t  shapeIndex  = std::numeric_limits<uint32_t>::max();
     };
 
 
@@ -84,7 +88,7 @@ namespace SHADE
 
   /**
    * @brief
-   * Encapsulates a functor to hash a CollisionShapeKey
+   * Encapsulates a functor to hash a CollisionShapeID
   */
   struct SHCollisionShapeIDHash
   {

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

@@ -25,11 +25,11 @@ namespace SHADE
   {}
 
   inline SHCollisionShapeID::SHCollisionShapeID(const SHCollisionShapeID& rhs) noexcept
-  : ids { rhs.ids.entityID, rhs.ids.shapeID }
+  : ids { rhs.ids.entityID, rhs.ids.shapeIndex }
   {}
 
   inline SHCollisionShapeID::SHCollisionShapeID(SHCollisionShapeID&& rhs) noexcept
-  : ids { rhs.ids.entityID, rhs.ids.shapeID }
+  : ids { rhs.ids.entityID, rhs.ids.shapeIndex }
   {}
   
 
@@ -61,6 +61,20 @@ namespace SHADE
 
   inline std::size_t SHCollisionShapeIDHash::operator()(const SHCollisionShapeID& id) const
   {
-    return std::hash<uint64_t>()(id.value);
+    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/CollisionShapes/SHSphereCollisionShape.cpp

@@ -223,5 +223,10 @@ namespace SHADE
     );
   }
 
+  SHBox SHSphereCollisionShape::ComputeAABB() const noexcept
+  {
+    return SHBox{ Center, SHVec3{ Radius } };
+  }
+
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.h

@@ -46,6 +46,7 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     friend class SHCollider;
+    friend class SHCollision;
     friend class SHCompositeCollider;
     friend class SHCollisionShapeFactory;
 
@@ -127,6 +128,8 @@ namespace SHADE
 
     [[nodiscard]] SHMatrix        ComputeWorldTransform ()                    const noexcept override;
 
+    [[nodiscard]] SHBox           ComputeAABB           ()                    const noexcept override;
+
   private:
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */

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

@@ -0,0 +1,60 @@
+/****************************************************************************************
+ * \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 "SHCollisionID.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:
+    SHCollisionID     info;
+    SHCollisionState  state;
+  };
+
+  /**
+   * @brief
+   * Encapsulates the event for an intersection between two collision shapes that do
+   * have physical resolution.
+  */
+  struct SH_API SHCollisionEvent
+  {
+  public:
+    static constexpr int MAX_NUM_CONTACTS = 4;
+
+    SHCollisionID     info;
+    SHCollisionState  state;
+    SHVec3            normal;
+    SHVec3            contactPoints[MAX_NUM_CONTACTS];
+  };
+
+} // namespace SHADE

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

@@ -0,0 +1,152 @@
+/****************************************************************************************
+ * \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 "SHCollisionID.h"
+
+// Project Headers
+#include "Physics/Collision/SHCollider.h"
+#include "Physics/Interface/SHColliderComponent.h"
+
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor Definitions                                             */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionID::SHCollisionID() 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();
+  }
+
+  SHCollisionID::SHCollisionID(const SHCollisionID& rhs) noexcept
+  {
+    value[0] = rhs.value[0];
+    value[1] = rhs.value[1];
+  }
+
+  SHCollisionID::SHCollisionID(SHCollisionID&& rhs) noexcept
+  {
+    value[0] = rhs.value[0];
+    value[1] = rhs.value[1];
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Operator Overload Definitions                                                     */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollisionID& SHCollisionID::operator=(const SHCollisionID& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    value[0] = rhs.value[0];
+    value[1] = rhs.value[1];
+
+    return *this;
+  }
+
+  SHCollisionID& SHCollisionID::operator=(SHCollisionID&& rhs) noexcept
+  {
+    value[0] = rhs.value[0];
+    value[1] = rhs.value[1];
+
+    return *this;
+  }
+
+  bool SHCollisionID::operator==(const SHCollisionID& 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 SHCollisionID::GetEntityA() const noexcept
+  {
+    return ids[ENTITY_A];
+  }
+
+  EntityID SHCollisionID::GetEntityB() const noexcept
+  {
+    return ids[ENTITY_B];
+  }
+
+  uint32_t SHCollisionID::GetShapeIndexA() const noexcept
+  {
+    return ids[SHAPE_INDEX_A];
+  }
+
+  uint32_t SHCollisionID::GetShapeIndexB() const noexcept
+  {
+    return ids[SHAPE_INDEX_B];
+  }
+
+  const SHRigidBodyComponent* SHCollisionID::GetRigidBodyA() const noexcept
+  {
+    return SHComponentManager::GetComponent_s<SHRigidBodyComponent>(ids[ENTITY_A]);
+  }
+
+  const SHRigidBodyComponent* SHCollisionID::GetRigidBodyB() const noexcept
+  {
+    return SHComponentManager::GetComponent_s<SHRigidBodyComponent>(ids[ENTITY_B]);
+  }
+
+  const SHCollisionShape* SHCollisionID::GetCollisionShapeA() const noexcept
+  {
+    const auto* COLLIDER_COMPONENT = SHComponentManager::GetComponent<SHColliderComponent>(ids[ENTITY_A]);
+    return COLLIDER_COMPONENT->GetCollider()->GetCollisionShape(ids[SHAPE_INDEX_A]);
+  }
+
+  const SHCollisionShape* SHCollisionID::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 SHCollisionID::SetEntityA(EntityID entityID) noexcept
+  {
+    ids[ENTITY_A] = entityID;
+  }
+
+  void SHCollisionID::SetEntityB(EntityID entityID) noexcept
+  {
+    ids[ENTITY_B] = entityID;
+  }
+
+  void SHCollisionID::SetCollisionShapeA(uint32_t shapeIndexA) noexcept
+  {
+    ids[SHAPE_INDEX_A] = shapeIndexA;
+  }
+
+  void SHCollisionID::SetCollisionShapeB(uint32_t shapeIndexB) noexcept
+  {
+    ids[SHAPE_INDEX_B] = shapeIndexB;
+  }
+
+} // namespace SHADE

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

@@ -0,0 +1,121 @@
+/****************************************************************************************
+ * \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 SHCollisionIDHash;
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  /**
+   * @brief
+   * Encapsulates the information when two colliders intersect and do not have physical
+   * resolution.
+  */
+  class SH_API SHCollisionID
+  {
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Friends                                                                         */
+    /*---------------------------------------------------------------------------------*/
+
+    friend struct SHCollisionIDHash;
+
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionID   ()                          noexcept;
+    SHCollisionID   (const SHCollisionID& rhs)  noexcept;
+    SHCollisionID   (SHCollisionID&& rhs)       noexcept;
+
+    ~SHCollisionID  () noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHCollisionID& operator= (const SHCollisionID& rhs) noexcept;
+    SHCollisionID& operator= (SHCollisionID&& rhs)      noexcept;
+
+    bool operator==(const SHCollisionID& 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 CollisionID
+  */
+  struct SHCollisionIDHash
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    inline std::size_t operator()(const SHCollisionID& id) const
+    {
+      return std::hash<std::u32string_view>{}(std::u32string_view(reinterpret_cast<std::basic_string_view<char32_t>::const_pointer>(id.ids), 4));
+    }
+  };
+
+} // namespace SHADE

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

@@ -0,0 +1,70 @@
+/****************************************************************************************
+ * \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/CollisionShapes/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 incomingIncident;
+      uint8_t outgoingIncident;
+      uint8_t incomingReference;
+      uint8_t outgoingReference;
+    };
+
+    uint32_t key = 0;
+  };
+
+  /**
+   * @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;
+    float             normalImpulse                 = 0.0f;
+    float             normalMass                    = 0.0f;
+    float             tangentImpulse[NUM_TANGENTS]  = { 0.0f };
+    float             tangentMass[NUM_TANGENTS]     = { 0.0f };
+
+    SHVec3            position;
+    SHContactFeatures featurePair;
+  };
+}
+
+#pragma once

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

@@ -0,0 +1,46 @@
+/****************************************************************************************
+ * \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/CollisionShapes/SHCollisionShape.h"
+#include "SHContact.h"
+#include "SHCollisionEvents.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  struct SH_API SHManifold
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    // We only need 4 contact points to build a stable manifold.
+    static constexpr int MAX_NUM_CONTACTS = 4;
+
+    SHCollisionShape* A = nullptr;
+    SHCollisionShape* B = nullptr;
+
+    SHVec3            normal;
+    SHVec3            tangents[SHContact::NUM_TANGENTS];
+
+    SHContact         contacts[MAX_NUM_CONTACTS];
+    uint32_t          numContacts = 0;
+    SHCollisionState  state;
+  };
+  
+}

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

@@ -0,0 +1,52 @@
+/****************************************************************************************
+ * \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 "Physics/Collision/CollisionShapes/SHCollisionShape.h"
+#include "Physics/Collision/Contacts/SHManifold.h"
+#include "Physics/Collision/Contacts/SHCollisionID.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;
+    
+
+    /* Capsule VS X */
+
+    /* Polygon VS X */
+
+  private:
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+  };
+} // namespace SHADE

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

@@ -0,0 +1,95 @@
+/****************************************************************************************
+ * \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]
+  {
+    // TODO
+    // <SHAPE> vs Sphere / Box / Capsule
+
+    { SHCollision::SphereVsSphere, nullptr, nullptr }  // Sphere
+  , { nullptr, nullptr, nullptr }  // Box
+  , { nullptr, nullptr, nullptr }  // Capsule
+  };
+
+  const SHCollisionDispatcher::TriggerCollide SHCollisionDispatcher::triggerCollide[NUM_SHAPES][NUM_SHAPES]
+  {
+    // TODO
+    // <SHAPE> vs Sphere / Box / Capsule
+
+    { SHCollision::SphereVsSphere, nullptr, nullptr }  // Sphere
+  , { nullptr, nullptr, nullptr }  // Box
+  , { nullptr, nullptr, nullptr }  // Capsule
+  };
+
+  const bool SHCollisionDispatcher::collisionTable[NUM_TYPES][NUM_TYPES]
+  {
+    /*          S       ST      K       KT      D       DT    */
+    /* S  */  { false,  false,  false,  true,   true,   true }
+  , /* ST */  { false,  false,  true,   true,   true,   true }
+  , /* K  */  { false,  true,   false,  true,   true,   true }
+  , /* KT */  { true,   true,   true,   true,   true,   true }
+  , /* D  */  { true,   true,   true,   true,   true,   true }
+  , /* DT */  { true,   true,   true,   true,   true,   true }
+  };
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+  bool SHCollisionDispatcher::ShouldCollide(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    // Filter through collision table
+    const int TYPE_A = SHUtilities::ConvertEnum(A.GetType()) + A.IsTrigger() ? TYPE_OFFSET : 0;
+    const int TYPE_B = SHUtilities::ConvertEnum(B.GetType()) + B.IsTrigger() ? TYPE_OFFSET : 0;
+
+    if (!collisionTable[TYPE_A][TYPE_B])
+      return false;
+
+    // Filter through tags
+    const uint16_t TAG_A = A.GetCollisionTag().GetMask();
+    const uint16_t TAG_B = B.GetCollisionTag().GetMask();
+
+    const uint16_t MATCH = TAG_A & TAG_B;
+    return MATCH > 0;
+  }
+
+  bool SHCollisionDispatcher::Collide(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const int TYPE_A = SHUtilities::ConvertEnum(A.GetType());
+    const int TYPE_B = SHUtilities::ConvertEnum(B.GetType());
+
+    return manifoldCollide[TYPE_A][TYPE_B](manifold, A, B);
+  }
+
+  bool SHCollisionDispatcher::Collide(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const int TYPE_A = SHUtilities::ConvertEnum(A.GetType());
+    const int TYPE_B = SHUtilities::ConvertEnum(B.GetType());
+
+    return triggerCollide[TYPE_A][TYPE_B](A, B);
+  }
+
+} // namespace SHADE

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

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

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

@@ -0,0 +1,78 @@
+/****************************************************************************************
+ * \file      SHSphereVsSphere.cpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation for the Detecting Collisions between two spheres
+ *
+ * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
+ *            disclosure of this file or its contents without the prior written consent
+ *            of DigiPen Institute of Technology is prohibited.
+****************************************************************************************/
+
+#include <SHpch.h>
+
+// Primary Header
+#include "SHCollision.h"
+
+// Project Headers
+#include "Math/SHMathHelpers.h"
+#include "Physics/Collision/CollisionShapes/SHSphereCollisionShape.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Public Member Functions Definitions                                               */
+  /*-----------------------------------------------------------------------------------*/
+
+   bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    const SHSphereCollisionShape& SPHERE_A = dynamic_cast<const SHSphereCollisionShape&>(A);
+    const SHSphereCollisionShape& SPHERE_B = dynamic_cast<const SHSphereCollisionShape&>(B);
+
+    return SHSphere::Intersect(SPHERE_A, SPHERE_B);
+  }
+
+  bool SHCollision::SphereVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    // Convert to spheres
+    const SHSphereCollisionShape& SPHERE_A = dynamic_cast<const SHSphereCollisionShape&>(A);
+    const SHSphereCollisionShape& SPHERE_B = dynamic_cast<const SHSphereCollisionShape&>(B);
+
+    const SHVec3  A_TO_B                           = SPHERE_B.GetCenter() - SPHERE_A.GetCenter();
+    const float   DISTANCE_BETWEEN_CENTERS_SQUARED  = A_TO_B.LengthSquared();
+
+    const float   COMBINED_RADIUS                   = (SPHERE_A.GetWorldRadius() + SPHERE_B.GetWorldRadius());
+    const float   COMBINED_RADIUS_SQUARED           = COMBINED_RADIUS * COMBINED_RADIUS;
+
+    if (DISTANCE_BETWEEN_CENTERS_SQUARED > COMBINED_RADIUS_SQUARED)
+      return false;
+
+    // Only populate the manifold if there is a collision
+
+    uint32_t numContacts = 0;
+
+    SHContact contact;
+    contact.featurePair.key = 0;
+
+    if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f))
+    {
+      manifold.normal     = SHVec3::UnitY;
+      contact.position    = SPHERE_A.GetCenter();
+      contact.penetration = SPHERE_B.GetWorldRadius();
+
+      manifold.contacts[numContacts++] = contact;
+    }
+    else
+    {
+      manifold.normal     = SHVec3::Normalise(A_TO_B);
+      contact.position    = SPHERE_B.GetCenter() - (manifold.normal * SPHERE_B.GetWorldRadius());
+      contact.penetration = COMBINED_RADIUS - A_TO_B.Length();
+
+      manifold.contacts[numContacts++] = contact;
+    }
+
+    manifold.numContacts = numContacts;
+    
+    return true;
+  }
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCollider.cpp

@@ -14,6 +14,7 @@
 #include "SHCollider.h"
 
 // Project Headers
+#include "Broadphase/SHDynamicAABBTree.h"
 #include "Events/SHEvent.h"
 #include "Math/SHMathHelpers.h"
 #include "Physics/SHPhysicsEvents.h"
@@ -28,19 +29,21 @@ namespace SHADE
 
   SHCollider::SHCollider(EntityID eid, const SHTransform& worldTransform) noexcept
   : entityID        { eid }
-  , shapeIDCounter  { 0 }
   , debugDraw       { false }
+  , hasMoved        { true }
   , rigidBody       { nullptr }
   , shapeFactory    { nullptr }
+  , broadphase      { nullptr }
   , transform       { worldTransform }
   {}
 
   SHCollider::SHCollider(const SHCollider& rhs) noexcept
   : entityID        { rhs.entityID }
-  , shapeIDCounter  { rhs.shapeIDCounter }
   , debugDraw       { rhs.debugDraw }
+  , hasMoved        { rhs.hasMoved }
   , rigidBody       { rhs.rigidBody }
   , shapeFactory    { rhs.shapeFactory }
+  , broadphase      { rhs.broadphase }
   , transform       { rhs.transform }
   {
     if (!shapeFactory)
@@ -54,10 +57,11 @@ namespace SHADE
 
   SHCollider::SHCollider(SHCollider&& rhs) noexcept
   : entityID        { rhs.entityID }
-  , shapeIDCounter  { rhs.shapeIDCounter }
   , debugDraw       { rhs.debugDraw }
+  , hasMoved        { rhs.hasMoved }
   , rigidBody       { rhs.rigidBody }
   , shapeFactory    { rhs.shapeFactory }
+  , broadphase      { rhs.broadphase }
   , transform       { rhs.transform }
   {
     if (!shapeFactory)
@@ -98,8 +102,10 @@ namespace SHADE
 
     entityID      = rhs.entityID;
     debugDraw     = rhs.debugDraw;
+    hasMoved      = rhs.hasMoved;
     rigidBody     = rhs.rigidBody;
     shapeFactory  = rhs.shapeFactory;
+    broadphase    = rhs.broadphase;
     transform     = rhs.transform;
 
     copyShapes(rhs);
@@ -117,8 +123,10 @@ namespace SHADE
 
     entityID      = rhs.entityID;
     debugDraw     = rhs.debugDraw;
+    hasMoved      = rhs.hasMoved;
     rigidBody     = rhs.rigidBody;
     shapeFactory  = rhs.shapeFactory;
+    broadphase    = rhs.broadphase;
     transform     = rhs.transform;
 
     copyShapes(rhs);
@@ -199,21 +207,25 @@ namespace SHADE
 
   void SHCollider::SetTransform(const SHTransform& newTransform) noexcept
   {
+    hasMoved = true;
     transform = newTransform;
   }
 
   void SHCollider::SetPosition(const SHVec3& newPosition) noexcept
   {
+    hasMoved = true;
     transform.position = newPosition;
   }
 
   void SHCollider::SetOrientation(const SHQuaternion& newOrientation) noexcept
   {
+    hasMoved = true;
     transform.orientation = newOrientation;
   }
 
   void SHCollider::SetScale(const SHVec3& newScale) noexcept
   {
+    hasMoved = true;
     transform.scale = newScale;
   }
 
@@ -255,10 +267,10 @@ namespace SHADE
     , .Scale          = SPHERE_SCALE
     };
 
-    const SHCollisionShapeID NEW_SHAPE_ID{ entityID, shapeIDCounter };
+    const uint32_t NEW_INDEX = static_cast<uint32_t>(shapes.size());
+    const SHCollisionShapeID NEW_SHAPE_ID{ entityID, NEW_INDEX };
 
     SHSphereCollisionShape* sphere = shapeFactory->CreateSphere(NEW_SHAPE_ID, SPHERE_CREATE_INFO);
-    ++shapeIDCounter;
 
     // Set offsets
     sphere->SetParentTransform(transform);
@@ -267,12 +279,15 @@ namespace SHADE
 
     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>(shapes.size())
+    , .colliderIndex  = static_cast<int>(NEW_INDEX)
     };
 
     SHEventManager::BroadcastEvent<SHPhysicsColliderAddedEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_ADDED_EVENT);
@@ -280,7 +295,7 @@ namespace SHADE
     if (rigidBody)
       rigidBody->ComputeMassData();
 
-    return static_cast<int>(shapes.size());
+    return static_cast<int>(NEW_INDEX);
   }
 
   void SHCollider::RemoveCollisionShape(int index)
@@ -296,8 +311,8 @@ namespace SHADE
     if (index < 0 || index >= NUM_SHAPES)
       throw std::invalid_argument("Out-of-range index!");
 
-    auto shapeIter = shapes.begin();
+    auto shape = shapes.begin();
-    for (int i = 0; i < NUM_SHAPES; ++i, ++shapeIter)
+    for (int i = 0; i < NUM_SHAPES; ++i, ++shape)
     {
       if (i == index)
         break;
@@ -306,15 +321,21 @@ namespace SHADE
     const SHPhysicsColliderRemovedEvent EVENT_DATA
     {
       .entityID       = entityID
-    , .colliderType   = (*shapeIter)->GetType()
+    , .colliderType   = (*shape)->GetType()
     , .colliderIndex  = index
     };
 
-    shapeFactory->DestroyShape(*shapeIter);
+     // Remove from broadphase
-    *shapeIter = nullptr;
+    if (broadphase)
+      broadphase->Remove((*shape)->id);
+
+    shapeFactory->DestroyShape(*shape);
+    *shape = nullptr;
 
     // Remove the shape from the container to prevent accessing a nullptr
-    shapeIter = shapes.erase(shapeIter);
+    shape = shapes.erase(shape);
+
+   
 
     // Broadcast Event for removing a shape
     SHEventManager::BroadcastEvent<SHPhysicsColliderRemovedEvent>(EVENT_DATA, SH_PHYSICS_COLLIDER_REMOVED_EVENT);
@@ -337,12 +358,8 @@ namespace SHADE
 
   void SHCollider::copyShapes(const SHCollider& rhsCollider)
   {
-    shapeIDCounter = 0;
     for (const auto* shape : rhsCollider.shapes)
-    {
       copyShape(shape);
-      ++shapeIDCounter;
-    }
   }
 
   void SHCollider::copyShape(const SHCollisionShape* rhsShape)
@@ -365,7 +382,8 @@ namespace SHADE
         , .Scale          = RHS_SPHERE->scale
         };
 
-        const SHCollisionShapeID NEW_SHAPE_ID{ entityID, shapeIDCounter };
+        const uint32_t NEW_INDEX = static_cast<uint32_t>(shapes.size());
+        const SHCollisionShapeID NEW_SHAPE_ID{ entityID, NEW_INDEX };
 
         SHSphereCollisionShape* sphere = shapeFactory->CreateSphere(NEW_SHAPE_ID, SPHERE_CREATE_INFO);
         *sphere = *RHS_SPHERE;
@@ -380,8 +398,6 @@ namespace SHADE
       }
       default: break;
     }
-
-    ++shapeIDCounter;
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHCollider.h

@@ -22,6 +22,7 @@ namespace SHADE
   /*-----------------------------------------------------------------------------------*/
 
   class SHRigidBody;
+  class SHAABBTree;
 
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
@@ -155,12 +156,13 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     EntityID                  entityID;
-    uint32_t                  shapeIDCounter;  // This increments everytime a shape is added to differentiate shapes.
 
     bool                      debugDraw;
+    bool                      hasMoved;
 
     SHRigidBody*              rigidBody;
     SHCollisionShapeFactory*  shapeFactory;
+    SHAABBTree*               broadphase;
 
     SHTransform               transform;
 

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp

@@ -14,6 +14,8 @@
 #include "SHPhysicsWorld.h"
 
 // Project Headers
+#include "Physics/Collision/Narrowphase/SHCollision.h"
+#include "Physics/Collision/Narrowphase/SHCollisionDispatch.h"
 
 
 namespace SHADE
@@ -25,13 +27,52 @@ namespace SHADE
   SHPhysicsWorld::SHPhysicsWorld(const WorldSettings& worldSettings) noexcept
   : settings    { worldSettings }
   {
-    rigidBodies.clear();
     SHLOG_INFO_D("Creating Physics World")
   }
 
   SHPhysicsWorld::~SHPhysicsWorld() noexcept
   {
-    rigidBodies.clear();
+    
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Getter Functions Definitions                                                      */
+  /*-----------------------------------------------------------------------------------*/
+
+  const SHPhysicsWorld::TriggerEvents& SHPhysicsWorld::GetTriggerEvents() const noexcept
+  {
+    static TriggerEvents triggerEvents;
+
+    triggerEvents.clear();
+
+    for (auto& [id, state] : triggers)
+      triggerEvents.emplace_back(SHTriggerEvent{ id, state });
+
+    return triggerEvents;
+  }
+
+  const SHPhysicsWorld::CollisionEvents& SHPhysicsWorld::GetCollisionEvents() const noexcept
+  {
+    static CollisionEvents collisionEvents;
+
+    collisionEvents.clear();
+
+    for (auto& [id, manifold] : manifolds)
+    {
+      SHCollisionEvent collisionEvent
+      {
+        .info   = id
+      , .state  = manifold.state
+      , .normal = manifold.normal
+      };
+
+      for (uint32_t i = 0; i < manifold.numContacts; ++i)
+        collisionEvent.contactPoints[i] = manifold.contacts[i].position;
+
+      collisionEvents.emplace_back(collisionEvent);
+    }
+
+    return collisionEvents;
   }
 
   /*-----------------------------------------------------------------------------------*/
@@ -40,7 +81,7 @@ namespace SHADE
 
   void SHPhysicsWorld::AddRigidBody(SHRigidBody* rigidBody) noexcept
   {
-    const bool INSERTED = rigidBodies.emplace(rigidBody).second;
+    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)
@@ -49,37 +90,89 @@ namespace SHADE
 
   void SHPhysicsWorld::RemoveRigidBody(SHRigidBody* rigidBody) noexcept
   {
-    rigidBodies.erase(rigidBody);
+    rigidBodies.erase(rigidBody->entityID);
 
-    // Run through the rigid body's contact graph and wake all of its non-static bodies that are asleep
+    // Attempt to remove any invalidated manifolds
+    if (manifolds.empty())
+      return;
+
+    for (auto manifoldPair = manifolds.begin(); manifoldPair != manifolds.end();)
+    {
+      const auto& ID = manifoldPair->first;
+
+      const bool MATCHES_A = ID.GetEntityA() == rigidBody->entityID;
+      const bool MATCHES_B = ID.GetEntityB() == rigidBody->entityID;
+
+      if (MATCHES_A || MATCHES_B)
+        manifoldPair = manifolds.erase(manifoldPair);
+      else
+        ++manifoldPair;
+    }
+
+    // TODO: Run through the rigid body's contact graph and wake all of its non-static bodies that are asleep
   }
 
   void SHPhysicsWorld::AddCollider(SHCollider* collider) noexcept
   {
-    const bool INSERTED = colliders.emplace(collider).second;
+    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 SHPhysicsWorld::RemoveCollider(SHCollider* collider) noexcept
   {
-    colliders.erase(collider);
+    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)
+    {
+      const SHCollisionShape* SHAPE = collider->shapes[i];
+
+      broadphase.Remove(SHAPE->id);
+
+      if (SHAPE->IsTrigger())
+        removeInvalidatedTrigger(collider->entityID, i);
+      else
+        removeInvalidatedManifold(collider->entityID, i);
+    }
+
+    /*
+     * 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
+     */
+
     
-    // 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 SHPhysicsWorld::Step(float dt)
   {
-    for (auto* rigidBody : rigidBodies)
+    // Clear containers of exit state collisions
+    updateEvents();
+
+    // TODO: Profile each of these
+    runBroadphase ();
+    runNarrowphase();
+
+    for (auto* rigidBody : rigidBodies | std::views::values)
     {
       rigidBody->ComputeWorldData();
       integrateForces(*rigidBody, dt);
     }
 
-    for (auto* rigidBody : rigidBodies)
+    for (auto* rigidBody : rigidBodies | std::views::values)
       integrateVelocities(*rigidBody, dt);
   }
 
@@ -158,4 +251,262 @@ namespace SHADE
     // We clear forces for static bodies as well for redundancy
     rigidBody.ClearForces();
   }
+
+  void SHPhysicsWorld::runBroadphase() noexcept
+  {
+    // Update any colliders that have moved
+    for (auto& collider : colliders | std::views::values)
+    {
+      if (!collider->hasMoved)
+        continue;
+      
+      // Clear hasMoved flag here
+      collider->hasMoved = false;
+
+      // Update moved shapes in broadphase
+      for (auto* shape : collider->shapes)
+        broadphase.Update(shape->id, shape->ComputeAABB());
+    }
+
+    // Query: Kinematic Triggers, Awake Dynamic Bodies & Dynamic Triggers
+    for (auto& collider : colliders | std::views::values)
+    {
+      // Default static bodies
+      if (!collider->rigidBody) 
+        continue;
+
+      // Explicit static bodies
+      const bool IS_STATIC = collider->rigidBody->GetType() == SHRigidBody::Type::STATIC;
+      if (IS_STATIC)
+        continue;
+
+      // All remaining are kinematic or dynamic
+      // Iterate through shapes: if kinematic / dynamic trigger, else if dynamic & awake
+      if (collider->rigidBody->GetType() == SHRigidBody::Type::KINEMATIC)
+        queryKinematic(collider);
+
+      if (collider->rigidBody->GetType() == SHRigidBody::Type::DYNAMIC)
+        queryDynamic(collider);
+    }
+
+  }
+
+  void SHPhysicsWorld::queryKinematic(SHCollider* collider) noexcept
+  {
+    for (auto* shape : collider->shapes)
+    {
+      // For kinematic shapes, we only query triggers against everything else
+      if (!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
+        SHCollisionID 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.
+        auto narrowphasePair = narrowphaseBatch.find(collisionKey);
+        if (narrowphasePair == narrowphaseBatch.end())
+          narrowphaseBatch.emplace(collisionKey, NarrowphasePair{ shapeA, shapeB });
+      }
+    }
+  }
+
+  void SHPhysicsWorld::queryDynamic(SHCollider* collider) noexcept
+  {
+    for (auto* shape : collider->shapes)
+    {
+      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
+        SHCollisionID 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.
+        auto narrowphasePair = narrowphaseBatch.find(collisionKey);
+        if (narrowphasePair == narrowphaseBatch.end())
+          narrowphaseBatch.emplace(collisionKey, NarrowphasePair{ shapeA, shapeB });
+      }
+    }
+  }
+
+  void SHPhysicsWorld::runNarrowphase() noexcept
+  {
+    for (auto& [id, narrowphasePair] : narrowphaseBatch)
+    {
+      const bool IS_A_TRIGGER = narrowphasePair.A->IsTrigger();
+      const bool IS_B_TRIGGER = narrowphasePair.B->IsTrigger();
+
+      // Check if ID exists in trigger
+      if (IS_A_TRIGGER || IS_B_TRIGGER)
+        collideTriggers(id, narrowphasePair);
+      else
+        collideManifolds(id, narrowphasePair);
+    }
+  }
+
+  void SHPhysicsWorld::collideTriggers(const SHCollisionID& id, NarrowphasePair& narrowphasePair) noexcept
+  {
+    const auto* A = narrowphasePair.A;
+    const auto* B = narrowphasePair.B;
+
+    const bool COLLIDING = SHCollisionDispatcher::Collide(*A, *B);
+
+    auto trigger = triggers.find(id);
+
+    // If id not found, emplace new object.
+    // New object is in the invalid state
+    if (trigger == triggers.end())
+      trigger = triggers.emplace(id, SHCollisionState::INVALID).first;
+
+    SHCollisionState& state = trigger->second;
+    updateCollisionState(COLLIDING, state);
+
+    // If it was a false positive, remove the manifold immediately.
+    // Remove using iterator as it is on average faster.
+    if (state == SHCollisionState::INVALID)
+      trigger = triggers.erase(trigger);
+  }
+
+  void SHPhysicsWorld::collideManifolds(const SHCollisionID& id, NarrowphasePair& narrowphasePair) noexcept
+  {
+    auto* A = narrowphasePair.A;
+    auto* B = narrowphasePair.B;
+
+    SHManifold newManifold { .A = A, .B = B };
+    const bool COLLIDING = SHCollisionDispatcher::Collide(newManifold, *A, *B);
+
+    auto manifold = manifolds.find(id);
+
+    // If id not found, emplace new manifold
+    if (manifold == manifolds.end())
+      manifold = manifolds.emplace(id, newManifold).first;
+    else
+    {
+      // TODO: Update existing manifolds with new data
+    }
+
+    SHCollisionState& state = manifold->second.state;
+    updateCollisionState(COLLIDING, state);
+
+    // If it was a false positive, remove the manifold immediately.
+    // Remove using iterator as it is on average faster.
+    if (state == SHCollisionState::INVALID)
+      manifold = manifolds.erase(manifold);
+  }
+
+  void SHPhysicsWorld::updateCollisionState(bool isColliding, SHCollisionState& state) noexcept
+  {
+     if (isColliding)
+      state = state == SHCollisionState::INVALID ? SHCollisionState::ENTER : SHCollisionState::STAY;
+    else
+      state = SHCollisionState::EXIT;
+  }
+
+  void SHPhysicsWorld::updateEvents() noexcept
+  {
+    // Clear expired or invalid collisions
+    for (auto manifold = manifolds.begin(); manifold != manifolds.end();)
+    {
+      const auto COLLISION_STATE  = manifold->second.state;
+
+      const bool IS_EXIT          = COLLISION_STATE == SHCollisionState::EXIT;
+      const bool IS_INVALID       = COLLISION_STATE == SHCollisionState::INVALID;
+
+      if (IS_EXIT || IS_INVALID)
+        manifold = manifolds.erase(manifold);
+      else
+        ++manifold;
+    }
+
+    // Clear expired or invalid triggers
+    for (auto trigger = triggers.begin(); trigger != triggers.end();)
+    {
+      const auto COLLISION_STATE  = trigger->second;
+
+      const bool IS_EXIT          = COLLISION_STATE == SHCollisionState::EXIT;
+      const bool IS_INVALID       = COLLISION_STATE == SHCollisionState::INVALID;
+
+      if (IS_EXIT || IS_INVALID)
+        trigger = triggers.erase(trigger);
+      else
+        ++trigger;
+    }
+  }
+
+  void SHPhysicsWorld::removeInvalidatedTrigger(EntityID eid, uint32_t shapeIndex)
+  {
+    if (triggers.empty())
+      return;
+
+    for (auto invalidatedTrigger = triggers.begin(); invalidatedTrigger != triggers.end();)
+    {
+      const auto& ID = invalidatedTrigger->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)
+        invalidatedTrigger = triggers.erase(invalidatedTrigger);
+      else
+        ++invalidatedTrigger;
+    }
+  }
+
+  void SHPhysicsWorld::removeInvalidatedManifold(EntityID eid, uint32_t shapeIndex)
+  {
+    if (manifolds.empty())
+      return;
+
+    for (auto invalidatedManifold = manifolds.begin(); invalidatedManifold != manifolds.end();)
+    {
+      const auto& ID = invalidatedManifold->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)
+        invalidatedManifold = manifolds.erase(invalidatedManifold);
+      else
+        ++invalidatedManifold;
+    }
+  }
+
+
+
 } // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h

@@ -10,9 +10,12 @@
 
 #pragma once
 
-#include <unordered_set>
+#include <unordered_map>
 
 // Project Headers
+#include "Physics/Collision/Broadphase/SHDynamicAABBTree.h"
+#include "Physics/Collision/Contacts/SHCollisionEvents.h"
+#include "Physics/Collision/Contacts/SHManifold.h"
 #include "Physics/Collision/SHCollider.h"
 #include "SHRigidBody.h"
 
@@ -44,6 +47,9 @@ namespace SHADE
       bool      sleepingEnabled             = true;
     };
 
+    using TriggerEvents     = std::vector<SHTriggerEvent>;
+    using CollisionEvents   = std::vector<SHCollisionEvent>;
+
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
     /*---------------------------------------------------------------------------------*/
@@ -54,7 +60,6 @@ namespace SHADE
     SHPhysicsWorld  (const SHPhysicsWorld&) = delete;
     SHPhysicsWorld  (SHPhysicsWorld&&)      = delete;
 
-
     /*---------------------------------------------------------------------------------*/
     /* Operator Overloads                                                              */
     /*---------------------------------------------------------------------------------*/
@@ -63,7 +68,14 @@ namespace SHADE
     SHPhysicsWorld& operator=(SHPhysicsWorld&&)       = delete;
 
     /*---------------------------------------------------------------------------------*/
-    /* Function Members                                                                */
+    /* Getter Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    const TriggerEvents&    GetTriggerEvents    ()  const noexcept;
+    const CollisionEvents&  GetCollisionEvents  ()  const noexcept;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
     void AddRigidBody     (SHRigidBody* rigidBody)  noexcept;
@@ -79,10 +91,22 @@ namespace SHADE
     /* Type Definitions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    //! I realise using a set may be dangerous with pointers. An unordered_map may be better, but I don't need the entityIDs so...
+    struct NarrowphasePair
+    {
+      SHCollisionShape* A = nullptr;
+      SHCollisionShape* B = nullptr;
+    };
+
+    // EntityIDs are used to map resolved contraints back to bodies
+    using RigidBodies       = std::unordered_map<EntityID, SHRigidBody*>;
+    using Colliders         = std::unordered_map<EntityID, SHCollider*>;
+
+    // Collisions
+
+    using NarrowphaseBatch  = std::unordered_map<SHCollisionID, NarrowphasePair, SHCollisionIDHash>;
+    using Manifolds         = std::unordered_map<SHCollisionID, SHManifold, SHCollisionIDHash>;
+    using Triggers          = std::unordered_map<SHCollisionID, SHCollisionState, SHCollisionIDHash>;
 
-    using Colliders   = std::unordered_set<SHCollider*>;
-    using RigidBodies = std::unordered_set<SHRigidBody*>;
     
 
     /*---------------------------------------------------------------------------------*/
@@ -91,16 +115,44 @@ namespace SHADE
 
     WorldSettings     settings;
 
-    Colliders     colliders;
+    // Containers
+
     RigidBodies       rigidBodies;
+    Colliders         colliders;
+
+    NarrowphaseBatch  narrowphaseBatch;
+    Manifolds         manifolds;
+    Triggers          triggers;
+
+    // World components
+
+    SHAABBTree        broadphase;
 
     /*---------------------------------------------------------------------------------*/
     /* 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;
 
+    // Broadphase helpers
+
+    void runBroadphase              () noexcept;
+    void queryKinematic             (SHCollider* collider) noexcept;
+    void queryDynamic               (SHCollider* collider) noexcept;
+
+    // Narrowphase helpers
+
+    void runNarrowphase             () noexcept;
+    void collideTriggers            (const SHCollisionID& id, NarrowphasePair& narrowphasePair) noexcept;
+    void collideManifolds           (const SHCollisionID& id, NarrowphasePair& narrowphasePair) noexcept;
+    void updateCollisionState       (bool isColliding, SHCollisionState& state) noexcept;
+    
+    void updateEvents               () noexcept;
+    void removeInvalidatedTrigger   (EntityID eid, uint32_t shapeIndex);
+    void removeInvalidatedManifold  (EntityID eid, uint32_t shapeIndex);
+
   };