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

@@ -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

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,
+    CreateContext, DT_SUCCESS, RC_MESH_NULL_IDX, dtCreateNavMeshData, dtNavMeshCreateParams,
-    dtNavMesh_addTile, dtNavMesh_init, dtNavMeshCreateParams, dtNavMeshParams, dtNavMeshQuery,
+    dtNavMeshQuery, dtNavMeshQuery_findNearestPoly, dtPolyRef, dtQueryFilter,
-    dtNavMeshQuery_findNearestPoly, dtNavMeshQuery_findPath, dtNavMeshQuery_findStraightPath,
-    dtNavMeshQuery_init, dtPolyRef, dtQueryFilter, dtQueryFilter_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<Vec3>,
+    from_position: Vec3,
+    to_position: Vec3,
 }
 
 impl NavigationState {
-    pub fn calculate_path(&mut self, from_position: Vec3) {
+    pub fn calculate_path(&mut self) {
-        unsafe {
+        let start_pos = [
-            let start_pos = [from_position.x, from_position.y, from_position.z];
+            self.from_position.x,
-            let end_pos = [0.0, 0.0, 0.0];
+            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 {
+        self.path = self
-                m_areaCost: [0.0; 64],
+            .navmesh
-                m_includeFlags: 0,
+            .calculate_path(start_pos, end_pos)
-                m_excludeFlags: 0,
+            .iter()
-            };
+            .map(|x| Vec3::from_slice(x))
-            dtQueryFilter_dtQueryFilter(&mut filter);
+            .collect();
-
-            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],
-                });
-            }
-        }
     }
 }
 
@@ -111,16 +79,38 @@ fn main() {
 
     App::new()
         .add_event::<Navigate>()
-        .add_plugins((DefaultPlugins, MeshPickingPlugin))
+        .add_event::<SetOrigin>()
+        .add_event::<SetTarget>()
+        .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 {
+                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),
-                },
             ))
             .observe(
-                |mut trigger: Trigger<Pointer<Click>>, mut events: EventWriter<Navigate>| {
+                |mut trigger: Trigger<Pointer<Click>>,
+                 mut navigate_events: EventWriter<Navigate>,
+                 mut target_events: EventWriter<SetTarget>,
+                 mut origin_events: EventWriter<SetOrigin>| {
                     let click_event: &Pointer<Click> = 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<i32> = indices.iter().map(|x| *x as i32).collect();
         let mut tri_area_ids: Vec<u8> = 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<Assets<Mesh>>,
+    materials: &mut ResMut<Assets<StandardMaterial>>,
+    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<Pointer<Click>>,
+                 mut navigate_events: EventWriter<Navigate>,
+                 mut target_events: EventWriter<SetTarget>,
+                 mut origin_events: EventWriter<SetOrigin>| {
+                    let click_event: &Pointer<Click> = 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<i32> = part.indices.iter().map(|x| *x as i32).collect();
+        let mut tri_area_ids: Vec<u8> = 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 {
+        navigation_state.navmesh = Navmesh::new(
-            orig: [0.0; 3],
+            NavmeshParams {
-            tileWidth: 100.0,
+                orig: (*poly_mesh).bmin,
-            tileHeight: 100.0,
+                tile_width: (*poly_mesh).bmax[0] - (*poly_mesh).bmin[0],
-            maxTiles: 1000,
+                tile_height: (*poly_mesh).bmax[2] - (*poly_mesh).bmin[2],
-            maxPolys: 1000,
+                max_tiles: 1,
-        };
+                max_polys: (*poly_mesh).npolys,
-
+            },
-        let navmesh = dtAllocNavMesh();
+            Vec::from_raw_parts(out_data, out_data_size as usize, out_data_size as usize),
-        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.query = dtAllocNavMeshQuery();
+        // TODO: output in the correct directory
-        dtNavMeshQuery_init(navigation_state.query, navmesh, 1024);
+        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<Navigate>,
+    mut origin_events: EventReader<SetOrigin>,
+    mut target_events: EventReader<SetTarget>,
     mut navigation_state: ResMut<NavigationState>,
 ) {
+    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() {
+    for event in origin_events.read() {
-        navigation_state.calculate_path(event.0);
+        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();
     }
 }

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

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),

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

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<u8>,
+
+    #[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<u8>) -> 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<Self> {
+        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<Vec<u8>> {
+        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);
+        }
+    }
+}

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<ObjectId>,
+        last_position: Option<Position>,
+        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<ObjectId, NetworkedActor>,
+    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<ObjectId> {
+        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<ToServer>) -> 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<ToServer>) -> Result<(), std::i
                 let mut data = data.lock().unwrap();
 
                 // create a new instance if necessary
+                if !data.instances.contains_key(&zone_id) {
+                    let mut game_data = game_data.lock().unwrap();
                     data.instances
-                    .entry(zone_id)
+                        .insert(zone_id, Instance::new(zone_id, &mut game_data));
-                    .or_insert_with(Instance::default);
+                }
 
                 // 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<ToServer>) -> 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::Player(spawn) => spawn,
-                                    NetworkedActor::Npc(npc_spawn) => npc_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<ToServer>) -> 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;

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::{
+use crate::common::{GameData, TerritoryNameKind};
-    common::{GameData, TerritoryNameKind},
-    config::get_config,
-};
 
 /// 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<LayerGroup> {
             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(())
-    }
 }