From d7a36889b27059a529b984e5c7fe2a38384caaa6 Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Tue, 9 Oct 2018 08:38:58 -0700 Subject: [PATCH] some of the pieces to render smart arrows in front of agents --- editor/src/render/car.rs | 22 ++++++------------ geom/src/polyline.rs | 48 ++++++++++++++++++++++++++++++++++++++++ map_model/src/lib.rs | 2 ++ map_model/src/trace.rs | 40 +++++++++++++++++++++++++++++++++ map_model/src/turn.rs | 1 - sim/src/driving.rs | 18 ++++++++++++--- sim/src/lib.rs | 4 ++-- sim/src/parking.rs | 3 +-- 8 files changed, 115 insertions(+), 23 deletions(-) create mode 100644 map_model/src/trace.rs diff --git a/editor/src/render/car.rs b/editor/src/render/car.rs index 8a43443269..93919f4c52 100644 --- a/editor/src/render/car.rs +++ b/editor/src/render/car.rs @@ -1,5 +1,4 @@ use colors::Colors; -use dimensioned::si; use ezgui::{shift_color, GfxCtx}; use geom::{Angle, Bounds, Line, PolyLine, Polygon, Pt2D}; use map_model::Map; @@ -16,9 +15,7 @@ pub struct DrawCar { // TODO ideally, draw the turn icon inside the car quad. how can we do that easily? turn_arrow: Option, // TODO maybe also draw lookahead buffer to know what the car is considering - // TODO it would be really neat to project the stopping buffer onto the actual route that'll be - // taken - stopping_buffer_arrow: Option, + stopping_buffer: Option, } impl DrawCar { @@ -33,14 +30,9 @@ impl DrawCar { None }; - let stopping_buffer_arrow = if input.stopping_dist == 0.0 * si::M { - None - } else { - let arrow_pt = input - .front - .project_away(input.stopping_dist.value_unsafe, input.angle); - Some(Line::new(input.front, arrow_pt)) - }; + let stopping_buffer = input + .stopping_trace + .map(|t| t.polyline.make_polygons_blindly(CAR_WIDTH)); let front_window_length_gap = 0.2; let front_window_thickness = 0.3; @@ -85,7 +77,7 @@ impl DrawCar { input.angle.rotate_degs(90.0), ), ], - stopping_buffer_arrow, + stopping_buffer, } } } @@ -115,8 +107,8 @@ impl Renderable for DrawCar { g.draw_arrow([0.0, 1.0, 1.0, 1.0], 0.25, 1.0, a); } - if let Some(ref a) = self.stopping_buffer_arrow { - g.draw_arrow([1.0, 0.0, 0.0, 0.7], 0.25, 1.0, a); + if let Some(ref t) = self.stopping_buffer { + g.draw_polygon([1.0, 0.0, 0.0, 0.7], t); } } diff --git a/geom/src/polyline.rs b/geom/src/polyline.rs index c5490fca2a..df37cfb19a 100644 --- a/geom/src/polyline.rs +++ b/geom/src/polyline.rs @@ -34,6 +34,11 @@ impl PolyLine { self.pts[len - 1] = pt2; } + pub fn extend(&mut self, other: PolyLine) { + assert_eq!(*self.pts.last().unwrap(), other.pts[0]); + self.pts.extend(other.pts.iter().skip(1)); + } + pub fn points(&self) -> &Vec { &self.pts } @@ -52,6 +57,49 @@ impl PolyLine { .fold(0.0 * si::M, |so_far, l| so_far + l.length()) } + // Returns the excess distance left over from the end. + pub fn slice(&self, start: si::Meter, end: si::Meter) -> (PolyLine, si::Meter) { + if start >= end { + panic!("Can't get a polyline slice [{}, {}]", start, end); + } + + let mut result: Vec = Vec::new(); + let mut dist_so_far = 0.0 * si::M; + + for line in self.lines().iter() { + let length = line.length(); + + // Does this line contain the first point of the slice? + if result.is_empty() && dist_so_far + length >= start { + result.push(line.dist_along(start - dist_so_far)); + } + + // Does this line contain the last point of the slice? + if dist_so_far + length >= end { + result.push(line.dist_along(end - dist_so_far)); + return (PolyLine::new(result), 0.0 * si::M); + } + + // If we're in the middle, just collect the endpoint. + if !result.is_empty() { + result.push(line.pt2()); + } + + dist_so_far += length; + } + + if result.is_empty() { + panic!( + "Slice [{}, {}] has a start too big for polyline of length {}", + start, + end, + self.length() + ); + } + + (PolyLine::new(result), end - dist_so_far) + } + // TODO return result with an error message pub fn safe_dist_along(&self, dist_along: si::Meter) -> Option<(Pt2D, Angle)> { if dist_along < 0.0 * si::M { diff --git a/map_model/src/lib.rs b/map_model/src/lib.rs index a521ffe364..3283a8bf87 100644 --- a/map_model/src/lib.rs +++ b/map_model/src/lib.rs @@ -29,6 +29,7 @@ mod parcel; mod pathfind; pub mod raw_data; mod road; +mod trace; mod turn; pub use area::{Area, AreaID, AreaType}; @@ -41,6 +42,7 @@ pub use map::Map; pub use parcel::{Parcel, ParcelID}; pub use pathfind::Pathfinder; pub use road::{Road, RoadID}; +pub use trace::Trace; pub use turn::{Turn, TurnID}; pub const LANE_THICKNESS: f64 = 2.5; diff --git a/map_model/src/trace.rs b/map_model/src/trace.rs new file mode 100644 index 0000000000..bdd80e0655 --- /dev/null +++ b/map_model/src/trace.rs @@ -0,0 +1,40 @@ +use dimensioned::si; +use geom::PolyLine; +use {LaneID, Map}; + +pub struct Trace { + // The rendered form + pub polyline: PolyLine, +} + +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, + 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 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); + result.extend(piece); + + dist_left = new_dist_left; + idx += 1; + } + + Trace { polyline: result } + } +} diff --git a/map_model/src/turn.rs b/map_model/src/turn.rs index cc3059cc0d..ba22572989 100644 --- a/map_model/src/turn.rs +++ b/map_model/src/turn.rs @@ -32,7 +32,6 @@ pub struct Turn { pub dst: LaneID, pub between_sidewalks: bool, - /// GeomTurn stuff pub line: Line, } diff --git a/sim/src/driving.rs b/sim/src/driving.rs index 527e836f29..4a627c898d 100644 --- a/sim/src/driving.rs +++ b/sim/src/driving.rs @@ -6,7 +6,7 @@ use geom::EPSILON_DIST; use intersections::{IntersectionSimState, Request}; use kinematics; use kinematics::Vehicle; -use map_model::{LaneID, Map, TurnID, LANE_THICKNESS}; +use map_model::{LaneID, Map, Trace, TurnID, LANE_THICKNESS}; use multimap::MultiMap; use ordered_float::NotNaN; use parking::ParkingSimState; @@ -748,7 +748,6 @@ impl DrivingSimState { pub fn get_draw_car(&self, id: CarID, time: Tick, map: &Map) -> Option { let c = self.cars.get(&id)?; let (base_pos, angle) = c.on.dist_along(c.dist_along, map); - let stopping_dist = c.vehicle.stopping_distance(c.speed); // TODO arguably, this math might belong in DrawCar. let pos = if let Some(ref parking) = c.parking { @@ -766,13 +765,26 @@ impl DrivingSimState { base_pos }; + let stopping_dist = c.vehicle.stopping_distance(c.speed); + let stopping_trace = if stopping_dist <= EPSILON_DIST { + None + } else { + // TODO amend the route? + Some(Trace::new( + c.dist_along, + &self.get_current_route(id).unwrap(), + stopping_dist, + map, + )) + }; + Some(DrawCarInput { id: c.id, vehicle_length: c.vehicle.length, waiting_for_turn: c.waiting_for.and_then(|on| on.maybe_turn()), front: pos, angle, - stopping_dist, + stopping_trace, }) } diff --git a/sim/src/lib.rs b/sim/src/lib.rs index 03b5aa0abc..d886fc7140 100644 --- a/sim/src/lib.rs +++ b/sim/src/lib.rs @@ -60,7 +60,7 @@ pub use events::Event; use geom::{Angle, Pt2D}; pub use helpers::{load, SimFlags}; pub use instrument::save_backtraces; -use map_model::{LaneID, Map, TurnID}; +use map_model::{LaneID, Map, Trace, TurnID}; pub use scenario::{Neighborhood, Scenario, SeedParkedCars, SpawnOverTime}; pub use sim::{Benchmark, Sim}; use std::fmt; @@ -379,7 +379,7 @@ pub struct DrawCarInput { pub waiting_for_turn: Option, pub front: Pt2D, pub angle: Angle, - pub stopping_dist: Distance, + pub stopping_trace: Option, } // We have to do this in the crate where these types are defined. Bit annoying, since it's really diff --git a/sim/src/parking.rs b/sim/src/parking.rs index 7be2204264..5407b1f2d2 100644 --- a/sim/src/parking.rs +++ b/sim/src/parking.rs @@ -1,4 +1,3 @@ -use dimensioned::si; use geom::{Angle, Polygon, Pt2D}; use kinematics::Vehicle; use map_model; @@ -212,7 +211,7 @@ impl ParkingLane { waiting_for_turn: None, front: front, angle: angle, - stopping_dist: 0.0 * si::M, + stopping_trace: None, }) }) }).collect()