Add a trait and function for recording avoidance data in bevy_landmass.

crates/bevy_landmass/src/debug.rs

@@ -18,8 +18,14 @@ use bevy::{
   transform::components::Transform,
 };
 
+#[cfg(feature = "debug-avoidance")]
+use bevy::math::Vec2;
+
 pub use landmass::debug::DebugDrawError;
 
+#[cfg(feature = "debug-avoidance")]
+pub use landmass::debug::ConstraintKind;
+
 /// The type of debug points.
 #[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
 pub enum PointType {
@@ -150,6 +156,76 @@ pub fn draw_archipelago_debug<CS: CoordinateSystem>(
   )
 }
 
+#[cfg(feature = "debug-avoidance")]
+/// A constraint in velocity-space for an agent's velocity for local collision
+/// avoidance. The constraint restricts the velocity to lie on one side of a
+/// line (aka., only a half-plane is considered valid). This is equivalent to
+/// [`landmass::debug::ConstraintLine`].
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub struct ConstraintLine {
+  /// A point on the line separating the valid and invalid velocities.
+  pub point: Vec2,
+  /// The normal of the line separating the valid and invalid velocities. The
+  /// normal always points towards the valid velocities.
+  pub normal: Vec2,
+}
+
+#[cfg(feature = "debug-avoidance")]
+/// A trait for reporting agent local collision avoidance constraints.
+pub trait AvoidanceDrawer {
+  /// Reports a single avoidance constraint.
+  fn add_constraint(
+    &mut self,
+    agent: Entity,
+    constraint: ConstraintLine,
+    kind: ConstraintKind,
+  );
+}
+
+#[cfg(feature = "debug-avoidance")]
+impl ConstraintLine {
+  fn from_landmass(line: &landmass::debug::ConstraintLine) -> Self {
+    Self {
+      point: Vec2::new(line.point.x, line.point.y),
+      normal: Vec2::new(line.normal.x, line.normal.y),
+    }
+  }
+}
+
+/// Draws the avoidance data for any agent marked with TODO
+#[cfg(feature = "debug-avoidance")]
+pub fn draw_avoidance_data<CS: CoordinateSystem>(
+  archipelago: &crate::Archipelago<CS>,
+  avoidance_drawer: &mut impl AvoidanceDrawer,
+) {
+  struct AvoidanceDrawerAdapter<'a, CS: CoordinateSystem, D: AvoidanceDrawer> {
+    archipelago: &'a crate::Archipelago<CS>,
+    drawer: &'a mut D,
+  }
+
+  impl<CS: CoordinateSystem, D: AvoidanceDrawer>
+    landmass::debug::AvoidanceDrawer for AvoidanceDrawerAdapter<'_, CS, D>
+  {
+    fn add_constraint(
+      &mut self,
+      agent: landmass::AgentId,
+      constraint: landmass::debug::ConstraintLine,
+      kind: landmass::debug::ConstraintKind,
+    ) {
+      self.drawer.add_constraint(
+        *self.archipelago.reverse_agents.get(&agent).unwrap(),
+        ConstraintLine::from_landmass(&constraint),
+        kind,
+      );
+    }
+  }
+
+  landmass::debug::draw_avoidance_data(
+    &archipelago.archipelago,
+    &mut AvoidanceDrawerAdapter { archipelago, drawer: avoidance_drawer },
+  );
+}
+
 /// A plugin to draw landmass debug data with Bevy gizmos.
 pub struct LandmassDebugPlugin<CS: CoordinateSystem> {
   /// Whether to begin drawing on startup.

crates/bevy_landmass/src/debug_test.rs

@@ -212,3 +212,205 @@ fn draws_archipelago_debug() {
     ]
   );
 }
+
+#[cfg(feature = "debug-avoidance")]
+#[googletest::gtest]
+fn draws_avoidance_data_when_requested() {
+  use std::collections::HashMap;
+
+  use crate::{
+    debug::{
+      draw_avoidance_data, AvoidanceDrawer, ConstraintKind, ConstraintLine,
+    },
+    AgentTarget3d, KeepAvoidanceData, NavigationMesh, Velocity3d,
+  };
+
+  use bevy::{math::Vec2, prelude::Entity};
+  use googletest::{matcher::MatcherResult, prelude::*};
+
+  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 {
+      neighbourhood: 100.0,
+      avoidance_time_horizon: 100.0,
+      obstacle_avoidance_time_horizon: 100.0,
+      ..AgentOptions::default_for_agent_radius(0.5)
+    }))
+    .id();
+
+  let nav_mesh = Arc::new(
+    NavigationMesh {
+      vertices: vec![
+        Vec3::new(1.0, 1.0, 1.0),
+        Vec3::new(11.0, 1.0, 1.0),
+        Vec3::new(11.0, 1.0, 11.0),
+        Vec3::new(1.0, 1.0, 11.0),
+      ],
+      polygons: vec![vec![3, 2, 1, 0]],
+      polygon_type_indices: vec![0],
+    }
+    .validate()
+    .expect("The mesh 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((Island3dBundle {
+    island: Island,
+    archipelago_ref: ArchipelagoRef3d::new(archipelago_id),
+    nav_mesh: NavMeshHandle(nav_mesh_handle.clone()),
+  },));
+
+  let agent_1 = app
+    .world_mut()
+    .spawn((
+      Transform::from_translation(Vec3::new(6.0, 1.0, 2.0)),
+      Agent3dBundle {
+        agent: Agent::default(),
+        archipelago_ref: ArchipelagoRef3d::new(archipelago_id),
+        settings: AgentSettings {
+          radius: 0.5,
+          max_speed: 1.0,
+          desired_speed: 1.0,
+        },
+      },
+      Velocity3d {
+        // Use a velocity that allows both agents to agree on their "passing
+        // side".
+        velocity: Vec3::new(1.0, 0.0, 1.0),
+      },
+      AgentTarget3d::Point(Vec3::new(6.0, 1.0, 10.0)),
+      KeepAvoidanceData,
+    ))
+    .id();
+
+  app.world_mut().spawn((
+    Transform::from_translation(Vec3::new(6.0, 1.0, 10.0)),
+    Agent3dBundle {
+      agent: Agent::default(),
+      archipelago_ref: ArchipelagoRef3d::new(archipelago_id),
+      settings: AgentSettings {
+        radius: 0.5,
+        max_speed: 1.0,
+        desired_speed: 1.0,
+      },
+    },
+    AgentTarget3d::Point(Vec3::new(6.0, 1.0, 2.0)),
+  ));
+
+  // We now have avoidance data for agent_1.
+  app.update();
+
+  struct FakeAvoidanceDrawer(
+    HashMap<Entity, HashMap<ConstraintKind, Vec<ConstraintLine>>>,
+  );
+
+  impl AvoidanceDrawer for FakeAvoidanceDrawer {
+    fn add_constraint(
+      &mut self,
+      agent: Entity,
+      constraint: ConstraintLine,
+      kind: ConstraintKind,
+    ) {
+      self
+        .0
+        .entry(agent)
+        .or_default()
+        .entry(kind)
+        .or_default()
+        .push(constraint);
+    }
+  }
+
+  let mut drawer = FakeAvoidanceDrawer(Default::default());
+
+  let archipelago =
+    app.world().entity(archipelago_id).get::<Archipelago3d>().unwrap();
+
+  draw_avoidance_data(archipelago, &mut drawer);
+
+  #[derive(MatcherBase)]
+  struct EquivLineMatcher(ConstraintLine);
+
+  impl Matcher<&ConstraintLine> for EquivLineMatcher {
+    fn matches(&self, actual: &ConstraintLine) -> MatcherResult {
+      if self.0.normal.angle_to(actual.normal).abs() >= 1e-3 {
+        // The lines don't point in the same direction.
+        return MatcherResult::NoMatch;
+      }
+      if (self.0.point - actual.point).dot(actual.normal).abs() >= 1e-3 {
+        // The expected line point is not on the actual line.
+        return MatcherResult::NoMatch;
+      }
+      MatcherResult::Match
+    }
+
+    fn describe(
+      &self,
+      matcher_result: MatcherResult,
+    ) -> googletest::description::Description {
+      match matcher_result {
+        MatcherResult::Match => {
+          format!("is equivalent to the line {:?}", self.0).into()
+        }
+        MatcherResult::NoMatch => {
+          format!("isn't equivalent to the line {:?}", self.0).into()
+        }
+      }
+    }
+  }
+
+  fn equiv_line<'a>(line: ConstraintLine) -> impl Matcher<&'a ConstraintLine> {
+    EquivLineMatcher(line)
+  }
+
+  // I would ideally use three levels of unordered_elements_are here instead,
+  // but due to https://github.com/rust-lang/rust/issues/134719
+  expect_eq!(drawer.0.len(), 1);
+  let agent_constraints = drawer.0.get(&agent_1).unwrap();
+  expect_eq!(agent_constraints.len(), 1);
+  let agent_constraints =
+    agent_constraints.get(&ConstraintKind::Original).unwrap();
+
+  // Only one of the agents was rendered.
+  expect_that!(
+    agent_constraints,
+    unordered_elements_are!(
+      // Lines for the edges of the nav mesh.
+      equiv_line(ConstraintLine {
+        normal: Vec2::new(-1.0, 0.0),
+        point: Vec2::new(0.05, 0.0),
+      }),
+      equiv_line(ConstraintLine {
+        normal: Vec2::new(0.0, 1.0),
+        point: Vec2::new(0.0, -0.09),
+      }),
+      equiv_line(ConstraintLine {
+        normal: Vec2::new(1.0, 0.0),
+        point: Vec2::new(-0.05, 0.0),
+      }),
+      equiv_line(ConstraintLine {
+        normal: Vec2::new(0.0, -1.0),
+        point: Vec2::new(0.0, 0.01),
+      }),
+      // Line for the other agent.
+      equiv_line(ConstraintLine {
+        normal: Vec2::new(1.007905, -8.0).normalize().perp(),
+        point: Vec2::new(0.0, 0.0),
+      }),
+    )
+  );
+}