From c484a088fdc13471a1e16d057dae61409ebeec76 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Thu, 5 Jan 2023 01:12:25 +0800 Subject: [PATCH] Added first half of Gauss Map Optimised SAT --- Assets/Scenes/PhysicsSandbox.shade | 88 ++++++++++- .../Collision/Narrowphase/SHCollision.h | 25 ++- .../Narrowphase/SHConvexVsConvex.cpp | 144 ++++++++++++++++-- .../src/Physics/Collision/Shapes/SHBox.cpp | 24 +++ .../src/Physics/Collision/Shapes/SHBox.h | 11 +- .../Shapes/SHCollisionShapeLibrary.cpp | 8 +- .../Collision/Shapes/SHConvexPolyhedron.h | 20 ++- 7 files changed, 291 insertions(+), 29 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 6dea778f..d0985215 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -45,7 +45,7 @@ NumberOfChildren: 0 Components: Camera Component: - Position: {x: 0, y: 4, z: 5} + Position: {x: 1, y: 10, z: 3} Pitch: 0 Yaw: 0 Roll: 0 @@ -62,7 +62,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -1.45715916, y: 7, z: 0.227711335} + Translate: {x: -1.45715916, y: 7.37748241, z: 0.227711335} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true @@ -77,7 +77,7 @@ Interpolate: true Sleeping Enabled: true Freeze Position X: false - Freeze Position Y: false + Freeze Position Y: true Freeze Position Z: false Freeze Rotation X: false Freeze Rotation Y: false @@ -273,4 +273,86 @@ Position Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0} IsActive: true + Scripts: ~ +- EID: 7 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 0.899999976, y: 10, z: 0} + Rotate: {x: -0, y: 0, z: 0.785398185} + Scale: {x: 1, y: 1, z: 1} + IsActive: true + RigidBody Component: + Type: Dynamic + Auto Mass: false + Mass: 1 + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + Use Gravity: true + Gravity Scale: 1 + Interpolate: true + Sleeping Enabled: true + Freeze Position X: false + Freeze Position Y: true + Freeze Position Z: false + Freeze Rotation X: false + Freeze Rotation Y: false + Freeze Rotation Z: false + IsActive: true + Collider Component: + DrawColliders: false + Colliders: + - Is Trigger: true + Collision Tag: 1 + Type: Box + Half Extents: {x: 1, y: 1, z: 1} + Friction: 0.400000006 + Bounciness: 0 + Density: 1 + Position Offset: {x: 0, y: 0, z: 0} + Rotation Offset: {x: 0, y: 0, z: 0} + IsActive: true + Scripts: ~ +- EID: 8 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: -0.5, y: 10, z: 0} + Rotate: {x: -0, y: 0.785398185, z: -0} + Scale: {x: 1, y: 1, z: 1} + IsActive: true + RigidBody Component: + Type: Dynamic + Auto Mass: false + Mass: 1 + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + Use Gravity: true + Gravity Scale: 1 + Interpolate: true + Sleeping Enabled: true + Freeze Position X: false + Freeze Position Y: true + Freeze Position Z: false + Freeze Rotation X: false + Freeze Rotation Y: false + Freeze Rotation Z: false + IsActive: true + Collider Component: + DrawColliders: false + Colliders: + - Is Trigger: true + Collision Tag: 1 + Type: Box + Half Extents: {x: 1, y: 1, z: 1} + Friction: 0.400000006 + Bounciness: 0 + Density: 1 + Position Offset: {x: 0, y: 0, z: 0} + Rotation Offset: {x: 0, y: 0, z: 0} + IsActive: true Scripts: ~ \ 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 index 0ce1f688..8a465d4a 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -13,6 +13,7 @@ // Project Headers #include "Physics/Collision/Contacts/SHManifold.h" #include "Physics/Collision/Contacts/SHCollisionKey.h" +#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h" namespace SHADE @@ -72,11 +73,18 @@ namespace SHADE struct FaceQuery { - bool colliding = false; + bool colliding = false; // Allows for early out int32_t closestFace = -1; float bestDistance = std::numeric_limits::lowest(); }; + struct EdgeQuery + { + int32_t halfEdgeA = -1; + int32_t halfEdgeB = -1; + float bestDistance = std::numeric_limits::lowest(); + }; + /*---------------------------------------------------------------------------------*/ /* Member Functions */ /*---------------------------------------------------------------------------------*/ @@ -93,14 +101,19 @@ namespace SHADE // Convex VS Convex - static bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; + static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; + static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; + + static bool buildMinkowskiFace (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; + static bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; + static float distanceBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; /* * TODO: - * static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; - * static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; - * static bool buildMinkowskiFace (const SHConvexPolyhedron::HalfEdge& edgeA, const SHConvexPolyhedron::HalfEdge& edgeB) noexcept; - * static float distanceBetweenEdges(const SHConvexPolyhedron::HalfEdge& edgeA, const SHConvexPolyhedron::HalfEdge& edgeB, SHConvexPolyhedron& poly) noexcept; + * + * + * + * * static int32_t findIncidentFace (SHConvexPolyhedron& poly, const SHVec3& normal) noexcept; * static uint32_t clip [sutherland-hodgemann clipping] * diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index 6753c453..5eedc2c3 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -15,9 +15,9 @@ // Project Headers #include "Math/SHMathHelpers.h" +#include "Math/Geometry/SHPlane.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h" - namespace SHADE { /*-----------------------------------------------------------------------------------*/ @@ -26,15 +26,22 @@ namespace SHADE bool SHCollision::ConvexVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - /* - * TODO: - * - * 1. Query face directiosn of a to b. Exit early if separation found. - * 2. query face directions of b to a. Exit early if separation found. - * 3. Query edge directions of a & b. Exit early if separation found. - */ + const SHConvexPolyhedron& POLY_A = dynamic_cast(A); + const SHConvexPolyhedron& POLY_B = dynamic_cast(B); - return false; + const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); + if (FACE_QUERY_A.bestDistance > 0.0f) + return false; + + const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); + if (FACE_QUERY_B.bestDistance > 0.0f) + return false; + + const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); + if (EDGE_QUERY.bestDistance > 0.0f) + return false; + + return true; } bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept @@ -61,4 +68,123 @@ namespace SHADE return false; } + /*-----------------------------------------------------------------------------------*/ + /* Private Member Functions Definitions */ + /*-----------------------------------------------------------------------------------*/ + + SHCollision::FaceQuery SHCollision::queryFaceDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept + { + FaceQuery faceQuery; + + const int32_t NUM_FACES = A.GetFaceCount(); + for (const int32_t i : std::views::iota(0, NUM_FACES)) + { + const SHHalfEdgeStructure::Face& FACE_A = A.GetFace(i); + const SHVec3 NORMAL_A = A.GetNormal(i); + + // Smallest penetration is point closest to face normal + const SHVec3 SUPPORT_POINT = B.FindSupportPoint(-NORMAL_A); + + const SHVec3 VERTEX_A = A.GetVertex(FACE_A.vertexIndices[0].index); + + const SHPlane FACE_PLANE { VERTEX_A, NORMAL_A }; + const float DISTANCE = FACE_PLANE.SignedDistance(SUPPORT_POINT); + + if (DISTANCE > faceQuery.bestDistance) + { + faceQuery.bestDistance = DISTANCE; + faceQuery.closestFace = i; + } + } + + return faceQuery; + } + + SHCollision::EdgeQuery SHCollision::queryEdgeDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept + { + EdgeQuery edgeQuery; + + const int32_t EDGE_COUNT_A = A.GetHalfEdgeCount(); + const int32_t EDGE_COUNT_B = B.GetHalfEdgeCount(); + + for (int32_t i = 0; i < EDGE_COUNT_A; i += 2) + { + for (int32_t j = 0; j < EDGE_COUNT_B; j += 2) + { + const bool IS_MINKOWSKI_FACE = buildMinkowskiFace(A, B, i, j); + if (!IS_MINKOWSKI_FACE) + continue; + + const float SEPARATION = distanceBetweenEdges(A, B, i, j); + if (SEPARATION > edgeQuery.bestDistance) + { + edgeQuery.bestDistance = SEPARATION; + edgeQuery.halfEdgeA = i; + edgeQuery.halfEdgeB = j; + } + } + } + + return edgeQuery; + } + + bool SHCollision::buildMinkowskiFace(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept + { + // Get Half Edge from both polygons + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); + const SHHalfEdgeStructure::HalfEdge& TWIN_EDGE_A = A.GetHalfEdge(HALF_EDGE_A.twinEdgeIndex); + + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB); + const SHHalfEdgeStructure::HalfEdge& TWIN_EDGE_B = B.GetHalfEdge(HALF_EDGE_B.twinEdgeIndex); + + + + // Get normals from face and twin edge face + const SHVec3 NA = A.GetNormal(HALF_EDGE_A.faceIndex); + const SHVec3 NB = A.GetNormal(TWIN_EDGE_A.faceIndex); + const SHVec3 NC = B.GetNormal(HALF_EDGE_B.faceIndex); + const SHVec3 ND = B.GetNormal(TWIN_EDGE_B.faceIndex); + + return isMinkowskiFace(NA, NB, -NC, -ND); + } + + bool SHCollision::isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept + { + const SHVec3 BXA = SHVec3::Cross(b, a); + const SHVec3 DXC = SHVec3::Cross(d, c); + + const float CBA = SHVec3::Dot(c, BXA); + const float DBA = SHVec3::Dot(d, BXA); + const float ADC = SHVec3::Dot(a, DXC); + const float BDC = SHVec3::Dot(b, DXC); + + return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f; + } + + float SHCollision::distanceBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept + { + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB); + + const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex); + const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex); + const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex); + const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex); + + const SHVec3 DIR_A = SHVec3::Normalise(HEAD_A - TAIL_A); + const SHVec3 DIR_B = SHVec3::Normalise(HEAD_B - TAIL_B); + + // Check if the edges are parallel (abs dot product is 1) + const float DOT_BETWEEN_EDGES = std::fabs(SHVec3::Dot(DIR_A, DIR_B)); + if (SHMath::CompareFloat(DOT_BETWEEN_EDGES, 1.0f)) + return std::numeric_limits::lowest(); + + SHVec3 normal = SHVec3::Cross(DIR_A, DIR_B); + // Flip normal if need to ( A -> B) + if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) + normal = -normal; + + return SHVec3::Dot(normal, HEAD_B - HEAD_A); + } + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Shapes/SHBox.cpp b/SHADE_Engine/src/Physics/Collision/Shapes/SHBox.cpp index efe40f7d..8022e487 100644 --- a/SHADE_Engine/src/Physics/Collision/Shapes/SHBox.cpp +++ b/SHADE_Engine/src/Physics/Collision/Shapes/SHBox.cpp @@ -291,5 +291,29 @@ namespace SHADE return SHAABB{ CENTROID, HALF_EXTENTS }; } + SHVec3 SHBox::FindSupportPoint(const SHVec3& direction) const noexcept + { + float bestDistance = std::numeric_limits::lowest(); + + + SHVec3 vertices[NUM_VERTICES]; + GetCorners(vertices); + + // No reason to put the center really.. + SHVec3 bestPoint = Center; + for (auto& vertex : vertices) + { + const float PROJECTION = SHVec3::Dot(vertex, direction); + + if (PROJECTION > bestDistance) + { + bestDistance = PROJECTION; + bestPoint = vertex; + } + } + + return bestPoint; + } + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Shapes/SHBox.h b/SHADE_Engine/src/Physics/Collision/Shapes/SHBox.h index 044af40f..480b2501 100644 --- a/SHADE_Engine/src/Physics/Collision/Shapes/SHBox.h +++ b/SHADE_Engine/src/Physics/Collision/Shapes/SHBox.h @@ -109,11 +109,12 @@ namespace SHADE /* Function Members */ /*---------------------------------------------------------------------------------*/ - void Update () noexcept override; - [[nodiscard]] bool TestPoint (const SHVec3& point) const noexcept override; - [[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept override; - [[nodiscard]] SHMatrix GetTRS () const noexcept override; - [[nodiscard]] SHAABB ComputeAABB () const noexcept override; + void Update () noexcept override; + [[nodiscard]] bool TestPoint (const SHVec3& point) const noexcept override; + [[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept override; + [[nodiscard]] SHMatrix GetTRS () const noexcept override; + [[nodiscard]] SHAABB ComputeAABB () const noexcept override; + [[nodiscard]] SHVec3 FindSupportPoint (const SHVec3& direction) const noexcept override; private: /*---------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp b/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp index e0202d67..c70728f3 100644 --- a/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp +++ b/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp @@ -150,11 +150,11 @@ namespace SHADE const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE] { { 0, 1, 2, 3 } - , { 1, 5, 6, 2 } + , { 5, 6, 2, 1 } , { 5, 4, 7, 6 } - , { 4, 0, 3, 7 } - , { 3, 2, 6, 7 } - , { 4, 5, 1, 0 } + , { 0, 3, 7, 4 } + , { 2, 6, 7, 3 } + , { 5, 1, 0, 4 } }; for (int i = 0; i < NUM_FACES; ++i) diff --git a/SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.h b/SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.h index dd4595d1..3944c556 100644 --- a/SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.h +++ b/SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.h @@ -63,8 +63,24 @@ namespace SHADE [[nodiscard]] int32_t GetHalfEdgeCount () const noexcept; [[nodiscard]] const SHHalfEdgeStructure::HalfEdge& GetHalfEdge (int index) const; - [[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0; - [[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0; + // Virtual Methods + + [[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0; + [[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0; + + /*---------------------------------------------------------------------------------*/ + /* Member Functions */ + /*---------------------------------------------------------------------------------*/ + + /** + * @brief + * Finds the most extreme point on the polygon in a given direction. + * @param direction + * The direction to find the support point in. + * @return + * The most extreme vertex in the given direction. + */ + [[nodiscard]] virtual SHVec3 FindSupportPoint (const SHVec3& direction) const noexcept = 0; protected: /*---------------------------------------------------------------------------------*/