-
-
Save Lambdanaut/fff2e5a92f31220824bd4b7d7c6bfa24 to your computer and use it in GitHub Desktop.
Pathfinding.rs
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| use std::cmp; | |
| use std::cmp::Reverse; | |
| use std::collections::BinaryHeap; | |
| use std::collections::HashMap; | |
| use std::sync::mpsc; | |
| use std::thread; | |
| use std::vec::Vec; | |
| use gdnative::prelude::*; | |
| use gdnative::api::TileMap; | |
| use ordered_float::OrderedFloat; | |
| use chrono; | |
| // OPTIMIZATIONS | |
| // * Make pathfinding return a VariantArray so we don't have to turn it into one in _process() | |
| // The TileMap tile ids of different tiles we want to use in pathfinding | |
| const GRASS_TERRAIN: i16 = 47; | |
| const ROCKY_INDOORS_TERRAIN: i16 = 62; | |
| const BRICK_INDOORS_TERRAIN: i16 = 67; | |
| const CLOUDS_TERRAIN: i16 = 96; | |
| const GRASS_RAMP_LEFT: i16 = 53; | |
| const GRASS_RAMP_RIGHT: i16 = 54; | |
| const SOLID_WHITE_BLOCK: i16 = 61; | |
| const ONE_WAY_BRIDGE_PLATFORM: i16 = 92; | |
| const ONE_WAY_INVISIBLE_PLATFORM: i16 = 88; | |
| const BLOCKING_TILE_TYPES: [i16; 5] = [ | |
| GRASS_TERRAIN, | |
| SOLID_WHITE_BLOCK, | |
| ROCKY_INDOORS_TERRAIN, | |
| BRICK_INDOORS_TERRAIN, | |
| CLOUDS_TERRAIN, | |
| ]; | |
| const RAMP_TILE_TYPES: [i16; 2] = [ | |
| GRASS_RAMP_LEFT, | |
| GRASS_RAMP_RIGHT, | |
| ]; | |
| const ONE_WAY_PLATFORM_TILE_TYPES: [i16; 2] = [ | |
| ONE_WAY_BRIDGE_PLATFORM, | |
| ONE_WAY_INVISIBLE_PLATFORM, | |
| ]; | |
| // MAX_JUMP_TO_SEARCH is for ignoring nodes where the jump is obscenely high | |
| const MAX_JUMP_TO_SEARCH: IType = 30; | |
| // Configure to i32 or more if desired for further pathfinding. | |
| // If the pathfinding distance is too stupidly huge, then odd errors will occur with i16 | |
| // But if the distance that huge, this pathfinding lib probably isn't the one for the job. | |
| type IType = i16; | |
| type ITypeTuple2 = (IType, IType); | |
| type ITypeTuple3 = (IType, IType, IType); | |
| // Used by Binheap | |
| type BinHeap4Tuple = (OrderedFloat<f32>, IType, IType, IType); | |
| pub trait DistanceTo { | |
| fn distance_to(&mut self, other: ITypeTuple2) -> f32; | |
| } | |
| impl DistanceTo for ITypeTuple2 { | |
| fn distance_to(&mut self, other: ITypeTuple2) -> f32 { | |
| return (((other.0 - self.0).pow(2) + (other.1 - self.1).pow(2) * 1) as f32).sqrt(); | |
| } | |
| } | |
| pub trait RemoveMultiple<T> { | |
| /// Remove multiple indices | |
| fn remove_multiple(&mut self, to_remove: Vec<usize>); | |
| } | |
| impl<T> RemoveMultiple<T> for Vec<T> { | |
| fn remove_multiple(&mut self, mut to_remove: Vec<usize>) { | |
| to_remove.sort(); | |
| to_remove.reverse(); | |
| for r in to_remove { | |
| self.remove(r); | |
| } | |
| } | |
| } | |
| #[derive(NativeClass)] | |
| #[inherit(Node)] | |
| #[register_with(Self::register_pathfinding)] | |
| pub struct Pathfinding { | |
| pathfinding_results: Vec<mpsc::Receiver<(i32, Vec<Vector3>)>>, | |
| #[property] | |
| pub cached_blocking_tiles: VariantArray, | |
| #[property] | |
| pub cached_ramp_tiles: VariantArray, | |
| #[property] | |
| pub cached_one_way_platform_tiles: VariantArray, | |
| #[property] | |
| pub BLOCKING_TILE_TYPES: VariantArray, | |
| #[property] | |
| pub RAMP_TILE_TYPES: VariantArray, | |
| #[property] | |
| pub ONE_WAY_PLATFORM_TILE_TYPES: VariantArray, | |
| } | |
| #[methods] | |
| impl Pathfinding { | |
| fn register_pathfinding(builder: &ClassBuilder<Self>) { | |
| builder.signal("path_found").done(); | |
| } | |
| fn new(_owner: &Node) -> Self { | |
| // Convert tile type Arrays to VariantArrays | |
| let blocking_tile_types = VariantArray::new(); | |
| for value in BLOCKING_TILE_TYPES { | |
| blocking_tile_types.push(value.to_variant()); | |
| } | |
| let ramp_tile_types = VariantArray::new(); | |
| for value in RAMP_TILE_TYPES { | |
| ramp_tile_types.push(value.to_variant()); | |
| } | |
| let one_way_platform_tile_types = VariantArray::new(); | |
| for value in ONE_WAY_PLATFORM_TILE_TYPES { | |
| one_way_platform_tile_types.push(value.to_variant()); | |
| } | |
| let mut cached_blocking_tiles = VariantArray::new().into_shared(); | |
| let mut cached_ramp_tiles = VariantArray::new().into_shared(); | |
| let mut cached_one_way_platform_tiles = VariantArray::new().into_shared(); | |
| Pathfinding { | |
| pathfinding_results: Vec::new(), | |
| cached_blocking_tiles, | |
| cached_ramp_tiles, | |
| cached_one_way_platform_tiles, | |
| BLOCKING_TILE_TYPES: blocking_tile_types.into_shared(), | |
| RAMP_TILE_TYPES: ramp_tile_types.into_shared(), | |
| ONE_WAY_PLATFORM_TILE_TYPES: one_way_platform_tile_types.into_shared(), | |
| } | |
| } | |
| #[export] | |
| pub fn _ready(&self, _owner: &Node) { | |
| } | |
| #[export] | |
| pub fn _process(&mut self, _owner: &Node, delta: f32) { | |
| // Listen to messages and emit signals of paths found here | |
| let mut exhausted_receiver_indexes = Vec::new(); | |
| for (i, receiver) in self.pathfinding_results.iter().enumerate() { | |
| // Listen for new paths | |
| match receiver.try_recv() { | |
| Ok(result) => { | |
| let (caller_instance_id, path) = result; | |
| exhausted_receiver_indexes.push(i); | |
| _owner.emit_signal("path_found", &[caller_instance_id.to_variant(), path.to_variant()]); | |
| }, | |
| Err(err) => continue | |
| } | |
| } | |
| // Remove any exhausted receivers from the pathfinding_results | |
| if !exhausted_receiver_indexes.is_empty() { | |
| self.pathfinding_results.remove_multiple(exhausted_receiver_indexes); | |
| } | |
| } | |
| #[export] | |
| pub fn initialize( | |
| &mut self, | |
| _owner: &Node, | |
| terrain_node: Ref<TileMap>, | |
| one_way_platforms_node: Ref<TileMap> | |
| ) { | |
| // Initializes the graph for the terrain on this world. | |
| // Must be called before any pathfinding can be done | |
| (self.cached_blocking_tiles, self.cached_ramp_tiles, self.cached_one_way_platform_tiles) = self.get_walkable_cells(_owner, terrain_node, one_way_platforms_node); | |
| } | |
| #[export] | |
| pub fn run_pathfinding_thread( | |
| &mut self, | |
| _owner: &Node, | |
| caller_instance_id: i32, | |
| mut start: Variant, | |
| mut goal: Variant, | |
| jump_limit: IType, | |
| grid: VariantArray, | |
| ramp_grid: VariantArray, | |
| one_way_platform_grid: VariantArray, | |
| _max_f_score: Option<f32> | |
| ) { | |
| let (tx, rx): (mpsc::Sender<(i32, Vec<Vector3>)>, mpsc::Receiver<(i32, Vec<Vector3>)>) = mpsc::channel(); | |
| self.pathfinding_results.push(rx); | |
| thread::spawn(move || { | |
| let path = pathfinding( | |
| Vector2::from_variant(&start).unwrap(), | |
| Vector2::from_variant(&goal).unwrap(), | |
| jump_limit, | |
| grid, | |
| ramp_grid, | |
| one_way_platform_grid, | |
| _max_f_score); | |
| tx.send((caller_instance_id, path)); | |
| }); | |
| } | |
| pub fn get_walkable_cells( | |
| &self, | |
| _owner: &Node, | |
| terrain_node: Ref<TileMap>, | |
| one_way_platforms_node: Ref<TileMap> | |
| ) -> (VariantArray, VariantArray, VariantArray) { | |
| // Returns three Arrays | |
| // * One containing all unwalkable grid cells. | |
| // * One containing all ramp cells. | |
| // * One containing all one way platforms. | |
| let terrain_node = unsafe { terrain_node.assume_safe() }; | |
| let one_way_platforms_node = unsafe { one_way_platforms_node.assume_safe() }; | |
| // Generate VariantArray of all non-traversable cells | |
| let mut blocking_tile_list = VariantArray::new(); | |
| for id in &self.BLOCKING_TILE_TYPES { | |
| let _id = i64::from_variant(&id).unwrap(); | |
| blocking_tile_list.extend(terrain_node.get_used_cells_by_id(_id).iter()); | |
| } | |
| // Generate VariantArray of all ramps | |
| let mut ramp_list = VariantArray::new(); | |
| for id in &self.RAMP_TILE_TYPES { | |
| let _id = i64::from_variant(&id).unwrap(); | |
| ramp_list.extend(terrain_node.get_used_cells_by_id(_id).iter()); | |
| } | |
| // Generate VariantArray of all one-way-platforms | |
| let mut one_way_platform_list = VariantArray::new(); | |
| for id in &self.ONE_WAY_PLATFORM_TILE_TYPES { | |
| let _id = i64::from_variant(&id).unwrap(); | |
| one_way_platform_list.extend(one_way_platforms_node.get_used_cells_by_id(_id).iter()); | |
| } | |
| return (blocking_tile_list.into_shared(), ramp_list.into_shared(), one_way_platform_list.into_shared()); | |
| } | |
| } | |
| pub fn pathfinding( | |
| mut _start: Vector2, | |
| mut _goal: Vector2, | |
| jump_limit: IType, | |
| grid: VariantArray, | |
| ramp_grid: VariantArray, | |
| one_way_platform_grid: VariantArray, | |
| _max_f_score: Option<f32>) -> Vec<Vector3> { | |
| // Returns the path from start to goal, as an array of arrays [[p1, jumpval], [p2, jumpval] ...] | |
| let x = chrono::offset::Utc::now(); | |
| // Update start and goal nodes to not be inside of ramps | |
| if ramp_grid.contains(&_start) { _start.y -= 1.0; } | |
| if ramp_grid.contains(&_goal) { _goal.y -= 1.0; } | |
| let start = vector2_to_tuple(_start); | |
| let goal = vector2_to_tuple(_goal); | |
| // Determine maximum f_score allowed | |
| let max_f_score; | |
| if let Some(value) = &_max_f_score { | |
| if value > &0.0 { | |
| max_f_score = *value; | |
| } else { | |
| max_f_score = estimate_max_f_score(start, goal, jump_limit); | |
| } | |
| } else { | |
| max_f_score = estimate_max_f_score(start, goal, jump_limit); | |
| } | |
| // 3D start node with jump set to 0 | |
| let start_node_3d: ITypeTuple3 = (start.0, start.1, 0); | |
| // Create a 4D tuple to add to the binary heap | |
| let start_node_4d_tuple = ( | |
| OrderedFloat(pathfinding_h(start, goal)), | |
| start_node_3d.0 as IType, | |
| start_node_3d.1 as IType, | |
| start_node_3d.2 as IType); | |
| // Initialize the open set to only include the starting node | |
| let mut open_set: BinaryHeap<Reverse<BinHeap4Tuple>> = BinaryHeap::from([Reverse(start_node_4d_tuple)]); | |
| // Map from node N to parent of node N, storing jump values | |
| let mut came_from: HashMap<ITypeTuple3, ITypeTuple3> = HashMap::new(); | |
| // Map from node N to the cost of the cheapest path to node N known | |
| let mut g_scores: HashMap<ITypeTuple3, f32> = HashMap::new(); | |
| // Insert the starting node into the g_scores with a value of 0 | |
| g_scores.insert((start_node_3d.0 as IType, start_node_3d.1 as IType, start_node_3d.2 as IType),0.0); | |
| while !open_set.is_empty() { | |
| // Unpack the current node we're investigating | |
| let mut current_node_3d: ITypeTuple3; | |
| match open_set.pop() { | |
| None => panic!("ERROR: The open set isn't empty as checked above, but it seems to be empty now."), | |
| Some(Reverse(v)) => { | |
| current_node_3d = (v.1, v.2, v.3); | |
| } | |
| } | |
| let mut current_node_2d: ITypeTuple2 = (current_node_3d.0, current_node_3d.1); | |
| if current_node_2d == goal { | |
| // Goal found! reconstruct the path working backwards from the goal node to the start node. | |
| let mut final_path: Vec<Vector3> = Vec::new(); | |
| final_path.push(tuple_to_vector3(current_node_3d)); | |
| while current_node_2d != start { | |
| // Get next node in came_from, working backwards. | |
| match came_from.get(¤t_node_3d) { | |
| None => panic!("BREAKING PATHFINDING ERROR: Couldn't find a came_from node along the way back."), | |
| Some(v) => { | |
| current_node_3d = (v.0, v.1, v.2); | |
| current_node_2d = (v.0, v.1); | |
| } | |
| } | |
| final_path.push(tuple_to_vector3(current_node_3d)); | |
| } | |
| // Reverse the path and return it | |
| final_path.reverse(); | |
| let y = chrono::offset::Utc::now(); | |
| let z = y - x; | |
| godot_print!("{}", z); | |
| return final_path; | |
| } | |
| // Fetch the g_score for our current node | |
| let g_score; | |
| match g_scores.get(¤t_node_3d) { | |
| None => panic!("Couldn't find point in g_scores"), | |
| Some(result) => { | |
| g_score = *result; | |
| } | |
| } | |
| for neighbor_node_3d in get_neighbors(current_node_3d, jump_limit, &grid, &one_way_platform_grid) { | |
| let neighbor_node_2d: ITypeTuple2 = (neighbor_node_3d.0, neighbor_node_3d.1); | |
| if grid.contains(&tuple_to_vector2(neighbor_node_2d)) { | |
| // We can't move here | |
| continue; | |
| } | |
| if neighbor_node_3d.2 > MAX_JUMP_TO_SEARCH { | |
| // Don't search further if the jump is too far | |
| // This is a MASSIVE boost in algorithm efficiency for harder paths, because it | |
| // cuts out a lot of silly searching. | |
| continue; | |
| } | |
| // pathfinding_tile_move_cost(current, neighbor) is the weight of the edge from current to neighbor | |
| // tentative_gScore is the distance from start to the neighbor through current | |
| let mut tentative_g_score = g_score + pathfinding_tile_move_cost(current_node_3d, neighbor_node_3d); | |
| // Fetch the g_score for our neighbor node. Tend towards INF if it doesn't exist. | |
| let neighbors_g_score; | |
| match g_scores.get(&neighbor_node_3d) { | |
| None => neighbors_g_score = f32::INFINITY, | |
| Some(result) => { | |
| neighbors_g_score = *result; | |
| } | |
| } | |
| if tentative_g_score < neighbors_g_score { | |
| // This path to the neighbor is better than any previous one. Record it! | |
| came_from.insert(neighbor_node_3d, current_node_3d); | |
| g_scores.insert(neighbor_node_3d, tentative_g_score); | |
| let new_f_score = tentative_g_score + pathfinding_h(neighbor_node_2d, goal); | |
| // If we've searched too deep, return. | |
| if new_f_score > max_f_score { | |
| godot_print!("During pathfinding, hit max f score of: `{}` with: `{}`", max_f_score, new_f_score); | |
| return Vec::new(); | |
| } | |
| let neighbor_node_4d_tuple = ( | |
| OrderedFloat(new_f_score), | |
| neighbor_node_3d.0, | |
| neighbor_node_3d.1, | |
| neighbor_node_3d.2); | |
| open_set.push(Reverse(neighbor_node_4d_tuple)); | |
| } | |
| } | |
| } | |
| godot_print!("During pathfinding, found that path was impossible before hitting f score"); | |
| // We exhausted the open set. Return an empty VariantArray | |
| return Vec::new(); | |
| } | |
| pub fn estimate_max_f_score(mut start: ITypeTuple2, goal: ITypeTuple2, jump_limit: IType) -> f32 { | |
| // Assuming start and goal are tileset relative vectors and not absolute positions | |
| let mut jump_limit_factor = 1.0; | |
| if jump_limit <= 2 { | |
| jump_limit_factor = 0.333; | |
| } | |
| return (((start.distance_to(goal)) * 12.0 + 90.0) * jump_limit_factor).into(); | |
| } | |
| fn pathfinding_tile_move_cost(from_node_3d: ITypeTuple3, to_node_3d: ITypeTuple3) -> f32 { | |
| // Prefer searching vertically first when doing a horizontal jump | |
| return if from_node_3d.1 == to_node_3d.1 && to_node_3d.2 != 0 && from_node_3d.2 == 0 {3.0} else {1.0}; | |
| } | |
| pub fn pathfinding_h(mut vector_1: ITypeTuple2, vector_2: ITypeTuple2) -> f32 { | |
| return vector_1.distance_to(vector_2) | |
| } | |
| fn get_neighbors( | |
| n: ITypeTuple3, | |
| jump_limit: IType, | |
| grid: &VariantArray, | |
| one_way_platform_grid: &VariantArray, | |
| ) -> Vec<ITypeTuple3> { | |
| let mut neighbors = Vec::new(); | |
| let x: IType = n.0; | |
| let y: IType = n.1; | |
| let current_jump = n.2; | |
| let next_vertical_jump_val = current_jump + 1 + (if current_jump % 2 == 0 {1} else {0}); | |
| // If the character doesn't jump, don't let them go up or down | |
| if jump_limit > 0 { | |
| // DO DOWN | |
| let bottom_down: ITypeTuple2 = (x, y+2); | |
| let down_jump_val: IType; | |
| if point_is_ground(bottom_down, grid, one_way_platform_grid) { | |
| down_jump_val = 0; | |
| } else { | |
| down_jump_val = cmp::max(jump_limit+1, next_vertical_jump_val); | |
| } | |
| let down: ITypeTuple3 = (x, y+1, down_jump_val); | |
| neighbors.push(down); | |
| // DO UP | |
| if current_jump < jump_limit { | |
| let up:ITypeTuple3 = (x, y-1, next_vertical_jump_val); | |
| neighbors.push(up); | |
| } | |
| } | |
| let fall_increment = if (current_jump as f32) < (jump_limit as f32) * 1.6 {2} else {4}; | |
| // For jumping to further spots that are just barely reachable | |
| let bonus_horizontal_movement_at_peak = current_jump == jump_limit + 1; | |
| if current_jump % fall_increment == 0 || bonus_horizontal_movement_at_peak { | |
| // DO LEFT | |
| let mut left: ITypeTuple3 = (x-1, y, 0); | |
| let bottom_left: ITypeTuple2 = (x-1, y+1); | |
| if !point_is_ground(bottom_left, grid, one_way_platform_grid) { | |
| // left has no ground below. require jump | |
| left.2 = current_jump + 1; | |
| } | |
| neighbors.push(left); | |
| // DO RIGHT | |
| let mut right: ITypeTuple3 = (x+1, y, 0); | |
| let bottom_right: ITypeTuple2 = (x+1, y+1); | |
| if !point_is_ground(bottom_right, grid, one_way_platform_grid) { | |
| // right has no ground below. require jump | |
| right.2 = current_jump + 1; | |
| } | |
| neighbors.push(right); | |
| } | |
| return neighbors | |
| } | |
| fn point_is_ground(p: ITypeTuple2, grid: &VariantArray, one_way_platform_grid: &VariantArray) -> bool { | |
| let vector = tuple_to_vector2(p); | |
| return grid.contains(vector) || one_way_platform_grid.contains(vector); | |
| } | |
| fn vector2_to_tuple(vector: Vector2) -> ITypeTuple2 { | |
| // Helper utility to turn a vector3 into a tuple | |
| return (vector.x as IType, vector.y as IType); | |
| } | |
| fn vector3_to_tuple(vector: Vector3) -> ITypeTuple3 { | |
| // Helper utility to turn a vector3 into a tuple | |
| return (vector.x as IType, vector.y as IType, vector.z as IType); | |
| } | |
| fn tuple_to_vector3(tuple: ITypeTuple3) -> Vector3 { | |
| // Helper utility to turn a tuple into a Vector3 | |
| return Vector3::new(tuple.0 as f32, tuple.1 as f32, tuple.2 as f32); | |
| } | |
| fn tuple_to_vector2(tuple: ITypeTuple2) -> Vector2 { | |
| // Helper utility to turn a tuple into a Vector2 | |
| return Vector2::new(tuple.0 as f32, tuple.1 as f32); | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment