/// <summary>
/// Copies the queue elements to a new array.
/// </summary>
/// <returns>A new array containing elements copied from the Queue.</returns>
public PriorityQueueItem <TValue, TPriority>[] ToArray()
{
PriorityQueueItem <TValue, TPriority>[] newItems =
new PriorityQueueItem <TValue, TPriority> [numItems];
Array.Copy(items, newItems, numItems);
return(newItems);
}
// Remove a node at a particular position in the queue.
private PriorityQueueItem <TValue, TPriority> RemoveAt(Int32 index)
{
// remove an item from the heap
PriorityQueueItem <TValue, TPriority> o = items[index];
PriorityQueueItem <TValue, TPriority> tmp = items[numItems - 1];
items[--numItems] = default(PriorityQueueItem <TValue, TPriority>);
if (numItems > 0)
{
int i = index;
int j = i + 1;
while (i < Count / 2)
{
if ((j < Count - 1) && (compareFunc(items[j].Priority, items[j + 1].Priority) < 0))
{
j++;
}
if (compareFunc(items[j].Priority, tmp.Priority) <= 0)
{
break;
}
items[i] = items[j];
i = j;
j *= 2;
}
items[i] = tmp;
}
return(o);
}
/// <summary>
/// Adds an object to the queue, in order by priority.
/// </summary>
/// <param name="value">The object to be added.</param>
/// <param name="priority">Priority of the object to be added.</param>
public void Enqueue(TValue value, TPriority priority)
{
if (numItems == capacity)
{
// need to increase capacity
// grow by 50 percent
SetCapacity((3 * Capacity) / 2);
}
// Create the new item
PriorityQueueItem <TValue, TPriority> newItem =
new PriorityQueueItem <TValue, TPriority>(value, priority);
int i = numItems;
++numItems;
// and insert it into the heap.
while ((i > 0) && (compareFunc(items[i / 2].Priority, newItem.Priority) < 0))
{
items[i] = items[i / 2];
i /= 2;
}
items[i] = newItem;
}
/// <summary>
/// Berechnet den kürzesten Weg zum targetNode und speichert diesen als Stack in WayToGo
/// Implementierung des A*-Algorithmus' frei nach Wikipedia :)
/// </summary>
/// <param name="startNode">Startknoten von dem aus der kürzeste Weg berechnet werden soll</param>
/// <param name="targetNodes">Liste von Zielknoten zu einem von denen der kürzeste Weg berechnet werden soll</param>
/// <param name="vehicleType">Vehicle type</param>
public static Routing CalculateShortestConenction(LineNode startNode, List <LineNode> targetNodes, Vehicle.IVehicle.VehicleTypes vehicleType)
{
PriorityQueue <LineNode.LinkedLineNode, double> openlist = new PriorityQueue <LineNode.LinkedLineNode, double>();
Stack <LineNode.LinkedLineNode> closedlist = new Stack <LineNode.LinkedLineNode>();
Routing toReturn = new Routing();
// Initialisierung der Open List, die Closed List ist noch leer
// (die Priorität bzw. der f Wert des Startknotens ist unerheblich)
openlist.Enqueue(new LineNode.LinkedLineNode(startNode, null, false), 0);
// diese Schleife wird durchlaufen bis entweder
// - die optimale Lösung gefunden wurde oder
// - feststeht, dass keine Lösung existiert
do
{
// Knoten mit dem geringsten (in diesem Fall größten) f Wert aus der Open List entfernen
PriorityQueueItem <LineNode.LinkedLineNode, double> currentNode = openlist.Dequeue();
// wurde das Ziel gefunden?
if (targetNodes.Contains(currentNode.Value.node))
{
// nun noch die closedList in eine Routing umwandeln
closedlist.Push(currentNode.Value);
LineNode.LinkedLineNode endnode = closedlist.Pop();
LineNode.LinkedLineNode startnode = endnode.parent;
while (startnode != null)
{
// einfacher/direkter Weg über eine NodeConnection
if (!endnode.lineChangeNeeded)
{
toReturn.Push(new RouteSegment(startnode.node.GetNodeConnectionTo(endnode.node), endnode.node, false, startnode.node.GetNodeConnectionTo(endnode.node).lineSegment.length));
}
// Spurwechsel nötig
else
{
NodeConnection formerConnection = startnode.parent.node.GetNodeConnectionTo(startnode.node);
double length = formerConnection.GetLengthToLineNodeViaLineChange(endnode.node) + Constants.lineChangePenalty;
// Anfangs-/ oder Endknoten des Spurwechsels ist eine Ampel => Kosten-Penalty, da hier verstärktes Verkehrsaufkommen zu erwarten ist
if ((endnode.node.tLight != null) || (startnode.node.tLight != null))
{
length += Constants.lineChangeBeforeTrafficLightPenalty;
}
toReturn.Push(new RouteSegment(formerConnection, endnode.node, true, length));
// TODO: Erklären: hier wird irgendwas doppelt gemacht - ich meine mich zu Erinnern,
// das das so soll, aber nicht warum. Bitte beizeiten analysieren und erklären
endnode = startnode;
startnode = startnode.parent;
}
endnode = startnode;
startnode = startnode.parent;
}
return(toReturn);
}
#region Nachfolgeknoten auf die Open List setzen
// Nachfolgeknoten auf die Open List setzen
// überprüft alle Nachfolgeknoten und fügt sie der Open List hinzu, wenn entweder
// - der Nachfolgeknoten zum ersten Mal gefunden wird oder
// - ein besserer Weg zu diesem Knoten gefunden wird
#region nächste LineNodes ohne Spurwechsel untersuchen
foreach (NodeConnection nc in currentNode.Value.node.nextConnections)
{
// prüfen, ob ich auf diesem NodeConnection überhaupt fahren darf
if (!nc.CheckForSuitability(vehicleType))
{
continue;
}
LineNode.LinkedLineNode successor = new LineNode.LinkedLineNode(nc.endNode, null, false);
bool nodeInClosedList = false;
foreach (LineNode.LinkedLineNode lln in closedlist)
{
if (lln.node == successor.node)
{
nodeInClosedList = true;
continue;
}
}
// wenn der Nachfolgeknoten bereits auf der Closed List ist - tue nichts
if (!nodeInClosedList)
{
NodeConnection theConnection = currentNode.Value.node.GetNodeConnectionTo(successor.node);
// f Wert für den neuen Weg berechnen: g Wert des Vorgängers plus die Kosten der
// gerade benutzten Kante plus die geschätzten Kosten von Nachfolger bis Ziel
double f = currentNode.Value.length // exakte Länge des bisher zurückgelegten Weges
+ theConnection.lineSegment.length; // exakte Länge des gerade untersuchten Segmentes
if (currentNode.Value.countOfParents < 3) // Stau kostet extra, aber nur, wenn innerhalb
{ // der nächsten 2 Connections
f += theConnection.vehicles.Count * Constants.vehicleOnRoutePenalty;
}
f += GetMinimumEuklidDistance(successor.node, targetNodes); // Minimumweg zum Ziel (Luftlinie)
f *= 14 / theConnection.targetVelocity;
f *= -1;
// gucke, ob der Node schon in der Liste drin ist und wenn ja, dann evtl. rausschmeißen
bool nodeInOpenlist = false;
foreach (PriorityQueueItem <LineNode.LinkedLineNode, double> pqi in openlist)
{
if (pqi.Value.node == successor.node)
{
if (f <= pqi.Priority)
{
nodeInOpenlist = true;
}
else
{
openlist.Remove(pqi.Value); // erst entfernen
}
break;
}
}
if (!nodeInOpenlist)
{
// Vorgängerzeiger setzen
successor.parent = currentNode.Value;
openlist.Enqueue(successor, f); // dann neu einfügen
}
}
}
#endregion
#region nächste LineNodes mit Spurwechsel untersuchen
if (currentNode.Value.parent != null)
{
NodeConnection currentConnection = currentNode.Value.parent.node.GetNodeConnectionTo(currentNode.Value.node);
if (currentConnection != null)
{
foreach (LineNode ln in currentConnection.viaLineChangeReachableNodes)
{
// prüfen, ob ich diesen LineNode überhaupt anfahren darf
if (!CheckLineNodeForIncomingSuitability(ln, vehicleType))
{
continue;
}
// neuen LinkedLineNode erstellen
LineNode.LinkedLineNode successor = new LineNode.LinkedLineNode(ln, null, true);
bool nodeInClosedList = false;
foreach (LineNode.LinkedLineNode lln in closedlist)
{
if (lln.node == successor.node)
{
nodeInClosedList = true;
break;
}
}
// wenn der Nachfolgeknoten bereits auf der Closed List ist - tue nichts
if (!nodeInClosedList)
{
// passendes LineChangeInterval finden
NodeConnection.LineChangeInterval lci;
currentConnection.lineChangeIntervals.TryGetValue(ln.hashcode, out lci);
if (lci.length < Constants.minimumLineChangeLength)
{
break;
}
// f-Wert für den neuen Weg berechnen: g Wert des Vorgängers plus die Kosten der
// gerade benutzten Kante plus die geschätzten Kosten von Nachfolger bis Ziel
double f = currentNode.Value.parent.length; // exakte Länge des bisher zurückgelegten Weges
f += currentConnection.GetLengthToLineNodeViaLineChange(successor.node);
// Kostenanteil, für den Spurwechsel dazuaddieren
f += (lci.length < 2 * Constants.minimumLineChangeLength) ? 2 * Constants.lineChangePenalty : Constants.lineChangePenalty;
// Anfangs-/ oder Endknoten des Spurwechsels ist eine Ampel => Kosten-Penalty, da hier verstärktes Verkehrsaufkommen zu erwarten ist
if ((lci.targetNode.tLight != null) || (currentConnection.startNode.tLight != null))
{
f += Constants.lineChangeBeforeTrafficLightPenalty;
}
f += GetMinimumEuklidDistance(successor.node, targetNodes); // Minimumweg zum Ziel (Luftlinie)
f *= -1;
// gucke, ob der Node schon in der Liste drin ist und wenn ja, dann evtl. rausschmeißen
bool nodeInOpenlist = false;
foreach (PriorityQueueItem <LineNode.LinkedLineNode, double> pqi in openlist)
{
if (pqi.Value.node == successor.node)
{
if (f <= pqi.Priority)
{
nodeInOpenlist = true;
}
else
{
openlist.Remove(pqi.Value); // erst entfernen
}
break;
}
}
if (!nodeInOpenlist)
{
// Vorgängerzeiger setzen
successor.parent = currentNode.Value;
openlist.Enqueue(successor, f); // dann neu einfügen
}
}
}
}
}
#endregion
#endregion
// der aktuelle Knoten ist nun abschließend untersucht
closedlist.Push(currentNode.Value);
}while (openlist.Count != 0);
// Es wurde kein Weg gefunden - dann lassen wir das Auto sich selbst zerstören:
return(toReturn);
}
