private void Algorithm(int destX, int destZ, int origX, int origZ, box[,] table)
{
Node minimo = null;
//timeTaken = Time.time;
DateTime d = DateTime.Now;
timeTaken = d.Millisecond;
// bucle principal a partir del cual se ejecutara la logica del algoritmo
while (abiertos.Count > 0 && !FOUND)
{
// sacamos el nodo minimo de la cola
minimo = abiertos.Top(); abiertos.Pop();
// lo guardamos en la cola de cerrados, ya esta comprobado
cerrados.Add(minimo);
// este metodo devuelve las coordenadas de las casillas adyacentes a
// la que estamos comprobando (cada nodo tiene la informacion de en
// que posicion esta su casilla)
List <coords> adyacentes = calcAdyacentes(minimo.i_, minimo.j_, table);
foreach (coords c in adyacentes)
{
// si el nodo a comprobar esta en cerrados no hacemos nada
if (cerrados.Contains(nodeMatrix[c.x_, c.z_]))
{
continue;
}
// si en esa posicion no hay un nodo asociado lo creamos,
// le ponemos los valores en funcion de su coste y la heuristica
// a utilizar y le asignamos el padre
if (nodeMatrix[c.x_, c.z_] == null)
{
Node ady = new Node();
ady.setPosition(c.x_, c.z_);
ady.Parent_ = minimo;
ady.h_ = calculateH(HEURISTIC, ady.i_, ady.j_, origX, origZ);
ady.g_ = calculateG(ady, table);
ady.f_ = ady.h_ + ady.g_;
// si la casilla a la que se le asocia dicho nodo no es una habitación
// lo metemos en abiertos
if (table[ady.i_, ady.j_].cell != Cells.Room)
{
abiertos.Insert(ady.i_ + table.GetLength(0) * ady.j_, ady);
}
//// en caso contrario lo metemos en cerrados
//else
//{
// cerrados.Add(ady);
//}
// actualizamos matriz de nodos
nodeMatrix[ady.i_, ady.j_] = ady;
}
// si el nodo ya esta creado lo que haremos sera
// ver si el nuevo camino que va a el es mejor
else
{
Node ady = nodeMatrix[c.x_, c.z_];
int weight = 0;
if (table[ady.i_, ady.j_].cell == Cells.Empty)
{
weight = 1;
}
else if (table[ady.i_, ady.j_].cell == Cells.Door)
{
weight = 4;
}
else if (table[ady.i_, ady.j_].cell == Cells.Corridor)
{
weight = 4;
}
else if (table[ady.i_, ady.j_].cell == Cells.Room)
{
weight = int.MaxValue;
}
int g = minimo.g_ + weight;
// si el nuevo camino es mejor, actualizamos el nodo
if (g + ady.h_ < ady.f_)
{
ady.Parent_ = minimo;
ady.g_ = g;
ady.f_ = ady.g_ + ady.h_;
}
}
}
// si las coordenadas asociadas al nodo coinciden con el destino,
// hemos llegado, salimos del bucle
if (minimo.i_ == destX && minimo.j_ == destZ)
{
FOUND = true;
}
}
if (FOUND)
{
// para saber el tiempo que ha tardado en finalizar el algoritmo
//DateTime da = DateTime.Now;
//float auxTime = da.Millisecond - timeTaken;
// ahora calculamos el camino a seguir recorriendo los nodos que nos han llevado a la solucion
calculatePath(minimo, origX, origZ);
// feedback visual
//GameObject.FindGameObjectWithTag("Destiny").GetComponent<Renderer>().material.color = Color.green;
//FindObjectOfType<HUDControllScript>().setTime(auxTime);
}
else
{
// feedbakc visual
// si entramos aqui es que no hay solucion
//GameObject.FindGameObjectWithTag("Destiny").GetComponent<Renderer>().material.color = Color.red;
}
}
/// <summary>
/// Builds a search tree moving outward from Start.
/// If there is a specific End Node, search towards that and stop when it is found.
/// Otherwise, just search the entire graph space starting from Start.
/// </summary>
/// <param name="maxSearchCost">The maximum Edge search cost this pathfinder can search from start.
/// This can be used to limit the graph search space.</param>
public void CalculatePathTree(float maxSearchCost)
{
ResetFinder();
//We're using A* algorithm.
KeyValuePair <float, Edge <N> > closest;
N tempCenter = DefaultVal;
N closestN = DefaultVal;
float closestCost, tempCost,
closestSearchCost, tempSearchCost;
//Start searching from the source node.
AddNodeToSearchSpace(MakeEdge(DefaultVal, Start), 0.0f, 0.0f);
//Keep track of the possible destination Nodes (in case the actual destination is too far away).
List <N> possibleDestNodes = new List <N>();
bool goesOn, noBranches;
//While the search frontier is not empty, keep grabbing the nearest Node to search.
while (!NodesToSearch.IsEmpty)
{
//Get the closest Node.
closest = NodesToSearch.Pop();
closestN = closest.Value.End;
closestCost = closest.Key;
closestSearchCost = GetToNodeSearchCost[closestN];
//Put it into the Path.
//If it was already in the Path, then a shorter route to it has already been found.
if (!EndToStart.ContainsKey(closestN))
{
EndToStart.Add(closestN, closest.Value.Start);
}
//If the target has been found, or the graph search distance has been exceeded, exit.
if ((HasSpecificEnd && closestN.Equals(End)) || GetToNodeSearchCost[closestN] >= maxSearchCost)
{
break;
}
//Now process all the connected nodes.
//"goesOn" holds whether or not there is a usable Node connected to the current one.
goesOn = false;
//"noBranches" holds whether or not there exists a usable Node
// connected to the current one that can be added to the search fronter
// (e.x. a connected Node with a search cost above the threshold
// would set goesOn to true, and noBranches to true).
noBranches = true;
foreach (Edge <N> e in Graph.GetConnections(closestN))
{
//Add the cell to the search space if it hasn't been processed already,
// and if the cell is the pathing destination.
if (!Considered.Contains(e.End))
{
//We know the path goes on.
goesOn = true;
//Get the search/traversal costs.
tempCost = closestCost + e.Cost(this);
tempSearchCost = closestSearchCost + e.SearchCost(this);
//If the search cost is small enough, add the edge to the search space.
if (tempSearchCost <= maxSearchCost)
{
noBranches = false;
AddNodeToSearchSpace(e, tempCost, tempSearchCost);
}
}
}
//If there are usable nodes pointing out of the current one, but they are all too far away,
// check this Node as a possible destination Node in case the real destination is too far away.
if (goesOn && noBranches && HasSpecificEnd)
{
PathEnds.Add(closestN);
}
}
FinishedCalculatingTree = true;
}
private void Algorithm(int destX, int destZ, int origX, int origZ)
{
Node minimo = null;
//timeTaken = Time.time;
DateTime d = DateTime.Now;
timeTaken = d.Millisecond;
// bucle principal a partir del cual se ejecutara la logica del algoritmo
while (abiertos.Count > 0 && !FOUND)
{
// sacamos el nodo minimo de la cola
minimo = abiertos.Top(); abiertos.Pop();
// lo guardamos en la cola de cerrados, ya esta comprobado
cerrados.Add(minimo);
// este metodo devuelve las coordenadas de las casillas adyacentes a
// la que estamos comprobando (cada nodo tiene la informacion de en
// que posicion esta su casilla)
List <coords> adyacentes = calcAdyacentes(minimo.i_, minimo.j_);
foreach (coords c in adyacentes)
{
// si el nodo a comprobar esta en cerrados no hacemos nada
if (cerrados.Contains(nodeMatrix[c.x_, c.z_]))
{
continue;
}
// si en esa posicion no hay un nodo asociado lo creamos,
// le ponemos los valores en funcion de su coste y la heuristica
// a utilizar y le asignamos el padre
if (nodeMatrix[c.x_, c.z_] == null)
{
Node ady = new Node();
ady.setPosition(c.x_, c.z_);
ady.Parent_ = minimo;
ady.h_ = calculateH(HEURISTIC, ady.i_, ady.j_, origX, origZ);
ady.g_ = calculateG(ady);
ady.f_ = ady.h_ + ady.g_;
abiertos.Insert(ady.i_ + 7 * ady.j_, ady);
// actualizamos matriz de nodos
nodeMatrix[ady.i_, ady.j_] = ady;
}
// si el nodo ya esta creado lo que haremos sera
// ver si el nuevo camino que va a el es mejor
else
{
Node ady = nodeMatrix[c.x_, c.z_];
int weight = 0;
int g = minimo.g_ + weight;
// si el nuevo camino es mejor, actualizamos el nodo
if (g + ady.h_ < ady.f_)
{
ady.Parent_ = minimo;
ady.g_ = g;
ady.f_ = ady.g_ + ady.h_;
}
}
}
// si las coordenadas asociadas al nodo coinciden con el destino,
// hemos llegado, salimos del bucle
if (minimo.i_ == destX && minimo.j_ == destZ)
{
FOUND = true;
}
}
if (FOUND)
{
// para saber el tiempo que ha tardado en finalizar el algoritmo
DateTime da = DateTime.Now;
float auxTime = da.Millisecond - timeTaken;
// ahora calculamos el camino a seguir recorriendo los nodos que nos han llevado a la solucion
calculatePath(minimo, origX, origZ);
}
else
{
Debug.Log("No hay solución");
}
}
