lib/geometry/src/intersect.zig
daab053ee43316e1809a84551d573ddd1e5bf3d2
1 //! Ray casts and overlap tests between shapes.
2 //!
3 //! Ray queries report the parameter `t` of the first point on or inside the
4 //! shape, in units of the ray direction, and ignore the part of the ray behind
5 //! its origin. A ray starting inside a solid reports `t = 0`. Triangles are
6 //! two-sided.
7 const std = @import("std");
8 const linear = @import("linear");
9 const closest = @import("closest.zig");
10 const primitive = @import("primitive.zig");
11 const tolerance = @import("tolerance.zig");
12
13 const Aabb = primitive.Aabb;
14 const Capsule = primitive.Capsule;
15 const Obb = primitive.Obb;
16 const Plane = primitive.Plane;
17 const Ray = primitive.Ray;
18 const Sphere = primitive.Sphere;
19 const Triangle = primitive.Triangle;
20 const Vec3 = linear.Vec3;
21
22 /// Where a ray meets a triangle: the ray parameter and the barycentric
23 /// weights of the second and third corners.
24 pub const TriangleHit = struct {
25 t: f32,
26 u: f32,
27 v: f32,
28 };
29
30 /// Möller–Trumbore over the whole line, from both sides. The caller filters
31 /// `t` to its span. A ray that grazes the triangle plane, or meets a
32 /// degenerate triangle, misses, because its determinant is parallel noise.
33 pub fn rayTriangle(ray: Ray, triangle: Triangle) ?TriangleHit {
34 const edge1 = triangle.b.sub(triangle.a);
35 const edge2 = triangle.c.sub(triangle.a);
36 const pvec = ray.direction.cross(edge2);
37 const determinant = edge1.dot(pvec);
38 const factors_sq = edge1.lengthSq() * edge2.lengthSq() * ray.direction.lengthSq();
39 if (tolerance.isParallel(determinant * determinant, factors_sq)) return null;
40
41 const inverse = 1.0 / determinant;
42 const tvec = ray.origin.sub(triangle.a);
43 const u = tvec.dot(pvec) * inverse;
44 if (u < 0.0 or u > 1.0) return null;
45
46 const qvec = tvec.cross(edge1);
47 const v = ray.direction.dot(qvec) * inverse;
48 if (v < 0.0 or u + v > 1.0) return null;
49
50 return .{ .t = edge2.dot(qvec) * inverse, .u = u, .v = v };
51 }
52
53 /// The two roots of `|origin + direction t - center|^2 = radius^2`, or null
54 /// when the line stays outside the sphere or the direction has no length.
55 pub fn lineSphereRoots(origin: Vec3, direction: Vec3, center: Vec3, radius: f32) ?[2]f32 {
56 const m = origin.sub(center);
57 const a = direction.lengthSq();
58 if (a <= tolerance.closing_sq_floor) return null;
59 const b = m.dot(direction);
60 const c = m.lengthSq() - radius * radius;
61 const discriminant = b * b - a * c;
62 if (discriminant < 0) return null;
63 const root = @sqrt(discriminant);
64 return .{ (-b - root) / a, (-b + root) / a };
65 }
66
67 pub fn raySphere(ray: Ray, sphere: Sphere) ?f32 {
68 const m = ray.origin.sub(sphere.center);
69 if (m.lengthSq() <= sphere.radius * sphere.radius) return 0;
70 const roots = lineSphereRoots(ray.origin, ray.direction, sphere.center, sphere.radius) orelse return null;
71 if (roots[1] < 0) return null;
72 return @max(roots[0], 0);
73 }
74
75 /// The span of `t` over which a ray lies inside a box, clipped to `[0, t_max]`,
76 /// or null when the ray misses within that span.
77 pub fn raySlab(ray: Ray, box: Aabb, t_max: f32) ?[2]f32 {
78 const origin = ray.origin.toArray();
79 const direction = ray.direction.toArray();
80 const low = box.min.toArray();
81 const high = box.max.toArray();
82 var near: f32 = 0;
83 var far: f32 = t_max;
84 for (0..3) |axis| {
85 if (@abs(direction[axis]) <= tolerance.direction_component_floor) {
86 if (origin[axis] < low[axis] or origin[axis] > high[axis]) return null;
87 continue;
88 }
89 const inverse = 1.0 / direction[axis];
90 const first = (low[axis] - origin[axis]) * inverse;
91 const second = (high[axis] - origin[axis]) * inverse;
92 near = @max(near, @min(first, second));
93 far = @min(far, @max(first, second));
94 if (near > far) return null;
95 }
96 return .{ near, far };
97 }
98
99 pub fn rayAabb(ray: Ray, box: Aabb) ?f32 {
100 const span = raySlab(ray, box, std.math.inf(f32)) orelse return null;
101 return span[0];
102 }
103
104 pub fn rayObb(ray: Ray, box: Obb) ?f32 {
105 const local = Ray{
106 .origin = box.toLocal(ray.origin),
107 .direction = .{
108 .x = ray.direction.dot(box.axes.cols[0]),
109 .y = ray.direction.dot(box.axes.cols[1]),
110 .z = ray.direction.dot(box.axes.cols[2]),
111 },
112 };
113 return rayAabb(local, .{ .min = box.half_extents.negate(), .max = box.half_extents });
114 }
115
116 pub fn rayPlane(ray: Ray, plane: Plane) ?f32 {
117 const closing = plane.normal.dot(ray.direction);
118 if (tolerance.isParallel(closing * closing, ray.direction.lengthSq())) return null;
119 const t = -plane.signedDistance(ray.origin) / closing;
120 if (t < 0) return null;
121 return t;
122 }
123
124 pub fn sphereSphere(a: Sphere, b: Sphere) bool {
125 const reach = a.radius + b.radius;
126 return a.center.distanceSq(b.center) <= reach * reach;
127 }
128
129 pub fn sphereAabb(sphere: Sphere, box: Aabb) bool {
130 return closest.pointAabb(sphere.center, box).distanceSq(sphere.center) <= sphere.radius * sphere.radius;
131 }
132
133 pub fn sphereObb(sphere: Sphere, box: Obb) bool {
134 return closest.pointObb(sphere.center, box).distanceSq(sphere.center) <= sphere.radius * sphere.radius;
135 }
136
137 pub fn sphereTriangle(sphere: Sphere, triangle: Triangle) bool {
138 const nearest = closest.pointTriangle(sphere.center, triangle);
139 return nearest.distanceSq(sphere.center) <= sphere.radius * sphere.radius;
140 }
141
142 pub fn sphereCapsule(sphere: Sphere, capsule: Capsule) bool {
143 const reach = sphere.radius + capsule.radius;
144 return closest.pointSegment(sphere.center, capsule.axis()).distanceSq(sphere.center) <= reach * reach;
145 }
146
147 pub fn capsuleCapsule(a: Capsule, b: Capsule) bool {
148 const reach = a.radius + b.radius;
149 return closest.segmentSegment(a.axis(), b.axis()).distanceSq() <= reach * reach;
150 }
151
152 pub fn capsuleTriangle(capsule: Capsule, triangle: Triangle) bool {
153 return closest.segmentTriangle(capsule.axis(), triangle).distanceSq() <= capsule.radius * capsule.radius;
154 }
155
156 pub fn aabbAabb(a: Aabb, b: Aabb) bool {
157 return a.overlaps(b);
158 }
159
160 const testing = std.testing;
161 const unit_triangle = Triangle{ .a = .{}, .b = Vec3.init(1, 0, 0), .c = Vec3.init(0, 1, 0) };
162 const down = Vec3.init(0, 0, -1);
163
164 test "a ray meets a triangle from either side with barycentric weights" {
165 const hit = rayTriangle(.{ .origin = Vec3.init(0.25, 0.5, 3), .direction = down }, unit_triangle).?;
166 try testing.expectEqual(@as(f32, 3), hit.t);
167 try testing.expectEqual(@as(f32, 0.25), hit.u);
168 try testing.expectEqual(@as(f32, 0.5), hit.v);
169 const below = rayTriangle(.{ .origin = Vec3.init(0.25, 0.25, -2), .direction = down.negate() }, unit_triangle).?;
170 try testing.expectEqual(@as(f32, 2), below.t);
171 try testing.expectEqual(@as(?TriangleHit, null), rayTriangle(.{ .origin = Vec3.init(0.9, 0.9, 1), .direction = down }, unit_triangle));
172 const grazing = Ray{ .origin = Vec3.init(-1, 0.25, 0), .direction = Vec3.init(1, 0, 0) };
173 try testing.expectEqual(@as(?TriangleHit, null), rayTriangle(grazing, unit_triangle));
174 }
175
176 test "a ray enters a sphere, starts inside it, or misses" {
177 const sphere = Sphere{ .center = Vec3.init(0, 0, -5), .radius = 1 };
178 try testing.expectEqual(@as(?f32, 4), raySphere(.{ .origin = .{}, .direction = down }, sphere));
179 try testing.expectEqual(@as(?f32, 0), raySphere(.{ .origin = Vec3.init(0, 0, -5), .direction = down }, sphere));
180 try testing.expectEqual(@as(?f32, null), raySphere(.{ .origin = .{}, .direction = down.negate() }, sphere));
181 try testing.expectEqual(@as(?f32, null), raySphere(.{ .origin = Vec3.init(3, 0, 0), .direction = down }, sphere));
182 }
183
184 test "a ray enters a box through the nearest slab" {
185 const box = Aabb{ .min = Vec3.init(-1, -1, -1), .max = Vec3.init(1, 1, 1) };
186 try testing.expectEqual(@as(?f32, 4), rayAabb(.{ .origin = Vec3.init(0, 0, 5), .direction = down }, box));
187 try testing.expectEqual(@as(?f32, 0), rayAabb(.{ .origin = .{}, .direction = down }, box));
188 try testing.expectEqual(@as(?f32, null), rayAabb(.{ .origin = Vec3.init(2, 0, 5), .direction = down }, box));
189 const span = raySlab(.{ .origin = Vec3.init(0, 0, 5), .direction = down }, box, 5).?;
190 try testing.expectEqual([2]f32{ 4, 5 }, span);
191 try testing.expectEqual(@as(?[2]f32, null), raySlab(.{ .origin = Vec3.init(0, 0, 5), .direction = down }, box, 3));
192 }
193
194 test "a ray enters a turned box in its own frame" {
195 const turned = Obb{
196 .center = Vec3.init(0, 0, -5),
197 .axes = linear.Mat3.fromCols(Vec3.init(0, 1, 0), Vec3.init(-1, 0, 0), Vec3.init(0, 0, 1)),
198 .half_extents = Vec3.init(3, 0.5, 1),
199 };
200 try testing.expectEqual(@as(?f32, 4), rayObb(.{ .origin = Vec3.init(0, 2, 0), .direction = down }, turned));
201 try testing.expectEqual(@as(?f32, null), rayObb(.{ .origin = Vec3.init(2, 0, 0), .direction = down }, turned));
202 }
203
204 test "a ray meets a plane ahead of it" {
205 const floor = Plane.fromPointNormal(.{}, Vec3.init(0, 0, 1));
206 try testing.expectEqual(@as(?f32, 2), rayPlane(.{ .origin = Vec3.init(1, 1, 2), .direction = down }, floor));
207 try testing.expectEqual(@as(?f32, null), rayPlane(.{ .origin = Vec3.init(1, 1, 2), .direction = down.negate() }, floor));
208 try testing.expectEqual(@as(?f32, null), rayPlane(.{ .origin = Vec3.init(1, 1, 2), .direction = Vec3.init(1, 0, 0) }, floor));
209 }
210
211 test "overlaps hold at contact and fail past it" {
212 const ball = Sphere{ .center = Vec3.init(0.25, 0.25, 1), .radius = 1 };
213 try testing.expect(sphereTriangle(ball, unit_triangle));
214 try testing.expect(!sphereTriangle(.{ .center = ball.center, .radius = 0.99 }, unit_triangle));
215 try testing.expect(sphereSphere(ball, .{ .center = Vec3.init(0.25, 0.25, 3), .radius = 1 }));
216 try testing.expect(!sphereSphere(ball, .{ .center = Vec3.init(0.25, 0.25, 3.5), .radius = 1 }));
217 const box = Aabb{ .min = Vec3.init(2, 0, 0), .max = Vec3.init(3, 1, 1) };
218 try testing.expect(sphereAabb(.{ .center = Vec3.init(1, 0.5, 0.5), .radius = 1 }, box));
219 try testing.expect(!sphereAabb(.{ .center = Vec3.init(0.5, 0.5, 0.5), .radius = 1 }, box));
220 try testing.expect(aabbAabb(box, .{ .min = Vec3.init(3, 1, 1), .max = Vec3.init(4, 2, 2) }));
221 try testing.expect(!aabbAabb(box, .{ .min = Vec3.init(3.5, 0, 0), .max = Vec3.init(4, 1, 1) }));
222
223 const upright = Capsule{ .a = Vec3.init(0.25, 0.25, 0.5), .b = Vec3.init(0.25, 0.25, 2), .radius = 0.5 };
224 try testing.expect(capsuleTriangle(upright, unit_triangle));
225 try testing.expect(!capsuleTriangle(.{ .a = upright.a, .b = upright.b, .radius = 0.25 }, unit_triangle));
226 const other = Capsule{ .a = Vec3.init(-1, 0, 3), .b = Vec3.init(1, 0, 3), .radius = 0.5 };
227 const lying = Capsule{ .a = Vec3.init(0, -1, 2), .b = Vec3.init(0, 1, 2), .radius = 0.5 };
228 try testing.expect(capsuleCapsule(other, lying));
229 try testing.expect(!capsuleCapsule(other, .{ .a = lying.a, .b = lying.b, .radius = 0.25 }));
230 try testing.expect(sphereCapsule(.{ .center = Vec3.init(0, 0, 1), .radius = 0.5 }, lying));
231 try testing.expect(!sphereCapsule(.{ .center = Vec3.init(0, 0, 0.5), .radius = 0.5 }, lying));
232
233 const turned = Obb{ .center = .{}, .half_extents = Vec3.init(1, 2, 3) };
234 try testing.expect(sphereObb(.{ .center = Vec3.init(0, 0, 3.5), .radius = 0.5 }, turned));
235 try testing.expect(!sphereObb(.{ .center = Vec3.init(1.5, 2.5, 0), .radius = 0.5 }, turned));
236 }