// 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 }
// Here's the heuristic function that estimates the distance from a Node // to the Goal. public float GoalDistanceEstimate(MapSearchNode nodeGoal) { double X = (double)position.x - (double)nodeGoal.position.x; double Y = (double)position.y - (double)nodeGoal.position.y; return((float)System.Math.Sqrt((X * X) + (Y * Y))); }
void AddNeighbourNode(int xOffset, int yOffset, NodePosition parentPos, AStarPathfinder aStarSearch) { if (ValidNeigbour(xOffset, yOffset) && !(parentPos.x == position.x + xOffset && parentPos.y == position.y + yOffset)) { NodePosition neighbourPos = new NodePosition(position.x + xOffset, position.y + yOffset); MapSearchNode newNode = pathfinder.AllocateMapSearchNode(neighbourPos); aStarSearch.AddSuccessor(newNode); } }
// 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) { NodePosition parentPos = new NodePosition(0, 0); if (parentNode != null) { parentPos = parentNode.position; } // 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); return(true); }
// 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); }
// constructor just initialises private data public AStarPathfinder() { // 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); } }
public bool IsSameState(MapSearchNode rhs) { return(position.x == rhs.position.x && position.y == rhs.position.y); }
// 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 float GetCost(MapSearchNode successor) { // Implementation specific return(Map.Instance.GetMap(successor.position.x, successor.position.y)); }
public bool IsGoal(MapSearchNode nodeGoal) { return(position.x == nodeGoal.position.x && position.y == nodeGoal.position.y); }
static bool Pathfind(NodePosition startPos, NodePosition goalPos, AStarPathfinder pathfinder) { // Reset the allocated MapSearchNode pointer 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 Path newPath = new Path(); int numSolutionNodes = 0; // Don't count the starting cell in the path length // Get the start node MapSearchNode node = pathfinder.GetSolutionStart(); newPath.Add(node.position); ++numSolutionNodes; // Get all remaining solution nodes for (; ;) { node = pathfinder.GetSolutionNext(); if (node == null) { break; } ++numSolutionNodes; newPath.Add(node.position); // DRAW PATH TO GOAL var vnName = $"vnx{node.position.x}y{node.position.y}"; VisualNode foundVisualNode = _visualGrid.FindName(vnName) as VisualNode; foundVisualNode?.SetDot(true); } ; // Once you're done with the solution we can free the nodes up pathfinder.FreeSolutionNodes(); Debug.WriteLine("Solution path length: " + numSolutionNodes); Debug.WriteLine("Solution: " + newPath.ToString()); } else if (searchState == AStarPathfinder.SearchState.Failed) { // FAILED, no path to goal Debug.WriteLine("Pathfind FAILED!"); } return(pathfindSucceeded); }