making trace handle lanes and turns

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<ID>) -> bool {
+    pub fn event(
+        &mut self,
+        input: &mut UserInput,
+        sim: &Sim,
+        map: &Map,
+        selected: Option<ID>,
+    ) -> 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,

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,
                     )
                 }),

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<f64>,
-        route: &Vec<LaneID>,
+        // Starting with the current traversable
+        route: &Vec<Traversable>,
         length: si::Meter<f64>,
         map: &Map,
     ) -> Trace {
         assert!(!route.is_empty());
 
-        let (mut result, mut dist_left) = map
+        let (mut result, mut dist_left) =
-            .get_l(route[0])
+            route[0].slice(map, start_dist_along, start_dist_along + length);
-            .lane_center_pts
-            .slice(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
+            let (piece, new_dist_left) = route[idx].slice(map, 0.0 * si::M, dist_left);
-                .get_l(route[idx])
-                .lane_center_pts
-                .slice(0.0 * si::M, dist_left);
             result.extend(piece);
 
             dist_left = new_dist_left;

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<LaneID> {
+        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<f64> {
         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<f64>,
+        end: si::Meter<f64>,
+    ) -> (PolyLine, si::Meter<f64>) {
+        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<f64>, map: &Map) -> (Pt2D, Angle) {
         match self {
             &Traversable::Lane(id) => map.get_l(id).dist_along(dist),

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<Vec<LaneID>> {
+    pub fn get_current_route(&self, id: CarID, map: &Map) -> Option<Vec<Traversable>> {
-        self.routers.get(&id).map(|r| r.get_current_route())
+        let r = self.routers.get(&id)?;
+        Some(r.get_current_route(self.cars[&id].on, map))
     }
 }

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) {
+        pick_turn(from, self.path[0], map)
-            if t.dst == self.path[0] {
-                return t.id;
-            }
-        }
-        panic!("No turn from {} to {}", from, self.path[0]);
     }
 
     pub fn advance_to(&mut self, next_lane: LaneID) {
@@ -165,8 +160,18 @@ impl Router {
         None
     }
 
-    pub fn get_current_route(&self) -> Vec<LaneID> {
+    pub fn get_current_route(&self, start: Traversable, map: &Map) -> Vec<Traversable> {
-        self.path.iter().map(|id| *id).collect()
+        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);
+}

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<Vec<LaneID>> {
+    pub fn get_current_route(&self, id: AgentID, map: &Map) -> Option<Vec<Traversable>> {
         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),
         }
     }

sim/src/walking.rs

@@ -577,10 +577,14 @@ impl WalkingSimState {
         self.peds.is_empty()
     }
 
-    pub fn get_current_route(&self, id: PedestrianID) -> Option<Vec<LaneID>> {
+    pub fn get_current_route(&self, id: PedestrianID) -> Option<Vec<Traversable>> {
-        self.peds
+        // TODO turns. will be hard because of contraflow.
-            .get(&id)
+        let p = self.peds.get(&id)?;
-            .map(|p| p.path.iter().map(|id| *id).collect())
+        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<PedestrianID> {