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