2 files changed, 12 insertions, 5 deletions
diff --git a/azalea/src/pathfinder/astar.rs b/azalea/src/pathfinder/astar.rs
index 4e1d1039..98525e03 100644
--- a/azalea/src/pathfinder/astar.rs
+++ b/azalea/src/pathfinder/astar.rs
@@ -1,6 +1,5 @@
 use std::{
     cmp::Reverse,
-    collections::HashMap,
     fmt::Debug,
     hash::Hash,
     time::{Duration, Instant},
@@ -8,6 +7,7 @@ use std::{
 
 use log::{debug, trace, warn};
 use priority_queue::PriorityQueue;
+use rustc_hash::FxHashMap;
 
 pub struct Path<P, M>
 where
@@ -40,7 +40,7 @@ where
 
     let mut open_set = PriorityQueue::new();
     open_set.push(start, Reverse(Weight(0.)));
-    let mut nodes: HashMap<P, Node<P, M>> = HashMap::new();
+    let mut nodes: FxHashMap<P, Node<P, M>> = FxHashMap::default();
     nodes.insert(
         start,
         Node {
@@ -134,7 +134,7 @@ where
     best_paths[0]
 }
 
-fn reconstruct_path<P, M>(mut nodes: HashMap<P, Node<P, M>>, current: P) -> Vec<Movement<P, M>>
+fn reconstruct_path<P, M>(mut nodes: FxHashMap<P, Node<P, M>>, current: P) -> Vec<Movement<P, M>>
 where
     P: Eq + Hash + Copy + Debug,
 {
diff --git a/azalea/src/pathfinder/moves/mod.rs b/azalea/src/pathfinder/moves/mod.rs
index f65e556f..49615a3a 100644
--- a/azalea/src/pathfinder/moves/mod.rs
+++ b/azalea/src/pathfinder/moves/mod.rs
@@ -58,6 +58,7 @@ pub struct PathfinderCtx {
 #[derive(Default)]
 pub struct CachedSections {
     pub last_index: usize,
+    pub second_last_index: usize,
     pub sections: Vec<CachedSection>,
 }
 
@@ -67,15 +68,20 @@ impl CachedSections {
         if let Some(last_item) = self.sections.get(self.last_index) {
             if last_item.pos == pos {
                 return Some(&mut self.sections[self.last_index]);
+            } else if let Some(second_last_item) = self.sections.get(self.second_last_index) {
+                if second_last_item.pos == pos {
+                    return Some(&mut self.sections[self.second_last_index]);
+                }
             }
         }
 
         let index = self
             .sections
-            .iter_mut()
-            .position(|section| section.pos == pos);
+            .binary_search_by(|section| section.pos.cmp(&pos))
+            .ok();
 
         if let Some(index) = index {
+            self.second_last_index = self.last_index;
             self.last_index = index;
             return Some(&mut self.sections[index]);
         }
@@ -85,6 +91,7 @@ impl CachedSections {
     #[inline]
     pub fn insert(&mut self, section: CachedSection) {
         self.sections.push(section);
+        self.sections.sort_unstable_by(|a, b| a.pos.cmp(&b.pos));
     }
 }