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

9 files changed, 219 insertions, 209 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 {
diff --git a/azalea-core/src/bitset.rs b/azalea-core/src/bitset.rs
index d878bbf5..d878bbf5 100644..100755
--- a/azalea-core/src/bitset.rs
+++ b/azalea-core/src/bitset.rs
diff --git a/azalea-core/src/block_hit_result.rs b/azalea-core/src/block_hit_result.rs
index 80c9b8fc..80c9b8fc 100644..100755
--- a/azalea-core/src/block_hit_result.rs
+++ b/azalea-core/src/block_hit_result.rs
diff --git a/azalea-core/src/cursor3d.rs b/azalea-core/src/cursor3d.rs
index 180301e3..180301e3 100644..100755
--- a/azalea-core/src/cursor3d.rs
+++ b/azalea-core/src/cursor3d.rs
diff --git a/azalea-core/src/delta.rs b/azalea-core/src/delta.rs
index d1f72c17..4f730d14 100644..100755
--- a/azalea-core/src/delta.rs
+++ b/azalea-core/src/delta.rs
@@ -1,5 +1,3 @@
-use std::ops::{Add, AddAssign};
-
 use crate::Vec3;
 pub use azalea_buf::McBuf;
 
@@ -76,24 +74,3 @@ impl Vec3 {
         self.multiply(amount, amount, amount)
     }
 }
-
-// impl + and +=
-impl Add for Vec3 {
-    type Output = Vec3;
-
-    fn add(self, other: Vec3) -> Vec3 {
-        Vec3 {
-            x: self.x + other.x,
-            y: self.y + other.y,
-            z: self.z + other.z,
-        }
-    }
-}
-
-impl AddAssign for Vec3 {
-    fn add_assign(&mut self, other: Vec3) {
-        self.x += other.x;
-        self.y += other.y;
-        self.z += other.z;
-    }
-}
diff --git a/azalea-core/src/direction.rs b/azalea-core/src/direction.rs
index 22a19ee0..5a7f601a 100644..100755
--- a/azalea-core/src/direction.rs
+++ b/azalea-core/src/direction.rs
@@ -13,6 +13,15 @@ pub enum Direction {
     East,
 }
 
+// TODO: make azalea_block use this instead of FacingCardinal
+#[derive(Clone, Copy, Debug, McBuf)]
+pub enum CardinalDirection {
+    North,
+    South,
+    West,
+    East,
+}
+
 #[derive(Clone, Copy, Debug)]
 pub enum Axis {
     X = 0,
@@ -27,6 +36,55 @@ pub enum AxisCycle {
     Backward = 2,
 }
 
+impl CardinalDirection {
+    #[inline]
+    pub fn x(self) -> i32 {
+        match self {
+            CardinalDirection::East => 1,
+            CardinalDirection::West => -1,
+            _ => 0,
+        }
+    }
+    #[inline]
+    pub fn z(self) -> i32 {
+        match self {
+            CardinalDirection::South => 1,
+            CardinalDirection::North => -1,
+            _ => 0,
+        }
+    }
+
+    pub fn iter() -> impl Iterator<Item = CardinalDirection> {
+        [
+            CardinalDirection::North,
+            CardinalDirection::South,
+            CardinalDirection::West,
+            CardinalDirection::East,
+        ]
+        .iter()
+        .copied()
+    }
+
+    #[inline]
+    pub fn right(self) -> CardinalDirection {
+        match self {
+            CardinalDirection::North => CardinalDirection::East,
+            CardinalDirection::South => CardinalDirection::West,
+            CardinalDirection::West => CardinalDirection::North,
+            CardinalDirection::East => CardinalDirection::South,
+        }
+    }
+    #[inline]
+    pub fn left(self) -> CardinalDirection {
+        match self {
+            CardinalDirection::North => CardinalDirection::West,
+            CardinalDirection::South => CardinalDirection::East,
+            CardinalDirection::West => CardinalDirection::South,
+            CardinalDirection::East => CardinalDirection::North,
+        }
+    }
+}
+
 impl Axis {
     /// Pick x, y, or z from the arguments depending on the axis.
     #[inline]
diff --git a/azalea-core/src/particle/mod.rs b/azalea-core/src/particle/mod.rs
index 44c7c2f5..44c7c2f5 100644..100755
--- a/azalea-core/src/particle/mod.rs
+++ b/azalea-core/src/particle/mod.rs
diff --git a/azalea-core/src/position.rs b/azalea-core/src/position.rs
index 85ffc774..61368b28 100644..100755
--- a/azalea-core/src/position.rs
+++ b/azalea-core/src/position.rs
@@ -2,114 +2,136 @@ use crate::ResourceLocation;
 use azalea_buf::{BufReadError, McBufReadable, McBufWritable};
 use std::{
     io::{Cursor, Write},
-    ops::{Add, Mul, Rem},
+    ops::{Add, AddAssign, Mul, Rem, Sub},
 };
 
-pub trait PositionXYZ<T>
-where
-    T: Add<T, Output = T> + Mul<T, Output = T>,
-{
-    fn x(&self) -> T;
-    fn y(&self) -> T;
-    fn z(&self) -> T;
-
-    fn set_x(&self, n: T) -> Self;
-    fn set_y(&self, n: T) -> Self;
-    fn set_z(&self, n: T) -> Self;
-
-    // hopefully these get optimized
-    fn add_x(&self, n: T) -> Self
-    where
-        Self: Sized,
-    {
-        self.set_x(self.x() + n)
-    }
-    fn add_y(&self, n: T) -> Self
-    where
-        Self: Sized,
-    {
-        self.set_y(self.y() + n)
-    }
-    fn add_z(&self, n: T) -> Self
-    where
-        Self: Sized,
-    {
-        self.set_z(self.z() + n)
-    }
+macro_rules! vec3_impl {
+    ($name:ident, $type:ty) => {
+        impl $name {
+            pub fn new(x: $type, y: $type, z: $type) -> Self {
+                Self { x, y, z }
+            }
+
+            pub fn length_sqr(&self) -> $type {
+                self.x * self.x + self.y * self.y + self.z * self.z
+            }
+
+            /// Return a new instance of this position with the y coordinate
+            /// decreased by the given number.
+            pub fn down(&self, y: $type) -> Self {
+                Self {
+                    x: self.x,
+                    y: self.y - y,
+                    z: self.z,
+                }
+            }
+            /// Return a new instance of this position with the y coordinate
+            /// increased by the given number.
+            pub fn up(&self, y: $type) -> Self {
+                Self {
+                    x: self.x,
+                    y: self.y + y,
+                    z: self.z,
+                }
+            }
+        }
 
-    fn add(&self, x: T, y: T, z: T) -> Self
-    where
-        Self: Sized,
-    {
-        self.add_x(x).add_y(y).add_z(z)
-    }
+        impl Add for &$name {
+            type Output = $name;
 
-    fn length_sqr(&self) -> T
-    where
-        Self: Sized,
-    {
-        self.x() * self.x() + self.y() * self.y() + self.z() * self.z()
-    }
+            fn add(self, rhs: Self) -> Self::Output {
+                $name {
+                    x: self.x + rhs.x,
+                    y: self.y + rhs.y,
+                    z: self.z + rhs.z,
+                }
+            }
+        }
+
+        impl Add for $name {
+            type Output = $name;
+
+            fn add(self, rhs: Self) -> Self::Output {
+                (&self).add(&rhs)
+            }
+        }
+
+        impl AddAssign for $name {
+            fn add_assign(&mut self, rhs: Self) {
+                self.x += rhs.x;
+                self.y += rhs.y;
+                self.z += rhs.z;
+            }
+        }
+        impl Rem<$type> for $name {
+            type Output = Self;
+
+            fn rem(self, rhs: $type) -> Self::Output {
+                Self {
+                    x: self.x % rhs,
+                    y: self.y % rhs,
+                    z: self.z % rhs,
+                }
+            }
+        }
+
+        impl Sub for &$name {
+            type Output = $name;
+
+            /// Find the difference between two positions.
+            fn sub(self, other: Self) -> Self::Output {
+                Self::Output {
+                    x: self.x - other.x,
+                    y: self.y - other.y,
+                    z: self.z - other.z,
+                }
+            }
+        }
+        impl Sub for $name {
+            type Output = Self;
+
+            fn sub(self, other: Self) -> Self::Output {
+                (&self).sub(&other)
+            }
+        }
+
+        impl Mul<$type> for $name {
+            type Output = Self;
+
+            fn mul(self, multiplier: $type) -> Self::Output {
+                Self {
+                    x: self.x * multiplier,
+                    y: self.y * multiplier,
+                    z: self.z * multiplier,
+                }
+            }
+        }
+    };
 }
 
-#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
+#[derive(Clone, Copy, Debug, Default, PartialEq)]
+pub struct Vec3 {
+    pub x: f64,
+    pub y: f64,
+    pub z: f64,
+}
+vec3_impl!(Vec3, f64);
+
+#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)]
 pub struct BlockPos {
     pub x: i32,
     pub y: i32,
     pub z: i32,
 }
+vec3_impl!(BlockPos, i32);
 
 impl BlockPos {
-    pub fn new(x: i32, y: i32, z: i32) -> Self {
-        BlockPos { x, y, z }
-    }
-
-    pub fn below(&self) -> Self {
-        self.add(0, -1, 0)
-    }
-}
-
-impl Rem<i32> for BlockPos {
-    type Output = Self;
-
-    fn rem(self, rhs: i32) -> Self {
-        BlockPos {
-            x: self.x % rhs,
-            y: self.y % rhs,
-            z: self.z % rhs,
-        }
-    }
-}
-
-impl PositionXYZ<i32> for BlockPos {
-    fn x(&self) -> i32 {
-        self.x
-    }
-    fn y(&self) -> i32 {
-        self.y
-    }
-    fn z(&self) -> i32 {
-        self.z
-    }
-    fn set_x(&self, n: i32) -> Self {
-        BlockPos {
-            x: n,
-            y: self.y,
-            z: self.z,
-        }
-    }
-    fn set_y(&self, n: i32) -> Self {
-        BlockPos {
-            x: self.x,
-            y: n,
-            z: self.z,
-        }
-    }
-    fn set_z(&self, n: i32) -> Self {
-        BlockPos {
-            x: self.x,
-            y: self.y,
-            z: n,
+    /// Get the absolute center of a block position by adding 0.5 to each coordinate.
+    pub fn center(&self) -> Vec3 {
+        Vec3 {
+            x: self.x as f64 + 0.5,
+            y: self.y as f64 + 0.5,
+            z: self.z as f64 + 0.5,
         }
     }
 }
@@ -127,17 +149,15 @@ impl ChunkPos {
 }
 
 /// The coordinates of a chunk section in the world.
-#[derive(Clone, Copy, Debug, Default)]
+#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
 pub struct ChunkSectionPos {
     pub x: i32,
     pub y: i32,
     pub z: i32,
 }
+vec3_impl!(ChunkSectionPos, i32);
 
 impl ChunkSectionPos {
-    pub fn new(x: i32, y: i32, z: i32) -> Self {
-        ChunkSectionPos { x, y, z }
-    }
     pub fn block_to_section_coord(block: i32) -> i32 {
         block >> 4
     }
@@ -155,32 +175,25 @@ impl ChunkBlockPos {
         ChunkBlockPos { x, y, z }
     }
 }
-/// The coordinates of a block inside a chunk section.
-#[derive(Clone, Debug, Default)]
+
+/// The coordinates of a block inside a chunk section. Each coordinate must be in the range [0, 15].
+#[derive(Clone, Debug, Default, PartialEq, Eq)]
 pub struct ChunkSectionBlockPos {
-    /// A number between 0 and 16.
     pub x: u8,
-    /// A number between 0 and 16.
     pub y: u8,
-    /// A number between 0 and 16.
     pub z: u8,
 }
-
-impl ChunkSectionBlockPos {
-    pub fn new(x: u8, y: u8, z: u8) -> Self {
-        ChunkSectionBlockPos { x, y, z }
-    }
-}
+vec3_impl!(ChunkSectionBlockPos, u8);
 
 impl Add<ChunkSectionBlockPos> for ChunkSectionPos {
     type Output = BlockPos;
 
     fn add(self, rhs: ChunkSectionBlockPos) -> Self::Output {
-        BlockPos {
-            x: self.x * 16 + rhs.x as i32,
-            y: self.y * 16 + rhs.y as i32,
-            z: self.z * 16 + rhs.z as i32,
-        }
+        BlockPos::new(
+            self.x * 16 + rhs.x as i32,
+            self.y * 16 + rhs.y as i32,
+            self.z * 16 + rhs.z as i32,
+        )
     }
 }
 
@@ -192,47 +205,6 @@ pub struct GlobalPos {
     pub dimension: ResourceLocation,
 }
 
-/// An exact point in the world.
-#[derive(Debug, Clone, Copy, Default, PartialEq)]
-pub struct Vec3 {
-    pub x: f64,
-    pub y: f64,
-    pub z: f64,
-}
-
-impl PositionXYZ<f64> for Vec3 {
-    fn x(&self) -> f64 {
-        self.x
-    }
-    fn y(&self) -> f64 {
-        self.y
-    }
-    fn z(&self) -> f64 {
-        self.z
-    }
-    fn set_x(&self, n: f64) -> Self {
-        Vec3 {
-            x: n,
-            y: self.y,
-            z: self.z,
-        }
-    }
-    fn set_y(&self, n: f64) -> Self {
-        Vec3 {
-            x: self.x,
-            y: n,
-            z: self.z,
-        }
-    }
-    fn set_z(&self, n: f64) -> Self {
-        Vec3 {
-            x: self.x,
-            y: self.y,
-            z: n,
-        }
-    }
-}
-
 impl From<&BlockPos> for ChunkPos {
     fn from(pos: &BlockPos) -> Self {
         ChunkPos {
@@ -261,9 +233,9 @@ impl From<ChunkSectionPos> for ChunkPos {
 impl From<&BlockPos> for ChunkBlockPos {
     fn from(pos: &BlockPos) -> Self {
         ChunkBlockPos {
-            x: pos.x.rem_euclid(16).unsigned_abs() as u8,
+            x: pos.x.rem_euclid(16) as u8,
             y: pos.y,
-            z: pos.z.rem_euclid(16).unsigned_abs() as u8,
+            z: pos.z.rem_euclid(16) as u8,
         }
     }
 }
@@ -271,9 +243,9 @@ impl From<&BlockPos> for ChunkBlockPos {
 impl From<&BlockPos> for ChunkSectionBlockPos {
     fn from(pos: &BlockPos) -> Self {
         ChunkSectionBlockPos {
-            x: pos.x.rem(16).unsigned_abs() as u8,
-            y: pos.y.rem(16).unsigned_abs() as u8,
-            z: pos.z.rem(16).unsigned_abs() as u8,
+            x: pos.x.rem_euclid(16) as u8,
+            y: pos.y.rem_euclid(16) as u8,
+            z: pos.z.rem_euclid(16) as u8,
         }
     }
 }
@@ -282,7 +254,7 @@ impl From<&ChunkBlockPos> for ChunkSectionBlockPos {
     fn from(pos: &ChunkBlockPos) -> Self {
         ChunkSectionBlockPos {
             x: pos.x,
-            y: pos.y.rem(16).unsigned_abs() as u8,
+            y: pos.y.rem_euclid(16) as u8,
             z: pos.z,
         }
     }
@@ -419,4 +391,13 @@ mod tests {
         let block_pos = BlockPos::read_from(&mut buf).unwrap();
         assert_eq!(block_pos, BlockPos::new(49, -43, -3));
     }
+
+    #[test]
+    fn test_into_chunk_section_block_pos() {
+        let block_pos = BlockPos::new(0, -60, 0);
+        assert_eq!(
+            ChunkSectionBlockPos::from(&block_pos),
+            ChunkSectionBlockPos::new(0, 4, 0)
+        );
+    }
 }
diff --git a/azalea-core/src/slot.rs b/azalea-core/src/slot.rs
index f1cd4f0b..f1cd4f0b 100644..100755
--- a/azalea-core/src/slot.rs
+++ b/azalea-core/src/slot.rs