diff --git a/geom/src/lib.rs b/geom/src/lib.rs index 30e2d8e2b2..c7511d6f87 100644 --- a/geom/src/lib.rs +++ b/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 = 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 { + 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)) + } +} diff --git a/geom/src/line.rs b/geom/src/line.rs index a64e84cf50..95685da8d3 100644 --- a/geom/src/line.rs +++ b/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) -> 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 } } } diff --git a/geom/src/polyline.rs b/geom/src/polyline.rs index a2e738d19e..3c7a0c689e 100644 --- a/geom/src/polyline.rs +++ b/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::() * scale, rand::random::() * scale); diff --git a/geom/src/util.rs b/geom/src/util.rs deleted file mode 100644 index dcb0f8aa0a..0000000000 --- a/geom/src/util.rs +++ /dev/null @@ -1,31 +0,0 @@ -use dimensioned::si; -use std::marker; -use {Line, Pt2D}; - -pub(crate) const EPSILON_METERS: si::Meter = 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 { - 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)) - } -} diff --git a/headless/src/main.rs b/headless/src/main.rs index fc43ae0c72..4d120a354b 100644 --- a/headless/src/main.rs +++ b/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); } diff --git a/sim/Cargo.toml b/sim/Cargo.toml index 8c257c7437..7179478299 100644 --- a/sim/Cargo.toml +++ b/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 = "*" diff --git a/sim/src/driving.rs b/sim/src/driving.rs index f6fa173958..8fd0783346 100644 --- a/sim/src/driving.rs +++ b/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. diff --git a/sim/src/intersections.rs b/sim/src/intersections.rs index 9939882b68..abc2c492bd 100644 --- a/sim/src/intersections.rs +++ b/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 diff --git a/sim/src/kinematics.rs b/sim/src/kinematics.rs index cb8d5f745b..21d9ae3535 100644 --- a/sim/src/kinematics.rs +++ b/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) } diff --git a/sim/src/lib.rs b/sim/src/lib.rs index 2306f318b5..4098dfdcb1 100644 --- a/sim/src/lib.rs +++ b/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 {