GCC Code Coverage Report

Directory: ./
File: src/eiquadprog.cpp
Date: 2024-08-26 22:54:11
Exec Total Coverage
Lines: 134 207 64.7%
Branches: 134 324 41.4%
Line Branch Exec Source
1 #include <eiquadprog/eiquadprog.hpp>
2 namespace eiquadprog {
3 namespace solvers {
4
5 using namespace Eigen;
6
7 /* solve_quadprog is used for on-demand QP solving */
8
9 10 double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
10 const VectorXd &ce0, const MatrixXd &CI,
11 const VectorXd &ci0, VectorXd &x, VectorXi &activeSet,
12 size_t &activeSetSize) {
13 10 Eigen::DenseIndex p = CE.cols();
14 10 Eigen::DenseIndex m = CI.cols();
15
16
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 VectorXd y(p + m);
17
18
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 return solve_quadprog(G, g0, CE, ce0, CI, ci0, x, y, activeSet,
19 20 activeSetSize);
20 10 }
21
22 10 double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
23 const VectorXd &ce0, const MatrixXd &CI,
24 const VectorXd &ci0, VectorXd &x, VectorXd &y,
25 VectorXi &activeSet, size_t &activeSetSize) {
26
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 LLT<MatrixXd, Lower> chol(G.cols());
27 double c1;
28 /* compute the trace of the original matrix G */
29
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 c1 = G.trace();
30
31 /* decompose the matrix G in the form LL^T */
32
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 chol.compute(G);
33
34
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
35 20 activeSetSize);
36 10 }
37
38 ✗ double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
39 const MatrixXd &CE, const VectorXd &ce0,
40 const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
41 VectorXi &activeSet, size_t &activeSetSize) {
42 ✗ Eigen::DenseIndex p = CE.cols();
43 ✗ Eigen::DenseIndex m = CI.cols();
44
45 ✗ VectorXd y(p + m);
46
47 ✗ return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
48 ✗ activeSetSize);
49 }
50
51 /* solve_quadprog2 is used for when the Cholesky decomposition of G is
52 * pre-computed
53 * @param A Output vector containing the indexes of the active constraints.
54 * @param q Output value representing the size of the active set.
55 */
56 10 double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
57 const MatrixXd &CE, const VectorXd &ce0,
58 const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
59 VectorXd &u, VectorXi &A, size_t &q) {
60 size_t i, k, l; /* indices */
61 size_t ip, me, mi;
62 10 size_t n = g0.size();
63 10 size_t p = CE.cols();
64 10 size_t m = CI.cols();
65
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 10 MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size());
66
67
5/10
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 VectorXd s(m + p), z(n), r(m + p), d(n), np(n);
68
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 VectorXd x_old(n), u_old(m + p);
69 double f_value, psi, c2, sum, ss, R_norm;
70 10 const double inf = std::numeric_limits<double>::infinity();
71 double t, t1, t2; /* t is the step length, which is the minimum of the partial
72 * step length t1 and the full step length t2 */
73 // VectorXi A(m + p); // Del Prete: active set is now an output
74 // parameter
75
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 10 if (static_cast<size_t>(A.size()) != m + p) A.resize(m + p);
76
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 VectorXi A_old(m + p), iai(m + p), iaexcl(m + p);
77 // int q;
78 10 size_t iq, iter = 0;
79
80 10 me = p; /* number of equality constraints */
81 10 mi = m; /* number of inequality constraints */
82 10 q = 0; /* size of the active set A (containing the indices of the active
83 constraints) */
84
85 /*
86 * Preprocessing phase
87 */
88
89 /* initialize the matrix R */
90
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 d.setZero();
91
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 R.setZero();
92 10 R_norm = 1.0; /* this variable will hold the norm of the matrix R */
93
94 /* compute the inverse of the factorized matrix G^-1, this is the initial
95 * value for H */
96 // J = L^-T
97
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 J.setIdentity();
98
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 chol.matrixU().solveInPlace(J);
99
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 c2 = J.trace();
100 #ifdef EIQGUADPROG_TRACE_SOLVER
101 utils::print_matrix("J", J);
102 #endif
103
104 /* c1 * c2 is an estimate for cond(G) */
105
106 /*
107 * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
108 * this is a feasible point in the dual space
109 * x = G^-1 * g0
110 */
111
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 x = -g0;
112
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 chol.solveInPlace(x);
113 /* and compute the current solution value */
114
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 f_value = 0.5 * g0.dot(x);
115 #ifdef EIQGUADPROG_TRACE_SOLVER
116 std::cerr << "Unconstrained solution: " << f_value << std::endl;
117 utils::print_vector("x", x);
118 #endif
119
120 /* Add equality constraints to the working set A */
121 10 iq = 0;
122
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 9 times.
 14 for (i = 0; i < me; i++) {
123
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 np = CE.col(i);
124
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 compute_d(d, J, np);
125
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 update_z(z, J, d, iq);
126
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 update_r(R, r, d, iq);
127 #ifdef EIQGUADPROG_TRACE_SOLVER
128 utils::print_matrix("R", R);
129 utils::print_vector("z", z);
130 utils::print_vector("r", r);
131 utils::print_vector("d", d);
132 #endif
133
134 /* compute full step length t2: i.e., the minimum step in primal space s.t.
135 the contraint becomes feasible */
136 5 t2 = 0.0;
137
3/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 1 times.
 10 if (std::abs(z.dot(z)) >
138 5 std::numeric_limits<double>::epsilon()) // i.e. z != 0
139
3/6
✓ 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.
 4 t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
140
141
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 x += t2 * z;
142
143 /* set u = u+ */
144
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 u(iq) = t2;
145
4/8
✓ 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.
 5 u.head(iq) -= t2 * r.head(iq);
146
147 /* compute the new solution value */
148
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 f_value += 0.5 * (t2 * t2) * z.dot(np);
149
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 A(i) = static_cast<VectorXi::Scalar>(-i - 1);
150
151
3/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 4 times.
 5 if (!add_constraint(R, J, d, iq, R_norm)) {
152 // FIXME: it should raise an error
153 // Equality constraints are linearly dependent
154 1 return f_value;
155 }
156 }
157
158 /* set iai = K \ A */
159
3/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
✓ Branch 4 taken 9 times.
 20 for (i = 0; i < mi; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
160
161 9 l1:
162 14 iter++;
163 #ifdef EIQGUADPROG_TRACE_SOLVER
164 utils::print_vector("x", x);
165 #endif
166 /* step 1: choose a violated constraint */
167
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 14 times.
 20 for (i = me; i < iq; i++) {
168
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 ip = A(i);
169
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 iai(ip) = -1;
170 }
171
172 /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
173 14 ss = 0.0;
174 14 psi = 0.0; /* this value will contain the sum of all infeasibilities */
175 14 ip = 0; /* ip will be the index of the chosen violated constraint */
176
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 14 times.
 34 for (i = 0; i < mi; i++) {
177
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 iaexcl(i) = 1;
178
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 20 sum = CI.col(i).dot(x) + ci0(i);
179
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 s(i) = sum;
180 20 psi += std::min(0.0, sum);
181 }
182 #ifdef EIQGUADPROG_TRACE_SOLVER
183 utils::print_vector("s", s);
184 #endif
185
186 14 if (std::abs(psi) <= static_cast<double>(mi) *
187
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 7 times.
 14 std::numeric_limits<double>::epsilon() * c1 * c2 *
188 100.0) {
189 /* numerically there are not infeasibilities anymore */
190 7 q = iq;
191 7 return f_value;
192 }
193
194 /* save old values for u, x and A */
195
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 u_old.head(iq) = u.head(iq);
196
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 A_old.head(iq) = A.head(iq);
197
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 x_old = x;
198
199 7 l2: /* Step 2: check for feasibility and determine a new S-pair */
200
2/2
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 7 times.
 20 for (i = 0; i < mi; i++) {
201
9/14
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✓ Branch 4 taken 6 times.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 7 times.
✓ Branch 16 taken 6 times.
 13 if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
202
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ss = s(i);
203 7 ip = i;
204 }
205 }
206
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
 7 if (ss >= 0.0) {
207 ✗ q = iq;
208 ✗ return f_value;
209 }
210
211 /* set np = n(ip) */
212
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 np = CI.col(ip);
213 /* set u = (u 0)^T */
214
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 u(iq) = 0.0;
215 /* add ip to the active set A */
216
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 A(iq) = static_cast<VectorXi::Scalar>(ip);
217
218 #ifdef EIQGUADPROG_TRACE_SOLVER
219 std::cerr << "Trying with constraint " << ip << std::endl;
220 utils::print_vector("np", np);
221 #endif
222
223 7 l2a: /* Step 2a: determine step direction */
224 /* compute z = H np: the step direction in the primal space (through J, see
225 * the paper) */
226
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 compute_d(d, J, np);
227
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 update_z(z, J, d, iq);
228 /* compute N* np (if q > 0): the negative of the step direction in the dual
229 * space */
230
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 update_r(R, r, d, iq);
231 #ifdef EIQGUADPROG_TRACE_SOLVER
232 std::cerr << "Step direction z" << std::endl;
233 utils::print_vector("z", z);
234 utils::print_vector("r", r);
235 utils::print_vector("u", u);
236 utils::print_vector("d", d);
237 utils::print_vector("A", A);
238 #endif
239
240 /* Step 2b: compute step length */
241 7 l = 0;
242 /* Compute t1: partial step length (maximum step in dual space without
243 * violating dual feasibility */
244 7 t1 = inf; /* +inf */
245 /* find the index l s.t. it reaches the minimum of u+(x) / r */
246
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 7 times.
 10 for (k = me; k < iq; k++) {
247 double tmp;
248
3/12
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 3 times.
 3 if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
249 ✗ t1 = tmp;
250 ✗ l = A(k);
251 }
252 }
253 /* Compute t2: full step length (minimum step in primal space such that the
254 * constraint ip becomes feasible */
255
3/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 2 times.
 14 if (std::abs(z.dot(z)) >
256 7 std::numeric_limits<double>::epsilon()) // i.e. z != 0
257
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 t2 = -s(ip) / z.dot(np);
258 else
259 2 t2 = inf; /* +inf */
260
261 /* the step is chosen as the minimum of t1 and t2 */
262 7 t = std::min(t1, t2);
263 #ifdef EIQGUADPROG_TRACE_SOLVER
264 std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
265 << ") ";
266 #endif
267
268 /* Step 2c: determine new S-pair and take step: */
269
270 /* case (i): no step in primal or dual space */
271
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 5 times.
 7 if (t >= inf) {
272 /* QPP is infeasible */
273 // FIXME: unbounded to raise
274 2 q = iq;
275 2 return inf;
276 }
277 /* case (ii): step in dual space */
278
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (t2 >= inf) {
279 /* set u = u + t * [-r 1) and drop constraint l from the active set A */
280 ✗ u.head(iq) -= t * r.head(iq);
281 ✗ u(iq) += t;
282 ✗ iai(l) = static_cast<VectorXi::Scalar>(l);
283 ✗ delete_constraint(R, J, A, u, p, iq, l);
284 #ifdef EIQGUADPROG_TRACE_SOLVER
285 std::cerr << " in dual space: " << f_value << std::endl;
286 utils::print_vector("x", x);
287 utils::print_vector("z", z);
288 utils::print_vector("A", A);
289 #endif
290 ✗ goto l2a;
291 }
292
293 /* case (iii): step in primal and dual space */
294
295
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 x += t * z;
296 /* update the solution value */
297
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 f_value += t * z.dot(np) * (0.5 * t + u(iq));
298
299
4/8
✓ 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.
 5 u.head(iq) -= t * r.head(iq);
300
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 u(iq) += t;
301 #ifdef EIQGUADPROG_TRACE_SOLVER
302 std::cerr << " in both spaces: " << f_value << std::endl;
303 utils::print_vector("x", x);
304 utils::print_vector("u", u);
305 utils::print_vector("r", r);
306 utils::print_vector("A", A);
307 #endif
308
309
1/2
✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
 5 if (t == t2) {
310 #ifdef EIQGUADPROG_TRACE_SOLVER
311 std::cerr << "Full step has taken " << t << std::endl;
312 utils::print_vector("x", x);
313 #endif
314 /* full step has taken */
315 /* add constraint ip to the active set*/
316
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
 5 if (!add_constraint(R, J, d, iq, R_norm)) {
317 ✗ iaexcl(ip) = 0;
318 ✗ delete_constraint(R, J, A, u, p, iq, ip);
319 #ifdef EIQGUADPROG_TRACE_SOLVER
320 utils::print_matrix("R", R);
321 utils::print_vector("A", A);
322 #endif
323 ✗ for (i = 0; i < m; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
324 ✗ for (i = 0; i < iq; i++) {
325 ✗ A(i) = A_old(i);
326 ✗ iai(A(i)) = -1;
327 ✗ u(i) = u_old(i);
328 }
329 ✗ x = x_old;
330 ✗ goto l2; /* go to step 2 */
331 } else
332
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 iai(ip) = -1;
333 #ifdef EIQGUADPROG_TRACE_SOLVER
334 utils::print_matrix("R", R);
335 utils::print_vector("A", A);
336 #endif
337 5 goto l1;
338 }
339
340 /* a patial step has taken */
341 #ifdef EIQGUADPROG_TRACE_SOLVER
342 std::cerr << "Partial step has taken " << t << std::endl;
343 utils::print_vector("x", x);
344 #endif
345 /* drop constraint l */
346 ✗ iai(l) = static_cast<VectorXi::Scalar>(l);
347 ✗ delete_constraint(R, J, A, u, p, iq, l);
348 #ifdef EIQGUADPROG_TRACE_SOLVER
349 utils::print_matrix("R", R);
350 utils::print_vector("A", A);
351 #endif
352
353 ✗ s(ip) = CI.col(ip).dot(x) + ci0(ip);
354
355 #ifdef EIQGUADPROG_TRACE_SOLVER
356 utils::print_vector("s", s);
357 #endif
358 ✗ goto l2a;
359 10 }
360
361 10 bool add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d, size_t &iq,
362 double &R_norm) {
363 10 size_t n = J.rows();
364 #ifdef EIQGUADPROG_TRACE_SOLVER
365 std::cerr << "Add constraint " << iq << '/';
366 #endif
367 size_t j, k;
368 double cc, ss, h, t1, t2, xny;
369
370 /* we have to find the Givens rotation which will reduce the element
371 d(j) to zero.
372 if it is already zero we don't have to do anything, except of
373 decreasing j */
374
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 10 times.
 16 for (j = n - 1; j >= iq + 1; j--) {
375 /* The Givens rotation is done with the matrix (cc cs, cs -cc).
376 If cc is one, then element (j) of d is zero compared with element
377 (j - 1). Hence we don't have to do anything.
378 If cc is zero, then we just have to switch column (j) and column (j - 1)
379 of J. Since we only switch columns in J, we have to be careful how we
380 update d depending on the sign of gs.
381 Otherwise we have to apply the Givens rotation to these columns.
382 The i - 1 element of d has to be updated to h. */
383 6 cc = d(j - 1);
384 6 ss = d(j);
385 6 h = utils::distance(cc, ss);
386
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 6 if (h == 0.0) continue;
387 6 d(j) = 0.0;
388 6 ss = ss / h;
389 6 cc = cc / h;
390
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 6 if (cc < 0.0) {
391 ✗ cc = -cc;
392 ✗ ss = -ss;
393 ✗ d(j - 1) = -h;
394 } else
395 6 d(j - 1) = h;
396 6 xny = ss / (1.0 + cc);
397
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 6 times.
 18 for (k = 0; k < n; k++) {
398 12 t1 = J(k, j - 1);
399 12 t2 = J(k, j);
400 12 J(k, j - 1) = t1 * cc + t2 * ss;
401 12 J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
402 }
403 }
404 /* update the number of constraints added*/
405 10 iq++;
406 /* To update R we have to put the iq components of the d vector
407 into column iq - 1 of R
408 */
409
3/6
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 10 R.col(iq - 1).head(iq) = d.head(iq);
410 #ifdef EIQGUADPROG_TRACE_SOLVER
411 std::cerr << iq << std::endl;
412 #endif
413
414
2/2
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 9 times.
 10 if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
415 // problem degenerate
416 1 return false;
417 9 R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
418 9 return true;
419 }
420
421 ✗ void delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A, VectorXd &u,
422 size_t p, size_t &iq, size_t l) {
423 ✗ size_t n = R.rows();
424 #ifdef EIQGUADPROG_TRACE_SOLVER
425 std::cerr << "Delete constraint " << l << ' ' << iq;
426 #endif
427 ✗ size_t i, j, k, qq = 0;
428 double cc, ss, h, xny, t1, t2;
429
430 /* Find the index qq for active constraint l to be removed */
431 ✗ for (i = p; i < iq; i++)
432 ✗ if (static_cast<size_t>(A(i)) == l) {
433 ✗ qq = i;
434 ✗ break;
435 }
436
437 /* remove the constraint from the active set and the duals */
438 ✗ for (i = qq; i < iq - 1; i++) {
439 ✗ A(i) = A(i + 1);
440 ✗ u(i) = u(i + 1);
441 ✗ R.col(i) = R.col(i + 1);
442 }
443
444 ✗ A(iq - 1) = A(iq);
445 ✗ u(iq - 1) = u(iq);
446 ✗ A(iq) = 0;
447 ✗ u(iq) = 0.0;
448 ✗ for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
449 /* constraint has been fully removed */
450 ✗ iq--;
451 #ifdef EIQGUADPROG_TRACE_SOLVER
452 std::cerr << '/' << iq << std::endl;
453 #endif
454
455 ✗ if (iq == 0) return;
456
457 ✗ for (j = qq; j < iq; j++) {
458 ✗ cc = R(j, j);
459 ✗ ss = R(j + 1, j);
460 ✗ h = utils::distance(cc, ss);
461 ✗ if (h == 0.0) continue;
462 ✗ cc = cc / h;
463 ✗ ss = ss / h;
464 ✗ R(j + 1, j) = 0.0;
465 ✗ if (cc < 0.0) {
466 ✗ R(j, j) = -h;
467 ✗ cc = -cc;
468 ✗ ss = -ss;
469 } else
470 ✗ R(j, j) = h;
471
472 ✗ xny = ss / (1.0 + cc);
473 ✗ for (k = j + 1; k < iq; k++) {
474 ✗ t1 = R(j, k);
475 ✗ t2 = R(j + 1, k);
476 ✗ R(j, k) = t1 * cc + t2 * ss;
477 ✗ R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
478 }
479 ✗ for (k = 0; k < n; k++) {
480 ✗ t1 = J(k, j);
481 ✗ t2 = J(k, j + 1);
482 ✗ J(k, j) = t1 * cc + t2 * ss;
483 ✗ J(k, j + 1) = xny * (J(k, j) + t1) - t2;
484 }
485 }
486 }
487
488 } // namespace solvers
489 } // namespace eiquadprog
490