Skip to content
GitLab
Commit 59e13e74 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

reformat for standard line length

parent 9102494e
Pipeline #17945 passed with stage
in 49 seconds
Hide whitespace changes
Inline Side-by-side
include/eiquadprog/eiquadprog-fast.hpp
View file @ 59e13e74
......@@ -49,7 +49,8 @@
#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \
"EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
......@@ -132,8 +133,10 @@ class EiquadprogFast {
* s.t. CE x + ce0 = 0
* CI x + ci0 >= 0
*/
EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0,
const MatrixXd& CE, const VectorXd& ce0,
const MatrixXd& CI, const VectorXd& ci0,
VectorXd& x);
MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
bool is_inverse_provided_;
......@@ -148,7 +151,8 @@ class EiquadprogFast {
Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the
/// matrix of active constraints
MatrixXd R; // <nVars,nVars>::d
/// CI*x+ci0
......@@ -182,8 +186,8 @@ class EiquadprogFast {
/// initialized as [1, ..., 1, .]
/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
/// if adding ineq constraint i fails => iaexcl(i)=0
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
/// iaexcl(i)=1 otherwise
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
/// constraints iaexcl(i)=1 otherwise
VectorXi iaexcl; //<nIneqCon>::i
VectorXd x_old; // old value of x <nVars>::d
......@@ -194,7 +198,8 @@ class EiquadprogFast {
VectorXd T1; /// tmp variable used in add_constraint
#endif
/// size of the active set A (containing the indices of the active constraints)
/// size of the active set A (containing the indices of the active
/// constraints)
size_t q;
/// number of active-set iterations
......@@ -208,7 +213,8 @@ class EiquadprogFast {
#endif
}
inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, size_t iq) {
inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,
size_t iq) {
#ifdef OPTIMIZE_UPDATE_Z
z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
#else
......@@ -216,14 +222,18 @@ class EiquadprogFast {
#endif
}
inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, size_t iq) {
inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d,
size_t iq) {
r.head(iq) = d.head(iq);
R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
r.head(iq));
}
inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm);
inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq,
double& R_norm);
inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
VectorXd& u, size_t nEqCon, size_t& iq,
size_t l);
};
......
include/eiquadprog/eiquadprog-rt.hpp
View file @ 59e13e74
......@@ -20,6 +20,7 @@
#define __eiquadprog_rt_hpp__
#include <Eigen/Dense>
#include "eiquadprog/eiquadprog-utils.hxx"
#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
......@@ -49,7 +50,8 @@
#define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1"
#define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1"
#define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2"
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM \
"EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
#define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2"
#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
......@@ -118,7 +120,10 @@ class RtEiquadprog {
/**
* @return The Lagrange multipliers
*/
const typename RtVectorX<nIneqCon + nEqCon>::d& getLagrangeMultipliers() const { return u; }
const typename RtVectorX<nIneqCon + nEqCon>::d& getLagrangeMultipliers()
const {
return u;
}
/**
* Return the active set, namely the indeces of active constraints.
......@@ -128,7 +133,9 @@ class RtEiquadprog {
* is the size of the active set.
* @return The set of indexes of the active constraints.
*/
const typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const { return A; }
const typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const {
return A;
}
/**
* solves the problem
......@@ -136,12 +143,14 @@ class RtEiquadprog {
* s.t. CE x + ce0 = 0
* CI x + ci0 >= 0
*/
RtEiquadprog_status 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);
RtEiquadprog_status 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);
typename RtMatrixX<nVars, nVars>::d m_J; // J * J' = Hessian
bool is_inverse_provided_;
......@@ -153,7 +162,8 @@ class RtEiquadprog {
Eigen::LLT<typename RtMatrixX<nVars, nVars>::d, Eigen::Lower> chol_;
double solver_return_;
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the
/// matrix of active constraints
typename RtMatrixX<nVars, nVars>::d R;
/// CI*x+ci0
......@@ -187,8 +197,8 @@ class RtEiquadprog {
/// initialized as [1, ..., 1, .]
/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
/// if adding ineq constraint i fails => iaexcl(i)=0
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
/// iaexcl(i)=1 otherwise
/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
/// constraints iaexcl(i)=1 otherwise
typename RtVectorX<nIneqCon>::i iaexcl;
typename RtVectorX<nVars>::d x_old; // old value of x
......@@ -199,7 +209,8 @@ class RtEiquadprog {
typename RtVectorX<nVars>::d T1; // tmp vector
#endif
/// size of the active set A (containing the indices of the active constraints)
/// size of the active set A (containing the indices of the active
/// constraints)
int q;
/// number of active-set iterations
......@@ -220,7 +231,8 @@ class RtEiquadprog {
return a1 * std::sqrt(2.0);
}
inline void compute_d(typename RtVectorX<nVars>::d& d, const typename RtMatrixX<nVars, nVars>::d& J,
inline void compute_d(typename RtVectorX<nVars>::d& d,
const typename RtMatrixX<nVars, nVars>::d& J,
const typename RtVectorX<nVars>::d& np) {
#ifdef OPTIMIZE_COMPUTE_D
d.noalias() = J.adjoint() * np;
......@@ -229,7 +241,8 @@ class RtEiquadprog {
#endif
}
inline void update_z(typename RtVectorX<nVars>::d& z, const typename RtMatrixX<nVars, nVars>::d& J,
inline void update_z(typename RtVectorX<nVars>::d& z,
const typename RtMatrixX<nVars, nVars>::d& J,
const typename RtVectorX<nVars>::d& d, int iq) {
#ifdef OPTIMIZE_UPDATE_Z
z.noalias() = J.rightCols(nVars - iq) * d.tail(nVars - iq);
......@@ -238,24 +251,31 @@ class RtEiquadprog {
#endif
}
inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R, typename RtVectorX<nIneqCon + nEqCon>::d& r,
inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R,
typename RtVectorX<nIneqCon + nEqCon>::d& r,
const typename RtVectorX<nVars>::d& d, int iq) {
r.head(iq) = d.head(iq);
R.topLeftCorner(iq, iq).template triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
R.topLeftCorner(iq, iq)
.template triangularView<Eigen::Upper>()
.solveInPlace(r.head(iq));
}
bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
typename RtMatrixX<nVars, nVars>::d& J,
typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
void 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 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);
};
} /* namespace solvers */
} /* namespace eiquadprog */
#include "eiquadprog/eiquadprog-rt.hxx"
/* --- Details -------------------------------------------------------------------- */
/* --- Details
* -------------------------------------------------------------------- */
#endif /* __eiquadprog_rt_hpp__ */
include/eiquadprog/eiquadprog-rt.hxx
View file @ 59e13e74
......@@ -29,7 +29,7 @@ template <int nVars, int nEqCon, int nIneqCon>
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;
}
......@@ -68,8 +68,7 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(
cc = d(j - 1);
ss = d(j);
h = utils::distance(cc, ss);
if (h == 0.0)
continue;
if (h == 0.0) continue;
d(j) = 0.0;
ss = ss / h;
cc = cc / h;
......@@ -82,8 +81,8 @@ bool RtEiquadprog<nVars, nEqCon, nIneqCon>::add_constraint(
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;
......@@ -148,23 +147,20 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(
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 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 = utils::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;
......@@ -200,20 +196,20 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
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
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
......@@ -286,8 +282,7 @@ 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);
......@@ -303,7 +298,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
the contraint becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
......@@ -327,8 +322,7 @@ 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
......@@ -345,7 +339,7 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
the contraint becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
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")
......@@ -443,8 +437,7 @@ 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 */
......@@ -501,7 +494,7 @@ l2a: /* Step 2a: determine step direction */
/* 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
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
......@@ -570,8 +563,7 @@ l2a: /* Step 2a: determine step direction */
utils::print_matrix("R", R, nVars);
utils::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;
......@@ -595,8 +587,7 @@ 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 EIQGUADPROG_TRACE_SOLVER
std::cerr << "Partial step has taken " << t << std::endl;
......
include/eiquadprog/eiquadprog-utils.hxx
View file @ 59e13e74
......@@ -8,7 +8,8 @@ namespace eiquadprog {
namespace utils {
/// 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);
......@@ -31,6 +32,7 @@ void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) {
std::cerr << name << std::endl << x << std::endl;
}
}} // namespace eiquadprog::utils
} // namespace utils
} // namespace eiquadprog
#endif
include/eiquadprog/eiquadprog.hpp
View file @ 59e13e74
......@@ -142,7 +142,7 @@ double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0,
Eigen::VectorXi &activeSet, size_t &activeSetSize);
// }
} // namespace solvers
} // namespace eiquadprog
} // namespace solvers
} // namespace eiquadprog
#endif
src/eiquadprog-fast.cpp
View file @ 59e13e74
......@@ -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;
......@@ -70,8 +70,7 @@ bool EiquadprogFast::add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d,
cc = d(j - 1);
ss = d(j);
h = utils::distance(cc, ss);
if (h == 0.0)
continue;
if (h == 0.0) continue;
d(j) = 0.0;
ss = ss / h;
cc = cc / h;
......@@ -84,8 +83,8 @@ bool EiquadprogFast::add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d,
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;
......@@ -147,23 +146,20 @@ void EiquadprogFast::delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A,
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 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 = utils::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;
......@@ -210,20 +206,20 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(
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
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
iter = 0; // active-set iteration number
/*
* Preprocessing phase
......@@ -338,8 +334,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(
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);
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
......@@ -357,7 +352,7 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(
becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
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")
......@@ -582,8 +577,7 @@ l2a: /* Step 2a: determine step direction */
utils::print_matrix("R", R, nVars);
utils::print_vector("A", A, iq);
#endif
for (i = 0; i < nIneqCon; i++)
iai(i) = static_cast<VectorXi::Scalar>(i);
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;
......
src/eiquadprog.cpp
View file @ 59e13e74
......@@ -10,7 +10,6 @@ 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();
......@@ -40,7 +39,6 @@ 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();
......@@ -74,8 +72,7 @@ double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
* 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);
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;
......@@ -138,7 +135,7 @@ double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
the contraint becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
......@@ -159,8 +156,7 @@ double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
}
/* set iai = K \ A */
for (i = 0; i < mi; i++)
iai(i) = static_cast<VectorXi::Scalar>(i);
for (i = 0; i < mi; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
l1:
iter++;
......@@ -257,7 +253,7 @@ l2a: /* Step 2a: determine step direction */
/* 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
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
......@@ -324,8 +320,7 @@ l2a: /* Step 2a: determine step direction */
utils::print_matrix("R", R, n);
utils::print_vector("A", A, iq);
#endif
for (i = 0; i < m; i++)
iai(i) = static_cast<VectorXi::Scalar>(i);
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;
......@@ -388,8 +383,7 @@ bool add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d, size_t &iq,
cc = d(j - 1);
ss = d(j);
h = utils::distance(cc, ss);
if (h == 0.0)
continue;
if (h == 0.0) continue;
d(j) = 0.0;