public void Move (MapRenderer mapRenderer, int goal_minitile_x, int goal_minitile_y) { if (false /*flying unit*/) { // easier case, take the direct route } else { // Console.WriteLine ("FindPath from {0},{1} -> {2},{3}", // x >> 2, y >> 2, // goal_minitile_x, goal_minitile_y); navigateDestination = new MapPoint (goal_minitile_x, goal_minitile_y); AStarSolver astar = new AStarSolver (mapRenderer); navigatePath = astar.FindPath (new MapPoint (x >> 2, y >> 2), navigateDestination); sprite.Debug = true; if (navigatePath != null) NavigateAlongPath (); } }
void NavigateAlongPath () { int sprite_x, sprite_y; sprite.GetPosition (out sprite_x, out sprite_y); Console.WriteLine ("starting pixel position = {0},{1}", sprite_x, sprite_y); startCurrentSegment = navigatePath[0]; navigatePath.RemoveAt (0); endCurrentSegment = navigatePath[0]; sprite.Face (ClassifyDirection (startCurrentSegment, endCurrentSegment)); int start_pixel_x = X * 4 + 4; int start_pixel_y = X * 4 + 4; dest_pixel_x = endCurrentSegment.X * 4 + 4; dest_pixel_y = endCurrentSegment.Y * 4 + 4; delta_x = dest_pixel_x - start_pixel_x; delta_y = dest_pixel_y - start_pixel_y; pixel_x = start_pixel_x; pixel_y = start_pixel_y; sprite.RunScript (AnimationType.Walking); Events.Tick += NavigateTick; }
int ClassifyDirection (MapPoint startCurrentSegment, MapPoint endCurrentSegment) { if (startCurrentSegment.X < endCurrentSegment.X) { if (startCurrentSegment.Y < endCurrentSegment.Y) { Console.WriteLine ("1 startCurrentSegment.Y < endCurrentSegment.Y"); return 5; } else if (startCurrentSegment.Y == endCurrentSegment.Y) { Console.WriteLine ("face = 12"); return 12; } else { Console.WriteLine ("1 startCurrentSegment.Y > endCurrentSegment.Y"); return 2; } } else if (startCurrentSegment.X == endCurrentSegment.X) { if (startCurrentSegment.Y < endCurrentSegment.Y) { Console.WriteLine ("2 startCurrentSegment.Y < endCurrentSegment.Y"); return 7; } else if (startCurrentSegment.Y > endCurrentSegment.Y) { Console.WriteLine ("2 startCurrentSegment.Y > endCurrentSegment.Y"); return 0; } else { Console.WriteLine ("@#(*@!#&( shouldn't happen"); return 0; } } else { if (startCurrentSegment.Y < endCurrentSegment.Y) { Console.WriteLine ("3 startCurrentSegment.Y < endCurrentSegment.Y"); return 9; } else if (startCurrentSegment.Y == endCurrentSegment.Y) { Console.WriteLine ("face = 4"); return 4; } else { Console.WriteLine ("3 startCurrentSegment.Y > endCurrentSegment.Y"); return 0; } } }
void NavigateTick (object sender, TickEventArgs e) { totalElapsed += e.TicksElapsed; if (totalElapsed < millisDelay) return; sprite.Invalidate (); pixel_x += delta_x; pixel_y += delta_y; if (dest_pixel_x - pixel_x < 2) pixel_x = dest_pixel_x; if (dest_pixel_y - pixel_y < 2) pixel_y = dest_pixel_y; sprite.SetPosition (pixel_x, pixel_y); sprite.Invalidate (); x = pixel_x << 2; y = pixel_y << 2; if (pixel_x == dest_pixel_x && pixel_y == dest_pixel_y) { startCurrentSegment = endCurrentSegment; navigatePath.RemoveAt (0); // if we're at the destination, remove the tick handler if (navigatePath.Count == 0) { sprite.RunScript (AnimationType.WalkingToIdle); Events.Tick -= NavigateTick; } else { endCurrentSegment = navigatePath[0]; sprite.Face (ClassifyDirection (startCurrentSegment, endCurrentSegment)); dest_pixel_x = endCurrentSegment.X * 4 + 4; dest_pixel_y = endCurrentSegment.Y * 4 + 4; delta_x = dest_pixel_x - pixel_x; delta_y = dest_pixel_y - pixel_y; } } }
double heuristic_estimate_of_distance (MapPoint point, MapPoint goal) { return Math.Abs(point.X-goal.X) + Math.Abs(point.Y-goal.Y); }
public bool Navigable (MapPoint point) { // this assumes that point corresponds to a mini tile location // first calculate the megatile int megatile_x, megatile_y; megatile_x = point.X >> 3; megatile_y = point.Y >> 3; if ((megatile_x >= chk.Width || megatile_x < 0) || (megatile_y >= chk.Height || megatile_y < 0)) return false; int mapTile = chk.MapTiles[megatile_x,megatile_y]; int tile_group = mapTile >> 4; /* the tile's group in the cv5 file */ int tile_number = mapTile & 0x0F; /* the megatile within the tile group */ int megatile_id = Util.ReadWord (cv5, (tile_group * 26 + 10 + tile_number) * 2); ushort minitile_flags = Util.ReadWord (vf4, megatile_id * 32 + point.Y * 8 + point.X * 2); // Console.WriteLine ("minitile {0},{1} is navigable? {2}", point.X, point.Y, (minitile_flags & 0x0001) == 0x0001); return (minitile_flags & 0x0001) == 0x0001; }
double dist_between (MapPoint point2, MapPoint point1) { // euclidian distance return Math.Sqrt ((point2.X - point1.X) * (point2.X - point1.X) + (point2.Y - point1.Y) * (point2.Y - point1.Y)); }
public static void Main (string[] args) { Map m = new Map (50, 50); for (int i = 0; i < 25; i ++) m.AddWall (new MapPoint (15, i)); AStarSolver solver = new AStarSolver (m); List<MapPoint> path = solver.FindPath (new MapPoint (10, 10), new MapPoint (20, 10)); Dictionary<MapPoint,bool> path_points = new Dictionary<MapPoint,bool>(); foreach (var p in path) path_points.Add (p, true); for (int y = 0; y < 50; y ++) { for (int x = 0; x < 50; x ++) { MapPoint point = new MapPoint (x,y); if (point == new MapPoint (20,10)) Console.Write ("X"); else if (path_points.ContainsKey (point)) Console.Write ("x"); else if (!m.Navigable (point)) Console.Write ("O"); else if (solver.closedset.ContainsKey (point)) Console.Write ("."); else Console.Write (" "); } Console.WriteLine(); } // Console.WriteLine ("Path = {{"); // foreach (MapPoint point in path) { // Console.WriteLine (" {0}, {1}", point.X, point.Y); // } // Console.WriteLine ("}"); }
List<MapPoint> ReconstructPath (MapPoint current_node) { List<MapPoint> path = new List<MapPoint>(); while (current_node != null) { path.Insert (0, current_node); if (came_from.ContainsKey (current_node)) current_node = came_from[current_node]; else current_node = null; } return path; }
Dictionary<MapPoint,bool> closedset; // The set of nodes already evaluated. public List<MapPoint> FindPath (MapPoint start, MapPoint goal) { if (!map.Navigable (start)) throw new Exception ("is it valid to start in a non-navigable spot?"); closedset = new Dictionary<MapPoint,bool>(); var openset = new Dictionary<MapPoint,bool>(); // The set of tentative nodes to be evaluated. openset.Add (start, true); var g_score = new Dictionary<MapPoint,double>(); var h_score = new Dictionary<MapPoint,double>(); var f_score = new Dictionary<MapPoint,double>(); g_score[start] = 0.0; // Distance from start along optimal path. h_score[start] = heuristic_estimate_of_distance(start, goal); f_score[start] = h_score[start]; // Estimated total distance from start to goal through y. while (openset.Keys.Count > 0) { var x = FindLowestScoringNode (openset, f_score); if (x == goal) { Console.WriteLine ("done!"); return ReconstructPath(goal); } openset.Remove (x); closedset.Add (x, true); var neighbors = NeighborNodes(x); foreach (var y in neighbors) { bool tentative_is_better; if (closedset.ContainsKey (y)) continue; var tentative_g_score = g_score[x] + dist_between(x,y); if (!openset.ContainsKey (y)) { openset.Add (y, true); tentative_is_better = true; } else if (tentative_g_score < g_score[y]) { tentative_is_better = true; } else { tentative_is_better = false; } if (tentative_is_better) { came_from[y] = x; g_score[y] = tentative_g_score; h_score[y] = heuristic_estimate_of_distance(y, goal); f_score[y] = g_score[y] + h_score[y]; } } } Console.WriteLine ("failed to find path"); return null; }
List<MapPoint> NeighborNodes (MapPoint point) { List<MapPoint> neighbors = new List<MapPoint>(); // Console.WriteLine ("checking neighbors of {0}", point); // Console.WriteLine ("map minitile size = {0}x{1}", map.Chk.Width, map.Chk.Height); for (int x = -1; x <= 1; x ++) { for (int y = -1; y <= 1; y ++) { MapPoint p = new MapPoint (x+point.X, y+point.Y); // Console.WriteLine ("checking neighbor {0}", p); if (x == 0 && y == 0) { // disallow non-moves // Console.WriteLine ("no (1)"); continue; } if (p.X < 0 || p.X >= (map.Chk.Width << 3)) { // Console.WriteLine ("no (2)"); continue; } if (p.Y < 0 || p.Y >= (map.Chk.Height << 3)) { // Console.WriteLine ("no (3)"); continue; } // don't move into walls if (!map.Navigable (p)) { // Console.WriteLine ("no (4)"); continue; } // Console.WriteLine ("adding neighbor of {0}", p); neighbors.Add (p); } } return neighbors; }