Skip to main content

rayengine_core/pathfinding/
smooth.rs

1use super::{
2    DiagonalRule, NavGrid, Neighborhood, PathError, STEPS, check_bounds, checked_cost, corner_open,
3    offset,
4};
5use glam::{DVec2, IVec2, UVec2, Vec2};
6
7/// Whether a straight segment between two cell centers crosses only walkable
8/// cells.
9///
10/// A segment through an exact cell corner must also satisfy `neighborhood`'s
11/// diagonal rule; four-way movement requires both side cells to be walkable.
12/// With a nonzero `clearance`, the segment is swept by a box with those half
13/// dimensions in cell units (see [`GridLayout::clearance`](super::GridLayout::clearance)),
14/// and every blocked cell it would overlap fails the check; zero checks a
15/// point. Errors if either cell is outside the grid, or with
16/// [`PathError::InvalidOptions`] unless both clearance components are in
17/// `0.0..0.5`.
18pub fn line_of_sight<G: NavGrid + ?Sized>(
19    grid: &G,
20    from: UVec2,
21    to: UVec2,
22    neighborhood: Neighborhood,
23    clearance: Vec2,
24) -> Result<bool, PathError> {
25    check_bounds(grid.size(), from)?;
26    check_bounds(grid.size(), to)?;
27    check_clearance(clearance)?;
28    trace(
29        grid,
30        from,
31        to,
32        neighborhood.corner_rule(),
33        clearance,
34        |cell, _| Ok(grid.walkable(cell)),
35    )
36}
37
38/// Removes waypoints that a straight segment can skip, in place.
39///
40/// Keeps the first and last cells. A waypoint is skipped when the segment
41/// between its neighbors has [`line_of_sight`] with `clearance` and costs no
42/// more than the cells it replaces, measured as each cell's cost times the
43/// length of the segment inside it. A shortcut may still cross a costlier cell when it saves
44/// more elsewhere, but it never makes the path more expensive. The path should
45/// come from [`PathFinder`](super::PathFinder) with the same `neighborhood`.
46/// Steps of a stale path that now cross blocked cells are kept unless a clear
47/// shortcut replaces them; smoothing never adds a blocked segment.
48///
49/// Pass the following body's half size in cell units as `clearance`
50/// ([`GridLayout::clearance`](super::GridLayout::clearance)), so shortcuts
51/// keep it off wall corners; with zero, segments are checked as
52/// lines between cell centers and a wide body can catch on a corner. Errors
53/// leave the path unchanged for cells outside the grid or an invalid
54/// `clearance` (see [`line_of_sight`]), but may leave it partly simplified
55/// for a cost that is not finite and positive.
56pub fn smooth_path<G: NavGrid + ?Sized>(
57    grid: &G,
58    path: &mut Vec<UVec2>,
59    neighborhood: Neighborhood,
60    clearance: Vec2,
61) -> Result<(), PathError> {
62    for &cell in path.iter() {
63        check_bounds(grid.size(), cell)?;
64    }
65    check_clearance(clearance)?;
66    if path.len() < 3 {
67        return Ok(());
68    }
69    let rule = neighborhood.corner_rule();
70    // Original path segments may cut corners the rule forbids for shortcuts.
71    let along = |from, to| segment_cost(grid, from, to, DiagonalRule::Always, Vec2::ZERO);
72    let mut kept = 1;
73    let mut anchor = path[0];
74    let mut original = 0.0;
75    for next in 1..path.len() {
76        // A blocked step (a stale path) makes any clear shortcut an
77        // improvement; without one, its endpoints are kept as they are.
78        original += along(path[next - 1], path[next])?.unwrap_or(f32::INFINITY);
79        if next == 1 {
80            continue;
81        }
82        let direct = segment_cost(grid, anchor, path[next], rule, clearance)?;
83        if direct.is_some_and(|direct| direct <= original * (1.0 + 1e-5)) {
84            continue;
85        }
86        anchor = path[next - 1];
87        path[kept] = anchor;
88        kept += 1;
89        original = along(anchor, path[next])?.unwrap_or(f32::INFINITY);
90    }
91    path[kept] = path[path.len() - 1];
92    path.truncate(kept + 1);
93    Ok(())
94}
95
96fn check_clearance(clearance: Vec2) -> Result<(), PathError> {
97    if clearance
98        .to_array()
99        .iter()
100        .all(|axis| (0.0..0.5).contains(axis))
101    {
102        Ok(())
103    } else {
104        Err(PathError::InvalidOptions)
105    }
106}
107
108/// Cost-weighted length of a segment between cell centers, or `None` when it
109/// crosses a blocked cell, a corner the rule forbids, or comes within
110/// `clearance` of a blocked cell.
111fn segment_cost<G: NavGrid + ?Sized>(
112    grid: &G,
113    from: UVec2,
114    to: UVec2,
115    rule: DiagonalRule,
116    clearance: Vec2,
117) -> Result<Option<f32>, PathError> {
118    let mut total = 0.0;
119    let clear = trace(grid, from, to, rule, clearance, |cell, length| {
120        Ok(match checked_cost(grid, cell, 0.0)? {
121            Some(cost) => {
122                total += cost * length;
123                true
124            }
125            None => false,
126        })
127    })?;
128    Ok(clear.then_some(total))
129}
130
131/// Visits each cell a center-to-center segment passes through with the
132/// segment length inside it, in cell units. Stops early when `visit` returns
133/// `false`, a corner crossing fails `rule`, or a box with half dimensions
134/// `clearance` swept along the segment overlaps a blocked cell.
135fn trace<G: NavGrid + ?Sized>(
136    grid: &G,
137    from: UVec2,
138    to: UVec2,
139    rule: DiagonalRule,
140    clearance: Vec2,
141    mut visit: impl FnMut(UVec2, f32) -> Result<bool, PathError>,
142) -> Result<bool, PathError> {
143    // With clearance below half a cell, any blocked cell the swept box
144    // overlaps neighbors a traversed cell.
145    let (start, end) = (from.as_dvec2() + 0.5, to.as_dvec2() + 0.5);
146    let crowded = |cell: UVec2| {
147        clearance.cmpgt(Vec2::ZERO).any()
148            && STEPS.iter().any(|&step| {
149                offset(grid.size(), cell, step).is_some_and(|side| {
150                    !grid.walkable(side)
151                        && sweeps(start, end, side.as_dvec2(), clearance.as_dvec2())
152                })
153            })
154    };
155    let delta = to.as_ivec2() - from.as_ivec2();
156    let (dx, dy) = (delta.x.unsigned_abs(), delta.y.unsigned_abs());
157    let step = delta.signum();
158    let length = delta.as_vec2().length();
159    // Centers sit at half-cell offsets, so the k-th x boundary is crossed at
160    // t = (2k + 1) / (2 dx). Compare crossings exactly with integers.
161    let (mut crossed_x, mut crossed_y) = (0_u64, 0_u64);
162    let mut cell = from;
163    let mut entered = 0.0_f32;
164    loop {
165        let next_x =
166            (dx > 0 && crossed_x < u64::from(dx)).then(|| (2 * crossed_x + 1) * u64::from(dy));
167        let next_y =
168            (dy > 0 && crossed_y < u64::from(dy)).then(|| (2 * crossed_y + 1) * u64::from(dx));
169        let (move_x, move_y) = match (next_x, next_y) {
170            (None, None) => return Ok(visit(cell, (1.0 - entered) * length)? && !crowded(cell)),
171            (Some(_), None) => (true, false),
172            (None, Some(_)) => (false, true),
173            (Some(x), Some(y)) => (x <= y, y <= x),
174        };
175        let exit = if move_x {
176            (2 * crossed_x + 1) as f32 / (2 * dx) as f32
177        } else {
178            (2 * crossed_y + 1) as f32 / (2 * dy) as f32
179        };
180        if !visit(cell, (exit - entered) * length)? || crowded(cell) {
181            return Ok(false);
182        }
183        entered = exit;
184        let advance = IVec2::new(
185            if move_x { step.x } else { 0 },
186            if move_y { step.y } else { 0 },
187        );
188        if move_x && move_y && !corner_open(grid, cell, advance, rule) {
189            return Ok(false);
190        }
191        crossed_x += u64::from(move_x);
192        crossed_y += u64::from(move_y);
193        cell = (cell.as_ivec2() + advance).as_uvec2();
194    }
195}
196
197/// Whether the segment from `start` to `end` enters the open square around
198/// the cell with minimum corner `min`, grown by `clearance` on each axis.
199fn sweeps(start: DVec2, end: DVec2, min: DVec2, clearance: DVec2) -> bool {
200    let (low, high) = (min - clearance, min + 1.0 + clearance);
201    let delta = end - start;
202    let (mut enter, mut exit) = (0.0_f64, 1.0_f64);
203    for axis in 0..2 {
204        if delta[axis] == 0.0 {
205            if start[axis] <= low[axis] || start[axis] >= high[axis] {
206                return false;
207            }
208        } else {
209            let a = (low[axis] - start[axis]) / delta[axis];
210            let b = (high[axis] - start[axis]) / delta[axis];
211            enter = enter.max(a.min(b));
212            exit = exit.min(a.max(b));
213        }
214    }
215    enter < exit
216}