path_finding_lib/search/
dijkstra.rs

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}