From 3a7336fe15d918ec281252f833da001040811bd4 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Sat, 31 Dec 2022 18:43:46 +0800 Subject: [PATCH] Improved stability of sphere vs convex polyhedron except for one edge case --- Assets/Scenes/PhysicsSandbox.shade | 10 +- .../SHCollisionShapeLibrary.cpp | 6 +- .../CollisionShapes/SHCollisionShapeLibrary.h | 6 +- .../CollisionShapes/SHHalfEdgeStructure.cpp | 68 +-- .../CollisionShapes/SHHalfEdgeStructure.h | 53 ++- .../Collision/Narrowphase/SHCollision.h | 16 + .../Narrowphase/SHSphereVsConvex.cpp | 391 +++++++++++------- .../src/Physics/Collision/SHPhysicsMaterial.h | 4 + 8 files changed, 304 insertions(+), 250 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 731df7ce..4230dcb8 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -4,9 +4,9 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 2.58071685, y: 0.456140399, z: 0} - Rotate: {x: -0, y: 0, z: 0.469853699} - Scale: {x: 4.61071014, y: 0.999995053, z: 1} + Translate: {x: 2.72256827, y: 0.501797795, z: -0.0273017883} + Rotate: {x: -1.48352849, y: -4.78713309e-06, z: 0.469859391} + Scale: {x: 4.61070776, y: 0.99999392, z: 0.999996722} IsActive: true RigidBody Component: Type: Static @@ -66,7 +66,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -1.98839664, y: 7.7662077, z: 0} + Translate: {x: -2.49145222, y: 6.17996597, z: 0.811235607} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true @@ -112,7 +112,7 @@ Components: Transform Component: Translate: {x: 0, y: 4.09544182, z: 0} - Rotate: {x: -0, y: 0, z: -0.437829614} + Rotate: {x: -0, y: 0, z: 0} Scale: {x: 4.61071014, y: 0.999995887, z: 1} IsActive: true RigidBody Component: diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp index 338083c7..46adb47b 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp @@ -71,7 +71,7 @@ namespace SHADE box->scale = createInfo.Scale; // Set halfEdge data structure for the box - box->halfEdgeStructure = &boxHalfEdgeDS; + box->halfEdgeStructure = &boxHalfEdgeStructure; return box; } @@ -165,10 +165,10 @@ namespace SHADE for (int j = 0; j < NUM_VERTICES_PER_FACE; ++j) newFace.vertexIndices.emplace_back(FACE_VERTICES[i][j]); - boxHalfEdgeDS.AddFace(newFace); + boxHalfEdgeStructure.AddFace(newFace); } - boxHalfEdgeDS.Build(); + boxHalfEdgeStructure.Build(); } } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.h b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.h index bb9c2807..dbfc4fc7 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.h +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.h @@ -92,10 +92,10 @@ namespace SHADE /* Data Members */ /*---------------------------------------------------------------------------------*/ - SHHalfEdgeStructure boxHalfEdgeDS; + SHHalfEdgeStructure boxHalfEdgeStructure; - Spheres spheres; - Boxes boxes; + Spheres spheres; + Boxes boxes; // TODO: Add capsules and hulls /*---------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.cpp index 4c9304c4..a25b5b8b 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.cpp @@ -23,30 +23,6 @@ namespace SHADE /* Constructors & Destructor Definitions */ /*-----------------------------------------------------------------------------------*/ - SHHalfEdgeStructure::HalfEdge::HalfEdge() noexcept - : tailVertexIndex { -1 } - , headVertexIndex { -1 } - , edgeIndex { -1 } - , twinEdgeIndex { -1 } - , faceIndex { -1 } - {} - - SHHalfEdgeStructure::HalfEdge::HalfEdge(const HalfEdge& rhs) noexcept - : tailVertexIndex { rhs.tailVertexIndex } - , headVertexIndex { rhs.headVertexIndex } - , edgeIndex { rhs.edgeIndex } - , twinEdgeIndex { rhs.twinEdgeIndex } - , faceIndex { rhs.faceIndex } - {} - - SHHalfEdgeStructure::HalfEdge::HalfEdge(HalfEdge&& rhs) noexcept - : tailVertexIndex { rhs.tailVertexIndex } - , headVertexIndex { rhs.headVertexIndex } - , edgeIndex { rhs.edgeIndex } - , twinEdgeIndex { rhs.twinEdgeIndex } - , faceIndex { rhs.faceIndex } - {} - SHHalfEdgeStructure::Face::Face(const Face& rhs) noexcept : normal { rhs.normal } { @@ -63,31 +39,6 @@ namespace SHADE /* Operator Overload Definitions */ /*-----------------------------------------------------------------------------------*/ - SHHalfEdgeStructure::HalfEdge& SHHalfEdgeStructure::HalfEdge::operator=(const HalfEdge& rhs) noexcept - { - if (this == &rhs) - return *this; - - tailVertexIndex = rhs.tailVertexIndex; - headVertexIndex = rhs.headVertexIndex; - edgeIndex = rhs.edgeIndex ; - twinEdgeIndex = rhs.twinEdgeIndex ; - faceIndex = rhs.faceIndex ; - - return *this; - } - - SHHalfEdgeStructure::HalfEdge& SHHalfEdgeStructure::HalfEdge::operator=(HalfEdge&& rhs) noexcept - { - tailVertexIndex = rhs.tailVertexIndex; - headVertexIndex = rhs.headVertexIndex; - edgeIndex = rhs.edgeIndex ; - twinEdgeIndex = rhs.twinEdgeIndex ; - faceIndex = rhs.faceIndex ; - - return *this; - } - SHHalfEdgeStructure::Face& SHHalfEdgeStructure::Face::operator=(const Face& rhs) noexcept { if (this == &rhs) @@ -165,19 +116,19 @@ namespace SHADE // For each face, build half edges for (size_t i = 0; i < faces.size(); ++i) { - const Face& FACE = faces[i]; + Face& face = faces[i]; - if (FACE.vertexIndices.empty()) + if (face.vertexIndices.empty()) { SHLOGV_CRITICAL("Unable to build convex polyhedron, no vertices have been added to face {}!", i) return; } // Iterate through vertices and build half-edges - for (size_t j = 0; j < FACE.vertexIndices.size(); ++j) + for (size_t j = 0; j < face.vertexIndices.size(); ++j) { - const int32_t TAIL = FACE.vertexIndices[j]; - const int32_t HEAD = FACE.vertexIndices[(j + 1) % FACE.vertexIndices.size()]; + const int32_t TAIL = face.vertexIndices[j].index; + const int32_t HEAD = face.vertexIndices[(j + 1) % face.vertexIndices.size()].index; const uint64_t NEW_EDGE_ID = BUILD_UINT64_FROM_UINT32S(TAIL, HEAD); const uint64_t TWIN_EDGE_ID = BUILD_UINT64_FROM_UINT32S(HEAD, TAIL); @@ -197,6 +148,9 @@ namespace SHADE // Set edge index of the newly inserted edge as the size of the map - 1 // Since it is an unordered map, it will just be at the back newHalfEdge.edgeIndex = static_cast(edgeMap.size()) - 1; + + // Map vertex to this edge index + face.vertexIndices[j].edgeIndex = newHalfEdge.edgeIndex; } // Find twin edge if one exists @@ -217,6 +171,12 @@ namespace SHADE // At this point, no duplicates should be in the map and all edges should be linked. for (auto& halfEdge : edgeMap | std::views::values) halfEdges.emplace_back(halfEdge); + + // Sort based on edge indices + std::ranges::sort(halfEdges.begin(), halfEdges.end(), [](const HalfEdge& lhs, const HalfEdge& rhs) + { + return lhs.edgeIndex < rhs.edgeIndex; + }); } } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.h b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.h index c08803ce..8982ae23 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.h +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.h @@ -32,6 +32,10 @@ namespace SHADE /* Type Definitions */ /*---------------------------------------------------------------------------------*/ + /** + * @brief + * Encapsulates the data half edge of a face on a polyhedron. + */ struct HalfEdge { /*-------------------------------------------------------------------------------*/ @@ -40,36 +44,35 @@ namespace SHADE //Head and tail forms the edge. //Head <----- Tail - int32_t tailVertexIndex; + int32_t tailVertexIndex = -1; // Head is also tail of the next edge. - int32_t headVertexIndex; + int32_t headVertexIndex = -1; - int32_t edgeIndex; + int32_t edgeIndex = -1; // Other half of the edge on a different face. // Important for extrapolating face normals. - int32_t twinEdgeIndex; + int32_t twinEdgeIndex = -1; // Adjacent face of this edge. - int32_t faceIndex; - - /*-------------------------------------------------------------------------------*/ - /* Constructors & Destructor */ - /*-------------------------------------------------------------------------------*/ - - HalfEdge () noexcept; - HalfEdge (const HalfEdge& rhs) noexcept; - HalfEdge (HalfEdge&& rhs) noexcept; - ~HalfEdge () noexcept = default; - - /*-------------------------------------------------------------------------------*/ - /* Operator Overloads */ - /*-------------------------------------------------------------------------------*/ - - HalfEdge& operator= (const HalfEdge& rhs) noexcept; - HalfEdge& operator= (HalfEdge&& rhs) noexcept; + int32_t faceIndex = -1; }; + struct Vertex + { + public: + /*-------------------------------------------------------------------------------*/ + /* Data Members */ + /*-------------------------------------------------------------------------------*/ + + int32_t index = -1; + int32_t edgeIndex = -1; // the half-edge that this vertex is a tail of. + }; + + /** + * @brief + * Encapsulates the data of a face on a polyhedron. + */ struct Face { public: @@ -77,10 +80,8 @@ namespace SHADE /* Data Members */ /*-------------------------------------------------------------------------------*/ - // TODO: Store face offset - SHVec3 normal; - std::vector vertexIndices; // Must be in CCW order + std::vector vertexIndices; // Must be in CCW order /*-------------------------------------------------------------------------------*/ /* Constructors & Destructor */ @@ -133,10 +134,6 @@ namespace SHADE /*---------------------------------------------------------------------------------*/ /* Data Members */ /*---------------------------------------------------------------------------------*/ - - float radius = 0.2f; // Default Radius is 2 cm - - // Store the faces and half-edges std::vector faces; std::vector halfEdges; diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index b99cc202..942c6919 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -67,10 +67,26 @@ namespace SHADE [[nodiscard]] static bool ConvexVsConvex (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept; private: + /*---------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*---------------------------------------------------------------------------------*/ + + struct FaceQuery + { + bool colliding = false; + int32_t closestFace = -1; + float bestDistance = std::numeric_limits::lowest(); + }; + /*---------------------------------------------------------------------------------*/ /* Member Functions */ /*---------------------------------------------------------------------------------*/ + // Sphere VS Convex + + static FaceQuery findClosestFace (const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron) noexcept; + static int32_t findClosestPoint (const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron, int32_t faceIndex) noexcept; + static bool isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; // TODO: buildMinkowskiFace, queryEdgeDirection, distanceBetweenTwoEdges, diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index e62bc241..e84bd16b 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -19,16 +19,6 @@ #include "Physics/Collision/CollisionShapes/SHCollisionShape.h" #include "Physics/Collision/CollisionShapes/SHBoxCollisionShape.h" -// When testing against convex polyhedrons, we do not care so much as whether it is a box -// or something else. We only need the vertices to build half edge structures for use -// with a gauss map. Regardless, we still cast it to the type just to get vertices -// since spheres and capsules do not implement the same method. - -// I did consider having another base class that encapsulates methods that convex polyhedrons -// would implement, but for the sake of my sanity, I won't do that. - -// TODO - namespace SHADE { /*-----------------------------------------------------------------------------------*/ @@ -37,6 +27,21 @@ namespace SHADE bool SHCollision::SphereVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept { + const SHSphereCollisionShape& SPHERE = dynamic_cast(A); + const SHConvexPolyhedronCollisionShape& POLYHEDRON = dynamic_cast(B); + + const SHVec3 CENTER = SPHERE.GetCenter(); + const float RADIUS = SPHERE.GetWorldRadius(); + + const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = POLYHEDRON.GetHalfEdgeStructure(); + + const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); + if (!FACE_QUERY.colliding) + return false; + + if (FACE_QUERY.bestDistance < SHMath::EPSILON) + return true; + return false; } @@ -49,19 +54,197 @@ namespace SHADE { // Convert to underlying types // For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual. - const SHSphereCollisionShape& SPHERE = dynamic_cast(A); - const SHConvexPolyhedronCollisionShape& CONVEX = dynamic_cast(B); + const SHSphereCollisionShape& SPHERE = dynamic_cast(A); + const SHConvexPolyhedronCollisionShape& POLYHEDRON = dynamic_cast(B); const SHVec3 CENTER = SPHERE.GetCenter(); const float RADIUS = SPHERE.GetWorldRadius(); + const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = POLYHEDRON.GetHalfEdgeStructure(); + + const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); + if (!FACE_QUERY.colliding) + return false; + + uint32_t numContacts = 0; + const float PENETRATION = RADIUS - FACE_QUERY.bestDistance; + + SHContact contact; + contact.featurePair.key = 0; + + // Check if center is inside polyhedron (below the face) + if (FACE_QUERY.bestDistance < SHMath::EPSILON) + { + manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace); + + contact.penetration = PENETRATION; + contact.position = SPHERE.GetCenter(); + + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; + return true; + } + // Find closest face of polygon to circle + const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace); - int32_t closestFaceIndex = -1; - int32_t closestPointIndex = -1; - float bestDistance = std::numeric_limits::lowest(); + const SHHalfEdgeStructure::Face& FACE = POLYHEDRON.GetHalfEdgeStructure()->GetFace(FACE_QUERY.closestFace); - const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = CONVEX.GetHalfEdgeStructure(); + const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace); + const int32_t NUM_VERTICES = static_cast(FACE.vertexIndices.size()); + + // Check against voronoi regions of the face to determine the type of the intersection test + // We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center + // If none of these are true, the sphere is above the face but not separating + + /* + * | 2 + * _ _ _ _ _ _ | _ _ _ + * / / + * | / regionD | / regionA + * |/ _ _ _ _ _|/ _ _ _ + * 3/ regionB /1 + * / / regionC + * + */ + + const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index); + const SHVec3 P1_TO_CENTER = CENTER - P1; + + // To be inside either region A or B, 2 conditions must be satisfied + // 1. Same side as tangent + // 2. Same side as adjacent normal + + // Check in regions A + { + const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; + const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); + + const SHVec3 TANGENT = SHVec3::Normalise(P2 - P1); + + float projection = SHVec3::Dot(P1_TO_CENTER, TANGENT); + + if (projection >= 0.0f) + { + // Find closest point + const SHVec3 CP = P1 + projection * TANGENT; + + // Check 2nd condition + // Get adjacent normal from twin half edge + const int32_t EDGE_INDEX = FACE.vertexIndices[CLOSEST_POINT].edgeIndex; + const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex; + + const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex; + const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE); + + projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); + if (projection >= 0.0f) + { + // Must be smaller than radius + if (projection >= RADIUS) + return false; + + const SHVec3 CP_TO_CENTER = CENTER - CP; + + manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); + + contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); + contact.position = CP; + + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; + + return true; + } + } + } + + // Check in region B + { + const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1; + const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); + + const SHVec3 TANGENT = SHVec3::Normalise(P3 - P1); + + float projection = SHVec3::Dot(P1_TO_CENTER, TANGENT); + + if (projection >= 0.0f) + { + // Find closest point + const SHVec3 CP = P1 + projection * TANGENT; + + // Check 2nd condition + // Get adjacent normal from twin half edge + const int32_t EDGE_INDEX = FACE.vertexIndices[P3_INDEX].edgeIndex; + const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex; + + const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex; + const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE); + + projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); + if (projection >= 0.0f) + { + // Must be smaller than radius + if (projection >= RADIUS) + return false; + + const SHVec3 CP_TO_CENTER = CENTER - CP; + + manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); + + contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); + contact.position = CP; + + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; + + return true; + } + } + } + + // Either region C or D. Take whichever depth is smaller + + const float C_PENETRATION = RADIUS - P1_TO_CENTER.Length(); + + if (std::fabs(C_PENETRATION) < RADIUS && std::fabs(C_PENETRATION) < PENETRATION) + { + manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); + + contact.penetration = C_PENETRATION; + contact.position = P1; + } + else + { + manifold.normal = -FACE_NORMAL; + + contact.penetration = PENETRATION; + contact.position = CENTER - FACE_NORMAL * RADIUS; + } + + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; + + return true; + } + + bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept + { + return SphereVsConvex(manifold, B, A); + } + + /*-----------------------------------------------------------------------------------*/ + /* Private Member Functions Definitions */ + /*-----------------------------------------------------------------------------------*/ + + SHCollision::FaceQuery SHCollision::findClosestFace(const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron) noexcept + { + FaceQuery faceQuery; + + const SHVec3 CENTER = sphere.GetCenter(); + const float RADIUS = sphere.GetWorldRadius(); + + const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = polyhedron.GetHalfEdgeStructure(); /* * Test against each face. @@ -76,8 +259,8 @@ namespace SHADE // Build plane equation - const SHVec3 POINT = CONVEX.GetVertex(FACE.vertexIndices[0]); // use the first vertex to build a plane - const SHVec3& NORMAL = CONVEX.GetNormal(i); + const SHVec3 POINT = polyhedron.GetVertex(FACE.vertexIndices[0].index); // use the first vertex to build a plane + const SHVec3& NORMAL = polyhedron.GetNormal(i); const float D = -SHVec3::Dot(NORMAL, POINT); // Find signed distance of center to plane @@ -87,153 +270,47 @@ namespace SHADE // If face is facing away from center, signed dist is negative. // Therefore signed distance is only positive when sphere is in front of the face. if (SIGNED_DIST > RADIUS) - return false; + return faceQuery; - if (SIGNED_DIST > bestDistance) + if (SIGNED_DIST > faceQuery.bestDistance) { - bestDistance = SIGNED_DIST; - closestFaceIndex = i; + faceQuery.bestDistance = SIGNED_DIST; + faceQuery.closestFace = i; } } - uint32_t numContacts = 0; - const float PENETRATION = RADIUS - bestDistance; - - // Check if center is inside polyhedron (below the face) - if (bestDistance < SHMath::EPSILON) - { - manifold.normal = -CONVEX.GetNormal(closestFaceIndex); - - SHContact newContact; - newContact.penetration = PENETRATION; - newContact.position = SPHERE.GetCenter(); - newContact.featurePair.key = 0; - - manifold.contacts[numContacts++] = newContact; - manifold.numContacts = numContacts; - return true; - } - - // Check against voronoi regions of the face to determine the type of the intersection test - // We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center - // If none of these are true, the sphere is above the face but not separating - - /* - * | A - * _ _ _ _ _ _ | _ _ _ - * / / - * | / | / regionA - * |/ _ _ _ _ _|/ _ _ _ - * B/ regionB /C - * / / regionC - */ - - const SHHalfEdgeStructure::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex); - const SHVec3& NORMAL = CONVEX.GetNormal(closestFaceIndex); - - const int32_t NUM_VERTICES_ON_FACE = static_cast(FACE.vertexIndices.size()); - - // Find closest point on face - closestPointIndex = 0; - float smallestDist = SHVec3::Dot(CENTER - CONVEX.GetVertex(FACE.vertexIndices[0]), NORMAL); - - for (int32_t i = 1; i < NUM_VERTICES_ON_FACE; ++i) - { - const SHVec3 POINT = CONVEX.GetVertex(FACE.vertexIndices[i]); - const float PROJECTION = SHVec3::Dot(CENTER - POINT, NORMAL); - - if (PROJECTION < smallestDist) - closestPointIndex = i; - } - - const SHVec3 C = CONVEX.GetVertex(FACE.vertexIndices[closestPointIndex]); - const SHVec3 C_TO_CENTER = SPHERE.GetCenter() - C; - - const int32_t INDEX_A = (closestPointIndex + 1) % NUM_VERTICES_ON_FACE; - const int32_t INDEX_B = closestPointIndex == 0 ? NUM_VERTICES_ON_FACE - 1 : closestPointIndex - 1; - - const SHVec3 POINTS[2] = - { - CONVEX.GetVertex(FACE.vertexIndices[INDEX_A]) // A - , CONVEX.GetVertex(FACE.vertexIndices[INDEX_B]) // B - }; - - // To be inside either region A or B, 2 conditions must be satisfied - // 1. Same side as tangent - // 2. Same side as normal from edge to sphere - - // Check in regions A & B - for (int i = 0; i < 2; ++i) - { - const SHVec3 TANGENT = SHVec3::Normalise(POINTS[i] - C); - - float projection = SHVec3::Dot(C_TO_CENTER, TANGENT); - if (projection >= 0.0f) - { - // Check 2nd condition - // Find closest point - const SHVec3 CP = C + projection * TANGENT; - const SHVec3 CP_TO_CENTER = SHVec3::Normalise(C - CP); - - projection = SHVec3::Dot(C_TO_CENTER, CP_TO_CENTER); - if (projection >= 0.0f) - { - // Sphere Within region - manifold.normal = -CP_TO_CENTER; - - SHContact newContact; - newContact.penetration = RADIUS - projection; - newContact.position = CP; - newContact.featurePair.key = 0; - - manifold.contacts[numContacts++] = newContact; - manifold.numContacts = numContacts; - - return true; - } - } - } - - // Check region C (closest point) - { - if (C_TO_CENTER.LengthSquared() < RADIUS * RADIUS) - { - manifold.normal = -SHVec3::Normalise(C_TO_CENTER); - - SHContact newContact; - newContact.penetration = PENETRATION; - newContact.position = C; - newContact.featurePair.key = 0; - - manifold.contacts[numContacts++] = newContact; - manifold.numContacts = numContacts; - - return true; - } - } - - // Region D - if (PENETRATION <= RADIUS) - { - manifold.normal = -NORMAL; - - SHContact newContact; - newContact.penetration = PENETRATION; - newContact.position = SPHERE.GetCenter() - NORMAL * RADIUS; - newContact.featurePair.key = 0; - - manifold.contacts[numContacts++] = newContact; - manifold.numContacts = numContacts; - - return true; - } - - return false; + faceQuery.colliding = true; + return faceQuery; } - bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept + int32_t SHCollision::findClosestPoint(const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron, int32_t faceIndex) noexcept { - return SphereVsConvex(manifold, B, A); + // Find closest point on face + int32_t closestPointIndex = -1; + + const SHVec3 CENTER = sphere.GetCenter(); + const float RADIUS = sphere.GetWorldRadius(); + + const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = polyhedron.GetHalfEdgeStructure(); + + + const SHHalfEdgeStructure::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(faceIndex); + const int32_t NUM_VERITICES = static_cast(FACE.vertexIndices.size()); + + float smallestDist = std::numeric_limits::max(); + for (int32_t i = 0; i < NUM_VERITICES; ++i) + { + const SHVec3 POINT = polyhedron.GetVertex(FACE.vertexIndices[i].index); + const float DIST = SHVec3::DistanceSquared(CENTER, POINT); + + if (DIST < smallestDist) + { + smallestDist = DIST; + closestPointIndex = i; + } + } + + return closestPointIndex; } } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/SHPhysicsMaterial.h b/SHADE_Engine/src/Physics/Collision/SHPhysicsMaterial.h index b3db1655..773ac4c1 100644 --- a/SHADE_Engine/src/Physics/Collision/SHPhysicsMaterial.h +++ b/SHADE_Engine/src/Physics/Collision/SHPhysicsMaterial.h @@ -19,6 +19,10 @@ namespace SHADE /* Type Definitions */ /*-----------------------------------------------------------------------------------*/ + /** + * @brief + * Encapsulates the data of a physics material for physics simulations. + */ class SH_API SHPhysicsMaterial { public: