com_croftsoft_core/ai/astar/methods/
mod.rs

1// =============================================================================
2//! - Methods for the A* algorithm
3//!
4//! # Metadata
5//! - Copyright: &copy; 2022-2023 [`CroftSoft Inc`]
6//! - Author: [`David Wallace Croft`]
7//! - Java created: 2002-04-21
8//! - Java updated: 2003-05-09
9//! - Rust created: 2022-10-28
10//! - Rust updated: 2023-07-23
11//!
12//! # History
13//! - Adapted from the classes in the Java-based [`CroftSoft Core Library`]
14//!   - com.croftsoft.core.ai.astar.AStar
15//!
16//! [`CroftSoft Core Library`]: https://www.croftsoft.com/library/code/
17//! [`CroftSoft Inc`]: https://www.croftsoft.com/
18//! [`David Wallace Croft`]: https://www.croftsoft.com/people/david/
19// =============================================================================
20
21#[cfg(test)]
22mod test;
23
24use super::structures::{AStar, NodeInfo};
25use super::traits::Cartographer;
26use core::f64::INFINITY;
27use core::hash::Hash;
28use std::collections::HashMap;
29use std::collections::VecDeque;
30
31impl<N: Copy + Eq + Hash> AStar<N> {
32  pub fn get_first_step(&self) -> Option<N> {
33    let mut node_option: Option<N> = self.goal_node_option;
34    if node_option.is_none() {
35      node_option = self.best_node_option;
36    }
37    node_option?;
38    let mut child_node_option: Option<N> = None;
39    let mut node: N = node_option.unwrap();
40    let mut parent_node_option = self.node_to_parent_node_map.get(&node);
41    while parent_node_option.is_some() {
42      child_node_option = node_option;
43      node_option = parent_node_option.copied();
44      node = node_option.unwrap();
45      parent_node_option = self.node_to_parent_node_map.get(&node);
46    }
47    child_node_option
48  }
49
50  pub fn get_path(&self) -> VecDeque<N> {
51    let mut path_list = VecDeque::new();
52    let mut node_option: Option<N> = self.goal_node_option;
53    if node_option.is_none() {
54      node_option = self.best_node_option;
55    }
56    if node_option.is_none() {
57      return path_list;
58    }
59    let mut node: N = node_option.unwrap();
60    let mut parent_node_option = self.node_to_parent_node_map.get(&node);
61    while parent_node_option.is_some() {
62      path_list.push_front(node);
63      node_option = parent_node_option.copied();
64      node = node_option.unwrap();
65      parent_node_option = self.node_to_parent_node_map.get(&node);
66    }
67    path_list
68  }
69
70  pub fn is_goal_found(&self) -> bool {
71    self.goal_node_option.is_some()
72  }
73
74  pub fn loop_once(
75    &mut self,
76    cartographer: &dyn Cartographer<N>,
77  ) -> bool {
78    let Some(node) = self.open_node_sorted_list.pop_front() else {
79      self.list_empty = true;
80      return false;
81    };
82    let node_info: NodeInfo = *self.node_to_node_info_map.get(&node).unwrap();
83    if cartographer.is_goal_node(&node) {
84      if let Some(goal_node) = self.goal_node_option {
85        let goal_node_info =
86          self.node_to_node_info_map.get(&goal_node).unwrap();
87        if goal_node_info.cost_from_start <= node_info.cost_from_start {
88          return false;
89        }
90      }
91      self.goal_node_option = Some(node);
92      return false;
93    }
94    let adjacent_nodes: Vec<N> = cartographer.get_adjacent_nodes(&node);
95    for adjacent_node in adjacent_nodes {
96      let new_cost_from_start: f64 = node_info.cost_from_start
97        + cartographer.get_cost_to_adjacent_node(&node, &adjacent_node);
98      let adjacent_node_info_option: Option<&NodeInfo> =
99        self.node_to_node_info_map.get(&adjacent_node);
100      if let Some(adjacent_node_info) = adjacent_node_info_option {
101        if adjacent_node_info.cost_from_start <= new_cost_from_start {
102          continue;
103        }
104        // TODO: Do something better here, maybe a sorted set
105        let position_option = self
106          .open_node_sorted_list
107          .iter()
108          .position(|&node| node == adjacent_node);
109        if let Some(position) = position_option {
110          self.open_node_sorted_list.remove(position);
111        }
112      }
113      let total_cost: f64 = new_cost_from_start
114        + cartographer.estimate_cost_to_goal(&adjacent_node);
115      let adjacent_node_info = NodeInfo {
116        cost_from_start: new_cost_from_start,
117        total_cost,
118      };
119      self.node_to_node_info_map.insert(adjacent_node, adjacent_node_info);
120      self.open_node_sorted_list.push_back(adjacent_node);
121      self.open_node_sorted_list.make_contiguous().sort_by(|a, b| {
122        let node_info_a = self.node_to_node_info_map.get(a).unwrap();
123        let node_info_b = self.node_to_node_info_map.get(b).unwrap();
124        node_info_a.cmp(node_info_b)
125      });
126      self.node_to_parent_node_map.insert(adjacent_node, node);
127      if total_cost < self.best_total_cost {
128        self.best_node_option = Some(adjacent_node);
129        self.best_total_cost = total_cost;
130      }
131    }
132    true
133  }
134
135  pub fn reset(
136    &mut self,
137    start_node: N,
138  ) {
139    self.goal_node_option = None;
140    self.list_empty = false;
141    self.open_node_sorted_list = VecDeque::new();
142    self.node_to_node_info_map = HashMap::new();
143    self.node_to_parent_node_map = HashMap::new();
144    let start_node_info = NodeInfo::default();
145    self.node_to_node_info_map.insert(start_node, start_node_info);
146    self.open_node_sorted_list.push_front(start_node);
147    self.best_total_cost = INFINITY;
148  }
149}
com_croftsoft_core/ai/astar/methods/mod.rs

com_croftsoft_core/ai/astar/methods/
mod.rs
