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

core: rename macro variable

parent ccb3f9ed
Pipeline #17926 failed with stage
in 9 minutes and 11 seconds
...@@ -63,8 +63,8 @@ ADD_LIBRARY(${PROJECT_NAME} SHARED ...@@ -63,8 +63,8 @@ ADD_LIBRARY(${PROJECT_NAME} SHARED
) )
IF(TRACE_SOLVER) IF(TRACE_SOLVER)
TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PRIVATE TRACE_SOLVER)
ENDIF(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} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} INTERFACE $<INSTALL_INTERFACE:include>) TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} INTERFACE $<INSTALL_INTERFACE:include>)
......
...@@ -41,7 +41,7 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nV ...@@ -41,7 +41,7 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nV
typename RtMatrixX<nVars, nVars>::d& J, typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm) { typename RtVectorX<nVars>::d& d, int& iq, double& R_norm) {
// int n=J.rows(); // int n=J.rows();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/'; std::cerr << "Add constraint " << iq << '/';
#endif #endif
int j, k; int j, k;
...@@ -102,7 +102,7 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nV ...@@ -102,7 +102,7 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(typename RtMatrixX<nV
into column iq - 1 of R into column iq - 1 of R
*/ */
R.col(iq - 1).head(iq) = d.head(iq); R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << iq << std::endl; std::cerr << iq << std::endl;
#endif #endif
...@@ -120,7 +120,7 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX ...@@ -120,7 +120,7 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX
typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq, typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq,
int l) { int l) {
// int n = J.rows(); // int n = J.rows();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq; std::cerr << "Delete constraint " << l << ' ' << iq;
#endif #endif
int i, j, k; int i, j, k;
...@@ -148,7 +148,7 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX ...@@ -148,7 +148,7 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX
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 */ /* constraint has been fully removed */
iq--; iq--;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << '/' << iq << std::endl; std::cerr << '/' << iq << std::endl;
#endif #endif
...@@ -238,7 +238,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog( ...@@ -238,7 +238,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
} }
c2 = m_J.trace(); c2 = m_J.trace();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("m_J", m_J, nVars); print_matrix("m_J", m_J, nVars);
#endif #endif
...@@ -264,7 +264,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog( ...@@ -264,7 +264,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
#endif #endif
/* and compute the current solution value */ /* and compute the current solution value */
f_value = 0.5 * g0.dot(x); f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Unconstrained solution: " << f_value << std::endl; std::cerr << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
#endif #endif
...@@ -282,7 +282,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog( ...@@ -282,7 +282,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
update_z(z, m_J, d, iq); update_z(z, m_J, d, iq);
update_r(R, r, d, iq); update_r(R, r, d, iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, iq); print_matrix("R", R, iq);
print_vector("z", z, nVars); print_vector("z", z, nVars);
print_vector("r", r, iq); print_vector("r", r, iq);
...@@ -363,7 +363,7 @@ l1: ...@@ -363,7 +363,7 @@ l1:
return RT_EIQUADPROG_MAX_ITER_REACHED; return RT_EIQUADPROG_MAX_ITER_REACHED;
} }
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("x", x, nVars); print_vector("x", x, nVars);
#endif #endif
/* step 1: choose a violated constraint */ /* step 1: choose a violated constraint */
...@@ -391,7 +391,7 @@ l1: ...@@ -391,7 +391,7 @@ l1:
} }
#endif #endif
STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_2); STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_2);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("s", s, nIneqCon); print_vector("s", s, nIneqCon);
#endif #endif
...@@ -434,7 +434,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */ ...@@ -434,7 +434,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
// DEBUG_STREAM("Add constraint "<<ip<<" to active set\n") // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Trying with constraint " << ip << std::endl; std::cerr << "Trying with constraint " << ip << std::endl;
print_vector("np", np, nVars); print_vector("np", np, nVars);
#endif #endif
...@@ -453,7 +453,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -453,7 +453,7 @@ l2a: /* Step 2a: determine step direction */
} }
/* 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); update_r(R, r, d, iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step direction z" << std::endl; std::cerr << "Step direction z" << std::endl;
print_vector("z", z, nVars); print_vector("z", z, nVars);
print_vector("r", r, iq + 1); print_vector("r", r, iq + 1);
...@@ -485,7 +485,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -485,7 +485,7 @@ l2a: /* Step 2a: determine step direction */
/* the step is chosen as the minimum of t1 and t2 */ /* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2); t = std::min(t1, t2);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") "; std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif #endif
STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2B); STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2B);
...@@ -506,7 +506,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -506,7 +506,7 @@ l2a: /* Step 2a: determine step direction */
u(iq) += t; u(iq) += t;
iai(l) = l; iai(l) = l;
delete_constraint(R, m_J, A, u, iq, 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; std::cerr << " in dual space: " << f_value << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
print_vector("z", z, nVars); print_vector("z", z, nVars);
...@@ -524,7 +524,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -524,7 +524,7 @@ l2a: /* Step 2a: determine step direction */
u.head(iq) -= t * r.head(iq); u.head(iq) -= t * r.head(iq);
u(iq) += t; u(iq) += t;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << " in both spaces: " << f_value << std::endl; std::cerr << " in both spaces: " << f_value << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
print_vector("u", u, iq + 1); print_vector("u", u, iq + 1);
...@@ -533,7 +533,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -533,7 +533,7 @@ l2a: /* Step 2a: determine step direction */
#endif #endif
if (t == t2) { if (t == t2) {
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Full step has taken " << t << std::endl; std::cerr << "Full step has taken " << t << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
#endif #endif
...@@ -542,7 +542,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -542,7 +542,7 @@ l2a: /* Step 2a: determine step direction */
if (!add_constraint(R, m_J, d, iq, R_norm)) { if (!add_constraint(R, m_J, d, iq, R_norm)) {
iaexcl(ip) = 0; iaexcl(ip) = 0;
delete_constraint(R, m_J, A, u, iq, ip); delete_constraint(R, m_J, A, u, iq, ip);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, nVars); print_matrix("R", R, nVars);
print_vector("A", A, iq); print_vector("A", A, iq);
#endif #endif
...@@ -557,7 +557,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -557,7 +557,7 @@ l2a: /* Step 2a: determine step direction */
goto l2; /* go to step 2 */ goto l2; /* go to step 2 */
} else } else
iai(ip) = -1; iai(ip) = -1;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, nVars); print_matrix("R", R, nVars);
print_vector("A", A, iq); print_vector("A", A, iq);
#endif #endif
...@@ -572,7 +572,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -572,7 +572,7 @@ l2a: /* Step 2a: determine step direction */
s(ip) = ci0(ip); 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; std::cerr << "Partial step has taken " << t << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
print_matrix("R", R, nVars); print_matrix("R", R, nVars);
......
...@@ -21,11 +21,11 @@ inline Scalar distance(Scalar a, Scalar b) { ...@@ -21,11 +21,11 @@ inline Scalar distance(Scalar a, Scalar b) {
template <class Derived> template <class Derived>
void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) { void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cerr << name << x.transpose() << std::endl; std::cerr << name << x.transpose() << std::endl;
} }
template <class Derived> template <class Derived>
void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) { void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cerr << name << std::endl << x << std::endl; std::cerr << name << std::endl << x << std::endl;
} }
#endif #endif
...@@ -44,7 +44,7 @@ void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) { ...@@ -44,7 +44,7 @@ void EiquadprogFast::reset(size_t nVars, size_t nEqCon, size_t nIneqCon) {
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(); size_t nVars = J.rows();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/'; std::cerr << "Add constraint " << iq << '/';
#endif #endif
size_t j, k; size_t j, k;
...@@ -105,7 +105,7 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_ ...@@ -105,7 +105,7 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_
into column iq - 1 of R into column iq - 1 of R
*/ */
R.col(iq - 1).head(iq) = d.head(iq); R.col(iq - 1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << iq << std::endl; std::cerr << iq << std::endl;
#endif #endif
...@@ -119,7 +119,7 @@ into column iq - 1 of R ...@@ -119,7 +119,7 @@ into column iq - 1 of R
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 l) {
size_t nVars = R.rows(); size_t nVars = R.rows();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq; std::cerr << "Delete constraint " << l << ' ' << iq;
#endif #endif
size_t i, j, k; size_t i, j, k;
...@@ -147,7 +147,7 @@ void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, Ve ...@@ -147,7 +147,7 @@ void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, Ve
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 */ /* constraint has been fully removed */
iq--; iq--;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << '/' << iq << std::endl; std::cerr << '/' << iq << std::endl;
#endif #endif
...@@ -248,7 +248,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const ...@@ -248,7 +248,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
} }
c2 = m_J.trace(); c2 = m_J.trace();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("m_J", m_J, nVars); print_matrix("m_J", m_J, nVars);
#endif #endif
...@@ -275,7 +275,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const ...@@ -275,7 +275,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
#endif #endif
/* and compute the current solution value */ /* and compute the current solution value */
f_value = 0.5 * g0.dot(x); f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Unconstrained solution: " << f_value << std::endl; std::cerr << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
#endif #endif
...@@ -292,7 +292,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const ...@@ -292,7 +292,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
update_z(z, m_J, d, iq); update_z(z, m_J, d, iq);
update_r(R, r, d, iq); update_r(R, r, d, iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, iq); print_matrix("R", R, iq);
print_vector("z", z, nVars); print_vector("z", z, nVars);
print_vector("r", r, iq); print_vector("r", r, iq);
...@@ -378,7 +378,7 @@ l1: ...@@ -378,7 +378,7 @@ l1:
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1); START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("x", x, nVars); print_vector("x", x, nVars);
#endif #endif
/* step 1: choose a violated constraint */ /* step 1: choose a violated constraint */
...@@ -406,7 +406,7 @@ l1: ...@@ -406,7 +406,7 @@ l1:
} }
#endif #endif
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2); STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("s", s, nIneqCon); print_vector("s", s, nIneqCon);
#endif #endif
...@@ -450,7 +450,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */ ...@@ -450,7 +450,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
// DEBUG_STREAM("Add constraint "<<ip<<" to active set\n") // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n")
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Trying with constraint " << ip << std::endl; std::cerr << "Trying with constraint " << ip << std::endl;
print_vector("np", np, nVars); print_vector("np", np, nVars);
#endif #endif
...@@ -471,7 +471,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -471,7 +471,7 @@ l2a: /* Step 2a: determine step direction */
/* compute N* np (if q > 0): the negative of the /* compute N* np (if q > 0): the negative of the
step direction in the dual space */ step direction in the dual space */
update_r(R, r, d, iq); update_r(R, r, d, iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step direction z" << std::endl; std::cerr << "Step direction z" << std::endl;
print_vector("z", z, nVars); print_vector("z", z, nVars);
print_vector("r", r, iq + 1); print_vector("r", r, iq + 1);
...@@ -506,7 +506,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -506,7 +506,7 @@ l2a: /* Step 2a: determine step direction */
/* the step is chosen as the minimum of t1 and t2 */ /* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2); t = std::min(t1, t2);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") "; std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif #endif
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B); STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
...@@ -527,7 +527,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -527,7 +527,7 @@ l2a: /* Step 2a: determine step direction */
u(iq) += t; u(iq) += t;
iai(l) = static_cast<VectorXi::Scalar>(l); iai(l) = static_cast<VectorXi::Scalar>(l);
delete_constraint(R, m_J, A, u, nEqCon, iq, l); delete_constraint(R, m_J, A, u, nEqCon, iq, l);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << " in dual space: " << f_value << std::endl; std::cerr << " in dual space: " << f_value << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
print_vector("z", z, nVars); print_vector("z", z, nVars);
...@@ -545,7 +545,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -545,7 +545,7 @@ l2a: /* Step 2a: determine step direction */
u.head(iq) -= t * r.head(iq); u.head(iq) -= t * r.head(iq);
u(iq) += t; u(iq) += t;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << " in both spaces: " << f_value << std::endl; std::cerr << " in both spaces: " << f_value << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
print_vector("u", u, iq + 1); print_vector("u", u, iq + 1);
...@@ -554,7 +554,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -554,7 +554,7 @@ l2a: /* Step 2a: determine step direction */
#endif #endif
if (t == t2) { if (t == t2) {
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Full step has taken " << t << std::endl; std::cerr << "Full step has taken " << t << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
#endif #endif
...@@ -563,7 +563,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -563,7 +563,7 @@ l2a: /* Step 2a: determine step direction */
if (!add_constraint(R, m_J, d, iq, R_norm)) { if (!add_constraint(R, m_J, d, iq, R_norm)) {
iaexcl(ip) = 0; iaexcl(ip) = 0;
delete_constraint(R, m_J, A, u, nEqCon, iq, ip); delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, nVars); print_matrix("R", R, nVars);
print_vector("A", A, iq); print_vector("A", A, iq);
#endif #endif
...@@ -578,7 +578,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -578,7 +578,7 @@ l2a: /* Step 2a: determine step direction */
goto l2; /* go to step 2 */ goto l2; /* go to step 2 */
} else } else
iai(ip) = -1; iai(ip) = -1;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, nVars); print_matrix("R", R, nVars);
print_vector("A", A, iq); print_vector("A", A, iq);
#endif #endif
...@@ -591,7 +591,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -591,7 +591,7 @@ l2a: /* Step 2a: determine step direction */
delete_constraint(R, m_J, A, u, nEqCon, iq, l); delete_constraint(R, m_J, A, u, nEqCon, iq, l);
s(ip) = CI.row(ip).dot(x) + ci0(ip); s(ip) = CI.row(ip).dot(x) + ci0(ip);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Partial step has taken " << t << std::endl; std::cerr << "Partial step has taken " << t << std::endl;
print_vector("x", x, nVars); print_vector("x", x, nVars);
print_matrix("R", R, nVars); print_matrix("R", R, nVars);
......
...@@ -84,7 +84,7 @@ double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const ...@@ -84,7 +84,7 @@ double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const
J.setIdentity(); J.setIdentity();
chol.matrixU().solveInPlace(J); chol.matrixU().solveInPlace(J);
c2 = J.trace(); c2 = J.trace();
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("J", J, n); print_matrix("J", J, n);
#endif #endif
...@@ -99,7 +99,7 @@ double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const ...@@ -99,7 +99,7 @@ double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const
chol.solveInPlace(x); chol.solveInPlace(x);
/* and compute the current solution value */ /* and compute the current solution value */
f_value = 0.5 * g0.dot(x); f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Unconstrained solution: " << f_value << std::endl; std::cerr << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x, n); print_vector("x", x, n);
#endif #endif
...@@ -111,7 +111,7 @@ double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const ...@@ -111,7 +111,7 @@ double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const
compute_d(d, J, np); compute_d(d, J, np);
update_z(z, J, d, iq); update_z(z, J, d, iq);
update_r(R, r, d, iq); update_r(R, r, d, iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_matrix("R", R, iq); print_matrix("R", R, iq);
print_vector("z", z, n); print_vector("z", z, n);
print_vector("r", r, iq); print_vector("r", r, iq);
...@@ -147,7 +147,7 @@ double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const ...@@ -147,7 +147,7 @@ double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const
l1: l1:
iter++; iter++;
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("x", x, n); print_vector("x", x, n);
#endif #endif
/* step 1: choose a violated constraint */ /* step 1: choose a violated constraint */
...@@ -166,7 +166,7 @@ l1: ...@@ -166,7 +166,7 @@ l1:
s(i) = sum; s(i) = sum;
psi += std::min(0.0, sum); psi += std::min(0.0, sum);
} }
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
print_vector("s", s, mi); print_vector("s", s, mi);
#endif #endif
...@@ -200,7 +200,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */ ...@@ -200,7 +200,7 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
/* add ip to the active set A */ /* add ip to the active set A */
A(iq) = static_cast<VectorXi::Scalar>(ip); A(iq) = static_cast<VectorXi::Scalar>(ip);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Trying with constraint " << ip << std::endl; std::cerr << "Trying with constraint " << ip << std::endl;
print_vector("np", np, n); print_vector("np", np, n);
#endif #endif
...@@ -211,7 +211,7 @@ l2a: /* Step 2a: determine step direction */ ...@@ -211,7 +211,7 @@ l2a: /* Step 2a: determine step direction */
update_z(z, J, d, iq); update_z(z, 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); update_r(R, r, d, iq);
#ifdef TRACE_SOLVER #ifdef EIQGUADPROG_TRACE_SOLVER
std::cerr << "Step direction z" << std::endl; std::cerr << "Step direction z" << std::endl;
print_vector("z", z, n); print_vector("z", z, n);