Fix capsule moment of inertia

include/hpp/fcl/shape/geometric_shapes.h

@@ -178,7 +178,9 @@ public:
     FCL_REAL v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi<FCL_REAL>();
     FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
     
-    FCL_REAL ix = v_cyl * halfLength * halfLength / 3. + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + v_sph * halfLength * halfLength;
+    FCL_REAL h2 = halfLength * halfLength;
+    FCL_REAL r2 = radius * radius;
+    FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
     FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
 
     return (Matrix3f() << ix, 0, 0,

test/python_unit/geometric_shapes.py

@@ -30,15 +30,16 @@ class TestGeometricShapes(TestCase):
         Iz_sphere = 0.4 * V_sphere * capsule.radius * capsule.radius
         Iz_ref = Iz_cylinder + Iz_sphere
         Ix_cylinder = V_cylinder*(3 * capsule.radius**2 + 4 * capsule.halfLength**2)/12.
-# TODO: check this code
-#        Ix_sphere = Iz_sphere + V_sphere*capsule.halfLength**2
-#        Ix_ref = Ix_cylinder + Ix_sphere
-#        print(Ix_cylinder)
-#        print(Ix_sphere) 
-#        I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref])
-#        self.assertApprox(I0.diagonal(), I0_ref.diagonal())
-#        Ic = capsule.computeMomentofInertiaRelatedToCOM()
-#        self.assertApprox(Ic, I0_ref)
+        V_hemi = 0.5 * V_sphere                                          # volume of hemisphere
+        I0x_hemi = 0.5 * Iz_sphere                                       # inertia of hemisphere w.r.t. origin
+        com_hemi = 3. * capsule.radius / 8.                              # CoM of hemisphere w.r.t. origin
+        Icx_hemi = I0x_hemi - V_hemi * com_hemi * com_hemi               # inertia of hemisphere w.r.t. CoM
+        Ix_hemi = Icx_hemi + V_hemi * (capsule.halfLength + com_hemi)**2 # inertia of hemisphere w.r.t. tip of cylinder
+        Ix_ref = Ix_cylinder + 2*Ix_hemi                                 # total inertia of capsule
+        I0_ref = np.diag([Ix_ref,Ix_ref,Iz_ref])
+        self.assertApprox(I0, I0_ref)
+        Ic = capsule.computeMomentofInertiaRelatedToCOM()
+        self.assertApprox(Ic, I0_ref)
 
     def test_box1(self):
         box = hppfcl.Box(np.matrix([1.,2.,3.]).T)