/// <summary> /// Find the shortest path using the A* algorithm and a Euclidean distance heuristic /// </summary> public TilePath Plan(TilePosition start, TileRect end) { heap.Clear(); searchedTilesOverlay.Clear(); var startNode = heap.NodeAt(start); startNode.Predecessor = null; heap.DecreaseKey(startNode, 0, 0); var currentNode = startNode; while (!heap.IsEmpty && !end.Contains((currentNode = heap.ExtractMin()).Position)) { searchedTilesOverlay.Add(currentNode.Position); foreach (var n in currentNode.Neighbors) { float newDistanceFromStart = currentNode.DistanceFromStart // Cost so far + TilePosition.EuclideanDistance(currentNode.Position, n.Position) // Edge cost + TilePosition.EuclideanDistance(n.Position, end); // Estimated cost to end if (n.DistanceFromStart > newDistanceFromStart) { n.Predecessor = currentNode; heap.DecreaseKey(n, newDistanceFromStart, newDistanceFromStart); } } } heap.SetOverlayToComputedPath(this.computedPathOverlay, currentNode); searchedTilesOverlay.Clear(); searchedTilesOverlay.SetRect(end); if (!end.Contains(currentNode.Position)) return null; return this.MakePath(currentNode); }
/// <summary> /// Returns the distance from the TilePosition to the closest tile in the TileRect /// </summary> /// <param name="a">Tile to find the distance from</param> /// <param name="r">TileRect to find the distance to</param> /// <returns></returns> public static float EuclideanDistance(TilePosition a, TileRect r) { if (r.Contains(a)) { // Inside rect return(0); } if (r.CMin <= a.Column && a.Column <= r.CMax) { // Closest point is on one of the horiztonal edges return(Math.Min(Math.Abs(a.Row - r.RMin), Math.Abs(a.Row - r.RMax))); } if (r.RMin <= a.Row && a.Row <= r.RMax) { // Closest point is on one of the vertical edges return(Math.Min(Math.Abs(a.Column - r.CMin), Math.Abs(a.Column - r.CMax))); } // Closest point is one of the corners. return (Math.Min( Math.Min( EuclideanDistance(a, new TilePosition(r.CMin, r.RMin)), EuclideanDistance(a, new TilePosition(r.CMin, r.RMax))), Math.Min( EuclideanDistance(a, new TilePosition(r.CMax, r.RMin)), EuclideanDistance(a, new TilePosition(r.CMax, r.RMax))))); }
/// <summary> /// Find the shortest path using the A* algorithm and a Euclidean distance heuristic /// </summary> public TilePath Plan(TilePosition start, TileRect end) { heap.Clear(); searchedTilesOverlay.Clear(); var startNode = heap.NodeAt(start); startNode.Predecessor = null; heap.DecreaseKey(startNode, 0, 0); var currentNode = startNode; while (!heap.IsEmpty && !end.Contains((currentNode = heap.ExtractMin()).Position)) { searchedTilesOverlay.Add(currentNode.Position); foreach (var n in currentNode.Neighbors) { float newDistanceFromStart = currentNode.DistanceFromStart // Cost so far + TilePosition.EuclideanDistance(currentNode.Position, n.Position) // Edge cost + TilePosition.EuclideanDistance(n.Position, end); // Estimated cost to end if (n.DistanceFromStart > newDistanceFromStart) { n.Predecessor = currentNode; heap.DecreaseKey(n, newDistanceFromStart, newDistanceFromStart); } } } heap.SetOverlayToComputedPath(this.computedPathOverlay, currentNode); searchedTilesOverlay.Clear(); searchedTilesOverlay.SetRect(end); if (!end.Contains(currentNode.Position)) { return(null); } return(this.MakePath(currentNode)); }
/// <summary> /// Returns the distance from the TilePosition to the closest tile in the TileRect /// </summary> /// <param name="a">Tile to find the distance from</param> /// <param name="r">TileRect to find the distance to</param> /// <returns></returns> public static float EuclideanDistance(TilePosition a, TileRect r) { if (r.Contains(a)) // Inside rect return 0; if (r.CMin <= a.Column && a.Column <= r.CMax) // Closest point is on one of the horiztonal edges return Math.Min(Math.Abs(a.Row - r.RMin), Math.Abs(a.Row - r.RMax)); if (r.RMin <= a.Row && a.Row <= r.RMax) // Closest point is on one of the vertical edges return Math.Min(Math.Abs(a.Column - r.CMin), Math.Abs(a.Column - r.CMax)); // Closest point is one of the corners. return Math.Min( Math.Min( EuclideanDistance(a, new TilePosition(r.CMin, r.RMin)), EuclideanDistance(a, new TilePosition(r.CMin, r.RMax))), Math.Min( EuclideanDistance(a, new TilePosition(r.CMax, r.RMin)), EuclideanDistance(a, new TilePosition(r.CMax, r.RMax)))); }