Begin implementing navimesh path visualization

It doesn't look quite right - probably because the navmesh
generation is bad. But it's progress!

Cargo.lock

@@ -725,6 +725,7 @@ dependencies = [
  "bevy_input_focus",
  "bevy_math",
  "bevy_pbr",
+ "bevy_picking",
  "bevy_platform",
  "bevy_ptr",
  "bevy_reflect",
@@ -857,6 +858,31 @@ dependencies = [
  "tracing",
 ]
 
+[[package]]
+name = "bevy_picking"
+version = "0.16.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8ed04757938655ed8094ea1efb533f99063a8b22abffc22010c694d291522850"
+dependencies = [
+ "bevy_app",
+ "bevy_asset",
+ "bevy_derive",
+ "bevy_ecs",
+ "bevy_input",
+ "bevy_math",
+ "bevy_mesh",
+ "bevy_platform",
+ "bevy_reflect",
+ "bevy_render",
+ "bevy_time",
+ "bevy_transform",
+ "bevy_utils",
+ "bevy_window",
+ "crossbeam-channel",
+ "tracing",
+ "uuid",
+]
+
 [[package]]
 name = "bevy_platform"
 version = "0.16.1"

Cargo.toml

@@ -95,6 +95,9 @@ bevy = { version = "0.16", features = ["std",
   "bevy_window",
   "bevy_winit",
   "tonemapping_luts",
+  "bevy_picking",
+  "bevy_mesh_picking_backend",
+  "bevy_gizmos",
   "x11"], default-features = false, optional = true }
 
 # for navimesh generation

src/bin/kawari-navimesh.rs

@@ -2,6 +2,8 @@ use std::ptr::{null, null_mut};
 
 use bevy::{
     asset::RenderAssetUsages,
+    color::palettes::tailwind::{PINK_100, RED_500},
+    picking::pointer::PointerInteraction,
     prelude::*,
     render::mesh::{Indices, PrimitiveTopology},
 };
@@ -17,18 +19,90 @@ use physis::{
 use recastnavigation_sys::{
     CreateContext, DT_SUCCESS, dtAllocNavMesh, dtAllocNavMeshQuery, dtCreateNavMeshData,
     dtNavMesh_addTile, dtNavMesh_init, dtNavMeshCreateParams, dtNavMeshParams, dtNavMeshQuery,
-    dtNavMeshQuery_findNearestPoly, dtNavMeshQuery_findPath, dtNavMeshQuery_init, dtPolyRef,
+    dtNavMeshQuery_findNearestPoly, dtNavMeshQuery_findPath, dtNavMeshQuery_findStraightPath,
-    dtQueryFilter, dtQueryFilter_dtQueryFilter, rcAllocCompactHeightfield, rcAllocContourSet,
+    dtNavMeshQuery_init, dtPolyRef, dtQueryFilter, dtQueryFilter_dtQueryFilter,
-    rcAllocHeightfield, rcAllocPolyMesh, rcAllocPolyMeshDetail, rcBuildCompactHeightfield,
+    rcAllocCompactHeightfield, rcAllocContourSet, rcAllocHeightfield, rcAllocPolyMesh,
-    rcBuildContours, rcBuildContoursFlags_RC_CONTOUR_TESS_WALL_EDGES, rcBuildDistanceField,
+    rcAllocPolyMeshDetail, rcBuildCompactHeightfield, rcBuildContours,
-    rcBuildPolyMesh, rcBuildPolyMeshDetail, rcBuildRegions, rcCalcGridSize, rcContext,
+    rcBuildContoursFlags_RC_CONTOUR_TESS_WALL_EDGES, rcBuildDistanceField, rcBuildPolyMesh,
-    rcCreateHeightfield, rcErodeWalkableArea, rcHeightfield, rcMarkWalkableTriangles,
+    rcBuildPolyMeshDetail, rcBuildRegions, rcCalcGridSize, rcContext, rcCreateHeightfield,
-    rcRasterizeTriangles,
+    rcErodeWalkableArea, rcHeightfield, rcMarkWalkableTriangles, rcRasterizeTriangles,
 };
 
 #[derive(Resource)]
 struct ZoneToLoad(u16);
 
+#[derive(Resource, Default)]
+struct NavigationState {
+    query: *mut dtNavMeshQuery,
+    path: Vec<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];
+
+            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],
+                });
+            }
+        }
+    }
+}
+
+unsafe impl Send for NavigationState {}
+unsafe impl Sync for NavigationState {}
+
 fn main() {
     tracing_subscriber::fmt::init();
 
@@ -36,12 +110,18 @@ fn main() {
     let zone_id: u16 = args[1].parse().unwrap();
 
     App::new()
-        .add_plugins(DefaultPlugins)
+        .add_event::<Navigate>()
+        .add_plugins((DefaultPlugins, MeshPickingPlugin))
         .add_systems(Startup, setup)
+        .add_systems(Update, draw_mesh_intersections)
         .insert_resource(ZoneToLoad(zone_id))
+        .insert_resource(NavigationState::default())
         .run();
 }
 
+#[derive(Event, Reflect, Clone, Debug)]
+struct Navigate(Vec3);
+
 /// Walk each node, add it's collision model to the scene.
 fn walk_node(
     node: &ResourceNode,
@@ -53,10 +133,7 @@ fn walk_node(
     height_field: *mut rcHeightfield,
 ) {
     if !node.vertices.is_empty() {
-        let mut mesh = Mesh::new(
+        let mut mesh = Mesh::new(PrimitiveTopology::TriangleList, RenderAssetUsages::all());
-            PrimitiveTopology::TriangleList,
-            RenderAssetUsages::RENDER_WORLD,
-        );
 
         let mut positions = Vec::new();
         for vec in &node.vertices {
@@ -77,25 +154,35 @@ fn walk_node(
 
         mesh.insert_indices(Indices::U32(indices.clone()));
 
+        mesh.compute_normals();
+
         // insert into 3d scene
-        commands.spawn((
+        commands
-            Mesh3d(meshes.add(mesh)),
+            .spawn((
-            MeshMaterial3d(materials.add(Color::srgb(
+                Mesh3d(meshes.add(mesh)),
-                fastrand::f32(),
+                MeshMaterial3d(materials.add(Color::srgb(
-                fastrand::f32(),
+                    fastrand::f32(),
-                fastrand::f32(),
+                    fastrand::f32(),
-            ))),
+                    fastrand::f32(),
-            Transform {
+                ))),
-                translation: Vec3::from_array(transform.translation),
+                Transform {
-                rotation: Quat::from_euler(
+                    translation: Vec3::from_array(transform.translation),
-                    EulerRot::XYZ,
+                    rotation: Quat::from_euler(
-                    transform.rotation[0],
+                        EulerRot::XYZ,
-                    transform.rotation[1],
+                        transform.rotation[0],
-                    transform.rotation[2],
+                        transform.rotation[1],
-                ),
+                        transform.rotation[2],
-                scale: Vec3::from_array(transform.scale),
+                    ),
-            },
+                    scale: Vec3::from_array(transform.scale),
-        ));
+                },
+            ))
+            .observe(
+                |mut trigger: Trigger<Pointer<Click>>, mut events: EventWriter<Navigate>| {
+                    let click_event: &Pointer<Click> = trigger.event();
+                    events.write(Navigate(click_event.hit.position.unwrap()));
+                    trigger.propagate(false);
+                },
+            );
 
         // Step 2: insert geoemtry into heightfield
         let tile_indices: Vec<i32> = indices.iter().map(|x| *x as i32).collect();
@@ -161,7 +248,6 @@ fn get_polygon_at_location(
                 nearest_pt.as_mut_ptr()
             ) == DT_SUCCESS
         );
-        assert!(nearest_ref != 0);
 
         return (nearest_ref, nearest_pt);
     }
@@ -173,6 +259,7 @@ fn setup(
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
     zone_id: Res<ZoneToLoad>,
+    mut navigation_state: ResMut<NavigationState>,
 ) {
     let zone_id = zone_id.0;
     let config = get_config();
@@ -418,38 +505,8 @@ fn setup(
             dtNavMesh_addTile(navmesh, out_data, out_data_size, 0, 0, null_mut()) == DT_SUCCESS
         );
 
-        let query = dtAllocNavMeshQuery();
+        navigation_state.query = dtAllocNavMeshQuery();
-        dtNavMeshQuery_init(query, navmesh, 1024);
+        dtNavMeshQuery_init(navigation_state.query, navmesh, 1024);
-
-        let start_pos = [0.0, 0.0, 0.0];
-        let end_pos = [5.0, 0.0, 0.0];
-
-        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(query, start_pos, &filter);
-        let (end_poly, end_poly_pos) = get_polygon_at_location(query, end_pos, &filter);
-
-        let mut path = [0; 128];
-        let mut path_count = 0;
-        dtNavMeshQuery_findPath(
-            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
-        assert!(path_count > 0);
-
-        dbg!(path);
     }
 
     // camera
@@ -458,3 +515,27 @@ fn setup(
         Transform::from_xyz(15.0, 15.0, 15.0).looking_at(Vec3::ZERO, Vec3::Y),
     ));
 }
+
+fn draw_mesh_intersections(
+    pointers: Query<&PointerInteraction>,
+    mut gizmos: Gizmos,
+    mut navigate_events: EventReader<Navigate>,
+    mut navigation_state: ResMut<NavigationState>,
+) {
+    for pos in &navigation_state.path {
+        gizmos.sphere(*pos, 0.05, RED_500);
+    }
+
+    for (point, normal) in pointers
+        .iter()
+        .filter_map(|interaction| interaction.get_nearest_hit())
+        .filter_map(|(_entity, hit)| hit.position.zip(hit.normal))
+    {
+        gizmos.sphere(point, 0.05, RED_500);
+        gizmos.arrow(point, point + normal.normalize() * 0.5, PINK_100);
+    }
+
+    for event in navigate_events.read() {
+        navigation_state.calculate_path(event.0);
+    }
+}