9 #ifndef _CLASS_QUADRATIC_VARIABLE 10 #define _CLASS_QUADRATIC_VARIABLE 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;
32 b_ = point_t::Zero(1);
33 A_ = matrix_x_t::Zero(1, 1);
38 if (A.cols() != b.rows() || A.cols() != A.rows()) {
39 throw std::invalid_argument(
"The dimensions of A and b are incorrect.");
44 : c_(
c), b_(b), A_(matrix_x_t::
Zero((int)(b.rows()), (int)(b.rows()))), zero(false) {}
51 throw std::runtime_error(
"Not initialized! (isZero)");
53 return val.transpose() *
A() * val +
b().transpose() * val +
c();
57 if (w1.isZero())
return *
this;
71 if (w1.isZero())
return *
this;
104 const matrix_x_t&
A()
const {
106 throw std::runtime_error(
"Not initialized! (isZero)");
110 const point_t&
b()
const {
112 throw std::runtime_error(
"Not initialized! (isZero)");
118 throw std::runtime_error(
"Not initialized! (isZero)");
123 std::size_t
size()
const {
return zero ? 0 : std::max(A_.cols(), (std::max(b_.cols(), c_.size()))); }
133 template <
typename N>
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);
144 template <
typename N>
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);
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);
162 template <
typename N>
168 template <
typename N>
174 template <
typename N>
180 template <
typename N>
186 template <
typename N>
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
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
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