Beispiel #1
0
        private static void DestinationReached()
        {
            //Faster to do this to end loop than to check every AnalyzeNode () call
            destinationIsReached = true;
            GridHeap.FastClear();

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

            currentNode = endNode;

            //GridNode oldNode = null;

            StartNodeIndex = startNode.gridIndex;
            while (currentNode.gridIndex != StartNodeIndex)
            {
                //Sets CombinePathVersion while tracing path
                TracePath.Add(currentNode);
                //oldNode = currentNode;
                currentNode = currentNode.parent;
            }

            currentNode = TracePath[TracePath.Count - 1];
            for (i = TracePath.Count - 2; i >= 0; i--)
            {
                //oldNode = currentNode;
                currentNode = TracePath.innerArray[i];
                outputPath.Add(currentNode);
            }
        }
Beispiel #2
0
 static void AnalyzeNode()
 {
     if (!GridHeap.Contains(neighbor))
     {
         AddBestNode();
         GridHeap.Add(neighbor);
     }
     else if (newMovementCostToNeighbor < neighbor.gCost)
     {
         AddBestNode();
         GridHeap.UpdateItem(neighbor);
     }
 }
Beispiel #3
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);
            }
        }
Beispiel #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 FindRawPath(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
            if (_endNode.Unwalkable)
            {
                return(false);
            }
            while (GridHeap.Count > 0)
            {
                currentNode = GridHeap.RemoveFirst();
#if false
                Gizmos.DrawCube(currentNode.WorldPos.ToVector3(), Vector3.one);
#endif

                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 = 2;
                    #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();
                        }
                    }
                }
                GridClosedSet.Add(currentNode);
            }
            #endregion
            return(false);
        }
 public static void Reset()
 {
     //Reset combine value so it doesn't overflow with multiple scenes
     CombineIteration = 0;
     GridHeap.Reset();
 }
 static bool CheckNeighborSearchable()
 {
     return(neighbor.IsNull() || GridHeap.Closed(neighbor));
 }
        /*
         * static int GetObstructionCount(int index1, int index2)
         * {
         * if (CheckInvalid(currentNode.NeighborNodes[index1])) {
         * if (CheckInvalid(currentNode.NeighborNodes[index2])) {
         *  return 2;
         * }
         * return 1;
         * }
         * if (CheckInvalid(currentNode.NeighborNodes[index2]))
         * return 1;
         * return 0;
         * }*/

        static bool CheckInvalid(GridNode gridNode)
        {
            return(gridNode.IsNull() || GridHeap.Closed(gridNode) || gridNode.Unpassable());
        }
        /// <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 FindRawPath(GridNode _startNode, GridNode _endNode, FastList <GridNode> _outputPath, int _unitSize)
        {
            //TODO: Not critical but there's a lot of room for better organization
            //i.e. All these static variables and methods goes into individual singleton classes
            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();
            //POSBUG: Hash for end destination and frame count. *Most likely* won't overflow
            //Or no need to factor in frame count
            #endregion

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



            GridNode.PrepareUnpassableCheck(unitSize); //Prepare Unpassable check optimizations
            if (_endNode.Unwalkable)
            {
                return(false);
            }
            destinationIsReached = false;
            SearchCount          = 0;
            CombineVersionSet    = CombineIteration * GridManager.MaxIndex + endNode.gridIndex;
            if (lastGridIndex == endNode.gridIndex)
            {
                CombineVersionCheck = CombineVersionSet;
            }
            else
            {
                if (CombineVersionCheck != DefaultCombineVersion)
                {
                    CombineIteration++;
                    CombineVersionCheck = DefaultCombineVersion;
                }
            }
            lastGridIndex = endNode.gridIndex;
            while (GridHeap.Count > 0)
            {
                SearchCount++;
                rawNode = GridHeap.RemoveFirst();


                if (rawNode.gridIndex == endNode.gridIndex)
                {
                    //We found our way to the end node!
                    DestinationReached();
                    return(true);
                }

                if (CombineVersionCheck != DefaultCombineVersion)
                {
                    if (rawNode.CombinePathVersion == CombineVersionCheck)
                    {
                        //We found our way onto an existing path!
                        DestinationReached(true);
                        return(true);
                    }
                }
                                #if true
                #region Allows diagonal access when edges are blocked
                for (i = 0; i < 4; i++)
                {
                    neighbor = rawNode.NeighborNodes [i];

                    neighbor = rawNode.NeighborNodes [i];
                    if (CheckNeighborSearchable() == false)
                    {
                        if (neighbor.Unpassable() == false)
                        {
                            newMovementCostToNeighbor = rawNode.gCost + 141;
                            ProcessNode();
                        }
                        else if (neighbor.gridIndex == EndNodeIndex)
                        {
                            AddBestNode();
                            DestinationReached();
                            return(true);
                        }
                    }
                }

                for (int i = 4; i < 8; i++)
                {
                    neighbor = rawNode.NeighborNodes [i];
                    if (CheckNeighborSearchable() == false)
                    {
                        if (neighbor.Unpassable() == false)
                        {
                            newMovementCostToNeighbor = rawNode.gCost + 141;
                            ProcessNode();
                        }
                        else if (neighbor.gridIndex == EndNodeIndex)
                        {
                            AddBestNode();
                            DestinationReached();
                            return(true);
                        }
                    }
                }
                GridHeap.Close(rawNode);
                #endregion
                                #else
                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 = 2;
                    #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();
                        }
                    }
                }
                                #endif
            }
            #endregion
            return(destinationIsReached);
        }
Beispiel #9
0
 static bool CheckNeighborSearchable()
 {
     return(neighbor.IsNotNull() && GridHeap.Closed(neighbor) == false);
 }