From: James Preiss <japreiss@usc.edu>
Date: Thu, 7 May 2020 00:30:04 +0000 (-0700)
Subject: ray-polytope intersection
X-Git-Url: https://git.owens.tech/assets/editable-focus.html/assets/editable-focus.html/git?a=commitdiff_plain;h=c8a20b9e1b456985cc6b71835b2345916657b779;p=forks%2Fcmath3d.git
ray-polytope intersection
---
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;