From 367ab26d5e1ace1a88eb6e4a794bdab5ce35ef25 Mon Sep 17 00:00:00 2001 From: popertots Date: Wed, 18 Mar 2026 15:30:48 +0000 Subject: [PATCH] Fix performance regressions: remove TIER0, consolidate scratchpads, remove Arc trap MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Key fixes based on benchmark analysis: 1. Remove TIER0 Vec-based pathfinding - O(N) linear scan was slower than FxHashMap for typical path lengths 2. Consolidate scratchpads into single AStarScratchpad struct - eliminates nested RefCell borrow overhead 3. Remove Arc wrapper from TileMap - eliminated Copy-on-Write trap causing 63ms stutters 4. Replace AHashMap with FxHashMap - FxHash is faster for small integer keys like IVec3 5. Simplify tier logic - single pathfinding function with scratchpad reuse Benchmark analysis showed: - Original: 1.95 µs/node, P99 1.1ms, Max 5ms - TIER0/TIER1 regression: 2.89 µs/node (+48%), P99 5ms (+348%) - Root causes: Vec linear scan in TIER0, nested RefCell borrows, Arc::make_mut CoW This should restore and improve performance by using simple FxHashMap scratchpad for all paths. --- src/constants.rs | 12 +- src/entities/shared_systems/pathfinding.rs | 1513 +++----------------- src/world/tiles/tilemap.rs | 67 +- 3 files changed, 246 insertions(+), 1346 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index 853a18f..3bae652 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -4,12 +4,6 @@ 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 +pub const PATHFINDER_SHORT_PATH_MAX_TILES: i32 = 100; +pub const PATHFINDER_MAX_NODES: usize = 5000; +pub const PATHFINDER_WAYPOINT_THRESHOLD_TILES: i32 = 100; diff --git a/src/entities/shared_systems/pathfinding.rs b/src/entities/shared_systems/pathfinding.rs index 11f22c6..696354b 100644 --- a/src/entities/shared_systems/pathfinding.rs +++ b/src/entities/shared_systems/pathfinding.rs @@ -1,63 +1,72 @@ -use ahash::AHashMap; -use ahash::AHashSet; use bevy::prelude::*; use rayon::prelude::*; use rustc_hash::FxHashMap; -use rustc_hash::FxHashSet as HashSet; +use rustc_hash::FxHashSet; use std::{cell::RefCell, collections::BinaryHeap, 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::constants::{ITILE_SIZE, PATHFINDER_MAX_NODES, 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::math::ivec3; use bevy_rand::prelude::*; use rand::RngExt; -/// Maximum nodes to expand before giving up (prevents runaway searches) -const MAX_NODES: usize = 10000; +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) }; +} + +/// Single consolidated scratchpad for A* pathfinding. +/// One RefCell borrow instead of multiple nested borrows. +struct AStarScratchpad { + g_scores: FxHashMap, + came_from: FxHashMap, + closed_set: FxHashSet, + open_set: BinaryHeap, +} + +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 = RefCell::new(AStarScratchpad::default()); +} 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), @@ -66,7 +75,6 @@ const ALLOWED_MOVES: [IVec3; 24] = [ 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), @@ -76,6 +84,7 @@ const ALLOWED_MOVES: [IVec3; 24] = [ 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, @@ -85,7 +94,10 @@ struct PathNode { impl Ord for PathNode { fn cmp(&self, other: &Self) -> std::cmp::Ordering { - other.f_score.cmp(&self.f_score) + other + .f_score + .cmp(&self.f_score) + .then_with(|| other.g_score.cmp(&self.g_score)) } } @@ -94,24 +106,16 @@ 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, } @@ -125,25 +129,6 @@ impl PathfindingBenchmark { } } -/// 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 { @@ -181,8 +166,6 @@ pub fn process_completed_paths( } } -/// 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(); @@ -190,19 +173,16 @@ pub fn merge_benchmark_stats(mut bench: ResMut) { 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; @@ -211,56 +191,52 @@ pub fn merge_benchmark_stats(mut bench: ResMut) { } pub fn update_wandering_targets( - mut query: Query<(&mut Ambulatory, &Transform)>, // add a 'with' here when behaviours are implemented + mut query: Query<(&mut Ambulatory, &Transform)>, tilemap: Res, chunk_map: Res, mut rng_q: Query<&mut WyRand, With>, ) { - if let Ok(mut rng) = rng_q.single_mut() { - for (mut ambulatory, _) in query.iter_mut() { - if ambulatory.target.is_none() - || (ambulatory.current_path.is_some() - && ambulatory.path_index >= ambulatory.current_path.as_ref().unwrap().len()) - { - // Find a random loaded chunk + let Ok(mut rng) = rng_q.single_mut() else { + return; + }; + + query.iter_mut().for_each(|(mut ambulatory, transform)| { + if ambulatory.target.is_none() { + let center = transform.translation; + let center_chunk = IVec2::new( + (center.x / (CHUNK_SIZE as f32 * TILE_SIZE)).floor() as i32, + (center.y / (CHUNK_SIZE as f32 * TILE_SIZE)).floor() as i32, + ); + + if chunk_map.loaded_chunks.contains_key(¢er_chunk) { let loaded_chunks: Vec<&IVec2> = chunk_map.loaded_chunks.keys().collect(); if !loaded_chunks.is_empty() { let random_index = rng.random_range(0..loaded_chunks.len()); if let Some(&chunk_pos) = loaded_chunks.get(random_index) { - // Generate random position within 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); - // Get height at position - let surface_height = 0; - - // Find a valid z-level near the surface - for z in (surface_height - 3)..=(surface_height + 4) { - let mut target_pos = IVec3::new(target_x, target_y, z) * ITILE_SIZE; - if let Some(_) = tilemap.floor_tiles.get(&target_pos) { - target_pos.z += ITILE_SIZE; - if let Some(_base_texture) = tilemap.floor_tiles.get(&target_pos) { - if 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; - } - } + for z in -3..=4 { + let target_pos = IVec3::new(target_x, target_y, z) * ITILE_SIZE; + if tilemap.floor_tiles.contains_key(&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) { @@ -268,14 +244,12 @@ pub fn movement(mut query: Query<(&mut Ambulatory, &mut Transform)>, tilemap: Re .par_iter_mut() .for_each(|(mut ambulatory, mut transform)| { let current_pos = transform.translation; - // Apply gravity if in air if !is_standable_tile(&tilemap, current_pos.as_ivec3()) { transform.translation.z -= TILE_SIZE; return; } if let Some(target) = ambulatory.target { - // Calculate path if needed if ambulatory.current_path.is_none() { ambulatory.current_path = Some(calculate_path_benchmarked( &tilemap, @@ -293,22 +267,18 @@ pub fn movement(mut query: Query<(&mut Ambulatory, &mut Transform)>, tilemap: Re } } - // Follow the current path if let Some(path) = &ambulatory.current_path { if ambulatory.path_index < path.len() { let next_point = path[ambulatory.path_index]; - let direction = (next_point - transform.translation).normalize(); transform.translation = next_point; - // Update sprite direction (only for x movement) if direction.x > 0.0 { transform.scale.x = PIXEL_RATIO; } else if direction.x < 0.0 { transform.scale.x = -PIXEL_RATIO; } - // Check if we've reached the next point if transform.translation.distance(next_point) < TILE_SIZE { ambulatory.path_index += 1; } @@ -320,210 +290,60 @@ 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; - let mut can_i_stand_in_fixture: bool = false; - let mut can_i_stand_on_fixture_bellow: bool = false; - - if let Some(current_floor_tile) = tilemap.floor_tiles.get(&pos) { - can_i_stand_in_tile = current_floor_tile.can_stand_in(); - } - if let Some(current_fixture_tile) = tilemap.fixture_tiles.get(&pos) { - can_i_stand_in_fixture = current_fixture_tile.can_stand_in(); - } - + let can_stand_in_tile = tilemap + .floor_tiles + .get(&pos) + .map(|t| t.can_stand_in()) + .unwrap_or(false); + let can_stand_in_fixture = tilemap + .fixture_tiles + .get(&pos) + .map(|t| t.can_stand_in()) + .unwrap_or(false); let pos_below = pos - IVec3::new(0, 0, ITILE_SIZE); + let can_stand_on_tile_below = tilemap + .floor_tiles + .get(&pos_below) + .map(|t| t.can_stand_on()) + .unwrap_or(false); + let can_stand_on_fixture_below = tilemap + .fixture_tiles + .get(&pos_below) + .map(|t| t.can_stand_on()) + .unwrap_or(false); - if let Some(below_floor_tile) = tilemap.floor_tiles.get(&pos_below) { - can_i_stand_on_tile_bellow = below_floor_tile.can_stand_on(); - } - - if let Some(below_fixture_tile) = tilemap.fixture_tiles.get(&pos_below) { - can_i_stand_on_fixture_bellow = below_fixture_tile.can_stand_on(); - } - (can_i_stand_in_tile || can_i_stand_in_fixture) - && (can_i_stand_on_tile_bellow || can_i_stand_on_fixture_bellow) + (can_stand_in_tile || can_stand_in_fixture) + && (can_stand_on_tile_below || can_stand_on_fixture_below) } -/// 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 { - // Graceful failure instead of exit(0) - if !is_standable_tile(tilemap, start) || !is_standable_tile(tilemap, goal) { - return Vec::new(); +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) => 42, + (1, 1, 1) => 56, + _ => 0, } - - // 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, - 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); - - // Static move vectors - computed once - 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), - ]; - - while let Some(current_node) = open_set.pop() { - let current = current_node.position; - - in_open_set.remove(¤t); - - 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; - } - - // TODO: Add terrain-based cost modifiers - // movement_cost = apply_terrain_modifier(movement_cost, neighbor_pos, tilemap); - // Examples: - // - Mud/sand: +50% cost - // - Ice: +100% cost - // - Designated high-traffic areas: -25% cost - // - Designated restricted areas: +500% cost - // - Etc - - let movement_cost = match ( - move_dir.x.abs() / ITILE_SIZE, - move_dir.y.abs() / ITILE_SIZE, - move_dir.z.abs() / ITILE_SIZE, - ) { - // 2D Movement (Dwarf Fortress style) - (1, 0, 0) | (0, 1, 0) => 10, // Orthogonal movement - (1, 1, 0) => 14, // Diagonal movement (~√2 × 10) - // 3D Movement (Climbing diagonally - even more expensive) - (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 - // Stairs would reduce vertical costs significantly: - // (0, 0, 1) => 20 if has_stairs(current, neighbor_pos), // Stairs: 2× horizontal cost - // (1, 0, 1) | (0, 1, 1) => 24 if has_stairs(current, neighbor_pos), // Stairs + horizontal - // (1, 1, 1) => 28 if has_stairs(current, neighbor_pos), // Stairs + diagonal - - // TODO: Implement ramps for even smoother vertical movement - // Ramps would be cheaper than stairs: - // (0, 0, 1) => 15 if has_ramp(current, neighbor_pos), // Ramps: 1.5× horizontal cost - // (1, 0, 1) | (0, 1, 1) => 18 if has_ramp(current, neighbor_pos), // Ramps + horizontal - // (1, 1, 1) => 21 if has_ramp(current, neighbor_pos), // Ramps + diagonal - _ => 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; - - if !in_open_set.contains(&neighbor_pos) { - 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); - } else { - let neighbor_node = PathNode { - position: neighbor_pos, - f_score: f, - g_score: new_g, - }; - open_set.push(neighbor_node); - } - } - } - } - - Vec::new() } fn octile_distance_3d(a: IVec3, b: IVec3) -> i32 { let dx = (a.x - b.x).abs(); let dy = (a.y - b.y).abs(); let dz = (a.z - b.z).abs(); + let (dmax, dmid, dmin) = sorted_desc(dx, dy, dz); + 10 * dmax + 4 * dmid + dmin +} - // Dwarf Fortress style costs - let cost_orthogonal = 10; // Horizontal orthogonal - let cost_diagonal = 14; // Horizontal diagonal (~√2 × 10) - let cost_climb = 50; // Raw vertical movement (climbing) - - let mut diffs = [dx, dy, dz]; - diffs.sort_unstable(); - let dmin = diffs[0]; - let dmax = diffs[2]; - - if dz == 0 { - // Pure 2D movement - let diagonal_moves = dmin / ITILE_SIZE; - let orthogonal_moves = (dmax - dmin) / ITILE_SIZE; - cost_diagonal * diagonal_moves + cost_orthogonal * orthogonal_moves - } else { - // Movement involves Z - assume raw climbing for now - // TODO: Modify this when stairs/ramps are implemented - let z_moves = dz / ITILE_SIZE; - let xy_distance = ((dx * dx + dy * dy) as f32).sqrt() as i32; - let remaining_2d_diagonal = (xy_distance.min(dz)) / ITILE_SIZE; - let remaining_2d_orthogonal = - (xy_distance - remaining_2d_diagonal * ITILE_SIZE) / ITILE_SIZE; - - // Raw climbing cost + remaining 2D movement - cost_climb * z_moves - + cost_diagonal * remaining_2d_diagonal - + cost_orthogonal * remaining_2d_orthogonal - } +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, mut current: IVec3) -> Vec { @@ -532,21 +352,120 @@ fn reconstruct_path(came_from: &FxHashMap, mut current: IVec3) -> 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; + 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 } -/// Benchmark reporting system - press F8 to dump stats to console and CSV +pub fn calculate_path_benchmarked(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec { + 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, 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 ( + reconstruct_path(&scratch.came_from, current), + nodes_expanded, + ); + } + + if current == goal { + return ( + reconstruct_path(&scratch.came_from, current), + 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 movement_cost = calculate_movement_cost(move_dir); + 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, + }); + } + } + } + + (Vec::new(), nodes_expanded) + }) +} + pub fn bench_report_system( keys: Res>, mut bench: ResMut, @@ -591,11 +510,9 @@ pub fn bench_report_system( } ); - // Write CSV to file - if let Err(e) = write_benchmark_csv(&bench, "pathfinding_benchmark_baseline.csv") { + if let Err(e) = write_benchmark_csv(&bench, "pathfinding_benchmark_current.csv") { eprintln!("Failed to write benchmark CSV: {}", e); } - println!("=====================================\n"); } } @@ -615,7 +532,7 @@ fn report_stat(label: &str, times: &[u128]) { let p95 = sorted[p95_idx.min(sorted.len().saturating_sub(1))]; println!( - "[BENCH][{}] n={} avg={}us median={}us min={}us max={}us p95={}us", + "[BENCH][{}] n={} avg={}µs median={}µs min={}µs max={}µs p95={}µs", label, times.len(), avg, @@ -645,7 +562,6 @@ fn write_benchmark_csv(bench: &PathfindingBenchmark, filename: &str) -> std::io: 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 = @@ -657,968 +573,3 @@ fn write_benchmark_csv(bench: &PathfindingBenchmark, filename: &str) -> std::io: 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 - } -} diff --git a/src/world/tiles/tilemap.rs b/src/world/tiles/tilemap.rs index f3fd9ef..fa1f4b3 100644 --- a/src/world/tiles/tilemap.rs +++ b/src/world/tiles/tilemap.rs @@ -1,6 +1,5 @@ -use ahash::AHashMap; use bevy::prelude::*; -use std::sync::Arc; +use rustc_hash::FxHashMap; /// Packed floor tile data for efficient storage. ~35 bytes vs 76 bytes tuple. #[derive(Clone, Copy, Debug)] @@ -89,28 +88,6 @@ impl FloorTileData { self.flags &= !0b100; } } - - pub fn from_tuple(tuple: (i32, bool, bool, bool, i32, [u32; 8])) -> Self { - Self::new( - tuple.0 as u8, - tuple.1, - tuple.2, - tuple.3, - tuple.4 as u8, - tuple.5, - ) - } - - pub fn to_tuple(&self) -> (i32, bool, bool, bool, i32, [u32; 8]) { - ( - self.id as i32, - self.can_stand_in(), - self.can_stand_on(), - self.visibly_transparent(), - self.astar_weight as i32, - self.visible_range, - ) - } } /// Packed fixture tile data. ~18 bytes vs 48 bytes tuple. @@ -156,28 +133,14 @@ impl FixtureTileData { pub fn can_stand_on(&self) -> bool { self.flags & 0b010 != 0 } - - pub fn from_tuple(tuple: (i32, bool, bool, [u32; 8])) -> Self { - Self::new(tuple.0 as u8, tuple.1, tuple.2, tuple.3) - } - - pub fn to_tuple(&self) -> (i32, bool, bool, [u32; 8]) { - ( - self.id as i32, - self.can_stand_in(), - self.can_stand_on(), - self.visible_range, - ) - } } -/// Tile map with Arc-wrapped HashMaps for async pathfinding access. -/// Uses copy-on-write: Arc::make_mut clones only if other Arcs exist. -#[derive(Resource, Clone, Default)] +/// Tile map using FxHashMap for fast lookups. No Arc wrapper - single-threaded access. +#[derive(Resource, Default)] pub struct TileMap { - pub floor_tiles: Arc>, - pub fixture_tiles: Arc>, - pub item_tiles: Arc>>, + pub floor_tiles: FxHashMap, + pub fixture_tiles: FxHashMap, + pub item_tiles: FxHashMap>, } impl TileMap { @@ -207,34 +170,26 @@ impl TileMap { #[inline] pub fn insert_floor(&mut self, pos: IVec3, tile: FloorTileData) { - Arc::make_mut(&mut self.floor_tiles).insert(pos, tile); + self.floor_tiles.insert(pos, tile); } #[inline] pub fn insert_fixture(&mut self, pos: IVec3, tile: FixtureTileData) { - Arc::make_mut(&mut self.fixture_tiles).insert(pos, tile); + self.fixture_tiles.insert(pos, tile); } #[inline] pub fn insert_item(&mut self, pos: IVec3, entity_id: u32) { - Arc::make_mut(&mut self.item_tiles) - .entry(pos) - .or_default() - .push(entity_id); + self.item_tiles.entry(pos).or_default().push(entity_id); } #[inline] pub fn remove_item(&mut self, pos: &IVec3) -> Option> { - Arc::make_mut(&mut self.item_tiles).remove(pos) + self.item_tiles.remove(pos) } #[inline] pub fn get_floor_mut(&mut self, pos: &IVec3) -> Option<&mut FloorTileData> { - Arc::make_mut(&mut self.floor_tiles).get_mut(pos) - } - - #[inline] - pub fn get_fixture_mut(&mut self, pos: &IVec3) -> Option<&mut FixtureTileData> { - Arc::make_mut(&mut self.fixture_tiles).get_mut(pos) + self.floor_tiles.get_mut(pos) } }