static public List <Node> HPAStar(grafo g)
{
//Generación de supernodos...
//TODO
//Generar lista secundaria de owners... buscar alrededor de cada rectangulo y los owners ligarlos...
int startx = g.start.posX;
int starty = g.start.posY;
int goalx = g.goal.posX;
int goaly = g.goal.posY;
List <Rectangle> rects = new List <Rectangle>();
Rectangle[,] owner = new Rectangle[g.size, g.size];
for (int y = 0; y < g.size; y++)
{
for (int x = 0; x < g.size; x++)
{
if (g.nodes[x, y] != null)
{//If the node can be grouped... Groups it with its siblings...
Rectangle r = new Rectangle();
int addlength = 0;
while (x + addlength < g.size && g.nodes[x + addlength, y] != null)/* && !(x+length+1 == m.entradax && y == m.entraday) && !(x + length + 1 == m.salidax && y == m.saliday)*/
{
++addlength;
}
//Base length calculated...
bool obstructed = false;
int height = 0;
for (int j = y; j < g.size; j++)
{
for (int i = x; i < x + addlength; i++)
{
if (g.nodes[i, j] == null)
{
obstructed = true;
break;
}
}
if (obstructed)
{
break;
}
else
{
for (int i = x; i < x + addlength; i++)
{
g.nodes[i, j] = null;
owner[i, j] = r;
}
}
++height;
}
//Height calculated...
r.assign(x, y, x + addlength - 1, y + height - 1);
if (startx >= r.posaX && startx <= r.posbX && starty >= r.posaY && starty <= r.posbY)
{
r.containsStart = true;
//Console.WriteLine("fs: "+m.entradax+" "+m.entraday+" "+r.posaX+" " + r.posaY + " " + r.posbX + " " + r.posbY + " ");
}
if (goalx >= r.posaX && goalx <= r.posbX && goaly >= r.posaY && goaly <= r.posbY)
{
r.containsGoal = true;
//Console.WriteLine("fg");
}
if (r.containsStart && r.containsGoal) //Se tendria que devolver la distancia con esto...
{
getManhattan(startx, goalx, starty, goaly); //Se haria algo con esto si fuera necesario saber la ruta... Solo realizo la operación por el tiempo de procesamiento.
return(new List <Node>()); //Realmente es trivial el moverse dentro de un rectangulo.
}
//Console.WriteLine(addlength + " " + height);
rects.Add(r);
}
}
}
//Console.WriteLine("dividido listo " + rects.Count);
//Ya tengo todo dividido en rectangulos... Falta volver a los rectángulos nodos, ya se cuales contienen entrada y salida...
//Tenemos asegurado que el mismo rectangulo no contiene entrada y salida. Ese caso de salida ya se dió...
//Revisar los recuadros alrededor de cada rectangulo, owner!!!!
Node[,] nodos = new Node[g.size, g.size];
foreach (Rectangle a in rects)
{
List <Rectangle> adj = new List <Rectangle>();
int ax = -1;
int ay = -1;
int bx = 1;
int by = 1;
if (a.posaX == 0)
{
ax = 0;
}
if (a.posaY == 0)
{
ay = 0;
}
if (a.posbX == g.size - 1)
{
bx = 0;
}
if (a.posbY == g.size - 1)
{
by = 0;
}
if (ay != 0)
{
for (int i = a.posaX; i <= a.posbX; i++)
{
if (owner[i, a.posaY + ay] != null && !adj.Contains(owner[i, a.posaY + ay]))
{
adj.Add(owner[i, a.posaY + ay]);
}
}
}
if (by != 0)
{
for (int i = a.posaX; i <= a.posbX; i++)
{
if (owner[i, a.posbY + by] != null && !adj.Contains(owner[i, a.posbY + by]))
{
adj.Add(owner[i, a.posbY + by]);
}
}
}
if (ax != 0)
{
for (int i = a.posaY; i <= a.posbY; i++)
{
if (owner[a.posaX + ax, i] != null && !adj.Contains(owner[a.posaX + ax, i]))
{
adj.Add(owner[a.posaX + ax, i]);
}
}
}
if (bx != 0)
{
for (int i = a.posaY; i <= a.posbY; i++)//OJO
{
if (owner[a.posbX + bx, i] != null && !adj.Contains(owner[a.posbX + bx, i]))
{
adj.Add(owner[a.posbX + bx, i]);
}
}
}
int px = a.getSignificantX(startx, goalx);
int py = a.getSignificantY(starty, goaly);
if (nodos[px, py] == null)
{
nodos[px, py] = new Node();
nodos[px, py].ac = new Dictionary <Node, int>();
nodos[px, py].posX = px;
nodos[px, py].posY = py;
}
foreach (Rectangle b in adj)
{
int qx = b.getSignificantX(startx, goalx);
int qy = b.getSignificantY(starty, goaly);
if (!(nodos[qx, qy] != null && nodos[px, py].ac.ContainsKey(nodos[qx, qy])))
{
int caso = 0;
if ((b.posaX >= a.posaX && b.posaX <= a.posbX) || (b.posbX >= a.posaX && b.posbX <= a.posbX))
//Si se cumple cualquiera de los casos, sabemos que
{ //Casos 1 y 2... b arriba o abajo de a...
if (a.posaY - b.posbY == 1) //Caso 1, b sobre a.
{
caso = 1;
}
else if (b.posaY - a.posbY == 1)//Caso 2, b bajo a.
{
caso = 2;
}
}
else if ((b.posaY >= a.posaY && b.posaY <= a.posbY) || (b.posbY >= a.posaY && b.posbY <= a.posbY))
//Casos 3 y 4.... b a la derecha o izquierda de a...
{
if (a.posaX - b.posbX == 1)//Caso 3, b a la izquierda de a.
{
caso = 3;
}
else if (b.posaX - a.posbX == 1)//Caso 4, b a la derecha de a.
{
caso = 4;
}
}
//Console.WriteLine(ax + " " + ay + " " + bx + " " + by);
if (caso != 0)
{
int distancia = 0;
if (b != a)
{
if (nodos[qx, qy] == null)
{
nodos[qx, qy] = new Node();
nodos[qx, qy].ac = new Dictionary <Node, int>();
nodos[qx, qy].posX = qx;
nodos[qx, qy].posY = qy;
}
distancia = getManhattan(px, qx, py, qy);
if (!(nodos[px, py].ac.ContainsKey(nodos[qx, qy])))
{
nodos[px, py].ac.Add(nodos[qx, qy], distancia);
nodos[qx, qy].ac.Add(nodos[px, py], distancia);
}
}
}
}
}
}
int gsize = g.size;
g = new grafo();
g.goal = nodos[goalx, goaly];
g.start = nodos[startx, starty];
g.size = gsize;
g.nodes = nodos;
//Console.WriteLine("ASTAR CALLED");
return(AStar(g));
}
static public List <Node> Dijkstra(grafo g)
{
List <Node> unvisited = new List <Node>();
Node goal = g.goal;
Node start = g.start;
Node current = start;
List <Node> discovered = new List <Node>();
Node[,] parent = new Node[g.size, g.size];
int mindistance;
foreach (Node n in g.nodes)
{
if (n != null)
{
unvisited.Add(n);
}
}
int[,] gVal = new int[g.size, g.size];
for (int x = 0; x < g.size; x++)
{
for (int y = 0; y < g.size; y++)
{
/*if (g.nodes[x, y] != null)
* {*/
gVal[x, y] = 100000000;
//}
}
}
gVal[current.posX, current.posY] = 0;
parent[current.posX, current.posY] = current;
discovered.Add(current);
do
{
foreach (KeyValuePair <Node, int> n in current.ac)
{
if (!discovered.Contains(n.Key))
{
discovered.Add(n.Key);
}
if (n.Value + gVal[current.posX, current.posY] < gVal[n.Key.posX, n.Key.posY])
{
gVal[n.Key.posX, n.Key.posY] = n.Value + gVal[current.posX, current.posY];
parent[n.Key.posX, n.Key.posY] = current;
}
}
unvisited.Remove(current);
mindistance = 100000000;
foreach (Node n in unvisited)
{
if (gVal[n.posX, n.posY] < mindistance)
{
mindistance = gVal[n.posX, n.posY];
current = n;
}
}
if (mindistance == 100000000 && unvisited.Contains(goal))
{
Console.WriteLine("DERROR:Unreachable destination");
return(null);
}
} while (unvisited.Contains(goal));
List <Node> Path = new List <Node>();
current = goal;
do
{
Path.Add(current);
current = parent[current.posX, current.posY];
} while (current != start);
Path.Add(start);
posNodes.Add(discovered.Count);
return(Path);
}
static public List <Node> AStar(grafo g)
{
List <Node> unvisited = new List <Node>();
//int sNodes = 0;
Node goal = g.goal;
Node start = g.start;
Node current = g.start;
int mindistance;
int[,] gVal = new int[g.size, g.size];
int[,] fVal = new int[g.size, g.size];
int[,] hVal = new int[g.size, g.size];
Node[,] parent = new Node[g.size, g.size];
foreach (Node n in g.nodes)
{
if (n != null)
{
unvisited.Add(n);
gVal[n.posX, n.posY] = 100000000;
fVal[n.posX, n.posY] = 100000000;
hVal[n.posX, n.posY] = getManhattan(n.posX, goal.posX, n.posY, goal.posY);
}
}
gVal[current.posX, current.posY] = 0;
fVal[current.posX, current.posY] = hVal[current.posX, current.posY] + gVal[current.posX, current.posY];
parent[current.posX, current.posY] = current;
do
{
foreach (KeyValuePair <Node, int> n in current.ac)
{
if (n.Value + hVal[n.Key.posX, n.Key.posY] + gVal[current.posX, current.posY] < fVal[n.Key.posX, n.Key.posY])
{
gVal[n.Key.posX, n.Key.posY] = n.Value + gVal[current.posX, current.posY];
fVal[n.Key.posX, n.Key.posY] = gVal[n.Key.posX, n.Key.posY] + hVal[n.Key.posX, n.Key.posY];
parent[n.Key.posX, n.Key.posY] = current;
}
}
unvisited.Remove(current);
mindistance = 100000000;
foreach (Node n in unvisited)
{
if (fVal[n.posX, n.posY] < mindistance)
{
mindistance = fVal[n.posX, n.posY];
current = n;
}
}
if (mindistance == 100000000 && unvisited.Contains(goal))
{
Console.WriteLine("AERROR:Unreachable destination");
return(null);
}
} while (unvisited.Contains(goal));
List <Node> Path = new List <Node>();
current = goal;
do
{
Path.Add(current);
current = parent[current.posX, current.posY];
} while (current != start);
Path.Add(start);
return(Path);
}
public grafo convertToGrafo()
{
//Console.WriteLine("GSTART");
grafo res = new grafo();
res.nodes = new Node[size, size];
res.size = size;
for (int x = 0; x < size; x++)
{
for (int y = 0; y < size; y++)
{
if (!array[x, y])//true is obstacle, therefore false is free = node.
{
res.nodes[x, y] = new Node();
res.nodes[x, y].ac = new Dictionary <Node, int>();
res.nodes[x, y].posX = x;
res.nodes[x, y].posY = y;
}
else
{
res.nodes[x, y] = null;
}
}
}
for (int x = 0; x < size; x++)
{
for (int y = 0; y < size; y++)
{
if (res.nodes[x, y] != null)
{
int xs = -1;
int xe = 1;
int ys = -1;
int ye = 1;
if (x == 0)
{
xs = 0;
}
if (x == size - 1)
{
xe = 0;
}
if (y == 0)
{
ys = 0;
}
if (y == size - 1)
{
ye = 0;
}
for (int p = x + xs; p <= x + xe; p++)
{
for (int q = y + ys; q <= y + ye; q++)
{
if (res.nodes[p, q] != null && !(p == x && q == y))
{
/*if (p != x && q != y)
* {
* res.nodes[x, y].ac.Add(res.nodes[p, q], 14);
* }
* else
* {
* res.nodes[x, y].ac.Add(res.nodes[p, q], 10);
* }*/
if (!(p != x && q != y))
{
res.nodes[x, y].ac.Add(res.nodes[p, q], 1);
}
}
}
}
}
}
}
/* //Si llega a existir un nodo completamente aislado, se elimina...
* for (int x = 0; x < size; x++)
* {
* for (int y = 0; y < size; y++)
* {
* if(res.nodes[x,y]!= null && res.nodes[x,y].ac.Count == 0)
* {
* res.nodes[x, y] = null;
* }
* }
* }*/
res.start = res.nodes[entradax, entraday];
res.goal = res.nodes[salidax, saliday];
//Console.WriteLine("GEND");
return(res);
}