swh:1:snp:f2e49a686b032f144627ec47d1bed74dff82e01c
Tip revision: ad8e9d4f15917a78050e26fb7a846e17f90636e6 authored by Merve Asiler on 03 July 2024, 20:45:15 UTC
Update README.md
Update README.md
Tip revision: ad8e9d4
sdlp.h
#pragma once
// Taken from https://github.com/ZJU-FAST-Lab/SDLP
/*
* 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.
* Permission is granted to modify and re-distribute this code in any manner
* as long as this notice is preserved. All standard disclaimers apply.
*
* Reference: Seidel, R. (1991), "Small-dimensional linear programming and convex hulls made easy",
* Discrete & Computational Geometry 6 (1): 423–434, doi:10.1007/BF02574699
*/
#include <Eigen/Eigen>
#include <vector>
#include <cmath>
#include <random>
namespace sdlp
{
constexpr double prog_epsilon = 2.0e-16;
enum PROG_STATE
{
/* 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], double eps)
{
double size;
size = sqrt(a[0] * a[0] + a[1] * a[1]);
if (size < 2 * eps)
{
return true;
}
b[0] = a[0] / size;
b[1] = a[1] / size;
return false;
}
/* unitize a d+1 dimensional point */
inline bool lp_d_unit(int d, const double* a, double* b)
{
int i;
double size;
size = 0.0;
for (i = 0; i <= d; i++)
{
size += a[i] * a[i];
}
if (size < (d + 1) * prog_epsilon * prog_epsilon)
{
return true;
}
size = 1.0 / sqrt(size);
for (i = 0; i <= d; i++)
{
b[i] = a[i] * size;
}
return false;
}
/* optimize the objective function when there are no contraints */
inline int lp_no_constraints(int d, const double* n_vec, const double* d_vec, double* opt)
{
int i;
double n_dot_d, d_dot_d;
n_dot_d = 0.0;
d_dot_d = 0.0;
for (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 < prog_epsilon * prog_epsilon)
{
d_dot_d = 1.0;
n_dot_d = 0.0;
}
for (i = 0; i <= d; i++)
{
opt[i] = -n_vec[i] + d_vec[i] * n_dot_d / d_dot_d;
}
/* normalize the optimal point */
if (lp_d_unit(d, opt, opt))
{
opt[d] = 1.0;
return AMBIGUOUS;
}
else
{
return MINIMUM;
}
}
/* returns the index of the plane that is in i's place */
inline int move_to_front(int i, int* next, int* prev)
{
int previ;
if (i == 0 || i == next[0])
{
return i;
}
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(int degen,
const double cw_vec[2],
const double ccw_vec[2],
const double n_vec[2],
const double d_vec[2],
double opt[2])
{
double d_cw, d_ccw, n_cw, n_ccw;
/* linear rational function case */
d_cw = dot2(cw_vec, d_vec);
d_ccw = dot2(ccw_vec, d_vec);
n_cw = dot2(cw_vec, n_vec);
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 that the clock-wise and counter clockwise bounds are not near a poles */
}
else if (fabs(d_cw) > 2.0 * prog_epsilon &&
fabs(d_ccw) > 2.0 * prog_epsilon)
{
/* 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 (fabs(d_cw) > 2.0 * prog_epsilon)
{
/* the counter clockwise bound is near a pole */
if (n_ccw * d_cw > 0.0)
{
/* counter clockwise bound is a positive pole */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
/* counter clockwise bound is a negative pole */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
}
else if (fabs(d_ccw) > 2.0 * prog_epsilon)
{
/* the clockwise bound is near a pole */
if (n_cw * d_ccw > 2 * prog_epsilon)
{
/* clockwise bound is at a positive pole */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
else
{
/* clockwise 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],
int m,
int* next,
int* prev,
double cw_vec[2],
double ccw_vec[2],
int* degen)
{
int i;
double d_cw, d_ccw;
int offensive;
*degen = 0;
for (i = 0; i != m; i = next[i])
{
if (!unit2(halves[i], ccw_vec, prog_epsilon))
{
/* clock-wise */
cw_vec[0] = ccw_vec[1];
cw_vec[1] = -ccw_vec[0];
/* counter-clockwise */
ccw_vec[0] = -cw_vec[0];
ccw_vec[1] = -cw_vec[1];
break;
}
}
if (i == m)
{
return UNBOUNDED;
}
i = 0;
while (i != m)
{
offensive = 0;
d_cw = dot2(cw_vec, halves[i]);
d_ccw = dot2(ccw_vec, halves[i]);
if (d_ccw >= 2 * prog_epsilon)
{
if (d_cw <= -2 * prog_epsilon)
{
cw_vec[0] = halves[i][1];
cw_vec[1] = -halves[i][0];
unit2(cw_vec, cw_vec, prog_epsilon);
offensive = 1;
}
}
else if (d_cw >= 2 * prog_epsilon)
{
if (d_ccw <= -2 * prog_epsilon)
{
ccw_vec[0] = -halves[i][1];
ccw_vec[1] = halves[i][0];
unit2(ccw_vec, ccw_vec, prog_epsilon);
offensive = 1;
}
}
else if (d_ccw <= -2 * prog_epsilon && d_cw <= -2 * prog_epsilon)
{
return INFEASIBLE;
}
else if ((d_cw <= -2 * prog_epsilon) ||
(d_ccw <= -2 * prog_epsilon) ||
(cross2(cw_vec, halves[i]) < 0.0))
{
/* degenerate */
if (d_cw <= -2 * prog_epsilon)
{
unit2(ccw_vec, cw_vec, prog_epsilon);
}
else if (d_ccw <= -2 * prog_epsilon)
{
unit2(cw_vec, ccw_vec, prog_epsilon);
}
*degen = 1;
offensive = 1;
}
/* 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 * prog_epsilon)
{
if (d_ccw < -2 * prog_epsilon)
{
return INFEASIBLE;
}
else
{
cw_vec[0] = ccw_vec[0];
cw_vec[1] = ccw_vec[1];
}
}
else if (d_ccw < -2 * prog_epsilon)
{
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 */
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];
int degen;
int status;
double ab;
/* 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_constraints(1, n_vec, d_vec, opt);
}
ab = fabs(cross2(n_vec, d_vec));
if (ab < 2 * prog_epsilon * prog_epsilon)
{
if (dot2(n_vec, n_vec) < 2 * prog_epsilon * prog_epsilon ||
dot2(d_vec, d_vec) > 2 * prog_epsilon * prog_epsilon)
{
/* 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 counter-clockwise boundary */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
else
{
/* optimum is at clockwise 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 */
inline void findimax(const double* pln, int idim, int* imax)
{
double rmax;
int i;
*imax = 0;
rmax = fabs(pln[0]);
for (i = 1; i <= idim; i++)
{
double ab;
ab = fabs(pln[i]);
if (ab > rmax)
{
*imax = i;
rmax = ab;
}
}
}
inline void vector_up(const double* equation, int ivar, int idim,
const double* low_vector, double* vector)
{
int i;
vector[ivar] = 0.0;
for (i = 0; i < ivar; i++)
{
vector[i] = low_vector[i];
vector[ivar] -= equation[i] * low_vector[i];
}
for (i = ivar + 1; i <= idim; i++)
{
vector[i] = low_vector[i - 1];
vector[ivar] -= equation[i] * low_vector[i - 1];
}
vector[ivar] /= equation[ivar];
}
inline void vector_down(const double* elim_eqn, int ivar, int idim,
const double* old_vec, double* new_vec)
{
int i;
double fac, ve, ee;
ve = 0.0;
ee = 0.0;
for (i = 0; i <= idim; i++)
{
ve += old_vec[i] * elim_eqn[i];
ee += elim_eqn[i] * elim_eqn[i];
}
fac = ve / ee;
for (i = 0; i < ivar; i++)
{
new_vec[i] = old_vec[i] - elim_eqn[i] * fac;
}
for (i = ivar + 1; i <= idim; i++)
{
new_vec[i - 1] = old_vec[i] - elim_eqn[i] * fac;
}
}
inline void plane_down(const double* elim_eqn, int ivar, int idim,
const double* old_plane, double* new_plane)
{
double crit;
int i;
crit = old_plane[ivar] / elim_eqn[ivar];
for (i = 0; i < ivar; i++)
{
new_plane[i] = old_plane[i] - elim_eqn[i] * crit;
}
for (i = ivar + 1; i <= idim; i++)
{
new_plane[i - 1] = old_plane[i] - elim_eqn[i] * crit;
}
}
inline int linfracprog(const double* halves, /* halves --- half spaces */
int istart, /* istart --- should be zero unless doing incremental algorithm */
int m, /* m --- terminal marker */
const double* n_vec, /* n_vec --- numerator vector */
const double* d_vec, /* d_vec --- denominator vector */
int d, /* d --- projective dimension */
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 */
int max_size) /* max_size --- size of halves array */
/*
**
** 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[istart]
** the next plane that needs to be tested is istart
**
** 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;
int i, j, imax;
double* new_opt, * new_n_vec, * new_d_vec, * new_halves, * new_work;
const double* plane_i;
double val;
if (d == 1 && m != 0)
{
return lp_base_case((const double(*)[2])halves, m, n_vec,
d_vec, opt, next, prev);
}
else
{
int d_vec_zero;
val = 0.0;
for (j = 0; j <= d; j++)
{
val += d_vec[j] * d_vec[j];
}
d_vec_zero = (val < (d + 1)* prog_epsilon* prog_epsilon);
/* find the unconstrained minimum */
if (!istart)
{
status = lp_no_constraints(d, n_vec, d_vec, opt);
}
else
{
status = MINIMUM;
}
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 (i = istart; 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 (j = 0; j <= d; j++)
{
val += opt[j] * plane_i[j];
}
if (val < -(d + 1) * prog_epsilon)
{
/* find the largest of the coefficients to eliminate */
findimax(plane_i, d, &imax);
/* eliminate that variable */
if (i != 0)
{
double fac;
fac = 1.0 / plane_i[imax];
for (j = 0; j != i; j = next[j])
{
const double* old_plane;
double* new_plane;
int k;
double crit;
old_plane = halves + j * (d + 1);
new_plane = new_halves + j * d;
crit = old_plane[imax] * fac;
for (k = 0; k < imax; k++)
{
new_plane[k] = old_plane[k] - plane_i[k] * crit;
}
for (k = imax + 1; k <= d; k++)
{
new_plane[k - 1] = old_plane[k] - plane_i[k] * crit;
}
}
}
/* project the objective function to lower dimension */
if (d_vec_zero)
{
vector_down(plane_i, imax, d, n_vec, new_n_vec);
for (j = 0; j < d; j++)
{
new_d_vec[j] = 0.0;
}
}
else
{
plane_down(plane_i, imax, d, n_vec, new_n_vec);
plane_down(plane_i, imax, d, d_vec, new_d_vec);
}
/* solve sub problem */
status = linfracprog(new_halves, 0, i, new_n_vec,
new_d_vec, d - 1, new_opt, new_work, next, prev, max_size);
/* back substitution */
if (status != INFEASIBLE)
{
vector_up(plane_i, imax, d, new_opt, opt);
/* in line code for unit */
double size;
size = 0.0;
for (j = 0; j <= d; j++)
size += opt[j] * opt[j];
size = 1.0 / sqrt(size);
for (j = 0; j <= d; j++)
opt[j] *= size;
}
else
{
return status;
}
/* place this offensive plane in second place */
i = move_to_front(i, next, prev);
}
}
return status;
}
}
inline void rand_permutation(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 i, j, t;
for (i = 0; i < n; i++)
{
p[i] = i;
}
for (i = 0; i < n; i++)
{
rdi.param(rand_range(0, n - i - 1));
j = rdi(gen) + i;
t = p[j];
p[j] = p[i];
p[i] = t;
}
}
inline double linprog(const Eigen::VectorXd& c,
const Eigen::MatrixXd& A,
const Eigen::VectorXd& b,
Eigen::VectorXd& x)
/*
** min cTx, s.t. Ax<=b
** dim(x) << dim(b)
*/
{
int d = c.size();
int m = b.size() + 1;
x = Eigen::VectorXd::Zero(d);
double minimum = INFINITY;
int* perm, * next, * prev;
double* halves, * n_vec, * d_vec, * work, * opt;
int i, status = sdlp::AMBIGUOUS;
perm = (int*)malloc((m - 1) * sizeof(int));
next = (int*)malloc(m * sizeof(int));
/* original allocated size is m, here changed by m + 1 for legal tail accessing */
prev = (int*)malloc((m + 1) * sizeof(int));
halves = (double*)malloc(m * (d + 1) * sizeof(double));
n_vec = (double*)malloc((d + 1) * sizeof(double));
d_vec = (double*)malloc((d + 1) * sizeof(double));
work = (double*)malloc((m + 3) * (d + 2) * (d - 1) / 2 * sizeof(double));
opt = (double*)malloc((d + 1) * sizeof(double));
Eigen::Map<Eigen::MatrixXd> Af(halves, d + 1, m);
Eigen::Map<Eigen::VectorXd> nv(n_vec, d + 1);
Eigen::Map<Eigen::VectorXd> dv(d_vec, d + 1);
Eigen::Map<Eigen::VectorXd> xf(opt, d + 1);
Af.col(0).setZero();
Af(d, 0) = 1.0;
Af.topRightCorner(d, m - 1) = -A.transpose();
Af.bottomRightCorner(1, m - 1) = b.transpose();
nv.head(d) = c;
nv(d) = 0.0;
dv.setZero();
dv(d) = 1.0;
/* randomize the input planes */
rand_permutation(m - 1, perm);
/* 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 (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;
status = linfracprog(halves, 0, m, n_vec, d_vec,
d, opt, work, next, prev, m);
/* handle states for linprog whose definitions differ from linfracprog */
if (status != sdlp::INFEASIBLE)
{
if (xf(d) != 0.0 && status != sdlp::UNBOUNDED)
{
x = xf.head(d) / xf(d);
minimum = c.dot(x);
}
if (xf(d) == 0.0 || status == sdlp::UNBOUNDED)
{
x = xf.head(d);
minimum = -INFINITY;
}
}
free(perm);
free(next);
free(prev);
free(halves);
free(n_vec);
free(d_vec);
free(work);
free(opt);
return minimum;
}
} // namespace sdlp
struct HalfSpace;
double* sdlpMain(double extremeDirection[3], std::vector<HalfSpace>& halfSpaceSet);
class Mesh;
double* sdlpMain(const Mesh& hostMesh, double extremeDirection[3]);
