Esempio n. 1
0
    //find and mark Neighbour Node if != Obstacle
    private static void MarkNeighbourNode(int[][] grid, bool[][] exploredGrid, currentNode currentPos, int moveCost)
    {
        var nextCost = grid[currentPos.XPos][currentPos.YPos] + moveCost;

        if (currentPos.XPos + 1 < grid.Length && !exploredGrid[currentPos.XPos + 1][currentPos.YPos])
        {
            grid[currentPos.XPos + 1][currentPos.YPos] = nextCost;
        }

        if (currentPos.XPos - 1 >= 0 && !exploredGrid[currentPos.XPos - 1][currentPos.YPos])
        {
            grid[currentPos.XPos - 1][currentPos.YPos] = nextCost;
        }


        if (currentPos.YPos + 1 < grid[currentPos.XPos].Length && !exploredGrid[currentPos.XPos][currentPos.YPos + 1])
        {
            grid[currentPos.XPos][currentPos.YPos + 1] = nextCost;
        }

        if (currentPos.YPos - 1 >= 0 && !exploredGrid[currentPos.XPos][currentPos.YPos - 1])
        {
            grid[currentPos.XPos][currentPos.YPos - 1] = nextCost;
        }
    }
Esempio n. 2
0
    private static currentNode MoveToNextNode(int[][] grid, bool[][] exploredGrid, currentNode currentPos, int moveCost)
    {
        var minNode = grid[0][0];
        var xMin    = 0;
        var yMin    = 0;

        for (int i = 0; i < grid.Length; i++)
        {
            for (int j = 0; j < grid[i].Length; j++)
            {
                if (grid[i][j] <= minNode && !exploredGrid[i][j])
                {
                    minNode = grid[i][j];
                    xMin    = i;
                    yMin    = j;
                }
            }
        }
        currentPos.XPos          = xMin;
        currentPos.YPos          = yMin;
        exploredGrid[xMin][yMin] = true;
        return(currentPos);
    }
Esempio n. 3
0
    private static currentNode MoveToNextNodeWithHeuristic(int[][] grid, int[][] heuristicGrid, bool[][] exploredGrid, currentNode currentPos)
    {
        var minNode = Mathf.Abs(grid[0][0] - heuristicGrid[0][0]);
        var xMin    = 0;
        var yMin    = 0;

        for (var i = 0; i < grid.Length; i++)
        {
            for (var j = 0; j < grid[i].Length; j++)
            {
                if (Mathf.Abs(grid[i][j] - heuristicGrid[i][j]) < minNode && !exploredGrid[i][j] && grid[i][j] != int.MaxValue)
                {
                    minNode = Mathf.Abs(grid[i][j] - heuristicGrid[i][j]);
                    xMin    = i;
                    yMin    = j;
                }
            }
        }
        currentPos.XPos          = xMin;
        currentPos.YPos          = yMin;
        exploredGrid[xMin][yMin] = true;
        return(currentPos);
    }
Esempio n. 4
0
    public IEnumerator AStar()
    {
        _isRunning = true;

        // Recovering the environment as a matrix
        var matrix = MatrixFromRaycast.CreateMatrixFromRayCast();

        /*
         * // Convert as boolean grid
         * // true => wall || explored
         * // false => unknow
         */
        bool[][] ExploredGrid = new bool[matrix.Length][];
        for (int i = 0; i < matrix.Length; i++)
        {
            ExploredGrid[i] = new bool[matrix[i].Length];
            for (int j = 0; j < matrix[i].Length; j++)
            {
                ExploredGrid[i][j] = (matrix[i][j] == LayerMask.NameToLayer("Obstacle")) ? true : false;
            }
        }

        //Label all the nodes with an infinite score
        int[][] Grid = new int[matrix.Length][];
        for (int i = 0; i < matrix.Length; i++)
        {
            Grid[i] = new int[matrix[i].Length];
            for (int j = 0; j < matrix[i].Length; j++)
            {
                Grid[i][j] = int.MaxValue;
            }
        }

        int[][] HeuristicGrid = new int[matrix.Length][];
        for (int i = 0; i < matrix.Length; i++)
        {
            HeuristicGrid[i] = new int[matrix[i].Length];
            for (int j = 0; j < matrix[i].Length; j++)
            {
                HeuristicGrid[i][j] = ManhattanScore(i, j);
            }
        }

        // get position
        var startPosX = PlayerScript.StartXPositionInMatrix;
        var startPosY = PlayerScript.StartYPositionInMatrix;
        var endPosX   = PlayerScript.GoalXPositionInMatrix;
        var endPosY   = PlayerScript.GoalYPositionInMatrix;

        // init start pos at 0
        Grid[startPosX][startPosY]         = 0;
        ExploredGrid[startPosX][startPosY] = true;

        //define move cost
        const int moveCost = 1;

        //define the current case
        var currentPos = new currentNode
        {
            XPos = startPosX,
            YPos = startPosY
        };

        var it = 0;

        while (true)
        {
            Instantiate(CubeCurrentNode, new Vector3(currentPos.XPos - 25, -0.9f, currentPos.YPos - 25), Quaternion.identity);
            if (currentPos.XPos == endPosX && currentPos.YPos == endPosY)
            {
                Debug.Log("Solution found in " + it + " iterations");
                //todo return the optimal path
                break;
            }
            MarkNeighbourNode(Grid, ExploredGrid, currentPos, moveCost);
            currentPos = MoveToNextNodeWithHeuristic(Grid, HeuristicGrid, ExploredGrid, currentPos);

            it++;
            yield return(null);
        }
        _isRunning = false;
        yield return(null);
    }