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