From: jpreiss Date: Mon, 1 Jun 2020 20:56:39 +0000 (-0700) Subject: revert polytopes for now, refine API on branch X-Git-Url: https://git.owens.tech///git?a=commitdiff_plain;h=58f48eaa61e1d50c46f5345500c621f1b93e2b22;p=forks%2Fcmath3d.git revert polytopes for now, refine API on branch --- diff --git a/Makefile b/Makefile index c51dd8b..833525a 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,5 @@ test_math3d: test.c math3d.h - $(CC) -std=c99 -Wall -Wpedantic -Wdouble-promotion -g -DCMATH3D_ASSERTS -o test_math3d test.c -lm + $(CC) -std=c99 -Wall -Wpedantic -Wdouble-promotion -g -o test_math3d test.c -lm test: ./test_math3d ./test_math3d diff --git a/README.md b/README.md index 7e40725..2397871 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # cmath3d -3d math library for C. Vectors, 3x3 matrices, quaternions, polytopes. 32-bit floats. +3d math library for C. Vectors, 3x3 matrices, quaternions. 32-bit floats. ## motivation This library is intended for embedded projects where C++ is not used. diff --git a/math3d.h b/math3d.h index b3e861b..0671d7b 100644 --- a/math3d.h +++ b/math3d.h @@ -28,10 +28,6 @@ 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) @@ -49,22 +45,6 @@ static inline float clamp(float value, float min, float max) { if (value > max) return max; return value; } -// test two floats for approximate equality using the "consecutive floats have -// consecutive bit representations" property. Argument `ulps` is the number of -// steps to allow. this does not work well for numbers near zero. -// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ -static inline bool fcloseulps(float a, float b, int ulps) { - if ((a < 0.0f) != (b < 0.0f)) { - // Handle negative zero. - if (a == b) { - return true; - } - return false; - } - int ia = *((int *)&a); - int ib = *((int *)&b); - return abs(ia - ib) <= ulps; -} // ---------------------------- 3d vectors ------------------------------ @@ -91,12 +71,6 @@ 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 @@ -159,14 +133,6 @@ 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); @@ -821,161 +787,4 @@ 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; -} - -// Finds the intersection between a ray and a convex polytope boundary. -// The polytope is defined by the linear inequalities Ax <= b. -// The ray must originate within the polytope. -// -// Args: -// origin: Origin of the ray. Must lie within the polytope. -// direction: Direction of the ray. -// A: n x 3 matrix, row-major. Each row must have L2 norm of 1. -// b: n vector. -// n: Number of inequalities (rows in A). -// -// Returns: -// s: positive float such that (origin + s * direction) is on the polytope -// boundary. If the ray does not intersect the polytope -- for example, if -// the polytope is unbounded -- then float INFINITY will be returned. -// If the polytope is empty, then a negative number will be returned. -// If `origin` does not lie within the polytope, return value is undefined. -// active_row: output argument. The row in A (face of the polytope) that -// the ray intersects. The point (origin + s * direction) will satisfy the -// equation in that row with equality. If the ray intersects the polytope -// at an intersection of two or more faces, active_row will be an arbitrary -// member of the intersecting set. -// -static inline float rayintersectpolytope(struct vec origin, struct vec direction, float const A[], float const b[], int n, int *active_row) -{ - #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 min_s = INFINITY; - int min_row = -1; - - for (int i = 0; i < n; ++i) { - struct vec a = vloadf(A + 3 * i); - float a_dir = vdot(a, direction); - if (a_dir <= 0.0f) { - // The ray points away from or parallel to the polytope face, - // so it will never intersect. - continue; - } - // Solve for the intersection point of this halfspace algebraically. - float s = (b[i] - vdot(a, origin)) / a_dir; - if (s < min_s) { - min_s = s; - min_row = i; - } - } - - *active_row = min_row; - return min_s; -} - -// Projects v onto the convex polytope defined by linear inequalities Ax <= b. -// Returns argmin_{x: Ax <= b} |x - v|_2. Uses Dykstra's (not Dijkstra's!) -// projection algorithm [1] with robust stopping criteria [2]. -// -// 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: Stop when *approximately* violates the polytope constraints -// by no more than this value. Not exact - be conservative if needed. -// maxiters: Terminate after this many iterations regardless of convergence. -// -// 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 -// [2] Birgin, E. G., and Raydan, M. (2005). Robust Stopping Criteria for -// Dykstra's Algorithm. SIAM J. Scientific Computing 26(4): 1405-1414. -// doi:10.1137/03060062X -// -static inline struct vec vprojectpolytope(struct vec v, float const A[], float const b[], float work[], int n, float tolerance, int maxiters) -{ - // 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; - } - - // For user-friendliness, we accept a tolerance value in terms of - // the Euclidean magnitude of the polytope violation. However, the - // stopping criteria we wish to use is the robust one of [2] - - // specifically, the expression called c_I^k - which is based on the - // sum of squared projection residuals. This is a feeble attempt to get - // a ballpark tolerance value that is roughly equivalent. - float const tolerance2 = n * fsqr(tolerance) / 10.0f; - struct vec x = v; - - for (int iter = 0; iter < maxiters; ++iter) { - float c = 0.0f; - for (int i = 0; i < n; ++i) { - struct vec x_old = x; - struct vec ai = vloadf(A + 3 * i); - struct vec zi_old = vloadf(z + 3 * i); - x = vprojecthalfspace(vsub(x_old, zi_old), ai, b[i]); - struct vec zi = vadd3(x, vneg(x_old), zi_old); - vstoref(zi, z + 3 * i); - c += vdist2(zi_old, zi); - } - if (c < tolerance2) { - return x; - } - } - return x; -} - - // Overall TODO: lines? segments? planes? axis-aligned boxes? spheres? diff --git a/test.c b/test.c index 8cf05eb..626587a 100644 --- a/test.c +++ b/test.c @@ -28,13 +28,6 @@ struct vec randcube() { return mkvec(randu(-1.0f, 1.0f), randu(-1.0f, 1.0f), randu(-1.0f, 1.0f)); } -// returns a random vector sampled from a Gaussian with expected L2 norm of 1. -// Note that this is *not* an identity covariance matrix. -struct vec randnvec() { - struct vec v = mkvec(randn(), randn(), randn()); - return vscl(1.0 / sqrtf(3.0), v); -} - // returns a random vector approximately uniformly sampled from the unit sphere struct vec randsphere() { struct vec v = mkvec(randn(), randn(), randn()); @@ -61,34 +54,6 @@ do { \ } \ } while(0) \ -// Generates n linear inequalities Ax <= b representing a convex polytope. -// -// The rows of A will be normalized to have L2 norm of 1. -// The polytope interior will be non-empty. -// The polytope may or may not contain the origin. -// The polytope may or may not be bounded, but will be with high probability -// as n increases. -// The returned interior point will be, on average, around 1 unit away -// from any polytope face. -// -// Returns a point in the polytope interior. -struct vec randpolytope(float A[], float b[], int n) { - // 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 = vscl(randu(0.0f, 4.0f), randsphere()); - - for (int i = 0; i < n; ++i) { - struct vec a = randsphere(); - vstoref(a, A + 3 * i); - b[i] = randu(0.01f, 2.0f) + vdot(a, shift); - } - - return shift; -} - // // tests - when adding new tests, make sure to add to test_fns array @@ -189,159 +154,6 @@ void test_qvectovec() printf("%s passed\n", __func__); } -void test_polytope_projection() -{ - 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 = 100; - float const TOLERANCE = 1e-6; - float const MAXITERS = 500; - - 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; - - struct vec interior = randpolytope(A, b, n); - assert(vinpolytope(interior, A, b, n, 1e-10)); - - for (int point = 0; point < N_PROJECTIONS; ++point) { - - struct vec x = randsphere(); - struct vec xp = vprojectpolytope(x, A, b, work, n, TOLERANCE, MAXITERS); - - // 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, 10 * TOLERANCE)); - - // 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(randnvec(), interior); - 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__); -} - -void test_polytope_ray() -{ - srand(1); // deterministic - - // 1. Basic checks using an infinite rectangular "tube". - { - float const A[4][3] = { - { 1.0f, 0.0f, 0.0f}, - { 0.0f, 1.0f, 0.0f}, - {-1.0f, 0.0f, 0.0f}, - { 0.0f, -1.0f, 0.0f}, - }; - float const b[4] = { - 1.0, - 2.0, - 3.0, - 4.0, - }; - - float const *Aflat = &A[0][0]; - int row = -1; - - for (int i = 0; i < 4; ++i) { - // Test the face. - struct vec dir = vloadf(&A[i][0]); - float s = rayintersectpolytope(vzero(), dir, Aflat, b, 4, &row); - assert(s == b[i]); // No floating-point error expected. - assert(row == i); - - // Test the corner. - int j = (i + 1) % 4; - struct vec next_dir = vloadf(&A[j][0]); - struct vec corner_us = vadd( - vscl(b[i] + 1e-6, dir), - vscl(b[j], next_dir) - ); - rayintersectpolytope(vzero(), corner_us, Aflat, b, 4, &row); - assert(row == i); - - struct vec corner_next = vadd( - vscl(b[i], dir), - vscl(b[j] + 1e-6, next_dir) - ); - rayintersectpolytope(vzero(), corner_next, Aflat, b, 4, &row); - assert(row == j); - } - - // Test the open ends of the tube. - float s = rayintersectpolytope(vzero(), mkvec(0, 0, 1), Aflat, b, 4, &row); - assert(isinf(s)); - - s = rayintersectpolytope(vzero(), mkvec(0, 0, -1), Aflat, b, 4, &row); - assert(isinf(s)); - - // Test that we can find very far away interesections. - s = rayintersectpolytope(vzero(), mkvec(0, 1e-9, -1), Aflat, b, 4, &row); - assert(!isinf(s)); - } - - // 2. Test that we handle loose-everywhere constraints. - { - float A[2][3]; - float b[2]; - struct vec v = randsphere(); - - vstoref(v, A[0]); - b[0] = 1.0; - - vstoref(v, A[1]); - b[1] = 1.1; - - int row; - float s = rayintersectpolytope(vscl(0.5, v), v, A[0], b, 2, &row); - assert(fcloseulps(s, 0.5, 10)); - } - - // 3. Test the stated behavior for empty polytopes. - { - float A[2][3]; - float b[2]; - struct vec v = randsphere(); - - vstoref(v, A[0]); - b[0] = -1.0; - - vstoref(vneg(v), A[1]); - b[1] = -1.0; - - int row; - float s = rayintersectpolytope(vscl(0.5, v), v, A[0], b, 2, &row); - assert(s < 0.0f); - } - - // 4. Test random polytopes. - - printf("%s passed\n", __func__); -} - // micro test framework typedef void (*voidvoid_fn)(void); voidvoid_fn test_fns[] = { @@ -350,8 +162,6 @@ voidvoid_fn test_fns[] = { test_quat_rpy_conversions, test_quat_mat_conversions, test_qvectovec, - test_polytope_projection, - test_polytope_ray, }; static int i_test = -1;