lib/geometry/src/sweep.zig
daab053ee43316e1809a84551d573ddd1e5bf3d2
1 //! Swept shapes against triangles.
2 //!
3 //! A sweep translates a shape by `displacement` and reports the first contact
4 //! with a two-sided triangle. First contact is the earliest of three feature
5 //! families: the face, an edge and a vertex. For a capsule, the face family
6 //! reduces to its endpoint spheres, and the edge and vertex families gain the
7 //! capsule axis, whose line-to-line distance to an edge changes linearly under
8 //! translation. Taken together the families are exact.
9 const std = @import("std");
10 const linear = @import("linear");
11 const closest = @import("closest.zig");
12 const intersect = @import("intersect.zig");
13 const primitive = @import("primitive.zig");
14 const tolerance = @import("tolerance.zig");
15
16 const assert = std.debug.assert;
17 const Capsule = primitive.Capsule;
18 const Segment = primitive.Segment;
19 const Sphere = primitive.Sphere;
20 const Triangle = primitive.Triangle;
21 const Vec3 = linear.Vec3;
22
23 /// First contact of a sweep.
24 pub const Hit = struct {
25 /// The fraction of the displacement travelled at contact, in [0, 1]. Zero
26 /// means the shapes already touch or overlap at the start.
27 t: f32,
28 /// The contact point on the triangle.
29 point: Vec3,
30 /// The unit normal at contact, from the triangle toward the moving shape.
31 normal: Vec3,
32 };
33
34 /// The direction to push a shape out of a triangle when their closest points
35 /// coincide: the face normal turned against the motion, else against the
36 /// motion itself, else up.
37 fn fallbackNormal(triangle: Triangle, displacement: Vec3) Vec3 {
38 if (triangle.normal()) |n| {
39 if (n.dot(displacement) > 0) return n.negate();
40 return n;
41 }
42 if (displacement.normalized(0)) |direction| return direction.negate();
43 return Vec3.init(0, 0, 1);
44 }
45
46 fn separationNormal(separation: Vec3, triangle: Triangle, displacement: Vec3) Vec3 {
47 if (separation.lengthSq() <= tolerance.length_sq_floor) return fallbackNormal(triangle, displacement);
48 return separation.normalized(0) orelse fallbackNormal(triangle, displacement);
49 }
50
51 const Best = struct {
52 hit: ?Hit = null,
53
54 fn consider(best: *Best, candidate: Hit) void {
55 assert(candidate.t >= 0);
56 assert(candidate.t <= 1);
57 if (best.hit) |held| {
58 if (candidate.t >= held.t) return;
59 }
60 best.hit = candidate;
61 }
62 };
63
64 /// The first time in [0, 1] at which the moving point `origin + motion t`
65 /// comes within `radius` of the line through `anchor` along `axis`, with the
66 /// point starting farther than `radius` from the line.
67 fn cylinderEntry(origin: Vec3, motion: Vec3, anchor: Vec3, axis: Vec3, radius: f32) ?f32 {
68 const axis_sq = axis.lengthSq();
69 assert(axis_sq > tolerance.length_sq_floor);
70 const m = origin.sub(anchor);
71 const m_perp = m.sub(axis.scale(m.dot(axis) / axis_sq));
72 const d_perp = motion.sub(axis.scale(motion.dot(axis) / axis_sq));
73 const a = d_perp.lengthSq();
74 if (a <= tolerance.closing_sq_floor) return null;
75 const c = m_perp.lengthSq() - radius * radius;
76 if (c <= 0) return null;
77 const b = m_perp.dot(d_perp);
78 const discriminant = b * b - a * c;
79 if (discriminant < 0) return null;
80 const t = (-b - @sqrt(discriminant)) / a;
81 if (t < 0) return null;
82 if (t > 1) return null;
83 return t;
84 }
85
86 fn edges(triangle: Triangle) [3]Segment {
87 return .{
88 .{ .a = triangle.a, .b = triangle.b },
89 .{ .a = triangle.b, .b = triangle.c },
90 .{ .a = triangle.c, .b = triangle.a },
91 };
92 }
93
94 fn insideFace(p: Vec3, triangle: Triangle, n: Vec3) bool {
95 for (edges(triangle)) |edge| {
96 if (edge.b.sub(edge.a).cross(p.sub(edge.a)).dot(n) < 0) return false;
97 }
98 return true;
99 }
100
101 /// Face, edge and vertex contacts of a sphere that starts clear of the
102 /// triangle.
103 fn sphereFeatures(best: *Best, center: Vec3, radius: f32, displacement: Vec3, triangle: Triangle) void {
104 if (triangle.normal()) |n| {
105 const distance = n.dot(center.sub(triangle.a));
106 const closing = n.dot(displacement);
107 const side: f32 = if (distance >= 0) 1 else -1;
108 if (side * closing < 0 and closing * closing > tolerance.closing_sq_floor) {
109 const t = (side * radius - distance) / closing;
110 if (t >= 0 and t <= 1) {
111 const facing = n.scale(side);
112 const point = center.add(displacement.scale(t)).sub(facing.scale(radius));
113 if (insideFace(point, triangle, n)) best.consider(.{ .t = t, .point = point, .normal = facing });
114 }
115 }
116 }
117 for (edges(triangle)) |edge| {
118 const axis = edge.b.sub(edge.a);
119 if (axis.lengthSq() <= tolerance.length_sq_floor) continue;
120 const t = cylinderEntry(center, displacement, edge.a, axis, radius) orelse continue;
121 const moved = center.add(displacement.scale(t));
122 const along = moved.sub(edge.a).dot(axis) / axis.lengthSq();
123 if (along < 0 or along > 1) continue;
124 const point = edge.a.add(axis.scale(along));
125 best.consider(.{ .t = t, .point = point, .normal = separationNormal(moved.sub(point), triangle, displacement) });
126 }
127 for (0..3) |index| {
128 const corner = triangle.vertex(index);
129 const roots = intersect.lineSphereRoots(center, displacement, corner, radius) orelse continue;
130 const t = roots[0];
131 if (t < 0 or t > 1) continue;
132 const moved = center.add(displacement.scale(t));
133 best.consider(.{ .t = t, .point = corner, .normal = separationNormal(moved.sub(corner), triangle, displacement) });
134 }
135 }
136
137 /// First contact of `sphere` moving by `displacement` with `triangle`, or null
138 /// when it passes clear.
139 pub fn sphereTriangle(sphere: Sphere, displacement: Vec3, triangle: Triangle) ?Hit {
140 assert(sphere.radius >= 0);
141 const nearest = closest.pointTriangle(sphere.center, triangle);
142 const separation = sphere.center.sub(nearest);
143 if (separation.lengthSq() <= sphere.radius * sphere.radius) {
144 return .{ .t = 0, .point = nearest, .normal = separationNormal(separation, triangle, displacement) };
145 }
146 var best = Best{};
147 sphereFeatures(&best, sphere.center, sphere.radius, displacement, triangle);
148 return best.hit;
149 }
150
151 /// First contact of `capsule` moving by `displacement` with `triangle`, or
152 /// null when it passes clear.
153 pub fn capsuleTriangle(capsule: Capsule, displacement: Vec3, triangle: Triangle) ?Hit {
154 assert(capsule.radius >= 0);
155 const radius = capsule.radius;
156 const start = closest.segmentTriangle(capsule.axis(), triangle);
157 const separation = start.first.sub(start.second);
158 if (separation.lengthSq() <= radius * radius) {
159 return .{ .t = 0, .point = start.second, .normal = separationNormal(separation, triangle, displacement) };
160 }
161 var best = Best{};
162 sphereFeatures(&best, capsule.a, radius, displacement, triangle);
163 sphereFeatures(&best, capsule.b, radius, displacement, triangle);
164
165 const axis = capsule.b.sub(capsule.a);
166 const axis_sq = axis.lengthSq();
167 if (axis_sq <= tolerance.length_sq_floor) return best.hit;
168 for (edges(triangle)) |edge| axisEdge(&best, capsule, displacement, edge);
169 for (0..3) |index| {
170 const corner = triangle.vertex(index);
171 const t = cylinderEntry(corner, displacement.negate(), capsule.a, axis, radius) orelse continue;
172 const moved_a = capsule.a.add(displacement.scale(t));
173 const along = corner.sub(moved_a).dot(axis) / axis_sq;
174 if (along < 0 or along > 1) continue;
175 const axis_point = moved_a.add(axis.scale(along));
176 best.consider(.{ .t = t, .point = corner, .normal = separationNormal(axis_point.sub(corner), triangle, displacement) });
177 }
178 return best.hit;
179 }
180
181 /// The contact between the capsule axis and an edge interior. The distance
182 /// between the two lines is `f0 + f1 t`, so contact is the root of
183 /// `|f0 + f1 t| = radius` where the lines' closest points lie inside both
184 /// segments.
185 fn axisEdge(best: *Best, capsule: Capsule, displacement: Vec3, edge: Segment) void {
186 const u = capsule.b.sub(capsule.a);
187 const v = edge.b.sub(edge.a);
188 const n = u.cross(v);
189 if (tolerance.isParallel(n.lengthSq(), u.lengthSq() * v.lengthSq())) return;
190 const unit = n.normalized(0) orelse return;
191 const f0 = capsule.a.sub(edge.a).dot(unit);
192 const f1 = displacement.dot(unit);
193 if (@abs(f0) <= capsule.radius) return;
194 const side: f32 = if (f0 > 0) 1 else -1;
195 if (side * f1 >= 0) return;
196 if (f1 * f1 <= tolerance.closing_sq_floor) return;
197 const t = (side * capsule.radius - f0) / f1;
198 if (t < 0 or t > 1) return;
199
200 const w = capsule.a.add(displacement.scale(t)).sub(edge.a);
201 const aa = u.lengthSq();
202 const bb = u.dot(v);
203 const cc = v.lengthSq();
204 const du = u.dot(w);
205 const ev = v.dot(w);
206 const denominator = aa * cc - bb * bb;
207 if (denominator <= 0) return;
208 const along_axis = (bb * ev - cc * du) / denominator;
209 const along_edge = (aa * ev - bb * du) / denominator;
210 if (along_axis < 0 or along_axis > 1) return;
211 if (along_edge < 0 or along_edge > 1) return;
212 best.consider(.{ .t = t, .point = edge.at(along_edge), .normal = unit.scale(side) });
213 }
214
215 const testing = std.testing;
216 const unit_triangle = Triangle{ .a = .{}, .b = Vec3.init(1, 0, 0), .c = Vec3.init(0, 1, 0) };
217
218 fn expectHit(expected: Hit, actual: ?Hit) !void {
219 const hit = actual orelse return error.TestExpectedHit;
220 try testing.expectApproxEqAbs(expected.t, hit.t, 1e-6);
221 try testing.expectApproxEqAbs(expected.point.x, hit.point.x, 1e-6);
222 try testing.expectApproxEqAbs(expected.point.y, hit.point.y, 1e-6);
223 try testing.expectApproxEqAbs(expected.point.z, hit.point.z, 1e-6);
224 try testing.expectApproxEqAbs(expected.normal.x, hit.normal.x, 1e-6);
225 try testing.expectApproxEqAbs(expected.normal.y, hit.normal.y, 1e-6);
226 try testing.expectApproxEqAbs(expected.normal.z, hit.normal.z, 1e-6);
227 }
228
229 test "a falling sphere lands on the face" {
230 const ball = Sphere{ .center = Vec3.init(0.25, 0.25, 2), .radius = 0.5 };
231 try expectHit(
232 .{ .t = 0.375, .point = Vec3.init(0.25, 0.25, 0), .normal = Vec3.init(0, 0, 1) },
233 sphereTriangle(ball, Vec3.init(0, 0, -4), unit_triangle),
234 );
235 const below = Sphere{ .center = Vec3.init(0.25, 0.25, -2), .radius = 0.5 };
236 try expectHit(
237 .{ .t = 0.375, .point = Vec3.init(0.25, 0.25, 0), .normal = Vec3.init(0, 0, -1) },
238 sphereTriangle(below, Vec3.init(0, 0, 4), unit_triangle),
239 );
240 }
241
242 test "a sliding sphere strikes an edge and a vertex" {
243 const edge_ball = Sphere{ .center = Vec3.init(0.5, -2, 0), .radius = 0.5 };
244 try expectHit(
245 .{ .t = 0.375, .point = Vec3.init(0.5, 0, 0), .normal = Vec3.init(0, -1, 0) },
246 sphereTriangle(edge_ball, Vec3.init(0, 4, 0), unit_triangle),
247 );
248 const vertex_ball = Sphere{ .center = Vec3.init(3, 0, 0), .radius = 0.5 };
249 try expectHit(
250 .{ .t = 0.375, .point = Vec3.init(1, 0, 0), .normal = Vec3.init(1, 0, 0) },
251 sphereTriangle(vertex_ball, Vec3.init(-4, 0, 0), unit_triangle),
252 );
253 }
254
255 test "a sphere that starts touching reports time zero and one that passes misses" {
256 const touching = Sphere{ .center = Vec3.init(0.25, 0.25, 0.5), .radius = 0.5 };
257 try expectHit(
258 .{ .t = 0, .point = Vec3.init(0.25, 0.25, 0), .normal = Vec3.init(0, 0, 1) },
259 sphereTriangle(touching, Vec3.init(1, 0, 0), unit_triangle),
260 );
261 const clear = Sphere{ .center = Vec3.init(3, 3, 1), .radius = 0.5 };
262 try testing.expectEqual(@as(?Hit, null), sphereTriangle(clear, Vec3.init(0, 0, -4), unit_triangle));
263 try testing.expectEqual(@as(?Hit, null), sphereTriangle(.{ .center = Vec3.init(0.25, 0.25, 2), .radius = 0.5 }, Vec3.init(0, 0, -1), unit_triangle));
264 }
265
266 test "a capsule lands on an endpoint, an edge crossing and a vertex" {
267 const upright = Capsule{ .a = Vec3.init(0.25, 0.25, 1), .b = Vec3.init(0.25, 0.25, 3), .radius = 0.5 };
268 try expectHit(
269 .{ .t = 0.25, .point = Vec3.init(0.25, 0.25, 0), .normal = Vec3.init(0, 0, 1) },
270 capsuleTriangle(upright, Vec3.init(0, 0, -2), unit_triangle),
271 );
272 const lying = Capsule{ .a = Vec3.init(0.5, -1, 2), .b = Vec3.init(0.5, 1, 2), .radius = 0.5 };
273 try expectHit(
274 .{ .t = 0.375, .point = Vec3.init(0.5, 0, 0), .normal = Vec3.init(0, 0, 1) },
275 capsuleTriangle(lying, Vec3.init(0, 0, -4), unit_triangle),
276 );
277 const post = Capsule{ .a = Vec3.init(-2, 0, -1), .b = Vec3.init(-2, 0, 1), .radius = 0.5 };
278 try expectHit(
279 .{ .t = 0.375, .point = Vec3.init(0, 0, 0), .normal = Vec3.init(-1, 0, 0) },
280 capsuleTriangle(post, Vec3.init(4, 0, 0), unit_triangle),
281 );
282 }
283
284 test "a capsule that starts overlapping reports time zero" {
285 const through = Capsule{ .a = Vec3.init(0.25, 0.25, -1), .b = Vec3.init(0.25, 0.25, 1), .radius = 0.1 };
286 const hit = capsuleTriangle(through, Vec3.init(0, 0, 1), unit_triangle).?;
287 try testing.expectEqual(@as(f32, 0), hit.t);
288 try testing.expectEqual(Vec3.init(0, 0, -1), hit.normal);
289 const clear = Capsule{ .a = Vec3.init(3, 3, 1), .b = Vec3.init(3, 3, 2), .radius = 0.5 };
290 try testing.expectEqual(@as(?Hit, null), capsuleTriangle(clear, Vec3.init(0, 0, -4), unit_triangle));
291 }