static private void AddNeighbour(List <MapPosition> list, MapPosition pos) { int Result = GetWalkMap(pos); if (Result != -1) { list.Add(new MapPosition { X = pos.X, Y = pos.Y, Z = pos.Z }); } }
public void FindsPath() { var Start = new MapPosition { Z = 0, X = 0, Y = 0 }; var End = new MapPosition { Z = 4, X = 9, Y = 9 }; var PathFinder = new AStar <MapPosition>(GetNeighbours); var Result = new List <MapPosition>(15); PathFinder.Search(Start, End, ref Result, MapPosition.Heuristic); PrintGrid(Result); Assert.IsTrue(Result.Count > 0); }
static private void PrintGrid(List <MapPosition> solution = null) { Trace.WriteLine(""); for (int z = 0; z <= 4; z++) { for (int x = 0; x <= 9; x++) { for (int y = 0; y <= 9; y++) { var Pos = new MapPosition { X = x, Y = y, Z = z }; var Current = GetWalkMap(Pos); var InSolution = false; if (solution != null) { foreach (var Step in solution) { if (Pos.Equals(Step)) { InSolution = true; break; } } } if (InSolution) { Trace.Write("# "); } else { Trace.Write((Current == -1 ? "X" : Current.ToString()) + " "); } } Trace.WriteLine(""); } Trace.WriteLine(System.Environment.NewLine); } }
static private IEnumerable <MapPosition> GetNeighbours(MapPosition pos) { var Result = new List <MapPosition>(); AddNeighbour(Result, new MapPosition { X = pos.X - 1, Y = pos.Y, Z = pos.Z }); // left AddNeighbour(Result, new MapPosition { X = pos.X, Y = pos.Y - 1, Z = pos.Z }); // bottom AddNeighbour(Result, new MapPosition { X = pos.X + 1, Y = pos.Y, Z = pos.Z }); // right AddNeighbour(Result, new MapPosition { X = pos.X, Y = pos.Y + 1, Z = pos.Z }); // top AddNeighbour(Result, new MapPosition { X = pos.X - 1, Y = pos.Y + 1, Z = pos.Z }); // left bottom AddNeighbour(Result, new MapPosition { X = pos.X + 1, Y = pos.Y + 1, Z = pos.Z }); // right bottom AddNeighbour(Result, new MapPosition { X = pos.X - 1, Y = pos.Y - 1, Z = pos.Z }); // left top AddNeighbour(Result, new MapPosition { X = pos.X + 1, Y = pos.Y - 1, Z = pos.Z }); // right top AddNeighbour(Result, new MapPosition { X = pos.X, Y = pos.Y, Z = pos.Z + 1 }); // up AddNeighbour(Result, new MapPosition { X = pos.X, Y = pos.Y, Z = pos.Z - 1 }); // bottom return(Result); }
public static float Heuristic(MapPosition current, MapPosition goal) { return(current.Cost(goal)); }
static private int GetWalkMap(MapPosition pos) { int[,,] Map = { { { 1, -1, 1, 1, 1, -1, 1, 1, 1, 1 }, { 1, -1, 1, -1, 1, -1, 1, 1, 1, 1 }, { 1, -1, 1, -1, 1, -1, 1, 1, 1, 1 }, { 1, -1, 1, -1, 1, -1, 1, 1, 1, 1 }, { 1, -1, 1, -1, 1, -1, 1, 1, 1, 1 }, { 1, -1, 1, -1, 1, -1, 1, 1, 1, 1 }, { 1, -1, 1, -1, 1, -1, 1, 1, 1, 1 }, { 1, -1, 1, -1, 1, -1, 1, 1, 1, 1 }, { 1, -1, 1, -1, 1, -1, 1, 1, 2, 1 }, { 1, 1, 1, -1, 1, 1, 1, 3, 1, 1 } }, { { -1, -1, -1, -1, -1, -1, -1, -1, -1, 1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, 9, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 } }, { { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, -1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, -1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, -1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, -1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, -1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, -1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, -1, 1 }, { 1, -1, -1, -1, -1, -1, -1, -1, -1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 } }, { { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, 1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 }, { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 } }, { { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }, { 1, 1, 1, 1, 1, 2, 1, 1, 1, 1 }, { 1, 1, 1, 1, 2, 3, 2, 1, 1, 1 }, { 1, 1, 1, 2, 3, 4, 3, 2, 1, 1 }, { 1, 1, 2, 3, 4, 5, 4, 3, 2, 1 }, { 1, 1, 1, 2, 3, 4, 3, 2, 1, 1 }, { 1, 1, 1, 1, 2, 3, 2, 1, 1, 1 }, { 1, 1, 1, 1, 1, 2, 1, 1, 1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }, { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 } } }; if ((pos.X < 0) || (pos.X > 9) || (pos.Y < 0) || (pos.Y > 9) || (pos.Z < 0) || (pos.Z > 4)) { return(-1); } return(Map[pos.Z, pos.X, pos.Y]); }