Inertia matrice in Rigids is not taken correctly into account

[bakpaul]

This PR #4935 adds a unit test showing that the inertia matrix in rigids is not working properly. After rotation of the rigid, the inertia matrix is not rotated accordingly.

You can try a 'hand-on' example with the following :

#Required import for python
import Sofa
import SofaRuntime
from Sofa import SofaLinearSolver
import numpy as np

# Function called when the scene graph is being created
def createScene(root):

    root.gravity=[0,0,0]

    root.addObject('VisualStyle', displayFlags="showBehaviorModels showForceFields")

    root.addObject("RequiredPlugin", pluginName=['Sofa.Component.Mass',
                                                    'Sofa.Component.StateContainer',
                                                    'Sofa.Component.Topology.Container.Grid',
                                                    'Sofa.Component.Visual'
                                                    ])

    root.addObject('DefaultAnimationLoop')
    root.addObject('DefaultVisualManagerLoop')

    root.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward')
    root.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct')
    root.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select')
    root.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Projective')
    root.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic')

    root.addObject(RotatorController('RotatorController', name='matrixAccessor', root=root))


    totalMass = 300
    x_side = 0.15
    y_side = 0.15
    z_side = 0.7
    volume = x_side*y_side*z_side
    inertiaMatrix = [(y_side**2 + z_side**2)/12, 0.0, 0.0, 0.0, (x_side**2 + z_side**2)/12, 0.0, 0.0, 0.0, (x_side**2 + y_side**2)/12]

    print([totalMass, volume, inertiaMatrix[:]])

    root.addObject('EulerImplicitSolver', rayleighStiffness="0.0", rayleighMass="0.0")
    root.addObject('SparseLDLSolver', applyPermutation="false", template="CompressedRowSparseMatrixd")


    aligned = root.addChild("Aligned")


    aligned.addObject('MechanicalObject', template='Rigid3', position=[0, 0, 0, 0, 0, 0, 1], showObject=True,
                                showObjectScale=0.1)
    aligned.addObject('UniformMass', name="mass", vertexMass=[totalMass, volume, inertiaMatrix[:]])
    aligned.addObject('ConstantForceField', name='ConstantForceField', forces=[0, 0, 0, 0, 0, 0])

    rotated = root.addChild("Rotated")

    rotated.addObject('MechanicalObject', name="MO", template='Rigid3', position=[1, 0, 0, 0,0,0,1], showObject=True,
                                showObjectScale=0.1)
    rotated.addObject('UniformMass', name="mass", vertexMass=[totalMass, volume, inertiaMatrix[:]])
    rotated.addObject('ConstantForceField',name='ConstantForceField', forces=[0, 0, 0, 0, 0, 0])

    return root


class RotatorController(Sofa.Core.Controller):
    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.root = kwargs["root"]
        self.step = 0

    def onAnimateEndEvent(self, event):
        self.step += 1
        if(self.step == 2):
            self.root.Aligned.ConstantForceField.forces = [[0, 0, 0, 0, 0, 1]]
            self.root.Rotated.ConstantForceField.forces = [[0, 0, 0, 1, 0, 0]]
            self.root.Rotated.MO.position = np.array([[1, 0, 0, 0.707106781,0,0.707106781,0]])