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 #include <utility>
21 #include <vector>
22 namespace ndcurves {
25 template <typename _Matrix_Type_>
26 void PseudoInverse(_Matrix_Type_& pinvmat) {
27  Eigen::JacobiSVD<_Matrix_Type_> svd(
28  pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
29  _Matrix_Type_ m_sigma = svd.singularValues();
30  double pinvtoler = 1.e-6; // choose your tolerance widely!
31  _Matrix_Type_ m_sigma_inv =
32  _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
33  for (long i = 0; i < m_sigma.rows(); ++i) {
34  if (m_sigma(i) > pinvtoler) {
35  m_sigma_inv(i, i) = 1.0 / m_sigma(i);
36  }
37  }
38  pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
39 }
40 
41 template <typename Matrix3, typename Point>
42 Matrix3 skew(const Point& x) {
43  Matrix3 res = Matrix3::Zero(3, 3);
44  res(0, 1) = -x(2);
45  res(0, 2) = x(1);
46  res(1, 0) = x(2);
47  res(1, 2) = -x(0);
48  res(2, 0) = -x(1);
49  res(2, 1) = x(0);
50  return res;
51 }
52 
53 static const double MARGIN(0.001);
54 
55 } // namespace ndcurves
56 #endif //_SPLINEMATH
Definition: bernstein.h:20
void PseudoInverse(_Matrix_Type_ &pinvmat)
An inverse kinematics architecture enforcing an arbitrary number of strict priority levels (Reference...
Definition: MathDefs.h:26
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Matrix3 skew(const Point &x)
Definition: MathDefs.h:42