From e15b2e06ae0e6355f559f3aabf63ba32d2a7c7fb Mon Sep 17 00:00:00 2001 From: Alexander Medvedev Date: Mon, 6 Jan 2025 11:41:05 +0100 Subject: [PATCH] Add Path finding (#459) * add initial path finding * Make everything work :D --- pumpkin-core/src/math/vector3.rs | 11 ++- pumpkin/src/entity/ai/goal/mod.rs | 1 + pumpkin/src/entity/ai/goal/target_goal.rs | 63 +++++++++++++ pumpkin/src/entity/ai/mod.rs | 1 + pumpkin/src/entity/ai/path/mod.rs | 107 ++++++++++++++++++++++ pumpkin/src/entity/mob/mod.rs | 8 +- pumpkin/src/entity/mob/zombie.rs | 7 +- pumpkin/src/entity/mod.rs | 1 + pumpkin/src/server/mod.rs | 2 + 9 files changed, 198 insertions(+), 3 deletions(-) create mode 100644 pumpkin/src/entity/ai/goal/target_goal.rs create mode 100644 pumpkin/src/entity/ai/path/mod.rs diff --git a/pumpkin-core/src/math/vector3.rs b/pumpkin-core/src/math/vector3.rs index 553e07f61..a56be9e16 100644 --- a/pumpkin-core/src/math/vector3.rs +++ b/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 Add for Vector3 { } } +impl AddAssign for Vector3 { + fn add_assign(&mut self, other: Self) { + self.x += other.x; + self.y += other.y; + self.z += other.z; + } +} + /* impl Neg for Vector3 { type Output = Self; @@ -124,6 +132,7 @@ pub trait Math: Mul //+ Neg + Add + + AddAssign<> + Div + Sub + Sized diff --git a/pumpkin/src/entity/ai/goal/mod.rs b/pumpkin/src/entity/ai/goal/mod.rs index 9405603ed..d04169acc 100644 --- a/pumpkin/src/entity/ai/goal/mod.rs +++ b/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 { diff --git a/pumpkin/src/entity/ai/goal/target_goal.rs b/pumpkin/src/entity/ai/goal/target_goal.rs new file mode 100644 index 000000000..15dbfce2c --- /dev/null +++ b/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>>, + 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, + }); + } + } +} diff --git a/pumpkin/src/entity/ai/mod.rs b/pumpkin/src/entity/ai/mod.rs index b3dc9aaa2..7d1d1d1af 100644 --- a/pumpkin/src/entity/ai/mod.rs +++ b/pumpkin/src/entity/ai/mod.rs @@ -1 +1,2 @@ pub mod goal; +pub mod path; diff --git a/pumpkin/src/entity/ai/path/mod.rs b/pumpkin/src/entity/ai/path/mod.rs new file mode 100644 index 000000000..2329b78ce --- /dev/null +++ b/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, +} + +pub struct NavigatorGoal { + pub current_progress: Vector3, + pub destination: Vector3, + 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, +} + +impl Node { + #[must_use] + pub fn new(location: Vector3) -> 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 { + self.location.squared_distance_to_vec(end).sqrt() + } +} diff --git a/pumpkin/src/entity/mob/mod.rs b/pumpkin/src/entity/mob/mod.rs index 798b53932..82b93a94e 100644 --- a/pumpkin/src/entity/mob/mod.rs +++ b/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, pub goals: Mutex, bool)>>, + pub navigator: Mutex, } 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; } } diff --git a/pumpkin/src/entity/mob/zombie.rs b/pumpkin/src/entity/mob/zombie.rs index 44443a9a1..554efdd43 100644 --- a/pumpkin/src/entity/mob/zombie.rs +++ b/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) } } diff --git a/pumpkin/src/entity/mod.rs b/pumpkin/src/entity/mod.rs index 861adcaac..052d18c13 100644 --- a/pumpkin/src/entity/mod.rs +++ b/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 diff --git a/pumpkin/src/server/mod.rs b/pumpkin/src/server/mod.rs index 28cba6b4f..44d94de94 100644 --- a/pumpkin/src/server/mod.rs +++ b/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)