From cf436a4f41e43c87e506c366de5df3fbe28ebd15 Mon Sep 17 00:00:00 2001 From: andriyDev Date: Thu, 27 Jun 2024 00:42:10 -0700 Subject: [PATCH] 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 | 43 +++++++++++++++++++- src/avoidance_test.rs | 95 +++++++++++++++++++++++++++++++++++++++++-- src/lib.rs | 14 +++++++ 3 files changed, 148 insertions(+), 4 deletions(-) diff --git a/src/avoidance.rs b/src/avoidance.rs index 18e2070..9d9f449 100644 --- a/src/avoidance.rs +++ b/src/avoidance.rs @@ -8,7 +8,8 @@ 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 @@ -16,6 +17,8 @@ use crate::{ pub(crate) fn apply_avoidance_to_agents( agents: &mut HopSlotMap, agent_id_to_agent_node: &HashMap, + characters: &HopSlotMap, + character_id_to_nav_mesh_point: &HashMap, 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::>(); let mut nearby_obstacles = nav_mesh_borders_to_dodgy_obstacles( diff --git a/src/avoidance_test.rs b/src/avoidance_test.rs index 28a9a55..c568d4b 100644 --- a/src/avoidance_test.rs +++ b/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::::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::::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)" + ); +} diff --git a/src/lib.rs b/src/lib.rs index 7bd76ef..6235ddc 100644 --- a/src/lib.rs +++ b/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,