Velocity-corrected bezier pathfinding control, swimming control

common/src/path.rs

@@ -59,31 +59,96 @@ impl From<Path<Vec3<i32>>> for Route {
 impl Route {
     pub fn path(&self) -> &Path<Vec3<i32>> { &self.path }
 
-    pub fn next(&self) -> Option<Vec3<i32>> { self.path.nodes.get(self.next_idx).copied() }
+    pub fn next(&self, i: usize) -> Option<Vec3<i32>> {
+        self.path.nodes.get(self.next_idx + i).copied()
+    }
 
-    pub fn is_finished(&self) -> bool { self.next().is_none() }
+    pub fn is_finished(&self) -> bool { self.next(0).is_none() }
 
     pub fn traverse<V>(
         &mut self,
         vol: &V,
         pos: Vec3<f32>,
+        vel: Vec3<f32>,
         traversal_tolerance: f32,
-    ) -> Option<Vec3<f32>>
+    ) -> Option<(Vec3<f32>, f32)>
     where
         V: BaseVol<Vox = Block> + ReadVol,
     {
-        let next = self.next()?;
-        if vol.get(next).map(|b| b.is_solid()).unwrap_or(false) {
+        let next0 = self.next(0).unwrap_or(pos.map(|e| e.floor() as i32));
+        let next1 = self.next(1).unwrap_or(next0);
+        if vol.get(next0).map(|b| b.is_solid()).unwrap_or(false) {
             None
         } else {
-            let next_tgt = next.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0);
+            let next_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0);
             if pos.xy().distance_squared(next_tgt.xy()) < traversal_tolerance.powf(2.0)
                 && next_tgt.z - pos.z < 0.2
                 && next_tgt.z - pos.z > -2.2
+                && vel.z <= 0.0
+                && vol
+                    .ray(pos + Vec3::unit_z() * 0.5, next_tgt + Vec3::unit_z() * 0.5)
+                    .until(|block| block.is_solid())
+                    .cast()
+                    .0
+                    > pos.distance(next_tgt) * 0.9
             {
                 self.next_idx += 1;
             }
-            Some(next_tgt - pos)
+
+            let line = LineSegment2 {
+                start: pos.xy(),
+                end: pos.xy() + vel.xy() * 100.0,
+            };
+
+            let align = |block_pos: Vec3<i32>| {
+                (0..2)
+                    .map(|i| (0..2).map(move |j| Vec2::new(i, j)))
+                    .flatten()
+                    .map(|rpos| block_pos + rpos)
+                    .map(|block_pos| {
+                        let block_posf = block_pos.xy().map(|e| e as f32);
+                        let proj = line.projected_point(block_posf);
+                        let clamped = proj.clamped(
+                            block_pos.xy().map(|e| e as f32),
+                            block_pos.xy().map(|e| e as f32),
+                        );
+
+                        (proj.distance_squared(clamped), clamped)
+                    })
+                    .min_by_key(|(d2, _)| (d2 * 1000.0) as i32)
+                    .unwrap()
+                    .1
+            };
+
+            let cb = CubicBezier2 {
+                start: pos.xy(),
+                ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or(Vec2::zero()),
+                ctrl1: align(next0),
+                end: align(next1),
+            };
+
+            let tgt2d = cb.evaluate(0.5);
+            let tgt = Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z;
+            let tgt_dir = (tgt - pos).xy().try_normalized().unwrap_or(Vec2::unit_y());
+            let next_dir = cb
+                .evaluate_derivative(0.5)
+                .try_normalized()
+                .unwrap_or(tgt_dir);
+
+            //let vel_dir = vel.xy().try_normalized().unwrap_or(Vec2::zero());
+            //let avg_dir = (tgt_dir * 0.2 + vel_dir *
+            // 0.8).try_normalized().unwrap_or(Vec2::zero()); let bearing =
+            // Vec3::<f32>::from(avg_dir * (tgt - pos).xy().magnitude()) + Vec3::unit_z() *
+            // (tgt.z - pos.z);
+
+            Some((
+                tgt - pos,
+                next_dir
+                    .dot(vel.xy().try_normalized().unwrap_or(Vec2::zero()))
+                    .max(0.0)
+                    * 0.75
+                    + 0.25,
+            ))
         }
     }
 }
@@ -106,10 +171,11 @@ impl Chaser {
         &mut self,
         vol: &V,
         pos: Vec3<f32>,
+        vel: Vec3<f32>,
         tgt: Vec3<f32>,
         min_dist: f32,
         traversal_tolerance: f32,
-    ) -> Option<Vec3<f32>>
+    ) -> Option<(Vec3<f32>, f32)>
     where
         V: BaseVol<Vox = Block> + ReadVol,
     {
@@ -125,14 +191,12 @@ impl Chaser {
                 None
             } else {
                 if thread_rng().gen::<f32>() < 0.005 {
+                    // Randomly repath to avoid getting stuck
                     None
                 } else {
                     self.route
                         .as_mut()
-                        .and_then(|r| r.traverse(vol, pos, traversal_tolerance))
-                        .filter(|b| {
-                            b.xy().magnitude_squared() < (traversal_tolerance + 1.0).powf(2.0)
-                        })
+                        .and_then(|r| r.traverse(vol, pos, vel, traversal_tolerance))
                 }
             }
         } else {
@@ -156,7 +220,7 @@ impl Chaser {
                 }
             }
 
-            Some((tgt - pos) * Vec3::new(1.0, 1.0, 0.0))
+            Some(((tgt - pos) * Vec3::new(1.0, 1.0, 0.0), 0.75))
         }
     }
 }
@@ -276,8 +340,16 @@ where
                     .map(move |(dir, _)| pos + *dir),
             )
     };
-    let transition =
-        |_a: &Vec3<i32>, b: &Vec3<i32>| 1.0 + endf.distance((*b).map(|e| e as f32 + 0.5)) * 0.02;
+
+    let crow_line = LineSegment2 {
+        start: startf.xy(),
+        end: endf.xy(),
+    };
+
+    let transition = |a: &Vec3<i32>, b: &Vec3<i32>| {
+        1.0 + crow_line.distance_to_point(b.xy().map(|e| e as f32)) * 0.025
+            + (b.z - a.z - 1).max(0) as f32 * 3.0
+    };
     let satisfied = |pos: &Vec3<i32>| pos == &end;
 
     let mut new_astar = match astar.take() {