Add Path finding (#459)

* add initial path finding

* Make everything work

:D

pumpkin-core/src/math/vector3.rs

@@ -1,5 +1,5 @@
 use bytes::BufMut;
-use std::ops::{Add, Div, Mul, Sub};
+use std::ops::{Add, AddAssign, Div, Mul, Sub};
 
 use num_traits::Float;
 
@@ -92,6 +92,14 @@ impl<T: Math + Copy> Add for Vector3<T> {
     }
 }
 
+impl<T: Math + Copy> AddAssign for Vector3<T> {
+    fn add_assign(&mut self, other: Self) {
+        self.x += other.x;
+        self.y += other.y;
+        self.z += other.z;
+    }
+}
+
 /*
 impl<T: Math + Copy> Neg for Vector3<T> {
     type Output = Self;
@@ -124,6 +132,7 @@ pub trait Math:
     Mul<Output = Self>
     //+ Neg<Output = Self>
     + Add<Output = Self>
+    + AddAssign<>
     + Div<Output = Self>
     + Sub<Output = Self>
     + Sized

pumpkin/src/entity/ai/goal/mod.rs

@@ -3,6 +3,7 @@ use async_trait::async_trait;
 use crate::entity::mob::MobEntity;
 
 pub mod look_at_entity;
+pub mod target_goal;
 
 #[async_trait]
 pub trait Goal: Send + Sync {

pumpkin/src/entity/ai/goal/target_goal.rs

@@ -0,0 +1,63 @@
+use std::sync::Arc;
+
+use async_trait::async_trait;
+use tokio::sync::Mutex;
+
+use crate::entity::{ai::path::NavigatorGoal, mob::MobEntity, player::Player};
+
+use super::Goal;
+
+pub struct TargetGoal {
+    // TODO: make this an entity
+    target: Mutex<Option<Arc<Player>>>,
+    range: f64,
+}
+
+impl TargetGoal {
+    #[must_use]
+    pub fn new(range: f64) -> Self {
+        Self {
+            target: Mutex::new(None),
+            range,
+        }
+    }
+}
+
+#[async_trait]
+impl Goal for TargetGoal {
+    async fn can_start(&self, mob: &MobEntity) -> bool {
+        // TODO: make this an entity
+        let mut target = self.target.lock().await;
+
+        // gets the closest entity (currently player)
+        *target = mob
+            .living_entity
+            .entity
+            .world
+            .get_closest_player(mob.living_entity.entity.pos.load(), self.range)
+            .await;
+
+        target.is_some()
+    }
+    async fn should_continue(&self, mob: &MobEntity) -> bool {
+        // if an entity is found, lets check so its in range
+        if let Some(target) = self.target.lock().await.as_ref() {
+            let mob_pos = mob.living_entity.entity.pos.load();
+            let target_pos = target.living_entity.entity.pos.load();
+            return mob_pos.squared_distance_to_vec(target_pos) <= (self.range * self.range);
+        }
+        false
+    }
+    async fn tick(&self, mob: &MobEntity) {
+        if let Some(target) = self.target.lock().await.as_ref() {
+            let mut navigator = mob.navigator.lock().await;
+            let target_player = target.living_entity.entity.pos.load();
+
+            navigator.set_progress(NavigatorGoal {
+                current_progress: mob.living_entity.entity.pos.load(),
+                destination: target_player,
+                speed: 0.1,
+            });
+        }
+    }
+}

pumpkin/src/entity/ai/mod.rs

@@ -1 +1,2 @@
 pub mod goal;
+pub mod path;

pumpkin/src/entity/ai/path/mod.rs

@@ -0,0 +1,107 @@
+use pumpkin_core::math::vector3::Vector3;
+use pumpkin_protocol::client::play::CUpdateEntityPos;
+
+use crate::entity::living::LivingEntity;
+
+#[derive(Default)]
+pub struct Navigator {
+    current_goal: Option<NavigatorGoal>,
+}
+
+pub struct NavigatorGoal {
+    pub current_progress: Vector3<f64>,
+    pub destination: Vector3<f64>,
+    pub speed: f64,
+}
+
+impl Navigator {
+    pub fn set_progress(&mut self, goal: NavigatorGoal) {
+        self.current_goal = Some(goal);
+    }
+
+    pub fn cancel(&mut self) {
+        self.current_goal = None;
+    }
+
+    pub async fn tick(&mut self, entity: &LivingEntity) {
+        if let Some(goal) = &mut self.current_goal {
+            // first lets check if we reached destination
+            if goal.current_progress == goal.destination {
+                // if yes, we are done here
+                self.current_goal = None;
+                return;
+            }
+
+            // A star algorithm
+            let mut best_move = Vector3::new(0.0, 0.0, 0.0);
+            let mut lowest_cost = f64::MAX;
+
+            for x in -1..=1 {
+                for z in -1..=1 {
+                    let x = f64::from(x);
+                    let z = f64::from(z);
+                    let potential_pos = Vector3::new(
+                        goal.current_progress.x + x,
+                        goal.current_progress.y,
+                        goal.current_progress.z + z,
+                    );
+                    let node = Node::new(potential_pos);
+                    let cost = node.get_expense(goal.destination);
+
+                    if cost < lowest_cost {
+                        lowest_cost = cost;
+                        best_move = Vector3::new(x, 0.0, z);
+                    }
+                }
+            }
+
+            // this is important, first this saves us many packets when we don't actually move, and secound this prevents division using zero
+            // when normalize
+            if best_move.x == 0.0 && best_move.z == 0.0 {
+                return;
+            }
+            // Update current progress based on the best move
+            goal.current_progress += best_move.normalize() * goal.speed;
+
+            // now lets move
+            entity.set_pos(goal.current_progress);
+            let pos = entity.entity.pos.load();
+            let last_pos = entity.last_pos.load();
+
+            entity
+                .entity
+                .world
+                .broadcast_packet_all(&CUpdateEntityPos::new(
+                    entity.entity.entity_id.into(),
+                    Vector3::new(
+                        pos.x.mul_add(4096.0, -(last_pos.x * 4096.0)) as i16,
+                        pos.y.mul_add(4096.0, -(last_pos.y * 4096.0)) as i16,
+                        pos.z.mul_add(4096.0, -(last_pos.z * 4096.0)) as i16,
+                    ),
+                    entity
+                        .entity
+                        .on_ground
+                        .load(std::sync::atomic::Ordering::Relaxed),
+                ))
+                .await;
+        }
+    }
+}
+
+pub struct Node {
+    pub location: Vector3<f64>,
+}
+
+impl Node {
+    #[must_use]
+    pub fn new(location: Vector3<f64>) -> Self {
+        Self { location }
+    }
+    /// How expensive is it to go to a location
+    ///
+    /// Returns a f64, Higher = More Expensive
+    #[must_use]
+    pub fn get_expense(&self, end: Vector3<f64>) -> f64 {
+        self.location.squared_distance_to_vec(end).sqrt()
+    }
+}

pumpkin/src/entity/mob/mod.rs

@@ -8,13 +8,17 @@ use zombie::Zombie;
 
 use crate::{server::Server, world::World};
 
-use super::{ai::goal::Goal, living::LivingEntity};
+use super::{
+    ai::{goal::Goal, path::Navigator},
+    living::LivingEntity,
+};
 
 pub mod zombie;
 
 pub struct MobEntity {
     pub living_entity: Arc<LivingEntity>,
     pub goals: Mutex<Vec<(Arc<dyn Goal>, bool)>>,
+    pub navigator: Mutex<Navigator>,
 }
 
 impl MobEntity {
@@ -31,6 +35,8 @@ impl MobEntity {
                 *running = goal.can_start(self).await;
             }
         }
+        let mut navigator = self.navigator.lock().await;
+        navigator.tick(&self.living_entity).await;
     }
 }
 

pumpkin/src/entity/mob/zombie.rs

@@ -4,7 +4,11 @@ use pumpkin_core::math::vector3::Vector3;
 use pumpkin_entity::entity_type::EntityType;
 use uuid::Uuid;
 
-use crate::{entity::ai::goal::look_at_entity::LookAtEntityGoal, server::Server, world::World};
+use crate::{
+    entity::ai::goal::{look_at_entity::LookAtEntityGoal, target_goal::TargetGoal},
+    server::Server,
+    world::World,
+};
 
 use super::MobEntity;
 
@@ -20,6 +24,7 @@ impl Zombie {
             .add_mob_entity(EntityType::Zombie, position, world)
             .await;
         zombie_entity.goal(LookAtEntityGoal::new(8.0)).await;
+        zombie_entity.goal(TargetGoal::new(16.0)).await;
         (zombie_entity, uuid)
     }
 }

pumpkin/src/entity/mod.rs

@@ -157,6 +157,7 @@ impl Entity {
         self.yaw.store(yaw);
 
         // send packet
+        // TODO: do caching, only send packet when needed
         let yaw = (yaw * 256.0 / 360.0).rem_euclid(256.0);
         let pitch = (pitch * 256.0 / 360.0).rem_euclid(256.0);
         self.world

pumpkin/src/server/mod.rs

@@ -32,6 +32,7 @@ use uuid::Uuid;
 
 use crate::block::block_manager::BlockManager;
 use crate::block::default_block_manager;
+use crate::entity::ai::path::Navigator;
 use crate::entity::living::LivingEntity;
 use crate::entity::mob::MobEntity;
 use crate::entity::Entity;
@@ -207,6 +208,7 @@ impl Server {
         let mob = Arc::new(MobEntity {
             living_entity,
             goals: Mutex::new(vec![]),
+            navigator: Mutex::new(Navigator::default()),
         });
         world.add_mob_entity(uuid, mob.clone()).await;
         (mob, uuid)