rayengine_core/pathfinding/
field.rs1use super::{
2 NavGrid, Neighborhood, PathError, cell_count, cell_of, check_bounds, checked_cost, corner_open,
3 index, offset, step_length,
4};
5use glam::UVec2;
6use std::{cmp::Ordering, collections::BinaryHeap};
7
8#[derive(Clone, Copy, Debug)]
9struct Open {
10 distance: f32,
11 index: u32,
12}
13
14impl Ord for Open {
15 fn cmp(&self, other: &Self) -> Ordering {
17 other
18 .distance
19 .total_cmp(&self.distance)
20 .then_with(|| other.index.cmp(&self.index))
21 }
22}
23
24impl PartialOrd for Open {
25 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
26 Some(self.cmp(other))
27 }
28}
29
30impl PartialEq for Open {
31 fn eq(&self, other: &Self) -> bool {
32 self.cmp(other) == Ordering::Equal
33 }
34}
35
36impl Eq for Open {}
37
38#[derive(Clone, Debug, Default)]
62pub struct DistanceField {
63 size: UVec2,
64 neighborhood: Neighborhood,
65 distances: Vec<f32>,
66 open: BinaryHeap<Open>,
67}
68
69impl DistanceField {
70 pub fn new() -> Self {
72 Self::default()
73 }
74
75 pub fn compute<G: NavGrid + ?Sized>(
81 &mut self,
82 grid: &G,
83 goals: impl IntoIterator<Item = UVec2>,
84 neighborhood: Neighborhood,
85 ) -> Result<(), PathError> {
86 let result = self.fill(grid, goals, neighborhood);
87 if result.is_err() {
88 self.size = UVec2::ZERO;
89 self.distances.clear();
90 }
91 self.open.clear();
92 result
93 }
94
95 fn fill<G: NavGrid + ?Sized>(
96 &mut self,
97 grid: &G,
98 goals: impl IntoIterator<Item = UVec2>,
99 neighborhood: Neighborhood,
100 ) -> Result<(), PathError> {
101 let size = grid.size();
102 let count = cell_count(size)?;
103 self.size = size;
104 self.neighborhood = neighborhood;
105 self.distances.clear();
106 self.distances.resize(count, f32::INFINITY);
107 self.open.clear();
108 for goal in goals {
109 check_bounds(size, goal)?;
110 let goal_index = index(size, goal);
111 if checked_cost(grid, goal, 0.0)?.is_some() && self.distances[goal_index] != 0.0 {
112 self.distances[goal_index] = 0.0;
113 self.open.push(Open {
114 distance: 0.0,
115 index: goal_index as u32,
116 });
117 }
118 }
119 let rule = neighborhood.corner_rule();
120 while let Some(open) = self.open.pop() {
121 let current = open.index as usize;
122 if open.distance > self.distances[current] {
123 continue;
124 }
125 let cell = cell_of(size, current);
126 let Some(cost) = checked_cost(grid, cell, 0.0)? else {
128 continue;
129 };
130 for &step in neighborhood.steps() {
131 let Some(from) = offset(size, cell, step) else {
132 continue;
133 };
134 if !corner_open(grid, from, -step, rule) || !grid.walkable(from) {
136 continue;
137 }
138 let from_index = index(size, from);
139 let distance = open.distance + cost * step_length(step);
140 if distance < self.distances[from_index] {
141 self.distances[from_index] = distance;
142 self.open.push(Open {
143 distance,
144 index: from_index as u32,
145 });
146 }
147 }
148 }
149 Ok(())
150 }
151
152 pub fn size(&self) -> UVec2 {
154 self.size
155 }
156
157 pub fn distance(&self, cell: UVec2) -> Option<f32> {
160 if !cell.cmplt(self.size).all() {
161 return None;
162 }
163 let distance = self.distances[index(self.size, cell)];
164 distance.is_finite().then_some(distance)
165 }
166
167 pub fn next_step<G: NavGrid + ?Sized>(&self, grid: &G, cell: UVec2) -> Option<UVec2> {
173 if grid.size() != self.size {
174 return None;
175 }
176 let here = self.distance(cell)?;
177 let rule = self.neighborhood.corner_rule();
178 let mut best: Option<(f32, UVec2)> = None;
179 for &step in self.neighborhood.steps() {
180 let Some(next) = offset(self.size, cell, step) else {
181 continue;
182 };
183 let (Some(distance), Some(cost)) = (self.distance(next), grid.cost(next)) else {
184 continue;
185 };
186 if !corner_open(grid, cell, step, rule) {
187 continue;
188 }
189 let total = distance + cost * step_length(step);
190 if best.is_none_or(|(best, _)| total < best) {
191 best = Some((total, next));
192 }
193 }
194 best.filter(|&(_, next)| self.distances[index(self.size, next)] < here)
196 .map(|(_, next)| next)
197 }
198}