Velocity-corrected bezier pathfinding control, swimming control

common/src/comp/agent.rs

@@ -81,7 +81,6 @@ pub enum Activity {
     Follow {
         target: EcsEntity,
         chaser: Chaser,
-        move_dir: Vec2<f32>,
     },
     Attack {
         target: EcsEntity,

common/src/comp/controller.rs

@@ -157,6 +157,7 @@ pub struct ControllerInputs {
     pub wall_leap: Input,
     pub charge: Input,
     pub climb: Option<Climb>,
+    pub swim: Input,
     pub move_dir: Vec2<f32>,
     pub look_dir: Dir,
 }
@@ -180,6 +181,7 @@ impl ControllerInputs {
         self.glide.tick(dt);
         self.wall_leap.tick(dt);
         self.charge.tick(dt);
+        self.swim.tick(dt);
     }
 
     pub fn tick_freshness(&mut self) {
@@ -191,6 +193,7 @@ impl ControllerInputs {
         self.glide.tick_freshness();
         self.wall_leap.tick_freshness();
         self.charge.tick_freshness();
+        self.swim.tick_freshness();
     }
 
     /// Updates Controller inputs with new version received from the client
@@ -204,6 +207,7 @@ impl ControllerInputs {
         self.wall_leap.update_with_new(new.wall_leap);
         self.charge.update_with_new(new.charge);
         self.climb = new.climb;
+        self.swim.update_with_new(new.swim);
         self.move_dir = new.move_dir;
         self.look_dir = new.look_dir;
     }

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()?;
+        let next0 = self.next(0).unwrap_or(pos.map(|e| e.floor() as i32));
-        if vol.get(next).map(|b| b.is_solid()).unwrap_or(false) {
+        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))
+                        .and_then(|r| r.traverse(vol, pos, vel, traversal_tolerance))
-                        .filter(|b| {
-                            b.xy().magnitude_squared() < (traversal_tolerance + 1.0).powf(2.0)
-                        })
                 }
             }
         } 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() {

common/src/states/utils.rs

@@ -100,7 +100,7 @@ fn swim_move(data: &JoinData, update: &mut StateUpdate, efficiency: f32) {
     handle_orientation(data, update, if data.physics.on_ground { 9.0 } else { 2.0 });
 
     // Swim
-    if data.inputs.jump.is_pressed() {
+    if data.inputs.swim.is_pressed() {
         update.vel.0.z =
             (update.vel.0.z + data.dt.0 * GRAVITY * 2.25).min(BASE_HUMANOID_WATER_SPEED);
     }

common/src/sys/agent.rs

@@ -126,7 +126,7 @@ impl<'a> System<'a> for Sys {
             // and so can afford to be less precise when trying to move around
             // the world (especially since they would otherwise get stuck on
             // obstacles that smaller entities would not).
-            let traversal_tolerance = scale + vel.0.magnitude() * 0.25;
+            let traversal_tolerance = scale + vel.0.magnitude() * 0.3;
 
             let mut do_idle = false;
             let mut choose_target = false;
@@ -187,30 +187,24 @@ impl<'a> System<'a> for Sys {
                             choose_target = true;
                         }
                     },
-                    Activity::Follow {
+                    Activity::Follow { target, chaser } => {
-                        target,
-                        chaser,
-                        move_dir,
-                    } => {
                         if let (Some(tgt_pos), _tgt_stats) =
                             (positions.get(*target), stats.get(*target))
                         {
                             let dist_sqrd = pos.0.distance_squared(tgt_pos.0);
                             // Follow, or return to idle
                             if dist_sqrd > AVG_FOLLOW_DIST.powf(2.0) {
-                                if let Some(bearing) = chaser.chase(
+                                if let Some((bearing, speed)) = chaser.chase(
                                     &*terrain,
                                     pos.0,
+                                    vel.0,
                                     tgt_pos.0,
                                     AVG_FOLLOW_DIST,
                                     traversal_tolerance,
                                 ) {
-                                    *move_dir = 0.9f32 * *move_dir
+                                    inputs.move_dir =
-                                        + 0.1f32
+                                        bearing.xy().try_normalized().unwrap_or(Vec2::zero())
-                                            * Vec2::from(bearing)
+                                            * speed;
-                                                .try_normalized()
-                                                .unwrap_or(Vec2::zero());
-                                    inputs.move_dir = *move_dir;
                                     inputs.jump.set_state(bearing.z > 1.5);
                                 }
                             } else {
@@ -314,16 +308,18 @@ impl<'a> System<'a> for Sys {
                                 }
 
                                 // Long-range chase
-                                if let Some(bearing) = chaser.chase(
+                                if let Some((bearing, speed)) = chaser.chase(
                                     &*terrain,
                                     pos.0,
+                                    vel.0,
                                     tgt_pos.0,
                                     1.25,
                                     traversal_tolerance,
                                 ) {
                                     inputs.move_dir = Vec2::from(bearing)
                                         .try_normalized()
-                                        .unwrap_or(Vec2::zero());
+                                        .unwrap_or(Vec2::zero())
+                                        * speed;
                                     inputs.jump.set_state(bearing.z > 1.5);
                                 }
 
@@ -429,7 +425,6 @@ impl<'a> System<'a> for Sys {
                         agent.activity = Activity::Follow {
                             target: owner,
                             chaser: Chaser::default(),
-                            move_dir: Vec2::zero(),
                         };
                     }
 

voxygen/src/session.rs

@@ -347,6 +347,9 @@ impl PlayState for SessionState {
                     Event::InputUpdate(GameInput::Jump, state) => {
                         self.inputs.jump.set_state(state);
                     },
+                    Event::InputUpdate(GameInput::Swim, state) => {
+                        self.inputs.swim.set_state(state);
+                    },
                     Event::InputUpdate(GameInput::Sit, state)
                         if state != self.key_state.toggle_sit =>
                     {

voxygen/src/settings.rs

@@ -120,6 +120,7 @@ impl ControlSettings {
             GameInput::Glide => KeyMouse::Key(VirtualKeyCode::LShift),
             GameInput::Climb => KeyMouse::Key(VirtualKeyCode::Space),
             GameInput::ClimbDown => KeyMouse::Key(VirtualKeyCode::LControl),
+            GameInput::Swim => KeyMouse::Key(VirtualKeyCode::Space),
             //GameInput::WallLeap => MIDDLE_CLICK_KEY,
             GameInput::ToggleLantern => KeyMouse::Key(VirtualKeyCode::G),
             GameInput::Mount => KeyMouse::Key(VirtualKeyCode::F),
@@ -178,6 +179,7 @@ impl Default for ControlSettings {
             GameInput::Glide,
             GameInput::Climb,
             GameInput::ClimbDown,
+            GameInput::Swim,
             //GameInput::WallLeap,
             GameInput::ToggleLantern,
             GameInput::Mount,
@@ -281,6 +283,7 @@ pub mod con_settings {
         pub glide: Button,
         pub climb: Button,
         pub climb_down: Button,
+        pub swim: Button,
         //pub wall_leap: Button,
         pub toggle_lantern: Button,
         pub mount: Button,
@@ -369,6 +372,7 @@ pub mod con_settings {
                 glide: Button::Simple(GilButton::LeftTrigger),
                 climb: Button::Simple(GilButton::South),
                 climb_down: Button::Simple(GilButton::Unknown),
+                swim: Button::Simple(GilButton::South),
                 //wall_leap: Button::Simple(GilButton::Unknown),
                 toggle_lantern: Button::Simple(GilButton::East),
                 mount: Button::Simple(GilButton::North),

voxygen/src/window.rs

@@ -39,6 +39,7 @@ pub enum GameInput {
     Glide,
     Climb,
     ClimbDown,
+    Swim,
     //WallLeap,
     ToggleLantern,
     Mount,
@@ -82,6 +83,7 @@ impl GameInput {
             GameInput::Glide => "gameinput.glide",
             GameInput::Climb => "gameinput.climb",
             GameInput::ClimbDown => "gameinput.climbdown",
+            GameInput::Swim => "gameinput.swim",
             //GameInput::WallLeap => "gameinput.wallleap",
             GameInput::ToggleLantern => "gameinput.togglelantern",
             GameInput::Mount => "gameinput.mount",