// 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)));
}
// 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
}
// 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);
}
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);
}
}
// 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);
}
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);
}
}
// 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);
}
public bool IsGoal(MapSearchNode nodeGoal)
{
return(nodeIndex.x == nodeGoal.nodeIndex.x && nodeIndex.y == nodeGoal.nodeIndex.y);
}
public bool IsSameState(MapSearchNode rhs)
{
return(nodeIndex.x == rhs.nodeIndex.x &&
nodeIndex.y == rhs.nodeIndex.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 FP GetCost(MapSearchNode successor)
{
// Implementation specific
return(GetMap(successor.nodeIndex.x, successor.nodeIndex.y));
}