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
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
//! Pathfinding for cars, bikes, buses, and trains using contraction hierarchies

use std::cell::RefCell;

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

use abstutil::MultiMap;
use geom::{Duration, Speed};

use crate::pathfind::ch::round;
use crate::pathfind::node_map::{deserialize_nodemap, NodeMap};
use crate::pathfind::uber_turns::{IntersectionCluster, UberTurn};
use crate::pathfind::zone_cost;
use crate::{
    DrivingSide, Lane, LaneID, Map, Path, PathConstraints, PathRequest, PathStep, RoutingParams,
    Turn, TurnID, TurnType,
};

#[derive(Serialize, Deserialize)]
pub struct VehiclePathfinder {
    #[serde(serialize_with = "serialize_32", deserialize_with = "deserialize_32")]
    graph: FastGraph,
    #[serde(deserialize_with = "deserialize_nodemap")]
    nodes: NodeMap<Node>,
    uber_turns: Vec<UberTurn>,
    constraints: PathConstraints,

    #[serde(skip_serializing, skip_deserializing)]
    path_calc: ThreadLocal<RefCell<PathCalculator>>,
}

#[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Debug, Serialize, Deserialize)]
enum Node {
    Lane(LaneID),
    UberTurn(usize),
}

impl VehiclePathfinder {
    pub fn new(
        map: &Map,
        constraints: PathConstraints,
        seed: Option<&VehiclePathfinder>,
    ) -> VehiclePathfinder {
        // Insert every lane as a node. Even if the lane type is wrong now, it might change later,
        // and we want the node in the graph. Do this first, so the IDs of all the nodes doesn't
        // depend on lane types and turns and such.
        let mut nodes = NodeMap::new();
        for l in map.all_lanes() {
            nodes.get_or_insert(Node::Lane(l.id));
        }

        // Find all uber-turns and make a node for them too.
        let mut uber_turns = Vec::new();
        for ic in IntersectionCluster::find_all(map) {
            for ut in ic.uber_turns {
                nodes.get_or_insert(Node::UberTurn(uber_turns.len()));
                uber_turns.push(ut);
            }
        }

        let input_graph = make_input_graph(map, &nodes, &uber_turns, constraints);

        // All VehiclePathfinders have the same nodes (lanes), so if we're not the first being
        // built, seed from the node ordering.
        let graph = if let Some(seed) = seed {
            let node_ordering = seed.graph.get_node_ordering();
            fast_paths::prepare_with_order(&input_graph, &node_ordering).unwrap()
        } else {
            fast_paths::prepare(&input_graph)
        };

        VehiclePathfinder {
            graph,
            nodes,
            uber_turns,
            constraints,
            path_calc: ThreadLocal::new(),
        }
    }

    pub fn pathfind(&self, req: &PathRequest, map: &Map) -> Option<(Path, usize)> {
        assert!(!map.get_l(req.start.lane()).is_walkable());
        let mut calc = self
            .path_calc
            .get_or(|| RefCell::new(fast_paths::create_calculator(&self.graph)))
            .borrow_mut();
        let raw_path = calc.calc_path(
            &self.graph,
            self.nodes.get(Node::Lane(req.start.lane())),
            self.nodes.get(Node::Lane(req.end.lane())),
        )?;
        let mut steps = Vec::new();
        let mut uber_turns = Vec::new();
        for pair in self.nodes.translate(&raw_path).windows(2) {
            match (pair[0], pair[1]) {
                (Node::Lane(l1), Node::Lane(l2)) => {
                    steps.push(PathStep::Lane(l1));
                    // We don't need to look for this turn in the map; we know it exists.
                    steps.push(PathStep::Turn(TurnID {
                        parent: map.get_l(l1).dst_i,
                        src: l1,
                        dst: l2,
                    }));
                }
                (Node::Lane(l), Node::UberTurn(ut)) => {
                    steps.push(PathStep::Lane(l));
                    let ut = self.uber_turns[ut].clone();
                    for t in &ut.path {
                        steps.push(PathStep::Turn(*t));
                        steps.push(PathStep::Lane(t.dst));
                    }
                    steps.pop();
                    uber_turns.push(ut);
                }
                (Node::UberTurn(_), Node::Lane(_)) => {
                    // Don't add anything; the lane will be added by some other case
                }
                (Node::UberTurn(_), Node::UberTurn(_)) => unreachable!(),
            }
        }
        steps.push(PathStep::Lane(req.end.lane()));
        Some((
            Path::new(map, steps, req.clone(), uber_turns),
            raw_path.get_weight(),
        ))
    }

    pub fn apply_edits(&mut self, map: &Map) {
        // The NodeMap is just all lanes and uber-turns -- it won't change. So we can also reuse
        // the node ordering.
        // TODO Make sure the result of this is deterministic and equivalent to computing from
        // scratch.
        let input_graph = make_input_graph(map, &self.nodes, &self.uber_turns, self.constraints);
        let node_ordering = self.graph.get_node_ordering();
        self.graph = fast_paths::prepare_with_order(&input_graph, &node_ordering).unwrap();
    }
}

fn make_input_graph(
    map: &Map,
    nodes: &NodeMap<Node>,
    uber_turns: &Vec<UberTurn>,
    constraints: PathConstraints,
) -> InputGraph {
    let mut input_graph = InputGraph::new();

    // From some lanes, instead of adding edges to turns, add edges to these (indexed) uber-turns.
    let mut uber_turn_entrances: MultiMap<LaneID, usize> = MultiMap::new();
    for (idx, ut) in uber_turns.iter().enumerate() {
        // Force the nodes to always match up in the graph for different vehicle types.
        nodes.get(Node::UberTurn(idx));

        // But actually, make sure this uber-turn only contains lanes that can be used by this
        // vehicle.
        // TODO Need to test editing lanes inside an IntersectionCluster very carefully. See Mercer
        // and Dexter.
        if ut
            .path
            .iter()
            .all(|t| constraints.can_use(map.get_l(t.dst), map))
        {
            uber_turn_entrances.insert(ut.entry(), idx);
        }
    }

    let num_lanes = map.all_lanes().len();
    let mut used_last_uber_turn = false;
    for l in map.all_lanes() {
        let from = nodes.get(Node::Lane(l.id));
        let mut any = false;
        if constraints.can_use(l, map) {
            let indices = uber_turn_entrances.get(l.id);
            if indices.is_empty() {
                for turn in map.get_turns_for(l.id, constraints) {
                    any = true;
                    input_graph.add_edge(
                        from,
                        nodes.get(Node::Lane(turn.id.dst)),
                        round(
                            vehicle_cost(l, turn, constraints, map.routing_params(), map)
                                + zone_cost(turn, constraints, map),
                        ),
                    );
                }
            } else {
                for idx in indices {
                    any = true;
                    let ut = &uber_turns[*idx];

                    let mut sum_cost = Duration::ZERO;
                    for t in &ut.path {
                        let turn = map.get_t(*t);
                        sum_cost += vehicle_cost(
                            map.get_l(t.src),
                            turn,
                            constraints,
                            map.routing_params(),
                            map,
                        ) + zone_cost(turn, constraints, map);
                    }
                    input_graph.add_edge(from, nodes.get(Node::UberTurn(*idx)), round(sum_cost));
                    input_graph.add_edge(
                        nodes.get(Node::UberTurn(*idx)),
                        nodes.get(Node::Lane(ut.exit())),
                        // The cost is already captured for entering the uber-turn
                        1,
                    );
                    if *idx == uber_turns.len() - 1 {
                        used_last_uber_turn = true;
                    }
                }
            }
        }
        // The nodes in the graph MUST exactly be all of the lanes, so we can reuse node ordering
        // later. If the last lane doesn't have any edges, then this won't work -- fast_paths trims
        // out unused nodes at the end. So pretend like it points to some arbitrary other node.
        // Since no paths will start from this unused node, this won't affect results.
        // TODO Upstream a method in InputGraph to do this more clearly.
        if !any && l.id.0 == num_lanes - 1 {
            input_graph.add_edge(from, nodes.get(Node::Lane(LaneID(0))), 1);
        }
    }

    // Same as the hack above for unused lanes
    if !used_last_uber_turn && !uber_turns.is_empty() {
        input_graph.add_edge(
            nodes.get(Node::UberTurn(uber_turns.len() - 1)),
            nodes.get(Node::UberTurn(0)),
            1,
        );
    }

    input_graph.freeze();
    input_graph
}

/// This returns the pathfinding cost of crossing one lane and turn. This is also expressed in
/// units of time. It factors in the ideal time to cross the space, along with penalties for
/// entering an access-restricted zone, taking an unprotected turn, and so on.
pub fn vehicle_cost(
    lane: &Lane,
    turn: &Turn,
    constraints: PathConstraints,
    params: &RoutingParams,
    map: &Map,
) -> Duration {
    let base = match constraints {
        PathConstraints::Car | PathConstraints::Train => {
            let t1 = lane.length() / map.get_r(lane.parent).speed_limit;
            let t2 = turn.geom.length() / map.get_parent(turn.id.dst).speed_limit;
            t1 + t2
        }
        PathConstraints::Bike => {
            // TODO Copied from sim. Probably move to map_model.
            let max_bike_speed = Speed::miles_per_hour(10.0);
            // Usually the bike's speed limit matters, not the road's.
            let t1 = lane.length() / map.get_r(lane.parent).speed_limit.min(max_bike_speed);
            let t2 =
                turn.geom.length() / map.get_parent(turn.id.dst).speed_limit.min(max_bike_speed);

            // TODO Elevation gain is bad, loss is good.
            // TODO If we're on a driving lane, higher speed limit is worse.
            // TODO Bike lanes next to parking is dangerous.

            // TODO Prefer bike lanes, then bus lanes, then driving lanes. For now, express that by
            // multiplying the base cost.
            let lt_penalty = if lane.is_biking() {
                params.bike_lane_penalty
            } else if lane.is_bus() {
                params.bus_lane_penalty
            } else {
                assert!(lane.is_driving());
                params.driving_lane_penalty
            };

            lt_penalty * (t1 + t2)
        }
        PathConstraints::Bus => {
            // Like Car, but prefer bus lanes.
            let t1 = lane.length() / map.get_r(lane.parent).speed_limit;
            let t2 = turn.geom.length() / map.get_parent(turn.id.dst).speed_limit;
            let lt_penalty = if lane.is_bus() {
                1.0
            } else {
                assert!(lane.is_driving());
                1.1
            };
            lt_penalty * (t1 + t2)
        }
        PathConstraints::Pedestrian => unreachable!(),
    };

    // Penalize unprotected turns at a stop sign from smaller to larger roads.
    let unprotected_turn_type = if map.get_config().driving_side == DrivingSide::Right {
        TurnType::Left
    } else {
        TurnType::Right
    };
    let rank_from = map.get_r(lane.parent).get_detailed_rank();
    let rank_to = map.get_parent(turn.id.dst).get_detailed_rank();
    let base = if turn.turn_type == unprotected_turn_type
        && rank_from < rank_to
        && map.get_i(turn.id.parent).is_stop_sign()
    {
        base + params.unprotected_turn_penalty
    } else {
        base
    };

    // Normally opportunistic lane-changing adjusts the path live, but that doesn't work near
    // uber-turns. So still use some of the penalties here.
    let (lt, lc, slow_lane) = turn.penalty(map);
    // TODO Since these costs wind up mattering most for particular lane choice, I guess just
    // adding is reasonable?
    let mut extra_penalty = lt + lc;
    if constraints == PathConstraints::Bike {
        extra_penalty = slow_lane;
    }
    // TODO These are small integers, just treat them as seconds for now to micro-adjust the
    // specific choice of lane.

    base + Duration::seconds(extra_penalty as f64)
}