eiquadprog-utils.hxx 1.09 KB
Newer Older
1
2
3
#ifndef EIQUADPROG_UTILS_HPP_
#define EIQUADPROG_UTILS_HPP_

4
#include <Eigen/Core>
Justin Carpentier's avatar
Justin Carpentier committed
5
#include <iostream>
6

7
namespace eiquadprog {
Justin Carpentier's avatar
Justin Carpentier committed
8
namespace utils {
9

10
/// Compute sqrt(a^2 + b^2)
11
12
template <typename Scalar>
inline Scalar distance(Scalar a, Scalar b) {
13
14
15
16
17
18
19
20
21
22
23
24
25
26
  Scalar a1, b1, t;
  a1 = std::abs(a);
  b1 = std::abs(b);
  if (a1 > b1) {
    t = (b1 / a1);
    return a1 * std::sqrt(1.0 + t * t);
  } else if (b1 > a1) {
    t = (a1 / b1);
    return b1 * std::sqrt(1.0 + t * t);
  }
  return a1 * std::sqrt(2.0);
}

template <class Derived>
27
void print_vector(const char *name, Eigen::MatrixBase<Derived> &x) {
Justin Carpentier's avatar
Justin Carpentier committed
28
  std::cerr << name << x.transpose() << std::endl;
29
30
}
template <class Derived>
31
void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x) {
Justin Carpentier's avatar
Justin Carpentier committed
32
  std::cerr << name << std::endl << x << std::endl;
33
34
}

35
36
37
38
39
40
41
42
43
template <class Derived>
void print_vector(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) {
  print_vector(name, x);
}
template <class Derived>
void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) {
  print_matrix(name, x);
}

44
45
}  // namespace utils
}  // namespace eiquadprog
46

47
#endif