From c83a29e62ec650d0b934ea20bb5c7e877010cdac Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 16 Oct 2026 17:45:44 +0200 Subject: [PATCH 01/18] add unit test for PolarDecomposition.h --- .../FEM/Elastic/tests/CMakeLists.txt | 1 + .../Elastic/tests/PolarDecomposition_test.cpp | 146 ++++++++++++++++++ 2 files changed, 147 insertions(+) create mode 100644 Sofa/Component/SolidMechanics/FEM/Elastic/tests/PolarDecomposition_test.cpp diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt index 756b079090b..de1f9c47f95 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt @@ -9,6 +9,7 @@ set(SOURCE_FILES HexahedralFEMForceField_test.cpp HexahedronFEMForceField_test.cpp LinearSmallStrainFEMForceField_test.cpp + PolarDecomposition_test.cpp StrainDisplacement_test.cpp TetrahedralCorotationalFEMForceField_test.cpp TetrahedronFEMForceField_stepTest.cpp diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/PolarDecomposition_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/PolarDecomposition_test.cpp new file mode 100644 index 00000000000..17fa8924120 --- /dev/null +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/PolarDecomposition_test.cpp @@ -0,0 +1,146 @@ +/****************************************************************************** +* 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 +{ + +using namespace sofa::component::solidmechanics::fem::elastic; + +namespace +{ + +using sofa::defaulttype::Vec2Types; +using sofa::defaulttype::Vec3Types; + +template +using Matrix = typename PolarDecomposition::RotationMatrix; + +template +using Nodes = std::array, N>; + +constexpr SReal degreeToRadian = std::numbers::pi_v / 180; + +Matrix rotation(SReal angle) +{ + return {{std::cos(angle), -std::sin(angle)}, {std::sin(angle), std::cos(angle)}}; +} + +/// Rotation by an angle about an axis. +Matrix rotation(const sofa::type::Vec3& axis, SReal angle) +{ + const auto K = sofa::type::crossProductMatrix(axis.normalized()); + return Matrix::Identity() + K * std::sin(angle) + K * K * (1 - std::cos(angle)); +} + +const Matrix stretch2D {{1.5, 0.0}, {0.0, 1.0}}; +const Matrix stretch3D {{1.5, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}; + +const Nodes square { + sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), + sofa::type::Vec2(1.0, 1.0), sofa::type::Vec2(0.0, 1.0)}; +const Nodes triangle { + sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(0.0, 1.0)}; +const Nodes cube { + sofa::type::Vec3(0.0, 0.0, 0.0), sofa::type::Vec3(1.0, 0.0, 0.0), + sofa::type::Vec3(1.0, 1.0, 0.0), sofa::type::Vec3(0.0, 1.0, 0.0), + sofa::type::Vec3(0.0, 0.0, 1.0), sofa::type::Vec3(1.0, 0.0, 1.0), + sofa::type::Vec3(1.0, 1.0, 1.0), sofa::type::Vec3(0.0, 1.0, 1.0)}; +const Nodes tetrahedron { + sofa::type::Vec3(0.0, 0.0, 0.0), sofa::type::Vec3(1.0, 0.0, 0.0), + sofa::type::Vec3(0.0, 1.0, 0.0), sofa::type::Vec3(0.0, 0.0, 1.0)}; + +const sofa::type::Vec3 axis(1.0, 2.0, 3.0); + +/// Moves the nodes by the deformation gradient F = R U and checks that PolarDecomposition extracts +/// R from their positions. The tests pass either a pure rotation, F = R, or a pure stretch, F = U +/// with R = I. +template +void expectRotation(const Nodes& restPositions, const Matrix& F, + const Matrix& expected) +{ + Nodes positions; + for (std::size_t j = 0; j < N; ++j) + { + positions[j] = F * restPositions[j]; + } + + Matrix R; + PolarDecomposition().computeRotation(R, Matrix::Identity(), positions, restPositions); + + for (std::size_t i = 0; i < DataTypes::spatial_dimensions; ++i) + { + for (std::size_t j = 0; j < DataTypes::spatial_dimensions; ++j) + { + EXPECT_NEAR(R(i, j), expected(i, j), 1e-10) << "R(" << i << ", " << j << ")"; + } + } +} + +} + +TEST(PolarDecomposition, squareRotated) +{ + expectRotation(square, rotation(30 * degreeToRadian), rotation(30 * degreeToRadian)); +} + +TEST(PolarDecomposition, triangleRotated) +{ + expectRotation(triangle, rotation(30 * degreeToRadian), rotation(30 * degreeToRadian)); +} + +TEST(PolarDecomposition, squareStretched) +{ + expectRotation(square, stretch2D, Matrix::Identity()); +} + +TEST(PolarDecomposition, triangleStretched) +{ + expectRotation(triangle, stretch2D, Matrix::Identity()); +} + +TEST(PolarDecomposition, cubeRotated) +{ + expectRotation(cube, rotation(axis, 30 * degreeToRadian), rotation(axis, 30 * degreeToRadian)); +} + +TEST(PolarDecomposition, tetrahedronRotated) +{ + expectRotation(tetrahedron, rotation(axis, 30 * degreeToRadian), rotation(axis, 30 * degreeToRadian)); +} + +TEST(PolarDecomposition, cubeStretched) +{ + expectRotation(cube, stretch3D, Matrix::Identity()); +} + +TEST(PolarDecomposition, tetrahedronStretched) +{ + expectRotation(tetrahedron, stretch3D, Matrix::Identity()); +} + +} From 3543422035b905e99c9585ba146a265398d25a31 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 16 Oct 2026 18:19:52 +0200 Subject: [PATCH 02/18] Fix the polar decomposition --- .../fem/elastic/impl/rotations/PolarDecomposition.h | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index fc96cd6db28..90cbe335167 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -66,7 +66,18 @@ struct PolarDecomposition const auto H = P.multTranspose(Q); - sofa::helper::Decompose>::polarDecomposition(H, rotationMatrix); + // H = F M, with F the deformation gradient and M = Q^T Q + auto M = Q.multTranspose(Q); + + // Scale M so that small elements do not test as singular. Polar ignores the scale. + M = M / sofa::type::trace(M); + + // F = H M^-1. Embedded elements, e.g. triangles in 3D, keep H due to singular M. + RotationMatrix inverseM(sofa::type::NOINIT); + const bool isInvertible = inverseM.invert(M); + const auto F = isInvertible ? H * inverseM : H; + + sofa::helper::Decompose>::polarDecomposition(F, rotationMatrix); } static constexpr sofa::helper::Item getItem() From bd65817675475b3ba958454cf6c1c3d5dcaf33ca Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 00:23:31 +0200 Subject: [PATCH 03/18] Rename test class --- .../{PolarDecomposition_test.cpp => RotationMethods_test.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename Sofa/Component/SolidMechanics/FEM/Elastic/tests/{PolarDecomposition_test.cpp => RotationMethods_test.cpp} (100%) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/PolarDecomposition_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp similarity index 100% rename from Sofa/Component/SolidMechanics/FEM/Elastic/tests/PolarDecomposition_test.cpp rename to Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp From ec4da52b051de8ccb8a62b5f11c7bcbc08c5f8be Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 00:23:41 +0200 Subject: [PATCH 04/18] Temporarily set back rigid fit --- .../impl/rotations/PolarDecomposition.h | 23 ++++++++++--------- .../FEM/Elastic/tests/CMakeLists.txt | 2 +- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index 90cbe335167..ad26632cbee 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -66,18 +66,19 @@ struct PolarDecomposition const auto H = P.multTranspose(Q); - // H = F M, with F the deformation gradient and M = Q^T Q - auto M = Q.multTranspose(Q); + // TODO: least-squares fit F = H M^-1, on hold until embedded elements, where M is singular, are handled. + // // H = F M, with F the deformation gradient and M = Q^T Q + // auto M = Q.multTranspose(Q); + // + // // Scale M so that small elements do not test as singular. Polar ignores the scale. + // M = M / sofa::type::trace(M); + // + // // F = H M^-1. Embedded elements, e.g. triangles in 3D, keep H due to singular M. + // RotationMatrix inverseM(sofa::type::NOINIT); + // const bool isInvertible = inverseM.invert(M); + // const auto F = isInvertible ? H * inverseM : H; - // Scale M so that small elements do not test as singular. Polar ignores the scale. - M = M / sofa::type::trace(M); - - // F = H M^-1. Embedded elements, e.g. triangles in 3D, keep H due to singular M. - RotationMatrix inverseM(sofa::type::NOINIT); - const bool isInvertible = inverseM.invert(M); - const auto F = isInvertible ? H * inverseM : H; - - sofa::helper::Decompose>::polarDecomposition(F, rotationMatrix); + sofa::helper::Decompose>::polarDecomposition(H, rotationMatrix); } static constexpr sofa::helper::Item getItem() diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt index de1f9c47f95..3c2c042bae7 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt @@ -9,7 +9,7 @@ set(SOURCE_FILES HexahedralFEMForceField_test.cpp HexahedronFEMForceField_test.cpp LinearSmallStrainFEMForceField_test.cpp - PolarDecomposition_test.cpp + RotationMethods_test.cpp StrainDisplacement_test.cpp TetrahedralCorotationalFEMForceField_test.cpp TetrahedronFEMForceField_stepTest.cpp From 65f198489ee227633c83ec6abd6ba4a0d7f4a75f Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 01:09:27 +0200 Subject: [PATCH 05/18] Add stretch patch test --- .../Elastic/tests/RotationMethods_test.cpp | 295 +++++++++++++++--- 1 file changed, 258 insertions(+), 37 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 17fa8924120..b2ffc37aecd 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -19,7 +19,10 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ +#include #include +#include +#include #include #include @@ -57,90 +60,308 @@ Matrix rotation(const sofa::type::Vec3& axis, SReal angle) return Matrix::Identity() + K * std::sin(angle) + K * K * (1 - std::cos(angle)); } +const sofa::type::Vec3 axis(1.0, 2.0, 3.0); + +/// Maps the plane z = 0 to a plane that contains no coordinate axis. +const Matrix tilt = rotation(sofa::type::Vec3(1.0, -1.0, 2.0), 40 * degreeToRadian); + const Matrix stretch2D {{1.5, 0.0}, {0.0, 1.0}}; const Matrix stretch3D {{1.5, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}; -const Nodes square { +// Stretches along a direction that follows no element edge. +const Matrix offAxisStretch2D = + rotation(30 * degreeToRadian) * stretch2D * rotation(30 * degreeToRadian).transposed(); +const Matrix offAxisStretch3D = + rotation(axis, 30 * degreeToRadian) * stretch3D * rotation(axis, 30 * degreeToRadian).transposed(); + +/// offAxisStretch2D within the tilted plane. +const Matrix inPlaneStretch3D = [] +{ + Matrix stretch = Matrix::Identity(); + for (std::size_t i = 0; i < 2; ++i) + { + for (std::size_t j = 0; j < 2; ++j) + { + stretch(i, j) = offAxisStretch2D(i, j); + } + } + return tilt * stretch * tilt.transposed(); +}(); + +/// The 2D element placed in the tilted plane. +template +Nodes tiltElement(const Nodes& nodes) +{ + Nodes tiltedNodes; + for (std::size_t j = 0; j < N; ++j) + { + tiltedNodes[j] = tilt * sofa::type::Vec3(nodes[j][0], nodes[j][1], 0.0); + } + return tiltedNodes; +} + +// Regular shapes: M = sum_j q_j q_j^T is proportional to the identity. + +const Nodes regularQuad2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(1.0, 1.0), sofa::type::Vec2(0.0, 1.0)}; -const Nodes triangle { - sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(0.0, 1.0)}; -const Nodes cube { +const Nodes regularQuad3D = tiltElement(regularQuad2D); +const Nodes regularTriangle2D { + sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(0.5, std::sqrt(3.0) / 2)}; +const Nodes regularTriangle3D = tiltElement(regularTriangle2D); +const Nodes regularTetrahedron { + sofa::type::Vec3(1.0, 1.0, 1.0), sofa::type::Vec3(1.0, -1.0, -1.0), + sofa::type::Vec3(-1.0, 1.0, -1.0), sofa::type::Vec3(-1.0, -1.0, 1.0)}; +const Nodes regularHexahedron { sofa::type::Vec3(0.0, 0.0, 0.0), sofa::type::Vec3(1.0, 0.0, 0.0), sofa::type::Vec3(1.0, 1.0, 0.0), sofa::type::Vec3(0.0, 1.0, 0.0), sofa::type::Vec3(0.0, 0.0, 1.0), sofa::type::Vec3(1.0, 0.0, 1.0), sofa::type::Vec3(1.0, 1.0, 1.0), sofa::type::Vec3(0.0, 1.0, 1.0)}; -const Nodes tetrahedron { + +// Irregular shapes: M is not proportional to the identity. + +const Nodes irregularQuad2D { + sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(3.0, 0.0), + sofa::type::Vec2(3.0, 1.0), sofa::type::Vec2(0.0, 1.0)}; +const Nodes irregularQuad3D = tiltElement(irregularQuad2D); +const Nodes irregularTriangle2D { + sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(0.0, 1.0)}; +const Nodes irregularTriangle3D = tiltElement(irregularTriangle2D); +const Nodes irregularTetrahedron { sofa::type::Vec3(0.0, 0.0, 0.0), sofa::type::Vec3(1.0, 0.0, 0.0), sofa::type::Vec3(0.0, 1.0, 0.0), sofa::type::Vec3(0.0, 0.0, 1.0)}; +const Nodes irregularHexahedron { + sofa::type::Vec3(0.0, 0.0, 0.0), sofa::type::Vec3(3.0, 0.0, 0.0), + sofa::type::Vec3(3.0, 2.0, 0.0), sofa::type::Vec3(0.0, 2.0, 0.0), + sofa::type::Vec3(0.0, 0.0, 1.0), sofa::type::Vec3(3.0, 0.0, 1.0), + sofa::type::Vec3(3.0, 2.0, 1.0), sofa::type::Vec3(0.0, 2.0, 1.0)}; -const sofa::type::Vec3 axis(1.0, 2.0, 3.0); +/// Whether R equals the expected rotation, entry by entry, within 1e-10. +template +::testing::AssertionResult sameRotation(const RotationMatrix& R, const RotationMatrix& expected) +{ + for (std::size_t i = 0; i < RotationMatrix::nbLines; ++i) + { + for (std::size_t j = 0; j < RotationMatrix::nbCols; ++j) + { + if (std::abs(R(i, j) - expected(i, j)) > 1e-10) + { + return ::testing::AssertionFailure() + << "R(" << i << ", " << j << ") = " << R(i, j) << ", expected " << expected(i, j); + } + } + } + return ::testing::AssertionSuccess() << "R equals the expected rotation"; +} -/// Moves the nodes by the deformation gradient F = R U and checks that PolarDecomposition extracts -/// R from their positions. The tests pass either a pure rotation, F = R, or a pure stretch, F = U -/// with R = I. -template -void expectRotation(const Nodes& restPositions, const Matrix& F, - const Matrix& expected) +/// Rotation from the rest to the current positions. Some methods need the initial rotation, which +/// is computed first from the rest positions. +template +typename Method::RotationMatrix extractRotation(const std::array& positions, + const std::array& restPositions) { - Nodes positions; + using RotationMatrix = typename Method::RotationMatrix; + + Method method; + RotationMatrix initialRotation; + method.computeRotation(initialRotation, RotationMatrix::Identity(), restPositions, restPositions); + initialRotation.transpose(); + + RotationMatrix R; + method.computeRotation(R, initialRotation, positions, restPositions); + return R; +} + +/// A rigid motion x = R0 X + c gives R = R0. +template +::testing::AssertionResult meetsRigidMotion(const std::array& restPositions, + const typename Method::RotationMatrix& R0) +{ + Coord c; + for (std::size_t i = 0; i < Coord::spatial_dimensions; ++i) + { + c[i] = 0.5 * (i + 1); + } + + std::array positions; for (std::size_t j = 0; j < N; ++j) { - positions[j] = F * restPositions[j]; + positions[j] = R0 * restPositions[j] + c; } - Matrix R; - PolarDecomposition().computeRotation(R, Matrix::Identity(), positions, restPositions); + return sameRotation(extractRotation(positions, restPositions), R0); +} - for (std::size_t i = 0; i < DataTypes::spatial_dimensions; ++i) +/// A uniform stretch x = U X gives R = I. +template +::testing::AssertionResult meetsStretchPatch(const std::array& restPositions, + const typename Method::RotationMatrix& U) +{ + std::array positions; + for (std::size_t j = 0; j < N; ++j) { - for (std::size_t j = 0; j < DataTypes::spatial_dimensions; ++j) - { - EXPECT_NEAR(R(i, j), expected(i, j), 1e-10) << "R(" << i << ", " << j << ")"; - } + positions[j] = U * restPositions[j]; } + + return sameRotation(extractRotation(positions, restPositions), Method::RotationMatrix::Identity()); } } -TEST(PolarDecomposition, squareRotated) +TEST(RotationMethods, regularQuad2DRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation(30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation(30 * degreeToRadian))); +} + +TEST(RotationMethods, regularQuad3DRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(regularQuad3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularQuad3D, rotation(axis, 30 * degreeToRadian))); +} + +TEST(RotationMethods, regularTriangle2DRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation(30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation(30 * degreeToRadian))); +} + +TEST(RotationMethods, regularTriangle3DRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation(axis, 30 * degreeToRadian))); +} + +TEST(RotationMethods, regularTetrahedronRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation(axis, 30 * degreeToRadian))); +} + +TEST(RotationMethods, regularHexahedronRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation(axis, 30 * degreeToRadian))); +} + +TEST(RotationMethods, irregularQuad2DRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation(30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation(30 * degreeToRadian))); +} + +TEST(RotationMethods, irregularQuad3DRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(irregularQuad3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularQuad3D, rotation(axis, 30 * degreeToRadian))); +} + +TEST(RotationMethods, irregularTriangle2DRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation(30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation(30 * degreeToRadian))); +} + +TEST(RotationMethods, irregularTriangle3DRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation(axis, 30 * degreeToRadian))); +} + +TEST(RotationMethods, irregularTetrahedronRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation(axis, 30 * degreeToRadian))); +} + +TEST(RotationMethods, irregularHexahedronRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation(axis, 30 * degreeToRadian))); +} + +TEST(RotationMethods, regularQuad2DStretchPatch) +{ + EXPECT_TRUE(meetsStretchPatch>(regularQuad2D, offAxisStretch2D)); + EXPECT_TRUE(meetsStretchPatch>(regularQuad2D, offAxisStretch2D)); +} + +TEST(RotationMethods, regularQuad3DStretchPatch) +{ + EXPECT_TRUE(meetsStretchPatch>(regularQuad3D, inPlaneStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularQuad3D, inPlaneStretch3D)); +} + +TEST(RotationMethods, regularTriangle2DStretchPatch) +{ + EXPECT_TRUE(meetsStretchPatch>(regularTriangle2D, offAxisStretch2D)); + EXPECT_TRUE(meetsStretchPatch>(regularTriangle2D, offAxisStretch2D)); +} + +TEST(RotationMethods, regularTriangle3DStretchPatch) +{ + EXPECT_TRUE(meetsStretchPatch>(regularTriangle3D, inPlaneStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularTriangle3D, inPlaneStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(regularTriangle3D, inPlaneStretch3D)); +} + +TEST(RotationMethods, regularTetrahedronStretchPatch) { - expectRotation(square, rotation(30 * degreeToRadian), rotation(30 * degreeToRadian)); + EXPECT_TRUE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); } -TEST(PolarDecomposition, triangleRotated) +TEST(RotationMethods, regularHexahedronStretchPatch) { - expectRotation(triangle, rotation(30 * degreeToRadian), rotation(30 * degreeToRadian)); + EXPECT_TRUE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); } -TEST(PolarDecomposition, squareStretched) +TEST(RotationMethods, irregularQuad2DStretchPatch) { - expectRotation(square, stretch2D, Matrix::Identity()); + EXPECT_FALSE(meetsStretchPatch>(irregularQuad2D, offAxisStretch2D)); + EXPECT_FALSE(meetsStretchPatch>(irregularQuad2D, offAxisStretch2D)); } -TEST(PolarDecomposition, triangleStretched) +TEST(RotationMethods, irregularQuad3DStretchPatch) { - expectRotation(triangle, stretch2D, Matrix::Identity()); + EXPECT_FALSE(meetsStretchPatch>(irregularQuad3D, inPlaneStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularQuad3D, inPlaneStretch3D)); } -TEST(PolarDecomposition, cubeRotated) +TEST(RotationMethods, irregularTriangle2DStretchPatch) { - expectRotation(cube, rotation(axis, 30 * degreeToRadian), rotation(axis, 30 * degreeToRadian)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D)); } -TEST(PolarDecomposition, tetrahedronRotated) +TEST(RotationMethods, irregularTriangle3DStretchPatch) { - expectRotation(tetrahedron, rotation(axis, 30 * degreeToRadian), rotation(axis, 30 * degreeToRadian)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangle3D, inPlaneStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangle3D, inPlaneStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangle3D, inPlaneStretch3D)); } -TEST(PolarDecomposition, cubeStretched) +TEST(RotationMethods, irregularTetrahedronStretchPatch) { - expectRotation(cube, stretch3D, Matrix::Identity()); + EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); } -TEST(PolarDecomposition, tetrahedronStretched) +TEST(RotationMethods, irregularHexahedronStretchPatch) { - expectRotation(tetrahedron, stretch3D, Matrix::Identity()); + EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); } } From 952014f15a48c7cc793a318da2c4a418a740d6df Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 01:12:53 +0200 Subject: [PATCH 06/18] Add the renumbering test --- .../Elastic/tests/RotationMethods_test.cpp | 113 ++++++++++++++++++ 1 file changed, 113 insertions(+) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index b2ffc37aecd..5258628ad2b 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -88,6 +88,17 @@ const Matrix inPlaneStretch3D = [] return tilt * stretch * tilt.transposed(); }(); +// Stretches followed by a rotation. +const Matrix rotatedStretch2D = rotation(30 * degreeToRadian) * offAxisStretch2D; +const Matrix rotatedStretch3D = rotation(axis, 30 * degreeToRadian) * offAxisStretch3D; +const Matrix rotatedInPlaneStretch3D = rotation(axis, 30 * degreeToRadian) * inPlaneStretch3D; + +// Renumberings that keep the element valid. +const std::array triangleNumbering {1, 2, 0}; +const std::array quadNumbering {1, 2, 3, 0}; +const std::array tetrahedronNumbering {1, 2, 0, 3}; +const std::array hexahedronNumbering {1, 2, 3, 0, 5, 6, 7, 4}; + /// The 2D element placed in the tilted plane. template Nodes tiltElement(const Nodes& nodes) @@ -206,6 +217,30 @@ ::testing::AssertionResult meetsStretchPatch(const std::array& restPos return sameRotation(extractRotation(positions, restPositions), Method::RotationMatrix::Identity()); } +/// Renumbering the nodes of a deformed element x = F X leaves R unchanged. +template +::testing::AssertionResult meetsRenumbering(const std::array& restPositions, + const typename Method::RotationMatrix& F, + const std::array& numbering) +{ + std::array positions; + for (std::size_t j = 0; j < N; ++j) + { + positions[j] = F * restPositions[j]; + } + + std::array renumberedPositions; + std::array renumberedRestPositions; + for (std::size_t j = 0; j < N; ++j) + { + renumberedPositions[j] = positions[numbering[j]]; + renumberedRestPositions[j] = restPositions[numbering[j]]; + } + + return sameRotation(extractRotation(renumberedPositions, renumberedRestPositions), + extractRotation(positions, restPositions)); +} + } TEST(RotationMethods, regularQuad2DRigidMotion) @@ -364,4 +399,82 @@ TEST(RotationMethods, irregularHexahedronStretchPatch) EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); } +TEST(RotationMethods, regularQuad2DRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering)); +} + +TEST(RotationMethods, regularQuad3DRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(regularQuad3D, rotatedInPlaneStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularQuad3D, rotatedInPlaneStretch3D, quadNumbering)); +} + +TEST(RotationMethods, regularTriangle2DRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering)); +} + +TEST(RotationMethods, regularTriangle3DRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(regularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); + EXPECT_FALSE(meetsRenumbering>(regularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); +} + +TEST(RotationMethods, regularTetrahedronRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); + EXPECT_FALSE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); +} + +TEST(RotationMethods, regularHexahedronRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); + EXPECT_FALSE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); +} + +TEST(RotationMethods, irregularQuad2DRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering)); +} + +TEST(RotationMethods, irregularQuad3DRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(irregularQuad3D, rotatedInPlaneStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularQuad3D, rotatedInPlaneStretch3D, quadNumbering)); +} + +TEST(RotationMethods, irregularTriangle2DRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering)); +} + +TEST(RotationMethods, irregularTriangle3DRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(irregularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); + EXPECT_FALSE(meetsRenumbering>(irregularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); +} + +TEST(RotationMethods, irregularTetrahedronRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); + EXPECT_FALSE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); +} + +TEST(RotationMethods, irregularHexahedronRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); + EXPECT_FALSE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); +} + } From 0660b02663c745ed194f7a8319a0cfdf0d88d606 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 01:18:04 +0200 Subject: [PATCH 07/18] Re-organize constants --- .../Elastic/tests/RotationMethods_test.cpp | 134 +++++++++--------- 1 file changed, 69 insertions(+), 65 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 5258628ad2b..4a8f4ad2bb3 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -60,45 +60,11 @@ Matrix rotation(const sofa::type::Vec3& axis, SReal angle) return Matrix::Identity() + K * std::sin(angle) + K * K * (1 - std::cos(angle)); } -const sofa::type::Vec3 axis(1.0, 2.0, 3.0); +// Shapes /// Maps the plane z = 0 to a plane that contains no coordinate axis. const Matrix tilt = rotation(sofa::type::Vec3(1.0, -1.0, 2.0), 40 * degreeToRadian); -const Matrix stretch2D {{1.5, 0.0}, {0.0, 1.0}}; -const Matrix stretch3D {{1.5, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}; - -// Stretches along a direction that follows no element edge. -const Matrix offAxisStretch2D = - rotation(30 * degreeToRadian) * stretch2D * rotation(30 * degreeToRadian).transposed(); -const Matrix offAxisStretch3D = - rotation(axis, 30 * degreeToRadian) * stretch3D * rotation(axis, 30 * degreeToRadian).transposed(); - -/// offAxisStretch2D within the tilted plane. -const Matrix inPlaneStretch3D = [] -{ - Matrix stretch = Matrix::Identity(); - for (std::size_t i = 0; i < 2; ++i) - { - for (std::size_t j = 0; j < 2; ++j) - { - stretch(i, j) = offAxisStretch2D(i, j); - } - } - return tilt * stretch * tilt.transposed(); -}(); - -// Stretches followed by a rotation. -const Matrix rotatedStretch2D = rotation(30 * degreeToRadian) * offAxisStretch2D; -const Matrix rotatedStretch3D = rotation(axis, 30 * degreeToRadian) * offAxisStretch3D; -const Matrix rotatedInPlaneStretch3D = rotation(axis, 30 * degreeToRadian) * inPlaneStretch3D; - -// Renumberings that keep the element valid. -const std::array triangleNumbering {1, 2, 0}; -const std::array quadNumbering {1, 2, 3, 0}; -const std::array tetrahedronNumbering {1, 2, 0, 3}; -const std::array hexahedronNumbering {1, 2, 3, 0, 5, 6, 7, 4}; - /// The 2D element placed in the tilted plane. template Nodes tiltElement(const Nodes& nodes) @@ -147,6 +113,44 @@ const Nodes irregularHexahedron { sofa::type::Vec3(0.0, 0.0, 1.0), sofa::type::Vec3(3.0, 0.0, 1.0), sofa::type::Vec3(3.0, 2.0, 1.0), sofa::type::Vec3(0.0, 2.0, 1.0)}; +// Rigid motion + +const Matrix rotation2D = rotation(30 * degreeToRadian); +const Matrix rotation3D = rotation(sofa::type::Vec3(1.0, 2.0, 3.0), 30 * degreeToRadian); + +// Stretch patch: stretches along a direction that follows no element edge + +const Matrix stretch2D {{1.5, 0.0}, {0.0, 1.0}}; +const Matrix stretch3D {{1.5, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}; + +const Matrix offAxisStretch2D = rotation2D * stretch2D * rotation2D.transposed(); +const Matrix offAxisStretch3D = rotation3D * stretch3D * rotation3D.transposed(); + +/// offAxisStretch2D within the tilted plane. +const Matrix inPlaneStretch3D = [] +{ + Matrix stretch = Matrix::Identity(); + for (std::size_t i = 0; i < 2; ++i) + { + for (std::size_t j = 0; j < 2; ++j) + { + stretch(i, j) = offAxisStretch2D(i, j); + } + } + return tilt * stretch * tilt.transposed(); +}(); + +// Renumbering: the stretches followed by a rotation, and renumberings that keep the element valid + +const Matrix rotatedStretch2D = rotation2D * offAxisStretch2D; +const Matrix rotatedStretch3D = rotation3D * offAxisStretch3D; +const Matrix rotatedInPlaneStretch3D = rotation3D * inPlaneStretch3D; + +const std::array triangleNumbering {1, 2, 0}; +const std::array quadNumbering {1, 2, 3, 0}; +const std::array tetrahedronNumbering {1, 2, 0, 3}; +const std::array hexahedronNumbering {1, 2, 3, 0, 5, 6, 7, 4}; + /// Whether R equals the expected rotation, entry by entry, within 1e-10. template ::testing::AssertionResult sameRotation(const RotationMatrix& R, const RotationMatrix& expected) @@ -245,80 +249,80 @@ ::testing::AssertionResult meetsRenumbering(const std::array& restPosi TEST(RotationMethods, regularQuad2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation(30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation(30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation2D)); + EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation2D)); } TEST(RotationMethods, regularQuad3DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuad3D, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularQuad3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularQuad3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularQuad3D, rotation3D)); } TEST(RotationMethods, regularTriangle2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation(30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation(30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation2D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation2D)); } TEST(RotationMethods, regularTriangle3DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation3D)); } TEST(RotationMethods, regularTetrahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); } TEST(RotationMethods, regularHexahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); } TEST(RotationMethods, irregularQuad2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation(30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation(30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation2D)); + EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation2D)); } TEST(RotationMethods, irregularQuad3DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuad3D, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularQuad3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularQuad3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularQuad3D, rotation3D)); } TEST(RotationMethods, irregularTriangle2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation(30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation(30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation2D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation2D)); } TEST(RotationMethods, irregularTriangle3DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation3D)); } TEST(RotationMethods, irregularTetrahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); } TEST(RotationMethods, irregularHexahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation(axis, 30 * degreeToRadian))); - EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation(axis, 30 * degreeToRadian))); + EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); } TEST(RotationMethods, regularQuad2DStretchPatch) From d5405d942487a198348bf173c5bc1a25d6a5081b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 11:05:01 +0200 Subject: [PATCH 08/18] Add flat 2D elements in 3D and check rotation determinant --- .../Elastic/tests/RotationMethods_test.cpp | 234 +++++++++++++----- 1 file changed, 178 insertions(+), 56 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 4a8f4ad2bb3..5b7a6a93c51 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -65,14 +65,26 @@ Matrix rotation(const sofa::type::Vec3& axis, SReal angle) /// Maps the plane z = 0 to a plane that contains no coordinate axis. const Matrix tilt = rotation(sofa::type::Vec3(1.0, -1.0, 2.0), 40 * degreeToRadian); -/// The 2D element placed in the tilted plane. +/// The 2D element placed in the plane z = 0. template -Nodes tiltElement(const Nodes& nodes) +Nodes embedElement(const Nodes& nodes) { - Nodes tiltedNodes; + Nodes embeddedNodes; for (std::size_t j = 0; j < N; ++j) { - tiltedNodes[j] = tilt * sofa::type::Vec3(nodes[j][0], nodes[j][1], 0.0); + embeddedNodes[j] = sofa::type::Vec3(nodes[j][0], nodes[j][1], 0.0); + } + return embeddedNodes; +} + +/// The 2D element placed in a tilted plane. +template +Nodes tiltElement(const Nodes& nodes) +{ + Nodes tiltedNodes = embedElement(nodes); + for (auto& node : tiltedNodes) + { + node = tilt * node; } return tiltedNodes; } @@ -82,10 +94,12 @@ Nodes tiltElement(const Nodes& nodes) const Nodes regularQuad2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(1.0, 1.0), sofa::type::Vec2(0.0, 1.0)}; -const Nodes regularQuad3D = tiltElement(regularQuad2D); +const Nodes regularQuadFlat = embedElement(regularQuad2D); +const Nodes regularQuadTilted = tiltElement(regularQuad2D); const Nodes regularTriangle2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(0.5, std::sqrt(3.0) / 2)}; -const Nodes regularTriangle3D = tiltElement(regularTriangle2D); +const Nodes regularTriangleFlat = embedElement(regularTriangle2D); +const Nodes regularTriangleTilted = tiltElement(regularTriangle2D); const Nodes regularTetrahedron { sofa::type::Vec3(1.0, 1.0, 1.0), sofa::type::Vec3(1.0, -1.0, -1.0), sofa::type::Vec3(-1.0, 1.0, -1.0), sofa::type::Vec3(-1.0, -1.0, 1.0)}; @@ -100,10 +114,12 @@ const Nodes regularHexahedron { const Nodes irregularQuad2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(3.0, 0.0), sofa::type::Vec2(3.0, 1.0), sofa::type::Vec2(0.0, 1.0)}; -const Nodes irregularQuad3D = tiltElement(irregularQuad2D); +const Nodes irregularQuadFlat = embedElement(irregularQuad2D); +const Nodes irregularQuadTilted = tiltElement(irregularQuad2D); const Nodes irregularTriangle2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(0.0, 1.0)}; -const Nodes irregularTriangle3D = tiltElement(irregularTriangle2D); +const Nodes irregularTriangleFlat = embedElement(irregularTriangle2D); +const Nodes irregularTriangleTilted = tiltElement(irregularTriangle2D); const Nodes irregularTetrahedron { sofa::type::Vec3(0.0, 0.0, 0.0), sofa::type::Vec3(1.0, 0.0, 0.0), sofa::type::Vec3(0.0, 1.0, 0.0), sofa::type::Vec3(0.0, 0.0, 1.0)}; @@ -126,8 +142,8 @@ const Matrix stretch3D {{1.5, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, const Matrix offAxisStretch2D = rotation2D * stretch2D * rotation2D.transposed(); const Matrix offAxisStretch3D = rotation3D * stretch3D * rotation3D.transposed(); -/// offAxisStretch2D within the tilted plane. -const Matrix inPlaneStretch3D = [] +/// offAxisStretch2D within the plane z = 0. +const Matrix flatStretch3D = [] { Matrix stretch = Matrix::Identity(); for (std::size_t i = 0; i < 2; ++i) @@ -137,14 +153,18 @@ const Matrix inPlaneStretch3D = [] stretch(i, j) = offAxisStretch2D(i, j); } } - return tilt * stretch * tilt.transposed(); + return stretch; }(); +/// offAxisStretch2D within the tilted plane. +const Matrix tiltedStretch3D = tilt * flatStretch3D * tilt.transposed(); + // Renumbering: the stretches followed by a rotation, and renumberings that keep the element valid const Matrix rotatedStretch2D = rotation2D * offAxisStretch2D; const Matrix rotatedStretch3D = rotation3D * offAxisStretch3D; -const Matrix rotatedInPlaneStretch3D = rotation3D * inPlaneStretch3D; +const Matrix rotatedFlatStretch3D = rotation3D * flatStretch3D; +const Matrix rotatedTiltedStretch3D = rotation3D * tiltedStretch3D; const std::array triangleNumbering {1, 2, 0}; const std::array quadNumbering {1, 2, 3, 0}; @@ -169,8 +189,31 @@ ::testing::AssertionResult sameRotation(const RotationMatrix& R, const RotationM return ::testing::AssertionSuccess() << "R equals the expected rotation"; } -/// Rotation from the rest to the current positions. Some methods need the initial rotation, which -/// is computed first from the rest positions. +/// Whether R is orthogonal with determinant +1, within 1e-10. +template +::testing::AssertionResult isProperRotation(const RotationMatrix& R) +{ + const auto RtR = R.multTranspose(R); + for (std::size_t i = 0; i < RotationMatrix::nbLines; ++i) + { + for (std::size_t j = 0; j < RotationMatrix::nbCols; ++j) + { + if (std::abs(RtR(i, j) - (i == j ? 1 : 0)) > 1e-10) + { + return ::testing::AssertionFailure() + << "R is not orthogonal: (R^T R)(" << i << ", " << j << ") = " << RtR(i, j); + } + } + } + if (std::abs(sofa::type::determinant(R) - 1) > 1e-10) + { + return ::testing::AssertionFailure() << "R is not a rotation: det R = " << sofa::type::determinant(R); + } + return ::testing::AssertionSuccess(); +} + +/// Rotation from the rest to the current positions, expected to be a proper rotation. Some methods +/// need the initial rotation, which is computed first from the rest positions. template typename Method::RotationMatrix extractRotation(const std::array& positions, const std::array& restPositions) @@ -184,6 +227,7 @@ typename Method::RotationMatrix extractRotation(const std::array& posi RotationMatrix R; method.computeRotation(R, initialRotation, positions, restPositions); + EXPECT_TRUE(isProperRotation(R)); return R; } @@ -253,10 +297,16 @@ TEST(RotationMethods, regularQuad2DRigidMotion) EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation2D)); } -TEST(RotationMethods, regularQuad3DRigidMotion) +TEST(RotationMethods, regularQuadFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuad3D, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularQuad3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularQuadFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularQuadFlat, rotation3D)); +} + +TEST(RotationMethods, regularQuadTiltedRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(regularQuadTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularQuadTilted, rotation3D)); } TEST(RotationMethods, regularTriangle2DRigidMotion) @@ -265,11 +315,18 @@ TEST(RotationMethods, regularTriangle2DRigidMotion) EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation2D)); } -TEST(RotationMethods, regularTriangle3DRigidMotion) +TEST(RotationMethods, regularTriangleFlatRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); +} + +TEST(RotationMethods, regularTriangleTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularTriangle3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); } TEST(RotationMethods, regularTetrahedronRigidMotion) @@ -292,10 +349,16 @@ TEST(RotationMethods, irregularQuad2DRigidMotion) EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation2D)); } -TEST(RotationMethods, irregularQuad3DRigidMotion) +TEST(RotationMethods, irregularQuadFlatRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(irregularQuadFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularQuadFlat, rotation3D)); +} + +TEST(RotationMethods, irregularQuadTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuad3D, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularQuad3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularQuadTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularQuadTilted, rotation3D)); } TEST(RotationMethods, irregularTriangle2DRigidMotion) @@ -304,11 +367,18 @@ TEST(RotationMethods, irregularTriangle2DRigidMotion) EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation2D)); } -TEST(RotationMethods, irregularTriangle3DRigidMotion) +TEST(RotationMethods, irregularTriangleFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle3D, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); +} + +TEST(RotationMethods, irregularTriangleTiltedRigidMotion) +{ + EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); } TEST(RotationMethods, irregularTetrahedronRigidMotion) @@ -331,10 +401,16 @@ TEST(RotationMethods, regularQuad2DStretchPatch) EXPECT_TRUE(meetsStretchPatch>(regularQuad2D, offAxisStretch2D)); } -TEST(RotationMethods, regularQuad3DStretchPatch) +TEST(RotationMethods, regularQuadFlatStretchPatch) +{ + EXPECT_TRUE(meetsStretchPatch>(regularQuadFlat, flatStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularQuadFlat, flatStretch3D)); +} + +TEST(RotationMethods, regularQuadTiltedStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularQuad3D, inPlaneStretch3D)); - EXPECT_TRUE(meetsStretchPatch>(regularQuad3D, inPlaneStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularQuadTilted, tiltedStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularQuadTilted, tiltedStretch3D)); } TEST(RotationMethods, regularTriangle2DStretchPatch) @@ -343,11 +419,18 @@ TEST(RotationMethods, regularTriangle2DStretchPatch) EXPECT_TRUE(meetsStretchPatch>(regularTriangle2D, offAxisStretch2D)); } -TEST(RotationMethods, regularTriangle3DStretchPatch) +TEST(RotationMethods, regularTriangleFlatStretchPatch) +{ + EXPECT_TRUE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); +} + +TEST(RotationMethods, regularTriangleTiltedStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTriangle3D, inPlaneStretch3D)); - EXPECT_TRUE(meetsStretchPatch>(regularTriangle3D, inPlaneStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(regularTriangle3D, inPlaneStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, regularTetrahedronStretchPatch) @@ -370,10 +453,16 @@ TEST(RotationMethods, irregularQuad2DStretchPatch) EXPECT_FALSE(meetsStretchPatch>(irregularQuad2D, offAxisStretch2D)); } -TEST(RotationMethods, irregularQuad3DStretchPatch) +TEST(RotationMethods, irregularQuadFlatStretchPatch) +{ + EXPECT_FALSE(meetsStretchPatch>(irregularQuadFlat, flatStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularQuadFlat, flatStretch3D)); +} + +TEST(RotationMethods, irregularQuadTiltedStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularQuad3D, inPlaneStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularQuad3D, inPlaneStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D)); } TEST(RotationMethods, irregularTriangle2DStretchPatch) @@ -382,11 +471,18 @@ TEST(RotationMethods, irregularTriangle2DStretchPatch) EXPECT_FALSE(meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D)); } -TEST(RotationMethods, irregularTriangle3DStretchPatch) +TEST(RotationMethods, irregularTriangleFlatStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTriangle3D, inPlaneStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularTriangle3D, inPlaneStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularTriangle3D, inPlaneStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); +} + +TEST(RotationMethods, irregularTriangleTiltedStretchPatch) +{ + EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, irregularTetrahedronStretchPatch) @@ -409,10 +505,16 @@ TEST(RotationMethods, regularQuad2DRenumbering) EXPECT_TRUE(meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering)); } -TEST(RotationMethods, regularQuad3DRenumbering) +TEST(RotationMethods, regularQuadFlatRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering)); +} + +TEST(RotationMethods, regularQuadTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularQuad3D, rotatedInPlaneStretch3D, quadNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularQuad3D, rotatedInPlaneStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); } TEST(RotationMethods, regularTriangle2DRenumbering) @@ -421,11 +523,18 @@ TEST(RotationMethods, regularTriangle2DRenumbering) EXPECT_TRUE(meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering)); } -TEST(RotationMethods, regularTriangle3DRenumbering) +TEST(RotationMethods, regularTriangleFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); - EXPECT_FALSE(meetsRenumbering>(regularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); + EXPECT_FALSE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); +} + +TEST(RotationMethods, regularTriangleTiltedRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); + EXPECT_FALSE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } TEST(RotationMethods, regularTetrahedronRenumbering) @@ -448,10 +557,16 @@ TEST(RotationMethods, irregularQuad2DRenumbering) EXPECT_TRUE(meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering)); } -TEST(RotationMethods, irregularQuad3DRenumbering) +TEST(RotationMethods, irregularQuadFlatRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering)); +} + +TEST(RotationMethods, irregularQuadTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularQuad3D, rotatedInPlaneStretch3D, quadNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularQuad3D, rotatedInPlaneStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); } TEST(RotationMethods, irregularTriangle2DRenumbering) @@ -460,11 +575,18 @@ TEST(RotationMethods, irregularTriangle2DRenumbering) EXPECT_TRUE(meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering)); } -TEST(RotationMethods, irregularTriangle3DRenumbering) +TEST(RotationMethods, irregularTriangleFlatRenumbering) +{ + EXPECT_TRUE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); + EXPECT_FALSE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); +} + +TEST(RotationMethods, irregularTriangleTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); - EXPECT_FALSE(meetsRenumbering>(irregularTriangle3D, rotatedInPlaneStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); + EXPECT_FALSE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } TEST(RotationMethods, irregularTetrahedronRenumbering) From f9fc8d8cef76d9bd289010b0331b26aad8555ca8 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 11:42:58 +0200 Subject: [PATCH 09/18] Add normal completion for lower dimensional elements --- .../fem/elastic/CorotationalFEMForceField.h | 22 ++--- .../impl/rotations/PolarDecomposition.h | 27 ++++- .../Elastic/tests/RotationMethods_test.cpp | 99 ++++++++++--------- Sofa/framework/Type/src/sofa/type/Mat.h | 19 ++++ 4 files changed, 105 insertions(+), 62 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h index beaaf5995ef..8beb71d1a2f 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h @@ -54,10 +54,10 @@ namespace sofa::component::solidmechanics::fem::elastic */ template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation + StablePolarDecomposition, PolarDecomposition, IdentityRotation > { - using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation>; + using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation>; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) {} }; @@ -68,11 +68,11 @@ using Vec3Real = sofa::defaulttype::StdVectorTypes, sof //partial specialization for linear triangle in 3D template struct RotationMethods, sofa::geometry::Triangle> : RotationMethodsContainer, sofa::geometry::Triangle, - StablePolarDecomposition>, PolarDecomposition>, IdentityRotation, TriangleRotation> + StablePolarDecomposition>, PolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation> > { using Inherit = RotationMethodsContainer, sofa::geometry::Triangle, - StablePolarDecomposition>, PolarDecomposition>, IdentityRotation, TriangleRotation> >; + StablePolarDecomposition>, PolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation> >; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { @@ -83,11 +83,11 @@ struct RotationMethods, sofa::geometry::Triangle> : RotationMetho //partial specialization for linear tetrahedron template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation, TriangleRotation + StablePolarDecomposition, PolarDecomposition, IdentityRotation, TriangleRotation > { using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation, TriangleRotation >; + StablePolarDecomposition, PolarDecomposition, IdentityRotation, TriangleRotation >; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { @@ -98,11 +98,11 @@ struct RotationMethods : RotationMethods //partial specialization for linear hexahedron template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation, HexahedronRotation + StablePolarDecomposition, PolarDecomposition, IdentityRotation, HexahedronRotation > { using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation, HexahedronRotation >; + StablePolarDecomposition, PolarDecomposition, IdentityRotation, HexahedronRotation >; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { @@ -113,15 +113,15 @@ struct RotationMethods : RotationMethodsC //partial specialization for quadratic tetrahedron template struct RotationMethods : RotationMethodsContainer, IdentityRotation + PolarDecomposition, IdentityRotation > { using Inherit = RotationMethodsContainer, IdentityRotation>; + PolarDecomposition, IdentityRotation>; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { - this->d_rotationMethod.setValue(PolarDecomposition::getItem().key); + this->d_rotationMethod.setValue(PolarDecomposition::getItem().key); } }; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index ad26632cbee..b89b7735419 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -22,17 +22,25 @@ #pragma once #include +#include #include #include +#include + namespace sofa::component::solidmechanics::fem::elastic { -template +template struct PolarDecomposition { using RotationMatrix = sofa::type::Mat>; + /// The remainder of the dimensions of an embedded element within a higher-dimensional space. + // TODO: edges in 3D are not handled. + static constexpr sofa::Size Codimension = + DataTypes::spatial_dimensions - sofa::fem::FiniteElement::TopologicalDimension; + template sofa::Coord_t computeCentroid(const std::array, NumberOfNodesInElement>& nodes) { @@ -78,7 +86,22 @@ struct PolarDecomposition // const bool isInvertible = inverseM.invert(M); // const auto F = isInvertible ? H * inverseM : H; - sofa::helper::Decompose>::polarDecomposition(H, rotationMatrix); + // The H of an embedded element is singular, as it holds nothing along the element normal. + // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. + if constexpr (Codimension == 1) + { + const auto cofactorM = sofa::type::cofactor(Q.multTranspose(Q)); + // The unit normal of the element at rest. Choose the greatest one for accuracy. + const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), + [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); + const auto n = (sofa::type::cofactor(H) * n0).normalized(); + + sofa::helper::Decompose>::polarDecomposition(H + sofa::type::dyad(n, n0), rotationMatrix); + } + else + { + sofa::helper::Decompose>::polarDecomposition(H, rotationMatrix); + } } static constexpr sofa::helper::Item getItem() diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 5b7a6a93c51..657460c4729 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -41,7 +42,7 @@ using sofa::defaulttype::Vec2Types; using sofa::defaulttype::Vec3Types; template -using Matrix = typename PolarDecomposition::RotationMatrix; +using Matrix = sofa::type::Mat>; template using Nodes = std::array, N>; @@ -293,312 +294,312 @@ ::testing::AssertionResult meetsRenumbering(const std::array& restPosi TEST(RotationMethods, regularQuad2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation2D)); + EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation2D)); EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation2D)); } TEST(RotationMethods, regularQuadFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuadFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularQuadFlat, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularQuadFlat, rotation3D)); } TEST(RotationMethods, regularQuadTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuadTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularQuadTilted, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularQuadTilted, rotation3D)); } TEST(RotationMethods, regularTriangle2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation2D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation2D)); EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation2D)); } TEST(RotationMethods, regularTriangleFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); } TEST(RotationMethods, regularTriangleTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); } TEST(RotationMethods, regularTetrahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); } TEST(RotationMethods, regularHexahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); } TEST(RotationMethods, irregularQuad2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation2D)); + EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation2D)); EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation2D)); } TEST(RotationMethods, irregularQuadFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuadFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularQuadFlat, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularQuadFlat, rotation3D)); } TEST(RotationMethods, irregularQuadTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuadTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularQuadTilted, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularQuadTilted, rotation3D)); } TEST(RotationMethods, irregularTriangle2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation2D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation2D)); EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation2D)); } TEST(RotationMethods, irregularTriangleFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); } TEST(RotationMethods, irregularTriangleTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); } TEST(RotationMethods, irregularTetrahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); } TEST(RotationMethods, irregularHexahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); + EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); } TEST(RotationMethods, regularQuad2DStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularQuad2D, offAxisStretch2D)); + EXPECT_TRUE(meetsStretchPatch>(regularQuad2D, offAxisStretch2D)); EXPECT_TRUE(meetsStretchPatch>(regularQuad2D, offAxisStretch2D)); } TEST(RotationMethods, regularQuadFlatStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularQuadFlat, flatStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularQuadFlat, flatStretch3D)); EXPECT_TRUE(meetsStretchPatch>(regularQuadFlat, flatStretch3D)); } TEST(RotationMethods, regularQuadTiltedStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularQuadTilted, tiltedStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularQuadTilted, tiltedStretch3D)); EXPECT_TRUE(meetsStretchPatch>(regularQuadTilted, tiltedStretch3D)); } TEST(RotationMethods, regularTriangle2DStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTriangle2D, offAxisStretch2D)); + EXPECT_TRUE(meetsStretchPatch>(regularTriangle2D, offAxisStretch2D)); EXPECT_TRUE(meetsStretchPatch>(regularTriangle2D, offAxisStretch2D)); } TEST(RotationMethods, regularTriangleFlatStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); EXPECT_TRUE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); EXPECT_FALSE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, regularTriangleTiltedStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); EXPECT_TRUE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); EXPECT_FALSE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, regularTetrahedronStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); EXPECT_TRUE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); EXPECT_FALSE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); } TEST(RotationMethods, regularHexahedronStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); + EXPECT_TRUE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); EXPECT_TRUE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); EXPECT_FALSE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); } TEST(RotationMethods, irregularQuad2DStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularQuad2D, offAxisStretch2D)); + EXPECT_FALSE(meetsStretchPatch>(irregularQuad2D, offAxisStretch2D)); EXPECT_FALSE(meetsStretchPatch>(irregularQuad2D, offAxisStretch2D)); } TEST(RotationMethods, irregularQuadFlatStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularQuadFlat, flatStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularQuadFlat, flatStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularQuadFlat, flatStretch3D)); } TEST(RotationMethods, irregularQuadTiltedStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D)); } TEST(RotationMethods, irregularTriangle2DStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D)); EXPECT_FALSE(meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D)); } TEST(RotationMethods, irregularTriangleFlatStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, irregularTriangleTiltedStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, irregularTetrahedronStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); } TEST(RotationMethods, irregularHexahedronStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); + EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); } TEST(RotationMethods, regularQuad2DRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering)); EXPECT_TRUE(meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering)); } TEST(RotationMethods, regularQuadFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering)); EXPECT_TRUE(meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering)); } TEST(RotationMethods, regularQuadTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); EXPECT_TRUE(meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); } TEST(RotationMethods, regularTriangle2DRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering)); EXPECT_TRUE(meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering)); } TEST(RotationMethods, regularTriangleFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); EXPECT_TRUE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); EXPECT_FALSE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); } TEST(RotationMethods, regularTriangleTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); EXPECT_TRUE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); EXPECT_FALSE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } TEST(RotationMethods, regularTetrahedronRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); EXPECT_TRUE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); EXPECT_FALSE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); } TEST(RotationMethods, regularHexahedronRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); + EXPECT_TRUE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); EXPECT_TRUE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); EXPECT_FALSE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); } TEST(RotationMethods, irregularQuad2DRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering)); EXPECT_TRUE(meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering)); } TEST(RotationMethods, irregularQuadFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering)); EXPECT_TRUE(meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering)); } TEST(RotationMethods, irregularQuadTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); EXPECT_TRUE(meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); } TEST(RotationMethods, irregularTriangle2DRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering)); EXPECT_TRUE(meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering)); } TEST(RotationMethods, irregularTriangleFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); EXPECT_TRUE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); EXPECT_FALSE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); } TEST(RotationMethods, irregularTriangleTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); EXPECT_TRUE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); EXPECT_FALSE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } TEST(RotationMethods, irregularTetrahedronRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); EXPECT_TRUE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); EXPECT_FALSE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); } TEST(RotationMethods, irregularHexahedronRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); + EXPECT_TRUE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); EXPECT_TRUE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); EXPECT_FALSE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); } diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h index 32db3108afc..6526b2e1076 100644 --- a/Sofa/framework/Type/src/sofa/type/Mat.h +++ b/Sofa/framework/Type/src/sofa/type/Mat.h @@ -1043,6 +1043,25 @@ constexpr real determinant(const Mat<2,2,real>& m) noexcept - m(1,0)*m(0,1); } +/// Matrix of cofactors, such that cofactor(A) * cross(u, v) == cross(A * u, A * v). +template +constexpr Mat<3,3,real> cofactor(const Mat<3,3,real>& A) noexcept +{ + Mat<3,3,real> C(NOINIT); + C[0] = cross(A[1], A[2]); + C[1] = cross(A[2], A[0]); + C[2] = cross(A[0], A[1]); + return C; +} + +/// Matrix of cofactors, such that cofactor(A) * perp(u) == perp(A * u), with perp(u) the vector u +/// turned by 90 degrees. +template +constexpr Mat<2,2,real> cofactor(const Mat<2,2,real>& A) noexcept +{ + return {{A(1,1), -A(1,0)}, {-A(0,1), A(0,0)}}; +} + /// Determinant of a 1x1 matrix. template constexpr real determinant(const Mat<1,1,real>& m) noexcept From 3b6df92aac3bf4db24e9b39fdd8ed852cfa09387 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 11:59:38 +0200 Subject: [PATCH 10/18] Add the normal completion to stable polar as well --- .../fem/elastic/CorotationalFEMForceField.h | 16 +- .../impl/rotations/StablePolarDecomposition.h | 27 ++- .../Elastic/tests/RotationMethods_test.cpp | 192 +++++++++--------- 3 files changed, 129 insertions(+), 106 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h index 8beb71d1a2f..5706ee46433 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h @@ -54,10 +54,10 @@ namespace sofa::component::solidmechanics::fem::elastic */ template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation + StablePolarDecomposition, PolarDecomposition, IdentityRotation > { - using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation>; + using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation>; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) {} }; @@ -68,11 +68,11 @@ using Vec3Real = sofa::defaulttype::StdVectorTypes, sof //partial specialization for linear triangle in 3D template struct RotationMethods, sofa::geometry::Triangle> : RotationMethodsContainer, sofa::geometry::Triangle, - StablePolarDecomposition>, PolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation> + StablePolarDecomposition, sofa::geometry::Triangle>, PolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation> > { using Inherit = RotationMethodsContainer, sofa::geometry::Triangle, - StablePolarDecomposition>, PolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation> >; + StablePolarDecomposition, sofa::geometry::Triangle>, PolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation> >; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { @@ -83,11 +83,11 @@ struct RotationMethods, sofa::geometry::Triangle> : RotationMetho //partial specialization for linear tetrahedron template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation, TriangleRotation + StablePolarDecomposition, PolarDecomposition, IdentityRotation, TriangleRotation > { using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation, TriangleRotation >; + StablePolarDecomposition, PolarDecomposition, IdentityRotation, TriangleRotation >; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { @@ -98,11 +98,11 @@ struct RotationMethods : RotationMethods //partial specialization for linear hexahedron template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation, HexahedronRotation + StablePolarDecomposition, PolarDecomposition, IdentityRotation, HexahedronRotation > { using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation, HexahedronRotation >; + StablePolarDecomposition, PolarDecomposition, IdentityRotation, HexahedronRotation >; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h index 96f84d4569f..ddd5ea735c3 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h @@ -22,17 +22,25 @@ #pragma once #include +#include #include #include +#include + namespace sofa::component::solidmechanics::fem::elastic { -template +template struct StablePolarDecomposition { using RotationMatrix = sofa::type::Mat>; + /// The remainder of the dimensions of an embedded element within a higher-dimensional space. + // TODO: edges in 3D are not handled. + static constexpr sofa::Size Codimension = + DataTypes::spatial_dimensions - sofa::fem::FiniteElement::TopologicalDimension; + template sofa::Coord_t computeCentroid(const std::array, NumberOfNodesInElement>& nodes) { @@ -66,7 +74,22 @@ struct StablePolarDecomposition const auto H = P.multTranspose(Q); - sofa::helper::Decompose>::polarDecomposition_stable(H, rotationMatrix); + // The H of an embedded element is singular, as it holds nothing along the element normal. + // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. + if constexpr (Codimension == 1) + { + const auto cofactorM = sofa::type::cofactor(Q.multTranspose(Q)); + // The unit normal of the element at rest. Choose the greatest one for accuracy. + const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), + [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); + const auto n = (sofa::type::cofactor(H) * n0).normalized(); + + sofa::helper::Decompose>::polarDecomposition_stable(H + sofa::type::dyad(n, n0), rotationMatrix); + } + else + { + sofa::helper::Decompose>::polarDecomposition_stable(H, rotationMatrix); + } } static constexpr sofa::helper::Item getItem() diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 657460c4729..2acd93d3137 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -294,313 +294,313 @@ ::testing::AssertionResult meetsRenumbering(const std::array& restPosi TEST(RotationMethods, regularQuad2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation2D)); - EXPECT_TRUE(meetsRigidMotion>(regularQuad2D, rotation2D)); + EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); } TEST(RotationMethods, regularQuadFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuadFlat, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularQuadFlat, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); } TEST(RotationMethods, regularQuadTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularQuadTilted, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularQuadTilted, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); } TEST(RotationMethods, regularTriangle2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation2D)); - EXPECT_TRUE(meetsRigidMotion>(regularTriangle2D, rotation2D)); + EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); } TEST(RotationMethods, regularTriangleFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); } TEST(RotationMethods, regularTriangleTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); } TEST(RotationMethods, regularTetrahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(regularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTetrahedron, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); } TEST(RotationMethods, regularHexahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(regularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularHexahedron, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); } TEST(RotationMethods, irregularQuad2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation2D)); - EXPECT_TRUE(meetsRigidMotion>(irregularQuad2D, rotation2D)); + EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); } TEST(RotationMethods, irregularQuadFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuadFlat, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularQuadFlat, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); } TEST(RotationMethods, irregularQuadTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularQuadTilted, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularQuadTilted, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); } TEST(RotationMethods, irregularTriangle2DRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation2D)); - EXPECT_TRUE(meetsRigidMotion>(irregularTriangle2D, rotation2D)); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); } TEST(RotationMethods, irregularTriangleFlatRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); } TEST(RotationMethods, irregularTriangleTiltedRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); } TEST(RotationMethods, irregularTetrahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(irregularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTetrahedron, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); } TEST(RotationMethods, irregularHexahedronRigidMotion) { - EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); - EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); + EXPECT_TRUE((meetsRigidMotion>(irregularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularHexahedron, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); } TEST(RotationMethods, regularQuad2DStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularQuad2D, offAxisStretch2D)); - EXPECT_TRUE(meetsStretchPatch>(regularQuad2D, offAxisStretch2D)); + EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); } TEST(RotationMethods, regularQuadFlatStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularQuadFlat, flatStretch3D)); - EXPECT_TRUE(meetsStretchPatch>(regularQuadFlat, flatStretch3D)); + EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); } TEST(RotationMethods, regularQuadTiltedStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularQuadTilted, tiltedStretch3D)); - EXPECT_TRUE(meetsStretchPatch>(regularQuadTilted, tiltedStretch3D)); + EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); } TEST(RotationMethods, regularTriangle2DStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTriangle2D, offAxisStretch2D)); - EXPECT_TRUE(meetsStretchPatch>(regularTriangle2D, offAxisStretch2D)); + EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); } TEST(RotationMethods, regularTriangleFlatStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); - EXPECT_TRUE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, regularTriangleTiltedStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); - EXPECT_TRUE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, regularTetrahedronStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); - EXPECT_TRUE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); + EXPECT_TRUE((meetsStretchPatch>(regularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTetrahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); } TEST(RotationMethods, regularHexahedronStretchPatch) { - EXPECT_TRUE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); - EXPECT_TRUE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); + EXPECT_TRUE((meetsStretchPatch>(regularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularHexahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); } TEST(RotationMethods, irregularQuad2DStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularQuad2D, offAxisStretch2D)); - EXPECT_FALSE(meetsStretchPatch>(irregularQuad2D, offAxisStretch2D)); + EXPECT_FALSE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); } TEST(RotationMethods, irregularQuadFlatStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularQuadFlat, flatStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularQuadFlat, flatStretch3D)); + EXPECT_FALSE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); } TEST(RotationMethods, irregularQuadTiltedStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D)); + EXPECT_FALSE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); } TEST(RotationMethods, irregularTriangle2DStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D)); - EXPECT_FALSE(meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D)); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); } TEST(RotationMethods, irregularTriangleFlatStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, irregularTriangleTiltedStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, irregularTetrahedronStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); + EXPECT_FALSE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); } TEST(RotationMethods, irregularHexahedronStretchPatch) { - EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); - EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); + EXPECT_FALSE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); } TEST(RotationMethods, regularQuad2DRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering)); + EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); } TEST(RotationMethods, regularQuadFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering)); + EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); } TEST(RotationMethods, regularQuadTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); + EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); } TEST(RotationMethods, regularTriangle2DRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering)); + EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); } TEST(RotationMethods, regularTriangleFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); + EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); } TEST(RotationMethods, regularTriangleTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); + EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } TEST(RotationMethods, regularTetrahedronRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); + EXPECT_TRUE((meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); EXPECT_FALSE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); } TEST(RotationMethods, regularHexahedronRenumbering) { - EXPECT_TRUE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); - EXPECT_TRUE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); + EXPECT_TRUE((meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering))); EXPECT_FALSE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); } TEST(RotationMethods, irregularQuad2DRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering)); + EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); } TEST(RotationMethods, irregularQuadFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering)); + EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); } TEST(RotationMethods, irregularQuadTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering)); + EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); } TEST(RotationMethods, irregularTriangle2DRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering)); + EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); } TEST(RotationMethods, irregularTriangleFlatRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); } TEST(RotationMethods, irregularTriangleTiltedRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } TEST(RotationMethods, irregularTetrahedronRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); + EXPECT_TRUE((meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); } TEST(RotationMethods, irregularHexahedronRenumbering) { - EXPECT_TRUE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); - EXPECT_TRUE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); + EXPECT_TRUE((meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); } From 389b7d4ecf57bc20b4e491ac098f99f5ff089c3d Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 12:02:52 +0200 Subject: [PATCH 11/18] Switch on pass expectation for stretch patch test for irregular elements --- .../Elastic/tests/RotationMethods_test.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 2acd93d3137..ad24992d887 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -450,53 +450,53 @@ TEST(RotationMethods, regularHexahedronStretchPatch) TEST(RotationMethods, irregularQuad2DStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); - EXPECT_FALSE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); } TEST(RotationMethods, irregularQuadFlatStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); } TEST(RotationMethods, irregularQuadTiltedStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); } TEST(RotationMethods, irregularTriangle2DStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); - EXPECT_FALSE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); } TEST(RotationMethods, irregularTriangleFlatStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, irregularTriangleTiltedStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, irregularTetrahedronStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); } TEST(RotationMethods, irregularHexahedronStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); } From d4eaaa3b1c1ec4e6a0edc2106bfd5afc0288acfe Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 15:00:34 +0200 Subject: [PATCH 12/18] Add a template parameter for the nodal fit to estimate the rotation --- .../fem/elastic/CorotationalFEMForceField.h | 33 +- .../impl/rotations/PolarDecomposition.h | 56 ++-- .../impl/rotations/StablePolarDecomposition.h | 42 ++- .../Elastic/tests/RotationMethods_test.cpp | 288 ++++++++++++------ 4 files changed, 288 insertions(+), 131 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h index 5706ee46433..37a2df3fd56 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h @@ -54,10 +54,13 @@ namespace sofa::component::solidmechanics::fem::elastic */ template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation + RigidFitStablePolarDecomposition, RigidFitPolarDecomposition, IdentityRotation, + AffineFitStablePolarDecomposition, AffineFitPolarDecomposition > { - using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation>; + using Inherit = RotationMethodsContainer, RigidFitPolarDecomposition, IdentityRotation, + AffineFitStablePolarDecomposition, AffineFitPolarDecomposition>; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) {} }; @@ -68,11 +71,13 @@ using Vec3Real = sofa::defaulttype::StdVectorTypes, sof //partial specialization for linear triangle in 3D template struct RotationMethods, sofa::geometry::Triangle> : RotationMethodsContainer, sofa::geometry::Triangle, - StablePolarDecomposition, sofa::geometry::Triangle>, PolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation> + RigidFitStablePolarDecomposition, sofa::geometry::Triangle>, RigidFitPolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation>, + AffineFitStablePolarDecomposition, sofa::geometry::Triangle>, AffineFitPolarDecomposition, sofa::geometry::Triangle> > { using Inherit = RotationMethodsContainer, sofa::geometry::Triangle, - StablePolarDecomposition, sofa::geometry::Triangle>, PolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation> >; + RigidFitStablePolarDecomposition, sofa::geometry::Triangle>, RigidFitPolarDecomposition, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation>, + AffineFitStablePolarDecomposition, sofa::geometry::Triangle>, AffineFitPolarDecomposition, sofa::geometry::Triangle>>; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { @@ -83,11 +88,13 @@ struct RotationMethods, sofa::geometry::Triangle> : RotationMetho //partial specialization for linear tetrahedron template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation, TriangleRotation + RigidFitStablePolarDecomposition, RigidFitPolarDecomposition, IdentityRotation, TriangleRotation, + AffineFitStablePolarDecomposition, AffineFitPolarDecomposition > { using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation, TriangleRotation >; + RigidFitStablePolarDecomposition, RigidFitPolarDecomposition, IdentityRotation, TriangleRotation, + AffineFitStablePolarDecomposition, AffineFitPolarDecomposition>; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { @@ -98,11 +105,13 @@ struct RotationMethods : RotationMethods //partial specialization for linear hexahedron template struct RotationMethods : RotationMethodsContainer, PolarDecomposition, IdentityRotation, HexahedronRotation + RigidFitStablePolarDecomposition, RigidFitPolarDecomposition, IdentityRotation, HexahedronRotation, + AffineFitStablePolarDecomposition, AffineFitPolarDecomposition > { using Inherit = RotationMethodsContainer, PolarDecomposition, IdentityRotation, HexahedronRotation >; + RigidFitStablePolarDecomposition, RigidFitPolarDecomposition, IdentityRotation, HexahedronRotation, + AffineFitStablePolarDecomposition, AffineFitPolarDecomposition>; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { @@ -113,15 +122,17 @@ struct RotationMethods : RotationMethodsC //partial specialization for quadratic tetrahedron template struct RotationMethods : RotationMethodsContainer, IdentityRotation + RigidFitPolarDecomposition, IdentityRotation, + AffineFitPolarDecomposition > { using Inherit = RotationMethodsContainer, IdentityRotation>; + RigidFitPolarDecomposition, IdentityRotation, + AffineFitPolarDecomposition>; explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent) { - this->d_rotationMethod.setValue(PolarDecomposition::getItem().key); + this->d_rotationMethod.setValue(RigidFitPolarDecomposition::getItem().key); } }; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index b89b7735419..fba5d096bb8 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -31,7 +31,10 @@ namespace sofa::component::solidmechanics::fem::elastic { -template +/// How the motion of the nodes is fitted before the polar decomposition. +enum class Fit { Rigid, Affine }; + +template struct PolarDecomposition { using RotationMatrix = sofa::type::Mat>; @@ -73,41 +76,58 @@ struct PolarDecomposition } const auto H = P.multTranspose(Q); - - // TODO: least-squares fit F = H M^-1, on hold until embedded elements, where M is singular, are handled. - // // H = F M, with F the deformation gradient and M = Q^T Q - // auto M = Q.multTranspose(Q); - // - // // Scale M so that small elements do not test as singular. Polar ignores the scale. - // M = M / sofa::type::trace(M); - // - // // F = H M^-1. Embedded elements, e.g. triangles in 3D, keep H due to singular M. - // RotationMatrix inverseM(sofa::type::NOINIT); - // const bool isInvertible = inverseM.invert(M); - // const auto F = isInvertible ? H * inverseM : H; + RotationMatrix F = H; // The H of an embedded element is singular, as it holds nothing along the element normal. // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. if constexpr (Codimension == 1) { - const auto cofactorM = sofa::type::cofactor(Q.multTranspose(Q)); + const auto M = Q.multTranspose(Q); + const auto cofactorM = sofa::type::cofactor(M); // The unit normal of the element at rest. Choose the greatest one for accuracy. const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); const auto n = (sofa::type::cofactor(H) * n0).normalized(); - sofa::helper::Decompose>::polarDecomposition(H + sofa::type::dyad(n, n0), rotationMatrix); + if constexpr (FitType == Fit::Affine) + { + // M is scaled by its trace so that small elements do not test as singular. Polar + // ignores the scale. M has nothing along n0, which n0 n0^T fills in. + F = H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted(); + } + F += sofa::type::dyad(n, n0); } - else + else if constexpr (Codimension > 1) { - sofa::helper::Decompose>::polarDecomposition(H, rotationMatrix); + // Not handled: H is decomposed as it is. } + else if constexpr (FitType == Fit::Affine) + { + const auto M = Q.multTranspose(Q); + // M is scaled by its trace so that small elements do not test as singular. Polar ignores the scale. + F = H * (M / sofa::type::trace(M)).inverted(); + } + + sofa::helper::Decompose>::polarDecomposition(F, rotationMatrix); } static constexpr sofa::helper::Item getItem() { - return {"polar", "Polar decomposition"}; + if constexpr (FitType == Fit::Rigid) + { + return {"polar", "Polar decomposition of the rigid fit"}; + } + else + { + return {"polar_affine", "Polar decomposition of the affine fit"}; + } } }; +template +using RigidFitPolarDecomposition = PolarDecomposition; + +template +using AffineFitPolarDecomposition = PolarDecomposition; + } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h index ddd5ea735c3..b67875391b7 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h @@ -21,6 +21,7 @@ ******************************************************************************/ #pragma once +#include #include #include #include @@ -31,7 +32,7 @@ namespace sofa::component::solidmechanics::fem::elastic { -template +template struct StablePolarDecomposition { using RotationMatrix = sofa::type::Mat>; @@ -73,29 +74,58 @@ struct StablePolarDecomposition } const auto H = P.multTranspose(Q); + RotationMatrix F = H; // The H of an embedded element is singular, as it holds nothing along the element normal. // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. if constexpr (Codimension == 1) { - const auto cofactorM = sofa::type::cofactor(Q.multTranspose(Q)); + const auto M = Q.multTranspose(Q); + const auto cofactorM = sofa::type::cofactor(M); // The unit normal of the element at rest. Choose the greatest one for accuracy. const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); const auto n = (sofa::type::cofactor(H) * n0).normalized(); - sofa::helper::Decompose>::polarDecomposition_stable(H + sofa::type::dyad(n, n0), rotationMatrix); + if constexpr (FitType == Fit::Affine) + { + // M is scaled by its trace so that small elements do not test as singular. Polar + // ignores the scale. M has nothing along n0, which n0 n0^T fills in. + F = H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted(); + } + F += sofa::type::dyad(n, n0); } - else + else if constexpr (Codimension > 1) + { + // Not handled: H is decomposed as it is. + } + else if constexpr (FitType == Fit::Affine) { - sofa::helper::Decompose>::polarDecomposition_stable(H, rotationMatrix); + const auto M = Q.multTranspose(Q); + // M is scaled by its trace so that small elements do not test as singular. Polar ignores the scale. + F = H * (M / sofa::type::trace(M)).inverted(); } + + sofa::helper::Decompose>::polarDecomposition_stable(F, rotationMatrix); } static constexpr sofa::helper::Item getItem() { - return {"stable_polar", "Stable polar decomposition"}; + if constexpr (FitType == Fit::Rigid) + { + return {"stable_polar", "Stable polar decomposition of the rigid fit"}; + } + else + { + return {"stable_polar_affine", "Stable polar decomposition of the affine fit"}; + } } }; +template +using RigidFitStablePolarDecomposition = StablePolarDecomposition; + +template +using AffineFitStablePolarDecomposition = StablePolarDecomposition; + } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index ad24992d887..4a42a2fc17c 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -294,313 +294,409 @@ ::testing::AssertionResult meetsRenumbering(const std::array& restPosi TEST(RotationMethods, regularQuad2DRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); } TEST(RotationMethods, regularQuadFlatRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); } TEST(RotationMethods, regularQuadTiltedRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); } TEST(RotationMethods, regularTriangle2DRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); } TEST(RotationMethods, regularTriangleFlatRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); } TEST(RotationMethods, regularTriangleTiltedRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); } TEST(RotationMethods, regularTetrahedronRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularTetrahedron, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularTetrahedron, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularTetrahedron, rotation3D)); } TEST(RotationMethods, regularHexahedronRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularHexahedron, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(regularHexahedron, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularHexahedron, rotation3D)); } TEST(RotationMethods, irregularQuad2DRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); } TEST(RotationMethods, irregularQuadFlatRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); } TEST(RotationMethods, irregularQuadTiltedRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); } TEST(RotationMethods, irregularTriangle2DRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); } TEST(RotationMethods, irregularTriangleFlatRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); } TEST(RotationMethods, irregularTriangleTiltedRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); } TEST(RotationMethods, irregularTetrahedronRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularTetrahedron, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTetrahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularTetrahedron, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularTetrahedron, rotation3D)); } TEST(RotationMethods, irregularHexahedronRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularHexahedron, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularHexahedron, rotation3D))); + EXPECT_TRUE((meetsRigidMotion>(irregularHexahedron, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); } TEST(RotationMethods, regularQuad2DStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); } TEST(RotationMethods, regularQuadFlatStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); } TEST(RotationMethods, regularQuadTiltedStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); } TEST(RotationMethods, regularTriangle2DStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); } TEST(RotationMethods, regularTriangleFlatStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, regularTriangleTiltedStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, regularTetrahedronStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularTetrahedron, offAxisStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularTetrahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularTetrahedron, offAxisStretch3D)); } TEST(RotationMethods, regularHexahedronStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularHexahedron, offAxisStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(regularHexahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularHexahedron, offAxisStretch3D)); } TEST(RotationMethods, irregularQuad2DStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); - EXPECT_TRUE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); } TEST(RotationMethods, irregularQuadFlatStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); } TEST(RotationMethods, irregularQuadTiltedStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); } TEST(RotationMethods, irregularTriangle2DStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); - EXPECT_TRUE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); } TEST(RotationMethods, irregularTriangleFlatStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, irregularTriangleTiltedStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); } TEST(RotationMethods, irregularTetrahedronStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTetrahedron, offAxisStretch3D)); } TEST(RotationMethods, irregularHexahedronStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); + EXPECT_FALSE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); + EXPECT_TRUE((meetsStretchPatch>(irregularHexahedron, offAxisStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); } TEST(RotationMethods, regularQuad2DRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); } TEST(RotationMethods, regularQuadFlatRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); } TEST(RotationMethods, regularQuadTiltedRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); } TEST(RotationMethods, regularTriangle2DRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); } TEST(RotationMethods, regularTriangleFlatRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); } TEST(RotationMethods, regularTriangleTiltedRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } TEST(RotationMethods, regularTetrahedronRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); EXPECT_FALSE(meetsRenumbering>(regularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); } TEST(RotationMethods, regularHexahedronRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering))); EXPECT_FALSE(meetsRenumbering>(regularHexahedron, rotatedStretch3D, hexahedronNumbering)); } TEST(RotationMethods, irregularQuad2DRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); } TEST(RotationMethods, irregularQuadFlatRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); } TEST(RotationMethods, irregularQuadTiltedRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); } TEST(RotationMethods, irregularTriangle2DRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); } TEST(RotationMethods, irregularTriangleFlatRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); } TEST(RotationMethods, irregularTriangleTiltedRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } TEST(RotationMethods, irregularTetrahedronRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularTetrahedron, rotatedStretch3D, tetrahedronNumbering)); } TEST(RotationMethods, irregularHexahedronRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering))); + EXPECT_TRUE((meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); } From d1660877b44c529c89f94a58de4a26d64f285e84 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 15:08:51 +0200 Subject: [PATCH 13/18] Lambdify expression for compact --- .../impl/rotations/PolarDecomposition.h | 69 +++++++++++-------- .../impl/rotations/StablePolarDecomposition.h | 69 +++++++++++-------- 2 files changed, 80 insertions(+), 58 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index fba5d096bb8..a4e5e8ec3e1 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -75,38 +75,49 @@ struct PolarDecomposition Q[j] = nodesRestPosition[j] - t0; } - const auto H = P.multTranspose(Q); - RotationMatrix F = H; - - // The H of an embedded element is singular, as it holds nothing along the element normal. - // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. - if constexpr (Codimension == 1) + // The motion fitted to the node offsets, which the polar decomposition splits. + const auto F = [&P, &Q]() -> RotationMatrix { - const auto M = Q.multTranspose(Q); - const auto cofactorM = sofa::type::cofactor(M); - // The unit normal of the element at rest. Choose the greatest one for accuracy. - const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), - [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); - const auto n = (sofa::type::cofactor(H) * n0).normalized(); - - if constexpr (FitType == Fit::Affine) + const auto H = P.multTranspose(Q); + + // The H of an embedded element is singular, as it holds nothing along the element normal. + // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. + if constexpr (Codimension == 1) { - // M is scaled by its trace so that small elements do not test as singular. Polar - // ignores the scale. M has nothing along n0, which n0 n0^T fills in. - F = H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted(); + const auto M = Q.multTranspose(Q); + const auto cofactorM = sofa::type::cofactor(M); + // The unit normal of the element at rest. Choose the greatest one for accuracy. + const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), + [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); + const auto n = (sofa::type::cofactor(H) * n0).normalized(); + + if constexpr (FitType == Fit::Affine) + { + // M is scaled by its trace so that small elements do not test as singular. Polar + // ignores the scale. M has nothing along n0, which n0 n0^T fills in. + return H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted() + sofa::type::dyad(n, n0); + } + else + { + return H + sofa::type::dyad(n, n0); + } } - F += sofa::type::dyad(n, n0); - } - else if constexpr (Codimension > 1) - { - // Not handled: H is decomposed as it is. - } - else if constexpr (FitType == Fit::Affine) - { - const auto M = Q.multTranspose(Q); - // M is scaled by its trace so that small elements do not test as singular. Polar ignores the scale. - F = H * (M / sofa::type::trace(M)).inverted(); - } + else if constexpr (Codimension > 1) + { + // Not handled: H is decomposed as it is. + return H; + } + else if constexpr (FitType == Fit::Affine) + { + const auto M = Q.multTranspose(Q); + // M is scaled by its trace so that small elements do not test as singular. Polar ignores the scale. + return H * (M / sofa::type::trace(M)).inverted(); + } + else + { + return H; + } + }(); sofa::helper::Decompose>::polarDecomposition(F, rotationMatrix); } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h index b67875391b7..5e097577a95 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h @@ -73,38 +73,49 @@ struct StablePolarDecomposition Q[j] = nodesRestPosition[j] - t0; } - const auto H = P.multTranspose(Q); - RotationMatrix F = H; - - // The H of an embedded element is singular, as it holds nothing along the element normal. - // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. - if constexpr (Codimension == 1) + // The motion fitted to the node offsets, which the polar decomposition splits. + const auto F = [&P, &Q]() -> RotationMatrix { - const auto M = Q.multTranspose(Q); - const auto cofactorM = sofa::type::cofactor(M); - // The unit normal of the element at rest. Choose the greatest one for accuracy. - const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), - [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); - const auto n = (sofa::type::cofactor(H) * n0).normalized(); - - if constexpr (FitType == Fit::Affine) + const auto H = P.multTranspose(Q); + + // The H of an embedded element is singular, as it holds nothing along the element normal. + // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. + if constexpr (Codimension == 1) { - // M is scaled by its trace so that small elements do not test as singular. Polar - // ignores the scale. M has nothing along n0, which n0 n0^T fills in. - F = H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted(); + const auto M = Q.multTranspose(Q); + const auto cofactorM = sofa::type::cofactor(M); + // The unit normal of the element at rest. Choose the greatest one for accuracy. + const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), + [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); + const auto n = (sofa::type::cofactor(H) * n0).normalized(); + + if constexpr (FitType == Fit::Affine) + { + // M is scaled by its trace so that small elements do not test as singular. Polar + // ignores the scale. M has nothing along n0, which n0 n0^T fills in. + return H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted() + sofa::type::dyad(n, n0); + } + else + { + return H + sofa::type::dyad(n, n0); + } } - F += sofa::type::dyad(n, n0); - } - else if constexpr (Codimension > 1) - { - // Not handled: H is decomposed as it is. - } - else if constexpr (FitType == Fit::Affine) - { - const auto M = Q.multTranspose(Q); - // M is scaled by its trace so that small elements do not test as singular. Polar ignores the scale. - F = H * (M / sofa::type::trace(M)).inverted(); - } + else if constexpr (Codimension > 1) + { + // Not handled: H is decomposed as it is. + return H; + } + else if constexpr (FitType == Fit::Affine) + { + const auto M = Q.multTranspose(Q); + // M is scaled by its trace so that small elements do not test as singular. Polar ignores the scale. + return H * (M / sofa::type::trace(M)).inverted(); + } + else + { + return H; + } + }(); sofa::helper::Decompose>::polarDecomposition_stable(F, rotationMatrix); } From 47ef9b9f4b21639a9947d56d8be232fb46324745 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 15:23:59 +0200 Subject: [PATCH 14/18] Merge Stable and PolarDecomposition --- .../SolidMechanics/FEM/Elastic/CMakeLists.txt | 1 - .../fem/elastic/CorotationalFEMForceField.h | 1 - .../impl/rotations/PolarDecomposition.h | 44 +++++- .../impl/rotations/StablePolarDecomposition.h | 142 ------------------ .../Elastic/tests/RotationMethods_test.cpp | 1 - 5 files changed, 37 insertions(+), 152 deletions(-) delete mode 100644 Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt b/Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt index 85447e27cdc..e62a5419031 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt @@ -62,7 +62,6 @@ set(HEADER_FILES ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/IdentityRotation.h ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/PolarDecomposition.h ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/RotationMethodsContainer.h - ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/StablePolarDecomposition.h ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/TriangleRotation.h ) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h index 37a2df3fd56..4f610a84d83 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h @@ -27,7 +27,6 @@ #include #include #include -#include #include #include #include diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index a4e5e8ec3e1..5ee38cc5717 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -34,7 +34,10 @@ namespace sofa::component::solidmechanics::fem::elastic /// How the motion of the nodes is fitted before the polar decomposition. enum class Fit { Rigid, Affine }; -template +/// How the polar decomposition is computed: by Newton iterations, or through an SVD. +enum class Algorithm { Newton, Stable }; + +template struct PolarDecomposition { using RotationMatrix = sofa::type::Mat>; @@ -119,26 +122,53 @@ struct PolarDecomposition } }(); - sofa::helper::Decompose>::polarDecomposition(F, rotationMatrix); + if constexpr (AlgorithmType == Algorithm::Newton) + { + sofa::helper::Decompose>::polarDecomposition(F, rotationMatrix); + } + else + { + sofa::helper::Decompose>::polarDecomposition_stable(F, rotationMatrix); + } } static constexpr sofa::helper::Item getItem() { - if constexpr (FitType == Fit::Rigid) + if constexpr (AlgorithmType == Algorithm::Newton) { - return {"polar", "Polar decomposition of the rigid fit"}; + if constexpr (FitType == Fit::Rigid) + { + return {"polar", "Polar decomposition of the rigid fit"}; + } + else + { + return {"polar_affine", "Polar decomposition of the affine fit"}; + } } else { - return {"polar_affine", "Polar decomposition of the affine fit"}; + if constexpr (FitType == Fit::Rigid) + { + return {"stable_polar", "Stable polar decomposition of the rigid fit"}; + } + else + { + return {"stable_polar_affine", "Stable polar decomposition of the affine fit"}; + } } } }; template -using RigidFitPolarDecomposition = PolarDecomposition; +using RigidFitPolarDecomposition = PolarDecomposition; + +template +using AffineFitPolarDecomposition = PolarDecomposition; + +template +using RigidFitStablePolarDecomposition = PolarDecomposition; template -using AffineFitPolarDecomposition = PolarDecomposition; +using AffineFitStablePolarDecomposition = PolarDecomposition; } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h deleted file mode 100644 index 5e097577a95..00000000000 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h +++ /dev/null @@ -1,142 +0,0 @@ -/****************************************************************************** -* 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 * -******************************************************************************/ -#pragma once - -#include -#include -#include -#include -#include - -#include - -namespace sofa::component::solidmechanics::fem::elastic -{ - -template -struct StablePolarDecomposition -{ - using RotationMatrix = sofa::type::Mat>; - - /// The remainder of the dimensions of an embedded element within a higher-dimensional space. - // TODO: edges in 3D are not handled. - static constexpr sofa::Size Codimension = - DataTypes::spatial_dimensions - sofa::fem::FiniteElement::TopologicalDimension; - - template - sofa::Coord_t computeCentroid(const std::array, NumberOfNodesInElement>& nodes) - { - sofa::Coord_t centroid; - for (const auto& node : nodes) - { - centroid += node; - } - centroid /= static_cast>(NumberOfNodesInElement); - return centroid; - } - - template - void computeRotation(RotationMatrix& rotationMatrix, const RotationMatrix& initialRotationMatrix, - const std::array, NumberOfNodesInElement>& nodesPosition, - const std::array, NumberOfNodesInElement>& nodesRestPosition) - { - SOFA_UNUSED(initialRotationMatrix); - - const auto t = computeCentroid(nodesPosition); - const auto t0 = computeCentroid(nodesRestPosition); - - sofa::type::Mat> P(sofa::type::NOINIT); - sofa::type::Mat> Q(sofa::type::NOINIT); - - for (sofa::Size j = 0; j < NumberOfNodesInElement; ++j) - { - P[j] = nodesPosition[j] - t; - Q[j] = nodesRestPosition[j] - t0; - } - - // The motion fitted to the node offsets, which the polar decomposition splits. - const auto F = [&P, &Q]() -> RotationMatrix - { - const auto H = P.multTranspose(Q); - - // The H of an embedded element is singular, as it holds nothing along the element normal. - // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. - if constexpr (Codimension == 1) - { - const auto M = Q.multTranspose(Q); - const auto cofactorM = sofa::type::cofactor(M); - // The unit normal of the element at rest. Choose the greatest one for accuracy. - const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), - [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); - const auto n = (sofa::type::cofactor(H) * n0).normalized(); - - if constexpr (FitType == Fit::Affine) - { - // M is scaled by its trace so that small elements do not test as singular. Polar - // ignores the scale. M has nothing along n0, which n0 n0^T fills in. - return H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted() + sofa::type::dyad(n, n0); - } - else - { - return H + sofa::type::dyad(n, n0); - } - } - else if constexpr (Codimension > 1) - { - // Not handled: H is decomposed as it is. - return H; - } - else if constexpr (FitType == Fit::Affine) - { - const auto M = Q.multTranspose(Q); - // M is scaled by its trace so that small elements do not test as singular. Polar ignores the scale. - return H * (M / sofa::type::trace(M)).inverted(); - } - else - { - return H; - } - }(); - - sofa::helper::Decompose>::polarDecomposition_stable(F, rotationMatrix); - } - - static constexpr sofa::helper::Item getItem() - { - if constexpr (FitType == Fit::Rigid) - { - return {"stable_polar", "Stable polar decomposition of the rigid fit"}; - } - else - { - return {"stable_polar_affine", "Stable polar decomposition of the affine fit"}; - } - } -}; - -template -using RigidFitStablePolarDecomposition = StablePolarDecomposition; - -template -using AffineFitStablePolarDecomposition = StablePolarDecomposition; - -} diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 4a42a2fc17c..6b3cc45a969 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -21,7 +21,6 @@ ******************************************************************************/ #include #include -#include #include #include #include From 1c1a0dbdae3d7ea71daf2da04cb7adb403d3a9f2 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 15:27:25 +0200 Subject: [PATCH 15/18] Rename Algorithm to Standard and Stable --- .../fem/elastic/impl/rotations/PolarDecomposition.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index 5ee38cc5717..3736b09b46c 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -34,8 +34,9 @@ namespace sofa::component::solidmechanics::fem::elastic /// How the motion of the nodes is fitted before the polar decomposition. enum class Fit { Rigid, Affine }; -/// How the polar decomposition is computed: by Newton iterations, or through an SVD. -enum class Algorithm { Newton, Stable }; +/// How the polar decomposition is computed: Standard calls Decompose::polarDecomposition, Stable +/// calls Decompose::polarDecomposition_stable, which returns a rotation even for an inverted element. +enum class Algorithm { Standard, Stable }; template struct PolarDecomposition @@ -122,7 +123,7 @@ struct PolarDecomposition } }(); - if constexpr (AlgorithmType == Algorithm::Newton) + if constexpr (AlgorithmType == Algorithm::Standard) { sofa::helper::Decompose>::polarDecomposition(F, rotationMatrix); } @@ -134,7 +135,7 @@ struct PolarDecomposition static constexpr sofa::helper::Item getItem() { - if constexpr (AlgorithmType == Algorithm::Newton) + if constexpr (AlgorithmType == Algorithm::Standard) { if constexpr (FitType == Fit::Rigid) { @@ -160,10 +161,10 @@ struct PolarDecomposition }; template -using RigidFitPolarDecomposition = PolarDecomposition; +using RigidFitPolarDecomposition = PolarDecomposition; template -using AffineFitPolarDecomposition = PolarDecomposition; +using AffineFitPolarDecomposition = PolarDecomposition; template using RigidFitStablePolarDecomposition = PolarDecomposition; From d34e6c4e907546b5307ef6d699e891bae3a21576 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 15:36:18 +0200 Subject: [PATCH 16/18] Add Edges in 2D tests --- .../Elastic/tests/RotationMethods_test.cpp | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 6b3cc45a969..8ca86ca5d4f 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -129,6 +129,12 @@ const Nodes irregularHexahedron { sofa::type::Vec3(0.0, 0.0, 1.0), sofa::type::Vec3(3.0, 0.0, 1.0), sofa::type::Vec3(3.0, 2.0, 1.0), sofa::type::Vec3(0.0, 2.0, 1.0)}; +// Edges in 2D: along the x axis, and turned by 25 degrees. + +const Matrix edgeTilt = rotation(25 * degreeToRadian); +const Nodes edgeFlat {sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(2.0, 0.0)}; +const Nodes edgeTilted {edgeTilt * edgeFlat[0], edgeTilt * edgeFlat[1]}; + // Rigid motion const Matrix rotation2D = rotation(30 * degreeToRadian); @@ -142,6 +148,10 @@ const Matrix stretch3D {{1.5, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, const Matrix offAxisStretch2D = rotation2D * stretch2D * rotation2D.transposed(); const Matrix offAxisStretch3D = rotation3D * stretch3D * rotation3D.transposed(); +// An edge is stretched along its own line. +const Matrix flatEdgeStretch = stretch2D; +const Matrix tiltedEdgeStretch = edgeTilt * stretch2D * edgeTilt.transposed(); + /// offAxisStretch2D within the plane z = 0. const Matrix flatStretch3D = [] { @@ -166,6 +176,10 @@ const Matrix rotatedStretch3D = rotation3D * offAxisStretch3D; const Matrix rotatedFlatStretch3D = rotation3D * flatStretch3D; const Matrix rotatedTiltedStretch3D = rotation3D * tiltedStretch3D; +const Matrix rotatedFlatEdgeStretch = rotation2D * flatEdgeStretch; +const Matrix rotatedTiltedEdgeStretch = rotation2D * tiltedEdgeStretch; + +const std::array edgeNumbering {1, 0}; const std::array triangleNumbering {1, 2, 0}; const std::array quadNumbering {1, 2, 3, 0}; const std::array tetrahedronNumbering {1, 2, 0, 3}; @@ -427,6 +441,22 @@ TEST(RotationMethods, irregularHexahedronRigidMotion) EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); } +TEST(RotationMethods, edgeFlatRigidMotion) +{ + EXPECT_TRUE((meetsRigidMotion>(edgeFlat, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(edgeFlat, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(edgeFlat, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(edgeFlat, rotation2D))); +} + +TEST(RotationMethods, edgeTiltedRigidMotion) +{ + EXPECT_TRUE((meetsRigidMotion>(edgeTilted, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(edgeTilted, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(edgeTilted, rotation2D))); + EXPECT_TRUE((meetsRigidMotion>(edgeTilted, rotation2D))); +} + TEST(RotationMethods, regularQuad2DStretchPatch) { EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); @@ -563,6 +593,22 @@ TEST(RotationMethods, irregularHexahedronStretchPatch) EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); } +TEST(RotationMethods, edgeFlatStretchPatch) +{ + EXPECT_TRUE((meetsStretchPatch>(edgeFlat, flatEdgeStretch))); + EXPECT_TRUE((meetsStretchPatch>(edgeFlat, flatEdgeStretch))); + EXPECT_TRUE((meetsStretchPatch>(edgeFlat, flatEdgeStretch))); + EXPECT_TRUE((meetsStretchPatch>(edgeFlat, flatEdgeStretch))); +} + +TEST(RotationMethods, edgeTiltedStretchPatch) +{ + EXPECT_TRUE((meetsStretchPatch>(edgeTilted, tiltedEdgeStretch))); + EXPECT_TRUE((meetsStretchPatch>(edgeTilted, tiltedEdgeStretch))); + EXPECT_TRUE((meetsStretchPatch>(edgeTilted, tiltedEdgeStretch))); + EXPECT_TRUE((meetsStretchPatch>(edgeTilted, tiltedEdgeStretch))); +} + TEST(RotationMethods, regularQuad2DRenumbering) { EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); @@ -699,4 +745,20 @@ TEST(RotationMethods, irregularHexahedronRenumbering) EXPECT_FALSE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); } +TEST(RotationMethods, edgeFlatRenumbering) +{ + EXPECT_TRUE((meetsRenumbering>(edgeFlat, rotatedFlatEdgeStretch, edgeNumbering))); + EXPECT_TRUE((meetsRenumbering>(edgeFlat, rotatedFlatEdgeStretch, edgeNumbering))); + EXPECT_TRUE((meetsRenumbering>(edgeFlat, rotatedFlatEdgeStretch, edgeNumbering))); + EXPECT_TRUE((meetsRenumbering>(edgeFlat, rotatedFlatEdgeStretch, edgeNumbering))); +} + +TEST(RotationMethods, edgeTiltedRenumbering) +{ + EXPECT_TRUE((meetsRenumbering>(edgeTilted, rotatedTiltedEdgeStretch, edgeNumbering))); + EXPECT_TRUE((meetsRenumbering>(edgeTilted, rotatedTiltedEdgeStretch, edgeNumbering))); + EXPECT_TRUE((meetsRenumbering>(edgeTilted, rotatedTiltedEdgeStretch, edgeNumbering))); + EXPECT_TRUE((meetsRenumbering>(edgeTilted, rotatedTiltedEdgeStretch, edgeNumbering))); +} + } From e3ce45ca5f5324628633eafd4b1217d505ffa599 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 23 Oct 2026 17:50:32 +0200 Subject: [PATCH 17/18] Re-write comments --- .../elastic/impl/rotations/PolarDecomposition.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index 3736b09b46c..8d751954197 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -34,8 +34,9 @@ namespace sofa::component::solidmechanics::fem::elastic /// How the motion of the nodes is fitted before the polar decomposition. enum class Fit { Rigid, Affine }; -/// How the polar decomposition is computed: Standard calls Decompose::polarDecomposition, Stable -/// calls Decompose::polarDecomposition_stable, which returns a rotation even for an inverted element. +/// How the polar decomposition is computed: +/// Standard calls Decompose::polarDecomposition +/// Stable calls Decompose::polarDecomposition_stable enum class Algorithm { Standard, Stable }; template @@ -79,26 +80,26 @@ struct PolarDecomposition Q[j] = nodesRestPosition[j] - t0; } - // The motion fitted to the node offsets, which the polar decomposition splits. const auto F = [&P, &Q]() -> RotationMatrix { const auto H = P.multTranspose(Q); - // The H of an embedded element is singular, as it holds nothing along the element normal. - // Adding n n0^T completes it, so that R turns the rest normal n0 into the current normal n. if constexpr (Codimension == 1) { + // The H of an embedded element is singular. It holds nothing along the element + // normal. It is completed by mapping the current normal to the rest normal. const auto M = Q.multTranspose(Q); const auto cofactorM = sofa::type::cofactor(M); // The unit normal of the element at rest. Choose the greatest one for accuracy. const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); + // The current normal of the element. const auto n = (sofa::type::cofactor(H) * n0).normalized(); if constexpr (FitType == Fit::Affine) { - // M is scaled by its trace so that small elements do not test as singular. Polar - // ignores the scale. M has nothing along n0, which n0 n0^T fills in. + // M is scaled by its trace so that small elements do not test as singular. + // Polar ignores the scale. M has nothing along n0, which n0 n0^T fills in. return H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted() + sofa::type::dyad(n, n0); } else @@ -114,7 +115,6 @@ struct PolarDecomposition else if constexpr (FitType == Fit::Affine) { const auto M = Q.multTranspose(Q); - // M is scaled by its trace so that small elements do not test as singular. Polar ignores the scale. return H * (M / sofa::type::trace(M)).inverted(); } else From 53c9e7a4ecdeda81385f63d6bc71ecd334316fdc Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 3 Nov 2026 17:37:57 +0100 Subject: [PATCH 18/18] Remove handling of embedded elements --- .../impl/rotations/PolarDecomposition.h | 29 +-- .../Elastic/tests/RotationMethods_test.cpp | 210 ------------------ Sofa/framework/Type/src/sofa/type/Mat.h | 19 -- 3 files changed, 2 insertions(+), 256 deletions(-) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h index 8d751954197..f9b413d911a 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h @@ -26,8 +26,6 @@ #include #include -#include - namespace sofa::component::solidmechanics::fem::elastic { @@ -45,7 +43,7 @@ struct PolarDecomposition using RotationMatrix = sofa::type::Mat>; /// The remainder of the dimensions of an embedded element within a higher-dimensional space. - // TODO: edges in 3D are not handled. + // TODO: embedded elements (Codimension > 0) are not handled. static constexpr sofa::Size Codimension = DataTypes::spatial_dimensions - sofa::fem::FiniteElement::TopologicalDimension; @@ -84,30 +82,7 @@ struct PolarDecomposition { const auto H = P.multTranspose(Q); - if constexpr (Codimension == 1) - { - // The H of an embedded element is singular. It holds nothing along the element - // normal. It is completed by mapping the current normal to the rest normal. - const auto M = Q.multTranspose(Q); - const auto cofactorM = sofa::type::cofactor(M); - // The unit normal of the element at rest. Choose the greatest one for accuracy. - const auto n0 = std::max_element(cofactorM.begin(), cofactorM.end(), - [](const auto& a, const auto& b) { return a.norm2() < b.norm2(); })->normalized(); - // The current normal of the element. - const auto n = (sofa::type::cofactor(H) * n0).normalized(); - - if constexpr (FitType == Fit::Affine) - { - // M is scaled by its trace so that small elements do not test as singular. - // Polar ignores the scale. M has nothing along n0, which n0 n0^T fills in. - return H * (M / sofa::type::trace(M) + sofa::type::dyad(n0, n0)).inverted() + sofa::type::dyad(n, n0); - } - else - { - return H + sofa::type::dyad(n, n0); - } - } - else if constexpr (Codimension > 1) + if constexpr (Codimension > 0) { // Not handled: H is decomposed as it is. return H; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp index 8ca86ca5d4f..59ba5091d2d 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/RotationMethods_test.cpp @@ -94,8 +94,6 @@ Nodes tiltElement(const Nodes& nodes) const Nodes regularQuad2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(1.0, 1.0), sofa::type::Vec2(0.0, 1.0)}; -const Nodes regularQuadFlat = embedElement(regularQuad2D); -const Nodes regularQuadTilted = tiltElement(regularQuad2D); const Nodes regularTriangle2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(0.5, std::sqrt(3.0) / 2)}; const Nodes regularTriangleFlat = embedElement(regularTriangle2D); @@ -114,8 +112,6 @@ const Nodes regularHexahedron { const Nodes irregularQuad2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(3.0, 0.0), sofa::type::Vec2(3.0, 1.0), sofa::type::Vec2(0.0, 1.0)}; -const Nodes irregularQuadFlat = embedElement(irregularQuad2D); -const Nodes irregularQuadTilted = tiltElement(irregularQuad2D); const Nodes irregularTriangle2D { sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(1.0, 0.0), sofa::type::Vec2(0.0, 1.0)}; const Nodes irregularTriangleFlat = embedElement(irregularTriangle2D); @@ -129,12 +125,6 @@ const Nodes irregularHexahedron { sofa::type::Vec3(0.0, 0.0, 1.0), sofa::type::Vec3(3.0, 0.0, 1.0), sofa::type::Vec3(3.0, 2.0, 1.0), sofa::type::Vec3(0.0, 2.0, 1.0)}; -// Edges in 2D: along the x axis, and turned by 25 degrees. - -const Matrix edgeTilt = rotation(25 * degreeToRadian); -const Nodes edgeFlat {sofa::type::Vec2(0.0, 0.0), sofa::type::Vec2(2.0, 0.0)}; -const Nodes edgeTilted {edgeTilt * edgeFlat[0], edgeTilt * edgeFlat[1]}; - // Rigid motion const Matrix rotation2D = rotation(30 * degreeToRadian); @@ -148,10 +138,6 @@ const Matrix stretch3D {{1.5, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, const Matrix offAxisStretch2D = rotation2D * stretch2D * rotation2D.transposed(); const Matrix offAxisStretch3D = rotation3D * stretch3D * rotation3D.transposed(); -// An edge is stretched along its own line. -const Matrix flatEdgeStretch = stretch2D; -const Matrix tiltedEdgeStretch = edgeTilt * stretch2D * edgeTilt.transposed(); - /// offAxisStretch2D within the plane z = 0. const Matrix flatStretch3D = [] { @@ -176,10 +162,6 @@ const Matrix rotatedStretch3D = rotation3D * offAxisStretch3D; const Matrix rotatedFlatStretch3D = rotation3D * flatStretch3D; const Matrix rotatedTiltedStretch3D = rotation3D * tiltedStretch3D; -const Matrix rotatedFlatEdgeStretch = rotation2D * flatEdgeStretch; -const Matrix rotatedTiltedEdgeStretch = rotation2D * tiltedEdgeStretch; - -const std::array edgeNumbering {1, 0}; const std::array triangleNumbering {1, 2, 0}; const std::array quadNumbering {1, 2, 3, 0}; const std::array tetrahedronNumbering {1, 2, 0, 3}; @@ -313,22 +295,6 @@ TEST(RotationMethods, regularQuad2DRigidMotion) EXPECT_TRUE((meetsRigidMotion>(regularQuad2D, rotation2D))); } -TEST(RotationMethods, regularQuadFlatRigidMotion) -{ - EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuadFlat, rotation3D))); -} - -TEST(RotationMethods, regularQuadTiltedRigidMotion) -{ - EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularQuadTilted, rotation3D))); -} - TEST(RotationMethods, regularTriangle2DRigidMotion) { EXPECT_TRUE((meetsRigidMotion>(regularTriangle2D, rotation2D))); @@ -339,19 +305,11 @@ TEST(RotationMethods, regularTriangle2DRigidMotion) TEST(RotationMethods, regularTriangleFlatRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangleFlat, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularTriangleFlat, rotation3D)); } TEST(RotationMethods, regularTriangleTiltedRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(regularTriangleTilted, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(regularTriangleTilted, rotation3D)); } @@ -381,22 +339,6 @@ TEST(RotationMethods, irregularQuad2DRigidMotion) EXPECT_TRUE((meetsRigidMotion>(irregularQuad2D, rotation2D))); } -TEST(RotationMethods, irregularQuadFlatRigidMotion) -{ - EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuadFlat, rotation3D))); -} - -TEST(RotationMethods, irregularQuadTiltedRigidMotion) -{ - EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularQuadTilted, rotation3D))); -} - TEST(RotationMethods, irregularTriangle2DRigidMotion) { EXPECT_TRUE((meetsRigidMotion>(irregularTriangle2D, rotation2D))); @@ -407,19 +349,11 @@ TEST(RotationMethods, irregularTriangle2DRigidMotion) TEST(RotationMethods, irregularTriangleFlatRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleFlat, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleFlat, rotation3D)); } TEST(RotationMethods, irregularTriangleTiltedRigidMotion) { - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); - EXPECT_TRUE((meetsRigidMotion>(irregularTriangleTilted, rotation3D))); EXPECT_TRUE(meetsRigidMotion>(irregularTriangleTilted, rotation3D)); } @@ -441,22 +375,6 @@ TEST(RotationMethods, irregularHexahedronRigidMotion) EXPECT_TRUE(meetsRigidMotion>(irregularHexahedron, rotation3D)); } -TEST(RotationMethods, edgeFlatRigidMotion) -{ - EXPECT_TRUE((meetsRigidMotion>(edgeFlat, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(edgeFlat, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(edgeFlat, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(edgeFlat, rotation2D))); -} - -TEST(RotationMethods, edgeTiltedRigidMotion) -{ - EXPECT_TRUE((meetsRigidMotion>(edgeTilted, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(edgeTilted, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(edgeTilted, rotation2D))); - EXPECT_TRUE((meetsRigidMotion>(edgeTilted, rotation2D))); -} - TEST(RotationMethods, regularQuad2DStretchPatch) { EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); @@ -465,22 +383,6 @@ TEST(RotationMethods, regularQuad2DStretchPatch) EXPECT_TRUE((meetsStretchPatch>(regularQuad2D, offAxisStretch2D))); } -TEST(RotationMethods, regularQuadFlatStretchPatch) -{ - EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuadFlat, flatStretch3D))); -} - -TEST(RotationMethods, regularQuadTiltedStretchPatch) -{ - EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularQuadTilted, tiltedStretch3D))); -} - TEST(RotationMethods, regularTriangle2DStretchPatch) { EXPECT_TRUE((meetsStretchPatch>(regularTriangle2D, offAxisStretch2D))); @@ -491,19 +393,11 @@ TEST(RotationMethods, regularTriangle2DStretchPatch) TEST(RotationMethods, regularTriangleFlatStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangleFlat, flatStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, regularTriangleTiltedStretchPatch) { - EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D))); EXPECT_FALSE(meetsStretchPatch>(regularTriangleTilted, tiltedStretch3D)); } @@ -533,22 +427,6 @@ TEST(RotationMethods, irregularQuad2DStretchPatch) EXPECT_TRUE((meetsStretchPatch>(irregularQuad2D, offAxisStretch2D))); } -TEST(RotationMethods, irregularQuadFlatStretchPatch) -{ - EXPECT_FALSE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularQuadFlat, flatStretch3D))); -} - -TEST(RotationMethods, irregularQuadTiltedStretchPatch) -{ - EXPECT_FALSE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularQuadTilted, tiltedStretch3D))); -} - TEST(RotationMethods, irregularTriangle2DStretchPatch) { EXPECT_FALSE((meetsStretchPatch>(irregularTriangle2D, offAxisStretch2D))); @@ -559,19 +437,11 @@ TEST(RotationMethods, irregularTriangle2DStretchPatch) TEST(RotationMethods, irregularTriangleFlatStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularTriangleFlat, flatStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleFlat, flatStretch3D)); } TEST(RotationMethods, irregularTriangleTiltedStretchPatch) { - EXPECT_FALSE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); - EXPECT_FALSE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); - EXPECT_TRUE((meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D))); EXPECT_FALSE(meetsStretchPatch>(irregularTriangleTilted, tiltedStretch3D)); } @@ -593,22 +463,6 @@ TEST(RotationMethods, irregularHexahedronStretchPatch) EXPECT_FALSE(meetsStretchPatch>(irregularHexahedron, offAxisStretch3D)); } -TEST(RotationMethods, edgeFlatStretchPatch) -{ - EXPECT_TRUE((meetsStretchPatch>(edgeFlat, flatEdgeStretch))); - EXPECT_TRUE((meetsStretchPatch>(edgeFlat, flatEdgeStretch))); - EXPECT_TRUE((meetsStretchPatch>(edgeFlat, flatEdgeStretch))); - EXPECT_TRUE((meetsStretchPatch>(edgeFlat, flatEdgeStretch))); -} - -TEST(RotationMethods, edgeTiltedStretchPatch) -{ - EXPECT_TRUE((meetsStretchPatch>(edgeTilted, tiltedEdgeStretch))); - EXPECT_TRUE((meetsStretchPatch>(edgeTilted, tiltedEdgeStretch))); - EXPECT_TRUE((meetsStretchPatch>(edgeTilted, tiltedEdgeStretch))); - EXPECT_TRUE((meetsStretchPatch>(edgeTilted, tiltedEdgeStretch))); -} - TEST(RotationMethods, regularQuad2DRenumbering) { EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); @@ -617,22 +471,6 @@ TEST(RotationMethods, regularQuad2DRenumbering) EXPECT_TRUE((meetsRenumbering>(regularQuad2D, rotatedStretch2D, quadNumbering))); } -TEST(RotationMethods, regularQuadFlatRenumbering) -{ - EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuadFlat, rotatedFlatStretch3D, quadNumbering))); -} - -TEST(RotationMethods, regularQuadTiltedRenumbering) -{ - EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); -} - TEST(RotationMethods, regularTriangle2DRenumbering) { EXPECT_TRUE((meetsRenumbering>(regularTriangle2D, rotatedStretch2D, triangleNumbering))); @@ -643,19 +481,11 @@ TEST(RotationMethods, regularTriangle2DRenumbering) TEST(RotationMethods, regularTriangleFlatRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(regularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); } TEST(RotationMethods, regularTriangleTiltedRenumbering) { - EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(regularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } @@ -685,22 +515,6 @@ TEST(RotationMethods, irregularQuad2DRenumbering) EXPECT_TRUE((meetsRenumbering>(irregularQuad2D, rotatedStretch2D, quadNumbering))); } -TEST(RotationMethods, irregularQuadFlatRenumbering) -{ - EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuadFlat, rotatedFlatStretch3D, quadNumbering))); -} - -TEST(RotationMethods, irregularQuadTiltedRenumbering) -{ - EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularQuadTilted, rotatedTiltedStretch3D, quadNumbering))); -} - TEST(RotationMethods, irregularTriangle2DRenumbering) { EXPECT_TRUE((meetsRenumbering>(irregularTriangle2D, rotatedStretch2D, triangleNumbering))); @@ -711,19 +525,11 @@ TEST(RotationMethods, irregularTriangle2DRenumbering) TEST(RotationMethods, irregularTriangleFlatRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularTriangleFlat, rotatedFlatStretch3D, triangleNumbering)); } TEST(RotationMethods, irregularTriangleTiltedRenumbering) { - EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); - EXPECT_TRUE((meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering))); EXPECT_FALSE(meetsRenumbering>(irregularTriangleTilted, rotatedTiltedStretch3D, triangleNumbering)); } @@ -745,20 +551,4 @@ TEST(RotationMethods, irregularHexahedronRenumbering) EXPECT_FALSE(meetsRenumbering>(irregularHexahedron, rotatedStretch3D, hexahedronNumbering)); } -TEST(RotationMethods, edgeFlatRenumbering) -{ - EXPECT_TRUE((meetsRenumbering>(edgeFlat, rotatedFlatEdgeStretch, edgeNumbering))); - EXPECT_TRUE((meetsRenumbering>(edgeFlat, rotatedFlatEdgeStretch, edgeNumbering))); - EXPECT_TRUE((meetsRenumbering>(edgeFlat, rotatedFlatEdgeStretch, edgeNumbering))); - EXPECT_TRUE((meetsRenumbering>(edgeFlat, rotatedFlatEdgeStretch, edgeNumbering))); -} - -TEST(RotationMethods, edgeTiltedRenumbering) -{ - EXPECT_TRUE((meetsRenumbering>(edgeTilted, rotatedTiltedEdgeStretch, edgeNumbering))); - EXPECT_TRUE((meetsRenumbering>(edgeTilted, rotatedTiltedEdgeStretch, edgeNumbering))); - EXPECT_TRUE((meetsRenumbering>(edgeTilted, rotatedTiltedEdgeStretch, edgeNumbering))); - EXPECT_TRUE((meetsRenumbering>(edgeTilted, rotatedTiltedEdgeStretch, edgeNumbering))); -} - } diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h index 6526b2e1076..32db3108afc 100644 --- a/Sofa/framework/Type/src/sofa/type/Mat.h +++ b/Sofa/framework/Type/src/sofa/type/Mat.h @@ -1043,25 +1043,6 @@ constexpr real determinant(const Mat<2,2,real>& m) noexcept - m(1,0)*m(0,1); } -/// Matrix of cofactors, such that cofactor(A) * cross(u, v) == cross(A * u, A * v). -template -constexpr Mat<3,3,real> cofactor(const Mat<3,3,real>& A) noexcept -{ - Mat<3,3,real> C(NOINIT); - C[0] = cross(A[1], A[2]); - C[1] = cross(A[2], A[0]); - C[2] = cross(A[0], A[1]); - return C; -} - -/// Matrix of cofactors, such that cofactor(A) * perp(u) == perp(A * u), with perp(u) the vector u -/// turned by 90 degrees. -template -constexpr Mat<2,2,real> cofactor(const Mat<2,2,real>& A) noexcept -{ - return {{A(1,1), -A(1,0)}, {-A(0,1), A(0,0)}}; -} - /// Determinant of a 1x1 matrix. template constexpr real determinant(const Mat<1,1,real>& m) noexcept