| Directory: | ./ |
|---|---|
| File: | include/parametric-curves/MathDefs.h |
| Date: | 2025-04-07 13:04:42 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 9 | 9 | 100.0% |
| Branches: | 18 | 32 | 56.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | * \file Math.h | ||
| 3 | * \brief Linear algebra and other maths definitions. Based on Eigen 3 or more | ||
| 4 | * \author Steve T. | ||
| 5 | * \version 0.1 | ||
| 6 | * \date 06/17/2013 | ||
| 7 | * | ||
| 8 | * This file contains math definitions used | ||
| 9 | * used throughout the library. | ||
| 10 | * Preprocessors definition are used to use either float | ||
| 11 | * or double values, and 3 dimensional vectors for | ||
| 12 | * the Point structure. | ||
| 13 | */ | ||
| 14 | |||
| 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 | 4 | void PseudoInverse(_Matrix_Type_& pinvmat) { | |
| 29 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | Eigen::JacobiSVD<_Matrix_Type_> svd( |
| 30 | pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV); | ||
| 31 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | _Matrix_Type_ m_sigma = svd.singularValues(); |
| 32 | |||
| 33 | 4 | double pinvtoler = 1.e-6; // choose your tolerance widely! | |
| 34 | |||
| 35 |
2/4✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
4 | _Matrix_Type_ m_sigma_inv = |
| 36 | _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows()); | ||
| 37 |
2/2✓ Branch 1 taken 17 times.
✓ Branch 2 taken 4 times.
|
21 | for (long i = 0; i < m_sigma.rows(); ++i) { |
| 38 |
5/8✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 8 times.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
|
17 | if (m_sigma(i) > pinvtoler) m_sigma_inv(i, i) = 1.0 / m_sigma(i); |
| 39 | } | ||
| 40 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
|
4 | pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose()); |
| 41 | 4 | } | |
| 42 | |||
| 43 | } // namespace parametriccurves | ||
| 44 | #endif //_SPLINEMATH | ||
| 45 |