1use config::*;
2use direction::*;
3use error::*;
4use grid::*;
5use metadata::*;
6use num_traits::*;
7use path;
8use search::*;
9use std::ops::*;
10
11fn octile_distance<C>(a: Coord, b: Coord) -> C
12where
13 C: Add<C, Output = C> + Mul<C, Output = C> + FloatConst + NumCast,
14{
15 let dx = (a.x - b.x).abs();
16 let dy = (a.y - b.y).abs();
17 let (cardinal, ordinal) = if dx > dy {
18 (dx - dy, dy)
19 } else {
20 (dy - dx, dx)
21 };
22
23 let sqrt_2: C = FloatConst::SQRT_2();
24 let cardinal: C = NumCast::from(cardinal).expect("Failed to cast to Cost");
25 let ordinal: C = NumCast::from(ordinal).expect("Failed to cast to Cost");
26
27 cardinal + ordinal * sqrt_2
28}
29
30fn has_forced_neighbour_cardinal<G>(grid: &G, coord: Coord, direction: CardinalDirection) -> bool
31where
32 G: SolidGrid,
33{
34 if grid.is_solid_or_outside(coord + direction.left90().coord()) {
35 return !grid.is_solid_or_outside(coord + direction.left45().coord());
36 }
37
38 if grid.is_solid_or_outside(coord + direction.right90().coord()) {
39 return !grid.is_solid_or_outside(coord + direction.right45().coord());
40 }
41
42 false
43}
44
45fn has_forced_neighbour_ordinal<G>(grid: &G, coord: Coord, direction: OrdinalDirection) -> bool
46where
47 G: SolidGrid,
48{
49 if grid.is_solid_or_outside(coord + direction.left135().coord()) {
50 return !grid.is_solid_or_outside(coord + direction.left90().coord());
51 }
52
53 if grid.is_solid_or_outside(coord + direction.right135().coord()) {
54 return !grid.is_solid_or_outside(coord + direction.right90().coord());
55 }
56
57 false
58}
59
60fn jump_cardinal<G, C>(
61 grid: &G,
62 coord: Coord,
63 direction: CardinalDirection,
64 goal: Coord,
65) -> Option<(Coord, C)>
66where
67 G: SolidGrid,
68 C: Add<C, Output = C> + One,
69{
70 let neighbour_coord = coord + direction.coord();
71
72 if grid.is_solid_or_outside(neighbour_coord) {
73 return None;
74 }
75
76 if neighbour_coord == goal {
77 return Some((neighbour_coord, One::one()));
78 }
79
80 if has_forced_neighbour_cardinal(grid, neighbour_coord, direction) {
81 return Some((neighbour_coord, One::one()));
82 }
83
84 jump_cardinal(grid, neighbour_coord, direction, goal)
85 .map(|(coord, cost): (_, C)| (coord, cost + One::one()))
86}
87
88fn jump_ordinal<G, C>(
89 grid: &G,
90 coord: Coord,
91 direction: OrdinalDirection,
92 goal: Coord,
93) -> Option<(Coord, C)>
94where
95 G: SolidGrid,
96 C: Add<C, Output = C> + One + FloatConst,
97{
98 let neighbour_coord = coord + direction.coord();
99
100 if grid.is_solid_or_outside(neighbour_coord) {
101 return None;
102 }
103
104 if neighbour_coord == goal {
105 return Some((neighbour_coord, FloatConst::SQRT_2()));
106 }
107
108 if has_forced_neighbour_ordinal(grid, neighbour_coord, direction) {
109 return Some((neighbour_coord, FloatConst::SQRT_2()));
110 }
111
112 let (card0, card1) = direction.to_cardinals();
113
114 if jump_cardinal::<_, C>(grid, neighbour_coord, card0, goal).is_some()
115 || jump_cardinal::<_, C>(grid, neighbour_coord, card1, goal).is_some()
116 {
117 return Some((neighbour_coord, FloatConst::SQRT_2()));
118 }
119
120 jump_ordinal(grid, neighbour_coord, direction, goal)
121 .map(|(coord, cost): (_, C)| (coord, cost + FloatConst::SQRT_2()))
122}
123
124impl<C> SearchContext<C>
125where
126 C: Copy
127 + Zero
128 + One
129 + Add<C, Output = C>
130 + Mul<C, Output = C>
131 + FloatConst
132 + NumCast
133 + PartialOrd<C>,
134{
135 fn expand_cardinal<G>(
136 &mut self,
137 grid: &G,
138 current_coord: Coord,
139 current_cost: C,
140 direction: CardinalDirection,
141 goal: Coord,
142 ) -> Result<(), Error>
143 where
144 G: SolidGrid,
145 {
146 if let Some((successor_coord, successor_cost)) =
147 jump_cardinal::<_, C>(grid, current_coord, direction, goal)
148 {
149 self.see_successor(
150 current_cost + successor_cost,
151 successor_coord,
152 direction.direction(),
153 octile_distance,
154 goal,
155 )?;
156 }
157
158 Ok(())
159 }
160
161 fn expand_ordinal<G>(
162 &mut self,
163 grid: &G,
164 current_coord: Coord,
165 current_cost: C,
166 direction: OrdinalDirection,
167 goal: Coord,
168 ) -> Result<(), Error>
169 where
170 G: SolidGrid,
171 {
172 if let Some((successor_coord, successor_cost)) =
173 jump_ordinal::<_, C>(grid, current_coord, direction, goal)
174 {
175 self.see_successor(
176 current_cost + successor_cost,
177 successor_coord,
178 direction.direction(),
179 octile_distance,
180 goal,
181 )?;
182 }
183
184 Ok(())
185 }
186
187 fn expand_general<G>(
188 &mut self,
189 grid: &G,
190 current_coord: Coord,
191 current_cost: C,
192 direction: Direction,
193 goal: Coord,
194 ) -> Result<(), Error>
195 where
196 G: SolidGrid,
197 {
198 match direction.typ() {
199 DirectionType::Cardinal(direction) => {
200 self.expand_cardinal(grid, current_coord, current_cost, direction, goal)
201 }
202 DirectionType::Ordinal(direction) => {
203 self.expand_ordinal(grid, current_coord, current_cost, direction, goal)
204 }
205 }
206 }
207
208 pub fn jump_point_search_octile_distance_heuristic<G>(
209 &mut self,
210 grid: &G,
211 start: Coord,
212 goal: Coord,
213 config: SearchConfig,
214 path: &mut Vec<Direction>,
215 ) -> Result<SearchMetadata<C>, Error>
216 where
217 G: SolidGrid,
218 {
219 let initial_entry = match self.init(start, |c| c == goal, grid, config, path) {
220 Ok(initial_entry) => initial_entry,
221 Err(result) => return result,
222 };
223
224 let goal_index = self
225 .node_grid
226 .index_of_coord(goal)
227 .ok_or(Error::VisitOutsideContext)?;
228
229 for direction in Directions {
230 self.expand_general(grid, start, initial_entry.cost, direction, goal)?;
231 }
232
233 let mut num_nodes_visited = 0;
234
235 while let Some(current_entry) = self.priority_queue.pop() {
236 num_nodes_visited += 1;
237
238 if current_entry.node_index == goal_index {
239 let node = &self.node_grid[goal_index];
240 path::make_path_jump_points(&self.node_grid, goal, self.seq, path);
241 return Ok(SearchMetadata {
242 num_nodes_visited,
243 cost: node.cost,
244 length: path.len(),
245 });
246 }
247
248 let (current_coord, current_cost, direction) = {
249 let node = &mut self.node_grid[current_entry.node_index];
250 if node.visited == self.seq {
251 continue;
252 }
253 node.visited = self.seq;
254 let direction = node.from_parent.expect("Open set node without direction");
255 (node.coord, node.cost, direction)
256 };
257
258 match direction.typ() {
259 DirectionType::Cardinal(direction) => {
260 self.expand_cardinal(grid, current_coord, current_cost, direction, goal)?;
261 let left = direction.left90();
262 if grid.is_solid_or_outside(current_coord + left.coord()) {
263 self.expand_ordinal(
264 grid,
265 current_coord,
266 current_cost,
267 direction.left45(),
268 goal,
269 )?;
270 }
271 let right = direction.right90();
272 if grid.is_solid_or_outside(current_coord + right.coord()) {
273 self.expand_ordinal(
274 grid,
275 current_coord,
276 current_cost,
277 direction.right45(),
278 goal,
279 )?;
280 }
281 }
282 DirectionType::Ordinal(direction) => {
283 self.expand_ordinal(grid, current_coord, current_cost, direction, goal)?;
284 let (left, right) = direction.to_cardinals();
285 self.expand_cardinal(grid, current_coord, current_cost, left, goal)?;
286 self.expand_cardinal(grid, current_coord, current_cost, right, goal)?;
287
288 let (check_right, check_left) = direction.opposite().to_cardinals();
289
290 if grid.is_solid_or_outside(current_coord + check_left.coord()) {
291 self.expand_ordinal(
292 grid,
293 current_coord,
294 current_cost,
295 direction.left90(),
296 goal,
297 )?;
298 }
299 if grid.is_solid_or_outside(current_coord + check_right.coord()) {
300 self.expand_ordinal(
301 grid,
302 current_coord,
303 current_cost,
304 direction.right90(),
305 goal,
306 )?;
307 }
308 }
309 }
310 }
311
312 Err(Error::NoPath)
313 }
314}