Make debug drawing in bevy_landmass expose entities instead of agent IDs.

crates/bevy_landmass/src/agent.rs

@@ -209,6 +209,7 @@ pub(crate) fn add_agents_to_archipelagos<CS: CoordinateSystem>(
       match new_agent_map.remove(agent_entity) {
         None => {
           archipelago.archipelago.remove_agent(*agent_id);
+          archipelago.reverse_agents.remove(agent_id);
           false
         }
         Some(_) => true,
@@ -225,6 +226,7 @@ pub(crate) fn add_agents_to_archipelagos<CS: CoordinateSystem>(
           new_agent.max_speed,
         ));
       archipelago.agents.insert(new_agent_entity, agent_id);
+      archipelago.reverse_agents.insert(agent_id, new_agent_entity);
     }
   }
 }

crates/bevy_landmass/src/debug.rs

@@ -10,15 +10,63 @@ use bevy::{
   gizmos::AppGizmoBuilder,
   math::{Isometry3d, Quat},
   prelude::{
-    Deref, DerefMut, GizmoConfig, GizmoConfigGroup, Gizmos, IntoSystemConfigs,
-    Plugin, Query, Res, Resource,
+    Deref, DerefMut, Entity, GizmoConfig, GizmoConfigGroup, Gizmos,
+    IntoSystemConfigs, Plugin, Query, Res, Resource,
   },
   reflect::Reflect,
   time::Time,
   transform::components::Transform,
 };
 
-pub use landmass::debug::*;
+pub use landmass::debug::DebugDrawError;
+
+/// The type of debug points.
+#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
+pub enum PointType {
+  /// The position of an agent.
+  AgentPosition(Entity),
+  /// The target of an agent.
+  TargetPosition(Entity),
+  /// The waypoint of an agent.
+  Waypoint(Entity),
+}
+
+/// The type of debug lines.
+#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
+pub enum LineType {
+  /// An edge of a node that is the boundary of a nav mesh.
+  BoundaryEdge,
+  /// An edge of a node that is connected to another node.
+  ConnectivityEdge,
+  /// A link between two islands along their boundary edge.
+  BoundaryLink,
+  /// Part of an agent's current path. The corridor follows the path along
+  /// nodes, not the actual path the agent will travel.
+  AgentCorridor(Entity),
+  /// Line from an agent to its target.
+  Target(Entity),
+  /// Line to the waypoint of an agent.
+  Waypoint(Entity),
+}
+
+/// The type of debug triangles.
+#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
+pub enum TriangleType {
+  /// Part of a node/polygon in a nav mesh.
+  Node,
+}
+
+/// Trait to "draw" Archipelago state to. Users should implement this to
+/// visualize the state of their Archipelago.
+pub trait DebugDrawer<CS: CoordinateSystem> {
+  fn add_point(&mut self, point_type: PointType, point: CS::Coordinate);
+  fn add_line(&mut self, line_type: LineType, line: [CS::Coordinate; 2]);
+  fn add_triangle(
+    &mut self,
+    triangle_type: TriangleType,
+    triangle: [CS::Coordinate; 3],
+  );
+}
 
 /// Draws all parts of `archipelago` to `debug_drawer`. This is a lower level
 /// API to allow custom debug drawing. For a pre-made implementation, use
@@ -27,9 +75,78 @@ pub fn draw_archipelago_debug<CS: CoordinateSystem>(
   archipelago: &crate::Archipelago<CS>,
   debug_drawer: &mut impl DebugDrawer<CS>,
 ) -> Result<(), DebugDrawError> {
+  struct DebugDrawerAdapter<'a, CS: CoordinateSystem, D: DebugDrawer<CS>> {
+    archipelago: &'a crate::Archipelago<CS>,
+    drawer: &'a mut D,
+  }
+
+  impl<CS: CoordinateSystem, D: DebugDrawer<CS>>
+    landmass::debug::DebugDrawer<CS> for DebugDrawerAdapter<'_, CS, D>
+  {
+    fn add_point(
+      &mut self,
+      point_type: landmass::debug::PointType,
+      point: CS::Coordinate,
+    ) {
+      let point_type = match point_type {
+        landmass::debug::PointType::AgentPosition(agent_id) => {
+          PointType::AgentPosition(
+            *self.archipelago.reverse_agents.get(&agent_id).unwrap(),
+          )
+        }
+        landmass::debug::PointType::TargetPosition(agent_id) => {
+          PointType::TargetPosition(
+            *self.archipelago.reverse_agents.get(&agent_id).unwrap(),
+          )
+        }
+        landmass::debug::PointType::Waypoint(agent_id) => PointType::Waypoint(
+          *self.archipelago.reverse_agents.get(&agent_id).unwrap(),
+        ),
+      };
+      self.drawer.add_point(point_type, point);
+    }
+
+    fn add_line(
+      &mut self,
+      line_type: landmass::debug::LineType,
+      line: [CS::Coordinate; 2],
+    ) {
+      let line_type = match line_type {
+        landmass::debug::LineType::BoundaryEdge => LineType::BoundaryEdge,
+        landmass::debug::LineType::ConnectivityEdge => {
+          LineType::ConnectivityEdge
+        }
+        landmass::debug::LineType::BoundaryLink => LineType::BoundaryLink,
+        landmass::debug::LineType::AgentCorridor(agent_id) => {
+          LineType::AgentCorridor(
+            *self.archipelago.reverse_agents.get(&agent_id).unwrap(),
+          )
+        }
+        landmass::debug::LineType::Target(agent_id) => LineType::Target(
+          *self.archipelago.reverse_agents.get(&agent_id).unwrap(),
+        ),
+        landmass::debug::LineType::Waypoint(agent_id) => LineType::Waypoint(
+          *self.archipelago.reverse_agents.get(&agent_id).unwrap(),
+        ),
+      };
+      self.drawer.add_line(line_type, line);
+    }
+
+    fn add_triangle(
+      &mut self,
+      triangle_type: landmass::debug::TriangleType,
+      triangle: [CS::Coordinate; 3],
+    ) {
+      let triangle_type = match triangle_type {
+        landmass::debug::TriangleType::Node => TriangleType::Node,
+      };
+      self.drawer.add_triangle(triangle_type, triangle);
+    }
+  }
+
   landmass::debug::draw_archipelago_debug(
     &archipelago.archipelago,
-    debug_drawer,
+    &mut DebugDrawerAdapter { archipelago, drawer: debug_drawer },
   )
 }
 
@@ -149,3 +266,7 @@ fn draw_archipelagos_default<CS: CoordinateSystem>(
       .expect("the archipelago can be debug-drawn");
   }
 }
+
+#[cfg(test)]
+#[path = "debug_test.rs"]
+mod test;

crates/bevy_landmass/src/debug_test.rs

@@ -0,0 +1,214 @@
+use std::{cmp::Ordering, sync::Arc};
+
+use bevy::{
+  app::App,
+  asset::{AssetPlugin, Assets},
+  math::Vec3,
+  prelude::{Transform, TransformPlugin},
+  MinimalPlugins,
+};
+use landmass::AgentOptions;
+
+use crate::{
+  coords::ThreeD, Agent, Agent3dBundle, AgentSettings, Archipelago3d,
+  ArchipelagoRef3d, Island, Island3dBundle, Landmass3dPlugin, NavMesh3d,
+  NavMeshHandle, NavigationMesh3d,
+};
+
+use super::{
+  draw_archipelago_debug, DebugDrawer, LineType, PointType, TriangleType,
+};
+
+struct FakeDrawer {
+  points: Vec<(PointType, Vec3)>,
+  lines: Vec<(LineType, [Vec3; 2])>,
+  triangles: Vec<(TriangleType, [Vec3; 3])>,
+}
+impl DebugDrawer<ThreeD> for FakeDrawer {
+  fn add_point(&mut self, point_type: crate::debug::PointType, point: Vec3) {
+    self.points.push((point_type, point));
+  }
+
+  fn add_line(&mut self, line_type: crate::debug::LineType, line: [Vec3; 2]) {
+    self.lines.push((line_type, line));
+  }
+
+  fn add_triangle(
+    &mut self,
+    triangle_type: crate::debug::TriangleType,
+    triangle: [Vec3; 3],
+  ) {
+    self.triangles.push((triangle_type, triangle));
+  }
+}
+
+impl FakeDrawer {
+  fn new() -> Self {
+    Self { points: vec![], lines: vec![], triangles: vec![] }
+  }
+
+  fn sort(&mut self) {
+    fn lex_order_points(a: Vec3, b: Vec3) -> Ordering {
+      a.x
+        .partial_cmp(&b.x)
+        .unwrap()
+        .then(a.y.partial_cmp(&b.y).unwrap())
+        .then(a.z.partial_cmp(&b.z).unwrap())
+    }
+    self.points.sort_by(|a, b| a.0.cmp(&b.0).then(lex_order_points(a.1, b.1)));
+    self.lines.sort_by(|a, b| {
+      a.0
+        .cmp(&b.0)
+        .then(lex_order_points(a.1[0], b.1[0]))
+        .then(lex_order_points(a.1[1], b.1[1]))
+    });
+    self.triangles.sort_by(|a, b| {
+      a.0
+        .cmp(&b.0)
+        .then(lex_order_points(a.1[0], b.1[0]))
+        .then(lex_order_points(a.1[1], b.1[1]))
+        .then(lex_order_points(a.1[2], b.1[2]))
+    });
+  }
+}
+
+#[test]
+fn draws_archipelago_debug() {
+  let mut app = App::new();
+
+  app
+    .add_plugins(MinimalPlugins)
+    .add_plugins(TransformPlugin)
+    .add_plugins(AssetPlugin::default())
+    .add_plugins(Landmass3dPlugin::default());
+  // Update early to allow the time to not be 0.0.
+  app.update();
+
+  let archipelago_id = app
+    .world_mut()
+    .spawn(Archipelago3d::new(AgentOptions::default_for_agent_radius(0.5)))
+    .id();
+
+  let nav_mesh = Arc::new(
+    NavigationMesh3d {
+      vertices: vec![
+        Vec3::new(1.0, 0.0, 1.0),
+        Vec3::new(4.0, 0.0, 1.0),
+        Vec3::new(4.0, 0.0, 4.0),
+        Vec3::new(1.0, 0.0, 4.0),
+      ],
+      polygons: vec![vec![3, 2, 1, 0]],
+      polygon_type_indices: vec![0],
+    }
+    .validate()
+    .expect("is valid"),
+  );
+
+  let nav_mesh_handle = app
+    .world_mut()
+    .resource_mut::<Assets<NavMesh3d>>()
+    .add(NavMesh3d { nav_mesh, type_index_to_node_type: Default::default() });
+
+  app.world_mut().spawn((
+    Transform::from_translation(Vec3::new(1.0, 1.0, 1.0)),
+    Island3dBundle {
+      island: Island,
+      archipelago_ref: ArchipelagoRef3d::new(archipelago_id),
+      nav_mesh: NavMeshHandle(nav_mesh_handle.clone()),
+    },
+  ));
+
+  let agent = app
+    .world_mut()
+    .spawn((
+      Transform::from_translation(Vec3::new(3.0, 1.0, 3.0)),
+      Agent3dBundle {
+        agent: Agent::default(),
+        archipelago_ref: ArchipelagoRef3d::new(archipelago_id),
+        settings: AgentSettings {
+          radius: 0.5,
+          max_speed: 1.0,
+          desired_speed: 1.0,
+        },
+      },
+    ))
+    .id();
+
+  // Sync the islands with landmass.
+  app.update();
+
+  let archipelago = app
+    .world()
+    .get::<Archipelago3d>(archipelago_id)
+    .expect("archipelago exists");
+
+  let mut fake_drawer = FakeDrawer::new();
+  draw_archipelago_debug(archipelago, &mut fake_drawer).unwrap();
+
+  fake_drawer.sort();
+
+  assert_eq!(
+    fake_drawer.points,
+    [(PointType::AgentPosition(agent), Vec3::new(3.0, 1.0, 3.0))]
+  );
+
+  assert_eq!(
+    fake_drawer.lines,
+    [
+      (
+        LineType::BoundaryEdge,
+        [Vec3::new(2.0, 1.0, 2.0), Vec3::new(2.0, 1.0, 5.0)]
+      ),
+      (
+        LineType::BoundaryEdge,
+        [Vec3::new(2.0, 1.0, 5.0), Vec3::new(5.0, 1.0, 5.0)]
+      ),
+      (
+        LineType::BoundaryEdge,
+        [Vec3::new(5.0, 1.0, 2.0), Vec3::new(2.0, 1.0, 2.0)]
+      ),
+      (
+        LineType::BoundaryEdge,
+        [Vec3::new(5.0, 1.0, 5.0), Vec3::new(5.0, 1.0, 2.0)]
+      ),
+    ]
+  );
+
+  assert_eq!(
+    fake_drawer.triangles,
+    [
+      (
+        TriangleType::Node,
+        [
+          Vec3::new(2.0, 1.0, 2.0),
+          Vec3::new(2.0, 1.0, 5.0),
+          Vec3::new(3.5, 1.0, 3.5),
+        ]
+      ),
+      (
+        TriangleType::Node,
+        [
+          Vec3::new(2.0, 1.0, 5.0),
+          Vec3::new(5.0, 1.0, 5.0),
+          Vec3::new(3.5, 1.0, 3.5),
+        ]
+      ),
+      (
+        TriangleType::Node,
+        [
+          Vec3::new(5.0, 1.0, 2.0),
+          Vec3::new(2.0, 1.0, 2.0),
+          Vec3::new(3.5, 1.0, 3.5),
+        ]
+      ),
+      (
+        TriangleType::Node,
+        [
+          Vec3::new(5.0, 1.0, 5.0),
+          Vec3::new(5.0, 1.0, 2.0),
+          Vec3::new(3.5, 1.0, 3.5),
+        ]
+      ),
+    ]
+  );
+}