public void GenerateRoute(ref Junction pStart, ref Junction pTarget)
{
// The set of nodes already evaluated
List<Junction> closedSet = new List<Junction>();
// The set of tentative nodes to be evaluated
List<Junction> openSet = new List<Junction>();
openSet.Add(pStart);
// The map of navigated nodes
List<Junction> cameFrom = new List<Junction>();
// Distance from start along optimal path
double gScore = 0;
// heuristic_estimate_of_distance
double hScore = pStart.Position().Subtract(pTarget.Position()).Length();
// Estimated total distance from start to goal
double fScore = hScore;
// Iterative value
int depth = 0;
// While openset is not empty
while (openSet.Count() != 0)
{
// Iterative value
depth = depth + 1;
// x := the node in openset having the lowest f_score[] value
// if x = goal - // return list
if(openSet[0].Index() == pTarget.Index())
break;
// remove x from openset
// add x to closedset
Junction considered = openSet[0];
openSet.Remove(considered);
closedSet.Add(considered);
//foreach y in neightbor_nodes(x)
double best_g_score = 10000000;
bool tentative_is_better = false;
foreach (Junction j in considered._ConnectedJunctions)
{
// if y in closedset
if(closedSet.Contains(j))
continue;
// tentative_g_score := g_score[x] + dist_between(x,y)
double tentative_g_score = j.Position().Subtract(pTarget.Position()).Length();
// if y not in openset
if(!openSet.Contains(j))
{
openSet.Add(j);
tentative_is_better = true;
} // else if tentative_g_score < g_score[y]
else if (tentative_g_score < best_g_score)
{
tentative_is_better = true;
}
else
tentative_is_better = false;
//if tentative_is_better = true
if(tentative_is_better)
{
cameFrom.Add(j);
gScore = tentative_g_score;
hScore = j.Position().Subtract(pTarget.Position()).Length();
fScore = tentative_g_score + hScore;
}
}
}
cameFrom.Reverse();
_CompleteRoute = cameFrom;
}