Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/eiquadprog-fast.cpp	Lines:	151	222	68.0 %
Date:	2023-11-29 12:38:05	Branches:	163	386	42.2 %


#include "eiquadprog/eiquadprog-fast.hpp"

#include <iostream>

namespace eiquadprog {
namespace solvers {

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);

#ifdef OPTIMIZE_ADD_CONSTRAINT
  T1.resize(nVars);
#endif
}

bool EiquadprogFast::add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d,
                                    size_t &iq, double &R_norm) {
  size_t nVars = J.rows();
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Add constraint " << iq << '/';
#endif
  size_t j, k;
  double cc, ss, h, t1, t2, xny;

#ifdef OPTIMIZE_ADD_CONSTRAINT
  Eigen::Vector2d cc_ss;
#endif

  /* 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 = utils::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);

// #define OPTIMIZE_ADD_CONSTRAINT
#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);
#else
    // 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;
    }
#endif
  }
  /* 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);
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << iq << std::endl;
#endif

  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;
}

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();
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
  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;
    }

  /* 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--;
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << '/' << iq << std::endl;
#endif

  if (iq == 0) return;

  for (j = qq; j < iq; j++) {
    cc = R(j, j);
    ss = R(j + 1, j);
    h = utils::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;
    }
    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;
    }
  }
}

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);
#ifdef OPTIMIZE_HESSIAN_INVERSE
    chol_.matrixU().solveInPlace(m_J);
#else
    m_J = chol_.matrixU().solve(m_J);
#endif
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
  }

  c2 = m_J.trace();
#ifdef EIQGUADPROG_TRACE_SOLVER
  utils::print_matrix("m_J", m_J);
#endif

  /* 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 {
#ifdef OPTIMIZE_UNCONSTR_MINIM
    x = -g0;
    chol_.solveInPlace(x);
  }
#else
    x = chol_.solve(g0);
  }
  x = -x;
#endif
  /* and compute the current solution value */
  f_value = 0.5 * g0.dot(x);
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
  utils::print_vector("x", x);
#endif
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);

  /* Add equality constraints to the working set A */

  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);

#ifdef EIQGUADPROG_TRACE_SOLVER
    utils::print_matrix("R", R);
    utils::print_vector("z", z);
    utils::print_vector("r", r);
    utils::print_vector("d", d);
#endif

    /* 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;

    /* 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);

#ifdef USE_WARM_START
  //      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;
    }
  }
#else

#endif

l1:
  iter++;
  if (iter >= m_maxIter) {
    q = iq;
    return EIQUADPROG_FAST_MAX_ITER_REACHED;
  }

  START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);

#ifdef EIQGUADPROG_TRACE_SOLVER
  utils::print_vector("x", x);
#endif
  /* step 1: choose a violated constraint */
  for (i = nEqCon; i < iq; i++) {
    ip = A(i);
    iai(ip) = -1;
  }

  /* 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 */

#ifdef OPTIMIZE_STEP_1_2
  s = ci0;
  s.noalias() += CI * x;
  iaexcl.setOnes();
  psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
#else
  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));
  }
#endif
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
#ifdef EIQGUADPROG_TRACE_SOLVER
  utils::print_vector("s", s);
#endif

  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);

  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;
  }

  /* 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;

l2: /* Step 2: check for feasibility and determine a new S-pair */
  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")

#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Trying with constraint " << ip << std::endl;
  utils::print_vector("np", np);
#endif
  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);
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Step direction z" << std::endl;
  utils::print_vector("z", z);
  utils::print_vector("r", r);
  utils::print_vector("u", u);
  utils::print_vector("d", d);
  utils::print_vector("A", A);
#endif
  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);
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
            << ") ";
#endif
  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);
#ifdef EIQGUADPROG_TRACE_SOLVER
    std::cerr << " in dual space: " << f_value << std::endl;
    utils::print_vector("x", x);
    utils::print_vector("z", z);
    utils::print_vector("A", A);
#endif
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
    goto l2a;
  }

  /* 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));

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

#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << " in both spaces: " << f_value << std::endl;
  utils::print_vector("x", x);
  utils::print_vector("u", u);
  utils::print_vector("r", r);
  utils::print_vector("A", A);
#endif

  if (t == t2) {
#ifdef EIQGUADPROG_TRACE_SOLVER
    std::cerr << "Full step has taken " << t << std::endl;
    utils::print_vector("x", x);
#endif
    /* 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);
#ifdef EIQGUADPROG_TRACE_SOLVER
      utils::print_matrix("R", R);
      utils::print_vector("A", A);
#endif
      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;
#ifdef EIQGUADPROG_TRACE_SOLVER
    utils::print_matrix("R", R);
    utils::print_vector("A", A);
#endif
    STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
    goto l1;
  }

  /* 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);

#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Partial step has taken " << t << std::endl;
  utils::print_vector("x", x);
  utils::print_matrix("R", R);
  utils::print_vector("A", A);
  utils::print_vector("s", s);
#endif
  STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);

  goto l2a;
}

} /* namespace solvers */
} /* namespace eiquadprog */


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#include "eiquadprog/eiquadprog-fast.hpp"
2
3			#include <iostream>
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		24	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,
46			size_t &iq, double &R_norm) {
47		10	size_t nVars = J.rows();
48			#ifdef EIQGUADPROG_TRACE_SOLVER
49			std::cerr << "Add constraint " << iq << '/';
50			#endif
51			size_t j, k;
52			double cc, ss, h, t1, t2, xny;
53
54			#ifdef OPTIMIZE_ADD_CONSTRAINT
55			Eigen::Vector2d cc_ss;
56			#endif
57
58			/* we have to find the Givens rotation which will reduce the element
59			d(j) to zero.
60			if it is already zero we don't have to do anything, except of
61			decreasing j */
62	✓✓	16	for (j = d.size() - 1; j >= iq + 1; j--) {
63			/* The Givens rotation is done with the matrix (cc cs, cs -cc).
64			If cc is one, then element (j) of d is zero compared with
65			element (j - 1). Hence we don't have to do anything. If cc is zero, then
66			we just have to switch column (j) and column (j - 1) of J. Since we only
67			switch columns in J, we have to be careful how we update d depending on
68			the sign of gs. Otherwise we have to apply the Givens rotation to these
69			columns. 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 = utils::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
87			// the original
88			T1 = J.col(j - 1);
89			cc_ss(0) = cc;
90			cc_ss(1) = ss;
91			J.col(j - 1).noalias() = J.middleCols<2>(j - 1) * cc_ss;
92			J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j);
93			#else
94			// J.col(j-1) = J[:,j-1:j] * [cc; ss]
95	✓✓	18	for (k = 0; k < nVars; k++) {
96		12	t1 = J(k, j - 1);
97		12	t2 = J(k, j);
98		12	J(k, j - 1) = t1 * cc + t2 * ss;
99		12	J(k, j) = xny * (t1 + J(k, j - 1)) - t2;
100			}
101			#endif
102			}
103			/* update the number of constraints added*/
104		10	iq++;
105			/* To update R we have to put the iq components of the d vector
106			into column iq - 1 of R
107			*/
108	✓✗✓✗ ✓✗	10	R.col(iq - 1).head(iq) = d.head(iq);
109			#ifdef EIQGUADPROG_TRACE_SOLVER
110			std::cerr << iq << std::endl;
111			#endif
112
113	✓✓	10	if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
114			// problem degenerate
115		1	return false;
116		9	R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
117		9	return true;
118			}
119
120			void EiquadprogFast::delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A,
121			VectorXd &u, size_t nEqCon, size_t &iq,
122			size_t l) {
123			size_t nVars = R.rows();
124			#ifdef EIQGUADPROG_TRACE_SOLVER
125			std::cerr << "Delete constraint " << l << ' ' << iq;
126			#endif
127			size_t i, j, k;
128			size_t qq = 0;
129			double cc, ss, h, xny, t1, t2;
130
131			/* Find the index qq for active constraint l to be removed */
132			for (i = nEqCon; i < iq; i++)
133			if (A(i) == static_cast<VectorXi::Scalar>(l)) {
134			qq = i;
135			break;
136			}
137
138			/* remove the constraint from the active set and the duals */
139			for (i = qq; i < iq - 1; i++) {
140			A(i) = A(i + 1);
141			u(i) = u(i + 1);
142			R.col(i) = R.col(i + 1);
143			}
144
145			A(iq - 1) = A(iq);
146			u(iq - 1) = u(iq);
147			A(iq) = 0;
148			u(iq) = 0.0;
149			for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
150			/* constraint has been fully removed */
151			iq--;
152			#ifdef EIQGUADPROG_TRACE_SOLVER
153			std::cerr << '/' << iq << std::endl;
154			#endif
155
156			if (iq == 0) return;
157
158			for (j = qq; j < iq; j++) {
159			cc = R(j, j);
160			ss = R(j + 1, j);
161			h = utils::distance(cc, ss);
162			if (h == 0.0) continue;
163			cc = cc / h;
164			ss = ss / h;
165			R(j + 1, j) = 0.0;
166			if (cc < 0.0) {
167			R(j, j) = -h;
168			cc = -cc;
169			ss = -ss;
170			} else
171			R(j, j) = h;
172
173			xny = ss / (1.0 + cc);
174			for (k = j + 1; k < iq; k++) {
175			t1 = R(j, k);
176			t2 = R(j + 1, k);
177			R(j, k) = t1 * cc + t2 * ss;
178			R(j + 1, k) = xny * (t1 + R(j, k)) - t2;
179			}
180			for (k = 0; k < nVars; k++) {
181			t1 = J(k, j);
182			t2 = J(k, j + 1);
183			J(k, j) = t1 * cc + t2 * ss;
184			J(k, j + 1) = xny * (J(k, j) + t1) - t2;
185			}
186			}
187			}
188
189		12	EiquadprogFast_status EiquadprogFast::solve_quadprog(
190			const MatrixXd &Hess, const VectorXd &g0, const MatrixXd &CE,
191			const VectorXd &ce0, const MatrixXd &CI, const VectorXd &ci0, VectorXd &x) {
192	✓✗	12	const size_t nVars = g0.size();
193	✓✗	12	const size_t nEqCon = ce0.size();
194	✓✗	12	const size_t nIneqCon = ci0.size();
195
196	✓✗✓✗ ✗✓	12	if (nVars != m_nVars \|\| nEqCon != m_nEqCon \|\| nIneqCon != m_nIneqCon)
197			reset(nVars, nEqCon, nIneqCon);
198
199	✓✗✓✗ ✓✗✓✗	12	assert(static_cast<size_t>(Hess.rows()) == m_nVars &&
200			static_cast<size_t>(Hess.cols()) == m_nVars);
201	✓✗✗✓	12	assert(static_cast<size_t>(g0.size()) == m_nVars);
202	✓✗✓✗ ✓✗✓✗	12	assert(static_cast<size_t>(CE.rows()) == m_nEqCon &&
203			static_cast<size_t>(CE.cols()) == m_nVars);
204	✓✗✗✓	12	assert(static_cast<size_t>(ce0.size()) == m_nEqCon);
205	✓✗✓✗ ✓✗✓✗	12	assert(static_cast<size_t>(CI.rows()) == m_nIneqCon &&
206			static_cast<size_t>(CI.cols()) == m_nVars);
207	✓✗✗✓	12	assert(static_cast<size_t>(ci0.size()) == m_nIneqCon);
208
209			size_t i, k, l; // indices
210			size_t ip; // index of the chosen violated constraint
211			size_t iq; // current number of active constraints
212			double psi; // current sum of constraint violations
213			double c1; // Hessian trace
214			double c2; // Hessian Cholesky factor trace
215			double ss; // largest constraint violation (negative for violation)
216			double R_norm; // norm of matrix R
217		12	const double inf = std::numeric_limits<double>::infinity();
218			double t, t1, t2;
219			/* t is the step length, which is the minimum of the partial step length t1
220			* and the full step length t2 */
221
222		12	iter = 0; // active-set iteration number
223
224			/*
225			* Preprocessing phase
226			*/
227			/* compute the trace of the original matrix Hess */
228	✓✗	12	c1 = Hess.trace();
229
230			/* decompose the matrix Hess in the form LL^T */
231	✓✗	12	if (!is_inverse_provided_) {
232			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
233	✓✗	12	chol_.compute(Hess);
234			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION);
235			}
236
237			/* initialize the matrix R */
238	✓✗	12	d.setZero(nVars);
239	✓✗	12	R.setZero(nVars, nVars);
240		12	R_norm = 1.0;
241
242			/* compute the inverse of the factorized matrix Hess^-1,
243			this is the initial value for H */
244			// m_J = L^-T
245	✓✗	12	if (!is_inverse_provided_) {
246			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
247	✓✗	12	m_J.setIdentity(nVars, nVars);
248			#ifdef OPTIMIZE_HESSIAN_INVERSE
249	✓✗✓✗	12	chol_.matrixU().solveInPlace(m_J);
250			#else
251			m_J = chol_.matrixU().solve(m_J);
252			#endif
253			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOLESKY_INVERSE);
254			}
255
256	✓✗	12	c2 = m_J.trace();
257			#ifdef EIQGUADPROG_TRACE_SOLVER
258			utils::print_matrix("m_J", m_J);
259			#endif
260
261			/* c1 * c2 is an estimate for cond(Hess) */
262
263			/*
264			* Find the unconstrained minimizer of the quadratic
265			* form 0.5 * x Hess x + g0 x
266			* this is a feasible point in the dual space
267			* x = Hess^-1 * g0
268			*/
269			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
270	✗✓	12	if (is_inverse_provided_) {
271			x = m_J * (m_J.transpose() * g0);
272			} else {
273			#ifdef OPTIMIZE_UNCONSTR_MINIM
274	✓✗✓✗	12	x = -g0;
275	✓✗	12	chol_.solveInPlace(x);
276			}
277			#else
278			x = chol_.solve(g0);
279			}
280			x = -x;
281			#endif
282			/* and compute the current solution value */
283	✓✗	12	f_value = 0.5 * g0.dot(x);
284			#ifdef EIQGUADPROG_TRACE_SOLVER
285			std::cerr << "Unconstrained solution: " << f_value << std::endl;
286			utils::print_vector("x", x);
287			#endif
288			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
289
290			/* Add equality constraints to the working set A */
291
292			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
293		12	iq = 0;
294	✓✓	16	for (i = 0; i < nEqCon; i++) {
295			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
296	✓✗✓✗	5	np = CE.row(i);
297	✓✗	5	compute_d(d, m_J, np);
298	✓✗	5	update_z(z, m_J, d, iq);
299	✓✗	5	update_r(R, r, d, iq);
300
301			#ifdef EIQGUADPROG_TRACE_SOLVER
302			utils::print_matrix("R", R);
303			utils::print_vector("z", z);
304			utils::print_vector("r", r);
305			utils::print_vector("d", d);
306			#endif
307
308			/* compute full step length t2: i.e.,
309			the minimum step in primal space s.t. the contraint
310			becomes feasible */
311		5	t2 = 0.0;
312	✓✗✓✓	5	if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
313			// i.e. z != 0
314	✓✗✓✗ ✓✗	4	t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
315
316	✓✗✓✗	5	x += t2 * z;
317
318			/* set u = u+ */
319	✓✗	5	u(iq) = t2;
320	✓✗✓✗ ✓✗✓✗	5	u.head(iq) -= t2 * r.head(iq);
321
322			/* compute the new solution value */
323	✓✗	5	f_value += 0.5 * (t2 * t2) * z.dot(np);
324	✓✗	5	A(i) = static_cast<VectorXi::Scalar>(-i - 1);
325			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1);
326
327			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
328	✓✗✓✓	5	if (!add_constraint(R, m_J, d, iq, R_norm)) {
329			// Equality constraints are linearly dependent
330		1	return EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
331			}
332			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2);
333			}
334			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR);
335
336			/* set iai = K \ A */
337	✓✓✓✗	22	for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
338
339			#ifdef USE_WARM_START
340			// DEBUG_STREAM("Gonna warm start using previous active
341			// set:\n"<<A.transpose()<<"\n")
342			for (i = nEqCon; i < q; i++) {
343			iai(i - nEqCon) = -1;
344			ip = A(i);
345			np = CI.row(ip);
346			compute_d(d, m_J, np);
347			update_z(z, m_J, d, iq);
348			update_r(R, r, d, iq);
349
350			/* compute full step length t2: i.e.,
351			the minimum step in primal space s.t. the contraint
352			becomes feasible */
353			t2 = 0.0;
354			if (std::abs(z.dot(z)) >
355			std::numeric_limits<double>::epsilon()) // i.e. z != 0
356			t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
357			else
358			DEBUG_STREAM("[WARM START] z=0\n")
359
360			x += t2 * z;
361
362			/* set u = u+ */
363			u(iq) = t2;
364			u.head(iq) -= t2 * r.head(iq);
365
366			/* compute the new solution value */
367			f_value += 0.5 * (t2 * t2) * z.dot(np);
368
369			if (!add_constraint(R, m_J, d, iq, R_norm)) {
370			// constraints are linearly dependent
371			std::cerr << "[WARM START] Constraints are linearly dependent\n";
372			return RT_EIQUADPROG_REDUNDANT_EQUALITIES;
373			}
374			}
375			#else
376
377			#endif
378
379		11	l1:
380		16	iter++;
381	✗✓	16	if (iter >= m_maxIter) {
382			q = iq;
383			return EIQUADPROG_FAST_MAX_ITER_REACHED;
384			}
385
386			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
387
388			#ifdef EIQGUADPROG_TRACE_SOLVER
389			utils::print_vector("x", x);
390			#endif
391			/* step 1: choose a violated constraint */
392	✓✓	22	for (i = nEqCon; i < iq; i++) {
393	✓✗	6	ip = A(i);
394	✓✗	6	iai(ip) = -1;
395			}
396
397			/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
398			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
399		16	ss = 0.0;
400		16	ip = 0; /* ip will be the index of the chosen violated constraint */
401
402			#ifdef OPTIMIZE_STEP_1_2
403	✓✗	16	s = ci0;
404	✓✗✓✗ ✓✗	16	s.noalias() += CI * x;
405	✓✗	16	iaexcl.setOnes();
406	✓✗✓✗ ✓✗	16	psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum();
407			#else
408			psi = 0.0; /* this value will contain the sum of all infeasibilities */
409			for (i = 0; i < nIneqCon; i++) {
410			iaexcl(i) = 1;
411			s(i) = CI.row(i).dot(x) + ci0(i);
412			psi += std::min(0.0, s(i));
413			}
414			#endif
415			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
416			#ifdef EIQGUADPROG_TRACE_SOLVER
417			utils::print_vector("s", s);
418			#endif
419
420			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
421
422		16	if (std::abs(psi) <= static_cast<double>(nIneqCon) *
423	✓✓	16	std::numeric_limits<double>::epsilon() * c1 * c2 *
424			100.0) {
425			/* numerically there are not infeasibilities anymore */
426		9	q = iq;
427			// DEBUG_STREAM("Optimal active
428			// set:\n"<<A.head(iq).transpose()<<"\n\n")
429		9	return EIQUADPROG_FAST_OPTIMAL;
430			}
431
432			/* save old values for u, x and A */
433	✓✗✓✗ ✓✗	7	u_old.head(iq) = u.head(iq);
434	✓✗✓✗ ✓✗	7	A_old.head(iq) = A.head(iq);
435	✓✗	7	x_old = x;
436
437		7	l2: /* Step 2: check for feasibility and determine a new S-pair */
438			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
439			// find constraint with highest violation
440			// (what about normalizing constraints?)
441	✓✓	20	for (i = 0; i < nIneqCon; i++) {
442	✓✗✓✓ ✓✗✓✗ ✓✗✓✗ ✓✓	13	if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
443	✓✗	7	ss = s(i);
444		7	ip = i;
445			}
446			}
447	✗✓	7	if (ss >= 0.0) {
448			q = iq;
449			// DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n")
450			return EIQUADPROG_FAST_OPTIMAL;
451			}
452
453			/* set np = n(ip) */
454	✓✗✓✗	7	np = CI.row(ip);
455			/* set u = (u 0)^T */
456	✓✗	7	u(iq) = 0.0;
457			/* add ip to the active set A */
458	✓✗	7	A(iq) = static_cast<VectorXi::Scalar>(ip);
459
460			// DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
461
462			#ifdef EIQGUADPROG_TRACE_SOLVER
463			std::cerr << "Trying with constraint " << ip << std::endl;
464			utils::print_vector("np", np);
465			#endif
466			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
467
468		7	l2a: /* Step 2a: determine step direction */
469			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
470			/* compute z = H np: the step direction in the primal space
471			(through m_J, see the paper) */
472	✓✗	7	compute_d(d, m_J, np);
473			// update_z(z, m_J, d, iq);
474	✓✓	7	if (iq >= nVars) {
475			// throw std::runtime_error("iq >= m_J.cols()");
476	✓✗	1	z.setZero();
477			} else {
478	✓✗	6	update_z(z, m_J, d, iq);
479			}
480			/* compute N* np (if q > 0): the negative of the
481			step direction in the dual space */
482	✓✗	7	update_r(R, r, d, iq);
483			#ifdef EIQGUADPROG_TRACE_SOLVER
484			std::cerr << "Step direction z" << std::endl;
485			utils::print_vector("z", z);
486			utils::print_vector("r", r);
487			utils::print_vector("u", u);
488			utils::print_vector("d", d);
489			utils::print_vector("A", A);
490			#endif
491			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
492
493			/* Step 2b: compute step length */
494			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
495		7	l = 0;
496			/* Compute t1: partial step length (maximum step in dual
497			space without violating dual feasibility */
498		7	t1 = inf; /* +inf */
499			/* find the index l s.t. it reaches the minimum of u+(x) / r */
500			// l: index of constraint to drop (maybe)
501	✓✓	10	for (k = nEqCon; k < iq; k++) {
502			double tmp;
503	✓✗✗✓ ✗✗✗✗ ✗✗✗✓	3	if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) {
504			t1 = tmp;
505			l = A(k);
506			}
507			}
508			/* Compute t2: full step length (minimum step in primal
509			space such that the constraint ip becomes feasible */
510	✓✗✓✓	7	if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
511			// i.e. z != 0
512	✓✗✓✗	5	t2 = -s(ip) / z.dot(np);
513			else
514		2	t2 = inf; /* +inf */
515
516			/* the step is chosen as the minimum of t1 and t2 */
517		7	t = std::min(t1, t2);
518			#ifdef EIQGUADPROG_TRACE_SOLVER
519			std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
520			<< ") ";
521			#endif
522			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
523
524			/* Step 2c: determine new S-pair and take step: */
525			START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
526			/* case (i): no step in primal or dual space */
527	✓✓	7	if (t >= inf) {
528			/* QPP is infeasible */
529		2	q = iq;
530			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
531		2	return EIQUADPROG_FAST_UNBOUNDED;
532			}
533			/* case (ii): step in dual space */
534	✗✓	5	if (t2 >= inf) {
535			/* set u = u + t * [-r 1) and drop constraint l from the active set A */
536			u.head(iq) -= t * r.head(iq);
537			u(iq) += t;
538			iai(l) = static_cast<VectorXi::Scalar>(l);
539			delete_constraint(R, m_J, A, u, nEqCon, iq, l);
540			#ifdef EIQGUADPROG_TRACE_SOLVER
541			std::cerr << " in dual space: " << f_value << std::endl;
542			utils::print_vector("x", x);
543			utils::print_vector("z", z);
544			utils::print_vector("A", A);
545			#endif
546			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
547			goto l2a;
548			}
549
550			/* case (iii): step in primal and dual space */
551	✓✗✓✗	5	x += t * z;
552			/* update the solution value */
553	✓✗✓✗	5	f_value += t * z.dot(np) * (0.5 * t + u(iq));
554
555	✓✗✓✗ ✓✗✓✗	5	u.head(iq) -= t * r.head(iq);
556	✓✗	5	u(iq) += t;
557
558			#ifdef EIQGUADPROG_TRACE_SOLVER
559			std::cerr << " in both spaces: " << f_value << std::endl;
560			utils::print_vector("x", x);
561			utils::print_vector("u", u);
562			utils::print_vector("r", r);
563			utils::print_vector("A", A);
564			#endif
565
566	✓✗	5	if (t == t2) {
567			#ifdef EIQGUADPROG_TRACE_SOLVER
568			std::cerr << "Full step has taken " << t << std::endl;
569			utils::print_vector("x", x);
570			#endif
571			/* full step has taken */
572			/* add constraint ip to the active set*/
573	✓✗✗✓	5	if (!add_constraint(R, m_J, d, iq, R_norm)) {
574			iaexcl(ip) = 0;
575			delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
576			#ifdef EIQGUADPROG_TRACE_SOLVER
577			utils::print_matrix("R", R);
578			utils::print_vector("A", A);
579			#endif
580			for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
581			for (i = 0; i < iq; i++) {
582			A(i) = A_old(i);
583			iai(A(i)) = -1;
584			u(i) = u_old(i);
585			}
586			x = x_old;
587			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
588			goto l2; /* go to step 2 */
589			} else
590	✓✗	5	iai(ip) = -1;
591			#ifdef EIQGUADPROG_TRACE_SOLVER
592			utils::print_matrix("R", R);
593			utils::print_vector("A", A);
594			#endif
595			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
596		5	goto l1;
597			}
598
599			/* a partial step has been taken => drop constraint l */
600			iai(l) = static_cast<VectorXi::Scalar>(l);
601			delete_constraint(R, m_J, A, u, nEqCon, iq, l);
602			s(ip) = CI.row(ip).dot(x) + ci0(ip);
603
604			#ifdef EIQGUADPROG_TRACE_SOLVER
605			std::cerr << "Partial step has taken " << t << std::endl;
606			utils::print_vector("x", x);
607			utils::print_matrix("R", R);
608			utils::print_vector("A", A);
609			utils::print_vector("s", s);
610			#endif
611			STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
612
613			goto l2a;
614			}
615
616			} /* namespace solvers */
617			} /* namespace eiquadprog */