Allow dodging from a standstill

whatisaphone · Dec 1, 2018 · cc73eed · cc73eed
1 parent e51b85a
commit cc73eed
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 2 deletions.
diff --git a/brain/src/routing/models.rs b/brain/src/routing/models.rs
@@ -52,6 +52,8 @@ impl CarState2D {
     }
 
     pub fn to_3d(&self) -> CarState {
+        assert!(!self.loc.x.is_nan());
+        assert!(!self.vel.x.is_nan());
         CarState {
             loc: self.loc.to_3d(rl::OCTANE_NEUTRAL_Z),
             rot: self.rot.around_z_axis(),

diff --git a/brain/src/routing/segments/forward_dodge.rs b/brain/src/routing/segments/forward_dodge.rs
@@ -22,10 +22,15 @@ impl SegmentPlan for ForwardDodge {
     }
 
     fn end(&self) -> CarState {
+        assert!((self.start.vel.norm() - self.dodge.start_speed).abs() < 1.0);
+        assert!(self.dodge.end_speed >= self.dodge.start_speed);
+        let forward_axis = self.start.forward_axis_2d().unwrap();
+        let vel =
+            self.start.vel.to_2d() + forward_axis * (self.dodge.end_speed - self.dodge.start_speed);
         CarState2D {
-            loc: self.start.loc.to_2d() + self.start.vel.to_2d().normalize() * self.dodge.end_dist,
+            loc: self.start.loc.to_2d() + vel.normalize() * self.dodge.end_dist,
             rot: self.start.rot.to_2d(),
-            vel: self.start.vel.to_2d().normalize() * self.dodge.end_speed,
+            vel,
             boost: self.start.boost,
         }
         .to_3d()
@@ -100,3 +105,26 @@ impl SegmentRunner for ForwardDodgeRunner {
         }
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use nalgebra::{Point2, UnitComplex, Vector2};
+    use simulate::CarForwardDodge;
+
+    #[test]
+    fn zero_vel() {
+        let start = CarState2D {
+            loc: Point2::origin(),
+            rot: UnitComplex::identity(),
+            vel: Vector2::zeros(),
+            boost: 0.0,
+        }
+        .to_3d();
+        let dodge = CarForwardDodge::calc_1d(0.0);
+        let segment = ForwardDodge::new(start, dodge);
+        let end = segment.end();
+        assert!(end.loc.x >= 500.0);
+        assert_eq!(end.vel.x, 500.0);
+    }
+}
diff --git a/simulate/src/car_forward_dodge.rs b/simulate/src/car_forward_dodge.rs
@@ -14,6 +14,7 @@ impl CarForwardDodge {
         let land_loc = dodge_loc + dodge_vel * LANDING_TIME;
 
         CarForwardDodge1D {
+            start_speed,
             end_dist: land_loc,
             end_speed: dodge_vel,
             jump_duration: JUMP_TIME,
@@ -25,6 +26,7 @@ impl CarForwardDodge {
 
 #[derive(Clone)]
 pub struct CarForwardDodge1D {
+    pub start_speed: f32,
     pub end_dist: f32,
     pub end_speed: f32,
     pub jump_duration: f32,