stop right BEFORE intersections

geom/src/lib.rs

@@ -12,11 +12,40 @@ mod gps;
 mod line;
 mod polyline;
 mod pt;
-mod util;
 
 pub use angle::Angle;
 pub use bounds::Bounds;
+use dimensioned::si;
 pub use gps::LonLat;
 pub use line::Line;
 pub use polyline::PolyLine;
 pub use pt::{HashablePt2D, Pt2D};
+use std::marker;
+
+pub(crate) const EPSILON_DIST: si::Meter<f64> = si::Meter {
+    value_unsafe: 0.00001,
+    _marker: marker::PhantomData,
+};
+
+// NOT segment. Fails for parallel lines.
+// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
+pub(crate) fn line_intersection(l1: &Line, l2: &Line) -> Option<Pt2D> {
+    let x1 = l1.pt1().x();
+    let y1 = l1.pt1().y();
+    let x2 = l1.pt2().x();
+    let y2 = l1.pt2().y();
+
+    let x3 = l2.pt1().x();
+    let y3 = l2.pt1().y();
+    let x4 = l2.pt2().x();
+    let y4 = l2.pt2().y();
+
+    let numer_x = (x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4);
+    let numer_y = (x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4);
+    let denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+    if denom == 0.0 {
+        None
+    } else {
+        Some(Pt2D::new(numer_x / denom, numer_y / denom))
+    }
+}

geom/src/line.rs

@@ -1,5 +1,5 @@
 use dimensioned::si;
-use {util, Angle, Pt2D};
+use {line_intersection, Angle, Pt2D, EPSILON_DIST};
 
 // Segment, technically
 #[derive(Serialize, Deserialize, Debug)]
@@ -33,7 +33,7 @@ impl Line {
         if !self.intersects(other) {
             None
         } else {
-            util::line_intersection(self, other)
+            line_intersection(self, other)
         }
     }
 
@@ -63,7 +63,7 @@ impl Line {
 
     pub fn dist_along(&self, dist: si::Meter<f64>) -> Pt2D {
         let len = self.length();
-        if dist > len + util::EPSILON_METERS {
+        if dist > len + EPSILON_DIST {
             panic!("cant do {} along a line of length {}", dist, len);
         }
 
@@ -98,9 +98,9 @@ impl Line {
     pub fn contains_pt(&self, pt: Pt2D) -> bool {
         let dist = Line(self.0, pt).length() + Line(pt, self.1).length() - self.length();
         if dist < 0.0 * si::M {
-            -1.0 * dist < util::EPSILON_METERS
+            -1.0 * dist < EPSILON_DIST
         } else {
-            dist < util::EPSILON_METERS
+            dist < EPSILON_DIST
         }
     }
 }

geom/src/polyline.rs

@@ -2,7 +2,7 @@ use dimensioned::si;
 use graphics::math::Vec2d;
 use std::f64;
 use std::fmt;
-use {util, Angle, Line, Pt2D};
+use {line_intersection, Angle, Line, Pt2D, EPSILON_DIST};
 
 #[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
 pub struct PolyLine {
@@ -59,7 +59,7 @@ impl PolyLine {
             let l = Line::new(pair[0], pair[1]);
             let length = l.length();
             let epsilon = if idx == self.pts.len() - 2 {
-                util::EPSILON_METERS
+                EPSILON_DIST
             } else {
                 0.0 * si::M
             };
@@ -115,7 +115,7 @@ impl PolyLine {
             let l2 = Line::new(pt2_raw, pt3_raw).shift(width);
             // When the lines are perfectly parallel, it means pt2_shift_1st == pt2_shift_2nd and the
             // original geometry is redundant.
-            let pt2_shift = util::line_intersection(&l1, &l2).unwrap_or(l1.pt2());
+            let pt2_shift = line_intersection(&l1, &l2).unwrap_or(l1.pt2());
 
             if pt3_idx == 2 {
                 result.push(l1.pt1());
@@ -248,8 +248,8 @@ impl fmt::Display for PolyLine {
 
 #[test]
 fn shift_polyline_equivalence() {
+    use line_intersection;
     use rand;
-    use util::line_intersection;
 
     let scale = 1000.0;
     let pt1 = Pt2D::new(rand::random::<f64>() * scale, rand::random::<f64>() * scale);

geom/src/util.rs

@@ -1,31 +0,0 @@
-use dimensioned::si;
-use std::marker;
-use {Line, Pt2D};
-
-pub(crate) const EPSILON_METERS: si::Meter<f64> = si::Meter {
-    value_unsafe: 0.00001,
-    _marker: marker::PhantomData,
-};
-
-// NOT segment. Fails for parallel lines.
-// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
-pub(crate) fn line_intersection(l1: &Line, l2: &Line) -> Option<Pt2D> {
-    let x1 = l1.pt1().x();
-    let y1 = l1.pt1().y();
-    let x2 = l1.pt2().x();
-    let y2 = l1.pt2().y();
-
-    let x3 = l2.pt1().x();
-    let y3 = l2.pt1().y();
-    let x4 = l2.pt2().x();
-    let y4 = l2.pt2().y();
-
-    let numer_x = (x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4);
-    let numer_y = (x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4);
-    let denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
-    if denom == 0.0 {
-        None
-    } else {
-        Some(Pt2D::new(numer_x / denom, numer_y / denom))
-    }
-}

headless/src/main.rs

@@ -38,12 +38,10 @@ fn main() {
     sim.seed_pedestrians(&map, 100);
     sim.start_many_parked_cars(&map, 100);
 
-    let mut counter = 0;
     let mut benchmark = sim.start_benchmark();
     loop {
-        counter += 1;
         sim.step(&map, &control_map);
-        if counter % 1000 == 0 {
+        if sim.time.is_multiple_of_minute() {
             let speed = sim.measure_speed(&mut benchmark);
             println!("{0}, speed = {1:.2}x", sim.summary(), speed);
         }

sim/Cargo.toml

@@ -11,6 +11,7 @@ dimensioned = { git = "https://github.com/paholg/dimensioned", rev = "0e1076ebfa
 ezgui = { path = "../ezgui" }
 geom = { path = "../geom" }
 map_model = { path = "../map_model" }
+more-asserts = "0.2.1"
 multimap = "0.4.0"
 ordered-float = "0.5.0"
 piston2d-graphics = "*"

sim/src/driving.rs

@@ -162,7 +162,12 @@ impl Car {
 
         loop {
             let leftover_dist = self.dist_along - self.on.length(map);
-            if leftover_dist < 0.0 * si::M {
+            // == 0.0 is important! If no floating point imprecision happens, cars will stop RIGHT
+            // at the end of a lane, with exactly 0 leftover distance. We don't want to bump them
+            // into the turn and illegally enter the intersection in that case. The alternative
+            // from AORTA, IIRC, is to make cars stop anywhere in a small buffer at the end of the
+            // lane.
+            if leftover_dist <= 0.0 * si::M {
                 break;
             }
             let next_on = match self.on {
@@ -178,7 +183,15 @@ impl Car {
             self.waiting_for = None;
             self.on = next_on;
             if let On::Turn(t) = self.on {
-                intersections.on_enter(Request::for_car(self.id, t))?;
+                // TODO easier way to attach more debug info?
+                intersections
+                    .on_enter(Request::for_car(self.id, t))
+                    .map_err(|e| {
+                        InvariantViolated(format!(
+                            "{}. new speed {}, leftover dist {}",
+                            e, self.speed, leftover_dist
+                        ))
+                    })?;
             }
             self.dist_along = leftover_dist;
         }
@@ -213,7 +226,7 @@ impl SimQueue {
     ) -> Result<(), InvariantViolated> {
         let old_queue = self.cars_queue.clone();
 
-        assert!(ids.len() <= self.capacity);
+        assert_le!(ids.len(), self.capacity);
         self.cars_queue.clear();
         self.cars_queue.extend(ids);
         // Sort descending.

sim/src/intersections.rs

@@ -298,6 +298,7 @@ impl TrafficSignal {
                 continue;
             }
             // How long will it take the agent to cross the turn?
+            // TODO assuming they accelerate!
             let crossing_time = turn.length() / speeds[&agent];
             // TODO account for TIMESTEP
 

sim/src/kinematics.rs

@@ -1,8 +1,15 @@
 use dimensioned::si;
 use models::FOLLOWING_DISTANCE;
+use std;
 use {Acceleration, Distance, Speed, Time, TIMESTEP};
 
+pub const EPSILON_SPEED: Speed = si::MeterPerSecond {
+    value_unsafe: 0.00000001,
+    _marker: std::marker::PhantomData,
+};
+
 // TODO unit test all of this
+// TODO handle floating point issues uniformly here
 
 pub struct Vehicle {
     // > 0
@@ -36,7 +43,11 @@ impl Vehicle {
 
     // TODO this needs unit tests and some careful checking
     pub fn accel_to_stop_in_dist(&self, speed: Speed, dist: Distance) -> Acceleration {
-        assert!(dist > 0.0 * si::M);
+        assert_ge!(dist, 0.0 * si::M);
+        // Don't NaN out
+        if dist == 0.0 * si::M {
+            return 0.0 * si::MPS2;
+        }
 
         // d = (v_1)(t) + (1/2)(a)(t^2)
         // 0 = (v_1) + (a)(t)
@@ -60,7 +71,7 @@ impl Vehicle {
     // Assume we accelerate as much as possible this tick (restricted only by the speed limit),
     // then stop as fast as possible.
     pub fn max_lookahead_dist(&self, current_speed: Speed, speed_limit: Speed) -> Distance {
-        assert!(current_speed <= speed_limit);
+        assert_le!(current_speed, speed_limit);
         let max_next_accel = min_accel(self.max_accel, (speed_limit - current_speed) / TIMESTEP);
         let max_next_dist = dist_at_constant_accel(max_next_accel, TIMESTEP, current_speed);
         let max_next_speed = current_speed + max_next_accel * TIMESTEP;
@@ -69,7 +80,7 @@ impl Vehicle {
 
     // TODO share with max_lookahead_dist
     fn max_next_dist(&self, current_speed: Speed, speed_limit: Speed) -> Distance {
-        assert!(current_speed <= speed_limit);
+        assert_le!(current_speed, speed_limit);
         let max_next_accel = min_accel(self.max_accel, (speed_limit - current_speed) / TIMESTEP);
         dist_at_constant_accel(max_next_accel, TIMESTEP, current_speed)
     }
@@ -154,9 +165,13 @@ pub fn results_of_accel_for_one_tick(
         min_time(TIMESTEP, -1.0 * initial_speed / accel)
     };
     let dist = (initial_speed * actual_time) + (0.5 * accel * (actual_time * actual_time));
-    assert!(dist >= 0.0 * si::M);
-    let new_speed = initial_speed + (accel * actual_time);
-    assert!(new_speed >= 0.0 * si::MPS);
+    assert_ge!(dist, 0.0 * si::M);
+    let mut new_speed = initial_speed + (accel * actual_time);
+    // Handle some floating point imprecision
+    if new_speed < 0.0 * si::MPS && new_speed >= -1.0 * EPSILON_SPEED {
+        new_speed = 0.0 * si::MPS;
+    }
+    assert_ge!(new_speed, 0.0 * si::MPS);
     (dist, new_speed)
 }
 

sim/src/lib.rs

@@ -9,6 +9,8 @@ extern crate ezgui;
 extern crate geom;
 extern crate graphics;
 extern crate map_model;
+#[macro_use]
+extern crate more_asserts;
 extern crate multimap;
 extern crate ordered_float;
 #[macro_use]
@@ -86,6 +88,11 @@ impl Tick {
     pub fn increment(&mut self) {
         self.0 += 1;
     }
+
+    // TODO er, little weird
+    pub fn is_multiple_of_minute(&self) -> bool {
+        self.0 % 600 == 0
+    }
 }
 
 impl std::ops::Sub for Tick {