GCC Code Coverage Report

Directory: ./
File: include/ndcurves/optimization/details.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 192 203 94.6%
Branches: 180 322 55.9%
Line Branch Exec Source
1 /**
2 * \file bezier_curve.h
3 * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
4 * \author Steve T.
5 * \version 0.1
6 * \date 06/17/2013
7 */
8
9 #ifndef _CLASS_LINEAR_PROBLEM_DETAILS
10 #define _CLASS_LINEAR_PROBLEM_DETAILS
11
12 #include <ndcurves/bernstein.h>
13 #include <ndcurves/bezier_curve.h>
14 #include <ndcurves/curve_constraint.h>
15 #include <ndcurves/linear_variable.h>
16 #include <ndcurves/optimization/definitions.h>
17
18 namespace ndcurves {
19 namespace optimization {
20 template <typename Point, typename Numeric, bool Safe = true>
21 struct problem_data {
22 29 problem_data(const std::size_t dim) : bezier(0), dim_(dim) {}
23 37 ~problem_data() {
24
2/4
✓ Branch 0 taken 37 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 37 times.
✗ Branch 3 not taken.
 37 if (bezier) delete bezier;
25 37 }
26
27 typedef linear_variable<Numeric> var_t;
28 typedef std::vector<var_t> T_var_t;
29 typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> >
30 bezier_t;
31
32 std::vector<var_t> variables_; // includes constant variables
33 std::size_t numVariables; // total number of variable (/ DIM for total size)
34 std::size_t numControlPoints; // total number of control Points (variables +
35 // waypoints) / DIM )
36 std::size_t startVariableIndex; // before that index, variables are constant
37 std::size_t numStateConstraints;
38 bezier_t* bezier;
39 const std::size_t dim_;
40
41 8 problem_data(const problem_data& other)
42 8 : variables_(other.variables_),
43 8 numVariables(other.numVariables),
44 8 numControlPoints(other.numControlPoints),
45 8 startVariableIndex(other.startVariableIndex),
46 8 numStateConstraints(other.numStateConstraints),
47 8 dim_(other.dim_) {
48 8 const bezier_t& b = *other.bezier;
49
1/2
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 bezier = new bezier_t(b.waypoints().begin(), b.waypoints().end(), b.T_min_,
50
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 b.T_max_, b.mult_T_);
51 8 }
52 };
53
54 33 inline std::size_t num_active_constraints(const constraint_flag& flag) {
55 33 long lValue = (long)(flag);
56 33 std::size_t iCount = 0;
57
2/2
✓ Branch 0 taken 89 times.
✓ Branch 1 taken 33 times.
 122 while (lValue != 0) {
58 89 lValue = lValue & (lValue - 1);
59 89 iCount++;
60 }
61
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 30 times.
 33 return (flag & NONE) ? iCount - 1 : iCount;
62 }
63
64 template <typename Numeric, typename LinearVar>
65 223 LinearVar fill_with_zeros(const LinearVar& var, const std::size_t i,
66 const std::size_t startVariableIndex,
67 const std::size_t numVariables,
68 const std::size_t Dim) {
69 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
70
1/2
✓ Branch 1 taken 223 times.
✗ Branch 2 not taken.
 223 typename LinearVar::matrix_x_t B;
71
2/4
✓ Branch 1 taken 223 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 223 times.
✗ Branch 5 not taken.
 223 B = matrix_t::Zero(Dim, numVariables * Dim);
72
7/8
✓ Branch 0 taken 193 times.
✓ Branch 1 taken 30 times.
✓ Branch 2 taken 168 times.
✓ Branch 3 taken 25 times.
✓ Branch 4 taken 168 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 168 times.
✓ Branch 7 taken 55 times.
 391 if (startVariableIndex <= i && i <= startVariableIndex + numVariables - 1 &&
73 168 var.size() > 0)
74
2/4
✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 168 times.
✗ Branch 6 not taken.
 168 B.block(0, Dim * (i - startVariableIndex), Dim, Dim) = var.B();
75
1/2
✓ Branch 2 taken 223 times.
✗ Branch 3 not taken.
 446 return LinearVar(B, var.c());
76 223 }
77
78 template <typename Point, typename Numeric, typename Bezier, typename LinearVar>
79 29 Bezier* compute_linear_control_points(const problem_data<Point, Numeric>& pData,
80 const std::vector<LinearVar>& linearVars,
81 const Numeric totalTime) {
82 29 std::vector<LinearVar> res;
83 // now need to fill all this with zeros...
84 29 std::size_t totalvar = linearVars.size();
85
2/2
✓ Branch 0 taken 223 times.
✓ Branch 1 taken 29 times.
 252 for (std::size_t i = 0; i < totalvar; ++i)
86
2/4
✓ Branch 1 taken 223 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 223 times.
✗ Branch 5 not taken.
 223 res.push_back(fill_with_zeros<Numeric, LinearVar>(
87 223 linearVars[i], i, pData.startVariableIndex, pData.numVariables,
88 223 pData.dim_));
89
2/4
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 29 times.
✗ Branch 7 not taken.
 58 return new Bezier(res.begin(), res.end(), 0., totalTime);
90 29 }
91
92 template <typename Point, typename Numeric, bool Safe>
93 33 problem_data<Point, Numeric, Safe> setup_control_points(
94 const problem_definition<Point, Numeric>& pDef) {
95 typedef Numeric num_t;
96 typedef Point point_t;
97 typedef linear_variable<Numeric> var_t;
98 typedef problem_data<Point, Numeric> problem_data_t;
99
100 33 const std::size_t& degree = pDef.degree;
101 33 const constraint_flag& flag = pDef.flag;
102
103 33 const std::size_t numControlPoints = pDef.degree + 1;
104 33 const std::size_t numActiveConstraints = num_active_constraints(flag);
105
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 29 times.
 33 if (numActiveConstraints >= numControlPoints)
106
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 throw std::runtime_error(
107 "In setup_control_points; too many constraints for the considered "
108 "degree");
109
110 29 problem_data_t problemData(pDef.dim_);
111 29 typename problem_data_t::T_var_t& variables_ = problemData.variables_;
112
113 29 std::size_t numConstants = 0;
114 29 std::size_t i = 0;
115
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 14 times.
 29 if (flag & INIT_POS) {
116
3/6
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 15 variables_.push_back(var_t(pDef.init_pos));
117 15 ++numConstants;
118 15 ++i;
119
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 5 times.
 15 if (flag & INIT_VEL) {
120
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 point_t vel =
121
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 pDef.init_pos + (pDef.init_vel / (num_t)degree) / pDef.totalTime;
122
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 10 variables_.push_back(var_t(vel));
123 10 ++numConstants;
124 10 ++i;
125
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 6 times.
 10 if (flag & INIT_ACC) {
126
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 4 point_t acc = (pDef.init_acc / (num_t)(degree * (degree - 1))) /
127
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 (pDef.totalTime * pDef.totalTime) +
128
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 2 * vel - pDef.init_pos;
129 ;
130
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 4 variables_.push_back(var_t(acc));
131 4 ++numConstants;
132 4 ++i;
133
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 3 times.
 4 if (flag & INIT_JERK) {
134
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 point_t jerk = pDef.init_jerk * pDef.totalTime * pDef.totalTime *
135 1 pDef.totalTime /
136
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (num_t)(degree * (degree - 1) * (degree - 2)) +
137
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 3 * acc - 3 * vel + pDef.init_pos;
138
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 variables_.push_back(var_t(jerk));
139 1 ++numConstants;
140 1 ++i;
141 }
142 2 }
143 4 }
144 }
145 29 const std::size_t first_variable_idx = i;
146 // variables
147
2/2
✓ Branch 0 taken 85 times.
✓ Branch 1 taken 29 times.
 114 for (; i + 4 < numControlPoints; ++i)
148
2/4
✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
 85 variables_.push_back(var_t::X(pDef.dim_));
149 // end constraints
150
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 17 times.
 29 if (flag & END_POS) {
151
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 5 times.
 12 if (flag & END_VEL) {
152
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 14 point_t vel =
153
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 14 pDef.end_pos - (pDef.end_vel / (num_t)degree) / pDef.totalTime;
154
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 2 times.
 7 if (flag & END_ACC) {
155
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 point_t acc = (pDef.end_acc / (num_t)(degree * (degree - 1))) /
156
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 (pDef.totalTime) * (pDef.totalTime) +
157
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 2 * vel - pDef.end_pos;
158
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 4 times.
 5 if (flag & END_JERK) {
159
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
 1 point_t jerk = -pDef.end_jerk * pDef.totalTime * pDef.totalTime *
160 1 pDef.totalTime /
161
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (num_t)(degree * (degree - 1) * (degree - 2)) +
162
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 3 * acc - 3 * vel + pDef.end_pos;
163
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 variables_.push_back(var_t(jerk));
164 1 ++numConstants;
165 1 ++i;
166 ✗ } else
167
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
 8 while (i < numControlPoints - 3) {
168
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 variables_.push_back(var_t::X(pDef.dim_));
169 4 ++i;
170 }
171
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 5 variables_.push_back(var_t(acc));
172 5 ++numConstants;
173 5 ++i;
174 2 } else
175
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
 6 while (i < numControlPoints - 2) {
176
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 variables_.push_back(var_t::X(pDef.dim_));
177 4 ++i;
178 }
179
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 7 variables_.push_back(var_t(vel));
180 7 ++numConstants;
181 7 ++i;
182 2 } else {
183
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 5 times.
 15 while (i < numControlPoints - 1) {
184
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 variables_.push_back(var_t::X(pDef.dim_));
185 10 ++i;
186 }
187 }
188
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 12 variables_.push_back(var_t(pDef.end_pos));
189 12 ++numConstants;
190 12 ++i;
191 }
192 // add remaining variables (only if no end_pos constraints)
193
4/6
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 65 times.
✓ Branch 8 taken 29 times.
 94 for (; i < numControlPoints; ++i) variables_.push_back(var_t::X(pDef.dim_));
194
195
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
 29 if (numControlPoints <= numConstants) {
196 ✗ throw std::runtime_error("numControlPoints < numConstants");
197 }
198
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 29 times.
 29 if (numControlPoints != variables_.size()) {
199 ✗ throw std::runtime_error("numControlPoints != variables_.size()");
200 }
201
202 29 problemData.numControlPoints = numControlPoints;
203 29 problemData.numVariables = numControlPoints - numConstants;
204 29 problemData.startVariableIndex = first_variable_idx;
205 29 problemData.numStateConstraints =
206 29 numActiveConstraints - problemData.numVariables;
207 29 problemData.bezier = compute_linear_control_points<
208 58 Point, Numeric, bezier_curve<Numeric, Numeric, true, var_t>, var_t>(
209
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 29 problemData, variables_, pDef.totalTime);
210 29 return problemData;
211 }
212
213 // TODO assumes constant are inside constraints...
214 template <typename Point, typename Numeric>
215 2 long compute_num_ineq_control_points(
216 const problem_definition<Point, Numeric>& pDef,
217 const problem_data<Point, Numeric>& pData) {
218 typedef problem_definition<Point, Numeric> problem_definition_t;
219 2 long rows(0);
220 // rows depends on each constraint size, and the number of waypoints
221 2 for (typename problem_definition_t::CIT_vector_x_t cit =
222 2 pDef.inequalityVectors_.begin();
223
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 2 times.
 3 cit != pDef.inequalityVectors_.end(); ++cit)
224 1 rows += cit->rows() * pData.numControlPoints;
225 2 return rows;
226 }
227
228 template <typename Point, typename Numeric>
229 std::vector<bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > >
230 1 split(const problem_definition<Point, Numeric>& pDef,
231 problem_data<Point, Numeric>& pData) {
232 typedef linear_variable<Numeric> linear_variable_t;
233 typedef bezier_curve<Numeric, Numeric, true, linear_variable_t> bezier_t;
234 typedef std::vector<bezier_t> T_bezier_t;
235
236 1 const Eigen::VectorXd& times = pDef.splitTimes_;
237 1 T_bezier_t res;
238 1 bezier_t* current = pData.bezier;
239 1 Numeric current_time = 0.;
240 Numeric tmp;
241
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 for (int i = 0; i < times.rows(); ++i) {
242 ✗ tmp = times[i];
243 ✗ std::pair<bezier_t, bezier_t> pairsplit =
244 current->split(tmp - current_time);
245 ✗ res.push_back(pairsplit.first);
246 ✗ current = &(pairsplit.second);
247 ✗ current_time += tmp - current_time;
248 }
249
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res.push_back(*current);
250 1 return res;
251 }
252
253 template <typename Point, typename Numeric>
254 2 void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef,
255 problem_data<Point, Numeric>& pData,
256 quadratic_problem<Point, Numeric>& prob) {
257 2 const std::size_t& Dim = pData.dim_;
258 typedef problem_definition<Point, Numeric> problem_definition_t;
259 typedef typename problem_definition_t::matrix_x_t matrix_x_t;
260 typedef typename problem_definition_t::vector_x_t vector_x_t;
261 typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> >
262 bezier_t;
263 typedef std::vector<bezier_t> T_bezier_t;
264 typedef typename T_bezier_t::const_iterator CIT_bezier_t;
265 typedef typename bezier_t::t_point_t t_point;
266 typedef typename bezier_t::t_point_t::const_iterator cit_point;
267
268 2 long cols = pData.numVariables * Dim;
269 2 long rows = compute_num_ineq_control_points<Point, Numeric>(pDef, pData);
270
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 prob.ineqMatrix = matrix_x_t::Zero(rows, cols);
271
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 prob.ineqVector = vector_x_t::Zero(rows);
272
273
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
 2 if (pDef.inequalityMatrices_.size() == 0) return;
274
275 // compute sub-bezier curves
276
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
277
278
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (pDef.inequalityMatrices_.size() != pDef.inequalityVectors_.size()) {
279 ✗ throw std::invalid_argument(
280 "The sizes of the inequality matrices and vectors do not match.");
281 }
282
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (pDef.inequalityMatrices_.size() != beziers.size()) {
283 ✗ throw std::invalid_argument(
284 "The sizes of the inequality matrices and the bezier degree do not "
285 "match.");
286 }
287
288 1 long currentRowIdx = 0;
289 typename problem_definition_t::CIT_matrix_x_t cmit =
290 1 pDef.inequalityMatrices_.begin();
291 typename problem_definition_t::CIT_vector_x_t cvit =
292 1 pDef.inequalityVectors_.begin();
293 // for each bezier split ..
294
2/2
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
 2 for (CIT_bezier_t bit = beziers.begin(); bit != beziers.end();
295 1 ++bit, ++cvit, ++cmit) {
296 // compute vector of linear expressions of each control point
297 1 const t_point& wps = bit->waypoints();
298 // each control has a linear expression depending on all variables
299
2/2
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 1 times.
 6 for (cit_point cit = wps.begin(); cit != wps.end(); ++cit) {
300
2/4
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
 5 prob.ineqMatrix.block(currentRowIdx, 0, cmit->rows(), cols) =
301
1/2
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 (*cmit) * (cit->B()); // constraint inequality for current bezier *
302 // expression of control point
303
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
 5 prob.ineqVector.segment(currentRowIdx, cmit->rows()) =
304
1/2
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 *cvit - (*cmit) * (cit->c());
305 5 currentRowIdx += cmit->rows();
306 }
307 }
308
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 assert(rows == currentRowIdx); // we filled all the constraints - NB: leave
309 // assert for Debug tests
310 1 }
311
312 template <typename Point, typename Numeric, typename In>
313 2 quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1,
314 In PointsBegin2, In PointsEnd2,
315 const std::size_t /*Dim*/) {
316 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
317 2 unsigned int nPoints1 =
318
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 (unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
319 2 nPoints2 =
320
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 (unsigned int)(std::distance(PointsBegin2, PointsEnd2));
321
2/4
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 2 if (nPoints1 <= 0 || nPoints2 <= 0) {
322 ✗ throw std::runtime_error(
323 "This should never happen because an unsigned int cannot go negative "
324 "without underflowing.");
325 }
326 2 unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
327 2 unsigned int newDeg = (deg1 + deg2);
328 // the integral of the primitive will simply be the last control points of the
329 // primitive, divided by the degree of the primitive, newDeg. We will store
330 // this in matrices for bilinear terms, and a vector for the linear terms, as
331 // well as another one for the constants.
332
3/6
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 quadratic_variable<Numeric> res(vector_x_t::Zero(PointsBegin1->B().cols()));
333 // depending on the index, the fraction coefficient of the bernstein polynom
334 // is either the fraction given by (i+j)/ (deg1+deg2), or 1 - (i+j)/
335 // (deg1+deg2). The trick is that the condition is given by whether the
336 // current index in the combinatorial is odd or even. time parametrization is
337 // not relevant for the cost
338
339 Numeric ratio;
340
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 2 times.
 16 for (unsigned int i = 0; i < newDeg + 1; ++i) {
341
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 8 times.
 14 unsigned int j = i > deg2 ? i - deg2 : 0;
342
2/2
✓ Branch 1 taken 32 times.
✓ Branch 2 taken 14 times.
 46 for (; j < std::min(deg1, i) + 1; ++j) {
343
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 32 ratio = (Numeric)(bin(deg1, j) * bin(deg2, i - j)) /
344
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 (Numeric)(bin(newDeg, i));
345 32 In itj = PointsBegin1 + j;
346 32 In iti = PointsBegin2 + (i - j);
347
3/6
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
 32 res += ((*itj) * (*iti)) * ratio;
348 }
349 }
350
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 return res / (newDeg + 1);
351 2 }
352
353 inline constraint_flag operator~(constraint_flag a) {
354 return static_cast<constraint_flag>(~static_cast<int>(a));
355 }
356
357 21 inline constraint_flag operator|(constraint_flag a, constraint_flag b) {
358 21 return static_cast<constraint_flag>(static_cast<int>(a) |
359 21 static_cast<int>(b));
360 }
361
362 111 inline constraint_flag operator&(constraint_flag a, constraint_flag b) {
363 111 return static_cast<constraint_flag>(static_cast<int>(a) &
364 111 static_cast<int>(b));
365 }
366
367 inline constraint_flag operator^(constraint_flag a, constraint_flag b) {
368 return static_cast<constraint_flag>(static_cast<int>(a) ^
369 static_cast<int>(b));
370 }
371
372 inline constraint_flag& operator|=(constraint_flag& a, constraint_flag b) {
373 return (constraint_flag&)((int&)(a) |= static_cast<int>(b));
374 }
375
376 inline constraint_flag& operator&=(constraint_flag& a, constraint_flag b) {
377 return (constraint_flag&)((int&)(a) &= static_cast<int>(b));
378 }
379
380 inline constraint_flag& operator^=(constraint_flag& a, constraint_flag b) {
381 return (constraint_flag&)((int&)(a) ^= static_cast<int>(b));
382 }
383
384 } // namespace optimization
385 } // namespace ndcurves
386 #endif //_CLASS_LINEAR_PROBLEM_DETAILS
387