MathDefs.h 1.21 KB
Newer Older
 stonneau committed Jun 19, 2013 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 ``````/** * \file Math.h * \brief Linear algebra and other maths definitions. Based on Eigen 3 or more * \author Steve T. * \version 0.1 * \date 06/17/2013 * * This file contains math definitions used * used throughout the library. * Preprocessors definition are used to use eitheir float * or double values, and 3 dimensional vectors for * the Point structure. */ #ifndef _SPLINEMATH #define _SPLINEMATH #include #include `````` stonneau committed Jun 27, 2013 21 ``````#include `````` stonneau committed Jun 19, 2013 22 23 24 25 26 27 28 29 30 ``````#include namespace spline{ //REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels template void PseudoInverse(_Matrix_Type_& pinvmat) { Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV); `````` stonneau committed Jun 27, 2013 31 `````` _Matrix_Type_ m_sigma = svd.singularValues(); `````` stonneau committed Jun 19, 2013 32 `````` `````` stonneau committed Jun 27, 2013 33 `````` double pinvtoler= 1.e-6; // choose your tolerance widely! `````` stonneau committed Jun 19, 2013 34 `````` `````` stonneau committed Jun 27, 2013 35 `````` _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows()); `````` stonneau committed Jun 19, 2013 36 37 38 39 40 41 42 43 44 45 `````` for (long i=0; i pinvtoler) m_sigma_inv(i,i)=1.0/m_sigma(i); } pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose()); } } // namespace spline #endif //_SPLINEMATH `````` stonneau committed Jun 27, 2013 46