// Ax = b void Solver::LinearSolve(ARRAY<2, double> a, ARRAY<1, double> b, ARRAY<1, double>& x) { int n = a.dim(1); for (int i = 1; i <= n; ++i) { int j = i; while (j <= n && a(j,i) == 0) ++j; if (j > n) { continue; } if (j != i) { for (int k = i; k <= n; ++k) swap(a(j,k), a(i,k)); swap(b(j), b(i)); } for (int k = j + 1; k <= n; ++k) { if (a(k,i) != 0) { double w = a(k,i) / a(i,i); for (int l = i; l <= n; ++l) { a(k,l) -= w * a(i,l); } b(k) -= w * b(i); } } } for (int i = n; i >= 1; --i) { x(i) = b(i) / a(i,i); for (int j = i - 1; j >= 1; --j) { b(j) -= x(i) * a(j,i); } } }
void Solver::QPSolve(ARRAY<2, double>& a, ARRAY<1, double>& b, ARRAY<1, double>& f) { int n = f.dim(1); f.Fill(0); ARRAY<1, double> c = b; ARRAY<1, double> delta_f(n), delta_a(n); set<int> C, NC; set<int> ad_minus; for (int i = 1; i <= n; ++i) { if (c(i) < -1e-4) { ad_minus.insert(i); } } while (!ad_minus.empty()) { set<int>::iterator it = ad_minus.begin(); int d = *it; NC.erase(d); while (true) { fdirection(d, a, C, delta_f, delta_a); pair<double, int> sj = maxstep(f,c,delta_f,delta_a,C,NC,d); if (sj.first > 1e20) { return; } for (set<int>::iterator it = C.begin(); it != C.end(); ++it) if (*it != d) f(*it) += sj.first * delta_f(*it); f(d) += sj.first * delta_f(d); for (int i = 1; i <= n; ++i) { c(i) += delta_a(i) * sj.first; if (c(i) < -1e-4) { ad_minus.insert(i); } else { if (i == d) { sj.second = d; NC.erase(d); } ad_minus.erase(i); } } it = C.find(sj.second); if (it != C.end()) { NC.insert(*it); C.erase(it); } else { it = NC.find(sj.second); if (it != NC.end()) { C.insert(*it); NC.erase(it); } else { C.insert(d); ad_minus.erase(d); break; } } } } }
void Solver::fdirection(int d, ARRAY<2, double>& a, set<int> &C, ARRAY<1, double> &delta_f, ARRAY<1, double> &delta_a) { int n = delta_f.dim(1); for (int i = 1; i <= n; ++i) { delta_a(i) = a(i,d); delta_f(i) = 0; } delta_f(d) = 1; bool flag = false; for (int i = 1; i <= n; ++i) { double ttt = 0; for (int j = 1; j <= n; ++j) ttt += a(i,j) * delta_f(j); if (fabs(ttt - delta_a(i)) > 1e-4) { flag = true; break; } } if (!C.empty()) { ARRAY<1, double> vi((int)C.size()), X((int)C.size()); ARRAY<2, double> A((int)C.size(), (int)C.size()); int s = 0; for (set<int>::iterator it = C.begin(); it != C.end(); ++it) { s++; int s1 = 0; for (set<int>::iterator it1 = C.begin(); it1 != C.end(); ++it1) { s1++; A(s,s1) = a(*it,*it1); } vi(s) = -a(*it, d); } LinearSolve(A, vi, X); s = 0; for (set<int>::iterator it = C.begin(); it != C.end(); ++it) { s++; delta_f(*it) = X(s); for (int i = 1; i <= n; ++i) { delta_a(i) += a(i,*it) * X(s); } } } for (int i = 1; i <= n; ++i) { double t = 0; for (int j = 1; j <= n; ++j) { t += a(i,j) * delta_f(j); } } }