From c4bbac9d502b6169c0c246b265a2c37b8b90ade0 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Fri, 27 Feb 2026 15:55:00 +0900 Subject: [PATCH 1/7] 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. --- Sofa/framework/Geometry/src/sofa/geometry/Edge.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Sofa/framework/Geometry/src/sofa/geometry/Edge.h b/Sofa/framework/Geometry/src/sofa/geometry/Edge.h index 934199ef46f..a20dac1fb63 100644 --- a/Sofa/framework/Geometry/src/sofa/geometry/Edge.h +++ b/Sofa/framework/Geometry/src/sofa/geometry/Edge.h @@ -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; } From 719327e8d9b81529d6fadda95985e6a6738b11de Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Fri, 27 Feb 2026 15:57:01 +0900 Subject: [PATCH 2/7] 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::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. --- Sofa/framework/Geometry/src/sofa/geometry/Edge.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Sofa/framework/Geometry/src/sofa/geometry/Edge.h b/Sofa/framework/Geometry/src/sofa/geometry/Edge.h index a20dac1fb63..3988a6849bb 100644 --- a/Sofa/framework/Geometry/src/sofa/geometry/Edge.h +++ b/Sofa/framework/Geometry/src/sofa/geometry/Edge.h @@ -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::epsilon()) // collinear + if (std::abs(alphaDenom) < std::numeric_limits::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; From bdfc335c03ecf34bfc110fcde6171301762d9b8a Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Fri, 27 Feb 2026 16:14:08 +0900 Subject: [PATCH 3/7] fix equality for 2d case of ispointonedge --- Sofa/framework/Geometry/src/sofa/geometry/Edge.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Sofa/framework/Geometry/src/sofa/geometry/Edge.h b/Sofa/framework/Geometry/src/sofa/geometry/Edge.h index 3988a6849bb..f3c70e76df9 100644 --- a/Sofa/framework/Geometry/src/sofa/geometry/Edge.h +++ b/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; From 04615bd46a74477eaf7a198a2e2541f1350e5fa3 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Fri, 27 Feb 2026 16:29:29 +0900 Subject: [PATCH 4/7] Mat_solve_LCP: fix when reaching max nb loop solveLCP returns true when solver fails to converge (max iterations reached) --- Sofa/framework/Type/src/sofa/type/Mat_solve_LCP.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Sofa/framework/Type/src/sofa/type/Mat_solve_LCP.h b/Sofa/framework/Type/src/sofa/type/Mat_solve_LCP.h index cfb71c9e47a..4cba662c45c 100644 --- a/Sofa/framework/Type/src/sofa/type/Mat_solve_LCP.h +++ b/Sofa/framework/Type/src/sofa/type/Mat_solve_LCP.h @@ -161,12 +161,14 @@ bool solveLCP(const Vec &q, const Mat &M, Vec= MAX_NB_LOOP) { - for (ii = 0; ii < dim; ii++) - res[base[ii]] = mat[ii][dim_mult2]; + return false; } + for (ii = 0; ii < dim; ii++) + res[base[ii]] = mat[ii][dim_mult2]; + return true; } From d03240ceb0e0e97da45642132db012507974d2ef Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Fri, 27 Feb 2026 16:35:08 +0900 Subject: [PATCH 5/7] 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. --- .../Geometry/src/sofa/geometry/Triangle.h | 62 ++++++++++++++----- 1 file changed, 45 insertions(+), 17 deletions(-) diff --git a/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h b/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h index 9ab3fb8a33d..28fc1d99e99 100644 --- a/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h +++ b/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::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>) { - 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::epsilon() * std::numeric_limits::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::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::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::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::epsilon()){ + baryCoefs[2] = 0; + } + + return baryCoefs; } - - return baryCoefs; } From 98f5e339902b711544024a0136f03f9d397c3d3a Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Tue, 3 Mar 2026 08:56:58 +0900 Subject: [PATCH 6/7] isPointInTriangle(3d) was not checking the plane, and add a boolean to bypass the plane check (as before) --- .../Geometry/src/sofa/geometry/Triangle.h | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h b/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h index 28fc1d99e99..7891a5af9d0 100644 --- a/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h +++ b/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h @@ -191,10 +191,30 @@ struct Triangle typename T = std::decay_t()))>, typename = std::enable_if_t> > - static constexpr bool isPointInTriangle(const Node& p0, const Node& n0, const Node& n1, const Node& n2, sofa::type::Vec<3, T>& baryCoefs) + static constexpr bool isPointInTriangle(const Node& p0, const Node& n0, const Node& n1, const Node& n2, sofa::type::Vec<3, T>& baryCoefs, bool assumePointIsInPlane = true) { baryCoefs = Triangle::getBarycentricCoordinates(p0, n0, n1, n2); + // In 3D, check if the point is in the plane of the triangle + if constexpr (std::is_same_v>) + { + if(!assumePointIsInPlane) + { + const auto normal = Triangle::normal(n0, n1, n2); + const auto normalNorm2 = sofa::type::dot(normal, normal); + if (normalNorm2 > std::numeric_limits::epsilon()) + { + const auto d = sofa::type::dot(p0 - n0, normal); + if (d * d / normalNorm2 > std::numeric_limits::epsilon()) + return false; + } + } + } + else + { + SOFA_UNUSED(assumePointIsInPlane); + } + for (int i = 0; i < 3; ++i) { if (baryCoefs[i] < 0 || baryCoefs[i] > 1) From 69cba4f32423ae4f9f3e8ad44cbf48d674f6ab72 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Tue, 3 Mar 2026 08:57:11 +0900 Subject: [PATCH 7/7] add more unit tests --- Sofa/framework/Geometry/test/CMakeLists.txt | 1 + Sofa/framework/Geometry/test/Edge_test.cpp | 63 +++++ .../Geometry/test/Proximity_test.cpp | 263 ++++++++++++++++++ Sofa/framework/Geometry/test/Quad_test.cpp | 23 ++ .../Geometry/test/Tetrahedron_test.cpp | 60 ++++ .../framework/Geometry/test/Triangle_test.cpp | 112 +++++++- 6 files changed, 515 insertions(+), 7 deletions(-) create mode 100644 Sofa/framework/Geometry/test/Proximity_test.cpp diff --git a/Sofa/framework/Geometry/test/CMakeLists.txt b/Sofa/framework/Geometry/test/CMakeLists.txt index 1698c5eddcd..ade56d771f1 100644 --- a/Sofa/framework/Geometry/test/CMakeLists.txt +++ b/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}) diff --git a/Sofa/framework/Geometry/test/Edge_test.cpp b/Sofa/framework/Geometry/test/Edge_test.cpp index 50ec5a2c3ca..452cbc55c13 100644 --- a/Sofa/framework/Geometry/test/Edge_test.cpp +++ b/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 diff --git a/Sofa/framework/Geometry/test/Proximity_test.cpp b/Sofa/framework/Geometry/test/Proximity_test.cpp new file mode 100644 index 00000000000..03ca7d5a1e8 --- /dev/null +++ b/Sofa/framework/Geometry/test/Proximity_test.cpp @@ -0,0 +1,263 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include +#include +#include + +#include + +#include + +namespace sofa +{ + +// ============================================================================ +// computeClosestPointOnTriangleToPoint +// ============================================================================ + +TEST(GeometryProximity_test, closestPointOnTriangle_pointAboveCenter) +{ + // Triangle in XY plane + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 2., 0., 0. }; + const sofa::type::Vec3d t2{ 0., 2., 0. }; + + // Point above the centroid + const sofa::type::Vec3d q{ 2. / 3., 2. / 3., 5. }; + sofa::type::Vec3d closest; + + bool res = sofa::geometry::proximity::computeClosestPointOnTriangleToPoint(t0, t1, t2, q, closest); + EXPECT_TRUE(res); + // Closest should be the projection onto the triangle plane: centroid + EXPECT_NEAR(closest[0], 2. / 3., 1e-4); + EXPECT_NEAR(closest[1], 2. / 3., 1e-4); + EXPECT_NEAR(closest[2], 0., 1e-4); +} + +TEST(GeometryProximity_test, closestPointOnTriangle_pointAboveVertex) +{ + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 2., 0., 0. }; + const sofa::type::Vec3d t2{ 0., 2., 0. }; + + // Point directly above vertex t0 + const sofa::type::Vec3d q{ 0., 0., 3. }; + sofa::type::Vec3d closest; + + bool res = sofa::geometry::proximity::computeClosestPointOnTriangleToPoint(t0, t1, t2, q, closest); + EXPECT_TRUE(res); + EXPECT_NEAR(closest[0], 0., 1e-4); + EXPECT_NEAR(closest[1], 0., 1e-4); + EXPECT_NEAR(closest[2], 0., 1e-4); +} + +TEST(GeometryProximity_test, closestPointOnTriangle_pointAboveEdge) +{ + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 2., 0., 0. }; + const sofa::type::Vec3d t2{ 0., 2., 0. }; + + // Point above the midpoint of edge t0-t1 + const sofa::type::Vec3d q{ 1., 0., 4. }; + sofa::type::Vec3d closest; + + bool res = sofa::geometry::proximity::computeClosestPointOnTriangleToPoint(t0, t1, t2, q, closest); + EXPECT_TRUE(res); + EXPECT_NEAR(closest[0], 1., 1e-4); + EXPECT_NEAR(closest[1], 0., 1e-4); + EXPECT_NEAR(closest[2], 0., 1e-4); +} + +TEST(GeometryProximity_test, closestPointOnTriangle_pointFarOutside) +{ + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 2., 0., 0. }; + const sofa::type::Vec3d t2{ 0., 2., 0. }; + + // Point far from triangle, nearest to edge t1-t2 + const sofa::type::Vec3d q{ 3., 3., 0. }; + sofa::type::Vec3d closest; + + bool res = sofa::geometry::proximity::computeClosestPointOnTriangleToPoint(t0, t1, t2, q, closest); + EXPECT_TRUE(res); + // The closest point on the triangle to (3,3,0) should be on the hypotenuse + // The hypotenuse t1-t2: parameterized as t1 + t*(t2-t1) = (2,0,0) + t*(-2,2,0) + // Project q onto the line: t = dot(q-t1, t2-t1) / dot(t2-t1, t2-t1) = dot((1,3,0),(-2,2,0))/8 = 4/8 = 0.5 + // Closest = (2,0,0) + 0.5*(-2,2,0) = (1,1,0) + EXPECT_NEAR(closest[0], 1., 1e-4); + EXPECT_NEAR(closest[1], 1., 1e-4); + EXPECT_NEAR(closest[2], 0., 1e-4); +} + +TEST(GeometryProximity_test, closestPointOnTriangle_degenerateTriangle) +{ + // Degenerate triangle (all points collinear) + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 1., 0., 0. }; + const sofa::type::Vec3d t2{ 2., 0., 0. }; + + const sofa::type::Vec3d q{ 1., 1., 0. }; + sofa::type::Vec3d closest; + + // LCP solver may return false for degenerate triangles + sofa::geometry::proximity::computeClosestPointOnTriangleToPoint(t0, t1, t2, q, closest); + // Just verify it doesn't crash; result depends on LCP solver behavior +} + + +// ============================================================================ +// computeClosestPointsSegmentAndTriangle +// ============================================================================ + +TEST(GeometryProximity_test, closestPointsSegmentTriangle_intersecting) +{ + // Triangle in XY plane + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 2., 0., 0. }; + const sofa::type::Vec3d t2{ 0., 2., 0. }; + + // Segment piercing the triangle vertically through its centroid + const sofa::type::Vec3d s0{ 2. / 3., 2. / 3., -1. }; + const sofa::type::Vec3d s1{ 2. / 3., 2. / 3., 1. }; + + sofa::type::Vec3d closestP, closestQ; + bool res = sofa::geometry::proximity::computeClosestPointsSegmentAndTriangle(t0, t1, t2, s0, s1, closestP, closestQ); + EXPECT_TRUE(res); + // Both closest points should be at the intersection: centroid at z=0 + EXPECT_NEAR(closestP[0], 2. / 3., 1e-4); + EXPECT_NEAR(closestP[1], 2. / 3., 1e-4); + EXPECT_NEAR(closestP[2], 0., 1e-4); + EXPECT_NEAR(closestQ[0], 2. / 3., 1e-4); + EXPECT_NEAR(closestQ[1], 2. / 3., 1e-4); + EXPECT_NEAR(closestQ[2], 0., 1e-4); +} + +TEST(GeometryProximity_test, closestPointsSegmentTriangle_parallel) +{ + // Triangle in XY plane + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 2., 0., 0. }; + const sofa::type::Vec3d t2{ 0., 2., 0. }; + + // Segment parallel to triangle, above midpoint of edge t0-t1 + const sofa::type::Vec3d s0{ 0., 0., 1. }; + const sofa::type::Vec3d s1{ 2., 0., 1. }; + + sofa::type::Vec3d closestP, closestQ; + bool res = sofa::geometry::proximity::computeClosestPointsSegmentAndTriangle(t0, t1, t2, s0, s1, closestP, closestQ); + EXPECT_TRUE(res); + // Closest points: on the triangle edge t0-t1 and on the segment, both should have z-distance=1 + EXPECT_NEAR(closestP[2], 0., 1e-4); + EXPECT_NEAR(closestQ[2], 1., 1e-4); + // x coordinates should match between the two closest points + EXPECT_NEAR(closestP[0], closestQ[0], 1e-4); +} + +TEST(GeometryProximity_test, closestPointsSegmentTriangle_endpointClosest) +{ + // Triangle in XY plane + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 2., 0., 0. }; + const sofa::type::Vec3d t2{ 0., 2., 0. }; + + // Segment above and to the side, nearest endpoint is s0 + const sofa::type::Vec3d s0{ 1., 1., 1. }; + const sofa::type::Vec3d s1{ 1., 1., 5. }; + + sofa::type::Vec3d closestP, closestQ; + bool res = sofa::geometry::proximity::computeClosestPointsSegmentAndTriangle(t0, t1, t2, s0, s1, closestP, closestQ); + EXPECT_TRUE(res); + // Closest on segment should be s0 (gamma=0) + EXPECT_NEAR(closestQ[0], 1., 1e-4); + EXPECT_NEAR(closestQ[1], 1., 1e-4); + EXPECT_NEAR(closestQ[2], 1., 1e-4); + // Closest on triangle should be (1,1,0) which is on the hypotenuse + EXPECT_NEAR(closestP[2], 0., 1e-4); +} + + +// ============================================================================ +// computeClosestPointsInTwoTriangles +// ============================================================================ + +TEST(GeometryProximity_test, closestPointsTwoTriangles_overlapping) +{ + // Two identical coplanar triangles + const sofa::type::Vec3d t0{ 0., 0., 0. }; + const sofa::type::Vec3d t1{ 2., 0., 0. }; + const sofa::type::Vec3d t2{ 0., 2., 0. }; + + sofa::type::Vec3d closestP, closestQ; + bool res = sofa::geometry::proximity::computeClosestPointsInTwoTriangles(t0, t1, t2, t0, t1, t2, closestP, closestQ); + EXPECT_TRUE(res); + // Closest points should be identical (distance=0) + EXPECT_NEAR((closestP - closestQ).norm(), 0., 1e-4); +} + +TEST(GeometryProximity_test, closestPointsTwoTriangles_separated) +{ + // Two parallel triangles separated by distance 2 along Z + const sofa::type::Vec3d p0{ 0., 0., 0. }; + const sofa::type::Vec3d p1{ 2., 0., 0. }; + const sofa::type::Vec3d p2{ 0., 2., 0. }; + + const sofa::type::Vec3d q0{ 0., 0., 2. }; + const sofa::type::Vec3d q1{ 2., 0., 2. }; + const sofa::type::Vec3d q2{ 0., 2., 2. }; + + sofa::type::Vec3d closestP, closestQ; + bool res = sofa::geometry::proximity::computeClosestPointsInTwoTriangles(p0, p1, p2, q0, q1, q2, closestP, closestQ); + EXPECT_TRUE(res); + // Closest points should have z=0 for P and z=2 for Q + EXPECT_NEAR(closestP[2], 0., 1e-4); + EXPECT_NEAR(closestQ[2], 2., 1e-4); + // Same x,y coordinates + EXPECT_NEAR(closestP[0], closestQ[0], 1e-4); + EXPECT_NEAR(closestP[1], closestQ[1], 1e-4); +} + +TEST(GeometryProximity_test, closestPointsTwoTriangles_edgeToEdge) +{ + // Two triangles that share no overlap, closest approach is edge-to-edge + const sofa::type::Vec3d p0{ 0., 0., 0. }; + const sofa::type::Vec3d p1{ 1., 0., 0. }; + const sofa::type::Vec3d p2{ 0., 1., 0. }; + + // Second triangle offset in x, perpendicular in z + const sofa::type::Vec3d q0{ 2., 0., 0. }; + const sofa::type::Vec3d q1{ 3., 0., 0. }; + const sofa::type::Vec3d q2{ 2., 0., 1. }; + + sofa::type::Vec3d closestP, closestQ; + bool res = sofa::geometry::proximity::computeClosestPointsInTwoTriangles(p0, p1, p2, q0, q1, q2, closestP, closestQ); + EXPECT_TRUE(res); + // Closest on P should be near vertex p1=(1,0,0), closest on Q near vertex q0=(2,0,0) + EXPECT_NEAR(closestP[0], 1., 1e-4); + EXPECT_NEAR(closestP[1], 0., 1e-4); + EXPECT_NEAR(closestP[2], 0., 1e-4); + EXPECT_NEAR(closestQ[0], 2., 1e-4); + EXPECT_NEAR(closestQ[1], 0., 1e-4); + EXPECT_NEAR(closestQ[2], 0., 1e-4); +} + +}// namespace sofa diff --git a/Sofa/framework/Geometry/test/Quad_test.cpp b/Sofa/framework/Geometry/test/Quad_test.cpp index f44a275e3b9..179d905ff88 100644 --- a/Sofa/framework/Geometry/test/Quad_test.cpp +++ b/Sofa/framework/Geometry/test/Quad_test.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -54,4 +55,26 @@ TEST(GeometryQuad_test, quad_area3f_stdarray) EXPECT_FLOAT_EQ(testArea, expectedArea); } +TEST(GeometryQuad_test, square_area2f_vec2) +{ + const sofa::type::Vec2f a{ 0.f, 0.f }; + const sofa::type::Vec2f b{ 4.f, 0.f }; + const sofa::type::Vec2f c{ 4.f, 4.f }; + const sofa::type::Vec2f d{ 0.f, 4.f }; + + const auto testArea = sofa::geometry::Quad::area(a, b, c, d); + EXPECT_FLOAT_EQ(testArea, 16.f); +} + +TEST(GeometryQuad_test, rect_area2f_vec2) +{ + const sofa::type::Vec2f a{ 0.f, 0.f }; + const sofa::type::Vec2f b{ 4.f, 0.f }; + const sofa::type::Vec2f c{ 4.f, 2.f }; + const sofa::type::Vec2f d{ 0.f, 2.f }; + + const auto testArea = sofa::geometry::Quad::area(a, b, c, d); + EXPECT_FLOAT_EQ(testArea, 8.f); +} + }// namespace sofa diff --git a/Sofa/framework/Geometry/test/Tetrahedron_test.cpp b/Sofa/framework/Geometry/test/Tetrahedron_test.cpp index f40851f8593..8bb99248d3e 100644 --- a/Sofa/framework/Geometry/test/Tetrahedron_test.cpp +++ b/Sofa/framework/Geometry/test/Tetrahedron_test.cpp @@ -40,4 +40,64 @@ TEST(GeometryTetrahedron_test, volume2_vec3f) EXPECT_NEAR(testVolume, 2.f/3.f, 1e-5); } +TEST(GeometryTetrahedron_test, signedVolume_positive_vec3f) +{ + // Regular tetrahedron with positive orientation + const sofa::type::Vec3f a{ 0.f, 0.f, 0.f }; + const sofa::type::Vec3f b{ 1.f, 0.f, 0.f }; + const sofa::type::Vec3f c{ 0.f, 1.f, 0.f }; + const sofa::type::Vec3f d{ 0.f, 0.f, 1.f }; + + const auto sv = sofa::geometry::Tetrahedron::signedVolume(a, b, c, d); + EXPECT_NEAR(sv, 1.f / 6.f, 1e-5); +} + +TEST(GeometryTetrahedron_test, signedVolume_negative_vec3f) +{ + // Swapping two vertices reverses orientation + const sofa::type::Vec3f a{ 0.f, 0.f, 0.f }; + const sofa::type::Vec3f b{ 1.f, 0.f, 0.f }; + const sofa::type::Vec3f c{ 0.f, 1.f, 0.f }; + const sofa::type::Vec3f d{ 0.f, 0.f, 1.f }; + + const auto sv = sofa::geometry::Tetrahedron::signedVolume(a, c, b, d); + EXPECT_NEAR(sv, -1.f / 6.f, 1e-5); +} + +TEST(GeometryTetrahedron_test, volume_unit_vec3f) +{ + const sofa::type::Vec3f a{ 0.f, 0.f, 0.f }; + const sofa::type::Vec3f b{ 1.f, 0.f, 0.f }; + const sofa::type::Vec3f c{ 0.f, 1.f, 0.f }; + const sofa::type::Vec3f d{ 0.f, 0.f, 1.f }; + + const auto v = sofa::geometry::Tetrahedron::volume(a, b, c, d); + EXPECT_NEAR(v, 1.f / 6.f, 1e-5); +} + +TEST(GeometryTetrahedron_test, volume_degenerate_vec3f) +{ + // Degenerate: all four points coplanar + const sofa::type::Vec3f a{ 0.f, 0.f, 0.f }; + const sofa::type::Vec3f b{ 1.f, 0.f, 0.f }; + const sofa::type::Vec3f c{ 0.f, 1.f, 0.f }; + const sofa::type::Vec3f d{ 1.f, 1.f, 0.f }; + + const auto v = sofa::geometry::Tetrahedron::volume(a, b, c, d); + EXPECT_NEAR(v, 0.f, 1e-5); +} + +TEST(GeometryTetrahedron_test, volume_scaled_vec3d) +{ + // Scaled tetrahedron (double precision) + const sofa::type::Vec3d a{ 0., 0., 0. }; + const sofa::type::Vec3d b{ 2., 0., 0. }; + const sofa::type::Vec3d c{ 0., 2., 0. }; + const sofa::type::Vec3d d{ 0., 0., 2. }; + + // Volume = (2*2*2) / 6 = 8/6 = 4/3 + const auto v = sofa::geometry::Tetrahedron::volume(a, b, c, d); + EXPECT_NEAR(v, 4.0 / 3.0, 1e-10); +} + }// namespace sofa diff --git a/Sofa/framework/Geometry/test/Triangle_test.cpp b/Sofa/framework/Geometry/test/Triangle_test.cpp index 8e6c2552373..0dbc59eb7ea 100644 --- a/Sofa/framework/Geometry/test/Triangle_test.cpp +++ b/Sofa/framework/Geometry/test/Triangle_test.cpp @@ -247,21 +247,29 @@ TYPED_TEST(GeometryVec3DTriangle_test, isPointInTriangle) EXPECT_EQ(bary[2], 1); // False cases - // out of plan + // out of plan with assumePointIsInPlane=false p0 = { 1., 0.2, 0.2 }; - res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary, false); EXPECT_FALSE(res); - EXPECT_NEAR(bary[0], 0.69282, 1e-4); - EXPECT_NEAR(bary[1], 0.360555, 1e-4); - EXPECT_NEAR(bary[2], -0.0533754, 1e-4); + EXPECT_NEAR(bary[0], 0.6, 1e-4); + EXPECT_NEAR(bary[1], 0.3, 1e-4); + EXPECT_NEAR(bary[2], 0.1, 1e-4); + + // out of plan with assumePointIsInPlane=true (default): bary coords are all positive + // so the function returns true even though the point is not in the triangle plane + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_NEAR(bary[0], 0.6, 1e-4); + EXPECT_NEAR(bary[1], 0.3, 1e-4); + EXPECT_NEAR(bary[2], 0.1, 1e-4); // in plan but out of triangle p0 = { 2., 2., 2. }; res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); EXPECT_FALSE(res); - EXPECT_EQ(bary[0], 1); + EXPECT_EQ(bary[0], -1); EXPECT_EQ(bary[1], 1); - EXPECT_EQ(bary[2], -1); + EXPECT_EQ(bary[2], 1); // Special cases @@ -276,6 +284,96 @@ TYPED_TEST(GeometryVec3DTriangle_test, isPointInTriangle) } +TYPED_TEST(GeometryVec2DTriangle_test, getBarycentricCoordinates) +{ + using Scalar = typename TypeParam::value_type; + + const TypeParam a{ 0., 0. }; + const TypeParam b{ 2., 0. }; + const TypeParam c{ 2., 2. }; + + // point inside + TypeParam p0{ 1.5, 0.5 }; + auto bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, c); + EXPECT_FLOAT_EQ(float(bary[0]), 0.25f); + EXPECT_FLOAT_EQ(float(bary[1]), 0.5f); + EXPECT_FLOAT_EQ(float(bary[2]), 0.25f); + + // on vertex n0 + p0 = { 0., 0. }; + bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, c); + EXPECT_FLOAT_EQ(float(bary[0]), 1.0f); + EXPECT_FLOAT_EQ(float(bary[1]), 0.0f); + EXPECT_FLOAT_EQ(float(bary[2]), 0.0f); + + // on edge n0-n1 + p0 = { 1., 0. }; + bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, c); + EXPECT_NEAR(bary[0], 0.5f, 1e-4); + EXPECT_NEAR(bary[1], 0.5f, 1e-4); + EXPECT_NEAR(bary[2], 0.0f, 1e-4); + + // outside triangle + p0 = { 4., 10. }; + bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, c); + EXPECT_NEAR(bary[0], -1.0f, 1e-4); + EXPECT_NEAR(bary[1], -3.0f, 1e-4); + EXPECT_NEAR(bary[2], 5.0f, 1e-4); + + // degenerate (flat) triangle + const TypeParam d{ 1., 0. }; + bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, d); + EXPECT_EQ(bary[0], -1); + EXPECT_EQ(bary[1], -1); + EXPECT_EQ(bary[2], -1); +} + + +TYPED_TEST(GeometryVec3DTriangle_test, getBarycentricCoordinates) +{ + using Scalar = typename TypeParam::value_type; + + const TypeParam a{ 0., 0., 0. }; + const TypeParam b{ 2., 0., 2. }; + const TypeParam c{ 0., 2., 0. }; + + // point inside (in-plane) + TypeParam p0{ 0.5, 0.5, 0.5 }; + auto bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, c); + EXPECT_NEAR(bary[0], 0.5f, 1e-4); + EXPECT_NEAR(bary[1], 0.25f, 1e-4); + EXPECT_NEAR(bary[2], 0.25f, 1e-4); + + // on vertex n2 + p0 = { 0., 2., 0. }; + bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, c); + EXPECT_EQ(bary[0], 0); + EXPECT_EQ(bary[1], 0); + EXPECT_EQ(bary[2], 1); + + // outside but in-plane + p0 = { 2., 2., 2. }; + bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, c); + EXPECT_NEAR(bary[0], -1.0f, 1e-4); + EXPECT_NEAR(bary[1], 1.0f, 1e-4); + EXPECT_NEAR(bary[2], 1.0f, 1e-4); + + // out-of-plane point (projects inside) + p0 = { 1., 0.2, 0.2 }; + bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, c); + EXPECT_NEAR(bary[0], 0.6f, 1e-4); + EXPECT_NEAR(bary[1], 0.3f, 1e-4); + EXPECT_NEAR(bary[2], 0.1f, 1e-4); + + // degenerate (flat) triangle + const TypeParam d{ 1., 0., 1. }; + bary = sofa::geometry::Triangle::getBarycentricCoordinates(p0, a, b, d); + EXPECT_EQ(bary[0], -1); + EXPECT_EQ(bary[1], -1); + EXPECT_EQ(bary[2], -1); +} + + TEST(GeometryTriangle_test, rayIntersectionVec3) { const sofa::type::Vec3 a{ 0., 3., 0. };