stara/
lib.rs

1/*
2 * Copyright (c) Peter Bjorklund. All rights reserved. https://github.com/piot/stara-rs
3 * Licensed under the MIT License. See LICENSE in the project root for license information.
4 */
5pub mod prelude;
6#[cfg(test)]
7mod test;
8
9pub mod grid;
10mod node;
11use node::Node;
12
13use crate::grid::Grid;
14use int_math::VectorU;
15use std::collections::{BinaryHeap, HashMap, HashSet};
16
17/// The movement cost to go into a cell
18pub type Cost = u8;
19type Score = u16;
20
21/// Represents the cost of an impassable tile in the grid.
22pub const IMPASSABLE: u8 = u8::MAX;
23
24#[allow(unused)]
25
26fn neighbors(pos: VectorU) -> Vec<VectorU> {
27    let mut v = vec![
28        VectorU {
29            x: pos.x + 1,
30            y: pos.y,
31        },
32        VectorU {
33            x: pos.x,
34            y: pos.y + 1,
35        },
36    ];
37
38    if pos.x > 0 {
39        v.push(VectorU {
40            x: pos.x - 1,
41            y: pos.y,
42        });
43    }
44
45    if pos.y > 0 {
46        v.push(VectorU {
47            x: pos.x,
48            y: pos.y - 1,
49        });
50    }
51
52    v
53}
54
55/// The `AStar` struct implements the A* pathfinding algorithm on a 2D grid.
56/// It finds the shortest path from a start point to a goal point, considering
57/// the cost of each tile in the grid.
58#[allow(unused)]
59pub struct AStar {}
60
61impl AStar {
62    /// Calculates the heuristic estimate of the cost from a given point to the goal.
63    /// Currently using the [Manhattan Distance](https://en.wikipedia.org/wiki/Taxicab_geometry).
64    #[allow(unused)]
65    fn heuristic(p1: VectorU, p2: VectorU) -> Score {
66        ((p1.x as i32 - p2.x as i32).abs() + (p1.y as i32 - p2.y as i32).abs()) as Score
67    }
68
69    /// Performs the A* search algorithm to find the shortest path from the start point to the goal.
70    ///
71    /// # Arguments
72    ///
73    /// * `start` - A [`VectorU`] representing the start position in the grid.
74    /// * `goal` - A [`VectorU`] representing the goal position in the grid.
75    /// * `grid` - The `Grid` on which the pathfinding will be performed.
76    ///
77    /// # Returns
78    ///
79    /// Returns an `Option<Vec<VectorU>>` containing the sequence of points representing the path from start to goal.
80    /// If no path is found, returns `None`.
81    ///
82    /// # Examples
83    ///
84    /// ```
85    /// use int_math::VectorU;
86    /// use stara::prelude::*;
87    ///
88    /// let grid = Grid::new(VectorU::new(10,10), 1);
89    /// let path = AStar::search(VectorU::new(0,0), VectorU::new(9,9), &grid);
90    ///
91    /// if let Some(path) = path {
92    ///     println!("Path found: {:?}", path);
93    /// } else {
94    ///     println!("No path found.");
95    /// }
96    /// ```
97    #[allow(unused)]
98    pub fn search(start: VectorU, goal: VectorU, grid: &Grid) -> Option<Vec<VectorU>> {
99        let mut open_set = BinaryHeap::new();
100        let mut open_set_positions = HashMap::new();
101        let mut closed_set = HashSet::new();
102
103        let start_node = Node {
104            position: start,
105            g: 0,
106            f: Self::heuristic(start, goal),
107            parent: None,
108        };
109
110        open_set.push(start_node.clone());
111        open_set_positions.insert(start, start_node);
112
113        while let Some(current_node) = open_set.pop() {
114            let current_position = current_node.position;
115            open_set_positions.remove(&current_position);
116
117            if current_position == goal {
118                let mut path = vec![current_position];
119                let mut current = current_node;
120                while let Some(parent_node) = current.parent {
121                    path.push(parent_node.position);
122                    current = *parent_node;
123                }
124                path.reverse();
125                return Some(path);
126            }
127
128            closed_set.insert(current_position);
129
130            for neighbor_position in neighbors(current_position) {
131                if !grid.in_bounds(neighbor_position) {
132                    continue;
133                }
134                if closed_set.contains(&neighbor_position) {
135                    continue;
136                }
137
138                let neighbor_cost = grid.cost(neighbor_position);
139                if neighbor_cost == IMPASSABLE {
140                    continue;
141                }
142
143                let tentative_g_score = &current_node.g + 1 + neighbor_cost as Score;
144
145                if let Some(open_neighbor_node) = open_set_positions.get(&neighbor_position) {
146                    // If we already have a better path to this neighbor node, skip this path to the neighbor
147                    if tentative_g_score >= open_neighbor_node.g {
148                        continue;
149                    }
150                }
151
152                if open_set_positions.contains_key(&neighbor_position) {
153                    continue;
154                }
155
156                let f_score = tentative_g_score + Self::heuristic(neighbor_position, goal);
157
158                let neighbor_node = Node {
159                    position: neighbor_position,
160                    g: tentative_g_score,
161                    f: f_score,
162                    parent: Some(Box::new(current_node.clone())),
163                };
164
165                open_set.push(neighbor_node.clone());
166                open_set_positions.insert(neighbor_position, neighbor_node);
167            }
168        }
169
170        None
171    }
172}
stara/lib.rs

stara/
lib.rs
