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