123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599 |
- #include "quadprog.h"
- #include <vector>
- #include <Eigen/Dense>
- IGL_INLINE bool igl::copyleft::quadprog(
- const Eigen::MatrixXd & G,
- const Eigen::VectorXd & g0,
- const Eigen::MatrixXd & CE,
- const Eigen::VectorXd & ce0,
- const Eigen::MatrixXd & CI,
- const Eigen::VectorXd & ci0,
- Eigen::VectorXd& x)
- {
- using namespace Eigen;
- typedef double Scalar;
- const auto distance = [](Scalar a, Scalar b)->Scalar
- {
- Scalar a1, b1, t;
- a1 = std::abs(a);
- b1 = std::abs(b);
- if (a1 > b1)
- {
- t = (b1 / a1);
- return a1 * std::sqrt(1.0 + t * t);
- }
- else
- if (b1 > a1)
- {
- t = (a1 / b1);
- return b1 * std::sqrt(1.0 + t * t);
- }
- return a1 * std::sqrt(2.0);
- };
- const auto compute_d = [](VectorXd &d, const MatrixXd& J, const VectorXd& np)
- {
- d = J.adjoint() * np;
- };
- const auto update_z =
- [](VectorXd& z, const MatrixXd& J, const VectorXd& d, int iq)
- {
- z = J.rightCols(z.size()-iq) * d.tail(d.size()-iq);
- };
- const auto update_r =
- [](const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq)
- {
- r.head(iq) =
- R.topLeftCorner(iq,iq).triangularView<Upper>().solve(d.head(iq));
- };
- const auto add_constraint = [&distance](
- MatrixXd& R,
- MatrixXd& J,
- VectorXd& d,
- int& iq,
- double& R_norm)->bool
- {
- int n=J.rows();
- #ifdef TRACE_SOLVER
- std::cerr << "Add constraint " << iq << '/';
- #endif
- int i, j, k;
- double cc, ss, h, t1, t2, xny;
-
-
-
- for (j = n - 1; j >= iq + 1; j--)
- {
-
- cc = d(j - 1);
- ss = d(j);
- h = distance(cc, ss);
- if (h == 0.0)
- continue;
- d(j) = 0.0;
- ss = ss / h;
- cc = cc / h;
- if (cc < 0.0)
- {
- cc = -cc;
- ss = -ss;
- d(j - 1) = -h;
- }
- else
- d(j - 1) = h;
- xny = ss / (1.0 + cc);
- for (k = 0; k < n; k++)
- {
- t1 = J(k,j - 1);
- t2 = J(k,j);
- J(k,j - 1) = t1 * cc + t2 * ss;
- J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
- }
- }
-
- iq++;
-
- R.col(iq-1).head(iq) = d.head(iq);
- #ifdef TRACE_SOLVER
- std::cerr << iq << std::endl;
- #endif
-
- if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
- {
-
- return false;
- }
- R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
- return true;
- };
- const auto delete_constraint = [&distance](
- MatrixXd& R,
- MatrixXd& J,
- VectorXi& A,
- VectorXd& u,
- int p,
- int& iq,
- int l)
- {
- int n = R.rows();
- #ifdef TRACE_SOLVER
- std::cerr << "Delete constraint " << l << ' ' << iq;
- #endif
- int i, j, k, qq;
- double cc, ss, h, xny, t1, t2;
-
- for (i = p; i < iq; i++)
- if (A(i) == l)
- {
- qq = i;
- break;
- }
-
- for (i = qq; i < iq - 1; i++)
- {
- A(i) = A(i + 1);
- u(i) = u(i + 1);
- R.col(i) = R.col(i+1);
- }
- A(iq - 1) = A(iq);
- u(iq - 1) = u(iq);
- A(iq) = 0;
- u(iq) = 0.0;
- for (j = 0; j < iq; j++)
- R(j,iq - 1) = 0.0;
-
- iq--;
- #ifdef TRACE_SOLVER
- std::cerr << '/' << iq << std::endl;
- #endif
- if (iq == 0)
- return;
- for (j = qq; j < iq; j++)
- {
- cc = R(j,j);
- ss = R(j + 1,j);
- h = distance(cc, ss);
- if (h == 0.0)
- continue;
- cc = cc / h;
- ss = ss / h;
- R(j + 1,j) = 0.0;
- if (cc < 0.0)
- {
- R(j,j) = -h;
- cc = -cc;
- ss = -ss;
- }
- else
- R(j,j) = h;
- xny = ss / (1.0 + cc);
- for (k = j + 1; k < iq; k++)
- {
- t1 = R(j,k);
- t2 = R(j + 1,k);
- R(j,k) = t1 * cc + t2 * ss;
- R(j + 1,k) = xny * (t1 + R(j,k)) - t2;
- }
- for (k = 0; k < n; k++)
- {
- t1 = J(k,j);
- t2 = J(k,j + 1);
- J(k,j) = t1 * cc + t2 * ss;
- J(k,j + 1) = xny * (J(k,j) + t1) - t2;
- }
- }
- };
- int i, j, k, l;
- int ip, me, mi;
- int n=g0.size(); int p=ce0.size(); int m=ci0.size();
- MatrixXd R(G.rows(),G.cols()), J(G.rows(),G.cols());
-
- LLT<MatrixXd,Lower> chol(G.cols());
-
- VectorXd s(m+p), z(n), r(m + p), d(n), np(n), u(m + p);
- VectorXd x_old(n), u_old(m + p);
- double f_value, psi, c1, c2, sum, ss, R_norm;
- const double inf = std::numeric_limits<double>::infinity();
- double t, t1, t2;
- VectorXi A(m + p), A_old(m + p), iai(m + p);
- int q;
- int iq, iter = 0;
- std::vector<bool> iaexcl(m + p);
-
- me = p;
- mi = m;
- q = 0;
-
-
-
-
- c1 = G.trace();
-
-
- chol.compute(G);
-
-
- d.setZero();
- R.setZero();
- R_norm = 1.0;
-
-
-
- J.setIdentity();
- J = chol.matrixU().solve(J);
- c2 = J.trace();
- #ifdef TRACE_SOLVER
- print_matrix("J", J, n);
- #endif
-
-
-
-
- x = chol.solve(g0);
- x = -x;
-
- f_value = 0.5 * g0.dot(x);
- #ifdef TRACE_SOLVER
- std::cerr << "Unconstrained solution: " << f_value << std::endl;
- print_vector("x", x, n);
- #endif
-
-
- iq = 0;
- for (i = 0; i < me; i++)
- {
- np = CE.col(i);
- compute_d(d, J, np);
- update_z(z, J, d, iq);
- update_r(R, r, d, iq);
- #ifdef TRACE_SOLVER
- print_matrix("R", R, iq);
- print_vector("z", z, n);
- print_vector("r", r, iq);
- print_vector("d", d, n);
- #endif
-
-
- t2 = 0.0;
- if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
- t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
-
- x += t2 * z;
-
- u(iq) = t2;
- u.head(iq) -= t2 * r.head(iq);
-
-
- f_value += 0.5 * (t2 * t2) * z.dot(np);
- A(i) = -i - 1;
-
- if (!add_constraint(R, J, d, iq, R_norm))
- {
-
-
- return false;
- }
- }
-
-
- for (i = 0; i < mi; i++)
- iai(i) = i;
-
- l1: iter++;
- #ifdef TRACE_SOLVER
- print_vector("x", x, n);
- #endif
-
- for (i = me; i < iq; i++)
- {
- ip = A(i);
- iai(ip) = -1;
- }
-
-
- ss = 0.0;
- psi = 0.0;
- ip = 0;
- for (i = 0; i < mi; i++)
- {
- iaexcl[i] = true;
- sum = CI.col(i).dot(x) + ci0(i);
- s(i) = sum;
- psi += std::min(0.0, sum);
- }
- #ifdef TRACE_SOLVER
- print_vector("s", s, mi);
- #endif
-
- if (std::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
- {
-
- q = iq;
- return true;
- }
-
-
- u_old.head(iq) = u.head(iq);
- A_old.head(iq) = A.head(iq);
- x_old = x;
-
- l2:
- for (i = 0; i < mi; i++)
- {
- if (s(i) < ss && iai(i) != -1 && iaexcl[i])
- {
- ss = s(i);
- ip = i;
- }
- }
- if (ss >= 0.0)
- {
- q = iq;
- return true;
- }
-
-
- np = CI.col(ip);
-
- u(iq) = 0.0;
-
- A(iq) = ip;
- #ifdef TRACE_SOLVER
- std::cerr << "Trying with constraint " << ip << std::endl;
- print_vector("np", np, n);
- #endif
-
- l2a:
-
- compute_d(d, J, np);
- update_z(z, J, d, iq);
-
- update_r(R, r, d, iq);
- #ifdef TRACE_SOLVER
- std::cerr << "Step direction z" << std::endl;
- print_vector("z", z, n);
- print_vector("r", r, iq + 1);
- print_vector("u", u, iq + 1);
- print_vector("d", d, n);
- print_ivector("A", A, iq + 1);
- #endif
-
-
- l = 0;
-
- t1 = inf;
-
- for (k = me; k < iq; k++)
- {
- double tmp;
- if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
- {
- t1 = tmp;
- l = A(k);
- }
- }
-
- if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon())
- t2 = -s(ip) / z.dot(np);
- else
- t2 = inf;
-
- t = std::min(t1, t2);
- #ifdef TRACE_SOLVER
- std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
- #endif
-
-
-
-
- if (t >= inf)
- {
-
-
- q = iq;
- return false;
- }
-
- if (t2 >= inf)
- {
-
- u.head(iq) -= t * r.head(iq);
- u(iq) += t;
- iai(l) = l;
- delete_constraint(R, J, A, u, p, iq, l);
- #ifdef TRACE_SOLVER
- std::cerr << " in dual space: "
- << f_value << std::endl;
- print_vector("x", x, n);
- print_vector("z", z, n);
- print_ivector("A", A, iq + 1);
- #endif
- goto l2a;
- }
-
-
-
- x += t * z;
-
- f_value += t * z.dot(np) * (0.5 * t + u(iq));
-
- u.head(iq) -= t * r.head(iq);
- u(iq) += t;
- #ifdef TRACE_SOLVER
- std::cerr << " in both spaces: "
- << f_value << std::endl;
- print_vector("x", x, n);
- print_vector("u", u, iq + 1);
- print_vector("r", r, iq + 1);
- print_ivector("A", A, iq + 1);
- #endif
-
- if (t == t2)
- {
- #ifdef TRACE_SOLVER
- std::cerr << "Full step has taken " << t << std::endl;
- print_vector("x", x, n);
- #endif
-
-
- if (!add_constraint(R, J, d, iq, R_norm))
- {
- iaexcl[ip] = false;
- delete_constraint(R, J, A, u, p, iq, ip);
- #ifdef TRACE_SOLVER
- print_matrix("R", R, n);
- print_ivector("A", A, iq);
- #endif
- for (i = 0; i < m; i++)
- iai(i) = i;
- for (i = 0; i < iq; i++)
- {
- A(i) = A_old(i);
- iai(A(i)) = -1;
- u(i) = u_old(i);
- }
- x = x_old;
- goto l2;
- }
- else
- iai(ip) = -1;
- #ifdef TRACE_SOLVER
- print_matrix("R", R, n);
- print_ivector("A", A, iq);
- #endif
- goto l1;
- }
-
-
- #ifdef TRACE_SOLVER
- std::cerr << "Partial step has taken " << t << std::endl;
- print_vector("x", x, n);
- #endif
-
- iai(l) = l;
- delete_constraint(R, J, A, u, p, iq, l);
- #ifdef TRACE_SOLVER
- print_matrix("R", R, n);
- print_ivector("A", A, iq);
- #endif
-
- s(ip) = CI.col(ip).dot(x) + ci0(ip);
- #ifdef TRACE_SOLVER
- print_vector("s", s, mi);
- #endif
- goto l2a;
- }
|