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 4f610a84d83..f7b91c7e180 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 @@ -171,6 +171,9 @@ class CorotationalFEMForceField : StressVoigtVector computeStress(const DeformationGradient& F, sofa::Size elementId) override; + sofa::Data d_consistentTangent; + sofa::Data d_symmetrizeTangent; + protected: void beforeElementForce(const sofa::core::MechanicalParams* mparams, @@ -192,6 +195,10 @@ class CorotationalFEMForceField : sofa::type::vector m_rotations; sofa::type::vector m_initialRotationsTransposed; + /// Element stiffness plus the stiffness due to the element rotation, in the element frame, + /// accumulated in computeElementsForces. Empty if consistentTangent is off. + sofa::type::vector m_tangents; + /// Local displacement of the element nodes in the element's own rotated frame. Flat vector. ElementDisplacement computeElementLocalDisplacement( const std::array, trait::NumberOfNodesInElement>& nodes, diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl index fc5fc9fd809..0b2d6a4041d 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.inl @@ -32,7 +32,9 @@ namespace sofa::component::solidmechanics::fem::elastic template CorotationalFEMForceField::CorotationalFEMForceField() - : m_rotationMethods(this) + : d_consistentTangent(initData(&d_consistentTangent, true, "consistentTangent", "Include the change of the element rotations in the force derivatives")) + , d_symmetrizeTangent(initData(&d_symmetrizeTangent, true, "symmetrizeTangent", "Use the symmetric part of the consistent tangent")) + , m_rotationMethods(this) { this->addUpdateCallback("selectRotationMethod", {&this->m_rotationMethods.d_rotationMethod}, [this](const sofa::core::DataTracker&) @@ -74,6 +76,7 @@ void CorotationalFEMForceField::beforeElementForce( SOFA_UNUSED(x); const auto& elements = trait::FiniteElement::getElementSequence(*this->l_topology); m_rotations.resize(elements.size(), RotationMatrix::Identity()); + m_tangents.resize(d_consistentTangent.getValue() ? elements.size() : 0); } template @@ -129,6 +132,48 @@ void CorotationalFEMForceField::computeElementsForces( auto& elementForce = elementForces[elementId]; elementForce = stiffnessMatrix * displacement; + if (!m_tangents.empty()) + { + typename RotationMethods::RotationJacobian jacobian; + if (!m_rotationMethods.computeRotationJacobian(jacobian, elementRotation, elementNodesCoordinates, restElementNodesCoordinates)) + { + msg_warning_once() << "The rotation jacobian of an element could not be computed. Its tangent ignores the change of rotation."; + } + + const auto t0 = translation(restElementNodesCoordinates); + std::array()).transposed()), trait::NumberOfNodesInElement> localPositions; + sofa::Coord_t nodeValue { sofa::type::NOINIT }; + for (sofa::Size c = 0; c < trait::NumberOfNodesInElement; ++c) + { + displacement.getsub(c * DIM, nodeValue); + localPositions[c] = sofa::type::crossProductMatrix(nodeValue + restElementNodesCoordinates[c] - t0).transposed(); + } + + auto& tangent = m_tangents[elementId]; + tangent = stiffnessMatrix; + RotationMatrix block { sofa::type::NOINIT }; + for (sofa::Size a = 0; a < trait::NumberOfNodesInElement; ++a) + { + elementForce.getsub(a * DIM, nodeValue); + auto W = sofa::type::crossProductMatrix(nodeValue).transposed(); + for (sofa::Size c = 0; c < trait::NumberOfNodesInElement; ++c) + { + stiffnessMatrix.getsub(a * DIM, c * DIM, block); + W -= block * localPositions[c]; + } + for (sofa::Size b = 0; b < trait::NumberOfNodesInElement; ++b) + { + tangent.getsub(a * DIM, b * DIM, block); + tangent.setsub(a * DIM, b * DIM, block + W * jacobian[b]); + } + } + + if (d_symmetrizeTangent.getValue()) + { + tangent = (tangent + tangent.transposed()) / 2; + } + } + sofa::type::Vec> nodeForce { sofa::type::NOINIT }; for (sofa::Size n = 0; n < trait::NumberOfNodesInElement; ++n) { @@ -149,6 +194,7 @@ void CorotationalFEMForceField::computeElementsForcesDer SOFA_UNUSED(mparams); const auto& elements = trait::FiniteElement::getElementSequence(*this->l_topology); auto elementStiffness = sofa::helper::getReadAccessor(this->d_elementStiffness); + const auto& elementTangentStiffness = m_tangents.empty() ? elementStiffness.ref() : m_tangents; for (std::size_t elementId = range.start; elementId < range.end; ++elementId) { @@ -163,7 +209,7 @@ void CorotationalFEMForceField::computeElementsForcesDer element_dx.setsub(n * trait::spatial_dimensions, elementRotation.multTranspose(nodeDx[element[n]])); } - const auto& stiffnessMatrix = elementStiffness[elementId]; + const auto& stiffnessMatrix = elementTangentStiffness[elementId]; auto& df = elementForcesDeriv[elementId]; df = stiffnessMatrix * element_dx; @@ -194,7 +240,8 @@ void CorotationalFEMForceField::buildStiffnessMatrix( return; } - auto elementStiffnessIt = elementStiffness.begin(); + const auto& elementTangentStiffness = m_tangents.empty() ? elementStiffness.ref() : m_tangents; + auto elementStiffnessIt = elementTangentStiffness.begin(); auto rotationMatrixIt = m_rotations.begin(); for (const auto& element : elements) { diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/HexahedronRotation.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/HexahedronRotation.h index 996ba550949..134890a6ee5 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/HexahedronRotation.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/HexahedronRotation.h @@ -64,6 +64,45 @@ struct HexahedronRotation rotationMatrix = currentRotation.multTranspose(initialRotationMatrix); } + template + bool computeRotationJacobian(std::array& jacobian, + const RotationMatrix& rotationMatrix, + const std::array, NumberOfNodesInElement>& nodesPosition, + const std::array, NumberOfNodesInElement>& nodesRestPosition) + { + using Real = sofa::Real_t; + SOFA_UNUSED(nodesRestPosition); + + RotationMatrix currentFrame(sofa::type::NOINIT); + computeOrientationFromHexahedron(currentFrame, nodesPosition); + const RotationMatrix restFrame = currentFrame * rotationMatrix; + + const auto a = (nodesPosition[1] - nodesPosition[0] + + nodesPosition[2] - nodesPosition[3] + + nodesPosition[5] - nodesPosition[4] + + nodesPosition[6] - nodesPosition[7]) * 0.25; + const auto b = (nodesPosition[3] - nodesPosition[0] + + nodesPosition[2] - nodesPosition[1] + + nodesPosition[7] - nodesPosition[4] + + nodesPosition[6] - nodesPosition[5]) * 0.25; + const auto lengthA = a.norm(); + const auto b1 = b * currentFrame[0]; + const auto b2 = b * currentFrame[1]; + + const RotationMatrix Ga {{0, 0, -b1 / (lengthA * b2)}, {0, 0, -1 / lengthA}, {0, 1 / lengthA, 0}}; + const RotationMatrix Gb {{0, 0, 1 / b2}, {0, 0, 0}, {0, 0, 0}}; + + constexpr std::array alpha {-0.25, 0.25, 0.25, -0.25, -0.25, 0.25, 0.25, -0.25}; + constexpr std::array beta {-0.25, -0.25, 0.25, 0.25, -0.25, -0.25, 0.25, 0.25}; + + jacobian = {}; + for (std::size_t k = 0; k < 8; ++k) + { + jacobian[k] = restFrame.multTranspose((Ga * alpha[k] + Gb * beta[k]) * restFrame); + } + return true; + } + static constexpr sofa::helper::Item getItem() { return {"hexahedron", "Compute the rotation based on two average edges in the hexahedron"}; 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 f9b413d911a..14d0560affd 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 @@ -41,6 +41,7 @@ template >; + using StretchMatrix = RotationMatrix; /// The remainder of the dimensions of an embedded element within a higher-dimensional space. // TODO: embedded elements (Codimension > 0) are not handled. @@ -108,6 +109,81 @@ struct PolarDecomposition } } + /// Computes the jacobian of the rotation matrix + template + bool computeRotationJacobian(std::array& jacobian, + const RotationMatrix& rotationMatrix, + const std::array, NumberOfNodesInElement>& nodesPosition, + const std::array, NumberOfNodesInElement>& nodesRestPosition) + { + if constexpr (DataTypes::spatial_dimensions == 1) + { + // In 1D, there is no rotation. + return true; + } + else if constexpr (Codimension > 0) + { + // TODO: embedded elements. + jacobian = {}; + return false; + } + else + { + 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; + } + + const auto M = Q.multTranspose(Q); + const auto traceM = sofa::type::trace(M); + + const auto w = [&Q, &M, traceM]() + { + if constexpr (FitType == Fit::Affine) + { + return Q * (M / traceM).inverted() / traceM; + } + else + { + return Q / traceM; + } + }(); + + const auto F = P.multTranspose(w); + const StretchMatrix S = rotationMatrix.multTranspose(F); + + sofa::type::Mat> A; + if constexpr (DataTypes::spatial_dimensions == 3) + { + A = StretchMatrix::Identity() * sofa::type::trace(S) - S; + } + else + { + A(0, 0) = sofa::type::trace(S); + } + + decltype(A) inverseA(sofa::type::NOINIT); + if (!inverseA.invert(A)) + { + jacobian = {}; + return false; + } + + for (sofa::Size k = 0; k < NumberOfNodesInElement; ++k) + { + jacobian[k] = inverseA * sofa::type::crossProductMatrix(w[k]); + } + return true; + } + } + static constexpr sofa::helper::Item getItem() { if constexpr (AlgorithmType == Algorithm::Standard) diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/RotationMethodsContainer.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/RotationMethodsContainer.h index f4f84998014..322a46c3d8b 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/RotationMethodsContainer.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/RotationMethodsContainer.h @@ -71,6 +71,7 @@ struct RotationMethodsContainer using RotationMatrix = sofa::type::Mat>; static constexpr std::size_t NumberOfNodesInElement = ElementType::NumberOfNodes; + using RotationJacobian = std::array())), NumberOfNodesInElement>; /** * Computes the current rotation state relative to the initial configuration. @@ -92,6 +93,35 @@ struct RotationMethodsContainer m_rotationComputer); } + /** + * Computes the jacobian of the rotation matrix. Zero for methods that do not provide one. + * + * @param jacobian Output: Derivative of the rotation with respect to each node position, in the element frame + * @param rotationMatrix Current rotation matrix, as computed by computeRotation + * @param nodesPosition Current positions of element nodes + * @param nodesRestPosition Rest positions of element nodes (initial configuration) + * @return False if the method failed to compute the jacobian + */ + bool computeRotationJacobian(RotationJacobian& jacobian, const RotationMatrix& rotationMatrix, + const std::array, NumberOfNodesInElement>& nodesPosition, + const std::array, NumberOfNodesInElement>& nodesRestPosition) + { + return std::visit( + [&](auto& rotationComputer) + { + if constexpr (requires { rotationComputer.computeRotationJacobian(jacobian, rotationMatrix, nodesPosition, nodesRestPosition); }) + { + return rotationComputer.computeRotationJacobian(jacobian, rotationMatrix, nodesPosition, nodesRestPosition); + } + else + { + jacobian = {}; + return true; + } + }, + m_rotationComputer); + } + struct RotationMethodsItems final : sofa::helper::SelectableItem { using sofa::helper::SelectableItem::SelectableItem; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/TriangleRotation.h b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/TriangleRotation.h index 4247b921dba..c86cbe2e4f6 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/TriangleRotation.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/TriangleRotation.h @@ -45,6 +45,34 @@ struct TriangleRotation rotationMatrix = currentRotation.multTranspose(initialRotationMatrix); } + template + bool computeRotationJacobian(std::array& jacobian, + const RotationMatrix& rotationMatrix, + const std::array, NumberOfNodesInElement>& nodesPosition, + const std::array, NumberOfNodesInElement>& nodesRestPosition) + { + SOFA_UNUSED(nodesRestPosition); + + RotationMatrix currentFrame(sofa::type::NOINIT); + computeRotationFrom3Points(currentFrame, {nodesPosition[0], nodesPosition[1], nodesPosition[2]}); + const RotationMatrix restFrame = currentFrame * rotationMatrix; + + const auto a = nodesPosition[1] - nodesPosition[0]; + const auto b = nodesPosition[2] - nodesPosition[0]; + const auto lengthA = a.norm(); + const auto b1 = b * currentFrame[0]; + const auto b2 = b * currentFrame[1]; + + const RotationMatrix Ga {{0, 0, -b1 / (lengthA * b2)}, {0, 0, -1 / lengthA}, {0, 1 / lengthA, 0}}; + const RotationMatrix Gb {{0, 0, 1 / b2}, {0, 0, 0}, {0, 0, 0}}; + + jacobian = {}; + jacobian[0] = restFrame.multTranspose((-Ga - Gb) * restFrame); + jacobian[1] = restFrame.multTranspose(Ga * restFrame); + jacobian[2] = restFrame.multTranspose(Gb * restFrame); + return true; + } + static constexpr sofa::helper::Item getItem() { return {"triangle", "Compute the rotation based on the Gram-Schmidt frame alignment"}; diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt index c6d6e9e1b00..7f4149bb8c4 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt @@ -5,6 +5,7 @@ project(Sofa.Component.SolidMechanics.FEM.Elastic_test) set(SOURCE_FILES BaseTetrahedronFEMForceField_test.h BeamFEMForceField_test.cpp + CorotationalFEMForceField_test.cpp FastTetrahedralCorotationalForceField_test.cpp FEMSourceTermIntegrator_test.cpp HexahedralFEMForceField_test.cpp diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CorotationalFEMForceField_test.cpp b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CorotationalFEMForceField_test.cpp new file mode 100644 index 00000000000..14269e6098b --- /dev/null +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/CorotationalFEMForceField_test.cpp @@ -0,0 +1,272 @@ +/****************************************************************************** +* 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 +#include + +namespace sofa +{ + +using namespace sofa::component::solidmechanics::fem::elastic; + +namespace +{ + +using sofa::defaulttype::Vec2Types; +using sofa::defaulttype::Vec3Types; + +template +using Matrix = sofa::type::Mat; + +constexpr SReal degreeToRadian = std::numbers::pi_v / 180; + +/// Helpers for computing rotation matrices in 2D and 3D +Matrix rotationMatrix2D(SReal angle) +{ + return {{std::cos(angle), -std::sin(angle)}, {std::sin(angle), std::cos(angle)}}; +} + +Matrix rotationMatrix3D(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)); +} + +// The matrix for the applied rotation in 2D and 3D +const Matrix prescribedRotationMatrix2D = rotationMatrix2D(30 * degreeToRadian); +const Matrix prescribedRotationMatrix3D = rotationMatrix3D(sofa::type::Vec3(1.0, 2.0, 3.0), 30 * degreeToRadian); + +// Stretches by 50% along the axes +// (0.866, 0.5) in 2D and +// (0.876, 0.420, -0.239) in 3D +// This axis was chosen so that it does not align with any edge of the test elements. If placed +// along an edge, the "triangle" and "hexahedron" rotation methods can extract the exact rotation +// by coincidence. +const Matrix prescribedStretchMatrix2D = + prescribedRotationMatrix2D * Matrix{{1.5, 0.0}, {0.0, 1.0}} * prescribedRotationMatrix2D.transposed(); +const Matrix prescribedStretchMatrix3D = + prescribedRotationMatrix3D * Matrix{{1.5, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}} * prescribedRotationMatrix3D.transposed(); + +/// prescribedStretchMatrix2D within the plane z = 0, for the triangles in 3D. +const Matrix prescribedFlatStretchMatrix3D { + {prescribedStretchMatrix2D(0, 0), prescribedStretchMatrix2D(0, 1), 0.0}, + {prescribedStretchMatrix2D(1, 0), prescribedStretchMatrix2D(1, 1), 0.0}, + {0.0, 0.0, 1.0}}; + +struct PolarMethod { static constexpr const char* key = "polar"; }; +struct PolarAffineMethod { static constexpr const char* key = "polar_affine"; }; +struct StablePolarMethod { static constexpr const char* key = "stable_polar"; }; +struct StablePolarAffineMethod { static constexpr const char* key = "stable_polar_affine"; }; +struct TriangleMethod { static constexpr const char* key = "triangle"; }; +struct HexahedronMethod { static constexpr const char* key = "hexahedron"; }; + +using Quad2D = CorotationalFEMForceField; +using Triangle2D = CorotationalFEMForceField; +using Triangle3D = CorotationalFEMForceField; +using Hexahedron3D = CorotationalFEMForceField; +using Tetrahedron3D = CorotationalFEMForceField; + +template +struct TestCase +{ + using ForceField = ForceFieldType; + using Method = MethodType; +}; + +} + +/// Checks the force derivatives against a finite difference of the force, on a single element. +template +struct CorotationalFEMForceFieldTangent_test : public sofa::ForceField_test +{ + using ForceField = typename TestCase::ForceField; + using DataTypes = typename ForceField::DataTypes; + using VecCoord = sofa::VecCoord_t; + using VecDeriv = sofa::VecDeriv_t; + using Coord = sofa::Coord_t; + + /// Rest positions of the element. + VecCoord restPositions; + + Matrix rotation; + Matrix stretch; + + CorotationalFEMForceFieldTangent_test() + { + const auto topology = sofa::core::objectmodel::New(); + this->node->addObject(topology); + + /// An irregular shape is chosen for all elements that follow. This skews the rigid fit of + /// the polar decomposition methods. Rigid and affine fits extract different rotations and + /// the tangent of each must follow how its own rotation changes. + if constexpr (std::is_same_v) + { + topology->addQuad(0, 1, 2, 3); + restPositions = VecCoord{Coord(0.0, 0.0), Coord(3.0, 0.0), Coord(3.0, 1.0), Coord(0.0, 1.0)}; + rotation = prescribedRotationMatrix2D; + stretch = prescribedStretchMatrix2D; + } + else if constexpr (std::is_same_v) + { + topology->addTriangle(0, 1, 2); + restPositions = VecCoord{Coord(0.0, 0.0), Coord(1.0, 0.0), Coord(0.0, 1.0)}; + rotation = prescribedRotationMatrix2D; + stretch = prescribedStretchMatrix2D; + } + else if constexpr (std::is_same_v) + { + topology->addTriangle(0, 1, 2); + restPositions = VecCoord{Coord(0.0, 0.0, 0.0), Coord(1.0, 0.0, 0.0), Coord(0.0, 1.0, 0.0)}; + rotation = prescribedRotationMatrix3D; + stretch = prescribedFlatStretchMatrix3D; + } + else if constexpr (std::is_same_v) + { + topology->addHexa(0, 1, 2, 3, 4, 5, 6, 7); + restPositions = VecCoord{Coord(0.0, 0.0, 0.0), Coord(3.0, 0.0, 0.0), Coord(3.0, 2.0, 0.0), Coord(0.0, 2.0, 0.0), + Coord(0.0, 0.0, 1.0), Coord(3.0, 0.0, 1.0), Coord(3.0, 2.0, 1.0), Coord(0.0, 2.0, 1.0)}; + rotation = prescribedRotationMatrix3D; + stretch = prescribedStretchMatrix3D; + } + else if constexpr (std::is_same_v) + { + topology->addTetra(0, 1, 2, 3); + restPositions = VecCoord{Coord(0.0, 0.0, 0.0), Coord(1.0, 0.0, 0.0), Coord(0.0, 1.0, 0.0), Coord(0.0, 0.0, 1.0)}; + rotation = prescribedRotationMatrix3D; + stretch = prescribedStretchMatrix3D; + } + else + { + static_assert(sizeof(ForceField) == 0, "No element defined for this force field"); + } + + this->dof->writeOnlyRestPositions().wref() = restPositions; + + this->force->d_youngModulus.setValue({1}); + this->force->d_poissonRatio.setValue({0}); + this->force->d_symmetrizeTangent.setValue(false); + + this->deltaRange = {5e5, 5e6}; + this->flags &= ~sofa::ForceField_test::TEST_ADD_K_TO_MATRIX; + } + + /// The forces of the force field at the positions x. + VecDeriv computeForcesAt(const VecCoord& x, const VecDeriv& v) + { + this->setupState(x, v); + sofa::simulation::node::initRoot(this->node.get()); + core::MechanicalParams mparams; + this->computeForce(&mparams); + VecDeriv forces; + sofa::testing::copyFromData(forces, this->dof->readForces()); + return forces; + } + + /// Moves the nodes by the deformation gradient F = R U and runs the ForceField test there. + /// The consistency of the tangent stiffness is checked therein. + void runTest(const Matrix& R, const Matrix& U) + { + // The element at rest, stretched by U, then rotated by R. + VecCoord stretchedPositions(restPositions.size()); + VecCoord x(restPositions.size()); + VecDeriv v(restPositions.size()); + for (std::size_t j = 0; j < restPositions.size(); ++j) + { + stretchedPositions[j] = U * restPositions[j]; + x[j] = R * stretchedPositions[j]; + } + + // The expected forces: the linear elastic forces of the stretch, computed by the identity + // method, rotated by R. + this->force->findData("rotationMethod")->read("identity"); + VecDeriv expectedForces = computeForcesAt(stretchedPositions, v); + for (auto& force : expectedForces) + { + force = R * force; + } + + using Method = typename TestCase::Method; + this->force->findData("rotationMethod")->read(Method::key); + + // For irregular elements, only the polar decomposition of the affine fit extracts the + // rotation that produces the correct forces. + if constexpr (std::is_same_v || std::is_same_v) + { + // Therefore the expected force is used as reference. + this->run_test(x, v, expectedForces); + } + else + { + // The rest of the methods produce incorrect forces on irregular shapes. So the check is + // implicitly bypassed by using the force they compute themselves as reference. These + // methods are also expected to miscompute the potential energy on irregular shapes. + this->flags &= ~sofa::ForceField_test::TEST_POTENTIAL_ENERGY; + this->run_test(x, v, computeForcesAt(x, v)); + } + } +}; + +using TestTypes = ::testing::Types< + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase, + TestCase>; + +TYPED_TEST_SUITE(CorotationalFEMForceFieldTangent_test, TestTypes); + +TYPED_TEST(CorotationalFEMForceFieldTangent_test, rotated) +{ + using Matrix = Matrix; + this->runTest(this->rotation, Matrix::Identity()); +} + +TYPED_TEST(CorotationalFEMForceFieldTangent_test, stretched) +{ + using Matrix = Matrix; + this->runTest(Matrix::Identity(), this->stretch); +} + +TYPED_TEST(CorotationalFEMForceFieldTangent_test, stretchedRotated) +{ + this->runTest(this->rotation, this->stretch); +} + +} diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h index 32db3108afc..897eb090654 100644 --- a/Sofa/framework/Type/src/sofa/type/Mat.h +++ b/Sofa/framework/Type/src/sofa/type/Mat.h @@ -1463,6 +1463,13 @@ constexpr Mat<3, 3, Real> crossProductMatrix(const Vec<3, Real>& v) noexcept return res; } +/// crossProductMatrix(v) * x = v.cross(x), a scalar in 2D +template +constexpr Mat<1, 2, Real> crossProductMatrix(const Vec<2, Real>& v) noexcept +{ + return {{-v[1], v[0]}}; +} + /// return a * b^T template