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