Exemple #1
0
        // Here's the heuristic function that estimates the distance from a Node
        // to the Goal.
        public FP GoalDistanceEstimate(MapSearchNode nodeGoal)
        {
            FP X = nodeIndex.x - nodeGoal.nodeIndex.x;
            FP Y = nodeIndex.y - nodeGoal.nodeIndex.y;

            return(FPMath.Sqrt((X * X) + (Y * Y)));
        }
Exemple #2
0
        // Set Start and goal states
        public void SetStartAndGoalStates(MapSearchNode Start, MapSearchNode Goal)
        {
            m_Start = AllocateNode();
            m_Goal  = AllocateNode();

            System.Diagnostics.Debug.Assert((m_Start != null && m_Goal != null));

            m_Start.m_UserState = Start;
            m_Goal.m_UserState  = Goal;

            m_State = SearchState.Searching;

            // Initialise the AStar specific parts of the Start Node
            // The user only needs fill out the state information
            m_Start.g      = 0;
            m_Start.h      = m_Start.m_UserState.GoalDistanceEstimate(m_Goal.m_UserState);
            m_Start.f      = m_Start.g + m_Start.h;
            m_Start.parent = null;

            // Push the start node on the Open list
            m_OpenList.Add(m_Start);

            // Initialise counter for search steps
            m_Steps = 0;

#if PATHFIND_DEBUG
            //System.Console.WriteLine("Starting pathfind. Start: " + m_Start.m_UserState.position + ", Goal: " + m_Goal.m_UserState.position);
#endif
        }
Exemple #3
0
        // This generates the successors to the given Node. It uses a helper function called
        // AddSuccessor to give the successors to the AStar class. The A* specific initialisation
        // is done for each node internally, so here you just set the state information that
        // is specific to the application
        public bool GetSuccessors(AStarPathfinder aStarSearch, MapSearchNode parentNode)
        {
            Vector2Int parentPos = new Vector2Int(0, 0);

            if (parentNode != null)
            {
                parentPos = parentNode.nodeIndex;
            }

            if (pathfinder.volume > 0)
            {
                AddNeighbourNodeOfAttachVolume(1, 0, parentPos, aStarSearch);
                AddNeighbourNodeOfAttachVolume(-1, 0, parentPos, aStarSearch);
                AddNeighbourNodeOfAttachVolume(0, 1, parentPos, aStarSearch);
                AddNeighbourNodeOfAttachVolume(0, -1, parentPos, aStarSearch);
                AddNeighbourNodeOfAttachVolume(1, -1, parentPos, aStarSearch);
                AddNeighbourNodeOfAttachVolume(-1, 1, parentPos, aStarSearch);
                AddNeighbourNodeOfAttachVolume(1, 1, parentPos, aStarSearch);
                AddNeighbourNodeOfAttachVolume(-1, -1, parentPos, aStarSearch);
            }
            else
            {
                // push each possible move except allowing the search to go backwards
                AddNeighbourNode(-1, 0, parentPos, aStarSearch);
                AddNeighbourNode(0, -1, parentPos, aStarSearch);
                AddNeighbourNode(1, 0, parentPos, aStarSearch);
                AddNeighbourNode(0, 1, parentPos, aStarSearch);
                AddNeighbourNode(1, -1, parentPos, aStarSearch);
                AddNeighbourNode(-1, 1, parentPos, aStarSearch);
                AddNeighbourNode(1, 1, parentPos, aStarSearch);
                AddNeighbourNode(-1, -1, parentPos, aStarSearch);
            }
            return(true);
        }
Exemple #4
0
 void AddNeighbourNode(int xOffset, int yOffset, Vector2Int parentPos, AStarPathfinder aStarSearch)
 {
     if (ValidNeigbour(xOffset, yOffset) &&
         !(parentPos.x == nodeIndex.x + xOffset && parentPos.y == nodeIndex.y + yOffset))
     {
         Vector2Int neighbourPos = new Vector2Int(nodeIndex.x + xOffset, nodeIndex.y + yOffset);
         //Debug.Log("neighbourPos:" + neighbourPos);
         MapSearchNode newNode = pathfinder.AllocateMapSearchNode(neighbourPos);
         aStarSearch.AddSuccessor(newNode);
     }
 }
Exemple #5
0
        // User calls this to add a successor to a list of successors
        // when expanding the search frontier
        public bool AddSuccessor(MapSearchNode state)
        {
            Node node = AllocateNode();

            if (node != null)
            {
                node.m_UserState = state;
                m_Successors.Add(node);

                if (m_Successors.Count > successorListHighWaterMark)
                {
                    successorListHighWaterMark = m_Successors.Count;
                }
                return(true);
            }

            return(false);
        }
Exemple #6
0
        void AddNeighbourNodeOfAttachVolume(int xOffset, int yOffset, Vector2Int parentPos, AStarPathfinder aStarSearch)
        {
            int src_xOffset = xOffset;
            int src_yOffset = yOffset;

            bool flag   = true;
            int  volume = aStarSearch.volume;

            if (src_xOffset != 0)
            {
                xOffset += ((xOffset >= 0) ? volume : -volume);
                for (int y = -volume; y <= volume; y++)
                {
                    flag = ValidNeigbour(xOffset, y + src_yOffset);
                    if (flag == false)
                    {
                        return;
                    }
                }
            }

            if (src_yOffset != 0)
            {
                yOffset += ((yOffset >= 0) ? volume : -volume);
                for (int x = -volume; x <= volume; x++)
                {
                    flag = ValidNeigbour(x + src_xOffset, yOffset);
                    if (flag == false)
                    {
                        return;
                    }
                }
            }

            if (flag && !(parentPos.x == nodeIndex.x + src_xOffset && parentPos.y == nodeIndex.y + src_yOffset))
            {
                Vector2Int    neighbourPos = new Vector2Int(nodeIndex.x + src_xOffset, nodeIndex.y + src_yOffset);
                MapSearchNode newNode      = pathfinder.AllocateMapSearchNode(neighbourPos);
                aStarSearch.AddSuccessor(newNode);
            }
        }
Exemple #7
0
        // constructor just initialises private data
        public AStarPathfinder(Grid map, int volume = 0)
        {
            this.gridMap = map;
            this.volume  = volume;

            // Allocate all lists
            m_OpenList   = new List <Node>(kPreallocatedOpenListSlots);
            m_ClosedList = new List <Node>(kPreallocatedClosedListSlots);
            m_Successors = new List <Node>(kPreallocatedSuccessorSlots);

            m_FixedSizeAllocator = new List <Node>(kPreallocatedNodes);
            for (int i = 0; i < kPreallocatedNodes; ++i)
            {
                Node n = new Node();
                m_FixedSizeAllocator.Add(n);
            }

            mapSearchNodePool = new List <MapSearchNode>(kPreallocatedMapSearchNodes);
            for (int i = 0; i < kPreallocatedMapSearchNodes; ++i)
            {
                MapSearchNode msn = new MapSearchNode(this);
                mapSearchNodePool.Add(msn);
            }
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="grid"></param>
        /// <param name="startPos"></param>
        /// <param name="goalPos"></param>
        /// <param name="pathfinder"></param>
        /// <returns></returns>
        public static List <Vector2Int> Pathfind(Grid grid, Vector2Int startPos, Vector2Int goalPos, AStarPathfinder pathfinder = null)
        {
            // Reset the allocated MapSearchNode pointer
            if (pathfinder == null)
            {
                pathfinder = new AStarPathfinder(grid);
            }

            pathfinder.InitiatePathfind();

            // Create a start state
            MapSearchNode nodeStart = pathfinder.AllocateMapSearchNode(startPos);

            // Define the goal state
            MapSearchNode nodeEnd = pathfinder.AllocateMapSearchNode(goalPos);

            // Set Start and goal states
            pathfinder.SetStartAndGoalStates(nodeStart, nodeEnd);

            // Set state to Searching and perform the search
            AStarPathfinder.SearchState searchState = AStarPathfinder.SearchState.Searching;
            uint searchSteps = 0;

            do
            {
                searchState = pathfinder.SearchStep();
                searchSteps++;
            }while (searchState == AStarPathfinder.SearchState.Searching);

            // Search complete
            bool pathfindSucceeded = (searchState == AStarPathfinder.SearchState.Succeeded);

            if (pathfindSucceeded)
            {
                // Success
                List <Vector2Int> newPath = new List <Vector2Int>();
                int numSolutionNodes      = 0; // Don't count the starting cell in the path length

                // Get the start node
                MapSearchNode node = pathfinder.GetSolutionStart();
                newPath.Add(node.nodeIndex);
                ++numSolutionNodes;

                // Get all remaining solution nodes
                for (; ;)
                {
                    node = pathfinder.GetSolutionNext();

                    if (node == null)
                    {
                        break;
                    }

                    ++numSolutionNodes;
                    newPath.Add(node.nodeIndex);
                }
                ;

                // Once you're done with the solution we can free the nodes up
                pathfinder.FreeSolutionNodes();

                return(newPath);
                //System.Console.WriteLine("Solution path length: " + numSolutionNodes);
                //System.Console.WriteLine("Solution: " + newPath.ToString());
            }
            else if (searchState == AStarPathfinder.SearchState.Failed)
            {
                // FAILED, no path to goal
                Debug.LogError("Pathfind FAILED!");
            }

            return(null);
        }
Exemple #9
0
 public bool IsGoal(MapSearchNode nodeGoal)
 {
     return(nodeIndex.x == nodeGoal.nodeIndex.x && nodeIndex.y == nodeGoal.nodeIndex.y);
 }
Exemple #10
0
 public bool IsSameState(MapSearchNode rhs)
 {
     return(nodeIndex.x == rhs.nodeIndex.x &&
            nodeIndex.y == rhs.nodeIndex.y);
 }
Exemple #11
0
 // given this node, what does it cost to move to successor. In the case
 // of our map the answer is the map terrain value at this node since that is
 // conceptually where we're moving
 public FP GetCost(MapSearchNode successor)
 {
     // Implementation specific
     return(GetMap(successor.nodeIndex.x, successor.nodeIndex.y));
 }