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