/// <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); }