Skip to content

Commit 2c61228

Browse files
hugtalbothugtalbot
authored
Merge branch 'master' into 2026_template_method_full
2 parents d30395e + 912335e commit 2c61228

12 files changed

Lines changed: 677 additions & 37 deletions

File tree

‎Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/ElementCorotationalFEMForceField.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232
#include <sofa/core/behavior/ForceField.h>
3333

3434
#if !defined(ELASTICITY_COMPONENT_ELEMENT_COROTATIONAL_FEM_FORCE_FIELD_CPP)
35-
#include <sofa/component/solidmechanics/fem/elastic/finiteelement/FiniteElement[all].h>
35+
#include <sofa/fem/FiniteElement[all].h>
3636
#endif
3737

3838
namespace sofa::component::solidmechanics::fem::elastic

‎Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/ElementLinearSmallStrainFEMForceField.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828
#include <sofa/component/solidmechanics/fem/elastic/FEMForceField.h>
2929

3030
#if !defined(ELASTICITY_COMPONENT_ELEMENT_LINEAR_SMALL_STRAIN_FEM_FORCE_FIELD_CPP)
31-
#include <sofa/component/solidmechanics/fem/elastic/finiteelement/FiniteElement[all].h>
31+
#include <sofa/fem/FiniteElement[all].h>
3232
#endif
3333

3434
namespace sofa::component::solidmechanics::fem::elastic

‎Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/FEMForceField.inl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -208,7 +208,7 @@ template <class DataTypes, class ElementType>
208208
sofa::simulation::ForEachExecutionPolicy FEMForceField<DataTypes, ElementType>::getExecutionPolicy(
209209
const sofa::Data<ComputeStrategy>& strategy) const
210210
{
211-
auto computeForceStrategyAccessor = sofa::helper::getReadAccessor(d_computeForceStrategy);
211+
auto computeForceStrategyAccessor = sofa::helper::getReadAccessor(strategy);
212212
const auto& computeForceStrategy = computeForceStrategyAccessor->key();
213213

214214
return (computeForceStrategy == parallelComputeStrategy)

‎Sofa/framework/Geometry/src/sofa/geometry/Edge.h‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -165,7 +165,7 @@ struct Edge
165165
N2 = sofa::type::cross(AB, AC).norm2();
166166
}
167167

168-
if (N2 > EQUALITY_THRESHOLD || N2 < 0)
168+
if (std::abs(N2) > EQUALITY_THRESHOLD)
169169
return false;
170170

171171
return true;
@@ -197,7 +197,7 @@ struct Edge
197197

198198
//compute intersection between line and plane equation
199199
const T denominator = planeNorm * (n1 - n0);
200-
if (denominator < EQUALITY_THRESHOLD)
200+
if (std::abs(denominator) < EQUALITY_THRESHOLD)
201201
{
202202
return false;
203203
}
@@ -307,15 +307,19 @@ struct Edge
307307
const T alphaNom = AC[1] * CD[0] - AC[0] * CD[1];
308308
const T alphaDenom = AB[1] * CD[0] - AB[0] * CD[1];
309309

310-
if (alphaDenom < std::numeric_limits<T>::epsilon()) // collinear
310+
if (std::abs(alphaDenom) < std::numeric_limits<T>::epsilon()) // collinear
311311
{
312312
intersectionBaryCoord = sofa::type::Vec<2, T>(0, 0);
313313
return false;
314314
}
315315

316316
const T alpha = alphaNom / alphaDenom;
317+
// beta: pC + beta * CD = pA + alpha * AB
318+
const T beta = (std::abs(CD[0]) > std::abs(CD[1]))
319+
? (-AC[0] + alpha * AB[0]) / CD[0]
320+
: (-AC[1] + alpha * AB[1]) / CD[1];
317321

318-
if (alpha < 0 || alpha > 1)
322+
if (alpha < 0 || alpha > 1 || beta < 0 || beta > 1)
319323
{
320324
intersectionBaryCoord = sofa::type::Vec<2, T>(0, 0);
321325
return false;

‎Sofa/framework/Geometry/src/sofa/geometry/Triangle.h‎

Lines changed: 101 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -88,26 +88,54 @@ struct Triangle
8888
static constexpr auto getBarycentricCoordinates(const Node& p0, const Node& n0, const Node& n1, const Node& n2)
8989
{
9090
// Point can be written: p0 = a*n0 + b*n1 + c*n2
91-
// with a = area(n1n2p0)/area(n0n1n2), b = area(n0n2p0)/area(n0n1n2) and c = area(n0n1p0)/area(n0n1n2)
92-
const auto area = Triangle::area(n0, n1, n2);
93-
if (fabs(area) < std::numeric_limits<T>::epsilon()) // triangle is flat
91+
// with a = area(n1n2p0)/area(n0n1n2), b = area(n0n2p0)/area(n0n1n2) and c = area(n0n1p0)/area(n0n1n2)
92+
if constexpr (std::is_same_v<Node, sofa::type::Vec<3, T>>)
9493
{
95-
return sofa::type::Vec<3, T>(-1, -1, -1);
94+
// In 3D, use signed areas via dot product with triangle normal
95+
// to get correct sign for outside-triangle points
96+
const auto N = sofa::type::cross(n1 - n0, n2 - n0);
97+
const auto NdotN = sofa::type::dot(N, N);
98+
99+
if (NdotN < std::numeric_limits<T>::epsilon() * std::numeric_limits<T>::epsilon()) // triangle is flat
100+
{
101+
return sofa::type::Vec<3, T>(-1, -1, -1);
102+
}
103+
104+
sofa::type::Vec<3, T> baryCoefs(type::NOINIT);
105+
baryCoefs[0] = sofa::type::dot(N, sofa::type::cross(n2 - n1, p0 - n1)) / NdotN;
106+
baryCoefs[1] = sofa::type::dot(N, sofa::type::cross(n0 - n2, p0 - n2)) / NdotN;
107+
baryCoefs[2] = 1 - baryCoefs[0] - baryCoefs[1];
108+
109+
if (fabs(baryCoefs[2]) <= std::numeric_limits<T>::epsilon()){
110+
baryCoefs[2] = 0;
111+
}
112+
113+
return baryCoefs;
96114
}
97-
98-
const auto A0 = Triangle::area(n1, n2, p0);
99-
const auto A1 = Triangle::area(n0, p0, n2);
100-
101-
sofa::type::Vec<3, T> baryCoefs(type::NOINIT);
102-
baryCoefs[0] = A0 / area;
103-
baryCoefs[1] = A1 / area;
104-
baryCoefs[2] = 1 - baryCoefs[0] - baryCoefs[1];
105-
106-
if (fabs(baryCoefs[2]) <= std::numeric_limits<T>::epsilon()){
107-
baryCoefs[2] = 0;
115+
else
116+
{
117+
// In 2D, Triangle::area() returns a signed value (shoelace formula),
118+
// so the ratio of sub-areas to total area preserves correct signs
119+
const auto area = Triangle::area(n0, n1, n2);
120+
if (fabs(area) < std::numeric_limits<T>::epsilon()) // triangle is flat
121+
{
122+
return sofa::type::Vec<3, T>(-1, -1, -1);
123+
}
124+
125+
const auto A0 = Triangle::area(n1, n2, p0);
126+
const auto A1 = Triangle::area(n0, p0, n2);
127+
128+
sofa::type::Vec<3, T> baryCoefs(type::NOINIT);
129+
baryCoefs[0] = A0 / area;
130+
baryCoefs[1] = A1 / area;
131+
baryCoefs[2] = 1 - baryCoefs[0] - baryCoefs[1];
132+
133+
if (fabs(baryCoefs[2]) <= std::numeric_limits<T>::epsilon()){
134+
baryCoefs[2] = 0;
135+
}
136+
137+
return baryCoefs;
108138
}
109-
110-
return baryCoefs;
111139
}
112140

113141

@@ -148,23 +176,76 @@ struct Triangle
148176
}
149177

150178
}
151-
152-
179+
180+
/**
181+
* @brief Test if input point is on the plane defined by the Triangle (n0, n1, n2)
182+
* @tparam Node iterable container
183+
* @tparam T scalar
184+
* @param p0: position of the point to test
185+
* @param n0, n1, n2: nodes of the triangle
186+
* @return bool result if point is on the plane of the triangle.
187+
*/
188+
template<typename Node,
189+
typename T = std::decay_t<decltype(*std::begin(std::declval<Node>()))>,
190+
typename = std::enable_if_t<std::is_scalar_v<T>>
191+
>
192+
[[nodiscard]]
193+
static constexpr bool isPointOnPlane(const Node& p0, const Node& n0, const Node& n1, const Node& n2)
194+
{
195+
if constexpr (std::is_same_v<Node, sofa::type::Vec<3, T>>)
196+
{
197+
const auto normal = Triangle::normal(n0, n1, n2);
198+
const auto normalNorm2 = sofa::type::dot(normal, normal);
199+
if (normalNorm2 > std::numeric_limits<T>::epsilon())
200+
{
201+
const auto d = sofa::type::dot(p0 - n0, normal);
202+
if (d * d / normalNorm2 > std::numeric_limits<T>::epsilon())
203+
return false;
204+
}
205+
206+
return true;
207+
}
208+
else
209+
{
210+
// all points are trivially in the same plane
211+
return true;
212+
}
213+
}
214+
153215
/**
154216
* @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.
155217
* @tparam Node iterable container
156218
* @tparam T scalar
157219
* @param p0: position of the point to test
158220
* @param n0, n1, n2: nodes of the triangle
159221
* @param output parameter: sofa::type::Vec<3, T> barycentric coordinates of the input point in Triangle
222+
* @param assumePointIsOnPlane: optional bool to avoid testing if the point is on the plane defined by the triangle
160223
* @return bool result if point is inside Triangle.
161224
*/
162225
template<typename Node,
163226
typename T = std::decay_t<decltype(*std::begin(std::declval<Node>()))>,
164227
typename = std::enable_if_t<std::is_scalar_v<T>>
165228
>
166-
static constexpr bool isPointInTriangle(const Node& p0, const Node& n0, const Node& n1, const Node& n2, sofa::type::Vec<3, T>& baryCoefs)
229+
[[nodiscard]]
230+
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)
167231
{
232+
// In 3D, check if the point is in the plane of the triangle
233+
if constexpr (std::is_same_v<Node, sofa::type::Vec<3, T>>)
234+
{
235+
if(!assumePointIsOnPlane)
236+
{
237+
if(!isPointOnPlane(p0, n0, n1, n2))
238+
{
239+
baryCoefs = {static_cast<T>(-1), static_cast<T>(-1), static_cast<T>(-1)};
240+
return false;
241+
}
242+
}
243+
}
244+
else
245+
{
246+
SOFA_UNUSED(assumePointIsOnPlane);
247+
}
248+
168249
baryCoefs = Triangle::getBarycentricCoordinates(p0, n0, n1, n2);
169250

170251
for (int i = 0; i < 3; ++i)

‎Sofa/framework/Geometry/test/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ set(SOURCE_FILES
88
Quad_test.cpp
99
Tetrahedron_test.cpp
1010
Hexahedron_test.cpp
11+
Proximity_test.cpp
1112
)
1213

1314
add_executable(${PROJECT_NAME} ${SOURCE_FILES})

‎Sofa/framework/Geometry/test/Edge_test.cpp‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -492,4 +492,67 @@ TEST(GeometryEdge_test, intersectionWithPlane3f)
492492
EXPECT_FLOAT_EQ(inter[2], 0.0f);
493493
}
494494

495+
TEST(GeometryEdge_test, closestPointWithEdge3f_perpendicular)
496+
{
497+
// Two perpendicular edges crossing at (1,1,1)
498+
const sofa::type::Vec3f pA{ 0.f, 0.f, 0.f };
499+
const sofa::type::Vec3f pB{ 2.f, 2.f, 2.f };
500+
const sofa::type::Vec3f pC{ 0.f, 0.f, 2.f };
501+
const sofa::type::Vec3f pD{ 2.f, 2.f, 0.f };
502+
503+
sofa::type::Vec<2, float> baryCoord(sofa::type::NOINIT);
504+
sofa::geometry::Edge::closestPointWithEdge(pA, pB, pC, pD, baryCoord);
505+
506+
// alpha and beta should both be 0.5 (midpoint of each edge)
507+
EXPECT_NEAR(baryCoord[0], 0.5f, 1e-4);
508+
EXPECT_NEAR(baryCoord[1], 0.5f, 1e-4);
509+
510+
// Verify closest points
511+
const auto pX = pA + baryCoord[0] * (pB - pA);
512+
const auto pY = pC + baryCoord[1] * (pD - pC);
513+
EXPECT_NEAR(pX[0], 1.f, 1e-4);
514+
EXPECT_NEAR(pX[1], 1.f, 1e-4);
515+
EXPECT_NEAR(pX[2], 1.f, 1e-4);
516+
EXPECT_NEAR(pY[0], 1.f, 1e-4);
517+
EXPECT_NEAR(pY[1], 1.f, 1e-4);
518+
EXPECT_NEAR(pY[2], 1.f, 1e-4);
519+
}
520+
521+
TEST(GeometryEdge_test, closestPointWithEdge3f_parallel)
522+
{
523+
// Two parallel edges offset in z
524+
const sofa::type::Vec3f pA{ 0.f, 0.f, 0.f };
525+
const sofa::type::Vec3f pB{ 2.f, 0.f, 0.f };
526+
const sofa::type::Vec3f pC{ 0.f, 0.f, 1.f };
527+
const sofa::type::Vec3f pD{ 2.f, 0.f, 1.f };
528+
529+
sofa::type::Vec<2, float> baryCoord(sofa::type::NOINIT);
530+
sofa::geometry::Edge::closestPointWithEdge(pA, pB, pC, pD, baryCoord);
531+
532+
// Parallel/collinear case: denominator is near zero, should return (0,0)
533+
EXPECT_FLOAT_EQ(baryCoord[0], 0.f);
534+
EXPECT_FLOAT_EQ(baryCoord[1], 0.f);
535+
}
536+
537+
TEST(GeometryEdge_test, closestPointWithEdge3f_skew)
538+
{
539+
// Two skew edges (not intersecting, not parallel)
540+
const sofa::type::Vec3f pA{ 0.f, 0.f, 0.f };
541+
const sofa::type::Vec3f pB{ 2.f, 0.f, 0.f };
542+
const sofa::type::Vec3f pC{ 1.f, 1.f, -1.f };
543+
const sofa::type::Vec3f pD{ 1.f, 1.f, 1.f };
544+
545+
sofa::type::Vec<2, float> baryCoord(sofa::type::NOINIT);
546+
sofa::geometry::Edge::closestPointWithEdge(pA, pB, pC, pD, baryCoord);
547+
548+
// Closest point on edge1 is at alpha=0.5 (x=1), on edge2 at beta=0.5 (z=0)
549+
EXPECT_NEAR(baryCoord[0], 0.5f, 1e-4);
550+
EXPECT_NEAR(baryCoord[1], 0.5f, 1e-4);
551+
552+
const auto pX = pA + baryCoord[0] * (pB - pA);
553+
const auto pY = pC + baryCoord[1] * (pD - pC);
554+
EXPECT_NEAR(pX[0], 1.f, 1e-4);
555+
EXPECT_NEAR(pY[2], 0.f, 1e-4);
556+
}
557+
495558
}// namespace sofa

0 commit comments

Comments
 (0)