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
# 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.
#include <math.h>
#include <stdbool.h>
-#ifdef CMATH3D_ASSERTS
-#include <assert.h>
-#endif
-
#ifndef M_PI_F
#define M_PI_F (3.14159265358979323846f)
#define M_1_PI_F (0.31830988618379067154f)
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 ------------------------------
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
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);
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?
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());
} \
} 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
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[] = {
test_quat_rpy_conversions,
test_quat_mat_conversions,
test_qvectovec,
- test_polytope_projection,
- test_polytope_ray,
};
static int i_test = -1;
projects / forks / cmath3d.git / commitdiff