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() 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()); +} + +}