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 |
✓✓ |
19 |
if (a1 > b1) { |
17 |
|
9 |
t = (b1 / a1); |
18 |
|
9 |
return a1 * std::sqrt(1.0 + t * t); |
19 |
✗✓ |
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 |