예제 #1
0
    void CreateGrid()
    {
        Debug.Log(gridSizeX);
        Debug.Log(gridSizeY);
        grid = new NodeE[gridSizeX, gridSizeY];
        Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.forward * gridWorldSize.y / 2;


        for (int i = 0; i < gridSizeX; i++)
        {
            for (int j = 0; j < gridSizeY; j++)
            {
                Vector3 worldPoint = worldBottomLeft + Vector3.right * (i * nodeDiameter + nodeRadius) + Vector3.forward * (j * nodeDiameter + nodeRadius);
                bool    drivable   = !(Physics.CheckSphere(worldPoint, nodeRadius, undrivableMask));

                int movementPenalty = 0;

                if (drivable)
                {
                    Ray        ray = new Ray(worldPoint + Vector3.up * 50, Vector3.down);
                    RaycastHit hit;
                    if (Physics.Raycast(ray, out hit, 100, drivableMask))
                    {
                        drivableRegionsDictionnary.TryGetValue(hit.collider.gameObject.layer, out movementPenalty);
                    }
                }

                grid[i, j] = new NodeE(drivable, worldPoint, i, j, movementPenalty);
            }
        }
    }
예제 #2
0
    int GetDistance(NodeE nodeA, NodeE nodeB)
    {
        int distX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
        int distY = Mathf.Abs(nodeA.gridY - nodeB.gridY);

        if (distX > distY)
        {
            return(14 * distY + (10 * (distX - distY)));
        }
        return(14 * distX + (10 * (distY - distX)));
    }
예제 #3
0
    Vector3[] RetracePath(NodeE startNode, NodeE targetNode)
    {
        List <NodeE> path        = new List <NodeE>();
        NodeE        currentNode = targetNode;

        while (currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }
        Vector3[] waypoints = SimplifyPath(path);
        Array.Reverse(waypoints);
        return(waypoints);
    }
예제 #4
0
    public List <NodeE> GetNeighbours(NodeE node)
    {
        List <NodeE> neighbours = new List <NodeE>();

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

                int checkX = node.gridX + i;
                int checkY = node.gridY + j;

                if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
                {
                    neighbours.Add(grid[checkX, checkY]);
                }
            }
        }
        return(neighbours);
    }
예제 #5
0
    IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
    {
        Stopwatch sw = new Stopwatch();

        sw.Start();

        Vector3[] waypoints   = new Vector3[0];
        bool      pathSuccess = false;

        NodeE startNode  = grid.NodeFromWorldPoint(startPos);
        NodeE targetNode = grid.NodeFromWorldPoint(targetPos);

        if (startNode.drivable && targetNode.drivable)
        {
            HeapE <NodeE>   openSet   = new HeapE <NodeE>(grid.MaxSize);
            HashSet <NodeE> closedSet = new HashSet <NodeE>();
            openSet.Add(startNode);

            while (openSet.Count > 0)
            {
                NodeE currentNode = openSet.RemoveFirst();
                closedSet.Add(currentNode);

                if (currentNode == targetNode)
                {
                    sw.Stop();

                    pathSuccess = true;
                    break;
                }

                foreach (NodeE neighbour in grid.GetNeighbours(currentNode))
                {
                    if (!neighbour.drivable || closedSet.Contains(neighbour))
                    {
                        continue;
                    }

                    int newMovementCostTONeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
                    if (newMovementCostTONeighbour < neighbour.gCost || !openSet.Contains(neighbour))
                    {
                        neighbour.gCost  = newMovementCostTONeighbour;
                        neighbour.hCost  = GetDistance(neighbour, targetNode);
                        neighbour.parent = currentNode;

                        if (!openSet.Contains(neighbour))
                        {
                            openSet.Add(neighbour);
                        }
                        else
                        {
                            openSet.UpdateItem(neighbour);
                        }
                    }
                }
            }
        }
        yield return(null);

        if (pathSuccess)
        {
            waypoints = RetracePath(startNode, targetNode);
        }
        requester.FinishedProcessingPath(waypoints, pathSuccess);
    }