Poprawianie etykiet

Dobrze, to mój pierwszy post więc przy okazji witam się ze wszystkimi :)

Ale przechodząc do problemu...
Otóż mam napisać program, który za pomocą algorytmu poprawiania etykiet wyszuka mi odpowiednią drogę pomiędzy dwoma wierzchołkami w grafie.

public List<int> LabelCorrectingAlg(int StartNode, int EndNode, float SendSize)
{
FIFO = new Queue<int>();
FIFO.Enqueue(StartNode);
double[] cost = new double[Nodes.Count];
labeled = new Boolean[Nodes.Count];
visited = new Boolean[Nodes.Count];
cost[StartNode] = 0;
labeled[StartNode] = true;
while(FIFO.Count!=0)
{
int k = FIFO.Dequeue();
if (visited[k] != true)
{
visited[k] = true;
for (int z = 0; z < Nodes.Count; z++)
{
if (ConnectionMatrix[z, k] == true)
{
help1 = k;
help2 = z;
Edge edd = Edges.Find(Arc);
double Help = CalcCost(edd, SendSize);
if (labeled[z] == true )
{
if (cost[z] > cost[k] + Help)
{
cost[z] = cost[k] + Help;
}
}
else
{
cost[z] = cost[k] + Help;
labeled[z] = true;
}
FIFO.Enqueue(z);
}
}
if (labeled[EndNode] == true && visited[EndNode]==false) FIFO.Clear();
}
}
int u = -1;
double value = cost[EndNode];
int Helper = EndNode;
List<int> Path = new List<int>();
Path.Add(EndNode);
while (u != StartNode)
{
for (int i = 0; i < Nodes.Count; i++)
{
if (ConnectionMatrix[i, Helper] == true && Path.Contains(i) == false)
{
help1 = i;
help2 = Helper;
Edge edd = Edges.Find(Arc);
Console.WriteLine(i + " " + cost[Helper] +" " + (CalcCost(edd, SendSize) + cost[i]));
if (cost[i] + CalcCost(edd, SendSize) == cost[Helper])
{
Helper = i;
Path.Add(i);
if (i == StartNode)
{
u = StartNode;
i = Nodes.Count;
}
}
}
}
}
return Path;
}

Tak wygląda to co naskrobałem... I otóż, mój problem polega na tym, że sam algorytm działa poprawnie, oznacza wszystkie wierzchołki odpowiednio... Ale jest jedno ale. Czasami się "zacina", mianowicie nie może znaleźć odpowiedniej krawędzi żeby przeskoczyć do następnej iteracji, i pomimo mojego wysiłku umysłowego nie udało mi się dojść, czemu...
ConnectionMatrix to oczywiście tablica połączeń wierzchołków, CalcCost to funkcja licząca koszt. Będę wdzięczny za wszelkie rady :)
Pozdrawiam

Najwyraźniej samo napisanie tutaj dało mi wenę :D
Ale dla potomnych napiszę, iż wystarczy dodatkowo stworzyć tablicę PreviousNode, która będzie zapisywała poprzednie nody dla danego poprawionego, i z jej pomocą cofnąć się z ostatniego wierzchołka do pierwszego, o tak:
List<int> Path = new List<int>();
int Alfa = EndNode;
while (Alfa != StartNode)
{
Path.Add(Alfa);
Alfa = PreviousNode[Alfa];
}
Path.Add(StartNode);
return Path;