Pathfinder (#25)

Pathfinding is very much not done, but it works enough and I want to get this merged.
TODO: fast replanning, goals that aren't a single node, falling moves (it should be able to play the dropper), parkour moves

1 files changed, 18 insertions, 24 deletions

diff --git a/azalea-core/src/aabb.rs b/azalea-core/src/aabb.rs
index 9ebdbcb9..58f079e7 100644..100755
--- a/azalea-core/src/aabb.rs
+++ b/azalea-core/src/aabb.rs
@@ -1,4 +1,4 @@
-use crate::{Axis, BlockHitResult, BlockPos, Direction, PositionXYZ, Vec3};
+use crate::{Axis, BlockHitResult, BlockPos, Direction, Vec3};
 
 pub const EPSILON: f64 = 1.0E-7;
 
@@ -15,7 +15,7 @@ pub struct AABB {
 }
 
 pub struct ClipPointOpts<'a> {
-    pub t: &'a mut [f64],
+    pub t: &'a mut f64,
     pub approach_dir: Option<Direction>,
     pub delta: &'a Vec3,
     pub begin: f64,
@@ -225,13 +225,10 @@ impl AABB {
     }
 
     pub fn clip(&self, min: &Vec3, max: &Vec3) -> Option<Vec3> {
-        let mut t = [1.0];
-        let x = max.x - min.x;
-        let y = max.y - min.y;
-        let z = max.z - min.z;
-        let _dir = self.get_direction(self, min, &mut t, None, &Vec3 { x, y, z })?;
-        let t = t[0];
-        Some(min.add(t * x, t * y, t * z))
+        let mut t = 1.0;
+        let delta = max - min;
+        let _dir = self.get_direction(self, min, &mut t, None, &delta)?;
+        Some(min + &(delta * t))
     }
 
     pub fn clip_iterable(
@@ -241,19 +238,16 @@ impl AABB {
         to: &Vec3,
         pos: &BlockPos,
     ) -> Option<BlockHitResult> {
-        let mut t = [1.0];
+        let mut t = 1.0;
         let mut dir = None;
-        let x = to.x - from.x;
-        let y = to.y - from.y;
-        let z = to.z - from.z;
+        let delta = to - from;
 
         for aabb in boxes {
-            dir = self.get_direction(aabb, from, &mut t, dir, &Vec3 { x, y, z });
+            dir = self.get_direction(aabb, from, &mut t, dir, &delta);
         }
         let dir = dir?;
-        let t = t[0];
         Some(BlockHitResult {
-            location: from.add(t * x, t * y, t * z),
+            location: from + &(delta * t),
             direction: dir,
             block_pos: *pos,
             inside: false,
@@ -265,7 +259,7 @@ impl AABB {
         &self,
         aabb: &AABB,
         from: &Vec3,
-        t: &mut [f64],
+        t: &mut f64,
         dir: Option<Direction>,
         delta: &Vec3,
     ) -> Option<Direction> {
@@ -393,13 +387,13 @@ impl AABB {
         let t_y = (opts.start.y + t_x) / opts.delta.y;
         let t_z = (opts.start.z + t_x) / opts.delta.z;
         if 0.0 < t_x
-            && t_x < opts.t[0]
+            && t_x < *opts.t
             && opts.min_x - EPSILON < t_y
             && t_y < opts.max_x + EPSILON
             && opts.min_z - EPSILON < t_z
             && t_z < opts.max_z + EPSILON
         {
-            opts.t[0] = t_x;
+            *opts.t = t_x;
             Some(opts.result_dir)
         } else {
             opts.approach_dir
@@ -416,11 +410,11 @@ impl AABB {
     }
 
     pub fn get_center(&self) -> Vec3 {
-        Vec3 {
-            x: (self.min_x + self.max_x) / 2.0,
-            y: (self.min_y + self.max_y) / 2.0,
-            z: (self.min_z + self.max_z) / 2.0,
-        }
+        Vec3::new(
+            (self.min_x + self.max_x) / 2.0,
+            (self.min_y + self.max_y) / 2.0,
+            (self.min_z + self.max_z) / 2.0,
+        )
     }
 
     pub fn of_size(center: Vec3, dx: f64, dy: f64, dz: f64) -> AABB {