diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index 9ee6f4b9..bc1f5cf6 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -33,14 +33,37 @@ namespace SHADE const SHVec3 CENTER = SPHERE.GetCenter(); const float RADIUS = SPHERE.GetWorldRadius(); + // Find closest face const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); if (!FACE_QUERY.colliding) return false; + // If center of sphere is inside the polyhedron (below the face) if (FACE_QUERY.bestDistance < SHMath::EPSILON) return true; - return false; + // Find closest face of polygon to circle + const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace); + + const auto& 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()); + + // 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 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); + const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); + + const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1); + const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1); + + // Get the voronoi region it belongs in + const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2); + + return REGION > 0; } bool SHCollision::ConvexVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept @@ -69,13 +92,12 @@ namespace SHADE contact.featurePair.key = 0; // Check if center is inside polyhedron (below the face) - // TODO: Change to a point in polygon test. if (FACE_QUERY.bestDistance < SHMath::EPSILON) { manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace); - contact.penetration = PENETRATION; - contact.position = SPHERE.GetCenter(); + contact.penetration = PENETRATION; + contact.position = SPHERE.GetCenter(); manifold.contacts[numContacts++] = contact; manifold.numContacts = numContacts; @@ -86,10 +108,9 @@ namespace SHADE // Find closest face of polygon to circle const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, 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 auto& 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()); // Get points and build tangents const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; @@ -238,7 +259,7 @@ namespace SHADE 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 + // We have 3 voronoi regions to check: 1 -> 2, 1 -> 3 and 1 -> center // If none of these are true, the sphere is above the face but not separating /*