From c8a20b9e1b456985cc6b71835b2345916657b779 Mon Sep 17 00:00:00 2001 From: James Preiss Date: Wed, 6 May 2020 17:30:04 -0700 Subject: [PATCH] ray-polytope intersection --- math3d.h | 72 +++++++++++++++++++++++++++++++++++++++++ test.c | 98 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 170 insertions(+) diff --git a/math3d.h b/math3d.h index 796e802..b3e861b 100644 --- a/math3d.h +++ b/math3d.h @@ -49,6 +49,22 @@ 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 ------------------------------ @@ -834,6 +850,62 @@ static inline bool vinpolytope(struct vec v, float const A[], float const b[], i 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]. diff --git a/test.c b/test.c index bfa1bc1..8cf05eb 100644 --- a/test.c +++ b/test.c @@ -244,6 +244,103 @@ void test_polytope_projection() 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); @@ -254,6 +351,7 @@ voidvoid_fn test_fns[] = { test_quat_mat_conversions, test_qvectovec, test_polytope_projection, + test_polytope_ray, }; static int i_test = -1; -- 2.20.1