public PathFindingVertex(PathFindingNode s, PathFindingNode e)
{
START = s;
END = e;
}
//Pathfinding tattico: costruisce il percorso a costo minimo con A* con costi delle connessioni che tengono conto della mappa di influenza
public List<PathFindingVertex> GetPath(Graph graph, int start, int goal)
{
PathFindingNode startNode = new PathFindingNode (start, 0, EstimateFrom(start,goal));
PathFindingNode current = new PathFindingNode(-1,0,0);
//Inizializza lista nodi chiusi (già visitati) e aperti (da visitare)
PathFindingList open = new PathFindingList();
PathFindingList closed = new PathFindingList();
open.AddNode(startNode);
//Itera tutti i nodi da visitare
while (open.Length() > 0) {
current = open.SmallestElement();
//Se il nodo corrente da visitare è l'obiettivo, termina iterazione
if (current.ID == goal){
break;
}
List<Vertex> connections = graph.GetVertices(current.ID);
//Itera tutte le connessioni che partono dal nodo corrente
foreach (Vertex connection in connections){
Node end = connection.END;
float endCost = current.costSoFar + connection.Cost;
float endHeuristic;
PathFindingNode endRecord;
//Se il nodo al termine della connessione è tra i chiusi....
if (closed.Contains(end)){
//Record corrispondente al nodo terminale nella lista dei visitati
endRecord = closed.Find(end);
/*Se è stato trovato un percorso migliore verso il nodo terminale
rispetto a quello già registrato tra gli aperti...*/
if (endRecord!=null && endRecord.costSoFar > endCost) {
//Rimuove il nodo dai chiusi
closed.RemoveNode(endRecord);
//Usa i vecchi valori del nodo per ricalcolare l'euristica
endHeuristic = endRecord.estimatedTotalCost - endRecord.costSoFar;
}
else continue;
}
//Se invece il nodo terminale è tra gli aperti....
else if (open.Contains(end)) {
//Record corrispondente al nodo terminale nella lista degli aperti
endRecord = open.Find(end);
/*Se è stato trovato un percorso migliore verso il nodo terminale
rispetto a quello già registrato tra gli aperti....*/
if (endRecord.costSoFar > endCost) {
//Uso i vecchi valori del nodo per ricalcolare l'euristica
endHeuristic = endRecord.estimatedTotalCost - endRecord.costSoFar;
}
else continue;
}
/*Se invece il nodo terminale non è nè tra i chiusi nè tra gli aperti, crea un nuovo record
* ed effettua la stima della distanza dall'obiettivo*/
else {
endRecord = new PathFindingNode(end.ID);
endHeuristic = EstimateFrom(end.ID,goal);
}
//Aggiorno record con nuovi valori
endRecord.costSoFar = endCost;
endRecord.connection = new PathFindingVertex(current,endRecord);
endRecord.estimatedTotalCost = endCost + endHeuristic;
if (!open.Contains(end))
open.AddNode(endRecord);
}
open.RemoveNode(current);
closed.AddNode(current);
}
//Se il nodo corrente non è l'obiettivo, il nodo non è stato trovato e restituisce null
if (current.ID!=goal)
return null;
//Altrimenti ricostruisce il percorso all'indietro partendo dal nodo corrente.
else{
List<PathFindingVertex> path = new List<PathFindingVertex>();
while (current.ID!=start){
path.Add(current.connection);
current = current.connection.START;
}
path.Reverse();
return path;
}
}
public void AddNode(PathFindingNode node)
{
list.Add (node);
}
public void RemoveNode(PathFindingNode node)
{
list.Remove(node);
}
public override List <PathFindingNode> FindPath(List <List <PathFindingNode> > grid, PathFindingNode origin, PathFindingNode destination)
{
_openNodes.Add(origin);
while (_openNodes.Count != 0)
{
PathFindingNode node = _openNodes[_openNodes.Count - 1];
if (node == destination)
{
List <PathFindingNode> path = GetPath(node);
CleanValues(grid);
return(path);
}
for (int i = 0; i < node.Adjacents.Count; i++)
{
PathFindingNode actualAdjacentNode = node.Adjacents[i];
bool isOnClosed = false;
bool isOnOpen = false;
for (int j = 0; j < _closedNodes.Count; j++)
{
if (actualAdjacentNode == _closedNodes[j])
{
isOnClosed = true;
j = _closedNodes.Count;
}
}
for (int j = 0; j < _openNodes.Count; j++)
{
if (actualAdjacentNode == _openNodes[j])
{
isOnOpen = true;
j = _openNodes.Count;
}
}
if (!isOnClosed && !isOnOpen)
{
actualAdjacentNode.Parent = node;
_openNodes.Add(actualAdjacentNode);
}
}
_openNodes.Remove(node);
_closedNodes.Add(node);
}
return(null);
}
protected void ChaseSection(MazeSection destination)
{
//Localizações dos elementos
var currLocation = this.Ghost.CurrentLocation;
var currEndLocation = destination;
var prevLocation = this.Ghost.PreviousLocation;
//Essa é a seção inicial,
PathFindingNode currNode = new PathFindingNode();
currNode.h = 0;
currNode.g = 0;
currNode.parent = null;
currNode.location = currLocation;
List<PathFindingNode> closedSet = new List<PathFindingNode>();
List<PathFindingNode> openSet = new List<PathFindingNode>();
closedSet.Add(currNode);
do
{
//Percorrendo as opções de caminho que temos
List<MazeSection> opcoes = new List<MazeSection> { currLocation.W, currLocation.N, currLocation.E, currLocation.S };
foreach (var o in opcoes)
{
if (o != null)
{
//A seção precisa estar ativa e não ser a seção anteriormente visitada
if (o.Allowed && (prevLocation == null || o.ID != prevLocation.ID))
{
//Seção não deve existir na lista de seções fechadas
if (!closedSet.Any(x => x.location.ID == o.ID))
{
//...nem na lista de seções abertas
if (!openSet.Any(x => x.location.ID == o.ID))
{
PathFindingNode node = new PathFindingNode();
node.location = o;
node.parent = currNode;
node.h = this.Heuristics(node.location, currEndLocation); //heuristica manhattan
//Ok, esse nó participará do teste
openSet.Add(node);
}
}
}
}
}
if (openSet.Count == 0)
{
break;
}
//Procuramos o melhor nó localizado (custo)
var minNode = openSet[0];
foreach (var m in openSet)
{
if (m.h < minNode.h)
{
minNode = m;
}
}
currNode = minNode;
currLocation = currNode.location;
//Removendo da lista de nós abertos para pesquisa e incluíndo na lista de nós proibidos
openSet.Remove(minNode);
closedSet.Add(currNode);
} while (currNode.location.ID != currEndLocation.ID);
if (currNode.location.ID == currEndLocation.ID)
{
///*
LinkedList<PathFindingNode> path = new LinkedList<PathFindingNode>();
PathFindingNode lastPathNode = currNode;
MazeSection nextSectionToMove = null;
MazeSection mySection = this.Ghost.CurrentLocation;
while (lastPathNode != null)
{
path.AddFirst(lastPathNode);
lastPathNode = lastPathNode.parent;
}
nextSectionToMove = path.Count > 1 ? path.First.Next.Value.location : path.First.Value.location;
if (nextSectionToMove.ID == mySection.N.ID)
{
this.nextDirection = EnumDirections.North;
}
else if (nextSectionToMove == mySection.S)
{
this.nextDirection = EnumDirections.South;
}
else if (nextSectionToMove == mySection.E)
{
this.nextDirection = EnumDirections.East;
}
else if (nextSectionToMove == mySection.W)
{
this.nextDirection = EnumDirections.West;
}
this.Ghost.GoTo(this.nextDirection);
//*/
}
}
//DEBUG///////////////////////////////////////////////////////////
public override void FindPathToDebug(List <List <PathFindingNode> > grid, PathFindingNode origin,
PathFindingNode destination)
{
_debugModeEnabled = true;
StartCoroutine(FindPathDebug(grid, origin, destination));
}
/// <summary>
/// Calculates the shortest Path from the startNode to the endNode.
/// </summary>
/// <param name="startNode">The node this path is supposed to start on</param>
/// <param name="endNode">The node this path is supposed to end on</param>
/// <returns></returns>
public override Path FindPath(PathFindingNode startNode, PathFindingNode endNode)
{
List <NetworkNode> openSet = new List <NetworkNode>(); // List of nodes that need to be checked
HashSet <PathFindingNode> closedSet = new HashSet <PathFindingNode>(); // List of nodes that have been visited
openSet.Add(new NetworkNode(startNode)); // Add the first entry
while (openSet.Count > 0)
{
// Move entry to closedSet
NetworkNode currentNetworkNode = openSet[0];
openSet.RemoveAt(0);
closedSet.Add(currentNetworkNode.PathFindingNode);
// Loop over all neighbors
foreach (PathFindingNode neighbor in currentNetworkNode.PathFindingNode.NeighborNodes)
{
if (!neighbor)
{
continue;
}
// Check if the end of this path is in the neighborhood
if (neighbor.Equals(endNode))
{
int endNetworkNodeGCost = currentNetworkNode.GCost + GetDistance(currentNetworkNode.PathFindingNode, neighbor);
int endNetworkNodeHCost = GetDistance(neighbor, endNode);
NetworkNode endNetworkNode = new NetworkNode(neighbor, endNetworkNodeHCost, endNetworkNodeGCost)
{
Parent = currentNetworkNode
};
return(RetracePath(startNode, endNetworkNode));
}
// Check if the current neighbor is supposed to be visited
bool isWalkable = neighbor.IsTraversable();
bool isInClosedSet = closedSet.Contains(neighbor);
if (!isWalkable || isInClosedSet)
{
continue; // Continue if it is not supposed to be visited
}
// Calculate heuristics
int gCost = currentNetworkNode.GCost + GetDistance(currentNetworkNode.PathFindingNode, neighbor);
int hCost = GetDistance(neighbor, endNode);
// Update heuristics values on the neighbor or create a new instance
NetworkNode neighborNode = openSet.Find(x => x.PathFindingNode.Equals(neighbor));
if (neighborNode != null)
{
if (gCost < neighborNode.GCost)
{
neighborNode.GCost = gCost;
neighborNode.HCost = hCost;
neighborNode.Parent = currentNetworkNode;
}
}
else
{
neighborNode = new NetworkNode(neighbor, hCost, gCost)
{
Parent = currentNetworkNode
};
openSet.Add(neighborNode);
}
openSet.Sort(); // Sort ascending on fCost
}
}
return(null); // There is no path from startNode to endNode
}
public ScheduledMover(WaypointMoverController waypointMover, Vector3 intersectionVec3, PathFindingNode from,
PathFindingNode to)
{
_waypointMover = waypointMover;
_intersectionVec3 = intersectionVec3;
_from = from;
_to = to;
}
protected void FindPath()
{
path.Clear();
openNodes.Clear();
closedNodes.Clear();
Vector2Int goal = (Vector2Int)FullWallTilemap.WorldToCell(player.transform.position);
Vector2Int nodePos = (Vector2Int)FullWallTilemap.WorldToCell(transform.position);
openNodes.Add(new PathFindingNode(nodePos, null, 0f, (goal - nodePos).magnitude));
PathFindingNode?node = null;
bool success = false;
while (openNodes.Count > 0)
{
node = openNodes.PopBestNode();
if (node.Value == goal)
{
success = true;
break;
}
closedNodes.Add(node.Value);
float goalDistance = (goal - node.Value).magnitude;
foreach (Vector2Int direction in adjacents)
{
Vector2Int newNodePos = node.Value + direction;
Vector3Int newNodePos3 = new Vector3Int(newNodePos.x, newNodePos.y, 0);
PathFindingNode newNode = new PathFindingNode(newNodePos, node, node.Distance + 1, goalDistance);
if (!closedNodes.Contains(newNodePos))
{
if (FullWallTilemap.HasTile(newNodePos3) || HalfWallTilemap.HasTile(newNodePos3))
{
closedNodes.Add(newNodePos);
}
else
{
openNodes.Add(newNode);
}
}
}
foreach (Vector2Int direction in diagonals)
{
Vector2Int newNodePos = node.Value + direction;
Vector3Int newNodePos3 = new Vector3Int(newNodePos.x, newNodePos.y, 0);
PathFindingNode newNode = new PathFindingNode(newNodePos, node, node.Distance + 1.41421356f, goalDistance);
Vector3Int[] cornerAdjacents = new Vector3Int[] {
new Vector3Int(newNodePos.x, node.Value.y, 0),
new Vector3Int(node.Value.x, newNodePos.y, 0)
};
if (!closedNodes.Contains(newNodePos))
{
if (FullWallTilemap.HasTile(newNodePos3) || HalfWallTilemap.HasTile(newNodePos3) ||
FullWallTilemap.HasTile(cornerAdjacents[0]) || HalfWallTilemap.HasTile(cornerAdjacents[0]) ||
FullWallTilemap.HasTile(cornerAdjacents[1]) || HalfWallTilemap.HasTile(cornerAdjacents[1]))
{
closedNodes.Add(newNodePos);
}
else
{
openNodes.Add(newNode);
}
}
}
}
if (success)
{
while (node != null)
{
path.AddFirst((Vector2)FullWallTilemap.CellToWorld(new Vector3Int(node.Value.x, node.Value.y, 0)) + new Vector2(0.5f, 0.5f));
node = node.Parent;
}
}
}
private bool CanReachNode(PathFindingNode a, PathFindingNode b)
{
bool hit = Physics.Linecast(a.Position, b.Position);
return(!hit);
}
