| Directory: | ./ |
|---|---|
| File: | include/eiquadprog/eiquadprog-utils.hxx |
| Date: | 2024-08-26 22:54:11 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 8 | 10 | 80.0% |
| Branches: | 3 | 4 | 75.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #ifndef EIQUADPROG_UTILS_HPP_ | ||
| 2 | #define EIQUADPROG_UTILS_HPP_ | ||
| 3 | |||
| 4 | #include <Eigen/Core> | ||
| 5 | #include <iostream> | ||
| 6 | |||
| 7 | namespace eiquadprog { | ||
| 8 | namespace utils { | ||
| 9 | |||
| 10 | /// Compute sqrt(a^2 + b^2) | ||
| 11 | template <typename Scalar> | ||
| 12 | 19 | inline Scalar distance(Scalar a, Scalar b) { | |
| 13 | Scalar a1, b1, t; | ||
| 14 | 19 | a1 = std::abs(a); | |
| 15 | 19 | b1 = std::abs(b); | |
| 16 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 10 times.
|
19 | if (a1 > b1) { |
| 17 | 9 | t = (b1 / a1); | |
| 18 | 9 | return a1 * std::sqrt(1.0 + t * t); | |
| 19 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
10 | } else if (b1 > a1) { |
| 20 | ✗ | t = (a1 / b1); | |
| 21 | ✗ | return b1 * std::sqrt(1.0 + t * t); | |
| 22 | } | ||
| 23 | 10 | return a1 * std::sqrt(2.0); | |
| 24 | } | ||
| 25 | |||
| 26 | template <class Derived> | ||
| 27 | void print_vector(const char *name, Eigen::MatrixBase<Derived> &x) { | ||
| 28 | std::cerr << name << x.transpose() << std::endl; | ||
| 29 | } | ||
| 30 | template <class Derived> | ||
| 31 | void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x) { | ||
| 32 | std::cerr << name << std::endl << x << std::endl; | ||
| 33 | } | ||
| 34 | |||
| 35 | template <class Derived> | ||
| 36 | void print_vector(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) { | ||
| 37 | print_vector(name, x); | ||
| 38 | } | ||
| 39 | template <class Derived> | ||
| 40 | void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) { | ||
| 41 | print_matrix(name, x); | ||
| 42 | } | ||
| 43 | |||
| 44 | } // namespace utils | ||
| 45 | } // namespace eiquadprog | ||
| 46 | |||
| 47 | #endif | ||
| 48 |