[Geometry] Bugfixes and add unit tests #5987
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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) | ||
| if constexpr (std::is_same_v<Node, sofa::type::Vec<3, T>>) | ||
| { | ||
| // 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; | ||
|
Comment on lines
+94
to
+113
Contributor
There was a problem hiding this comment. Ok, before this the barycentric mapping was a bit wrong when the point wasn't on the triangle. Because of 104, the value of the third barycentric coordinate was always underestimated because the other ones where overestimated.
Contributor
There was a problem hiding this comment. @epernod this fix the cases when the point is not on the same plane as the triangle. But does it ever happen ? Anyway I think that the api should not expect anything and check because otherwise we would rely on the user to make the check before. |
||
| } | ||
| 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; | ||
| } | ||
| } | ||
|
|
||
|
|
||
|
|
@@ -148,25 +176,75 @@ 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>> | ||
| > | ||
| [[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) | ||
| { | ||
| 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(!assumePointIsOnPlane) | ||
| { | ||
| if(!isPointOnPlane(p0, n0, n1, n2)) | ||
| return false; | ||
| } | ||
| } | ||
fredroy marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| else | ||
| { | ||
| SOFA_UNUSED(assumePointIsOnPlane); | ||
| } | ||
|
|
||
| for (int i = 0; i < 3; ++i) | ||
| { | ||
| if (baryCoefs[i] < 0 || baryCoefs[i] > 1) | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.