From c9138d913c1f6c0af0004f43b7936bc152b3d429 Mon Sep 17 00:00:00 2001
From: Joshua Barretto <joshua.s.barretto@gmail.com>
Date: Sat, 25 Jan 2020 23:39:38 +0000
Subject: [PATCH] Fixed pathfinding limit bug, improvements to idle AI

---
 common/src/astar.rs               |  3 ++-
 common/src/comp/agent.rs          |  4 ++--
 common/src/path.rs                |  5 +++--
 common/src/sys/agent.rs           | 25 +++++++++++++++++++++----
 common/src/volumes/vol_grid_2d.rs |  4 +++-
 5 files changed, 31 insertions(+), 10 deletions(-)

diff --git a/common/src/astar.rs b/common/src/astar.rs
index 3c980e5a63..a74ac64e67 100644
--- a/common/src/astar.rs
+++ b/common/src/astar.rs
@@ -82,7 +82,8 @@ impl<S: Clone + Eq + Hash> Astar<S> {
     where
         I: Iterator<Item = S>,
     {
-        while self.iter < self.max_iters.min(self.iter + iters) {
+        let iter_limit = self.max_iters.min(self.iter + iters);
+        while self.iter < iter_limit {
             if let Some(PathEntry { node, .. }) = self.potential_nodes.pop() {
                 if satisfied(&node) {
                     return PathResult::Path(self.reconstruct_path_to(node));
diff --git a/common/src/comp/agent.rs b/common/src/comp/agent.rs
index 156f6105ab..41d6050381 100644
--- a/common/src/comp/agent.rs
+++ b/common/src/comp/agent.rs
@@ -48,7 +48,7 @@ impl Component for Agent {
 
 #[derive(Clone, Debug)]
 pub enum Activity {
-    Idle(Option<Vec3<f32>>, Chaser),
+    Idle(Vec2<f32>),
     Follow(EcsEntity, Chaser),
     Attack(EcsEntity, Chaser, f64),
 }
@@ -71,6 +71,6 @@ impl Activity {
 
 impl Default for Activity {
     fn default() -> Self {
-        Activity::Idle(None, Chaser::default())
+        Activity::Idle(Vec2::zero())
     }
 }
diff --git a/common/src/path.rs b/common/src/path.rs
index ca32734b38..a47a386af2 100644
--- a/common/src/path.rs
+++ b/common/src/path.rs
@@ -80,7 +80,8 @@ impl Route {
             None
         } else {
             let next_tgt = next.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0);
-            if next_tgt.distance_squared(pos) < 1.0f32.powf(2.0) {
+            if ((pos - next_tgt) * Vec3::new(1.0, 1.0, 0.3)).magnitude_squared() < 1.0f32.powf(2.0)
+            {
                 self.next_idx += 1;
             }
             Some(next_tgt - pos)
@@ -246,7 +247,7 @@ where
         Some(astar) => astar,
     };
 
-    let path_result = new_astar.poll(20, heuristic, neighbors, transition, satisfied);
+    let path_result = new_astar.poll(60, heuristic, neighbors, transition, satisfied);
 
     *astar = Some(new_astar);
 
diff --git a/common/src/sys/agent.rs b/common/src/sys/agent.rs
index 8302c1da9b..f23f15a7be 100644
--- a/common/src/sys/agent.rs
+++ b/common/src/sys/agent.rs
@@ -76,12 +76,28 @@ impl<'a> System<'a> for Sys {
             const MAX_CHASE_DIST: f32 = 24.0;
             const SIGHT_DIST: f32 = 30.0;
             const MIN_ATTACK_DIST: f32 = 3.25;
-            const PATROL_DIST: f32 = 32.0;
 
             let mut do_idle = false;
 
             match &mut agent.activity {
-                Activity::Idle(wander_pos, chaser) => {
+                Activity::Idle(bearing) => {
+                    *bearing += Vec2::new(
+                        thread_rng().gen::<f32>() - 0.5,
+                        thread_rng().gen::<f32>() - 0.5,
+                    ) * 0.1
+                        - *bearing * 0.01
+                        - if let Some(patrol_origin) = agent.patrol_origin {
+                            Vec2::<f32>::from(pos.0 - patrol_origin) * 0.0002
+                        } else {
+                            Vec2::zero()
+                        };
+
+                    if bearing.magnitude_squared() > 0.25f32.powf(2.0) {
+                        inputs.move_dir = bearing.normalized() * 0.65;
+                    }
+
+                    /*
+                    // TODO: Improve pathfinding performance so that this is ok to do
                     if let Some(patrol_origin) = agent.patrol_origin {
                         if thread_rng().gen::<f32>() < 0.005 {
                             *wander_pos =
@@ -104,9 +120,10 @@ impl<'a> System<'a> for Sys {
                             }
                         }
                     }
+                    */
 
                     // Sometimes try searching for new targets
-                    if thread_rng().gen::<f32>() < 0.025 {
+                    if thread_rng().gen::<f32>() < 0.1 {
                         // Search for new targets
                         let entities = (&entities, &positions, &stats, alignments.maybe())
                             .join()
@@ -177,7 +194,7 @@ impl<'a> System<'a> for Sys {
             }
 
             if do_idle {
-                agent.activity = Activity::Idle(None, Chaser::default());
+                agent.activity = Activity::Idle(Vec2::zero());
             }
 
             // --- Activity overrides (in reverse order of priority: most important goes last!) ---
diff --git a/common/src/volumes/vol_grid_2d.rs b/common/src/volumes/vol_grid_2d.rs
index ac97fb5738..1c9a936c00 100644
--- a/common/src/volumes/vol_grid_2d.rs
+++ b/common/src/volumes/vol_grid_2d.rs
@@ -171,14 +171,16 @@ pub struct CachedVolGrid2d<'a, V: RectRasterableVol> {
     // reference to the `VolGrid2d`
     cache: Option<(Vec2<i32>, Arc<V>)>,
 }
+
 impl<'a, V: RectRasterableVol> CachedVolGrid2d<'a, V> {
-    pub fn new(vol_grid_2d: &'a VolGrid2d<V>) -> Self {
+    fn new(vol_grid_2d: &'a VolGrid2d<V>) -> Self {
         Self {
             vol_grid_2d,
             cache: None,
         }
     }
 }
+
 impl<'a, V: RectRasterableVol + ReadVol> CachedVolGrid2d<'a, V> {
     #[inline(always)]
     pub fn get(&mut self, pos: Vec3<i32>) -> Result<&V::Vox, VolGrid2dError<V>> {