From 90bc73580ddcd4b78ed15ab55bdfe2ff210253e2 Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Tue, 19 May 2020 14:03:01 -0700 Subject: [PATCH] the 'has the coffee really kicked in yet' change: make the contraction hierarchy prevent illegal uber-turns. probably doesnt properly handle changing lane types inside a cluster yet. --- data/MANIFEST.txt | 24 ++--- game/src/sandbox/uber_turns.rs | 8 +- map_model/src/map.rs | 4 - map_model/src/pathfind/driving.rs | 135 +++++++++++++++++++++------ map_model/src/pathfind/mod.rs | 4 +- map_model/src/pathfind/uber_turns.rs | 60 ++++++------ 6 files changed, 158 insertions(+), 77 deletions(-) diff --git a/data/MANIFEST.txt b/data/MANIFEST.txt index 4cd02369ae..0685b43e71 100644 --- a/data/MANIFEST.txt +++ b/data/MANIFEST.txt @@ -184,22 +184,22 @@ data/input/seattle/popdat.bin,8b88fa7f2036277d2f26ad393f6509b9,https://www.dropb data/input/seattle/sidewalks.bin,034dd47ab77902dbc81c0107f13d8965,https://www.dropbox.com/s/ma9bmisijc7v7xa/sidewalks.bin.zip?dl=0 data/input/seattle/sidewalks.kml,94d385ba03ef1b57a5ba10965913ec6c,https://www.dropbox.com/s/vn8amar9xi6vbvh/sidewalks.kml.zip?dl=0 data/input/seattle/trips_2014.csv,d4a8e733045b28c0385fb81359d6df03,https://www.dropbox.com/s/5ppravwmk6bf20d/trips_2014.csv.zip?dl=0 -data/system/maps/23rd.bin,5d0d039fd4330e2308ae4bb9983b0d7b,https://www.dropbox.com/s/wjl45codk6rqfg4/23rd.bin.zip?dl=0 -data/system/maps/ballard.bin,17102a34e17ef941a8d4c461b043cbf5,https://www.dropbox.com/s/u4rvz50she3yrk0/ballard.bin.zip?dl=0 -data/system/maps/caphill.bin,46f08feb1a032dd5366fc1613f428841,https://www.dropbox.com/s/bh20pn3wxygetw8/caphill.bin.zip?dl=0 -data/system/maps/downtown.bin,3baf70c7bd1944662e3a56a999d7f23c,https://www.dropbox.com/s/4do5cg4vc17lafo/downtown.bin.zip?dl=0 +data/system/maps/23rd.bin,4e726219b23a105037415e2c92eee824,https://www.dropbox.com/s/wjl45codk6rqfg4/23rd.bin.zip?dl=0 +data/system/maps/ballard.bin,b66db3fcf6f4043ed1e7edfb9d380bf1,https://www.dropbox.com/s/u4rvz50she3yrk0/ballard.bin.zip?dl=0 +data/system/maps/caphill.bin,7d77357c0e5b1bc1b3ac5132890f0818,https://www.dropbox.com/s/bh20pn3wxygetw8/caphill.bin.zip?dl=0 +data/system/maps/downtown.bin,96892058c01319e5e2f892404a2ee6b6,https://www.dropbox.com/s/4do5cg4vc17lafo/downtown.bin.zip?dl=0 data/system/maps/downtown_atx.bin,b2d634b8aa1244261d3a5e3a6e8b8ab7,https://www.dropbox.com/s/5avnbkd4oxby2hs/downtown_atx.bin.zip?dl=0 data/system/maps/huge_austin.bin,1993956d19e43b1d27d8417435820f23,https://www.dropbox.com/s/khy0m6v9yt0gjnt/huge_austin.bin.zip?dl=0 -data/system/maps/huge_seattle.bin,7e932f8a3650c8670fda511b1c0fff31,https://www.dropbox.com/s/btvr3qajshnivhb/huge_seattle.bin.zip?dl=0 -data/system/maps/intl_district.bin,268083962eabb667aae50213632ea098,https://www.dropbox.com/s/fohppni52ekc5l3/intl_district.bin.zip?dl=0 -data/system/maps/lakeslice.bin,6aca01bb2ce29de57e0759eff182a2bb,https://www.dropbox.com/s/99zi0gcbyvqrkud/lakeslice.bin.zip?dl=0 -data/system/maps/montlake.bin,88fdf4673e94cb25cbf131a41ec2e9c9,https://www.dropbox.com/s/zvhm2j5lavixxcr/montlake.bin.zip?dl=0 -data/system/maps/mt_baker.bin,3e6fbfcb5267455a86175d67c62b3cb4,https://www.dropbox.com/s/cetje663p04cbgp/mt_baker.bin.zip?dl=0 -data/system/maps/udistrict.bin,8050fb96058a0c707e1cd7574bae5158,https://www.dropbox.com/s/zqt2je8fadssz5j/udistrict.bin.zip?dl=0 -data/system/maps/west_seattle.bin,980cd951a689e38a4b48b055c71e0145,https://www.dropbox.com/s/5pp1ik9l40yj3wh/west_seattle.bin.zip?dl=0 +data/system/maps/huge_seattle.bin,f55c0dea20db6416af06df411a9af7dd,https://www.dropbox.com/s/btvr3qajshnivhb/huge_seattle.bin.zip?dl=0 +data/system/maps/intl_district.bin,56624cae54178792df4a179aa5d9ab42,https://www.dropbox.com/s/fohppni52ekc5l3/intl_district.bin.zip?dl=0 +data/system/maps/lakeslice.bin,912c305846162eb15c588f77dd1a939a,https://www.dropbox.com/s/99zi0gcbyvqrkud/lakeslice.bin.zip?dl=0 +data/system/maps/montlake.bin,a0f71eea156b92493f82be959a476047,https://www.dropbox.com/s/zvhm2j5lavixxcr/montlake.bin.zip?dl=0 +data/system/maps/mt_baker.bin,d19b1621ba5b85ec9b5334a7c4535a2a,https://www.dropbox.com/s/cetje663p04cbgp/mt_baker.bin.zip?dl=0 +data/system/maps/udistrict.bin,4bc03c72ab3ffa4edd1dd2b6e0cb3765,https://www.dropbox.com/s/zqt2je8fadssz5j/udistrict.bin.zip?dl=0 +data/system/maps/west_seattle.bin,c9aaa8e2d30f47e060403090e6622750,https://www.dropbox.com/s/5pp1ik9l40yj3wh/west_seattle.bin.zip?dl=0 data/system/prebaked_results/lakeslice/weekday.bin,871031678281975b30a83dd61ca184c3,https://www.dropbox.com/s/1c1sohvy50263wg/weekday.bin.zip?dl=0 data/system/prebaked_results/montlake/car vs bike contention.bin,fda2f60658aeebba1145151866ea41dc,https://www.dropbox.com/s/jefg0ikjy9dsrdd/car%20vs%20bike%20contention.bin.zip?dl=0 -data/system/prebaked_results/montlake/weekday.bin,81bfca5bad697583de7787f5d18f1a99,https://www.dropbox.com/s/1aq7n9ow8tfqb5d/weekday.bin.zip?dl=0 +data/system/prebaked_results/montlake/weekday.bin,bf9ef0af965da30ac1e101b8e67ae882,https://www.dropbox.com/s/1aq7n9ow8tfqb5d/weekday.bin.zip?dl=0 data/system/scenarios/23rd/weekday.bin,b42a0c7446f33b4de32df097b1a59c8f,https://www.dropbox.com/s/tgo7gztfodbljyi/weekday.bin.zip?dl=0 data/system/scenarios/ballard/weekday.bin,6f3ebddac29e7d84ffe76c8efc073a97,https://www.dropbox.com/s/67hys1v7m7oe979/weekday.bin.zip?dl=0 data/system/scenarios/caphill/weekday.bin,e9dda6bebdde82e92b9d467e22fa2855,https://www.dropbox.com/s/9ifg7ogd6p5f1qg/weekday.bin.zip?dl=0 diff --git a/game/src/sandbox/uber_turns.rs b/game/src/sandbox/uber_turns.rs index 5bb83029ab..43ec9a1983 100644 --- a/game/src/sandbox/uber_turns.rs +++ b/game/src/sandbox/uber_turns.rs @@ -10,16 +10,16 @@ use ezgui::{ use geom::{ArrowCap, Polygon}; use map_model::{IntersectionCluster, IntersectionID}; use sim::DontDrawAgents; -use std::collections::HashSet; +use std::collections::BTreeSet; pub struct UberTurnPicker { - members: HashSet, + members: BTreeSet, composite: Composite, } impl UberTurnPicker { pub fn new(ctx: &mut EventCtx, app: &App, i: IntersectionID) -> Box { - let mut members = HashSet::new(); + let mut members = BTreeSet::new(); members.insert(i); Box::new(UberTurnPicker { members, @@ -116,7 +116,7 @@ impl UberTurnViewer { pub fn new( ctx: &mut EventCtx, app: &mut App, - members: HashSet, + members: BTreeSet, idx: usize, legal_turns: bool, ) -> Box { diff --git a/map_model/src/map.rs b/map_model/src/map.rs index 4cafbf858a..ca56ed4f79 100644 --- a/map_model/src/map.rs +++ b/map_model/src/map.rs @@ -90,10 +90,6 @@ impl Map { ); } - if false { - crate::pathfind::uber_turns::find(&map); - } - return map; } Err(err) => { diff --git a/map_model/src/pathfind/driving.rs b/map_model/src/pathfind/driving.rs index ad0976a5bd..00f145fba3 100644 --- a/map_model/src/pathfind/driving.rs +++ b/map_model/src/pathfind/driving.rs @@ -1,5 +1,7 @@ use crate::pathfind::node_map::{deserialize_nodemap, NodeMap}; +use crate::pathfind::uber_turns::{IntersectionCluster, UberTurn}; use crate::{Lane, LaneID, Map, Path, PathConstraints, PathRequest, PathStep, Turn, TurnID}; +use abstutil::MultiMap; use fast_paths::{deserialize_32, serialize_32, FastGraph, InputGraph, PathCalculator}; use serde_derive::{Deserialize, Serialize}; use std::cell::RefCell; @@ -10,13 +12,20 @@ pub struct VehiclePathfinder { #[serde(serialize_with = "serialize_32", deserialize_with = "deserialize_32")] graph: FastGraph, #[serde(deserialize_with = "deserialize_nodemap")] - nodes: NodeMap, + nodes: NodeMap, + uber_turns: Vec, constraints: PathConstraints, #[serde(skip_serializing, skip_deserializing)] path_calc: ThreadLocal>, } +#[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Debug, Serialize, Deserialize)] +enum Node { + Lane(LaneID), + UberTurn(usize), +} + impl VehiclePathfinder { pub fn new( map: &Map, @@ -28,9 +37,19 @@ impl VehiclePathfinder { // depend on lane types and turns and such. let mut nodes = NodeMap::new(); for l in map.all_lanes() { - nodes.get_or_insert(l.id); + nodes.get_or_insert(Node::Lane(l.id)); } - let input_graph = make_input_graph(map, &nodes, constraints); + + // 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. @@ -44,6 +63,7 @@ impl VehiclePathfinder { VehiclePathfinder { graph, nodes, + uber_turns, constraints, path_calc: ThreadLocal::new(), } @@ -57,18 +77,34 @@ impl VehiclePathfinder { .borrow_mut(); let raw_path = calc.calc_path( &self.graph, - self.nodes.get(req.start.lane()), - self.nodes.get(req.end.lane()), + self.nodes.get(Node::Lane(req.start.lane())), + self.nodes.get(Node::Lane(req.end.lane())), )?; let mut steps = Vec::new(); for pair in self.nodes.translate(&raw_path).windows(2) { - steps.push(PathStep::Lane(pair[0])); - // 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(pair[0]).dst_i, - src: pair[0], - dst: pair[1], - })); + 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)); + for t in &self.uber_turns[ut].path { + steps.push(PathStep::Turn(*t)); + steps.push(PathStep::Lane(t.dst)); + } + steps.pop(); + } + (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(( @@ -78,11 +114,11 @@ impl VehiclePathfinder { } pub fn apply_edits(&mut self, map: &Map) { - // The NodeMap is just all lanes -- it won't change. So we can also reuse the node - // ordering. + // 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.constraints); + 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(); } @@ -90,23 +126,70 @@ impl VehiclePathfinder { fn make_input_graph( map: &Map, - nodes: &NodeMap, + nodes: &NodeMap, + uber_turns: &Vec, 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 = MultiMap::new(); + for (idx, ut) in uber_turns.iter().enumerate() { + // 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.path[0].src, idx); + } else { + // Similar to the hack below for unused lanes + if idx == uber_turns.len() - 1 { + input_graph.add_edge( + nodes.get(Node::UberTurn(idx)), + nodes.get(Node::UberTurn(0)), + 1, + ); + } + } + } + let num_lanes = map.all_lanes().len(); for l in map.all_lanes() { - let from = nodes.get(l.id); + let from = nodes.get(Node::Lane(l.id)); let mut any = false; if constraints.can_use(l, map) { - for turn in map.get_turns_for(l.id, constraints) { - any = true; - input_graph.add_edge( - from, - nodes.get(turn.id.dst), - // Round up! 0 cost edges are ignored - cost(l, turn, constraints, map).max(1), - ); + 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 up! 0 cost edges are ignored + cost(l, turn, constraints, map).max(1), + ); + } + } else { + for idx in indices { + any = true; + let ut = &uber_turns[*idx]; + + let mut sum_cost = 0; + for t in &ut.path { + sum_cost += cost(map.get_l(t.src), map.get_t(*t), constraints, map); + } + input_graph.add_edge(from, nodes.get(Node::UberTurn(*idx)), sum_cost.max(1)); + input_graph.add_edge( + nodes.get(Node::UberTurn(*idx)), + nodes.get(Node::Lane(ut.path.last().unwrap().dst)), + // The cost is already captured for entering the uber-turn + 1, + ); + } } } // The nodes in the graph MUST exactly be all of the lanes, so we can reuse node @@ -115,7 +198,7 @@ fn make_input_graph( // 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(LaneID(0)), 1); + input_graph.add_edge(from, nodes.get(Node::Lane(LaneID(0))), 1); } } input_graph.freeze(); diff --git a/map_model/src/pathfind/mod.rs b/map_model/src/pathfind/mod.rs index 05a9ebcecd..913e2a1332 100644 --- a/map_model/src/pathfind/mod.rs +++ b/map_model/src/pathfind/mod.rs @@ -11,7 +11,7 @@ use crate::{ osm, BusRouteID, BusStopID, Lane, LaneID, LaneType, Map, Position, Traversable, TurnID, }; use abstutil::Timer; -use geom::{Distance, PolyLine}; +use geom::{Distance, PolyLine, EPSILON_DIST}; use serde_derive::{Deserialize, Serialize}; use std::collections::VecDeque; use std::fmt; @@ -420,7 +420,7 @@ fn validate_continuity(map: &Map, steps: &Vec) { PathStep::Turn(id) => map.get_t(id).geom.first_pt(), }; let len = from.dist_to(to); - if len > Distance::ZERO { + if len > EPSILON_DIST { println!("All steps in invalid path:"); for s in steps { match s { diff --git a/map_model/src/pathfind/uber_turns.rs b/map_model/src/pathfind/uber_turns.rs index 05b6783308..03a0a6c578 100644 --- a/map_model/src/pathfind/uber_turns.rs +++ b/map_model/src/pathfind/uber_turns.rs @@ -1,50 +1,52 @@ use crate::{IntersectionID, Map, TurnID}; use geom::PolyLine; use petgraph::graphmap::UnGraphMap; -use std::collections::{HashMap, HashSet}; +use serde_derive::{Deserialize, Serialize}; +use std::collections::{BTreeMap, BTreeSet}; // This only applies to VehiclePathfinder; walking through these intersections is nothing special. // TODO I haven't seen any cases yet with "interior" intersections. Some stuff might break. -#[derive(Clone)] +#[derive(Clone, Serialize, Deserialize)] pub struct IntersectionCluster { - pub members: HashSet, + pub members: BTreeSet, pub uber_turns: Vec, } -#[derive(Clone)] +#[derive(Clone, Serialize, Deserialize)] pub struct UberTurn { pub path: Vec, } -pub fn find(map: &Map) -> Vec { - let mut clusters = Vec::new(); - let mut graph: UnGraphMap = UnGraphMap::new(); - for from in map.all_roads() { - for (via, _) in &from.complicated_turn_restrictions { - // Each of these tells us 2 intersections to group together - let r = map.get_r(*via); - graph.add_edge(r.src_i, r.dst_i, ()); - } - } - for intersections in petgraph::algo::kosaraju_scc(&graph) { - let members: HashSet = intersections.iter().cloned().collect(); - // Discard the illegal movements - let (ic, _) = IntersectionCluster::new(members, map); - clusters.push(ic); - } - - clusters -} - impl IntersectionCluster { + // Based on turn restrictions + pub fn find_all(map: &Map) -> Vec { + let mut clusters = Vec::new(); + let mut graph: UnGraphMap = UnGraphMap::new(); + for from in map.all_roads() { + for (via, _) in &from.complicated_turn_restrictions { + // Each of these tells us 2 intersections to group together + let r = map.get_r(*via); + graph.add_edge(r.src_i, r.dst_i, ()); + } + } + for intersections in petgraph::algo::kosaraju_scc(&graph) { + let members: BTreeSet = intersections.iter().cloned().collect(); + // Discard the illegal movements + let (ic, _) = IntersectionCluster::new(members, map); + clusters.push(ic); + } + + clusters + } + // (legal, illegal) pub fn new( - members: HashSet, + members: BTreeSet, map: &Map, ) -> (IntersectionCluster, IntersectionCluster) { // Find all entrances and exits through this group of intersections let mut entrances = Vec::new(); - let mut exits = HashSet::new(); + let mut exits = BTreeSet::new(); for i in &members { for turn in map.get_turns_in_intersection(*i) { if turn.between_sidewalks() { @@ -108,13 +110,13 @@ impl IntersectionCluster { } } -fn flood(start: TurnID, map: &Map, exits: &HashSet) -> Vec { +fn flood(start: TurnID, map: &Map, exits: &BTreeSet) -> Vec { if exits.contains(&start) { return vec![UberTurn { path: vec![start] }]; } let mut results = Vec::new(); - let mut preds: HashMap = HashMap::new(); + let mut preds: BTreeMap = BTreeMap::new(); let mut queue = vec![start]; while !queue.is_empty() { @@ -137,7 +139,7 @@ fn flood(start: TurnID, map: &Map, exits: &HashSet) -> Vec { results } -fn trace_back(end: TurnID, preds: &HashMap) -> Vec { +fn trace_back(end: TurnID, preds: &BTreeMap) -> Vec { let mut path = vec![end]; let mut current = end; loop {