1use std::collections::{HashMap, HashSet};
2
3use ordered_float::NotNan;
4use priority_queue::DoublePriorityQueue;
5
6use crate::graph::{Edge, Graph};
7use crate::node::Node;
8use crate::path::PathFinding;
9
10pub struct BreadthFirstSearch {}
11
12pub struct Dijkstra {}
13
14pub(crate) fn dijkstra(source: Node,
15 target: Node,
16 graph: &Graph,
17 heuristic: &dyn Fn(usize, usize, &Graph) -> f32) -> Graph {
18 let mut visited: HashSet<usize> = HashSet::new();
19 let mut edges_for_node_id: HashMap<usize, Vec<Edge>> = HashMap::new();
20 let mut queue: DoublePriorityQueue<usize, NotNan<f32>> = DoublePriorityQueue::new();
21
22 queue.push(source.id, NotNan::new(0.0).unwrap());
23 edges_for_node_id.insert(source.id, Vec::new());
24
25 while !visited.contains(&target.id) && !queue.is_empty() {
26 let node = queue.pop_min().unwrap();
27 visited.insert(node.0);
28
29 let edges = graph.nodes_lookup.get(&node.0).unwrap().edges.clone();
30 for edge in edges {
31 if !visited.contains(&edge.destination) {
32 let cost = node.1 + edge.weight + heuristic(edge.destination, target.id, graph);
33 queue.push(edge.destination, cost);
34
35 let mut from_edges = edges_for_node_id.get(&node.0).unwrap().clone();
36 from_edges.push(edge.clone());
37 edges_for_node_id.insert(edge.destination, from_edges);
38 }
39 }
40 }
41
42 return match edges_for_node_id.get(&target.id) {
43 None => Graph::from(Vec::new()),
44 Some(edges) => Graph::from(edges.clone())
45 };
46}
47
48fn dijkstra_heuristic(_source: usize, _destination: usize, _graph: &Graph) -> f32 {
49 return 0.0;
50}
51
52impl PathFinding for Dijkstra {
53 fn execute(&self, source: Node, target: Node, graph: &Graph) -> Graph {
54 return dijkstra(source, target, graph, &dijkstra_heuristic);
55 }
56}
57
58#[test]
59fn should_find_path_with_dijkstra_between_a_and_b() {
60 let graph = graph();
61
62 let dij = Dijkstra {};
63 let path = dij.execute(graph.nodes_lookup.get(&0).unwrap().clone(),
64 graph.nodes_lookup.get(&1).unwrap().clone(), &graph);
65
66 assert_eq!(3.0, calc_cost(&path.edges));
67 assert_eq!(2, path.edges.len());
68}
69
70#[test]
71fn should_find_path_with_dijkstra_between_a_and_c() {
72 let graph = graph();
73
74 let dij = Dijkstra {};
75 let path = dij.execute(get_node(0, &graph), get_node(2, &graph), &graph);
76
77
78 assert_eq!(2.0, calc_cost(&path.edges));
79 assert_eq!(1, path.edges.len());
80}
81
82#[test]
83fn should_find_path_with_dijkstra_between_a_and_d() {
84 let graph = graph();
85
86 let dij = Dijkstra {};
87 let path = dij.execute(get_node(0, &graph), get_node(3, &graph), &graph);
88
89
90 assert_eq!(5.0, calc_cost(&path.edges));
91 assert_eq!(3, path.edges.len());
92}
93
94#[test]
95fn should_find_path_with_dijkstra_between_a_and_e() {
96 let graph = graph();
97
98 let dij = Dijkstra {};
99 let path = dij.execute(get_node(0, &graph), get_node(4, &graph), &graph);
100
101
102 assert_eq!(6.0, calc_cost(&path.edges));
103 assert_eq!(3, path.edges.len());
104}
105
106#[test]
107fn should_find_path_with_disjoint_graphs() {
108 let graph = disjoint_graph();
109
110 let dij = Dijkstra {};
111 let path = dij.execute(get_node(0, &graph), get_node(3, &graph), &graph);
112
113 assert_eq!(0.0, calc_cost(&path.edges));
114 assert_eq!(0, path.edges.len());
115}
116
117#[cfg(test)]
118fn graph() -> Graph {
119 return Graph::from(Vec::from([
120 Edge::from(0, 0, 1, 4.0),
121 Edge::from(1, 0, 2, 2.0),
122 Edge::from(2, 1, 2, 3.0),
123 Edge::from(3, 1, 3, 2.0),
124 Edge::from(4, 1, 4, 3.0),
125 Edge::from(5, 2, 1, 1.0),
126 Edge::from(6, 2, 3, 4.0),
127 Edge::from(7, 2, 4, 5.0),
128 Edge::from(8, 4, 3, 1.0)
129 ]));
130}
131
132#[cfg(test)]
133fn disjoint_graph() -> Graph {
134 return Graph::from(Vec::from([
135 Edge::from(0, 0, 1, 4.0),
136 Edge::from(1, 2, 3, 2.0),
137 ]));
138}
139
140#[cfg(test)]
141fn get_node(id: usize, graph: &Graph) -> Node {
142 return graph.nodes_lookup.get(&id).unwrap().clone();
143}
144
145#[cfg(test)]
146fn calc_cost(edges: &Vec<Edge>) -> f32 {
147 let mut total_cost: f32 = 0.0;
148 for edge in edges {
149 total_cost += edge.weight;
150 }
151
152 return total_cost;
153}