map_model/pathfind/
uber_turns.rs
1use 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
13pub 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 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 clusters.push(IntersectionCluster::new(members, map).0);
37 }
38 }
39 }
40
41 let mut graph: UnGraphMap<IntersectionID, ()> = UnGraphMap::new();
43 for from in map.all_roads() {
44 for (via, _) in &from.complicated_turn_restrictions {
45 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 if clusters.iter().any(|ic| ic.members.is_subset(&members)) {
54 continue;
55 }
56
57 let mut existing: Vec<&mut IntersectionCluster> = clusters
59 .iter_mut()
60 .filter(|ic| ic.members.intersection(&members).next().is_some())
61 .collect();
62 if existing.is_empty() {
64 clusters.push(IntersectionCluster::new(members, map).0);
65 continue;
66 }
67
68 if existing.len() == 1 {
69 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 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 pub fn new(
90 members: BTreeSet<IntersectionID>,
91 map: &Map,
92 ) -> (IntersectionCluster, IntersectionCluster) {
93 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 let mut uber_turns = Vec::new();
118 for entrance in entrances {
119 uber_turns.extend(flood(entrance, map, &exits));
120 }
121
122 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 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 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 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 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(¤t) {
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#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
270pub struct UberTurnV2 {
271 pub path: Vec<MovementID>,
272}
273
274impl IntersectionCluster {
275 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}