private static pathNode_t[] getAllNeighborNodes(pathNode_t center, ref Map map)
{
pathNode_t[] neighbors = new pathNode_t[4];
neighbors[0] = new pathNode_t(center.X, center.Y - 1, map.MoveCost(center.X, center.Y, center.X, center.Y - 1)); //NORTH
neighbors[1] = new pathNode_t(center.X - 1, center.Y, map.MoveCost(center.X, center.Y, center.X - 1, center.Y)); //WEST
neighbors[2] = new pathNode_t(center.X + 1, center.Y, map.MoveCost(center.X, center.Y, center.X + 1, center.Y)); //EAST
neighbors[3] = new pathNode_t(center.X, center.Y + 1, map.MoveCost(center.X, center.Y, center.X, center.Y + 1)); //SOUTH
return(neighbors);
}
public static pathNode_t[] AStar(ref Map map, pathNode_t start, pathNode_t end, int maxRadius)
{
if (maxRadius == 0)
{
return(new pathNode_t[0]);
}
PriorityQueue <pathNode_t> frontier = new PriorityQueue <pathNode_t>(new PathNodeCostComparer());
frontier.Enqueue(start);
Dictionary <pathNode_t, pathNode_t> destinationToSourceMap = new Dictionary <pathNode_t, pathNode_t>();
destinationToSourceMap[start] = start;
Dictionary <pathNode_t, int> pathCosts = new Dictionary <pathNode_t, int>();
pathCosts[start] = 0;
while (frontier.Count != 0)
{
pathNode_t curNode = frontier.Dequeue();
if (curNode.Equals(end))
{
break;
}
pathNode_t[] neighbors = getAllNeighborNodes(curNode, ref map);
for (int index = 0; index < neighbors.Length; index++)
{
pathNode_t neighbor = neighbors[index];
if (!map.IsValidEntityPosition(neighbor.X, neighbor.Y))
{
int newCost = pathCosts[curNode] + neighbor.Cost;
if (!pathCosts.ContainsKey(neighbor))
{
pathCosts[neighbor] = newCost;
frontier.Enqueue(new pathNode_t(neighbor.X, neighbor.Y, newCost));
destinationToSourceMap[neighbor] = curNode;
}
else if (newCost < pathCosts[neighbor])
{
pathCosts[neighbor] = newCost;
frontier.Enqueue(new pathNode_t(neighbor.X, neighbor.Y, newCost));
destinationToSourceMap[neighbor] = curNode;
}
}
}
}
return(new pathNode_t[0]);
}
public bool Equals(pathNode_t otherNode)
{
return(this.X == otherNode.X && this.Y == otherNode.Y);
}
