387 lines
15 KiB
Rust
387 lines
15 KiB
Rust
use crate::constants::TILE_SIZE;
|
||
use crate::tile::TileMap;
|
||
use crate::{
|
||
constants::*,
|
||
entities::shared_components::Ambulatory,
|
||
tilemap::{ChunkMap, CHUNK_SIZE},
|
||
};
|
||
use bevy::{math::ivec3, prelude::*};
|
||
use bevy_rand::prelude::*;
|
||
use rand::Rng;
|
||
use std::{
|
||
collections::{BinaryHeap, HashMap, HashSet},
|
||
process::exit,
|
||
};
|
||
|
||
#[derive(Clone, Eq, PartialEq, Debug)]
|
||
struct PathNode {
|
||
position: IVec3,
|
||
f_score: i32,
|
||
g_score: i32,
|
||
}
|
||
|
||
impl Ord for PathNode {
|
||
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
|
||
other.f_score.cmp(&self.f_score)
|
||
}
|
||
}
|
||
|
||
impl PartialOrd for PathNode {
|
||
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
|
||
Some(self.cmp(other))
|
||
}
|
||
}
|
||
|
||
pub struct PathfindingPlugin;
|
||
|
||
impl Plugin for PathfindingPlugin {
|
||
fn build(&self, app: &mut App) {
|
||
app.add_systems(FixedUpdate, (update_wandering_targets, movement).chain());
|
||
}
|
||
}
|
||
|
||
pub fn update_wandering_targets(
|
||
mut query: Query<(&mut Ambulatory, &Transform)>, // add a 'with' here when behaviours are implemented
|
||
tilemap: Res<TileMap>,
|
||
chunk_map: Res<ChunkMap>,
|
||
mut rng_q: Query<&mut Entropy<WyRand>, With<Global>>,
|
||
) {
|
||
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 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;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn movement(mut query: Query<(&mut Ambulatory, &mut Transform)>, tilemap: Res<TileMap>) {
|
||
query
|
||
.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(
|
||
&tilemap,
|
||
transform.translation.as_ivec3(),
|
||
target.as_ivec3() - ivec3(0, 0, 1),
|
||
));
|
||
ambulatory.path_index = 0;
|
||
}
|
||
if ambulatory.walk_speed > 0. {
|
||
if ambulatory.step_recovery <= ambulatory.walk_speed as u32 {
|
||
ambulatory.step_recovery += 1;
|
||
return;
|
||
} else {
|
||
ambulatory.step_recovery = 0;
|
||
}
|
||
}
|
||
|
||
// 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;
|
||
}
|
||
} else {
|
||
ambulatory.current_path = None;
|
||
ambulatory.target = None;
|
||
}
|
||
}
|
||
}
|
||
});
|
||
}
|
||
|
||
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;
|
||
|
||
// Check if current position has a blocking floor tile
|
||
if let Some(current_floor_tile) = tilemap.floor_tiles.get(&pos) {
|
||
can_i_stand_in_tile = current_floor_tile.1;
|
||
}
|
||
// Check if current position has a solid fixture tile (e.g., log)
|
||
if let Some(current_fixture_tile) = tilemap.fixture_tiles.get(&pos) {
|
||
can_i_stand_in_fixture = current_fixture_tile.1;
|
||
}
|
||
|
||
// Check if there's solid ground below (fixture or floor)
|
||
let pos_below = pos - IVec3::new(0, 0, ITILE_SIZE);
|
||
|
||
if let Some(below_floor_tile) = tilemap.floor_tiles.get(&pos_below) {
|
||
can_i_stand_on_tile_bellow = below_floor_tile.2;
|
||
}
|
||
|
||
if let Some(below_fixture_tile) = tilemap.fixture_tiles.get(&pos_below) {
|
||
can_i_stand_on_fixture_bellow = below_fixture_tile.2;
|
||
}
|
||
return (can_i_stand_in_tile || can_i_stand_in_fixture)
|
||
&& (can_i_stand_on_tile_bellow || can_i_stand_on_fixture_bellow);
|
||
}
|
||
|
||
fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec<Vec3> {
|
||
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);
|
||
}
|
||
|
||
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();
|
||
|
||
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 allowed_moves = vec![
|
||
// 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 (left/negative preference)
|
||
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 {
|
||
// println!("path found");
|
||
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)
|
||
|
||
// 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, 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;
|
||
|
||
// Only add to open set if not already there
|
||
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();
|
||
|
||
// 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 reconstruct_path(came_from: HashMap<IVec3, IVec3>, mut current: IVec3) -> Vec<Vec3> {
|
||
let mut path = vec![Vec3::new(
|
||
current.x as f32,
|
||
current.y as f32,
|
||
current.z as f32,
|
||
)];
|
||
|
||
while let Some(&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
|
||
}
|