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?
User | Revision | Line number | New 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 |