From 23db7cc646287168c511541e6261b509202083c4 Mon Sep 17 00:00:00 2001 From: Dustin Carlino Date: Wed, 30 Jan 2019 22:24:47 -0800 Subject: [PATCH] try really dumb trimming... it works :O --- docs/TODO_quality.md | 8 +++++++- geom/src/angle.rs | 11 ++++++----- geom/src/lib.rs | 4 ++++ geom/src/line.rs | 16 ++++++++-------- geom/src/polyline.rs | 4 ++++ geom/src/pt.rs | 6 +++--- geom/src/units.rs | 9 +++++---- sim/src/kinematics.rs | 4 ++-- 8 files changed, 39 insertions(+), 23 deletions(-) diff --git a/docs/TODO_quality.md b/docs/TODO_quality.md index fd731f9e4d..8a785f5af0 100644 --- a/docs/TODO_quality.md +++ b/docs/TODO_quality.md @@ -5,9 +5,14 @@ - try fixed pt again, for determinism purposes mostly - very different approaches - v1: keep f64 internally, but constantly drop small bits + - we just want deterministic serialization really, so... - v2: try some rational number thing internally - - clamp distances first, not points? + - retry integers for everything (probably all at once) + - dont round too early... line intersection, dist to pt, kinematics + - adjust epsilon values to match actual smallest difference, or start doing direct comparisons now + + - clamp distances first, not points? This one is working well! - note contains_pt needs to use 2 or 3 * epsilon, because of the error that may accumulate... - audit all EPSILON_DIST usages - make natively orderable @@ -34,6 +39,7 @@ - VERY overeager... ate half of the map - can we capture snapshots of incremental changes? - save initial map at every step, be able to load raw + initial with a focus point + - generic viewer should be easy... something that stores polygon and ID, wraps the quadtree, etc - try merging the shortest roads first - deal with loop roads diff --git a/geom/src/angle.rs b/geom/src/angle.rs index 05871241a6..e66bec316a 100644 --- a/geom/src/angle.rs +++ b/geom/src/angle.rs @@ -9,19 +9,20 @@ pub struct Angle(f64); impl Angle { pub(crate) fn new(rads: f64) -> Angle { - Angle(rads) + // Retain more precision for angles... + Angle((rads * 10_000_000.0).round() / 10_000_000.0) } pub fn new_degs(degs: f64) -> Angle { - Angle(degs.to_radians()) + Angle::new(degs.to_radians()) } pub fn opposite(self) -> Angle { - Angle(self.0 + f64::consts::PI) + Angle::new(self.0 + f64::consts::PI) } pub fn rotate_degs(self, degrees: f64) -> Angle { - Angle(self.0 + degrees.to_radians()) + Angle::new(self.0 + degrees.to_radians()) } pub fn normalized_radians(self) -> f64 { @@ -64,6 +65,6 @@ impl std::ops::Sub for Angle { type Output = Angle; fn sub(self, other: Angle) -> Angle { - Angle(self.0 - other.0) + Angle::new(self.0 - other.0) } } diff --git a/geom/src/lib.rs b/geom/src/lib.rs index 650752bc66..5ea752d655 100644 --- a/geom/src/lib.rs +++ b/geom/src/lib.rs @@ -18,3 +18,7 @@ pub use crate::units::{Acceleration, Distance, Duration, Speed}; // About 0.4 inches... which is quite tiny on the scale of things. :) pub const EPSILON_DIST: Distance = Distance::const_meters(0.01); + +pub(crate) fn trim_f64(x: f64) -> f64 { + (x * 10_000.0).round() / 10_000.0 +} diff --git a/geom/src/line.rs b/geom/src/line.rs index 7d9f6e7393..4f2cd95589 100644 --- a/geom/src/line.rs +++ b/geom/src/line.rs @@ -188,7 +188,7 @@ impl Line { pub fn dist_along_of_point(&self, pt: Pt2D) -> Option { let dist1 = self.pt1().dist_to(pt); let dist2 = pt.dist_to(self.pt2()); - if (dist1 + dist2 - self.length()).abs() < EPSILON_DIST { + if (dist1 + dist2 - self.length()).abs() < EPSILON_DIST * 3.0 { Some(dist1) } else { None @@ -217,26 +217,26 @@ impl InfiniteLine { // https://stackoverflow.com/a/565282 by way of // https://github.com/ucarion/line_intersection/blob/master/src/lib.rs pub fn intersection(&self, other: &InfiniteLine) -> Option { - fn cross(a: Pt2D, b: Pt2D) -> f64 { - a.x() * b.y() - a.y() * b.x() + fn cross(a: (f64, f64), b: (f64, f64)) -> f64 { + a.0 * b.1 - a.1 * b.0 } let p = self.0; let q = other.0; - let r = Pt2D::new(self.1.x() - self.0.x(), self.1.y() - self.0.y()); - let s = Pt2D::new(other.1.x() - other.0.x(), other.1.y() - other.0.y()); + let r = (self.1.x() - self.0.x(), self.1.y() - self.0.y()); + let s = (other.1.x() - other.0.x(), other.1.y() - other.0.y()); let r_cross_s = cross(r, s); - let q_minus_p = Pt2D::new(q.x() - p.x(), q.y() - p.y()); + let q_minus_p = (q.x() - p.x(), q.y() - p.y()); //let q_minus_p_cross_r = cross(q_minus_p, r); if r_cross_s == 0.0 { // Parallel None } else { - let t = cross(q_minus_p, Pt2D::new(s.x() / r_cross_s, s.y() / r_cross_s)); + let t = cross(q_minus_p, (s.0 / r_cross_s, s.1 / r_cross_s)); //let u = cross(q_minus_p, Pt2D::new(r.x() / r_cross_s, r.y() / r_cross_s)); - Some(Pt2D::new(p.x() + t * r.x(), p.y() + t * r.y())) + Some(Pt2D::new(p.x() + t * r.0, p.y() + t * r.1)) } } } diff --git a/geom/src/polyline.rs b/geom/src/polyline.rs index 214d7a90ba..eab868e230 100644 --- a/geom/src/polyline.rs +++ b/geom/src/polyline.rs @@ -126,6 +126,10 @@ impl PolyLine { result.pop(); } result.push(last_pt); + if result.len() == 1 { + // TODO Understand what happened here. + return None; + } return Some((PolyLine::new(result), Distance::ZERO)); } diff --git a/geom/src/pt.rs b/geom/src/pt.rs index c16572e00f..5210366bb7 100644 --- a/geom/src/pt.rs +++ b/geom/src/pt.rs @@ -1,4 +1,4 @@ -use crate::{Angle, Distance, GPSBounds, LonLat}; +use crate::{trim_f64, Angle, Distance, GPSBounds, LonLat}; use aabb_quadtree::geom::{Point, Rect}; use ordered_float::NotNan; use serde_derive::{Deserialize, Serialize}; @@ -21,8 +21,8 @@ impl Pt2D { // TODO enforce >=0 Pt2D { - inner_x: x, - inner_y: y, + inner_x: trim_f64(x), + inner_y: trim_f64(y), } } diff --git a/geom/src/units.rs b/geom/src/units.rs index 8ab70662eb..75d68946a9 100644 --- a/geom/src/units.rs +++ b/geom/src/units.rs @@ -1,3 +1,4 @@ +use crate::trim_f64; use ordered_float::NotNan; use serde_derive::{Deserialize, Serialize}; use std::{f64, fmt, ops}; @@ -15,7 +16,7 @@ impl Distance { panic!("Bad Distance {}", value); } - Distance(value) + Distance(trim_f64(value)) } // TODO Can't panic inside a const fn, seemingly. Don't pass in anything bad! @@ -136,7 +137,7 @@ impl Duration { panic!("Bad Duration {}", value); } - Duration(value) + Duration(trim_f64(value)) } pub const fn const_seconds(value: f64) -> Duration { @@ -210,7 +211,7 @@ impl Speed { panic!("Bad Speed {}", value); } - Speed(value) + Speed(trim_f64(value)) } pub const fn const_meters_per_second(value: f64) -> Speed { @@ -299,7 +300,7 @@ impl Acceleration { panic!("Bad Acceleration {}", value); } - Acceleration(value) + Acceleration(trim_f64(value)) } pub const fn const_meters_per_second_squared(value: f64) -> Acceleration { diff --git a/sim/src/kinematics.rs b/sim/src/kinematics.rs index 8ba9439b6a..f384f057c1 100644 --- a/sim/src/kinematics.rs +++ b/sim/src/kinematics.rs @@ -6,7 +6,7 @@ use rand::Rng; use rand_xorshift::XorShiftRng; use serde_derive::{Deserialize, Serialize}; -pub const EPSILON_SPEED: Speed = Speed::const_meters_per_second(0.000_000_01); +pub const EPSILON_SPEED: Speed = Speed::const_meters_per_second(0.0_001); // http://pccsc.net/bicycle-parking-info/ says 68 inches, which is 1.73m const MIN_BIKE_LENGTH: Distance = Distance::const_meters(1.7); @@ -180,7 +180,7 @@ impl Vehicle { // absurd amount of time to finish, with tiny little steps. But need to tune and understand // this value better. Higher initial speeds or slower max_deaccel's mean this is naturally // going to take longer. We don't want to start stopping now if we can't undo it next tick. - if !required_time.is_nan() && Duration::seconds(required_time) < Duration::seconds(15.0) { + if required_time.is_finite() && Duration::seconds(required_time) < Duration::seconds(15.0) { return Ok(normal_case); }