use super::components::NavMeshAgentState;
use super::funnel::{simplify_path, smooth_path};
use super::pathfinding::{
PathRequest, PathStatus, find_containing_triangle, find_path_with_algorithm,
sample_navmesh_height_near,
};
use crate::ecs::world::{Entity, World};
use nalgebra_glm::Vec3;
pub fn run_navmesh_systems(world: &mut World) {
update_agent_triangles_system(world);
process_path_requests_system(world);
agent_avoidance_system(world);
agent_movement_system(world);
snap_idle_agents_to_navmesh(world);
super::debug::navmesh_debug_draw_system(world);
}
pub fn update_agent_triangles_system(world: &mut World) {
let entities: Vec<_> = world
.core
.query_entities(crate::ecs::world::NAVMESH_AGENT | crate::ecs::world::LOCAL_TRANSFORM)
.collect();
for entity in entities {
let position = match world.core.get_local_transform(entity) {
Some(transform) => transform.translation,
None => continue,
};
let triangle = find_containing_triangle(&world.resources.navmesh, position);
if let Some(agent) = world.core.get_navmesh_agent_mut(entity) {
agent.current_triangle = triangle;
}
}
}
pub fn process_path_requests_system(world: &mut World) {
let entities: Vec<_> = world
.core
.query_entities(crate::ecs::world::NAVMESH_AGENT | crate::ecs::world::LOCAL_TRANSFORM)
.collect();
let mut path_updates = Vec::new();
for entity in &entities {
let should_process = world
.core
.get_navmesh_agent(*entity)
.map(|agent| agent.state == NavMeshAgentState::PathPending)
.unwrap_or(false);
if !should_process {
continue;
}
let (start, target, radius) = {
let transform = world.core.get_local_transform(*entity);
let agent = world.core.get_navmesh_agent(*entity);
match (transform, agent) {
(Some(t), Some(a)) if a.target_position.is_some() => {
(t.translation, a.target_position.unwrap(), a.agent_radius)
}
_ => continue,
}
};
let request = PathRequest::new(start, target).with_radius(radius);
let algorithm = world.resources.navmesh.algorithm;
let result = find_path_with_algorithm(&world.resources.navmesh, &request, algorithm);
match result.status {
PathStatus::Found | PathStatus::PartialPath => {
let smoothed = smooth_path(
&world.resources.navmesh,
&result.triangle_path,
start,
target,
);
let simplified = simplify_path(&smoothed, 1.5);
let path = if simplified.len() > 1 {
simplified[1..].to_vec()
} else {
simplified
};
path_updates.push((*entity, path, NavMeshAgentState::Moving));
}
PathStatus::StartOffMesh | PathStatus::EndOffMesh | PathStatus::NoPath => {
path_updates.push((*entity, Vec::new(), NavMeshAgentState::NoPath));
}
}
}
for (entity, path, state) in path_updates {
if let Some(agent) = world.core.get_navmesh_agent_mut(entity) {
agent.current_path = path;
agent.current_waypoint_index = 0;
agent.state = state;
}
}
}
pub fn agent_avoidance_system(world: &mut World) {
let entities: Vec<_> = world
.core
.query_entities(crate::ecs::world::NAVMESH_AGENT | crate::ecs::world::LOCAL_TRANSFORM)
.collect();
let agent_data: Vec<(Entity, Vec3, f32, bool)> = entities
.iter()
.filter_map(|&entity| {
let transform = world.core.get_local_transform(entity)?;
let agent = world.core.get_navmesh_agent(entity)?;
Some((
entity,
transform.translation,
agent.agent_radius,
agent.state == NavMeshAgentState::Moving,
))
})
.collect();
let mut avoidance_updates: Vec<(Entity, Vec3)> = Vec::new();
for (entity, position, radius, is_moving) in &agent_data {
if !is_moving {
avoidance_updates.push((*entity, Vec3::zeros()));
continue;
}
let mut avoidance = Vec3::zeros();
let avoidance_radius = radius * 2.5;
for (other_entity, other_position, other_radius, _) in &agent_data {
if entity == other_entity {
continue;
}
let diff = *position - *other_position;
let diff_xz = Vec3::new(diff.x, 0.0, diff.z);
let distance = nalgebra_glm::length(&diff_xz);
let min_distance = avoidance_radius + other_radius;
if distance < min_distance && distance > 0.001 {
let strength = (min_distance - distance) / min_distance;
let direction = nalgebra_glm::normalize(&diff_xz);
avoidance += direction * strength;
}
}
avoidance_updates.push((*entity, avoidance));
}
for (entity, avoidance) in avoidance_updates {
if let Some(agent) = world.core.get_navmesh_agent_mut(entity) {
agent.avoidance_velocity = avoidance;
}
}
}
pub fn snap_idle_agents_to_navmesh(world: &mut World) {
let entities: Vec<_> = world
.core
.query_entities(crate::ecs::world::NAVMESH_AGENT | crate::ecs::world::LOCAL_TRANSFORM)
.collect();
for entity in entities {
let is_idle = world
.core
.get_navmesh_agent(entity)
.map(|agent| {
agent.state == NavMeshAgentState::Idle || agent.state == NavMeshAgentState::Arrived
})
.unwrap_or(true);
if !is_idle {
continue;
}
let current_pos = match world.core.get_local_transform(entity) {
Some(t) => t.translation,
None => continue,
};
if let Some(height) = sample_navmesh_height_near(
&world.resources.navmesh,
current_pos.x,
current_pos.z,
Some(current_pos.y),
) && (height - current_pos.y).abs() > 0.01
{
if let Some(transform) = world.core.get_local_transform_mut(entity) {
transform.translation.y = height;
}
world.mark_local_transform_dirty(entity);
}
}
}
pub fn agent_movement_system(world: &mut World) {
let delta_time = world.resources.window.timing.delta_time;
let entities: Vec<_> = world
.core
.query_entities(crate::ecs::world::NAVMESH_AGENT | crate::ecs::world::LOCAL_TRANSFORM)
.collect();
for entity in entities {
let should_move = world
.core
.get_navmesh_agent(entity)
.map(|agent| agent.state == NavMeshAgentState::Moving && !agent.current_path.is_empty())
.unwrap_or(false);
if !should_move {
if let Some(agent) = world.core.get_navmesh_agent_mut(entity) {
agent.distance_to_destination = 0.0;
}
continue;
}
let movement_data = {
let agent = world.core.get_navmesh_agent(entity).unwrap();
let transform = world.core.get_local_transform(entity).unwrap();
if agent.current_waypoint_index >= agent.current_path.len() {
Some((transform.translation, transform.rotation, true, 0, 0.0))
} else {
let mut current_position = transform.translation;
let avoidance = agent.avoidance_velocity;
let base_speed = agent.movement_speed;
let mut remaining_movement = base_speed * delta_time;
let mut waypoint_index = agent.current_waypoint_index;
let mut final_rotation = transform.rotation;
while remaining_movement > 0.0 && waypoint_index < agent.current_path.len() {
let target_waypoint = agent.current_path[waypoint_index];
let mut direction = target_waypoint - current_position;
direction.y = 0.0;
let distance_to_waypoint = nalgebra_glm::length(&direction);
if distance_to_waypoint < agent.arrival_threshold {
waypoint_index += 1;
continue;
}
let normalized = nalgebra_glm::normalize(&direction);
if normalized.x.abs() > 0.001 || normalized.z.abs() > 0.001 {
let angle = normalized.z.atan2(normalized.x);
final_rotation = nalgebra_glm::quat_angle_axis(
-angle + std::f32::consts::FRAC_PI_2,
&nalgebra_glm::vec3(0.0, 1.0, 0.0),
);
}
if remaining_movement >= distance_to_waypoint {
current_position = target_waypoint;
remaining_movement -= distance_to_waypoint;
waypoint_index += 1;
} else {
let move_dir = normalized + avoidance * 0.25;
let move_dir_normalized = if nalgebra_glm::length(&move_dir) > 0.001 {
nalgebra_glm::normalize(&move_dir)
} else {
normalized
};
current_position += move_dir_normalized * remaining_movement;
remaining_movement = 0.0;
}
}
let path_distance: f32 = agent
.current_path
.iter()
.skip(waypoint_index)
.zip(agent.current_path.iter().skip(waypoint_index + 1))
.map(|(a, b)| nalgebra_glm::distance(a, b))
.sum::<f32>()
+ if waypoint_index < agent.current_path.len() {
nalgebra_glm::distance(
¤t_position,
&agent.current_path[waypoint_index],
)
} else {
0.0
};
let arrived = waypoint_index >= agent.current_path.len();
Some((
current_position,
final_rotation,
arrived,
waypoint_index,
path_distance,
))
}
};
if let Some((new_position, new_rotation, arrived, new_waypoint_index, distance)) =
movement_data
{
let current_y = world
.core
.get_local_transform(entity)
.map(|t| t.translation.y)
.unwrap_or(new_position.y);
let final_position = if let Some(height) = sample_navmesh_height_near(
&world.resources.navmesh,
new_position.x,
new_position.z,
Some(current_y),
) {
let max_step_height = 1.0;
if (height - current_y).abs() <= max_step_height {
Vec3::new(new_position.x, height, new_position.z)
} else {
Vec3::new(new_position.x, current_y, new_position.z)
}
} else {
new_position
};
if let Some(transform) = world.core.get_local_transform_mut(entity) {
transform.translation = final_position;
transform.rotation = new_rotation;
}
world.mark_local_transform_dirty(entity);
if let Some(agent) = world.core.get_navmesh_agent_mut(entity) {
agent.current_waypoint_index = new_waypoint_index;
agent.distance_to_destination = distance;
if arrived {
agent.state = NavMeshAgentState::Arrived;
agent.current_path.clear();
agent.target_position = None;
agent.distance_to_destination = 0.0;
}
}
}
}
}