diff --git a/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h b/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h index 75096a504ff..c85ead5314b 100644 --- a/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h +++ b/Sofa/framework/Geometry/src/sofa/geometry/Triangle.h @@ -71,6 +71,45 @@ struct Triangle } } + + /** + * @brief Compute the barycentric coordinates of the input point in the Triangle. It can be interpreted as masses placed at the vertices of Triangle (n0, n1, n2), such that the point is the center of mass of these masses. + * @tparam Node iterable container + * @tparam T scalar + * @param p0: position of the input point to compute the coefficients + * @param n0, n1, n2: nodes of the triangle + * @return sofa::type::Vec<3, T> barycentric coefficients of each vertex of the Triangle. These masses can be zero or negative; they are all positive if and only if the point is inside the Triangle. + */ + template()))>, + typename = std::enable_if_t> + > + 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 + { + 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; + } + + /** * @brief Compute the normal of a triangle * @remark triangle normal computation is only possible in 3D @@ -109,6 +148,34 @@ struct Triangle } + + /** + * @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 + * @return bool result if point is inside Triangle. + */ + template()))>, + 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) + { + baryCoefs = Triangle::getBarycentricCoordinates(p0, n0, n1, n2); + + for (int i = 0; i < 3; ++i) + { + if (baryCoefs[i] < 0 || baryCoefs[i] > 1) + return false; + } + + return true; + } + + /** * @brief Test if a ray intersects a triangle, and gives barycentric coordinates of the intersection if applicable * @remark Implementation for 3D only diff --git a/Sofa/framework/Geometry/test/Triangle_test.cpp b/Sofa/framework/Geometry/test/Triangle_test.cpp index f84b28d328d..bb3889f3ccd 100644 --- a/Sofa/framework/Geometry/test/Triangle_test.cpp +++ b/Sofa/framework/Geometry/test/Triangle_test.cpp @@ -39,6 +39,15 @@ class Geometry3DTriangle_test : public ::testing::Test { }; +template +class GeometryVec2DTriangle_test : public ::testing::Test +{ +}; +template +class GeometryVec3DTriangle_test : public ::testing::Test +{ +}; + using Vec2Types = ::testing::Types < std::array, sofa::type::fixed_array, sofa::type::Vec < 2, float >, std::array, sofa::type::fixed_array, sofa::type::Vec < 2, double >>; @@ -49,6 +58,13 @@ using Vec3Types = ::testing::Types < std::array, sofa::type::fixed_array, sofa::type::Vec < 3, double >>; TYPED_TEST_SUITE(Geometry3DTriangle_test, Vec3Types); +using SofaVec2Types = ::testing::Types , sofa::type::Vec < 2, double > >; +TYPED_TEST_SUITE(GeometryVec2DTriangle_test, SofaVec2Types); + +using SofaVec3Types = ::testing::Types , sofa::type::Vec < 3, double > >; +TYPED_TEST_SUITE(GeometryVec3DTriangle_test, SofaVec3Types); + + TYPED_TEST(Geometry2DTriangle_test, area) { @@ -58,7 +74,7 @@ TYPED_TEST(Geometry2DTriangle_test, area) const auto testArea = sofa::geometry::Triangle::area(a, b, c); - EXPECT_FLOAT_EQ(testArea, 12.5f); + EXPECT_FLOAT_EQ(float(testArea), 12.5f); } TYPED_TEST(Geometry3DTriangle_test, area) @@ -68,7 +84,7 @@ TYPED_TEST(Geometry3DTriangle_test, area) const TypeParam c{ 2.f, -3.f, 4.f }; const auto testArea = sofa::geometry::Triangle::area(a, b, c); - EXPECT_FLOAT_EQ(testArea, 19.306734f); + EXPECT_FLOAT_EQ(float(testArea), 19.306734f); } TYPED_TEST(Geometry2DTriangle_test, flat_area) @@ -78,7 +94,7 @@ TYPED_TEST(Geometry2DTriangle_test, flat_area) const TypeParam c{ 0.f, 1.f }; const auto testArea = sofa::geometry::Triangle::area(a, b, c); - EXPECT_FLOAT_EQ(testArea, 0.f); + EXPECT_FLOAT_EQ(testArea, 0); } TYPED_TEST(Geometry3DTriangle_test, flat_area) @@ -88,7 +104,7 @@ TYPED_TEST(Geometry3DTriangle_test, flat_area) const TypeParam c{ 0.f, 1.f, 0.f }; const auto testArea = sofa::geometry::Triangle::area(a, b, c); - EXPECT_FLOAT_EQ(testArea, 0.f); + EXPECT_EQ(testArea, 0); } @@ -98,11 +114,11 @@ TYPED_TEST(Geometry3DTriangle_test, normal) const TypeParam a{ 0.f, 0.f, 0.f }; const TypeParam b{ 0.f, 2.f, 0.f }; const TypeParam c{ 0.f, 0.f, 2.f }; - + auto normal = sofa::geometry::Triangle::normal(a, b, c); - EXPECT_FLOAT_EQ(normal[0], 4.f); - EXPECT_FLOAT_EQ(normal[1], 0.f); - EXPECT_FLOAT_EQ(normal[2], 0.f); + EXPECT_FLOAT_EQ(float(normal[0]), 4.f); + EXPECT_FLOAT_EQ(float(normal[1]), 0.f); + EXPECT_FLOAT_EQ(float(normal[2]), 0.f); // flat triangle case const TypeParam a2{ 0.f, 0.f, 0.f }; @@ -110,11 +126,152 @@ TYPED_TEST(Geometry3DTriangle_test, normal) const TypeParam c2{ 0.f, 1.f, 0.f }; normal = sofa::geometry::Triangle::normal(a2, b2, c2); - EXPECT_FLOAT_EQ(normal[0], 0.f); - EXPECT_FLOAT_EQ(normal[1], 0.f); - EXPECT_FLOAT_EQ(normal[2], 0.f); + EXPECT_FLOAT_EQ(float(normal[0]), 0.f); + EXPECT_FLOAT_EQ(float(normal[1]), 0.f); + EXPECT_FLOAT_EQ(float(normal[2]), 0.f); +} + + +TYPED_TEST(GeometryVec2DTriangle_test, isPointInTriangle) +{ + const TypeParam a{ 0., 0. }; + const TypeParam b{ 2., 0. }; + const TypeParam c{ 2., 2. }; + sofa::type::Vec3 bary; + + //// point inside + TypeParam p0{ 1.5, 0.5 }; + auto res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_FLOAT_EQ(float(bary[0]), 0.25f); + EXPECT_FLOAT_EQ(float(bary[1]), 0.5f); + EXPECT_FLOAT_EQ(float(bary[2]), 0.25f); + + // barycenter + p0 = { 4./3., 2./3. }; + const auto bVal = float(1.f / 3.f); + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_NEAR(bary[0], bVal, 1e-4); + EXPECT_NEAR(bary[1], bVal, 1e-4); + EXPECT_NEAR(bary[2], bVal, 1e-4); + + // on edge + p0 = { 1., 0. }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_NEAR(bary[0], 0.5f, 1e-4); + EXPECT_NEAR(bary[1], 0.5f, 1e-4); + EXPECT_NEAR(bary[2], 0.0f, 1e-4); + + // on corner + p0 = { 2., 0. }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_FLOAT_EQ(float(bary[0]), 0.0f); + EXPECT_FLOAT_EQ(float(bary[1]), 1.0f); + EXPECT_FLOAT_EQ(float(bary[2]), 0.0f); + + // False case + p0 = { 4., 10. }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_FALSE(res); + EXPECT_NEAR(bary[0], -1.0f, 1e-4); + EXPECT_NEAR(bary[1], -3.0f, 1e-4); + EXPECT_NEAR(bary[2], 5.0f, 1e-4); + + + // Special cases + // flat triangle + const TypeParam d{ 1., 0. }; + p0 = { 0.5, 0. }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, d, bary); + EXPECT_FALSE(res); + EXPECT_EQ(bary[0], -1); + EXPECT_EQ(bary[1], -1); + EXPECT_EQ(bary[2], -1); + + //special False cases along edge + p0 = { 3., 3. }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_FALSE(res); + EXPECT_NEAR(bary[0], -0.5f, 1e-4); + EXPECT_NEAR(bary[1], 0.f, 1e-4); + EXPECT_NEAR(bary[2], 1.5f, 1e-4); } + +TYPED_TEST(GeometryVec3DTriangle_test, isPointInTriangle) +{ + const TypeParam a{ 0., 0., 0. }; + const TypeParam b{ 2., 0., 2. }; + const TypeParam c{ 0., 2., 0. }; + + sofa::type::Vec3 bary; + + // point inside + TypeParam p0{ 0.5, 0.5, 0.5 }; + auto res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_NEAR(bary[0], 0.5f, 1e-4); + EXPECT_NEAR(bary[1], 0.25f, 1e-4); + EXPECT_NEAR(bary[2], 0.25f, 1e-4); + + // barycenter + p0 = { 2. / 3., 2. / 3. , 2. / 3. }; + const auto bVal = float(1.f / 3.f); + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_NEAR(bary[0], bVal, 1e-4); + EXPECT_NEAR(bary[1], bVal, 1e-4); + EXPECT_NEAR(bary[2], bVal, 1e-4); + + // on edge + p0 = { 1., 1., 1. }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_NEAR(bary[0], 0.f, 1e-4); + EXPECT_NEAR(bary[1], 0.5f, 1e-4); + EXPECT_NEAR(bary[2], 0.5f, 1e-4); + + // on corner + p0 = { 0., 2., 0. }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + EXPECT_TRUE(res); + EXPECT_EQ(bary[0], 0); + EXPECT_EQ(bary[1], 0); + EXPECT_EQ(bary[2], 1); + + // False cases + // out of plan + p0 = { 1., 0.2, 0.2 }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, c, bary); + 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); + + // 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[1], 1); + EXPECT_EQ(bary[2], -1); + + + // Special cases + // flat triangle + const TypeParam d{ 1., 0., 1. }; + p0 = { 0.5, 0., 0.5 }; + res = sofa::geometry::Triangle::isPointInTriangle(p0, a, b, d, bary); + EXPECT_FALSE(res); + 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. };