refactoring

VirtualRobot/SceneObject.cpp

@@ -988,13 +988,18 @@ namespace VirtualRobot
         return physics.inertiaMatrix;
     }
 
-    Eigen::Matrix3f SceneObject::getInertiaMatrix(const Eigen::Vector3f& shift)
+    Eigen::Matrix3f SceneObject::shiftInertia(const Eigen::Matrix3f inertiaMatrix, const Eigen::Vector3f &shift, float mass)
     {
         Eigen::Matrix3f skew;
         skew << 0, -shift(2), +shift(1),
              +shift(2), 0, -shift(0),
              -shift(1), +shift(0), 0;
-        return getInertiaMatrix() + getMass() * skew.transpose() * skew;
+        return inertiaMatrix - mass * skew.transpose() * skew;
+    }
+
+    Eigen::Matrix3f SceneObject::getInertiaMatrix(const Eigen::Vector3f& shift)
+    {
+        return shiftInertia(getInertiaMatrix(), shift, getMass());
     }
 
     Eigen::Matrix3f SceneObject::getInertiaMatrix(const Eigen::Vector3f& shift, const Eigen::Matrix3f& rotation)
@@ -1079,6 +1084,8 @@ namespace VirtualRobot
         physics.inertiaMatrix = im;
     }
 
+
+
     SceneObject::Physics SceneObject::getPhysics()
     {
         return physics;

VirtualRobot/SceneObject.h

@@ -296,6 +296,15 @@ namespace VirtualRobot
 
         void setInertiaMatrix(const Eigen::Matrix3f& im);
 
+        /**
+         * @brief The shift is done using the parallel axis theorem (https://en.wikipedia.org/wiki/Parallel_axis_theorem)
+         * @param inertiaMatrix
+         * @param shift
+         * @param mass
+         * @return
+         */
+        static Eigen::Matrix3f shiftInertia(const Eigen::Matrix3f inertiaMatrix, const Eigen::Vector3f& shift, float mass);
+
         float getFriction();
         void setFriction(float friction);