From ac785365d385266294d436a4c8d256fec24fd4cd Mon Sep 17 00:00:00 2001 From: Joshua Goins Date: Mon, 14 Jul 2025 20:30:14 -0400 Subject: [PATCH] Begin implementing pathfinding, all NPCs now converge to you This is incredibly simple behavior, and navimesh generation is currently manually done. But it's progress! See #38 --- Cargo.toml | 4 +- src/bin/kawari-navimesh.rs | 410 +++++++++++++++++++++++++++---------- src/common/position.rs | 19 ++ src/world/common.rs | 1 + src/world/mod.rs | 3 + src/world/navmesh.rs | 194 ++++++++++++++++++ src/world/server.rs | 244 +++++++++++++++++++++- src/world/zone.rs | 32 +-- 8 files changed, 760 insertions(+), 147 deletions(-) create mode 100644 src/world/navmesh.rs diff --git a/Cargo.toml b/Cargo.toml index de42878..79223dd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -53,7 +53,7 @@ default = [] oodle = [] # Navmesh visualizer -visualizer = ["dep:bevy", "dep:recastnavigation-sys"] +visualizer = ["dep:bevy"] [build-dependencies] # Serialization of IPC opcodes @@ -102,7 +102,7 @@ bevy = { version = "0.16", features = ["std", "x11"], default-features = false, optional = true } # for navimesh generation -recastnavigation-sys = { git = "https://github.com/redstrate/recastnavigation-rs-sys", features = ["recast", "detour"], optional = true } +recastnavigation-sys = { git = "https://github.com/redstrate/recastnavigation-rs-sys", features = ["recast", "detour"] } [target.'cfg(not(target_family = "wasm"))'.dependencies] # Used for the web servers diff --git a/src/bin/kawari-navimesh.rs b/src/bin/kawari-navimesh.rs index 3ca8be4..f20c05f 100644 --- a/src/bin/kawari-navimesh.rs +++ b/src/bin/kawari-navimesh.rs @@ -2,25 +2,36 @@ use std::ptr::{null, null_mut}; use bevy::{ asset::RenderAssetUsages, - color::palettes::tailwind::{PINK_100, RED_500}, + color::palettes::{ + css::WHITE, + tailwind::{BLUE_100, GREEN_100, PINK_100, RED_500}, + }, + pbr::wireframe::{Wireframe, WireframeConfig, WireframePlugin}, picking::pointer::PointerInteraction, prelude::*, - render::mesh::{Indices, PrimitiveTopology}, + render::{ + RenderPlugin, + mesh::{Indices, PrimitiveTopology}, + settings::{RenderCreation, WgpuFeatures, WgpuSettings}, + }, }; use icarus::TerritoryType::TerritoryTypeSheet; -use kawari::config::get_config; +use kawari::{ + config::get_config, + world::{Navmesh, NavmeshParams}, +}; use physis::{ common::{Language, Platform}, layer::{LayerEntryData, LayerGroup, ModelCollisionType, Transformation}, lvb::Lvb, + model::MDL, pcb::{Pcb, ResourceNode}, resource::{Resource, SqPackResource}, + tera::{PlateModel, Terrain}, }; use recastnavigation_sys::{ - CreateContext, DT_SUCCESS, dtAllocNavMesh, dtAllocNavMeshQuery, dtCreateNavMeshData, - dtNavMesh_addTile, dtNavMesh_init, dtNavMeshCreateParams, dtNavMeshParams, dtNavMeshQuery, - dtNavMeshQuery_findNearestPoly, dtNavMeshQuery_findPath, dtNavMeshQuery_findStraightPath, - dtNavMeshQuery_init, dtPolyRef, dtQueryFilter, dtQueryFilter_dtQueryFilter, + CreateContext, DT_SUCCESS, RC_MESH_NULL_IDX, dtCreateNavMeshData, dtNavMeshCreateParams, + dtNavMeshQuery, dtNavMeshQuery_findNearestPoly, dtPolyRef, dtQueryFilter, rcAllocCompactHeightfield, rcAllocContourSet, rcAllocHeightfield, rcAllocPolyMesh, rcAllocPolyMeshDetail, rcBuildCompactHeightfield, rcBuildContours, rcBuildContoursFlags_RC_CONTOUR_TESS_WALL_EDGES, rcBuildDistanceField, rcBuildPolyMesh, @@ -33,70 +44,27 @@ struct ZoneToLoad(u16); #[derive(Resource, Default)] struct NavigationState { - query: *mut dtNavMeshQuery, + navmesh: Navmesh, path: Vec, + from_position: Vec3, + to_position: Vec3, } impl NavigationState { - pub fn calculate_path(&mut self, from_position: Vec3) { - unsafe { - let start_pos = [from_position.x, from_position.y, from_position.z]; - let end_pos = [0.0, 0.0, 0.0]; + pub fn calculate_path(&mut self) { + let start_pos = [ + self.from_position.x, + self.from_position.y, + self.from_position.z, + ]; + let end_pos = [self.to_position.x, self.to_position.y, self.to_position.z]; - let mut filter = dtQueryFilter { - m_areaCost: [0.0; 64], - m_includeFlags: 0, - m_excludeFlags: 0, - }; - dtQueryFilter_dtQueryFilter(&mut filter); - - let (start_poly, start_poly_pos) = - get_polygon_at_location(self.query, start_pos, &filter); - let (end_poly, end_poly_pos) = get_polygon_at_location(self.query, end_pos, &filter); - - let mut path = [0; 128]; - let mut path_count = 0; - dtNavMeshQuery_findPath( - self.query, - start_poly, - end_poly, - start_poly_pos.as_ptr(), - end_poly_pos.as_ptr(), - &filter, - path.as_mut_ptr(), - &mut path_count, - 128, - ); // TODO: error check - - let mut straight_path = [0.0; 128 * 3]; - let mut straight_path_count = 0; - - // now calculate the positions in the path - dtNavMeshQuery_findStraightPath( - self.query, - start_poly_pos.as_ptr(), - end_poly_pos.as_ptr(), - path.as_ptr(), - path_count, - straight_path.as_mut_ptr(), - null_mut(), - null_mut(), - &mut straight_path_count, - 128, - 0, - ); - - dbg!(&straight_path[..straight_path_count as usize * 3]); - - self.path.clear(); - for pos in straight_path[..straight_path_count as usize * 3].chunks(3) { - self.path.push(Vec3 { - x: pos[0], - y: pos[1], - z: pos[2], - }); - } - } + self.path = self + .navmesh + .calculate_path(start_pos, end_pos) + .iter() + .map(|x| Vec3::from_slice(x)) + .collect(); } } @@ -111,16 +79,38 @@ fn main() { App::new() .add_event::() - .add_plugins((DefaultPlugins, MeshPickingPlugin)) + .add_event::() + .add_event::() + .add_plugins(( + DefaultPlugins.set(RenderPlugin { + render_creation: RenderCreation::Automatic(WgpuSettings { + features: WgpuFeatures::POLYGON_MODE_LINE, + ..default() + }), + ..default() + }), + MeshPickingPlugin, + WireframePlugin::default(), + )) .add_systems(Startup, setup) .add_systems(Update, draw_mesh_intersections) + .insert_resource(WireframeConfig { + global: false, + default_color: WHITE.into(), + }) .insert_resource(ZoneToLoad(zone_id)) .insert_resource(NavigationState::default()) .run(); } #[derive(Event, Reflect, Clone, Debug)] -struct Navigate(Vec3); +struct Navigate(); + +#[derive(Event, Reflect, Clone, Debug)] +struct SetOrigin(Vec3); + +#[derive(Event, Reflect, Clone, Debug)] +struct SetTarget(Vec3); /// Walk each node, add it's collision model to the scene. fn walk_node( @@ -137,7 +127,7 @@ fn walk_node( let mut positions = Vec::new(); for vec in &node.vertices { - positions.push(vec.clone()); + positions.push(Vec3::from_slice(vec)); } mesh.insert_attribute(Mesh::ATTRIBUTE_POSITION, positions.clone()); @@ -156,6 +146,17 @@ fn walk_node( mesh.compute_normals(); + let transform = Transform { + translation: Vec3::from_array(transform.translation), + rotation: Quat::from_euler( + EulerRot::XYZ, + transform.rotation[0], + transform.rotation[1], + transform.rotation[2], + ), + scale: Vec3::from_array(transform.scale), + }; + // insert into 3d scene commands .spawn(( @@ -165,21 +166,25 @@ fn walk_node( fastrand::f32(), fastrand::f32(), ))), - Transform { - translation: Vec3::from_array(transform.translation), - rotation: Quat::from_euler( - EulerRot::XYZ, - transform.rotation[0], - transform.rotation[1], - transform.rotation[2], - ), - scale: Vec3::from_array(transform.scale), - }, + transform, )) .observe( - |mut trigger: Trigger>, mut events: EventWriter| { + |mut trigger: Trigger>, + mut navigate_events: EventWriter, + mut target_events: EventWriter, + mut origin_events: EventWriter| { let click_event: &Pointer = trigger.event(); - events.write(Navigate(click_event.hit.position.unwrap())); + match click_event.button { + PointerButton::Primary => { + target_events.write(SetTarget(click_event.hit.position.unwrap())); + } + PointerButton::Secondary => { + origin_events.write(SetOrigin(click_event.hit.position.unwrap())); + } + PointerButton::Middle => { + navigate_events.write(Navigate()); + } + } trigger.propagate(false); }, ); @@ -188,6 +193,18 @@ fn walk_node( let tile_indices: Vec = indices.iter().map(|x| *x as i32).collect(); let mut tri_area_ids: Vec = vec![0; tile_indices.len() / 3]; + // transform the vertices on the CPU + let mut tile_vertices: Vec<[f32; 3]> = Vec::new(); + let transform_matrix = transform.compute_matrix(); + for vertex in &positions { + let transformed_vertex = transform_matrix.transform_point3(*vertex); + tile_vertices.push([ + transformed_vertex.x, + transformed_vertex.y, + transformed_vertex.z, + ]); + } + unsafe { let ntris = tile_indices.len() as i32 / 3; @@ -195,7 +212,7 @@ fn walk_node( rcMarkWalkableTriangles( context, 45.0, - std::mem::transmute::<*const [f32; 3], *const f32>(positions.as_ptr()), + std::mem::transmute::<*const [f32; 3], *const f32>(tile_vertices.as_ptr()), positions.len() as i32, tile_indices.as_ptr(), ntris, @@ -204,7 +221,7 @@ fn walk_node( assert!(rcRasterizeTriangles( context, - std::mem::transmute::<*const [f32; 3], *const f32>(positions.as_ptr()), + std::mem::transmute::<*const [f32; 3], *const f32>(tile_vertices.as_ptr()), positions.len() as i32, tile_indices.as_ptr(), tri_area_ids.as_ptr(), @@ -228,12 +245,119 @@ fn walk_node( } } +fn add_plate( + plate: &PlateModel, + tera_path: &str, + sqpack_resource: &mut SqPackResource, + commands: &mut Commands, + meshes: &mut ResMut>, + materials: &mut ResMut>, + context: *mut rcContext, + height_field: *mut rcHeightfield, +) { + let mdl_path = format!("{}/bgplate/{}", tera_path, plate.filename); + let mdl_bytes = sqpack_resource.read(&mdl_path).unwrap(); + let mdl = MDL::from_existing(&mdl_bytes).unwrap(); + + let lod = &mdl.lods[0]; + for part in &lod.parts { + let mut mesh = Mesh::new(PrimitiveTopology::TriangleList, RenderAssetUsages::all()); + + let mut positions = Vec::new(); + let mut normals = Vec::new(); + for vec in &part.vertices { + positions.push(Vec3::from_slice(&vec.position)); + normals.push(Vec3::from_slice(&vec.normal)); + } + mesh.insert_attribute(Mesh::ATTRIBUTE_POSITION, positions.clone()); + mesh.insert_attribute(Mesh::ATTRIBUTE_NORMAL, normals.clone()); + + mesh.insert_indices(Indices::U16(part.indices.clone())); + + let transform = Transform::from_xyz(plate.position.0, 0.0, plate.position.1); + + // insert into 3d scene + commands + .spawn(( + Mesh3d(meshes.add(mesh)), + MeshMaterial3d(materials.add(Color::srgb( + fastrand::f32(), + fastrand::f32(), + fastrand::f32(), + ))), + transform, + )) + .observe( + |mut trigger: Trigger>, + mut navigate_events: EventWriter, + mut target_events: EventWriter, + mut origin_events: EventWriter| { + let click_event: &Pointer = trigger.event(); + match click_event.button { + PointerButton::Primary => { + target_events.write(SetTarget(click_event.hit.position.unwrap())); + } + PointerButton::Secondary => { + origin_events.write(SetOrigin(click_event.hit.position.unwrap())); + } + PointerButton::Middle => { + navigate_events.write(Navigate()); + } + } + trigger.propagate(false); + }, + ); + + // Step 2: insert geoemtry into heightfield + let tile_indices: Vec = part.indices.iter().map(|x| *x as i32).collect(); + let mut tri_area_ids: Vec = vec![0; tile_indices.len() / 3]; + + // transform the vertices on the CPU + let mut tile_vertices: Vec<[f32; 3]> = Vec::new(); + let transform_matrix = transform.compute_matrix(); + for vertex in &positions { + let transformed_vertex = transform_matrix.transform_point3(*vertex); + tile_vertices.push([ + transformed_vertex.x, + transformed_vertex.y, + transformed_vertex.z, + ]); + } + + unsafe { + let ntris = tile_indices.len() as i32 / 3; + + // mark areas as walkable + rcMarkWalkableTriangles( + context, + 45.0, + std::mem::transmute::<*const [f32; 3], *const f32>(tile_vertices.as_ptr()), + positions.len() as i32, + tile_indices.as_ptr(), + ntris, + tri_area_ids.as_mut_ptr(), + ); + + assert!(rcRasterizeTriangles( + context, + std::mem::transmute::<*const [f32; 3], *const f32>(tile_vertices.as_ptr()), + positions.len() as i32, + tile_indices.as_ptr(), + tri_area_ids.as_ptr(), + ntris, + height_field, + 2 + )); + } + } +} + fn get_polygon_at_location( query: *const dtNavMeshQuery, position: [f32; 3], filter: &dtQueryFilter, ) -> (dtPolyRef, [f32; 3]) { - let extents = [3.0, 5.0, 3.0]; + let extents = [2.0, 4.0, 2.0]; unsafe { let mut nearest_ref = 0; @@ -315,7 +439,29 @@ fn setup( )); } - for path in &lvb.scns[0].header.path_layer_group_resources { + let scene = &lvb.scns[0]; + + let tera_bytes = sqpack_resource + .read(&*format!( + "{}/bgplate/terrain.tera", + scene.general.path_terrain + )) + .unwrap(); + let tera = Terrain::from_existing(&tera_bytes).unwrap(); + for plate in tera.plates { + add_plate( + &plate, + &scene.general.path_terrain, + &mut sqpack_resource, + &mut commands, + &mut meshes, + &mut materials, + context, + height_field, + ); + } + + for path in &scene.header.path_layer_group_resources { if path.contains("bg.lgb") { tracing::info!("Processing {path}..."); @@ -418,6 +564,54 @@ fn setup( assert!((*poly_mesh).verts != null_mut()); assert!((*poly_mesh).nverts > 0); + let nvp = (*poly_mesh).nvp; + let cs = (*poly_mesh).cs; + let ch = (*poly_mesh).ch; + let orig = (*poly_mesh).bmin; + + // add polymesh to visualization + { + let mut mesh = Mesh::new(PrimitiveTopology::TriangleList, RenderAssetUsages::all()); + + let mut positions = Vec::new(); + for i in 0..(*poly_mesh).nverts as usize { + let v = (*poly_mesh).verts.wrapping_add(i * 3); + let x = orig[0] + *v as f32 * cs as f32; + let y = orig[1] + (*v.wrapping_add(1) + 1) as f32 * ch as f32 + 0.1; + let z = orig[2] + (*v.wrapping_add(2)) as f32 * cs as f32; + + positions.push(Vec3::new(x, y, z)); + } + + mesh.insert_attribute(Mesh::ATTRIBUTE_POSITION, positions.clone()); + + let mut indices = Vec::new(); + for i in 0..(*poly_mesh).npolys as usize { + let p = (*poly_mesh).polys.wrapping_add(i * nvp as usize * 2); + for j in 2..nvp as usize { + if *(p.wrapping_add(j)) == RC_MESH_NULL_IDX { + break; + } + + indices.push(*p); + indices.push(*p.wrapping_add(j - 1)); + indices.push(*p.wrapping_add(j)); + } + } + + mesh.insert_indices(Indices::U16(indices.clone())); + + //mesh.compute_normals(); + + // insert into 3d scene + commands.spawn(( + Mesh3d(meshes.add(mesh)), + MeshMaterial3d(materials.add(Color::srgba(0.0, 0.0, 1.0, 0.5))), + Pickable::IGNORE, + Wireframe, + )); + } + let flags = std::slice::from_raw_parts_mut((*poly_mesh).flags, (*poly_mesh).npolys as usize); for flag in flags { @@ -490,29 +684,26 @@ fn setup( assert!(out_data != null_mut()); assert!(out_data_size > 0); - let navmesh_params = dtNavMeshParams { - orig: [0.0; 3], - tileWidth: 100.0, - tileHeight: 100.0, - maxTiles: 1000, - maxPolys: 1000, - }; - - let navmesh = dtAllocNavMesh(); - assert!(dtNavMesh_init(navmesh, &navmesh_params) == DT_SUCCESS); - - assert!( - dtNavMesh_addTile(navmesh, out_data, out_data_size, 0, 0, null_mut()) == DT_SUCCESS + navigation_state.navmesh = Navmesh::new( + NavmeshParams { + orig: (*poly_mesh).bmin, + tile_width: (*poly_mesh).bmax[0] - (*poly_mesh).bmin[0], + tile_height: (*poly_mesh).bmax[2] - (*poly_mesh).bmin[2], + max_tiles: 1, + max_polys: (*poly_mesh).npolys, + }, + Vec::from_raw_parts(out_data, out_data_size as usize, out_data_size as usize), ); - navigation_state.query = dtAllocNavMeshQuery(); - dtNavMeshQuery_init(navigation_state.query, navmesh, 1024); + // TODO: output in the correct directory + let serialized_navmesh = navigation_state.navmesh.write_to_buffer().unwrap(); + std::fs::write("test.nvm", &serialized_navmesh).unwrap(); } // camera commands.spawn(( Camera3d::default(), - Transform::from_xyz(15.0, 15.0, 15.0).looking_at(Vec3::ZERO, Vec3::Y), + Transform::from_xyz(55.0, 55.0, 55.0).looking_at(Vec3::ZERO, Vec3::Y), )); } @@ -520,8 +711,13 @@ fn draw_mesh_intersections( pointers: Query<&PointerInteraction>, mut gizmos: Gizmos, mut navigate_events: EventReader, + mut origin_events: EventReader, + mut target_events: EventReader, mut navigation_state: ResMut, ) { + gizmos.sphere(navigation_state.from_position, 0.05, GREEN_100); + gizmos.sphere(navigation_state.to_position, 0.05, BLUE_100); + for pos in &navigation_state.path { gizmos.sphere(*pos, 0.05, RED_500); } @@ -535,7 +731,15 @@ fn draw_mesh_intersections( gizmos.arrow(point, point + normal.normalize() * 0.5, PINK_100); } - for event in navigate_events.read() { - navigation_state.calculate_path(event.0); + for event in origin_events.read() { + navigation_state.from_position = event.0; + } + + for event in target_events.read() { + navigation_state.to_position = event.0; + } + + for _ in navigate_events.read() { + navigation_state.calculate_path(); } } diff --git a/src/common/position.rs b/src/common/position.rs index 6e3a7e6..81bdeaa 100644 --- a/src/common/position.rs +++ b/src/common/position.rs @@ -8,3 +8,22 @@ pub struct Position { pub y: f32, pub z: f32, } + +impl Position { + pub fn lerp(a: Position, b: Position, t: f32) -> Position { + let lerp = |v0: f32, v1: f32, t: f32| v0 + t * (v1 - v0); + + Position { + x: lerp(a.x, b.x, t), + y: lerp(a.y, b.y, t), + z: lerp(a.z, b.z, t), + } + } + + pub fn distance(a: Position, b: Position) -> f32 { + let delta_x = b.x - a.x; + let delta_y = b.y - a.y; + let delta_z = b.z - a.z; + delta_x.powi(2) + delta_y.powi(2) + delta_z.powi(2) + } +} diff --git a/src/world/common.rs b/src/world/common.rs index fae660f..66ebe9c 100644 --- a/src/world/common.rs +++ b/src/world/common.rs @@ -22,6 +22,7 @@ use super::Actor; #[derive(Copy, Clone, Debug, Eq, PartialEq, Hash)] pub struct ClientId(usize); +#[derive(Clone)] pub enum FromServer { /// A chat message. Message(String), diff --git a/src/world/mod.rs b/src/world/mod.rs index d2b4929..f8cb825 100644 --- a/src/world/mod.rs +++ b/src/world/mod.rs @@ -30,3 +30,6 @@ pub use custom_ipc_handler::handle_custom_ipc; mod common; pub use common::{ClientHandle, ClientId, FromServer, ServerHandle, ToServer}; + +mod navmesh; +pub use navmesh::{Navmesh, NavmeshParams}; diff --git a/src/world/navmesh.rs b/src/world/navmesh.rs new file mode 100644 index 0000000..bc4749b --- /dev/null +++ b/src/world/navmesh.rs @@ -0,0 +1,194 @@ +// TODO: use tiles + +use std::{io::Cursor, ptr::null_mut}; + +use binrw::{BinRead, BinWrite, binrw}; +use recastnavigation_sys::{ + DT_SUCCESS, dtAllocNavMesh, dtAllocNavMeshQuery, dtNavMesh, dtNavMesh_addTile, dtNavMesh_init, + dtNavMeshParams, dtNavMeshQuery, dtNavMeshQuery_findNearestPoly, dtNavMeshQuery_findPath, + dtNavMeshQuery_findStraightPath, dtNavMeshQuery_init, dtPolyRef, dtQueryFilter, + dtQueryFilter_dtQueryFilter, +}; + +#[binrw] +#[brw(little)] +#[derive(Default, Debug, Clone)] +pub struct NavmeshParams { + pub orig: [f32; 3], + pub tile_width: f32, + pub tile_height: f32, + pub max_tiles: i32, + pub max_polys: i32, +} + +/// Represents a navmesh for a zone. +/// NOTE: We reuse the .nvm file extension used by the retail server. These have no relations to ours. +#[binrw] +#[brw(little)] +#[derive(Default, Debug, Clone)] +pub struct Navmesh { + nav_mesh_params: NavmeshParams, + #[br(temp)] + #[bw(calc = data.len() as u32)] + data_size: u32, + #[br(count = data_size)] + data: Vec, + + #[bw(ignore)] + #[br(default)] + navmesh: *mut dtNavMesh, + #[bw(ignore)] + #[br(default)] + navmesh_query: *mut dtNavMeshQuery, +} + +// To send the pointers between threads. +unsafe impl Send for Navmesh {} +unsafe impl Sync for Navmesh {} + +impl Navmesh { + /// Creates a new Navmesh. + pub fn new(nav_mesh_params: NavmeshParams, data: Vec) -> Self { + let mut navmesh = Navmesh { + nav_mesh_params, + data, + navmesh: null_mut(), + navmesh_query: null_mut(), + }; + navmesh.initialize(); + navmesh + } + + /// Reads an existing NVM file. + pub fn from_existing(buffer: &[u8]) -> Option { + let mut cursor = Cursor::new(buffer); + if let Some(mut navmesh) = Self::read(&mut cursor).ok() { + navmesh.initialize(); + return Some(navmesh); + } + + None + } + + /// Writes to the NVM file format. + pub fn write_to_buffer(&self) -> Option> { + let mut buffer = Vec::new(); + + { + let mut cursor = Cursor::new(&mut buffer); + self.write_le(&mut cursor).ok()?; + } + + Some(buffer) + } + + /// Initializes Detour data. + fn initialize(&mut self) { + let navmesh_params = dtNavMeshParams { + orig: self.nav_mesh_params.orig, + tileWidth: self.nav_mesh_params.tile_width, + tileHeight: self.nav_mesh_params.tile_height, + maxTiles: self.nav_mesh_params.max_tiles, + maxPolys: self.nav_mesh_params.max_polys, + }; + + unsafe { + self.navmesh = dtAllocNavMesh(); + assert!(dtNavMesh_init(self.navmesh, &navmesh_params) == DT_SUCCESS); + + assert!( + dtNavMesh_addTile( + self.navmesh, + self.data.as_mut_ptr(), + self.data.len() as i32, + 0, + 0, + null_mut() + ) == DT_SUCCESS + ); + + self.navmesh_query = dtAllocNavMeshQuery(); + assert!(dtNavMeshQuery_init(self.navmesh_query, self.navmesh, 2048) == DT_SUCCESS); + } + } + + pub fn calculate_path(&self, start_pos: [f32; 3], end_pos: [f32; 3]) -> Vec<[f32; 3]> { + unsafe { + let mut filter = dtQueryFilter { + m_areaCost: [1.0; 64], + m_includeFlags: 0xffff, + m_excludeFlags: 0, + }; + dtQueryFilter_dtQueryFilter(&mut filter); + + let (start_poly, start_poly_pos) = + Self::get_polygon_at_location(self.navmesh_query, start_pos, &filter); + let (end_poly, end_poly_pos) = + Self::get_polygon_at_location(self.navmesh_query, end_pos, &filter); + + let mut path = [0; 128]; + let mut path_count = 0; + dtNavMeshQuery_findPath( + self.navmesh_query, + start_poly, + end_poly, + start_poly_pos.as_ptr(), + end_poly_pos.as_ptr(), + &filter, + path.as_mut_ptr(), + &mut path_count, + 128, + ); // TODO: error check + + let mut straight_path = [0.0; 128 * 3]; + let mut straight_path_count = 0; + + // now calculate the positions in the path + dtNavMeshQuery_findStraightPath( + self.navmesh_query, + start_poly_pos.as_ptr(), + end_poly_pos.as_ptr(), + path.as_ptr(), + path_count, + straight_path.as_mut_ptr(), + null_mut(), + null_mut(), + &mut straight_path_count, + 128, + 0, + ); + + let mut path = Vec::new(); + for pos in straight_path[..straight_path_count as usize * 3].chunks(3) { + path.push([pos[0], pos[1], pos[2]]); + } + + path + } + } + + fn get_polygon_at_location( + query: *const dtNavMeshQuery, + position: [f32; 3], + filter: &dtQueryFilter, + ) -> (dtPolyRef, [f32; 3]) { + let extents = [2.0, 4.0, 2.0]; + + unsafe { + let mut nearest_ref = 0; + let mut nearest_pt = [0.0; 3]; + assert!( + dtNavMeshQuery_findNearestPoly( + query, + position.as_ptr(), + extents.as_ptr(), + filter, + &mut nearest_ref, + nearest_pt.as_mut_ptr() + ) == DT_SUCCESS + ); + + return (nearest_ref, nearest_pt); + } + } +} diff --git a/src/world/server.rs b/src/world/server.rs index a98319f..065e661 100644 --- a/src/world/server.rs +++ b/src/world/server.rs @@ -1,6 +1,8 @@ use binrw::{BinRead, BinWrite}; +use icarus::TerritoryType::TerritoryTypeSheet; +use physis::{common::Language, lvb::Lvb, resource::Resource}; use std::{ - collections::HashMap, + collections::{HashMap, VecDeque}, io::Cursor, path::PathBuf, sync::{Arc, Mutex}, @@ -9,7 +11,8 @@ use std::{ use tokio::sync::mpsc::Receiver; use crate::{ - common::{CustomizeData, GameData, ObjectId, ObjectTypeId, timestamp_secs}, + common::{CustomizeData, GameData, ObjectId, ObjectTypeId, Position, timestamp_secs}, + config::get_config, ipc::zone::{ ActorControl, ActorControlCategory, ActorControlSelf, ActorControlTarget, BattleNpcSubKind, ClientTriggerCommand, CommonSpawn, NpcSpawn, ObjectKind, ServerZoneIpcData, @@ -19,7 +22,7 @@ use crate::{ packet::{PacketSegment, SegmentData, SegmentType}, }; -use super::{Actor, ClientHandle, ClientId, FromServer, ToServer}; +use super::{Actor, ClientHandle, ClientId, FromServer, Navmesh, ToServer}; /// Used for the debug NPC. pub const CUSTOMIZE_DATA: CustomizeData = CustomizeData { @@ -54,16 +57,81 @@ pub const CUSTOMIZE_DATA: CustomizeData = CustomizeData { #[derive(Debug, Clone)] enum NetworkedActor { Player(NpcSpawn), - Npc(NpcSpawn), + Npc { + current_path: VecDeque<[f32; 3]>, + current_path_lerp: f32, + current_target: Option, + last_position: Option, + spawn: NpcSpawn, + }, +} + +impl NetworkedActor { + pub fn get_common_spawn(&self) -> &CommonSpawn { + match &self { + NetworkedActor::Player(npc_spawn) => &npc_spawn.common, + NetworkedActor::Npc { spawn, .. } => &spawn.common, + } + } } #[derive(Default, Debug, Clone)] struct Instance { // structure temporary, of course actors: HashMap, + navmesh: Navmesh, } impl Instance { + pub fn new(id: u16, game_data: &mut GameData) -> Self { + let mut instance = Self::default(); + + let sheet = TerritoryTypeSheet::read_from(&mut game_data.resource, Language::None).unwrap(); + let Some(row) = sheet.get_row(id as u32) else { + tracing::warn!("Invalid zone id {id}, allowing anyway..."); + return instance; + }; + + // e.g. ffxiv/fst_f1/fld/f1f3/level/f1f3 + let bg_path = row.Bg().into_string().unwrap(); + + let path = format!("bg/{}.lvb", &bg_path); + tracing::info!("Loading {}", path); + let lgb_file = game_data.resource.read(&path).unwrap(); + let lgb = Lvb::from_existing(&lgb_file).unwrap(); + + let mut navimesh_path = None; + for layer_set in &lgb.scns[0].unk3.unk2 { + // FIXME: this is wrong. I think there might be multiple, separate navimeshes in really big zones but I'm not sure yet. + navimesh_path = Some(layer_set.path_nvm.replace("/server/data/", "").to_string()); + } + + if navimesh_path.is_none() { + tracing::info!("No navimesh path found, monsters will not function correctly!"); + return instance; + } + + let config = get_config(); + if config.filesystem.navimesh_path.is_empty() { + tracing::warn!("Navimesh path is not set! Monsters will not function correctly!"); + } else { + let mut nvm_path = PathBuf::from(config.filesystem.navimesh_path); + nvm_path.push(&navimesh_path.unwrap()); + + if let Ok(nvm_bytes) = std::fs::read(&nvm_path) { + instance.navmesh = Navmesh::from_existing(&nvm_bytes).unwrap(); + + tracing::info!("Successfully loaded navimesh from {nvm_path:?}"); + } else { + tracing::warn!( + "Failed to read {nvm_path:?}, monsters will not function correctly!" + ); + } + } + + instance + } + fn find_actor(&self, id: ObjectId) -> Option<&NetworkedActor> { self.actors.get(&id) } @@ -73,13 +141,30 @@ impl Instance { } fn insert_npc(&mut self, id: ObjectId, spawn: NpcSpawn) { - self.actors.insert(id, NetworkedActor::Npc(spawn)); + self.actors.insert( + id, + NetworkedActor::Npc { + current_path: VecDeque::default(), + current_path_lerp: 0.0, + current_target: None, + last_position: None, + spawn, + }, + ); } fn generate_actor_id() -> u32 { // TODO: ensure we don't collide with another actor fastrand::u32(..) } + + fn find_all_players(&self) -> Vec { + self.actors + .iter() + .filter(|(_, y)| matches!(y, NetworkedActor::Player(_))) + .map(|(x, _)| *x) + .collect() + } } #[derive(Default, Debug, Clone)] @@ -128,10 +213,145 @@ impl WorldServer { } } +fn server_logic_tick(data: &mut WorldServer) { + for (_, instance) in &mut data.instances { + let mut actor_moves = Vec::new(); + let players = instance.find_all_players(); + + // const pass + let instance_copy = instance.clone(); // TODO: refactor out please + for (id, actor) in &instance.actors { + if let NetworkedActor::Npc { + current_path, + current_path_lerp, + current_target, + spawn, + last_position, + } = actor + { + if current_target.is_some() { + let needs_repath = current_path.is_empty(); + if !needs_repath { + // follow current path + let next_position = Position { + x: current_path[0][0], + y: current_path[0][1], + z: current_path[0][2], + }; + let current_position = last_position.unwrap_or(spawn.common.pos); + + let dir_x = current_position.x - next_position.x; + let dir_z = current_position.z - next_position.z; + let rotation = f32::atan2(-dir_z, dir_x).to_degrees(); + + actor_moves.push(FromServer::ActorMove( + id.0, + Position::lerp(current_position, next_position, *current_path_lerp), + rotation, + )); + } + } + } + } + + // mut pass + for (id, actor) in &mut instance.actors { + if let NetworkedActor::Npc { + current_path, + current_path_lerp, + current_target, + spawn, + last_position, + } = actor + { + // switch to the next node if we passed this one + if *current_path_lerp >= 1.0 { + *current_path_lerp = 0.0; + if !current_path.is_empty() { + *last_position = Some(Position { + x: current_path[0][0], + y: current_path[0][1], + z: current_path[0][2], + }); + current_path.pop_front(); + } + } + + if current_target.is_none() { + // find a player + if !players.is_empty() { + *current_target = Some(players[0]); + } + } else if !current_path.is_empty() { + let next_position = Position { + x: current_path[0][0], + y: current_path[0][1], + z: current_path[0][2], + }; + let current_position = last_position.unwrap_or(spawn.common.pos); + let distance = Position::distance(current_position, next_position); + + // TODO: this doesn't work like it should + *current_path_lerp += (10.0 / distance).clamp(0.0, 1.0); + } + + let target_actor = instance_copy.find_actor(current_target.unwrap()); + let target_pos = target_actor.unwrap().get_common_spawn().pos; + let distance = Position::distance(spawn.common.pos, target_pos); + let needs_repath = current_path.is_empty() && distance > 5.0; // TODO: confirm distance this in retail + if needs_repath && current_target.is_some() { + let current_pos = spawn.common.pos; + let target_actor = instance_copy.find_actor(current_target.unwrap()); + let target_pos = target_actor.unwrap().get_common_spawn().pos; + *current_path = instance + .navmesh + .calculate_path( + [current_pos.x, current_pos.y, current_pos.z], + [target_pos.x, target_pos.y, target_pos.z], + ) + .into(); + } + + // update common spawn + for msg in &actor_moves { + if let FromServer::ActorMove(msg_id, pos, rotation) = msg { + if id.0 == *msg_id { + spawn.common.pos = *pos; + spawn.common.rotation = *rotation; + } + } + } + } + } + + // inform clients of the NPCs new positions + for msg in actor_moves { + for (_, (handle, _)) in &mut data.clients { + if handle.send(msg.clone()).is_err() { + //to_remove.push(id); + } + } + } + } +} + pub async fn server_main_loop(mut recv: Receiver) -> Result<(), std::io::Error> { let data = Arc::new(Mutex::new(WorldServer::default())); let game_data = Arc::new(Mutex::new(GameData::new())); + { + let data = data.clone(); + tokio::task::spawn(async move { + let mut interval = tokio::time::interval(Duration::from_millis(500)); + interval.tick().await; + loop { + interval.tick().await; + let mut data = data.lock().unwrap(); + server_logic_tick(&mut data); + } + }); + } + while let Some(msg) = recv.recv().await { let mut to_remove = Vec::new(); @@ -146,9 +366,11 @@ pub async fn server_main_loop(mut recv: Receiver) -> Result<(), std::i let mut data = data.lock().unwrap(); // create a new instance if necessary - data.instances - .entry(zone_id) - .or_insert_with(Instance::default); + if !data.instances.contains_key(&zone_id) { + let mut game_data = game_data.lock().unwrap(); + data.instances + .insert(zone_id, Instance::new(zone_id, &mut game_data)); + } // Send existing player data, if any if let Some(instance) = data.find_instance(zone_id).cloned() { @@ -161,8 +383,8 @@ pub async fn server_main_loop(mut recv: Receiver) -> Result<(), std::i // send existing player data for (id, spawn) in &instance.actors { let npc_spawn = match spawn { - NetworkedActor::Player(npc_spawn) => npc_spawn, - NetworkedActor::Npc(npc_spawn) => npc_spawn, + NetworkedActor::Player(spawn) => spawn, + NetworkedActor::Npc { spawn, .. } => spawn, }; // Note that we currently only support spawning via the NPC packet, hence why we don't need to differentiate here @@ -284,7 +506,7 @@ pub async fn server_main_loop(mut recv: Receiver) -> Result<(), std::i { let common = match spawn { NetworkedActor::Player(npc_spawn) => &mut npc_spawn.common, - NetworkedActor::Npc(npc_spawn) => &mut npc_spawn.common, + NetworkedActor::Npc { spawn, .. } => &mut spawn.common, }; common.pos = position; common.rotation = rotation; diff --git a/src/world/zone.rs b/src/world/zone.rs index 2d38b5d..93274a8 100644 --- a/src/world/zone.rs +++ b/src/world/zone.rs @@ -1,5 +1,3 @@ -use std::path::PathBuf; - use icarus::TerritoryType::TerritoryTypeSheet; use physis::{ common::Language, @@ -10,10 +8,7 @@ use physis::{ resource::Resource, }; -use crate::{ - common::{GameData, TerritoryNameKind}, - config::get_config, -}; +use crate::common::{GameData, TerritoryNameKind}; /// Represents a loaded zone #[derive(Default, Debug)] @@ -50,21 +45,6 @@ impl Zone { let lgb_file = game_data.resource.read(&path).unwrap(); let lgb = Lvb::from_existing(&lgb_file).unwrap(); - for layer_set in &lgb.scns[0].unk3.unk2 { - // FIXME: this is wrong. I think there might be multiple, separate navimeshes in really big zones but I'm not sure yet. - zone.navimesh_path = layer_set.path_nvm.replace("/server/data/", "").to_string(); - } - - let config = get_config(); - if config.filesystem.navimesh_path.is_empty() { - tracing::warn!("Navimesh path is not set! Monsters will not function correctly!"); - } else { - let mut nvm_path = PathBuf::from(config.filesystem.navimesh_path); - nvm_path.push(&zone.navimesh_path); - - Self::load_navimesh(nvm_path.to_str().unwrap()); - } - let mut load_lgb = |path: &str| -> Option { let lgb_file = game_data.resource.read(path)?; tracing::info!("Loading {path}"); @@ -138,14 +118,4 @@ impl Zone { None } - - // TODO: add better error handling here - fn load_navimesh(path: &str) -> Option<()> { - if !std::fs::exists(path).unwrap_or_default() { - tracing::warn!("Navimesh {path} does not exist, monsters will not function correctly!"); - return None; - } - - Some(()) - } }