From 71b7e1ef908e0c57b9b0f934a7585e6015dce2ac Mon Sep 17 00:00:00 2001
From: Joshua Barretto <joshua.s.barretto@gmail.com>
Date: Sat, 14 Nov 2020 17:00:32 +0000
Subject: [PATCH] Better non-climbing NPC pathfinding in rivers

---
 common/src/path.rs         | 25 +++++++++++++++++++------
 common/src/states/utils.rs |  9 ++++++++-
 common/src/sys/agent.rs    | 34 ++++++++++++++--------------------
 3 files changed, 41 insertions(+), 27 deletions(-)

diff --git a/common/src/path.rs b/common/src/path.rs
index 10f6fbcdba..a3f6fd881e 100644
--- a/common/src/path.rs
+++ b/common/src/path.rs
@@ -69,6 +69,8 @@ pub struct TraversalConfig {
     pub in_liquid: bool,
     /// The distance to the target below which it is considered reached.
     pub min_tgt_dist: f32,
+    /// Whether the agent can climb.
+    pub can_climb: bool,
 }
 
 const DIAGONALS: [Vec2<i32>; 8] = [
@@ -389,7 +391,7 @@ impl Chaser {
             {
                 self.last_search_tgt = Some(tgt);
 
-                let (path, complete) = find_path(&mut self.astar, vol, pos, tgt);
+                let (path, complete) = find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg);
 
                 self.route = path.map(|path| {
                     let start_index = path
@@ -455,6 +457,7 @@ fn find_path<V>(
     vol: &V,
     startf: Vec3<f32>,
     endf: Vec3<f32>,
+    traversal_cfg: &TraversalConfig,
 ) -> (Option<Path<Vec3<i32>>>, bool)
 where
     V: BaseVol<Vox = Block> + ReadVol,
@@ -483,30 +486,33 @@ where
     let heuristic = |pos: &Vec3<i32>| (pos.distance_squared(end) as f32).sqrt();
     let neighbors = |pos: &Vec3<i32>| {
         let pos = *pos;
-        const DIRS: [Vec3<i32>; 21] = [
+        const DIRS: [Vec3<i32>; 17] = [
             Vec3::new(0, 1, 0),   // Forward
             Vec3::new(0, 1, 1),   // Forward upward
-            Vec3::new(0, 1, 2),   // Forward Upwardx2
             Vec3::new(0, 1, -1),  // Forward downward
             Vec3::new(0, 1, -2),  // Forward downwardx2
             Vec3::new(1, 0, 0),   // Right
             Vec3::new(1, 0, 1),   // Right upward
-            Vec3::new(1, 0, 2),   // Right Upwardx2
             Vec3::new(1, 0, -1),  // Right downward
             Vec3::new(1, 0, -2),  // Right downwardx2
             Vec3::new(0, -1, 0),  // Backwards
             Vec3::new(0, -1, 1),  // Backward Upward
-            Vec3::new(0, -1, 2),  // Backward Upwardx2
             Vec3::new(0, -1, -1), // Backward downward
             Vec3::new(0, -1, -2), // Backward downwardx2
             Vec3::new(-1, 0, 0),  // Left
             Vec3::new(-1, 0, 1),  // Left upward
-            Vec3::new(-1, 0, 2),  // Left Upwardx2
             Vec3::new(-1, 0, -1), // Left downward
             Vec3::new(-1, 0, -2), // Left downwardx2
             Vec3::new(0, 0, -1),  // Downwards
         ];
 
+        const JUMPS: [Vec3<i32>; 4] = [
+            Vec3::new(0, 1, 2),   // Forward Upwardx2
+            Vec3::new(1, 0, 2),   // Right Upwardx2
+            Vec3::new(0, -1, 2),  // Backward Upwardx2
+            Vec3::new(-1, 0, 2),  // Left Upwardx2
+        ];
+
         // let walkable = [
         //     is_walkable(&(pos + Vec3::new(1, 0, 0))),
         //     is_walkable(&(pos + Vec3::new(-1, 0, 0))),
@@ -526,6 +532,13 @@ where
         // ];
 
         DIRS.iter()
+            .chain(Some(JUMPS.iter())
+                .filter(|_| vol
+                    .get(pos)
+                    .map(|b| !b.is_liquid())
+                    .unwrap_or(true) || traversal_cfg.can_climb)
+                .into_iter()
+                .flatten())
             .map(move |dir| (pos, dir))
             .filter(move |(pos, dir)| {
                 is_walkable(pos)
diff --git a/common/src/states/utils.rs b/common/src/states/utils.rs
index 822d91a7e9..c57c1ce883 100644
--- a/common/src/states/utils.rs
+++ b/common/src/states/utils.rs
@@ -94,6 +94,13 @@ impl Body {
             _ => false,
         }
     }
+
+    pub fn can_climb(&self) -> bool {
+        match self {
+            Body::Humanoid(_) => true,
+            _ => false,
+        }
+    }
 }
 
 /// Handles updating `Components` to move player based on state of `JoinData`
@@ -284,7 +291,7 @@ pub fn handle_climb(data: &JoinData, update: &mut StateUpdate) {
             .map(|depth| depth > 1.0)
             .unwrap_or(false)
         //&& update.vel.0.z < 0.0
-        && data.body.is_humanoid()
+        && data.body.can_climb()
         && update.energy.current() > 100
     {
         update.character = CharacterState::Climb;
diff --git a/common/src/sys/agent.rs b/common/src/sys/agent.rs
index bf6758ade9..df8ef63ab5 100644
--- a/common/src/sys/agent.rs
+++ b/common/src/sys/agent.rs
@@ -207,6 +207,15 @@ impl<'a> System<'a> for Sys {
             let node_tolerance = scale * 1.5;
             let slow_factor = body.map(|b| b.base_accel() / 250.0).unwrap_or(0.0).min(1.0);
 
+            let traversal_config = TraversalConfig {
+                node_tolerance,
+                slow_factor,
+                on_ground: physics_state.on_ground,
+                in_liquid: physics_state.in_liquid.is_some(),
+                min_tgt_dist: 1.0,
+                can_climb: body.map(|b| b.can_climb()).unwrap_or(false),
+            };
+
             let mut do_idle = false;
             let mut choose_target = false;
 
@@ -220,11 +229,8 @@ impl<'a> System<'a> for Sys {
                                 vel.0,
                                 travel_to,
                                 TraversalConfig {
-                                    node_tolerance,
-                                    slow_factor,
-                                    on_ground: physics_state.on_ground,
-                                    in_liquid: physics_state.in_liquid.is_some(),
                                     min_tgt_dist: 1.25,
+                                    ..traversal_config
                                 },
                             ) {
                                 inputs.move_dir = bearing
@@ -279,10 +285,7 @@ impl<'a> System<'a> for Sys {
                             }
                         }
 
-                        // Put away weapon
-                        if thread_rng().gen::<f32>() < 0.005 {
-                            controller.actions.push(ControlAction::Unwield);
-                        }
+                        controller.actions.push(ControlAction::Unwield);
 
                         // Sometimes try searching for new targets
                         if thread_rng().gen::<f32>() < 0.1 {
@@ -302,11 +305,8 @@ impl<'a> System<'a> for Sys {
                                     vel.0,
                                     tgt_pos.0,
                                     TraversalConfig {
-                                        node_tolerance,
-                                        slow_factor,
-                                        on_ground: physics_state.on_ground,
-                                        in_liquid: physics_state.in_liquid.is_some(),
                                         min_tgt_dist: AVG_FOLLOW_DIST,
+                                        ..traversal_config
                                     },
                                 ) {
                                     inputs.move_dir =
@@ -429,11 +429,8 @@ impl<'a> System<'a> for Sys {
                                                 .unwrap_or_else(Vec3::unit_y)
                                                 * 8.0,
                                         TraversalConfig {
-                                            node_tolerance,
-                                            slow_factor,
-                                            on_ground: physics_state.on_ground,
-                                            in_liquid: physics_state.in_liquid.is_some(),
                                             min_tgt_dist: 1.25,
+                                            ..traversal_config
                                         },
                                     ) {
                                         inputs.move_dir =
@@ -591,11 +588,8 @@ impl<'a> System<'a> for Sys {
                                     vel.0,
                                     tgt_pos.0,
                                     TraversalConfig {
-                                        node_tolerance,
-                                        slow_factor,
-                                        on_ground: physics_state.on_ground,
-                                        in_liquid: physics_state.in_liquid.is_some(),
                                         min_tgt_dist: 1.25,
+                                        ..traversal_config
                                     },
                                 ) {
                                     if can_see_tgt {