From: jpreiss Date: Tue, 24 Mar 2020 02:45:37 +0000 (-0700) Subject: polytope membership and projection, minor vec utils X-Git-Url: https://git.owens.tech/projects.html/projects.html/git?a=commitdiff_plain;h=34a0ea72a9455f70ece1d44b49c4e4fce5eb0179;p=forks%2Fcmath3d.git polytope membership and projection, minor vec utils --- diff --git a/Makefile b/Makefile index 833525a..c51dd8b 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,5 @@ test_math3d: test.c math3d.h - $(CC) -std=c99 -Wall -Wpedantic -Wdouble-promotion -g -o test_math3d test.c -lm + $(CC) -std=c99 -Wall -Wpedantic -Wdouble-promotion -g -DCMATH3D_ASSERTS -o test_math3d test.c -lm test: ./test_math3d ./test_math3d diff --git a/math3d.h b/math3d.h index 0671d7b..a240bcc 100644 --- a/math3d.h +++ b/math3d.h @@ -28,6 +28,10 @@ SOFTWARE. #include #include +#ifdef CMATH3D_ASSERTS +#include +#endif + #ifndef M_PI_F #define M_PI_F (3.14159265358979323846f) #define M_1_PI_F (0.31830988618379067154f) @@ -71,6 +75,12 @@ static inline struct vec vrepeat(float x) { static inline struct vec vzero(void) { return vrepeat(0.0f); } +// construct the i'th basis vector, i.e. vbasis(0) == (1, 0, 0). +static inline struct vec vbasis(int i) { + float a[3] = {0.0f, 0.0f, 0.0f}; + a[i] = 1.0f; + return mkvec(a[0], a[1], a[2]); +} // // operators @@ -133,6 +143,14 @@ static inline float vdist(struct vec a, struct vec b) { static inline struct vec vnormalize(struct vec v) { return vdiv(v, vmag(v)); } +// clamp the Euclidean norm of a vector if it exceeds given maximum. +static inline struct vec vclampnorm(struct vec v, float maxnorm) { + float const norm = vmag(v); + if (norm > maxnorm) { + return vscl(maxnorm / norm, v); + } + return v; +} // vector cross product. static inline struct vec vcross(struct vec a, struct vec b) { return mkvec(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x); @@ -787,4 +805,94 @@ static inline void qstoref(struct quat q, float *f) { f[0] = q.x; f[1] = q.y; f[2] = q.z; f[3] = q.w; } + +// ------------------------ convex sets in R^3 --------------------------- + +// project v onto the halfspace H = {x : a^T x <= b}, where a is a unit vector. +// If v lies in H, returns v. Otherwise, returns the closest point, which +// minimizes |x - v|_2, and will satisfy a^T x = b. +static inline struct vec vprojecthalfspace(struct vec x, struct vec a_unit, float b) { + float ax = vdot(a_unit, x); + if (ax <= b) { + return x; + } + return vadd(x, vscl(b - ax, a_unit)); +} + +// test if v lies in the convex polytope defined by linear inequalities Ax <= b. +// A: n x 3 matrix, row-major. Each row must have L2 norm of 1. +// b: n vector. +// tolerance: Allow violations up to Ax <= b + tolerance. +static inline bool vinpolytope(struct vec v, float const A[], float const b[], int n, float tolerance) +{ + for (int i = 0; i < n; ++i) { + struct vec a = vloadf(A + 3 * i); + if (vdot(a, v) > b[i] + tolerance) { + return false; + } + } + return true; +} + +// Projects v onto the convex polytope defined by linear inequalities Ax <= b. +// Returns argmin_{x: Ax <= b} |x - v|_2. Uses Diykstra's (not Dijkstra's!) +// projection algorithm [1]. +// +// Args: +// v: vector to project into the polytope. +// A: n x 3 matrix, row-major. Each row must have L2 norm of 1. +// b: n vector. +// work: n x 3 matrix. will be overwritten. input values are not used. +// tolerance: Halt once iterates satisfy |x_{k + 1} - x_k|_2 < tolerance. +// Roughly, but not actually, equivalent to allowing constraint violations +// up to Ax >= b + tolerance. +// +// Returns: +// The projection of v into the polytope. +// +// References: +// [1] Boyle, J. P., and Dykstra, R. L. (1986). A Method for Finding +// Projections onto the Intersection of Convex Sets in Hilbert Spaces. +// Lecture Notes in Statistics, 28–47. doi:10.1007/978-1-4613-9940-7_3 +// +static inline struct vec vprojectpolytope(struct vec v, float const A[], float const b[], float work[], int n, float tolerance) +{ + // early bailout. + if (vinpolytope(v, A, b, n, tolerance)) { + return v; + } + + #ifdef CMATH3D_ASSERTS + // check for normalized input. + for (int i = 0; i < n; ++i) { + struct vec a = vloadf(A + 3 * i); + assert(fabsf(vmag2(a) - 1.0f) < 1e-6f); + } + #endif + + float *z = work; + for (int i = 0; i < 3 * n; ++i) { + z[i] = 0.0f; + } + + float const tolerance2 = fsqr(tolerance); + struct vec x = v; + + while (true) { + struct vec x_last_iter = x; + for (int i = 0; i < n; ++i) { + struct vec x_old = x; + struct vec ai = vloadf(A + 3 * i); + struct vec zi = vloadf(z + 3 * i); + x = vprojecthalfspace(vadd(x_old, zi), ai, b[i]); + struct vec zi_new = vadd3(x_old, zi, vneg(x)); + vstoref(zi_new, z + 3 * i); + } + if (vdist2(x_last_iter, x) < tolerance2) { + return x; + } + } +} + + // Overall TODO: lines? segments? planes? axis-aligned boxes? spheres? diff --git a/test.c b/test.c index 626587a..2ea12d8 100644 --- a/test.c +++ b/test.c @@ -154,6 +154,68 @@ void test_qvectovec() printf("%s passed\n", __func__); } +void test_polytope() +{ + srand(1); // deterministic + + // For each polytope, generate a few points, project them, + // and check that the projections are closer than other points. + int const N_POLYTOPES = 100; + int const N_PROJECTIONS = 10; + int const N_OTHERS = 1000; + + int const MAX_FACES = 30; + float *A = malloc(sizeof(float) * MAX_FACES * 3); + float *b = malloc(sizeof(float) * MAX_FACES); + float *work = malloc(sizeof(float) * MAX_FACES * 3); + + + for (int trial = 0; trial < N_POLYTOPES; ++trial) { + + // Random number of polytope faces. + int n = rand() % MAX_FACES + 1; + + // If we generate random Ax <= b with b always positive, we ensure that + // x = 0 is always a solution; hence the polytope is non-empty. However, + // we also want to test nonempty polytopes that don't contain x = 0. + // Therefore, we use A(x + shift) <= b with b positive, which is + // equivalent to Ax <= b - Ashift. + struct vec shift = randsphere(); + + for (int i = 0; i < n; ++i) { + struct vec a = randsphere(); + vstoref(a, A + 3 * i); + b[i] = randu(0.01, 10) - vdot(a, shift); + } + + for (int point = 0; point < N_PROJECTIONS; ++point) { + struct vec x = randsphere(); + struct vec xp = vprojectpolytope(x, A, b, work, n, 1e-6); + + // Feasibility check: projection is inside polytope. + // The tolerance is looser than the vprojectpolytope tolerance + // because that tolerance doesn't actually guarantee a rigid bound + // on constraint violations. + assert(vinpolytope(xp, A, b, n, 1e-5)); + + // Optimality check: projection is closer than other random points + // to query point. Very large N_OTHERS would be more thorough... + for (int other = 0; other < N_OTHERS; ++other) { + struct vec other = vadd(randsphere(), shift); + if (vinpolytope(other, A, b, n, 0.0f)) { + assert (vdist2(xp, x) <= vdist2(other, x)); + } + } + } + } + + free(A); + free(b); + free(work); + + printf("%s passed\n", __func__); +} + // micro test framework typedef void (*voidvoid_fn)(void); voidvoid_fn test_fns[] = { @@ -162,6 +224,7 @@ voidvoid_fn test_fns[] = { test_quat_rpy_conversions, test_quat_mat_conversions, test_qvectovec, + test_polytope, }; static int i_test = -1;