rayengine_core/pathfinding/
smooth.rs1use super::{
2 DiagonalRule, NavGrid, Neighborhood, PathError, STEPS, check_bounds, checked_cost, corner_open,
3 offset,
4};
5use glam::{DVec2, IVec2, UVec2, Vec2};
6
7pub 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
38pub 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 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 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
108fn 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
131fn 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 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 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
197fn 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}