Repository: stack-of-tasks/eiquadprog (language: python)

Issue:
Topic/modernize



Discussion / hints:

Fix patch:
diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs
new file mode 100644
index 0000000..40cbdb5
--- /dev/null
+++ b/.git-blame-ignore-revs
@@ -0,0 +1,8 @@
+# format (Guilhem Saurel, 2022-09-05)
+7ff83998ab53df52eda4f8d1e99992c9a295a4ad
+
+# reformat for clang-format-12 (Guilhem Saurel, 2022-03-30)
+05b84fd4956c0ac55b306e8d836d0640913e8c01
+
+# clang-format using gepetto linters. (Olivier Stasse, 2020-04-25)
+f7875e67a4ca0a62bbe8adc3ec47b8668eda889d
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 0000000..6292b0e
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,30 @@
+ci:
+    autoupdate_branch: 'devel'
+repos:
+-   repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v14.0.6
+    hooks:
+    -   id: clang-format
+        args: [--style=Google]
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.3.0
+    hooks:
+    -   id: check-added-large-files
+    -   id: check-ast
+    -   id: check-executables-have-shebangs
+    -   id: check-json
+    -   id: check-merge-conflict
+    -   id: check-symlinks
+    -   id: check-toml
+    -   id: check-yaml
+    -   id: debug-statements
+    -   id: destroyed-symlinks
+    -   id: detect-private-key
+    -   id: end-of-file-fixer
+    -   id: fix-byte-order-marker
+    -   id: mixed-line-ending
+    -   id: trailing-whitespace
+-   repo: https://github.com/cheshirekow/cmake-format-precommit
+    rev: v0.6.13
+    hooks:
+    - id: cmake-format
diff --git a/CMakeLists.txt b/CMakeLists.txt
index dcf6a53..c0da035 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,69 +3,81 @@
 #
 # 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 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.
+# 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/>.
+# You should have received a copy of the GNU Lesser General Public License along
+# with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
 #
 
-CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
+cmake_minimum_required(VERSION 3.1)
 
 # Project properties
-SET(PROJECT_ORG stack-of-tasks)
-SET(PROJECT_NAME eiquadprog)
-SET(PROJECT_DESCRIPTION "C++ reimplementation of eiquadprog")
-SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
+set(PROJECT_ORG stack-of-tasks)
+set(PROJECT_NAME eiquadprog)
+set(PROJECT_DESCRIPTION "C++ reimplementation of eiquadprog")
+set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
 
 # Project options
-OPTION(TRACE_SOLVER "trace solver on stderr" OFF)
+option(TRACE_SOLVER "trace solver on stderr" OFF)
 
 # Project configuration
-SET(PROJECT_USE_CMAKE_EXPORT TRUE)
-SET(CXX_DISABLE_WERROR TRUE)
-SET(CMAKE_VERBOSE_MAKEFILE TRUE)
-SET(PROJECT_CUSTOM_HEADER_EXTENSION "hpp")
+set(PROJECT_USE_CMAKE_EXPORT TRUE)
+set(CXX_DISABLE_WERROR TRUE)
+set(CMAKE_VERBOSE_MAKEFILE TRUE)
+set(PROJECT_CUSTOM_HEADER_EXTENSION "hpp")
 
 # JRL-cmakemodule setup
-INCLUDE(cmake/base.cmake)
-INCLUDE(cmake/boost.cmake)
+include(cmake/base.cmake)
+include(cmake/boost.cmake)
 
 # Project definition
-COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
-PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
+compute_project_args(PROJECT_ARGS LANGUAGES CXX)
+project(${PROJECT_NAME} ${PROJECT_ARGS})
 
 # Project dependencies
-ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED)
-FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
+add_project_dependency(Eigen3 REQUIRED)
+if(BUILD_TESTING)
+  find_package(Boost REQUIRED COMPONENTS unit_test_framework)
+endif(BUILD_TESTING)
 
 # Main Library
-SET(${PROJECT_NAME}_HEADERS
-  include/${PROJECT_NAME}/eiquadprog.hpp
-  include/${PROJECT_NAME}/eiquadprog-fast.hpp
-  include/${PROJECT_NAME}/eiquadprog-rt.hpp
-  include/${PROJECT_NAME}/eiquadprog-rt.hxx
-  include/${PROJECT_NAME}/eiquadprog-utils.hxx
-  )
+set(${PROJECT_NAME}_HEADERS
+    include/${PROJECT_NAME}/eiquadprog.hpp
+    include/${PROJECT_NAME}/eiquadprog-fast.hpp
+    include/${PROJECT_NAME}/eiquadprog-rt.hpp
+    include/${PROJECT_NAME}/eiquadprog-rt.hxx
+    include/${PROJECT_NAME}/eiquadprog-utils.hxx)
 
-ADD_LIBRARY(${PROJECT_NAME} SHARED
-  src/eiquadprog-fast.cpp
-  src/eiquadprog.cpp
-  )
+add_library(${PROJECT_NAME} SHARED src/eiquadprog-fast.cpp src/eiquadprog.cpp)
 
-IF(TRACE_SOLVER)
-  TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PRIVATE TRACE_SOLVER)
-ENDIF(TRACE_SOLVER)
+if(TRACE_SOLVER)
 
-TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
-TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} INTERFACE $<INSTALL_INTERFACE:include>)
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
+endif(TRACE_SOLVER)
+target_compile_definitions(${PROJECT_NAME} PRIVATE EIQGUADPROG_TRACE_SOLVER)
 
-ADD_SUBDIRECTORY(tests)
+modernize_target_link_libraries(
+  ${PROJECT_NAME}
+  SCOPE
+  PUBLIC
+  TARGETS
+  Eigen3::Eigen
+  INCLUDE_DIRS
+  ${EIGEN3_INCLUDE_DIR})
+target_include_directories(${PROJECT_NAME}
+                           INTERFACE $<INSTALL_INTERFACE:include>)
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${TARGETS_EXPORT_NAME}
+  DESTINATION lib)
+
+if(BUILD_TESTING)
+  add_subdirectory(tests)
+endif(BUILD_TESTING)
diff --git a/README.md b/README.md
index f41725e..6c11675 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,8 @@
 # eiquadprog
 
 [![Pipeline status](https://gitlab.laas.fr/stack-of-tasks/eiquadprog/badges/master/pipeline.svg)](https://gitlab.laas.fr/stack-of-tasks/eiquadprog/commits/master)
-[![Coverage report](https://gitlab.laas.fr/stack-of-tasks/eiquadprog/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/stack-of-tasks/eiquadprog/master/coverage/)
+[![Coverage report](https://gitlab.laas.fr/stack-of-tasks/eiquadprog/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/stack-of-tasks/eiquadprog/master/coverage/)
+[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/stack-of-tasks/eiquadprog/master.svg)](https://results.pre-commit.ci/latest/github/stack-of-tasks/eiquadprog)
 
 This repo contains different C++ implementations of the algorithm of Goldfarb and Idnani for the solution of a (convex) Quadratic Programming problem by means of a dual method.
 
diff --git a/cmake b/cmake
index 7e797cb..a558ff9 160000
--- a/cmake
+++ b/cmake
@@ -1,1 +1,1 @@
-Subproject commit 7e797cbcae0947acce802c0bc63765a2b7c75828
+Subproject commit a558ff948ce65384b3ccd9e5cd9a295a2c0016bc
diff --git a/include/eiquadprog/eiquadprog-fast.hpp b/include/eiquadprog/eiquadprog-fast.hpp
index 9659c0e..32ad97d 100644
--- a/include/eiquadprog/eiquadprog-fast.hpp
+++ b/include/eiquadprog/eiquadprog-fast.hpp
@@ -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"
@@ -57,12 +58,12 @@
 
 #define DEFAULT_MAX_ITER 1000
 
+#include "eiquadprog/eiquadprog-utils.hxx"
+
 namespace eiquadprog {
 
 namespace solvers {
 
-#include "eiquadprog/eiquadprog-utils.hxx"
-
 /**
  * Possible states of the solver.
  */
@@ -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);
 };
 
diff --git a/include/eiquadprog/eiquadprog-rt.hpp b/include/eiquadprog/eiquadprog-rt.hpp
index eaf2e26..a397095 100644
--- a/include/eiquadprog/eiquadprog-rt.hpp
+++ b/include/eiquadprog/eiquadprog-rt.hpp
@@ -21,6 +21,8 @@
 
 #include <Eigen/Dense>
 
+#include "eiquadprog/eiquadprog-utils.hxx"
+
 #define OPTIMIZE_STEP_1_2         // compute s(x) = ci^T * x + ci0
 #define OPTIMIZE_COMPUTE_D        // use noalias
 #define OPTIMIZE_UPDATE_Z         // use noalias
@@ -48,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"
@@ -71,7 +74,6 @@ namespace eiquadprog {
 
 namespace solvers {
 
-#include "eiquadprog/eiquadprog-utils.hxx"
 /**
  * Possible states of the solver.
  */
@@ -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__ */
diff --git a/include/eiquadprog/eiquadprog-rt.hxx b/include/eiquadprog/eiquadprog-rt.hxx
index 2df1f7a..b96c7f2 100644
--- a/include/eiquadprog/eiquadprog-rt.hxx
+++ b/include/eiquadprog/eiquadprog-rt.hxx
@@ -26,7 +26,8 @@ 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
   // (containing the indices of the active constraints)
@@ -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,16 +59,15 @@ 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);
+    h = utils::distance(cc, ss);
     if (h == 0.0) continue;
     d(j) = 0.0;
     ss = ss / h;
@@ -80,7 +81,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 +104,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 +116,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;
@@ -148,7 +150,7 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX
   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
 
@@ -157,7 +159,7 @@ void RtEiquadprog<nVars, nEqCon, nIneqCon>::delete_constraint(typename RtMatrixX
   for (j = qq; j < iq; j++) {
     cc = R(j, j);
     ss = R(j + 1, j);
-    h = distance(cc, ss);
+    h = utils::distance(cc, ss);
     if (h == 0.0) continue;
     cc = cc / h;
     ss = ss / h;
@@ -187,10 +189,13 @@ 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) {
+    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
@@ -224,7 +229,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 +244,15 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
   }
 
   c2 = m_J.trace();
-#ifdef TRACE_SOLVER
-  print_matrix("m_J", m_J, nVars);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_matrix("m_J", m_J);
 #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,9 +269,9 @@ 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);
+  utils::print_vector("x", x);
 #endif
   STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM);
 
@@ -282,17 +287,18 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
     update_z(z, m_J, d, iq);
     update_r(R, r, d, iq);
 
-#ifdef TRACE_SOLVER
-    print_matrix("R", R, iq);
-    print_vector("z", z, nVars);
-    print_vector("r", r, iq);
-    print_vector("d", d, nVars);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+    utils::print_matrix("R", R);
+    utils::print_vector("z", z);
+    utils::print_vector("r", r);
+    utils::print_vector("d", d);
 #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;
@@ -319,7 +325,8 @@ RtEiquadprog_status RtEiquadprog<nVars, nEqCon, nIneqCon>::solve_quadprog(
   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 +335,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,8 +371,8 @@ l1:
     return RT_EIQUADPROG_MAX_ITER_REACHED;
   }
 
-#ifdef TRACE_SOLVER
-  print_vector("x", x, nVars);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_vector("x", x);
 #endif
   /* step 1: choose a violated constraint */
   for (i = nEqCon; i < iq; i++) {
@@ -391,16 +399,18 @@ l1:
   }
 #endif
   STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_1_2);
-#ifdef TRACE_SOLVER
-  print_vector("s", s, nIneqCon);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_vector("s", s);
 #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 +421,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);
@@ -434,15 +445,16 @@ 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);
+  utils::print_vector("np", np);
 #endif
   STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2);
 
 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,22 +463,24 @@ 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);
-  print_vector("u", u, iq + 1);
-  print_vector("d", d, nVars);
-  print_vector("A", A, iq + 1);
+  utils::print_vector("z", z);
+  utils::print_vector("r", r);
+  utils::print_vector("u", u);
+  utils::print_vector("d", d);
+  utils::print_vector("A", A);
 #endif
   STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2A);
 
   /* 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 +491,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,11 +523,11 @@ 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);
-    print_vector("A", A, iq + 1);
+    utils::print_vector("x", x);
+    utils::print_vector("z", z);
+    utils::print_vector("A", A);
 #endif
     STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
     goto l2a;
@@ -524,27 +541,27 @@ 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);
-  print_vector("r", r, iq + 1);
-  print_vector("A", A, iq + 1);
+  utils::print_vector("x", x);
+  utils::print_vector("u", u);
+  utils::print_vector("r", r);
+  utils::print_vector("A", A);
 #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);
+    utils::print_vector("x", x);
 #endif
     /* full step has taken */
     /* add constraint ip to the active set*/
     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
-      print_matrix("R", R, nVars);
-      print_vector("A", A, iq);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+      utils::print_matrix("R", R);
+      utils::print_vector("A", A);
 #endif
       for (i = 0; i < nIneqCon; i++) iai(i) = i;
       for (i = 0; i < iq; i++) {
@@ -557,9 +574,9 @@ l2a: /* Step 2a: determine step direction */
       goto l2; /* go to step 2 */
     } else
       iai(ip) = -1;
-#ifdef TRACE_SOLVER
-    print_matrix("R", R, nVars);
-    print_vector("A", A, iq);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+    utils::print_matrix("R", R);
+    utils::print_vector("A", A);
 #endif
     STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
     goto l1;
@@ -572,12 +589,12 @@ l2a: /* Step 2a: determine step direction */
   s(ip) = ci0(ip);
   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);
-  print_vector("A", A, iq);
-  print_vector("s", s, nIneqCon);
+  utils::print_vector("x", x);
+  utils::print_matrix("R", R);
+  utils::print_vector("A", A);
+  utils::print_vector("s", s);
 #endif
   STOP_PROFILER_EIQUADPROG_RT(PROFILE_EIQUADPROG_STEP_2C);
 
diff --git a/include/eiquadprog/eiquadprog-utils.hxx b/include/eiquadprog/eiquadprog-utils.hxx
index 4deaad4..d432590 100644
--- a/include/eiquadprog/eiquadprog-utils.hxx
+++ b/include/eiquadprog/eiquadprog-utils.hxx
@@ -2,6 +2,10 @@
 #define EIQUADPROG_UTILS_HPP_
 
 #include <Eigen/Core>
+#include <iostream>
+
+namespace eiquadprog {
+namespace utils {
 
 /// Compute sqrt(a^2 + b^2)
 template <typename Scalar>
@@ -20,12 +24,24 @@ 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) {
+  std::cerr << name << x.transpose() << std::endl;
+}
+template <class Derived>
+void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x) {
+  std::cerr << name << std::endl << x << 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_vector(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) {
+  print_vector(name, x);
 }
+template <class Derived>
+void print_matrix(const char *name, Eigen::MatrixBase<Derived> &x, int /*n*/) {
+  print_matrix(name, x);
+}
+
+}  // namespace utils
+}  // namespace eiquadprog
 
 #endif
diff --git a/include/eiquadprog/eiquadprog.hpp b/include/eiquadprog/eiquadprog.hpp
index 56e9a20..891002a 100644
--- a/include/eiquadprog/eiquadprog.hpp
+++ b/include/eiquadprog/eiquadprog.hpp
@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2019 CNRS
+// Copyright (c) 2019,2022 CNRS INRIA
 //
 // This file is part of eiquadprog.
 //
@@ -21,9 +21,9 @@
 
 /*
  FILE eiquadprog.hpp
- NOTE: this is a modified of uQuadProg++ package, working with Eigen data structures.
- uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++ originally developed by
- Luca Di Gaspero, working with ublas data structures.
+ NOTE: this is a modified of uQuadProg++ package, working with Eigen data
+ structures. uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++
+ originally developed by Luca Di Gaspero, working with ublas data structures.
  The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
  for the solution of a (convex) Quadratic Programming problem
  by means of a dual method.
@@ -41,22 +41,19 @@
  ci0: m
  x: n
  The function will return the cost of the solution written in the x vector or
- std::numeric_limits::infinity() if the problem is infeasible. In the latter case
- the value of the x vector is not correct.
- References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
- strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
- Notes:
+ std::numeric_limits::infinity() if the problem is infeasible. In the latter
+ case the value of the x vector is not correct. References: D. Goldfarb, A.
+ Idnani. A numerically stable dual method for solving strictly convex quadratic
+ programs. Mathematical Programming 27 (1983) pp. 1-33. Notes:
  1. pay attention in setting up the vectors ce0 and ci0.
  If the constraints of your problem are specified in the form
  A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
  2. The matrix G is modified within the function since it is used to compute
- the G = L^T L cholesky factorization for further computations inside the function.
- If you need the original matrix G you should make a copy of it and pass the copy
- to the function.
- The author will be grateful if the researchers using this software will
- acknowledge the contribution of this modified function and of Di Gaspero's
- original version in their research papers.
- LICENSE
+ the G = L^T L cholesky factorization for further computations inside the
+ function. If you need the original matrix G you should make a copy of it and
+ pass the copy to the function. The author will be grateful if the researchers
+ using this software will acknowledge the contribution of this modified function
+ and of Di Gaspero's original version in their research papers. LICENSE
  Copyright (2011) Benjamin Stephens
  Copyright (2010) Gael Guennebaud
  Copyright (2008) Angelo Furfaro
@@ -77,43 +74,72 @@
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
 
-#include <Eigen/Core>
 #include <Eigen/Cholesky>
+#include <Eigen/Core>
 
-#include <iostream>
-
+#include "eiquadprog/deprecated.hpp"
 #include "eiquadprog/eiquadprog-utils.hxx"
 
 // namespace internal {
 namespace eiquadprog {
 namespace solvers {
 
-inline void compute_d(Eigen::VectorXd& d, const Eigen::MatrixXd& J, const Eigen::VectorXd& np) {
-  d = J.adjoint() * np;
+inline void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J,
+                      const Eigen::VectorXd &np) {
+  d.noalias() = J.adjoint() * np;
 }
 
-inline void update_z(Eigen::VectorXd& z, const Eigen::MatrixXd& J, const Eigen::VectorXd& d, size_t iq) {
-  z = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
+inline void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J,
+                     const Eigen::VectorXd &d, size_t iq) {
+  z.noalias() = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
 }
 
-inline void update_r(const Eigen::MatrixXd& R, Eigen::VectorXd& r, const Eigen::VectorXd& d, size_t iq) {
-  r.head(iq) = R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solve(d.head(iq));
+inline void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r,
+                     const Eigen::VectorXd &d, size_t iq) {
+  r.head(iq) = d.head(iq);
+  R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
+      r.head(iq));
 }
 
-bool add_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXd& d, size_t& iq, double& R_norm);
-void delete_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXi& A, Eigen::VectorXd& u, size_t p,
-                       size_t& iq, size_t l);
-
-/* solve_quadprog2 is used when the Cholesky decomposition of the G
-   matrix is precomputed */
-double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol, double c1, Eigen::VectorXd& g0,
-                       const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0, const Eigen::MatrixXd& CI,
-                       const Eigen::VectorXd& ci0, Eigen::VectorXd& x, Eigen::VectorXi& A, size_t& q);
+bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d,
+                    size_t &iq, double &R_norm);
+void delete_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J,
+                       Eigen::VectorXi &A, Eigen::VectorXd &u, size_t p,
+                       size_t &iq, size_t l);
+
+double solve_quadprog(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
+                      double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE,
+                      const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI,
+                      const Eigen::VectorXd &ci0, Eigen::VectorXd &x,
+                      Eigen::VectorXi &A, size_t &q);
+
+double solve_quadprog(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
+                      double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE,
+                      const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI,
+                      const Eigen::VectorXd &ci0, Eigen::VectorXd &x,
+                      Eigen::VectorXd &y, Eigen::VectorXi &A, size_t &q);
+
+EIQUADPROG_DEPRECATED
+double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
+                       double c1, Eigen::VectorXd &g0,
+                       const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0,
+                       const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0,
+                       Eigen::VectorXd &x, Eigen::VectorXi &A, size_t &q) {
+  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, A, q);
+}
 
 /* solve_quadprog is used for on-demand QP solving */
-double solve_quadprog(Eigen::MatrixXd& G, Eigen::VectorXd& g0, const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
-                      const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0, Eigen::VectorXd& x,
-                      Eigen::VectorXi& activeSet, size_t& activeSetSize);
+double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0,
+                      const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0,
+                      const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0,
+                      Eigen::VectorXd &x, Eigen::VectorXi &activeSet,
+                      size_t &activeSetSize);
+
+double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0,
+                      const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0,
+                      const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0,
+                      Eigen::VectorXd &x, Eigen::VectorXd &y,
+                      Eigen::VectorXi &activeSet, size_t &activeSetSize);
 // }
 
 }  // namespace solvers
diff --git a/package.xml b/package.xml
index b69423c..d707e3c 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>eiquadprog</name>
-  <version>1.2.3</version>
+  <version>1.2.4</version>
   <description>
     Eiquadprog a QP solver using active sets
   </description>
diff --git a/src/eiquadprog-fast.cpp b/src/eiquadprog-fast.cpp
index b3efdee..b80ecbc 100644
--- a/src/eiquadprog-fast.cpp
+++ b/src/eiquadprog-fast.cpp
@@ -1,7 +1,7 @@
-#include <iostream>
-
 #include "eiquadprog/eiquadprog-fast.hpp"
 
+#include <iostream>
+
 namespace eiquadprog {
 namespace solvers {
 
@@ -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,16 +61,15 @@ 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);
+    h = utils::distance(cc, ss);
     if (h == 0.0) continue;
     d(j) = 0.0;
     ss = ss / h;
@@ -83,7 +83,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 +106,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 +117,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;
@@ -147,7 +149,7 @@ void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, Ve
   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
 
@@ -156,7 +158,7 @@ void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, Ve
   for (j = qq; j < iq; j++) {
     cc = R(j, j);
     ss = R(j + 1, j);
-    h = distance(cc, ss);
+    h = utils::distance(cc, ss);
     if (h == 0.0) continue;
     cc = cc / h;
     ss = ss / h;
@@ -184,20 +186,24 @@ 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);
+  assert(static_cast<size_t>(CI.rows()) == m_nIneqCon &&
+         static_cast<size_t>(CI.cols()) == m_nVars);
   assert(static_cast<size_t>(ci0.size()) == m_nIneqCon);
 
   size_t i, k, l;  // indices
@@ -248,8 +254,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
   }
 
   c2 = m_J.trace();
-#ifdef TRACE_SOLVER
-  print_matrix("m_J", m_J, nVars);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_matrix("m_J", m_J);
 #endif
 
   /* c1 * c2 is an estimate for cond(Hess) */
@@ -275,9 +281,9 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
 #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);
+  utils::print_vector("x", x);
 #endif
   STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
 
@@ -292,11 +298,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
     update_z(z, m_J, d, iq);
     update_r(R, r, d, iq);
 
-#ifdef TRACE_SOLVER
-    print_matrix("R", R, iq);
-    print_vector("z", z, nVars);
-    print_vector("r", r, iq);
-    print_vector("d", d, nVars);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+    utils::print_matrix("R", R);
+    utils::print_vector("z", z);
+    utils::print_vector("r", r);
+    utils::print_vector("d", d);
 #endif
 
     /* compute full step length t2: i.e.,
@@ -345,7 +351,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
        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")
@@ -378,8 +385,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
 
   START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
 
-#ifdef TRACE_SOLVER
-  print_vector("x", x, nVars);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_vector("x", x);
 #endif
   /* step 1: choose a violated constraint */
   for (i = nEqCon; i < iq; i++) {
@@ -406,13 +413,15 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
   }
 #endif
   STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2);
-#ifdef TRACE_SOLVER
-  print_vector("s", s, nIneqCon);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_vector("s", s);
 #endif
 
   STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
 
-  if (std::abs(psi) <= static_cast<double>(nIneqCon) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
+  if (std::abs(psi) <= static_cast<double>(nIneqCon) *
+                           std::numeric_limits<double>::epsilon() * c1 * c2 *
+                           100.0) {
     /* numerically there are not infeasibilities anymore */
     q = iq;
     //        DEBUG_STREAM("Optimal active
@@ -450,9 +459,9 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
 
   //      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);
+  utils::print_vector("np", np);
 #endif
   STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
 
@@ -471,13 +480,13 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
   /* 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);
-  print_vector("u", u, iq + 1);
-  print_vector("d", d, nVars);
-  print_vector("A", A, iq + 1);
+  utils::print_vector("z", z);
+  utils::print_vector("r", r);
+  utils::print_vector("u", u);
+  utils::print_vector("d", d);
+  utils::print_vector("A", A);
 #endif
   STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
 
@@ -506,8 +515,9 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
 
   /* 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_FAST(EIQUADPROG_FAST_STEP_2B);
 
@@ -527,11 +537,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
     u(iq) += t;
     iai(l) = static_cast<VectorXi::Scalar>(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;
-    print_vector("x", x, nVars);
-    print_vector("z", z, nVars);
-    print_vector("A", A, iq + 1);
+    utils::print_vector("x", x);
+    utils::print_vector("z", z);
+    utils::print_vector("A", A);
 #endif
     STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
     goto l2a;
@@ -545,27 +555,27 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
   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);
-  print_vector("r", r, iq + 1);
-  print_vector("A", A, iq + 1);
+  utils::print_vector("x", x);
+  utils::print_vector("u", u);
+  utils::print_vector("r", r);
+  utils::print_vector("A", A);
 #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);
+    utils::print_vector("x", x);
 #endif
     /* full step has taken */
     /* add constraint ip to the active set*/
     if (!add_constraint(R, m_J, d, iq, R_norm)) {
       iaexcl(ip) = 0;
       delete_constraint(R, m_J, A, u, nEqCon, iq, ip);
-#ifdef TRACE_SOLVER
-      print_matrix("R", R, nVars);
-      print_vector("A", A, iq);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+      utils::print_matrix("R", R);
+      utils::print_vector("A", A);
 #endif
       for (i = 0; i < nIneqCon; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
       for (i = 0; i < iq; i++) {
@@ -578,9 +588,9 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
       goto l2; /* go to step 2 */
     } else
       iai(ip) = -1;
-#ifdef TRACE_SOLVER
-    print_matrix("R", R, nVars);
-    print_vector("A", A, iq);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+    utils::print_matrix("R", R);
+    utils::print_vector("A", A);
 #endif
     STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
     goto l1;
@@ -591,12 +601,12 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
   delete_constraint(R, m_J, A, u, nEqCon, iq, l);
   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;
-  print_vector("x", x, nVars);
-  print_matrix("R", R, nVars);
-  print_vector("A", A, iq);
-  print_vector("s", s, nIneqCon);
+  utils::print_vector("x", x);
+  utils::print_matrix("R", R);
+  utils::print_vector("A", A);
+  utils::print_vector("s", s);
 #endif
   STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C);
 
diff --git a/src/eiquadprog.cpp b/src/eiquadprog.cpp
index e583920..746f4cc 100644
--- a/src/eiquadprog.cpp
+++ b/src/eiquadprog.cpp
@@ -5,8 +5,24 @@ namespace solvers {
 using namespace Eigen;
 
 /* solve_quadprog is used for on-demand QP solving */
-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) {
+
+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();
+
+  VectorXd y(p + m);
+
+  return solve_quadprog(G, g0, CE, ce0, CI, ci0, x, y, activeSet,
+                        activeSetSize);
+}
+
+double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
+                      const VectorXd &ce0, const MatrixXd &CI,
+                      const VectorXd &ci0, VectorXd &x, VectorXd &y,
+                      VectorXi &activeSet, size_t &activeSetSize) {
   LLT<MatrixXd, Lower> chol(G.cols());
   double c1;
   /* compute the trace of the original matrix G */
@@ -15,15 +31,32 @@ double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const Vecto
   /* decompose the matrix G in the form LL^T */
   chol.compute(G);
 
-  return solve_quadprog2(chol, c1, g0, CE, ce0, CI, ci0, x, activeSet, activeSetSize);
+  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
+                        activeSetSize);
+}
+
+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();
+
+  VectorXd y(p + m);
+
+  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
+                        activeSetSize);
 }
 
-/* solve_quadprog2 is used for when the Cholesky decomposition of G is pre-computed
+/* solve_quadprog2 is used for when the Cholesky decomposition of G is
+ * pre-computed
  * @param A Output vector containing the indexes of the active constraints.
  * @param q Output value representing the size of the active set.
  */
-double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
-                       const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A, size_t& q) {
+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,
+                      VectorXd &u, VectorXi &A, size_t &q) {
   size_t i, k, l; /* indices */
   size_t ip, me, mi;
   size_t n = g0.size();
@@ -31,13 +64,14 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
   size_t m = CI.cols();
   MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size());
 
-  VectorXd s(m + p), z(n), r(m + p), d(n), np(n), u(m + p);
+  VectorXd s(m + p), z(n), r(m + p), d(n), np(n);
   VectorXd x_old(n), u_old(m + p);
   double f_value, psi, c2, sum, ss, R_norm;
   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 */
-  //        VectorXi A(m + p); // Del Prete: active set is now an output parameter
+  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 */
+  //        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);
   VectorXi A_old(m + p), iai(m + p), iaexcl(m + p);
   //        int q;
@@ -45,7 +79,8 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
 
   me = p; /* number of equality constraints */
   mi = m; /* number of inequality constraints */
-  q = 0;  /* size of the active set A (containing the indices of the active constraints) */
+  q = 0;  /* size of the active set A (containing the indices of the active
+             constraints) */
 
   /*
    * Preprocessing phase
@@ -56,13 +91,14 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
   R.setZero();
   R_norm = 1.0; /* this variable will hold the norm of the matrix R */
 
-  /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
+  /* compute the inverse of the factorized matrix G^-1, this is the initial
+   * value for H */
   // J = L^-T
   J.setIdentity();
-  J = chol.matrixU().solve(J);
+  chol.matrixU().solveInPlace(J);
   c2 = J.trace();
-#ifdef TRACE_SOLVER
-  print_matrix("J", J, n);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_matrix("J", J);
 #endif
 
   /* c1 * c2 is an estimate for cond(G) */
@@ -72,13 +108,13 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
    * this is a feasible point in the dual space
    * x = G^-1 * g0
    */
-  x = chol.solve(g0);
-  x = -x;
+  x = -g0;
+  chol.solveInPlace(x);
   /* 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, n);
+  utils::print_vector("x", x);
 #endif
 
   /* Add equality constraints to the working set A */
@@ -88,17 +124,18 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
     compute_d(d, J, np);
     update_z(z, J, d, iq);
     update_r(R, r, d, iq);
-#ifdef TRACE_SOLVER
-    print_matrix("R", R, iq);
-    print_vector("z", z, n);
-    print_vector("r", r, iq);
-    print_vector("d", d, n);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+    utils::print_matrix("R", R);
+    utils::print_vector("z", z);
+    utils::print_vector("r", r);
+    utils::print_vector("d", d);
 #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;
@@ -123,8 +160,8 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
 
 l1:
   iter++;
-#ifdef TRACE_SOLVER
-  print_vector("x", x, n);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_vector("x", x);
 #endif
   /* step 1: choose a violated constraint */
   for (i = me; i < iq; i++) {
@@ -142,11 +179,13 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
     s(i) = sum;
     psi += std::min(0.0, sum);
   }
-#ifdef TRACE_SOLVER
-  print_vector("s", s, mi);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_vector("s", s);
 #endif
 
-  if (std::abs(psi) <= static_cast<double>(mi) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
+  if (std::abs(psi) <= static_cast<double>(mi) *
+                           std::numeric_limits<double>::epsilon() * c1 * c2 *
+                           100.0) {
     /* numerically there are not infeasibilities anymore */
     q = iq;
     return f_value;
@@ -176,29 +215,32 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
   /* add ip to the active set A */
   A(iq) = static_cast<VectorXi::Scalar>(ip);
 
-#ifdef TRACE_SOLVER
+#ifdef EIQGUADPROG_TRACE_SOLVER
   std::cerr << "Trying with constraint " << ip << std::endl;
-  print_vector("np", np, n);
+  utils::print_vector("np", np);
 #endif
 
 l2a: /* Step 2a: determine step direction */
-  /* compute z = H np: the step direction in the primal space (through J, see the paper) */
+  /* compute z = H np: the step direction in the primal space (through J, see
+   * the paper) */
   compute_d(d, J, np);
   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);
-#ifdef TRACE_SOLVER
+#ifdef EIQGUADPROG_TRACE_SOLVER
   std::cerr << "Step direction z" << std::endl;
-  print_vector("z", z, n);
-  print_vector("r", r, iq + 1);
-  print_vector("u", u, iq + 1);
-  print_vector("d", d, n);
-  print_vector("A", A, iq + 1);
+  utils::print_vector("z", z);
+  utils::print_vector("r", r);
+  utils::print_vector("u", u);
+  utils::print_vector("d", d);
+  utils::print_vector("A", A);
 #endif
 
   /* Step 2b: compute step length */
   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 */
   for (k = me; k < iq; k++) {
@@ -208,16 +250,19 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
       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
 
   /* Step 2c: determine new S-pair and take step: */
@@ -236,11 +281,11 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
     u(iq) += t;
     iai(l) = static_cast<VectorXi::Scalar>(l);
     delete_constraint(R, J, A, u, p, iq, l);
-#ifdef TRACE_SOLVER
+#ifdef EIQGUADPROG_TRACE_SOLVER
     std::cerr << " in dual space: " << f_value << std::endl;
-    print_vector("x", x, n);
-    print_vector("z", z, n);
-    print_vector("A", A, iq + 1);
+    utils::print_vector("x", x);
+    utils::print_vector("z", z);
+    utils::print_vector("A", A);
 #endif
     goto l2a;
   }
@@ -253,27 +298,27 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
 
   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, n);
-  print_vector("u", u, iq + 1);
-  print_vector("r", r, iq + 1);
-  print_vector("A", A, iq + 1);
+  utils::print_vector("x", x);
+  utils::print_vector("u", u);
+  utils::print_vector("r", r);
+  utils::print_vector("A", A);
 #endif
 
   if (t == t2) {
-#ifdef TRACE_SOLVER
+#ifdef EIQGUADPROG_TRACE_SOLVER
     std::cerr << "Full step has taken " << t << std::endl;
-    print_vector("x", x, n);
+    utils::print_vector("x", x);
 #endif
     /* full step has taken */
     /* add constraint ip to the active set*/
     if (!add_constraint(R, J, d, iq, R_norm)) {
       iaexcl(ip) = 0;
       delete_constraint(R, J, A, u, p, iq, ip);
-#ifdef TRACE_SOLVER
-      print_matrix("R", R, n);
-      print_vector("A", A, iq);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+      utils::print_matrix("R", R);
+      utils::print_vector("A", A);
 #endif
       for (i = 0; i < m; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
       for (i = 0; i < iq; i++) {
@@ -285,37 +330,38 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
       goto l2; /* go to step 2 */
     } else
       iai(ip) = -1;
-#ifdef TRACE_SOLVER
-    print_matrix("R", R, n);
-    print_vector("A", A, iq);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+    utils::print_matrix("R", R);
+    utils::print_vector("A", A);
 #endif
     goto l1;
   }
 
   /* a patial step has taken */
-#ifdef TRACE_SOLVER
+#ifdef EIQGUADPROG_TRACE_SOLVER
   std::cerr << "Partial step has taken " << t << std::endl;
-  print_vector("x", x, n);
+  utils::print_vector("x", x);
 #endif
   /* drop constraint l */
   iai(l) = static_cast<VectorXi::Scalar>(l);
   delete_constraint(R, J, A, u, p, iq, l);
-#ifdef TRACE_SOLVER
-  print_matrix("R", R, n);
-  print_vector("A", A, iq);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_matrix("R", R);
+  utils::print_vector("A", A);
 #endif
 
   s(ip) = CI.col(ip).dot(x) + ci0(ip);
 
-#ifdef TRACE_SOLVER
-  print_vector("s", s, mi);
+#ifdef EIQGUADPROG_TRACE_SOLVER
+  utils::print_vector("s", s);
 #endif
   goto l2a;
 }
 
-bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
+bool add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d, size_t &iq,
+                    double &R_norm) {
   size_t n = J.rows();
-#ifdef TRACE_SOLVER
+#ifdef EIQGUADPROG_TRACE_SOLVER
   std::cerr << "Add constraint " << iq << '/';
 #endif
   size_t j, k;
@@ -336,7 +382,7 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
        The i - 1 element of d has to be updated to h. */
     cc = d(j - 1);
     ss = d(j);
-    h = distance(cc, ss);
+    h = utils::distance(cc, ss);
     if (h == 0.0) continue;
     d(j) = 0.0;
     ss = ss / h;
@@ -361,7 +407,7 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
      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
 
@@ -372,9 +418,10 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
   return true;
 }
 
-void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t p, size_t& iq, size_t l) {
+void delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A, VectorXd &u,
+                       size_t p, size_t &iq, size_t l) {
   size_t n = R.rows();
-#ifdef TRACE_SOLVER
+#ifdef EIQGUADPROG_TRACE_SOLVER
   std::cerr << "Delete constraint " << l << ' ' << iq;
 #endif
   size_t i, j, k, qq = 0;
@@ -401,7 +448,7 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_
   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
 
@@ -410,7 +457,7 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_
   for (j = qq; j < iq; j++) {
     cc = R(j, j);
     ss = R(j + 1, j);
-    h = distance(cc, ss);
+    h = utils::distance(cc, ss);
     if (h == 0.0) continue;
     cc = cc / h;
     ss = ss / h;


Test patch:
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 61c8630..89690d5 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -1,36 +1,31 @@
 #
 # 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 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.
+# 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/>.
+# You should have received a copy of the GNU Lesser General Public License along
+# with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
 #
 
-ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
+add_definitions(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
 
-#test paths and names (without .cpp extension)
-SET(TESTS
-  eiquadprog-basic
-  eiquadprog-fast
-  eiquadprog-rt
-  eiquadprog-both
-  test-integration
-  )
+# test paths and names (without .cpp extension)
+set(TESTS eiquadprog-basic eiquadprog-fast eiquadprog-rt eiquadprog-both
+          test-integration)
 
-FOREACH(test ${TESTS})
-  ADD_UNIT_TEST(${test} ${test}.cpp)
-  TARGET_LINK_LIBRARIES(${test} ${PROJECT_NAME} Boost::unit_test_framework)
-ENDFOREACH(test ${TESTS})
+foreach(test ${TESTS})
+  add_unit_test(${test} ${test}.cpp)
+  target_link_libraries(${test} ${PROJECT_NAME} Boost::unit_test_framework)
+endforeach(test ${TESTS})
 
-ADD_LIBRARY(testab SHARED TestA.cpp TestB.cpp)
-TARGET_LINK_LIBRARIES(testab ${PROJECT_NAME})
-TARGET_LINK_LIBRARIES(test-integration testab)
+add_library(testab SHARED TestA.cpp TestB.cpp)
+target_link_libraries(testab ${PROJECT_NAME})
+target_link_libraries(test-integration testab)
diff --git a/tests/TestA.cpp b/tests/TestA.cpp
index 91f9823..9c1ac36 100644
--- a/tests/TestA.cpp
+++ b/tests/TestA.cpp
@@ -1,8 +1,7 @@
-#include <iostream>
+#include "TestA.hpp"
 
 #include <Eigen/Core>
-
-#include "TestA.hpp"
+#include <iostream>
 
 using namespace eiquadprog::solvers;
 using namespace eiquadprog::tests;
diff --git a/tests/TestB.cpp b/tests/TestB.cpp
index 70d36a8..6c28377 100644
--- a/tests/TestB.cpp
+++ b/tests/TestB.cpp
@@ -1,8 +1,7 @@
-#include <iostream>
+#include "TestB.hpp"
 
 #include <Eigen/Core>
-
-#include "TestB.hpp"
+#include <iostream>
 
 using namespace eiquadprog::solvers;
 namespace eiquadprog {
@@ -20,7 +19,8 @@ bool B::do_something() {
   bool rstatus = true;
 
   if (status != expected) {
-    std::cerr << "Status not to true for A_" << expected << " " << status << std::endl;
+    std::cerr << "Status not to true for A_" << expected << " " << status
+              << std::endl;
     rstatus = false;
   }
 
diff --git a/tests/eiquadprog-basic.cpp b/tests/eiquadprog-basic.cpp
index 3ba6b41..a3353a6 100644
--- a/tests/eiquadprog-basic.cpp
+++ b/tests/eiquadprog-basic.cpp
@@ -16,11 +16,9 @@
 // You should have received a copy of the GNU Lesser General Public License
 // along with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
 
-#include <iostream>
-
 #include <Eigen/Core>
-
 #include <boost/test/unit_test.hpp>
+#include <iostream>
 
 #include "eiquadprog/eiquadprog.hpp"
 
@@ -68,7 +66,8 @@ BOOST_AUTO_TEST_CASE(test_unbiased) {
 
   double val = 0.0;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   BOOST_CHECK_CLOSE(out, val, 1e-6);
 
@@ -105,7 +104,8 @@ BOOST_AUTO_TEST_CASE(test_biased) {
 
   double val = -1.;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   BOOST_CHECK_CLOSE(out, val, 1e-6);
 
@@ -146,7 +146,8 @@ BOOST_AUTO_TEST_CASE(test_equality_constraints) {
 
   double val = 0.25;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   BOOST_CHECK_CLOSE(out, val, 1e-6);
 
@@ -189,7 +190,8 @@ BOOST_AUTO_TEST_CASE(test_inequality_constraints) {
 
   double val = 1.;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   BOOST_CHECK_CLOSE(out, val, 1e-6);
 
@@ -235,7 +237,8 @@ BOOST_AUTO_TEST_CASE(test_full) {
 
   double val = 1.5;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   BOOST_CHECK_CLOSE(out, val, 1e-6);
 
@@ -274,7 +277,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_equalities) {
   Eigen::VectorXi activeSet(2);
   size_t activeSetSize;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   // DOES NOT WORK!?
   BOOST_WARN(std::isinf(out));
@@ -311,7 +315,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_inequalities) {
   Eigen::VectorXi activeSet(2);
   size_t activeSetSize;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   BOOST_CHECK(std::isinf(out));
 }
@@ -351,7 +356,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_constraints) {
   Eigen::VectorXi activeSet(3);
   size_t activeSetSize;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   BOOST_CHECK(std::isinf(out));
 }
@@ -380,7 +386,8 @@ BOOST_AUTO_TEST_CASE(test_unbounded) {
   Eigen::VectorXi activeSet(0);
   size_t activeSetSize;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   // DOES NOT WORK!?
   BOOST_WARN(std::isinf(out));
@@ -428,7 +435,8 @@ BOOST_AUTO_TEST_CASE(test_nonconvex) {
 
   double val = -1.;
 
-  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x, activeSet, activeSetSize);
+  double out = eiquadprog::solvers::solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq,
+                                                   x, activeSet, activeSetSize);
 
   // DOES NOT WORK!?
   BOOST_WARN_CLOSE(out, val, 1e-6);
diff --git a/tests/eiquadprog-both.cpp b/tests/eiquadprog-both.cpp
index ee746d3..6334659 100644
--- a/tests/eiquadprog-both.cpp
+++ b/tests/eiquadprog-both.cpp
@@ -16,11 +16,9 @@
 // You should have received a copy of the GNU Lesser General Public License
 // along with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
 
-#include <iostream>
-
 #include <Eigen/Core>
-
 #include <boost/test/unit_test.hpp>
+#include <iostream>
 
 #include "eiquadprog/eiquadprog-fast.hpp"
 #include "eiquadprog/eiquadprog-rt.hpp"
@@ -67,7 +65,8 @@ BOOST_AUTO_TEST_CASE(test_unbiased) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -108,7 +107,8 @@ BOOST_AUTO_TEST_CASE(test_biased) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -153,7 +153,8 @@ BOOST_AUTO_TEST_CASE(test_equality_constraints) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
diff --git a/tests/eiquadprog-fast.cpp b/tests/eiquadprog-fast.cpp
index ac679fb..1b5e42e 100644
--- a/tests/eiquadprog-fast.cpp
+++ b/tests/eiquadprog-fast.cpp
@@ -16,13 +16,11 @@
 // You should have received a copy of the GNU Lesser General Public License
 // along with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
 
-#include <iostream>
+#include "eiquadprog/eiquadprog-fast.hpp"
 
 #include <Eigen/Core>
-
 #include <boost/test/unit_test.hpp>
-
-#include "eiquadprog/eiquadprog-fast.hpp"
+#include <iostream>
 
 using namespace eiquadprog::solvers;
 
@@ -66,7 +64,8 @@ BOOST_AUTO_TEST_CASE(test_unbiased) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -108,7 +107,8 @@ BOOST_AUTO_TEST_CASE(test_biased) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -154,7 +154,8 @@ BOOST_AUTO_TEST_CASE(test_equality_constraints) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -202,7 +203,8 @@ BOOST_AUTO_TEST_CASE(test_inequality_constraints) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -253,7 +255,8 @@ BOOST_AUTO_TEST_CASE(test_full) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -296,7 +299,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_equalities) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_REDUNDANT_EQUALITIES;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 }
@@ -337,7 +341,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_inequalities) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_INFEASIBLE;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_WARN_EQUAL(status, expected);
   BOOST_CHECK(status != EIQUADPROG_FAST_OPTIMAL);
@@ -383,7 +388,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_constraints) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_INFEASIBLE;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_WARN_EQUAL(status, expected);
   BOOST_CHECK(status != EIQUADPROG_FAST_OPTIMAL);
@@ -416,7 +422,8 @@ BOOST_AUTO_TEST_CASE(test_unbounded) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_UNBOUNDED;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_WARN_EQUAL(status, expected);
   BOOST_WARN(status != EIQUADPROG_FAST_OPTIMAL);  // SHOULD pass!
@@ -467,7 +474,8 @@ BOOST_AUTO_TEST_CASE(test_nonconvex) {
 
   EiquadprogFast_status expected = EIQUADPROG_FAST_OPTIMAL;
 
-  EiquadprogFast_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  EiquadprogFast_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
diff --git a/tests/eiquadprog-rt.cpp b/tests/eiquadprog-rt.cpp
index 8dace58..217af52 100644
--- a/tests/eiquadprog-rt.cpp
+++ b/tests/eiquadprog-rt.cpp
@@ -16,13 +16,11 @@
 // You should have received a copy of the GNU Lesser General Public License
 // along with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
 
-#include <iostream>
+#include "eiquadprog/eiquadprog-rt.hpp"
 
 #include <Eigen/Core>
-
 #include <boost/test/unit_test.hpp>
-
-#include "eiquadprog/eiquadprog-rt.hpp"
+#include <iostream>
 
 using namespace eiquadprog::solvers;
 
@@ -65,7 +63,8 @@ BOOST_AUTO_TEST_CASE(test_unbiased) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -106,7 +105,8 @@ BOOST_AUTO_TEST_CASE(test_biased) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -151,7 +151,8 @@ BOOST_AUTO_TEST_CASE(test_equality_constraints) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -198,7 +199,8 @@ BOOST_AUTO_TEST_CASE(test_inequality_constraints) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -248,7 +250,8 @@ BOOST_AUTO_TEST_CASE(test_full) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
@@ -290,7 +293,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_equalities) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_REDUNDANT_EQUALITIES;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 }
@@ -330,7 +334,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_inequalities) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_INFEASIBLE;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_WARN_EQUAL(status, expected);
   BOOST_CHECK(status != RT_EIQUADPROG_OPTIMAL);
@@ -375,7 +380,8 @@ BOOST_AUTO_TEST_CASE(test_unfeasible_constraints) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_INFEASIBLE;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_WARN_EQUAL(status, expected);
   BOOST_CHECK(status != RT_EIQUADPROG_OPTIMAL);
@@ -407,7 +413,8 @@ BOOST_AUTO_TEST_CASE(test_unbounded) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_UNBOUNDED;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_WARN_EQUAL(status, expected);
   BOOST_WARN(status != RT_EIQUADPROG_OPTIMAL);  // SHOULD pass!
@@ -457,7 +464,8 @@ BOOST_AUTO_TEST_CASE(test_nonconvex) {
 
   RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
 
-  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
+  RtEiquadprog_status status =
+      qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
 
   BOOST_CHECK_EQUAL(status, expected);
 
diff --git a/tests/test-integration.cpp b/tests/test-integration.cpp
index f9fa999..6fc9f35 100644
--- a/tests/test-integration.cpp
+++ b/tests/test-integration.cpp
@@ -1,8 +1,6 @@
-#include <iostream>
-
 #include <Eigen/Core>
-
 #include <boost/test/unit_test.hpp>
+#include <iostream>
 
 #include "TestA.hpp"
 #include "TestB.hpp"