use crate::{DirectedRoadID, Direction, IntersectionID, LaneID, Map};
use abstutil::MultiMap;
use geom::{Angle, Distance, PolyLine, Pt2D};
use serde::{Deserialize, Serialize};
use std::collections::{BTreeMap, BTreeSet};
use std::fmt;
#[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)]
pub struct TurnID {
pub parent: IntersectionID,
pub src: LaneID,
pub dst: LaneID,
}
impl fmt::Display for TurnID {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
write!(f, "TurnID({}, {}, {})", self.src, self.dst, self.parent)
}
}
#[derive(Clone, Copy, Debug, Eq, PartialOrd, Ord, PartialEq, Serialize, Deserialize)]
pub enum TurnType {
Crosswalk,
SharedSidewalkCorner,
Straight,
Right,
Left,
}
impl TurnType {
pub fn from_angles(from: Angle, to: Angle) -> TurnType {
let diff = from.shortest_rotation_towards(to).normalized_degrees();
if diff < 10.0 || diff > 350.0 {
TurnType::Straight
} else if diff > 180.0 {
TurnType::Right
} else {
TurnType::Left
}
}
}
#[derive(Serialize, Deserialize, Debug, PartialEq, Clone, Copy, PartialOrd)]
pub enum TurnPriority {
Banned,
Yield,
Protected,
}
#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)]
pub struct Turn {
pub id: TurnID,
pub turn_type: TurnType,
pub geom: PolyLine,
pub other_crosswalk_ids: BTreeSet<TurnID>,
}
impl Turn {
pub fn conflicts_with(&self, other: &Turn) -> bool {
if self.turn_type == TurnType::SharedSidewalkCorner
|| other.turn_type == TurnType::SharedSidewalkCorner
{
return false;
}
if self.id == other.id {
return false;
}
if self.between_sidewalks() && other.between_sidewalks() {
return false;
}
if self.geom.first_pt() == other.geom.first_pt() {
return false;
}
if self.geom.last_pt() == other.geom.last_pt() {
return true;
}
self.geom.intersection(&other.geom).is_some()
}
pub fn angle(&self) -> Angle {
self.geom.first_pt().angle_to(self.geom.last_pt())
}
pub fn between_sidewalks(&self) -> bool {
self.turn_type == TurnType::SharedSidewalkCorner || self.turn_type == TurnType::Crosswalk
}
pub fn penalty(&self, map: &Map) -> (usize, usize, usize) {
let from = map.get_l(self.id.src);
let to = map.get_l(self.id.dst);
let from_idx = {
let mut cnt = 0;
let r = map.get_r(from.parent);
for (l, lt) in r.children(r.dir(from.id)).iter().rev() {
if from.lane_type != *lt {
continue;
}
if map
.get_turns_from_lane(*l)
.into_iter()
.any(|t| map.get_l(t.id.dst).parent == to.parent)
{
cnt += 1;
if from.id == *l {
break;
}
}
}
cnt
};
let to_idx = {
let mut cnt = 0;
let r = map.get_r(to.parent);
for (l, lt) in r.children(r.dir(to.id)).iter().rev() {
if to.lane_type != *lt {
continue;
}
cnt += 1;
if to.id == *l {
break;
}
}
cnt
};
let lc_cost = ((from_idx as isize) - (to_idx as isize)).abs() as usize;
let lt_cost = if to.is_biking() || to.is_bus() { 0 } else { 1 };
let rightmost = if to_idx > 1 { 1 } else { 0 };
(lt_cost, lc_cost, rightmost)
}
}
#[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)]
pub struct MovementID {
pub from: DirectedRoadID,
pub to: DirectedRoadID,
pub parent: IntersectionID,
pub crosswalk: bool,
}
#[derive(Clone, Copy, Debug, Eq, Hash, PartialEq, PartialOrd, Ord, Serialize, Deserialize)]
pub struct CompressedMovementID {
pub i: IntersectionID,
pub idx: u8,
}
#[derive(Debug, Serialize, Deserialize, Clone, PartialEq)]
pub struct Movement {
pub id: MovementID,
pub turn_type: TurnType,
pub members: Vec<TurnID>,
pub geom: PolyLine,
pub angle: Angle,
}
impl Movement {
pub(crate) fn for_i(
i: IntersectionID,
map: &Map,
) -> Result<BTreeMap<MovementID, Movement>, String> {
let mut results = BTreeMap::new();
let mut movements: MultiMap<(DirectedRoadID, DirectedRoadID), TurnID> = MultiMap::new();
for turn in map.get_turns_in_intersection(i) {
let from = map.get_l(turn.id.src).get_directed_parent(map);
let to = map.get_l(turn.id.dst).get_directed_parent(map);
match turn.turn_type {
TurnType::SharedSidewalkCorner => {}
TurnType::Crosswalk => {
let id = MovementID {
from,
to,
parent: i,
crosswalk: true,
};
results.insert(
id,
Movement {
id,
turn_type: TurnType::Crosswalk,
members: vec![turn.id],
geom: turn.geom.clone(),
angle: turn.angle(),
},
);
}
_ => {
movements.insert((from, to), turn.id);
}
}
}
for ((from, to), members) in movements.consume() {
let geom = movement_geom(
members.iter().map(|t| &map.get_t(*t).geom).collect(),
from,
to,
)?;
let turn_types: BTreeSet<TurnType> =
members.iter().map(|t| map.get_t(*t).turn_type).collect();
if turn_types.len() > 1 {
println!(
"Movement between {} and {} has weird turn types! {:?}",
from, to, turn_types
);
}
let members: Vec<TurnID> = members.into_iter().collect();
let id = MovementID {
from,
to,
parent: i,
crosswalk: false,
};
results.insert(
id,
Movement {
id,
turn_type: *turn_types.iter().next().unwrap(),
angle: map.get_t(members[0]).angle(),
members,
geom,
},
);
}
if results.is_empty() {
return Err(format!(
"No Movements! Does the intersection have at least 2 roads?"
));
}
Ok(results)
}
pub fn src_center_and_width(&self, map: &Map) -> (PolyLine, Distance) {
let r = map.get_r(self.id.from.id);
let mut leftmost = Distance::meters(99999.0);
let mut rightmost = Distance::ZERO;
let mut left = Distance::ZERO;
for (l, _, _) in r.lanes_ltr() {
let right = left + map.get_l(l).width;
if self.members.iter().any(|t| t.src == l) {
leftmost = leftmost.min(left);
rightmost = rightmost.max(right);
}
left = right;
}
let mut pl = r
.get_left_side(map)
.must_shift_right((leftmost + rightmost) / 2.0);
if self.id.from.dir == Direction::Back {
pl = pl.reversed();
}
if !self.id.crosswalk || map.get_l(self.members[0].src).src_i != self.members[0].parent {
pl = pl.reversed()
};
(pl, rightmost - leftmost)
}
pub fn conflicts_with(&self, other: &Movement) -> bool {
if self.id == other.id {
return false;
}
if self.turn_type == TurnType::Crosswalk && other.turn_type == TurnType::Crosswalk {
return false;
}
if self.id.from == other.id.from
&& self.turn_type != TurnType::Crosswalk
&& other.turn_type != TurnType::Crosswalk
{
return false;
}
if self.id.to == other.id.to
&& self.turn_type != TurnType::Crosswalk
&& other.turn_type != TurnType::Crosswalk
{
return true;
}
self.geom.intersection(&other.geom).is_some()
}
}
fn movement_geom(
polylines: Vec<&PolyLine>,
from: DirectedRoadID,
to: DirectedRoadID,
) -> Result<PolyLine, String> {
let num_pts = polylines[0].points().len();
for pl in &polylines {
if num_pts != pl.points().len() {
println!(
"Movement between {} and {} can't make nice geometry",
from, to
);
return Ok(polylines[0].clone());
}
}
let mut pts = Vec::new();
for idx in 0..num_pts {
pts.push(Pt2D::center(
&polylines.iter().map(|pl| pl.points()[idx]).collect(),
));
}
PolyLine::deduping_new(pts)
}