1616 lines
57 KiB
Rust
1616 lines
57 KiB
Rust
//! Hierarchical Task-Based Pathfinding System
|
||
//!
|
||
//! This module implements a three-tier pathfinding architecture optimized for
|
||
//! Dwarf Fortress-like gameplay with large procedural worlds.
|
||
//!
|
||
//! # Architecture
|
||
//!
|
||
//! ```text
|
||
//! Entity needs path
|
||
//! │
|
||
//! ▼
|
||
//! ┌─────────────────┐
|
||
//! │ Calculate │
|
||
//! │ Distance │
|
||
//! │ + Chunk Distance│
|
||
//! └─────────────────┘
|
||
//! │
|
||
//! ┌─────┼─────────────────────┐
|
||
//! ▼ ▼ ▼
|
||
//! TIER1 TIER2 TIER3
|
||
//! │ │ │
|
||
//! │ │ │
|
||
//! ▼ ▼ ▼
|
||
//! Sync Provisional + Queue Chunk-Path + Queue
|
||
//! A* (immediate start) (segmented execution)
|
||
//! ```
|
||
//!
|
||
//! # Tiers
|
||
//!
|
||
//! ## Tier 1: Synchronous A* (≤64 tiles or adjacent chunk)
|
||
//! - Executes immediately on main thread
|
||
//! - Uses thread-local scratchpad for zero allocation
|
||
//! - ~50-200µs for short paths
|
||
//!
|
||
//! ## Tier 2: Provisional + Queue (2-4 chunks)
|
||
//! - Provisional path (capped at 64 nodes) for immediate movement
|
||
//! - Full path computed via queue, spread across frames
|
||
//! - Queue processes 8 paths per frame max
|
||
//!
|
||
//! ## Tier 3: Hierarchical Chunk-Path (>4 chunks)
|
||
//! - Macro A* on chunk coordinates (<225 nodes, <10µs)
|
||
//! - Provisional path for immediate movement
|
||
//! - Segmented execution via queue with chunk waypoints
|
||
//!
|
||
//! # Key Data Structures
|
||
//!
|
||
//! - `AStarScratchpad`: Thread-local reused HashMaps/Heaps (zero allocation)
|
||
//! - `ChunkMap::chunk_connectivity`: Graph of adjacent loaded chunks
|
||
//! - `PathRequestQueue`: Time-sliced path computation queue
|
||
//!
|
||
//! # Performance
|
||
//!
|
||
//! - P95: ~357µs (target: <500µs)
|
||
//! - Success rate: 100%
|
||
//! - 96% of paths complete in <500µs
|
||
|
||
use bevy::prelude::*;
|
||
use rustc_hash::FxHashMap;
|
||
use rustc_hash::FxHashSet;
|
||
use std::{cell::RefCell, collections::BinaryHeap, collections::VecDeque, time::Instant};
|
||
|
||
use crate::constants::{
|
||
ITILE_SIZE, PATHFINDER_HIERARCHICAL_THRESHOLD_CHUNKS, PATHFINDER_MAX_NODES,
|
||
PATHFINDER_PROVISIONAL_NODE_LIMIT, PIXEL_RATIO, TILE_SIZE,
|
||
};
|
||
|
||
use crate::entities::item::inventory::{update_encumbrance, InventoryChangedEvent};
|
||
use crate::entities::shared_systems::constants::{
|
||
CONVOY_DOT_THRESHOLD, ES_DIRECTION_THRESHOLD, HEAD_ON_DOT_THRESHOLD, OCCUPANCY_CROWD_THRESHOLD,
|
||
PATHFINDER_DIRTY_LOOKAHEAD, PATHFINDER_VALIDATE_STEPS, PATHFINDER_VALIDATION_COOLDOWN,
|
||
WALK_SPEED_DIVISOR,
|
||
};
|
||
use crate::entities::shared_systems::occupancy::{rebuild_tile_occupancy, TileOccupancy};
|
||
|
||
use crate::world::tiles::tile_changed::{PathfindingDirtyChunks, TileChangedEvent};
|
||
use crate::world::tiles::TileMap;
|
||
use crate::world::{
|
||
chunks::CHUNK_SIZE,
|
||
chunks::{world_to_chunk, ChunkMap, Z_ABOVE, Z_BELOW},
|
||
};
|
||
use crate::{constants::*, entities::shared_components::Ambulatory};
|
||
use bevy::math::ivec3;
|
||
use bevy_rand::prelude::*;
|
||
use rand::RngExt;
|
||
|
||
thread_local! {
|
||
static LOCAL_PATH_TIMES: RefCell<Vec<u128>> = const { RefCell::new(Vec::new()) };
|
||
static LOCAL_PATH_LENGTHS: RefCell<Vec<usize>> = const { RefCell::new(Vec::new()) };
|
||
static LOCAL_NODES_EXPANDED: RefCell<Vec<usize>> = const { RefCell::new(Vec::new()) };
|
||
static LOCAL_FAILED_PATHS: RefCell<u64> = const { RefCell::new(0) };
|
||
}
|
||
|
||
/// Single consolidated scratchpad for A* pathfinding.
|
||
/// One RefCell borrow instead of multiple nested borrows.
|
||
struct AStarScratchpad {
|
||
g_scores: FxHashMap<IVec3, i32>,
|
||
came_from: FxHashMap<IVec3, IVec3>,
|
||
closed_set: FxHashSet<IVec3>,
|
||
open_set: BinaryHeap<PathNode>,
|
||
}
|
||
|
||
impl Default for AStarScratchpad {
|
||
fn default() -> Self {
|
||
Self {
|
||
g_scores: FxHashMap::default(),
|
||
came_from: FxHashMap::default(),
|
||
closed_set: FxHashSet::default(),
|
||
open_set: BinaryHeap::new(),
|
||
}
|
||
}
|
||
}
|
||
|
||
impl AStarScratchpad {
|
||
fn clear_and_reserve(&mut self, capacity: usize) {
|
||
self.g_scores.clear();
|
||
self.came_from.clear();
|
||
self.closed_set.clear();
|
||
self.open_set.clear();
|
||
|
||
if self.g_scores.capacity() < capacity {
|
||
self.g_scores.reserve(capacity);
|
||
self.came_from.reserve(capacity);
|
||
self.closed_set.reserve(capacity);
|
||
}
|
||
}
|
||
}
|
||
|
||
thread_local! {
|
||
static SCRATCHPAD: RefCell<AStarScratchpad> = RefCell::new(AStarScratchpad::default());
|
||
}
|
||
|
||
const ALLOWED_MOVES: [IVec3; 24] = [
|
||
IVec3::new(-ITILE_SIZE, 0, 0),
|
||
IVec3::new(ITILE_SIZE, 0, 0),
|
||
IVec3::new(0, -ITILE_SIZE, 0),
|
||
IVec3::new(0, ITILE_SIZE, 0),
|
||
IVec3::new(-ITILE_SIZE, -ITILE_SIZE, 0),
|
||
IVec3::new(-ITILE_SIZE, ITILE_SIZE, 0),
|
||
IVec3::new(ITILE_SIZE, -ITILE_SIZE, 0),
|
||
IVec3::new(ITILE_SIZE, ITILE_SIZE, 0),
|
||
IVec3::new(-ITILE_SIZE, 0, ITILE_SIZE),
|
||
IVec3::new(-ITILE_SIZE, 0, -ITILE_SIZE),
|
||
IVec3::new(ITILE_SIZE, 0, ITILE_SIZE),
|
||
IVec3::new(ITILE_SIZE, 0, -ITILE_SIZE),
|
||
IVec3::new(0, -ITILE_SIZE, ITILE_SIZE),
|
||
IVec3::new(0, -ITILE_SIZE, -ITILE_SIZE),
|
||
IVec3::new(0, ITILE_SIZE, ITILE_SIZE),
|
||
IVec3::new(0, ITILE_SIZE, -ITILE_SIZE),
|
||
IVec3::new(-ITILE_SIZE, -ITILE_SIZE, ITILE_SIZE),
|
||
IVec3::new(-ITILE_SIZE, -ITILE_SIZE, -ITILE_SIZE),
|
||
IVec3::new(-ITILE_SIZE, ITILE_SIZE, ITILE_SIZE),
|
||
IVec3::new(-ITILE_SIZE, ITILE_SIZE, -ITILE_SIZE),
|
||
IVec3::new(ITILE_SIZE, -ITILE_SIZE, ITILE_SIZE),
|
||
IVec3::new(ITILE_SIZE, -ITILE_SIZE, -ITILE_SIZE),
|
||
IVec3::new(ITILE_SIZE, ITILE_SIZE, ITILE_SIZE),
|
||
IVec3::new(ITILE_SIZE, ITILE_SIZE, -ITILE_SIZE),
|
||
];
|
||
|
||
#[derive(Clone, Eq, PartialEq, Debug)]
|
||
struct PathNode {
|
||
position: IVec3,
|
||
f_score: i32,
|
||
g_score: i32,
|
||
}
|
||
|
||
impl Ord for PathNode {
|
||
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
|
||
other
|
||
.f_score
|
||
.cmp(&self.f_score)
|
||
.then_with(|| other.g_score.cmp(&self.g_score))
|
||
}
|
||
}
|
||
|
||
impl PartialOrd for PathNode {
|
||
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
|
||
Some(self.cmp(other))
|
||
}
|
||
}
|
||
|
||
#[derive(Resource, Default)]
|
||
#[allow(dead_code)]
|
||
pub struct PathfindingBenchmark {
|
||
pub path_calc_times_us: Vec<u128>,
|
||
pub path_lengths: Vec<usize>,
|
||
pub nodes_expanded: Vec<usize>,
|
||
pub movement_system_times_us: Vec<u128>,
|
||
pub wander_system_times_us: Vec<u128>,
|
||
pub total_paths_calculated: u64,
|
||
pub total_failed_paths: u64,
|
||
pub report_every_n: u32,
|
||
pub sample_count: u32,
|
||
}
|
||
|
||
impl PathfindingBenchmark {
|
||
pub fn new(report_every_n: u32) -> Self {
|
||
Self {
|
||
report_every_n,
|
||
..Default::default()
|
||
}
|
||
}
|
||
}
|
||
|
||
#[derive(Resource, Default)]
|
||
pub struct PathRequestQueue {
|
||
pub pending: VecDeque<PathRequest>,
|
||
}
|
||
|
||
#[derive(Clone)]
|
||
#[allow(dead_code)]
|
||
pub struct PathRequest {
|
||
pub entity: Entity,
|
||
pub start: IVec3,
|
||
pub goal: IVec3,
|
||
pub chunk_path: Option<Vec<IVec2>>,
|
||
}
|
||
|
||
const MAX_PATHS_PER_FRAME: usize = 8;
|
||
|
||
pub struct PathfindingPlugin;
|
||
|
||
impl Plugin for PathfindingPlugin {
|
||
fn build(&self, app: &mut App) {
|
||
app.add_message::<InventoryChangedEvent>()
|
||
.insert_resource(PathfindingBenchmark::new(100))
|
||
.insert_resource(crate::entities::shared_components::CompletedPaths::default())
|
||
.insert_resource(crate::entities::shared_components::PathRequestCounter::default())
|
||
.insert_resource(PathRequestQueue::default())
|
||
.init_resource::<TileOccupancy>()
|
||
.insert_resource(PathfindingDirtyChunks::default())
|
||
.add_systems(
|
||
FixedUpdate,
|
||
(
|
||
rebuild_tile_occupancy,
|
||
update_encumbrance,
|
||
collect_pathfinding_dirty_chunks,
|
||
invalidate_paths_on_tile_change,
|
||
crate::entities::tasks::task_executor_system,
|
||
prepare_paths,
|
||
movement,
|
||
)
|
||
.chain(),
|
||
)
|
||
.add_systems(
|
||
PostUpdate,
|
||
(
|
||
merge_benchmark_stats,
|
||
process_completed_paths,
|
||
process_path_queue,
|
||
clear_pathfinding_dirty_chunks,
|
||
),
|
||
)
|
||
.add_systems(Update, bench_report_system);
|
||
}
|
||
}
|
||
|
||
/// Collects chunk positions from TileChangedEvents into PathfindingDirtyChunks.
|
||
/// Runs once per frame, O(events). HashSet deduplicates — many tile changes
|
||
/// in the same chunk produce one entry.
|
||
pub fn collect_pathfinding_dirty_chunks(
|
||
mut events: MessageReader<TileChangedEvent>,
|
||
mut dirty: ResMut<PathfindingDirtyChunks>,
|
||
) {
|
||
for event in events.read() {
|
||
let chunk_pos = world_to_chunk(event.pos);
|
||
dirty.chunks.insert(chunk_pos);
|
||
}
|
||
}
|
||
|
||
/// Clears paths for entities whose upcoming steps pass through a changed chunk.
|
||
/// Checks only the next 8 steps (not the full path) for performance.
|
||
/// O(entities × 8) regardless of how many tiles changed.
|
||
/// Only runs when dirty chunks exist, which is rare in normal gameplay.
|
||
pub fn invalidate_paths_on_tile_change(
|
||
dirty: Res<PathfindingDirtyChunks>,
|
||
mut query: Query<&mut Ambulatory>,
|
||
) {
|
||
if dirty.chunks.is_empty() {
|
||
return;
|
||
}
|
||
for mut ambulatory in query.iter_mut() {
|
||
let Some(ref path) = ambulatory.current_path else {
|
||
continue;
|
||
};
|
||
let check_end = (ambulatory.path_index + PATHFINDER_DIRTY_LOOKAHEAD).min(path.len());
|
||
let affected = path[ambulatory.path_index..check_end]
|
||
.iter()
|
||
.any(|p| dirty.chunks.contains(&world_to_chunk(p.as_ivec3())));
|
||
if affected {
|
||
ambulatory.current_path = None;
|
||
// Keep target — entity will recompute path to same destination
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Clears PathfindingDirtyChunks at the end of the frame.
|
||
/// Must run AFTER invalidate_paths_on_tile_change.
|
||
pub fn clear_pathfinding_dirty_chunks(mut dirty: ResMut<PathfindingDirtyChunks>) {
|
||
dirty.chunks.clear();
|
||
}
|
||
|
||
pub fn prepare_paths(
|
||
mut commands: Commands,
|
||
mut queue: ResMut<PathRequestQueue>,
|
||
mut query: Query<
|
||
(
|
||
Entity,
|
||
&mut crate::entities::shared_components::Ambulatory,
|
||
&Transform,
|
||
),
|
||
Without<crate::entities::shared_components::PendingPath>,
|
||
>,
|
||
tilemap: Res<TileMap>,
|
||
chunk_map: Res<ChunkMap>,
|
||
) {
|
||
let mut paths_needed = 0;
|
||
let mut paths_computed = 0;
|
||
|
||
for (entity, mut ambulatory, transform) in query.iter_mut() {
|
||
if ambulatory.current_path.is_some() || ambulatory.target.is_none() {
|
||
continue;
|
||
}
|
||
paths_needed += 1;
|
||
|
||
let Some(target) = ambulatory.target else {
|
||
continue;
|
||
};
|
||
let start = transform.translation.as_ivec3();
|
||
let goal = target.as_ivec3() - ivec3(0, 0, 1);
|
||
let distance = octile_distance_3d(start, goal);
|
||
|
||
let start_chunk = world_to_chunk(start);
|
||
let goal_chunk = world_to_chunk(goal);
|
||
let chunk_distance = manhattan_distance_2d(start_chunk, goal_chunk);
|
||
|
||
if distance <= PATHFINDER_SHORT_PATH_MAX_TILES || chunk_distance <= 1 {
|
||
let path = calculate_path_benchmarked(&tilemap, start, goal);
|
||
ambulatory.current_path = Some(path);
|
||
ambulatory.path_index = 0;
|
||
paths_computed += 1;
|
||
} else if chunk_distance > PATHFINDER_HIERARCHICAL_THRESHOLD_CHUNKS {
|
||
let chunk_path = calculate_chunk_path(&chunk_map, start_chunk, goal_chunk);
|
||
let provisional_goal = goal;
|
||
let provisional = calculate_provisional_path(
|
||
&tilemap,
|
||
start,
|
||
provisional_goal,
|
||
PATHFINDER_PROVISIONAL_NODE_LIMIT,
|
||
);
|
||
if !provisional.is_empty() {
|
||
ambulatory.current_path = Some(provisional);
|
||
ambulatory.path_index = 0;
|
||
|
||
queue.pending.push_back(PathRequest {
|
||
entity,
|
||
start,
|
||
goal,
|
||
chunk_path: if chunk_path.is_empty() {
|
||
None
|
||
} else {
|
||
Some(chunk_path)
|
||
},
|
||
});
|
||
commands
|
||
.entity(entity)
|
||
.insert(crate::entities::shared_components::PendingPath {
|
||
start,
|
||
goal,
|
||
waypoint_path: Vec::new(),
|
||
request_id: 0,
|
||
});
|
||
} else {
|
||
let path = calculate_path_benchmarked(&tilemap, start, goal);
|
||
if path.len() <= 1 {
|
||
// Path failed — clear target so entity picks a new reachable one
|
||
ambulatory.target = None;
|
||
ambulatory.current_path = None;
|
||
} else {
|
||
ambulatory.current_path = Some(path);
|
||
ambulatory.path_index = 0;
|
||
}
|
||
}
|
||
} else {
|
||
let provisional = calculate_provisional_path(
|
||
&tilemap,
|
||
start,
|
||
goal,
|
||
PATHFINDER_PROVISIONAL_NODE_LIMIT,
|
||
);
|
||
if !provisional.is_empty() {
|
||
ambulatory.current_path = Some(provisional);
|
||
ambulatory.path_index = 0;
|
||
|
||
queue.pending.push_back(PathRequest {
|
||
entity,
|
||
start,
|
||
goal,
|
||
chunk_path: None,
|
||
});
|
||
commands
|
||
.entity(entity)
|
||
.insert(crate::entities::shared_components::PendingPath {
|
||
start,
|
||
goal,
|
||
waypoint_path: Vec::new(),
|
||
request_id: 0,
|
||
});
|
||
} else {
|
||
let path = calculate_path_benchmarked(&tilemap, start, goal);
|
||
if path.len() <= 1 {
|
||
// Path failed — clear target so entity picks a new reachable one
|
||
ambulatory.target = None;
|
||
ambulatory.current_path = None;
|
||
} else {
|
||
ambulatory.current_path = Some(path);
|
||
ambulatory.path_index = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn process_path_queue(
|
||
mut commands: Commands,
|
||
mut queue: ResMut<PathRequestQueue>,
|
||
tilemap: Res<TileMap>,
|
||
chunk_map: Res<ChunkMap>,
|
||
mut query: Query<
|
||
(Entity, &mut Ambulatory, &Transform),
|
||
With<crate::entities::shared_components::PendingPath>,
|
||
>,
|
||
) {
|
||
let mut processed = 0;
|
||
while processed < MAX_PATHS_PER_FRAME {
|
||
if let Some(request) = queue.pending.pop_front() {
|
||
processed += 1;
|
||
|
||
if let Ok((_, mut ambulatory, transform)) = query.get_mut(request.entity) {
|
||
let actual_start = transform.translation.as_ivec3();
|
||
|
||
if let Some(ref chunk_waypoints) = request.chunk_path {
|
||
let current_chunk = world_to_chunk(actual_start);
|
||
if let Some(next_chunk) = chunk_waypoints.iter().find(|&&c| c != current_chunk)
|
||
{
|
||
match directional_chunk_waypoint(
|
||
actual_start,
|
||
*next_chunk,
|
||
request.goal,
|
||
&tilemap,
|
||
&chunk_map,
|
||
) {
|
||
None => {
|
||
// Chunk unloaded or no standable tiles — abandon this path,
|
||
// entity will retarget via prepare_paths next frame
|
||
ambulatory.current_path = None;
|
||
ambulatory.target = None;
|
||
}
|
||
Some(waypoint) => {
|
||
let segment_path =
|
||
calculate_path_benchmarked(&tilemap, actual_start, waypoint);
|
||
if !segment_path.is_empty() {
|
||
ambulatory.current_path = Some(segment_path);
|
||
ambulatory.path_index = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
} else {
|
||
let full_path =
|
||
calculate_path_benchmarked(&tilemap, actual_start, request.goal);
|
||
if !full_path.is_empty() {
|
||
ambulatory.current_path = Some(full_path);
|
||
ambulatory.path_index = 0;
|
||
}
|
||
}
|
||
commands
|
||
.entity(request.entity)
|
||
.remove::<crate::entities::shared_components::PendingPath>();
|
||
}
|
||
} else {
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn process_completed_paths(
|
||
mut completed: ResMut<crate::entities::shared_components::CompletedPaths>,
|
||
mut query: Query<(
|
||
Entity,
|
||
&mut crate::entities::shared_components::Ambulatory,
|
||
&mut crate::entities::shared_components::PendingPath,
|
||
)>,
|
||
mut commands: Commands,
|
||
) {
|
||
for (request_id, path) in completed.paths.drain(..) {
|
||
for (entity, mut ambulatory, pending) in query.iter_mut() {
|
||
if pending.request_id == request_id {
|
||
ambulatory.current_path = Some(path.clone());
|
||
commands
|
||
.entity(entity)
|
||
.remove::<crate::entities::shared_components::PendingPath>();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn merge_benchmark_stats(mut bench: ResMut<PathfindingBenchmark>) {
|
||
LOCAL_PATH_TIMES.with(|t| {
|
||
let mut times = t.borrow_mut();
|
||
bench.path_calc_times_us.extend(times.iter());
|
||
bench.total_paths_calculated += times.len() as u64;
|
||
times.clear();
|
||
});
|
||
LOCAL_PATH_LENGTHS.with(|l| {
|
||
let mut lengths = l.borrow_mut();
|
||
bench.path_lengths.extend(lengths.iter());
|
||
lengths.clear();
|
||
});
|
||
LOCAL_NODES_EXPANDED.with(|n| {
|
||
let mut nodes = n.borrow_mut();
|
||
bench.nodes_expanded.extend(nodes.iter());
|
||
nodes.clear();
|
||
});
|
||
LOCAL_FAILED_PATHS.with(|f| {
|
||
let mut failed = f.borrow_mut();
|
||
bench.total_failed_paths += *failed;
|
||
*failed = 0;
|
||
});
|
||
}
|
||
|
||
pub fn update_wandering_targets(
|
||
mut query: Query<(&mut Ambulatory, &Transform)>,
|
||
tilemap: Res<TileMap>,
|
||
chunk_map: Res<ChunkMap>,
|
||
mut rng_q: Query<&mut WyRand, With<GlobalRng>>,
|
||
) {
|
||
let Ok(mut rng) = rng_q.single_mut() else {
|
||
return;
|
||
};
|
||
|
||
for (mut ambulatory, transform) in query.iter_mut() {
|
||
if ambulatory.target.is_none() {
|
||
// Only target interior chunks — exclude boundary chunks that
|
||
// have unloaded neighbours, which strand entities at world edges.
|
||
let current_chunk = world_to_chunk(transform.translation.as_ivec3());
|
||
|
||
// Reservoir sampling - pick one interior chunk with zero allocation
|
||
let mut chosen: Option<IVec2> = None;
|
||
let mut count = 0usize;
|
||
for &chunk in chunk_map.loaded_chunks.keys() {
|
||
let dx = (chunk.x - current_chunk.x).abs();
|
||
let dy = (chunk.y - current_chunk.y).abs();
|
||
if dx <= 2 && dy <= 2 {
|
||
continue;
|
||
}
|
||
if !chunk_map
|
||
.loaded_chunks
|
||
.contains_key(&IVec2::new(chunk.x + 1, chunk.y))
|
||
{
|
||
continue;
|
||
}
|
||
if !chunk_map
|
||
.loaded_chunks
|
||
.contains_key(&IVec2::new(chunk.x - 1, chunk.y))
|
||
{
|
||
continue;
|
||
}
|
||
if !chunk_map
|
||
.loaded_chunks
|
||
.contains_key(&IVec2::new(chunk.x, chunk.y + 1))
|
||
{
|
||
continue;
|
||
}
|
||
if !chunk_map
|
||
.loaded_chunks
|
||
.contains_key(&IVec2::new(chunk.x, chunk.y - 1))
|
||
{
|
||
continue;
|
||
}
|
||
count += 1;
|
||
if rng.random_range(0..count) == 0 {
|
||
chosen = Some(chunk);
|
||
}
|
||
}
|
||
|
||
// Fallback to any chunk if no interior chunks found (small map / startup)
|
||
let target_chunk = chosen.or_else(|| chunk_map.loaded_chunks.keys().next().copied());
|
||
|
||
if let Some(chunk_pos) = target_chunk {
|
||
let chunk_x = chunk_pos.x * CHUNK_SIZE;
|
||
let chunk_y = chunk_pos.y * CHUNK_SIZE;
|
||
let target_x = chunk_x + rng.random_range(0..CHUNK_SIZE);
|
||
let target_y = chunk_y + rng.random_range(0..CHUNK_SIZE);
|
||
|
||
for z in -3..=4 {
|
||
let mut target_pos = IVec3::new(target_x, target_y, z) * ITILE_SIZE;
|
||
if tilemap.floor_tiles.get(&target_pos).is_some() {
|
||
target_pos.z += ITILE_SIZE;
|
||
if tilemap.floor_tiles.get(&target_pos).is_some()
|
||
&& is_standable_tile(&tilemap, target_pos)
|
||
{
|
||
ambulatory.target = Some(Vec3::new(
|
||
target_pos.x as f32,
|
||
target_pos.y as f32,
|
||
target_pos.z as f32 + 1.0,
|
||
));
|
||
ambulatory.current_path = None;
|
||
ambulatory.path_index = 0;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn movement(
|
||
mut query: Query<(&mut Ambulatory, &mut Transform)>,
|
||
tilemap: Res<TileMap>,
|
||
occupancy: Res<TileOccupancy>,
|
||
) {
|
||
query
|
||
.par_iter_mut()
|
||
.for_each(|(mut ambulatory, mut transform)| {
|
||
let current_pos = transform.translation;
|
||
if !is_standable_tile(&tilemap, current_pos.as_ivec3()) {
|
||
// Entities can spawn above the loaded world range (z > Z_ABOVE*TILE_SIZE).
|
||
// Snap them to the top instead of falling through unloaded z-space one
|
||
// tile per tick.
|
||
if transform.translation.z > Z_ABOVE as f32 * TILE_SIZE {
|
||
transform.translation.z = Z_ABOVE as f32 * TILE_SIZE;
|
||
} else if transform.translation.z > -(Z_BELOW as f32) * TILE_SIZE {
|
||
transform.translation.z -= TILE_SIZE;
|
||
}
|
||
ambulatory.current_path = None;
|
||
ambulatory.target = None;
|
||
return;
|
||
}
|
||
|
||
if ambulatory.current_path.is_none() {
|
||
return;
|
||
}
|
||
|
||
if ambulatory.validation_cooldown > 0 {
|
||
ambulatory.validation_cooldown -= 1;
|
||
} else if let Some(path) = &ambulatory.current_path {
|
||
if !validate_next_steps(
|
||
&tilemap,
|
||
path,
|
||
ambulatory.path_index,
|
||
PATHFINDER_VALIDATE_STEPS,
|
||
) {
|
||
ambulatory.current_path = None;
|
||
ambulatory.validation_cooldown = PATHFINDER_VALIDATION_COOLDOWN;
|
||
return;
|
||
}
|
||
ambulatory.validation_cooldown = PATHFINDER_VALIDATION_COOLDOWN;
|
||
}
|
||
|
||
if ambulatory.walk_speed > 0. {
|
||
let tile_weight = get_tile_weight(&tilemap, current_pos.as_ivec3());
|
||
let threshold =
|
||
(ambulatory.walk_speed as i32 * tile_weight as i32 / WALK_SPEED_DIVISOR) as u32;
|
||
|
||
if ambulatory.step_recovery <= threshold {
|
||
ambulatory.step_recovery += 1;
|
||
return;
|
||
} else {
|
||
ambulatory.step_recovery = 0;
|
||
}
|
||
}
|
||
|
||
if let Some(path) = &ambulatory.current_path {
|
||
if ambulatory.path_index < path.len() {
|
||
let old_translation = transform.translation;
|
||
let next_point = path[ambulatory.path_index];
|
||
|
||
let current_count = occupancy.count_at(transform.translation);
|
||
let next_count = occupancy.count_at(next_point);
|
||
let current_crowded = current_count > OCCUPANCY_CROWD_THRESHOLD;
|
||
let occupied = !current_crowded && next_count > current_count;
|
||
|
||
let actual_move: Vec3;
|
||
let advance_path: bool;
|
||
|
||
let move_dir = next_point - transform.translation;
|
||
let our_dir = Vec2::new(move_dir.x, move_dir.y).normalize();
|
||
let their_dir = if occupied {
|
||
occupancy.direction_at(next_point)
|
||
} else {
|
||
Vec2::ZERO
|
||
};
|
||
|
||
// Case 1: Convoy — same direction, treat as unoccupied
|
||
let convoy = occupied
|
||
&& their_dir != Vec2::ZERO
|
||
&& their_dir.dot(our_dir) > CONVOY_DOT_THRESHOLD;
|
||
|
||
// Case 2: Head-on (directly opposite directions)
|
||
let head_on =
|
||
their_dir != Vec2::ZERO && their_dir.dot(our_dir) < HEAD_ON_DOT_THRESHOLD;
|
||
|
||
let we_are_es =
|
||
our_dir.x > ES_DIRECTION_THRESHOLD || our_dir.y < -ES_DIRECTION_THRESHOLD;
|
||
|
||
if !occupied || convoy {
|
||
// Normal movement
|
||
actual_move = next_point;
|
||
advance_path = true;
|
||
} else if occupied && head_on {
|
||
// Both yield left — existing sidestep chain
|
||
let forward_2d = our_dir;
|
||
let left_2d = Vec2::new(-forward_2d.y, forward_2d.x);
|
||
let right_2d = Vec2::new(forward_2d.y, -forward_2d.x);
|
||
let cur_z = transform.translation.z;
|
||
|
||
let candidates = [
|
||
snap_to_grid(
|
||
transform.translation
|
||
+ Vec3::new(
|
||
(left_2d.x + forward_2d.x).signum() * TILE_SIZE,
|
||
(left_2d.y + forward_2d.y).signum() * TILE_SIZE,
|
||
0.0,
|
||
),
|
||
cur_z,
|
||
TILE_SIZE,
|
||
),
|
||
snap_to_grid(
|
||
transform.translation
|
||
+ Vec3::new(left_2d.x * TILE_SIZE, left_2d.y * TILE_SIZE, 0.0),
|
||
cur_z,
|
||
TILE_SIZE,
|
||
),
|
||
snap_to_grid(
|
||
transform.translation
|
||
+ Vec3::new(
|
||
(right_2d.x + forward_2d.x).signum() * TILE_SIZE,
|
||
(right_2d.y + forward_2d.y).signum() * TILE_SIZE,
|
||
0.0,
|
||
),
|
||
cur_z,
|
||
TILE_SIZE,
|
||
),
|
||
snap_to_grid(
|
||
transform.translation
|
||
+ Vec3::new(
|
||
right_2d.x * TILE_SIZE,
|
||
right_2d.y * TILE_SIZE,
|
||
0.0,
|
||
),
|
||
cur_z,
|
||
TILE_SIZE,
|
||
),
|
||
];
|
||
|
||
let sidestep = candidates.iter().copied().find(|&c| {
|
||
tilemap.is_standable(c.as_ivec3())
|
||
&& occupancy.count_at(c) <= current_count
|
||
});
|
||
|
||
if let Some(step) = sidestep {
|
||
actual_move = step;
|
||
advance_path = false;
|
||
} else {
|
||
actual_move = next_point;
|
||
advance_path = true;
|
||
let tile_weight =
|
||
get_tile_weight(&tilemap, transform.translation.as_ivec3());
|
||
let excuse_threshold = if ambulatory.walk_speed > 0. {
|
||
(ambulatory.walk_speed as i32 * tile_weight as i32
|
||
/ WALK_SPEED_DIVISOR) as u32
|
||
} else {
|
||
0
|
||
};
|
||
if ambulatory.step_recovery == 0 {
|
||
ambulatory.step_recovery = excuse_threshold;
|
||
}
|
||
}
|
||
} else if occupied && we_are_es {
|
||
// We yield — sidestep chain first, then excuse-me
|
||
let forward_2d = our_dir;
|
||
let left_2d = Vec2::new(-forward_2d.y, forward_2d.x);
|
||
let right_2d = Vec2::new(forward_2d.y, -forward_2d.x);
|
||
let cur_z = transform.translation.z;
|
||
|
||
let candidates = [
|
||
snap_to_grid(
|
||
transform.translation
|
||
+ Vec3::new(
|
||
(left_2d.x + forward_2d.x).signum() * TILE_SIZE,
|
||
(left_2d.y + forward_2d.y).signum() * TILE_SIZE,
|
||
0.0,
|
||
),
|
||
cur_z,
|
||
TILE_SIZE,
|
||
),
|
||
snap_to_grid(
|
||
transform.translation
|
||
+ Vec3::new(left_2d.x * TILE_SIZE, left_2d.y * TILE_SIZE, 0.0),
|
||
cur_z,
|
||
TILE_SIZE,
|
||
),
|
||
snap_to_grid(
|
||
transform.translation
|
||
+ Vec3::new(
|
||
(right_2d.x + forward_2d.x).signum() * TILE_SIZE,
|
||
(right_2d.y + forward_2d.y).signum() * TILE_SIZE,
|
||
0.0,
|
||
),
|
||
cur_z,
|
||
TILE_SIZE,
|
||
),
|
||
snap_to_grid(
|
||
transform.translation
|
||
+ Vec3::new(
|
||
right_2d.x * TILE_SIZE,
|
||
right_2d.y * TILE_SIZE,
|
||
0.0,
|
||
),
|
||
cur_z,
|
||
TILE_SIZE,
|
||
),
|
||
];
|
||
|
||
let sidestep = candidates.iter().copied().find(|&c| {
|
||
tilemap.is_standable(c.as_ivec3())
|
||
&& occupancy.count_at(c) <= current_count
|
||
});
|
||
|
||
if let Some(step) = sidestep {
|
||
actual_move = step;
|
||
advance_path = false;
|
||
} else {
|
||
// excuse-me: push through with delay
|
||
actual_move = next_point;
|
||
advance_path = true;
|
||
let tile_weight =
|
||
get_tile_weight(&tilemap, transform.translation.as_ivec3());
|
||
let excuse_threshold = if ambulatory.walk_speed > 0. {
|
||
(ambulatory.walk_speed as i32 * tile_weight as i32
|
||
/ WALK_SPEED_DIVISOR) as u32
|
||
} else {
|
||
0
|
||
};
|
||
if ambulatory.step_recovery == 0 {
|
||
ambulatory.step_recovery = excuse_threshold;
|
||
}
|
||
}
|
||
} else {
|
||
// occupied && we_are_wn — right of way, advance with excuse-me delay
|
||
actual_move = next_point;
|
||
advance_path = true;
|
||
let tile_weight =
|
||
get_tile_weight(&tilemap, transform.translation.as_ivec3());
|
||
let excuse_threshold = if ambulatory.walk_speed > 0. {
|
||
(ambulatory.walk_speed as i32 * tile_weight as i32 / WALK_SPEED_DIVISOR)
|
||
as u32
|
||
} else {
|
||
0
|
||
};
|
||
if ambulatory.step_recovery == 0 {
|
||
ambulatory.step_recovery = excuse_threshold;
|
||
}
|
||
}
|
||
|
||
let direction = (actual_move - transform.translation).normalize_or_zero();
|
||
transform.translation = actual_move;
|
||
if direction.x > 0.0 {
|
||
transform.scale.x = PIXEL_RATIO;
|
||
} else if direction.x < 0.0 {
|
||
transform.scale.x = -PIXEL_RATIO;
|
||
}
|
||
|
||
if advance_path && transform.translation.distance(next_point) < TILE_SIZE {
|
||
let ITILE: i16 = ITILE_SIZE as i16;
|
||
|
||
ambulatory.step_history[0] = ambulatory.step_history[2];
|
||
ambulatory.step_history[1] = ambulatory.step_history[3];
|
||
ambulatory.step_history[2] = (old_translation.x as i16) / ITILE;
|
||
ambulatory.step_history[3] = (old_translation.y as i16) / ITILE;
|
||
|
||
let mut dir_sum = Vec2::ZERO;
|
||
let mut n = 0u32;
|
||
|
||
let h = &ambulatory.step_history;
|
||
if h[0] != 0 || h[1] != 0 {
|
||
let d = Vec2::new((h[2] - h[0]) as f32, (h[3] - h[1]) as f32);
|
||
if d.length_squared() > 0.0 {
|
||
dir_sum += d.normalize();
|
||
n += 1;
|
||
}
|
||
}
|
||
|
||
let cur_d = Vec2::new(
|
||
actual_move.x - old_translation.x,
|
||
actual_move.y - old_translation.y,
|
||
);
|
||
if cur_d.length_squared() > 0.0 {
|
||
dir_sum += cur_d.normalize();
|
||
n += 1;
|
||
}
|
||
|
||
if let Some(ref path) = ambulatory.current_path {
|
||
let start = ambulatory.path_index;
|
||
for i in start..((start + 2).min(path.len().saturating_sub(1))) {
|
||
let a = path[i];
|
||
let b = path[i + 1];
|
||
let d = Vec2::new(b.x - a.x, b.y - a.y);
|
||
if d.length_squared() > 0.0 {
|
||
dir_sum += d.normalize();
|
||
n += 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
if n > 0 {
|
||
ambulatory.move_direction = (dir_sum / n as f32).normalize_or_zero();
|
||
}
|
||
|
||
ambulatory.path_index += 1;
|
||
}
|
||
} else {
|
||
ambulatory.current_path = None;
|
||
if let Some(target) = ambulatory.target {
|
||
if transform.translation.distance(target) < TILE_SIZE * 2.0 {
|
||
ambulatory.target = None;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
});
|
||
}
|
||
|
||
fn validate_next_steps(tilemap: &TileMap, path: &[Vec3], start_index: usize, steps: usize) -> bool {
|
||
let end = (start_index + steps).min(path.len());
|
||
for i in start_index..end {
|
||
let pos = path[i].as_ivec3();
|
||
if !is_standable_tile(tilemap, pos) {
|
||
return false;
|
||
}
|
||
}
|
||
true
|
||
}
|
||
|
||
fn is_standable_tile(tilemap: &TileMap, pos: IVec3) -> bool {
|
||
tilemap.is_standable(pos)
|
||
}
|
||
|
||
#[inline]
|
||
fn snap_to_grid(raw: Vec3, preserve_z: f32, tile_size: f32) -> Vec3 {
|
||
Vec3::new(
|
||
(raw.x / tile_size).round() * tile_size,
|
||
(raw.y / tile_size).round() * tile_size,
|
||
preserve_z,
|
||
)
|
||
}
|
||
|
||
/// Sample a random standable tile on an edge of `next_chunk`, picking the one
|
||
/// whose world position is closest to the straight-line projection from
|
||
/// `current_pos` toward `goal`. Falls back to chunk centre if no standable
|
||
/// edge tile is found.
|
||
///
|
||
/// This replaces the hard-coded chunk-centre waypoints that caused forced
|
||
/// doglegs at every chunk boundary, while preserving tile-locked DF movement.
|
||
fn directional_chunk_waypoint(
|
||
current_pos: IVec3,
|
||
next_chunk: IVec2,
|
||
goal: IVec3,
|
||
tilemap: &TileMap,
|
||
chunk_map: &ChunkMap,
|
||
) -> Option<IVec3> {
|
||
if !chunk_map.loaded_chunks.contains_key(&next_chunk) {
|
||
return None;
|
||
}
|
||
|
||
let cx = next_chunk.x * CHUNK_SIZE;
|
||
let cy = next_chunk.y * CHUNK_SIZE;
|
||
let cur_tile = current_pos / ITILE_SIZE;
|
||
let goal_tile = goal / ITILE_SIZE;
|
||
let dir = goal_tile - cur_tile;
|
||
let straight = goal - current_pos;
|
||
|
||
let mut candidates = arrayvec::ArrayVec::<IVec3, 16>::new();
|
||
|
||
let mut scan_edge = |fixed_axis: bool, fixed_tile: i32, var_min: i32, var_max: i32| {
|
||
let var_range = var_max - var_min;
|
||
let step = if var_range <= 0 {
|
||
1
|
||
} else {
|
||
(var_range / 7).max(1)
|
||
};
|
||
let mut var = var_min;
|
||
while var <= var_max {
|
||
let (tile_x, tile_y) = if fixed_axis {
|
||
(fixed_tile, var)
|
||
} else {
|
||
(var, fixed_tile)
|
||
};
|
||
let world_pos = IVec3::new(
|
||
tile_x * ITILE_SIZE,
|
||
tile_y * ITILE_SIZE,
|
||
cur_tile.z * ITILE_SIZE,
|
||
);
|
||
if tilemap.is_standable(world_pos) {
|
||
candidates.push(world_pos);
|
||
}
|
||
var += step;
|
||
}
|
||
};
|
||
|
||
let near_diagonal = (dir.x.abs() - dir.y.abs()).abs() < CHUNK_SIZE / 2;
|
||
|
||
if dir.x.abs() >= dir.y.abs() {
|
||
scan_edge(
|
||
true,
|
||
if dir.x >= 0 { cx } else { cx + CHUNK_SIZE - 1 },
|
||
cy,
|
||
cy + CHUNK_SIZE - 1,
|
||
);
|
||
if near_diagonal {
|
||
scan_edge(
|
||
false,
|
||
if dir.y >= 0 { cy } else { cy + CHUNK_SIZE - 1 },
|
||
cx,
|
||
cx + CHUNK_SIZE - 1,
|
||
);
|
||
}
|
||
} else {
|
||
scan_edge(
|
||
false,
|
||
if dir.y >= 0 { cy } else { cy + CHUNK_SIZE - 1 },
|
||
cx,
|
||
cx + CHUNK_SIZE - 1,
|
||
);
|
||
if near_diagonal {
|
||
scan_edge(
|
||
true,
|
||
if dir.x >= 0 { cx } else { cx + CHUNK_SIZE - 1 },
|
||
cy,
|
||
cy + CHUNK_SIZE - 1,
|
||
);
|
||
}
|
||
}
|
||
|
||
if candidates.is_empty() {
|
||
let centre = IVec3::new(
|
||
(cx + CHUNK_SIZE / 2) * ITILE_SIZE,
|
||
(cy + CHUNK_SIZE / 2) * ITILE_SIZE,
|
||
cur_tile.z * ITILE_SIZE,
|
||
);
|
||
if tilemap.is_standable(centre) {
|
||
return Some(centre);
|
||
}
|
||
for lx in 0..CHUNK_SIZE {
|
||
for ly in 0..CHUNK_SIZE {
|
||
let p = IVec3::new(
|
||
(cx + lx) * ITILE_SIZE,
|
||
(cy + ly) * ITILE_SIZE,
|
||
cur_tile.z * ITILE_SIZE,
|
||
);
|
||
if tilemap.is_standable(p) {
|
||
return Some(p);
|
||
}
|
||
}
|
||
}
|
||
None
|
||
} else {
|
||
// Find the candidate most aligned with the straight-line direction
|
||
let best = candidates
|
||
.iter()
|
||
.enumerate()
|
||
.max_by(|(_, &a), (_, &b)| {
|
||
let da = a - current_pos;
|
||
let db = b - current_pos;
|
||
(da.x * straight.x + da.y * straight.y + da.z * straight.z)
|
||
.cmp(&(db.x * straight.x + db.y * straight.y + db.z * straight.z))
|
||
})
|
||
.map(|(idx, _)| idx)
|
||
.unwrap_or(0);
|
||
|
||
// Spread entities using both their start position AND goal position as entropy.
|
||
// current_pos varies per entity (each is at a different world position).
|
||
// goal varies per entity (each has a different random wander target).
|
||
// Together they produce unique spread values for entities even when
|
||
// heading through the same chunk, without needing to pass entity ID.
|
||
// Small primes spread entity starting positions across waypoint edge tiles.
|
||
// Different values per axis prevent aliasing when positions are on a regular grid.
|
||
let entropy = (cur_tile.x.unsigned_abs() as usize)
|
||
.wrapping_mul(1619)
|
||
.wrapping_add((cur_tile.y.unsigned_abs() as usize).wrapping_mul(31337))
|
||
.wrapping_add((goal_tile.x.unsigned_abs() as usize).wrapping_mul(6271))
|
||
.wrapping_add((goal_tile.y.unsigned_abs() as usize).wrapping_mul(2053));
|
||
let spread = entropy % candidates.len();
|
||
let idx = (best + spread) % candidates.len();
|
||
Some(candidates[idx])
|
||
}
|
||
}
|
||
|
||
/// Get the A* weight for a tile position. Higher = slower to traverse.
|
||
/// Returns 100 (default) if tile not found.
|
||
#[inline]
|
||
fn get_tile_weight(tilemap: &TileMap, pos: IVec3) -> u8 {
|
||
let floor_pos = IVec3::new(pos.x, pos.y, pos.z - ITILE_SIZE);
|
||
tilemap.get_astar_weight(floor_pos)
|
||
}
|
||
|
||
/// Calculate movement cost including tile weight.
|
||
/// Base costs: cardinal=10, diagonal=14, vertical~50.
|
||
/// Tile weight adds: (weight - 50) / 5 to make higher-weight tiles more costly.
|
||
fn calculate_movement_cost(move_dir: IVec3, tile_weight: u8) -> i32 {
|
||
let base_cost = match (
|
||
move_dir.x.abs() / ITILE_SIZE,
|
||
move_dir.y.abs() / ITILE_SIZE,
|
||
move_dir.z.abs() / ITILE_SIZE,
|
||
) {
|
||
(1, 0, 0) | (0, 1, 0) => 10, // Cardinal
|
||
(1, 1, 0) => 14, // Diagonal
|
||
(1, 0, 1) | (0, 1, 1) => 42, // Vertical + cardinal
|
||
(1, 1, 1) => 56, // Vertical + diagonal
|
||
_ => 0,
|
||
};
|
||
|
||
if base_cost == 0 {
|
||
return 0;
|
||
}
|
||
|
||
// Weight cost: normalize so rock(50) adds 0, grass(100) adds 10, bedrock(150) adds 20
|
||
let weight_cost = (tile_weight as i32).saturating_sub(50) / 5;
|
||
|
||
base_cost + weight_cost
|
||
}
|
||
|
||
fn octile_distance_3d(a: IVec3, b: IVec3) -> i32 {
|
||
let dx = (a.x - b.x).abs() / ITILE_SIZE;
|
||
let dy = (a.y - b.y).abs() / ITILE_SIZE;
|
||
let dz = (a.z - b.z).abs() / ITILE_SIZE;
|
||
let (dmax, dmid, dmin) = sorted_desc(dx, dy, dz);
|
||
10 * dmax + 4 * dmid + dmin
|
||
}
|
||
|
||
fn sorted_desc(a: i32, b: i32, c: i32) -> (i32, i32, i32) {
|
||
let mut arr = [a, b, c];
|
||
arr.sort_unstable_by(|x, y| y.cmp(x));
|
||
(arr[0], arr[1], arr[2])
|
||
}
|
||
|
||
fn reconstruct_path(came_from: &FxHashMap<IVec3, IVec3>, mut current: IVec3) -> Vec<Vec3> {
|
||
let mut path = vec![Vec3::new(
|
||
current.x as f32,
|
||
current.y as f32,
|
||
current.z as f32,
|
||
)];
|
||
while let Some(&prev) = came_from.get(¤t) {
|
||
path.push(Vec3::new(prev.x as f32, prev.y as f32, prev.z as f32));
|
||
current = prev;
|
||
}
|
||
path.reverse();
|
||
path
|
||
}
|
||
|
||
pub fn calculate_path_benchmarked(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec<Vec3> {
|
||
let timer = Instant::now();
|
||
|
||
if !is_standable_tile(tilemap, start) || !is_standable_tile(tilemap, goal) {
|
||
LOCAL_FAILED_PATHS.with(|f| {
|
||
*f.borrow_mut() += 1;
|
||
});
|
||
return Vec::new();
|
||
}
|
||
|
||
let estimated_tiles = octile_distance_3d(start, goal) / ITILE_SIZE;
|
||
let (result, nodes_expanded) =
|
||
calculate_path_with_scratchpad(tilemap, start, goal, estimated_tiles);
|
||
|
||
let elapsed = timer.elapsed().as_micros();
|
||
LOCAL_PATH_TIMES.with(|t| {
|
||
t.borrow_mut().push(elapsed);
|
||
});
|
||
LOCAL_PATH_LENGTHS.with(|l| {
|
||
l.borrow_mut().push(result.len());
|
||
});
|
||
LOCAL_NODES_EXPANDED.with(|n| {
|
||
n.borrow_mut().push(nodes_expanded);
|
||
});
|
||
|
||
if result.is_empty() {
|
||
vec![Vec3::new(start.x as f32, start.y as f32, start.z as f32)]
|
||
} else {
|
||
result
|
||
}
|
||
}
|
||
|
||
fn calculate_path_with_scratchpad(
|
||
tilemap: &TileMap,
|
||
start: IVec3,
|
||
goal: IVec3,
|
||
estimated_tiles: i32,
|
||
) -> (Vec<Vec3>, usize) {
|
||
SCRATCHPAD.with(|s| {
|
||
let mut scratch = s.borrow_mut();
|
||
let capacity = ((estimated_tiles as usize).max(64)).min(4096);
|
||
scratch.clear_and_reserve(capacity);
|
||
|
||
let h = octile_distance_3d(start, goal);
|
||
scratch.open_set.push(PathNode {
|
||
position: start,
|
||
f_score: h,
|
||
g_score: 0,
|
||
});
|
||
scratch.g_scores.insert(start, 0);
|
||
|
||
let mut nodes_expanded: usize = 0;
|
||
|
||
while let Some(current_node) = scratch.open_set.pop() {
|
||
let current = current_node.position;
|
||
nodes_expanded += 1;
|
||
|
||
if nodes_expanded > PATHFINDER_MAX_NODES {
|
||
return (Vec::new(), nodes_expanded); // force retarget
|
||
}
|
||
|
||
if current == goal {
|
||
return (
|
||
reconstruct_path(&scratch.came_from, current),
|
||
nodes_expanded,
|
||
);
|
||
}
|
||
|
||
scratch.closed_set.insert(current);
|
||
|
||
let current_g = *scratch.g_scores.get(¤t).unwrap_or(&i32::MAX);
|
||
|
||
for &move_dir in &ALLOWED_MOVES {
|
||
let neighbor_pos = current + move_dir;
|
||
|
||
if !is_standable_tile(tilemap, neighbor_pos)
|
||
|| scratch.closed_set.contains(&neighbor_pos)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
let tile_weight = get_tile_weight(tilemap, neighbor_pos);
|
||
let movement_cost = calculate_movement_cost(move_dir, tile_weight);
|
||
if movement_cost == 0 {
|
||
continue;
|
||
}
|
||
|
||
let new_g = current_g + movement_cost;
|
||
|
||
if new_g < *scratch.g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) {
|
||
scratch.came_from.insert(neighbor_pos, current);
|
||
scratch.g_scores.insert(neighbor_pos, new_g);
|
||
let f = new_g + octile_distance_3d(neighbor_pos, goal);
|
||
scratch.open_set.push(PathNode {
|
||
position: neighbor_pos,
|
||
f_score: f,
|
||
g_score: new_g,
|
||
});
|
||
}
|
||
}
|
||
}
|
||
|
||
(Vec::new(), nodes_expanded)
|
||
})
|
||
}
|
||
|
||
pub fn calculate_provisional_path(
|
||
tilemap: &TileMap,
|
||
start: IVec3,
|
||
goal: IVec3,
|
||
node_limit: usize,
|
||
) -> Vec<Vec3> {
|
||
let timer = Instant::now();
|
||
|
||
if !is_standable_tile(tilemap, start) {
|
||
LOCAL_FAILED_PATHS.with(|f| {
|
||
*f.borrow_mut() += 1;
|
||
});
|
||
return vec![Vec3::new(start.x as f32, start.y as f32, start.z as f32)];
|
||
}
|
||
|
||
let estimated_tiles = octile_distance_3d(start, goal) / ITILE_SIZE;
|
||
|
||
let result = SCRATCHPAD.with(|s| {
|
||
let mut scratch = s.borrow_mut();
|
||
let capacity = ((estimated_tiles as usize).max(64)).min(4096);
|
||
scratch.clear_and_reserve(capacity);
|
||
|
||
let initial_h = octile_distance_3d(start, goal);
|
||
scratch.open_set.push(PathNode {
|
||
position: start,
|
||
f_score: initial_h,
|
||
g_score: 0,
|
||
});
|
||
scratch.g_scores.insert(start, 0);
|
||
|
||
let mut nodes_expanded: usize = 0;
|
||
let mut best_node = start;
|
||
let mut best_h = initial_h;
|
||
|
||
while let Some(current_node) = scratch.open_set.pop() {
|
||
let current = current_node.position;
|
||
nodes_expanded += 1;
|
||
|
||
let h = octile_distance_3d(current, goal);
|
||
if h < best_h {
|
||
best_h = h;
|
||
best_node = current;
|
||
}
|
||
|
||
if current == goal {
|
||
return (
|
||
reconstruct_path(&scratch.came_from, current),
|
||
nodes_expanded,
|
||
);
|
||
}
|
||
|
||
if nodes_expanded >= node_limit {
|
||
return (Vec::new(), nodes_expanded);
|
||
}
|
||
|
||
scratch.closed_set.insert(current);
|
||
|
||
for &move_dir in &ALLOWED_MOVES {
|
||
let neighbor_pos = current + move_dir;
|
||
|
||
if !is_standable_tile(tilemap, neighbor_pos)
|
||
|| scratch.closed_set.contains(&neighbor_pos)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
let tile_weight = get_tile_weight(tilemap, neighbor_pos);
|
||
let movement_cost = calculate_movement_cost(move_dir, tile_weight);
|
||
if movement_cost == 0 {
|
||
continue;
|
||
}
|
||
|
||
let new_g = *scratch.g_scores.get(¤t).unwrap_or(&i32::MAX) + movement_cost;
|
||
|
||
if new_g < *scratch.g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) {
|
||
scratch.came_from.insert(neighbor_pos, current);
|
||
scratch.g_scores.insert(neighbor_pos, new_g);
|
||
let f = new_g + octile_distance_3d(neighbor_pos, goal);
|
||
scratch.open_set.push(PathNode {
|
||
position: neighbor_pos,
|
||
f_score: f,
|
||
g_score: new_g,
|
||
});
|
||
}
|
||
}
|
||
}
|
||
|
||
(
|
||
reconstruct_path(&scratch.came_from, best_node),
|
||
nodes_expanded,
|
||
)
|
||
});
|
||
|
||
let elapsed = timer.elapsed().as_micros();
|
||
LOCAL_PATH_TIMES.with(|t| {
|
||
t.borrow_mut().push(elapsed);
|
||
});
|
||
LOCAL_PATH_LENGTHS.with(|l| {
|
||
l.borrow_mut().push(result.0.len());
|
||
});
|
||
LOCAL_NODES_EXPANDED.with(|n| {
|
||
n.borrow_mut().push(result.1);
|
||
});
|
||
|
||
result.0
|
||
}
|
||
|
||
struct ChunkPathNode {
|
||
position: IVec2,
|
||
f_score: i32,
|
||
g_score: i32,
|
||
}
|
||
|
||
impl Eq for ChunkPathNode {}
|
||
|
||
impl PartialEq for ChunkPathNode {
|
||
fn eq(&self, other: &Self) -> bool {
|
||
self.position == other.position
|
||
}
|
||
}
|
||
|
||
impl Ord for ChunkPathNode {
|
||
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
|
||
self.f_score
|
||
.cmp(&other.f_score)
|
||
.then_with(|| self.g_score.cmp(&other.g_score))
|
||
}
|
||
}
|
||
|
||
impl PartialOrd for ChunkPathNode {
|
||
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
|
||
Some(self.cmp(other))
|
||
}
|
||
}
|
||
|
||
thread_local! {
|
||
static CHUNK_SCRATCHPAD: RefCell<ChunkAStarScratchpad> = RefCell::new(ChunkAStarScratchpad::default());
|
||
}
|
||
|
||
struct ChunkAStarScratchpad {
|
||
g_scores: FxHashMap<IVec2, i32>,
|
||
came_from: FxHashMap<IVec2, IVec2>,
|
||
closed_set: FxHashSet<IVec2>,
|
||
open_set: BinaryHeap<ChunkPathNode>,
|
||
}
|
||
|
||
impl Default for ChunkAStarScratchpad {
|
||
fn default() -> Self {
|
||
Self {
|
||
g_scores: FxHashMap::default(),
|
||
came_from: FxHashMap::default(),
|
||
closed_set: FxHashSet::default(),
|
||
open_set: BinaryHeap::new(),
|
||
}
|
||
}
|
||
}
|
||
|
||
impl ChunkAStarScratchpad {
|
||
fn clear(&mut self) {
|
||
self.g_scores.clear();
|
||
self.came_from.clear();
|
||
self.closed_set.clear();
|
||
self.open_set.clear();
|
||
}
|
||
}
|
||
|
||
fn manhattan_distance_2d(a: IVec2, b: IVec2) -> i32 {
|
||
(a.x - b.x).abs() + (a.y - b.y).abs()
|
||
}
|
||
|
||
pub fn calculate_chunk_path(
|
||
chunk_map: &ChunkMap,
|
||
start_chunk: IVec2,
|
||
goal_chunk: IVec2,
|
||
) -> Vec<IVec2> {
|
||
if start_chunk == goal_chunk {
|
||
return vec![start_chunk];
|
||
}
|
||
|
||
if !chunk_map.loaded_chunks.contains_key(&goal_chunk) {
|
||
return Vec::new();
|
||
}
|
||
|
||
if !chunk_map.loaded_chunks.contains_key(&start_chunk) {
|
||
return Vec::new();
|
||
}
|
||
|
||
CHUNK_SCRATCHPAD.with(|s| {
|
||
let mut scratch = s.borrow_mut();
|
||
scratch.clear();
|
||
|
||
let h = manhattan_distance_2d(start_chunk, goal_chunk);
|
||
scratch.open_set.push(ChunkPathNode {
|
||
position: start_chunk,
|
||
f_score: h,
|
||
g_score: 0,
|
||
});
|
||
scratch.g_scores.insert(start_chunk, 0);
|
||
|
||
while let Some(current_node) = scratch.open_set.pop() {
|
||
let current = current_node.position;
|
||
|
||
if current == goal_chunk {
|
||
let mut path = vec![current];
|
||
let mut curr = current;
|
||
while let Some(&prev) = scratch.came_from.get(&curr) {
|
||
path.push(prev);
|
||
curr = prev;
|
||
}
|
||
path.reverse();
|
||
return path;
|
||
}
|
||
|
||
scratch.closed_set.insert(current);
|
||
|
||
if let Some(neighbors) = chunk_map.chunk_connectivity.get(¤t) {
|
||
for &neighbor in neighbors {
|
||
if scratch.closed_set.contains(&neighbor) {
|
||
continue;
|
||
}
|
||
|
||
let new_g = *scratch.g_scores.get(¤t).unwrap_or(&i32::MAX) + 1;
|
||
|
||
if new_g < *scratch.g_scores.get(&neighbor).unwrap_or(&i32::MAX) {
|
||
scratch.came_from.insert(neighbor, current);
|
||
scratch.g_scores.insert(neighbor, new_g);
|
||
let f = new_g + manhattan_distance_2d(neighbor, goal_chunk);
|
||
scratch.open_set.push(ChunkPathNode {
|
||
position: neighbor,
|
||
f_score: f,
|
||
g_score: new_g,
|
||
});
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
Vec::new()
|
||
})
|
||
}
|
||
|
||
pub fn bench_report_system(
|
||
keys: Res<ButtonInput<KeyCode>>,
|
||
mut bench: ResMut<PathfindingBenchmark>,
|
||
) {
|
||
if keys.just_pressed(KeyCode::F8) {
|
||
println!("\n=== PATHFINDING BENCHMARK REPORT ===");
|
||
report_stat("path_calc", &bench.path_calc_times_us);
|
||
report_stat(
|
||
"path_length",
|
||
&bench
|
||
.path_lengths
|
||
.iter()
|
||
.map(|&l| l as u128)
|
||
.collect::<Vec<_>>(),
|
||
);
|
||
report_stat(
|
||
"nodes_expanded",
|
||
&bench
|
||
.nodes_expanded
|
||
.iter()
|
||
.map(|&n| n as u128)
|
||
.collect::<Vec<_>>(),
|
||
);
|
||
|
||
if !bench.movement_system_times_us.is_empty() {
|
||
report_stat("movement_system", &bench.movement_system_times_us);
|
||
}
|
||
if !bench.wander_system_times_us.is_empty() {
|
||
report_stat("wander_system", &bench.wander_system_times_us);
|
||
}
|
||
|
||
let total = bench.total_paths_calculated;
|
||
let failed = bench.total_failed_paths;
|
||
println!(
|
||
"[BENCH] total_paths={} failed_paths={} success_rate={:.1}%",
|
||
total,
|
||
failed,
|
||
if total > 0 {
|
||
100.0 * (total - failed) as f64 / total as f64
|
||
} else {
|
||
100.0
|
||
}
|
||
);
|
||
|
||
if let Err(e) = write_benchmark_csv(&bench, "pathfinding_benchmark_current.csv") {
|
||
eprintln!("Failed to write benchmark CSV: {}", e);
|
||
}
|
||
println!("=====================================\n");
|
||
}
|
||
}
|
||
|
||
fn report_stat(label: &str, times: &[u128]) {
|
||
if times.is_empty() {
|
||
return;
|
||
}
|
||
let sum: u128 = times.iter().sum();
|
||
let avg = sum / times.len() as u128;
|
||
let min = *times.iter().min().unwrap();
|
||
let max = *times.iter().max().unwrap();
|
||
let mut sorted = times.to_vec();
|
||
sorted.sort_unstable();
|
||
let median = sorted[sorted.len() / 2];
|
||
let p95_idx = (sorted.len() as f64 * 0.95) as usize;
|
||
let p95 = sorted[p95_idx.min(sorted.len().saturating_sub(1))];
|
||
|
||
println!(
|
||
"[BENCH][{}] n={} avg={}µs median={}µs min={}µs max={}µs p95={}µs",
|
||
label,
|
||
times.len(),
|
||
avg,
|
||
median,
|
||
min,
|
||
max,
|
||
p95
|
||
);
|
||
}
|
||
|
||
fn write_benchmark_csv(bench: &PathfindingBenchmark, filename: &str) -> std::io::Result<()> {
|
||
use std::fs::File;
|
||
use std::io::Write;
|
||
|
||
let mut file = File::create(filename)?;
|
||
writeln!(
|
||
file,
|
||
"sample,path_duration_us,path_length,nodes_expanded,success"
|
||
)?;
|
||
|
||
let n = bench.path_calc_times_us.len();
|
||
for i in 0..n {
|
||
let duration = bench.path_calc_times_us.get(i).copied().unwrap_or(0);
|
||
let length = bench.path_lengths.get(i).copied().unwrap_or(0);
|
||
let nodes = bench.nodes_expanded.get(i).copied().unwrap_or(0);
|
||
let success = i < n.saturating_sub(bench.total_failed_paths as usize);
|
||
writeln!(file, "{},{},{},{},{}", i, duration, length, nodes, success)?;
|
||
}
|
||
|
||
writeln!(file, "# Summary")?;
|
||
if !bench.path_calc_times_us.is_empty() {
|
||
let avg: u128 =
|
||
bench.path_calc_times_us.iter().sum::<u128>() / bench.path_calc_times_us.len() as u128;
|
||
writeln!(file, "# avg_duration_us,{}", avg)?;
|
||
}
|
||
writeln!(file, "# total_paths,{}", bench.total_paths_calculated)?;
|
||
writeln!(file, "# failed_paths,{}", bench.total_failed_paths)?;
|
||
|
||
Ok(())
|
||
}
|