Before moving to boost fusion.

CMakeLists.txt

@@ -75,6 +75,9 @@ PKG_CONFIG_USE_DEPENDENCY(rnea eigenpy)
 ADD_EXECUTABLE(dg EXCLUDE_FROM_ALL unittest/dg.cpp)
 PKG_CONFIG_USE_DEPENDENCY(dg eigenpy)
 
+ADD_EXECUTABLE(fusion EXCLUDE_FROM_ALL unittest/fusion.cpp)
+PKG_CONFIG_USE_DEPENDENCY(fusion eigenpy)
+
 ADD_EXECUTABLE(variant EXCLUDE_FROM_ALL unittest/variant.cpp)
 PKG_CONFIG_USE_DEPENDENCY(variant eigenpy)
 

src/multibody/joint.hpp

@@ -100,8 +100,6 @@ namespace se3
   };
 
 
-
-
   /* --- REVOLUTE X --------------------------------------------------------- */
   /* --- REVOLUTE X --------------------------------------------------------- */
   /* --- REVOLUTE X --------------------------------------------------------- */

unittest/fusion.cpp

@@ -35,7 +35,7 @@ struct CRTPDerived : public CRTPBase<CRTPDerived>
   int g() { std::cout << "g()" << std::endl; return 1; }
   int h(const double & x) { std::cout << "h(" << x << ")" << std::endl; return 2; }
   int hh(const double & x,const int & y, const Eigen::MatrixXd & z,const TestObj & ) 
-  { std::cout << "h(" << x << "," << y << "," << z << ",a)" << std::endl; return 3; }
+  { std::cout << "hh(" << x << "," << y << "," << z << ",a)" << std::endl; return 3; }
 };
 
 struct CRTPDerived2 : public CRTPBase<CRTPDerived2>
@@ -44,7 +44,7 @@ struct CRTPDerived2 : public CRTPBase<CRTPDerived2>
   int g() { std::cout << "g()" << std::endl; return 1; }
   int h(const double & x) { std::cout << "h(" << x << ")" << std::endl; return 2; }
   int hh(const double & x,const int & y, const Eigen::MatrixXd & z,const TestObj & ) 
-  { std::cout << "h(" << x << "," << y << "," << z << ",a)" << std::endl; return 3; }
+  { std::cout << "hh(" << x << "," << y << "," << z << ",a)" << std::endl; return 3; }
 };
 
 // template<typedef Launcher>
@@ -78,32 +78,58 @@ typedef boost::variant<CRTPDerived,CRTPDerived2> CRTPVariant;
 using namespace boost::fusion;
 
 
+// template<typename D>
+// int algo(CRTPBase<D> & crtp, const double & x,const int & y, const Eigen::MatrixXd & z,const TestObj & a) 
+// {
+//   return crtp.hh(x,y,z,a);
+// }
+
+// #define CRTP_VARIANT(ReturnType,function)	\
+// template<typename Args> \
+// struct Launcher : public boost::static_visitor<ReturnType> \
+// { \
+//   Args args; \
+//   Launcher(Args args) : args(args) {} \
+// \
+//   template<typename D> \
+//   ReturnType operator()( CRTPBase<D> & dref )  const	\
+//   { \
+//     return invoke(&function<D>,join(make_vector(boost::ref(dref)),args));	\
+//   } \
+// }; \
+// template<typename Args> \
+// ReturnType function( CRTPVariant & crtp, Args args ) \
+// { \
+//   return boost::apply_visitor( Launcher<Args>(args),crtp );  \
+// }
+
+// CRTP_VARIANT(int,algo)
+
+
 template<typename D>
 int algo(CRTPBase<D> & crtp, const double & x,const int & y, const Eigen::MatrixXd & z,const TestObj & a) 
 {
   return crtp.hh(x,y,z,a);
 }
 
-#define CRTP_VARIANT(ReturnType,function)	\
-template<typename Args> \
-struct Launcher : public boost::static_visitor<ReturnType> \
-{ \
-  Args args; \
-  Launcher(Args args) : args(args) {} \
-\
-  template<typename D> \
-  ReturnType operator()( CRTPBase<D> & dref )  const	\
-  { \
-    return invoke(&function<D>,join(make_vector(boost::ref(dref)),args));	\
-  } \
-}; \
-template<typename Args> \
-ReturnType function( CRTPVariant & crtp, Args args ) \
-{ \
-  return boost::apply_visitor( Launcher<Args>(args),crtp );  \
-}
+struct Launcher : public boost::static_visitor<int>
+{
+  typedef vector<const double &,const int &, const Eigen::MatrixXd &,const TestObj &> Args;
+  Args args;
 
-CRTP_VARIANT(int,algo)
+  Launcher(Args args) : args(args) {}
+
+  template<typename D>
+  int operator() ( CRTPBase<D> & dref )  const
+  {
+    return invoke(&algo<D>,push_front(args,boost::ref(dref)));
+  }
+
+  static int run(CRTPVariant & crtp, Args args )
+  {
+    return boost::apply_visitor( Launcher(args),crtp );
+  }
+};
 
 
 int main()
@@ -112,7 +138,12 @@ int main()
   CRTPBase<CRTPDerived> & dref = d;
   CRTPVariant v = d;
 
-  algo(v, make_vector(boost::cref(1.0),boost::cref(1),boost::cref(Eigen::MatrixXd::Zero(3,3)),boost::cref(TestObj(1))) );
+  //(CRTPBase<D> & crtp, const double & x,const int & y, const Eigen::MatrixXd & z,const TestObj & a)
+  
+
+  //Args args(1.0,1,Eigen::MatrixXd::Zero(3,3),TestObj(1));
+  Launcher::run(v,  Launcher::Args(1.0,1,Eigen::MatrixXd::Zero(3,3),TestObj(1)) );
+
 
   return 0;
 }

unittest/rnea.cpp

@@ -32,7 +32,7 @@ void rneaForwardStep(const se3::Model& model,
   if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
 
   jmodel.jointMotion(a);
-  //data.a[i] =  jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ; 
+  data.a[i] =  jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ; 
   if(parent>0) data.a[i] += data.liMi[i].actInv(data.a[parent]);
   
   data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
@@ -48,8 +48,8 @@ void rneaBackwardStep(const se3::Model& model,
   using namespace Eigen;
   using namespace se3;
   
-  const Model::Index & parent = model.parents[i];      
-  jmodel.jointForce(data.tau) = jdata.S().transpose()*data.f[i];
+  const Model::Index & parent  = model.parents[i];      
+  jmodel.jointForce(data.tau)  = jdata.S().transpose()*data.f[i];
   if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
 }
 
@@ -122,9 +122,19 @@ struct RneaBackwardStepVisitor : public boost::static_visitor<>
 
 };
 
+//#define __SSE3__
+#include <fenv.h>
+#ifdef __SSE3__
+#include <pmmintrin.h>
+#endif
 
 int main()
 {
+#ifdef __SSE3__
+_MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
+#endif
+
+
   using namespace Eigen;
   using namespace se3;