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