// 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); }
private void RenderTiles(AStarPathfinder pathfinder) { MapSearchNode node = pathfinder.GetSolutionStart(); for ( ;;) { node = pathfinder.GetSolutionNext(); if (node == null) { break; } GameObject obj = objs[node.position.x, node.position.y]; if (obj != null) { obj.GetComponent <MeshRenderer>().material.color = Color.green; } } ; }
// 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); } }
// 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); } }
// 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; }
// 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 }
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); } ; // Once you're done with the solution we can free the nodes up pathfinder.FreeSolutionNodes(); System.Console.WriteLine("Solution path length: " + numSolutionNodes); System.Console.WriteLine("Solution: " + newPath.ToString()); } else if (searchState == AStarPathfinder.SearchState.Failed) { // FAILED, no path to goal System.Console.WriteLine("Pathfind FAILED!"); } return(pathfindSucceeded); }
// Main public static int main(string[] args) { Console.WriteLine("STL A* Search implementation\n(C)2001 Justin Heyes-Jones"); // Our sample problem defines the world as a 2d array representing a terrain // Each element contains an integer from 0 to 5 which indicates the cost // of travel across the terrain. Zero means the least possible difficulty // in travelling (think ice rink if you can skate) whilst 5 represents the // most difficult. 9 indicates that we cannot pass. // Create an instance of the search class... AStarSearch astarsearch = new AStarSearch(); bool[] path = new bool[MAP_HEIGHT * MAP_WIDTH]; // Create a start state MapSearchNode nodeStart = new MapSearchNode(); nodeStart.x = 0; nodeStart.y = 0; // Define the goal state MapSearchNode nodeEnd = new MapSearchNode(); nodeEnd.x = 19; nodeEnd.y = 19; // Set Start and goal states astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd); SearchState searchState; uint searchSteps = 0; do { searchState = astarsearch.SearchStep(); searchSteps++; } while (searchState == SearchState.Searching); if (searchState == SearchState.Succeeded) { Console.Write("Search found goal state"); MapSearchNode node = astarsearch.GetSolutionStart() as MapSearchNode; Console.WriteLine( "Displaying solution"); int steps = 0; node.PrintNodeInfo(); for (; ; ) { node = astarsearch.GetSolutionNext() as MapSearchNode; if (node == null) { break; } path[node.y * MAP_HEIGHT + node.x] = true; //node.PrintNodeInfo(); steps++; }; for (int y = 0; y < MAP_HEIGHT; y++) { for (int x = 0; x < MAP_WIDTH; x++) { Console.Write(path[y * MAP_HEIGHT + x] ? "x" : "o"); } Console.WriteLine(); } Console.WriteLine("Solution steps {0}", steps); // Once you're done with the solution you can free the nodes up astarsearch.FreeSolutionNodes(); } else if (searchState == SearchState.Failed) { Console.WriteLine("Search terminated. Did not find goal state"); } // Display the number of loops the search went through Console.WriteLine("searchSteps : " + searchSteps); return 0; }
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(m_map.GetMap(successor.position.x, successor.position.y)); }
IEnumerator KK() { pathfinder = new AStarPathfinder(); // Pathfind(new NodePosition(0, 0), new NodePosition(2, 4), pathfinder); // Reset the allocated MapSearchNode pointer pathfinder.InitiatePathfind(); // Create a start state MapSearchNode nodeStart = pathfinder.AllocateMapSearchNode(new NodePosition(0, 0)); // Define the goal state MapSearchNode nodeEnd = pathfinder.AllocateMapSearchNode(new NodePosition(GOAL_X, GOAL_Y)); // 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; yield return(null); do { // if (Input.GetKey(KeyCode.J)) { Debug.Log("xx-- getkeydown"); searchState = pathfinder.SearchStep(); searchSteps++; RenderTiles(pathfinder); } if (searchState == AStarPathfinder.SearchState.Searching) { yield return(null); } }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); // GameObject obj = objs[node.position.x, node.position.y]; if (obj != null) { obj.GetComponent <MeshRenderer>().material.color = Color.green; } } ; // Once you're done with the solution we can free the nodes up pathfinder.FreeSolutionNodes(); Debug.Log("xx-- Solution path length: " + numSolutionNodes); Debug.Log("xx-- Solution: " + newPath.ToString()); } else if (searchState == AStarPathfinder.SearchState.Failed) { // FAILED, no path to goal Debug.Log("xx-- Pathfind FAILED!"); } }
// 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(AStarSearch astarsearch, PuzzleState parentState) { MapSearchNode parent_node = parentState as MapSearchNode; int parent_x = -1; int parent_y = -1; if (parent_node != null) { parent_x = (int)parent_node.x; parent_y = (int)parent_node.y; } MapSearchNode NewNode; // push each possible move except allowing the search to go backwards if ((GetMap(x - 1, y) < 9) && !((parent_x == x - 1) && (parent_y == y)) ) { NewNode = new MapSearchNode(x - 1, y); astarsearch.AddSuccessor(NewNode); } if ((GetMap(x, y - 1) < 9) && !((parent_x == x) && (parent_y == y - 1)) ) { NewNode = new MapSearchNode(x, y - 1); astarsearch.AddSuccessor(NewNode); } if ((GetMap(x + 1, y) < 9) && !((parent_x == x + 1) && (parent_y == y)) ) { NewNode = new MapSearchNode(x + 1, y); astarsearch.AddSuccessor(NewNode); } if ((GetMap(x, y + 1) < 9) && !((parent_x == x) && (parent_y == y + 1)) ) { NewNode = new MapSearchNode(x, y + 1); astarsearch.AddSuccessor(NewNode); } return true; }
public bool IsGoal(MapSearchNode nodeGoal) { return(position.x == nodeGoal.position.x && position.y == nodeGoal.position.y); }
private void FindPath() { AStarPathfinder pathfinder = new AStarPathfinder(); NodePosition startPos = m_startGrid.m_position; NodePosition endPos = m_endGrid.m_position; int width, height; var mapData = GetMapData(out width, out height); var map = new Map(mapData, width, height); pathfinder.InitiatePathfind(map); MapSearchNode nodeStart = pathfinder.AllocateMapSearchNode(startPos); MapSearchNode nodeEnd = pathfinder.AllocateMapSearchNode(endPos); pathfinder.SetStartAndGoalStates(nodeStart, nodeEnd); 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); } ; // Once you're done with the solution we can free the nodes up pathfinder.FreeSolutionNodes(); m_path = newPath; _sceneView.Repaint(); Debug.Log("Solution path length: " + numSolutionNodes); Debug.Log("Solution: " + newPath.ToString()); } else if (searchState == AStarPathfinder.SearchState.Failed) { // FAILED, no path to goal Debug.Log("Pathfind FAILED!"); } }