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