Verified Commit f2b788de authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: rename MACRO var

parent 4eb174fc
......@@ -63,8 +63,8 @@ ADD_LIBRARY(${PROJECT_NAME} SHARED
)
IF(TRACE_SOLVER)
TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PRIVATE TRACE_SOLVER)
ENDIF(TRACE_SOLVER)
TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PRIVATE EIQGUADPROG_TRACE_SOLVER)
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} INTERFACE $<INSTALL_INTERFACE:include>)
......
......@@ -26,9 +26,10 @@ namespace eiquadprog {
namespace solvers {
template <int nVars, int nEqCon, int nIneqCon>
RtEiquadprog<nVars, nEqCon, nIneqCon>::RtEiquadprog() : solver_return_(std::numeric_limits<double>::infinity()) {
RtEiquadprog<nVars, nEqCon, nIneqCon>::RtEiquadprog()
: solver_return_(std::numeric_limits<double>::infinity()) {
m_maxIter = DEFAULT_MAX_ITER;
q = 0; // size of the active set A
q = 0; // size of the active set A
// (containing the indices of the active constraints)
is_inverse_provided_ = false;
}
......@@ -37,11 +38,12 @@ template <int nVars, int nEqCon, int nIneqCon>
RtEiquadprog<nVars, nEqCon, nIneqCon>::~RtEiquadprog() {}
template <int nVars, int nEqCon, int nIneqCon>
bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm) {
bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(
typename RtMatrixX<nVars, nVars>::d &R,
typename RtMatrixX<nVars, nVars>::d &J, typename RtVectorX<nVars>::d &d,
int &iq, double &R_norm) {
// int n=J.rows();
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/';
#endif
int j, k;
......@@ -57,17 +59,17 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nV
decreasing j */
for (j = nVars - 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. */
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;
if (h == 0.0)
continue;
d(j) = 0.0;
ss = ss / h;
cc = cc / h;
......@@ -80,7 +82,8 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nV
xny = ss / (1.0 + cc);
// #define OPTIMIZE_ADD_CONSTRAINT
#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than the original
#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;
......@@ -102,7 +105,7 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nV
into column iq - 1 of R
*/
R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << iq << std::endl;
#endif
......@@ -114,13 +117,13 @@ into column iq - 1 of R
}
template <int nVars, int nEqCon, int nIneqCon>
void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX<nVars, nVars>::d& R,
typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nIneqCon + nEqCon>::i& A,
typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq,
int l) {
void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(
typename RtMatrixX<nVars, nVars>::d &R,
typename RtMatrixX<nVars, nVars>::d &J,
typename RtVectorX<nIneqCon + nEqCon>::i &A,
typename RtVectorX<nIneqCon + nEqCon>::d &u, int &iq, int l) {
// int n = J.rows();
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
int i, j, k;
......@@ -145,20 +148,23 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX
u(iq - 1) = u(iq);
A(iq) = 0;
u(iq) = 0.0;
for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
for (j = 0; j < iq; j++)
R(j, iq - 1) = 0.0;
/* constraint has been fully removed */
iq--;
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << '/' << iq << std::endl;
#endif
if (iq == 0) return;
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;
if (h == 0.0)
continue;
cc = cc / h;
ss = ss / h;
R(j + 1, j) = 0.0;
......@@ -187,24 +193,27 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX
template <int nVars, int nEqCon, int nIneqCon>
RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
const typename RtMatrixX<nVars, nVars>::d& Hess, const typename RtVectorX<nVars>::d& g0,
const typename RtMatrixX<nEqCon, nVars>::d& CE, const typename RtVectorX<nEqCon>::d& ce0,
const typename RtMatrixX<nIneqCon, nVars>::d& CI, const typename RtVectorX<nIneqCon>::d& ci0,
typename RtVectorX<nVars>::d& x) {
int i, k, l; // indices
int ip; // index of the chosen violated constraint
int iq; // current number of active constraints
double psi; // current sum of constraint violations
double c1; // Hessian trace
double c2; // Hessian Chowlesky factor trace
double ss; // largest constraint violation (negative for violation)
double R_norm; // norm of matrix R
const typename RtMatrixX<nVars, nVars>::d &Hess,
const typename RtVectorX<nVars>::d &g0,
const typename RtMatrixX<nEqCon, nVars>::d &CE,
const typename RtVectorX<nEqCon>::d &ce0,
const typename RtMatrixX<nIneqCon, nVars>::d &CI,
const typename RtVectorX<nIneqCon>::d &ci0,
typename RtVectorX<nVars>::d &x) {
int i, k, l; // indices
int ip; // index of the chosen violated constraint
int iq; // current number of active constraints
double psi; // current sum of constraint violations
double c1; // Hessian trace
double c2; // Hessian Chowlesky 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
iter = 0; // active-set iteration number
/*
* Preprocessing phase
......@@ -224,7 +233,8 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
R.setZero(nVars, nVars);
R_norm = 1.0;
/* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
/* 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_RT(PROFILE_EIQUADPROG_CHOWLESKY_INVERSE);
......@@ -238,16 +248,15 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
}
c2 = m_J.trace();
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("m_J", m_J, nVars);
#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
* 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_RT(PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM);
if (is_inverse_provided_) {
......@@ -264,7 +273,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
#endif
/* and compute the current solution value */
f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x, nVars);
#endif
......@@ -277,22 +286,24 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
for (i = 0; i < nEqCon; i++) {
START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1);
// np = CE.row(i); // does not compile if nEqCon=0
for (int tmp = 0; tmp < nVars; tmp++) np[tmp] = CE(i, tmp);
for (int tmp = 0; tmp < nVars; tmp++)
np[tmp] = CE(i, tmp);
compute_d(d, m_J, np);
update_z(z, m_J, d, iq);
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, iq);
print_vector("z", z, nVars);
print_vector("r", r, iq);
print_vector("d", d, nVars);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
/* 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
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;
......@@ -316,10 +327,12 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_ADD_EQ_CONSTR);
/* set iai = K \ A */
for (i = 0; i < nIneqCon; i++) iai(i) = i;
for (i = 0; i < nIneqCon; i++)
iai(i) = i;
#ifdef USE_WARM_START
// DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
// 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);
......@@ -328,10 +341,11 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
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 */
/* 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
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")
......@@ -363,7 +377,7 @@ l1:
return RT_EIQUADPROG_MAX_ITER_REACHED;
}
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("x", x, nVars);
#endif
/* step 1: choose a violated constraint */
......@@ -391,16 +405,18 @@ l1:
}
#endif
STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_2);
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("s", s, nIneqCon);
#endif
STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1);
if (std::abs(psi) <= nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
if (std::abs(psi) <=
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")
// DEBUG_STREAM("Optimal active
// set:\n"<<A.head(iq).transpose()<<"\n\n")
return RT_EIQUADPROG_OPTIMAL;
}
......@@ -411,7 +427,8 @@ l1:
l2: /* Step 2: check for feasibility and determine a new S-pair */
START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2);
// find constraint with highest violation (what about normalizing constraints?)
// 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);
......@@ -426,7 +443,8 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
/* set np = n(ip) */
// np = CI.row(ip); // does not compile if nIneqCon=0
for (int tmp = 0; tmp < nVars; tmp++) np[tmp] = CI(ip, tmp);
for (int tmp = 0; tmp < nVars; tmp++)
np[tmp] = CI(ip, tmp);
/* set u = (u 0)^T */
u(iq) = 0.0;
/* add ip to the active set A */
......@@ -434,7 +452,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
// DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Trying with constraint " << ip << std::endl;
print_vector("np", np, nVars);
#endif
......@@ -442,7 +460,8 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
l2a: /* Step 2a: determine step direction */
START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2A);
/* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
/* 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) {
......@@ -451,9 +470,10 @@ l2a: /* Step 2a: determine step direction */
} else {
update_z(z, m_J, d, iq);
}
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
/* compute N* np (if q > 0): the negative of the step direction in the dual
* space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step direction z" << std::endl;
print_vector("z", z, nVars);
print_vector("r", r, iq + 1);
......@@ -466,7 +486,8 @@ l2a: /* Step 2a: determine step direction */
/* Step 2b: compute step length */
START_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2B);
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
/* 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)
......@@ -477,16 +498,19 @@ l2a: /* Step 2a: determine step direction */
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
/* 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 TRACE_SOLVER
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
<< ") ";
#endif
STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2B);
......@@ -506,7 +530,7 @@ l2a: /* Step 2a: determine step direction */
u(iq) += t;
iai(l) = l;
delete_constraint(R, m_J, A, u, iq, l);
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << " in dual space: " << f_value << std::endl;
print_vector("x", x, nVars);
print_vector("z", z, nVars);
......@@ -524,7 +548,7 @@ l2a: /* Step 2a: determine step direction */
u.head(iq) -= t * r.head(iq);
u(iq) += t;
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << " in both spaces: " << f_value << std::endl;
print_vector("x", x, nVars);
print_vector("u", u, iq + 1);
......@@ -533,7 +557,7 @@ l2a: /* Step 2a: determine step direction */
#endif
if (t == t2) {
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Full step has taken " << t << std::endl;
print_vector("x", x, nVars);
#endif
......@@ -542,11 +566,12 @@ l2a: /* Step 2a: determine step direction */
if (!add_constraint(R, m_J, d, iq, R_norm)) {
iaexcl(ip) = 0;
delete_constraint(R, m_J, A, u, iq, ip);
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, nVars);
print_vector("A", A, iq);
#endif
for (i = 0; i < nIneqCon; i++) iai(i) = i;
for (i = 0; i < nIneqCon; i++)
iai(i) = i;
for (i = 0; i < iq; i++) {
A(i) = A_old(i);
iai(A(i)) = -1;
......@@ -557,7 +582,7 @@ l2a: /* Step 2a: determine step direction */
goto l2; /* go to step 2 */
} else
iai(ip) = -1;
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, nVars);
print_vector("A", A, iq);
#endif
......@@ -570,9 +595,10 @@ l2a: /* Step 2a: determine step direction */
delete_constraint(R, m_J, A, u, iq, l);
// s(ip) = CI.row(ip).dot(x) + ci0(ip); // does not compile if nIneqCon=0
s(ip) = ci0(ip);
for (int tmp = 0; tmp < nVars; tmp++) s(ip) += CI(ip, tmp) * x[tmp];
for (int tmp = 0; tmp < nVars; tmp++)
s(ip) += CI(ip, tmp) * x[tmp];
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Partial step has taken " << t << std::endl;
print_vector("x", x, nVars);
print_matrix("R", R, nVars);
......
......@@ -4,8 +4,7 @@
#include <Eigen/Core>
/// Compute sqrt(a^2 + b^2)
template <typename Scalar>
inline Scalar distance(Scalar a, Scalar b) {
template <typename Scalar> inline Scalar distance(Scalar a, Scalar b) {
Scalar a1, b1, t;
a1 = std::abs(a);
b1 = std::abs(b);
......@@ -20,12 +19,12 @@ inline Scalar distance(Scalar a, Scalar b) {
}
template <class Derived>
void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cerr << name << x.transpose() << std::endl;
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;
void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x, int n) {
std::cerr << name << std::endl << x << std::endl;
}
#endif
......@@ -7,7 +7,7 @@ namespace solvers {
EiquadprogFast::EiquadprogFast() {
m_maxIter = DEFAULT_MAX_ITER;
q = 0; // size of the active set A (containing the indices
q = 0; // size of the active set A (containing the indices
// of the active constraints)
is_inverse_provided_ = false;
m_nVars = 0;
......@@ -42,9 +42,10 @@ void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
#endif
}
bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
bool EiquadprogFast::add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d,
size_t &iq, double &R_norm) {
size_t nVars = J.rows();
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/';
#endif
size_t j, k;
......@@ -60,17 +61,17 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_
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. */
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;
if (h == 0.0)
continue;
d(j) = 0.0;
ss = ss / h;
cc = cc / h;
......@@ -83,7 +84,8 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_
xny = ss / (1.0 + cc);
// #define OPTIMIZE_ADD_CONSTRAINT
#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than the original
#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;
......@@ -105,7 +107,7 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_
into column iq - 1 of R
*/
R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << iq << std::endl;
#endif
......@@ -116,10 +118,11 @@ into column iq - 1 of R
return true;
}
void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
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 TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
size_t i, j, k;
......@@ -144,20 +147,23 @@ void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, Ve
u(iq - 1) = u(iq);
A(iq) = 0;
u(iq) = 0.0;
for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
for (j = 0; j < iq; j++)
R(j, iq - 1) = 0.0;
/* constraint has been fully removed */
iq--;
#ifdef TRACE_SOLVER
#ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << '/' << iq << std::endl;
#endif
if (iq == 0) return;
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;
if (h == 0.0)
continue;
cc = cc / h;
ss = ss / h;
R(j + 1, j) = 0.0;
......@@ -184,36 +190,40 @@ void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, Ve
}
}
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) {
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);
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>(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>(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);