block pos pathfinding stuff

azalea-client/src/client.rs

@@ -31,7 +31,7 @@ use azalea_world::{
     Dimension,
 };
 use log::{debug, error, warn};
-use parking_lot::{Mutex, MutexGuard, RwLock, RwLockReadGuard, RwLockWriteGuard};
+use parking_lot::{Mutex, RwLock, RwLockReadGuard, RwLockWriteGuard};
 use std::{
     fmt::Debug,
     io::{self, Cursor},

azalea-core/src/position.rs

@@ -125,6 +125,17 @@ pub struct BlockPos {
 }
 vec3_impl!(BlockPos, i32);
 
+impl BlockPos {
+    /// 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,
+        }
+    }
+}
+
 #[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)]
 pub struct ChunkPos {
     pub x: i32,

azalea-pathfinder/src/lib.rs

@@ -4,8 +4,10 @@ mod moves;
 mod mtdstarlite;
 
 use async_trait::async_trait;
+use azalea::prelude::*;
 use azalea::{Client, Event};
 use azalea_core::BlockPos;
+use azalea_world::entity::EntityData;
 use mtdstarlite::Edge;
 pub use mtdstarlite::MTDStarLite;
 use std::sync::{Arc, Mutex};
@@ -29,7 +31,7 @@ impl azalea::Plugin for Plugin {
 
 pub trait Trait {
     fn goto(&self, goal: impl Goal);
-    fn execute_path(&self, path: &Vec<Node>);
+    fn execute_path(&mut self, path: &Vec<Node>);
 }
 
 impl Trait for azalea_client::Client {
@@ -72,10 +74,10 @@ impl Trait for azalea_client::Client {
         let p = pf.find_path();
     }
 
-    fn execute_path(&self, path: &Vec<Node>) {
+    fn execute_path(&mut self, path: &Vec<Node>) {
         let start = path[0];
-        // self.entity_mut().set_rotation(y_rot, x_rot)
-        // self.look_at(start.pos);
+        let center = start.pos.center();
+        self.look_at(&center);
     }
 }
 
@@ -91,3 +93,11 @@ pub trait Goal {
     // being given a goal node
     fn goal_node(&self) -> Node;
 }
+
+impl Node {
+    /// Returns whether the entity is at the node and should start going to the
+    /// next node.
+    pub fn is_reached(&self, entity: &EntityData) -> bool {
+        entity.yya == 0. && BlockPos::from(entity.pos()) == self.pos
+    }
+}

azalea-world/src/entity/mod.rs

@@ -231,6 +231,7 @@ impl EntityData {
         }
     }
 
+    /// Get the position of the entity in the dimension.
     #[inline]
     pub fn pos(&self) -> &Vec3 {
         &self.pos