/// <summary>
/// Busca un camino desde una posición hasta otra en un mapa
/// </summary>
/// <param name="map">Matriz del mapa</param>
/// <param name="startx">Posición Inicial X</param>
/// <param name="starty">Posición Inicial Y</param>
/// <param name="targetx">Posición Destino X</param>
/// <param name="targety">Posición Destino Y</param>
/// <returns></returns>
public static bool findPath(char[,] map, int startx, int starty, int targetx, int targety)
{
Path path = new Path(starty, startx);
int cost;
cost = (Math.Max(Math.Abs(startx - targetx), Math.Abs(starty - targety)));
PathNode FirstNode = new PathNode(starty, startx, true, -1, -1, cost);
nodes.Add("" + starty + "," + startx + "", FirstNode);
path.uncheckedNeighbors.Add(FirstNode);
while (path.uncheckedNeighbors.Count > 0)
{
PathNode N = (PathNode)path.uncheckedNeighbors[0];
path.uncheckedNeighbors.RemoveAt(0);
if (N.x == targety && N.y == targetx)
{
makePath(N, map);
return true;
}
else
{
N.visited = true;
map[N.x, N.y] = 'V';
printMap(map);
//Thread.Sleep(100);
if (N.x + 1 < 16)
{
if (map[N.x + 1, N.y] != 'X' && map[N.x + 1, N.y] != 'V')
{
map[N.x + 1, N.y] = 'C';
//printMap(map);
//Thread.Sleep(100);
}
addNode(map, N, path, N.x + 1, N.y, targety, targetx);
}
if (N.x - 1 >= 0)
{
if (map[N.x - 1, N.y] != 'X' && map[N.x - 1, N.y] != 'V')
{
map[N.x - 1, N.y] = 'C';
//printMap(map);
//Thread.Sleep(100);
}
addNode(map, N, path, N.x - 1, N.y, targety, targetx);
}
if (N.y + 1 < 16)
{
if (map[N.x, N.y + 1] != 'X' && map[N.x, N.y + 1] != 'V')
{
map[N.x, N.y + 1] = 'C';
//printMap(map);
//Thread.Sleep(100);
}
addNode(map, N, path, N.x, N.y + 1, targety, targetx);
}
if (N.y - 1 >= 0)
{
if (map[N.x, N.y - 1] != 'X' && map[N.x, N.y - 1] != 'V')
{
map[N.x, N.y - 1] = 'C';
//printMap(map);
//Thread.Sleep(100);
}
addNode(map, N, path, N.x, N.y - 1, targety, targetx);
}
}
}
return false;
}
/// <summary>
/// Convierte el camino a una pila de coordenadas
/// </summary>
/// <param name="N">Camino elegido</param>
/// <param name="map">Matriz del mapa</param>
public static void makePath(PathNode N, char[,] map)
{
while (N.parentx != -1)
{
finalpath.Push(new Coord(N.x, N.y));
map[N.x, N.y] = 'P';
printMap(map);
Thread.Sleep(50);
N = (PathNode)nodes["" + N.parentx + "," + N.parenty + ""];
}
}
/// <summary>
/// Añade un Nodo a la tabla
/// </summary>
/// <param name="map">Mapa donde buscar el camino</param>
/// <param name="ob">Nodo Padre</param>
/// <param name="path">Camino a seguir</param>
/// <param name="x">Posición X del nodo</param>
/// <param name="y">Posición Y del nodo</param>
/// <param name="targetx">Posición X del destino</param>
/// <param name="targety">Posición Y del destino</param>
public static void addNode(char[,] map, PathNode ob, Path path, int x, int y, int targetx, int targety)
{
string nodekey = "" + x + "," + y + "";
if (map[x,y] != 'X')
{
int cost;
switch (Algorithm)
{
case 1:
{
cost = Math.Abs(x - targetx) + Math.Abs(y - targety);
break;
}
case 2:
{
cost = (int)(Math.Sqrt(Math.Pow((x - targetx), 2) + Math.Pow((y - targety), 2)));
break;
}
case 3:
{
cost = (int)(Math.Pow((x - targetx), 2) + Math.Pow((y - targety), 2));
break;
}
default:
{
cost = (Math.Max(Math.Abs(x - targetx), Math.Abs(y - targety)));
break;
}
}
if (!nodes.ContainsKey(nodekey))
{
PathNode NewNode = new PathNode(x, y, false, ob.x, ob.y, cost);
path.uncheckedNeighbors.Add(NewNode);
path.uncheckedNeighbors.Sort(new NodeCostComparer());
nodes.Add(nodekey, NewNode);
}
}
}