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