[Geometry] Bugfixes and add unit tests (sofa-framework#5987)

* fix test for intersectionWithPlane (if zero with tolerance)

if (denominator < EQUALITY_THRESHOLD)  the denominator is a signed dot product. When negative (edge crosses plane from the "back"), this is always true,
  so the function rejects ~half of all valid intersections.

* fix two bugs with 2d case for Edge::intersectionWithEdge

1-The 2D path computes alpha (parameter on edge AB) and checks 0 <= alpha <= 1, but never computes or checks beta (parameter on edge CD). Reports false
  intersections when the infinite line CD crosses segment AB but outside segment CD.
2-if (alphaDenom < std::numeric_limits<T>::epsilon())  same pattern as Bug 1. The cross product alphaDenom is signed. When negative (non-collinear edges in
  one orientation), incorrectly reports them as collinear.

* fix equality for 2d case of ispointonedge

* Mat_solve_LCP: fix when reaching max nb loop

solveLCP returns true when solver fails to converge (max iterations reached)

* fix get bary coord for triangle

While isPointInTriangle still works (it only checks sign pattern),
  any code that uses the returned coordinates for interpolation, projection to the nearest point on the triangle, or distance computation gets wrong values.
  This is a public utility function likely consumed by many parts of the codebase.

* isPointInTriangle(3d) was not checking the plane, and add a boolean to bypass the plane check (as before)

* add more unit tests

* Update Sofa/framework/Geometry/src/sofa/geometry/Edge.h (fix sign error for beta)

Co-authored-by: Paul Baksic <30337881+bakpaul@users.noreply.github.com>

* add isPointOnPlane function and add its unit tests

* compute coeffs only if in plane; set -1,-1,-1 if not

* update unit tests

---------

Co-authored-by: Paul Baksic <30337881+bakpaul@users.noreply.github.com>

Author: fredroy

Sofa/framework/Geometry/src/sofa/geometry/Edge.h

@@ -165,7 +165,7 @@ struct Edge
             N2 = sofa::type::cross(AB, AC).norm2();
         }
 
-        if (N2 > EQUALITY_THRESHOLD || N2 < 0)
+        if (std::abs(N2) > EQUALITY_THRESHOLD)
             return false;
 
         return true;
@@ -197,7 +197,7 @@ struct Edge
 
         //compute intersection between line and plane equation
         const T denominator = planeNorm * (n1 - n0);
-        if (denominator < EQUALITY_THRESHOLD)
+        if (std::abs(denominator) < EQUALITY_THRESHOLD)
         {
             return false;
         }
@@ -307,15 +307,19 @@ struct Edge
             const T alphaNom = AC[1] * CD[0] - AC[0] * CD[1];
             const T alphaDenom = AB[1] * CD[0] - AB[0] * CD[1];
 
-            if (alphaDenom < std::numeric_limits<T>::epsilon()) // collinear
+            if (std::abs(alphaDenom) < std::numeric_limits<T>::epsilon()) // collinear
             {
                 intersectionBaryCoord = sofa::type::Vec<2, T>(0, 0);
                 return false;
             }
 
             const T alpha = alphaNom / alphaDenom;
+            // beta: pC + beta * CD = pA + alpha * AB
+            const T beta = (std::abs(CD[0]) > std::abs(CD[1]))
+                ? (-AC[0] + alpha * AB[0]) / CD[0]
+                : (-AC[1] + alpha * AB[1]) / CD[1];
 
-            if (alpha < 0 || alpha > 1)
+            if (alpha < 0 || alpha > 1 || beta < 0 || beta > 1)
             {
                 intersectionBaryCoord = sofa::type::Vec<2, T>(0, 0);
                 return false;

Sofa/framework/Geometry/src/sofa/geometry/Triangle.h

@@ -88,26 +88,54 @@ struct Triangle
         static constexpr auto getBarycentricCoordinates(const Node& p0, const Node& n0, const Node& n1, const Node& n2)
     {
         // Point can be written: p0 = a*n0 + b*n1 + c*n2
-        // with a = area(n1n2p0)/area(n0n1n2), b = area(n0n2p0)/area(n0n1n2) and c = area(n0n1p0)/area(n0n1n2) 
-        const auto area = Triangle::area(n0, n1, n2);
-        if (fabs(area) < std::numeric_limits<T>::epsilon()) // triangle is flat
+        // with a = area(n1n2p0)/area(n0n1n2), b = area(n0n2p0)/area(n0n1n2) and c = area(n0n1p0)/area(n0n1n2)
+        if constexpr (std::is_same_v<Node, sofa::type::Vec<3, T>>)
         {
-            return sofa::type::Vec<3, T>(-1, -1, -1);
+            // In 3D, use signed areas via dot product with triangle normal
+            // to get correct sign for outside-triangle points
+            const auto N = sofa::type::cross(n1 - n0, n2 - n0);
+            const auto NdotN = sofa::type::dot(N, N);
+
+            if (NdotN < std::numeric_limits<T>::epsilon() * std::numeric_limits<T>::epsilon()) // triangle is flat
+            {
+                return sofa::type::Vec<3, T>(-1, -1, -1);
+            }
+
+            sofa::type::Vec<3, T> baryCoefs(type::NOINIT);
+            baryCoefs[0] = sofa::type::dot(N, sofa::type::cross(n2 - n1, p0 - n1)) / NdotN;
+            baryCoefs[1] = sofa::type::dot(N, sofa::type::cross(n0 - n2, p0 - n2)) / NdotN;
+            baryCoefs[2] = 1 - baryCoefs[0] - baryCoefs[1];
+
+            if (fabs(baryCoefs[2]) <= std::numeric_limits<T>::epsilon()){
+                baryCoefs[2] = 0;
+            }
+
+            return baryCoefs;
         }
-        
-        const auto A0 = Triangle::area(n1, n2, p0);
-        const auto A1 = Triangle::area(n0, p0, n2);
-
-        sofa::type::Vec<3, T> baryCoefs(type::NOINIT);
-        baryCoefs[0] = A0 / area;
-        baryCoefs[1] = A1 / area;
-        baryCoefs[2] = 1 - baryCoefs[0] - baryCoefs[1];
-
-        if (fabs(baryCoefs[2]) <= std::numeric_limits<T>::epsilon()){
-            baryCoefs[2] = 0;
+        else
+        {
+            // In 2D, Triangle::area() returns a signed value (shoelace formula),
+            // so the ratio of sub-areas to total area preserves correct signs
+            const auto area = Triangle::area(n0, n1, n2);
+            if (fabs(area) < std::numeric_limits<T>::epsilon()) // triangle is flat
+            {
+                return sofa::type::Vec<3, T>(-1, -1, -1);
+            }
+
+            const auto A0 = Triangle::area(n1, n2, p0);
+            const auto A1 = Triangle::area(n0, p0, n2);
+
+            sofa::type::Vec<3, T> baryCoefs(type::NOINIT);
+            baryCoefs[0] = A0 / area;
+            baryCoefs[1] = A1 / area;
+            baryCoefs[2] = 1 - baryCoefs[0] - baryCoefs[1];
+
+            if (fabs(baryCoefs[2]) <= std::numeric_limits<T>::epsilon()){
+                baryCoefs[2] = 0;
+            }
+
+            return baryCoefs;
         }
-        
-        return baryCoefs;
     }
 
 
@@ -148,23 +176,76 @@ struct Triangle
         }
 
     }
-
-
+    
+    /**
+    * @brief    Test if input point is on the plane defined by the Triangle (n0, n1, n2)
+    * @tparam   Node iterable container
+    * @tparam   T scalar
+    * @param    p0: position of the point to test
+    * @param    n0, n1, n2: nodes of the triangle
+    * @return    bool result if point is on the plane of the triangle.
+    */
+    template<typename Node,
+        typename T = std::decay_t<decltype(*std::begin(std::declval<Node>()))>,
+        typename = std::enable_if_t<std::is_scalar_v<T>>
+    >
+    [[nodiscard]]
+    static constexpr bool isPointOnPlane(const Node& p0, const Node& n0, const Node& n1, const Node& n2)
+    {
+        if constexpr (std::is_same_v<Node, sofa::type::Vec<3, T>>)
+        {
+            const auto normal = Triangle::normal(n0, n1, n2);
+            const auto normalNorm2 = sofa::type::dot(normal, normal);
+            if (normalNorm2 > std::numeric_limits<T>::epsilon())
+            {
+                const auto d = sofa::type::dot(p0 - n0, normal);
+                if (d * d / normalNorm2 > std::numeric_limits<T>::epsilon())
+                    return false;
+            }
+            
+            return true;
+        }
+        else
+        {
+            // all points are trivially in the same plane
+            return true;
+        }
+    }
+    
     /**
     * @brief	Test if input point is inside Triangle (n0, n1, n2) using Triangle @sa getBarycentricCoordinates . The point is inside the Triangle if and only if Those coordinates are all positive.
     * @tparam   Node iterable container
     * @tparam   T scalar
     * @param	p0: position of the point to test
     * @param	n0, n1, n2: nodes of the triangle
     * @param	output parameter: sofa::type::Vec<3, T> barycentric coordinates of the input point in Triangle
+    * @param assumePointIsOnPlane: optional bool to avoid testing if the point is on the plane defined by the triangle
     * @return	bool result if point is inside Triangle.
     */
     template<typename Node,
         typename T = std::decay_t<decltype(*std::begin(std::declval<Node>()))>,
         typename = std::enable_if_t<std::is_scalar_v<T>>
     >
-        static constexpr bool isPointInTriangle(const Node& p0, const Node& n0, const Node& n1, const Node& n2, sofa::type::Vec<3, T>& baryCoefs)
+    [[nodiscard]]
+    static constexpr bool isPointInTriangle(const Node& p0, const Node& n0, const Node& n1, const Node& n2, sofa::type::Vec<3, T>& baryCoefs, bool assumePointIsOnPlane = true)
     {
+        // In 3D, check if the point is in the plane of the triangle
+        if constexpr (std::is_same_v<Node, sofa::type::Vec<3, T>>)
+        {
+            if(!assumePointIsOnPlane)
+            {
+                if(!isPointOnPlane(p0, n0, n1, n2))
+                {
+                    baryCoefs = {static_cast<T>(-1), static_cast<T>(-1), static_cast<T>(-1)};
+                    return false;
+                }
+            }
+        }
+        else
+        {
+            SOFA_UNUSED(assumePointIsOnPlane);
+        }
+        
         baryCoefs = Triangle::getBarycentricCoordinates(p0, n0, n1, n2);
 
         for (int i = 0; i < 3; ++i)

Sofa/framework/Geometry/test/CMakeLists.txt

@@ -8,6 +8,7 @@ set(SOURCE_FILES
     Quad_test.cpp
     Tetrahedron_test.cpp
     Hexahedron_test.cpp
+    Proximity_test.cpp
 )
 
 add_executable(${PROJECT_NAME} ${SOURCE_FILES})

Sofa/framework/Geometry/test/Edge_test.cpp

@@ -492,4 +492,67 @@ TEST(GeometryEdge_test, intersectionWithPlane3f)
     EXPECT_FLOAT_EQ(inter[2], 0.0f);
 }
 
+TEST(GeometryEdge_test, closestPointWithEdge3f_perpendicular)
+{
+    // Two perpendicular edges crossing at (1,1,1)
+    const sofa::type::Vec3f pA{ 0.f, 0.f, 0.f };
+    const sofa::type::Vec3f pB{ 2.f, 2.f, 2.f };
+    const sofa::type::Vec3f pC{ 0.f, 0.f, 2.f };
+    const sofa::type::Vec3f pD{ 2.f, 2.f, 0.f };
+
+    sofa::type::Vec<2, float> baryCoord(sofa::type::NOINIT);
+    sofa::geometry::Edge::closestPointWithEdge(pA, pB, pC, pD, baryCoord);
+
+    // alpha and beta should both be 0.5 (midpoint of each edge)
+    EXPECT_NEAR(baryCoord[0], 0.5f, 1e-4);
+    EXPECT_NEAR(baryCoord[1], 0.5f, 1e-4);
+
+    // Verify closest points
+    const auto pX = pA + baryCoord[0] * (pB - pA);
+    const auto pY = pC + baryCoord[1] * (pD - pC);
+    EXPECT_NEAR(pX[0], 1.f, 1e-4);
+    EXPECT_NEAR(pX[1], 1.f, 1e-4);
+    EXPECT_NEAR(pX[2], 1.f, 1e-4);
+    EXPECT_NEAR(pY[0], 1.f, 1e-4);
+    EXPECT_NEAR(pY[1], 1.f, 1e-4);
+    EXPECT_NEAR(pY[2], 1.f, 1e-4);
+}
+
+TEST(GeometryEdge_test, closestPointWithEdge3f_parallel)
+{
+    // Two parallel edges offset in z
+    const sofa::type::Vec3f pA{ 0.f, 0.f, 0.f };
+    const sofa::type::Vec3f pB{ 2.f, 0.f, 0.f };
+    const sofa::type::Vec3f pC{ 0.f, 0.f, 1.f };
+    const sofa::type::Vec3f pD{ 2.f, 0.f, 1.f };
+
+    sofa::type::Vec<2, float> baryCoord(sofa::type::NOINIT);
+    sofa::geometry::Edge::closestPointWithEdge(pA, pB, pC, pD, baryCoord);
+
+    // Parallel/collinear case: denominator is near zero, should return (0,0)
+    EXPECT_FLOAT_EQ(baryCoord[0], 0.f);
+    EXPECT_FLOAT_EQ(baryCoord[1], 0.f);
+}
+
+TEST(GeometryEdge_test, closestPointWithEdge3f_skew)
+{
+    // Two skew edges (not intersecting, not parallel)
+    const sofa::type::Vec3f pA{ 0.f, 0.f, 0.f };
+    const sofa::type::Vec3f pB{ 2.f, 0.f, 0.f };
+    const sofa::type::Vec3f pC{ 1.f, 1.f, -1.f };
+    const sofa::type::Vec3f pD{ 1.f, 1.f, 1.f };
+
+    sofa::type::Vec<2, float> baryCoord(sofa::type::NOINIT);
+    sofa::geometry::Edge::closestPointWithEdge(pA, pB, pC, pD, baryCoord);
+
+    // Closest point on edge1 is at alpha=0.5 (x=1), on edge2 at beta=0.5 (z=0)
+    EXPECT_NEAR(baryCoord[0], 0.5f, 1e-4);
+    EXPECT_NEAR(baryCoord[1], 0.5f, 1e-4);
+
+    const auto pX = pA + baryCoord[0] * (pB - pA);
+    const auto pY = pC + baryCoord[1] * (pD - pC);
+    EXPECT_NEAR(pX[0], 1.f, 1e-4);
+    EXPECT_NEAR(pY[2], 0.f, 1e-4);
+}
+
 }// namespace sofa