map_model/pathfind/
uber_turns.rs

1//! To deal with complicated intersections and short roads in OSM, cluster intersections close
2//! together and then calculate UberTurns that string together several turns.
3
4use std::collections::{BTreeMap, BTreeSet};
5
6use petgraph::graphmap::UnGraphMap;
7use serde::{Deserialize, Serialize};
8
9use geom::{Distance, PolyLine};
10
11use crate::{DirectedRoadID, IntersectionID, LaneID, Map, MovementID, PathConstraints, TurnID};
12
13/// This only applies to VehiclePathfinder; walking through these intersections is nothing special.
14/// And in fact, even lanes only for buses/bikes are ignored.
15// TODO I haven't seen any cases yet with "interior" intersections. Some stuff might break.
16pub struct IntersectionCluster {
17    pub members: BTreeSet<IntersectionID>,
18    pub uber_turns: Vec<UberTurn>,
19}
20
21#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
22pub struct UberTurn {
23    pub path: Vec<TurnID>,
24}
25
26impl IntersectionCluster {
27    pub fn find_all(map: &Map) -> Vec<IntersectionCluster> {
28        // First autodetect based on traffic signals close together.
29        let mut clusters = Vec::new();
30        let mut seen_intersections = BTreeSet::new();
31        for i in map.all_intersections() {
32            if i.is_traffic_signal() && !seen_intersections.contains(&i.id) {
33                if let Some(members) = IntersectionCluster::autodetect(i.id, map) {
34                    seen_intersections.extend(members.clone());
35                    // Discard any illegal movements
36                    clusters.push(IntersectionCluster::new(members, map).0);
37                }
38            }
39        }
40
41        // Then look for intersections with complicated turn restrictions.
42        let mut graph: UnGraphMap<IntersectionID, ()> = UnGraphMap::new();
43        for from in map.all_roads() {
44            for (via, _) in &from.complicated_turn_restrictions {
45                // Each of these tells us 2 intersections to group together
46                let r = map.get_r(*via);
47                graph.add_edge(r.src_i, r.dst_i, ());
48            }
49        }
50        for intersections in petgraph::algo::kosaraju_scc(&graph) {
51            let members: BTreeSet<IntersectionID> = intersections.iter().cloned().collect();
52            // Is there already a cluster covering everything?
53            if clusters.iter().any(|ic| ic.members.is_subset(&members)) {
54                continue;
55            }
56
57            // Do any existing clusters partly cover this one?
58            let mut existing: Vec<&mut IntersectionCluster> = clusters
59                .iter_mut()
60                .filter(|ic| ic.members.intersection(&members).next().is_some())
61                .collect();
62            // None? Just add a new one.
63            if existing.is_empty() {
64                clusters.push(IntersectionCluster::new(members, map).0);
65                continue;
66            }
67
68            if existing.len() == 1 {
69                // Amend this existing one.
70                let mut all_members = members;
71                all_members.extend(existing[0].members.clone());
72                *existing[0] = IntersectionCluster::new(all_members, map).0;
73                continue;
74            }
75
76            // TODO Saw this is New Orleans
77            println!(
78                "Need a cluster containing {:?} for turn restrictions, but there's more than one \
79                 existing cluster that partly covers it. Union them?",
80                members
81            );
82            return Vec::new();
83        }
84
85        clusters
86    }
87
88    /// (legal, illegal)
89    pub fn new(
90        members: BTreeSet<IntersectionID>,
91        map: &Map,
92    ) -> (IntersectionCluster, IntersectionCluster) {
93        // Find all entrances and exits through this group of intersections
94        let mut entrances = Vec::new();
95        let mut exits = BTreeSet::new();
96        for i in &members {
97            for turn in &map.get_i(*i).turns {
98                if turn.between_sidewalks() {
99                    continue;
100                }
101                let src = map.get_l(turn.id.src);
102                let dst = map.get_l(turn.id.dst);
103                if src.is_footway() || dst.is_footway() {
104                    continue;
105                }
106
107                if !members.contains(&src.src_i) {
108                    entrances.push(turn.id);
109                }
110                if !members.contains(&dst.dst_i) {
111                    exits.insert(turn.id);
112                }
113            }
114        }
115
116        // Find all paths between entrances and exits
117        let mut uber_turns = Vec::new();
118        for entrance in entrances {
119            uber_turns.extend(flood(entrance, map, &exits));
120        }
121
122        // Filter illegal paths
123        let mut all_restrictions = Vec::new();
124        for from in map.all_roads() {
125            for (via, to) in &from.complicated_turn_restrictions {
126                all_restrictions.push((from.id, *via, *to));
127            }
128        }
129
130        // Filter out the restricted ones!
131        let mut illegal = Vec::new();
132        uber_turns.retain(|ut| {
133            let mut ok = true;
134            for pair in ut.path.windows(2) {
135                let r1 = pair[0].src.road;
136                let r2 = pair[0].dst.road;
137                let r3 = pair[1].dst.road;
138                if all_restrictions.contains(&(r1, r2, r3)) {
139                    ok = false;
140                    break;
141                }
142            }
143            if ok {
144                true
145            } else {
146                // TODO There's surely a method in Vec to do partition like this
147                illegal.push(ut.clone());
148                false
149            }
150        });
151
152        (
153            IntersectionCluster {
154                members: members.clone(),
155                uber_turns,
156            },
157            IntersectionCluster {
158                members,
159                uber_turns: illegal,
160            },
161        )
162    }
163
164    /// Find all other traffic signals "close" to one. Ignore stop sign intersections in between.
165    pub fn autodetect(from: IntersectionID, map: &Map) -> Option<BTreeSet<IntersectionID>> {
166        if !map.get_i(from).is_traffic_signal() {
167            return None;
168        }
169        let threshold = Distance::meters(25.0);
170
171        let mut found = BTreeSet::new();
172        let mut queue = vec![from];
173
174        while !queue.is_empty() {
175            let i = map.get_i(queue.pop().unwrap());
176            if found.contains(&i.id) {
177                continue;
178            }
179            found.insert(i.id);
180            for r in &i.roads {
181                let r = map.get_r(*r);
182                if r.length() > threshold {
183                    continue;
184                }
185                let other = if r.src_i == i.id { r.dst_i } else { r.src_i };
186                if map.get_i(other).is_traffic_signal() {
187                    queue.push(other);
188                }
189            }
190        }
191        if found.len() > 1 {
192            Some(found)
193        } else {
194            None
195        }
196    }
197}
198
199fn flood(start: TurnID, map: &Map, exits: &BTreeSet<TurnID>) -> Vec<UberTurn> {
200    if exits.contains(&start) {
201        return vec![UberTurn { path: vec![start] }];
202    }
203
204    let mut results = Vec::new();
205    let mut preds: BTreeMap<TurnID, TurnID> = BTreeMap::new();
206    let mut queue = vec![start];
207
208    while !queue.is_empty() {
209        let current = queue.pop().unwrap();
210        // Filtering for car lanes is necessary near https://www.openstreetmap.org/node/1283661439,
211        // where there's a bidirectional shared-use path
212        for next in map.get_turns_for(current.dst, PathConstraints::Car) {
213            if preds.contains_key(&next.id) {
214                continue;
215            }
216            preds.insert(next.id, current);
217            if exits.contains(&next.id) {
218                results.push(UberTurn {
219                    path: trace_back(next.id, &preds),
220                });
221            } else {
222                queue.push(next.id);
223            }
224        }
225    }
226
227    results
228}
229
230fn trace_back(end: TurnID, preds: &BTreeMap<TurnID, TurnID>) -> Vec<TurnID> {
231    let mut path = vec![end];
232    let mut current = end;
233    loop {
234        if let Some(prev) = preds.get(&current) {
235            path.push(*prev);
236            current = *prev;
237        } else {
238            path.reverse();
239            return path;
240        }
241    }
242}
243
244impl UberTurn {
245    pub fn entry(&self) -> LaneID {
246        self.path[0].src
247    }
248    pub fn exit(&self) -> LaneID {
249        self.path.last().unwrap().dst
250    }
251
252    pub fn geom(&self, map: &Map) -> PolyLine {
253        let mut pl = map.get_t(self.path[0]).geom.clone();
254        let mut first = true;
255        for pair in self.path.windows(2) {
256            if !first {
257                pl = pl.must_extend(map.get_t(pair[0]).geom.clone());
258                first = false;
259            }
260            pl = pl.must_extend(map.get_l(pair[0].dst).lane_center_pts.clone());
261            pl = pl.must_extend(map.get_t(pair[1]).geom.clone());
262        }
263        pl
264    }
265}
266
267/// A sequence of movements through a cluster of intersections. Like UberTurn, but at the
268/// granularity of directed roads, not lanes.
269#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
270pub struct UberTurnV2 {
271    pub path: Vec<MovementID>,
272}
273
274impl IntersectionCluster {
275    /// Group lane-based uber-turns into road-based UberTurnV2s.
276    pub fn into_v2(self, map: &Map) -> Vec<UberTurnV2> {
277        let mut result = BTreeSet::new();
278        for ut in self.uber_turns {
279            let mut path = Vec::new();
280            for turn in ut.path {
281                path.push(turn.to_movement(map));
282            }
283            result.insert(UberTurnV2 { path });
284        }
285        result.into_iter().collect()
286    }
287}
288
289impl UberTurnV2 {
290    pub fn entry(&self) -> DirectedRoadID {
291        self.path[0].from
292    }
293    pub fn exit(&self) -> DirectedRoadID {
294        self.path.last().unwrap().to
295    }
296}