public List<Node> AStar(Node start, Node dest)
{
for (int k = 0; k < graph.Length; k++) {
graph[k].g = 0;
graph[k].f = 0;
}
List <Node> path= new List<Node>();
Node[] Closed = new Node[graph.Length];
int closed_counter = 0;
JasonQ Open = new JasonQ();
JasonQ neighborSet;
Open.add(start);
for (int k = 0; k < graph.Length; k++) {
if (graph[k] != start) {
Open.add(graph[k]);
graph[k].g = 0;
graph[k].f = 0;
}
}
Node current;
double g_score = 0;
double f_score = g_score + start.H(dest);
start.g = g_score;
start.f = f_score;
double tempg = 0;
while (Open.Size() > 0)
{
current = Open.dequeue();
if (current == dest)
{
path.Add(current);
return path;
}
Closed[closed_counter++] = current;
for (int k = 0; k < current.neighbors.Count; k++) {
tempg = current.g + current.getTo(k);
if (isIn(Closed, current.neighbors[k]))
{
if (tempg >= current.neighbors[k].g)
{
}
else
{
//put camefrom to neighbor somehow. this following line is my attempt
path.Add(current.neighbors[k]);
current.neighbors[k].g = tempg;
current.neighbors[k].f = tempg + current.neighbors[k].H(dest);
if (!isIn(Open, current.neighbors[k]))
{
Open.add(current.neighbors[k]);
}
}
}
else
{
//put camefrom to neighbor somehow. this following line is my attempt
path.Add(current.neighbors[k]);
current.neighbors[k].g = tempg;
current.neighbors[k].f = tempg + current.neighbors[k].H(dest);
if (!isIn(Open, current.neighbors[k]))
{
Open.add(current.neighbors[k]);
}
}
}
}
return path;
}
public bool isIn(JasonQ o, Node node)
{
for (int k = 0; k < o.Size(); k++)
{
if (o.get(k) == node) return true;
}
return false;
}
