MathDefs.h 1.21 KB
Newer Older
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 <Eigen/Dense>
#include <Eigen/SVD>

21
#include <vector>
22
23
24
25
26
27
28
29
30
#include <utility> 

namespace spline{
	
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
	Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
31
	_Matrix_Type_ m_sigma = svd.singularValues();
32

33
	double pinvtoler= 1.e-6; // choose your tolerance widely!
34

35
	_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
36
37
38
39
40
41
42
43
44
45
	for (long i=0; i<m_sigma.rows(); ++i)
	{
		if (m_sigma(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
46