Improve A* Pathfinding Efficiency by Addressing Final Segment Penalty

libs/s25main/Ware.cpp

@@ -338,27 +338,25 @@ unsigned Ware::CheckNewGoalForLostWare(const noBaseBuilding& newgoal) const
 Ware::RouteParams Ware::CalcPathToGoal(const noBaseBuilding& newgoal) const
 {
     RTTR_Assert(location);
-    unsigned length = 0xFFFFFFFF;
+    unsigned length;
     RoadPathDirection possibledir = world->FindPathForWareOnRoads(*location, newgoal, &length);
     if(possibledir != RoadPathDirection::None) // there is a valid path to the goal? -> ordered!
     {
         // in case the ware is right in front of the goal building the ware has to be moved away 1 flag and then back
         // because non-warehouses cannot just carry in new wares they need a helper to do this
         if(possibledir == RoadPathDirection::NorthWest && newgoal.GetFlagPos() == location->GetPos())
         {
+            // Not executed for road from flag to the warehouse as that is handled directly by the warehouse
+            RTTR_Assert(!BuildingProperties::IsWareHouse(newgoal.GetBuildingType()));
             for(const auto dir : helpers::EnumRange<Direction>{})
             {
+                // Bounce of in this direction
                 if(dir != Direction::NorthWest && location->GetRoute(dir))
-                {
-                    possibledir = toRoadPathDirection(dir);
-                    break;
-                }
+                    return {1, toRoadPathDirection(dir)};
             }
-            if(possibledir == RoadPathDirection::NorthWest) // got no other route from the flag -> impossible
-                return {0xFFFFFFFF, RoadPathDirection::None};
+            // got no other route from the flag -> impossible
+            return {0xFFFFFFFF, RoadPathDirection::None};
         }
-        // at this point there either is a road to the goal
-        // or we are at the flag of the goal and have a road to a different flag to bounce off of to get to the goal
         return {length, possibledir};
     }
     return {0xFFFFFFFF, RoadPathDirection::None};

libs/s25main/buildings/nobHarborBuilding.cpp

@@ -839,8 +839,8 @@ std::vector<nobHarborBuilding::ShipConnection> nobHarborBuilding::GetShipConnect
     {
         ShipConnection sc;
         sc.dest = harbor_building;
-        // Als Kantengewicht nehmen wir die doppelte Entfernung (evtl muss ja das Schiff erst kommen)
-        // plus einer Kopfpauschale (Ein/Ausladen usw. dauert ja alles)
+        // Use twice the distance as cost (ship might need to arrive first) and a fixed value to represent
+        // loading&unloading
         sc.way_costs = 2 * world->CalcHarborDistance(GetHarborPosID(), harbor_building->GetHarborPosID()) + 10;
         connections.push_back(sc);
     }

libs/s25main/nodeObjs/noFlag.cpp

@@ -228,13 +228,14 @@ unsigned noFlag::GetNumWaresForRoad(const Direction dir) const
     return helpers::count_if(wares, [roadDir](const auto& ware) { return ware->GetNextDir() == roadDir; });
 }
 
-/**
- *  Gibt Wegstrafpunkte für das Pathfinden für Waren, die in eine bestimmte
- *  Richtung noch transportiert werden müssen.
- */
 unsigned noFlag::GetPunishmentPoints(const Direction dir) const
 {
-    // Waren zählen, die in diese Richtung transportiert werden müssen
+    constexpr auto PATHFINDING_PENALTY_CARRIER_ARRIVING = 50;
+    constexpr auto PATHFINDING_PENALTY_NO_CARRIER = 500;
+    // This must be the same as "NO_CARRIER" for replay compatibility
+    // TODO(Replay): Move to `way_costs` in nobHarborBuilding::GetShipConnections
+    constexpr auto PATHFINDING_PENALTY_START_SHIPPING = 500;
+    // 2 Points per ware as carrier has to walk to other point and back for each ware
     unsigned points = GetNumWaresForRoad(dir) * 2;
 
     const RoadSegment* routeInDir = GetRoute(dir);
@@ -243,9 +244,20 @@ unsigned noFlag::GetPunishmentPoints(const Direction dir) const
     {
         // normal carrier has been ordered from the warehouse but has not yet arrived and no donkey
         if(humanCarrier->GetCarrierState() == CarrierState::FigureWork && !routeInDir->hasCarrier(1))
-            points += 50;
+            points += PATHFINDING_PENALTY_CARRIER_ARRIVING;
     } else if(!routeInDir->hasCarrier(1))
-        points += 500; // No carrier at all -> Large penalty
+    {
+        // Routes are either between flags or from a flag to a building, see the ctor of noBaseBuilding
+        const bool isBuildingEntry = (dir == Direction::NorthWest) && (routeInDir->GetF2()->GetGOT() != GO_Type::Flag);
+        // For entering a building no carrier is required
+        // Only roads to buildings considered by path finding are those to harbors
+        if(isBuildingEntry)
+        {
+            RTTR_Assert(routeInDir->GetF2()->GetGOT() == GO_Type::NobHarborbuilding);
+            points += PATHFINDING_PENALTY_START_SHIPPING;
+        } else
+            points += PATHFINDING_PENALTY_NO_CARRIER;
+    }
 
     return points;
 }

libs/s25main/nodeObjs/noFlag.h

@@ -46,8 +46,7 @@ class noFlag : public noRoadNode
     std::unique_ptr<Ware> SelectWare(Direction roadDir, bool swap_wares, const noFigure* carrier);
     /// Prüft, ob es Waren gibt, die auf den Weg in Richtung dir getragen werden müssen.
     unsigned GetNumWaresForRoad(Direction dir) const;
-    /// Gibt Wegstrafpunkte für das Pathfinden für Waren, die in eine bestimmte Richtung noch transportiert werden
-    /// müssen.
+    /// Penalty for transporting a ware in a specific direction
     unsigned GetPunishmentPoints(Direction dir) const override;
     /// Zerstört evtl. vorhandenes Gebäude bzw. Baustelle vor der Flagge.
     void DestroyAttachedBuilding();

libs/s25main/pathfinding/RoadPathFinder.cpp

@@ -20,8 +20,7 @@ struct RoadNodeComperatorGreater
     {
         if(lhs->estimate == rhs->estimate)
         {
-            // Wenn die Wegkosten gleich sind, vergleichen wir die Koordinaten, da wir für std::set eine streng
-            // monoton steigende Folge brauchen
+            // Use a tie-breaker to get strict ordering
             return (lhs->GetObjId() > rhs->GetObjId());
         }
 
@@ -100,7 +99,9 @@ struct And : private T_Func1, private T_Func2
 };
 } // namespace SegmentConstraints
 
-/// Wegfinden ( A* ), O(v lg v) --> Wegfindung auf Stra�en
+/// Path finding on roads using A* O(n lg n)
+/// \tparam T_AdditionalCosts Cost for each road segment but the one to the goal building
+/// \tparam T_SegmentConstraints Predicate whether a road is allowed
 template<class T_AdditionalCosts, class T_SegmentConstraints>
 bool RoadPathFinder::FindPathImpl(const noRoadNode& start, const noRoadNode& goal, const unsigned max,
                                   const T_AdditionalCosts addCosts, const T_SegmentConstraints isSegmentAllowed,
@@ -124,9 +125,12 @@ bool RoadPathFinder::FindPathImpl(const noRoadNode& start, const noRoadNode& goa
         return true;
     }
 
-    // increase current_visit_on_roads, so we don't have to clear the visited-states at every run
-    currentVisit++;
+    // If the goal is a flag (unlikely) we have no goal building
+    // TODO(Replay): Change RoadPathFinder::FindPath to target flag instead of building for wares
+    const noRoadNode* goalBld = (goal.GetGOT() == GO_Type::Flag) ? nullptr : &goal;
 
+    // Use a counter for the visited-states so we don't have to reset them on every invocation
+    currentVisit++;
     // if the counter reaches its maximum, tidy up
     if(currentVisit == std::numeric_limits<unsigned>::max())
     {
@@ -163,19 +167,20 @@ bool RoadPathFinder::FindPathImpl(const noRoadNode& start, const noRoadNode& goa
             if(length)
                 *length = best.cost;
 
-            // Backtrace to get the last node that is not the start node (has a prev node) --> Next node from start on
-            // path
-            const noRoadNode* firstNode = &best;
-            while(firstNode->prev != &start)
+            // Backtrack to get the last node that is not the start node (has a prev node)
+            // --> Next node from start on path
+            if(firstDir || firstNodePos)
             {
-                firstNode = firstNode->prev;
-            }
+                const noRoadNode* firstNode = &best;
+                while(firstNode->prev != &start)
+                    firstNode = firstNode->prev;
 
-            if(firstDir)
-                *firstDir = firstNode->dir_;
+                if(firstDir)
+                    *firstDir = firstNode->dir_;
 
-            if(firstNodePos)
-                *firstNodePos = firstNode->GetPos();
+                if(firstNodePos)
+                    *firstNodePos = firstNode->GetPos();
+            }
 
             // Done, path found
             return true;
@@ -184,7 +189,7 @@ bool RoadPathFinder::FindPathImpl(const noRoadNode& start, const noRoadNode& goa
         const helpers::EnumArray<RoadSegment*, Direction> routes = best.getRoutes();
         const noRoadNode* prevNode = best.prev;
 
-        // Nachbarflagge bzw. Wege in allen 6 Richtungen verfolgen
+        // Check paths in all directions
         for(const auto dir : helpers::EnumRange<Direction>{})
         {
             const auto* route = routes[dir];
@@ -209,12 +214,11 @@ bool RoadPathFinder::FindPathImpl(const noRoadNode& start, const noRoadNode& goa
                     continue;
             }
 
-            // evtl verboten?
+            // Check if route is forbidden
             if(!isSegmentAllowed(*route))
                 continue;
 
-            unsigned cost = best.cost + route->GetLength();
-            cost += addCosts(best, dir);
+            const unsigned cost = best.cost + route->GetLength() + (neighbour != goalBld ? addCosts(best, dir) : 0);
 
             if(cost > max)
                 continue;
@@ -283,18 +287,19 @@ bool RoadPathFinder::FindPathImpl(const noRoadNode& start, const noRoadNode& goa
         }
     }
 
-    // Liste leer und kein Ziel erreicht --> kein Weg
+    // Queue is empty without reaching the goal --> No allowed, existing path
     return false;
 }
 
 bool RoadPathFinder::FindPath(const noRoadNode& start, const noRoadNode& goal, const bool wareMode, const unsigned max,
                               const RoadSegment* const forbidden, unsigned* const length,
                               RoadPathDirection* const firstDir, MapPoint* const firstNodePos)
 {
-    RTTR_Assert(length || firstDir || firstNodePos); // If none of them is set use the \ref PathExist function!
+    RTTR_Assert_Msg(length || firstDir || firstNodePos, "Use PathExists instead!");
 
     if(wareMode)
     {
+        // TODO(Replay): Change to target flag instead of its attached building
         if(forbidden)
             return FindPathImpl(start, goal, max, AdditonalCosts::Carrier(),
                                 SegmentConstraints::AvoidSegment(forbidden), length, firstDir, firstNodePos);
@@ -319,6 +324,8 @@ bool RoadPathFinder::PathExists(const noRoadNode& start, const noRoadNode& goal,
 {
     if(allowWaterRoads)
     {
+        // TODO(Replay): Change to target flag instead of its attached building.
+        // Likely combine with RoadPathFinder::FindPath
         if(forbidden)
             return FindPathImpl(start, goal, max, AdditonalCosts::None(), SegmentConstraints::AvoidSegment(forbidden));
         else