disabled DES quadratic distance... but some other improvements

editor/src/plugins/sim/des_model/car.rs

@@ -61,6 +61,16 @@ impl Car {
                 );
             }
 
+            // Now that distance isn't linear, we actually never directly use end_dist -- so sanity
+            // check that the interval works at all!
+            let actual_end_dist = i.dist(i.end_time);
+            if !actual_end_dist.epsilon_eq(i.end_dist) {
+                println!(
+                    "{} has an interval where they want to end at {}, but they really end at {}",
+                    self.id, i.end_dist, actual_end_dist
+                );
+            }
+
             if i.end_dist > lane_len {
                 println!(
                     "{} ends {} past the lane end",
@@ -68,6 +78,19 @@ impl Car {
                     i.end_dist - lane_len
                 );
             }
+
+            if i.end_dist < i.start_dist {
+                println!(
+                    "{} goes backwards from {} to {}",
+                    self.id, i.start_dist, i.end_dist
+                );
+            }
+            if i.end_time < i.start_time {
+                println!(
+                    "{} goes backwards IN TIME from {} to {}",
+                    self.id, i.start_time, i.end_time
+                );
+            }
         }
     }
 
@@ -268,9 +291,21 @@ impl Car {
         if true {
             {
                 let mut our_adjusted_last = self.intervals.pop().unwrap();
+
+                // TODO why's this happen? I think because we don't do the intersection test
+                // including dist_behind!
+                if hit_dist - dist_behind < our_adjusted_last.start_dist {
+                    println!("WHOA! actually collide an interval earlier than expected");
+                    // Woops!
+                    our_adjusted_last = self.intervals.pop().unwrap();
+                }
+
+                println!("retain these:");
+                self.dump_intervals();
                 our_adjusted_last.end_speed = hit_speed;
                 our_adjusted_last.end_time = hit_time;
                 our_adjusted_last.end_dist = hit_dist - dist_behind;
+                println!("first adjusted: {}", our_adjusted_last);
                 self.intervals.push(our_adjusted_last);
             }
 
@@ -284,6 +319,7 @@ impl Car {
                     self.intervals.last().as_ref().unwrap().end_speed,
                     them.end_speed,
                 ));
+                println!("second adjusted: {}", self.intervals.last().unwrap());
             }
         } else {
             // TODO This still causes impossible deaccel

editor/src/plugins/sim/des_model/interval.rs

@@ -51,9 +51,23 @@ impl Interval {
         }
     }
 
+    fn raw_accel(&self) -> f64 {
+        (self.end_speed - self.start_speed).inner_meters_per_second()
+            / (self.end_time - self.start_time).inner_seconds()
+    }
+
     pub fn dist(&self, t: Duration) -> Distance {
-        // Linearly interpolate
-        self.start_dist + self.percent(t) * (self.end_dist - self.start_dist)
+        if true {
+            // Linearly interpolate
+            self.start_dist + self.percent(t) * (self.end_dist - self.start_dist)
+        } else {
+            let relative_t = (t - self.start_time).inner_seconds();
+
+            let d = self.start_dist.inner_meters()
+                + self.start_speed.inner_meters_per_second() * relative_t
+                + 0.5 * self.raw_accel() * relative_t.powi(2);
+            Distance::meters(d)
+        }
     }
 
     pub fn speed(&self, t: Duration) -> Speed {
@@ -93,22 +107,54 @@ impl Interval {
         }
 
         // Set the two distance equations equal and solve for time. Long to type out here...
-        let x1 = self.start_dist.inner_meters();
-        let x2 = self.end_dist.inner_meters();
-        let a1 = self.start_time.inner_seconds();
-        let a2 = self.end_time.inner_seconds();
+        let t = if true {
+            let x1 = self.start_dist.inner_meters();
+            let x2 = self.end_dist.inner_meters();
+            let a1 = self.start_time.inner_seconds();
+            let a2 = self.end_time.inner_seconds();
 
-        let y1 = leader.start_dist.inner_meters();
-        let y2 = leader.end_dist.inner_meters();
-        let b1 = leader.start_time.inner_seconds();
-        let b2 = leader.end_time.inner_seconds();
+            let y1 = leader.start_dist.inner_meters();
+            let y2 = leader.end_dist.inner_meters();
+            let b1 = leader.start_time.inner_seconds();
+            let b2 = leader.end_time.inner_seconds();
 
-        let numer = a1 * (b2 * (y1 - x2) + b1 * (x2 - y2)) + a2 * (b2 * (x1 - y1) + b1 * (y2 - x1));
-        let denom = (a1 - a2) * (y1 - y2) + b2 * (x1 - x2) + b1 * (x2 - x1);
-        if denom == 0.0 {
-            return None;
-        }
-        let t = Duration::seconds(numer / denom);
+            let numer =
+                a1 * (b2 * (y1 - x2) + b1 * (x2 - y2)) + a2 * (b2 * (x1 - y1) + b1 * (y2 - x1));
+            let denom = (a1 - a2) * (y1 - y2) + b2 * (x1 - x2) + b1 * (x2 - x1);
+            if denom == 0.0 {
+                return None;
+            }
+            Duration::seconds(numer / denom)
+        } else {
+            let x_1 = self.start_dist.inner_meters();
+            let v_1 = self.start_speed.inner_meters_per_second();
+            let t_1 = self.start_time.inner_seconds();
+            let a_1 = self.raw_accel();
+
+            let x_3 = leader.start_dist.inner_meters();
+            let v_3 = leader.start_speed.inner_meters_per_second();
+            let t_3 = leader.start_time.inner_seconds();
+            let a_3 = leader.raw_accel();
+
+            let q = (-0.5
+                * ((2.0 * a_1 * t_1 - 2.0 * a_3 * t_3 - 2.0 * v_1 + 2.0 * v_3).powi(2)
+                    - 4.0
+                        * (a_3 - a_1)
+                        * (-a_1 * t_1.powi(2) + a_3 * t_3.powi(2) + 2.0 * t_1 * v_1
+                            - 2.0 * t_3 * v_3
+                            - 2.0 * x_1
+                            + 2.0 * x_3))
+                    .sqrt()
+                - a_1 * t_1
+                + a_3 * t_3
+                + v_1
+                - v_3)
+                / (a_3 - a_1);
+            if !q.is_finite() {
+                return None;
+            }
+            Duration::seconds(q)
+        };
 
         if !self.covers(t) || !leader.covers(t) {
             return None;
@@ -138,8 +184,12 @@ impl fmt::Display for Interval {
             "decelerate to rest"
         } else if self.start_speed == self.end_speed {
             "freeflow"
+        } else if self.start_speed < self.end_speed {
+            "speed up"
+        } else if self.start_speed > self.end_speed {
+            "slow down"
         } else {
-            "other"
+            panic!("How to describe {:?}", self);
         };
 
         write!(

editor/src/plugins/sim/des_model/mod.rs

@@ -32,7 +32,8 @@ impl World {
         leader.wait(Duration::seconds(5.0));
 
         let mut cars = vec![leader];
-        let num_followers = (lane.length() / Distance::meters(10.0)).floor() as usize;
+        //let num_followers = (lane.length() / Distance::meters(10.0)).floor() as usize;
+        let num_followers = 1;
         for i in 0..num_followers {
             let mut follower = Car {
                 id: CarID::tmp_new(cars.len(), VehicleType::Car),
@@ -58,10 +59,7 @@ impl World {
             println!();
         }
 
-        World {
-            lane: l,
-            cars,
-        }
+        World { lane: l, cars }
     }
 
     pub fn get_draw_cars(&self, time: Duration, map: &Map) -> Vec<DrawCarInput> {
@@ -91,4 +89,19 @@ impl World {
             }
         }
     }
+
+    pub fn dump_debug(&self, time: Duration) {
+        for car in &self.cars {
+            if let Some((d, idx)) = car.dist_at(time) {
+                println!(
+                    "- {} at {}, speed {}. interval {}/{}",
+                    car.id,
+                    d,
+                    car.intervals[idx].speed(time),
+                    idx + 1,
+                    car.intervals.len()
+                );
+            }
+        }
+    }
 }

editor/src/plugins/sim/simple_model.rs

@@ -87,6 +87,9 @@ impl BlockingPlugin for SimpleModelController {
         if ctx.input.modal_action("toggle tooltips") {
             self.show_tooltips = !self.show_tooltips;
         }
+        if ctx.input.modal_action("debug") {
+            self.world.dump_debug(self.current_tick.as_time());
+        }
         if ctx
             .input
             .modal_action("exhaustively test instantiation everywhere")

editor/src/ui.rs

@@ -149,6 +149,7 @@ impl<S: UIState> GUI<RenderingHints> for UI<S> {
                     (Key::M, "toggle backwards play"),
                     (Key::T, "toggle tooltips"),
                     (Key::E, "exhaustively test instantiation everywhere"),
+                    (Key::D, "debug"),
                 ],
             ),
             ModalMenu::new(