1use std::mem;
2
3use glam::{DVec3, IVec3};
4use rustc_hash::FxHashSet;
5use smallvec::SmallVec;
6use steel_registry::blocks::shapes::VoxelShape;
7use steel_utils::{BlockPos, WorldAabb, axis::Axis};
8
9const SMALL_MOVEMENT_EPSILON_SQ: f64 = 9.999_999_4e-11;
10const CLIP_EPSILON: f64 = 1.0e-7;
11const CORNER_HIT_EPSILON: f64 = 1.0e-5;
12const ENTITY_INSIDE_SWEEP_INFLATE_EPSILON: f64 = 1.0e-7;
13
14pub(super) enum VisitedBlockPositions {
16 Inline(SmallVec<[BlockPos; 16]>),
17 Hashed(FxHashSet<BlockPos>),
18}
19
20impl Default for VisitedBlockPositions {
21 fn default() -> Self {
22 Self::Inline(SmallVec::new())
23 }
24}
25
26impl VisitedBlockPositions {
27 pub(super) fn insert(&mut self, pos: BlockPos) -> bool {
28 match self {
29 Self::Hashed(visited) => visited.insert(pos),
30 Self::Inline(inline) => {
31 if inline.contains(&pos) {
32 return false;
33 }
34 if inline.len() < inline.inline_size() {
35 inline.push(pos);
36 return true;
37 }
38
39 let mut visited = FxHashSet::default();
40 visited.reserve(inline.len() + 1);
41 visited.extend(inline.drain(..));
42 let inserted = visited.insert(pos);
43 debug_assert!(inserted, "inline duplicate check must precede the fallback");
44 *self = Self::Hashed(visited);
45 true
46 }
47 }
48 }
49}
50
51pub(super) fn for_each_block_intersected_between(
52 from: DVec3,
53 to: DVec3,
54 aabb_at_target: WorldAabb,
55 mut visitor: impl FnMut(BlockPos, i32) -> bool,
56) -> Option<i32> {
57 let mut last_iteration = 0;
58 let travel = to - from;
59 if travel.length_squared() < SMALL_MOVEMENT_EPSILON_SQ {
60 if !for_each_block_in_aabb(aabb_at_target, |pos| {
61 last_iteration = 0;
62 visitor(pos, 0)
63 }) {
64 return None;
65 }
66
67 return Some(last_iteration + 1);
68 }
69
70 let mut visited = VisitedBlockPositions::default();
72 let aabb_at_start = aabb_at_target.translate(-travel);
73 if !for_each_between_corners_in_direction(aabb_at_start, travel, |pos| {
74 last_iteration = 0;
75 if !visitor(pos, 0) {
76 return false;
77 }
78 visited.insert(pos);
79 true
80 }) {
81 return None;
82 }
83
84 let iterations = ({
85 let mut traced_visitor = |pos, iteration| {
86 last_iteration = iteration;
87 visitor(pos, iteration)
88 };
89 add_collisions_along_travel(&mut visited, travel, aabb_at_target, &mut traced_visitor)
90 })?;
91
92 if !for_each_between_corners_in_direction(aabb_at_target, travel, |pos| {
93 if visited.insert(pos) {
94 last_iteration = iterations + 1;
95 if !visitor(pos, iterations + 1) {
96 return false;
97 }
98 }
99 true
100 }) {
101 return None;
102 }
103
104 Some(last_iteration + 1)
105}
106
107pub(super) fn collided_with_shape_moving_from(
108 entity_box_at_from: WorldAabb,
109 from: DVec3,
110 to: DVec3,
111 block_pos: BlockPos,
112 shape: VoxelShape,
113) -> bool {
114 shape.iter().any(|part| {
115 collided_with_aabb_moving_from(entity_box_at_from, from, to, part.at_block(block_pos))
116 })
117}
118
119pub(super) fn collided_with_aabb_moving_from(
120 entity_box_at_from: WorldAabb,
121 from: DVec3,
122 to: DVec3,
123 target_box: WorldAabb,
124) -> bool {
125 let from_center = center(entity_box_at_from);
126 let to_center = from_center + (to - from);
127 let inflate_x = entity_box_at_from.width() * 0.5 - ENTITY_INSIDE_SWEEP_INFLATE_EPSILON;
128 let inflate_y = entity_box_at_from.height() * 0.5 - ENTITY_INSIDE_SWEEP_INFLATE_EPSILON;
129 let inflate_z = entity_box_at_from.depth() * 0.5 - ENTITY_INSIDE_SWEEP_INFLATE_EPSILON;
130
131 let inflated_part = target_box.inflate_xyz(inflate_x, inflate_y, inflate_z);
132 contains(inflated_part, from_center)
133 || contains(inflated_part, to_center)
134 || clip_aabb(inflated_part, from_center, to_center).is_some()
135}
136
137#[expect(
138 clippy::too_many_lines,
139 reason = "keeps the vanilla BlockGetter.addCollisionsAlongTravel port auditable"
140)]
141fn add_collisions_along_travel(
142 visited: &mut VisitedBlockPositions,
143 travel: DVec3,
144 aabb_at_target: WorldAabb,
145 visitor: &mut impl FnMut(BlockPos, i32) -> bool,
146) -> Option<i32> {
147 let box_size = DVec3::new(
148 aabb_at_target.width(),
149 aabb_at_target.height(),
150 aabb_at_target.depth(),
151 );
152 let corner_dir = get_furthest_corner(travel);
153 let to_center = DVec3::new(
154 f64::midpoint(aabb_at_target.min(Axis::X), aabb_at_target.max(Axis::X)),
155 f64::midpoint(aabb_at_target.min(Axis::Y), aabb_at_target.max(Axis::Y)),
156 f64::midpoint(aabb_at_target.min(Axis::Z), aabb_at_target.max(Axis::Z)),
157 );
158 let to_corner = DVec3::new(
159 to_center.x + box_size.x * 0.5 * f64::from(corner_dir.x),
160 to_center.y + box_size.y * 0.5 * f64::from(corner_dir.y),
161 to_center.z + box_size.z * 0.5 * f64::from(corner_dir.z),
162 );
163 let from_corner = to_corner - travel;
164 let mut corner_block = IVec3::new(
165 from_corner.x.floor() as i32,
166 from_corner.y.floor() as i32,
167 from_corner.z.floor() as i32,
168 );
169 let sign_x = sign_i32(travel.x);
170 let sign_y = sign_i32(travel.y);
171 let sign_z = sign_i32(travel.z);
172 let t_delta_x = if sign_x == 0 {
173 f64::MAX
174 } else {
175 f64::from(sign_x) / travel.x
176 };
177 let t_delta_y = if sign_y == 0 {
178 f64::MAX
179 } else {
180 f64::from(sign_y) / travel.y
181 };
182 let t_delta_z = if sign_z == 0 {
183 f64::MAX
184 } else {
185 f64::from(sign_z) / travel.z
186 };
187 let mut t_x = t_delta_x
188 * if sign_x > 0 {
189 1.0 - frac(from_corner.x)
190 } else {
191 frac(from_corner.x)
192 };
193 let mut t_y = t_delta_y
194 * if sign_y > 0 {
195 1.0 - frac(from_corner.y)
196 } else {
197 frac(from_corner.y)
198 };
199 let mut t_z = t_delta_z
200 * if sign_z > 0 {
201 1.0 - frac(from_corner.z)
202 } else {
203 frac(from_corner.z)
204 };
205 let mut iterations = 0;
206
207 while t_x <= 1.0 || t_y <= 1.0 || t_z <= 1.0 {
208 if t_x < t_y {
209 if t_x < t_z {
210 corner_block.x += sign_x;
211 t_x += t_delta_x;
212 } else {
213 corner_block.z += sign_z;
214 t_z += t_delta_z;
215 }
216 } else if t_y < t_z {
217 corner_block.y += sign_y;
218 t_y += t_delta_y;
219 } else {
220 corner_block.z += sign_z;
221 t_z += t_delta_z;
222 }
223
224 let block_pos = BlockPos::new(corner_block.x, corner_block.y, corner_block.z);
225 if let Some(hit_point) = clip_block(block_pos, from_corner, to_corner) {
226 iterations += 1;
227 let corner_hit_x = hit_point.x.clamp(
228 f64::from(corner_block.x) + CORNER_HIT_EPSILON,
229 f64::from(corner_block.x + 1) - CORNER_HIT_EPSILON,
230 );
231 let corner_hit_y = hit_point.y.clamp(
232 f64::from(corner_block.y) + CORNER_HIT_EPSILON,
233 f64::from(corner_block.y + 1) - CORNER_HIT_EPSILON,
234 );
235 let corner_hit_z = hit_point.z.clamp(
236 f64::from(corner_block.z) + CORNER_HIT_EPSILON,
237 f64::from(corner_block.z + 1) - CORNER_HIT_EPSILON,
238 );
239 let opposite_corner = IVec3::new(
240 (corner_hit_x - box_size.x * f64::from(corner_dir.x)).floor() as i32,
241 (corner_hit_y - box_size.y * f64::from(corner_dir.y)).floor() as i32,
242 (corner_hit_z - box_size.z * f64::from(corner_dir.z)).floor() as i32,
243 );
244
245 if !for_each_between_corners_in_direction_between(
246 corner_block,
247 opposite_corner,
248 travel,
249 |pos| !visited.insert(pos) || visitor(pos, iterations),
250 ) {
251 return None;
252 }
253 }
254 }
255
256 Some(iterations)
257}
258
259pub(super) fn for_each_block_in_aabb(
260 aabb: WorldAabb,
261 mut visitor: impl FnMut(BlockPos) -> bool,
262) -> bool {
263 let min_x = aabb.min(Axis::X).floor() as i32;
264 let min_y = aabb.min(Axis::Y).floor() as i32;
265 let min_z = aabb.min(Axis::Z).floor() as i32;
266 let max_x = aabb.max(Axis::X).floor() as i32;
267 let max_y = aabb.max(Axis::Y).floor() as i32;
268 let max_z = aabb.max(Axis::Z).floor() as i32;
269
270 for x in min_x..=max_x {
271 for y in min_y..=max_y {
272 for z in min_z..=max_z {
273 if !visitor(BlockPos::new(x, y, z)) {
274 return false;
275 }
276 }
277 }
278 }
279
280 true
281}
282
283fn for_each_between_corners_in_direction(
284 aabb: WorldAabb,
285 direction: DVec3,
286 visitor: impl FnMut(BlockPos) -> bool,
287) -> bool {
288 let first_corner = IVec3::new(
289 aabb.min(Axis::X).floor() as i32,
290 aabb.min(Axis::Y).floor() as i32,
291 aabb.min(Axis::Z).floor() as i32,
292 );
293 let second_corner = IVec3::new(
294 aabb.max(Axis::X).floor() as i32,
295 aabb.max(Axis::Y).floor() as i32,
296 aabb.max(Axis::Z).floor() as i32,
297 );
298 for_each_between_corners_in_direction_between(first_corner, second_corner, direction, visitor)
299}
300
301fn for_each_between_corners_in_direction_between(
302 first_corner: IVec3,
303 second_corner: IVec3,
304 direction: DVec3,
305 mut visitor: impl FnMut(BlockPos) -> bool,
306) -> bool {
307 let min_corner = first_corner.min(second_corner);
308 let max_corner = first_corner.max(second_corner);
309 let diff = max_corner - min_corner;
310 let start = IVec3::new(
311 if direction.x >= 0.0 {
312 min_corner.x
313 } else {
314 max_corner.x
315 },
316 if direction.y >= 0.0 {
317 min_corner.y
318 } else {
319 max_corner.y
320 },
321 if direction.z >= 0.0 {
322 min_corner.z
323 } else {
324 max_corner.z
325 },
326 );
327 let axes = axis_step_order(direction);
328 let first_axis = axes[0];
329 let second_axis = axes[1];
330 let third_axis = axes[2];
331 let first_step = axis_step(first_axis, direction);
332 let second_step = axis_step(second_axis, direction);
333 let third_step = axis_step(third_axis, direction);
334 let first_max = axis_value(diff, first_axis);
335 let second_max = axis_value(diff, second_axis);
336 let third_max = axis_value(diff, third_axis);
337 for first_index in 0..=first_max {
338 for second_index in 0..=second_max {
339 for third_index in 0..=third_max {
340 let position = start
341 + first_step * first_index
342 + second_step * second_index
343 + third_step * third_index;
344 if !visitor(BlockPos::new(position.x, position.y, position.z)) {
345 return false;
346 }
347 }
348 }
349 }
350
351 true
352}
353
354fn clip_block(pos: BlockPos, from: DVec3, to: DVec3) -> Option<DVec3> {
355 let min = DVec3::new(f64::from(pos.x()), f64::from(pos.y()), f64::from(pos.z()));
356 let max = min + DVec3::ONE;
357 let direction = to - from;
358 let mut t_min = 0.0;
359 let mut t_max = 1.0;
360
361 for axis in [Axis::X, Axis::Y, Axis::Z] {
362 let start = component(from, axis);
363 let delta = component(direction, axis);
364 let axis_min = component(min, axis);
365 let axis_max = component(max, axis);
366 if delta.abs() < CLIP_EPSILON {
367 if start < axis_min || start > axis_max {
368 return None;
369 }
370 continue;
371 }
372
373 let inv_delta = 1.0 / delta;
374 let mut low = (axis_min - start) * inv_delta;
375 let mut high = (axis_max - start) * inv_delta;
376 if low > high {
377 mem::swap(&mut low, &mut high);
378 }
379
380 if low > t_min {
381 t_min = low;
382 }
383 if high < t_max {
384 t_max = high;
385 }
386 if t_min > t_max {
387 return None;
388 }
389 }
390
391 Some(from + direction * t_min)
392}
393
394fn clip_aabb(aabb: WorldAabb, from: DVec3, to: DVec3) -> Option<DVec3> {
395 let direction = to - from;
396 let mut t_min = 0.0;
397 let mut t_max = 1.0;
398
399 for axis in [Axis::X, Axis::Y, Axis::Z] {
400 let start = component(from, axis);
401 let delta = component(direction, axis);
402 let axis_min = aabb.min(axis);
403 let axis_max = aabb.max(axis);
404 if delta.abs() < CLIP_EPSILON {
405 if start < axis_min || start > axis_max {
406 return None;
407 }
408 continue;
409 }
410
411 let inv_delta = 1.0 / delta;
412 let mut low = (axis_min - start) * inv_delta;
413 let mut high = (axis_max - start) * inv_delta;
414 if low > high {
415 mem::swap(&mut low, &mut high);
416 }
417
418 if low > t_min {
419 t_min = low;
420 }
421 if high < t_max {
422 t_max = high;
423 }
424 if t_min > t_max {
425 return None;
426 }
427 }
428
429 Some(from + direction * t_min)
430}
431
432fn contains(aabb: WorldAabb, point: DVec3) -> bool {
433 point.x >= aabb.min(Axis::X)
434 && point.x < aabb.max(Axis::X)
435 && point.y >= aabb.min(Axis::Y)
436 && point.y < aabb.max(Axis::Y)
437 && point.z >= aabb.min(Axis::Z)
438 && point.z < aabb.max(Axis::Z)
439}
440
441fn center(aabb: WorldAabb) -> DVec3 {
442 DVec3::new(
443 f64::midpoint(aabb.min(Axis::X), aabb.max(Axis::X)),
444 f64::midpoint(aabb.min(Axis::Y), aabb.max(Axis::Y)),
445 f64::midpoint(aabb.min(Axis::Z), aabb.max(Axis::Z)),
446 )
447}
448
449fn get_furthest_corner(direction: DVec3) -> IVec3 {
450 let x_dot = direction.x.abs();
451 let y_dot = direction.y.abs();
452 let z_dot = direction.z.abs();
453 let x_sign = if direction.x >= 0.0 { 1 } else { -1 };
454 let y_sign = if direction.y >= 0.0 { 1 } else { -1 };
455 let z_sign = if direction.z >= 0.0 { 1 } else { -1 };
456 if x_dot <= y_dot && x_dot <= z_dot {
457 IVec3::new(-x_sign, -z_sign, y_sign)
458 } else if y_dot <= z_dot {
459 IVec3::new(z_sign, -y_sign, -x_sign)
460 } else {
461 IVec3::new(-y_sign, x_sign, -z_sign)
462 }
463}
464
465pub(super) fn axis_step_order(movement: DVec3) -> [Axis; 3] {
466 if movement.x.abs() < movement.z.abs() {
467 [Axis::Y, Axis::Z, Axis::X]
468 } else {
469 [Axis::Y, Axis::X, Axis::Z]
470 }
471}
472
473fn axis_step(axis: Axis, direction: DVec3) -> IVec3 {
474 let sign = if component(direction, axis) >= 0.0 {
475 1
476 } else {
477 -1
478 };
479 match axis {
480 Axis::X => IVec3::new(sign, 0, 0),
481 Axis::Y => IVec3::new(0, sign, 0),
482 Axis::Z => IVec3::new(0, 0, sign),
483 }
484}
485
486const fn axis_value(vector: IVec3, axis: Axis) -> i32 {
487 match axis {
488 Axis::X => vector.x,
489 Axis::Y => vector.y,
490 Axis::Z => vector.z,
491 }
492}
493
494pub(super) const fn component(vector: DVec3, axis: Axis) -> f64 {
495 match axis {
496 Axis::X => vector.x,
497 Axis::Y => vector.y,
498 Axis::Z => vector.z,
499 }
500}
501
502fn frac(value: f64) -> f64 {
503 value - value.floor()
504}
505
506fn sign_i32(value: f64) -> i32 {
507 if value > 0.0 {
508 1
509 } else if value < 0.0 {
510 -1
511 } else {
512 0
513 }
514}
515
516#[cfg(test)]
517mod tests {
518 use super::*;
519
520 #[test]
521 fn visited_positions_deduplicate_across_inline_capacity_boundary() {
522 let mut visited = VisitedBlockPositions::default();
523 let VisitedBlockPositions::Inline(inline) = &visited else {
524 panic!("new visited positions must start inline");
525 };
526 let inline_capacity = inline.inline_size() as i32;
527 for x in 0..inline_capacity {
528 assert!(visited.insert(BlockPos::new(x, 64, 0)));
529 }
530 for x in 0..inline_capacity {
531 assert!(!visited.insert(BlockPos::new(x, 64, 0)));
532 }
533 let VisitedBlockPositions::Inline(inline) = &visited else {
534 panic!("positions up to the inline capacity must remain inline");
535 };
536 assert!(!inline.spilled());
537
538 assert!(visited.insert(BlockPos::new(inline_capacity, 64, 0)));
539 let VisitedBlockPositions::Hashed(hashed) = &visited else {
540 panic!("exceeding the inline capacity must use the hash set");
541 };
542 assert_eq!(hashed.len(), inline_capacity as usize + 1);
543 for x in 0..=inline_capacity {
544 assert!(!visited.insert(BlockPos::new(x, 64, 0)));
545 }
546 }
547
548 fn visited_positions(from: DVec3, to: DVec3, aabb_at_target: WorldAabb) -> Vec<BlockPos> {
549 let mut positions = Vec::new();
550 assert!(
551 for_each_block_intersected_between(from, to, aabb_at_target, |pos, _iteration| {
552 positions.push(pos);
553 true
554 })
555 .is_some()
556 );
557 positions
558 }
559
560 #[test]
561 fn entity_inside_shape_uses_swept_entity_center_against_inflated_shape() {
562 let entity_box = WorldAabb::entity_box(0.5, 0.0, 0.5, 0.3, 1.8);
563 let from = DVec3::new(0.5, 0.0, 0.5);
564 let to = DVec3::new(2.5, 0.0, 2.5);
565
566 assert!(collided_with_shape_moving_from(
567 entity_box,
568 from,
569 to,
570 BlockPos::new(2, 0, 2),
571 VoxelShape::FULL_BLOCK,
572 ));
573 assert!(!collided_with_shape_moving_from(
574 entity_box,
575 from,
576 to,
577 BlockPos::new(2, 0, 0),
578 VoxelShape::FULL_BLOCK,
579 ));
580 }
581
582 #[test]
583 fn stationary_entity_inside_shape_uses_current_entity_box() {
584 let entity_box = WorldAabb::entity_box(0.5, 0.0, 0.5, 0.3, 1.8);
585 let position = DVec3::new(0.5, 0.0, 0.5);
586
587 assert!(collided_with_shape_moving_from(
588 entity_box,
589 position,
590 position,
591 BlockPos::new(0, 0, 0),
592 VoxelShape::FULL_BLOCK,
593 ));
594 assert!(!collided_with_shape_moving_from(
595 entity_box,
596 position,
597 position,
598 BlockPos::new(2, 0, 0),
599 VoxelShape::FULL_BLOCK,
600 ));
601 }
602
603 #[test]
604 fn stationary_trace_visits_target_aabb_blocks() {
605 let positions = visited_positions(
606 DVec3::new(0.5, 64.0, 0.5),
607 DVec3::new(0.5, 64.0, 0.5),
608 WorldAabb::new(0.2, 64.0, 0.2, 1.2, 64.9, 0.9),
609 );
610
611 assert_eq!(
612 positions,
613 vec![BlockPos::new(0, 64, 0), BlockPos::new(1, 64, 0)]
614 );
615 }
616
617 #[test]
618 fn horizontal_trace_includes_blocks_between_start_and_target() {
619 let positions = visited_positions(
620 DVec3::new(0.5, 64.0, 0.5),
621 DVec3::new(2.5, 64.0, 0.5),
622 WorldAabb::new(2.2, 64.0, 0.2, 2.8, 64.9, 0.8),
623 );
624
625 assert!(positions.contains(&BlockPos::new(0, 64, 0)));
626 assert!(positions.contains(&BlockPos::new(1, 64, 0)));
627 assert!(positions.contains(&BlockPos::new(2, 64, 0)));
628 }
629
630 #[test]
631 fn visitor_can_stop_trace() {
632 let mut visited = Vec::new();
633 let completed = for_each_block_intersected_between(
634 DVec3::new(0.5, 64.0, 0.5),
635 DVec3::new(2.5, 64.0, 0.5),
636 WorldAabb::new(2.2, 64.0, 0.2, 2.8, 64.9, 0.8),
637 |pos, _iteration| {
638 visited.push(pos);
639 false
640 },
641 );
642
643 assert!(completed.is_none());
644 assert_eq!(visited.len(), 1);
645 }
646
647 #[test]
648 fn long_trace_visits_each_block_once_in_order() {
649 let positions = visited_positions(
650 DVec3::new(0.5, 64.0, 0.5),
651 DVec3::new(24.5, 64.0, 0.5),
652 WorldAabb::new(24.2, 64.0, 0.2, 24.8, 64.9, 0.8),
653 );
654 let expected = (0..=24)
655 .map(|x| BlockPos::new(x, 64, 0))
656 .collect::<Vec<_>>();
657
658 assert_eq!(positions, expected);
659 }
660}