//--- || Dijkstra heap search / sort ||---\\
public void DijkstraSearchHeap(Vector3 startPos, Vector3 goalPos){
Stopwatch dsw = new Stopwatch();
dsw.Start();
DNode startNode = grid.DNodeFromWorldPoint(startPos);
DNode endNode = grid.DNodeFromWorldPoint(goalPos);
startNode.gCost = 0;
if (startNode.walkable && endNode.walkable){
openSet = new Heap<DNode>(grid.MaxSize);
HashSet<DNode> closedList = new HashSet<DNode>();
openSet.Add(startNode);
while(openSet.Count > 0){
DNode currentNode = openSet.RemoveFirst();
closedList.Add(currentNode);
if (currentNode == endNode){
dsw.Stop();
print("Path found in: " + dsw.ElapsedMilliseconds + " ms");
DijkstraMilliseconds = dsw.ElapsedMilliseconds;
RetracePath(startNode, endNode);
break;
}
foreach (DNode neighbour in grid.GetDNeighbours(currentNode)){
if(!neighbour.walkable || closedList.Contains(neighbour) {
continue;
}
int tentative_dist = currentNode.gCost + GetDistance(currentNode, neighbour);
if(tentative_dist < neighbour.gCost){
neighbour.gCost = tentative_dist;
neighbour.parent = currentNode;
closedList.Add(neighbour);
grid.dClosedList = closedList;
}
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else { openSet.UpdateItem(neighbour); }
}
}
}
}