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