Line data Source code
1 : //! The routing grid and the search over it.
2 : //!
3 : //! The grid is the Hanan grid of the obstacles grown by the margin: one
4 : //! vertical line through each grown obstacle's left and right edges and
5 : //! through each port's stub end, one horizontal line through each top
6 : //! and bottom edge and each stub end. Its vertices are where the lines
7 : //! cross, its edges join neighbouring vertices along a line. No obstacle
8 : //! edge lies strictly between two neighbouring lines, so a grid edge
9 : //! either crosses an obstacle's interior over its whole length or not at
10 : //! all, and an edge along an obstacle's boundary is free: routes run
11 : //! along the margin lines, at the margin's distance from the nodes.
12 : //!
13 : //! A route is a shortest path in that grid where each turn costs
14 : //! [`Options::bend_penalty`](super::Options::bend_penalty) on top of the
15 : //! length: A* over (vertex, direction) states, with the Manhattan
16 : //! distance to the goal as its heuristic (a lower bound, since a bend
17 : //! never costs less than nothing).
18 :
19 : use std::cmp::Ordering;
20 : use std::collections::BinaryHeap;
21 :
22 : use super::geom::{EPS, Point, Rect, Side};
23 :
24 : /// Sorted distinct coordinates, those within [`EPS`] of the previous
25 : /// one dropped.
26 37 : fn lines(mut v: Vec<f32>) -> Vec<f32> {
27 1071 : v.retain(|x| x.is_finite());
28 37 : v.sort_by(f32::total_cmp);
29 37 : let mut out: Vec<f32> = Vec::with_capacity(v.len());
30 1070 : for x in v {
31 1070 : if out.last().is_none_or(|l| x - l > EPS) {
32 253 : out.push(x);
33 817 : }
34 : }
35 37 : out
36 37 : }
37 :
38 : /// The range of lines in `[lo, hi]` (within [`EPS`]).
39 312 : fn closed(v: &[f32], lo: f32, hi: f32) -> (usize, usize) {
40 : (
41 1583 : v.partition_point(|&x| x < lo - EPS),
42 1583 : v.partition_point(|&x| x <= hi + EPS),
43 : )
44 312 : }
45 :
46 : /// The range of lines strictly inside `(lo, hi)` (within [`EPS`]).
47 312 : fn open(v: &[f32], lo: f32, hi: f32) -> (usize, usize) {
48 : (
49 1583 : v.partition_point(|&x| x <= lo + EPS),
50 1583 : v.partition_point(|&x| x < hi - EPS),
51 : )
52 312 : }
53 :
54 : /// The index of the line at `x`, if there is one.
55 442 : fn line_of(v: &[f32], x: f32) -> Option<usize> {
56 2316 : let i = v.partition_point(|&a| a < x - EPS);
57 442 : (i < v.len() && (v[i] - x).abs() <= EPS).then_some(i)
58 442 : }
59 :
60 : /// The routing grid.
61 : #[derive(Clone, Debug, Default)]
62 : pub(crate) struct Grid {
63 : xs: Vec<f32>,
64 : ys: Vec<f32>,
65 : /// Vertices strictly inside an obstacle, by `j * nx + i`.
66 : blocked: Vec<bool>,
67 : /// Edges from `(i, j)` to `(i + 1, j)` through an obstacle.
68 : h_blocked: Vec<bool>,
69 : /// Edges from `(i, j)` to `(i, j + 1)` through an obstacle.
70 : v_blocked: Vec<bool>,
71 : }
72 :
73 : impl Grid {
74 : /// The grid of grown obstacles, with lines through `points` too.
75 18 : pub(crate) fn build(obstacles: &[Rect], points: &[Point]) -> Grid {
76 18 : let xs = lines(
77 18 : obstacles
78 18 : .iter()
79 156 : .flat_map(|o| [o.min.x, o.max.x])
80 18 : .chain(points.iter().map(|p| p.x))
81 18 : .collect(),
82 : );
83 18 : let ys = lines(
84 18 : obstacles
85 18 : .iter()
86 156 : .flat_map(|o| [o.min.y, o.max.y])
87 18 : .chain(points.iter().map(|p| p.y))
88 18 : .collect(),
89 : );
90 18 : let nx = xs.len();
91 18 : let n = nx * ys.len();
92 18 : let mut grid = Grid {
93 18 : blocked: vec![false; n],
94 18 : h_blocked: vec![false; n],
95 18 : v_blocked: vec![false; n],
96 18 : xs,
97 18 : ys,
98 18 : };
99 156 : for o in obstacles {
100 156 : let (cxa, cxb) = closed(&grid.xs, o.min.x, o.max.x);
101 156 : let (cya, cyb) = closed(&grid.ys, o.min.y, o.max.y);
102 156 : let (oxa, oxb) = open(&grid.xs, o.min.x, o.max.x);
103 156 : let (oya, oyb) = open(&grid.ys, o.min.y, o.max.y);
104 187 : for j in oya..oyb {
105 216 : for i in cxa..cxb.saturating_sub(1) {
106 216 : grid.h_blocked[j * nx + i] = true;
107 216 : }
108 187 : for i in oxa..oxb {
109 29 : grid.blocked[j * nx + i] = true;
110 29 : }
111 : }
112 156 : for i in oxa..oxb {
113 44 : for j in cya..cyb.saturating_sub(1) {
114 44 : grid.v_blocked[j * nx + i] = true;
115 44 : }
116 : }
117 : }
118 18 : grid
119 18 : }
120 :
121 26552 : fn nx(&self) -> usize {
122 26552 : self.xs.len()
123 26552 : }
124 :
125 : /// The vertex at `p`, when `p` is on two lines and outside every
126 : /// obstacle.
127 221 : pub(crate) fn vertex(&self, p: Point) -> Option<usize> {
128 221 : let i = line_of(&self.xs, p.x)?;
129 221 : let j = line_of(&self.ys, p.y)?;
130 221 : let v = j * self.nx() + i;
131 221 : (!self.blocked[v]).then_some(v)
132 221 : }
133 :
134 : /// Where a vertex is.
135 11227 : pub(crate) fn point(&self, v: usize) -> Point {
136 11227 : Point::new(self.xs[v % self.nx()], self.ys[v / self.nx()])
137 11227 : }
138 :
139 : /// The vertex next to `v` going `d`, when the way is free.
140 3877 : fn step(&self, v: usize, d: Side) -> Option<usize> {
141 3877 : let nx = self.nx();
142 3877 : let (i, j) = (v % nx, v / nx);
143 3432 : let next = match d {
144 1081 : Side::Left if i > 0 && !self.h_blocked[v - 1] => v - 1,
145 1278 : Side::Right if i + 1 < nx && !self.h_blocked[v] => v + 1,
146 760 : Side::Top if j > 0 && !self.v_blocked[v - nx] => v - nx,
147 758 : Side::Bottom if j + 1 < self.ys.len() && !self.v_blocked[v] => v + nx,
148 445 : _ => return None,
149 : };
150 3432 : (!self.blocked[next]).then_some(next)
151 3877 : }
152 : }
153 :
154 : /// An open state, the cheapest estimate first.
155 : #[derive(Clone, Copy, Debug, PartialEq)]
156 : struct Open {
157 : f: f32,
158 : state: u32,
159 : }
160 :
161 : impl Eq for Open {}
162 :
163 : impl Ord for Open {
164 12224 : fn cmp(&self, other: &Self) -> Ordering {
165 : // A max-heap: reversed, and ties broken by the state so the
166 : // order (and the route) only depends on the input.
167 12224 : other
168 12224 : .f
169 12224 : .total_cmp(&self.f)
170 12224 : .then_with(|| other.state.cmp(&self.state))
171 12224 : }
172 : }
173 :
174 : impl PartialOrd for Open {
175 12224 : fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
176 12224 : Some(self.cmp(other))
177 12224 : }
178 : }
179 :
180 : /// The search's buffers, kept between searches.
181 : #[derive(Clone, Debug, Default)]
182 : pub(crate) struct Search {
183 : g: Vec<f32>,
184 : parent: Vec<u32>,
185 : done: Vec<bool>,
186 : heap: BinaryHeap<Open>,
187 : }
188 :
189 : const NONE: u32 = u32::MAX;
190 :
191 : impl Search {
192 : /// The cheapest path from `start`, leaving in direction `first`, to
193 : /// `goal`, arriving in direction `last`: its vertices, from `start`
194 : /// to `goal`. `None` when `goal` cannot be reached.
195 108 : pub(crate) fn find(
196 108 : &mut self,
197 108 : grid: &Grid,
198 108 : start: usize,
199 108 : first: Side,
200 108 : goal: usize,
201 108 : last: Side,
202 108 : bend: f32,
203 108 : ) -> Option<Vec<usize>> {
204 108 : let states = grid.blocked.len() * 4;
205 108 : if states > NONE as usize {
206 0 : return None;
207 108 : }
208 108 : self.g.clear();
209 108 : self.g.resize(states, f32::INFINITY);
210 108 : self.parent.clear();
211 108 : self.parent.resize(states, NONE);
212 108 : self.done.clear();
213 108 : self.done.resize(states, false);
214 108 : self.heap.clear();
215 :
216 108 : let target = grid.point(goal);
217 3501 : let h = |v: usize| {
218 3501 : let p = grid.point(v);
219 3501 : (p.x - target.x).abs() + (p.y - target.y).abs()
220 3501 : };
221 108 : let s0 = start * 4 + first.index();
222 108 : self.g[s0] = 0.0;
223 108 : self.heap.push(Open {
224 108 : f: h(start),
225 108 : state: s0 as u32,
226 108 : });
227 108 : let mut best = (f32::INFINITY, NONE);
228 1511 : while let Some(Open { f, state }) = self.heap.pop() {
229 1511 : if f >= best.0 {
230 108 : break;
231 1403 : }
232 1403 : let s = state as usize;
233 1403 : if self.done[s] {
234 0 : continue;
235 1403 : }
236 1403 : self.done[s] = true;
237 1403 : let (v, d) = (s / 4, Side::ALL[s % 4]);
238 1403 : let g = self.g[s];
239 1403 : if v == goal {
240 112 : let total = g + if d == last { 0.0 } else { bend };
241 112 : if total < best.0 {
242 108 : best = (total, state);
243 108 : }
244 112 : continue;
245 1291 : }
246 5164 : for nd in Side::ALL {
247 5164 : if nd == d.opposite() {
248 1291 : continue;
249 3873 : }
250 3873 : let Some(n) = grid.step(v, nd) else { continue };
251 3430 : let ns = n * 4 + nd.index();
252 3430 : if self.done[ns] {
253 13 : continue;
254 3417 : }
255 3417 : let (a, b) = (grid.point(v), grid.point(n));
256 3417 : let len = (a.x - b.x).abs() + (a.y - b.y).abs();
257 3417 : let cost = g + len + if nd == d { 0.0 } else { bend };
258 3417 : if cost < self.g[ns] {
259 3393 : self.g[ns] = cost;
260 3393 : self.parent[ns] = state;
261 3393 : self.heap.push(Open {
262 3393 : f: cost + h(n),
263 3393 : state: ns as u32,
264 3393 : });
265 3393 : }
266 : }
267 : }
268 108 : if best.1 == NONE {
269 0 : return None;
270 108 : }
271 108 : let mut path = Vec::new();
272 108 : let mut s = best.1;
273 892 : while s != NONE {
274 784 : path.push(s as usize / 4);
275 784 : s = self.parent[s as usize];
276 784 : }
277 108 : path.reverse();
278 108 : Some(path)
279 108 : }
280 : }
281 :
282 : #[cfg(test)]
283 : mod tests {
284 : use super::*;
285 :
286 : #[test]
287 1 : fn close_coordinates_make_one_line() {
288 1 : assert_eq!(
289 1 : lines(vec![2.0, 1.0, 1.0005, f32::NAN, 3.0]),
290 1 : vec![1.0, 2.0, 3.0]
291 : );
292 1 : }
293 :
294 : #[test]
295 1 : fn an_obstacle_blocks_its_inside_and_frees_its_edges() {
296 1 : let o = Rect::from_corners([0.0, 0.0], [10.0, 10.0]);
297 1 : let g = Grid::build(&[o], &[Point::new(5.0, 5.0), Point::new(20.0, 20.0)]);
298 : // The centre is inside.
299 1 : assert!(g.vertex(Point::new(5.0, 5.0)).is_none());
300 : // A corner is on the edge: free, and so is the way along it.
301 1 : let corner = g.vertex(Point::new(0.0, 0.0)).unwrap();
302 1 : assert!(g.step(corner, Side::Right).is_some());
303 1 : assert!(g.step(corner, Side::Bottom).is_some());
304 : // Going in from the middle of an edge is blocked.
305 1 : let mid = g.vertex(Point::new(5.0, 0.0)).unwrap();
306 1 : assert!(g.step(mid, Side::Bottom).is_none());
307 1 : assert!(g.step(mid, Side::Top).is_none(), "no line above");
308 1 : }
309 : }
|
