diff --git a/include/jrl-qp/GoldfarbIdnaniSolver.h b/include/jrl-qp/GoldfarbIdnaniSolver.h index e0ec9f1..1ff0d38 100644 --- a/include/jrl-qp/GoldfarbIdnaniSolver.h +++ b/include/jrl-qp/GoldfarbIdnaniSolver.h @@ -77,6 +77,7 @@ class JRLQP_DLLAPI GoldfarbIdnaniSolver : public DualSolver mutable internal::Workspace<> work_d_; internal::Workspace<> work_J_; internal::Workspace<> work_R_; + internal::Workspace<> work_tmp_; Problem pb_; }; diff --git a/src/GoldfarbIdnaniSolver.cpp b/src/GoldfarbIdnaniSolver.cpp index c2221bb..b768334 100644 --- a/src/GoldfarbIdnaniSolver.cpp +++ b/src/GoldfarbIdnaniSolver.cpp @@ -3,7 +3,6 @@ #include #include #include -#include #include namespace jrl::qp @@ -62,10 +61,8 @@ internal::InitTermination GoldfarbIdnaniSolver::init_() if(ret >= 0) return TerminationStatus::NON_POS_HESSIAN; auto J = work_J_.asMatrix(nbVar_, nbVar_, nbVar_); - auto x = work_x_.asVector(nbVar_); // J = L^-t - // x = -G^-1 * a switch(options_.gFactorization_) { case GFactorization::NONE: @@ -75,29 +72,29 @@ internal::InitTermination GoldfarbIdnaniSolver::init_() auto L = pb_.G.template triangularView(); J.setIdentity(); L.transpose().solveInPlace(J); - x = L.solve(pb_.a); - L.transpose().solveInPlace(x); // possible [OPTIM]: J already contains L^-T } break; case GFactorization::L_INV: { auto invL = pb_.G.template triangularView(); J = invL.transpose(); - x = invL * pb_.a; - x = J.template triangularView() * x; } break; case GFactorization::L_TINV: { auto invLT = pb_.G.template triangularView(); J = invLT; - x = invLT.transpose() * pb_.a; - x = invLT * x; } break; default: assert(false); } + + // x = -G^-1 * a + auto x = work_x_.asVector(nbVar_); + auto tmp = work_tmp_.asVector(nbVar_); + tmp.noalias() = J.template triangularView().transpose() * pb_.a; + x.noalias() = J.template triangularView() * tmp; x = -x; f_ = 0.5 * pb_.a.dot(x); @@ -291,6 +288,7 @@ void GoldfarbIdnaniSolver::resize_(int nbVar, int /*nbCstr*/, bool /*useBounds*/ work_d_.resize(nbVar); work_J_.resize(nbVar, nbVar); work_R_.resize(nbVar, nbVar); + work_tmp_.resize(nbVar); } }