From e6595fe47c7195728bd6a2820d182378244aec3e Mon Sep 17 00:00:00 2001 From: DeltaEpsilon Date: Sun, 9 Feb 2025 06:00:18 +0500 Subject: [PATCH 1/2] Pathfinding alpha --- .gitignore | 3 + src/pathfinding_dijikstra.cpp | 414 ++++++++++++++++++++++++++++++++++ src/pathfinding_dijikstra.h | 254 +++++++++++++++++++++ 3 files changed, 671 insertions(+) create mode 100644 src/pathfinding_dijikstra.cpp create mode 100644 src/pathfinding_dijikstra.h diff --git a/.gitignore b/.gitignore index 25e3ffc1e7b9..ec2917a71da2 100644 --- a/.gitignore +++ b/.gitignore @@ -227,3 +227,6 @@ test/Output test/.lit_test_times.txt affected_files.txt clang-tidy-trace + +CMakeFiles/ +CMakeCache.txt diff --git a/src/pathfinding_dijikstra.cpp b/src/pathfinding_dijikstra.cpp new file mode 100644 index 000000000000..d2fc4c9c9ac4 --- /dev/null +++ b/src/pathfinding_dijikstra.cpp @@ -0,0 +1,414 @@ +#include "pathfinding_dijikstra.h" + +#include + +#include "cursesdef.h" +#include "input.h" +#include "output.h" +#include "ui_manager.h" + +#include "game.h" +#include "map.h" +#include "submap.h" +#include "trap.h" +#include "veh_type.h" +#include "vehicle.h" +#include "vehicle_part.h" +#include "vpart_position.h" + +std::optional> DijikstraPathfinding::get_route( const tripoint from, + const RouteSettings route_settings ) +{ + if( !g->m.inbounds( from ) ) { + return std::nullopt; + } + + if( this->is_disconnected( from ) ) { + return std::nullopt; + } + + if( this->is_unvisited( from ) ) { + if( !this->expand_up_to( from ) ) { + return std::nullopt; + } + } + + bool can_fly = !std::isinf( this->settings.fly_cost ); // TODO: add to pathfinding settings + + std::vector dirs = { + tripoint( -1, -1, 0 ), + tripoint( -1, 0, 0 ), + tripoint( -1, 1, 0 ), + tripoint( 0, -1, 0 ), + tripoint( 0, 1, 0 ), + tripoint( 1, -1, 0 ), + tripoint( 1, 0, 0 ), + tripoint( 1, 1, 0 ), + }; + + if( can_fly ) { + dirs.push_back( tripoint( 0, 0, -1 ) ); + dirs.push_back( tripoint( 0, 0, 1 ) ); + } + + auto &map = g->m; + + const float objective_distance = rl_dist_exact( from, this->dest ); + + tripoint cur_point = from; + float cur_cost = this->at( cur_point ); + + const float max_g_cost = route_settings.max_path_g_coefficient * objective_distance; + if( cur_cost > max_g_cost ) { + return std::nullopt; + } + + const float max_steps = route_settings.max_path_length_coefficient * objective_distance; + + std::vector> candidates; + std::vector result; + result.push_back( from ); + + while( cur_point != this->dest ) { + // Is there a portal to here? [such as stairs] + // if( this->portals.contains( cur_point ) ) { + // const GraphPortal *portal = &this->portals[cur_point]; + // if( portal->from_cost < cur_cost ) { + // candidates.emplace_back( portal->from, portal->from_cost ); + // } + // }; + + for( tripoint &dir : dirs ) { + if( result.size() - 1 > max_steps ) { + // Path too long + return std::nullopt; + } + tripoint next_point = cur_point + dir; + const bool is_in_bounds = map.inbounds( next_point ); + if( !is_in_bounds ) { + continue; + } + + const bool has_valid_g_cost = !this->is_unvisited( next_point ) && + !this->is_disconnected( next_point ); + const bool is_not_forbidden = !this->forbidden_moves.contains( + std::make_pair( cur_point, next_point ) ); + + const bool is_in_search_radius = route_settings.is_in_search_radius( from, next_point, this->dest ); + + // Search cone stuff + bool is_in_search_cone = route_settings.is_in_search_cone( from, next_point, this->dest ); + + bool is_valid = has_valid_g_cost && + is_not_forbidden && + is_in_search_radius && + is_in_search_cone; + if( !is_valid ) { + continue; + }; + + float cost = this->at( next_point ); + if( cost < cur_cost ) { + candidates.emplace_back( cost, next_point ); + } + } + + // This should not be likely to happen, but... + if( candidates.size() == 0 ) { + return std::nullopt; + } + + std::sort( candidates.begin(), candidates.end(), []( auto & p1, auto & p2 ) { + return p1.first < p2.first; + } ); + + auto selected_pair = &candidates[route_settings.rank_weighted_rng( candidates.size() )]; + + result.push_back( selected_pair->second ); + cur_point = selected_pair->second; + cur_cost = selected_pair->first; + + candidates.clear(); + } + + g->draw_line( from, result ); + + return result; +} + +bool DijikstraPathfinding::expand_up_to( const tripoint &start, + const RouteSettings &route_settings ) +{ + std::vector dirs = { + tripoint( -1, -1, 0 ), + tripoint( -1, 0, 0 ), + tripoint( -1, 1, 0 ), + tripoint( 0, -1, 0 ), + tripoint( 0, 1, 0 ), + tripoint( 1, -1, 0 ), + tripoint( 1, 0, 0 ), + tripoint( 1, 1, 0 ), + }; + + bool can_fly = !std::isinf( this->settings.fly_cost ); + + if( can_fly ) { + dirs.push_back( tripoint( 0, 0, -1 ) ); + dirs.push_back( tripoint( 0, 0, 1 ) ); + } + + map &map = g->m; + + while( this->frontier.size() > 0 ) { + const tripoint next_point = this->frontier.top().second; + this->frontier.pop(); + + int cur_vehicle_part; + const vehicle *next_vehicle = map.veh_at_internal( next_point, cur_vehicle_part ); + + int i = -1; + + for( tripoint &dir : dirs ) { + i++; + + bool is_diag = false; + bool is_vertical = false; + switch( i ) { + case 0: + case 2: + case 5: + case 7: + is_diag = true; + break; + case 8: + case 9: + is_vertical = true; + break; + } + + // It's cur_point because we're working backwards from destination + tripoint cur_point = next_point + dir; + + { + const bool is_in_bounds = map.inbounds( cur_point ); + if( !is_in_bounds ) { + continue; + } + } + + { + const bool is_unvisited = this->is_unvisited( cur_point ); + if( !is_unvisited ) { + continue; + } + } + + { + bool is_in_search_radius = route_settings.is_in_search_radius( start, cur_point, this->dest ); + bool is_in_search_cone = route_settings.is_in_search_cone( start, cur_point, this->dest ); + const bool is_valid_to_check = is_in_search_radius && is_in_search_cone; + + if( !is_valid_to_check ) { + continue; + }; + } + + { + const bool is_valid_to_step_into = map.valid_move( cur_point, next_point, + this->settings.bash_strength > 0, can_fly, true ); + if( !is_valid_to_step_into ) { + DijikstraPathfinding::forbidden_moves.insert( std::make_pair( cur_point, next_point ) ); + continue; + } + } + + int new_vehicle_part; + const vehicle *cur_vehicle = map.veh_at_internal( cur_point, new_vehicle_part ); + + { + const bool is_valid_to_step_into = + cur_vehicle == nullptr ? + true : + cur_vehicle->allowed_move( cur_vehicle->tripoint_to_mount( cur_point ), + cur_vehicle->tripoint_to_mount( next_point ) ); + + const bool is_valid_to_step_out_of = + next_vehicle == nullptr ? + true : + next_vehicle->allowed_move( next_vehicle->tripoint_to_mount( cur_point ), + next_vehicle->tripoint_to_mount( next_point ) ); + + if( !( is_valid_to_step_into && is_valid_to_step_out_of ) ) { + DijikstraPathfinding::forbidden_moves.insert( std::make_pair( cur_point, next_point ) ); + continue; + } + } + + const maptile &new_tile = map.maptile_at( cur_point ); + const auto &terrain = new_tile.get_ter_t(); + const auto &furniture = new_tile.get_furn_t(); + + const float cur_cost = this->at( next_point ); + + float new_cost = 0.; + const int move_cost = map.move_cost( cur_point ); + + new_cost += is_diag ? 0.75 * move_cost : 0.5 * move_cost; + + // First, check for trivial cost modifiers + const bool care_about_mobs = this->settings.mob_presence_penalty > 0; + const bool is_rough = move_cost > 2; + const bool is_sharp = terrain.has_flag( TFLAG_SHARP ); + const trap maybe_tile_trap = new_tile.get_trap_t(); + const trap maybe_ter_trap = terrain.trap.obj(); + const bool is_trap = ( + ( !maybe_tile_trap.is_null() && !maybe_tile_trap.is_benign() ) || + ( !maybe_ter_trap.is_null() && !maybe_ter_trap.is_benign() ) + ); + + + new_cost += this->settings.rough_terrain_cost * is_rough; + new_cost += this->settings.sharp_terrain_cost * is_sharp; + new_cost += this->settings.trap_cost * is_trap; + + if( care_about_mobs ) { + new_cost += this->settings.mob_presence_penalty * ( g->critter_at( cur_point, false ) != nullptr ); + } + + const bool is_passable = move_cost != 0; + // Calculate the cost for if the tile is impassable + if( !is_passable ) { + const bool is_climbable = terrain.has_flag( TFLAG_CLIMBABLE ); + const bool can_climb = !std::isinf( this->settings.climb_cost ); + + const bool is_door = !!terrain.open || !!furniture.open; + const bool can_open_doors = !std::isinf( this->settings.door_open_cost ); + + const bool can_bash = this->settings.bash_strength > 0; + const bool is_ledge = is_trap && map.has_zlevels() && terrain.has_flag( TFLAG_NO_FLOOR ); + + const bool does_z_level_change = ( terrain.has_flag( TFLAG_GOES_DOWN ) || + terrain.has_flag( TFLAG_GOES_UP ) || + terrain.has_flag( TFLAG_RAMP ) || + terrain.has_flag( TFLAG_RAMP_UP ) || + terrain.has_flag( TFLAG_RAMP_DOWN ) ); + + float obstacle_bypass_cost = NAN; + if( cur_vehicle != nullptr ) { + // Do processing for possible vehicle first + const auto vpobst = vpart_position( const_cast( *cur_vehicle ), + new_vehicle_part ).obstacle_at_part(); + const int obstacle_part = vpobst ? vpobst->part_index() : -1; + + if( obstacle_part >= 0 ) { + int dummy; + const bool part_is_door = cur_vehicle->part_flag( obstacle_part, VPFLAG_OPENABLE ); + const bool part_opens_from_inside = cur_vehicle->part_flag( obstacle_part, "OPENCLOSE_INSIDE" ); + const bool is_cur_point_inside = map.veh_at_internal( cur_point, dummy ) == cur_vehicle; + const bool valid_to_open = part_is_door && ( part_opens_from_inside ? is_cur_point_inside : true ); + + if( can_open_doors && valid_to_open ) { + obstacle_bypass_cost = this->settings.door_open_cost; + } else if( can_bash ) { + float hp = cur_vehicle->cpart( obstacle_part ).hp(); + obstacle_bypass_cost = this->settings.bash_cost * hp / this->settings.bash_strength; + } else { + // Nothing can be done here. Don't bother with other checks since vehicles take priority. + obstacle_bypass_cost = INFINITY; + } + } + } + + if( std::isnan( obstacle_bypass_cost ) && is_climbable && can_climb ) { + obstacle_bypass_cost = this->settings.climb_cost; + } + if( std::isnan( obstacle_bypass_cost ) && is_door && can_open_doors ) { + // Doors that can only be open from the inside + const bool door_opens_from_inside = terrain.has_flag( "OPENCLOSE_INSIDE" ) || + furniture.has_flag( "OPENCLOSE_INSIDE" ); + const bool is_cur_point_inside = !map.is_outside( cur_point ); + const bool valid_to_open = door_opens_from_inside ? is_cur_point_inside : true; + if( valid_to_open ) { + obstacle_bypass_cost = this->settings.door_open_cost; + } + } + if( std::isnan( obstacle_bypass_cost ) && can_bash ) { + // Time to consider bashing the obstacle + const int rating = map.bash_rating( this->settings.bash_strength, cur_point ); + if( rating > 1 ) { + obstacle_bypass_cost = ( 10. / rating ) * this->settings.bash_cost; + } else if( rating == 1 ) { + obstacle_bypass_cost = 100.0 * + this->settings.bash_cost; // Extremely unlikely to take this route unless no other choice is present + } + } + if( std::isnan( obstacle_bypass_cost ) ) { + // We can do nothing anymore, close the tile + obstacle_bypass_cost = INFINITY; + } + + new_cost += obstacle_bypass_cost; + } + + // If this fails, somebody had a negative cost somewhere + assert( new_cost >= 0 ); + + // And now, set the score and expand frontier if non-disconnected + this->at( cur_point ) = cur_cost + new_cost; + + if( !std::isinf( new_cost ) ) { + float h = rl_dist_exact( cur_point, start ); + this->frontier.emplace( this->at( cur_point ) + route_settings.h_coeff * h, + cur_point ); + } + + if( cur_point == start ) { + // We have reached the target + // We need to remove the heurestic bias from frontier for next pathfinding + if( route_settings.h_coeff > 0 ) { + std::priority_queue, pair_greater_cmp_first> new_frontier; + while( frontier.size() > 0 ) { + const val_pair pair = frontier.top(); + const float h = rl_dist_exact( pair.second, start ); + new_frontier.emplace( pair.first - h, pair.second ); + frontier.pop(); + } + + this->frontier = new_frontier; + } + return !std::isinf( new_cost ); + } + } + } + + // The rest of the map must be disconnected, so propagate that + for( float &val : this->cost_array ) { + if( std::isnan( val ) ) { + val = INFINITY; + } + } + return false; +} + +std::vector DijikstraPathfinding::route( tripoint from, tripoint to, + const PathfindingSettings path_settings, + const RouteSettings route_settings ) +{ + const map &map = g->m; + + map.clip_to_bounds( from ); + map.clip_to_bounds( to ); + + for( auto &map : DijikstraPathfinding::maps ) { + if( map.dest == to && map.settings == path_settings ) { + auto result = map.get_route( from, route_settings ); + return result.has_value() ? *result : std::vector(); + } + } + + DijikstraPathfinding new_map( to, path_settings ); + DijikstraPathfinding::maps.push_back( std::move( new_map ) ); + + auto result = new_map.get_route( from, route_settings ); + return result.has_value() ? *result : std::vector(); +}; diff --git a/src/pathfinding_dijikstra.h b/src/pathfinding_dijikstra.h new file mode 100644 index 000000000000..065e00fef618 --- /dev/null +++ b/src/pathfinding_dijikstra.h @@ -0,0 +1,254 @@ +#include +#include +#include +#include + +#include "cata_utility.h" +#include "game_constants.h" +#include "map.h" +#include "point.h" +#include "rng.h" + +const size_t DIJIKSTRA_ARRAY_SIZE = OVERMAP_LAYERS * MAPSIZE_Y * MAPSIZE_X; +typedef std::pair val_pair; + +// A struct defining abilities of the actor and how to respond to various terrain features +struct PathfindingSettings { + // Our bash strength + int bash_strength = 0; + // Even if we can bash, multiply time needed to do it by this + // >1 to make bashes less attractive, <1 to make them more attractive. Do not use negative values. + float bash_cost = 3.0; + + // Cost of climbing terrain. INFINITY if we can't + float climb_cost = INFINITY; + + // Cost of moving through a trap tile, INFINITY to avoid completely + float trap_cost = 0.; + + // Cost of opening a door. INFINITY to never open doors, otherwise 2 to open and then move in. + float door_open_cost = INFINITY; + + // Extra penalty for moving through rough terrain + float rough_terrain_cost = 0.; + + // Extra penalty for moving through sharp terrain + float sharp_terrain_cost = 0.; + + // Cost of moving up/down from any position. INFINITY if we can't fly. + float fly_cost = INFINITY; + + // Cost of climbing stairs up and down. INFINITY if we can't. + float stair_movement_cost = INFINITY; + + // If a mob is in the way currently, add this extra cost. INFINITY to always path around other critters. + float mob_presence_penalty = 0.; + + PathfindingSettings() = default; + PathfindingSettings( PathfindingSettings const & ) = default; + + bool operator==( const PathfindingSettings &rhs ) const = default; +}; + +// A struct defining various coefficient used when creating/calculating a path from a dijikstra map +// or determining tiles a valid path can cross where valid means at each point in the path the cost function decreases +struct RouteSettings { + // How directed the pathfinding is. A value of 1.0 makes pathfinding equivalent to A*, 0.0 is raw Dijikstra; + // this adjusts precision, high values will converge quicker, but produce a possibly less than shortest path. + float h_coeff = 1.0; + + /* + ```plain + ----- + / |r'\ + / | \ + / --- \ + | / |r\ | + | | | | | + |--S--E--|--| + | | | | | + | t \ | / | + \ --- / + \ | / + \ | / + ----- + ``` + S -- `start` + E -- `end` + t -- candidate `t`ile + r -- `r`adius (distance from `S` to `E`) + r' -- search `r`adius (`r` * `search_radius_coeff`) + Test if `t` is inside circle of radius r'. + */ + float search_radius_coeff = INFINITY; + + // Test if `pos` is in the circle of radius distance from `start` to `end` by `search_radius_coeff` centered at `end` + bool is_in_search_radius( const tripoint start, const tripoint pos, const tripoint end ) const { + const float objective_distance = rl_dist_exact( start, end ); + const float search_radius = objective_distance * this->search_radius_coeff; + const float distance_to_objective = rl_dist_exact( pos, end ); + + return distance_to_objective <= search_radius; + } + + /* + ```plain + t + / . + / . + / . + / . + /a . + S----------E + ``` + S -- `start` + E -- `end` + t -- candidate `t`ile + a -- `a`ngle of pSE + `search_cone_angle` <= `a` <= `search_cone_angle`? + */ + float search_cone_angle = 180.0; + + // Test if `pos` is in the cone of `search_cone_angle` projected from `start` to `end` + bool is_in_search_cone( const tripoint start, const tripoint pos, const tripoint end ) const { + assert( 0.0 <= this->search_cone_angle ); + + if( this->search_cone_angle < 180. ) { + return true; + } + + const units::angle max_cone_angle = units::from_degrees( this->search_cone_angle ); + + const point objective_delta = end.xy() - start.xy(); + const units::angle objective_angle = units::atan2( objective_delta.y, objective_delta.x ); + + const point conic_delta = pos.xy() - start.xy(); + const units::angle conic_angle = units::atan2( conic_delta.y, conic_delta.x ); + + const units::angle deviation = conic_angle - objective_angle; + + return -max_cone_angle <= deviation && deviation <= max_cone_angle; + } + + // If multiple tiles are valid for a path, where valid means at each point in the path the cost function decreases, + // we may want to select one of them randomly to decongest routes across multiple pathfinds. + // To do so, we'll rank up all the tiles based on their cost with first one being the most optimal path and last one being a least optimal path. + // This coefficient determines how weights are distributed among these paths + // -1 -- always choose the longest path (why would you though...) + // 0 -- choose any tile with no bias + // 1 -- always choose the shortest path + float alpha = 1.0; + + unsigned int rank_weighted_rng( const unsigned int n ) const { + assert(-1. <= this->alpha && this->alpha <= 1. ); + + // Trivial cases + if( this->alpha >= 1. ) { + return 0; + } + + if( this->alpha <= -1. ) { + return n - 1; + } + + if( this->alpha == 0. ) { + return rng( 0, n - 1 ); + } + + const float r = rng_float( 0.0, 1.0 ); + return static_cast( n * powf( r, ( 1. + this->alpha ) / ( 1. - this->alpha ) ) ); + } + + // If our chosen path is longer than this coefficient mulplitied by the minimum amount of tiles needed to + // go from start tile to destination in a straight line, then the path is considered not found + float max_path_length_coefficient = INFINITY; + + // Limit our search only to paths whose g value is less than this coefficient multiplied by the distance + // between start and destination + float max_path_g_coefficient = INFINITY; + + RouteSettings() = default; + RouteSettings( RouteSettings const & ) = default; +}; + +struct GraphPortal { + const tripoint from; + const float from_cost; + + GraphPortal( const tripoint from, const float from_cost ) : from( from ), from_cost( from_cost ) {}; +}; + +class DijikstraPathfinding +{ + private: + // `dest`ination of this map + const tripoint dest; + // `settings` which were used to spawn this map + const PathfindingSettings settings; + + // 1D array containing our map + // NAN for unvisited tiles + // INF for disconnected tiles + float cost_array[DIJIKSTRA_ARRAY_SIZE]; + + // We don't want to calculate dijikstra of the whole map every time, so we store wave frontier to proceed from later if needed + std::priority_queue, pair_greater_cmp_first> frontier; + + // See DijikstraPathfinding::route + std::optional> get_route( const tripoint origin, + const RouteSettings route_settings = RouteSettings() ); + + // Moves in this map that are between adjacent non-disconnected tiles that may NOT be taken + std::set> forbidden_moves; + + // Continue expanding the dijikstra map until we reach `origin` or nothing remains of the frontier. Returns whether a route is present. + bool expand_up_to( const tripoint &origin, const RouteSettings &route_settings = RouteSettings() ); + + bool is_unvisited( const tripoint &p ) { + return std::isnan( this->at( p ) ); + } + + bool is_disconnected( const tripoint &p ) { + return std::isinf( this->at( p ) ); + } + + // A directed graph edge representing connected non-adjacent tiles + // (multi-level ledges dropping down, stairs, literal distant portals etc). + std::map portals; + + // Get cost `at` `p` + float &at( const tripoint &p ) { + assert( p.x >= 0 && p.y >= 0 && p.x < MAPSIZE_X && p.y < MAPSIZE_Y ); + + // Row major ordering + size_t index = 0; + index += p.x; + index += p.y * MAPSIZE_X; + index += ( p.z + OVERMAP_DEPTH ) * MAPSIZE_Y * MAPSIZE_X; + + assert( index < DIJIKSTRA_ARRAY_SIZE ); + + return this->cost_array[index]; + }; + + // Global state: memoized dijikstra maps. Clear every game turn. + inline static std::vector maps{}; + public: + DijikstraPathfinding( const tripoint dest, const PathfindingSettings settings ) + : dest( dest ), settings( settings ) { + std::fill_n( this->cost_array, std::size( this->cost_array ), NAN ); + this->at( dest ) = 0.; + this->frontier.emplace( 0., dest ); + }; + + // get `route` from `from` to `to` if available in accordance to `route_settings` while `path_settings` defines our capabilities, otherwise empty vector. + // Route will include `from` and `to`. + static std::vector route( const tripoint from, const tripoint to, + const PathfindingSettings path_settings = PathfindingSettings(), + const RouteSettings route_settings = RouteSettings() ); + + static void clear_paths() { + DijikstraPathfinding::maps.clear(); + } + +}; From 84a057dd6817a669f165cba903e9989b809e1902 Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Sun, 9 Feb 2025 01:35:45 +0000 Subject: [PATCH 2/2] style(autofix.ci): automated formatting --- src/pathfinding_dijikstra.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pathfinding_dijikstra.h b/src/pathfinding_dijikstra.h index 065e00fef618..e8226915aba5 100644 --- a/src/pathfinding_dijikstra.h +++ b/src/pathfinding_dijikstra.h @@ -140,7 +140,7 @@ struct RouteSettings { float alpha = 1.0; unsigned int rank_weighted_rng( const unsigned int n ) const { - assert(-1. <= this->alpha && this->alpha <= 1. ); + assert( -1. <= this->alpha && this->alpha <= 1. ); // Trivial cases if( this->alpha >= 1. ) {