GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/eiquadprog/eiquadprog-rt.hxx Lines: 116 186 62.4 %
Date: 2021-03-10 23:02:29 Branches: 548 2212 24.8 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2017 CNRS
3
//
4
// This file is part of eiquadprog.
5
//
6
// eiquadprog is free software: you can redistribute it and/or modify
7
// it under the terms of the GNU Lesser General Public License as published by
8
// the Free Software Foundation, either version 3 of the License, or
9
//(at your option) any later version.
10
11
// eiquadprog is distributed in the hope that it will be useful,
12
// but WITHOUT ANY WARRANTY; without even the implied warranty of
13
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14
// GNU Lesser General Public License for more details.
15
16
// You should have received a copy of the GNU Lesser General Public License
17
// along with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
18
19
#ifndef __eiquadprog_rt_hxx__
20
#define __eiquadprog_rt_hxx__
21
22
#include "eiquadprog/eiquadprog-utils.hxx"
23
24
namespace eiquadprog {
25
26
namespace solvers {
27
28
template <int nVars, int nEqCon, int nIneqCon>
29
12
RtEiquadprog<nVars, nEqCon, nIneqCon>::RtEiquadprog() : solver_return_(std::numeric_limits<double>::infinity()) {
30
12
  m_maxIter = DEFAULT_MAX_ITER;
31
12
  q = 0;  // size of the active set A
32
  // (containing the indices of the active constraints)
33
12
  is_inverse_provided_ = false;
34
12
}
35
36
template <int nVars, int nEqCon, int nIneqCon>
37
12
RtEiquadprog<nVars, nEqCon, nIneqCon>::~RtEiquadprog() {}
38
39
template <int nVars, int nEqCon, int nIneqCon>
40
11
bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
41
                                                           typename RtMatrixX<nVars, nVars>::d& J,
42
                                                           typename RtVectorX<nVars>::d& d, int& iq, double& R_norm) {
43
  //      int n=J.rows();
44
#ifdef TRACE_SOLVER
45
  std::cerr << "Add constraint " << iq << '/';
46
#endif
47
  int j, k;
48
  double cc, ss, h, t1, t2, xny;
49
50
#ifdef OPTIMIZE_ADD_CONSTRAINT
51
  Eigen::Vector2d cc_ss;
52
#endif
53
54
  /* we have to find the Givens rotation which will reduce the element
55
           d(j) to zero.
56
           if it is already zero we don't have to do anything, except of
57
           decreasing j */
58



18
  for (j = nVars - 1; j >= iq + 1; j--) {
59
    /* The Givens rotation is done with the matrix (cc cs, cs -cc).
60
                    If cc is one, then element (j) of d is zero compared with element
61
                    (j - 1). Hence we don't have to do anything.
62
                    If cc is zero, then we just have to switch column (j) and column (j - 1)
63
                    of J. Since we only switch columns in J, we have to be careful how we
64
                    update d depending on the sign of gs.
65
                    Otherwise we have to apply the Givens rotation to these columns.
66
                    The i - 1 element of d has to be updated to h. */
67
7
    cc = d(j - 1);
68
7
    ss = d(j);
69
7
    h = distance(cc, ss);
70



7
    if (h == 0.0) continue;
71
7
    d(j) = 0.0;
72
7
    ss = ss / h;
73
7
    cc = cc / h;
74



7
    if (cc < 0.0) {
75
      cc = -cc;
76
      ss = -ss;
77
      d(j - 1) = -h;
78
    } else
79
7
      d(j - 1) = h;
80
7
    xny = ss / (1.0 + cc);
81
82
// #define OPTIMIZE_ADD_CONSTRAINT
83
#ifdef OPTIMIZE_ADD_CONSTRAINT  // the optimized code is actually slower than the original
84
    T1 = J.col(j - 1);
85
    cc_ss(0) = cc;
86
    cc_ss(1) = ss;
87
    J.col(j - 1).noalias() = J.template middleCols<2>(j - 1) * cc_ss;
88
    J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
89
#else
90
    // J.col(j-1) = J[:,j-1:j] * [cc; ss]
91



21
    for (k = 0; k < nVars; k++) {
92
14
      t1 = J(k, j - 1);
93
14
      t2 = J(k, j);
94
14
      J(k, j - 1) = t1 * cc + t2 * ss;
95
14
      J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
96
    }
97
#endif
98
  }
99
  /* update the number of constraints added*/
100
11
  iq++;
101
  /* To update R we have to put the iq components of the d vector
102
into column iq - 1 of R
103
*/
104










11
  R.col(iq - 1).head(iq) = d.head(iq);
105
#ifdef TRACE_SOLVER
106
  std::cerr << iq << std::endl;
107
#endif
108
109



11
  if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
110
    // problem degenerate
111
1
    return false;
112
10
  R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
113
10
  return true;
114
}
115
116
template <int nVars, int nEqCon, int nIneqCon>
117
void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX<nVars, nVars>::d& R,
118
                                                              typename RtMatrixX<nVars, nVars>::d& J,
119
                                                              typename RtVectorX<nIneqCon + nEqCon>::i& A,
120
                                                              typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq,
121
                                                              int l) {
122
  //      int n = J.rows();
123
#ifdef TRACE_SOLVER
124
  std::cerr << "Delete constraint " << l << ' ' << iq;
125
#endif
126
  int i, j, k;
127
  int qq = 0;
128
  double cc, ss, h, xny, t1, t2;
129
130
  /* Find the index qq for active constraint l to be removed */
131
  for (i = nEqCon; i < iq; i++)
132
    if (A(i) == l) {
133
      qq = i;
134
      break;
135
    }
136
137
  /* remove the constraint from the active set and the duals */
138
  for (i = qq; i < iq - 1; i++) {
139
    A(i) = A(i + 1);
140
    u(i) = u(i + 1);
141
    R.col(i) = R.col(i + 1);
142
  }
143
144
  A(iq - 1) = A(iq);
145
  u(iq - 1) = u(iq);
146
  A(iq) = 0;
147
  u(iq) = 0.0;
148
  for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
149
  /* constraint has been fully removed */
150
  iq--;
151
#ifdef TRACE_SOLVER
152
  std::cerr << '/' << iq << std::endl;
153
#endif
154
155
  if (iq == 0) return;
156
157
  for (j = qq; j < iq; j++) {
158
    cc = R(j, j);
159
    ss = R(j + 1, j);
160
    h = distance(cc, ss);
161
    if (h == 0.0) continue;
162
    cc = cc / h;
163
    ss = ss / h;
164
    R(j + 1, j) = 0.0;
165
    if (cc < 0.0) {
166
      R(j, j) = -h;
167
      cc = -cc;
168
      ss = -ss;
169
    } else
170
      R(j, j) = h;
171
172
    xny = ss / (1.0 + cc);
173
    for (k = j + 1; k < iq; k++) {
174
      t1 = R(j, k);
175
      t2 = R(j + 1, k);
176
      R(j, k) = t1 * cc + t2 * ss;
177
      R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
178
    }
179
    for (k = 0; k < nVars; k++) {
180
      t1 = J(k, j);
181
      t2 = J(k, j + 1);
182
      J(k, j) = t1 * cc + t2 * ss;
183
      J(k, j + 1) = xny * (J(k, j) + t1) - t2;
184
    }
185
  }
186
}
187
188
template <int nVars, int nEqCon, int nIneqCon>
189
12
RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
190
    const typename RtMatrixX<nVars, nVars>::d& Hess, const typename RtVectorX<nVars>::d& g0,
191
    const typename RtMatrixX<nEqCon, nVars>::d& CE, const typename RtVectorX<nEqCon>::d& ce0,
192
    const typename RtMatrixX<nIneqCon, nVars>::d& CI, const typename RtVectorX<nIneqCon>::d& ci0,
193
    typename RtVectorX<nVars>::d& x) {
194
  int i, k, l;    // indices
195
  int ip;         // index of the chosen violated constraint
196
  int iq;         // current number of active constraints
197
  double psi;     // current sum of constraint violations
198
  double c1;      // Hessian trace
199
  double c2;      // Hessian Chowlesky factor trace
200
  double ss;      // largest constraint violation (negative for violation)
201
  double R_norm;  // norm of matrix R
202
12
  const double inf = std::numeric_limits<double>::infinity();
203
  double t, t1, t2;
204
  /* t is the step length, which is the minimum of the partial step length t1
205
   * and the full step length t2 */
206
207
12
  iter = 0;  // active-set iteration number
208
209
  /*
210
   * Preprocessing phase
211
   */
212
  /* compute the trace of the original matrix Hess */
213



12
  c1 = Hess.trace();
214
215
  /* decompose the matrix Hess in the form LL^T */
216



12
  if (!is_inverse_provided_) {
217
    START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION);
218



12
    chol_.compute(Hess);
219
    STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION);
220
  }
221
222
  /* initialize the matrix R */
223



12
  d.setZero(nVars);
224



12
  R.setZero(nVars, nVars);
225
12
  R_norm = 1.0;
226
227
  /* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
228
  // m_J = L^-T
229



12
  if (!is_inverse_provided_) {
230
    START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_INVERSE);
231



12
    m_J.setIdentity(nVars, nVars);
232
#ifdef OPTIMIZE_HESSIAN_INVERSE
233







12
    chol_.matrixU().solveInPlace(m_J);
234
#else
235
    m_J = chol_.matrixU().solve(m_J);
236
#endif
237
    STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_CHOWLESKY_INVERSE);
238
  }
239
240



12
  c2 = m_J.trace();
241
#ifdef TRACE_SOLVER
242
  print_matrix("m_J", m_J, nVars);
243
#endif
244
245
  /* c1 * c2 is an estimate for cond(Hess) */
246
247
  /*
248
   * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 x
249
   * this is a feasible point in the dual space
250
   * x = Hess^-1 * g0
251
   */
252
  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM);
253



12
  if (is_inverse_provided_) {
254
    x = m_J * (m_J.transpose() * g0);
255
  } else {
256
#ifdef OPTIMIZE_UNCONSTR_MINIM
257







12
    x = -g0;
258



12
    chol_.solveInPlace(x);
259
  }
260
#else
261
    x = chol_.solve(g0);
262
  }
263
  x = -x;
264
#endif
265
  /* and compute the current solution value */
266



12
  f_value = 0.5 * g0.dot(x);
267
#ifdef TRACE_SOLVER
268
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
269
  print_vector("x", x, nVars);
270
#endif
271
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM);
272
273
  /* Add equality constraints to the working set A */
274
275
  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR);
276
12
  iq = 0;
277



17
  for (i = 0; i < nEqCon; i++) {
278
    START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1);
279
    // np = CE.row(i); // does not compile if nEqCon=0
280










6
    for (int tmp = 0; tmp < nVars; tmp++) np[tmp] = CE(i, tmp);
281



6
    compute_d(d, m_J, np);
282



6
    update_z(z, m_J, d, iq);
283



6
    update_r(R, r, d, iq);
284
285
#ifdef TRACE_SOLVER
286
    print_matrix("R", R, iq);
287
    print_vector("z", z, nVars);
288
    print_vector("r", r, iq);
289
    print_vector("d", d, nVars);
290
#endif
291
292
    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
293
    becomes feasible */
294
6
    t2 = 0.0;
295







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










5
      t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
297
298







6
    x += t2 * z;
299
300
    /* set u = u+ */
301



6
    u(iq) = t2;
302














6
    u.head(iq) -= t2 * r.head(iq);
303
304
    /* compute the new solution value */
305



6
    f_value += 0.5 * (t2 * t2) * z.dot(np);
306



6
    A(i) = -i - 1;
307
    STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1);
308
309
    START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2);
310







6
    if (!add_constraint(R, m_J, d, iq, R_norm)) {
311
      // Equality constraints are linearly dependent
312
1
      return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
313
    }
314
    STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2);
315
  }
316
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR);
317
318
  /* set iai = K \ A */
319







11
  for (i = 0; i < nIneqCon; i++) iai(i) = i;
320
321
#ifdef USE_WARM_START
322
  //      DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
323
  for (i = nEqCon; i < q; i++) {
324
    iai(i - nEqCon) = -1;
325
    ip = A(i);
326
    np = CI.row(ip);
327
    compute_d(d, m_J, np);
328
    update_z(z, m_J, d, iq);
329
    update_r(R, r, d, iq);
330
331
    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
332
    becomes feasible */
333
    t2 = 0.0;
334
    if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())  // i.e. z != 0
335
      t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
336
    else
337
      DEBUG_STREAM("[WARM START] z=0\n")
338
339
    x += t2 * z;
340
341
    /* set u = u+ */
342
    u(iq) = t2;
343
    u.head(iq) -= t2 * r.head(iq);
344
345
    /* compute the new solution value */
346
    f_value += 0.5 * (t2 * t2) * z.dot(np);
347
348
    if (!add_constraint(R, m_J, d, iq, R_norm)) {
349
      // constraints are linearly dependent
350
      std::cerr << "[WARM START] Constraints are linearly dependent\n";
351
      return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
352
    }
353
  }
354
#else
355
356
#endif
357
358
11
l1:
359
  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1);
360
16
  iter++;
361



16
  if (iter >= m_maxIter) {
362
    q = iq;
363
    return RT_EIQUADPROG_MAX_ITER_REACHED;
364
  }
365
366
#ifdef TRACE_SOLVER
367
  print_vector("x", x, nVars);
368
#endif
369
  /* step 1: choose a violated constraint */
370



22
  for (i = nEqCon; i < iq; i++) {
371



6
    ip = A(i);
372



6
    iai(ip) = -1;
373
  }
374
375
  /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
376
  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_2);
377
16
  ss = 0.0;
378
16
  ip = 0; /* ip will be the index of the chosen violated constraint */
379
380
#ifdef OPTIMIZE_STEP_1_2
381



16
  s = ci0;
382










16
  s.noalias() += CI * x;
383



16
  iaexcl.setOnes();
384










16
  psi = (s.cwiseMin(RtVectorX<nIneqCon>::d::Zero())).sum();
385
#else
386
  psi = 0.0; /* this value will contain the sum of all infeasibilities */
387
  for (i = 0; i < nIneqCon; i++) {
388
    iaexcl(i) = 1;
389
    s(i) = CI.row(i).dot(x) + ci0(i);
390
    psi += std::min(0.0, s(i));
391
  }
392
#endif
393
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_2);
394
#ifdef TRACE_SOLVER
395
  print_vector("s", s, nIneqCon);
396
#endif
397
398
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1);
399
400



16
  if (std::abs(psi) <= nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
401
    /* numerically there are not infeasibilities anymore */
402
9
    q = iq;
403
    //        DEBUG_STREAM("Optimal active set:\n"<<A.head(iq).transpose()<<"\n\n")
404
9
    return RT_EIQUADPROG_OPTIMAL;
405
  }
406
407
  /* save old values for u, x and A */
408










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










7
  A_old.head(iq) = A.head(iq);
410



7
  x_old = x;
411
412
7
l2: /* Step 2: check for feasibility and determine a new S-pair */
413
  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2);
414
  // find constraint with highest violation (what about normalizing constraints?)
415



20
  for (i = 0; i < nIneqCon; i++) {
416
























13
    if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
417



7
      ss = s(i);
418
7
      ip = i;
419
    }
420
  }
421



7
  if (ss >= 0.0) {
422
    q = iq;
423
    //        DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
424
    return RT_EIQUADPROG_OPTIMAL;
425
  }
426
427
  /* set np = n(ip) */
428
  // np = CI.row(ip); // does not compile if nIneqCon=0
429










7
  for (int tmp = 0; tmp < nVars; tmp++) np[tmp] = CI(ip, tmp);
430
  /* set u = (u 0)^T */
431



7
  u(iq) = 0.0;
432
  /* add ip to the active set A */
433



7
  A(iq) = ip;
434
435
  //      DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
436
437
#ifdef TRACE_SOLVER
438
  std::cerr << "Trying with constraint " << ip << std::endl;
439
  print_vector("np", np, nVars);
440
#endif
441
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2);
442
443
7
l2a: /* Step 2a: determine step direction */
444
  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2A);
445
  /* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
446



7
  compute_d(d, m_J, np);
447
  //    update_z(z, m_J, d, iq);
448



7
  if (iq >= nVars) {
449
    //      throw std::runtime_error("iq >= m_J.cols()");
450



1
    z.setZero();
451
  } else {
452



6
    update_z(z, m_J, d, iq);
453
  }
454
  /* compute N* np (if q > 0): the negative of the step direction in the dual space */
455



7
  update_r(R, r, d, iq);
456
#ifdef TRACE_SOLVER
457
  std::cerr << "Step direction z" << std::endl;
458
  print_vector("z", z, nVars);
459
  print_vector("r", r, iq + 1);
460
  print_vector("u", u, iq + 1);
461
  print_vector("d", d, nVars);
462
  print_vector("A", A, iq + 1);
463
#endif
464
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2A);
465
466
  /* Step 2b: compute step length */
467
  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2B);
468
7
  l = 0;
469
  /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
470
7
  t1 = inf; /* +inf */
471
  /* find the index l s.t. it reaches the minimum of u+(x) / r */
472
  // l: index of constraint to drop (maybe)
473



10
  for (k = nEqCon; k < iq; k++) {
474
    double tmp;
475





















3
    if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
476
      t1 = tmp;
477
      l = A(k);
478
    }
479
  }
480
  /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
481







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







5
    t2 = -s(ip) / z.dot(np);
483
  else
484
2
    t2 = inf; /* +inf */
485
486
  /* the step is chosen as the minimum of t1 and t2 */
487
7
  t = std::min(t1, t2);
488
#ifdef TRACE_SOLVER
489
  std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
490
#endif
491
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2B);
492
493
  /* Step 2c: determine new S-pair and take step: */
494
  START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
495
  /* case (i): no step in primal or dual space */
496



7
  if (t >= inf) {
497
    /* QPP is infeasible */
498
2
    q = iq;
499
    STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
500
2
    return RT_EIQUADPROG_UNBOUNDED;
501
  }
502
  /* case (ii): step in dual space */
503



5
  if (t2 >= inf) {
504
    /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
505
    u.head(iq) -= t * r.head(iq);
506
    u(iq) += t;
507
    iai(l) = l;
508
    delete_constraint(R, m_J, A, u, iq, l);
509
#ifdef TRACE_SOLVER
510
    std::cerr << " in dual space: " << f_value << std::endl;
511
    print_vector("x", x, nVars);
512
    print_vector("z", z, nVars);
513
    print_vector("A", A, iq + 1);
514
#endif
515
    STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
516
    goto l2a;
517
  }
518
519
  /* case (iii): step in primal and dual space */
520







5
  x += t * z;
521
  /* update the solution value */
522







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














5
  u.head(iq) -= t * r.head(iq);
525



5
  u(iq) += t;
526
527
#ifdef TRACE_SOLVER
528
  std::cerr << " in both spaces: " << f_value << std::endl;
529
  print_vector("x", x, nVars);
530
  print_vector("u", u, iq + 1);
531
  print_vector("r", r, iq + 1);
532
  print_vector("A", A, iq + 1);
533
#endif
534
535



5
  if (t == t2) {
536
#ifdef TRACE_SOLVER
537
    std::cerr << "Full step has taken " << t << std::endl;
538
    print_vector("x", x, nVars);
539
#endif
540
    /* full step has taken */
541
    /* add constraint ip to the active set*/
542







5
    if (!add_constraint(R, m_J, d, iq, R_norm)) {
543
      iaexcl(ip) = 0;
544
      delete_constraint(R, m_J, A, u, iq, ip);
545
#ifdef TRACE_SOLVER
546
      print_matrix("R", R, nVars);
547
      print_vector("A", A, iq);
548
#endif
549
      for (i = 0; i < nIneqCon; i++) iai(i) = i;
550
      for (i = 0; i < iq; i++) {
551
        A(i) = A_old(i);
552
        iai(A(i)) = -1;
553
        u(i) = u_old(i);
554
      }
555
      x = x_old;
556
      STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
557
      goto l2; /* go to step 2 */
558
    } else
559



5
      iai(ip) = -1;
560
#ifdef TRACE_SOLVER
561
    print_matrix("R", R, nVars);
562
    print_vector("A", A, iq);
563
#endif
564
    STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
565
5
    goto l1;
566
  }
567
568
  /* a partial step has been taken => drop constraint l */
569
  iai(l) = l;
570
  delete_constraint(R, m_J, A, u, iq, l);
571
  // s(ip) = CI.row(ip).dot(x) + ci0(ip); // does not compile if nIneqCon=0
572
  s(ip) = ci0(ip);
573
  for (int tmp = 0; tmp < nVars; tmp++) s(ip) += CI(ip, tmp) * x[tmp];
574
575
#ifdef TRACE_SOLVER
576
  std::cerr << "Partial step has taken " << t << std::endl;
577
  print_vector("x", x, nVars);
578
  print_matrix("R", R, nVars);
579
  print_vector("A", A, iq);
580
  print_vector("s", s, nIneqCon);
581
#endif
582
  STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
583
584
  goto l2a;
585
}
586
} /* namespace solvers */
587
} /* namespace eiquadprog */
588
#endif /* __eiquadprog_rt_hxx__ */