1use std::collections::{BTreeMap, HashSet};
2
3use anyhow::Result;
4use lyon::geom::{CubicBezierSegment, Point, QuadraticBezierSegment};
5
6use geom::{PolyLine, Pt2D};
7
8use crate::{
9 map::turn_type_from_road_geom, Intersection, Lane, LaneID, LaneType, Map, RoadID, Turn, TurnID,
10 TurnType,
11};
12
13pub fn make_all_turns(map: &Map, i: &Intersection) -> Vec<Turn> {
15 let mut raw_turns: Vec<Turn> = Vec::new();
16 raw_turns.extend(make_vehicle_turns(i, map));
17 raw_turns.extend(crate::make::walking_turns::filter_turns(
18 crate::make::walking_turns::make_walking_turns(map, i),
19 map,
20 i,
21 ));
22 let unique_turns = ensure_unique(raw_turns);
23 let all_turns: Vec<Turn> = unique_turns
26 .into_iter()
27 .filter(|t| t.permitted_by_road(i, map))
28 .collect();
29
30 let filtered_turns: Vec<Turn> = all_turns
32 .clone()
33 .into_iter()
34 .filter(|t| t.permitted_by_lane(map))
35 .collect();
36 let filtered_turns = remove_merging_turns(map, filtered_turns, TurnType::Right);
39 let mut filtered_turns = remove_merging_turns(map, filtered_turns, TurnType::Left);
40 if i.merged {
41 filtered_turns.retain(|turn| {
42 if turn.turn_type == TurnType::UTurn {
43 let src_lane = map.get_l(turn.id.src);
44 if src_lane
47 .get_lane_level_turn_restrictions(map.get_r(src_lane.id.road), false)
48 .map(|set| !set.contains(&TurnType::UTurn))
49 .unwrap_or(true)
50 {
51 warn!("Removing u-turn from merged intersection: {}", turn.id);
52 false
53 } else {
54 true
55 }
56 } else {
57 true
58 }
59 });
60 }
61
62 match verify_vehicle_connectivity(&filtered_turns, i, map) {
64 Ok(()) => filtered_turns,
65 Err(err) => {
66 warn!("Not filtering turns. {}", err);
67 all_turns
68 }
69 }
70}
71
72fn ensure_unique(turns: Vec<Turn>) -> Vec<Turn> {
73 let mut ids = HashSet::new();
74 let mut keep: Vec<Turn> = Vec::new();
75 for t in turns.into_iter() {
76 if ids.contains(&t.id) {
77 warn!("Duplicate turns {}!", t.id);
81 } else {
82 ids.insert(t.id);
83 keep.push(t);
84 }
85 }
86 keep
87}
88
89pub fn verify_vehicle_connectivity(turns: &[Turn], i: &Intersection, map: &Map) -> Result<()> {
100 let mut incoming_missing: HashSet<LaneID> = HashSet::new();
101 for l in &i.incoming_lanes {
102 if map.get_l(*l).lane_type.is_for_moving_vehicles() {
103 incoming_missing.insert(*l);
104 }
105 }
106 let mut outgoing_missing: HashSet<LaneID> = HashSet::new();
107 for l in &i.outgoing_lanes {
108 if map.get_l(*l).lane_type.is_for_moving_vehicles() {
109 outgoing_missing.insert(*l);
110 }
111 }
112
113 for turn in turns {
114 let src_lt = map.get_l(turn.id.src).lane_type;
115 let dst_lt = map.get_l(turn.id.dst).lane_type;
116
117 if src_lt == dst_lt {
118 incoming_missing.remove(&turn.id.src);
119 outgoing_missing.remove(&turn.id.dst);
120 }
121
122 if src_lt == LaneType::Biking || src_lt == LaneType::Bus {
123 incoming_missing.remove(&turn.id.src);
124 }
125 if dst_lt == LaneType::Biking || dst_lt == LaneType::Bus {
126 outgoing_missing.remove(&turn.id.dst);
127 }
128 }
129
130 if !incoming_missing.is_empty() || !outgoing_missing.is_empty() {
131 bail!(
132 "Turns for {} orphan some lanes. Incoming: {:?}, outgoing: {:?}",
133 i.id,
134 incoming_missing,
135 outgoing_missing
136 );
137 }
138 Ok(())
139}
140
141fn make_vehicle_turns(i: &Intersection, map: &Map) -> Vec<Turn> {
142 let mut turns = Vec::new();
143
144 let is_deadend = i.is_deadend_for_driving(map);
146 for src in &i.incoming_lanes {
147 let src = map.get_l(*src);
148 if !src.lane_type.is_for_moving_vehicles() {
149 continue;
150 }
151 for dst in &i.outgoing_lanes {
152 let dst = map.get_l(*dst);
153 if !dst.lane_type.is_for_moving_vehicles() {
154 continue;
155 }
156 if src.id.road == dst.id.road && !is_deadend {
158 continue;
159 }
160 if src.is_light_rail() != dst.is_light_rail() {
162 continue;
163 }
164 if src.last_pt() == dst.first_pt() {
165 warn!(
166 "No turn from {} to {}; the endpoints are the same",
167 src.id, dst.id
168 );
169 continue;
170 }
171
172 let turn_type = turn_type_from_lane_geom(src, dst, i, map);
173
174 let geom = curvey_turn(src, dst, i)
175 .unwrap_or_else(|_| PolyLine::must_new(vec![src.last_pt(), dst.first_pt()]));
176
177 turns.push(Turn {
178 id: TurnID {
179 parent: i.id,
180 src: src.id,
181 dst: dst.id,
182 },
183 turn_type,
184 geom,
185 });
186 }
187 }
188
189 turns
190}
191
192fn turn_type_from_lane_geom(src: &Lane, dst: &Lane, i: &Intersection, map: &Map) -> TurnType {
193 turn_type_from_road_geom(
194 map.get_r(src.id.road),
195 src.last_line().angle(),
196 map.get_r(dst.id.road),
197 dst.last_line().angle(),
198 i,
199 map,
200 )
201}
202
203fn curvey_turn(src: &Lane, dst: &Lane, i: &Intersection) -> Result<PolyLine> {
204 fn to_pt(pt: Pt2D) -> Point<f64> {
205 Point::new(pt.x(), pt.y())
206 }
207
208 fn from_pt(pt: Point<f64>) -> Pt2D {
209 Pt2D::new(pt.x, pt.y)
210 }
211
212 let src_line = src.last_line();
215 let dst_line = dst.first_line();
216
217 let src_pt = src.last_pt();
218 let dst_pt = dst.first_pt();
219
220 let src_angle = src_line.angle();
221 let dst_angle = dst_line.angle();
222
223 let intersection = src_line
224 .infinite()
225 .intersection(&dst_line.infinite())
226 .unwrap_or(src_pt);
227
228 let curve =
229 if src_angle.approx_parallel(dst_angle, 5.0)
231 || src_pt.approx_eq(intersection, geom::EPSILON_DIST)
233 || dst_pt.approx_eq(intersection, geom::EPSILON_DIST)
234 || !i.polygon.contains_pt(intersection)
236 {
237 CubicBezierSegment {
239 from: to_pt(src_pt),
240 ctrl1: to_pt(src_pt.project_away(src_pt.dist_to(dst_pt) / 2.0, src_angle)),
241 ctrl2: to_pt(dst_pt.project_away(src_pt.dist_to(dst_pt) / 2.0, dst_angle.opposite())),
242 to: to_pt(dst_pt),
243 }
244 } else {
245 QuadraticBezierSegment {
247 from: to_pt(src_pt),
248 ctrl: to_pt(intersection),
249 to: to_pt(dst_pt),
250 }.to_cubic()
251 };
252
253 let pieces = 5;
254 let mut curve: Vec<Pt2D> = (0..=pieces)
255 .map(|i| from_pt(curve.sample(1.0 / f64::from(pieces) * f64::from(i))))
256 .collect();
257 curve.dedup();
258 PolyLine::new(curve)
259}
260
261fn remove_merging_turns(map: &Map, input: Vec<Turn>, turn_type: TurnType) -> Vec<Turn> {
262 let mut turns = Vec::new();
263
264 let mut pairs: BTreeMap<(RoadID, RoadID), Vec<Turn>> = BTreeMap::new();
266 for t in input {
267 if !map.get_l(t.id.src).is_driving() || !map.get_l(t.id.dst).is_driving() {
269 turns.push(t);
270 continue;
271 }
272
273 if t.turn_type == turn_type {
274 pairs
275 .entry((t.id.src.road, t.id.dst.road))
276 .or_insert_with(Vec::new)
277 .push(t);
278 } else {
279 turns.push(t);
281 }
282 }
283
284 for (_, group) in pairs {
285 if group.len() == 1 {
286 turns.extend(group);
287 continue;
288 }
289
290 let num_src_lanes = group.iter().map(|t| t.id.src).collect::<HashSet<_>>().len();
291 let num_dst_lanes = group.iter().map(|t| t.id.dst).collect::<HashSet<_>>().len();
292
293 if num_src_lanes == 1 {
295 turns.extend(group);
296 continue;
297 }
298
299 if num_src_lanes == num_dst_lanes {
302 let mut src_lanes_in_order: Vec<LaneID> = group.iter().map(|t| t.id.src).collect();
304 src_lanes_in_order.sort_by_key(|l| map.get_parent(*l).dir_and_offset(*l).1);
305 let mut dst_lanes_in_order: Vec<LaneID> = group.iter().map(|t| t.id.dst).collect();
306 dst_lanes_in_order.sort_by_key(|l| map.get_parent(*l).dir_and_offset(*l).1);
307
308 for t in group {
309 let src_order = src_lanes_in_order
310 .iter()
311 .position(|l| t.id.src == *l)
312 .unwrap();
313 let dst_order = dst_lanes_in_order
314 .iter()
315 .position(|l| t.id.dst == *l)
316 .unwrap();
317 if src_order == dst_order {
318 turns.push(t);
319 }
320 }
321 continue;
322 }
323
324 if num_src_lanes < num_dst_lanes {
330 turns.extend(group);
331 continue;
332 }
333
334 let road = map.get_parent(group[0].id.src);
343 let src = if turn_type == TurnType::Right {
344 group
345 .iter()
346 .max_by_key(|t| road.dir_and_offset(t.id.src).1)
347 .unwrap()
348 .id
349 .src
350 } else if turn_type == TurnType::Left {
351 group
352 .iter()
353 .min_by_key(|t| road.dir_and_offset(t.id.src).1)
354 .unwrap()
355 .id
356 .src
357 } else {
358 unreachable!()
359 };
360 for t in group {
361 if t.id.src == src {
362 turns.push(t);
363 }
364 }
365 }
366 turns
367}