map_model/pathfind/
engine.rs

1use std::cell::RefCell;
2use std::collections::HashMap;
3
4use fast_paths::{deserialize_32, serialize_32, FastGraph, InputGraph, PathCalculator};
5use petgraph::graph::{DiGraph, NodeIndex};
6use serde::{Deserialize, Serialize};
7use thread_local::ThreadLocal;
8
9/// This operates on raw IDs and costs; no type safety. The thing containing this transforms
10/// to/from higher-level types.
11#[allow(clippy::large_enum_variant)]
12#[derive(Serialize, Deserialize)]
13pub enum PathfindEngine {
14    Empty,
15    Dijkstra {
16        graph: DiGraph<usize, usize>,
17    },
18    CH {
19        #[serde(serialize_with = "serialize_32", deserialize_with = "deserialize_32")]
20        graph: FastGraph,
21        #[serde(skip_serializing, skip_deserializing)]
22        path_calc: ThreadLocal<RefCell<PathCalculator>>,
23    },
24}
25
26// Implemented manually to deal with the ThreadLocal
27impl Clone for PathfindEngine {
28    fn clone(&self) -> Self {
29        match self {
30            PathfindEngine::Empty => PathfindEngine::Empty,
31            PathfindEngine::Dijkstra { ref graph } => PathfindEngine::Dijkstra {
32                graph: graph.clone(),
33            },
34            PathfindEngine::CH { ref graph, .. } => PathfindEngine::CH {
35                graph: graph.clone(),
36                path_calc: ThreadLocal::new(),
37            },
38        }
39    }
40}
41
42impl PathfindEngine {
43    /// Returns (path cost, node IDs in path)
44    pub fn calculate_path(&self, start: usize, end: usize) -> Option<(usize, Vec<usize>)> {
45        self.calculate_path_multiple_sources_and_targets(vec![(start, 0)], vec![(end, 0)])
46    }
47
48    /// Returns (path cost, node IDs in path). Input is pairs of (node ID, extra weight)
49    pub fn calculate_path_multiple_sources_and_targets(
50        &self,
51        starts: Vec<(usize, usize)>,
52        ends: Vec<(usize, usize)>,
53    ) -> Option<(usize, Vec<usize>)> {
54        match self {
55            PathfindEngine::Empty => unreachable!(),
56            PathfindEngine::Dijkstra { ref graph } => {
57                // If there are multiple starts and ends, calculate each individual path and take
58                // the lowest cost.
59                let mut best_pair: Option<(usize, Vec<NodeIndex>)> = None;
60                for (start_node, weight1) in starts {
61                    let start_node = NodeIndex::new(start_node);
62                    for (end_node, weight2) in &ends {
63                        let end_node = NodeIndex::new(*end_node);
64                        if let Some((raw_weight, raw_nodes)) = petgraph::algo::astar(
65                            graph,
66                            start_node,
67                            |node| node == end_node,
68                            |edge| *edge.weight(),
69                            |_| 0,
70                        ) {
71                            let total_weight = raw_weight + weight1 + weight2;
72                            if best_pair
73                                .as_ref()
74                                .map(|pair| total_weight < pair.0)
75                                .unwrap_or(true)
76                            {
77                                best_pair = Some((total_weight, raw_nodes));
78                            }
79                        }
80                    }
81                }
82                let (raw_weight, raw_nodes) = best_pair?;
83                Some((
84                    raw_weight,
85                    raw_nodes.into_iter().map(|n| n.index()).collect(),
86                ))
87            }
88            PathfindEngine::CH {
89                ref graph,
90                ref path_calc,
91            } => {
92                let mut calc = path_calc
93                    .get_or(|| RefCell::new(fast_paths::create_calculator(graph)))
94                    .borrow_mut();
95                let path = calc.calc_path_multiple_sources_and_targets(graph, starts, ends)?;
96                // TODO Add an into_nodes to avoid this clone
97                Some((path.get_weight(), path.get_nodes().to_vec()))
98            }
99        }
100    }
101
102    pub fn reuse_ordering(&self) -> CreateEngine {
103        match self {
104            PathfindEngine::Empty => unreachable!(),
105            // Just don't reuse the ordering
106            PathfindEngine::Dijkstra { .. } => CreateEngine::Dijkstra,
107            PathfindEngine::CH { ref graph, .. } => CreateEngine::CHSeedingNodeOrdering(graph),
108        }
109    }
110
111    pub fn is_dijkstra(&self) -> bool {
112        matches!(self, PathfindEngine::Dijkstra { .. })
113    }
114
115    pub fn all_costs_from(&self, start: usize) -> HashMap<usize, usize> {
116        match self {
117            PathfindEngine::Empty => unreachable!(),
118            PathfindEngine::Dijkstra { ref graph } => {
119                petgraph::algo::dijkstra(graph, NodeIndex::new(start), None, |edge| *edge.weight())
120                    .into_iter()
121                    .map(|(k, v)| (k.index(), v))
122                    .collect()
123            }
124            PathfindEngine::CH { .. } => unreachable!(),
125        }
126    }
127}
128
129pub enum CreateEngine<'a> {
130    Dijkstra,
131    CH,
132    CHSeedingNodeOrdering(&'a FastGraph),
133}
134
135impl<'a> CreateEngine<'a> {
136    pub fn create(&self, input_graph: InputGraph) -> PathfindEngine {
137        match self {
138            CreateEngine::Dijkstra => {
139                let mut graph = DiGraph::new();
140                let dummy_weight = 42;
141                for node in 0..input_graph.get_num_nodes() {
142                    assert_eq!(graph.add_node(dummy_weight).index(), node);
143                }
144                for edge in input_graph.get_edges() {
145                    graph.add_edge(
146                        NodeIndex::new(edge.from),
147                        NodeIndex::new(edge.to),
148                        edge.weight,
149                    );
150                }
151                PathfindEngine::Dijkstra { graph }
152            }
153            CreateEngine::CH => {
154                info!(
155                    "Contraction hierarchy input graph has {} nodes",
156                    abstutil::prettyprint_usize(input_graph.get_num_nodes())
157                );
158
159                PathfindEngine::CH {
160                    graph: fast_paths::prepare_with_params(
161                        &input_graph,
162                        // see discussion about fast_paths parameters here: https://github.com/easbar/fast_paths/pull/37
163                        &fast_paths::Params::new(0.01, 100, 10, 100),
164                    ),
165                    path_calc: ThreadLocal::new(),
166                }
167            }
168            CreateEngine::CHSeedingNodeOrdering(prev_graph) => {
169                let node_ordering = prev_graph.get_node_ordering();
170                let graph = fast_paths::prepare_with_order_with_params(
171                    &input_graph,
172                    &node_ordering,
173                    &fast_paths::ParamsWithOrder::new(100),
174                )
175                .unwrap();
176                PathfindEngine::CH {
177                    graph,
178                    path_calc: ThreadLocal::new(),
179                }
180            }
181        }
182    }
183}