GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/eiquadprog-fast.cpp Lines: 151 221 68.3 %
Date: 2021-03-10 23:02:29 Branches: 165 390 42.3 %

Line Branch Exec Source
1
#include <iostream>
2
3
#include "eiquadprog/eiquadprog-fast.hpp"
4
5
namespace eiquadprog {
6
namespace solvers {
7
8







12
EiquadprogFast::EiquadprogFast() {
9
12
  m_maxIter = DEFAULT_MAX_ITER;
10
12
  q = 0;  // size of the active set A (containing the indices
11
  // of the active constraints)
12
12
  is_inverse_provided_ = false;
13
12
  m_nVars = 0;
14
12
  m_nEqCon = 0;
15
12
  m_nIneqCon = 0;
16
12
}
17
18
12
EiquadprogFast::~EiquadprogFast() {}
19
20
12
void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
21
12
  m_nVars = nVars;
22
12
  m_nEqCon = nEqCon;
23
12
  m_nIneqCon = nIneqCon;
24
12
  m_J.setZero(nVars, nVars);
25
12
  chol_.compute(m_J);
26
12
  R.resize(nVars, nVars);
27
12
  s.resize(nIneqCon);
28
12
  r.resize(nIneqCon + nEqCon);
29
12
  u.resize(nIneqCon + nEqCon);
30
12
  z.resize(nVars);
31
12
  d.resize(nVars);
32
12
  np.resize(nVars);
33
12
  A.resize(nIneqCon + nEqCon);
34
12
  iai.resize(nIneqCon);
35
12
  iaexcl.resize(nIneqCon);
36
12
  x_old.resize(nVars);
37
12
  u_old.resize(nIneqCon + nEqCon);
38
12
  A_old.resize(nIneqCon + nEqCon);
39
40
#ifdef OPTIMIZE_ADD_CONSTRAINT
41
  T1.resize(nVars);
42
#endif
43
12
}
44
45
10
bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
46
10
  size_t nVars = J.rows();
47
#ifdef TRACE_SOLVER
48
  std::cerr << "Add constraint " << iq << '/';
49
#endif
50
  size_t j, k;
51
  double cc, ss, h, t1, t2, xny;
52
53
#ifdef OPTIMIZE_ADD_CONSTRAINT
54
  Eigen::Vector2d cc_ss;
55
#endif
56
57
  /* we have to find the Givens rotation which will reduce the element
58
           d(j) to zero.
59
           if it is already zero we don't have to do anything, except of
60
           decreasing j */
61
16
  for (j = d.size() - 1; j >= iq + 1; j--) {
62
    /* The Givens rotation is done with the matrix (cc cs, cs -cc).
63
                    If cc is one, then element (j) of d is zero compared with element
64
                    (j - 1). Hence we don't have to do anything.
65
                    If cc is zero, then we just have to switch column (j) and column (j - 1)
66
                    of J. Since we only switch columns in J, we have to be careful how we
67
                    update d depending on the sign of gs.
68
                    Otherwise we have to apply the Givens rotation to these columns.
69
                    The i - 1 element of d has to be updated to h. */
70
6
    cc = d(j - 1);
71
6
    ss = d(j);
72
6
    h = distance(cc, ss);
73
6
    if (h == 0.0) continue;
74
6
    d(j) = 0.0;
75
6
    ss = ss / h;
76
6
    cc = cc / h;
77
6
    if (cc < 0.0) {
78
      cc = -cc;
79
      ss = -ss;
80
      d(j - 1) = -h;
81
    } else
82
6
      d(j - 1) = h;
83
6
    xny = ss / (1.0 + cc);
84
85
// #define OPTIMIZE_ADD_CONSTRAINT
86
#ifdef OPTIMIZE_ADD_CONSTRAINT  // the optimized code is actually slower than the original
87
    T1 = J.col(j - 1);
88
    cc_ss(0) = cc;
89
    cc_ss(1) = ss;
90
    J.col(j - 1).noalias() = J.middleCols<2>(j - 1) * cc_ss;
91
    J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
92
#else
93
    // J.col(j-1) = J[:,j-1:j] * [cc; ss]
94
18
    for (k = 0; k < nVars; k++) {
95
12
      t1 = J(k, j - 1);
96
12
      t2 = J(k, j);
97
12
      J(k, j - 1) = t1 * cc + t2 * ss;
98
12
      J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
99
    }
100
#endif
101
  }
102
  /* update the number of constraints added*/
103
10
  iq++;
104
  /* To update R we have to put the iq components of the d vector
105
into column iq - 1 of R
106
*/
107

10
  R.col(iq - 1).head(iq) = d.head(iq);
108
#ifdef TRACE_SOLVER
109
  std::cerr << iq << std::endl;
110
#endif
111
112
10
  if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
113
    // problem degenerate
114
1
    return false;
115
9
  R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
116
9
  return true;
117
}
118
119
void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
120
                                       size_t l) {
121
  size_t nVars = R.rows();
122
#ifdef TRACE_SOLVER
123
  std::cerr << "Delete constraint " << l << ' ' << iq;
124
#endif
125
  size_t i, j, k;
126
  size_t qq = 0;
127
  double cc, ss, h, xny, t1, t2;
128
129
  /* Find the index qq for active constraint l to be removed */
130
  for (i = nEqCon; i < iq; i++)
131
    if (A(i) == static_cast<VectorXi::Scalar>(l)) {
132
      qq = i;
133
      break;
134
    }
135
136
  /* remove the constraint from the active set and the duals */
137
  for (i = qq; i < iq - 1; i++) {
138
    A(i) = A(i + 1);
139
    u(i) = u(i + 1);
140
    R.col(i) = R.col(i + 1);
141
  }
142
143
  A(iq - 1) = A(iq);
144
  u(iq - 1) = u(iq);
145
  A(iq) = 0;
146
  u(iq) = 0.0;
147
  for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
148
  /* constraint has been fully removed */
149
  iq--;
150
#ifdef TRACE_SOLVER
151
  std::cerr << '/' << iq << std::endl;
152
#endif
153
154
  if (iq == 0) return;
155
156
  for (j = qq; j < iq; j++) {
157
    cc = R(j, j);
158
    ss = R(j + 1, j);
159
    h = distance(cc, ss);
160
    if (h == 0.0) continue;
161
    cc = cc / h;
162
    ss = ss / h;
163
    R(j + 1, j) = 0.0;
164
    if (cc < 0.0) {
165
      R(j, j) = -h;
166
      cc = -cc;
167
      ss = -ss;
168
    } else
169
      R(j, j) = h;
170
171
    xny = ss / (1.0 + cc);
172
    for (k = j + 1; k < iq; k++) {
173
      t1 = R(j, k);
174
      t2 = R(j + 1, k);
175
      R(j, k) = t1 * cc + t2 * ss;
176
      R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
177
    }
178
    for (k = 0; k < nVars; k++) {
179
      t1 = J(k, j);
180
      t2 = J(k, j + 1);
181
      J(k, j) = t1 * cc + t2 * ss;
182
      J(k, j + 1) = xny * (J(k, j) + t1) - t2;
183
    }
184
  }
185
}
186
187
12
EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
188
                                                     const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
189
                                                     VectorXd& x) {
190
12
  const size_t nVars = g0.size();
191
12
  const size_t nEqCon = ce0.size();
192
12
  const size_t nIneqCon = ci0.size();
193
194


12
  if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon) reset(nVars, nEqCon, nIneqCon);
195
196


12
  assert(static_cast<size_t>(Hess.rows()) == m_nVars && static_cast<size_t>(Hess.cols()) == m_nVars);
197

12
  assert(static_cast<size_t>(g0.size()) == m_nVars);
198


12
  assert(static_cast<size_t>(CE.rows()) == m_nEqCon && static_cast<size_t>(CE.cols()) == m_nVars);
199

12
  assert(static_cast<size_t>(ce0.size()) == m_nEqCon);
200


12
  assert(static_cast<size_t>(CI.rows()) == m_nIneqCon && static_cast<size_t>(CI.cols()) == m_nVars);
201

12
  assert(static_cast<size_t>(ci0.size()) == m_nIneqCon);
202
203
  size_t i, k, l;  // indices
204
  size_t ip;       // index of the chosen violated constraint
205
  size_t iq;       // current number of active constraints
206
  double psi;      // current sum of constraint violations
207
  double c1;       // Hessian trace
208
  double c2;       // Hessian Cholesky factor trace
209
  double ss;       // largest constraint violation (negative for violation)
210
  double R_norm;   // norm of matrix R
211
12
  const double inf = std::numeric_limits<double>::infinity();
212
  double t, t1, t2;
213
  /* t is the step length, which is the minimum of the partial step length t1
214
   * and the full step length t2 */
215
216
12
  iter = 0;  // active-set iteration number
217
218
  /*
219
   * Preprocessing phase
220
   */
221
  /* compute the trace of the original matrix Hess */
222
12
  c1 = Hess.trace();
223
224
  /* decompose the matrix Hess in the form LL^T */
225
12
  if (!is_inverse_provided_) {
226
    START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
227
12
    chol_.compute(Hess);
228
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
229
  }
230
231
  /* initialize the matrix R */
232
12
  d.setZero(nVars);
233
12
  R.setZero(nVars, nVars);
234
12
  R_norm = 1.0;
235
236
  /* compute the inverse of the factorized matrix Hess^-1,
237
     this is the initial value for H */
238
  // m_J = L^-T
239
12
  if (!is_inverse_provided_) {
240
    START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
241
12
    m_J.setIdentity(nVars, nVars);
242
#ifdef OPTIMIZE_HESSIAN_INVERSE
243

12
    chol_.matrixU().solveInPlace(m_J);
244
#else
245
    m_J = chol_.matrixU().solve(m_J);
246
#endif
247
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
248
  }
249
250
12
  c2 = m_J.trace();
251
#ifdef TRACE_SOLVER
252
  print_matrix("m_J", m_J, nVars);
253
#endif
254
255
  /* c1 * c2 is an estimate for cond(Hess) */
256
257
  /*
258
   * Find the unconstrained minimizer of the quadratic
259
   * form 0.5 * x Hess x + g0 x
260
   * this is a feasible point in the dual space
261
   * x = Hess^-1 * g0
262
   */
263
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
264
12
  if (is_inverse_provided_) {
265
    x = m_J * (m_J.transpose() * g0);
266
  } else {
267
#ifdef OPTIMIZE_UNCONSTR_MINIM
268

12
    x = -g0;
269
12
    chol_.solveInPlace(x);
270
  }
271
#else
272
    x = chol_.solve(g0);
273
  }
274
  x = -x;
275
#endif
276
  /* and compute the current solution value */
277
12
  f_value = 0.5 * g0.dot(x);
278
#ifdef TRACE_SOLVER
279
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
280
  print_vector("x", x, nVars);
281
#endif
282
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
283
284
  /* Add equality constraints to the working set A */
285
286
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
287
12
  iq = 0;
288
16
  for (i = 0; i < nEqCon; i++) {
289
    START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
290

5
    np = CE.row(i);
291
5
    compute_d(d, m_J, np);
292
5
    update_z(z, m_J, d, iq);
293
5
    update_r(R, r, d, iq);
294
295
#ifdef TRACE_SOLVER
296
    print_matrix("R", R, iq);
297
    print_vector("z", z, nVars);
298
    print_vector("r", r, iq);
299
    print_vector("d", d, nVars);
300
#endif
301
302
    /* compute full step length t2: i.e.,
303
       the minimum step in primal space s.t. the contraint
304
       becomes feasible */
305
5
    t2 = 0.0;
306

5
    if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
307
      // i.e. z != 0
308

4
      t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
309
310

5
    x += t2 * z;
311
312
    /* set u = u+ */
313
5
    u(iq) = t2;
314


5
    u.head(iq) -= t2 * r.head(iq);
315
316
    /* compute the new solution value */
317
5
    f_value += 0.5 * (t2 * t2) * z.dot(np);
318
5
    A(i) = static_cast<VectorXi::Scalar>(-i - 1);
319
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
320
321
    START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
322

5
    if (!add_constraint(R, m_J, d, iq, R_norm)) {
323
      // Equality constraints are linearly dependent
324
1
      return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
325
    }
326
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
327
  }
328
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
329
330
  /* set iai = K \ A */
331

11
  for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
332
333
#ifdef USE_WARM_START
334
  //      DEBUG_STREAM("Gonna warm start using previous active
335
  // set:\n"<<A.transpose()<<"\n")
336
  for (i = nEqCon; i < q; i++) {
337
    iai(i - nEqCon) = -1;
338
    ip = A(i);
339
    np = CI.row(ip);
340
    compute_d(d, m_J, np);
341
    update_z(z, m_J, d, iq);
342
    update_r(R, r, d, iq);
343
344
    /* compute full step length t2: i.e.,
345
       the minimum step in primal space s.t. the contraint
346
       becomes feasible */
347
    t2 = 0.0;
348
    if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())  // i.e. z != 0
349
      t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
350
    else
351
      DEBUG_STREAM("[WARM START] z=0\n")
352
353
    x += t2 * z;
354
355
    /* set u = u+ */
356
    u(iq) = t2;
357
    u.head(iq) -= t2 * r.head(iq);
358
359
    /* compute the new solution value */
360
    f_value += 0.5 * (t2 * t2) * z.dot(np);
361
362
    if (!add_constraint(R, m_J, d, iq, R_norm)) {
363
      // constraints are linearly dependent
364
      std::cerr << "[WARM START] Constraints are linearly dependent\n";
365
      return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
366
    }
367
  }
368
#else
369
370
#endif
371
372
11
l1:
373
16
  iter++;
374
16
  if (iter >= m_maxIter) {
375
    q = iq;
376
    return EIQUADPROG_FAST_MAX_ITER_REACHED;
377
  }
378
379
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
380
381
#ifdef TRACE_SOLVER
382
  print_vector("x", x, nVars);
383
#endif
384
  /* step 1: choose a violated constraint */
385
22
  for (i = nEqCon; i < iq; i++) {
386
6
    ip = A(i);
387
6
    iai(ip) = -1;
388
  }
389
390
  /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
391
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
392
16
  ss = 0.0;
393
16
  ip = 0; /* ip will be the index of the chosen violated constraint */
394
395
#ifdef OPTIMIZE_STEP_1_2
396
16
  s = ci0;
397

16
  s.noalias() += CI * x;
398
16
  iaexcl.setOnes();
399

16
  psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
400
#else
401
  psi = 0.0; /* this value will contain the sum of all infeasibilities */
402
  for (i = 0; i < nIneqCon; i++) {
403
    iaexcl(i) = 1;
404
    s(i) = CI.row(i).dot(x) + ci0(i);
405
    psi += std::min(0.0, s(i));
406
  }
407
#endif
408
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
409
#ifdef TRACE_SOLVER
410
  print_vector("s", s, nIneqCon);
411
#endif
412
413
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
414
415
16
  if (std::abs(psi) <= static_cast<double>(nIneqCon) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
416
    /* numerically there are not infeasibilities anymore */
417
9
    q = iq;
418
    //        DEBUG_STREAM("Optimal active
419
    // set:\n"<<A.head(iq).transpose()<<"\n\n")
420
9
    return EIQUADPROG_FAST_OPTIMAL;
421
  }
422
423
  /* save old values for u, x and A */
424

7
  u_old.head(iq) = u.head(iq);
425

7
  A_old.head(iq) = A.head(iq);
426
7
  x_old = x;
427
428
7
l2: /* Step 2: check for feasibility and determine a new S-pair */
429
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
430
  // find constraint with highest violation
431
  // (what about normalizing constraints?)
432
20
  for (i = 0; i < nIneqCon; i++) {
433



13
    if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
434
7
      ss = s(i);
435
7
      ip = i;
436
    }
437
  }
438
7
  if (ss >= 0.0) {
439
    q = iq;
440
    //        DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
441
    return EIQUADPROG_FAST_OPTIMAL;
442
  }
443
444
  /* set np = n(ip) */
445

7
  np = CI.row(ip);
446
  /* set u = (u 0)^T */
447
7
  u(iq) = 0.0;
448
  /* add ip to the active set A */
449
7
  A(iq) = static_cast<VectorXi::Scalar>(ip);
450
451
  //      DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
452
453
#ifdef TRACE_SOLVER
454
  std::cerr << "Trying with constraint " << ip << std::endl;
455
  print_vector("np", np, nVars);
456
#endif
457
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
458
459
7
l2a: /* Step 2a: determine step direction */
460
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
461
  /* compute z = H np: the step direction in the primal space
462
     (through m_J, see the paper) */
463
7
  compute_d(d, m_J, np);
464
  //    update_z(z, m_J, d, iq);
465
7
  if (iq >= nVars) {
466
    //      throw std::runtime_error("iq >= m_J.cols()");
467
1
    z.setZero();
468
  } else {
469
6
    update_z(z, m_J, d, iq);
470
  }
471
  /* compute N* np (if q > 0): the negative of the
472
     step direction in the dual space */
473
7
  update_r(R, r, d, iq);
474
#ifdef TRACE_SOLVER
475
  std::cerr << "Step direction z" << std::endl;
476
  print_vector("z", z, nVars);
477
  print_vector("r", r, iq + 1);
478
  print_vector("u", u, iq + 1);
479
  print_vector("d", d, nVars);
480
  print_vector("A", A, iq + 1);
481
#endif
482
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
483
484
  /* Step 2b: compute step length */
485
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
486
7
  l = 0;
487
  /* Compute t1: partial step length (maximum step in dual
488
     space without violating dual feasibility */
489
7
  t1 = inf; /* +inf */
490
  /* find the index l s.t. it reaches the minimum of u+(x) / r */
491
  // l: index of constraint to drop (maybe)
492
10
  for (k = nEqCon; k < iq; k++) {
493
    double tmp;
494



3
    if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
495
      t1 = tmp;
496
      l = A(k);
497
    }
498
  }
499
  /* Compute t2: full step length (minimum step in primal
500
     space such that the constraint ip becomes feasible */
501

7
  if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
502
    // i.e. z != 0
503

5
    t2 = -s(ip) / z.dot(np);
504
  else
505
2
    t2 = inf; /* +inf */
506
507
  /* the step is chosen as the minimum of t1 and t2 */
508
7
  t = std::min(t1, t2);
509
#ifdef TRACE_SOLVER
510
  std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
511
#endif
512
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
513
514
  /* Step 2c: determine new S-pair and take step: */
515
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
516
  /* case (i): no step in primal or dual space */
517
7
  if (t >= inf) {
518
    /* QPP is infeasible */
519
2
    q = iq;
520
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
521
2
    return EIQUADPROG_FAST_UNBOUNDED;
522
  }
523
  /* case (ii): step in dual space */
524
5
  if (t2 >= inf) {
525
    /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
526
    u.head(iq) -= t * r.head(iq);
527
    u(iq) += t;
528
    iai(l) = static_cast<VectorXi::Scalar>(l);
529
    delete_constraint(R, m_J, A, u, nEqCon, iq, l);
530
#ifdef TRACE_SOLVER
531
    std::cerr << " in dual space: " << f_value << std::endl;
532
    print_vector("x", x, nVars);
533
    print_vector("z", z, nVars);
534
    print_vector("A", A, iq + 1);
535
#endif
536
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
537
    goto l2a;
538
  }
539
540
  /* case (iii): step in primal and dual space */
541

5
  x += t * z;
542
  /* update the solution value */
543

5
  f_value += t * z.dot(np) * (0.5 * t + u(iq));
544
545


5
  u.head(iq) -= t * r.head(iq);
546
5
  u(iq) += t;
547
548
#ifdef TRACE_SOLVER
549
  std::cerr << " in both spaces: " << f_value << std::endl;
550
  print_vector("x", x, nVars);
551
  print_vector("u", u, iq + 1);
552
  print_vector("r", r, iq + 1);
553
  print_vector("A", A, iq + 1);
554
#endif
555
556
5
  if (t == t2) {
557
#ifdef TRACE_SOLVER
558
    std::cerr << "Full step has taken " << t << std::endl;
559
    print_vector("x", x, nVars);
560
#endif
561
    /* full step has taken */
562
    /* add constraint ip to the active set*/
563

5
    if (!add_constraint(R, m_J, d, iq, R_norm)) {
564
      iaexcl(ip) = 0;
565
      delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
566
#ifdef TRACE_SOLVER
567
      print_matrix("R", R, nVars);
568
      print_vector("A", A, iq);
569
#endif
570
      for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
571
      for (i = 0; i < iq; i++) {
572
        A(i) = A_old(i);
573
        iai(A(i)) = -1;
574
        u(i) = u_old(i);
575
      }
576
      x = x_old;
577
      STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
578
      goto l2; /* go to step 2 */
579
    } else
580
5
      iai(ip) = -1;
581
#ifdef TRACE_SOLVER
582
    print_matrix("R", R, nVars);
583
    print_vector("A", A, iq);
584
#endif
585
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
586
5
    goto l1;
587
  }
588
589
  /* a partial step has been taken => drop constraint l */
590
  iai(l) = static_cast<VectorXi::Scalar>(l);
591
  delete_constraint(R, m_J, A, u, nEqCon, iq, l);
592
  s(ip) = CI.row(ip).dot(x) + ci0(ip);
593
594
#ifdef TRACE_SOLVER
595
  std::cerr << "Partial step has taken " << t << std::endl;
596
  print_vector("x", x, nVars);
597
  print_matrix("R", R, nVars);
598
  print_vector("A", A, iq);
599
  print_vector("s", s, nIneqCon);
600
#endif
601
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
602
603
  goto l2a;
604
}
605
606
} /* namespace solvers */
607

12
} /* namespace eiquadprog */