Beispiel #1
0
    public static List <GridCell> FindPath(GridController grid, Vector2Int start, Vector2Int end, bool diag = false)
    {
        List <GridCell> Path       = null;
        GridCell        StartNode  = grid.GetGridPoint(start);
        GridCell        TargetNode = grid.GetGridPoint(end);

        if (TargetNode == null || StartNode == null)
        {
            return(null);
        }

        List <GridCell>    OpenList   = new List <GridCell>();
        HashSet <GridCell> ClosedList = new HashSet <GridCell>();

        OpenList.Add(StartNode);

        while (OpenList.Count > 0)
        {
            GridCell CurrentNode = OpenList[0];
            for (int i = 1; i < OpenList.Count; i++)
            {
                if (OpenList[i].FCost() < CurrentNode.FCost() || OpenList[i].FCost() == CurrentNode.FCost() && OpenList[i].hCost < CurrentNode.hCost)
                {
                    CurrentNode = OpenList[i];
                }
            }
            OpenList.Remove(CurrentNode);
            ClosedList.Add(CurrentNode);

            if (CurrentNode == TargetNode)
            {
                Path = GetFinalPath(StartNode, TargetNode);
            }

            foreach (GridCell NeighborNode in grid.GetNeighboringCells(CurrentNode.transform.position, diag))
            {
                if (NeighborNode.IsWall() || !NeighborNode.Walkable || ClosedList.Contains(NeighborNode))
                {
                    continue;
                }
                int MoveCost = CurrentNode.gCost + GetManhattenDistance(CurrentNode, NeighborNode) + NeighborNode.weight;

                if (MoveCost < NeighborNode.gCost || !OpenList.Contains(NeighborNode))
                {
                    NeighborNode.gCost      = MoveCost;
                    NeighborNode.hCost      = GetManhattenDistance(NeighborNode, TargetNode);
                    NeighborNode.ParentNode = CurrentNode;

                    if (!OpenList.Contains(NeighborNode))
                    {
                        OpenList.Add(NeighborNode);
                    }
                }
            }
        }

        return(Path);
    }
Beispiel #2
0
    public List <PathFinderNode> FindPath(int StartX, int StartY, int EndX, int EndY)
    {
        PathFinderNode StartNode = grid[StartY - MapOffset.y, StartX - MapOffset.x];
        PathFinderNode EndNode   = grid[EndY - MapOffset.y, EndX - MapOffset.x];

        OpenList = new List <PathFinderNode> {
            StartNode
        };
        ClosedList = new List <PathFinderNode>();

        for (int x = 0; x < width; x++)
        {
            for (int y = 0; y < height; y++)
            {
                PathFinderNode PathNode = grid[y, x];
                PathNode.GCost = int.MaxValue;
                PathNode.CalculateFCost();
                PathNode.CameFromNode = null;
            }
        }

        StartNode.GCost = 0;
        StartNode.HCost = CalculateDistanceCost(StartNode, EndNode);
        StartNode.CalculateFCost();

        while (OpenList.Count > 0)
        {
            PathFinderNode CurrentNode = GetLowestFCostNode(OpenList);
            if (CurrentNode == EndNode)
            {
                return(CalculatePath(EndNode));
            }

            OpenList.Remove(CurrentNode);
            ClosedList.Add(CurrentNode);

            foreach (PathFinderNode NeighborNode in GetNeighorNodes(CurrentNode))
            {
                if (ClosedList.Contains(NeighborNode))
                {
                    continue;
                }

                if (!IsNodeOpen(NeighborNode))
                {
                    if (!ClosedList.Contains(NeighborNode))
                    {
                        ClosedList.Add(NeighborNode);
                    }
                    continue;
                }
                else
                {
                    // Debug.Log("Open Path");
                }

                int TentativeGCost = CurrentNode.GCost + 1;
                if (TentativeGCost < NeighborNode.GCost)
                {
                    NeighborNode.CameFromNode = CurrentNode;
                    NeighborNode.GCost        = TentativeGCost;
                    NeighborNode.HCost        = CalculateDistanceCost(NeighborNode, EndNode);
                    NeighborNode.CalculateFCost();
                }

                if (!OpenList.Contains(NeighborNode))
                {
                    OpenList.Add(NeighborNode);
                }
            }
        }

        return(null);
    }
Beispiel #3
0
    public IList <ICell> FindPathOnMap(ICell cellStart, ICell cellEnd)
    {
        if (cellStart == null || cellEnd == null)
        {
            return(null);
        }

        List <ICell> OpenList   = new List <ICell>();
        List <ICell> ClosedList = new List <ICell>();

        OpenList.Add(cellStart);

        while (OpenList.Count > 0)
        {
            ICell CurrentNode = OpenList[0];
            for (int i = 1; i < OpenList.Count; i++)
            {
                CellNode _target  = OpenList[i].GetNode();
                CellNode _current = CurrentNode.GetNode();

                if (_target.FCost < _current.FCost || _target.FCost == _current.FCost && _target.ihCost < _current.ihCost)
                {
                    CurrentNode = OpenList[i];
                }
            }

            OpenList.Remove(CurrentNode);
            ClosedList.Add(CurrentNode);

            if (CurrentNode == cellEnd)
            {
                return(GetFinalPath(cellStart, cellEnd));
            }

            foreach (ICell NeighborNode in CurrentNode.GetNearestCells())
            {
                CellNode _target  = NeighborNode.GetNode();
                CellNode _current = CurrentNode.GetNode();

                if (CellBase.CompareStates(NeighborNode.GetState(), CellBase.State.Obstacle) ||
                    ClosedList.Contains(NeighborNode))
                {
                    continue;
                }

                int MoveCost = _current.igCost + GetManhattenDistance(_current, _target);

                if (MoveCost < _target.igCost || !OpenList.Contains(NeighborNode))
                {
                    NeighborNode.GetNode().igCost     = MoveCost;
                    NeighborNode.GetNode().ihCost     = GetManhattenDistance(_target, cellEnd.GetNode());
                    NeighborNode.GetNode().ParentNode = CurrentNode;

                    if (!OpenList.Contains(NeighborNode))
                    {
                        OpenList.Add(NeighborNode);
                    }
                }
            }
        }

        return(null);
    }
Beispiel #4
0
        public Vector3 GetValidPathFloorPos(Vector3 position)
        {
            // Save this value off, in case we need to use it to search for a valid location further along in this function.
            Vector3 originalPosition = position;

            // Find a position that is within the grid, at the same height as the grid.
            float  padding    = m_cellSize / 4.0f;
            Bounds gridBounds = GetGridBounds();

            position.x = Mathf.Clamp(position.x, gridBounds.min.x + padding, gridBounds.max.x - padding);
            position.y = Origin.y;
            position.z = Mathf.Clamp(position.z, gridBounds.min.z + padding, gridBounds.max.z - padding);

            // If this position is blocked, then look at all of the neighbors of this cell, and see if one of those cells is
            // unblocked. If one of those neighbors is unblocked, then we return the position of that neighbor, to ensure that
            // the agent is always pathing to and from valid positions.
            int cellIndex = GetCellIndex(position);

            /*if ( IsBlocked(cellIndex) )
             * {
             *      // Find the closest unblocked neighbor, if one exists.
             *      int[] neighbors = null;
             *      int numNeighbors = GetNeighbors(cellIndex, ref neighbors);
             *      float closestDistSq = float.MaxValue;
             *      for ( int i = 0; i < numNeighbors; i++ )
             *      {
             *              int neighborIndex = neighbors[i];
             *              if ( !IsBlocked(neighborIndex) )
             *              {
             *                      Vector3 neighborPos = GetCellCenter(neighborIndex);
             *                      float distToCellSq = (neighborPos - originalPosition).sqrMagnitude;
             *                      if ( distToCellSq < closestDistSq )
             *                      {
             *                              closestDistSq = distToCellSq;
             *                              position = neighborPos;
             *                      }
             *              }
             *      }
             * }*/
            BinaryHeap <NeighborNode> cellIndices = new BinaryHeap <NeighborNode>();
            HashSet <NeighborNode>    set = new HashSet <NeighborNode>();
            int iterCount = 0, iterMax = 3 * (int)eNeighborDirection.kNumNeighbors;

            while (IsBlocked(cellIndex) && (iterCount++) < iterMax)
            {
                int[] neighbors    = null;
                int   numNeighbors = GetNeighbors(cellIndex, ref neighbors);
                foreach (int neighborIndex in neighbors)
                {
                    NeighborNode newNode = new NeighborNode(neighborIndex, 0.0f);

                    if (set.Contains(newNode))
                    {
                        continue;
                    }

                    set.Add(newNode);

                    Vector3 neighborPos = GetCellCenter(neighborIndex);
                    newNode.distance = (neighborPos - originalPosition).sqrMagnitude;

                    cellIndices.Add(newNode);
                }

                cellIndex = cellIndices.Remove().nodeIndex;
            }

            if (!IsBlocked(cellIndex))
            {
                position = GetCellCenter(cellIndex);
            }

            return(position);
        }