/// <summary>
/// Returns "safe" coordinates 1 block (NSEW) from <paramref name="start"/> where they are not the first point, and the distance to end is less than <paramref name="maxDistance"/>
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="size"></param>
/// <param name="maxDistance"></param>
/// <returns></returns>
private PathCoordinate[] GetPathOptions(PathCoordinate start, PathCoordinate end, Size size, double maxDistance)
{
int height = 0;
if (CheckOffset(start.Coordinate.Offset(0, 1, 0), size) == 1)
{
height = 1;
}
List <PathCoordinate> pathOptions = new List <PathCoordinate>();
PathCoordinate eastPoint = GetSafeCoordinate(start.Coordinate.Offset(0, 0, 1), size, height);
PathCoordinate northPoint = GetSafeCoordinate(start.Coordinate.Offset(-1, 0, 0), size, height);
PathCoordinate southPoint = GetSafeCoordinate(start.Coordinate.Offset(1, 0, 0), size, height);
PathCoordinate westPoint = GetSafeCoordinate(start.Coordinate.Offset(0, 0, -1), size, height);
if (eastPoint != null && !eastPoint.Checked && eastPoint.DistanceTo(end) < maxDistance)
{
pathOptions.Add(eastPoint);
}
if (northPoint != null && !northPoint.Checked && northPoint.DistanceTo(end) < maxDistance)
{
pathOptions.Add(northPoint);
}
if (southPoint != null && !southPoint.Checked && southPoint.DistanceTo(end) < maxDistance)
{
pathOptions.Add(southPoint);
}
if (westPoint != null && !westPoint.Checked && westPoint.DistanceTo(end) < maxDistance)
{
pathOptions.Add(westPoint);
}
return(pathOptions.ToArray());
}
private List <PathCoordinate> GeneratePath(PathCoordinate start, PathCoordinate end, Size size, double maxDistance)
{
SortedSet <PathCoordinate> sortedPath = new SortedSet <PathCoordinate>(new PathCoordinateDistanceToTargetComparer());
List <PathCoordinate> path = new List <PathCoordinate>();
_firstCoordinate = start;
_firstCoordinate.DistanceToTarget = _firstCoordinate.DistanceTo(end);
PathCoordinate currentCoordinate = start;
int loopsLeft = 1000;
while (currentCoordinate != null && currentCoordinate != end && loopsLeft > 0)
{
currentCoordinate.Checked = true;
loopsLeft--;
PathCoordinate[] pathOptions = GetPathOptions(currentCoordinate, end, size, maxDistance);
foreach (var option in pathOptions)
{
double distance = currentCoordinate.DistanceFromStart + currentCoordinate.DistanceTo(option);
bool existsInPath = sortedPath.Contains(option);
if (!existsInPath || distance < option.DistanceFromStart)
{
option.PreviousCoordinate = currentCoordinate;
if (existsInPath)
{
sortedPath.Remove(option);
option.SetDistanceFromStartAndTarget(distance, option.DistanceTo(end));
sortedPath.Add(option);
}
else
{
option.SetDistanceFromStartAndTarget(distance, option.DistanceTo(end));
sortedPath.Add(option);
}
}
}
sortedPath.Remove(currentCoordinate);
if (sortedPath.Count > 0)
{
currentCoordinate = sortedPath.Min;
}
else
{
break;
}
}
if (currentCoordinate == null || currentCoordinate == _firstCoordinate)
{
return(null);
}
// currentCoordinate is destination so walk up previous coordinates and add to list, then reverse
List <PathCoordinate> result = new List <PathCoordinate> {
currentCoordinate
};
while (currentCoordinate.PreviousCoordinate != null)
{
result.Add(currentCoordinate.PreviousCoordinate);
currentCoordinate = currentCoordinate.PreviousCoordinate;
}
result.Reverse();
return(result);
}