public RegionLinkDijkstra(Map map, CellRect destination, IEnumerable <Region> startingRegions, IntVec3 target, TraverseParms parms, NewPathFinder.PawnPathCostSettings pathCosts)
        {
            this.map              = map;
            this.regionGrid       = RegionPathCostHeuristic.regionGridGet(map.regionGrid);
            this.traverseParms    = parms;
            this.targetCell       = target;
            this.rootCell         = destination.CenterCell;
            this.pathCostSettings = pathCosts;
            avoidGrid             = pathCosts.avoidGrid;
            area         = pathCosts.area;
            nodes_popped = 0;

            //if (DebugViewSettings.drawPaths && !NewPathFinder.disableDebugFlash)
            //{
            //	debugPathfinder = new NewPathFinder(map);
            //}

            //Init the starting region links from the start cell
            foreach (var region in startingRegions)
            {
                var minPathCost = RegionMedianPathCost(region);
                foreach (RegionLink current in region.links)
                {
                    var dist = RegionLinkDistance(rootCell, current, minPathCost);
                    if (distances.ContainsKey(current))
                    {
                        if (dist < distances[current])
                        {
                            linkTargetCells[current] = GetLinkTargetCell(rootCell, current);
                        }
                        dist = Math.Min(distances[current], dist);
                    }
                    else
                    {
                        linkTargetCells[current] = GetLinkTargetCell(rootCell, current);
                    }
                    distances[current] = dist;
                }
                foreach (var pair in PreciseRegionLinkDistances(region, destination))
                {
                    var current = pair.First;
                    var dist    = Math.Max(pair.Second, distances[current]);
                    distances[current] = dist;
                    queue.Push(new RegionLinkQueueEntry(region, current, dist, dist));
                }
            }
        }
예제 #2
0
        //The standard A* search algorithm has been modified to implement the bidirectional pathmax algorithm
        //("Inconsistent heuristics in theory and practice" Felner et al.) http://web.cs.du.edu/~sturtevant/papers/incnew.pdf
        internal PawnPath FindPathInner(IntVec3 start, LocalTargetInfo dest, TraverseParms traverseParms, PathEndMode peMode, HeuristicMode mode = HeuristicMode.Better)
        {
            //The initialization is largely unchanged from Core, aside from coding style in some spots
            #region initialization
            if (DebugSettings.pathThroughWalls)
            {
                traverseParms.mode = TraverseMode.PassAnything;
            }
            Pawn pawn            = traverseParms.pawn;
            bool canPassAnything = traverseParms.mode == TraverseMode.PassAnything;

            if (!ValidateFindPathParameters(pawn, start, dest, traverseParms, peMode, canPassAnything))
            {
                return(PawnPath.NotFound);
            }

            PfProfilerBeginSample(string.Concat("FindPath for ", pawn, " from ", start, " to ", dest, (!dest.HasThing) ? string.Empty : (" at " + dest.Cell)));
            destinationX = dest.Cell.x;
            destinationZ = dest.Cell.z;
            var cellIndices = this.map.cellIndices;
            curIndex         = cellIndices.CellToIndex(start);
            destinationIndex = cellIndices.CellToIndex(dest.Cell);
            if (!dest.HasThing || peMode == PathEndMode.OnCell)
            {
                destinationRect = CellRect.SingleCell(dest.Cell);
            }
            else
            {
                destinationRect = dest.Thing.OccupiedRect();
            }
            if (peMode == PathEndMode.Touch)
            {
                destinationRect = destinationRect.ExpandedBy(1);
            }
            destinationRect = destinationRect.ClipInsideMap(map);
            var regions = destinationRect.Cells.Select(c => this.map.regionGrid.GetValidRegionAt_NoRebuild(c)).Where(r => r != null);
            //Pretty sure this shouldn't be able to happen...
            if (mode == HeuristicMode.Better && !canPassAnything && !regions.Any())
            {
                mode = HeuristicMode.Vanilla;
                Log.Warning("Pathfinding destination not in region, must fall back to vanilla!");
            }
            destinationIsOneCell = (destinationRect.Width == 1 && destinationRect.Height == 1);
            this.pathGridDirect  = this.map.pathGrid.pathGrid;
            this.edificeGrid     = this.map.edificeGrid.InnerArray;
            statusOpenValue     += 2;
            statusClosedValue   += 2;
            if (statusClosedValue >= 65435)
            {
                ResetStatuses();
            }
            if (pawn?.RaceProps.Animal == true)
            {
                heuristicStrength = 30;
            }
            else
            {
                float lengthHorizontal = (start - dest.Cell).LengthHorizontal;
                heuristicStrength = (int)Math.Round(HeuristicStrengthHuman_DistanceCurve.Evaluate(lengthHorizontal));
            }
            closedCellCount = 0;
            openList.Clear();
            debug_pathFailMessaged   = false;
            debug_totalOpenListCount = 0;
            debug_openCellsPopped    = 0;

            PawnPathCostSettings pawnPathCosts = GetPawnPathCostSettings(traverseParms.pawn);

            moveTicksCardinal = pawnPathCosts.moveTicksCardinal;
            moveTicksDiagonal = pawnPathCosts.moveTicksDiagonal;

            //Where the magic happens
            RegionPathCostHeuristic regionCost = new RegionPathCostHeuristic(map, start, destinationRect, regions, traverseParms, pawnPathCosts);

            if (mode == HeuristicMode.Better)
            {
                if (canPassAnything)
                {
                    //Roughly preserves the Vanilla behavior of increasing path accuracy for shorter paths and slower pawns, though not as smoothly. Only applies to sappers.
                    heuristicStrength = Math.Max(1, (int)Math.Round(heuristicStrength / (float)moveTicksCardinal));
                }
                else
                {
                    var totalCostEst = (debug_totalHeuristicCostEstimate = regionCost.GetPathCostToRegion(curIndex)) + (moveTicksCardinal * 50);                     //Add constant cost so it tries harder on short paths
                    regionHeuristicWeightReal[1].x = totalCostEst / 2;
                    regionHeuristicWeightReal[2].x = totalCostEst;
                }
                regionHeuristicWeight = weightEnabled ? regionHeuristicWeightReal : regionHeuristicWeightNone;
            }
            else
            {
                regionHeuristicWeight = regionHeuristicWeightNone;
            }
            calcGrid[curIndex].knownCost     = 0;
            calcGrid[curIndex].heuristicCost = 0;
            calcGrid[curIndex].parentIndex   = curIndex;
            calcGrid[curIndex].status        = statusOpenValue;
            openList.Push(new CostNode(curIndex, 0));


            bool shouldCollideWithPawns = false;
            if (pawn != null)
            {
                shouldCollideWithPawns = PawnUtility.ShouldCollideWithPawns(pawn);
            }
            #endregion

            while (true)
            {
                PfProfilerBeginSample("Open cell pop");
                if (openList.Count <= 0)
                {
                    break;
                }
                debug_openCellsPopped++;
                var thisNode = openList.Pop();
                curIndex = thisNode.gridIndex;
                PfProfilerEndSample();
                PfProfilerBeginSample("Open cell");
                if (calcGrid[curIndex].status == statusClosedValue)
                {
                    PfProfilerEndSample();
                }
                else
                {
#if DEBUG
                    calcGrid[curIndex].timesPopped++;
#endif
                    curIntVec3 = cellIndices.IndexToCell(curIndex);
                    if (DebugViewSettings.drawPaths && !disableDebugFlash && debug_openCellsPopped < 20000)
                    {
                        //draw backpointer
                        var    arrow    = GetBackPointerArrow(cellIndices.IndexToCell(calcGrid[curIndex].parentIndex), curIntVec3);
                        string leading  = "";
                        string trailing = "";

#if DEBUG
                        switch (calcGrid[curIndex].timesPopped)
                        {
                        case 1:
                            trailing = "\n\n";                                     // $"\n\n\n{thisNode.totalCostEstimate}({calcGrid[curIndex].knownCost + calcGrid[curIndex].originalHeuristicCost})";
                            break;

                        case 2:
                            trailing = "\n"; break;

                        case 3: break;

                        case 4:
                            leading = "\n"; break;

                        default:
                            leading = "\n\n"; break;
                        }
#endif
                        DebugFlash(curIntVec3, calcGrid[curIndex].knownCost / 1500f, leading + calcGrid[curIndex].knownCost + " " + arrow + " " + debug_openCellsPopped + trailing);
                    }
                    if (curIndex == destinationIndex || (!destinationIsOneCell && destinationRect.Contains(curIntVec3)))
                    {
                        PfProfilerEndSample();
                        PfProfilerBeginSample("Finalize Path");
                        var ret = FinalizedPath(curIndex);
                        PfProfilerEndSample();
                        return(ret);
                    }
                    //With reopening closed nodes, this limit can be reached a lot more easily. I've left it as is because it gets users to report bad paths.
                    if (closedCellCount > 160000)
                    {
                        Log.Warning(string.Concat(pawn, " pathing from ", start, " to ", dest, " hit search limit of ", 160000, " cells."));
                        PfProfilerEndSample();
                        return(PawnPath.NotFound);
                    }
                    PfProfilerEndSample();
                    PfProfilerBeginSample("Neighbor consideration");
                    for (int i = 0; i < 8; i++)
                    {
                        neighIndexes[i] = -1;

                        neighX = (ushort)(curIntVec3.x + Directions[i]);
                        neighZ = (ushort)(curIntVec3.z + Directions[i + 8]);
                        if (neighX >= mapSizeX || neighZ >= mapSizeZ)
                        {
                            continue;
                        }

                        switch (i)
                        {
                        case 4:     //Northeast
                            if (!pathGridDirect.WalkableExtraFast(curIndex - mapSizeX) || !pathGridDirect.WalkableExtraFast(curIndex + 1))
                            {
                                continue;
                            }
                            break;

                        case 5:     //Southeast
                            if (!pathGridDirect.WalkableExtraFast(curIndex + mapSizeX) || !pathGridDirect.WalkableExtraFast(curIndex + 1))
                            {
                                continue;
                            }
                            break;

                        case 6:     //Southwest
                            if (!pathGridDirect.WalkableExtraFast(curIndex + mapSizeX) || !pathGridDirect.WalkableExtraFast(curIndex - 1))
                            {
                                continue;
                            }
                            break;

                        case 7:     //Northwest
                            if (!pathGridDirect.WalkableExtraFast(curIndex - mapSizeX) || !pathGridDirect.WalkableExtraFast(curIndex - 1))
                            {
                                continue;
                            }
                            break;
                        }

                        neighIndex = cellIndices.CellToIndex(neighX, neighZ);

                        if ((calcGrid[neighIndex].status != statusClosedValue) && (calcGrid[neighIndex].status != statusOpenValue))
                        {
                            if (10000 <= (calcGrid[neighIndex].perceivedPathCost = GetTotalPerceivedPathCost(traverseParms, canPassAnything, shouldCollideWithPawns, pawn, pawnPathCosts)))
                            {
                                continue;
                            }
#if DEBUG
                            calcGrid[neighIndex].timesPopped = 0;
#endif
                            #region heuristic
                            PfProfilerBeginSample("Heuristic");
                            switch (mode)
                            {
                            case HeuristicMode.Vanilla:
                                h = heuristicStrength * (Math.Abs(neighX - destinationX) + Math.Abs(neighZ - destinationZ));
                                break;

                            case HeuristicMode.AdmissableOctile:
                            {
                                var dx = Math.Abs(neighX - destinationX);
                                var dy = Math.Abs(neighZ - destinationZ);
                                h = moveTicksCardinal * (dx + dy) + (moveTicksDiagonal - 2 * moveTicksCardinal) * Math.Min(dx, dy);
                            }
                            break;

                            case HeuristicMode.Better:
                                if (canPassAnything)
                                {
                                    var dx = Math.Abs(neighX - destinationX);
                                    var dy = Math.Abs(neighZ - destinationZ);
                                    h = heuristicStrength * (moveTicksCardinal * (dx + dy) + (moveTicksDiagonal - 2 * moveTicksCardinal) * Math.Min(dx, dy));
                                }
                                else
                                {
                                    h = regionCost.GetPathCostToRegion(neighIndex);
                                }
                                break;
                            }
                            calcGrid[neighIndex].heuristicCost = h;
#if PATHMAX
                            calcGrid[neighIndex].originalHeuristicCost = h;
#endif
                            PfProfilerEndSample();
                            #endregion
                        }


                        if (calcGrid[neighIndex].perceivedPathCost < 10000)
                        {
                            neighIndexes[i] = neighIndex;
                        }
                        if (mode == HeuristicMode.Better && (calcGrid[neighIndex].status == statusOpenValue &&
                                                             Math.Max(i > 3 ? (int)(calcGrid[curIndex].perceivedPathCost * diagonalPerceivedCostWeight) + moveTicksDiagonal : calcGrid[curIndex].perceivedPathCost + moveTicksCardinal, 1) + calcGrid[neighIndex].knownCost < calcGrid[curIndex].knownCost))
                        {
                            calcGrid[curIndex].parentIndex = neighIndex;
                            calcGrid[curIndex].knownCost   = Math.Max(i > 3 ? (int)(calcGrid[curIndex].perceivedPathCost * diagonalPerceivedCostWeight) + moveTicksDiagonal : calcGrid[curIndex].perceivedPathCost + moveTicksCardinal, 1) + calcGrid[neighIndex].knownCost;
                        }
                    }
                    #region BPMX Best H
#if PATHMAX
                    PfProfilerBeginSample("BPMX Best H");
                    int bestH = calcGrid[curIndex].heuristicCost;
                    if (mode == HeuristicMode.Better && pathmaxEnabled)
                    {
                        for (int i = 0; i < 8; i++)
                        {
                            neighIndex = neighIndexes[i];
                            if (neighIndex < 0)
                            {
                                continue;
                            }
                            bestH = Math.Max(bestH, calcGrid[neighIndex].heuristicCost - (calcGrid[curIndex].perceivedPathCost + (i > 3 ? moveTicksDiagonal : moveTicksCardinal)));
                        }
                    }

                    //Pathmax Rule 3: set the current node heuristic to the best value of all connected nodes
                    calcGrid[curIndex].heuristicCost = bestH;
                    PfProfilerEndSample();
#endif
                    #endregion

                    #region Updating open list
                    for (int i = 0; i < 8; i++)
                    {
                        neighIndex = neighIndexes[i];
                        if (neighIndex < 0)
                        {
                            continue;
                        }
                        if (calcGrid[neighIndex].status == statusClosedValue && (canPassAnything || mode != HeuristicMode.Better))
                        {
                            continue;
                        }

                        //When path costs are significantly higher than move costs (e.g. snowy ice, or outside of allowed areas),
                        //small differences in the weighted heuristic overwhelm the added cost of diagonal movement, so nodes
                        //can often be visited in unnecessary zig-zags, causing lots of nodes to be reopened later, and weird looking
                        //paths if they are not revisited. Weighting the diagonal path cost slightly counteracts this behavior, and
                        //should result in natural looking paths when it does cause suboptimal behavior
                        var thisDirEdgeCost = (i > 3 ? (int)(calcGrid[neighIndex].perceivedPathCost * diagonalPerceivedCostWeight) + moveTicksDiagonal : calcGrid[neighIndex].perceivedPathCost + moveTicksCardinal);

                        //var thisDirEdgeCost = calcGrid[neighIndex].perceivedPathCost + (i > 3 ? moveTicksDiagonal : moveTicksCardinal);
                        //Some mods can result in negative path costs. That works well enough with Vanilla, since it won't revisit closed nodes, but when we do, it's an infinite loop.
                        thisDirEdgeCost     = (ushort)Math.Max(thisDirEdgeCost, 1);
                        neighCostThroughCur = thisDirEdgeCost + calcGrid[curIndex].knownCost;
#if PATHMAX
                        //Pathmax Rule 1
                        int nodeH = (mode == HeuristicMode.Better && pathmaxEnabled) ? Math.Max(calcGrid[neighIndex].heuristicCost, bestH - thisDirEdgeCost) : calcGrid[neighIndex].heuristicCost;
#endif
                        if (calcGrid[neighIndex].status == statusClosedValue || calcGrid[neighIndex].status == statusOpenValue)
                        {
#if PATHMAX
                            bool needsUpdate = false;
#endif
                            int minReopenGain = 0;
                            if (calcGrid[neighIndex].status == statusOpenValue)
                            {
#if PATHMAX
                                needsUpdate = nodeH > calcGrid[neighIndex].heuristicCost;
#endif
                            }
                            else
                            {                                   //Don't reopen closed nodes if the path cost difference isn't large enough to justify it; otherwise there can be cascades of revisiting the same nodes over and over for tiny path improvements each time
                                                                //Increasing the threshold as more cells get reopened further helps prevent cascades
                                minReopenGain = moveTicksCardinal + closedCellsReopened / 5;
                                if (pawnPathCosts.area?[neighIndex] == false)
                                {
                                    minReopenGain *= 10;
                                }
                            }
#if PATHMAX
                            calcGrid[neighIndex].heuristicCost = nodeH;
#endif
                            if (!(neighCostThroughCur + minReopenGain < calcGrid[neighIndex].knownCost))
                            {
#if PATHMAX
                                if (needsUpdate)                                 //if the heuristic cost was increased for an open node, we need to adjust its spot in the queue
                                {
                                    var neighCell = cellIndices.IndexToCell(neighIndex);
                                    var edgeCost  = Math.Max(calcGrid[neighIndex].parentX != neighCell.x && calcGrid[neighIndex].parentZ != neighCell.z ? (int)(calcGrid[neighIndex].perceivedPathCost * diagonalPercievedCostWeight) + moveTicksDiagonal : calcGrid[neighIndex].perceivedPathCost + moveTicksCardinal, 1);
                                    openList.PushOrUpdate(new CostNode(neighIndex, calcGrid[neighIndex].knownCost - edgeCost
                                                                       + (int)Math.Ceiling((edgeCost + nodeH) * regionHeuristicWeight.Evaluate(calcGrid[neighIndex].knownCost))));
                                }
#endif
                                continue;
                            }
                            if (calcGrid[neighIndex].status == statusClosedValue)
                            {
                                closedCellsReopened++;
                            }
                        }
                        //else
                        //{
                        //	DebugFlash(cellIndices.IndexToCell(neighIndex), 0.2f, $"\n\n{neighCostThroughCur} | {nodeH}\n{calcGrid[curIndex].knownCost + (int)Math.Ceiling((nodeH + thisDirEdgeCost) * regionHeuristicWeight.Evaluate(calcGrid[curIndex].knownCost))}");
                        //}

                        calcGrid[neighIndex].parentIndex = curIndex;
                        calcGrid[neighIndex].knownCost   = neighCostThroughCur;
                        calcGrid[neighIndex].status      = statusOpenValue;
#if PATHMAX
                        calcGrid[neighIndex].heuristicCost = nodeH;
#endif
                        PfProfilerBeginSample("Push Open");
                        openList.PushOrUpdate(new CostNode(neighIndex, calcGrid[curIndex].knownCost
                                                           + (int)Math.Ceiling((calcGrid[neighIndex].heuristicCost + thisDirEdgeCost) * regionHeuristicWeight.Evaluate(calcGrid[curIndex].knownCost))));
                        debug_totalOpenListCount++;
                        PfProfilerEndSample();
                    }
                    #endregion
                    PfProfilerEndSample();
                    closedCellCount++;
                    calcGrid[curIndex].status = statusClosedValue;
                }
            }
            if (!debug_pathFailMessaged)
            {
                string text  = pawn?.CurJob?.ToString() ?? "null";
                string text2 = pawn?.Faction?.ToString() ?? "null";
                Log.Warning(string.Concat(pawn, " pathing from ", start, " to ", dest, " ran out of cells to process.\nJob:", text, "\nFaction: ", text2));
                debug_pathFailMessaged = true;
            }
            PfProfilerEndSample();
            return(PawnPath.NotFound);
        }