コード例 #1
0
ファイル: pathFinding.cs プロジェクト: aballantyne/Isometric
    private void FindPath(int2 startingPostion, int2 endPostion)
    {
        int2 gridSize = new int2(4, 4);

        NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp);

        for (int x = 0; x < gridSize.x; x++)
        {
            for (int y = 0; y < gridSize.y; y++)
            {
                PathNode pathNode = new PathNode();
                pathNode.x     = x;
                pathNode.y     = y;
                pathNode.Index = CalculateIndex(x, y, gridSize.x);

                pathNode.gCost = int.MaxValue;
                pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPostion);
                pathNode.CalculateFcost();

                pathNode.isWalkble         = true;
                pathNode.cameFromNodeIndex = -1;

                pathNodeArray[pathNode.Index] = pathNode;
            }
        }
        NativeArray <int2> neighborOffsetArray = new  NativeArray <int2>(new int2[] {
            new int2(-1, 0),
            new int2(+1, 0),
            new int2(0, +1),
            new int2(0, -1),
        }, Allocator.Temp);

        int      endNodeIndex = CalculateIndex(endPostion.x, endPostion.y, gridSize.x);
        PathNode startNode    = pathNodeArray[CalculateIndex(startingPostion.x, startingPostion.y, gridSize.x)];

        startNode.gCost = 0;
        startNode.CalculateFcost();
        pathNodeArray[startNode.Index] = startNode;

        NativeList <int> openList   = new NativeList <int>(Allocator.Temp);
        NativeList <int> closedList = new NativeList <int>(Allocator.Temp);

        openList.Add(startNode.Index);

        while (openList.Length > 0)
        {
            int      currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
            PathNode currentNode      = pathNodeArray[currentNodeIndex];

            if (currentNodeIndex == endNodeIndex)
            {
                // we are at the end
                break;
            }
            // remove current
            for (int i = 0; i < openList.Length; i++)
            {
                if (openList[i] == currentNodeIndex)
                {
                    openList.RemoveAtSwapBack(i);
                    break;
                }
            }
            closedList.Add(currentNodeIndex);

            for (int i = 0; i < neighborOffsetArray.Length; i++)
            {
                int2 neighborOffset   = neighborOffsetArray[i];
                int2 neighborPosition = new int2(currentNode.x + neighborOffset.x, currentNode.y + neighborOffset.y);
                if (!IsPositionInsiderGrid(neighborPosition, gridSize))
                {
                    // Not valid
                    continue;
                }
                int neighborNodeIndex = CalculateIndex(neighborPosition.x, neighborPosition.y, gridSize.x);

                if (closedList.Contains(neighborNodeIndex))
                {
                    //Searched already
                    continue;
                }

                PathNode neighborNode = pathNodeArray[neighborNodeIndex];
                if (!neighborNode.isWalkble)
                {
                    //not walkable
                    continue;
                }

                int2 currentNodePos = new int2(currentNode.x, currentNode.y);

                int tentaiveGCost = currentNode.gCost + CalculateDistanceCost(currentNodePos, neighborPosition);
                if (tentaiveGCost < neighborNode.gCost)
                {
                    neighborNode.cameFromNodeIndex = currentNodeIndex;
                    neighborNode.gCost             = tentaiveGCost;
                    neighborNode.CalculateFcost();
                    pathNodeArray[neighborNodeIndex] = neighborNode;

                    if (!openList.Contains(neighborNode.Index))
                    {
                        openList.Add(neighborNode.Index);
                    }
                }
            }
        }

        PathNode endNode = pathNodeArray[endNodeIndex];

        if (endNode.cameFromNodeIndex == -1)
        {
            //no
            Debug.Log("didnt Find it");
        }
        else
        {
            //yes

            NativeList <int2> path = CalculatePath(pathNodeArray, endNode);
            foreach (int2 pathPosition in path)
            {
                Debug.Log(pathPosition);
            }
            path.Dispose();
        }

        pathNodeArray.Dispose();
        neighborOffsetArray.Dispose();
        openList.Dispose();
        closedList.Dispose();
    }