/// <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();
#if false
                Gizmos.DrawCube(currentNode.WorldPos.ToVector3(), Vector3.one);
#endif
                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 (CheckNeighborInvalid ()) {
                 *              //continue;
                 *              //microoptimization... continue is more expensive than letting the loop pass at the end
                 *      } else {
                 *              //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;
                 *              }
                 *
                 *              AnalyzeNode();
                 *      }
                 * }
                 */
                hasInvalidEdge = false;
                for (int i = 0; i < 4; i++)
                {
                    neighbor = currentNode.NeighborNodes[i];
                    if (CheckNeighborInvalid())
                    {
                        hasInvalidEdge = true;
                    }
                    else
                    {
                        newMovementCostToNeighbor = currentNode.gCost + 100;
                        AnalyzeNode();
                    }
                }

                if (hasInvalidEdge)
                {
                    const int maxCornerObstructions = 1;
                    #region inlining diagonals
                    neighbor = currentNode.NeighborNodes[4];
                    if (!CheckNeighborInvalid())
                    {
                        if (GetObstructionCount(0, 1) <= maxCornerObstructions)
                        {
                            newMovementCostToNeighbor = currentNode.gCost + 141;
                            AnalyzeNode();
                        }
                    }

                    neighbor = currentNode.NeighborNodes[5];
                    if (!CheckNeighborInvalid())
                    {
                        if (GetObstructionCount(0, 2) <= maxCornerObstructions)
                        {
                            newMovementCostToNeighbor = currentNode.gCost + 141;
                            AnalyzeNode();
                        }
                    }
                    neighbor = currentNode.NeighborNodes[6];
                    if (!CheckNeighborInvalid())
                    {
                        if (GetObstructionCount(3, 1) <= maxCornerObstructions)
                        {
                            newMovementCostToNeighbor = currentNode.gCost + 141;
                            AnalyzeNode();
                        }
                    }
                    neighbor = currentNode.NeighborNodes[7];
                    if (!CheckNeighborInvalid())
                    {
                        if (GetObstructionCount(3, 2) <= maxCornerObstructions)
                        {
                            newMovementCostToNeighbor = currentNode.gCost + 141;
                            AnalyzeNode();
                        }
                    }
                    #endregion
                }
                else
                {
                    //no need for specific stuff when edges are all valid
                    for (int i = 4; i < 8; i++)
                    {
                        neighbor = currentNode.NeighborNodes[i];
                        if (CheckNeighborInvalid())
                        {
                        }
                        else
                        {
                            newMovementCostToNeighbor = currentNode.gCost + 141;
                            AnalyzeNode();
                        }
                    }
                }
            }
            #endregion
            return(false);
        }
        /// <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);
        }
Example #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);
            }

            if (true)
            {
                #region Obstruction Test
                //Tests if there is a direct path. If there is, no need to run AStar.
                x0 = startNode.gridX;
                y0 = startNode.gridY;
                x1 = endNode.gridX;
                y1 = endNode.gridY;
                if (y1 > y0)
                {
                    compare1 = y1 - y0;
                }
                else
                {
                    compare1 = y0 - y1;
                }
                if (x1 > x0)
                {
                    compare2 = x1 - x0;
                }
                else
                {
                    compare2 = x0 - x1;
                }
                steep = compare1 > compare2;
                if (steep)
                {
                    t  = x0;                    // swap x0 and y0
                    x0 = y0;
                    y0 = t;
                    t  = x1;                    // swap x1 and y1
                    x1 = y1;
                    y1 = t;
                }
                if (x0 > x1)
                {
                    t  = x0;                    // swap x0 and x1
                    x0 = x1;
                    x1 = t;
                    t  = y0;                    // swap y0 and y1
                    y0 = y1;
                    y1 = t;
                }
                dx = x1 - x0;

                dy = (y1 - y0);
                if (dy < 0)
                {
                    dy = -dy;
                }

                error = dx / 2;
                ystep = (y0 < y1) ? 1 : -1;
                y     = y0;
                for (x = x0; x <= x1; x++)
                {
                    retX = (steep ? y : x);
                    retY = (steep ? x : y);

                    if (GridManager.Grid [retX * GridManager.NodeCount + retY].Unwalkable)
                    {
                        break;
                    }
                    else if (x == x1)
                    {
                        OutputPath.FastClear();
                        OutputPath.Add(startNode);
                        OutputPath.Add(endNode);
                        return(true);
                    }

                    error = error - dy;
                    if (error < 0)
                    {
                        y     += ystep;
                        error += dx;
                    }
                }
                #endregion
            }


            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)
                {
                    OutputPath.FastClear();

                    //Retraces the path then outputs it into OutputPath
                    //Also Simplifies the path


                    oldNode     = endNode;
                    currentNode = endNode.parent;

                    oldX = int.MaxValue;
                    oldY = int.MaxValue;

                    StartNodeIndex = startNode.gridIndex;



                    //if (!endNode.Obstructed) OutputPath.Add (endNode);

                    while (oldNode.gridIndex != StartNodeIndex)
                    {
                        newX = currentNode.gridX - oldNode.gridX;
                        newY = currentNode.gridY - oldNode.gridY;
                        if ((newX != oldX || newY != oldY))
                        {
                            OutputPath.Add(oldNode);
                            oldX = newX;
                            oldY = newY;
                        }

                        oldNode     = currentNode;
                        currentNode = currentNode.parent;
                    }


                    OutputPath.Add(startNode);
                    OutputPath.Reverse();
                    return(true);
                }

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


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

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

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

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

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

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

                        GridHeap.UpdateItem(neighbor);
                    }
                }
            }
            #endregion
            return(false);
        }
Example #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);
        }