diff --git a/Cargo.lock b/Cargo.lock index 4159cb0..58598e8 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2331,10 +2331,12 @@ dependencies = [ name = "dorf" version = "0.1.0" dependencies = [ + "ahash", "bevy", "bevy_platform", "bevy_rand", "image", + "nohash-hasher", "noise", "rand 0.10.0", "rayon", @@ -3567,6 +3569,12 @@ dependencies = [ "libc", ] +[[package]] +name = "nohash-hasher" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2bf50223579dc7cdcfb3bfcacf7069ff68243f8c363f62ffa99cf000a6b9c451" + [[package]] name = "noise" version = "0.9.0" diff --git a/Cargo.toml b/Cargo.toml index 8ffa1f3..998fdd6 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -12,6 +12,8 @@ image = "0.25.10" bevy_platform = "0.18.1" rayon = "1.11.0" rustc-hash = "2.1.1" +ahash = "0.8.12" +nohash-hasher = "0.2.0" # Enable max optimizations for dependencies, but not for our code: [profile.dev.package."*"] diff --git a/src/constants.rs b/src/constants.rs index a0c34b2..853a18f 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -3,3 +3,13 @@ pub const TILE_PIXELS: u32 = 16; pub const TILE_SIZE: f32 = TILE_PIXELS as f32 * PIXEL_RATIO; pub const ITILE_SIZE: i32 = TILE_SIZE as i32; pub const SEED: u32 = 420; + +// Pathfinding tier thresholds (in tiles) +pub const PATHFINDER_TIER0_MAX_TILES: i32 = 10; // Very short paths: Vec-based +pub const PATHFINDER_TIER1_MAX_TILES: i32 = 30; // Short paths: AHashMap scratchpad +pub const PATHFINDER_TIER2_MAX_TILES: i32 = 100; // Medium paths: AHashMap scratchpad + // TIER3: > 100 tiles → Chunk waypoints + async + +// Pathfinding configuration +pub const PATHFINDER_MAX_NODES: usize = 5000; // Max nodes before partial path +pub const PATHFINDER_ASYNC_THRESHOLD_TILES: i32 = 150; // Start async for very long paths diff --git a/src/entities/shared_components/ambulatory.rs b/src/entities/shared_components/ambulatory.rs index 8c7ae65..5b41470 100644 --- a/src/entities/shared_components/ambulatory.rs +++ b/src/entities/shared_components/ambulatory.rs @@ -9,3 +9,21 @@ pub struct Ambulatory { pub target: Option, pub step_recovery: u32, } + +#[derive(Component)] +pub struct PendingPath { + pub start: IVec3, + pub goal: IVec3, + pub waypoint_path: Vec, + pub request_id: u64, +} + +#[derive(Resource, Default)] +pub struct PathRequestCounter { + pub next_id: u64, +} + +#[derive(Resource, Default)] +pub struct CompletedPaths { + pub paths: Vec<(u64, Vec)>, +} diff --git a/src/entities/shared_systems/pathfinding.rs b/src/entities/shared_systems/pathfinding.rs index ee8c897..803e2a0 100644 --- a/src/entities/shared_systems/pathfinding.rs +++ b/src/entities/shared_systems/pathfinding.rs @@ -1,15 +1,81 @@ -use crate::constants::TILE_SIZE; +use ahash::AHashMap; +use ahash::AHashSet; +use bevy::tasks::{AsyncComputeTaskPool, Task}; +use rayon::prelude::*; +use rustc_hash::FxHashMap; +use rustc_hash::FxHashSet as HashSet; +use std::{cell::RefCell, collections::BinaryHeap, sync::Arc, time::Instant}; + +// Thread-local storage for collecting metrics during parallel execution +thread_local! { + static LOCAL_PATH_TIMES: RefCell> = const{ RefCell::new(Vec::new()) } ; + static LOCAL_PATH_LENGTHS: RefCell> = const{ RefCell::new(Vec::new()) }; + static LOCAL_NODES_EXPANDED: RefCell> = const{ RefCell::new(Vec::new()) }; + static LOCAL_FAILED_PATHS: RefCell = const{ RefCell::new(0) }; +} + +// Thread-local scratchpad for memory reuse (eliminates allocation overhead) +// Cleared before each use but retains allocated capacity +thread_local! { + static SCRATCH_G_SCORES_FX: RefCell> = RefCell::new(FxHashMap::default()); + static SCRATCH_CAME_FROM_FX: RefCell> = RefCell::new(FxHashMap::default()); + static SCRATCH_CLOSED_SET_FX: RefCell> = RefCell::new(HashSet::default()); + static SCRATCH_IN_OPEN_FX: RefCell> = RefCell::new(HashSet::default()); + static SCRATCH_G_SCORES_AH: RefCell> = RefCell::new(AHashMap::default()); + static SCRATCH_CAME_FROM_AH: RefCell> = RefCell::new(AHashMap::default()); + static SCRATCH_CLOSED_SET_AH: RefCell> = RefCell::new(AHashSet::default()); + static SCRATCH_IN_OPEN_AH: RefCell> = RefCell::new(AHashSet::default()); + static SCRATCH_TIER0_G_POS: RefCell> = RefCell::new(Vec::with_capacity(256)); + static SCRATCH_TIER0_G_VAL: RefCell> = RefCell::new(Vec::with_capacity(256)); + static SCRATCH_TIER0_CF_POS: RefCell> = RefCell::new(Vec::with_capacity(256)); + static SCRATCH_TIER0_CF_PREV: RefCell> = RefCell::new(Vec::with_capacity(256)); + static SCRATCH_TIER0_CLOSED: RefCell> = RefCell::new(Vec::with_capacity(256)); +} + +use crate::constants::{ + ITILE_SIZE, PATHFINDER_ASYNC_THRESHOLD_TILES, PATHFINDER_MAX_NODES, PATHFINDER_TIER0_MAX_TILES, + PATHFINDER_TIER1_MAX_TILES, PATHFINDER_TIER2_MAX_TILES, TILE_SIZE, +}; use crate::world::tiles::TileMap; use crate::world::{chunks::ChunkMap, chunks::CHUNK_SIZE}; use crate::{constants::*, entities::shared_components::Ambulatory}; use bevy::{math::ivec3, prelude::*}; use bevy_rand::prelude::*; use rand::RngExt; -use std::{ - collections::{BinaryHeap, HashMap, HashSet}, - process::exit, -}; +/// Maximum nodes to expand before giving up (prevents runaway searches) +const MAX_NODES: usize = 10000; + +const ALLOWED_MOVES: [IVec3; 24] = [ + //Orthogonal moves + IVec3::new(-ITILE_SIZE, 0, 0), + IVec3::new(ITILE_SIZE, 0, 0), + IVec3::new(0, -ITILE_SIZE, 0), + IVec3::new(0, ITILE_SIZE, 0), + // Diagonal moves + 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), + // Diagonal with vertical moves + 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), + // Full 3D diagonal moves + 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, @@ -28,15 +94,122 @@ impl PartialOrd for PathNode { Some(self.cmp(other)) } } +/// Benchmark statistics for pathfinding performance analysis. +/// Results are written to CSV on F8 keypress for comparison. +#[derive(Resource, Default)] +pub struct PathfindingBenchmark { + // Per-path metrics (collected via thread-local, merged at end) + pub path_calc_times_us: Vec, + pub path_lengths: Vec, + pub nodes_expanded: Vec, + + // System-level metrics + pub movement_system_times_us: Vec, + pub wander_system_times_us: Vec, + + // Counters + pub total_paths_calculated: u64, + pub total_failed_paths: u64, + + // Reporting config + 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() + } + } +} + +/// Individual path calculation metrics (returned from benchmarked function) +#[derive(Clone, Debug)] +pub struct PathMetrics { + pub duration_us: u128, + pub path_length: usize, + pub nodes_expanded: usize, + pub success: bool, +} + +/// CSV output row for benchmark comparison +#[derive(Debug, Clone)] +pub struct BenchmarkRow { + pub timestamp_ms: u128, + pub path_duration_us: u128, + pub path_length: usize, + pub nodes_expanded: usize, + pub success: bool, +} pub struct PathfindingPlugin; impl Plugin for PathfindingPlugin { fn build(&self, app: &mut App) { - app.add_systems(FixedUpdate, (update_wandering_targets, movement).chain()); + app.insert_resource(PathfindingBenchmark::new(100)) + .insert_resource(crate::entities::shared_components::CompletedPaths::default()) + .insert_resource(crate::entities::shared_components::PathRequestCounter::default()) + .add_systems(FixedUpdate, (update_wandering_targets, movement).chain()) + .add_systems( + PostUpdate, + (merge_benchmark_stats, process_completed_paths).chain(), + ) + .add_systems(Update, bench_report_system); } } +pub fn process_completed_paths( + mut completed: ResMut, + 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::(); + } + } + } +} + +/// Merge thread-local benchmark stats into the global resource. +/// This must run after all parallel work is complete. +pub fn merge_benchmark_stats(mut bench: ResMut) { + 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)>, // add a 'with' here when behaviours are implemented tilemap: Res, @@ -104,7 +277,7 @@ pub fn movement(mut query: Query<(&mut Ambulatory, &mut Transform)>, tilemap: Re if let Some(target) = ambulatory.target { // Calculate path if needed if ambulatory.current_path.is_none() { - ambulatory.current_path = Some(calculate_path( + ambulatory.current_path = Some(calculate_path_benchmarked( &tilemap, transform.translation.as_ivec3(), target.as_ivec3() - ivec3(0, 0, 1), @@ -147,7 +320,6 @@ pub fn movement(mut query: Query<(&mut Ambulatory, &mut Transform)>, tilemap: Re } }); } - fn is_standable_tile(tilemap: &TileMap, pos: IVec3) -> bool { let mut can_i_stand_in_tile: bool = false; let mut can_i_stand_on_tile_bellow: bool = false; @@ -177,23 +349,23 @@ fn is_standable_tile(tilemap: &TileMap, pos: IVec3) -> bool { && (can_i_stand_on_tile_bellow || can_i_stand_on_fixture_bellow); } +/// Original calculate_path - kept for reference, not currently used. +/// Use calculate_path_benchmarked for actual gameplay. +#[allow(dead_code)] fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { - if !is_standable_tile(tilemap, start) { - println!("Start pos invalid: {}", start); - println!("Bugger (1)"); - exit(0); - } - if !is_standable_tile(tilemap, goal) { - println!("Goal pos invalid: {}", goal); - println!("Bugger (2)"); - exit(0); + // Graceful failure instead of exit(0) + if !is_standable_tile(tilemap, start) || !is_standable_tile(tilemap, goal) { + return Vec::new(); } - let mut open_set = BinaryHeap::new(); - let mut came_from = HashMap::new(); - let mut g_scores = HashMap::new(); - let mut closed_set = HashSet::new(); - let mut in_open_set = HashSet::new(); + // Estimate capacity based on heuristic distance + let estimated_nodes = ((octile_distance_3d(start, goal) / 10).max(64) as usize).min(2048); + + let mut open_set = BinaryHeap::with_capacity(estimated_nodes); + let mut came_from = FxHashMap::with_capacity_and_hasher(estimated_nodes, Default::default()); + let mut g_scores = FxHashMap::with_capacity_and_hasher(estimated_nodes, Default::default()); + let mut closed_set = HashSet::with_capacity_and_hasher(estimated_nodes, Default::default()); + let mut in_open_set = HashSet::with_capacity_and_hasher(estimated_nodes, Default::default()); let start_node = PathNode { position: start, @@ -205,7 +377,8 @@ fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { in_open_set.insert(start); g_scores.insert(start, 0); - let allowed_moves = vec![ + // Static move vectors - computed once + const ALLOWED_MOVES: [IVec3; 24] = [ // Orthogonal moves IVec3::new(-ITILE_SIZE, 0, 0), IVec3::new(ITILE_SIZE, 0, 0), @@ -216,7 +389,7 @@ fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { IVec3::new(-ITILE_SIZE, ITILE_SIZE, 0), IVec3::new(ITILE_SIZE, -ITILE_SIZE, 0), IVec3::new(ITILE_SIZE, ITILE_SIZE, 0), - // Diagonal with vertical moves (left/negative preference) + // Diagonal with vertical moves IVec3::new(-ITILE_SIZE, 0, ITILE_SIZE), IVec3::new(-ITILE_SIZE, 0, -ITILE_SIZE), IVec3::new(ITILE_SIZE, 0, ITILE_SIZE), @@ -242,13 +415,12 @@ fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { in_open_set.remove(¤t); if current == goal { - // println!("path found"); - return reconstruct_path(came_from, current); + return reconstruct_path(&came_from, current); } closed_set.insert(current); - for &move_dir in &allowed_moves { + for &move_dir in &ALLOWED_MOVES { let neighbor_pos = current + move_dir; if !is_standable_tile(tilemap, neighbor_pos) || closed_set.contains(&neighbor_pos) { @@ -272,12 +444,8 @@ fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { // 2D Movement (Dwarf Fortress style) (1, 0, 0) | (0, 1, 0) => 10, // Orthogonal movement (1, 1, 0) => 14, // Diagonal movement (~√2 × 10) - - // Vertical Movement (Raw climbing - very expensive) - // (0, 0, 1) => 50, // Pure vertical climb/fall - // 3D Movement (Climbing diagonally - even more expensive) - (1, 0, 1) | (0, 1, 1) => 52, // Orthogonal + vertical climb + (1, 0, 1) | (0, 1, 1) => 42, // Orthogonal + vertical climb (1, 1, 1) => 56, // Diagonal + vertical climb // TODO: Implement stairs and ramps for efficient vertical movement @@ -294,7 +462,7 @@ fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { _ => continue, }; - let new_g = g_scores.get(¤t).unwrap_or(&i32::MAX) + movement_cost; + let new_g = *g_scores.get(¤t).unwrap_or(&i32::MAX) + movement_cost; if new_g < *g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) { came_from.insert(neighbor_pos, current); @@ -302,7 +470,6 @@ fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { let h = octile_distance_3d(neighbor_pos, goal); let f = new_g + h; - // Only add to open set if not already there if !in_open_set.contains(&neighbor_pos) { let neighbor_node = PathNode { position: neighbor_pos, @@ -362,7 +529,7 @@ fn octile_distance_3d(a: IVec3, b: IVec3) -> i32 { } } -fn reconstruct_path(came_from: HashMap, mut current: IVec3) -> Vec { +fn reconstruct_path(came_from: &FxHashMap, mut current: IVec3) -> Vec { let mut path = vec![Vec3::new( current.x as f32, current.y as f32, @@ -381,3 +548,1080 @@ fn reconstruct_path(came_from: HashMap, mut current: IVec3) -> Vec path.reverse(); path } + +/// Benchmark reporting system - press F8 to dump stats to console and CSV +pub fn bench_report_system( + keys: Res>, + mut bench: ResMut, +) { + 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::>(), + ); + report_stat( + "nodes_expanded", + &bench + .nodes_expanded + .iter() + .map(|&n| n as u128) + .collect::>(), + ); + + 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 + } + ); + + // Write CSV to file + if let Err(e) = write_benchmark_csv(&bench, "pathfinding_benchmark_baseline.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={}us median={}us min={}us max={}us p95={}us", + 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 - bench.total_failed_paths as usize); + writeln!(file, "{},{},{},{},{}", i, duration, length, nodes, success)?; + } + + // Summary stats at the end + writeln!(file, "# Summary")?; + if !bench.path_calc_times_us.is_empty() { + let avg: u128 = + bench.path_calc_times_us.iter().sum::() / 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(()) +} + +fn calculate_movement_cost(move_dir: IVec3) -> i32 { + 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, + (1, 1, 0) => 14, + (1, 0, 1) | (0, 1, 1) => 52, + (1, 1, 1) => 56, + _ => 0, + } +} + +fn calculate_path_tier0(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { + let max_nodes = 256usize; + let mut g_positions: Vec = Vec::with_capacity(max_nodes); + let mut g_values: Vec = Vec::with_capacity(max_nodes); + let mut came_from_positions: Vec = Vec::with_capacity(max_nodes); + let mut came_from_prev: Vec = Vec::with_capacity(max_nodes); + let mut closed_positions: Vec = Vec::with_capacity(max_nodes); + let mut open_set: BinaryHeap = BinaryHeap::with_capacity(max_nodes); + + open_set.push(PathNode { + position: start, + f_score: octile_distance_3d(start, goal), + g_score: 0, + }); + g_positions.push(start); + g_values.push(0); + + let get_g = |pos: IVec3, positions: &[IVec3], values: &[i32]| -> i32 { + for (i, &p) in positions.iter().enumerate() { + if p == pos { + return values[i]; + } + } + i32::MAX + }; + + while let Some(current_node) = open_set.pop() { + let current = current_node.position; + + if current == goal { + let mut path = vec![Vec3::new( + current.x as f32, + current.y as f32, + current.z as f32, + )]; + let mut curr = current; + loop { + let mut found = false; + for (i, &p) in came_from_positions.iter().enumerate() { + if p == curr { + let prev = came_from_prev[i]; + path.push(Vec3::new(prev.x as f32, prev.y as f32, prev.z as f32)); + curr = prev; + found = true; + break; + } + } + if !found { + break; + } + } + path.reverse(); + return path; + } + + if closed_positions.len() > 500 { + return vec![Vec3::new(start.x as f32, start.y as f32, start.z as f32)]; + } + + closed_positions.push(current); + + for &move_dir in &ALLOWED_MOVES { + let neighbor_pos = current + move_dir; + + if !is_standable_tile(tilemap, neighbor_pos) + || closed_positions.iter().any(|&p| p == neighbor_pos) + { + continue; + } + + let movement_cost = calculate_movement_cost(move_dir); + if movement_cost == 0 { + continue; + } + + let current_g = get_g(current, &g_positions, &g_values); + let new_g = if current_g == i32::MAX { + movement_cost + } else { + current_g + movement_cost + }; + let neighbor_g = get_g(neighbor_pos, &g_positions, &g_values); + + if new_g < neighbor_g { + let mut found = false; + for (i, &p) in came_from_positions.iter().enumerate() { + if p == neighbor_pos { + came_from_prev[i] = current; + found = true; + break; + } + } + if !found { + came_from_positions.push(neighbor_pos); + came_from_prev.push(current); + } + + let mut g_found = false; + for (i, &p) in g_positions.iter().enumerate() { + if p == neighbor_pos { + g_values[i] = new_g; + g_found = true; + break; + } + } + if !g_found { + g_positions.push(neighbor_pos); + g_values.push(new_g); + } + + let h = octile_distance_3d(neighbor_pos, goal); + open_set.push(PathNode { + position: neighbor_pos, + f_score: new_g + h, + g_score: new_g, + }); + } + } + } + + Vec::new() +} + +fn calculate_path_tier1(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { + let estimated_nodes = ((octile_distance_3d(start, goal) / 10).max(16) as usize).min(2048); + + let mut open_set = BinaryHeap::with_capacity(estimated_nodes); + let mut came_from = FxHashMap::with_capacity_and_hasher(estimated_nodes, Default::default()); + let mut g_scores = FxHashMap::with_capacity_and_hasher(estimated_nodes, Default::default()); + let mut closed_set = HashSet::with_capacity_and_hasher(estimated_nodes, Default::default()); + let mut in_open_set = HashSet::with_capacity_and_hasher(estimated_nodes, Default::default()); + + let start_node = PathNode { + position: start, + f_score: octile_distance_3d(start, goal), + g_score: 0, + }; + + open_set.push(start_node); + in_open_set.insert(start); + g_scores.insert(start, 0); + + let mut nodes_expanded: usize = 0; + + while let Some(current_node) = open_set.pop() { + let current = current_node.position; + + in_open_set.remove(¤t); + nodes_expanded += 1; + + if nodes_expanded > PATHFINDER_MAX_NODES { + return reconstruct_path(&came_from, current); + } + + if current == goal { + return reconstruct_path(&came_from, current); + } + + closed_set.insert(current); + + for &move_dir in &ALLOWED_MOVES { + let neighbor_pos = current + move_dir; + + if !is_standable_tile(tilemap, neighbor_pos) || closed_set.contains(&neighbor_pos) { + continue; + } + + let movement_cost = calculate_movement_cost(move_dir); + if movement_cost == 0 { + continue; + } + + let new_g = *g_scores.get(¤t).unwrap_or(&i32::MAX) + movement_cost; + + if new_g < *g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) { + came_from.insert(neighbor_pos, current); + g_scores.insert(neighbor_pos, new_g); + let h = octile_distance_3d(neighbor_pos, goal); + let f = new_g + h; + + let neighbor_node = PathNode { + position: neighbor_pos, + f_score: f, + g_score: new_g, + }; + open_set.push(neighbor_node); + in_open_set.insert(neighbor_pos); + } + } + } + + Vec::new() +} + +fn calculate_path_tier2(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { + let estimated_nodes = ((octile_distance_3d(start, goal) / 10).max(64) as usize).min(4096); + + let mut open_set = BinaryHeap::with_capacity(estimated_nodes); + let mut came_from: AHashMap = AHashMap::with_capacity(estimated_nodes); + let mut g_scores: AHashMap = AHashMap::with_capacity(estimated_nodes); + let mut closed_set: AHashSet = AHashSet::with_capacity(estimated_nodes); + let mut in_open_set: AHashSet = AHashSet::with_capacity(estimated_nodes); + + let start_node = PathNode { + position: start, + f_score: octile_distance_3d(start, goal), + g_score: 0, + }; + + open_set.push(start_node); + in_open_set.insert(start); + g_scores.insert(start, 0); + + let mut nodes_expanded: usize = 0; + + while let Some(current_node) = open_set.pop() { + let current = current_node.position; + + in_open_set.remove(¤t); + nodes_expanded += 1; + + if nodes_expanded > PATHFINDER_MAX_NODES { + return reconstruct_path_ahash(&came_from, current); + } + + if current == goal { + return reconstruct_path_ahash(&came_from, current); + } + + closed_set.insert(current); + + for &move_dir in &ALLOWED_MOVES { + let neighbor_pos = current + move_dir; + + if !is_standable_tile(tilemap, neighbor_pos) || closed_set.contains(&neighbor_pos) { + continue; + } + + let movement_cost = calculate_movement_cost(move_dir); + if movement_cost == 0 { + continue; + } + + let new_g = *g_scores.get(¤t).unwrap_or(&i32::MAX) + movement_cost; + + if new_g < *g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) { + came_from.insert(neighbor_pos, current); + g_scores.insert(neighbor_pos, new_g); + let h = octile_distance_3d(neighbor_pos, goal); + let f = new_g + h; + + let neighbor_node = PathNode { + position: neighbor_pos, + f_score: f, + g_score: new_g, + }; + open_set.push(neighbor_node); + in_open_set.insert(neighbor_pos); + } + } + } + + Vec::new() +} + +fn world_to_chunk(pos: IVec3) -> IVec2 { + IVec2::new( + pos.x.div_euclid(CHUNK_SIZE * ITILE_SIZE), + pos.y.div_euclid(CHUNK_SIZE * ITILE_SIZE), + ) +} + +fn generate_chunk_waypoints( + start: IVec3, + goal: IVec3, + start_chunk: IVec2, + goal_chunk: IVec2, +) -> Vec { + let mut waypoints = Vec::new(); + waypoints.push(start); + + let dx = goal_chunk.x - start_chunk.x; + let dy = goal_chunk.y - start_chunk.y; + let steps = dx.abs().max(dy.abs()); + + if steps > 1 { + for i in 1..steps { + let t = i as f32 / steps as f32; + let chunk_x = start_chunk.x as f32 + t * dx as f32; + let chunk_y = start_chunk.y as f32 + t * dy as f32; + + let waypoint = IVec3::new( + (chunk_x as i32 * CHUNK_SIZE + CHUNK_SIZE / 2) * ITILE_SIZE, + (chunk_y as i32 * CHUNK_SIZE + CHUNK_SIZE / 2) * ITILE_SIZE, + start.z, + ); + waypoints.push(waypoint); + } + } + + waypoints.push(goal); + waypoints +} + +fn reconstruct_path_ahash(came_from: &AHashMap, mut current: IVec3) -> Vec { + let mut path = vec![Vec3::new( + current.x as f32, + current.y as f32, + current.z as f32, + )]; + + while let Some(&previous) = came_from.get(¤t) { + path.push(Vec3::new( + previous.x as f32, + previous.y as f32, + previous.z as f32, + )); + current = previous; + } + + path.reverse(); + path +} + +fn calculate_path_tier3(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { + let start_chunk = world_to_chunk(start); + let goal_chunk = world_to_chunk(goal); + + let waypoints = generate_chunk_waypoints(start, goal, start_chunk, goal_chunk); + + if waypoints.len() <= 1 { + return calculate_path_tier2(tilemap, start, goal); + } + + let mut current = start; + let mut path_segments: Vec = Vec::new(); + + for (i, waypoint) in waypoints.iter().enumerate() { + let segment_distance = octile_distance_3d(current, *waypoint); + let segment_path = if segment_distance < PATHFINDER_TIER1_MAX_TILES * ITILE_SIZE { + calculate_path_tier1(tilemap, current, *waypoint) + } else { + calculate_path_tier2(tilemap, current, *waypoint) + }; + + if segment_path.is_empty() { + break; + } + + if i > 0 && !path_segments.is_empty() { + path_segments.pop(); + } + path_segments.extend(segment_path); + + current = *waypoint; + } + + path_segments +} + +pub fn calculate_path_auto(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { + if !is_standable_tile(tilemap, start) || !is_standable_tile(tilemap, goal) { + return Vec::new(); + } + + let estimated_tiles = octile_distance_3d(start, goal) / ITILE_SIZE; + + let result = if estimated_tiles < PATHFINDER_TIER0_MAX_TILES { + calculate_path_tier0(tilemap, start, goal) + } else if estimated_tiles < PATHFINDER_TIER1_MAX_TILES { + calculate_path_tier1(tilemap, start, goal) + } else if estimated_tiles < PATHFINDER_TIER2_MAX_TILES { + calculate_path_tier2(tilemap, start, goal) + } else { + calculate_path_tier3(tilemap, start, goal) + }; + + if result.is_empty() { + vec![Vec3::new(start.x as f32, start.y as f32, start.z as f32)] + } else { + result + } +} + +pub fn calculate_path_benchmarked(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { + let timer = Instant::now(); + let mut nodes_expanded: usize = 0; + + if !is_standable_tile(tilemap, start) { + LOCAL_FAILED_PATHS.with(|f| { + *f.borrow_mut() += 1; + }); + return Vec::new(); + } + if !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, tier_nodes) = if estimated_tiles < PATHFINDER_TIER0_MAX_TILES { + calculate_path_tier0_with_metrics(tilemap, start, goal) + } else if estimated_tiles < PATHFINDER_TIER1_MAX_TILES { + calculate_path_tier1_with_metrics(tilemap, start, goal) + } else if estimated_tiles < PATHFINDER_TIER2_MAX_TILES { + calculate_path_tier2_with_metrics(tilemap, start, goal) + } else { + calculate_path_tier3_with_metrics(tilemap, start, goal) + }; + + nodes_expanded = tier_nodes; + + let elapsed = timer.elapsed().as_micros(); + let path_len = result.len(); + + LOCAL_PATH_TIMES.with(|t| { + t.borrow_mut().push(elapsed); + }); + LOCAL_PATH_LENGTHS.with(|l| { + l.borrow_mut().push(path_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_tier0_with_metrics( + tilemap: &TileMap, + start: IVec3, + goal: IVec3, +) -> (Vec, usize) { + // Use scratchpad for TIER0 - no HashMap overhead, linear search in cached arrays + SCRATCH_TIER0_G_POS.with(|sgp| { + SCRATCH_TIER0_G_VAL.with(|sgv| { + SCRATCH_TIER0_CF_POS.with(|scp| { + SCRATCH_TIER0_CF_PREV.with(|scpr| { + SCRATCH_TIER0_CLOSED.with(|scl| { + let mut g_positions = sgp.borrow_mut(); + let mut g_values = sgv.borrow_mut(); + let mut came_from_positions = scp.borrow_mut(); + let mut came_from_prev = scpr.borrow_mut(); + let mut closed_positions = scl.borrow_mut(); + + g_positions.clear(); + g_values.clear(); + came_from_positions.clear(); + came_from_prev.clear(); + closed_positions.clear(); + + let mut open_set: BinaryHeap = BinaryHeap::with_capacity(256); + + open_set.push(PathNode { + position: start, + f_score: octile_distance_3d(start, goal), + g_score: 0, + }); + g_positions.push(start); + g_values.push(0); + + let mut nodes_expanded: usize = 0; + + while let Some(current_node) = open_set.pop() { + let current = current_node.position; + nodes_expanded += 1; + + if nodes_expanded > 300 { + let mut path = vec![Vec3::new( + current.x as f32, + current.y as f32, + current.z as f32, + )]; + for (i, &pos) in came_from_positions.iter().enumerate() { + if pos == current { + let mut prev = came_from_prev[i]; + loop { + path.push(Vec3::new( + prev.x as f32, + prev.y as f32, + prev.z as f32, + )); + let mut found = false; + for (j, &p) in came_from_positions.iter().enumerate() { + if p == prev { + prev = came_from_prev[j]; + found = true; + break; + } + } + if !found { + break; + } + } + path.reverse(); + return (path, nodes_expanded); + } + } + return ( + vec![Vec3::new(start.x as f32, start.y as f32, start.z as f32)], + nodes_expanded, + ); + } + + if current == goal { + let mut path = vec![Vec3::new( + current.x as f32, + current.y as f32, + current.z as f32, + )]; + let mut curr = current; + loop { + let mut found = false; + for (i, &pos) in came_from_positions.iter().enumerate() { + if pos == curr { + let prev = came_from_prev[i]; + path.push(Vec3::new( + prev.x as f32, + prev.y as f32, + prev.z as f32, + )); + curr = prev; + found = true; + break; + } + } + if !found { + break; + } + } + path.reverse(); + return (path, nodes_expanded); + } + + closed_positions.push(current); + + for &move_dir in &ALLOWED_MOVES { + let neighbor_pos = current + move_dir; + + if !is_standable_tile(tilemap, neighbor_pos) + || closed_positions.iter().any(|&p| p == neighbor_pos) + { + continue; + } + + let movement_cost = calculate_movement_cost(move_dir); + if movement_cost == 0 { + continue; + } + + let current_g = { + let mut g = i32::MAX; + for (i, &p) in g_positions.iter().enumerate() { + if p == current { + g = g_values[i]; + break; + } + } + g + }; + let new_g = if current_g == i32::MAX { + movement_cost + } else { + current_g + movement_cost + }; + + let neighbor_g = { + let mut g = i32::MAX; + for (i, &p) in g_positions.iter().enumerate() { + if p == neighbor_pos { + g = g_values[i]; + break; + } + } + g + }; + + if new_g < neighbor_g { + let mut found = false; + for (i, &p) in came_from_positions.iter().enumerate() { + if p == neighbor_pos { + came_from_prev[i] = current; + found = true; + break; + } + } + if !found { + came_from_positions.push(neighbor_pos); + came_from_prev.push(current); + } + + let mut g_found = false; + for (i, &p) in g_positions.iter().enumerate() { + if p == neighbor_pos { + g_values[i] = new_g; + g_found = true; + break; + } + } + if !g_found { + g_positions.push(neighbor_pos); + g_values.push(new_g); + } + + let h = octile_distance_3d(neighbor_pos, goal); + open_set.push(PathNode { + position: neighbor_pos, + f_score: new_g + h, + g_score: new_g, + }); + } + } + } + + (Vec::new(), nodes_expanded) + }) + }) + }) + }) + }) +} + +fn calculate_path_tier1_with_metrics( + tilemap: &TileMap, + start: IVec3, + goal: IVec3, +) -> (Vec, usize) { + let estimated_nodes = ((octile_distance_3d(start, goal) / 10).max(16) as usize).min(2048); + + // Use scratchpad for memory reuse + SCRATCH_G_SCORES_AH.with(|sg| { + SCRATCH_CAME_FROM_AH.with(|sc| { + SCRATCH_CLOSED_SET_AH.with(|scls| { + SCRATCH_IN_OPEN_AH.with(|sios| { + let mut g_scores = sg.borrow_mut(); + let mut came_from = sc.borrow_mut(); + let mut closed_set = scls.borrow_mut(); + let mut in_open_set = sios.borrow_mut(); + + // Clear but retain capacity + g_scores.clear(); + came_from.clear(); + closed_set.clear(); + in_open_set.clear(); + + // Reserve capacity if needed + if g_scores.capacity() < estimated_nodes { + g_scores.reserve(estimated_nodes); + came_from.reserve(estimated_nodes); + closed_set.reserve(estimated_nodes); + in_open_set.reserve(estimated_nodes); + } + + let mut open_set = BinaryHeap::with_capacity(estimated_nodes); + + let start_node = PathNode { + position: start, + f_score: octile_distance_3d(start, goal), + g_score: 0, + }; + + open_set.push(start_node); + in_open_set.insert(start); + g_scores.insert(start, 0); + + let mut nodes_expanded: usize = 0; + + while let Some(current_node) = open_set.pop() { + let current = current_node.position; + + in_open_set.remove(¤t); + nodes_expanded += 1; + + if nodes_expanded > PATHFINDER_MAX_NODES { + return (reconstruct_path_ahash(&came_from, current), nodes_expanded); + } + + if current == goal { + return (reconstruct_path_ahash(&came_from, current), nodes_expanded); + } + + closed_set.insert(current); + + for &move_dir in &ALLOWED_MOVES { + let neighbor_pos = current + move_dir; + + if !is_standable_tile(tilemap, neighbor_pos) + || closed_set.contains(&neighbor_pos) + { + continue; + } + + let movement_cost = calculate_movement_cost(move_dir); + if movement_cost == 0 { + continue; + } + + let new_g = + *g_scores.get(¤t).unwrap_or(&i32::MAX) + movement_cost; + + if new_g < *g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) { + came_from.insert(neighbor_pos, current); + g_scores.insert(neighbor_pos, new_g); + let h = octile_distance_3d(neighbor_pos, goal); + let f = new_g + h; + + let neighbor_node = PathNode { + position: neighbor_pos, + f_score: f, + g_score: new_g, + }; + open_set.push(neighbor_node); + in_open_set.insert(neighbor_pos); + } + } + } + + (Vec::new(), nodes_expanded) + }) + }) + }) + }) +} + +fn calculate_path_tier2_with_metrics( + tilemap: &TileMap, + start: IVec3, + goal: IVec3, +) -> (Vec, usize) { + let estimated_nodes = ((octile_distance_3d(start, goal) / 10).max(64) as usize).min(4096); + + let mut open_set = BinaryHeap::with_capacity(estimated_nodes); + let mut came_from: AHashMap = AHashMap::with_capacity(estimated_nodes); + let mut g_scores: AHashMap = AHashMap::with_capacity(estimated_nodes); + let mut closed_set: AHashSet = AHashSet::with_capacity(estimated_nodes); + let mut in_open_set: AHashSet = AHashSet::with_capacity(estimated_nodes); + + let start_node = PathNode { + position: start, + f_score: octile_distance_3d(start, goal), + g_score: 0, + }; + + open_set.push(start_node); + in_open_set.insert(start); + g_scores.insert(start, 0); + + let mut nodes_expanded: usize = 0; + + while let Some(current_node) = open_set.pop() { + let current = current_node.position; + + in_open_set.remove(¤t); + nodes_expanded += 1; + + if nodes_expanded > PATHFINDER_MAX_NODES { + return (reconstruct_path_ahash(&came_from, current), nodes_expanded); + } + + if current == goal { + return (reconstruct_path_ahash(&came_from, current), nodes_expanded); + } + + closed_set.insert(current); + + for &move_dir in &ALLOWED_MOVES { + let neighbor_pos = current + move_dir; + + if !is_standable_tile(tilemap, neighbor_pos) || closed_set.contains(&neighbor_pos) { + continue; + } + + let movement_cost = calculate_movement_cost(move_dir); + if movement_cost == 0 { + continue; + } + + let new_g = *g_scores.get(¤t).unwrap_or(&i32::MAX) + movement_cost; + + if new_g < *g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) { + came_from.insert(neighbor_pos, current); + g_scores.insert(neighbor_pos, new_g); + let h = octile_distance_3d(neighbor_pos, goal); + let f = new_g + h; + + let neighbor_node = PathNode { + position: neighbor_pos, + f_score: f, + g_score: new_g, + }; + open_set.push(neighbor_node); + in_open_set.insert(neighbor_pos); + } + } + } + + (Vec::new(), nodes_expanded) +} + +fn calculate_path_tier3_with_metrics( + tilemap: &TileMap, + start: IVec3, + goal: IVec3, +) -> (Vec, usize) { + let start_chunk = world_to_chunk(start); + let goal_chunk = world_to_chunk(goal); + + let waypoints = generate_chunk_waypoints(start, goal, start_chunk, goal_chunk); + + if waypoints.len() <= 1 { + return calculate_path_tier2_with_metrics(tilemap, start, goal); + } + + let mut current = start; + let mut path_segments: Vec = Vec::new(); + let mut total_nodes: usize = 0; + + for (i, waypoint) in waypoints.iter().enumerate() { + // Use non-metrics versions to avoid nested RefCell borrows + // Estimate nodes from segment distance (roughly 1-3 nodes per tile) + let segment_distance = octile_distance_3d(current, *waypoint); + let segment_path = if segment_distance < PATHFINDER_TIER1_MAX_TILES * ITILE_SIZE { + let path = calculate_path_tier1(tilemap, current, *waypoint); + total_nodes += (segment_distance / ITILE_SIZE).max(1) as usize * 3; + path + } else { + let path = calculate_path_tier2(tilemap, current, *waypoint); + total_nodes += (segment_distance / ITILE_SIZE).max(1) as usize * 5; + path + }; + + if segment_path.is_empty() { + break; + } + + if i > 0 && !path_segments.is_empty() { + path_segments.pop(); + } + path_segments.extend(segment_path); + + current = *waypoint; + } + + (path_segments, total_nodes) +} + +pub fn calculate_path_async(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { + if !is_standable_tile(tilemap, start) || !is_standable_tile(tilemap, goal) { + return Vec::new(); + } + + let estimated_tiles = octile_distance_3d(start, goal) / ITILE_SIZE; + + if estimated_tiles > PATHFINDER_ASYNC_THRESHOLD_TILES { + let start_chunk = world_to_chunk(start); + let goal_chunk = world_to_chunk(goal); + + let dx = goal_chunk.x - start_chunk.x; + let dy = goal_chunk.y - start_chunk.y; + let steps = dx.abs().max(dy.abs()) as usize; + + if steps > 2 { + let result = + calculate_path_parallel_chunks(tilemap, start, goal, start_chunk, goal_chunk); + if !result.is_empty() { + return result; + } + } + } + + calculate_path_auto(tilemap, start, goal) +} + +fn calculate_path_parallel_chunks( + tilemap: &TileMap, + start: IVec3, + goal: IVec3, + start_chunk: IVec2, + goal_chunk: IVec2, +) -> Vec { + let dx = goal_chunk.x - start_chunk.x; + let dy = goal_chunk.y - start_chunk.y; + let steps = dx.abs().max(dy.abs()) as usize; + + if steps == 0 { + return Vec::new(); + } + + let waypoints: Vec = (0..=steps) + .map(|i| { + let t = if steps == 0 { + 0.0f32 + } else { + i as f32 / steps as f32 + }; + let chunk_x = start_chunk.x as f32 + t * dx as f32; + let chunk_y = start_chunk.y as f32 + t * dy as f32; + IVec3::new( + (chunk_x as i32 * CHUNK_SIZE + CHUNK_SIZE / 2) * ITILE_SIZE, + (chunk_y as i32 * CHUNK_SIZE + CHUNK_SIZE / 2) * ITILE_SIZE, + start.z, + ) + }) + .collect(); + + let num_segments = waypoints.len().saturating_sub(1); + if num_segments < 2 { + return Vec::new(); + } + + let segment_results: Vec> = (0..num_segments) + .into_par_iter() + .map(|i| { + let seg_start = if i == 0 { start } else { waypoints[i] }; + let seg_end = waypoints[i + 1]; + + let est_tiles = octile_distance_3d(seg_start, seg_end) / ITILE_SIZE; + if est_tiles < PATHFINDER_TIER1_MAX_TILES { + calculate_path_tier1(tilemap, seg_start, seg_end) + } else { + calculate_path_tier2(tilemap, seg_start, seg_end) + } + }) + .collect(); + + let mut full_path = Vec::new(); + for (i, segment) in segment_results.iter().enumerate() { + if segment.is_empty() { + break; + } + + if i > 0 && !full_path.is_empty() { + full_path.pop(); + } + full_path.extend(segment.clone()); + } + + if full_path.is_empty() { + Vec::new() + } else { + full_path + } +}