void Dijkstra(IEnumerable<int> targets, double TerrainCost)
{
// Unvisited nodes at the border
var unvisited = new Queue();
for (int i = 0, n = Nodes.Length; i < n; i++)
{
Nodes[i].g_score = double.MaxValue;
Nodes[i].visited = false;
}
foreach (int i in targets)
{
Nodes[i].g_score = 0;
Nodes[i].next = i;
unvisited.Add(0, i);
}
while (!unvisited.Empty)
{
int currentIdx = unvisited.RemoveBest();
double currentDistance = Nodes[currentIdx].g_score;
// Update distances
foreach (int j in Map.GetNeighbors(currentIdx))
{
if (Nodes[j].visited)
continue;
if (!Map.IsNodeAllowed(j))
continue;
double edgeDistance = Map.Cost(j, currentIdx);
double distance = currentDistance + edgeDistance;
if (Nodes[j].g_score > distance)
{
double oldScore = Nodes[j].g_score;
Nodes[j].g_score = distance;
Nodes[j].next = currentIdx;
unvisited.ChangeScore(oldScore, Nodes[j].g_score, j);
}
}
Nodes[currentIdx].visited = true;
}
}
void AStar(int target, int origin, double TerrainCost)
{
// Unvisited nodes at the border
var open_set = new Queue();
for (int i = 0, n = Nodes.Length; i < n; i++)
{
Nodes[i].g_score = double.MaxValue;
Nodes[i].visited = false;
}
Nodes[target].f_score = Map.Heuristic(target, origin);
Nodes[target].g_score = 0;
Nodes[target].next = target;
open_set.Add(Nodes[target].f_score, target);
while (!open_set.Empty)
{
int currentIdx = open_set.RemoveBest();
// Update distances
foreach (int j in Map.GetNeighbors(currentIdx))
{
if (Nodes[j].visited)
continue;
if (!Map.IsNodeAllowed(j))
continue;
double edgeDistance = Map.Cost(j, currentIdx);
double distance = Nodes[currentIdx].g_score + edgeDistance;
if (Nodes[j].g_score > distance)
{
double oldScore = Nodes[j].f_score;
Nodes[j].f_score = distance + Map.Heuristic(j, origin);
Nodes[j].g_score = distance;
Nodes[j].next = currentIdx;
open_set.ChangeScore(oldScore, Nodes[j].f_score, j);
}
}
Nodes[currentIdx].visited = true;
if (currentIdx == origin)
return;
}
}