Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/eiquadprog.cpp	Lines:	131	204	64.2 %
Date:	2023-11-29 12:38:05	Branches:	145	350	41.4 %


#include <eiquadprog/eiquadprog.hpp>
namespace eiquadprog {
namespace solvers {

using namespace Eigen;

/* solve_quadprog is used for on-demand QP solving */

double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
                      const VectorXd &ce0, const MatrixXd &CI,
                      const VectorXd &ci0, VectorXd &x, VectorXi &activeSet,
                      size_t &activeSetSize) {
  Eigen::DenseIndex p = CE.cols();
  Eigen::DenseIndex m = CI.cols();

  VectorXd y(p + m);

  return solve_quadprog(G, g0, CE, ce0, CI, ci0, x, y, activeSet,
                        activeSetSize);
}

double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
                      const VectorXd &ce0, const MatrixXd &CI,
                      const VectorXd &ci0, VectorXd &x, VectorXd &y,
                      VectorXi &activeSet, size_t &activeSetSize) {
  LLT<MatrixXd, Lower> chol(G.cols());
  double c1;
  /* compute the trace of the original matrix G */
  c1 = G.trace();

  /* decompose the matrix G in the form LL^T */
  chol.compute(G);

  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
                        activeSetSize);
}

double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
                      const MatrixXd &CE, const VectorXd &ce0,
                      const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
                      VectorXi &activeSet, size_t &activeSetSize) {
  Eigen::DenseIndex p = CE.cols();
  Eigen::DenseIndex m = CI.cols();

  VectorXd y(p + m);

  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
                        activeSetSize);
}

/* solve_quadprog2 is used for when the Cholesky decomposition of G is
 * pre-computed
 * @param A Output vector containing the indexes of the active constraints.
 * @param q Output value representing the size of the active set.
 */
double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
                      const MatrixXd &CE, const VectorXd &ce0,
                      const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
                      VectorXd &u, VectorXi &A, size_t &q) {
  size_t i, k, l; /* indices */
  size_t ip, me, mi;
  size_t n = g0.size();
  size_t p = CE.cols();
  size_t m = CI.cols();
  MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size());

  VectorXd s(m + p), z(n), r(m + p), d(n), np(n);
  VectorXd x_old(n), u_old(m + p);
  double f_value, psi, c2, sum, ss, R_norm;
  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 */
  //        VectorXi A(m + p); // Del Prete: active set is now an output
  //        parameter
  if (static_cast<size_t>(A.size()) != m + p) A.resize(m + p);
  VectorXi A_old(m + p), iai(m + p), iaexcl(m + p);
  //        int q;
  size_t iq, iter = 0;

  me = p; /* number of equality constraints */
  mi = m; /* number of inequality constraints */
  q = 0;  /* size of the active set A (containing the indices of the active
             constraints) */

  /*
   * Preprocessing phase
   */

  /* initialize the matrix R */
  d.setZero();
  R.setZero();
  R_norm = 1.0; /* this variable will hold the norm of the matrix R */

  /* compute the inverse of the factorized matrix G^-1, this is the initial
   * value for H */
  // J = L^-T
  J.setIdentity();
  chol.matrixU().solveInPlace(J);
  c2 = J.trace();
#ifdef EIQGUADPROG_TRACE_SOLVER
  utils::print_matrix("J", J);
#endif

  /* c1 * c2 is an estimate for cond(G) */

  /*
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
   * this is a feasible point in the dual space
   * x = G^-1 * g0
   */
  x = -g0;
  chol.solveInPlace(x);
  /* 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

  /* Add equality constraints to the working set A */
  iq = 0;
  for (i = 0; i < me; i++) {
    np = CE.col(i);
    compute_d(d, J, np);
    update_z(z, 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);

    if (!add_constraint(R, J, d, iq, R_norm)) {
      // FIXME: it should raise an error
      // Equality constraints are linearly dependent
      return f_value;
    }
  }

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

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

  /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
  ss = 0.0;
  psi = 0.0; /* this value will contain the sum of all infeasibilities */
  ip = 0;    /* ip will be the index of the chosen violated constraint */
  for (i = 0; i < mi; i++) {
    iaexcl(i) = 1;
    sum = CI.col(i).dot(x) + ci0(i);
    s(i) = sum;
    psi += std::min(0.0, sum);
  }
#ifdef EIQGUADPROG_TRACE_SOLVER
  utils::print_vector("s", s);
#endif

  if (std::abs(psi) <= static_cast<double>(mi) *
                           std::numeric_limits<double>::epsilon() * c1 * c2 *
                           100.0) {
    /* numerically there are not infeasibilities anymore */
    q = iq;
    return f_value;
  }

  /* 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 */
  for (i = 0; i < mi; i++) {
    if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
      ss = s(i);
      ip = i;
    }
  }
  if (ss >= 0.0) {
    q = iq;
    return f_value;
  }

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

#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Trying with constraint " << ip << std::endl;
  utils::print_vector("np", np);
#endif

l2a: /* Step 2a: determine step direction */
  /* compute z = H np: the step direction in the primal space (through J, see
   * the paper) */
  compute_d(d, J, np);
  update_z(z, 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

  /* Step 2b: compute step length */
  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 */
  for (k = me; 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

  /* Step 2c: determine new S-pair and take step: */

  /* case (i): no step in primal or dual space */
  if (t >= inf) {
    /* QPP is infeasible */
    // FIXME: unbounded to raise
    q = iq;
    return inf;
  }
  /* 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, J, A, u, p, 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
    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, J, d, iq, R_norm)) {
      iaexcl(ip) = 0;
      delete_constraint(R, J, A, u, p, iq, ip);
#ifdef EIQGUADPROG_TRACE_SOLVER
      utils::print_matrix("R", R);
      utils::print_vector("A", A);
#endif
      for (i = 0; i < m; 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;
      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
    goto l1;
  }

  /* a patial step has taken */
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Partial step has taken " << t << std::endl;
  utils::print_vector("x", x);
#endif
  /* drop constraint l */
  iai(l) = static_cast<VectorXi::Scalar>(l);
  delete_constraint(R, J, A, u, p, iq, l);
#ifdef EIQGUADPROG_TRACE_SOLVER
  utils::print_matrix("R", R);
  utils::print_vector("A", A);
#endif

  s(ip) = CI.col(ip).dot(x) + ci0(ip);

#ifdef EIQGUADPROG_TRACE_SOLVER
  utils::print_vector("s", s);
#endif
  goto l2a;
}

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

  /* 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 = n - 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);
    for (k = 0; k < n; 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;
    }
  }
  /* 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 delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A, VectorXd &u,
                       size_t p, size_t &iq, size_t l) {
  size_t n = R.rows();
#ifdef EIQGUADPROG_TRACE_SOLVER
  std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
  size_t i, j, k, qq = 0;
  double cc, ss, h, xny, t1, t2;

  /* Find the index qq for active constraint l to be removed */
  for (i = p; i < iq; i++)
    if (static_cast<size_t>(A(i)) == 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 < n; 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;
    }
  }
}

}  // namespace solvers
}  // namespace eiquadprog


Generated by: GCOVR (Version 4.2)

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