Loading...
Searching...
No Matches
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
7namespace eiquadprog {
8namespace utils {
9
11template <typename Scalar>
12inline 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
26template <class Derived>
27void print_vector(const char *name, Eigen::MatrixBase<Derived> &x) {
28 std::cerr << name << x.transpose() << std::endl;
29}
30template <class Derived>
31void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x) {
32 std::cerr << name << std::endl << x << std::endl;
33}
34
35template <class Derived>
36void print_vector(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) {
37 print_vector(name, x);
38}
39template <class Derived>
40void 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