Refactor pathfinding.

src/a_star.h

@@ -0,0 +1,274 @@
+#pragma once
+#ifndef CATA_SRC_A_STAR_H
+#define CATA_SRC_A_STAR_H
+
+#include <algorithm>
+#include <limits>
+#include <optional>
+#include <queue>
+#include <tuple>
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+template <typename Node, typename Cost = int, typename VisitedSet = std::unordered_set<Node>, typename BestPathMap = std::unordered_map<Node, std::pair<Cost, Node>>>
+          class AStarState
+{
+public:
+    void reset( const Node &start, Cost cost );
+
+    std::optional<Node> get_next( Cost max );
+
+    template <typename StateCostFn, typename TransitionCostFn, typename HeuristicFn, typename NeighborsFn>
+    void generate_neighbors( const Node &current, StateCostFn &&s_cost_fn, TransitionCostFn &&t_cost_fn,
+                             HeuristicFn &&heuristic_fn, NeighborsFn &&neighbors_fn );
+
+    bool has_path_to( const Node &end ) const;
+
+    Cost path_cost( const Node &end ) const;
+
+    std::vector<Node> path_to( const Node &end ) const;
+
+private:
+    struct FirstElementGreaterThan {
+        template <typename T, typename... Ts>
+        bool operator()( const std::tuple<T, Ts...> &lhs, const std::tuple<T, Ts...> &rhs ) const {
+            return std::get<0>( lhs ) > std::get<0>( rhs );
+        }
+    };
+
+    using FrontierNode = std::tuple<Cost, Node>;
+    using FrontierQueue =
+        std::priority_queue<FrontierNode, std::vector<FrontierNode>, FirstElementGreaterThan>;
+
+    VisitedSet visited_;
+    BestPathMap best_paths_;
+    FrontierQueue frontier_;
+};
+
+template <typename Node, typename Cost = int, typename VisitedSet = std::unordered_set<Node>, typename BestStateMap = std::unordered_map<Node, std::pair<Cost, Node>>>
+          class AStarPathfinder
+{
+public:
+    template <typename StateCostFn, typename TransitionCostFn, typename HeuristicFn, typename NeighborsFn>
+    std::vector<Node> find_path( Cost max, const Node &from, const Node &to,
+                                 StateCostFn &&s_cost_fn, TransitionCostFn &&t_cost_fn, HeuristicFn &&heuristic_fn,
+                                 NeighborsFn &&neighbors_fn );
+
+private:
+    AStarState<Node, Cost, VisitedSet, BestStateMap> state_;
+};
+
+template <typename Node, typename Cost = int, typename VisitedSet = std::unordered_set<Node>, typename BestStateMap = std::unordered_map<Node, std::pair<Cost, Node>>>
+          class BidirectionalAStarPathfinder
+{
+public:
+    template <typename StateCostFn, typename TransitionCostFn, typename HeuristicFn, typename NeighborsFn>
+    std::vector<Node> find_path( Cost max, const Node &from, const Node &to,
+                                 StateCostFn &&s_cost_fn, TransitionCostFn &&t_cost_fn, HeuristicFn &&heuristic_fn,
+                                 NeighborsFn &&neighbors_fn );
+
+private:
+    AStarState<Node, Cost, VisitedSet, BestStateMap> forward_;
+    AStarState<Node, Cost, VisitedSet, BestStateMap> backward_;
+};
+
+// Implementation Details
+
+template <typename Node, typename Cost, typename VisitedSet, typename BestPathMap>
+void AStarState<Node, Cost, VisitedSet, BestPathMap>::reset( const Node &start, Cost cost )
+{
+    visited_.clear();
+    best_paths_.clear();
+
+    // priority_queue doesn't have a clear method, so we cannot reuse it, and it may
+    // get quite large, so we explicitly create the underlying container and reserve
+    // space beforehand.
+    std::vector<FrontierNode> queue_data;
+    queue_data.reserve( 400 );
+    frontier_ = FrontierQueue( FirstElementGreaterThan(), std::move( queue_data ) );
+
+    best_paths_.try_emplace( start, 0, start );
+    frontier_.emplace( cost, start );
+}
+
+template <typename Node, typename Cost, typename VisitedSet, typename BestPathMap>
+bool AStarState<Node, Cost, VisitedSet, BestPathMap>::has_path_to( const Node &end ) const
+{
+    return best_paths_.count( end );
+}
+
+template <typename Node, typename Cost, typename VisitedSet, typename BestPathMap>
+Cost AStarState<Node, Cost, VisitedSet, BestPathMap>::path_cost( const Node &end ) const
+{
+    return best_paths_.at( end ).first;
+}
+
+template <typename Node, typename Cost, typename VisitedSet, typename BestPathMap>
+std::vector<Node> AStarState<Node, Cost, VisitedSet, BestPathMap>::path_to( const Node &end ) const
+{
+    std::vector<Node> result;
+    if( has_path_to( end ) ) {
+        Node current = end;
+        while( best_paths_.at( current ).first != 0 ) {
+            result.push_back( current );
+            current = best_paths_.at( current ).second;
+        }
+        std::reverse( result.begin(), result.end() );
+    }
+    return result;
+}
+
+template <typename Node, typename Cost, typename VisitedSet, typename BestPathMap>
+std::optional<Node> AStarState<Node, Cost, VisitedSet, BestPathMap>::get_next( Cost max )
+{
+    while( !frontier_.empty() ) {
+        auto [cost, current] = frontier_.top();
+        frontier_.pop();
+
+        if( cost >= max ) {
+            return std::nullopt;
+        }
+
+        if( const auto& [_, inserted] = visited_.emplace( current ); !inserted ) {
+            continue;
+        }
+        return current;
+    }
+    return std::nullopt;
+}
+
+template <typename Node, typename Cost, typename VisitedSet, typename BestPathMap>
+template <typename StateCostFn, typename TransitionCostFn, typename HeuristicFn, typename NeighborsFn>
+void AStarState<Node, Cost, VisitedSet, BestPathMap>::generate_neighbors(
+    const Node &current, StateCostFn &&s_cost_fn, TransitionCostFn &&t_cost_fn,
+    HeuristicFn &&heuristic_fn,
+    NeighborsFn &&neighbors_fn )
+{
+    // Can't use structured bindings here due to a defect in Clang 10.
+    const std::pair<Cost, Node> &best_path = best_paths_[current];
+    const Cost current_cost = best_path.first;
+    const Node &current_parent = best_path.second;
+    neighbors_fn( current_parent, current, [this, &s_cost_fn, &t_cost_fn, &heuristic_fn, &current,
+          current_cost]( const Node & neighbour ) {
+        if( visited_.count( neighbour ) ) {
+            return;
+        }
+        if( const std::optional<Cost> s_cost = s_cost_fn( neighbour ) ) {
+            if( const std::optional<Cost> t_cost = t_cost_fn( current, neighbour ) ) {
+                const auto& [iter, _] = best_paths_.try_emplace( neighbour, std::numeric_limits<Cost>::max(),
+                                        Node() );
+                auto& [best_cost, parent] = *iter;
+                const Cost new_cost = current_cost + *s_cost + *t_cost;
+                if( new_cost < best_cost ) {
+                    best_cost = new_cost;
+                    parent = current;
+                    const Cost estimated_cost = new_cost + heuristic_fn( neighbour );
+                    frontier_.emplace( estimated_cost, neighbour );
+                }
+            }
+        } else {
+            visited_.emplace( neighbour );
+        }
+    } );
+}
+
+template <typename Node, typename Cost, typename VisitedSet, typename BestStateMap>
+template <typename StateCostFn, typename TransitionCostFn, typename HeuristicFn, typename NeighborsFn>
+std::vector<Node> AStarPathfinder<Node, Cost, VisitedSet, BestStateMap>::find_path(
+    Cost max_cost, const Node &from, const Node &to, StateCostFn &&s_cost_fn,
+    TransitionCostFn &&t_cost_fn,
+    HeuristicFn &&heuristic_fn, NeighborsFn &&neighbors_fn )
+{
+    if( !s_cost_fn( from ) || !s_cost_fn( to ) ) {
+        return {};
+    }
+
+    state_.reset( from, heuristic_fn( from, to ) );
+    while( const std::optional<Node> current = state_.get_next( max_cost ) ) {
+        if( *current == to ) {
+            return state_.path_to( to );
+        }
+        state_.generate_neighbors( *current, s_cost_fn, t_cost_fn, [&heuristic_fn,
+        to]( const Node & from ) {
+            return heuristic_fn( from, to );
+        }, neighbors_fn );
+    }
+    return {};
+}
+
+
+template <typename Node, typename Cost, typename VisitedSet, typename BestStateMap>
+template <typename StateCostFn, typename TransitionCostFn, typename HeuristicFn, typename NeighborsFn>
+std::vector<Node> BidirectionalAStarPathfinder<Node, Cost, VisitedSet, BestStateMap>::find_path(
+    Cost max_cost, const Node &from, const Node &to, StateCostFn &&s_cost_fn,
+    TransitionCostFn &&t_cost_fn,
+    HeuristicFn &&heuristic_fn, NeighborsFn &&neighbors_fn )
+{
+    if( !s_cost_fn( from ) || !s_cost_fn( to ) ) {
+        return {};
+    }
+
+    // The full cost is not used since that would result in paths that are up to 2x longer than
+    // intended. Half the cost cannot be used, since there is no guarantee that both searches
+    // proceed at the same pace. 2/3rds the cost is a fine balance between the two, and has the
+    // effect of the worst case still visiting less states than normal A*.
+    const Cost partial_max_cost = 2 * max_cost / 3;
+    forward_.reset( from, heuristic_fn( from, to ) );
+    backward_.reset( to, heuristic_fn( to, from ) );
+    for( ;; ) {
+        const std::optional<Node> f_current_state = forward_.get_next( partial_max_cost );
+        if( !f_current_state ) {
+            break;
+        }
+        const std::optional<Node> b_current_state = backward_.get_next( partial_max_cost );
+        if( !b_current_state ) {
+            break;
+        }
+
+        bool f_links = backward_.has_path_to( *f_current_state );
+        bool b_links = forward_.has_path_to( *b_current_state );
+
+        if( f_links && b_links ) {
+            const Cost f_cost = forward_.path_cost( *f_current_state ) + backward_.path_cost(
+                                    *f_current_state );
+            const Cost b_cost = forward_.path_cost( *b_current_state ) + backward_.path_cost(
+                                    *b_current_state );
+            if( b_cost < f_cost ) {
+                f_links = false;
+            } else {
+                b_links = false;
+            }
+        }
+
+        if( f_links || b_links ) {
+            const Node &midpoint = f_links ? *f_current_state : *b_current_state;
+            std::vector<Node> forward_path = forward_.path_to( midpoint );
+            std::vector<Node> backward_path = backward_.path_to( midpoint );
+            if( backward_path.empty() ) {
+                return forward_path;
+            }
+            backward_path.pop_back();
+            std::for_each( backward_path.rbegin(), backward_path.rend(), [&forward_path]( const Node & node ) {
+                forward_path.push_back( node );
+            } );
+            forward_path.push_back( to );
+            return forward_path;
+        }
+
+        forward_.generate_neighbors( *f_current_state, s_cost_fn, t_cost_fn, [&heuristic_fn,
+        &to]( const Node & from ) {
+            return heuristic_fn( from, to );
+        }, neighbors_fn );
+        backward_.generate_neighbors( *b_current_state, s_cost_fn, [&t_cost_fn]( const Node & from,
+        const Node & to ) {
+            return t_cost_fn( to, from );
+        }, [&heuristic_fn, &from]( const Node & to ) {
+            return heuristic_fn( to, from );
+        }, neighbors_fn );
+    }
+    return {};
+}
+
+#endif // CATA_SRC_A_STAR_H

src/activity_actor.cpp

@@ -7188,8 +7188,7 @@ void unload_loot_activity_actor::do_turn( player_activity &act, Character &you )
                     return;
                 }
                 std::vector<tripoint_bub_ms> route;
-                route = here.route( you.pos_bub(), src_loc, you.get_pathfinding_settings(),
-                                    you.get_path_avoid() );
+                route = here.route( you.pos_bub(), src_loc, you.get_pathfinding_settings() );
                 if( route.empty() ) {
                     // can't get there, can't do anything, skip it
                     continue;
@@ -7225,8 +7224,7 @@ void unload_loot_activity_actor::do_turn( player_activity &act, Character &you )
                 // get either direct route or route to nearest adjacent tile if
                 // source tile is impassable
                 if( here.passable( src_loc ) ) {
-                    route = here.route( you.pos_bub(), src_loc, you.get_pathfinding_settings(),
-                                        you.get_path_avoid() );
+                    route = here.route( you.pos_bub(), src_loc, you.get_pathfinding_settings() );
                 } else {
                     // impassable source tile (locker etc.),
                     // get route to nearest adjacent tile instead

src/activity_handlers.cpp

@@ -2911,8 +2911,7 @@ void activity_handlers::travel_do_turn( player_activity *act, Character *you )
             }
         }
         const std::vector<tripoint_bub_ms> route_to =
-            here.route( you->pos_bub(), centre_sub, you->get_pathfinding_settings(),
-                        you->get_path_avoid() );
+            here.route( you->pos_bub(), centre_sub, you->get_pathfinding_settings() );
         if( !route_to.empty() ) {
             const activity_id act_travel = ACT_TRAVELLING;
             you->set_destination( route_to, player_activity( act_travel ) );
@@ -3598,8 +3597,7 @@ static void perform_zone_activity_turn(
         const tripoint_bub_ms &tile_loc = here.bub_from_abs( tile );
 
         std::vector<tripoint_bub_ms> route =
-            here.route( you->pos_bub(), tile_loc, you->get_pathfinding_settings(),
-                        you->get_path_avoid() );
+            here.route( you->pos_bub(), tile_loc, you->get_pathfinding_settings() );
         if( route.size() > 1 ) {
             route.pop_back();
 

src/activity_item_handling.cpp

@@ -674,7 +674,7 @@ std::vector<tripoint_bub_ms> route_adjacent( const Character &you, const tripoin
     const auto &avoid = you.get_path_avoid();
     for( const tripoint_bub_ms &tp : sorted ) {
         std::vector<tripoint_bub_ms> route =
-            here.route( you.pos_bub(), tp, you.get_pathfinding_settings(), avoid );
+            here.route( you.pos_bub(), tp, you.get_pathfinding_settings() );
 
         if( !route.empty() ) {
             return route;
@@ -752,7 +752,7 @@ static std::vector<tripoint_bub_ms> route_best_workbench(
     }
     for( const tripoint_bub_ms &tp : sorted ) {
         std::vector<tripoint_bub_ms> route =
-            here.route( you.pos_bub(), tp, you.get_pathfinding_settings(), avoid );
+            here.route( you.pos_bub(), tp, you.get_pathfinding_settings() );
 
         if( !route.empty() ) {
             return route;
@@ -2055,8 +2055,7 @@ void activity_on_turn_move_loot( player_activity &act, Character &you )
                     return;
                 }
                 std::vector<tripoint_bub_ms> route;
-                route = here.route( you.pos_bub(), src_loc, you.get_pathfinding_settings(),
-                                    you.get_path_avoid() );
+                route = here.route( you.pos_bub(), src_loc, you.get_pathfinding_settings() );
                 if( route.empty() ) {
                     // can't get there, can't do anything, skip it
                     continue;
@@ -2112,8 +2111,7 @@ void activity_on_turn_move_loot( player_activity &act, Character &you )
                 // get either direct route or route to nearest adjacent tile if
                 // source tile is impassable
                 if( here.passable( src_loc ) ) {
-                    route = here.route( you.pos_bub(), src_loc, you.get_pathfinding_settings(),
-                                        you.get_path_avoid() );
+                    route = here.route( you.pos_bub(), src_loc, you.get_pathfinding_settings() );
                 } else {
                     // impassable source tile (locker etc.),
                     // get route to nearest adjacent tile instead

src/character.h

@@ -3264,8 +3264,8 @@ class Character : public Creature, public visitable
         /** Returns the player's modified base movement cost */
         int run_cost( int base_cost, bool diag = false ) const;
 
-        const pathfinding_settings &get_pathfinding_settings() const override;
-        std::function<bool( const tripoint & )> get_path_avoid() const override;
+        virtual const pathfinding_settings &get_pathfinding_settings() const;
+        virtual std::function<bool( const tripoint & )> get_path_avoid() const;
         /**
          * Get all hostile creatures currently visible to this player.
          */

src/coordinates.h

@@ -710,6 +710,13 @@ inline int octile_dist( const coords::coord_point<Point, Origin, Scale, LhsInBou
     return octile_dist( loc1.raw(), loc2.raw(), multiplier );
 }
 
+template<typename Point, coords::origin Origin, coords::scale Scale>
+inline float octile_dist_exact( const coords::coord_point<Point, Origin, Scale> &loc1,
+                                const coords::coord_point<Point, Origin, Scale> &loc2 )
+{
+    return octile_dist_exact( loc1.raw(), loc2.raw() );
+}
+
 template<typename Point, coords::origin Origin, coords::scale Scale, bool LhsInBounds, bool RhsInBounds>
 direction direction_from( const coords::coord_point<Point, Origin, Scale, LhsInBounds> &loc1,
                           const coords::coord_point<Point, Origin, Scale, RhsInBounds> &loc2 )

src/creature.h

@@ -945,11 +945,6 @@ class Creature : public viewer
 
         virtual units::mass weight_capacity() const;
 
-        /** Returns settings for pathfinding. */
-        virtual const pathfinding_settings &get_pathfinding_settings() const = 0;
-        /** Returns a set of points we do not want to path through. */
-        virtual std::function<bool( const tripoint & )> get_path_avoid() const = 0;
-
         bool underwater;
         void draw( const catacurses::window &w, const point_bub_ms &origin, bool inverted ) const;
         // TODO: Get rid of the untyped overload

src/debug_menu.cpp

@@ -3427,8 +3427,7 @@ static void set_automove()
 
     // TODO: fix point types
     auto rt = get_map().route( player_character.pos_bub(), tripoint_bub_ms( *dest ),
-                               player_character.get_pathfinding_settings(),
-                               player_character.get_path_avoid() );
+                               player_character.get_pathfinding_settings() );
     if( !rt.empty() ) {
         player_character.set_destination( rt );
     } else {