1 files changed, 54 insertions, 5 deletions

diff --git a/src/geom.zig b/src/geom.zig
index 80cf1db..d6dc036 100644
--- a/src/geom.zig
+++ b/src/geom.zig
@@ -19,6 +19,7 @@
 
 const Child = std.meta.Child;
 const degreesToRadians = std.math.degreesToRadians;
+const f32Eps = std.math.floatEps(f32);
 const std = @import("std");
 
 fn sqr(x: anytype) @TypeOf(x) {
@@ -29,6 +30,8 @@ fn dot(u: anytype, v: @TypeOf(u)) Child(@TypeOf(u)) {
     return @reduce(.Add, u * v);
 }
 
+pub const XYZ = extern struct { x: f32, y: f32, z: f32 };
+
 export fn sqrlen(v: XYZ) f32 {
     const u: @Vector(3, f32) = @bitCast(v);
     return dot(u, u);
@@ -43,8 +46,6 @@ export fn len(v: XYZ) f32 {
     return norm(u);
 }
 
-const XYZ = extern struct { x: f32, y: f32, z: f32 };
-
 export fn crossProduct(u: XYZ, v: XYZ) XYZ {
     return .{
         .x = u.y * v.z - u.z * v.y,
@@ -86,12 +87,10 @@ export fn rotate(v: XYZ, deg_x: f32, deg_y: f32, deg_z: f32) XYZ {
     return u;
 }
 
-export fn segmentIntersectsSphere(a: XYZ, b: XYZ, i: XYZ, r: f32) bool {
-    // FIXME: call directly with vectors
+export fn segCrossSphere(a: XYZ, b: XYZ, i: XYZ, r: f32) bool {
     const p: @Vector(3, f32) = @bitCast(a);
     const q: @Vector(3, f32) = @bitCast(b);
     const c: @Vector(3, f32) = @bitCast(i);
-
     if (@reduce(.Or, @max(p, q) < c - splat(3, r))) return false;
     if (@reduce(.Or, @min(p, q) > c + splat(3, r))) return false;
     // https://en.wikipedia.org/wiki/Line–sphere_intersection
@@ -100,6 +99,56 @@ export fn segmentIntersectsSphere(a: XYZ, b: XYZ, i: XYZ, r: f32) bool {
     return sqr(dot(u, (p - c))) >= @reduce(.Add, sqr(p - c)) - sqr(r);
 }
 
+export fn segCrossTrigon(start: XYZ, end: XYZ,
+                         p_a: *const XYZ, p_b: *const XYZ, p_c: *const XYZ,
+                         normal: *const XYZ, intersection: *XYZ) bool {
+    const p: @Vector(3, f32) = @bitCast(start);
+    const q: @Vector(3, f32) = @bitCast(end);
+    const n: @Vector(3, f32) = @bitCast(normal.*);
+    const denom = dot(q - p, n);
+    if (@fabs(denom) < f32Eps)
+        return false; // parallel segment and triangle
+
+    const a: @Vector(3, f32) = @bitCast(p_a.*);
+    const mu = (dot(a, n) - dot(p, n)) / denom;
+    const i = p + (q - p) * splat(3, mu);
+    if (mu < 0 or mu > 1)
+        return false; // intersection not within segment
+
+    const n_abs = @fabs(n);
+    const n_max = @reduce(.Max, n_abs);
+    const k: struct { usize, usize } = if (n_max == n_abs[0])
+        .{ 1, 2 }
+    else if (n_max == n_abs[1])
+        .{ 0, 2 }
+    else if (n_max == n_abs[2])
+        .{ 0, 1 }
+    else unreachable;
+
+    const b: @Vector(3, f32) = @bitCast(p_b.*);
+    const c: @Vector(3, f32) = @bitCast(p_c.*);
+    const u = @Vector(3, f32){ i[k[0]], b[k[0]], c[k[0]] } - splat(3, a[k[0]]);
+    const v = @Vector(3, f32){ i[k[1]], b[k[1]], c[k[1]] } - splat(3, a[k[1]]);
+    intersection.* = @bitCast(i);
+
+    if (@fabs(u[1]) < f32Eps) {
+        const s = u[0] / u[2];
+        if (s >= 0 and s <= 1) {
+            const t = (v[0] - s * v[2]) / v[1];
+            if (t >= 0 and s + t <= 1)
+                return true;
+        }
+    } else {
+        const s = (v[0] * u[1] - u[0] * v[1]) / (v[2] * u[1] - u[2] * v[1]);
+        if (s >= 0 and s <= 1) {
+            const t = (u[0] - s * u[2]) / u[1];
+            if (t >= 0 and s + t <= 1)
+                return true;
+        }
+    }
+    return false;
+}
+
 fn transpose(comptime n: comptime_int, m: [n]@Vector(n, f32)) @TypeOf(m) {
     const flat: @Vector(sqr(n), f32) = @bitCast(m);
     return @bitCast(@shuffle(f32, flat, undefined, blk: {