draw stopping buffer for cars

docs/TODO_phase3.md

@@ -13,7 +13,6 @@
 
 - better visualization
 	- draw moving / blocked colors (gradually more red as they wait longer)
-	- draw stop buffer in front/behind of cars
 
 - start implementing a second AORTAish driving model
 	- then make cars park/unpark at the correct position

docs/design.md

@@ -480,3 +480,9 @@ How to share common state in intersections?
 
 - serde tricks
 - unit tests
+
+## AORTA driving model ##
+
+- how to sub out different driving models? it'd be useful to maintain the current one for comparison
+
+- first draw stop buffer ahead... if slammed on brakes now, where would we end up?

editor/src/ui.rs

@@ -442,8 +442,8 @@ impl gui::GUI for UI {
         }
         if input.unimportant_key_pressed(Key::S, "Seed the map with agents") {
             self.sim_ctrl.sim.seed_parked_cars(0.5);
-            self.sim_ctrl.sim.seed_pedestrians(&self.map, 1000);
-            self.sim_ctrl.sim.start_many_parked_cars(&self.map, 1000);
+            self.sim_ctrl.sim.seed_pedestrians(&self.map, 100);
+            self.sim_ctrl.sim.start_many_parked_cars(&self.map, 100);
             return gui::EventLoopMode::InputOnly;
         }
 

sim/src/draw_car.rs

@@ -23,6 +23,10 @@ pub struct DrawCar {
     // TODO ideally, draw the turn icon inside the car quad. how can we do that easily?
     turn_arrow: Option<[f64; 4]>,
     front_pt: Pt2D,
+    // 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<[f64; 4]>,
 }
 
 impl DrawCar {
@@ -32,6 +36,7 @@ impl DrawCar {
         map: &Map,
         front: Pt2D,
         angle: Angle,
+        stopping_dist: si::Meter<f64>,
     ) -> DrawCar {
         let turn_arrow = if let Some(t) = waiting_for_turn {
             let angle = map.get_t(t).line.angle();
@@ -41,6 +46,13 @@ impl DrawCar {
             None
         };
 
+        let stopping_buffer_arrow = if stopping_dist == 0.0 * si::M {
+            None
+        } else {
+            let arrow_pt = front.project_away(stopping_dist.value_unsafe, angle);
+            Some([front.x(), front.y(), arrow_pt.x(), arrow_pt.y()])
+        };
+
         DrawCar {
             id: id,
             turn_arrow,
@@ -53,6 +65,7 @@ impl DrawCar {
                 angle.opposite(),
             ),
             front_pt: front,
+            stopping_buffer_arrow,
         }
     }
 
@@ -68,6 +81,14 @@ impl DrawCar {
                 1.0,
             );
         }
+
+        if let Some(a) = self.stopping_buffer_arrow {
+            g.draw_arrow(
+                &graphics::Line::new_round([1.0, 0.0, 0.0, 0.7], 0.25),
+                a,
+                1.0,
+            );
+        }
     }
 
     pub fn contains_pt(&self, x: f64, y: f64) -> bool {

sim/src/driving.rs

@@ -5,6 +5,7 @@ use dimensioned::si;
 use draw_car::DrawCar;
 use geom::{Angle, Pt2D};
 use intersections::{IntersectionSimState, Request};
+use kinematics::Vehicle;
 use map_model::{LaneID, LaneType, Map, TurnID};
 use multimap::MultiMap;
 use std;
@@ -189,6 +190,9 @@ impl SimQueue {
             return Vec::new();
         }
 
+        // TODO base this on actual speed ;)
+        let stopping_dist = Vehicle::typical_car().stopping_distance(SPEED_LIMIT);
+
         let mut results = Vec::new();
         let (pos1, angle1, dist_along1) =
             sim.cars[&self.cars_queue[0]].get_best_case_pos(time, map);
@@ -200,6 +204,7 @@ impl SimQueue {
             map,
             pos1,
             angle1,
+            stopping_dist,
         ));
         let mut dist_along_bound = dist_along1;
 
@@ -212,6 +217,7 @@ impl SimQueue {
                     map,
                     pos,
                     angle,
+                    stopping_dist,
                 ));
                 dist_along_bound = dist_along;
             } else {
@@ -225,6 +231,7 @@ impl SimQueue {
                     map,
                     pt,
                     angle,
+                    stopping_dist,
                 ));
             }
         }

sim/src/kinematics.rs

@@ -0,0 +1,45 @@
+use dimensioned::si;
+
+pub struct Vehicle {
+    // > 0
+    max_accel: si::MeterPerSecond2<f64>,
+    // < 0
+    max_deaccel: si::MeterPerSecond2<f64>,
+}
+
+impl Vehicle {
+    pub fn typical_car() -> Vehicle {
+        Vehicle {
+            max_accel: 2.7 * si::MPS2,
+            max_deaccel: -2.7 * si::MPS2,
+        }
+    }
+
+    pub fn stopping_distance(&self, speed: si::MeterPerSecond<f64>) -> si::Meter<f64> {
+        // v_f = v_0 + a*t
+        // TODO why isn't this part negative?
+        let stopping_time = speed / self.max_accel;
+        dist_at_constant_accel(self.max_deaccel, stopping_time, speed)
+    }
+}
+
+fn dist_at_constant_accel(
+    accel: si::MeterPerSecond2<f64>,
+    time: si::Second<f64>,
+    initial_speed: si::MeterPerSecond<f64>,
+) -> si::Meter<f64> {
+    // Don't deaccelerate into going backwards, just cap things off.
+    let actual_time = if accel >= 0.0 * si::MPS2 {
+        time
+    } else {
+        min(time, -1.0 * initial_speed / accel)
+    };
+    (initial_speed * actual_time) + (0.5 * accel * (actual_time * actual_time))
+}
+
+fn min(t1: si::Second<f64>, t2: si::Second<f64>) -> si::Second<f64> {
+    if t1 < t2 {
+        return t1;
+    }
+    t2
+}

sim/src/lib.rs

@@ -22,6 +22,7 @@ mod draw_car;
 mod draw_ped;
 mod driving;
 mod intersections;
+mod kinematics;
 mod parking;
 mod sim;
 mod walking;

sim/src/parking.rs

@@ -1,3 +1,4 @@
+use dimensioned::si;
 use draw_car;
 use draw_car::DrawCar;
 use map_model;
@@ -145,7 +146,7 @@ impl ParkingLane {
                     let (front, angle) = l.dist_along(
                         spot_start - (map_model::PARKING_SPOT_LENGTH - draw_car::CAR_LENGTH) / 2.0,
                     );
-                    Some(DrawCar::new(id, None, map, front, angle))
+                    Some(DrawCar::new(id, None, map, front, angle, 0.0 * si::M))
                 })
             })
             .collect()