Make agents avoid characters.

We now have agents avoiding characters! This means player characters will be walked around rather than bumped into (since we now have a way to describe how a player character is in the world).

src/avoidance.rs

@@ -8,14 +8,17 @@ use slotmap::HopSlotMap;
 use crate::{
   island::IslandNavigationData,
   nav_data::{ModifiedNode, NodeRef},
-  Agent, AgentId, AgentOptions, IslandId, NavigationData,
+  Agent, AgentId, AgentOptions, Character, CharacterId, IslandId,
+  NavigationData,
 };
 
 /// Adjusts the velocity of `agents` to apply local avoidance. `delta_time` must
 /// be positive.
 pub(crate) fn apply_avoidance_to_agents(
   agents: &mut HopSlotMap<AgentId, Agent>,
   agent_id_to_agent_node: &HashMap<AgentId, (Vec3, NodeRef)>,
+  characters: &HopSlotMap<CharacterId, Character>,
+  character_id_to_nav_mesh_point: &HashMap<CharacterId, Vec3>,
   nav_data: &NavigationData,
   agent_options: &AgentOptions,
   delta_time: f32,
@@ -45,6 +48,28 @@ pub(crate) fn apply_avoidance_to_agents(
     agent_max_radius = agent_max_radius.max(agent.radius);
   }
 
+  let mut character_kdtree = KdTree::new(/* dimensions= */ 3);
+  for (character_id, character) in characters.iter() {
+    let Some(character_point) =
+      character_id_to_nav_mesh_point.get(&character_id)
+    else {
+      continue;
+    };
+    character_kdtree
+      .add(
+        [character_point.x, character_point.y, character_point.z],
+        dodgy_2d::Agent {
+          position: to_dodgy_vec2(character_point.xz()),
+          velocity: to_dodgy_vec2(character.velocity.xz()),
+          radius: character.radius,
+          // Characters are not responsible for any avoidance since landmass has
+          // no control over them.
+          avoidance_responsibility: 0.0,
+        },
+      )
+      .expect("Character point is finite");
+  }
+
   let neighbourhood = agent_max_radius + agent_options.neighbourhood;
   let neighbourhood_squared = neighbourhood * neighbourhood;
   for (agent_id, agent) in agents.iter_mut() {
@@ -60,6 +85,13 @@ pub(crate) fn apply_avoidance_to_agents(
         &squared_euclidean,
       )
       .unwrap();
+    let nearby_characters = character_kdtree
+      .within(
+        &[agent_point.x, agent_point.y, agent_point.z],
+        neighbourhood_squared,
+        &squared_euclidean,
+      )
+      .unwrap();
 
     let nearby_agents = nearby_agents
       .iter()
@@ -77,6 +109,15 @@ pub(crate) fn apply_avoidance_to_agents(
           None
         }
       })
+      .chain(nearby_characters.iter().filter_map(
+        |&(distance_squared, dodgy_agent)| {
+          if distance_squared < neighbourhood_squared {
+            Some(std::borrow::Cow::Borrowed(dodgy_agent))
+          } else {
+            None
+          }
+        },
+      ))
       .collect::<Vec<_>>();
 
     let mut nearby_obstacles = nav_mesh_borders_to_dodgy_obstacles(

src/avoidance_test.rs

@@ -5,7 +5,8 @@ use slotmap::HopSlotMap;
 
 use crate::{
   avoidance::apply_avoidance_to_agents, island::Island, nav_data::NodeRef,
-  Agent, AgentId, AgentOptions, NavigationData, NavigationMesh, Transform,
+  Agent, AgentId, AgentOptions, Character, CharacterId, NavigationData,
+  NavigationMesh, Transform,
 };
 
 use super::nav_mesh_borders_to_dodgy_obstacles;
@@ -476,9 +477,11 @@ fn applies_no_avoidance_for_far_agents() {
   apply_avoidance_to_agents(
     &mut agents,
     &agent_id_to_agent_node,
+    /* characters= */ &HopSlotMap::with_key(),
+    /* character_id_to_nav_mesh_point= */ &HashMap::new(),
     &nav_data,
     &AgentOptions { neighbourhood: 5.0, ..Default::default() },
-    0.01,
+    /* delta_time= */ 0.01,
   );
 
   assert_eq!(
@@ -561,13 +564,15 @@ fn applies_avoidance_for_two_agents() {
   apply_avoidance_to_agents(
     &mut agents,
     &agent_id_to_agent_node,
+    /* characters= */ &HopSlotMap::with_key(),
+    /* character_id_to_nav_mesh_point= */ &HashMap::new(),
     &nav_data,
     &AgentOptions {
       neighbourhood: 15.0,
       avoidance_time_horizon: 15.0,
       ..Default::default()
     },
-    0.01,
+    /* delta_time= */ 0.01,
   );
 
   // The agents each have a radius of 1, and they are separated by a distance
@@ -588,3 +593,87 @@ fn applies_avoidance_for_two_agents() {
     "left={agent_2_desired_velocity}, right=Vec3(-0.98, 0.0, 0.2)"
   );
 }
+
+#[test]
+fn agent_avoids_character() {
+  let nav_mesh = NavigationMesh {
+    mesh_bounds: None,
+    vertices: vec![
+      Vec3::new(-1.0, 0.0, -1.0),
+      Vec3::new(13.0, 0.0, -1.0),
+      Vec3::new(13.0, 0.0, 3.0),
+      Vec3::new(-1.0, 0.0, 3.0),
+    ],
+    polygons: vec![vec![0, 1, 2, 3]],
+  }
+  .validate()
+  .expect("Validation succeeded.");
+
+  let mut nav_data = NavigationData::new();
+  let island_id = nav_data.islands.insert({
+    let mut island = Island::new();
+    island.set_nav_mesh(
+      Transform { translation: Vec3::ZERO, rotation: 0.0 },
+      Arc::new(nav_mesh),
+    );
+    island
+  });
+
+  let mut agents = HopSlotMap::<AgentId, _>::with_key();
+  let agent = agents.insert({
+    let mut agent = Agent::create(
+      /* position= */ Vec3::new(1.0, 0.0, 1.0),
+      /* velocity= */ Vec3::new(1.0, 0.0, 0.0),
+      /* radius= */ 1.0,
+      /* max_velocity= */ 1.0,
+    );
+    agent.current_desired_move = Vec3::new(1.0, 0.0, 0.0);
+    agent
+  });
+  let mut characters = HopSlotMap::<CharacterId, _>::with_key();
+  let character = characters.insert(Character {
+    position: Vec3::new(11.0, 0.0, 1.01),
+    velocity: Vec3::new(-1.0, 0.0, 0.0),
+    radius: 1.0,
+  });
+
+  let mut agent_id_to_agent_node = HashMap::new();
+  agent_id_to_agent_node.insert(
+    agent,
+    (
+      agents.get(agent).unwrap().position,
+      NodeRef { island_id, polygon_index: 0 },
+    ),
+  );
+  let mut character_id_to_nav_mesh_point = HashMap::new();
+  character_id_to_nav_mesh_point
+    .insert(character, characters.get(character).unwrap().position);
+
+  apply_avoidance_to_agents(
+    &mut agents,
+    &agent_id_to_agent_node,
+    &characters,
+    &character_id_to_nav_mesh_point,
+    &nav_data,
+    &AgentOptions {
+      neighbourhood: 15.0,
+      avoidance_time_horizon: 15.0,
+      ..Default::default()
+    },
+    /* delta_time= */ 0.01,
+  );
+
+  // The agent+character each have a radius of 1, and they are separated by a
+  // distance of 10 (they start at (1,0) and (11,0)). Only the agent is
+  // managed by landmass, so it must go to (6,2), since the character will go to
+  // (6,0). That's a rise over run of 2/5 or 0.4, which is our expected Z
+  // velocity. We derive the X velocity by just making the length of the
+  // vector 1 (the agent's max speed).
+  let agent_desired_velocity =
+    agents.get(agent).unwrap().get_desired_velocity();
+  assert!(
+    agent_desired_velocity
+      .abs_diff_eq(Vec3::new((1.0f32 - 0.4 * 0.4).sqrt(), 0.0, -0.4), 0.05),
+    "left={agent_desired_velocity}, right=Vec3(0.9165..., 0.0, -0.4)"
+  );
+}

src/lib.rs

@@ -188,6 +188,18 @@ impl Archipelago {
       }
     }
 
+    let mut character_id_to_nav_mesh_point = HashMap::new();
+    for (character_id, character) in self.characters.iter() {
+      let character_point = match self.nav_data.sample_point(
+        character.position,
+        self.agent_options.node_sample_distance,
+      ) {
+        None => continue,
+        Some(point_and_node) => point_and_node.0,
+      };
+      character_id_to_nav_mesh_point.insert(character_id, character_point);
+    }
+
     let mut agent_id_to_follow_path_indices = HashMap::new();
 
     for (agent_id, agent) in self.agents.iter_mut() {
@@ -312,6 +324,8 @@ impl Archipelago {
     apply_avoidance_to_agents(
       &mut self.agents,
       &agent_id_to_agent_node,
+      &self.characters,
+      &character_id_to_nav_mesh_point,
       &self.nav_data,
       &self.agent_options,
       delta_time,