1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174

use std::cell::RefCell;
use std::collections::HashMap;

use fast_paths::{deserialize_32, serialize_32, FastGraph, InputGraph, PathCalculator};
use petgraph::graph::{DiGraph, NodeIndex};
use serde::{Deserialize, Serialize};
use thread_local::ThreadLocal;

/// This operates on raw IDs and costs; no type safety. The thing containing this transforms
/// to/from higher-level types.
#[allow(clippy::large_enum_variant)]
#[derive(Serialize, Deserialize)]
pub enum PathfindEngine {
    Empty,
    Dijkstra {
        graph: DiGraph<usize, usize>,
    },
    CH {
        #[serde(serialize_with = "serialize_32", deserialize_with = "deserialize_32")]
        graph: FastGraph,
        #[serde(skip_serializing, skip_deserializing)]
        path_calc: ThreadLocal<RefCell<PathCalculator>>,
    },
}

// Implemented manually to deal with the ThreadLocal
impl Clone for PathfindEngine {
    fn clone(&self) -> Self {
        match self {
            PathfindEngine::Empty => PathfindEngine::Empty,
            PathfindEngine::Dijkstra { ref graph } => PathfindEngine::Dijkstra {
                graph: graph.clone(),
            },
            PathfindEngine::CH { ref graph, .. } => PathfindEngine::CH {
                graph: graph.clone(),
                path_calc: ThreadLocal::new(),
            },
        }
    }
}

impl PathfindEngine {
    /// Returns (path cost, node IDs in path)
    pub fn calculate_path(&self, start: usize, end: usize) -> Option<(usize, Vec<usize>)> {
        self.calculate_path_multiple_sources_and_targets(vec![(start, 0)], vec![(end, 0)])
    }

    /// Returns (path cost, node IDs in path). Input is pairs of (node ID, extra weight)
    pub fn calculate_path_multiple_sources_and_targets(
        &self,
        starts: Vec<(usize, usize)>,
        ends: Vec<(usize, usize)>,
    ) -> Option<(usize, Vec<usize>)> {
        match self {
            PathfindEngine::Empty => unreachable!(),
            PathfindEngine::Dijkstra { ref graph } => {
                // If there are multiple starts and ends, calculate each individual path and take
                // the lowest cost.
                let mut best_pair: Option<(usize, Vec<NodeIndex>)> = None;
                for (start_node, weight1) in starts {
                    let start_node = NodeIndex::new(start_node);
                    for (end_node, weight2) in &ends {
                        let end_node = NodeIndex::new(*end_node);
                        if let Some((raw_weight, raw_nodes)) = petgraph::algo::astar(
                            graph,
                            start_node,
                            |node| node == end_node,
                            |edge| *edge.weight(),
                            |_| 0,
                        ) {
                            let total_weight = raw_weight + weight1 + weight2;
                            if best_pair
                                .as_ref()
                                .map(|pair| total_weight < pair.0)
                                .unwrap_or(true)
                            {
                                best_pair = Some((total_weight, raw_nodes));
                            }
                        }
                    }
                }
                let (raw_weight, raw_nodes) = best_pair?;
                Some((
                    raw_weight,
                    raw_nodes.into_iter().map(|n| n.index()).collect(),
                ))
            }
            PathfindEngine::CH {
                ref graph,
                ref path_calc,
            } => {
                let mut calc = path_calc
                    .get_or(|| RefCell::new(fast_paths::create_calculator(graph)))
                    .borrow_mut();
                let path = calc.calc_path_multiple_sources_and_targets(graph, starts, ends)?;
                // TODO Add an into_nodes to avoid this clone
                Some((path.get_weight(), path.get_nodes().to_vec()))
            }
        }
    }

    pub fn reuse_ordering(&self) -> CreateEngine {
        match self {
            PathfindEngine::Empty => unreachable!(),
            // Just don't reuse the ordering
            PathfindEngine::Dijkstra { .. } => CreateEngine::Dijkstra,
            PathfindEngine::CH { ref graph, .. } => CreateEngine::CHSeedingNodeOrdering(graph),
        }
    }

    pub fn is_dijkstra(&self) -> bool {
        matches!(self, PathfindEngine::Dijkstra { .. })
    }

    pub fn all_costs_from(&self, start: usize) -> HashMap<usize, usize> {
        match self {
            PathfindEngine::Empty => unreachable!(),
            PathfindEngine::Dijkstra { ref graph } => {
                petgraph::algo::dijkstra(graph, NodeIndex::new(start), None, |edge| *edge.weight())
                    .into_iter()
                    .map(|(k, v)| (k.index(), v))
                    .collect()
            }
            PathfindEngine::CH { .. } => unreachable!(),
        }
    }
}

pub enum CreateEngine<'a> {
    Dijkstra,
    CH,
    CHSeedingNodeOrdering(&'a FastGraph),
}

impl<'a> CreateEngine<'a> {
    pub fn create(&self, input_graph: InputGraph) -> PathfindEngine {
        match self {
            CreateEngine::Dijkstra => {
                let mut graph = DiGraph::new();
                let dummy_weight = 42;
                for node in 0..input_graph.get_num_nodes() {
                    assert_eq!(graph.add_node(dummy_weight).index(), node);
                }
                for edge in input_graph.get_edges() {
                    graph.add_edge(
                        NodeIndex::new(edge.from),
                        NodeIndex::new(edge.to),
                        edge.weight,
                    );
                }
                PathfindEngine::Dijkstra { graph }
            }
            CreateEngine::CH => {
                info!(
                    "Contraction hierarchy input graph has {} nodes",
                    abstutil::prettyprint_usize(input_graph.get_num_nodes())
                );

                PathfindEngine::CH {
                    graph: fast_paths::prepare(&input_graph),
                    path_calc: ThreadLocal::new(),
                }
            }
            CreateEngine::CHSeedingNodeOrdering(prev_graph) => {
                let node_ordering = prev_graph.get_node_ordering();
                let graph = fast_paths::prepare_with_order(&input_graph, &node_ordering).unwrap();
                PathfindEngine::CH {
                    graph,
                    path_calc: ThreadLocal::new(),
                }
            }
        }
    }
}