quadratic_variable.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_QUADRATIC_VARIABLE
10 #define _CLASS_QUADRATIC_VARIABLE
11 
12 #include "curves/curve_abc.h"
13 #include "curves/linear_variable.h"
14 
15 #include "MathDefs.h"
16 
17 #include <math.h>
18 #include <vector>
19 #include <Eigen/Core>
20 #include <stdexcept>
21 
22 namespace curves {
23 
24 template <typename Numeric = double>
25 struct quadratic_variable {
26  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
27  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> point_t;
29 
31  c_ = 0.;
32  b_ = point_t::Zero(1);
33  A_ = matrix_x_t::Zero(1, 1);
34  zero = true;
35  }
36 
37  quadratic_variable(const matrix_x_t& A, const point_t& b, const Numeric c = 0) : c_(c), b_(b), A_(A), zero(false) {
38  if (A.cols() != b.rows() || A.cols() != A.rows()) {
39  throw std::invalid_argument("The dimensions of A and b are incorrect.");
40  }
41  }
42 
43  quadratic_variable(const point_t& b, const Numeric c = 0)
44  : c_(c), b_(b), A_(matrix_x_t::Zero((int)(b.rows()), (int)(b.rows()))), zero(false) {}
45 
46  static quadratic_variable_t Zero(size_t dim = 0) { return quadratic_variable_t(); }
47 
48  // linear evaluation
49  Numeric operator()(const Eigen::Ref<const point_t>& val) const {
50  if (isZero()) {
51  throw std::runtime_error("Not initialized! (isZero)");
52  }
53  return val.transpose() * A() * val + b().transpose() * val + c();
54  }
55 
57  if (w1.isZero()) return *this;
58  if (isZero()) {
59  this->A_ = w1.A_;
60  this->b_ = w1.b_;
61  this->c_ = w1.c_;
62  zero = false;
63  } else {
64  this->A_ += w1.A_;
65  this->b_ += w1.b_;
66  this->c_ += w1.c_;
67  }
68  return *this;
69  }
71  if (w1.isZero()) return *this;
72  if (isZero()) {
73  this->A_ = -w1.A_;
74  this->b_ = -w1.b_;
75  this->c_ = -w1.c_;
76  zero = false;
77  } else {
78  this->A_ -= w1.A_;
79  this->b_ -= w1.b_;
80  this->c_ -= w1.c_;
81  }
82  return *this;
83  }
84 
85  quadratic_variable& operator/=(const double d) {
86  // handling zero case
87  if (!isZero()) {
88  this->A_ /= d;
89  this->b_ /= d;
90  this->c_ /= d;
91  }
92  return *this;
93  }
94  quadratic_variable& operator*=(const double d) {
95  // handling zero case
96  if (!isZero()) {
97  this->A_ *= d;
98  this->b_ *= d;
99  this->c_ *= d;
100  }
101  return *this;
102  }
103 
104  const matrix_x_t& A() const {
105  if (isZero()) {
106  throw std::runtime_error("Not initialized! (isZero)");
107  }
108  return A_;
109  }
110  const point_t& b() const {
111  if (isZero()) {
112  throw std::runtime_error("Not initialized! (isZero)");
113  }
114  return b_;
115  }
116  const Numeric c() const {
117  if (isZero()) {
118  throw std::runtime_error("Not initialized! (isZero)");
119  }
120  return c_;
121  }
122  bool isZero() const { return zero; }
123  std::size_t size() const { return zero ? 0 : std::max(A_.cols(), (std::max(b_.cols(), c_.size()))); }
124 
125  private:
126  Numeric c_;
127  point_t b_;
128  matrix_x_t A_;
129  bool zero;
130 };
131 
133 template <typename N>
134 Eigen::Matrix<N, Eigen::Dynamic, Eigen::Dynamic> to_diagonal(
135  const Eigen::Ref<const Eigen::Matrix<N, Eigen::Dynamic, 1> > vec) {
136  typedef typename Eigen::Matrix<N, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
137  return vec.asDiagonal();
138  matrix_t res(matrix_t::Zero(vec.rows(), vec.rows()));
139  for (int i = 0; i < vec.rows(); ++i) res(i, i) = vec(i);
140  return res;
141 }
142 
143 // only works with diagonal linear variables
144 template <typename N>
146  typedef quadratic_variable<N> quad_var_t;
147  typedef linear_variable<N> lin_var_t;
148  typedef typename quad_var_t::matrix_x_t matrix_x_t;
149  typedef typename quad_var_t::point_t point_t;
150  typedef typename lin_var_t::vector_x_t point_dim_t;
151  point_dim_t ones = point_dim_t::Ones(w1.c().size());
152  point_t b1 = w1.B().transpose() * ones, b2 = w2.B().transpose() * ones;
153  matrix_x_t B1 = to_diagonal<N>(b1);
154  matrix_x_t B2 = to_diagonal<N>(b2); // b1.array().square()
155  // omitting all transposes since A matrices are diagonals
156  matrix_x_t A = B1.transpose() * B2;
157  point_t b = w1.c().transpose() * w2.B() + w2.c().transpose() * w1.B();
158  N c = w1.c().transpose() * w2.c();
159  return quad_var_t(A, b, c);
160 }
161 
162 template <typename N>
164  quadratic_variable<N> res(w1.A(), w1.b(), w1.c());
165  return res += w2;
166 }
167 
168 template <typename N>
170  quadratic_variable<N> res(w1.A(), w1.b(), w1.c());
171  return res -= w2;
172 }
173 
174 template <typename N>
176  quadratic_variable<N> res(w.A(), w.b(), w.c());
177  return res *= k;
178 }
179 
180 template <typename N>
182  quadratic_variable<N> res(w.A(), w.b(), w.c());
183  return res *= k;
184 }
185 
186 template <typename N>
188  quadratic_variable<N> res(w.A(), w.b(), w.c());
189  return res /= k;
190 }
191 
192 } // namespace curves
193 #endif //_CLASS_QUADRATIC_VARIABLE
bool isZero() const
Definition: quadratic_variable.h:122
quadratic_variable< Numeric > quadratic_variable_t
Definition: quadratic_variable.h:28
quadratic_variable & operator-=(const quadratic_variable &w1)
Definition: quadratic_variable.h:70
quadratic_variable & operator+=(const quadratic_variable &w1)
Definition: quadratic_variable.h:56
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > matrix_x_t
Definition: quadratic_variable.h:26
double Numeric
Definition: effector_spline.h:26
const point_t & b() const
Definition: quadratic_variable.h:110
quadratic_variable & operator*=(const double d)
Definition: quadratic_variable.h:94
const matrix_x_t & B() const
Definition: linear_variable.h:124
interface for a Curve of arbitrary dimension.
Definition: bernstein.h:20
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > point_t
Definition: quadratic_variable.h:27
linear_variable< N, S > operator/(const linear_variable< N, S > &w, const double k)
Definition: linear_variable.h:172
static quadratic_variable_t Zero(size_t dim=0)
Definition: quadratic_variable.h:46
linear_variable< N, S > operator-(const linear_variable< N, S > &w1, const linear_variable< N, S > &w2)
Definition: linear_variable.h:154
const vector_x_t & c() const
Definition: linear_variable.h:125
Definition: fwd.h:58
quadratic_variable()
Definition: quadratic_variable.h:30
quadratic_variable(const matrix_x_t &A, const point_t &b, const Numeric c=0)
Definition: quadratic_variable.h:37
quadratic_variable(const point_t &b, const Numeric c=0)
Definition: quadratic_variable.h:43
std::size_t size() const
Definition: quadratic_variable.h:123
const matrix_x_t & A() const
Definition: quadratic_variable.h:104
linear_variable< N, S > operator*(const double k, const linear_variable< N, S > &w)
Definition: linear_variable.h:160
const Numeric c() const
Definition: quadratic_variable.h:116
Definition: fwd.h:55
Eigen::Matrix< N, Eigen::Dynamic, Eigen::Dynamic > to_diagonal(const Eigen::Ref< const Eigen::Matrix< N, Eigen::Dynamic, 1 > > vec)
Transforms a vector into a diagonal matrix.
Definition: quadratic_variable.h:134
storage for variable points of the form p_i = B_i x + c_i
Numeric operator()(const Eigen::Ref< const point_t > &val) const
Definition: quadratic_variable.h:49
linear_variable< N, S > operator+(const linear_variable< N, S > &w1, const linear_variable< N, S > &w2)
Definition: linear_variable.h:148
quadratic_variable & operator/=(const double d)
Definition: quadratic_variable.h:85