Skip to content

[Geometry] Bugfixes and add unit tests #5987

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
fredroy wants to merge 7 commits into
base: master
Choose a base branch
from
Open

[Geometry] Bugfixes and add unit tests #5987

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
c4bbac9
fix test for intersectionWithPlane (if zero with tolerance)
fredroy Feb 27, 2026
719327e
fix two bugs with 2d case for Edge::intersectionWithEdge
fredroy Feb 27, 2026
bdfc335
fix equality for 2d case of ispointonedge
fredroy Feb 27, 2026
04615bd
Mat_solve_LCP: fix when reaching max nb loop
fredroy Feb 27, 2026
d03240c
fix get bary coord for triangle
fredroy Feb 27, 2026
98f5e33
isPointInTriangle(3d) was not checking the plane, and add a boolean t…
fredroy Mar 2, 2026
69cba4f
add more unit tests
fredroy Mar 2, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 8 additions & 4 deletions Sofa/framework/Geometry/src/sofa/geometry/Edge.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
Expand Down
84 changes: 66 additions & 18 deletions Sofa/framework/Geometry/src/sofa/geometry/Triangle.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}


Expand Down Expand Up @@ -163,10 +191,30 @@ struct Triangle
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)
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<Node, sofa::type::Vec<3, T>>)
{
if(!assumePointIsInPlane)
{
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;
}
}
}
else
{
SOFA_UNUSED(assumePointIsInPlane);
}

for (int i = 0; i < 3; ++i)
{
if (baryCoefs[i] < 0 || baryCoefs[i] > 1)
Expand Down
1 change: 1 addition & 0 deletions Sofa/framework/Geometry/test/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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})
Expand Down
63 changes: 63 additions & 0 deletions Sofa/framework/Geometry/test/Edge_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Loading
Loading