/// <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); }
/// <summary> /// legt einen neuen LinkedLineNode an /// </summary> /// <param name="node">der untersuchte Knoten</param> /// <param name="parent">Vorgängerknoten</param> /// <param name="lineChangeNeeded">Flag, ob ein Spurwechsel zum erreichen dieses Knotens nötig ist</param> public LinkedLineNode(LineNode node, LineNode.LinkedLineNode parent, bool lineChangeNeeded) { this.node = node; this.parent = parent; this.lineChangeNeeded = lineChangeNeeded; }