/* * Copyright (c) 1990 Michael E. Hohmeyer, * hohmeyer@icemcfd.com * Permission is granted to modify and re-distribute this code in any manner * as long as this notice is preserved. All standard disclaimers apply. * * R. Seidel's algorithm for solving LPs (linear programs.) */ /* * Copyright (c) 2021 Zhepei Wang, * wangzhepei@live.com * 1. Bug fix in "move_to_front" function that "prev[m]" is illegally accessed * while "prev" originally has only m ints. It is fixed by allocating a * "prev" with m + 1 ints. * 2. Add Eigen interface. * 3. Resursive template. * Permission is granted to modify and re-distribute this code in any manner * as long as this notice is preserved. All standard disclaimers apply. * * Ref: Seidel, R. (1991), "Small-dimensional linear programming and convex * hulls made easy", Discrete & Computational Geometry 6 (1): 423–434, * doi:10.1007/BF02574699 */ #ifndef SDLP_HPP #define SDLP_HPP #include <Eigen/Eigen> #include <cmath> #include <random> namespace sdlp { constexpr double eps = 1.0e-12; enum { /* minimum attained */ MINIMUM = 0, /* no feasible region */ INFEASIBLE, /* unbounded solution */ UNBOUNDED, /* only a vertex in the solution set */ AMBIGUOUS, }; inline double dot2(const double a[2], const double b[2]) { return a[0] * b[0] + a[1] * b[1]; } inline double cross2(const double a[2], const double b[2]) { return a[0] * b[1] - a[1] * b[0]; } inline bool unit2(const double a[2], double b[2]) { const double mag = std::sqrt(a[0] * a[0] + a[1] * a[1]); if (mag < 2.0 * eps) { return true; } b[0] = a[0] / mag; b[1] = a[1] / mag; return false; } /* unitize a d + 1 dimensional point */ template <int d> inline bool unit(double *a) { double mag = 0.0; for (int i = 0; i <= d; i++) { mag += a[i] * a[i]; } if (mag < (d + 1) * eps * eps) { return true; } mag = 1.0 / std::sqrt(mag); for (int i = 0; i <= d; i++) { a[i] *= mag; } return false; } /* optimize the unconstrained objective */ template <int d> inline int lp_no_con(const double *n_vec, const double *d_vec, double *opt) { double n_dot_d = 0.0; double d_dot_d = 0.0; for (int i = 0; i <= d; i++) { n_dot_d += n_vec[i] * d_vec[i]; d_dot_d += d_vec[i] * d_vec[i]; } if (d_dot_d < eps * eps) { n_dot_d = 0.0; d_dot_d = 1.0; } for (int i = 0; i <= d; i++) { opt[i] = -n_vec[i] + d_vec[i] * n_dot_d / d_dot_d; } /* normalize the optimal point */ if (unit<d>(opt)) { opt[d] = 1.0; return AMBIGUOUS; } else { return MINIMUM; } } /* returns the plane index that is in i's place */ inline int move_to_front(const int i, int *next, int *prev) { if (i == 0 || i == next[0]) { return i; } const int previ = prev[i]; /* remove i from it's current position */ next[prev[i]] = next[i]; prev[next[i]] = prev[i]; /* put i at the front */ next[i] = next[0]; prev[i] = 0; prev[next[i]] = i; next[0] = i; return previ; } inline void lp_min_lin_rat(const bool degen, const double cw_vec[2], const double ccw_vec[2], const double n_vec[2], const double d_vec[2], double opt[2]) { /* linear rational function case */ const double d_cw = dot2(cw_vec, d_vec); const double d_ccw = dot2(ccw_vec, d_vec); const double n_cw = dot2(cw_vec, n_vec); const double n_ccw = dot2(ccw_vec, n_vec); if (degen) { /* if degenerate simply compare values */ if (n_cw / d_cw < n_ccw / d_ccw) { opt[0] = cw_vec[0]; opt[1] = cw_vec[1]; } else { opt[0] = ccw_vec[0]; opt[1] = ccw_vec[1]; } /* check CW/CCW bounds are not near a poles */ } else if (std::fabs(d_cw) > 2.0 * eps && std::fabs(d_ccw) > 2.0 * eps) { /* the valid region does not contain a poles */ if (d_cw * d_ccw > 0.0) { /* find which end has the minimum value */ if (n_cw / d_cw < n_ccw / d_ccw) { opt[0] = cw_vec[0]; opt[1] = cw_vec[1]; } else { opt[0] = ccw_vec[0]; opt[1] = ccw_vec[1]; } } else { /* the valid region does contain a poles */ if (d_cw > 0.0) { opt[0] = -d_vec[1]; opt[1] = d_vec[0]; } else { opt[0] = d_vec[1]; opt[1] = -d_vec[0]; } } } else if (std::fabs(d_cw) > 2.0 * eps) { /* CCW bound is near a pole */ if (n_ccw * d_cw > 0.0) { /* CCW bound is a positive pole */ opt[0] = cw_vec[0]; opt[1] = cw_vec[1]; } else { /* CCW bound is a negative pole */ opt[0] = ccw_vec[0]; opt[1] = ccw_vec[1]; } } else if (std::fabs(d_ccw) > 2.0 * eps) { /* CW bound is near a pole */ if (n_cw * d_ccw > 2.0 * eps) { /* CW bound is at a positive pole */ opt[0] = ccw_vec[0]; opt[1] = ccw_vec[1]; } else { /* CW bound is at a negative pole */ opt[0] = cw_vec[0]; opt[1] = cw_vec[1]; } } else { /* both bounds are near poles */ if (cross2(d_vec, n_vec) > 0.0) { opt[0] = cw_vec[0]; opt[1] = cw_vec[1]; } else { opt[0] = ccw_vec[0]; opt[1] = ccw_vec[1]; } } } inline int wedge(const double (*halves)[2], const int m, int *next, int *prev, double cw_vec[2], double ccw_vec[2], bool *degen) { int i; double d_cw, d_ccw; bool offensive; *degen = false; for (i = 0; i != m; i = next[i]) { if (!unit2(halves[i], ccw_vec)) { /* CW */ cw_vec[0] = ccw_vec[1]; cw_vec[1] = -ccw_vec[0]; /* CCW */ ccw_vec[0] = -cw_vec[0]; ccw_vec[1] = -cw_vec[1]; break; } } if (i == m) { return UNBOUNDED; } i = 0; while (i != m) { offensive = false; d_cw = dot2(cw_vec, halves[i]); d_ccw = dot2(ccw_vec, halves[i]); if (d_ccw >= 2.0 * eps) { if (d_cw <= -2.0 * eps) { cw_vec[0] = halves[i][1]; cw_vec[1] = -halves[i][0]; unit2(cw_vec, cw_vec); offensive = true; } } else if (d_cw >= 2.0 * eps) { if (d_ccw <= -2.0 * eps) { ccw_vec[0] = -halves[i][1]; ccw_vec[1] = halves[i][0]; unit2(ccw_vec, ccw_vec); offensive = true; } } else if (d_ccw <= -2.0 * eps && d_cw <= -2.0 * eps) { return INFEASIBLE; } else if (d_cw <= -2.0 * eps || d_ccw <= -2.0 * eps || cross2(cw_vec, halves[i]) < 0.0) { /* degenerate */ if (d_cw <= -2.0 * eps) { unit2(ccw_vec, cw_vec); } else if (d_ccw <= -2.0 * eps) { unit2(cw_vec, ccw_vec); } *degen = true; offensive = true; } /* place this offensive plane in second place */ if (offensive) { i = move_to_front(i, next, prev); } i = next[i]; if (*degen) { break; } } if (*degen) { while (i != m) { d_cw = dot2(cw_vec, halves[i]); d_ccw = dot2(ccw_vec, halves[i]); if (d_cw < -2.0 * eps) { if (d_ccw < -2.0 * eps) { return INFEASIBLE; } else { cw_vec[0] = ccw_vec[0]; cw_vec[1] = ccw_vec[1]; } } else if (d_ccw < -2.0 * eps) { ccw_vec[0] = cw_vec[0]; ccw_vec[1] = cw_vec[1]; } i = next[i]; } } return MINIMUM; } /* return the minimum on the projective line */ inline int lp_base_case(const double (*halves)[2], /* halves --- half lines */ const int m, /* m --- terminal marker */ const double n_vec[2], /* n_vec --- numerator funciton */ const double d_vec[2], /* d_vec --- denominator function */ double opt[2], /* opt --- optimum */ int *next, /* next, prev --- double linked list of indices */ int *prev) { double cw_vec[2], ccw_vec[2]; bool degen; int status; /* find the feasible region of the line */ status = wedge(halves, m, next, prev, cw_vec, ccw_vec, °en); if (status == INFEASIBLE) { return status; } /* no non-trivial constraints one the plane: return the unconstrained optimum */ if (status == UNBOUNDED) { return lp_no_con<1>(n_vec, d_vec, opt); } if (std::fabs(cross2(n_vec, d_vec)) < 2.0 * eps * eps) { if (dot2(n_vec, n_vec) < 2.0 * eps * eps || dot2(d_vec, d_vec) > 2.0 * eps * eps) { /* numerator is zero or numerator and denominator are linearly dependent */ opt[0] = cw_vec[0]; opt[1] = cw_vec[1]; status = AMBIGUOUS; } else { /* numerator is non-zero and denominator is zero minimize linear functional on circle */ if (!degen && cross2(cw_vec, n_vec) <= 0.0 && cross2(n_vec, ccw_vec) <= 0.0) { /* optimum is in interior of feasible region */ opt[0] = -n_vec[0]; opt[1] = -n_vec[1]; } else if (dot2(n_vec, cw_vec) > dot2(n_vec, ccw_vec)) { /* optimum is at CCW boundary */ opt[0] = ccw_vec[0]; opt[1] = ccw_vec[1]; } else { /* optimum is at CW boundary */ opt[0] = cw_vec[0]; opt[1] = cw_vec[1]; } status = MINIMUM; } } else { /* niether numerator nor denominator is zero */ lp_min_lin_rat(degen, cw_vec, ccw_vec, n_vec, d_vec, opt); status = MINIMUM; } return status; } /* find the largest coefficient in a plane */ template <int d> inline void findimax(const double *pln, int *imax) { *imax = 0; double rmax = std::fabs(pln[0]); for (int i = 1; i <= d; i++) { const double ab = std::fabs(pln[i]); if (ab > rmax) { *imax = i; rmax = ab; } } } template <int d> inline void vector_up(const double *equation, const int ivar, const double *low_vector, double *vector) { vector[ivar] = 0.0; for (int i = 0; i <= d; i++) { if (i != ivar) { const int j = i < ivar ? i : i - 1; vector[i] = low_vector[j]; vector[ivar] -= equation[i] * low_vector[j]; } } vector[ivar] /= equation[ivar]; } template <int d> inline void vector_down(const double *elim_eqn, const int ivar, const double *old_vec, double *new_vec) { double ve = 0.0; double ee = 0.0; for (int i = 0; i <= d; i++) { ve += old_vec[i] * elim_eqn[i]; ee += elim_eqn[i] * elim_eqn[i]; } const double fac = ve / ee; for (int i = 0; i <= d; i++) { if (i != ivar) { new_vec[i < ivar ? i : i - 1] = old_vec[i] - elim_eqn[i] * fac; } } } template <int d> inline void plane_down(const double *elim_eqn, const int ivar, const double *old_plane, double *new_plane) { const double crit = old_plane[ivar] / elim_eqn[ivar]; for (int i = 0; i <= d; i++) { if (i != ivar) { new_plane[i < ivar ? i : i - 1] = old_plane[i] - elim_eqn[i] * crit; } } } template <int d> inline int linfracprog(const double *halves, /* halves --- half spaces */ const int max_size, /* max_size --- size of halves array */ const int m, /* m --- terminal marker */ const double *n_vec, /* n_vec --- numerator vector */ const double *d_vec, /* d_vec --- denominator vector */ double *opt, /* opt --- optimum */ double *work, /* work --- work space (see below) */ int *next, /* next --- array of indices into halves */ int *prev) /* prev --- array of indices into halves */ /* ** ** half-spaces are in the form ** halves[i][0]*x[0] + halves[i][1]*x[1] + ** ... + halves[i][d-1]*x[d-1] + halves[i][d]*x[d] >= 0 ** ** coefficients should be normalized ** half-spaces should be in random order ** the order of the half spaces is 0, next[0] next[next[0]] ... ** and prev[next[i]] = i ** ** halves: (max_size)x(d+1) ** ** the optimum has been computed for the half spaces ** 0 , next[0], next[next[0]] , ... , prev[0] ** the next plane that needs to be tested is 0 ** ** m is the index of the first plane that is NOT on the list ** i.e. m is the terminal marker for the linked list. ** ** the objective function is dot(x,nvec)/dot(x,dvec) ** if you want the program to solve standard d dimensional linear programming ** problems then n_vec = ( x0, x1, x2, ..., xd-1, 0) ** and d_vec = ( 0, 0, 0, ..., 0, 1) ** and halves[0] = (0, 0, ... , 1) ** ** work points to (max_size+3)*(d+2)*(d-1)/2 double space */ { int status, imax; double *new_opt, *new_n_vec, *new_d_vec, *new_halves, *new_work; const double *plane_i; double val = 0.0; for (int j = 0; j <= d; j++) { val += d_vec[j] * d_vec[j]; } const bool d_vec_zero = (val < (d + 1) * eps * eps); /* find the unconstrained minimum */ status = lp_no_con<d>(n_vec, d_vec, opt); if (m <= 0) { return status; } /* allocate memory for next level of recursion */ new_opt = work; new_n_vec = new_opt + d; new_d_vec = new_n_vec + d; new_halves = new_d_vec + d; new_work = new_halves + max_size * d; for (int i = 0; i != m; i = next[i]) { /* if the optimum is not in half space i then project the problem onto that plane */ plane_i = halves + i * (d + 1); /* determine if the optimum is on the correct side of plane_i */ val = 0.0; for (int j = 0; j <= d; j++) { val += opt[j] * plane_i[j]; } if (val < -(d + 1) * eps) { /* find the largest of the coefficients to eliminate */ findimax<d>(plane_i, &imax); /* eliminate that variable */ if (i != 0) { const double fac = 1.0 / plane_i[imax]; for (int j = 0; j != i; j = next[j]) { const double *old_plane = halves + j * (d + 1); const double crit = old_plane[imax] * fac; double *new_plane = new_halves + j * d; for (int k = 0; k <= d; k++) { const int l = k < imax ? k : k - 1; new_plane[l] = k != imax ? old_plane[k] - plane_i[k] * crit : new_plane[l]; } } } /* project the objective function to lower dimension */ if (d_vec_zero) { vector_down<d>(plane_i, imax, n_vec, new_n_vec); for (int j = 0; j < d; j++) { new_d_vec[j] = 0.0; } } else { plane_down<d>(plane_i, imax, n_vec, new_n_vec); plane_down<d>(plane_i, imax, d_vec, new_d_vec); } /* solve sub problem */ status = linfracprog<d - 1>(new_halves, max_size, i, new_n_vec, new_d_vec, new_opt, new_work, next, prev); /* back substitution */ if (status != INFEASIBLE) { vector_up<d>(plane_i, imax, new_opt, opt); /* inline code for unit */ double mag = 0.0; for (int j = 0; j <= d; j++) { mag += opt[j] * opt[j]; } mag = 1.0 / sqrt(mag); for (int j = 0; j <= d; j++) { opt[j] *= mag; } } else { return status; } /* place this offensive plane in second place */ i = move_to_front(i, next, prev); } } return status; } template <> inline int linfracprog<1>(const double *halves, const int max_size, const int m, const double *n_vec, const double *d_vec, double *opt, double *work, int *next, int *prev) { if (m > 0) { return lp_base_case((const double(*)[2])halves, m, n_vec, d_vec, opt, next, prev); } else { return lp_no_con<1>(n_vec, d_vec, opt); } } inline void rand_permutation(const int n, int *p) { typedef std::uniform_int_distribution<int> rand_int; typedef rand_int::param_type rand_range; static std::mt19937_64 gen; static rand_int rdi(0, 1); int j, k; for (int i = 0; i < n; i++) { p[i] = i; } for (int i = 0; i < n; i++) { rdi.param(rand_range(0, n - i - 1)); j = rdi(gen) + i; k = p[j]; p[j] = p[i]; p[i] = k; } } template <int d> inline double linprog(const Eigen::Matrix<double, d, 1> &c, const Eigen::Matrix<double, -1, d> &A, const Eigen::Matrix<double, -1, 1> &b, Eigen::Matrix<double, d, 1> &x) /* ** min cTx, s.t. Ax<=b ** dim(x) << dim(b) */ { int m = b.size() + 1; x.setZero(); if (m <= 1) { return c.cwiseAbs().maxCoeff() > 0.0 ? -INFINITY : 0.0; } Eigen::VectorXi perm(m - 1); Eigen::VectorXi next(m); /* original allocated size is m, here changed to m + 1 for legal tail accessing */ Eigen::VectorXi prev(m + 1); Eigen::Matrix<double, d + 1, 1> n_vec; Eigen::Matrix<double, d + 1, 1> d_vec; Eigen::Matrix<double, d + 1, 1> opt; Eigen::Matrix<double, d + 1, -1, Eigen::ColMajor> halves(d + 1, m); Eigen::VectorXd work((m + 3) * (d + 2) * (d - 1) / 2); halves.col(0).setZero(); halves(d, 0) = 1.0; halves.topRightCorner(d, m - 1) = -A.transpose(); halves.bottomRightCorner(1, m - 1) = b.transpose(); /* normalize all halves as required in linfracprog */ halves.colwise().normalize(); n_vec.head(d) = c; n_vec(d) = 0.0; d_vec.setZero(); d_vec(d) = 1.0; /* randomize the input planes */ rand_permutation(m - 1, perm.data()); /* previous to 0 is actually never used */ prev(0) = 0; /* link the zero position in at the beginning */ next(0) = perm(0) + 1; prev(perm(0) + 1) = 0; /* link the other planes */ for (int i = 0; i < m - 2; i++) { next(perm(i) + 1) = perm(i + 1) + 1; prev(perm(i + 1) + 1) = perm(i) + 1; } /* flag the last plane */ next(perm(m - 2) + 1) = m; int status = sdlp::linfracprog<d>(halves.data(), m, m, n_vec.data(), d_vec.data(), opt.data(), work.data(), next.data(), prev.data()); /* handle states for linprog whose definitions differ from linfracprog */ double minimum = INFINITY; if (status != sdlp::INFEASIBLE) { if (opt(d) != 0.0 && status != sdlp::UNBOUNDED) { x = opt.head(d) / opt(d); minimum = c.dot(x); } if (opt(d) == 0.0 || status == sdlp::UNBOUNDED) { x = opt.head(d); minimum = -INFINITY; } } return minimum; } } // namespace sdlp #endif