private static int RetracePath(Grid grid, Node startNode, Node endNode, ref PathData[] path)
{
Node currentNode = endNode;
int len = 0;
while (!Equals(currentNode, startNode))
{
path[len] = new PathData(currentNode.x,
currentNode.y,
Node.GetType(currentNode));
++len;
currentNode = Grid.GetParentNode(grid, currentNode);
}
return(len);
}
public int GetType(int x, int y)
{
return(Node.GetType(Grid.GetNode(grid, x, y)));
}
// grid: grid to search in
// startPos: starting position
// targetPos: ending position
// mask: the walkable nodes for this path
// path: an array for holding the path
public static int Find(Grid grid,
Point2 startPos,
Point2 targetPos,
int mask,
ref PathData[] path)
{
Node startNode = Grid.GetNode(grid, startPos.x, startPos.y);
Node targetNode = Grid.GetNode(grid, targetPos.x, targetPos.y);
int gridwidth = Grid.GetWidth(grid);
Grid.ClearBuffer(grid);
Grid.AddToOpenSet(grid, startNode);
while (Grid.GetOpenSetCount(grid) > 0)
{
Node currentNode = Grid.RemoveFirstFromOpenSet(grid);
int ci = Grid.GetIndex(gridwidth, currentNode.x, currentNode.y);
Grid.AddToClosedSet(grid, ci);
// Return when looking for a path that goes from and to the
// same node
if (currentNode.Equals(targetNode))
{
return(RetracePath(grid, startNode, targetNode, ref path));
}
int nodeIndex = Grid.GetIndex(gridwidth, currentNode.x, currentNode.y);
int numNeighbours = Grid.UpdateNeighboursCache(grid, nodeIndex);
for (int i = 0; i < numNeighbours; ++i)
{
Node neighbour = Grid.GetNeighbour(grid, i);
int ni = Grid.GetIndex(gridwidth, neighbour.x, neighbour.y);
int nt = Node.GetType(neighbour);
bool isWalkable = (mask & nt) == 0;
// Continue searching when neighbour is not walkable,
// or already checked
if (!isWalkable || Grid.ClosedSetContains(grid, ni))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode.x,
currentNode.y,
neighbour.x,
neighbour.y);
bool openContainsNeighbour = Grid.OpenSetContains(grid, neighbour);
if (newMovementCostToNeighbour >= neighbour.gCost && openContainsNeighbour)
{
continue;
}
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour.x, neighbour.y, targetNode.x, targetNode.y);
neighbour.fCost = neighbour.gCost + neighbour.hCost;
Grid.LinkNode(grid, ni, ci);
if (!openContainsNeighbour)
{
Grid.AddToOpenSet(grid, neighbour);
}
}
}
return(0);
}
