eiquadprog-fast.hxx 18.2 KB
Newer Older
1
2
3
4
5
6
//
// Copyright (c) 2017 CNRS
//

#ifndef EIQUADPROGFAST_HXX_
#define EIQUADPROGFAST_HXX_
Guilhem Saurel's avatar
Guilhem Saurel committed
7

Guilhem Saurel's avatar
Guilhem Saurel committed
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
namespace eiquadprog {
namespace solvers {

/// Compute sqrt(a^2 + b^2)
template <typename Scalar>
inline Scalar distance(Scalar a, Scalar b) {
  Scalar a1, b1, t;
  a1 = std::abs(a);
  b1 = std::abs(b);
  if (a1 > b1) {
    t = (b1 / a1);
    return a1 * std::sqrt(1.0 + t * t);
  } else if (b1 > a1) {
    t = (a1 / b1);
    return b1 * std::sqrt(1.0 + t * t);
  }
  return a1 * std::sqrt(2.0);
}

EiquadprogFast::EiquadprogFast() {
  m_maxIter = DEFAULT_MAX_ITER;
  q = 0;  // size of the active set A (containing the indices of the active constraints)
  is_inverse_provided_ = false;
  m_nVars = 0;
  m_nEqCon = 0;
  m_nIneqCon = 0;
}

EiquadprogFast::~EiquadprogFast() {}

void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
  m_nVars = nVars;
  m_nEqCon = nEqCon;
  m_nIneqCon = nIneqCon;
  m_J.setZero(nVars, nVars);
  chol_.compute(m_J);
  R.resize(nVars, nVars);
  s.resize(nIneqCon);
  r.resize(nIneqCon + nEqCon);
  u.resize(nIneqCon + nEqCon);
  z.resize(nVars);
  d.resize(nVars);
  np.resize(nVars);
  A.resize(nIneqCon + nEqCon);
  iai.resize(nIneqCon);
  iaexcl.resize(nIneqCon);
  x_old.resize(nVars);
  u_old.resize(nIneqCon + nEqCon);
  A_old.resize(nIneqCon + nEqCon);
57
58

#ifdef OPTIMIZE_ADD_CONSTRAINT
Guilhem Saurel's avatar
Guilhem Saurel committed
59
  T1.resize(nVars);
60
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
61
}
62

Guilhem Saurel's avatar
Guilhem Saurel committed
63
64
bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
  size_t nVars = J.rows();
65
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
66
  std::cerr << "Add constraint " << iq << '/';
67
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
68
69
  size_t j, k;
  double cc, ss, h, t1, t2, xny;
70
71

#ifdef OPTIMIZE_ADD_CONSTRAINT
Guilhem Saurel's avatar
Guilhem Saurel committed
72
  Eigen::Vector2d cc_ss;
73
74
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
  /* we have to find the Givens rotation which will reduce the element
           d(j) to zero.
           if it is already zero we don't have to do anything, except of
           decreasing j */
  for (j = d.size() - 1; j >= iq + 1; j--) {
    /* The Givens rotation is done with the matrix (cc cs, cs -cc).
                    If cc is one, then element (j) of d is zero compared with element
                    (j - 1). Hence we don't have to do anything.
                    If cc is zero, then we just have to switch column (j) and column (j - 1)
                    of J. Since we only switch columns in J, we have to be careful how we
                    update d depending on the sign of gs.
                    Otherwise we have to apply the Givens rotation to these columns.
                    The i - 1 element of d has to be updated to h. */
    cc = d(j - 1);
    ss = d(j);
    h = distance(cc, ss);
    if (h == 0.0) continue;
    d(j) = 0.0;
    ss = ss / h;
    cc = cc / h;
    if (cc < 0.0) {
      cc = -cc;
      ss = -ss;
      d(j - 1) = -h;
    } else
      d(j - 1) = h;
    xny = ss / (1.0 + cc);
102
103

// #define OPTIMIZE_ADD_CONSTRAINT
Guilhem Saurel's avatar
Guilhem Saurel committed
104
105
106
107
108
109
#ifdef OPTIMIZE_ADD_CONSTRAINT  // the optimized code is actually slower than the original
    T1 = J.col(j - 1);
    cc_ss(0) = cc;
    cc_ss(1) = ss;
    J.col(j - 1).noalias() = J.middleCols<2>(j - 1) * cc_ss;
    J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
110
#else
Guilhem Saurel's avatar
Guilhem Saurel committed
111
112
113
114
115
116
117
    // J.col(j-1) = J[:,j-1:j] * [cc; ss]
    for (k = 0; k < nVars; k++) {
      t1 = J(k, j - 1);
      t2 = J(k, j);
      J(k, j - 1) = t1 * cc + t2 * ss;
      J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
    }
118
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
119
120
121
122
123
124
125
  }
  /* update the number of constraints added*/
  iq++;
  /* To update R we have to put the iq components of the d vector
into column iq - 1 of R
*/
  R.col(iq - 1).head(iq) = d.head(iq);
126
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
127
  std::cerr << iq << std::endl;
128
129
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
130
131
132
133
134
135
  if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
    // problem degenerate
    return false;
  R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
  return true;
}
Guilhem Saurel's avatar
Guilhem Saurel committed
136

Guilhem Saurel's avatar
Guilhem Saurel committed
137
138
139
void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
                                       size_t l) {
  size_t nVars = R.rows();
140
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
141
  std::cerr << "Delete constraint " << l << ' ' << iq;
142
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
143
144
145
146
147
148
149
150
151
152
  size_t i, j, k;
  size_t qq = 0;
  double cc, ss, h, xny, t1, t2;

  /* Find the index qq for active constraint l to be removed */
  for (i = nEqCon; i < iq; i++)
    if (A(i) == static_cast<VectorXi::Scalar>(l)) {
      qq = i;
      break;
    }
153

Guilhem Saurel's avatar
Guilhem Saurel committed
154
155
156
157
158
159
160
161
162
163
164
165
166
167
  /* remove the constraint from the active set and the duals */
  for (i = qq; i < iq - 1; i++) {
    A(i) = A(i + 1);
    u(i) = u(i + 1);
    R.col(i) = R.col(i + 1);
  }

  A(iq - 1) = A(iq);
  u(iq - 1) = u(iq);
  A(iq) = 0;
  u(iq) = 0.0;
  for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
  /* constraint has been fully removed */
  iq--;
168
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
169
  std::cerr << '/' << iq << std::endl;
170
171
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
  if (iq == 0) return;

  for (j = qq; j < iq; j++) {
    cc = R(j, j);
    ss = R(j + 1, j);
    h = distance(cc, ss);
    if (h == 0.0) continue;
    cc = cc / h;
    ss = ss / h;
    R(j + 1, j) = 0.0;
    if (cc < 0.0) {
      R(j, j) = -h;
      cc = -cc;
      ss = -ss;
    } else
      R(j, j) = h;

    xny = ss / (1.0 + cc);
    for (k = j + 1; k < iq; k++) {
      t1 = R(j, k);
      t2 = R(j + 1, k);
      R(j, k) = t1 * cc + t2 * ss;
      R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
195
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
196
197
198
199
200
    for (k = 0; k < nVars; k++) {
      t1 = J(k, j);
      t2 = J(k, j + 1);
      J(k, j) = t1 * cc + t2 * ss;
      J(k, j + 1) = xny * (J(k, j) + t1) - t2;
201
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
  }
}

template <class Derived>
void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
  //  std::cerr << name << x.transpose() << std::endl;
}
template <class Derived>
void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
  //  std::cerr << name << std::endl << x << std::endl;
}

EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
                                                     const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
                                                     VectorXd& x) {
  const size_t nVars = g0.size();
  const size_t nEqCon = ce0.size();
  const size_t nIneqCon = ci0.size();

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

  assert(static_cast<size_t>(Hess.rows()) == m_nVars && static_cast<size_t>(Hess.cols()) == m_nVars);
  assert(static_cast<size_t>(g0.size()) == m_nVars);
  assert(static_cast<size_t>(CE.rows()) == m_nEqCon && static_cast<size_t>(CE.cols()) == m_nVars);
  assert(static_cast<size_t>(ce0.size()) == m_nEqCon);
  assert(static_cast<size_t>(CI.rows()) == m_nIneqCon && static_cast<size_t>(CI.cols()) == m_nVars);
  assert(static_cast<size_t>(ci0.size()) == m_nIneqCon);

  size_t i, k, l;  // indices
  size_t ip;       // index of the chosen violated constraint
  size_t iq;       // current number of active constraints
  double psi;      // current sum of constraint violations
  double c1;       // Hessian trace
  double c2;       // Hessian Cholesky factor trace
  double ss;       // largest constraint violation (negative for violation)
  double R_norm;   // norm of matrix R
  const double inf = std::numeric_limits<double>::infinity();
  double t, t1, t2;
  /* t is the step length, which is the minimum of the partial step length t1
   * and the full step length t2 */

  iter = 0;  // active-set iteration number

  /*
   * Preprocessing phase
   */
  /* compute the trace of the original matrix Hess */
  c1 = Hess.trace();

  /* decompose the matrix Hess in the form LL^T */
  if (!is_inverse_provided_) {
    START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
    chol_.compute(Hess);
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
  }

  /* initialize the matrix R */
  d.setZero(nVars);
  R.setZero(nVars, nVars);
  R_norm = 1.0;

  /* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
  // m_J = L^-T
  if (!is_inverse_provided_) {
    START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
    m_J.setIdentity(nVars, nVars);
268
#ifdef OPTIMIZE_HESSIAN_INVERSE
Guilhem Saurel's avatar
Guilhem Saurel committed
269
    chol_.matrixU().solveInPlace(m_J);
270
#else
Guilhem Saurel's avatar
Guilhem Saurel committed
271
    m_J = chol_.matrixU().solve(m_J);
272
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
273
274
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
  }
275

Guilhem Saurel's avatar
Guilhem Saurel committed
276
  c2 = m_J.trace();
277
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
278
  print_matrix("m_J", m_J, nVars);
279
280
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
281
282
283
284
285
286
287
288
289
290
291
  /* c1 * c2 is an estimate for cond(Hess) */

  /*
   * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 x
   * this is a feasible point in the dual space
   * x = Hess^-1 * g0
   */
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
  if (is_inverse_provided_) {
    x = m_J * (m_J.transpose() * g0);
  } else {
292
#ifdef OPTIMIZE_UNCONSTR_MINIM
Guilhem Saurel's avatar
Guilhem Saurel committed
293
294
295
    x = -g0;
    chol_.solveInPlace(x);
  }
296
#else
Guilhem Saurel's avatar
Guilhem Saurel committed
297
298
299
    x = chol_.solve(g0);
  }
  x = -x;
300
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
301
302
  /* and compute the current solution value */
  f_value = 0.5 * g0.dot(x);
303
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
304
305
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
  print_vector("x", x, nVars);
306
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
307
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
308

Guilhem Saurel's avatar
Guilhem Saurel committed
309
  /* Add equality constraints to the working set A */
310

Guilhem Saurel's avatar
Guilhem Saurel committed
311
312
313
314
315
316
317
318
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
  iq = 0;
  for (i = 0; i < nEqCon; i++) {
    START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
    np = CE.row(i);
    compute_d(d, m_J, np);
    update_z(z, m_J, d, iq);
    update_r(R, r, d, iq);
319
320

#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
321
322
323
324
    print_matrix("R", R, iq);
    print_vector("z", z, nVars);
    print_vector("r", r, iq);
    print_vector("d", d, nVars);
325
326
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
327
328
329
330
331
332
333
    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
    becomes feasible */
    t2 = 0.0;
    if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())  // i.e. z != 0
      t2 = (-np.dot(x) - ce0(i)) / z.dot(np);

    x += t2 * z;
334

Guilhem Saurel's avatar
Guilhem Saurel committed
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
    /* set u = u+ */
    u(iq) = t2;
    u.head(iq) -= t2 * r.head(iq);

    /* compute the new solution value */
    f_value += 0.5 * (t2 * t2) * z.dot(np);
    A(i) = static_cast<VectorXi::Scalar>(-i - 1);
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);

    START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
    if (!add_constraint(R, m_J, d, iq, R_norm)) {
      // Equality constraints are linearly dependent
      return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
    }
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
  }
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);

  /* set iai = K \ A */
  for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
355
356

#ifdef USE_WARM_START
Guilhem Saurel's avatar
Guilhem Saurel committed
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
  //      DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
  for (i = nEqCon; i < q; i++) {
    iai(i - nEqCon) = -1;
    ip = A(i);
    np = CI.row(ip);
    compute_d(d, m_J, np);
    update_z(z, m_J, d, iq);
    update_r(R, r, d, iq);

    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
    becomes feasible */
    t2 = 0.0;
    if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())  // i.e. z != 0
      t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
    else
      DEBUG_STREAM("[WARM START] z=0\n")

    x += t2 * z;

    /* set u = u+ */
    u(iq) = t2;
    u.head(iq) -= t2 * r.head(iq);

    /* compute the new solution value */
    f_value += 0.5 * (t2 * t2) * z.dot(np);

    if (!add_constraint(R, m_J, d, iq, R_norm)) {
      // constraints are linearly dependent
      std::cerr << "[WARM START] Constraints are linearly dependent\n";
      return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
    }
  }
389
390
391
392
393
#else

#endif

l1:
Guilhem Saurel's avatar
Guilhem Saurel committed
394
395
396
397
398
  iter++;
  if (iter >= m_maxIter) {
    q = iq;
    return EIQUADPROG_FAST_MAX_ITER_REACHED;
  }
399

Guilhem Saurel's avatar
Guilhem Saurel committed
400
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
401
402

#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
403
  print_vector("x", x, nVars);
404
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
405
406
407
408
409
  /* step 1: choose a violated constraint */
  for (i = nEqCon; i < iq; i++) {
    ip = A(i);
    iai(ip) = -1;
  }
410

Guilhem Saurel's avatar
Guilhem Saurel committed
411
412
413
414
  /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
  ss = 0.0;
  ip = 0; /* ip will be the index of the chosen violated constraint */
415
416

#ifdef OPTIMIZE_STEP_1_2
Guilhem Saurel's avatar
Guilhem Saurel committed
417
418
419
420
  s = ci0;
  s.noalias() += CI * x;
  iaexcl.setOnes();
  psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
421
#else
Guilhem Saurel's avatar
Guilhem Saurel committed
422
423
424
425
426
427
  psi = 0.0; /* this value will contain the sum of all infeasibilities */
  for (i = 0; i < nIneqCon; i++) {
    iaexcl(i) = 1;
    s(i) = CI.row(i).dot(x) + ci0(i);
    psi += std::min(0.0, s(i));
  }
428
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
429
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
430
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
431
  print_vector("s", s, nIneqCon);
432
433
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
434
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
435

Guilhem Saurel's avatar
Guilhem Saurel committed
436
437
438
439
440
441
  if (std::abs(psi) <= static_cast<double>(nIneqCon) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
    /* numerically there are not infeasibilities anymore */
    q = iq;
    //        DEBUG_STREAM("Optimal active set:\n"<<A.head(iq).transpose()<<"\n\n")
    return EIQUADPROG_FAST_OPTIMAL;
  }
442

Guilhem Saurel's avatar
Guilhem Saurel committed
443
444
445
446
  /* save old values for u, x and A */
  u_old.head(iq) = u.head(iq);
  A_old.head(iq) = A.head(iq);
  x_old = x;
447
448

l2: /* Step 2: check for feasibility and determine a new S-pair */
Guilhem Saurel's avatar
Guilhem Saurel committed
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
  // find constraint with highest violation (what about normalizing constraints?)
  for (i = 0; i < nIneqCon; i++) {
    if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
      ss = s(i);
      ip = i;
    }
  }
  if (ss >= 0.0) {
    q = iq;
    //        DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
    return EIQUADPROG_FAST_OPTIMAL;
  }

  /* set np = n(ip) */
  np = CI.row(ip);
  /* set u = (u 0)^T */
  u(iq) = 0.0;
  /* add ip to the active set A */
  A(iq) = static_cast<VectorXi::Scalar>(ip);

  //      DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
471
472

#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
473
474
  std::cerr << "Trying with constraint " << ip << std::endl;
  print_vector("np", np, nVars);
475
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);

l2a: /* Step 2a: determine step direction */
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
  /* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
  compute_d(d, m_J, np);
  //    update_z(z, m_J, d, iq);
  if (iq >= nVars) {
    //      throw std::runtime_error("iq >= m_J.cols()");
    z.setZero();
  } else {
    update_z(z, m_J, d, iq);
  }
  /* compute N* np (if q > 0): the negative of the step direction in the dual space */
  update_r(R, r, d, iq);
491
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
492
493
494
495
496
497
  std::cerr << "Step direction z" << std::endl;
  print_vector("z", z, nVars);
  print_vector("r", r, iq + 1);
  print_vector("u", u, iq + 1);
  print_vector("d", d, nVars);
  print_vector("A", A, iq + 1);
498
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);

  /* Step 2b: compute step length */
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
  l = 0;
  /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
  t1 = inf; /* +inf */
  /* find the index l s.t. it reaches the minimum of u+(x) / r */
  // l: index of constraint to drop (maybe)
  for (k = nEqCon; k < iq; k++) {
    double tmp;
    if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
      t1 = tmp;
      l = A(k);
    }
  }
  /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
  if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())  // i.e. z != 0
    t2 = -s(ip) / z.dot(np);
  else
    t2 = inf; /* +inf */

  /* the step is chosen as the minimum of t1 and t2 */
  t = std::min(t1, t2);
523
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
524
  std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
525
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);

  /* Step 2c: determine new S-pair and take step: */
  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
  /* case (i): no step in primal or dual space */
  if (t >= inf) {
    /* QPP is infeasible */
    q = iq;
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
    return EIQUADPROG_FAST_UNBOUNDED;
  }
  /* case (ii): step in dual space */
  if (t2 >= inf) {
    /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
    u.head(iq) -= t * r.head(iq);
    u(iq) += t;
    iai(l) = static_cast<VectorXi::Scalar>(l);
    delete_constraint(R, m_J, A, u, nEqCon, iq, l);
544
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
545
546
547
548
    std::cerr << " in dual space: " << f_value << std::endl;
    print_vector("x", x, nVars);
    print_vector("z", z, nVars);
    print_vector("A", A, iq + 1);
549
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
550
551
552
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
    goto l2a;
  }
553

Guilhem Saurel's avatar
Guilhem Saurel committed
554
555
556
557
  /* case (iii): step in primal and dual space */
  x += t * z;
  /* update the solution value */
  f_value += t * z.dot(np) * (0.5 * t + u(iq));
558

Guilhem Saurel's avatar
Guilhem Saurel committed
559
560
  u.head(iq) -= t * r.head(iq);
  u(iq) += t;
561
562

#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
563
564
565
566
567
  std::cerr << " in both spaces: " << f_value << std::endl;
  print_vector("x", x, nVars);
  print_vector("u", u, iq + 1);
  print_vector("r", r, iq + 1);
  print_vector("A", A, iq + 1);
568
569
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
570
  if (t == t2) {
571
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
572
573
    std::cerr << "Full step has taken " << t << std::endl;
    print_vector("x", x, nVars);
574
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
575
576
577
578
579
    /* full step has taken */
    /* add constraint ip to the active set*/
    if (!add_constraint(R, m_J, d, iq, R_norm)) {
      iaexcl(ip) = 0;
      delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
580
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
581
582
      print_matrix("R", R, nVars);
      print_vector("A", A, iq);
583
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
584
585
586
587
588
589
590
591
592
593
594
      for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
      for (i = 0; i < iq; i++) {
        A(i) = A_old(i);
        iai(A(i)) = -1;
        u(i) = u_old(i);
      }
      x = x_old;
      STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
      goto l2; /* go to step 2 */
    } else
      iai(ip) = -1;
595
#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
596
597
    print_matrix("R", R, nVars);
    print_vector("A", A, iq);
598
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
599
600
601
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
    goto l1;
  }
602

Guilhem Saurel's avatar
Guilhem Saurel committed
603
604
605
606
  /* a partial step has been taken => drop constraint l */
  iai(l) = static_cast<VectorXi::Scalar>(l);
  delete_constraint(R, m_J, A, u, nEqCon, iq, l);
  s(ip) = CI.row(ip).dot(x) + ci0(ip);
607
608

#ifdef TRACE_SOLVER
Guilhem Saurel's avatar
Guilhem Saurel committed
609
610
611
612
613
  std::cerr << "Partial step has taken " << t << std::endl;
  print_vector("x", x, nVars);
  print_matrix("R", R, nVars);
  print_vector("A", A, iq);
  print_vector("s", s, nIneqCon);
614
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
615
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
616

Guilhem Saurel's avatar
Guilhem Saurel committed
617
618
  goto l2a;
}
Guilhem Saurel's avatar
Guilhem Saurel committed
619

Guilhem Saurel's avatar
Guilhem Saurel committed
620
} /* namespace solvers */
621
622
} /* namespace eiquadprog */

Guilhem Saurel's avatar
Guilhem Saurel committed
623
#endif /* EIQUADPROGFAST_HXX_ */