Scriptum/Assets/_/Stuff/GridPathfinding/GridPathfinding.cs

/* 
    ------------------- Code Monkey -------------------

    Thank you for downloading this package
    I hope you find it useful in your projects
    If you have any questions let me know
    Cheers!

               unitycodemonkey.com
    --------------------------------------------------
 */
 
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;

namespace GridPathfindingSystem {

    public class GridPathfinding {

        public static GridPathfinding instance;

        public const int WALL_WEIGHT = 56000;

        public enum UnitMovementCallbackType {
            Simple,
        }

        //private List<PathNode> openList;
        private BinaryTree binaryTree;
        private int openListCount;
        private PathNode[][] mapNodes;
        private int widthMax, heightMax;

        private float nodeSize;
        private Vector3 worldOrigin;

        private bool foundTarget;
        private int movementCost = 10;
        private float timer;

        public delegate void OnPathCallback(List<PathNode> path, MapPos finalPos);
        public delegate void OnVoidDelegate();
        public OnVoidDelegate callbacks;

        public GridPathfinding(Vector3 worldLowerLeft, Vector3 worldUpperRight, float nodeSize) {
            instance = this;
            worldOrigin = worldLowerLeft;
            this.nodeSize = nodeSize;

            float worldWidth = worldUpperRight.x - worldLowerLeft.x;
            float worldHeight = worldUpperRight.y - worldLowerLeft.y;

            int mapWidth = Mathf.RoundToInt(worldWidth / nodeSize);
            int mapHeight = Mathf.RoundToInt(worldHeight / nodeSize);

            mapNodes = new PathNode[mapWidth][];
            for (int i = 0; i < mapWidth; i++) {
                mapNodes[i] = new PathNode[mapHeight];
            }
            widthMax = mapWidth;
            heightMax = mapHeight;

            Initialize(mapWidth, mapHeight);
        }

        public GridPathfinding(int mapWidth, int mapHeight, float nodeSize, Vector3 worldOrigin) {//, Texture2D map) {
            this.nodeSize = nodeSize;
            this.worldOrigin = worldOrigin;

            mapNodes = new PathNode[mapWidth][];
            for (int i = 0; i < mapWidth; i++) {
                mapNodes[i] = new PathNode[mapHeight];
            }
            widthMax = mapWidth;
            heightMax = mapHeight;

            Initialize(mapWidth, mapHeight);
        }

        public void RaycastWalkable() {
            for (int i = 0; i < widthMax; i++) {
                for (int j = 0; j < heightMax; j++) {
                    Vector3 nodeWorldPosition = mapNodes[i][j].GetWorldVector(worldOrigin, nodeSize);
                    RaycastHit2D raycastHit = Physics2D.Raycast(nodeWorldPosition, Vector2.zero, 0f);
                    if (raycastHit.collider != null) {
                        mapNodes[i][j].SetWalkable(false);
                    }
                }
            }
        }
        
        public void ModifySize(int modifyX, int modifyY, int newPathNodeWeight) {
            if (modifyX == 0 && modifyY == 0) {
                return;
            }

            int newWidth = widthMax + modifyX;
            int newHeight = heightMax + modifyY;
        
            PathNode[][] newMapNodes = new PathNode[newWidth][];
            for (int i = 0; i < newWidth; i++) {
                newMapNodes[i] = new PathNode[newHeight];
                for (int j = 0; j < newHeight; j++) {
                    if (i < mapNodes.Length && j < mapNodes[0].Length) {
                        newMapNodes[i][j] = mapNodes[i][j];
                    } else {
                        newMapNodes[i][j] = new PathNode(i, j);
                        newMapNodes[i][j].SetWeight(newPathNodeWeight);
                    }
                }
            }
            widthMax = newWidth;
            heightMax = newHeight;

            mapNodes = newMapNodes;

            UpdateNodeConnections();

            //if (OnSizeModified != null) OnSizeModified(this, EventArgs.Empty);
        }

        public Vector3 GetWorldOffset() {
            return worldOrigin;
        }

        public float GetNodeSize() {
            return nodeSize;
        }

        public void SetWalkable(int x, int y, bool walkable) {
            mapNodes[x][y].SetWalkable(walkable);
        }

        public void SetAllWalkable(bool walkable) {
            for (int x = 0; x < mapNodes.Length; x++) {
                for (int y = 0; y < mapNodes[x].Length; y++) {
                    mapNodes[x][y].SetWalkable(walkable);
                }
            }
        }

        public void SetWeight(int x, int y, int weight) {
            mapNodes[x][y].SetWeight(weight);
        }

        public void SetAllWeight(int weight) {
            for (int x = 0; x < mapNodes.Length; x++) {
                for (int y = 0; y < mapNodes[x].Length; y++) {
                    mapNodes[x][y].SetWeight(weight);
                }
            }
        }

        public int GetMapWidth() {
            return widthMax;
        }

        public int GetMapHeight() {
            return heightMax;
        }

        public void Initialize(int mapWidth, int mapHeight) {
            // Creates PathNodes
            for (int x = 0; x < mapNodes.Length; x++) {
                for (int y = 0; y < mapNodes[x].Length; y++) {
                    mapNodes[x][y] = new PathNode(x, y);
                }
            }
            UpdateNodeConnections();
        }

        private void UpdateNodeConnections() {
            for (int x = 0; x < mapNodes.Length; x++) {
                for (int y = 0; y < mapNodes[x].Length; y++) {
                    if (y < mapNodes[x].Length - 1)
                        mapNodes[x][y].north = mapNodes[x][y + 1];
                    if (y > 0)
                        mapNodes[x][y].south = mapNodes[x][y - 1];

                    if (x < mapNodes.Length - 1)
                        mapNodes[x][y].east = mapNodes[x + 1][y];
                    if (x > 0)
                        mapNodes[x][y].west = mapNodes[x - 1][y];
                }
            }
        }

        public void PrintMap(Transform prefabWalkable, Transform prefabUnwalkable) {
            for (int x = 0; x < mapNodes.Length; x++) {
                for (int y = 0; y < mapNodes[x].Length; y++) {
                    PathNode pathNode = mapNodes[x][y];
                    UnityEngine.Object.Instantiate(pathNode.IsWalkable() ? prefabWalkable : prefabUnwalkable, new Vector3(x * nodeSize, y * nodeSize), Quaternion.identity);
                }
            }
        }

        public void PrintMap(Action<int, int> printNode) {
            for (int x = 0; x < mapNodes.Length; x++) {
                for (int y = 0; y < mapNodes[x].Length; y++) {
                    printNode(x, y);
                }
            }
        }

        public void PrintMap(Action<int, int, Vector3> printNode) {
            for (int x = 0; x < mapNodes.Length; x++) {
                for (int y = 0; y < mapNodes[x].Length; y++) {
                    printNode(x, y, worldOrigin + new Vector3(x * nodeSize, y * nodeSize));
                }
            }
        }

        public void PrintMap(Action<int, int> printWalkable, Action<int, int> printUnwalkable) {
            for (int x = 0; x < mapNodes.Length; x++) {
                for (int y = 0; y < mapNodes[x].Length; y++) {
                    PathNode pathNode = mapNodes[x][y];
                    if (pathNode.IsWalkable()) {
                        printWalkable(x, y);
                    } else {
                        printUnwalkable(x, y);
                    }
                }
            }
        }

        public void PrintMap(Action<Vector3, Vector3, Color> createSprite) {
            PrintMap(
                (int x, int y) => {
                    createSprite(worldOrigin + new Vector3(x * nodeSize, y * nodeSize), new Vector3(2, 2), Color.green);
                //MyUtils.World_Sprite.Create(worldOrigin + new Vector3(x * nodeSize, y * nodeSize), new Vector3(2, 2), SpriteHolder.instance.s_White, Color.green);
            },
                (int x, int y) => {
                    createSprite(worldOrigin + new Vector3(x * nodeSize, y * nodeSize), new Vector3(2, 2), Color.red);
                //MyUtils.World_Sprite.Create(worldOrigin + new Vector3(x * nodeSize, y * nodeSize), new Vector3(2, 2), SpriteHolder.instance.s_White, Color.red);
            }
            );
        }

        /*public void PrintMapUpdateable() {
        }*/
        private bool IsValidShortcut(int startX, int startY, int endX, int endY) {
            //Debug.Log("Testing Shortcut: " + startX + ", " + startY + " -> " + endX + ", " + endY);
            int shortcutWeight = mapNodes[startX][startY].weight;
            Vector3 dir = (new Vector3(endX, endY) - new Vector3(startX, startY)).normalized;
            Vector3 test = new Vector3(startX, startY) + dir;
            int testX = Mathf.RoundToInt(test.x);
            int testY = Mathf.RoundToInt(test.y);
            // Check if shortcut is walkable
            //Debug.Log("Testing: "+testX+","+testY);
            while (!(testX == endX && testY == endY)) {
                if (!IsWalkable(testX, testY) || mapNodes[testX][testY].weight != shortcutWeight) {
                    // Not walkable
                    //Debug.Log("Shortcut invalid!");
                    return false;
                } else {
                    test += dir;
                    testX = Mathf.RoundToInt(test.x);
                    testY = Mathf.RoundToInt(test.y);
                    //Debug.Log("Testing: "+testX+","+testY);
                }
            }
            // Shortcut walkable
            //Debug.Log("Shortcut valid!");
            return true;
        }

        public List<PathNode> GetFindPath(MapPos startPos, MapPos finalPos) {
            int width = widthMax;
            int height = heightMax;

            startPos = GetClosestValidPos(startPos.x, startPos.y);

            if (startPos.x < 0 || startPos.y < 0 || finalPos.x < 0 || finalPos.y < 0 ||
                startPos.x >= width || finalPos.x >= width ||
                startPos.y >= height || finalPos.y >= height) {
                return null; //Out of bounds!
            }
            if (mapNodes[finalPos.x][finalPos.y].weight == WALL_WEIGHT ||
                mapNodes[startPos.x][startPos.y].weight == WALL_WEIGHT)
                return null; //Wall

            return findPath(startPos.x, startPos.y, finalPos.x, finalPos.y);
        }

        public List<PathNode> GetFindPathClosest(MapPos startPos, List<MapPos> allFinalPos) {
            List<PathNode> closest = null;

            for (int i = 0; i < allFinalPos.Count; i++) {
                List<PathNode> path = GetFindPath(startPos, allFinalPos[i]);
                if (path != null) {
                    if (closest == null)
                        closest = path;
                    else {
                        if (path.Count < closest.Count) {
                            closest = path;
                        }
                    }
                }
            }

            return closest;
        }

        public bool FindPath(int startX, int startY, MapPos finalPos, OnPathCallback callback) {
            return FindPath(startX, startY, new List<MapPos>() { finalPos }, callback);
        }

        public bool FindPath(int startX, int startY, List<MapPos> finalPositions, OnPathCallback callback) {
            int width = widthMax;
            int height = heightMax;

            MapPos start = GetClosestValidPos(startX, startY);
            startX = start.x;
            startY = start.y;
            List<PathRoute> paths = new List<PathRoute>();

            foreach (MapPos finalPos in finalPositions) {
                if (startX < 0 || startY < 0 || finalPos.x < 0 || finalPos.y < 0 ||
                    startX >= width || finalPos.x >= width ||
                    startY >= height || finalPos.y >= height) {
                    continue; // Out of bounds!
                }
                if (mapNodes[finalPos.x][finalPos.y].weight == WALL_WEIGHT ||
                    mapNodes[startX][startY].weight == WALL_WEIGHT) {
                    // Find close non-wall start/end
                    continue; // Wall
                }

                List<PathNode> currentPath = findPath(startX, startY, finalPos.x, finalPos.y);
                if (currentPath.Count <= 0 && (startX != finalPos.x || startY != finalPos.y)) {
                    // Don't add path if there's no path
                } else {
                    if (!finalPos.straightToOffset) {
                        // Don't go straight to offset, add dummy
                        currentPath.Add(currentPath[currentPath.Count - 1]);
                    }
                    paths.Add(new PathRoute(currentPath, worldOrigin, nodeSize, finalPos));
                }
            }
            int smallest = 0;
            for (int i = 1; i < paths.Count; i++) {
                if (paths[i].pathNodeList.Count < paths[smallest].pathNodeList.Count)
                    smallest = i;
            }

            if (paths.Count <= 0 || (paths.Count > 0 && paths[smallest].pathNodeList.Count <= 0)) {
                // No path
                return false;
            } else {
                callback(paths[smallest].pathNodeList, paths[smallest].finalPos);
            }
            return true;
        }

        public Vector3 GetClosestValidPosition(Vector3 position) {
            int mapX, mapY;
            ConvertVectorPositionValidate(position, out mapX, out mapY);
            MapPos closestValidMapPos = GetClosestValidPos(mapX, mapY);
            PathNode pathNode = mapNodes[closestValidMapPos.x][closestValidMapPos.y];
            return pathNode.GetWorldVector(worldOrigin, nodeSize);
        }

        private MapPos GetClosestValidPos(int mapX, int mapY) {
            int width = widthMax;
            int height = heightMax;
            // Inside bounds
            while (mapX < 0) mapX++;
            while (mapY < 0) mapY++;
            while (mapX >= width) mapX--;
            while (mapY >= height) mapY--;

            // Check inside walls
            if (mapNodes[mapX][mapY].weight == WALL_WEIGHT) {
                int radius = 1;
                MapPos valid = null;
                do {
                    valid = GetValidPosRadius(mapX, mapY, radius);
                    radius++;
                } while (valid == null && radius < 100);
                if (radius == 100) return new MapPos(0, 0);
                return valid;
            }
            return new MapPos(mapX, mapY);
        }

        private MapPos GetValidPosRadius(int mapX, int mapY, int radius) {
            int width = widthMax;
            int height = heightMax;

            int endX = mapX + radius;
            for (int i = mapX - radius; i <= endX; i++) {
                int j = mapY + radius;
                if (i < 0 || i >= width || j < 0 || j >= height) {
                    //Out of bounds
                } else {
                    if (mapNodes[i][j].weight != WALL_WEIGHT)
                        return new MapPos(i, j);
                }

                j = mapY - radius;
                if (i < 0 || i >= width || j < 0 || j >= height) {
                    //Out of bounds
                } else {
                    if (mapNodes[i][j].weight != WALL_WEIGHT)
                        return new MapPos(i, j);
                }

            }
            int endY = mapY + radius;
            for (int j = mapY - radius + 1; j < endY; j++) {
                int i = mapX - radius;
                if (i < 0 || i >= width || j < 0 || j >= height) {
                    //Out of bounds
                } else {
                    if (mapNodes[i][j].weight != WALL_WEIGHT)
                        return new MapPos(i, j);
                }

                i = mapX + radius;
                if (i < 0 || i >= width || j < 0 || j >= height) {
                    //Out of bounds
                } else {
                    if (mapNodes[i][j].weight != WALL_WEIGHT)
                        return new MapPos(i, j);
                }

            }
            return null;
        }

        public void ApplyShortcuts(ref List<PathNode> pathNodeList) {
            if (pathNodeList.Count > 1) {
                int testStartNodeIndex = 1;
                while (testStartNodeIndex < pathNodeList.Count - 2) { // Only test untils there's 3 nodes left
                    PathNode testStartNode = pathNodeList[testStartNodeIndex];
                    int testEndNodeIndex = testStartNodeIndex + 2;
                    // Test start node with node 2 indexes in front
                    PathNode testEndNode = pathNodeList[testEndNodeIndex];
                    while (IsValidShortcut(testStartNode.xPos, testStartNode.yPos, testEndNode.xPos, testEndNode.yPos)) {
                        // Valid shortcut
                        // Remove in between node
                        pathNodeList.RemoveAt(testStartNodeIndex + 1);
                        if (testEndNodeIndex >= pathNodeList.Count - 1) {
                            // No more nodes
                            break;
                        } else {
                            // Test next node
                            testEndNode = pathNodeList[testEndNodeIndex];
                        }
                    }
                    // Start next shortcut test from this end node
                    testStartNodeIndex = testEndNodeIndex;
                }
            }
        }

        public PathRoute GetPathRouteWithShortcuts(Vector3 start, Vector3 end) {
            List<PathNode> pathNodeList = GetPath(start, end);
            ApplyShortcuts(ref pathNodeList);
            return new PathRoute(pathNodeList, worldOrigin, nodeSize, null);
        }

        public PathRoute GetPathRoute(Vector3 start, Vector3 end) {
            List<PathNode> pathNodeList = GetPath(start, end);
            return new PathRoute(pathNodeList, worldOrigin, nodeSize, null);
        }

        public List<PathNode> GetPath(Vector3 start, Vector3 end) {
            start = start - worldOrigin;
            end = end - worldOrigin;
            start = start / nodeSize;
            end = end / nodeSize;
            MapPos startMapPos = GetClosestValidPos(Mathf.RoundToInt(start.x), Mathf.RoundToInt(start.y));
            MapPos endMapPos = GetClosestValidPos(Mathf.RoundToInt(end.x), Mathf.RoundToInt(end.y));
            return findPath(startMapPos.x, startMapPos.y, endMapPos.x, endMapPos.y);
        }

        public List<PathNode> findPath(int startX, int startY, int endX, int endY) {
            List<PathNode> ret = new List<PathNode>();
            // Calculate H for all nodes
            CalculateAllHeuristics(endX, endY);

            // Start finding target
            foundTarget = false;
            binaryTree = new BinaryTree();
            openListCount = 1;

            PathNode currentNode = mapNodes[startX][startY];
            PathNode targetNode = mapNodes[endX][endY];

            if (currentNode == targetNode) {
                return new List<PathNode> { currentNode };
            }
            int iterations = 0;
            do {
                iterations++;
                currentNode = FindTarget(currentNode, targetNode);
            } while (!foundTarget && openListCount > 0 && iterations < 60000);
            if (iterations >= 60000) UnityEngine.Debug.Log("iteration overload");

            if (foundTarget) {
                // Get path
                currentNode = targetNode;
                ret.Add(currentNode);
                while (currentNode.parent != null && currentNode.parent != currentNode) {
                    ret.Add(currentNode.parent);
                    currentNode = currentNode.parent;
                }
                if (currentNode.parent == currentNode) UnityEngine.Debug.Log("parent == child");
            } else {
                // No path possible
            }
            ret.Reverse();

            return ret;
        }

        private PathNode FindTarget(PathNode currentNode, PathNode targetNode) {
            // Check the north node
            if (currentNode.moveNorth) DetermineNodeValues(currentNode, currentNode.north, targetNode);
            // Check the east node
            if (currentNode.moveEast) DetermineNodeValues(currentNode, currentNode.east, targetNode);
            // Check the south node
            if (currentNode.moveSouth) DetermineNodeValues(currentNode, currentNode.south, targetNode);
            // Check the west node
            if (currentNode.moveWest) DetermineNodeValues(currentNode, currentNode.west, targetNode);

            if (!foundTarget) {
                // Once done checking add to the closed list and remove from the open list
                AddToClosedList(currentNode);
                RemoveFromOpenList(currentNode);

                // Get the next node with the smallest F value
                return GetSmallestFValueNode();
            } else {
                return null;
            }
        }

        private void DetermineNodeValues(PathNode currentNode, PathNode testing, PathNode targetNode) {
            // Dont work on null nodes
            if (testing == null)
                return;

            // Check to see if the node is the target
            if (testing == targetNode) {
                targetNode.parent = currentNode;
                foundTarget = true;
                return;
            }

            // Ignore Walls
            if (currentNode.weight == WALL_WEIGHT || testing.weight == WALL_WEIGHT)
                return;

            // While the node has not already been tested
            if (!testing.isOnClosedList) {
                // Check to see if the node is already on the open list
                if (testing.isOnOpenList) {
                    // Get a Gcost to move from this node to the testing node
                    int newGcost = currentNode.gValue + currentNode.weight + movementCost;

                    // If the G cost is better then change the nodes parent and update its costs.
                    if (newGcost < testing.gValue) {
                        testing.parent = currentNode;
                        testing.gValue = newGcost;
                        binaryTree.RemoveNode(testing);
                        testing.CalculateFValue();
                        binaryTree.AddNode(testing);
                    }
                } else {
                    // Set the testing nodes parent to the current location, calculate its costs, and add it to the open list
                    testing.parent = currentNode;
                    testing.gValue = currentNode.gValue + currentNode.weight + movementCost;
                    testing.CalculateFValue();
                    AddToOpenList(testing);
                }
            }
        }

        private void AddToOpenList(PathNode node) {
            binaryTree.AddNode(node);
            openListCount++;
            node.isOnOpenList = true;
        }

        private void AddToClosedList(PathNode currentNode) {
            currentNode.isOnClosedList = true;
        }

        private void RemoveFromOpenList(PathNode currentNode) {
            binaryTree.RemoveNode(currentNode);
            openListCount--;
            currentNode.isOnOpenList = false;
        }

        private PathNode GetSmallestFValueNode() {
            return binaryTree.GetSmallest();
        }

        private void CalculateManhattanDistance(PathNode currentNode, int currX, int currY, int targetX, int targetY) {
            currentNode.parent = null;
            currentNode.hValue = (Mathf.Abs(currX - targetX) + Mathf.Abs(currY - targetY));
            currentNode.isOnOpenList = false;
            currentNode.isOnClosedList = false;
        }
        private void CalculateAllHeuristics(int endX, int endY) {
            int rows = heightMax;
            int cols = widthMax;
            for (int x = 0; x < cols; x++) {
                for (int y = 0; y < rows; y++) {
                    CalculateManhattanDistance(mapNodes[x][y], x, y, endX, endY);
                }
            }
        }

        public void ResetRestrictions() {
            for (int i = 0; i < GetMapWidth(); i++) {
                for (int j = 0; j < GetMapHeight(); j++) {
                    mapNodes[i][j].ResetRestrictions();
                }
            }
        }

        public void RefreshAllHitboxes() {
            for (int x = 0; x < widthMax; x++) {
                for (int y = 0; y < heightMax; y++) {
                    mapNodes[x][y].TestHitbox();
                }
            }
            //Event_Speaker.Broadcast(Event_Trigger.Pathfinding_Refresh);
        }

        public void RefreshHitbox(MapPos mapPos) {
            mapNodes[mapPos.x][mapPos.y].TestHitbox();
            //Event_Speaker.Broadcast(Event_Trigger.Pathfinding_Refresh);
        }

        public bool IsWalkable(MapPos mapPos) {
            return mapNodes[mapPos.x][mapPos.y].weight != WALL_WEIGHT;
        }

        public bool IsWalkable(int x, int y) {
            return mapNodes[x][y].weight != WALL_WEIGHT;
        }

        public bool IsWall(int x, int y) {
            return mapNodes[x][y].weight == WALL_WEIGHT;
        }

        public bool HasWeight(int x, int y) {
            return mapNodes[x][y].weight > 0;
        }


        private void ConvertVectorPosition(Vector3 position, out int x, out int y) {
            x = (int)((position.x - worldOrigin.x) / nodeSize);
            y = (int)((position.y - worldOrigin.y) / nodeSize);
        }
        private void ConvertVectorPositionValidate(Vector3 position, out int x, out int y) {
            ConvertVectorPosition(position, out x, out y);

            if (x < 0) x = 0;
            if (y < 0) y = 0;
            if (x >= widthMax) x = widthMax - 1;
            if (y >= heightMax) y = heightMax - 1;
        }

    }

}