lib/geometry/src/bvh.zig
daab053ee43316e1809a84551d573ddd1e5bf3d2
1 //! A static bounding volume hierarchy over one mesh.
2 //!
3 //! `build` runs once into caller storage sized from the triangle count, and
4 //! no query allocates. Every query answers exactly what the mesh scan answers:
5 //! the tree tests the same primitive functions on the same triangles, orders
6 //! nearest candidates by `(t, triangle index)`, and inflates every node box by
7 //! `tolerance.boxSlack` so rounding in a box test never culls a triangle the
8 //! primitive test would accept.
9 //!
10 //! Nodes are 32 bytes and siblings are adjacent, so one 64-byte line holds
11 //! both children an interior node leads to. Leaves hold at most
12 //! `leaf_capacity` triangles, copied into tree order so a leaf reads one
13 //! contiguous run.
14 const std = @import("std");
15 const linear = @import("linear");
16 const intersect = @import("intersect.zig");
17 const triangle_mesh = @import("mesh.zig");
18 const primitive = @import("primitive.zig");
19 const sweep = @import("sweep.zig");
20 const tolerance = @import("tolerance.zig");
21
22 const assert = std.debug.assert;
23 const Aabb = primitive.Aabb;
24 const Allocator = std.mem.Allocator;
25 const Capsule = primitive.Capsule;
26 const Mesh = triangle_mesh.Mesh;
27 const Overlap = triangle_mesh.Overlap;
28 const Ray = primitive.Ray;
29 const RayHit = triangle_mesh.RayHit;
30 const Sphere = primitive.Sphere;
31 const SweepHit = triangle_mesh.SweepHit;
32 const Triangle = primitive.Triangle;
33 const Vec3 = linear.Vec3;
34
35 /// The most triangles one leaf holds.
36 pub const leaf_capacity = 4;
37
38 /// Every node sits at a depth below this, and every query stack holds this
39 /// many entries. The build spends the slack between a balanced tree's depth
40 /// and this bound on surface area splits, then falls back to median splits,
41 /// which halve the triangle count. A mesh of `max_triangles` needs at most 30
42 /// balanced levels, so the bound always holds.
43 pub const depth_limit = 32;
44
45 /// Surface area heuristic bins per axis.
46 pub const bin_count = 12;
47
48 pub const Error = error{
49 /// The tree is deeper than a query stack holds. `build` never makes such
50 /// a tree, so this names a tree assembled some other way.
51 DepthExceeded,
52 };
53
54 /// A node box with its payload. An interior node has `count` zero and its
55 /// children at `first` and `first + 1`. A leaf holds the `count` triangles
56 /// from slot `first` of the tree-ordered triangle list.
57 pub const Node = extern struct {
58 min: Vec3,
59 first: u32,
60 max: Vec3,
61 count: u32,
62
63 pub fn isLeaf(node: Node) bool {
64 return node.count > 0;
65 }
66
67 fn box(node: Node) Aabb {
68 return .{ .min = node.min, .max = node.max };
69 }
70 };
71
72 comptime {
73 assert(@sizeOf(Node) == 32);
74 assert(depth_limit > std.math.log2_int_ceil(usize, triangle_mesh.max_triangles / leaf_capacity));
75 }
76
77 /// Work a query did, for comparing against its floor.
78 pub const Stats = struct {
79 nodes_visited: u64 = 0,
80 triangles_tested: u64 = 0,
81 };
82
83 /// Caller-owned memory for one tree. `centroids` is build scratch, and the
84 /// finished tree borrows the other three.
85 pub const Storage = struct {
86 nodes: []Node,
87 triangles: []Triangle,
88 order: []u32,
89 centroids: []Vec3,
90
91 /// A tree has at most one leaf per triangle, so at most `2 n - 1` nodes.
92 pub fn nodeCapacity(triangle_count: usize) usize {
93 if (triangle_count == 0) return 1;
94 return 2 * triangle_count - 1;
95 }
96
97 pub fn alloc(gpa: Allocator, triangle_count: usize) Allocator.Error!Storage {
98 assert(triangle_count <= triangle_mesh.max_triangles);
99 const nodes = try gpa.alloc(Node, nodeCapacity(triangle_count));
100 errdefer gpa.free(nodes);
101 const triangles = try gpa.alloc(Triangle, triangle_count);
102 errdefer gpa.free(triangles);
103 const order = try gpa.alloc(u32, triangle_count);
104 errdefer gpa.free(order);
105 const centroids = try gpa.alloc(Vec3, triangle_count);
106 return .{ .nodes = nodes, .triangles = triangles, .order = order, .centroids = centroids };
107 }
108
109 pub fn free(storage: Storage, gpa: Allocator) void {
110 gpa.free(storage.centroids);
111 gpa.free(storage.order);
112 gpa.free(storage.triangles);
113 gpa.free(storage.nodes);
114 }
115
116 pub fn fits(storage: Storage, triangle_count: usize) bool {
117 return storage.nodes.len >= nodeCapacity(triangle_count) and
118 storage.triangles.len >= triangle_count and
119 storage.order.len >= triangle_count and
120 storage.centroids.len >= triangle_count;
121 }
122 };
123
124 /// Storage with room for `capacity` triangles inline, for a caller that sizes
125 /// its tree at compile time and keeps it in static memory.
126 pub fn FixedStorage(comptime capacity: usize) type {
127 return struct {
128 nodes: [Storage.nodeCapacity(capacity)]Node = undefined,
129 triangles: [capacity]Triangle = undefined,
130 order: [capacity]u32 = undefined,
131 centroids: [capacity]Vec3 = undefined,
132
133 pub fn storage(fixed: *@This()) Storage {
134 return .{ .nodes = &fixed.nodes, .triangles = &fixed.triangles, .order = &fixed.order, .centroids = &fixed.centroids };
135 }
136 };
137 }
138
139 const empty_nodes = [1]Node{.{ .min = Aabb.empty.min, .first = 0, .max = Aabb.empty.max, .count = 0 }};
140
141 /// The number of halvings that bring `count` triangles down to leaves.
142 fn levelsNeeded(count: usize) usize {
143 assert(count > 0);
144 const leaves = std.math.divCeil(usize, count, leaf_capacity) catch unreachable;
145 return std.math.log2_int_ceil(usize, leaves);
146 }
147
148 fn component(v: Vec3, axis: usize) f32 {
149 assert(axis < 3);
150 return switch (axis) {
151 0 => v.x,
152 1 => v.y,
153 else => v.z,
154 };
155 }
156
157 fn binOf(value: f32, low: f32, extent: f32) usize {
158 assert(extent > 0);
159 assert(value >= low);
160 const scaled = (value - low) / extent * @as(f32, bin_count);
161 return @intFromFloat(@min(@floor(scaled), @as(f32, bin_count - 1)));
162 }
163
164 const Split = struct {
165 axis: usize,
166 plane: usize,
167 low: f32,
168 extent: f32,
169 };
170
171 const Builder = struct {
172 mesh: Mesh,
173 centroids: []const Vec3,
174
175 fn surfaceAreaSplit(builder: Builder, range: []const u32, centroid_box: Aabb) ?Split {
176 var best: ?Split = null;
177 var best_cost: f32 = std.math.inf(f32);
178 for (0..3) |axis| {
179 const low = component(centroid_box.min, axis);
180 const extent = component(centroid_box.max, axis) - low;
181 if (!(extent > 0)) continue;
182 var boxes: [bin_count]Aabb = @splat(Aabb.empty);
183 var counts: [bin_count]usize = @splat(0);
184 for (range) |index| {
185 const bin = binOf(component(builder.centroids[index], axis), low, extent);
186 boxes[bin] = boxes[bin].join(builder.mesh.triangle(index).bounds());
187 counts[bin] += 1;
188 }
189 var right_areas: [bin_count]f32 = undefined;
190 var right_counts: [bin_count]usize = undefined;
191 var right_box = Aabb.empty;
192 var right_count: usize = 0;
193 var bin: usize = bin_count;
194 while (bin > 1) {
195 bin -= 1;
196 right_box = right_box.join(boxes[bin]);
197 right_count += counts[bin];
198 right_areas[bin] = right_box.halfArea();
199 right_counts[bin] = right_count;
200 }
201 var left_box = Aabb.empty;
202 var left_count: usize = 0;
203 for (1..bin_count) |plane| {
204 left_box = left_box.join(boxes[plane - 1]);
205 left_count += counts[plane - 1];
206 if (left_count == 0 or right_counts[plane] == 0) continue;
207 const cost = left_box.halfArea() * @as(f32, @floatFromInt(left_count)) +
208 right_areas[plane] * @as(f32, @floatFromInt(right_counts[plane]));
209 if (cost < best_cost) {
210 best_cost = cost;
211 best = .{ .axis = axis, .plane = plane, .low = low, .extent = extent };
212 }
213 }
214 }
215 return best;
216 }
217
218 /// Reorders `range` into two nonempty runs and returns the length of the
219 /// first. A median split sorts by `(centroid, triangle index)` so equal
220 /// centroids order the same way on every build.
221 fn split(builder: Builder, range: []u32, centroid_box: Aabb, allow_surface_area: bool) usize {
222 assert(range.len > leaf_capacity);
223 if (allow_surface_area) {
224 if (builder.surfaceAreaSplit(range, centroid_box)) |chosen| {
225 var head: usize = 0;
226 var tail: usize = range.len;
227 while (head < tail) {
228 const value = component(builder.centroids[range[head]], chosen.axis);
229 if (binOf(value, chosen.low, chosen.extent) < chosen.plane) {
230 head += 1;
231 } else {
232 tail -= 1;
233 std.mem.swap(u32, &range[head], &range[tail]);
234 }
235 }
236 assert(head > 0);
237 assert(head < range.len);
238 return head;
239 }
240 }
241 const extent = centroid_box.extent();
242 const axis: usize = if (extent.x >= extent.y and extent.x >= extent.z) 0 else if (extent.y >= extent.z) 1 else 2;
243 const Order = struct {
244 centroids: []const Vec3,
245 axis: usize,
246
247 fn lessThan(context: @This(), a: u32, b: u32) bool {
248 const ca = component(context.centroids[a], context.axis);
249 const cb = component(context.centroids[b], context.axis);
250 if (ca != cb) return ca < cb;
251 return a < b;
252 }
253 };
254 std.sort.pdq(u32, range, Order{ .centroids = builder.centroids, .axis = axis }, Order.lessThan);
255 return range.len / 2;
256 }
257 };
258
259 pub const Bvh = struct {
260 nodes: []const Node,
261 triangles: []const Triangle,
262 /// The mesh index of the triangle in each tree slot.
263 order: []const u32,
264 /// The depth of the deepest node, with the root at zero.
265 depth: u32,
266 /// The largest coordinate magnitude of the root box.
267 magnitude: f32,
268
269 /// The tree over no triangles, which answers every query with nothing.
270 pub const empty: Bvh = .{ .nodes = &empty_nodes, .triangles = &.{}, .order = &.{}, .depth = 0, .magnitude = 0 };
271
272 const Task = struct {
273 node: u32,
274 begin: u32,
275 end: u32,
276 depth: u32,
277 };
278
279 /// Builds the tree over `mesh` into `storage`, which must fit the mesh's
280 /// triangle count. The same mesh builds the same tree every time.
281 pub fn build(mesh: Mesh, storage: Storage) Bvh {
282 assert(mesh.isWellFormed());
283 const count = mesh.triangleCount();
284 assert(storage.fits(count));
285 const order = storage.order[0..count];
286 const centroids = storage.centroids[0..count];
287 for (order, centroids, 0..) |*slot, *centroid, index| {
288 slot.* = @intCast(index);
289 centroid.* = mesh.triangle(index).centroid();
290 }
291 const builder = Builder{ .mesh = mesh, .centroids = centroids };
292
293 var node_count: u32 = 1;
294 var depth: u32 = 0;
295 var tasks: [depth_limit]Task = undefined;
296 var task_count: usize = 1;
297 tasks[0] = .{ .node = 0, .begin = 0, .end = @intCast(count), .depth = 0 };
298 while (task_count > 0) {
299 task_count -= 1;
300 const task = tasks[task_count];
301 const range = order[task.begin..task.end];
302 var box = Aabb.empty;
303 var centroid_box = Aabb.empty;
304 for (range) |index| {
305 box = box.join(mesh.triangle(index).bounds());
306 centroid_box = centroid_box.joinPoint(centroids[index]);
307 }
308 depth = @max(depth, task.depth);
309 assert(task.depth < depth_limit);
310 if (range.len <= leaf_capacity) {
311 storage.nodes[task.node] = .{ .min = box.min, .first = task.begin, .max = box.max, .count = @intCast(range.len) };
312 continue;
313 }
314 assert(task.depth + levelsNeeded(range.len) < depth_limit);
315 const allow_surface_area = task.depth + levelsNeeded(range.len) + 1 < depth_limit;
316 const middle = task.begin + @as(u32, @intCast(builder.split(range, centroid_box, allow_surface_area)));
317 const left = node_count;
318 node_count += 2;
319 assert(node_count <= Storage.nodeCapacity(count));
320 storage.nodes[task.node] = .{ .min = box.min, .first = left, .max = box.max, .count = 0 };
321 assert(task_count + 2 <= depth_limit);
322 tasks[task_count] = .{ .node = left + 1, .begin = middle, .end = task.end, .depth = task.depth + 1 };
323 tasks[task_count + 1] = .{ .node = left, .begin = task.begin, .end = middle, .depth = task.depth + 1 };
324 task_count += 2;
325 }
326 for (storage.triangles[0..count], order) |*triangle, index| triangle.* = mesh.triangle(index);
327 const root = storage.nodes[0].box();
328 return .{
329 .nodes = storage.nodes[0..node_count],
330 .triangles = storage.triangles[0..count],
331 .order = order,
332 .depth = depth,
333 .magnitude = if (count == 0) 0 else root.magnitude(),
334 };
335 }
336
337 pub fn triangleCount(bvh: Bvh) usize {
338 return bvh.triangles.len;
339 }
340
341 /// The nearest triangle the ray meets with `t` in `[t_min, t_max]`, as
342 /// `Mesh.raycast` answers it.
343 pub fn raycast(bvh: Bvh, ray: Ray, t_min: f32, t_max: f32, stats: ?*Stats) Error!?RayHit {
344 assert(t_min <= t_max);
345 var walk = try Walk.begin(bvh, stats);
346 if (walk.done) return null;
347 const slack = tolerance.boxSlack(@max(bvh.magnitude, ray.origin.abs().maxComponent()));
348 var best: ?RayHit = null;
349 var best_t = t_max;
350 const root = intersect.raySlab(ray, bvh.nodes[0].box().inflate(slack), best_t) orelse return null;
351 try walk.stack.push(.{ .node = 0, .entry = root[0] });
352 while (walk.next(best_t)) |node| {
353 if (node.isLeaf()) {
354 for (node.first..node.first + node.count) |slot| {
355 walk.tested();
356 const hit = intersect.rayTriangle(ray, bvh.triangles[slot]) orelse continue;
357 if (hit.t < t_min or hit.t > best_t) continue;
358 const index = bvh.order[slot];
359 if (best) |held| {
360 if (hit.t == held.t and index > held.triangle) continue;
361 }
362 best = .{ .t = hit.t, .u = hit.u, .v = hit.v, .triangle = index };
363 best_t = hit.t;
364 }
365 continue;
366 }
367 const near = intersect.raySlab(ray, bvh.nodes[node.first].box().inflate(slack), best_t);
368 const far = intersect.raySlab(ray, bvh.nodes[node.first + 1].box().inflate(slack), best_t);
369 try walk.pushOrdered(node.first, near, far);
370 }
371 return best;
372 }
373
374 /// Whether the ray meets any triangle with `t` in `[t_min, t_max]`.
375 pub fn raycastAny(bvh: Bvh, ray: Ray, t_min: f32, t_max: f32, stats: ?*Stats) Error!bool {
376 assert(t_min <= t_max);
377 var walk = try Walk.begin(bvh, stats);
378 if (walk.done) return false;
379 const slack = tolerance.boxSlack(@max(bvh.magnitude, ray.origin.abs().maxComponent()));
380 const root = intersect.raySlab(ray, bvh.nodes[0].box().inflate(slack), t_max) orelse return false;
381 try walk.stack.push(.{ .node = 0, .entry = root[0] });
382 while (walk.next(t_max)) |node| {
383 if (node.isLeaf()) {
384 for (node.first..node.first + node.count) |slot| {
385 walk.tested();
386 const hit = intersect.rayTriangle(ray, bvh.triangles[slot]) orelse continue;
387 if (hit.t >= t_min and hit.t <= t_max) return true;
388 }
389 continue;
390 }
391 const near = intersect.raySlab(ray, bvh.nodes[node.first].box().inflate(slack), t_max);
392 const far = intersect.raySlab(ray, bvh.nodes[node.first + 1].box().inflate(slack), t_max);
393 try walk.pushOrdered(node.first, near, far);
394 }
395 return false;
396 }
397
398 /// The triangles the sphere touches, as `Mesh.overlapSphere` answers it.
399 pub fn overlapSphere(bvh: Bvh, sphere: Sphere, out: []u32, stats: ?*Stats) Error!Overlap {
400 assert(sphere.radius >= 0);
401 const reach = sphere.bounds();
402 const slack = tolerance.boxSlack(@max(bvh.magnitude, reach.magnitude()));
403 var collector = Collector{ .out = out };
404 var walk = try Walk.begin(bvh, stats);
405 if (walk.done) return collector.finish();
406 try walk.stack.push(.{ .node = 0, .entry = 0 });
407 while (walk.next(0)) |node| {
408 if (!intersect.sphereAabb(sphere, node.box().inflate(slack))) continue;
409 if (node.isLeaf()) {
410 for (node.first..node.first + node.count) |slot| {
411 walk.tested();
412 if (intersect.sphereTriangle(sphere, bvh.triangles[slot])) collector.add(bvh.order[slot]);
413 }
414 continue;
415 }
416 try walk.stack.push(.{ .node = node.first + 1, .entry = 0 });
417 try walk.stack.push(.{ .node = node.first, .entry = 0 });
418 }
419 return collector.finish();
420 }
421
422 /// The triangles the capsule touches, as `Mesh.overlapCapsule` answers
423 /// it.
424 pub fn overlapCapsule(bvh: Bvh, capsule: Capsule, out: []u32, stats: ?*Stats) Error!Overlap {
425 assert(capsule.radius >= 0);
426 const reach = capsule.bounds();
427 const slack = tolerance.boxSlack(@max(bvh.magnitude, reach.magnitude()));
428 var collector = Collector{ .out = out };
429 var walk = try Walk.begin(bvh, stats);
430 if (walk.done) return collector.finish();
431 try walk.stack.push(.{ .node = 0, .entry = 0 });
432 while (walk.next(0)) |node| {
433 if (!reach.overlaps(node.box().inflate(slack))) continue;
434 if (node.isLeaf()) {
435 for (node.first..node.first + node.count) |slot| {
436 walk.tested();
437 if (intersect.capsuleTriangle(capsule, bvh.triangles[slot])) collector.add(bvh.order[slot]);
438 }
439 continue;
440 }
441 try walk.stack.push(.{ .node = node.first + 1, .entry = 0 });
442 try walk.stack.push(.{ .node = node.first, .entry = 0 });
443 }
444 return collector.finish();
445 }
446
447 /// The first triangle the sphere touches as it moves by `displacement`,
448 /// as `Mesh.sweepSphere` answers it.
449 pub fn sweepSphere(bvh: Bvh, sphere: Sphere, displacement: Vec3, stats: ?*Stats) Error!?SweepHit {
450 assert(sphere.radius >= 0);
451 const moved = Sphere{ .center = sphere.center.add(displacement), .radius = sphere.radius };
452 const reach = sphere.bounds().join(moved.bounds());
453 const margin = Swept{
454 .low = Vec3.splat(-sphere.radius),
455 .high = Vec3.splat(sphere.radius),
456 .slack = tolerance.boxSlack(@max(bvh.magnitude, reach.magnitude())),
457 };
458 const path = Ray{ .origin = sphere.center, .direction = displacement };
459 return bvh.sweepWith(path, margin, stats, sphere, sweepSphereTriangle);
460 }
461
462 /// The first triangle the capsule touches as it moves by `displacement`,
463 /// as `Mesh.sweepCapsule` answers it.
464 pub fn sweepCapsule(bvh: Bvh, capsule: Capsule, displacement: Vec3, stats: ?*Stats) Error!?SweepHit {
465 assert(capsule.radius >= 0);
466 const moved = Capsule{ .a = capsule.a.add(displacement), .b = capsule.b.add(displacement), .radius = capsule.radius };
467 const reach = capsule.bounds().join(moved.bounds());
468 const axis = capsule.b.sub(capsule.a);
469 const margin = Swept{
470 .low = axis.min(.{}).sub(Vec3.splat(capsule.radius)),
471 .high = axis.max(.{}).add(Vec3.splat(capsule.radius)),
472 .slack = tolerance.boxSlack(@max(bvh.magnitude, reach.magnitude())),
473 };
474 const path = Ray{ .origin = capsule.a, .direction = displacement };
475 return bvh.sweepWith(path, margin, stats, capsule, sweepCapsuleTriangle);
476 }
477
478 fn sweepSphereTriangle(sphere: Sphere, displacement: Vec3, triangle: Triangle) ?sweep.Hit {
479 return sweep.sphereTriangle(sphere, displacement, triangle);
480 }
481
482 fn sweepCapsuleTriangle(capsule: Capsule, displacement: Vec3, triangle: Triangle) ?sweep.Hit {
483 return sweep.capsuleTriangle(capsule, displacement, triangle);
484 }
485
486 /// Walks the boxes a moving reference point can reach while the shape it
487 /// carries could touch them. A shape whose points lie at offsets in
488 /// `[low, high]` from the reference point touches a box only while that
489 /// point is inside the box grown by `-high` below and `-low` above.
490 fn sweepWith(
491 bvh: Bvh,
492 path: Ray,
493 margin: Swept,
494 stats: ?*Stats,
495 shape: anytype,
496 comptime test_triangle: fn (@TypeOf(shape), Vec3, Triangle) ?sweep.Hit,
497 ) Error!?SweepHit {
498 var walk = try Walk.begin(bvh, stats);
499 if (walk.done) return null;
500 var best: ?SweepHit = null;
501 var best_t: f32 = 1;
502 const root = intersect.raySlab(path, margin.grow(bvh.nodes[0]), best_t) orelse return null;
503 try walk.stack.push(.{ .node = 0, .entry = root[0] });
504 while (walk.next(best_t)) |node| {
505 if (node.isLeaf()) {
506 for (node.first..node.first + node.count) |slot| {
507 walk.tested();
508 const hit = test_triangle(shape, path.direction, bvh.triangles[slot]) orelse continue;
509 if (hit.t > best_t) continue;
510 const index = bvh.order[slot];
511 if (best) |held| {
512 if (hit.t == held.t and index > held.triangle) continue;
513 }
514 best = .{ .t = hit.t, .point = hit.point, .normal = hit.normal, .triangle = index };
515 best_t = hit.t;
516 }
517 continue;
518 }
519 const near = intersect.raySlab(path, margin.grow(bvh.nodes[node.first]), best_t);
520 const far = intersect.raySlab(path, margin.grow(bvh.nodes[node.first + 1]), best_t);
521 try walk.pushOrdered(node.first, near, far);
522 }
523 return best;
524 }
525 };
526
527 const Swept = struct {
528 low: Vec3,
529 high: Vec3,
530 slack: f32,
531
532 fn grow(swept: Swept, node: Node) Aabb {
533 return .{
534 .min = node.min.sub(swept.high).sub(Vec3.splat(swept.slack)),
535 .max = node.max.sub(swept.low).add(Vec3.splat(swept.slack)),
536 };
537 }
538 };
539
540 const Entry = struct {
541 node: u32,
542 entry: f32,
543 };
544
545 const Stack = struct {
546 items: [depth_limit]Entry = undefined,
547 len: usize = 0,
548
549 fn push(stack: *Stack, item: Entry) Error!void {
550 if (stack.len == depth_limit) return error.DepthExceeded;
551 stack.items[stack.len] = item;
552 stack.len += 1;
553 }
554
555 fn pop(stack: *Stack) ?Entry {
556 if (stack.len == 0) return null;
557 stack.len -= 1;
558 return stack.items[stack.len];
559 }
560 };
561
562 /// Depth-first traversal state shared by every query. A stack of
563 /// `depth_limit` entries holds any walk over a tree of depth below
564 /// `depth_limit`, because each level leaves at most one sibling behind.
565 const Walk = struct {
566 bvh: Bvh,
567 stats: ?*Stats,
568 stack: Stack = .{},
569 done: bool,
570
571 fn begin(bvh: Bvh, stats: ?*Stats) Error!Walk {
572 if (bvh.depth >= depth_limit) return error.DepthExceeded;
573 assert(bvh.nodes.len > 0);
574 return .{ .bvh = bvh, .stats = stats, .done = bvh.triangles.len == 0 };
575 }
576
577 /// The next node whose entry is not past `bound`. A node entered exactly
578 /// at `bound` is still visited, so a later triangle tied at the same `t`
579 /// with a lower index is found.
580 fn next(walk: *Walk, bound: f32) ?Node {
581 while (walk.stack.pop()) |item| {
582 if (item.entry > bound) continue;
583 if (walk.stats) |stats| stats.nodes_visited += 1;
584 return walk.bvh.nodes[item.node];
585 }
586 return null;
587 }
588
589 fn tested(walk: *Walk) void {
590 if (walk.stats) |stats| stats.triangles_tested += 1;
591 }
592
593 /// Pushes the children at `first` and `first + 1` that the query enters,
594 /// the farther first, so the nearer is visited first.
595 fn pushOrdered(walk: *Walk, first: u32, left: ?[2]f32, right: ?[2]f32) Error!void {
596 if (left) |l| {
597 if (right) |r| {
598 if (r[0] < l[0]) {
599 try walk.stack.push(.{ .node = first, .entry = l[0] });
600 try walk.stack.push(.{ .node = first + 1, .entry = r[0] });
601 } else {
602 try walk.stack.push(.{ .node = first + 1, .entry = r[0] });
603 try walk.stack.push(.{ .node = first, .entry = l[0] });
604 }
605 return;
606 }
607 try walk.stack.push(.{ .node = first, .entry = l[0] });
608 return;
609 }
610 if (right) |r| try walk.stack.push(.{ .node = first + 1, .entry = r[0] });
611 }
612 };
613
614 /// Gathers overlap indices so the answer is the lowest indices that fit,
615 /// sorted, whatever order the walk finds them in.
616 const Collector = struct {
617 out: []u32,
618 count: usize = 0,
619 complete: bool = true,
620 sorted: bool = false,
621
622 fn add(collector: *Collector, index: u32) void {
623 if (collector.count < collector.out.len) {
624 collector.out[collector.count] = index;
625 collector.count += 1;
626 return;
627 }
628 collector.complete = false;
629 if (collector.out.len == 0) return;
630 if (!collector.sorted) {
631 std.sort.pdq(u32, collector.out, {}, std.sort.asc(u32));
632 collector.sorted = true;
633 }
634 const last = collector.out.len - 1;
635 if (index > collector.out[last]) return;
636 var slot = last;
637 while (slot > 0 and collector.out[slot - 1] > index) : (slot -= 1) {
638 collector.out[slot] = collector.out[slot - 1];
639 }
640 collector.out[slot] = index;
641 }
642
643 fn finish(collector: *Collector) Overlap {
644 if (!collector.sorted) std.sort.pdq(u32, collector.out[0..collector.count], {}, std.sort.asc(u32));
645 return .{ .count = collector.count, .complete = collector.complete };
646 }
647 };
648
649 const testing = std.testing;
650
651 fn Grid(comptime side: usize) type {
652 return struct { positions: [(side + 1) * (side + 1)]Vec3, indices: [side * side * 6]u32 };
653 }
654
655 fn gridMesh(comptime side: usize) Grid(side) {
656 var result: Grid(side) = undefined;
657 for (0..side + 1) |row| {
658 for (0..side + 1) |column| {
659 result.positions[row * (side + 1) + column] = Vec3.init(@floatFromInt(column), @floatFromInt(row), 0);
660 }
661 }
662 for (0..side) |row| {
663 for (0..side) |column| {
664 const corner: u32 = @intCast(row * (side + 1) + column);
665 const above: u32 = corner + @as(u32, side) + 1;
666 const base = (row * side + column) * 6;
667 result.indices[base..][0..6].* = .{ corner, corner + 1, above + 1, corner, above + 1, above };
668 }
669 }
670 return result;
671 }
672
673 test "a built tree bounds its node count and depth" {
674 const grid = gridMesh(8);
675 const mesh = Mesh{ .positions = &grid.positions, .indices = &grid.indices };
676 const storage = try Storage.alloc(testing.allocator, mesh.triangleCount());
677 defer storage.free(testing.allocator);
678 const bvh = Bvh.build(mesh, storage);
679 try testing.expect(bvh.nodes.len <= Storage.nodeCapacity(mesh.triangleCount()));
680 try testing.expect(bvh.depth < depth_limit);
681 try testing.expectEqual(@as(usize, 128), bvh.triangleCount());
682 var seen: [128]bool = @splat(false);
683 for (bvh.order) |index| {
684 try testing.expect(!seen[index]);
685 seen[index] = true;
686 }
687 for (bvh.nodes) |node| {
688 if (node.isLeaf()) try testing.expect(node.count <= leaf_capacity);
689 }
690 }
691
692 test "the tree answers a ray on a shared edge with the lower index" {
693 const grid = gridMesh(8);
694 const mesh = Mesh{ .positions = &grid.positions, .indices = &grid.indices };
695 const storage = try Storage.alloc(testing.allocator, mesh.triangleCount());
696 defer storage.free(testing.allocator);
697 const bvh = Bvh.build(mesh, storage);
698 const ray = Ray{ .origin = Vec3.init(3.5, 3.5, 5), .direction = Vec3.init(0, 0, -1) };
699 var stats = Stats{};
700 const hit = (try bvh.raycast(ray, 0, 100, &stats)).?;
701 try testing.expectEqual(mesh.raycast(ray, 0, 100).?, hit);
702 try testing.expectEqual(@as(f32, 5), hit.t);
703 try testing.expect(stats.triangles_tested < mesh.triangleCount());
704 }
705
706 test "a tree too deep for the query stack is refused" {
707 const grid = gridMesh(1);
708 const mesh = Mesh{ .positions = &grid.positions, .indices = &grid.indices };
709 const storage = try Storage.alloc(testing.allocator, mesh.triangleCount());
710 defer storage.free(testing.allocator);
711 var bvh = Bvh.build(mesh, storage);
712 bvh.depth = depth_limit;
713 const ray = Ray{ .origin = Vec3.init(0.5, 0.5, 1), .direction = Vec3.init(0, 0, -1) };
714 try testing.expectError(error.DepthExceeded, bvh.raycast(ray, 0, 10, null));
715 try testing.expectError(error.DepthExceeded, bvh.sweepSphere(.{ .center = ray.origin, .radius = 0.1 }, Vec3.init(0, 0, -2), null));
716 }
717
718 test "a fixed storage holds a tree sized at compile time" {
719 const grid = gridMesh(2);
720 const mesh = Mesh{ .positions = &grid.positions, .indices = &grid.indices };
721 var fixed: FixedStorage(8) = .{};
722 const bvh = Bvh.build(mesh, fixed.storage());
723 const ray = Ray{ .origin = Vec3.init(1.25, 0.5, 1), .direction = Vec3.init(0, 0, -1) };
724 try testing.expectEqual(mesh.raycast(ray, 0, 10), try bvh.raycast(ray, 0, 10, null));
725 try testing.expectEqual(@as(?RayHit, null), try Bvh.empty.raycast(ray, 0, 10, null));
726 }
727
728 test "an empty mesh builds a tree that answers nothing" {
729 const mesh = Mesh{ .positions = &.{}, .indices = &.{} };
730 const storage = try Storage.alloc(testing.allocator, 0);
731 defer storage.free(testing.allocator);
732 const bvh = Bvh.build(mesh, storage);
733 const ray = Ray{ .origin = .{}, .direction = Vec3.init(0, 0, 1) };
734 try testing.expectEqual(@as(?RayHit, null), try bvh.raycast(ray, 0, 10, null));
735 var out: [4]u32 = undefined;
736 try testing.expectEqual(Overlap{ .count = 0, .complete = true }, try bvh.overlapSphere(.{ .center = .{}, .radius = 1 }, &out, null));
737 }
738
739 test "an overlap with a small buffer keeps the lowest indices" {
740 const grid = gridMesh(4);
741 const mesh = Mesh{ .positions = &grid.positions, .indices = &grid.indices };
742 const storage = try Storage.alloc(testing.allocator, mesh.triangleCount());
743 defer storage.free(testing.allocator);
744 const bvh = Bvh.build(mesh, storage);
745 const ball = Sphere{ .center = Vec3.init(2, 2, 0.5), .radius = 1.5 };
746 var tree_out: [5]u32 = undefined;
747 var scan_out: [5]u32 = undefined;
748 const tree = try bvh.overlapSphere(ball, &tree_out, null);
749 const scan = mesh.overlapSphere(ball, &scan_out);
750 try testing.expectEqual(scan, tree);
751 try testing.expect(!tree.complete);
752 try testing.expectEqualSlices(u32, scan_out[0..scan.count], tree_out[0..tree.count]);
753 }