From cf9d4ef04bf518a3a370ad6312dbbdbe352a5f42 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Mon, 19 Dec 2022 16:56:34 +0800 Subject: [PATCH] Implemented backbone for collision detection with broadphase --- Assets/Scenes/PhysicsSandbox.shade | 42 +- SHADE_Engine/src/Math/Geometry/SHBox.h | 2 +- .../Broadphase/SHDynamicAABBTree.cpp | 607 ++++++++++++++++++ .../Collision/Broadphase/SHDynamicAABBTree.h | 150 +++++ .../CollisionShapes/SHCollisionShape.h | 6 +- .../CollisionShapes/SHCollisionShapeID.h | 14 +- .../CollisionShapes/SHCollisionShapeID.hpp | 20 +- .../SHSphereCollisionShape.cpp | 7 +- .../CollisionShapes/SHSphereCollisionShape.h | 3 + .../Collision/Contacts/SHCollisionEvents.h | 60 ++ .../Collision/Contacts/SHCollisionID.cpp | 152 +++++ .../Collision/Contacts/SHCollisionID.h | 121 ++++ .../Physics/Collision/Contacts/SHContact.h | 70 ++ .../Physics/Collision/Contacts/SHManifold.h | 46 ++ .../Collision/Narrowphase/SHCollision.h | 52 ++ .../Narrowphase/SHCollisionDispatch.cpp | 95 +++ .../Narrowphase/SHCollisionDispatch.h | 87 +++ .../Narrowphase/SHSphereVsSphere.cpp | 78 +++ .../src/Physics/Collision/SHCollider.cpp | 56 +- .../src/Physics/Collision/SHCollider.h | 4 +- .../src/Physics/Dynamics/SHPhysicsWorld.cpp | 373 ++++++++++- .../src/Physics/Dynamics/SHPhysicsWorld.h | 74 ++- 22 files changed, 2064 insertions(+), 55 deletions(-) create mode 100644 SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp create mode 100644 SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.h create mode 100644 SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h create mode 100644 SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionID.cpp create mode 100644 SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionID.h create mode 100644 SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h create mode 100644 SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h create mode 100644 SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h create mode 100644 SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionDispatch.cpp create mode 100644 SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionDispatch.h create mode 100644 SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 7381b70c..8817b62e 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/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 @@ -58,4 +58,44 @@ Far: 10000 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: ~ \ No newline at end of file diff --git a/SHADE_Engine/src/Math/Geometry/SHBox.h b/SHADE_Engine/src/Math/Geometry/SHBox.h index a0ca9458..63383c00 100644 --- a/SHADE_Engine/src/Math/Geometry/SHBox.h +++ b/SHADE_Engine/src/Math/Geometry/SHBox.h @@ -77,7 +77,7 @@ namespace SHADE /*---------------------------------------------------------------------------------*/ [[nodiscard]] bool TestPoint (const SHVec3& point) const noexcept override; - [[nodiscard]] SHRaycastResult Raycast(const SHRay& ray) const noexcept override; + [[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept override; [[nodiscard]] bool Contains (const SHBox& rhs) const noexcept; [[nodiscard]] float Volume () const noexcept; diff --git a/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp b/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp new file mode 100644 index 00000000..db30222b --- /dev/null +++ b/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 + +#include + +// 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::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& SHAABBTree::GetAABBs() const noexcept + { + static std::vector aabbs; + static std::stack 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& SHAABBTree::Query(SHCollisionShapeID id, const SHBox& AABB) const noexcept + { + static std::vector potentialCollisions; + static std::stack 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& SHAABBTree::Query(const SHRay& ray, float distance) const noexcept + { + static std::vector 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::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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.h b/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.h new file mode 100644 index 00000000..973631a1 --- /dev/null +++ b/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 + +// 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& 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& Query(SHCollisionShapeID id, const SHBox& AABB) const noexcept; + [[nodiscard]] const std::vector& 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 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; + }; + + +} \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h index 3c08e21a..dad59881 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h +++ b/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() diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeID.h b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeID.h index a0f78177..09f712d1 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeID.h +++ b/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: /*---------------------------------------------------------------------------------*/ @@ -69,8 +73,8 @@ namespace SHADE struct IDs { public: - EntityID entityID = MAX_EID; - uint32_t shapeID = std::numeric_limits::max(); + EntityID entityID = MAX_EID; + uint32_t shapeIndex = std::numeric_limits::max(); }; @@ -84,7 +88,7 @@ namespace SHADE /** * @brief - * Encapsulates a functor to hash a CollisionShapeKey + * Encapsulates a functor to hash a CollisionShapeID */ struct SHCollisionShapeIDHash { diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeID.hpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeID.hpp index 234b2f6f..bb9ccb1f 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeID.hpp +++ b/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()(id.value); + return std::hash{}(id.value); + } + + /*-----------------------------------------------------------------------------------*/ + /* Getter Function Definitions */ + /*-----------------------------------------------------------------------------------*/ + + inline EntityID SHCollisionShapeID::GetEntityID() const noexcept + { + return ids.entityID; + } + + inline uint32_t SHCollisionShapeID::GetShapeIndex() const noexcept + { + return ids.shapeIndex; } } diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp index e4bf68c3..a871a5eb 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp @@ -220,7 +220,12 @@ namespace SHADE Center , ROTATION , SCALE -); + ); + } + + SHBox SHSphereCollisionShape::ComputeAABB() const noexcept + { + return SHBox{ Center, SHVec3{ Radius } }; } diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.h b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.h index 798c70b6..1325fc56 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.h +++ b/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 */ diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h new file mode 100644 index 00000000..7abb397b --- /dev/null +++ b/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 diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionID.cpp b/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionID.cpp new file mode 100644 index 00000000..fae14aca --- /dev/null +++ b/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 + +// 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::max(); + ids[SHAPE_INDEX_B] = std::numeric_limits::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(ids[ENTITY_A]); + } + + const SHRigidBodyComponent* SHCollisionID::GetRigidBodyB() const noexcept + { + return SHComponentManager::GetComponent_s(ids[ENTITY_B]); + } + + const SHCollisionShape* SHCollisionID::GetCollisionShapeA() const noexcept + { + const auto* COLLIDER_COMPONENT = SHComponentManager::GetComponent(ids[ENTITY_A]); + return COLLIDER_COMPONENT->GetCollider()->GetCollisionShape(ids[SHAPE_INDEX_A]); + } + + const SHCollisionShape* SHCollisionID::GetCollisionShapeB() const noexcept + { + const auto* COLLIDER_COMPONENT = SHComponentManager::GetComponent(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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionID.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionID.h new file mode 100644 index 00000000..78b28a96 --- /dev/null +++ b/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(reinterpret_cast::const_pointer>(id.ids), 4)); + } + }; + +} // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h new file mode 100644 index 00000000..c70f6e39 --- /dev/null +++ b/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 diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h new file mode 100644 index 00000000..da6fe233 --- /dev/null +++ b/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; + }; + +} \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h new file mode 100644 index 00000000..befb16f6 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionDispatch.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionDispatch.cpp new file mode 100644 index 00000000..c06e30ae --- /dev/null +++ b/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 + +// 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 + // 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 + // 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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionDispatch.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionDispatch.h new file mode 100644 index 00000000..4c132746 --- /dev/null +++ b/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(SHCollisionShape::Type::COUNT); + static constexpr int NUM_TYPES = static_cast(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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp new file mode 100644 index 00000000..b26a33dd --- /dev/null +++ b/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 + +// 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(A); + const SHSphereCollisionShape& SPHERE_B = dynamic_cast(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(A); + const SHSphereCollisionShape& SPHERE_B = dynamic_cast(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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/SHCollider.cpp b/SHADE_Engine/src/Physics/Collision/SHCollider.cpp index a6527e48..24ba26b7 100644 --- a/SHADE_Engine/src/Physics/Collision/SHCollider.cpp +++ b/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(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(shapes.size()) + , .colliderIndex = static_cast(NEW_INDEX) }; SHEventManager::BroadcastEvent(EVENT_DATA, SH_PHYSICS_COLLIDER_ADDED_EVENT); @@ -280,7 +295,7 @@ namespace SHADE if (rigidBody) rigidBody->ComputeMassData(); - return static_cast(shapes.size()); + return static_cast(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(); - for (int i = 0; i < NUM_SHAPES; ++i, ++shapeIter) + auto shape = shapes.begin(); + 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); - *shapeIter = nullptr; + // Remove from broadphase + 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(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(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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/SHCollider.h b/SHADE_Engine/src/Physics/Collision/SHCollider.h index 566bae3e..89ba5ff2 100644 --- a/SHADE_Engine/src/Physics/Collision/SHCollider.h +++ b/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; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp index b1b94792..4ff94b7d 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp +++ b/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); - // Get collider's rigid body - // Run through the rigid body's contact graph and wake all of its non-static bodies that are asleep + const uint32_t NUM_SHAPES = static_cast(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 + */ + + } 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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h index 499230dd..f0d2cd5b 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h @@ -10,9 +10,12 @@ #pragma once -#include +#include // 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; + using CollisionEvents = std::vector; + /*---------------------------------------------------------------------------------*/ /* 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,27 +91,67 @@ 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; + using Colliders = std::unordered_map; + + // Collisions + + using NarrowphaseBatch = std::unordered_map; + using Manifolds = std::unordered_map; + using Triggers = std::unordered_map; - using Colliders = std::unordered_set; - using RigidBodies = std::unordered_set; /*---------------------------------------------------------------------------------*/ /* Data Members */ /*---------------------------------------------------------------------------------*/ - WorldSettings settings; + WorldSettings settings; - Colliders colliders; - RigidBodies rigidBodies; + // Containers + + RigidBodies rigidBodies; + Colliders colliders; + + NarrowphaseBatch narrowphaseBatch; + Manifolds manifolds; + Triggers triggers; + + // World components + + SHAABBTree broadphase; /*---------------------------------------------------------------------------------*/ /* Function Members */ /*---------------------------------------------------------------------------------*/ - void integrateForces (SHRigidBody& rigidBody, float dt) const noexcept; - void integrateVelocities (SHRigidBody& rigidBody, float dt) const noexcept; + // 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); };