ray-polytope intersection

diff --git a/math3d.h b/math3d.h
index 796e802..b3e861b 100644 (file)
--- 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 (file)
--- 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;