GCC Code Coverage Report

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2019,2022 CNRS INRIA
3			//
4			// This file is part of eiquadprog.
5			//
6			// eiquadprog is free software: you can redistribute it and/or modify
7			// it under the terms of the GNU Lesser General Public License as published by
8			// the Free Software Foundation, either version 3 of the License, or
9			//(at your option) any later version.
10
11			// eiquadprog is distributed in the hope that it will be useful,
12			// but WITHOUT ANY WARRANTY; without even the implied warranty of
13			// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14			// GNU Lesser General Public License for more details.
15
16			// You should have received a copy of the GNU Lesser General Public License
17			// along with eiquadprog. If not, see <https://www.gnu.org/licenses/>.
18
19			#ifndef _EIGEN_QUADSOLVE_HPP_
20			#define _EIGEN_QUADSOLVE_HPP_
21
22			/*
23			FILE eiquadprog.hpp
24			NOTE: this is a modified of uQuadProg++ package, working with Eigen data
25			structures. uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++
26			originally developed by Luca Di Gaspero, working with ublas data structures.
27			The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
28			for the solution of a (convex) Quadratic Programming problem
29			by means of a dual method.
30			The problem is in the form:
31			min 0.5 * x G x + g0 x
32			s.t.
33			CE^T x + ce0 = 0
34			CI^T x + ci0 >= 0
35			The matrix and vectors dimensions are as follows:
36			G: n * n
37			g0: n
38			CE: n * p
39			ce0: p
40			CI: n * m
41			ci0: m
42			x: n
43			The function will return the cost of the solution written in the x vector or
44			std::numeric_limits::infinity() if the problem is infeasible. In the latter
45			case the value of the x vector is not correct. References: D. Goldfarb, A.
46			Idnani. A numerically stable dual method for solving strictly convex quadratic
47			programs. Mathematical Programming 27 (1983) pp. 1-33. Notes:
48			1. pay attention in setting up the vectors ce0 and ci0.
49			If the constraints of your problem are specified in the form
50			A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
51			2. The matrix G is modified within the function since it is used to compute
52			the G = L^T L cholesky factorization for further computations inside the
53			function. If you need the original matrix G you should make a copy of it and
54			pass the copy to the function. The author will be grateful if the researchers
55			using this software will acknowledge the contribution of this modified function
56			and of Di Gaspero's original version in their research papers. LICENSE
57			Copyright (2011) Benjamin Stephens
58			Copyright (2010) Gael Guennebaud
59			Copyright (2008) Angelo Furfaro
60			Copyright (2006) Luca Di Gaspero
61			This file is a porting of QuadProg++ routine, originally developed
62			by Luca Di Gaspero, exploiting uBlas data structures for vectors and
63			matrices instead of native C++ array.
64			uquadprog is free software; you can redistribute it and/or modify
65			it under the terms of the GNU General Public License as published by
66			the Free Software Foundation; either version 2 of the License, or
67			(at your option) any later version.
68			uquadprog is distributed in the hope that it will be useful,
69			but WITHOUT ANY WARRANTY; without even the implied warranty of
70			MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
71			GNU General Public License for more details.
72			You should have received a copy of the GNU General Public License
73			along with uquadprog; if not, write to the Free Software
74			Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
75			*/
76
77			#include <Eigen/Cholesky>
78			#include <Eigen/Core>
79
80			#include "eiquadprog/deprecated.hpp"
81			#include "eiquadprog/eiquadprog-utils.hxx"
82
83			// namespace internal {
84			namespace eiquadprog {
85			namespace solvers {
86
87		12	inline void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J,
88			const Eigen::VectorXd &np) {
89	3/6 ✓ Branch 2 taken 12 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 12 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 12 times. ✗ Branch 9 not taken.	12	d.noalias() = J.adjoint() * np;
90		12	}
91
92		12	inline void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J,
93			const Eigen::VectorXd &d, size_t iq) {
94	4/8 ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 12 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 12 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 12 times. ✗ Branch 14 not taken.	12	z.noalias() = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
95		12	}
96
97		12	inline void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r,
98			const Eigen::VectorXd &d, size_t iq) {
99	2/4 ✓ Branch 2 taken 12 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 12 times. ✗ Branch 6 not taken.	12	r.head(iq) = d.head(iq);
100	2/4 ✓ Branch 2 taken 12 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 12 times. ✗ Branch 6 not taken.	24	R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
101	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	r.head(iq));
102		12	}
103
104			bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d,
105			size_t &iq, double &R_norm);
106			void delete_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J,
107			Eigen::VectorXi &A, Eigen::VectorXd &u, size_t p,
108			size_t &iq, size_t l);
109
110			double solve_quadprog(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
111			double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE,
112			const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI,
113			const Eigen::VectorXd &ci0, Eigen::VectorXd &x,
114			Eigen::VectorXi &A, size_t &q);
115
116			double solve_quadprog(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
117			double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE,
118			const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI,
119			const Eigen::VectorXd &ci0, Eigen::VectorXd &x,
120			Eigen::VectorXd &y, Eigen::VectorXi &A, size_t &q);
121
122			EIQUADPROG_DEPRECATED
123			inline double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
124			double c1, Eigen::VectorXd &g0,
125			const Eigen::MatrixXd &CE,
126			const Eigen::VectorXd &ce0,
127			const Eigen::MatrixXd &CI,
128			const Eigen::VectorXd &ci0, Eigen::VectorXd &x,
129			Eigen::VectorXi &A, size_t &q) {
130			return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, A, q);
131			}
132
133			/* solve_quadprog is used for on-demand QP solving */
134			double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0,
135			const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0,
136			const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0,
137			Eigen::VectorXd &x, Eigen::VectorXi &activeSet,
138			size_t &activeSetSize);
139
140			double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0,
141			const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0,
142			const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0,
143			Eigen::VectorXd &x, Eigen::VectorXd &y,
144			Eigen::VectorXi &activeSet, size_t &activeSetSize);
145			// }
146
147			} // namespace solvers
148			} // namespace eiquadprog
149
150			#endif
151