blitz_path/
astar.rs

1use std::collections::BinaryHeap;
2
3use movingai::Coords2D;
4use movingai::Map2D;
5
6use crate::node::Node;
7use crate::utils::{distance, rewind};
8use crate::Route;
9
10///Creates a new route using the A* algorithm.
11///Returns a Route struct containing the distance to the goal and number of steps needed to get there.
12/// # Examples
13///
14/// ```
15/// # use std::io;
16/// use std::path::Path;
17///
18/// let map = movingai::parser::parse_map_file(Path::new("./tests/map/maze512-32-9.map"))?;
19/// let scenes = movingai::parser::parse_scen_file(Path::new("./tests/map/maze512-32-9.map.scen"))?;
20/// let scene = &scenes[0];
21///
22/// let path = blitz_path::a_star_path(&map, scene.start_pos, scene.goal_pos);
23///
24/// // using as f32 as scene.optimal_length is stored as f64,
25/// // but only seems to have precision to f32
26/// if let Some(path) = path {
27///     assert_eq!(scene.optimal_length as f32, path.distance() as f32);  
28/// }
29/// # Ok::<(), io::Error>(())
30/// ```
31
32pub fn a_star_path<U, T: Map2D<U>>(map: &T, start: Coords2D, goal: Coords2D) -> Option<Route> {
33    if start == goal {
34        return Some(Route::from((0.0, vec![])));
35    }
36
37    //Initialize open and closed lists
38    let capacity = (1 + start.0 + 1 - start.0 - 1) * (1 + start.1 + 1 - start.1 - 1);
39    let mut open = BinaryHeap::with_capacity(capacity);
40    let mut closed = Vec::<Node>::with_capacity(capacity);
41
42    //Push start node to open list
43    open.push(Node {
44        f: distance(start, goal),
45        g: 0.0,
46        h: distance(start, goal),
47        position: start,
48        parent: start,
49    });
50
51    //Examine the nodes
52    while let Some(node_current) = open.pop() {
53        //If this is the target node return the distance to get there
54        if node_current.position == goal {
55            let path = rewind(&node_current, &closed);
56            let route = Route::from((node_current.g, path));
57            return Some(route);
58        }
59
60        //Setup successor nodes
61        for successor in map.neighbors(node_current.position) {
62            //Calculate distances
63            let distance_to_goal = distance(successor, goal);
64            let distance_from_parent = distance(node_current.position, successor);
65            let total_distance = node_current.g + distance_from_parent;
66
67            //Check if node is on closed list
68            if let Some(closed) = closed.iter().find(|x| x.position == successor) {
69                //and skip if existing score is better
70                if closed.f < total_distance + distance_to_goal {
71                    continue;
72                }
73            }
74
75            //See if node is on open list
76            if let Some(old_open) = open.iter().find(|x| x.position == successor) {
77                //Continue if we already have a better result
78                if old_open.g < total_distance {
79                    continue;
80                }
81            }
82
83            //If sill best distance build node and push to open list
84            //Build node from distances
85            let node_successor = Node {
86                f: total_distance + distance_to_goal,
87                g: total_distance,
88                h: distance_to_goal,
89                position: successor,
90                parent: node_current.position,
91            };
92
93            open.push(node_successor);
94        }
95
96        //Push current node to closed list and remove existing entry if duplicate
97        if let Some(index) = closed
98            .iter()
99            .position(|x| x.position == node_current.position)
100        {
101            closed.remove(index);
102        }
103        closed.push(node_current);
104    }
105
106    None
107}
blitz_path/astar.rs

blitz_path/
astar.rs
