eiquadprog-utils.hxx
Go to the documentation of this file.
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 
11 template <typename Scalar>
12 inline Scalar distance(Scalar a, Scalar b) {
13  Scalar a1, b1, t;
14  a1 = std::abs(a);
15  b1 = std::abs(b);
16  if (a1 > b1) {
17  t = (b1 / a1);
18  return a1 * std::sqrt(1.0 + t * t);
19  } else if (b1 > a1) {
20  t = (a1 / b1);
21  return b1 * std::sqrt(1.0 + t * t);
22  }
23  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
Scalar distance(Scalar a, Scalar b)
Compute sqrt(a^2 + b^2)
Definition: eiquadprog-utils.hxx:12
void print_vector(const char *name, Eigen::MatrixBase< Derived > &x)
Definition: eiquadprog-utils.hxx:27
void print_matrix(const char *name, Eigen::MatrixBase< Derived > &x)
Definition: eiquadprog-utils.hxx:31
Definition: eiquadprog-fast.hpp:63