.. _program_listing_file_include_eiquadprog_eiquadprog-fast.hpp:

Program Listing for File eiquadprog-fast.hpp
============================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_eiquadprog_eiquadprog-fast.hpp>` (``include/eiquadprog/eiquadprog-fast.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   //
   // Copyright (c) 2017 CNRS
   //
   // This file is part of eiquadprog.
   //
   // eiquadprog is free software: you can redistribute it and/or modify
   // it under the terms of the GNU Lesser General Public License as published by
   // the Free Software Foundation, either version 3 of the License, or
   //(at your option) any later version.
   
   // eiquadprog is distributed in the hope that it will be useful,
   // but WITHOUT ANY WARRANTY; without even the implied warranty of
   // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   // GNU Lesser General Public License for more details.
   
   // You should have received a copy of the GNU Lesser General Public License
   // along with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
   
   #ifndef EIQUADPROGFAST_HPP_
   #define EIQUADPROGFAST_HPP_
   
   #include <Eigen/Dense>
   
   #define OPTIMIZE_STEP_1_2  // compute s(x) = ci^T * x + ci0
   #define OPTIMIZE_COMPUTE_D
   #define OPTIMIZE_UPDATE_Z
   #define OPTIMIZE_HESSIAN_INVERSE
   #define OPTIMIZE_UNCONSTR_MINIM
   
   // #define USE_WARM_START
   // #define PROFILE_EIQUADPROG
   
   // #define DEBUG_STREAM(msg) std::cout<<msg;
   #define DEBUG_STREAM(msg)
   
   #ifdef PROFILE_EIQUADPROG
   #define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
   #define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
   #else
   #define START_PROFILER_EIQUADPROG_FAST(x)
   #define STOP_PROFILER_EIQUADPROG_FAST(x)
   #endif
   
   #define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
   #define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv"
   #define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
   #define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
   #define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
   #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_2 "EIQUADPROG_FAST STEP_2"
   #define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
   #define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
   #define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
   
   #define DEFAULT_MAX_ITER 1000
   
   #include "eiquadprog/eiquadprog-utils.hxx"
   
   namespace eiquadprog {
   
   namespace solvers {
   
   enum EiquadprogFast_status {
     EIQUADPROG_FAST_OPTIMAL = 0,
     EIQUADPROG_FAST_INFEASIBLE = 1,
     EIQUADPROG_FAST_UNBOUNDED = 2,
     EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
     EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
   };
   
   class EiquadprogFast {
     typedef Eigen::MatrixXd MatrixXd;
     typedef Eigen::VectorXd VectorXd;
     typedef Eigen::VectorXi VectorXi;
   
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
     EiquadprogFast();
     virtual ~EiquadprogFast();
   
     void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);
   
     int getMaxIter() const { return m_maxIter; }
   
     bool setMaxIter(int maxIter) {
       if (maxIter < 0) return false;
       m_maxIter = maxIter;
       return true;
     }
   
     size_t getActiveSetSize() const { return q; }
   
     int getIteratios() const { return iter; }
   
     double getObjValue() const { return f_value; }
   
     const VectorXd& getLagrangeMultipliers() const { return u; }
   
     const VectorXi& getActiveSet() const { return A; }
   
     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_;
   
    private:
     size_t m_nVars;
     size_t m_nEqCon;
     size_t m_nIneqCon;
   
     int m_maxIter;   
     double f_value;  
   
     Eigen::LLT<MatrixXd, Eigen::Lower> chol_;  // <nVars,nVars>::d
   
     MatrixXd R;  // <nVars,nVars>::d
   
     VectorXd s;  // <nIneqCon>::d
   
     VectorXd r;  // <nIneqCon+nEqCon>::d
   
     VectorXd u;  // <nIneqCon+nEqCon>::d
   
     VectorXd z;  // <nVars>::d
   
     VectorXd d;  //<nVars>::d
   
     VectorXd np;  //<nVars>::d
   
     VectorXi A;  // <nIneqCon+nEqCon>
   
     VectorXi iai;  // <nIneqCon>::i
   
     VectorXi iaexcl;  //<nIneqCon>::i
   
     VectorXd x_old;  // old value of x <nVars>::d
     VectorXd u_old;  // old value of u <nIneqCon+nEqCon>::d
     VectorXi A_old;  // old value of A <nIneqCon+nEqCon>::i
   
   #ifdef OPTIMIZE_ADD_CONSTRAINT
     VectorXd T1;  
   #endif
   
     size_t q;
   
     int iter;
   
     inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
   #ifdef OPTIMIZE_COMPUTE_D
       d.noalias() = J.adjoint() * np;
   #else
       d = J.adjoint() * np;
   #endif
     }
   
     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
       z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
   #endif
     }
   
     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));
     }
   
     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,
                                   size_t l);
   };
   
   } /* namespace solvers */
   } /* namespace eiquadprog */
   
   #endif /* EIQUADPROGFAST_HPP_ */