00001 #include "../include/utilities.h" 00002 #include "../include/debug.h" 00003 #include "assert.h" 00004 00005 namespace ICR 00006 { 00007 //-------------------------------------------------------------------- 00008 Eigen::Matrix3d skewSymmetricMatrix(Eigen::Vector3d vector) 00009 { 00010 Eigen::Matrix3d skew_sym_m; 00011 00012 skew_sym_m.row(0) << 0, -vector(2), vector(1); 00013 skew_sym_m.row(1) << vector(2), 0, -vector(0); 00014 skew_sym_m.row(2) << -vector(1), vector(0), 0; 00015 00016 return skew_sym_m; 00017 } 00018 //-------------------------------------------------------------------- 00019 uint factorial(uint x) 00020 { 00021 assert(x>=1); 00022 return (x == 1 ? x : x * factorial(x - 1)); 00023 } 00024 //-------------------------------------------------------------------- 00025 uint dfactorial(uint x) 00026 { 00027 assert(x>=1); 00028 return(x <= 2 ? x : x*dfactorial(x-2)); 00029 00030 } 00031 //-------------------------------------------------------------------- 00032 }//namespace ICR