[Defaulttype] RigidTypes: Fix various bugs in RigidCoord/RigidMass and add unit tests (#5988)

* fix potential compilation error (member is not callable...)

* fix out of bounds access for rigid2

* add extensive unit tests for rigidtypes (coord, deriv..)

* fix rigidcoord2 neg operator (would be in-compilable, orientation is a scalar)

* add stream tests for rigidmass, exposing bug

* fix rigidmass stream bug

* add rigid coord tests on operator+ and +=, exposing bug

* fix bug about inconsistency between operator+ and +=

The fix was changing orientation *= a.getOrientation() (right-multiply) to orientation = a.getOrientation() * orientation (left-multiply), matching the order used in operator+.

* fix volume computation for rigidmass2d and add test

---------

Co-authored-by: erik pernod <erik.pernod@gmail.com>

Author: fredroy

Sofa/framework/DefaultType/src/sofa/defaulttype/RigidCoord.h

@@ -157,7 +157,7 @@ class RigidCoord<3,real>
     constexpr void operator +=(const RigidCoord<3, real>& a)
     {
         center += a.getCenter();
-        orientation *= a.getOrientation();
+        orientation = a.getOrientation() * orientation;
     }
 
     template<typename real2>
@@ -252,7 +252,7 @@ class RigidCoord<3,real>
     {
         RigidCoord r;
         r.center = c.center + center;
-        r.orientation = c.orientation();
+        r.orientation = c.orientation;
         return r;
     }
 
@@ -512,7 +512,7 @@ class RigidCoord<2,real>
 
     constexpr RigidCoord<2, real> operator-() const
     {
-        return RigidCoord<2, real>(-this->center, this->orientation.inverse());
+        return RigidCoord<2, real>(-this->center, -this->orientation);
     }
 
 
@@ -602,7 +602,7 @@ class RigidCoord<2,real>
     {
         RigidCoord r;
         r.center = c.center + center;
-        r.orientation = c.orientation();
+        r.orientation = c.orientation;
         return r;
     }
 
@@ -691,7 +691,7 @@ class RigidCoord<2,real>
         m[11] = 0;
         m[12] = (float)center[0];
         m[13] = (float)center[1];
-        m[14] = (float)center[2];
+        m[14] = 0;
         m[15] = 1;
     }
 

Sofa/framework/DefaultType/src/sofa/defaulttype/RigidMass.h

@@ -98,6 +98,7 @@ class RigidMass<3, real>
         in>>m.mass;
         in>>m.volume;
         in>>m.inertiaMatrix;
+        m.recalc();
         return in;
     }
     constexpr void operator *=(Real fact)
@@ -195,8 +196,8 @@ class RigidMass<2, real>
     RigidMass(Real m, Real xwidth, Real ywidth)
     {
         mass = m;
-        volume = xwidth*xwidth + ywidth*ywidth;
-        inertiaMatrix = volume/12;
+        volume = xwidth * ywidth;
+        inertiaMatrix = (xwidth*xwidth + ywidth*ywidth) / 12;
         recalc();
     }
 
@@ -226,6 +227,7 @@ class RigidMass<2, real>
         in>>m.mass;
         in>>m.volume;
         in>>m.inertiaMatrix;
+        m.recalc();
         return in;
     }
     constexpr void operator *=(Real fact)

Sofa/framework/DefaultType/test/CMakeLists.txt

@@ -10,7 +10,9 @@ set(HEADER_FILES
 set(SOURCE_FILES
     MapMapSparseMatrixEigenUtils_test.cpp
     QuaternionIntegration_test.cpp
+    RigidCoord_test.cpp
     RigidDeriv_test.cpp
+    RigidMass_test.cpp
     TypeInfoRepository_test.cpp
     TypeInfoRepository_tu1_test.cpp
     TypeInfo_test.cpp