path_finding_lib/search/
a_star.rs

1#[cfg(test)]
2use std::collections::HashMap;
3
4use crate::{graph::Graph, path::PathFinding};
5#[cfg(test)]
6use crate::graph::Edge;
7use crate::node::{Node, Position};
8use crate::search::dijkstra;
9
10pub fn get_position(node_id: usize, graph: &Graph) -> &Position {
11    match &graph.node_position_lookup {
12        None => panic!("You must offer node positions to the graph before using this heuristic."),
13        Some(positions) => {
14            return match positions.get(&node_id) {
15                None => panic!("Node position missing for given node id: {node_id}"),
16                Some(position) => position
17            };
18        }
19    };
20}
21
22pub fn euclidean_distance(source: usize, destination: usize, graph: &Graph) -> f32 {
23    let src = get_position(source, graph);
24    let dest = get_position(destination, graph);
25
26    return ((dest.x - src.x).powf(2.0) + (dest.y - src.y).powf(2.0) + (dest.z - src.z).powf(2.0)).sqrt();
27}
28
29pub fn manhattan_distance(source: usize, destination: usize, graph: &Graph) -> f32 {
30    let src = get_position(source, graph);
31    let dest = get_position(destination, graph);
32
33    return (dest.x - src.x).abs() + (dest.y - src.y).abs() + (dest.z - src.z).abs();
34}
35
36pub struct AStar {
37    pub heuristic: Box<dyn Fn(usize, usize, &Graph) -> f32>,
38}
39
40impl PathFinding for AStar {
41    fn execute(&self, source: Node, target: Node, graph: &Graph) -> Graph {
42        return dijkstra(source, target, graph, &self.heuristic);
43    }
44}
45
46
47#[test]
48#[should_panic(expected = "You must offer node positions to the graph before using this heuristic.")]
49fn missing_node_positions_should_cause_panic() {
50    get_position(1, &Graph::from(Vec::from([
51        Edge::from(0, 0, 1, 4.0)
52    ])));
53}
54
55#[test]
56#[should_panic(expected = "Node position missing for given node id: 1")]
57fn missing_node_position_should_cause_panic() {
58    let mut graph = Graph::from(Vec::from([
59        Edge::from(0, 0, 1, 4.0)
60    ]));
61
62    graph.offer_positions(HashMap::from([(0, Position::from(0.0, 0.0, 0.0))]));
63
64    get_position(1, &graph);
65}
66
67#[test]
68fn node_position_should_be_returned() {
69    let mut graph = Graph::from(Vec::from([
70        Edge::from(0, 0, 1, 4.0)
71    ]));
72
73    graph.offer_positions(HashMap::from([(1, Position::from(0.1, 0.2, 0.3))]));
74    let pos = get_position(1, &graph);
75
76    assert_eq!(0.1, pos.x);
77    assert_eq!(0.2, pos.y);
78    assert_eq!(0.3, pos.z);
79}
80
81#[test]
82fn euclidean_heuristic_should_return_dist() {
83    let mut graph = Graph::from(Vec::from([
84        Edge::from(0, 0, 1, 4.0)
85    ]));
86
87    graph.offer_positions(HashMap::from([
88        (0, Position::from(20.0, 30.0, 90.0)),
89        (1, Position::from(80.0, 44.0, 40.0))
90    ]));
91
92    assert_eq!(79.347336, euclidean_distance(0, 1, &graph));
93}
94
95#[test]
96fn manhattan_heuristic_should_return_dist() {
97    let mut graph = Graph::from(Vec::from([
98        Edge::from(0, 0, 1, 4.0)
99    ]));
100
101    graph.offer_positions(HashMap::from([
102        (0, Position::from(2.0, 9.0, 0.0)),
103        (1, Position::from(3.0, 5.0, 0.0))
104    ]));
105
106    assert_eq!(5.0, manhattan_distance(0, 1, &graph));
107}
path_finding_lib/search/a_star.rs

path_finding_lib/search/
a_star.rs
