[Data] Add explicit data.Dinv quantity for cholesky computations

src/algorithm/cholesky.hxx

 //
-// Copyright (c) 2015-2017 CNRS
+// Copyright (c) 2015-2018 CNRS
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -52,6 +52,7 @@ namespace se3
       Eigen::MatrixXd & M = data.M;
       Eigen::MatrixXd & U = data.U;
       Eigen::VectorXd & D = data.D;
+      Eigen::VectorXd & Dinv = data.Dinv;
       
       for(int j=model.nv-1;j>=0;--j )
       {
@@ -61,10 +62,10 @@ namespace se3
           DUt = U.row(j).segment(j+1,NVT).transpose()
           .cwiseProduct(D.segment(j+1,NVT));
         
-        D[j] = M(j,j) - U.row(j).segment(j+1,NVT) * DUt;
-        const double dj_inv(1./D[j]);
+        D[j] = M(j,j) - U.row(j).segment(j+1,NVT).dot(DUt);
+        Dinv[j] = 1./D[j];
         for(int _i = data.parents_fromRow[(Model::Index)j];_i >= 0;_i = data.parents_fromRow[(Model::Index)_i])
-          U(_i,j) = (M(_i,j) - U.row(_i).segment(j+1,NVT).dot(DUt)) * dj_inv;
+          U(_i,j) = (M(_i,j) - U.row(_i).segment(j+1,NVT).dot(DUt)) * Dinv[j];
       }
       
       return data.U;
@@ -181,8 +182,8 @@ namespace se3
                   Eigen::MatrixBase<Mat> & v)
       {
         Utv(model,data,v);
-        for( int k=0;k<model.nv;++k ) v.row(k) *= data.D[k];
         return Uv(model,data,v);
+        v_.array() *= data.D.array();
       }
     } // internal
     
@@ -208,8 +209,8 @@ namespace se3
       assert(model.check(data) && "data is not consistent with model.");
 #endif
       Uiv(model,data,v);
-      for(int k=0;k<model.nv;++k) v.row(k) /= data.D[k];
       return Utiv(model,data,v);
+      v_.array() *= data.Dinv.array();
     }
 
   } //   namespace cholesky

src/multibody/model.hpp

@@ -498,6 +498,9 @@ namespace se3
     /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
     Eigen::VectorXd D;
     
+    /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
+    Eigen::VectorXd Dinv;
+    
     /// \brief Temporary of size NV used in Cholesky Decomposition.
     Eigen::VectorXd tmp;
     

src/multibody/model.hxx

@@ -270,6 +270,7 @@ namespace se3
     ,nvSubtree((std::size_t)model.njoints)
     ,U(model.nv,model.nv)
     ,D(model.nv)
+    ,Dinv(model.nv)
     ,tmp(model.nv)
     ,parents_fromRow((std::size_t)model.nv)
     ,nvSubtree_fromRow((std::size_t)model.nv)