public GridNodeAStar(GridNode thisGridNode, GridNode originGridNode, GridNode targetGridNode)
{
X = thisGridNode.X;
Y = thisGridNode.Y;
GCost = Trigonometry.GetGridDistance(new Point(X, Y), new Point(originGridNode.X, originGridNode.Y));
HCost = Trigonometry.GetGridDistance(new Point(X, Y), new Point(targetGridNode.X, targetGridNode.Y));
}
/// <summary>Returns the node that need to be followed in order to reach the destination. Currently only works
/// on a Grid, without costs associated with the movements.</summary>
// This method is actually faster than the one above, even though it should not be. Might have to looks at some
// of the algorithm to improve performance.
public List <GridNodeAStar> GetAStarNodesWithList(string originId, string targetId)
{
var origin = GetNode(originId);
var target = GetNode(targetId);
var openSet = new List <GridNodeAStar>();
var closedSet = new HashSet <GridNodeAStar>();
var aStarOrigin = new GridNodeAStar(origin, origin, target);
openSet.Add(aStarOrigin);
while (openSet.Any())
{
var currentNode = openSet[0];
for (var i = 1; i < openSet.Count; i++)
{
if (openSet[i].FCost < currentNode.FCost || openSet[i].FCost == currentNode.FCost && openSet[i].HCost < currentNode.HCost)
{
currentNode = openSet[i];
}
}
openSet.Remove(currentNode);
closedSet.Add(currentNode);
if (currentNode.Id == target.Id)
{
// Found the target node. Retracing steps.
var path = new List <GridNodeAStar>();
var pathPosition = currentNode;
while (pathPosition.Id != origin.Id)
{
path.Add(pathPosition);
pathPosition = pathPosition.Parent;
}
path.Reverse();
return(path);
}
foreach (var neighbour in GetNeighbours(currentNode).Select(node => new GridNodeAStar(node, origin, target)))
{
if (closedSet.FirstOrDefault(node => node.Id == neighbour.Id) != null)
{
continue;
}
var newMovementCostToNeighbour = currentNode.GCost + Trigonometry.GetGridDistance(
new Point(currentNode.X, currentNode.Y),
new Point(neighbour.X, neighbour.Y));
var neighbourInOpenSet = openSet.FirstOrDefault(node => node.Id == neighbour.Id);
if (neighbourInOpenSet == null)
{
// Neighbour has not been analyzed yet, so we are generating the costs and adding to open set.
neighbour.GCost = newMovementCostToNeighbour;
neighbour.HCost = Trigonometry.GetGridDistance(
new Point(neighbour.X, neighbour.Y),
new Point(target.X, target.Y));
neighbour.Parent = currentNode;
openSet.Add(neighbour);
}
if (neighbourInOpenSet != null && newMovementCostToNeighbour > neighbourInOpenSet.GCost)
{
// Neighbour already exists in open set, but the new movement cost is cheaper, so we're updating it.
neighbourInOpenSet.GCost = newMovementCostToNeighbour;
neighbourInOpenSet.HCost = Trigonometry.GetDistance(
new Point(neighbourInOpenSet.X, neighbourInOpenSet.Y),
new Point(target.X, target.Y));
neighbourInOpenSet.Parent = currentNode;
}
}
}
return(new List <GridNodeAStar>());
}