MathDefs.h
Go to the documentation of this file.
1 
15 #ifndef _SPLINEMATH
16 #define _SPLINEMATH
17 
18 #include <Eigen/Dense>
19 #include <Eigen/SVD>
20 
21 #include <vector>
22 #include <utility>
23 
24 namespace parametriccurves {
25 
26 // REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
27 template <typename _Matrix_Type_>
28 void PseudoInverse(_Matrix_Type_& pinvmat) {
29  Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
30  _Matrix_Type_ m_sigma = svd.singularValues();
31 
32  double pinvtoler = 1.e-6; // choose your tolerance widely!
33 
34  _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
35  for (long i = 0; i < m_sigma.rows(); ++i) {
36  if (m_sigma(i) > pinvtoler) m_sigma_inv(i, i) = 1.0 / m_sigma(i);
37  }
38  pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
39 }
40 
41 } // namespace parametriccurves
42 #endif //_SPLINEMATH
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition: MathDefs.h:28
Definition: MathDefs.h:24