[Mapping.Linear] Test IdentityMapping (#5094)

* [Mapping.Linear] Test IdentityMapping * forgot the reference --------- Co-authored-by: erik pernod <[email protected]>
sofa-framework · Nov 11, 2024 · ea9ab57 · ea9ab57
1 parent 679a914
commit ea9ab57
Show file tree

Hide file tree

Showing 3 changed files with 113 additions and 3 deletions.
diff --git a/Sofa/Component/Mapping/Linear/tests/CMakeLists.txt b/Sofa/Component/Mapping/Linear/tests/CMakeLists.txt
@@ -4,6 +4,7 @@ project(Sofa.Component.Mapping.Linear_test)
 
 set(SOURCE_FILES
     BarycentricMapping_test.cpp
+    IdentityMapping_test.cpp
     SubsetMultiMapping_test.cpp
 )
 

diff --git a/Sofa/Component/Mapping/Linear/tests/IdentityMapping_test.cpp b/Sofa/Component/Mapping/Linear/tests/IdentityMapping_test.cpp
@@ -0,0 +1,99 @@
+/******************************************************************************
+*                 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 <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: [email protected]                             *
+******************************************************************************/
+#include <sofa/component/mapping/linear/IdentityMapping.h>
+#include <gtest/gtest.h>
+#include <sofa/component/mapping/testing/MappingTestCreation.h>
+#include <sofa/testing/LinearCongruentialRandomGenerator.h>
+
+
+namespace sofa
+{
+
+using component::mapping::linear::IdentityMapping;
+
+template <typename IdentityMapping>
+struct IdentityMappingTest : public sofa::mapping_test::Mapping_test<IdentityMapping>
+{
+    using In = typename IdentityMapping::In;
+    using Out = typename IdentityMapping::Out;
+
+    void generatePositions(VecCoord_t<In>& inCoord)
+    {
+        sofa::testing::LinearCongruentialRandomGenerator lcg(96547);
+        for (auto& x : inCoord)
+        {
+            for (std::size_t i = 0; i < Coord_t<In>::total_size; ++i)
+            {
+                using Real = Real_t<In>;
+                x[i] = lcg.generateInRange(static_cast<Real>(-1e2), static_cast<Real>(1e2));
+            }
+        }
+    }
+
+    bool test()
+    {
+        // parent positions
+        VecCoord_t<In> inCoord(150);
+        generatePositions(inCoord);
+
+        // expected child positions
+        VecCoord_t<Out> expectedOutCoord;
+        expectedOutCoord.reserve(inCoord.size());
+        for (const auto& x : inCoord)
+        {
+            Coord_t<Out> y;
+            for (std::size_t i = 0; i < Coord_t<Out>::total_size; ++i)
+            {
+                y[i] = x[i];
+            }
+            expectedOutCoord.emplace_back(y);
+        }
+
+        return this->runTest( inCoord, expectedOutCoord );
+    }
+
+};
+
+using ::testing::Types;
+typedef Types<
+    IdentityMapping<sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types>,
+    IdentityMapping<sofa::defaulttype::Vec2Types, sofa::defaulttype::Vec2Types>,
+    IdentityMapping<sofa::defaulttype::Vec1Types, sofa::defaulttype::Vec1Types>,
+    IdentityMapping<sofa::defaulttype::Vec6Types, sofa::defaulttype::Vec3Types>,
+    IdentityMapping<sofa::defaulttype::Vec6Types, sofa::defaulttype::Vec6Types>,
+    IdentityMapping<sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types>,
+    IdentityMapping<sofa::defaulttype::Rigid2Types, sofa::defaulttype::Rigid2Types>,
+    IdentityMapping<sofa::defaulttype::Rigid3Types, sofa::defaulttype::Vec3Types>,
+    IdentityMapping<sofa::defaulttype::Rigid2Types, sofa::defaulttype::Vec2Types>
+> DataTypes;
+
+TYPED_TEST_SUITE( IdentityMappingTest, DataTypes );
+
+// test case
+TYPED_TEST( IdentityMappingTest , test )
+{
+    this->flags |= TestFixture::TEST_applyJT_matrix;
+    this->errorMax = 1e2;
+    ASSERT_TRUE(this->test());
+}
+
+}
diff --git a/Sofa/framework/DefaultType/src/sofa/defaulttype/RigidTypes.h b/Sofa/framework/DefaultType/src/sofa/defaulttype/RigidTypes.h
@@ -153,12 +153,13 @@ class StdRigidTypes<3, real>
     static Deriv coordDifference(const Coord& c1, const Coord& c2)
     {
         type::Vec3 vCenter = c1.getCenter() - c2.getCenter();
-        type::Quat<SReal> quat, quat1(c1.getOrientation()), quat2(c2.getOrientation());
+        const type::Quat<real> quat2(c2.getOrientation());
+        const type::Quat<real> quat1(c1.getOrientation());
         // Transformation between c2 and c1 frames
-        quat = quat1 * quat2.inverse();
+        type::Quat<real> quat = quat1 * quat2.inverse();
         quat.normalize();
         type::Vec3 axis(type::NOINIT);
-        type::Quat<SReal>::value_type angle{};
+        real angle{};
         quat.quatToAxis(axis, angle);
         axis *= angle;
         return Deriv(vCenter, axis);
@@ -354,6 +355,15 @@ class StdRigidTypes<2, real>
         return result;
     }
 
+    static Deriv coordDifference(const Coord& c1, const Coord& c2)
+    {
+        const type::Vec2 vCenter = c1.getCenter() - c2.getCenter();
+        real angle = c1.getOrientation() - c2.getOrientation(); // Difference in orientation (angle between frames)
+        angle = std::fmod(angle + M_PI, 2 * M_PI) - M_PI; // Normalize angle to [-π, π]
+
+        return Deriv(vCenter, angle);
+    }
+
     static Coord interpolate(const type::vector< Coord > & ancestors, const type::vector< Real > & coefs)
     {
         assert(ancestors.size() == coefs.size());