diff --git a/Sofa/framework/Geometry/src/sofa/geometry/Edge.h b/Sofa/framework/Geometry/src/sofa/geometry/Edge.h index 934199ef46f..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; @@ -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::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; diff --git a/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h b/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h index 9ab3fb8a33d..7891a5af9d0 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; } @@ -163,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) 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. }; 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; }