Jaesung Oh / Mbed 2 deprecated HydraulicControlBoard_SW

HCB with MPC

Dependencies:   mbed Eigen FastPWM

quadprog.cpp@25:f83396e3d25c, 2019-10-14 (annotated)

Committer:
jsoh91
Date:
Mon Oct 14 10:08:13 2019 +0000
Revision:
25:f83396e3d25c
withMPC

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jsoh91 25:f83396e3d25c 1 /*
jsoh91 25:f83396e3d25c 2 File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $
jsoh91 25:f83396e3d25c 3
jsoh91 25:f83396e3d25c 4 Author: Luca Di Gaspero
jsoh91 25:f83396e3d25c 5 DIEGM - University of Udine, Italy
jsoh91 25:f83396e3d25c 6 luca.digaspero@uniud.it
jsoh91 25:f83396e3d25c 7 http://www.diegm.uniud.it/digaspero/
jsoh91 25:f83396e3d25c 8
jsoh91 25:f83396e3d25c 9 This software may be modified and distributed under the terms
jsoh91 25:f83396e3d25c 10 of the MIT license. See the LICENSE file for details.
jsoh91 25:f83396e3d25c 11
jsoh91 25:f83396e3d25c 12 */
jsoh91 25:f83396e3d25c 13
jsoh91 25:f83396e3d25c 14 #include <iostream>
jsoh91 25:f83396e3d25c 15 #include <algorithm>
jsoh91 25:f83396e3d25c 16 #include <cmath>
jsoh91 25:f83396e3d25c 17 #include <limits>
jsoh91 25:f83396e3d25c 18 #include <sstream>
jsoh91 25:f83396e3d25c 19 #include <stdexcept>
jsoh91 25:f83396e3d25c 20 #include "quadprog.h"
jsoh91 25:f83396e3d25c 21 #include "math.h"
jsoh91 25:f83396e3d25c 22
jsoh91 25:f83396e3d25c 23 //#define TRACE_SOLVER
jsoh91 25:f83396e3d25c 24
jsoh91 25:f83396e3d25c 25 namespace quadprogpp
jsoh91 25:f83396e3d25c 26 {
jsoh91 25:f83396e3d25c 27
jsoh91 25:f83396e3d25c 28 // Utility functions for updating some data needed by the solution method
jsoh91 25:f83396e3d25c 29 void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np);
jsoh91 25:f83396e3d25c 30 void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq);
jsoh91 25:f83396e3d25c 31 void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq);
jsoh91 25:f83396e3d25c 32 bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, int& iq, double& rnorm);
jsoh91 25:f83396e3d25c 33 void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, int n, int p, int& iq, int l);
jsoh91 25:f83396e3d25c 34
jsoh91 25:f83396e3d25c 35 // Utility functions for computing the Cholesky decomposition and solving
jsoh91 25:f83396e3d25c 36 // linear systems
jsoh91 25:f83396e3d25c 37 void cholesky_decomposition(Matrix<double>& A);
jsoh91 25:f83396e3d25c 38 void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b);
jsoh91 25:f83396e3d25c 39 void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b);
jsoh91 25:f83396e3d25c 40 void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y);
jsoh91 25:f83396e3d25c 41
jsoh91 25:f83396e3d25c 42 // Utility functions for computing the scalar product and the euclidean
jsoh91 25:f83396e3d25c 43 // distance between two numbers
jsoh91 25:f83396e3d25c 44 double scalar_product(const Vector<double>& x, const Vector<double>& y);
jsoh91 25:f83396e3d25c 45 double distance(double a, double b);
jsoh91 25:f83396e3d25c 46
jsoh91 25:f83396e3d25c 47 // Utility functions for printing vectors and matrices
jsoh91 25:f83396e3d25c 48 void print_matrix(char* name, const Matrix<double>& A, int n = -1, int m = -1);
jsoh91 25:f83396e3d25c 49
jsoh91 25:f83396e3d25c 50 template<typename T>
jsoh91 25:f83396e3d25c 51 void print_vector(char* name, const Vector<T>& v, int n = -1);
jsoh91 25:f83396e3d25c 52
jsoh91 25:f83396e3d25c 53 // The Solving function, implementing the Goldfarb-Idnani method
jsoh91 25:f83396e3d25c 54
jsoh91 25:f83396e3d25c 55 double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
jsoh91 25:f83396e3d25c 56 const Matrix<double>& CE, const Vector<double>& ce0,
jsoh91 25:f83396e3d25c 57 const Matrix<double>& CI, const Vector<double>& ci0,
jsoh91 25:f83396e3d25c 58 Vector<double>& x)
jsoh91 25:f83396e3d25c 59 {
jsoh91 25:f83396e3d25c 60 std::ostringstream msg;
jsoh91 25:f83396e3d25c 61 int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
jsoh91 25:f83396e3d25c 62 if (G.nrows() != n) {
jsoh91 25:f83396e3d25c 63 msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " << G.ncols() << ")";
jsoh91 25:f83396e3d25c 64 // throw std::logic_error(msg.str());
jsoh91 25:f83396e3d25c 65 }
jsoh91 25:f83396e3d25c 66 if (CE.nrows() != n) {
jsoh91 25:f83396e3d25c 67 msg << "The matrix CE is incompatible (incorrect number of rows " << CE.nrows() << " , expecting " << n << ")";
jsoh91 25:f83396e3d25c 68 // throw std::logic_error(msg.str());
jsoh91 25:f83396e3d25c 69 }
jsoh91 25:f83396e3d25c 70 if (ce0.size() != p) {
jsoh91 25:f83396e3d25c 71 msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() << ", expecting " << p << ")";
jsoh91 25:f83396e3d25c 72 // throw std::logic_error(msg.str());
jsoh91 25:f83396e3d25c 73 }
jsoh91 25:f83396e3d25c 74 if (CI.nrows() != n) {
jsoh91 25:f83396e3d25c 75 msg << "The matrix CI is incompatible (incorrect number of rows " << CI.nrows() << " , expecting " << n << ")";
jsoh91 25:f83396e3d25c 76 // throw std::logic_error(msg.str());
jsoh91 25:f83396e3d25c 77 }
jsoh91 25:f83396e3d25c 78 if (ci0.size() != m) {
jsoh91 25:f83396e3d25c 79 msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() << ", expecting " << m << ")";
jsoh91 25:f83396e3d25c 80 // throw std::logic_error(msg.str());
jsoh91 25:f83396e3d25c 81 }
jsoh91 25:f83396e3d25c 82 x.resize(n);
jsoh91 25:f83396e3d25c 83 register int i, j, k, l; /* indices */
jsoh91 25:f83396e3d25c 84 int ip; // this is the index of the constraint to be added to the active set
jsoh91 25:f83396e3d25c 85 Matrix<double> R(n, n), J(n, n);
jsoh91 25:f83396e3d25c 86 Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p);
jsoh91 25:f83396e3d25c 87 double f_value, psi, c1, c2, sum, ss, R_norm;
jsoh91 25:f83396e3d25c 88 double inf;
jsoh91 25:f83396e3d25c 89 if (std::numeric_limits<double>::has_infinity)
jsoh91 25:f83396e3d25c 90 inf = std::numeric_limits<double>::infinity();
jsoh91 25:f83396e3d25c 91 else
jsoh91 25:f83396e3d25c 92 inf = 1.0E300;
jsoh91 25:f83396e3d25c 93 double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1
jsoh91 25:f83396e3d25c 94 * and the full step length t2 */
jsoh91 25:f83396e3d25c 95 Vector<int> A(m + p), A_old(m + p), iai(m + p);
jsoh91 25:f83396e3d25c 96 int q, iq, iter = 0;
jsoh91 25:f83396e3d25c 97 Vector<bool> iaexcl(m + p);
jsoh91 25:f83396e3d25c 98
jsoh91 25:f83396e3d25c 99 /* p is the number of equality constraints */
jsoh91 25:f83396e3d25c 100 /* m is the number of inequality constraints */
jsoh91 25:f83396e3d25c 101 q = 0; /* size of the active set A (containing the indices of the active constraints) */
jsoh91 25:f83396e3d25c 102 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 103 std::cout << std::endl << "Starting solve_quadprog" << std::endl;
jsoh91 25:f83396e3d25c 104 print_matrix("G", G);
jsoh91 25:f83396e3d25c 105 print_vector("g0", g0);
jsoh91 25:f83396e3d25c 106 print_matrix("CE", CE);
jsoh91 25:f83396e3d25c 107 print_vector("ce0", ce0);
jsoh91 25:f83396e3d25c 108 print_matrix("CI", CI);
jsoh91 25:f83396e3d25c 109 print_vector("ci0", ci0);
jsoh91 25:f83396e3d25c 110 #endif
jsoh91 25:f83396e3d25c 111
jsoh91 25:f83396e3d25c 112 /*
jsoh91 25:f83396e3d25c 113 * Preprocessing phase
jsoh91 25:f83396e3d25c 114 */
jsoh91 25:f83396e3d25c 115
jsoh91 25:f83396e3d25c 116 /* compute the trace of the original matrix G */
jsoh91 25:f83396e3d25c 117 c1 = 0.0;
jsoh91 25:f83396e3d25c 118 for (i = 0; i < n; i++) {
jsoh91 25:f83396e3d25c 119 c1 += G[i][i];
jsoh91 25:f83396e3d25c 120 }
jsoh91 25:f83396e3d25c 121 /* decompose the matrix G in the form L^T L */
jsoh91 25:f83396e3d25c 122 cholesky_decomposition(G);
jsoh91 25:f83396e3d25c 123 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 124 print_matrix("G", G);
jsoh91 25:f83396e3d25c 125 #endif
jsoh91 25:f83396e3d25c 126 /* initialize the matrix R */
jsoh91 25:f83396e3d25c 127 for (i = 0; i < n; i++) {
jsoh91 25:f83396e3d25c 128 d[i] = 0.0;
jsoh91 25:f83396e3d25c 129 for (j = 0; j < n; j++)
jsoh91 25:f83396e3d25c 130 R[i][j] = 0.0;
jsoh91 25:f83396e3d25c 131 }
jsoh91 25:f83396e3d25c 132 R_norm = 1.0; /* this variable will hold the norm of the matrix R */
jsoh91 25:f83396e3d25c 133
jsoh91 25:f83396e3d25c 134 /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
jsoh91 25:f83396e3d25c 135 c2 = 0.0;
jsoh91 25:f83396e3d25c 136 for (i = 0; i < n; i++) {
jsoh91 25:f83396e3d25c 137 d[i] = 1.0;
jsoh91 25:f83396e3d25c 138 forward_elimination(G, z, d);
jsoh91 25:f83396e3d25c 139 for (j = 0; j < n; j++)
jsoh91 25:f83396e3d25c 140 J[i][j] = z[j];
jsoh91 25:f83396e3d25c 141 c2 += z[i];
jsoh91 25:f83396e3d25c 142 d[i] = 0.0;
jsoh91 25:f83396e3d25c 143 }
jsoh91 25:f83396e3d25c 144 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 145 print_matrix("J", J);
jsoh91 25:f83396e3d25c 146 #endif
jsoh91 25:f83396e3d25c 147
jsoh91 25:f83396e3d25c 148 /* c1 * c2 is an estimate for cond(G) */
jsoh91 25:f83396e3d25c 149
jsoh91 25:f83396e3d25c 150 /*
jsoh91 25:f83396e3d25c 151 * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
jsoh91 25:f83396e3d25c 152 * this is a feasible point in the dual space
jsoh91 25:f83396e3d25c 153 * x = G^-1 * g0
jsoh91 25:f83396e3d25c 154 */
jsoh91 25:f83396e3d25c 155 cholesky_solve(G, x, g0);
jsoh91 25:f83396e3d25c 156 for (i = 0; i < n; i++)
jsoh91 25:f83396e3d25c 157 x[i] = -x[i];
jsoh91 25:f83396e3d25c 158 /* and compute the current solution value */
jsoh91 25:f83396e3d25c 159 f_value = 0.5 * scalar_product(g0, x);
jsoh91 25:f83396e3d25c 160 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 161 std::cout << "Unconstrained solution: " << f_value << std::endl;
jsoh91 25:f83396e3d25c 162 print_vector("x", x);
jsoh91 25:f83396e3d25c 163 #endif
jsoh91 25:f83396e3d25c 164
jsoh91 25:f83396e3d25c 165 /* Add equality constraints to the working set A */
jsoh91 25:f83396e3d25c 166 iq = 0;
jsoh91 25:f83396e3d25c 167 for (i = 0; i < p; i++) {
jsoh91 25:f83396e3d25c 168 for (j = 0; j < n; j++)
jsoh91 25:f83396e3d25c 169 np[j] = CE[j][i];
jsoh91 25:f83396e3d25c 170 compute_d(d, J, np);
jsoh91 25:f83396e3d25c 171 update_z(z, J, d, iq);
jsoh91 25:f83396e3d25c 172 update_r(R, r, d, iq);
jsoh91 25:f83396e3d25c 173 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 174 print_matrix("R", R, n, iq);
jsoh91 25:f83396e3d25c 175 print_vector("z", z);
jsoh91 25:f83396e3d25c 176 print_vector("r", r, iq);
jsoh91 25:f83396e3d25c 177 print_vector("d", d);
jsoh91 25:f83396e3d25c 178 #endif
jsoh91 25:f83396e3d25c 179
jsoh91 25:f83396e3d25c 180 /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
jsoh91 25:f83396e3d25c 181 becomes feasible */
jsoh91 25:f83396e3d25c 182 t2 = 0.0;
jsoh91 25:f83396e3d25c 183 if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
jsoh91 25:f83396e3d25c 184 t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
jsoh91 25:f83396e3d25c 185
jsoh91 25:f83396e3d25c 186 /* set x = x + t2 * z */
jsoh91 25:f83396e3d25c 187 for (k = 0; k < n; k++)
jsoh91 25:f83396e3d25c 188 x[k] += t2 * z[k];
jsoh91 25:f83396e3d25c 189
jsoh91 25:f83396e3d25c 190 /* set u = u+ */
jsoh91 25:f83396e3d25c 191 u[iq] = t2;
jsoh91 25:f83396e3d25c 192 for (k = 0; k < iq; k++)
jsoh91 25:f83396e3d25c 193 u[k] -= t2 * r[k];
jsoh91 25:f83396e3d25c 194
jsoh91 25:f83396e3d25c 195 /* compute the new solution value */
jsoh91 25:f83396e3d25c 196 f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
jsoh91 25:f83396e3d25c 197 A[i] = -i - 1;
jsoh91 25:f83396e3d25c 198
jsoh91 25:f83396e3d25c 199 if (!add_constraint(R, J, d, iq, R_norm)) {
jsoh91 25:f83396e3d25c 200 // Equality constraints are linearly dependent
jsoh91 25:f83396e3d25c 201 // throw std::runtime_error("Constraints are linearly dependent");
jsoh91 25:f83396e3d25c 202 return f_value;
jsoh91 25:f83396e3d25c 203 }
jsoh91 25:f83396e3d25c 204 }
jsoh91 25:f83396e3d25c 205
jsoh91 25:f83396e3d25c 206 /* set iai = K \ A */
jsoh91 25:f83396e3d25c 207 for (i = 0; i < m; i++)
jsoh91 25:f83396e3d25c 208 iai[i] = i;
jsoh91 25:f83396e3d25c 209
jsoh91 25:f83396e3d25c 210 l1:
jsoh91 25:f83396e3d25c 211 iter++;
jsoh91 25:f83396e3d25c 212 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 213 print_vector("x", x);
jsoh91 25:f83396e3d25c 214 #endif
jsoh91 25:f83396e3d25c 215 /* step 1: choose a violated constraint */
jsoh91 25:f83396e3d25c 216 for (i = p; i < iq; i++) {
jsoh91 25:f83396e3d25c 217 ip = A[i];
jsoh91 25:f83396e3d25c 218 iai[ip] = -1;
jsoh91 25:f83396e3d25c 219 }
jsoh91 25:f83396e3d25c 220
jsoh91 25:f83396e3d25c 221 /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
jsoh91 25:f83396e3d25c 222 ss = 0.0;
jsoh91 25:f83396e3d25c 223 psi = 0.0; /* this value will contain the sum of all infeasibilities */
jsoh91 25:f83396e3d25c 224 ip = 0; /* ip will be the index of the chosen violated constraint */
jsoh91 25:f83396e3d25c 225 for (i = 0; i < m; i++) {
jsoh91 25:f83396e3d25c 226 iaexcl[i] = true;
jsoh91 25:f83396e3d25c 227 sum = 0.0;
jsoh91 25:f83396e3d25c 228 for (j = 0; j < n; j++)
jsoh91 25:f83396e3d25c 229 sum += CI[j][i] * x[j];
jsoh91 25:f83396e3d25c 230 sum += ci0[i];
jsoh91 25:f83396e3d25c 231 s[i] = sum;
jsoh91 25:f83396e3d25c 232 psi += std::min(0.0, sum);
jsoh91 25:f83396e3d25c 233 }
jsoh91 25:f83396e3d25c 234 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 235 print_vector("s", s, m);
jsoh91 25:f83396e3d25c 236 #endif
jsoh91 25:f83396e3d25c 237
jsoh91 25:f83396e3d25c 238
jsoh91 25:f83396e3d25c 239 if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0) {
jsoh91 25:f83396e3d25c 240 /* numerically there are not infeasibilities anymore */
jsoh91 25:f83396e3d25c 241 q = iq;
jsoh91 25:f83396e3d25c 242
jsoh91 25:f83396e3d25c 243 return f_value;
jsoh91 25:f83396e3d25c 244 }
jsoh91 25:f83396e3d25c 245
jsoh91 25:f83396e3d25c 246 /* save old values for u and A */
jsoh91 25:f83396e3d25c 247 for (i = 0; i < iq; i++) {
jsoh91 25:f83396e3d25c 248 u_old[i] = u[i];
jsoh91 25:f83396e3d25c 249 A_old[i] = A[i];
jsoh91 25:f83396e3d25c 250 }
jsoh91 25:f83396e3d25c 251 /* and for x */
jsoh91 25:f83396e3d25c 252 for (i = 0; i < n; i++)
jsoh91 25:f83396e3d25c 253 x_old[i] = x[i];
jsoh91 25:f83396e3d25c 254
jsoh91 25:f83396e3d25c 255 l2: /* Step 2: check for feasibility and determine a new S-pair */
jsoh91 25:f83396e3d25c 256 for (i = 0; i < m; i++) {
jsoh91 25:f83396e3d25c 257 if (s[i] < ss && iai[i] != -1 && iaexcl[i]) {
jsoh91 25:f83396e3d25c 258 ss = s[i];
jsoh91 25:f83396e3d25c 259 ip = i;
jsoh91 25:f83396e3d25c 260 }
jsoh91 25:f83396e3d25c 261 }
jsoh91 25:f83396e3d25c 262 if (ss >= 0.0) {
jsoh91 25:f83396e3d25c 263 q = iq;
jsoh91 25:f83396e3d25c 264
jsoh91 25:f83396e3d25c 265 return f_value;
jsoh91 25:f83396e3d25c 266 }
jsoh91 25:f83396e3d25c 267
jsoh91 25:f83396e3d25c 268 /* set np = n[ip] */
jsoh91 25:f83396e3d25c 269 for (i = 0; i < n; i++)
jsoh91 25:f83396e3d25c 270 np[i] = CI[i][ip];
jsoh91 25:f83396e3d25c 271 /* set u = [u 0]^T */
jsoh91 25:f83396e3d25c 272 u[iq] = 0.0;
jsoh91 25:f83396e3d25c 273 /* add ip to the active set A */
jsoh91 25:f83396e3d25c 274 A[iq] = ip;
jsoh91 25:f83396e3d25c 275
jsoh91 25:f83396e3d25c 276 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 277 std::cout << "Trying with constraint " << ip << std::endl;
jsoh91 25:f83396e3d25c 278 print_vector("np", np);
jsoh91 25:f83396e3d25c 279 #endif
jsoh91 25:f83396e3d25c 280
jsoh91 25:f83396e3d25c 281 l2a:/* Step 2a: determine step direction */
jsoh91 25:f83396e3d25c 282 /* compute z = H np: the step direction in the primal space (through J, see the paper) */
jsoh91 25:f83396e3d25c 283 compute_d(d, J, np);
jsoh91 25:f83396e3d25c 284 update_z(z, J, d, iq);
jsoh91 25:f83396e3d25c 285 /* compute N* np (if q > 0): the negative of the step direction in the dual space */
jsoh91 25:f83396e3d25c 286 update_r(R, r, d, iq);
jsoh91 25:f83396e3d25c 287 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 288 std::cout << "Step direction z" << std::endl;
jsoh91 25:f83396e3d25c 289 print_vector("z", z);
jsoh91 25:f83396e3d25c 290 print_vector("r", r, iq + 1);
jsoh91 25:f83396e3d25c 291 print_vector("u", u, iq + 1);
jsoh91 25:f83396e3d25c 292 print_vector("d", d);
jsoh91 25:f83396e3d25c 293 print_vector("A", A, iq + 1);
jsoh91 25:f83396e3d25c 294 #endif
jsoh91 25:f83396e3d25c 295
jsoh91 25:f83396e3d25c 296 /* Step 2b: compute step length */
jsoh91 25:f83396e3d25c 297 l = 0;
jsoh91 25:f83396e3d25c 298 /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
jsoh91 25:f83396e3d25c 299 t1 = inf; /* +inf */
jsoh91 25:f83396e3d25c 300 /* find the index l s.t. it reaches the minimum of u+[x] / r */
jsoh91 25:f83396e3d25c 301 for (k = p; k < iq; k++) {
jsoh91 25:f83396e3d25c 302 if (r[k] > 0.0) {
jsoh91 25:f83396e3d25c 303 if (u[k] / r[k] < t1) {
jsoh91 25:f83396e3d25c 304 t1 = u[k] / r[k];
jsoh91 25:f83396e3d25c 305 l = A[k];
jsoh91 25:f83396e3d25c 306 }
jsoh91 25:f83396e3d25c 307 }
jsoh91 25:f83396e3d25c 308 }
jsoh91 25:f83396e3d25c 309 /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
jsoh91 25:f83396e3d25c 310 if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) { // i.e. z != 0
jsoh91 25:f83396e3d25c 311 t2 = -s[ip] / scalar_product(z, np);
jsoh91 25:f83396e3d25c 312 if (t2 < 0) // patch suggested by Takano Akio for handling numerical inconsistencies
jsoh91 25:f83396e3d25c 313 t2 = inf;
jsoh91 25:f83396e3d25c 314 } else
jsoh91 25:f83396e3d25c 315 t2 = inf; /* +inf */
jsoh91 25:f83396e3d25c 316
jsoh91 25:f83396e3d25c 317 /* the step is chosen as the minimum of t1 and t2 */
jsoh91 25:f83396e3d25c 318 t = std::min(t1, t2);
jsoh91 25:f83396e3d25c 319 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 320 std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
jsoh91 25:f83396e3d25c 321 #endif
jsoh91 25:f83396e3d25c 322
jsoh91 25:f83396e3d25c 323 /* Step 2c: determine new S-pair and take step: */
jsoh91 25:f83396e3d25c 324
jsoh91 25:f83396e3d25c 325 /* case (i): no step in primal or dual space */
jsoh91 25:f83396e3d25c 326 if (t >= inf) {
jsoh91 25:f83396e3d25c 327 /* QPP is infeasible */
jsoh91 25:f83396e3d25c 328 // FIXME: unbounded to raise
jsoh91 25:f83396e3d25c 329 q = iq;
jsoh91 25:f83396e3d25c 330 return inf;
jsoh91 25:f83396e3d25c 331 }
jsoh91 25:f83396e3d25c 332 /* case (ii): step in dual space */
jsoh91 25:f83396e3d25c 333 if (t2 >= inf) {
jsoh91 25:f83396e3d25c 334 /* set u = u + t * [-r 1] and drop constraint l from the active set A */
jsoh91 25:f83396e3d25c 335 for (k = 0; k < iq; k++)
jsoh91 25:f83396e3d25c 336 u[k] -= t * r[k];
jsoh91 25:f83396e3d25c 337 u[iq] += t;
jsoh91 25:f83396e3d25c 338 iai[l] = l;
jsoh91 25:f83396e3d25c 339 delete_constraint(R, J, A, u, n, p, iq, l);
jsoh91 25:f83396e3d25c 340 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 341 std::cout << " in dual space: "
jsoh91 25:f83396e3d25c 342 << f_value << std::endl;
jsoh91 25:f83396e3d25c 343 print_vector("x", x);
jsoh91 25:f83396e3d25c 344 print_vector("z", z);
jsoh91 25:f83396e3d25c 345 print_vector("A", A, iq + 1);
jsoh91 25:f83396e3d25c 346 #endif
jsoh91 25:f83396e3d25c 347 goto l2a;
jsoh91 25:f83396e3d25c 348 }
jsoh91 25:f83396e3d25c 349
jsoh91 25:f83396e3d25c 350 /* case (iii): step in primal and dual space */
jsoh91 25:f83396e3d25c 351
jsoh91 25:f83396e3d25c 352 /* set x = x + t * z */
jsoh91 25:f83396e3d25c 353 for (k = 0; k < n; k++)
jsoh91 25:f83396e3d25c 354 x[k] += t * z[k];
jsoh91 25:f83396e3d25c 355 /* update the solution value */
jsoh91 25:f83396e3d25c 356 f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
jsoh91 25:f83396e3d25c 357 /* u = u + t * [-r 1] */
jsoh91 25:f83396e3d25c 358 for (k = 0; k < iq; k++)
jsoh91 25:f83396e3d25c 359 u[k] -= t * r[k];
jsoh91 25:f83396e3d25c 360 u[iq] += t;
jsoh91 25:f83396e3d25c 361 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 362 std::cout << " in both spaces: "
jsoh91 25:f83396e3d25c 363 << f_value << std::endl;
jsoh91 25:f83396e3d25c 364 print_vector("x", x);
jsoh91 25:f83396e3d25c 365 print_vector("u", u, iq + 1);
jsoh91 25:f83396e3d25c 366 print_vector("r", r, iq + 1);
jsoh91 25:f83396e3d25c 367 print_vector("A", A, iq + 1);
jsoh91 25:f83396e3d25c 368 #endif
jsoh91 25:f83396e3d25c 369
jsoh91 25:f83396e3d25c 370 if (fabs(t - t2) < std::numeric_limits<double>::epsilon()) {
jsoh91 25:f83396e3d25c 371 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 372 std::cout << "Full step has taken " << t << std::endl;
jsoh91 25:f83396e3d25c 373 print_vector("x", x);
jsoh91 25:f83396e3d25c 374 #endif
jsoh91 25:f83396e3d25c 375 /* full step has taken */
jsoh91 25:f83396e3d25c 376 /* add constraint ip to the active set*/
jsoh91 25:f83396e3d25c 377 if (!add_constraint(R, J, d, iq, R_norm)) {
jsoh91 25:f83396e3d25c 378 iaexcl[ip] = false;
jsoh91 25:f83396e3d25c 379 delete_constraint(R, J, A, u, n, p, iq, ip);
jsoh91 25:f83396e3d25c 380 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 381 print_matrix("R", R);
jsoh91 25:f83396e3d25c 382 print_vector("A", A, iq);
jsoh91 25:f83396e3d25c 383 print_vector("iai", iai);
jsoh91 25:f83396e3d25c 384 #endif
jsoh91 25:f83396e3d25c 385 for (i = 0; i < m; i++)
jsoh91 25:f83396e3d25c 386 iai[i] = i;
jsoh91 25:f83396e3d25c 387 for (i = p; i < iq; i++) {
jsoh91 25:f83396e3d25c 388 A[i] = A_old[i];
jsoh91 25:f83396e3d25c 389 u[i] = u_old[i];
jsoh91 25:f83396e3d25c 390 iai[A[i]] = -1;
jsoh91 25:f83396e3d25c 391 }
jsoh91 25:f83396e3d25c 392 for (i = 0; i < n; i++)
jsoh91 25:f83396e3d25c 393 x[i] = x_old[i];
jsoh91 25:f83396e3d25c 394 goto l2; /* go to step 2 */
jsoh91 25:f83396e3d25c 395 } else
jsoh91 25:f83396e3d25c 396 iai[ip] = -1;
jsoh91 25:f83396e3d25c 397 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 398 print_matrix("R", R);
jsoh91 25:f83396e3d25c 399 print_vector("A", A, iq);
jsoh91 25:f83396e3d25c 400 print_vector("iai", iai);
jsoh91 25:f83396e3d25c 401 #endif
jsoh91 25:f83396e3d25c 402 goto l1;
jsoh91 25:f83396e3d25c 403 }
jsoh91 25:f83396e3d25c 404
jsoh91 25:f83396e3d25c 405 /* a patial step has taken */
jsoh91 25:f83396e3d25c 406 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 407 std::cout << "Partial step has taken " << t << std::endl;
jsoh91 25:f83396e3d25c 408 print_vector("x", x);
jsoh91 25:f83396e3d25c 409 #endif
jsoh91 25:f83396e3d25c 410 /* drop constraint l */
jsoh91 25:f83396e3d25c 411 iai[l] = l;
jsoh91 25:f83396e3d25c 412 delete_constraint(R, J, A, u, n, p, iq, l);
jsoh91 25:f83396e3d25c 413 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 414 print_matrix("R", R);
jsoh91 25:f83396e3d25c 415 print_vector("A", A, iq);
jsoh91 25:f83396e3d25c 416 #endif
jsoh91 25:f83396e3d25c 417
jsoh91 25:f83396e3d25c 418 /* update s[ip] = CI * x + ci0 */
jsoh91 25:f83396e3d25c 419 sum = 0.0;
jsoh91 25:f83396e3d25c 420 for (k = 0; k < n; k++)
jsoh91 25:f83396e3d25c 421 sum += CI[k][ip] * x[k];
jsoh91 25:f83396e3d25c 422 s[ip] = sum + ci0[ip];
jsoh91 25:f83396e3d25c 423
jsoh91 25:f83396e3d25c 424 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 425 print_vector("s", s, m);
jsoh91 25:f83396e3d25c 426 #endif
jsoh91 25:f83396e3d25c 427 goto l2a;
jsoh91 25:f83396e3d25c 428 }
jsoh91 25:f83396e3d25c 429
jsoh91 25:f83396e3d25c 430 inline void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np)
jsoh91 25:f83396e3d25c 431 {
jsoh91 25:f83396e3d25c 432 register int i, j, n = d.size();
jsoh91 25:f83396e3d25c 433 register double sum;
jsoh91 25:f83396e3d25c 434
jsoh91 25:f83396e3d25c 435 /* compute d = H^T * np */
jsoh91 25:f83396e3d25c 436 for (i = 0; i < n; i++) {
jsoh91 25:f83396e3d25c 437 sum = 0.0;
jsoh91 25:f83396e3d25c 438 for (j = 0; j < n; j++)
jsoh91 25:f83396e3d25c 439 sum += J[j][i] * np[j];
jsoh91 25:f83396e3d25c 440 d[i] = sum;
jsoh91 25:f83396e3d25c 441 }
jsoh91 25:f83396e3d25c 442 }
jsoh91 25:f83396e3d25c 443
jsoh91 25:f83396e3d25c 444 inline void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq)
jsoh91 25:f83396e3d25c 445 {
jsoh91 25:f83396e3d25c 446 register int i, j, n = z.size();
jsoh91 25:f83396e3d25c 447
jsoh91 25:f83396e3d25c 448 /* setting of z = H * d */
jsoh91 25:f83396e3d25c 449 for (i = 0; i < n; i++) {
jsoh91 25:f83396e3d25c 450 z[i] = 0.0;
jsoh91 25:f83396e3d25c 451 for (j = iq; j < n; j++)
jsoh91 25:f83396e3d25c 452 z[i] += J[i][j] * d[j];
jsoh91 25:f83396e3d25c 453 }
jsoh91 25:f83396e3d25c 454 }
jsoh91 25:f83396e3d25c 455
jsoh91 25:f83396e3d25c 456 inline void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq)
jsoh91 25:f83396e3d25c 457 {
jsoh91 25:f83396e3d25c 458 register int i, j, n = d.size();
jsoh91 25:f83396e3d25c 459 register double sum;
jsoh91 25:f83396e3d25c 460
jsoh91 25:f83396e3d25c 461 /* setting of r = R^-1 d */
jsoh91 25:f83396e3d25c 462 for (i = iq - 1; i >= 0; i--) {
jsoh91 25:f83396e3d25c 463 sum = 0.0;
jsoh91 25:f83396e3d25c 464 for (j = i + 1; j < iq; j++)
jsoh91 25:f83396e3d25c 465 sum += R[i][j] * r[j];
jsoh91 25:f83396e3d25c 466 r[i] = (d[i] - sum) / R[i][i];
jsoh91 25:f83396e3d25c 467 }
jsoh91 25:f83396e3d25c 468 }
jsoh91 25:f83396e3d25c 469
jsoh91 25:f83396e3d25c 470 bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, int& iq, double& R_norm)
jsoh91 25:f83396e3d25c 471 {
jsoh91 25:f83396e3d25c 472 int n = d.size();
jsoh91 25:f83396e3d25c 473 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 474 std::cout << "Add constraint " << iq << '/';
jsoh91 25:f83396e3d25c 475 #endif
jsoh91 25:f83396e3d25c 476 register int i, j, k;
jsoh91 25:f83396e3d25c 477 double cc, ss, h, t1, t2, xny;
jsoh91 25:f83396e3d25c 478
jsoh91 25:f83396e3d25c 479 /* we have to find the Givens rotation which will reduce the element
jsoh91 25:f83396e3d25c 480 d[j] to zero.
jsoh91 25:f83396e3d25c 481 if it is already zero we don't have to do anything, except of
jsoh91 25:f83396e3d25c 482 decreasing j */
jsoh91 25:f83396e3d25c 483 for (j = n - 1; j >= iq + 1; j--) {
jsoh91 25:f83396e3d25c 484 /* The Givens rotation is done with the matrix (cc cs, cs -cc).
jsoh91 25:f83396e3d25c 485 If cc is one, then element (j) of d is zero compared with element
jsoh91 25:f83396e3d25c 486 (j - 1). Hence we don't have to do anything.
jsoh91 25:f83396e3d25c 487 If cc is zero, then we just have to switch column (j) and column (j - 1)
jsoh91 25:f83396e3d25c 488 of J. Since we only switch columns in J, we have to be careful how we
jsoh91 25:f83396e3d25c 489 update d depending on the sign of gs.
jsoh91 25:f83396e3d25c 490 Otherwise we have to apply the Givens rotation to these columns.
jsoh91 25:f83396e3d25c 491 The i - 1 element of d has to be updated to h. */
jsoh91 25:f83396e3d25c 492 cc = d[j - 1];
jsoh91 25:f83396e3d25c 493 ss = d[j];
jsoh91 25:f83396e3d25c 494 h = distance(cc, ss);
jsoh91 25:f83396e3d25c 495 if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
jsoh91 25:f83396e3d25c 496 continue;
jsoh91 25:f83396e3d25c 497 d[j] = 0.0;
jsoh91 25:f83396e3d25c 498 ss = ss / h;
jsoh91 25:f83396e3d25c 499 cc = cc / h;
jsoh91 25:f83396e3d25c 500 if (cc < 0.0) {
jsoh91 25:f83396e3d25c 501 cc = -cc;
jsoh91 25:f83396e3d25c 502 ss = -ss;
jsoh91 25:f83396e3d25c 503 d[j - 1] = -h;
jsoh91 25:f83396e3d25c 504 } else
jsoh91 25:f83396e3d25c 505 d[j - 1] = h;
jsoh91 25:f83396e3d25c 506 xny = ss / (1.0 + cc);
jsoh91 25:f83396e3d25c 507 for (k = 0; k < n; k++) {
jsoh91 25:f83396e3d25c 508 t1 = J[k][j - 1];
jsoh91 25:f83396e3d25c 509 t2 = J[k][j];
jsoh91 25:f83396e3d25c 510 J[k][j - 1] = t1 * cc + t2 * ss;
jsoh91 25:f83396e3d25c 511 J[k][j] = xny * (t1 + J[k][j - 1]) - t2;
jsoh91 25:f83396e3d25c 512 }
jsoh91 25:f83396e3d25c 513 }
jsoh91 25:f83396e3d25c 514 /* update the number of constraints added*/
jsoh91 25:f83396e3d25c 515 iq++;
jsoh91 25:f83396e3d25c 516 /* To update R we have to put the iq components of the d vector
jsoh91 25:f83396e3d25c 517 into column iq - 1 of R
jsoh91 25:f83396e3d25c 518 */
jsoh91 25:f83396e3d25c 519 for (i = 0; i < iq; i++)
jsoh91 25:f83396e3d25c 520 R[i][iq - 1] = d[i];
jsoh91 25:f83396e3d25c 521 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 522 std::cout << iq << std::endl;
jsoh91 25:f83396e3d25c 523 print_matrix("R", R, iq, iq);
jsoh91 25:f83396e3d25c 524 print_matrix("J", J);
jsoh91 25:f83396e3d25c 525 print_vector("d", d, iq);
jsoh91 25:f83396e3d25c 526 #endif
jsoh91 25:f83396e3d25c 527
jsoh91 25:f83396e3d25c 528 if (fabs(d[iq - 1]) <= std::numeric_limits<double>::epsilon() * R_norm) {
jsoh91 25:f83396e3d25c 529 // problem degenerate
jsoh91 25:f83396e3d25c 530 return false;
jsoh91 25:f83396e3d25c 531 }
jsoh91 25:f83396e3d25c 532 R_norm = std::max<double>(R_norm, fabs(d[iq - 1]));
jsoh91 25:f83396e3d25c 533 return true;
jsoh91 25:f83396e3d25c 534 }
jsoh91 25:f83396e3d25c 535
jsoh91 25:f83396e3d25c 536 void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, int n, int p, int& iq, int l)
jsoh91 25:f83396e3d25c 537 {
jsoh91 25:f83396e3d25c 538 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 539 std::cout << "Delete constraint " << l << ' ' << iq;
jsoh91 25:f83396e3d25c 540 #endif
jsoh91 25:f83396e3d25c 541 register int i, j, k, qq = -1; // just to prevent warnings from smart compilers
jsoh91 25:f83396e3d25c 542 double cc, ss, h, xny, t1, t2;
jsoh91 25:f83396e3d25c 543
jsoh91 25:f83396e3d25c 544 /* Find the index qq for active constraint l to be removed */
jsoh91 25:f83396e3d25c 545 for (i = p; i < iq; i++)
jsoh91 25:f83396e3d25c 546 if (A[i] == l) {
jsoh91 25:f83396e3d25c 547 qq = i;
jsoh91 25:f83396e3d25c 548 break;
jsoh91 25:f83396e3d25c 549 }
jsoh91 25:f83396e3d25c 550
jsoh91 25:f83396e3d25c 551 /* remove the constraint from the active set and the duals */
jsoh91 25:f83396e3d25c 552 for (i = qq; i < iq - 1; i++) {
jsoh91 25:f83396e3d25c 553 A[i] = A[i + 1];
jsoh91 25:f83396e3d25c 554 u[i] = u[i + 1];
jsoh91 25:f83396e3d25c 555 for (j = 0; j < n; j++)
jsoh91 25:f83396e3d25c 556 R[j][i] = R[j][i + 1];
jsoh91 25:f83396e3d25c 557 }
jsoh91 25:f83396e3d25c 558
jsoh91 25:f83396e3d25c 559 A[iq - 1] = A[iq];
jsoh91 25:f83396e3d25c 560 u[iq - 1] = u[iq];
jsoh91 25:f83396e3d25c 561 A[iq] = 0;
jsoh91 25:f83396e3d25c 562 u[iq] = 0.0;
jsoh91 25:f83396e3d25c 563 for (j = 0; j < iq; j++)
jsoh91 25:f83396e3d25c 564 R[j][iq - 1] = 0.0;
jsoh91 25:f83396e3d25c 565 /* constraint has been fully removed */
jsoh91 25:f83396e3d25c 566 iq--;
jsoh91 25:f83396e3d25c 567 #ifdef TRACE_SOLVER
jsoh91 25:f83396e3d25c 568 std::cout << '/' << iq << std::endl;
jsoh91 25:f83396e3d25c 569 #endif
jsoh91 25:f83396e3d25c 570
jsoh91 25:f83396e3d25c 571 if (iq == 0)
jsoh91 25:f83396e3d25c 572 return;
jsoh91 25:f83396e3d25c 573
jsoh91 25:f83396e3d25c 574 for (j = qq; j < iq; j++) {
jsoh91 25:f83396e3d25c 575 cc = R[j][j];
jsoh91 25:f83396e3d25c 576 ss = R[j + 1][j];
jsoh91 25:f83396e3d25c 577 h = distance(cc, ss);
jsoh91 25:f83396e3d25c 578 if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
jsoh91 25:f83396e3d25c 579 continue;
jsoh91 25:f83396e3d25c 580 cc = cc / h;
jsoh91 25:f83396e3d25c 581 ss = ss / h;
jsoh91 25:f83396e3d25c 582 R[j + 1][j] = 0.0;
jsoh91 25:f83396e3d25c 583 if (cc < 0.0) {
jsoh91 25:f83396e3d25c 584 R[j][j] = -h;
jsoh91 25:f83396e3d25c 585 cc = -cc;
jsoh91 25:f83396e3d25c 586 ss = -ss;
jsoh91 25:f83396e3d25c 587 } else
jsoh91 25:f83396e3d25c 588 R[j][j] = h;
jsoh91 25:f83396e3d25c 589
jsoh91 25:f83396e3d25c 590 xny = ss / (1.0 + cc);
jsoh91 25:f83396e3d25c 591 for (k = j + 1; k < iq; k++) {
jsoh91 25:f83396e3d25c 592 t1 = R[j][k];
jsoh91 25:f83396e3d25c 593 t2 = R[j + 1][k];
jsoh91 25:f83396e3d25c 594 R[j][k] = t1 * cc + t2 * ss;
jsoh91 25:f83396e3d25c 595 R[j + 1][k] = xny * (t1 + R[j][k]) - t2;
jsoh91 25:f83396e3d25c 596 }
jsoh91 25:f83396e3d25c 597 for (k = 0; k < n; k++) {
jsoh91 25:f83396e3d25c 598 t1 = J[k][j];
jsoh91 25:f83396e3d25c 599 t2 = J[k][j + 1];
jsoh91 25:f83396e3d25c 600 J[k][j] = t1 * cc + t2 * ss;
jsoh91 25:f83396e3d25c 601 J[k][j + 1] = xny * (J[k][j] + t1) - t2;
jsoh91 25:f83396e3d25c 602 }
jsoh91 25:f83396e3d25c 603 }
jsoh91 25:f83396e3d25c 604 }
jsoh91 25:f83396e3d25c 605
jsoh91 25:f83396e3d25c 606 inline double distance(double a, double b)
jsoh91 25:f83396e3d25c 607 {
jsoh91 25:f83396e3d25c 608 register double a1, b1, t;
jsoh91 25:f83396e3d25c 609 a1 = fabs(a);
jsoh91 25:f83396e3d25c 610 b1 = fabs(b);
jsoh91 25:f83396e3d25c 611 if (a1 > b1) {
jsoh91 25:f83396e3d25c 612 t = (b1 / a1);
jsoh91 25:f83396e3d25c 613 return a1 * sqrt(1.0 + t * t);
jsoh91 25:f83396e3d25c 614 } else if (b1 > a1) {
jsoh91 25:f83396e3d25c 615 t = (a1 / b1);
jsoh91 25:f83396e3d25c 616 return b1 * sqrt(1.0 + t * t);
jsoh91 25:f83396e3d25c 617 }
jsoh91 25:f83396e3d25c 618 return a1 * sqrt(2.0);
jsoh91 25:f83396e3d25c 619 }
jsoh91 25:f83396e3d25c 620
jsoh91 25:f83396e3d25c 621
jsoh91 25:f83396e3d25c 622 inline double scalar_product(const Vector<double>& x, const Vector<double>& y)
jsoh91 25:f83396e3d25c 623 {
jsoh91 25:f83396e3d25c 624 register int i, n = x.size();
jsoh91 25:f83396e3d25c 625 register double sum;
jsoh91 25:f83396e3d25c 626
jsoh91 25:f83396e3d25c 627 sum = 0.0;
jsoh91 25:f83396e3d25c 628 for (i = 0; i < n; i++)
jsoh91 25:f83396e3d25c 629 sum += x[i] * y[i];
jsoh91 25:f83396e3d25c 630 return sum;
jsoh91 25:f83396e3d25c 631 }
jsoh91 25:f83396e3d25c 632
jsoh91 25:f83396e3d25c 633 void cholesky_decomposition(Matrix<double>& A)
jsoh91 25:f83396e3d25c 634 {
jsoh91 25:f83396e3d25c 635 register int i, j, k, n = A.nrows();
jsoh91 25:f83396e3d25c 636 register double sum;
jsoh91 25:f83396e3d25c 637
jsoh91 25:f83396e3d25c 638 for (i = 0; i < n; i++) {
jsoh91 25:f83396e3d25c 639 for (j = i; j < n; j++) {
jsoh91 25:f83396e3d25c 640 sum = A[i][j];
jsoh91 25:f83396e3d25c 641 for (k = i - 1; k >= 0; k--)
jsoh91 25:f83396e3d25c 642 sum -= A[i][k]*A[j][k];
jsoh91 25:f83396e3d25c 643 if (i == j) {
jsoh91 25:f83396e3d25c 644 if (sum <= 0.0) {
jsoh91 25:f83396e3d25c 645 std::ostringstream os;
jsoh91 25:f83396e3d25c 646 // raise error
jsoh91 25:f83396e3d25c 647 print_matrix("A", A);
jsoh91 25:f83396e3d25c 648 os << "Error in cholesky decomposition, sum: " << sum;
jsoh91 25:f83396e3d25c 649 // throw std::logic_error(os.str());
jsoh91 25:f83396e3d25c 650 // exit(-1);
jsoh91 25:f83396e3d25c 651 }
jsoh91 25:f83396e3d25c 652 A[i][i] = sqrt(sum);
jsoh91 25:f83396e3d25c 653 } else
jsoh91 25:f83396e3d25c 654 A[j][i] = sum / A[i][i];
jsoh91 25:f83396e3d25c 655 }
jsoh91 25:f83396e3d25c 656 for (k = i + 1; k < n; k++)
jsoh91 25:f83396e3d25c 657 A[i][k] = A[k][i];
jsoh91 25:f83396e3d25c 658 }
jsoh91 25:f83396e3d25c 659 }
jsoh91 25:f83396e3d25c 660
jsoh91 25:f83396e3d25c 661 void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b)
jsoh91 25:f83396e3d25c 662 {
jsoh91 25:f83396e3d25c 663 int n = L.nrows();
jsoh91 25:f83396e3d25c 664 Vector<double> y(n);
jsoh91 25:f83396e3d25c 665
jsoh91 25:f83396e3d25c 666 /* Solve L * y = b */
jsoh91 25:f83396e3d25c 667 forward_elimination(L, y, b);
jsoh91 25:f83396e3d25c 668 /* Solve L^T * x = y */
jsoh91 25:f83396e3d25c 669 backward_elimination(L, x, y);
jsoh91 25:f83396e3d25c 670 }
jsoh91 25:f83396e3d25c 671
jsoh91 25:f83396e3d25c 672 inline void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b)
jsoh91 25:f83396e3d25c 673 {
jsoh91 25:f83396e3d25c 674 register int i, j, n = L.nrows();
jsoh91 25:f83396e3d25c 675
jsoh91 25:f83396e3d25c 676 y[0] = b[0] / L[0][0];
jsoh91 25:f83396e3d25c 677 for (i = 1; i < n; i++) {
jsoh91 25:f83396e3d25c 678 y[i] = b[i];
jsoh91 25:f83396e3d25c 679 for (j = 0; j < i; j++)
jsoh91 25:f83396e3d25c 680 y[i] -= L[i][j] * y[j];
jsoh91 25:f83396e3d25c 681 y[i] = y[i] / L[i][i];
jsoh91 25:f83396e3d25c 682 }
jsoh91 25:f83396e3d25c 683 }
jsoh91 25:f83396e3d25c 684
jsoh91 25:f83396e3d25c 685 inline void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y)
jsoh91 25:f83396e3d25c 686 {
jsoh91 25:f83396e3d25c 687 register int i, j, n = U.nrows();
jsoh91 25:f83396e3d25c 688
jsoh91 25:f83396e3d25c 689 x[n - 1] = y[n - 1] / U[n - 1][n - 1];
jsoh91 25:f83396e3d25c 690 for (i = n - 2; i >= 0; i--) {
jsoh91 25:f83396e3d25c 691 x[i] = y[i];
jsoh91 25:f83396e3d25c 692 for (j = i + 1; j < n; j++)
jsoh91 25:f83396e3d25c 693 x[i] -= U[i][j] * x[j];
jsoh91 25:f83396e3d25c 694 x[i] = x[i] / U[i][i];
jsoh91 25:f83396e3d25c 695 }
jsoh91 25:f83396e3d25c 696 }
jsoh91 25:f83396e3d25c 697
jsoh91 25:f83396e3d25c 698 void print_matrix(char* name, const Matrix<double>& A, int n, int m)
jsoh91 25:f83396e3d25c 699 {
jsoh91 25:f83396e3d25c 700 std::ostringstream s;
jsoh91 25:f83396e3d25c 701 std::string t;
jsoh91 25:f83396e3d25c 702 if (n == -1)
jsoh91 25:f83396e3d25c 703 n = A.nrows();
jsoh91 25:f83396e3d25c 704 if (m == -1)
jsoh91 25:f83396e3d25c 705 m = A.ncols();
jsoh91 25:f83396e3d25c 706
jsoh91 25:f83396e3d25c 707 s << name << ": " << std::endl;
jsoh91 25:f83396e3d25c 708 for (int i = 0; i < n; i++) {
jsoh91 25:f83396e3d25c 709 s << " ";
jsoh91 25:f83396e3d25c 710 for (int j = 0; j < m; j++)
jsoh91 25:f83396e3d25c 711 s << A[i][j] << ", ";
jsoh91 25:f83396e3d25c 712 s << std::endl;
jsoh91 25:f83396e3d25c 713 }
jsoh91 25:f83396e3d25c 714 t = s.str();
jsoh91 25:f83396e3d25c 715 t = t.substr(0, t.size() - 3); // To remove the trailing space, comma and newline
jsoh91 25:f83396e3d25c 716
jsoh91 25:f83396e3d25c 717 std::cout << t << std::endl;
jsoh91 25:f83396e3d25c 718 }
jsoh91 25:f83396e3d25c 719
jsoh91 25:f83396e3d25c 720 template<typename T>
jsoh91 25:f83396e3d25c 721 void print_vector(char* name, const Vector<T>& v, int n)
jsoh91 25:f83396e3d25c 722 {
jsoh91 25:f83396e3d25c 723 std::ostringstream s;
jsoh91 25:f83396e3d25c 724 std::string t;
jsoh91 25:f83396e3d25c 725 if (n == -1)
jsoh91 25:f83396e3d25c 726 n = v.size();
jsoh91 25:f83396e3d25c 727
jsoh91 25:f83396e3d25c 728 s << name << ": " << std::endl << " ";
jsoh91 25:f83396e3d25c 729 for (int i = 0; i < n; i++) {
jsoh91 25:f83396e3d25c 730 s << v[i] << ", ";
jsoh91 25:f83396e3d25c 731 }
jsoh91 25:f83396e3d25c 732 t = s.str();
jsoh91 25:f83396e3d25c 733 t = t.substr(0, t.size() - 2); // To remove the trailing space and comma
jsoh91 25:f83396e3d25c 734
jsoh91 25:f83396e3d25c 735 std::cout << t << std::endl;
jsoh91 25:f83396e3d25c 736 }
jsoh91 25:f83396e3d25c 737
jsoh91 25:f83396e3d25c 738 } // namespace quadprogpp