JisolGame/JNFrame/Assets/HPJ Pathfinding Pro/Scripts/2.Simulation/2.Pathfinding/AStar.cs

using HPJ.Simulation.Enums;
using HPJ.Simulation.Map;
using HPJ.Simulation.Utilities;
using System;
using System.Collections.Generic;
using System.Linq;

namespace HPJ.Simulation.Pathing
{
    public class AStar : PathingCalculation
    {
        public static AStar Instance = null;

        public override void Initialize()
        {
            NavigationType = Enums.NavigationTypes.AStar;
            if (!PathingTypes.ContainsKey(NavigationType))
            {
                PathingTypes.Add(NavigationType, this);
            }
        }

        public override void CalculatePath(NavigationPath Path, Map.Map OccuringMap, out string Status, out bool Succeeded)
        {
            try
            {
                List<IntVector2> OpenList = new List<IntVector2>();
                HashSet<IntVector2> ClosedList = new HashSet<IntVector2>();
                Dictionary<IntVector2, AStarTileData> SortedList = new Dictionary<IntVector2, AStarTileData>();

                IntVector2 StartTile = Path.Info.PointA;
                IntVector2 EndTile = Path.Info.PointB;

                OpenList.Add(StartTile);
                AStarTileData StartingTileData = new AStarTileData(new IntVector2(-1, -1), CalculateDistanceCost(StartTile.x, StartTile.y, EndTile.x, EndTile.y));
                StartingTileData.GCost = 0;
                StartingTileData.FCost = StartingTileData.GCost + StartingTileData.HCost;
                SortedList.Add(StartTile, StartingTileData);

                int TentativeGCost = 0;
                IntVector2 CurrentTile = StartTile;
                int Iterations = 0;

                bool MaxIterationsMade = false;
                int MaxIterations = 3000;

                // Get BitMap
                BytePointMap BitMap = Path.HostMap.GetBytePointMap(Path);
                TileTypes[] PrefferedTiles = Path.Info.PrefferedTilesKey.StringToTilesArray();

                while (OpenList.Count > 0)
                {
                    // Give a safty range of 2 times the range in terms of iterations, if it cannot find a path by then, it's failed. can probably cannot be reached
                    if (Iterations > MaxIterations)
                    {
                        MaxIterationsMade = true;
                        break;
                    }
                    Iterations++;

                    int CurrentNodeIndex = GetLowestFCostTileIndex(OpenList, Path.HostMap, StartTile, EndTile, CurrentTile, PrefferedTiles);
                    CurrentTile = OpenList[CurrentNodeIndex];
                    if (EndTile == CurrentTile)
                    {
                        // Reached the end
                        //Debug.Log("Found the End - Should be Path Complete");
                        break;
                    }

                    // Remove current
                    OpenList.RemoveAt(CurrentNodeIndex);

                    ClosedList.Add(CurrentTile);

                    AStarTileData CurrentData = SortedList[CurrentTile];

                    for (int x = -1; x <= 1; x++)
                    {
                        for (int y = -1; y <= 1; y++)
                        {
                            if (x == 0 && y == 0)
                            {
                                continue;
                            }

                            IntVector2 neighbourPosition = new IntVector2(CurrentTile.x + x, CurrentTile.y + y);
                            if (BitMap.GetTileSpeed(neighbourPosition.x, neighbourPosition.y) == 0)
                            {
                                continue;
                            }

                            if (ClosedList.Contains(neighbourPosition))
                            {
                                continue;
                            }


                            // Get Corner Types, if its a wall, continue (DO NOT WALK THROUGH WALLS)
                            if (!BitMap.CornerCutting)
                            {
                                if (x != 0 && y != 0)
                                {
                                    IntVector2 cornerAdjacentPosition = new IntVector2(CurrentTile.x + x, CurrentTile.y);

                                    if (BitMap.Tiles[cornerAdjacentPosition.x, cornerAdjacentPosition.y] == 0)
                                    {
                                        continue;
                                    }

                                    cornerAdjacentPosition.x = CurrentTile.x;
                                    cornerAdjacentPosition.y = CurrentTile.y + y;

                                    if (BitMap.Tiles[cornerAdjacentPosition.x, cornerAdjacentPosition.y] == 0)
                                    {
                                        continue;
                                    }
                                }
                            }

                            AStarTileData NeighbourData;
                            if (!SortedList.TryGetValue(neighbourPosition, out NeighbourData))
                            {
                                NeighbourData = new AStarTileData(new IntVector2(-1, -1), CalculateDistanceCost(neighbourPosition.x, neighbourPosition.y, EndTile.x, EndTile.y));
                                SortedList.Add(neighbourPosition, NeighbourData);
                            }

                            TentativeGCost = CurrentData.GCost + CalculateDistanceCost(CurrentTile.x, CurrentTile.y, neighbourPosition.x, neighbourPosition.y);
                            if (TentativeGCost < NeighbourData.GCost)
                            {
                                //Debug.Log("Lower G Cost");
                                NeighbourData.RouteFromTileIndex = CurrentTile;
                                NeighbourData.GCost = TentativeGCost;
                                NeighbourData.FCost = NeighbourData.GCost + NeighbourData.HCost;
                                SortedList[neighbourPosition] = NeighbourData;

                                if (!OpenList.Contains(neighbourPosition))
                                {
                                    //Debug.Log("LEnter To OpenList");
                                    OpenList.Add(neighbourPosition);
                                }
                            }

                        }
                    }
                }

                AStarTileData EndTileData;
                if (MaxIterationsMade)
                {
                    Succeeded = false;
                    Status = $"Path is too far away, or cannot be reached --> Iteration Max Reached {MaxIterations}";
                }
                else if (!SortedList.TryGetValue(EndTile, out EndTileData) || EndTileData.RouteFromTileIndex.x == -1)
                {
                    // No Valid Path Found
                    Succeeded = false;
                    Status = "Open list emptied - But did NOT reach the End, Likely Trapped";
                }
                else
                {
                    Succeeded = true;
                    // Path should be valid
                    SortPath(Path, SortedList, EndTile);
                    Status = $"Found a Valid Path 'Iterations ({Iterations} / {MaxIterations})' -> 'List Counts (Open:{OpenList.Count} / Closed:{ClosedList.Count})'";
                }
            }
            catch (Exception ex)
            {
                Status = "Faled";
                OccuringMap.LogError($"{ex.Message}: {ex.Source}: {ex.StackTrace}");
                Succeeded = false;
            }
        }

        protected int GetLowestFCostTileIndex(List<IntVector2> OpenList, Map.Map OccuringMap, IntVector2 Start, IntVector2 End, IntVector2 CurrentTileIndex, TileTypes[] PrefferedTiles)
        {
            int LowestCostIndex = 0;
            if (PrefferedTiles.Contains(OccuringMap.GetTileType(CurrentTileIndex)))
            {
                // Try to stick to the group tile unless the tile is farther from the start AND closer to the end
                int CurrentStartScore = CalculateDistanceCost(CurrentTileIndex.x, CurrentTileIndex.y, Start.x, Start.y);
                int CurrentEndScore = CalculateDistanceCost(CurrentTileIndex.x, CurrentTileIndex.y, End.x, End.y);

                int LowestSameCostIndex = -1;
                TileTypes ZeroTileType = OccuringMap.GetTileType(OpenList[0].x, OpenList[0].y);
                byte TileSpeed = OccuringMap.GetTileSpeedData(ZeroTileType);
                int ZeroStartScore = CalculateDistanceCost(OpenList[0].x, OpenList[0].y, Start.x, Start.y);
                int ZeroEndScore = CalculateDistanceCost(OpenList[0].x, OpenList[0].y, End.x, End.y);
                int CurrentLowestCost = ZeroEndScore / TileSpeed;

                int CurrentSameLowestCost = int.MaxValue;
                if (PrefferedTiles.Contains(ZeroTileType) || (ZeroStartScore > CurrentStartScore && ZeroEndScore < CurrentEndScore))
                {
                    LowestSameCostIndex = 0;
                    CurrentSameLowestCost = CurrentLowestCost;
                }

                for (int i = 1; i < OpenList.Count; i++)
                {
                    TileTypes iTileType = OccuringMap.GetTileType(OpenList[i].x, OpenList[i].y);
                    TileSpeed = OccuringMap.GetTileSpeedData(iTileType);
                    int iStartScore = CalculateDistanceCost(OpenList[i].x, OpenList[i].y, Start.x, Start.y);
                    int iEndScore = CalculateDistanceCost(OpenList[i].x, OpenList[i].y, End.x, End.y);
                    int testCost = iEndScore / TileSpeed;
                    if (testCost < CurrentLowestCost)
                    {
                        CurrentLowestCost = testCost;
                        LowestCostIndex = i;
                    }

                    if (PrefferedTiles.Contains(iTileType) || (iStartScore > CurrentStartScore && iEndScore < CurrentEndScore))
                    {
                        if (testCost < CurrentSameLowestCost)
                        {
                            LowestSameCostIndex = i;
                            CurrentSameLowestCost = testCost;
                        }
                    }
                }

                if (LowestSameCostIndex != -1)
                {
                    return LowestSameCostIndex;
                }
            }
            else
            {
                // Typical AStar pathfinding
                int TileSpeed = OccuringMap.GetTileSpeedData(OccuringMap.GetTileType(OpenList[0].x, OpenList[0].y));
                int CurrentLowestCost = CalculateDistanceCost(OpenList[0].x, OpenList[0].y, End.x, End.y) / TileSpeed;
                for (int i = 1; i < OpenList.Count; i++)
                {
                    TileSpeed = OccuringMap.GetTileSpeedData(OccuringMap.GetTileType(OpenList[i].x, OpenList[i].y));
                    int testCost = CalculateDistanceCost(OpenList[i].x, OpenList[i].y, End.x, End.y) / TileSpeed;
                    if (testCost < CurrentLowestCost)
                    {
                        CurrentLowestCost = testCost;
                        LowestCostIndex = i;
                    }
                }
            }

            return LowestCostIndex;
        }

        protected int CalculateDistanceCost(int x1, int y1, int x2, int y2)
        {
            int X_Distance = x1 - x2;
            X_Distance = X_Distance < 0 ? -X_Distance : X_Distance;
            int Y_Distance = y1 - y2;
            Y_Distance = Y_Distance < 0 ? -Y_Distance : Y_Distance;
            int remaining = X_Distance - Y_Distance;
            remaining = remaining < 0 ? -remaining : remaining;
            if (Y_Distance < X_Distance)
            {
                return 14 * Y_Distance + 10 * remaining;
            }
            else
            {
                return 14 * X_Distance + 10 * remaining;
            }
        }

        protected void SortPath(NavigationPath Path, Dictionary<IntVector2, AStarTileData> SortedData, IntVector2 EndTileIndex)
        {
            AStarTileData CurrentTile = SortedData[EndTileIndex];
            int InsertIndex = Path.Path.Count;
            Path.Path.Insert(InsertIndex, EndTileIndex);

            // The Route From Index of the starting tile should be the only one with a (-1, -1) index
            while (CurrentTile.RouteFromTileIndex.x != -1)
            {
                Path.Path.Insert(InsertIndex, CurrentTile.RouteFromTileIndex);
                CurrentTile = SortedData[CurrentTile.RouteFromTileIndex];
            }
        }

        public struct AStarTileData
        {
            public IntVector2 RouteFromTileIndex;
            public int FCost;
            public int GCost;
            public int HCost;

            public AStarTileData(IntVector2 RouteFromTile, int EndCost)
            {
                RouteFromTileIndex = RouteFromTile;
                FCost = int.MaxValue;
                GCost = int.MaxValue;
                HCost = EndCost;
            }
        }
    }
}