From 2716a2272b7a0d2d7af43764c27f5c0e4bc1651f Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Tue, 9 Oct 2018 13:10:24 -0700 Subject: [PATCH] making trace handle lanes and turns --- editor/src/plugins/show_route.rs | 18 ++++++++++++++---- editor/src/ui.rs | 1 + map_model/src/trace.rs | 18 ++++++------------ map_model/src/traversable.rs | 25 ++++++++++++++++++++++++- sim/src/driving.rs | 7 ++++--- sim/src/router.rs | 30 ++++++++++++++++++++++-------- sim/src/sim.rs | 6 +++--- sim/src/walking.rs | 12 ++++++++---- 8 files changed, 82 insertions(+), 35 deletions(-) diff --git a/editor/src/plugins/show_route.rs b/editor/src/plugins/show_route.rs index 9e7c67af4e..c0de922746 100644 --- a/editor/src/plugins/show_route.rs +++ b/editor/src/plugins/show_route.rs @@ -1,6 +1,6 @@ use colors::Colors; use ezgui::{Color, UserInput}; -use map_model::LaneID; +use map_model::{LaneID, Map}; use objects::{Ctx, ID}; use piston::input::Key; use plugins::Colorizer; @@ -14,7 +14,13 @@ pub enum ShowRouteState { } impl ShowRouteState { - pub fn event(&mut self, input: &mut UserInput, sim: &Sim, selected: Option) -> bool { + pub fn event( + &mut self, + input: &mut UserInput, + sim: &Sim, + map: &Map, + selected: Option, + ) -> bool { if *self == ShowRouteState::Empty { match selected { Some(ID::Car(id)) => { @@ -39,10 +45,14 @@ impl ShowRouteState { if input.key_pressed(Key::Return, "stop showing route") { true } else { - match sim.get_current_route(*agent) { + match sim.get_current_route(*agent, map) { Some(route) => { lanes.clear(); - lanes.extend(route); + for tr in &route { + if let Some(l) = tr.maybe_lane() { + lanes.insert(l); + } + } false } None => true, diff --git a/editor/src/ui.rs b/editor/src/ui.rs index 1f5b42a4b7..682c3a2c07 100644 --- a/editor/src/ui.rs +++ b/editor/src/ui.rs @@ -164,6 +164,7 @@ impl UIWrapper { ctx.ui.primary.show_route.event( ctx.input, &ctx.ui.primary.sim, + &ctx.ui.primary.map, ctx.ui.primary.current_selection, ) }), diff --git a/map_model/src/trace.rs b/map_model/src/trace.rs index bdd80e0655..ffe126d4c5 100644 --- a/map_model/src/trace.rs +++ b/map_model/src/trace.rs @@ -1,6 +1,6 @@ use dimensioned::si; use geom::PolyLine; -use {LaneID, Map}; +use {Map, Traversable}; pub struct Trace { // The rendered form @@ -8,27 +8,21 @@ pub struct Trace { } impl Trace { - // TODO what about when the route is empty and the caller is at the end? - // TODO what about turns? pub fn new( start_dist_along: si::Meter, - route: &Vec, + // Starting with the current traversable + route: &Vec, length: si::Meter, map: &Map, ) -> Trace { assert!(!route.is_empty()); - let (mut result, mut dist_left) = map - .get_l(route[0]) - .lane_center_pts - .slice(start_dist_along, start_dist_along + length); + let (mut result, mut dist_left) = + route[0].slice(map, start_dist_along, start_dist_along + length); let mut idx = 1; while dist_left > 0.0 * si::M && idx < route.len() { - let (piece, new_dist_left) = map - .get_l(route[idx]) - .lane_center_pts - .slice(0.0 * si::M, dist_left); + let (piece, new_dist_left) = route[idx].slice(map, 0.0 * si::M, dist_left); result.extend(piece); dist_left = new_dist_left; diff --git a/map_model/src/traversable.rs b/map_model/src/traversable.rs index 784c53b7dc..a7c3f0de4e 100644 --- a/map_model/src/traversable.rs +++ b/map_model/src/traversable.rs @@ -1,5 +1,5 @@ use dimensioned::si; -use geom::{Angle, Pt2D}; +use geom::{Angle, PolyLine, Pt2D}; use {LaneID, Map, TurnID}; // TODO also building paths? @@ -31,6 +31,14 @@ impl Traversable { } } + pub fn maybe_lane(&self) -> Option { + match self { + &Traversable::Turn(_) => None, + &Traversable::Lane(id) => Some(id), + } + } + + // TODO Just expose the PolyLine instead of all these layers of helpers pub fn length(&self, map: &Map) -> si::Meter { match self { &Traversable::Lane(id) => map.get_l(id).length(), @@ -38,6 +46,21 @@ impl Traversable { } } + pub fn slice( + &self, + map: &Map, + start: si::Meter, + end: si::Meter, + ) -> (PolyLine, si::Meter) { + match self { + &Traversable::Lane(id) => map.get_l(id).lane_center_pts.slice(start, end), + &Traversable::Turn(id) => { + let t = map.get_t(id); + PolyLine::new(vec![t.line.pt1(), t.line.pt2()]).slice(start, end) + } + } + } + pub fn dist_along(&self, dist: si::Meter, map: &Map) -> (Pt2D, Angle) { match self { &Traversable::Lane(id) => map.get_l(id).dist_along(dist), diff --git a/sim/src/driving.rs b/sim/src/driving.rs index 476e87ea2b..b74577706c 100644 --- a/sim/src/driving.rs +++ b/sim/src/driving.rs @@ -776,7 +776,7 @@ impl DrivingSimState { // TODO amend the route? Some(Trace::new( c.dist_along, - &self.get_current_route(id).unwrap(), + &self.get_current_route(id, map).unwrap(), stopping_dist, map, )) @@ -822,7 +822,8 @@ impl DrivingSimState { } } - pub fn get_current_route(&self, id: CarID) -> Option> { - self.routers.get(&id).map(|r| r.get_current_route()) + pub fn get_current_route(&self, id: CarID, map: &Map) -> Option> { + let r = self.routers.get(&id)?; + Some(r.get_current_route(self.cars[&id].on, map)) } } diff --git a/sim/src/router.rs b/sim/src/router.rs index ef0d0155e7..69a2fb76dc 100644 --- a/sim/src/router.rs +++ b/sim/src/router.rs @@ -124,12 +124,7 @@ impl Router { } pub fn choose_turn(&self, from: LaneID, map: &Map) -> TurnID { - for t in map.get_turns_from_lane(from) { - if t.dst == self.path[0] { - return t.id; - } - } - panic!("No turn from {} to {}", from, self.path[0]); + pick_turn(from, self.path[0], map) } pub fn advance_to(&mut self, next_lane: LaneID) { @@ -165,8 +160,18 @@ impl Router { None } - pub fn get_current_route(&self) -> Vec { - self.path.iter().map(|id| *id).collect() + pub fn get_current_route(&self, start: Traversable, map: &Map) -> Vec { + let mut route = vec![start]; + + let mut last_lane = start.maybe_lane(); + for next in &self.path { + if let Some(prev) = last_lane { + route.push(Traversable::Turn(pick_turn(prev, *next, map))); + } + route.push(Traversable::Lane(*next)); + last_lane = Some(*next); + } + route } } @@ -180,3 +185,12 @@ fn find_parking_spot( .find_parking_lane(driving_lane) .and_then(|l| parking_sim.get_first_free_spot(l, dist_along)) } + +fn pick_turn(from: LaneID, to: LaneID, map: &Map) -> TurnID { + for t in map.get_turns_from_lane(from) { + if t.dst == to { + return t.id; + } + } + panic!("No turn from {} to {}", from, to); +} diff --git a/sim/src/sim.rs b/sim/src/sim.rs index 8f7ddbe4c0..81ca70ca51 100644 --- a/sim/src/sim.rs +++ b/sim/src/sim.rs @@ -7,7 +7,7 @@ use driving::DrivingSimState; use failure::Error; use instrument::capture_backtrace; use intersections::IntersectionSimState; -use map_model::{IntersectionID, LaneID, LaneType, Map, Turn, TurnID}; +use map_model::{IntersectionID, LaneID, LaneType, Map, Traversable, Turn, TurnID}; use parking::ParkingSimState; use rand::{FromEntropy, SeedableRng, XorShiftRng}; use spawn::Spawner; @@ -348,9 +348,9 @@ impl Sim { } } - pub fn get_current_route(&self, id: AgentID) -> Option> { + pub fn get_current_route(&self, id: AgentID, map: &Map) -> Option> { match id { - AgentID::Car(car) => self.driving_state.get_current_route(car), + AgentID::Car(car) => self.driving_state.get_current_route(car, map), AgentID::Pedestrian(ped) => self.walking_state.get_current_route(ped), } } diff --git a/sim/src/walking.rs b/sim/src/walking.rs index b182f7934c..056e086394 100644 --- a/sim/src/walking.rs +++ b/sim/src/walking.rs @@ -577,10 +577,14 @@ impl WalkingSimState { self.peds.is_empty() } - pub fn get_current_route(&self, id: PedestrianID) -> Option> { - self.peds - .get(&id) - .map(|p| p.path.iter().map(|id| *id).collect()) + pub fn get_current_route(&self, id: PedestrianID) -> Option> { + // TODO turns. will be hard because of contraflow. + let p = self.peds.get(&id)?; + let mut route = vec![p.on]; + for l in &p.path { + route.push(Traversable::Lane(*l)); + } + Some(route) } pub fn get_peds_waiting_at_stop(&self, stop: BusStopID) -> Vec {