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, 143 insertions, 162 deletions

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)
+        );
+    }
 }