GCC Code Coverage Report

Directory: ./
File: include/ndcurves/linear_variable.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 98 140 70.0%
Branches: 85 216 39.4%
Line Branch Exec Source
1 /**
2 * \file linear_variable.h
3 * \brief storage for variable points of the form p_i = B_i x + c_i
4 * \author Steve T.
5 * \version 0.1
6 * \date 07/02/2019
7 */
8
9 #ifndef _CLASS_LINEAR_VARIABLE
10 #define _CLASS_LINEAR_VARIABLE
11
12 #include <math.h>
13
14 #include <Eigen/Core>
15 #include <stdexcept>
16 #include <vector>
17
18 #include "MathDefs.h"
19 #include "bezier_curve.h"
20 #include "curve_abc.h"
21 #include "serialization/archive.hpp"
22 #include "serialization/eigen-matrix.hpp"
23
24 namespace ndcurves {
25 template <typename Numeric = double, bool Safe = true>
26 struct linear_variable : public serialization::Serializable {
27 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
28 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
29 typedef Eigen::Matrix<Numeric, 3, 1> vector_3_t;
30 typedef Eigen::Matrix<Numeric, 3, 3> matrix_3_t;
31 typedef linear_variable<Numeric> linear_variable_t;
32
33 155 linear_variable()
34
1/2
✓ Branch 2 taken 155 times.
✗ Branch 3 not taken.
 155 : B_(matrix_x_t::Identity(0, 0)),
35
2/4
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155 times.
✗ Branch 5 not taken.
 155 c_(vector_x_t::Zero(0)),
36 155 zero(true) {} // variable
37 55 linear_variable(const vector_x_t& c)
38
1/2
✓ Branch 4 taken 55 times.
✗ Branch 5 not taken.
 55 : B_(matrix_x_t::Zero(c.size(), c.size())),
39
1/2
✓ Branch 1 taken 55 times.
✗ Branch 2 not taken.
 55 c_(c),
40 55 zero(false) {} // constant
41 3277 linear_variable(const matrix_x_t& B, const vector_x_t& c)
42
1/2
✓ Branch 2 taken 3277 times.
✗ Branch 3 not taken.
 3277 : B_(B), c_(c), zero(false) {} // mixed
43 5231 linear_variable(const linear_variable_t& other)
44 5231 : B_(other.B()),
45
1/2
✓ Branch 2 taken 5231 times.
✗ Branch 3 not taken.
 5231 c_(other.c()),
46 5231 zero(other.isZero()) {} // copy constructor
47
48 8718 ~linear_variable() {}
49
50 /// \brief Linear evaluation for vector x.
51 /// \param val : vector to evaluate the linear variable.
52 /// \return Evaluation of linear variable for vector x.
53 ///
54 170 vector_x_t operator()(const Eigen::Ref<const vector_x_t>& val) const {
55
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 170 times.
 170 if (isZero()) return c();
56
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 170 times.
 170 if (Safe && B().cols() != val.rows())
57 ✗ throw std::length_error(
58 "Cannot evaluate linear variable, variable value does not have the "
59 "correct dimension");
60
2/4
✓ Branch 4 taken 170 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
 170 return B() * val + c();
61 }
62
63 /// \brief Add another linear variable.
64 /// \param w1 : linear variable to add.
65 /// \return Linear variable after operation.
66 ///
67 934 linear_variable_t& operator+=(const linear_variable_t& w1) {
68
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 934 times.
 934 if (w1.isZero()) return *this;
69
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 934 times.
 934 if (isZero()) {
70 ✗ this->B_ = w1.B_;
71 ✗ this->c_ = w1.c_;
72 ✗ zero = w1.isZero();
73 } else {
74
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 934 times.
 934 if (Safe && B().rows() != w1.B().rows())
75 ✗ throw std::length_error(
76 "Cannot add linear variables, variables do not have the same "
77 "dimension");
78
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 934 times.
 1868 else if (B().cols() >
79 934 w1.B().cols()) { // new variables added left for primitive
80 ✗ B_.block(0, B().cols() - w1.B().cols(), B().rows(), w1.B().cols()) +=
81 ✗ w1.B();
82 ✗ c_.tail(w1.c().rows()) += w1.c();
83
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 934 times.
 1868 } else if (B().cols() <
84 934 w1.B().cols()) { // new variables added left for primitive
85 ✗ linear_variable_t opp = w1 + (*this);
86 ✗ this->B_ = opp.B_;
87 ✗ this->c_ = opp.c_;
88 ✗ } else {
89 934 this->B_ += w1.B_;
90 934 this->c_ += w1.c_;
91 }
92 }
93 934 return *this;
94 }
95
96 /// \brief Substract another linear variable.
97 /// \param w1 : linear variable to substract.
98 /// \return Linear variable after operation.
99 ///
100 16 linear_variable_t& operator-=(const linear_variable_t& w1) {
101
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 16 if (w1.isZero()) return *this;
102
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 16 if (isZero()) {
103 ✗ this->B_ = -w1.B_;
104 ✗ this->c_ = -w1.c_;
105 ✗ zero = w1.isZero();
106 } else {
107
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 16 times.
 16 if (Safe && B().rows() != w1.B().rows())
108 ✗ throw std::length_error(
109 "Cannot add linear variables, variables do not have the same "
110 "dimension");
111
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
 32 else if (B().cols() >
112 16 w1.B().cols()) { // new variables added left for primitive
113 ✗ B_.block(0, B().cols() - w1.B().cols(), B().rows(), w1.B().cols()) -=
114 ✗ w1.B();
115 ✗ c_.tail(w1.c().rows()) -= w1.c();
116
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
 32 } else if (B().cols() <
117 16 w1.B().cols()) { // new variables added left for primitive
118 ✗ linear_variable_t opp = -w1 + (*this);
119 ✗ this->B_ = opp.B_;
120 ✗ this->c_ = opp.c_;
121 ✗ } else {
122 16 this->B_ -= w1.B_;
123 16 this->c_ -= w1.c_;
124 }
125 }
126 16 return *this;
127 }
128
129 /// \brief Divide by a constant : p_i / d = B_i*x/d + c_i/d.
130 /// \param d : constant.
131 /// \return Linear variable after operation.
132 ///
133 ✗ linear_variable_t& operator/=(const double d) {
134 ✗ B_ /= d;
135 ✗ c_ /= d;
136 ✗ return *this;
137 }
138
139 /// \brief Multiply by a constant : p_i / d = B_i*x*d + c_i*d.
140 /// \param d : constant.
141 /// \return Linear variable after operation.
142 ///
143 1915 linear_variable_t& operator*=(const double d) {
144 1915 B_ *= d;
145 1915 c_ *= d;
146 1915 return *this;
147 }
148
149 /// \brief Compute the cross product of the current linear_variable and the
150 /// other.
151 /// This method of course only makes sense for dimension 3 curves and
152 /// dimension 3 unknown, since otherwise the result is non-linear. It assumes
153 /// that a method point_t cross(const point_t&, const point_t&) has been
154 /// defined
155 /// \param pOther other polynomial to compute the cross product with.
156 /// \return a new polynomial defining the cross product between this and
157 /// other
158 33 linear_variable_t cross(const linear_variable_t& other) const {
159
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 33 times.
 33 if (B().rows() != 3)
160 ✗ throw std::invalid_argument(
161 "Can't perform cross product on linear variables with dimensions != "
162 "3 ");
163
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 33 times.
 33 if (B().cols() != 3)
164 ✗ throw std::invalid_argument(
165 "Can't perform cross product on linear variables more than one "
166 "unknown ");
167
6/8
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✓ Branch 5 taken 18 times.
✓ Branch 6 taken 15 times.
✓ Branch 7 taken 18 times.
✓ Branch 9 taken 15 times.
✗ Branch 10 not taken.
 33 if (isZero() || other.isZero()) return linear_variable_t::Zero(3);
168
5/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 17 times.
✓ Branch 12 taken 1 times.
 35 if ((B().squaredNorm() - B().diagonal().squaredNorm() > MARGIN) ||
169
7/10
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 17 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 17 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✓ Branch 12 taken 16 times.
✓ Branch 13 taken 2 times.
✓ Branch 14 taken 16 times.
 35 (other.B().squaredNorm() - other.B().diagonal().squaredNorm() > MARGIN))
170
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument(
171 "Can't perform cross product on linear variables if B is not "
172 "diagonal ");
173 // (B1 x + c1) X (B2 x + c2) = (-c2X B1) x + (bX B2) x + b1Xb2
174
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 typename linear_variable_t::matrix_3_t newB =
175 skew<typename linear_variable_t::matrix_3_t,
176
4/8
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 16 times.
✗ Branch 12 not taken.
 16 typename linear_variable_t::vector_3_t>(-other.c()) *
177 16 B() +
178 skew<typename linear_variable_t::matrix_3_t,
179
3/6
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
 16 typename linear_variable_t::vector_3_t>(c()) *
180 16 other.B();
181
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 typename linear_variable_t::vector_3_t newC =
182 16 ndcurves::cross(c(), other.c());
183
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
 16 return linear_variable_t(newB, newC);
184 }
185
186 /// \brief Get a linear variable equal to zero.
187 /// \param dim : Dimension of linear variable.
188 /// \return Linear variable equal to zero.
189 ///
190 30 static linear_variable_t Zero(size_t dim = 0) {
191
4/8
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 30 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 30 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 30 times.
✗ Branch 12 not taken.
 30 return linear_variable_t(matrix_x_t::Zero(dim, dim), vector_x_t::Zero(dim));
192 }
193
194 /// \brief Get a linear variable equal to the variable
195 /// \param dim : Dimension of linear variable.
196 /// \return Linear variable equal to the variable.
197 ///
198 168 static linear_variable_t X(size_t dim = 0) {
199 return linear_variable_t(matrix_x_t::Identity(dim, dim),
200
4/8
✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 168 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 168 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 168 times.
✗ Branch 12 not taken.
 168 vector_x_t::Zero(dim));
201 }
202
203 /// \brief Get dimension of linear variable.
204 /// \return Dimension of linear variable.
205 ///
206
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 701 times.
 705 std::size_t size() const { return zero ? 0 : std::max(B_.rows(), c_.size()); }
207
208 /// \brief Get norm of linear variable (Norm of B plus norm of C).
209 /// \return Norm of linear variable.
210 ✗ Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
211
212 /// \brief Check if actual linear variable and other are approximately equal
213 /// given a precision threshold. Only two curves of the same class can be
214 /// approximately equal, \param prec : the precision threshold, default
215 /// Eigen::NumTraits<Numeric>::dummy_precision() \return true if the two
216 /// linear variables are approximately equal.
217 ✗ bool isApprox(
218 const linear_variable_t& other,
219 const double prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
220 ✗ return (*this - other).norm() < prec;
221 }
222
223 14763 const matrix_x_t& B() const { return B_; }
224 8874 const vector_x_t& c() const { return c_; }
225 7484 bool isZero() const { return zero; }
226
227 // Serialization of the class
228 friend class boost::serialization::access;
229
230 template <class Archive>
231 ✗ void serialize(Archive& ar, const unsigned int version) {
232 if (version) {
233 // Do something depending on version ?
234 }
235 ✗ ar& boost::serialization::make_nvp("B_", B_);
236 ✗ ar& boost::serialization::make_nvp("c_", c_);
237 ✗ ar& boost::serialization::make_nvp("zero", zero);
238 }
239
240 905 linear_variable& operator=(const linear_variable& other) {
241
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 905 times.
 905 if (this == &other) {
242 ✗ return *this;
243 }
244 // Perform a deep copy here to copy all necessary data.
245 // Make sure to handle memory allocation properly.
246 // You may need to copy the data contained within the linear_variable.
247 905 this->B_ = other.B_;
248 905 this->c_ = other.c_;
249 905 this->zero = other.zero;
250 905 return *this;
251 }
252
253 private:
254 matrix_x_t B_;
255 vector_x_t c_;
256 bool zero;
257 };
258
259 template <typename N, bool S>
260 897 inline linear_variable<N, S> operator+(const linear_variable<N, S>& w1,
261 const linear_variable<N, S>& w2) {
262
1/2
✓ Branch 3 taken 897 times.
✗ Branch 4 not taken.
 897 linear_variable<N, S> res(w1.B(), w1.c());
263
2/4
✓ Branch 1 taken 897 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 897 times.
✗ Branch 5 not taken.
 1794 return res += w2;
264 897 }
265
266 template <typename N, bool S>
267 16 linear_variable<N, S> operator-(const linear_variable<N, S>& w1,
268 const linear_variable<N, S>& w2) {
269
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 linear_variable<N, S> res(w1.B(), w1.c());
270
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 return res -= w2;
271 16 }
272
273 template <typename N, bool S>
274 ✗ linear_variable<N, S> operator-(const linear_variable<N, S>& w1) {
275 ✗ return linear_variable<N, S>(-w1.B(), -w1.c());
276 }
277
278 template <typename N, bool S>
279 955 linear_variable<N, S> operator*(const double k,
280 const linear_variable<N, S>& w) {
281
1/2
✓ Branch 3 taken 955 times.
✗ Branch 4 not taken.
 955 linear_variable<N, S> res(w.B(), w.c());
282
2/4
✓ Branch 1 taken 955 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 955 times.
✗ Branch 5 not taken.
 1910 return res *= k;
283 955 }
284
285 template <typename N, bool S>
286 960 linear_variable<N, S> operator*(const linear_variable<N, S>& w,
287 const double k) {
288
1/2
✓ Branch 3 taken 960 times.
✗ Branch 4 not taken.
 960 linear_variable<N, S> res(w.B(), w.c());
289
2/4
✓ Branch 1 taken 960 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 960 times.
✗ Branch 5 not taken.
 1920 return res *= k;
290 960 }
291
292 template <typename N, bool S>
293 ✗ linear_variable<N, S> operator/(const linear_variable<N, S>& w,
294 const double k) {
295 ✗ linear_variable<N, S> res(w.B(), w.c());
296 ✗ return res /= k;
297 }
298
299 template <typename BezierFixed, typename BezierLinear, typename X>
300 16 BezierFixed evaluateLinear(const BezierLinear& bIn, const X x) {
301
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 typename BezierFixed::t_point_t fixed_wps;
302 16 for (typename BezierLinear::cit_point_t cit = bIn.waypoints().begin();
303
2/2
✓ Branch 3 taken 140 times.
✓ Branch 4 taken 16 times.
 156 cit != bIn.waypoints().end(); ++cit)
304
3/6
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 140 times.
✗ Branch 9 not taken.
 140 fixed_wps.push_back(cit->operator()(x));
305 16 return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_,
306
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 32 bIn.T_max_);
307 16 }
308
309 template <typename N, bool S>
310 std::ostream& operator<<(std::ostream& os, const linear_variable<N, S>& l) {
311 return os << "linear_variable: \n \t B:\n"
312 << l.B() << "\t c: \n"
313 << l.c().transpose();
314 }
315
316 } // namespace ndcurves
317
318 DEFINE_CLASS_TEMPLATE_VERSION(
319 SINGLE_ARG(typename Numeric, bool Safe),
320 SINGLE_ARG(ndcurves::linear_variable<Numeric, Safe>))
321 #endif //_CLASS_LINEAR_VARIABLE
322