From f3c0bdbcfd45872ebbc4aa5666017db2be18ce10 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Sun, 1 Jan 2023 03:24:34 +0800 Subject: [PATCH] Clean up --- Assets/Scenes/PhysicsSandbox.shade | 2 +- .../SHConvexPolyhedronCollisionShape.cpp | 24 ++ .../SHConvexPolyhedronCollisionShape.h | 11 +- .../Collision/Narrowphase/SHCollision.h | 1 + .../Narrowphase/SHSphereVsConvex.cpp | 239 ++++++++---------- 5 files changed, 144 insertions(+), 133 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 266447e3..a8730476 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -66,7 +66,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -0.903104782, y: 7, z: -0.782080948} + Translate: {x: -1.96328545, y: 7, z: 0.743723333} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedronCollisionShape.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedronCollisionShape.cpp index db6d3621..51d4e684 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedronCollisionShape.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedronCollisionShape.cpp @@ -97,4 +97,28 @@ namespace SHADE { return halfEdgeStructure; } + + int32_t SHConvexPolyhedronCollisionShape::GetFaceCount() const noexcept + { + return halfEdgeStructure->GetFaceCount(); + } + + int32_t SHConvexPolyhedronCollisionShape::GetHalfEdgeCount() const noexcept + { + return halfEdgeStructure->GetHalfEdgeCount(); + } + + const SHHalfEdgeStructure::Face& SHConvexPolyhedronCollisionShape::GetFace(int index) const + { + // Assume it has already been initialised + return halfEdgeStructure->GetFace(index); + } + + const SHHalfEdgeStructure::HalfEdge& SHConvexPolyhedronCollisionShape::GetHalfEdge(int index) const + { + // Assume it has already been initialised + return halfEdgeStructure->GetHalfEdge(index); + } + + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedronCollisionShape.h b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedronCollisionShape.h index 7a0f6df0..6095cbca 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedronCollisionShape.h +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedronCollisionShape.h @@ -57,9 +57,14 @@ namespace SHADE /* Getter Functions */ /*---------------------------------------------------------------------------------*/ - [[nodiscard]] const SHHalfEdgeStructure* GetHalfEdgeStructure () const noexcept; - [[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0; - [[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0; + [[nodiscard]] const SHHalfEdgeStructure* GetHalfEdgeStructure () const noexcept; + [[nodiscard]] int32_t GetFaceCount () const noexcept; + [[nodiscard]] const SHHalfEdgeStructure::Face& GetFace (int index) const; + [[nodiscard]] int32_t GetHalfEdgeCount () const noexcept; + [[nodiscard]] const SHHalfEdgeStructure::HalfEdge& GetHalfEdge (int index) const; + + [[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0; + [[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0; protected: /*---------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 942c6919..4db1fc68 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -86,6 +86,7 @@ namespace SHADE 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 int32_t findVoronoiRegion (const SHSphereCollisionShape& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept; static bool isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index 4523fc5b..9ee6f4b9 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -33,8 +33,6 @@ namespace SHADE 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; @@ -60,8 +58,6 @@ namespace SHADE 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; @@ -83,147 +79,75 @@ namespace SHADE 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); - const SHHalfEdgeStructure::Face& FACE = POLYHEDRON.GetHalfEdgeStructure()->GetFace(FACE_QUERY.closestFace); + const SHHalfEdgeStructure::Face& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace); - const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace); - const int32_t NUM_VERTICES = static_cast(FACE.vertexIndices.size()); + 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 - * - */ + // Get points and build tangents + const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; + const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1; const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index); - const SHVec3 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); - - 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 the cross product (tangent x normal) - //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 = SHVec3::Cross(tangent, FACE_NORMAL); - - 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); - tangent = SHVec3::Normalise(P3 - P1); + const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1); + const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1); - projection = SHVec3::Dot(P1_TO_CENTER, tangent); + // Get the voronoi region it belongs in + const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2); + if (REGION == 0) + return false; - if (projection >= 0.0f) + // Create contact information based on region + + const SHVec3 P1_TO_CENTER = CENTER - P1; + switch (REGION) { - // Find closest point - const SHVec3 CP = P1 + projection * tangent; - - // Check 2nd condition - // Get adjacent normal from the cross product (normal x tangent) - //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); - const SHVec3 ADJ_NORMAL = SHVec3::Cross(FACE_NORMAL, tangent); - - - projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); - if (projection >= 0.0f) + case 1: // Region A + case 2: // Region B { - // Must be smaller than radius - if (projection >= RADIUS) - return false; - + // Find closest point + const SHVec3& TANGENT = REGION == 1 ? TANGENT_1 : TANGENT_2; + const SHVec3 CP = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT); const SHVec3 CP_TO_CENTER = CENTER - CP; - manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); + manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); - contact.position = CP; + contact.position = CP; - manifold.contacts[numContacts++] = contact; - manifold.numContacts = numContacts; - - return true; + break; } + case 3: // Region C + { + manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); + + contact.penetration = RADIUS - P1_TO_CENTER.Length(); + contact.position = P1; + + break; + } + case 4: // Region D + { + manifold.normal = -FACE_NORMAL; + + contact.penetration = PENETRATION; + contact.position = CENTER - FACE_NORMAL * RADIUS; + + break; + } + default: return false; // Should never happen } - // Region C has a negative dot product with any of the tangents. - - projection = SHVec3::Dot(P1_TO_CENTER, tangent); - if (projection < 0) - { - manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); - - contact.penetration = RADIUS - P1_TO_CENTER.Length(); - contact.position = P1; - - manifold.contacts[numContacts++] = contact; - manifold.numContacts = numContacts; - - return true; - } - - // Region D - manifold.normal = -FACE_NORMAL; - - contact.penetration = PENETRATION; - contact.position = CENTER - FACE_NORMAL * RADIUS; - manifold.contacts[numContacts++] = contact; manifold.numContacts = numContacts; @@ -246,8 +170,6 @@ namespace SHADE const SHVec3 CENTER = sphere.GetCenter(); const float RADIUS = sphere.GetWorldRadius(); - const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = polyhedron.GetHalfEdgeStructure(); - /* * Test against each face. * @@ -255,9 +177,9 @@ namespace SHADE * 2. Find the signed distance from plane to center of sphere. * 3. Save best distance and face. */ - for (int32_t i = 0; i < HALF_EDGE_STRUCTURE->GetFaceCount(); ++i) + for (int32_t i = 0; i < polyhedron.GetFaceCount(); ++i) { - const SHHalfEdgeStructure::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(i); + const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(i); // Build plane equation @@ -291,12 +213,8 @@ namespace SHADE 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 SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(faceIndex); const int32_t NUM_VERITICES = static_cast(FACE.vertexIndices.size()); float smallestDist = std::numeric_limits::max(); @@ -315,4 +233,67 @@ namespace SHADE return closestPointIndex; } + int32_t SHCollision::findVoronoiRegion(const SHSphereCollisionShape& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept + { + static constexpr int NUM_TANGENTS = 2; + + // Check against voronoi regions of the face to determine the type of the intersection test + // We have 3 voronoi regions to check: 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& CENTER = sphere.GetCenter(); + const float RADIUS = sphere.GetWorldRadius(); + + const SHVec3 TANGENTS [NUM_TANGENTS] { tangent1, tangent2 }; + const SHVec3 ADJACENT_NORMALS [NUM_TANGENTS] { SHVec3::Cross(tangent1, faceNormal), SHVec3::Cross(faceNormal, tangent2) }; + + const SHVec3 FACE_TO_CENTER = CENTER - faceVertex; + + // To be inside either region A or B, 2 conditions must be satisfied + // 1. Same side as tangent + // 2. Same side as adjacent normal + + // Check Region A & B + for (int i = 0; i < NUM_TANGENTS; ++i) + { + float projection = SHVec3::Dot(FACE_TO_CENTER, TANGENTS[i]); + if (projection >= 0.0f) + { + // Find closest point + const SHVec3 CLOSEST_POINT = faceVertex + projection * TANGENTS[i]; + + projection = SHVec3::Dot(FACE_TO_CENTER, ADJACENT_NORMALS[i]); + if (projection >= 0.0f) + { + if (projection > RADIUS) + return 0; + + // Region 1 or 2 ( A or B) + return i + 1; + } + } + } + + // Check Region C + // Face to vertex is in the opposite direction of any tangent. + const float PROJECTION = SHVec3::Dot(FACE_TO_CENTER, tangent1); + if (PROJECTION < 0) + return 3; + + // Belongs in region D by default + return 4; + } + + } // namespace SHADE \ No newline at end of file