diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index e38a66a7..76b948a3 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -306,18 +306,8 @@ namespace SHADE SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept { /* - * The two edges can be parameterised in the form p + tv - * - * LA(r) = A + rVA - * LB(s) = B + sVB - * - * The vector between the closest points is the cross product of VA and VB. - * Since the cross product is orthogonal to VA and VB, we can generalise this as: - * (LB(s) - LA(r)) /dot VA = 0 - * (LB(s) - LA(r)) /dot VB = 0 - * - * Where LB(s) - LA(r) is the same vector as VB X VA. - * + * A more robust solution can be found in Real-Time Collision Detection by + * Christer Ericson, page 148-149. */ const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); @@ -328,46 +318,47 @@ namespace SHADE const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex); const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex); - const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); - const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); - /* - * Find a1, a2, b1, b2, c1, c2 to solve the simultaneous equation - * - * C' = TAIL_B - TAIL_A - * - * a = VB /dot U - * b = -VA /dot U - * c = C' /dot U - * - * U is either VA or VB - * - * TODO: Check if segments degenerate into a point - */ + const SHVec3 D1 = TAIL_A - HEAD_A; + const SHVec3 D2 = TAIL_B - HEAD_B; + const SHVec3 R = HEAD_A - HEAD_B; - const SHVec3 C = TAIL_B - TAIL_A; + const float D1_LENSQ = D1.LengthSquared(); // a + const float D2_LENSQ = D2.LengthSquared(); // e - const float VB_DOT_VA = SHVec3::Dot(VB, VA); // a1 - const float VB_DOT_VB = SHVec3::Dot(VB, VB); // a2 - const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1 - const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2 - const float C_DOT_VA = SHVec3::Dot(C, VA); // c1 - const float C_DOT_VB = SHVec3::Dot(C, VB); // c2 + const float D1_DOT_D2 = SHVec3::Dot(D1, D2); // b + const float D1_DOT_R = SHVec3::Dot(D1, R); // c + const float D2_DOT_R = SHVec3::Dot(D2, R); // f - /* - * We only need to solve for R - * R = (c1a2 / a1 - c2) / ( b2 - a2b1/a1 ) - */ + float s = 0.0f; + float t = 0.0f; - const float A2_OVER_A1 = VB_DOT_VA == 0.0f ? 0.0f : VB_DOT_VB / VB_DOT_VA; - const float NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB; - const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1; + // If both segment degenerates into points + if (D1_LENSQ <= SHMath::EPSILON && D2_LENSQ <= SHMath::EPSILON) + return TAIL_A; - // We clamp it because the factor cannot be beyond [0,1] as it would be beyond the segment if it was so. - const float R = DENOMINATOR == 0.0f ? NUMERATOR : NUMERATOR / DENOMINATOR; + // Segment A degenerates into a point + if (D1_LENSQ <= SHMath::EPSILON) + t = std::clamp(D2_DOT_R / D2_LENSQ, 0.0f, 1.0f); - // Just take a point from A since it's A -> B - return TAIL_A + R * VA; + // Segment B degenerates into a point + if (D2_LENSQ <= SHMath::EPSILON) + s = std::clamp(-D1_DOT_R / D1_LENSQ, 0.0f, 1.0f); + + const float DENOMINATOR = D1_LENSQ * D2_LENSQ - D1_DOT_D2 * D1_DOT_D2; + if (!SHMath::CompareFloat(DENOMINATOR, 0.0f)) + s = std::clamp((D1_DOT_D2 * D2_DOT_R - D1_DOT_R * D2_LENSQ) / DENOMINATOR, 0.0f, 1.0f); + + t = (D1_DOT_D2 * s + D2_DOT_R) / D2_LENSQ; + + // We just need s since we are taking the point in A + if (t < 0.0f) + s = std::clamp(-D1_DOT_R / D1_LENSQ, 0.0f, 1.0f); + else if (t > 1.0f) + s = std::clamp((D1_DOT_D2 - D1_DOT_R) / D1_LENSQ, 0.0f, 1.0f); + + // Get the point on A + return HEAD_A + s * SHVec3::Normalise(D1); } int32_t SHCollision::findIncidentFace(const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept