diff --git a/crates/bevy_landmass/src/agent.rs b/crates/bevy_landmass/src/agent.rs index 4130590..2bdd839 100644 --- a/crates/bevy_landmass/src/agent.rs +++ b/crates/bevy_landmass/src/agent.rs @@ -209,6 +209,7 @@ pub(crate) fn add_agents_to_archipelagos( 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( new_agent.max_speed, )); archipelago.agents.insert(new_agent_entity, agent_id); + archipelago.reverse_agents.insert(agent_id, new_agent_entity); } } } diff --git a/crates/bevy_landmass/src/debug.rs b/crates/bevy_landmass/src/debug.rs index 96c8c61..584f2d9 100644 --- a/crates/bevy_landmass/src/debug.rs +++ b/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 { + 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( archipelago: &crate::Archipelago, debug_drawer: &mut impl DebugDrawer, ) -> Result<(), DebugDrawError> { + struct DebugDrawerAdapter<'a, CS: CoordinateSystem, D: DebugDrawer> { + archipelago: &'a crate::Archipelago, + drawer: &'a mut D, + } + + impl> + landmass::debug::DebugDrawer 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( .expect("the archipelago can be debug-drawn"); } } + +#[cfg(test)] +#[path = "debug_test.rs"] +mod test; diff --git a/crates/bevy_landmass/src/debug_test.rs b/crates/bevy_landmass/src/debug_test.rs new file mode 100644 index 0000000..85c7656 --- /dev/null +++ b/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 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::>() + .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::(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), + ] + ), + ] + ); +} diff --git a/crates/bevy_landmass/src/lib.rs b/crates/bevy_landmass/src/lib.rs index 8526aa4..7f8d5c4 100644 --- a/crates/bevy_landmass/src/lib.rs +++ b/crates/bevy_landmass/src/lib.rs @@ -158,6 +158,9 @@ pub struct Archipelago { /// A map from the Bevy entity to its associated agent ID in /// [`Self::archipelago`]. agents: HashMap, + /// A map from the agent ID to its associated Bevy entity in + /// [`Self::archipelago`]. This is just the reverse of [`Self::agents`]. + reverse_agents: HashMap, /// A map from the Bevy entity to its associated character ID in /// [`Self::archipelago`]. characters: HashMap, @@ -174,6 +177,7 @@ impl Archipelago { islands: HashMap::new(), reverse_islands: HashMap::new(), agents: HashMap::new(), + reverse_agents: HashMap::new(), characters: HashMap::new(), } } diff --git a/crates/bevy_landmass/src/lib_test.rs b/crates/bevy_landmass/src/lib_test.rs index 551977d..87bad6d 100644 --- a/crates/bevy_landmass/src/lib_test.rs +++ b/crates/bevy_landmass/src/lib_test.rs @@ -159,6 +159,10 @@ fn adds_and_removes_agents() { sorted(archipelago.agents.keys().copied().collect()), sorted(vec![agent_id_1, agent_id_2]), ); + assert_eq!( + sorted(archipelago.reverse_agents.values().copied().collect()), + sorted(vec![agent_id_1, agent_id_2]), + ); assert_eq!(archipelago.archipelago.get_agent_ids().len(), 2); let agent_id_3 = app @@ -185,6 +189,10 @@ fn adds_and_removes_agents() { sorted(archipelago.agents.keys().copied().collect()), sorted(vec![agent_id_1, agent_id_2, agent_id_3]), ); + assert_eq!( + sorted(archipelago.reverse_agents.values().copied().collect()), + sorted(vec![agent_id_1, agent_id_2, agent_id_3]), + ); assert_eq!(archipelago.archipelago.get_agent_ids().len(), 3); app.world_mut().despawn(agent_id_2); @@ -200,6 +208,10 @@ fn adds_and_removes_agents() { sorted(archipelago.agents.keys().copied().collect()), sorted(vec![agent_id_1, agent_id_3]), ); + assert_eq!( + sorted(archipelago.reverse_agents.values().copied().collect()), + sorted(vec![agent_id_1, agent_id_3]), + ); assert_eq!(archipelago.archipelago.get_agent_ids().len(), 2); app.world_mut().despawn(agent_id_1); @@ -213,6 +225,10 @@ fn adds_and_removes_agents() { .expect("archipelago exists"); assert_eq!(archipelago.agents.keys().copied().collect::>(), []); + assert_eq!( + archipelago.reverse_agents.values().copied().collect::>(), + [] + ); assert_eq!(archipelago.archipelago.get_agent_ids().len(), 0); }