예제 #1
0
        static void AnalyzeNode()
        {
            if (!GridHeap.Contains(neighbor))
            {
                neighbor.gCost = newMovementCostToNeighbor;

                //Optimized heuristic calculation
                neighbor.CalculateHeuristic();
                neighbor.parent = currentNode;

                GridHeap.Add(neighbor);
            }
            else if (newMovementCostToNeighbor < neighbor.gCost)
            {
                neighbor.gCost = newMovementCostToNeighbor;

                //Optimized heuristic calculation
                neighbor.CalculateHeuristic();
                neighbor.parent = currentNode;

                GridHeap.UpdateItem(neighbor);
            }
        }
예제 #2
0
        /// <summary>
        /// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed.
        /// </summary>
        /// <returns>
        /// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found.
        /// </returns>
        /// <param name="startNode">Start node.</param>
        /// <param name="endNode">End node.</param>
        /// <param name="outputPath">Return path.</param>
        public static bool FindPath(GridNode startNode, GridNode endNode, FastList <GridNode> outputPath)
        {
            #region Broadphase and Preperation
            if (endNode.Unwalkable)
            {
                return(false);
            }

            if (startNode.Unwalkable)
            {
                return(false);
            }

            outputPath.FastClear();

            if (System.Object.ReferenceEquals(startNode, endNode))
            {
                outputPath.Add(endNode);
                return(true);
            }
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            sw.Start();

            GridHeap.FastClear();
            GridClosedSet.FastClear();
            #endregion

            #region AStar Algorithm
            GridHeap.Add(startNode);
            GridNode.HeuristicTargetX = endNode.gridX;
            GridNode.HeuristicTargetY = endNode.gridY;
            while (GridHeap.Count > 0)
            {
                currentNode = GridHeap.RemoveFirst();

                GridClosedSet.Add(currentNode);

                if (currentNode.gridIndex == endNode.gridIndex)
                {
                    //Retraces the path then outputs it into outputPath
                    //Also Simplifies the path

                    outputPath.FastClear();
                    TracePath.FastClear();

                    currentNode = endNode;

                    StartNodeIndex = startNode.gridIndex;
                    while (currentNode.gridIndex != StartNodeIndex)
                    {
                        TracePath.Add(currentNode);
                        oldNode     = currentNode;
                        currentNode = currentNode.parent;
                    }

                    oldNode     = startNode;
                    currentNode = TracePath [TracePath.Count - 1];
                    oldX        = currentNode.gridX - oldNode.gridX;
                    oldY        = currentNode.gridY - oldNode.gridY;

                    for (i = TracePath.Count - 2; i >= 0; i--)
                    {
                        oldNode     = currentNode;
                        currentNode = TracePath.innerArray [i];
                        newX        = currentNode.gridX - oldNode.gridX;
                        newY        = currentNode.gridY - oldNode.gridY;

                        if (newX != oldX || newY != oldY)
                        {
                            outputPath.Add(oldNode);
                            oldX = newX;
                            oldY = newY;
                        }
                        //outputPath.Add (currentNode);
                    }
                    outputPath.Add(endNode);

                    return(true);
                }

                for (i = 0; i < 8; i++)
                {
                    neighbor = currentNode.NeighborNodes [i];

                    if (neighbor == null || neighbor.Unwalkable || GridClosedSet.Contains(neighbor))
                    {
                        continue;
                    }

                    newMovementCostToNeighbor = currentNode.gCost + (GridNode.IsNeighborDiagnal [i] ? 141 : 100);

                    if (!GridHeap.Contains(neighbor))
                    {
                        neighbor.gCost = newMovementCostToNeighbor;

                        //Optimized heuristic calculation
                        neighbor.CalculateHeuristic();
                        neighbor.parent = currentNode;

                        GridHeap.Add(neighbor);
                    }
                    else if (newMovementCostToNeighbor < neighbor.gCost)
                    {
                        neighbor.gCost = newMovementCostToNeighbor;

                        //Optimized heuristic calculation
                        neighbor.CalculateHeuristic();
                        neighbor.parent = currentNode;

                        GridHeap.UpdateItem(neighbor);
                    }
                }
            }
            #endregion
            return(false);
        }
예제 #3
0
        /// <summary>
        /// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed.
        /// </summary>
        /// <returns>
        /// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found.
        /// </returns>
        /// <param name="startNode">Start node.</param>
        /// <param name="endNode">End node.</param>
        /// <param name="outputPath">Return path.</param>
        public static bool FindPath(GridNode startNode, GridNode endNode, FastList<GridNode> outputPath)
        {
            #region Broadphase and Preperation
            if (endNode.Unwalkable) {
                return false;
            }

            if (startNode.Unwalkable) {
                return false;
            }

            outputPath.FastClear ();

            if (System.Object.ReferenceEquals (startNode, endNode)) {
                outputPath.Add (endNode);
                return true;
            }
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            sw.Start ();

            GridHeap.FastClear ();
            GridClosedSet.FastClear ();
            #endregion

            #region AStar Algorithm
            GridHeap.Add (startNode);
            GridNode.HeuristicTargetX = endNode.gridX;
            GridNode.HeuristicTargetY = endNode.gridY;
            while (GridHeap.Count > 0) {
                currentNode = GridHeap.RemoveFirst ();

                GridClosedSet.Add (currentNode);

                if (currentNode.gridIndex == endNode.gridIndex) {
                    //Retraces the path then outputs it into outputPath
                    //Also Simplifies the path

                    outputPath.FastClear ();
                    TracePath.FastClear ();

                    currentNode = endNode;

                    StartNodeIndex = startNode.gridIndex;
                    while (currentNode.gridIndex != StartNodeIndex) {
                        TracePath.Add (currentNode);
                        oldNode = currentNode;
                        currentNode = currentNode.parent;
                    }

                    oldNode = startNode;
                    currentNode = TracePath [TracePath.Count - 1];
                    oldX = currentNode.gridX - oldNode.gridX;
                    oldY = currentNode.gridY - oldNode.gridY;

                    for (i = TracePath.Count - 2; i >= 0; i--) {
                        oldNode = currentNode;
                        currentNode = TracePath.innerArray [i];
                        newX = currentNode.gridX - oldNode.gridX;
                        newY = currentNode.gridY - oldNode.gridY;

                        if (newX != oldX || newY != oldY) {

                            outputPath.Add (oldNode);
                            oldX = newX;
                            oldY = newY;
                        }
                        //outputPath.Add (currentNode);
                    }
                    outputPath.Add (endNode);

                    return true;
                }

                for (i = 0; i < 8; i++) {
                    neighbor = currentNode.NeighborNodes [i];

                    if (neighbor == null || neighbor.Unwalkable || GridClosedSet.Contains (neighbor)) {
                        continue;
                    }

                    newMovementCostToNeighbor = currentNode.gCost + (GridNode.IsNeighborDiagnal [i] ? 141 : 100);

                    if (!GridHeap.Contains (neighbor)) {
                        neighbor.gCost = newMovementCostToNeighbor;

                        //Optimized heuristic calculation
                        neighbor.CalculateHeuristic ();
                        neighbor.parent = currentNode;

                        GridHeap.Add (neighbor);
                    } else if (newMovementCostToNeighbor < neighbor.gCost) {
                        neighbor.gCost = newMovementCostToNeighbor;

                        //Optimized heuristic calculation
                        neighbor.CalculateHeuristic ();
                        neighbor.parent = currentNode;

                        GridHeap.UpdateItem (neighbor);
                    }
                }
            }
            #endregion
            return false;
        }
예제 #4
0
        /// <summary>
        /// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed.
        /// </summary>
        /// <returns>
        /// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found.
        /// </returns>
        /// <param name="startNode">Start node.</param>
        /// <param name="endNode">End node.</param>
        /// <param name="outputPath">Return path.</param>
        public static bool FindPath(GridNode _startNode, GridNode _endNode, FastList <GridNode> _outputPath, int _unitSize = 1)
        {
            startNode  = _startNode;
            endNode    = _endNode;
            outputPath = _outputPath;
            unitSize   = _unitSize;

            #region Broadphase and Preperation
            if (endNode.Unwalkable)
            {
                return(false);
            }

            if (startNode.Unwalkable)
            {
                return(false);
            }

            outputPath.FastClear();

            if (System.Object.ReferenceEquals(startNode, endNode))
            {
                outputPath.Add(endNode);
                return(true);
            }

            GridHeap.FastClear();
            GridClosedSet.FastClear();
            #endregion

            #region AStar Algorithm
            GridHeap.Add(startNode);
            GridNode.HeuristicTargetX = endNode.gridX;
            GridNode.HeuristicTargetY = endNode.gridY;

            GridNode.PrepareUnpassableCheck(unitSize); //Prepare Unpassable check optimizations
            while (GridHeap.Count > 0)
            {
                currentNode = GridHeap.RemoveFirst();

                GridClosedSet.Add(currentNode);

                if (currentNode.gridIndex == endNode.gridIndex)
                {
                    //Retraces the path then outputs it into outputPath
                    //Also Simplifies the path
                    DestinationReached();
                    return(true);
                }

                for (i = 0; i < 8; i++)
                {
                    neighbor = currentNode.NeighborNodes [i];
                    if (neighbor.IsNull() || currentNode.Unpassable() || GridClosedSet.Contains(neighbor))
                    {
                        continue;
                    }
                    //0-3 = sides, 4-7 = diagonals
                    if (i < 4)
                    {
                        newMovementCostToNeighbor = currentNode.gCost + 100;
                    }
                    else
                    {
                        if (i == 4)
                        {
                            if (!GridManager.UseDiagonalConnections)
                            {
                                break;
                            }
                        }
                        newMovementCostToNeighbor = currentNode.gCost + 141;
                    }

                    if (!GridHeap.Contains(neighbor))
                    {
                        neighbor.gCost = newMovementCostToNeighbor;

                        //Optimized heuristic calculation
                        neighbor.CalculateHeuristic();
                        neighbor.parent = currentNode;

                        GridHeap.Add(neighbor);
                    }
                    else if (newMovementCostToNeighbor < neighbor.gCost)
                    {
                        neighbor.gCost = newMovementCostToNeighbor;

                        //Optimized heuristic calculation
                        neighbor.CalculateHeuristic();
                        neighbor.parent = currentNode;

                        GridHeap.UpdateItem(neighbor);
                    }
                }
            }
            #endregion
            return(false);
        }