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);
            }
        }
        /// <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);
        }
        /// <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);
        }
        /// <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);
        }