Loading...
Searching...
No Matches
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
23
namespace
parametriccurves
{
24
25
// REF: boulic et al An inverse kinematics architecture enforcing an arbitrary
26
// number of strict priority levels
27
template
<
typename
_Matrix_Type_>
28
void
PseudoInverse
(
_Matrix_Type_
&
pinvmat
) {
29
Eigen::JacobiSVD<_Matrix_Type_>
svd
(
30
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
=
36
_Matrix_Type_::Zero(
pinvmat
.cols(),
pinvmat
.rows());
37
for
(
long
i
= 0;
i
<
m_sigma
.rows(); ++
i
) {
38
if
(
m_sigma
(
i
) >
pinvtoler
)
m_sigma_inv
(
i
,
i
) = 1.0 /
m_sigma
(
i
);
39
}
40
pinvmat
= (
svd
.matrixV() *
m_sigma_inv
*
svd
.matrixU().transpose());
41
}
42
43
}
// namespace parametriccurves
44
#endif
//_SPLINEMATH
parametriccurves
Definition
abstract-curve.hpp:16
parametriccurves::PseudoInverse
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition
MathDefs.h:28
parametriccurves::curve_constraints
Definition
curve-constraint.hpp:18
include
parametric-curves
MathDefs.h
Generated by
1.10.0