Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSDriveWay.cpp
15 : /// @author Jakob Erdmann
16 : /// @date December 2021
17 : ///
18 : // A sequende of rail tracks (lanes) that may be used as a "set route" (Fahrstraße)
19 : /****************************************************************************/
20 : #include <config.h>
21 : #include <cassert>
22 : #include <utility>
23 :
24 : #include <utils/xml/SUMOSAXAttributes.h>
25 : #include <utils/common/StringUtils.h>
26 : #include <microsim/MSStop.h>
27 : #include <microsim/MSLane.h>
28 : #include <microsim/MSEdge.h>
29 : #include <microsim/MSLink.h>
30 : #include <microsim/MSNet.h>
31 : #include <microsim/MSVehicleControl.h>
32 : #include <microsim/MSJunctionLogic.h>
33 : #include <mesosim/MELoop.h>
34 : #include "MSRailSignal.h"
35 : #include "MSDriveWay.h"
36 : #include "MSRailSignalControl.h"
37 :
38 : #define DRIVEWAY_SANITY_CHECK
39 : //#define SUBDRIVEWAY_WARN_NOCONFLICT
40 :
41 : //#define DEBUG_BUILD_DRIVEWAY
42 : //#define DEBUG_BUILD_SUBDRIVEWAY
43 : //#define DEBUG_ADD_FOES
44 : //#define DEBUG_BUILD_SIDINGS
45 : //#define DEBUG_DRIVEWAY_BUILDROUTE
46 : //#define DEBUG_CHECK_FLANKS
47 : //#define DEBUG_SIGNALSTATE_PRIORITY
48 : //#define DEBUG_SIGNALSTATE
49 : //#define DEBUG_FIND_PROTECTION
50 : //#define DEBUG_MOVEREMINDER
51 : //#define DEBUG_MATCH
52 :
53 : #define DEBUG_HELPER(obj) ((obj) != nullptr && (obj)->isSelected())
54 : //#define DEBUG_HELPER(obj) ((obj)->getID() == "")
55 : //#define DEBUG_HELPER(obj) (true)
56 :
57 : //#define DEBUG_COND_DW (dw->myNumericalID == 5)
58 : #define DEBUG_COND_DW (false)
59 :
60 : // ===========================================================================
61 : // static value definitions
62 : // ===========================================================================
63 : int MSDriveWay::myGlobalDriveWayIndex(0);
64 : std::set<const MSEdge*> MSDriveWay::myBlockLengthWarnings;
65 : bool MSDriveWay::myWriteVehicles(false);
66 : std::map<const MSLink*, std::vector<MSDriveWay*> > MSDriveWay::mySwitchDriveWays;
67 : std::map<const MSEdge*, std::vector<MSDriveWay*> > MSDriveWay::myReversalDriveWays;
68 : std::map<const MSEdge*, std::vector<MSDriveWay*>, ComparatorNumericalIdLess> MSDriveWay::myDepartureDriveways;
69 : std::map<const MSJunction*, int> MSDriveWay::myDepartDrivewayIndex;
70 : std::map<const MSEdge*, std::vector<MSDriveWay*> > MSDriveWay::myDepartureDrivewaysEnds;
71 : std::map<const MSEdge*, std::vector<MSDriveWay*>, ComparatorNumericalIdLess> MSDriveWay::myEndingDriveways;
72 : std::map<ConstMSEdgeVector, MSDriveWay*> MSDriveWay::myDriveWayRouteLookup;
73 : std::map<std::string, MSDriveWay*> MSDriveWay::myDriveWayLookup;
74 :
75 : // ---------------------------------------------------------------------------
76 : // static initialisation methods
77 : // ---------------------------------------------------------------------------
78 : void
79 40412 : MSDriveWay::init() {
80 40412 : myWriteVehicles = OptionsCont::getOptions().isSet("railsignal-vehicle-output");
81 40412 : }
82 :
83 : // ===========================================================================
84 : // MSDriveWay method definitions
85 : // ===========================================================================
86 :
87 :
88 20898 : MSDriveWay::MSDriveWay(const MSLink* origin, const std::string& id, bool temporary) :
89 41796 : MSMoveReminder("DriveWay_" + (temporary ? "tmp" : id)),
90 : Named(id),
91 10032 : myNumericalID(temporary ? -1 : myGlobalDriveWayIndex++),
92 20898 : myOrigin(origin),
93 20898 : myActive(nullptr),
94 20898 : myCoreSize(0),
95 20898 : myForwardEdgeCount(0),
96 20898 : myFoundSignal(false),
97 20898 : myFoundJump(false),
98 20898 : myTerminateRoute(false),
99 20898 : myAbortedBuild(false),
100 20898 : myBidiEnded(false),
101 72726 : myIsSubDriveway(false)
102 20898 : {}
103 :
104 :
105 30930 : MSDriveWay::~MSDriveWay() {
106 22964 : for (const MSDriveWay* sub : mySubDriveWays) {
107 2066 : delete sub;
108 : }
109 : mySubDriveWays.clear();
110 93624 : }
111 :
112 : void
113 40047 : MSDriveWay::cleanup() {
114 40047 : myGlobalDriveWayIndex = 0;
115 : myBlockLengthWarnings.clear();
116 40047 : myWriteVehicles = false;
117 :
118 42818 : for (auto item : myDepartureDriveways) {
119 5751 : for (MSDriveWay* dw : item.second) {
120 2980 : delete dw;
121 : }
122 : }
123 : MSDriveWay::mySwitchDriveWays.clear();
124 : MSDriveWay::myReversalDriveWays.clear();
125 : MSDriveWay::myDepartureDriveways.clear();
126 : MSDriveWay::myDepartDrivewayIndex.clear();
127 : MSDriveWay::myDepartureDrivewaysEnds.clear();
128 : MSDriveWay::myEndingDriveways.clear();
129 40047 : }
130 :
131 : void
132 187 : MSDriveWay::clearState() {
133 212 : for (auto item : myEndingDriveways) {
134 50 : for (MSDriveWay* dw : item.second) {
135 : dw->myTrains.clear();
136 : }
137 : }
138 187 : }
139 :
140 :
141 : bool
142 32628 : MSDriveWay::notifyEnter(SUMOTrafficObject& veh, Notification reason, const MSLane* enteredLane) {
143 : #ifdef DEBUG_MOVEREMINDER
144 : std::cout << SIMTIME << " notifyEnter " << getDescription() << " veh=" << veh.getID() << " lane=" << (MSGlobals::gUseMesoSim ? veh.getEdge()->getID() : Named::getIDSecure(enteredLane)) << " reason=" << reason << "\n";
145 : #endif
146 65256 : if (veh.isVehicle() && (enteredLane == myLane || (MSGlobals::gUseMesoSim && veh.getEdge() == &myLane->getEdge()))
147 63861 : && (reason == NOTIFICATION_DEPARTED || reason == NOTIFICATION_JUNCTION || reason == NOTIFICATION_PARKING)) {
148 31151 : SUMOVehicle& sveh = dynamic_cast<SUMOVehicle&>(veh);
149 31151 : MSRouteIterator firstIt = std::find(sveh.getCurrentRouteEdge(), sveh.getRoute().end(), myLane->getNextNormal());
150 31151 : if (match(firstIt, sveh.getRoute().end())) {
151 : if (myTrains.count(&sveh) == 0) {
152 19106 : enterDriveWay(sveh, reason);
153 : }
154 19128 : return true;
155 : }
156 1477 : } else if (reason == NOTIFICATION_REROUTE) {
157 : assert(veh.isVehicle());
158 1425 : SUMOVehicle& sveh = dynamic_cast<SUMOVehicle&>(veh);
159 : assert(myTrains.count(&sveh) == 0);
160 1425 : int movedPast = matchesPastRoute(sveh);
161 : // vehicle must still be one the drivway
162 1425 : if (movedPast >= 0 && movedPast < myForwardEdgeCount) {
163 719 : enterDriveWay(sveh, reason);
164 719 : return true;
165 : }
166 : }
167 : return false;
168 : }
169 :
170 :
171 : bool
172 105882 : MSDriveWay::notifyLeave(SUMOTrafficObject& veh, double /*lastPos*/, Notification reason, const MSLane* enteredLane) {
173 : UNUSED_PARAMETER(enteredLane);
174 : #ifdef DEBUG_MOVEREMINDER
175 : std::cout << SIMTIME << " notifyLeave " << getDescription() << " veh=" << veh.getID() << " lane=" << Named::getIDSecure(enteredLane) << " reason=" << toString(reason) << "\n";
176 : #endif
177 105882 : if (veh.isVehicle()) {
178 : // leaving network with departure, teleport etc
179 105882 : if (reason != MSMoveReminder::NOTIFICATION_JUNCTION && reason != MSMoveReminder::NOTIFICATION_SEGMENT) {
180 5786 : myTrains.erase(&dynamic_cast<SUMOVehicle&>(veh));
181 5786 : if (myWriteVehicles) {
182 524 : myVehicleEvents.push_back(VehicleEvent(SIMSTEP, false, veh.getID(), reason));
183 : }
184 5786 : return false;
185 100096 : } else if (MSGlobals::gUseMesoSim && reason != MSMoveReminder::NOTIFICATION_SEGMENT) {
186 : // notifyLeave is called before moving the route iterator
187 7138 : const MSLane* leftLane = (*(dynamic_cast<SUMOVehicle&>(veh).getCurrentRouteEdge()))->getLanes().front();
188 7138 : return notifyLeaveBack(veh, reason, leftLane);
189 : } else {
190 : return true;
191 : }
192 : } else {
193 : return false;
194 : }
195 : }
196 :
197 :
198 : bool
199 71762 : MSDriveWay::notifyLeaveBack(SUMOTrafficObject& veh, Notification reason, const MSLane* leftLane) {
200 : #ifdef DEBUG_MOVEREMINDER
201 : std::cout << SIMTIME << " notifyLeaveBack " << getDescription() << " veh=" << veh.getID() << " lane=" << Named::getIDSecure(leftLane) << " reason=" << toString(reason) << "\n";
202 : #endif
203 71762 : if (veh.isVehicle()) {
204 71762 : if (leftLane == myForward.back() && (veh.getBackLane() != leftLane->getBidiLane() || MSGlobals::gUseMesoSim)) {
205 13376 : myTrains.erase(&dynamic_cast<SUMOVehicle&>(veh));
206 13376 : if (myWriteVehicles) {
207 1234 : myVehicleEvents.push_back(VehicleEvent(SIMSTEP, false, veh.getID(), reason));
208 : }
209 13376 : return false;
210 : } else {
211 58386 : return true;
212 : }
213 : } else {
214 : return false;
215 : }
216 : }
217 :
218 :
219 : bool
220 1440 : MSDriveWay::notifyReroute(SUMOTrafficObject& veh) {
221 : #ifdef DEBUG_MOVEREMINDER
222 : std::cout << SIMTIME << " notifyReroute " << getDescription() << " veh=" << veh.getID() << "\n";
223 : #endif
224 : assert(veh.isVehicle());
225 1440 : SUMOVehicle* sveh = dynamic_cast<SUMOVehicle*>(&veh);
226 : assert(myTrains.count(sveh) != 0);
227 1440 : if (matchesPastRoute(*sveh) >= 0) {
228 : //std::cout << SIMTIME << " notifyReroute " << getDescription() << " veh=" << veh.getID() << " valid\n";
229 : return true;
230 : }
231 : // no match found, remove
232 : myTrains.erase(sveh);
233 701 : if (myWriteVehicles) {
234 30 : myVehicleEvents.push_back(VehicleEvent(SIMSTEP, false, veh.getID(), NOTIFICATION_REROUTE));
235 : }
236 : //std::cout << SIMTIME << " notifyReroute " << getDescription() << " veh=" << veh.getID() << " invalid\n";
237 : return false;
238 : }
239 :
240 :
241 : int
242 2865 : MSDriveWay::matchesPastRoute(SUMOVehicle& sveh) const {
243 : // look backwards along the route to find the driveway lane
244 2865 : const ConstMSEdgeVector& routeEdges = sveh.getRoute().getEdges();
245 18756 : for (int i = sveh.getRoutePosition(); i >= 0; i--) {
246 18338 : if (routeEdges[i] == myLane->getNextNormal()) {
247 : MSRouteIterator firstIt = routeEdges.begin() + i;
248 2447 : if (match(firstIt, sveh.getRoute().end())) {
249 : // driveway is still valid after rerouting
250 : //std::cout << SIMTIME << " notifyReroute " << getDescription() << " veh=" << veh.getID() << " valid\n";
251 1557 : return sveh.getRoutePosition() - i;
252 : }
253 890 : break;
254 : }
255 : }
256 : return -1;
257 : }
258 :
259 :
260 : void
261 19825 : MSDriveWay::enterDriveWay(SUMOVehicle& sveh, Notification reason) {
262 19825 : myTrains.insert(&sveh);
263 19825 : if (myOrigin != nullptr) {
264 13876 : MSRailSignalControl::getInstance().notifyApproach(myOrigin);
265 : }
266 65056 : for (const MSDriveWay* foe : myFoes) {
267 45231 : if (foe->myOrigin != nullptr) {
268 36225 : MSRailSignalControl::getInstance().notifyApproach(foe->myOrigin);
269 : }
270 : }
271 19825 : if (myWriteVehicles) {
272 1780 : myVehicleEvents.push_back(VehicleEvent(SIMSTEP, true, sveh.getID(), reason));
273 : }
274 19825 : }
275 :
276 : bool
277 596841 : MSDriveWay::reserve(const Approaching& closest, MSEdgeVector& occupied) {
278 596841 : if (foeDriveWayOccupied(true, closest.first, occupied)) {
279 : return false;
280 : }
281 493289 : for (MSLink* foeLink : myConflictLinks) {
282 135288 : if (hasLinkConflict(closest, foeLink)) {
283 : #ifdef DEBUG_SIGNALSTATE
284 : if (gDebugFlag4 || DEBUG_HELPER(closest.first)) {
285 : std::cout << getID() << " linkConflict with " << getTLLinkID(foeLink) << "\n";
286 : }
287 : #endif
288 : return false;
289 : }
290 : }
291 358001 : myActive = closest.first;
292 358001 : return true;
293 : }
294 :
295 :
296 : bool
297 135288 : MSDriveWay::hasLinkConflict(const Approaching& veh, const MSLink* foeLink) const {
298 : #ifdef DEBUG_SIGNALSTATE_PRIORITY
299 : if (gDebugFlag4) {
300 : std::cout << " checkLinkConflict foeLink=" << getTLLinkID(foeLink) << " ego=" << Named::getIDSecure(veh.first) << "\n";
301 : }
302 : #endif
303 135288 : if (foeLink->getApproaching().size() > 0) {
304 44642 : Approaching foe = foeLink->getClosest();
305 : #ifdef DEBUG_SIGNALSTATE_PRIORITY
306 : if (gDebugFlag4) {
307 : std::cout << " approaching foe=" << foe.first->getID() << "\n";
308 : }
309 : #endif
310 44642 : if (foe.first == veh.first) {
311 44642 : return false;
312 : }
313 : const MSTrafficLightLogic* foeTLL = foeLink->getTLLogic();
314 : assert(foeTLL != nullptr);
315 39831 : const MSRailSignal* constFoeRS = dynamic_cast<const MSRailSignal*>(foeTLL);
316 : MSRailSignal* foeRS = const_cast<MSRailSignal*>(constFoeRS);
317 39831 : if (foeRS != nullptr) {
318 39831 : const MSDriveWay& foeDriveWay = foeRS->retrieveDriveWayForVeh(foeLink->getTLIndex(), foe.first);
319 : MSEdgeVector occupied;
320 64032 : if (foeDriveWay.foeDriveWayOccupied(false, foe.first, occupied) ||
321 40424 : !foeRS->constraintsAllow(foe.first) ||
322 31876 : !overlap(foeDriveWay) ||
323 55484 : !isFoeOrSubFoe(&foeDriveWay) ||
324 12467 : canUseSiding(veh.first, &foeDriveWay).first) {
325 : #ifdef DEBUG_SIGNALSTATE_PRIORITY
326 : if (gDebugFlag4) {
327 : if (foeDriveWay.foeDriveWayOccupied(false, foe.first, occupied)) {
328 : std::cout << " foe blocked\n";
329 : } else if (!foeRS->constraintsAllow(foe.first)) {
330 : std::cout << " foe constrained\n";
331 : } else if (!overlap(foeDriveWay)) {
332 : std::cout << " no overlap\n";
333 : } else if (!isFoeOrSubFoe(&foeDriveWay)) {
334 : std::cout << " foeDW=" << foeDriveWay.getID() << " is not a foe to " << getID() << "\n";
335 : } else if (canUseSiding(veh.first, &foeDriveWay).first) {
336 : std::cout << " use siding\n";
337 : }
338 : }
339 : #endif
340 27502 : return false;
341 : }
342 : #ifdef DEBUG_SIGNALSTATE_PRIORITY
343 : if (gDebugFlag4) {
344 : std::cout
345 : << " aSB=" << veh.second.arrivalSpeedBraking << " foeASB=" << foe.second.arrivalSpeedBraking
346 : << " aT=" << veh.second.arrivalTime << " foeAT=" << foe.second.arrivalTime
347 : << " aS=" << veh.first->getSpeed() << " foeS=" << foe.first->getSpeed()
348 : << " aD=" << veh.second.dist << " foeD=" << foe.second.dist
349 : << " aW=" << veh.first->getWaitingTime() << " foeW=" << foe.first->getWaitingTime()
350 : << " aN=" << veh.first->getNumericalID() << " foeN=" << foe.first->getNumericalID()
351 : << "\n";
352 : }
353 : #endif
354 12329 : const bool yield = mustYield(veh, foe);
355 12329 : if (MSRailSignal::storeVehicles()) {
356 540 : MSRailSignal::rivalVehicles().push_back(foe.first);
357 540 : if (yield) {
358 315 : MSRailSignal::priorityVehicles().push_back(foe.first);
359 : }
360 : }
361 12329 : return yield;
362 39831 : }
363 : }
364 : return false;
365 : }
366 :
367 :
368 : bool
369 25437 : MSDriveWay::isFoeOrSubFoe(const MSDriveWay* foe) const {
370 25437 : if (std::find(myFoes.begin(), myFoes.end(), foe) != myFoes.end()) {
371 : return true;
372 : }
373 17491 : for (const MSDriveWay* sub : foe->mySubDriveWays) {
374 9784 : if (isFoeOrSubFoe(sub)) {
375 : return true;
376 : }
377 : }
378 : return false;
379 : }
380 :
381 :
382 : bool
383 12329 : MSDriveWay::mustYield(const Approaching& veh, const Approaching& foe) {
384 12329 : if (foe.second.arrivalSpeedBraking == veh.second.arrivalSpeedBraking) {
385 7954 : if (foe.second.arrivalTime == veh.second.arrivalTime) {
386 556 : if (foe.first->getSpeed() == veh.first->getSpeed()) {
387 536 : if (foe.second.dist == veh.second.dist) {
388 500 : if (foe.first->getWaitingTime() == veh.first->getWaitingTime()) {
389 482 : return foe.first->getNumericalID() < veh.first->getNumericalID();
390 : } else {
391 18 : return foe.first->getWaitingTime() > veh.first->getWaitingTime();
392 : }
393 : } else {
394 36 : return foe.second.dist < veh.second.dist;
395 : }
396 : } else {
397 20 : return foe.first->getSpeed() > veh.first->getSpeed();
398 : }
399 : } else {
400 7398 : return foe.second.arrivalTime < veh.second.arrivalTime;
401 : }
402 : } else {
403 4375 : return foe.second.arrivalSpeedBraking > veh.second.arrivalSpeedBraking;
404 : }
405 : }
406 :
407 :
408 : bool
409 16035 : MSDriveWay::conflictLaneOccupied(bool store, const SUMOVehicle* ego) const {
410 210513 : for (const MSLane* lane : myConflictLanes) {
411 : if (!lane->isEmpty()) {
412 4410 : std::string joinVehicle = "";
413 4410 : if (ego != nullptr && !MSGlobals::gUseMesoSim) {
414 0 : const SUMOVehicleParameter::Stop* stop = ego->getNextStopParameter();
415 0 : if (stop != nullptr) {
416 0 : joinVehicle = stop->join;
417 : }
418 : }
419 : #ifdef DEBUG_SIGNALSTATE
420 : if (gDebugFlag4) {
421 : std::cout << SIMTIME << " conflictLane " << lane->getID() << " occupied ego=" << Named::getIDSecure(ego) << " vehNumber=" << lane->getVehicleNumber() << "\n";
422 : if (joinVehicle != "") {
423 : std::cout << " joinVehicle=" << joinVehicle << " occupant=" << toString(lane->getVehiclesSecure()) << "\n";
424 : lane->releaseVehicles();
425 : }
426 : }
427 : #endif
428 4410 : if (lane->getVehicleNumberWithPartials() == 1) {
429 4410 : MSVehicle* foe = lane->getLastAnyVehicle();
430 4410 : if (joinVehicle != "") {
431 0 : if (foe->getID() == joinVehicle && foe->isStopped()) {
432 : #ifdef DEBUG_SIGNALSTATE
433 : if (gDebugFlag4) {
434 : std::cout << " ignore join-target '" << joinVehicle << "\n";
435 : }
436 : #endif
437 0 : continue;
438 : }
439 : }
440 4410 : if (ego != nullptr) {
441 0 : if (foe == ego && std::find(myForward.begin(), myForward.end(), lane) == myForward.end()) {
442 : #ifdef DEBUG_SIGNALSTATE
443 : if (gDebugFlag4) {
444 : std::cout << " ignore ego as oncoming '" << ego->getID() << "\n";
445 : }
446 : #endif
447 0 : continue;
448 : }
449 0 : if (foe->isStopped() && foe->getNextStopParameter()->join == ego->getID()) {
450 : #ifdef DEBUG_SIGNALSTATE
451 : if (gDebugFlag4) {
452 : std::cout << " ignore " << foe->getID() << " for which ego is join-target\n";
453 : }
454 : #endif
455 0 : continue;
456 : }
457 : }
458 : }
459 4410 : if (MSRailSignal::storeVehicles() && store) {
460 4410 : MSRailSignal::blockingVehicles().push_back(lane->getLastAnyVehicle());
461 : }
462 : return true;
463 : }
464 : }
465 11625 : return false;
466 : }
467 :
468 :
469 : bool
470 7122004 : MSDriveWay::foeDriveWayApproached() const {
471 14351308 : for (const MSDriveWay* foeDW : myFoes) {
472 14289132 : if (foeDW->myOrigin != nullptr && foeDW->myOrigin->getApproaching().size() > 0) {
473 : #ifdef DEBUG_SIGNALSTATE
474 : if (gDebugFlag4) {
475 : std::cout << SIMTIME << " foeLink=" << foeDW->myOrigin->getDescription() << " approachedBy=" << foeDW->myOrigin->getApproaching().begin()->first->getID() << "\n";
476 : }
477 : #endif
478 : return true;
479 : }
480 : }
481 : return false;
482 : }
483 :
484 :
485 : bool
486 8382952 : MSDriveWay::foeDriveWayOccupied(bool store, const SUMOVehicle* ego, MSEdgeVector& occupied) const {
487 24490957 : for (const MSDriveWay* foeDW : myFoes) {
488 16985689 : if (!foeDW->myTrains.empty()) {
489 : #ifdef DEBUG_SIGNALSTATE
490 : if (gDebugFlag4 || DEBUG_COND_DW || DEBUG_HELPER(ego)) {
491 : std::cout << SIMTIME << " " << getID() << " foeDriveWay " << foeDW->getID() << " occupied ego=" << Named::getIDSecure(ego) << " foeVeh=" << toString(foeDW->myTrains) << "\n";
492 : }
493 : #endif
494 903642 : if (foeDW->myTrains.size() == 1) {
495 902488 : SUMOVehicle* foe = *foeDW->myTrains.begin();
496 902488 : if (foe == ego) {
497 : #ifdef DEBUG_SIGNALSTATE
498 : if (gDebugFlag4 || DEBUG_HELPER(ego)) {
499 : std::cout << " ignore ego as foe '" << Named::getIDSecure(ego) << "\n";
500 : }
501 : #endif
502 26046 : continue;
503 : }
504 881689 : if (hasJoin(ego, foe)) {
505 36 : continue;
506 : }
507 : }
508 882807 : std::pair<bool, const MSDriveWay*> useSiding = canUseSiding(ego, foeDW);
509 : #ifdef DEBUG_SIGNALSTATE
510 : if (gDebugFlag4 || DEBUG_COND_DW || DEBUG_HELPER(ego)) {
511 : auto it = mySidings.find(foeDW);
512 : int numSidings = 0;
513 : if (it != mySidings.end()) {
514 : numSidings = it->second.size();
515 : }
516 : std::cout << " useSiding=" << useSiding.first << " sidingFoe=" << Named::getIDSecure(useSiding.second) << " numSidings=" << numSidings << "\n";
517 : }
518 : #endif
519 882807 : if (useSiding.first) {
520 5211 : continue;
521 : } else {
522 877596 : if (MSRailSignal::storeVehicles() && store) {
523 740 : for (SUMOVehicle* foe : foeDW->myTrains) {
524 370 : MSRailSignal::blockingVehicles().push_back(foe);
525 : }
526 370 : MSRailSignal::blockingDriveWays().push_back(foeDW);
527 : }
528 1756578 : for (const SUMOVehicle* foe : foeDW->myTrains) {
529 878982 : occupied.push_back(const_cast<MSEdge*>(foe->getEdge()));
530 878982 : MSEdge* bidi = const_cast<MSEdge*>(foe->getEdge()->getBidiEdge());
531 878982 : if (bidi != nullptr) {
532 478107 : occupied.push_back(bidi);
533 : }
534 : /// @todo: if foe occupies more than one edge we should add all of them to the occupied vector
535 : }
536 287924 : if (ego != nullptr && MSGlobals::gTimeToTeleportRSDeadlock > 0
537 1120094 : && (ego->getWaitingTime() > ego->getVehicleType().getCarFollowModel().getStartupDelay() || !ego->isOnRoad())) {
538 : // if there is an occupied siding, it becomes part of the waitRelation
539 136196 : SUMOVehicle* foe = *(useSiding.second == nullptr ? foeDW : useSiding.second)->myTrains.begin();
540 136196 : const MSRailSignal* rs = myOrigin != nullptr ? dynamic_cast<const MSRailSignal*>(myOrigin->getTLLogic()) : nullptr;
541 136196 : MSRailSignalControl::getInstance().addWaitRelation(ego, rs, foe);
542 : }
543 877596 : return true;
544 : }
545 16082047 : } else if (foeDW != this && isDepartDriveway() && !foeDW->isDepartDriveway()) {
546 9138 : if (foeDW->myOrigin->getApproaching().size() > 0) {
547 1601 : Approaching foeA = foeDW->myOrigin->getClosest();
548 1601 : const SUMOVehicle* foe = foeA.first;
549 1601 : if (foeA.second.dist < foe->getBrakeGap(true)) {
550 123 : MSRouteIterator firstIt = std::find(foe->getCurrentRouteEdge(), foe->getRoute().end(), foeDW->myRoute.front());
551 123 : if (firstIt != foe->getRoute().end()) {
552 123 : if (foeDW->match(firstIt, foe->getRoute().end())) {
553 88 : bool useSiding = canUseSiding(ego, foeDW).first;
554 : #ifdef DEBUG_SIGNALSTATE
555 : if (gDebugFlag4 || DEBUG_COND_DW || DEBUG_HELPER(ego)) {
556 : std::cout << SIMTIME << " " << getID() << " blocked by " << foeDW->getID() << " (approached by " << foe->getID() << ") useSiding=" << useSiding << "\n";
557 : }
558 : #endif
559 88 : if (useSiding) {
560 : //std::cout << SIMTIME << " " << getID() << " ego=" << ego->getID() << " foeDW=" << foeDW->getID() << " myFoes=" << toString(myFoes) << "\n";
561 0 : continue;
562 : } else {
563 88 : return true;
564 : }
565 : }
566 : }
567 : }
568 : }
569 : }
570 : }
571 7506238 : for (const std::set<const MSDriveWay*>& dlFoes : myDeadlocks) {
572 : bool allOccupied = true;
573 10051 : for (const MSDriveWay* dlFoe : dlFoes) {
574 7395 : if (dlFoe->myTrains.empty()) {
575 : allOccupied = false;
576 : //std::cout << SIMTIME << " " << getID() << " ego=" << Named::getIDSecure(ego) << " deadlockCheck clear " << dlFoe->getID() << "\n";
577 : break;
578 : }
579 : }
580 3626 : if (allOccupied) {
581 : #ifdef DEBUG_SIGNALSTATE
582 : if (gDebugFlag4 || DEBUG_COND_DW || DEBUG_HELPER(ego)) {
583 : std::cout << SIMTIME << " " << getID() << " ego=" << Named::getIDSecure(ego) << " deadlockCheck " << joinNamedToString(dlFoes, " ") << "\n";
584 : }
585 : #endif
586 8645 : for (const MSDriveWay* dlFoe : dlFoes) {
587 5989 : MSRailSignal::blockingDriveWays().push_back(dlFoe);
588 : }
589 : return true;
590 : }
591 : }
592 : return false;
593 : }
594 :
595 :
596 : bool
597 881689 : MSDriveWay::hasJoin(const SUMOVehicle* ego, const SUMOVehicle* foe) {
598 881689 : if (ego != nullptr && !MSGlobals::gUseMesoSim) {
599 225716 : std::string joinVehicle = "";
600 225716 : const SUMOVehicleParameter::Stop* stop = ego->getNextStopParameter();
601 225716 : if (stop != nullptr) {
602 91087 : joinVehicle = stop->join;
603 : }
604 225716 : if (joinVehicle == "" && !ego->hasDeparted() && ego->getStops().size() > 1) {
605 : // check one more stop
606 5968 : auto it = ego->getStops().begin();
607 : std::advance(it, 1);
608 5968 : joinVehicle = it->pars.join;
609 : }
610 225716 : if (joinVehicle != "") {
611 : #ifdef DEBUG_SIGNALSTATE
612 : if (gDebugFlag4 || DEBUG_COND_DW) {
613 : std::cout << " joinVehicle=" << joinVehicle << "\n";
614 : }
615 : #endif
616 345 : if (foe->getID() == joinVehicle && foe->isStopped()) {
617 : #ifdef DEBUG_SIGNALSTATE
618 : if (gDebugFlag4 || DEBUG_COND_DW) {
619 : std::cout << " ignore join-target '" << joinVehicle << "\n";
620 : }
621 : #endif
622 : return true;
623 : }
624 : }
625 :
626 225692 : if (foe->isStopped() && foe->getNextStopParameter()->join == ego->getID()) {
627 : #ifdef DEBUG_SIGNALSTATE
628 : if (gDebugFlag4 || DEBUG_COND_DW) {
629 : std::cout << " ignore " << foe->getID() << " for which ego is join-target\n";
630 : }
631 : #endif
632 : return true;
633 : }
634 : }
635 : return false;
636 : }
637 :
638 :
639 : std::pair<bool, const MSDriveWay*>
640 899151 : MSDriveWay::canUseSiding(const SUMOVehicle* ego, const MSDriveWay* foe, bool recurse) const {
641 : auto it = mySidings.find(foe);
642 899151 : if (it != mySidings.end()) {
643 14605 : for (auto siding : it->second) {
644 : // assume siding is usuable when computing state for unapproached signal (ego == nullptr)
645 13813 : if (ego == nullptr || siding.length >= ego->getLength()) {
646 : // if the siding is already "reserved" by another vehicle we cannot use it here
647 10462 : const MSEdge* sidingEnd = myRoute[siding.end];
648 23810 : for (MSDriveWay* sidingApproach : myEndingDriveways[sidingEnd]) {
649 18155 : if (!sidingApproach->myTrains.empty()) {
650 : // possibly the foe vehicle can use the other part of the siding
651 5113 : if (recurse) {
652 : const SUMOVehicle* foeVeh = nullptr;
653 3789 : if (!foe->myTrains.empty()) {
654 3775 : foeVeh = *foe->myTrains.begin();
655 14 : } else if (foe->myOrigin != nullptr && foe->myOrigin->getApproaching().size() > 0) {
656 14 : foeVeh = foe->myOrigin->getClosest().first;
657 : }
658 3789 : if (foeVeh == nullptr) {
659 0 : WRITE_WARNINGF("Invalid call to canUseSiding dw=% foe=% ego=% time=%", getID(), foe->getID(), Named::getIDSecure(ego), time2string(SIMSTEP));
660 0 : continue;
661 : }
662 3789 : if (foe->canUseSiding(foeVeh, this, false).first) {
663 306 : continue;
664 : }
665 : }
666 : // possibly the foe vehicle
667 : // @todo: in principle it might still be possible to continue if vehicle that approaches the siding can safely leave the situation
668 : #ifdef DEBUG_SIGNALSTATE
669 : if (gDebugFlag4 || DEBUG_COND_DW || DEBUG_HELPER(ego)) {
670 : std::cout << SIMTIME << " " << getID() << " ego=" << Named::getIDSecure(ego) << " foe=" << foe->getID()
671 : << " foeVeh=" << toString(foe->myTrains)
672 : << " sidingEnd=" << sidingEnd->getID() << " sidingApproach=" << sidingApproach->getID() << " approaching=" << toString(sidingApproach->myTrains) << "\n";
673 : }
674 : #endif
675 4807 : return std::make_pair(false, sidingApproach);
676 : }
677 : }
678 : //std::cout << SIMTIME << " " << getID() << " ego=" << Named::getIDSecure(ego) << " foe=" << foe->getID()
679 : // << " foeVeh=" << toString(foe->myTrains)
680 : // << " sidingEnd=" << sidingEnd->getID() << "usable\n";
681 5655 : return std::make_pair(true, nullptr);
682 : }
683 : }
684 : }
685 888689 : return std::make_pair(false, nullptr);
686 : }
687 :
688 : bool
689 19954 : MSDriveWay::overlap(const MSDriveWay& other) const {
690 32736 : for (int i = 0; i < myCoreSize; i++) {
691 373463 : for (int j = 0; j < other.myCoreSize; j++) {
692 360681 : const MSEdge* edge = myRoute[i];
693 360681 : const MSEdge* edge2 = other.myRoute[j];
694 : if (edge->getToJunction() == edge2->getToJunction()
695 360681 : || edge->getToJunction() == edge2->getFromJunction()) {
696 : // XXX might be rail_crossing with parallel tracks
697 : return true;
698 : }
699 : }
700 : }
701 : return false;
702 : }
703 :
704 :
705 : bool
706 73205 : MSDriveWay::flankConflict(const MSDriveWay& other) const {
707 243085 : for (const MSLane* lane : myForward) {
708 738198 : for (const MSLane* lane2 : other.myForward) {
709 562938 : if (lane == lane2) {
710 : return true;
711 : }
712 : }
713 2389573 : for (const MSLane* lane2 : other.myBidi) {
714 2216999 : if (lane == lane2) {
715 5258 : if (bidiBlockedBy(other)) {
716 : // it's only a deadlock if both trains block symmetrically
717 : return true;
718 : }
719 : }
720 : }
721 6082378 : for (const MSLane* lane2 : other.myBidiExtended) {
722 5912498 : if (lane == lane2) {
723 20270 : if (bidiBlockedBy(other)) {
724 : // it's only a deadlock if both trains block symmetrically
725 : return true;
726 : }
727 : }
728 : }
729 : }
730 : return false;
731 : }
732 :
733 :
734 : bool
735 62651 : MSDriveWay::crossingConflict(const MSDriveWay& other) const {
736 221759 : for (const MSLane* lane : myForward) {
737 642230 : for (const MSLane* lane2 : other.myForward) {
738 483122 : if (lane->isNormal() && lane2->isNormal() && lane->getEdge().getToJunction() == lane2->getEdge().getToJunction()) {
739 : return true;
740 : }
741 : }
742 : }
743 : return false;
744 : }
745 :
746 :
747 : bool
748 25868 : MSDriveWay::bidiBlockedBy(const MSDriveWay& other) const {
749 417492 : for (const MSLane* lane : myBidi) {
750 1376143 : for (const MSLane* lane2 : other.myForward) {
751 984519 : if (lane == lane2) {
752 : return true;
753 : }
754 : }
755 : }
756 442320 : for (const MSLane* lane : myBidiExtended) {
757 1434676 : for (const MSLane* lane2 : other.myForward) {
758 1014995 : if (lane == lane2) {
759 2250 : if (overlap(other)) {
760 : return true;
761 : }
762 : }
763 : }
764 : }
765 : return false;
766 : }
767 :
768 :
769 : bool
770 8018 : MSDriveWay::bidiBlockedByEnd(const MSDriveWay& other) const {
771 8018 : const MSLane* end = other.myForward.back();
772 63295 : for (const MSLane* lane : myBidi) {
773 58044 : if (lane == end) {
774 : return true;
775 : }
776 : }
777 41835 : for (const MSLane* lane : myBidiExtended) {
778 38065 : if (lane == end) {
779 1481 : if (overlap(other)) {
780 : return true;
781 : }
782 : }
783 : }
784 : return false;
785 : }
786 :
787 : bool
788 5769 : MSDriveWay::forwardRouteConflict(std::set<const MSEdge*> forward, const MSDriveWay& other, bool secondCheck) {
789 : int i = 0;
790 62373 : for (const MSEdge* edge2 : other.myRoute) {
791 61571 : if (i == other.myCoreSize) {
792 : return false;
793 : }
794 61571 : i++;
795 61571 : if (edge2 == myForward.front()->getNextNormal() && !secondCheck) {
796 : // foe should not pass from behind through our own forward section
797 : return false;
798 : }
799 : if (forward.count(edge2->getBidiEdge()) != 0) {
800 : return true;
801 : }
802 : }
803 : return false;
804 : }
805 :
806 : void
807 8057 : MSDriveWay::writeBlocks(OutputDevice& od) const {
808 14236 : od.openTag(myIsSubDriveway ? SUMO_TAG_SUBDRIVEWAY : SUMO_TAG_DRIVEWAY);
809 8057 : od.writeAttr(SUMO_ATTR_ID, myID);
810 8057 : od.writeAttr(SUMO_ATTR_VEHICLE, myFirstVehicle);
811 8057 : od.writeAttr(SUMO_ATTR_EDGES, toString(myRoute));
812 8057 : if (myCoreSize != (int)myRoute.size()) {
813 0 : od.writeAttr("core", myCoreSize);
814 : }
815 8057 : od.openTag("forward");
816 8057 : od.writeAttr(SUMO_ATTR_LANES, toString(myForward));
817 8057 : od.closeTag();
818 8057 : if (!myIsSubDriveway) {
819 6179 : od.openTag("bidi");
820 12358 : od.writeAttr(SUMO_ATTR_LANES, toString(myBidi));
821 6179 : if (myBidiExtended.size() > 0) {
822 1951 : od.lf();
823 1951 : od << " ";
824 3902 : od.writeAttr("deadlockCheck", toString(myBidiExtended));
825 : }
826 6179 : od.closeTag();
827 6179 : od.openTag("flank");
828 6179 : od.writeAttr(SUMO_ATTR_LANES, toString(myFlank));
829 6179 : od.closeTag();
830 :
831 12358 : od.openTag("conflictLinks");
832 :
833 : std::vector<std::string> signals;
834 9887 : for (MSLink* link : myConflictLinks) {
835 7416 : signals.push_back(getTLLinkID(link));
836 : }
837 12358 : od.writeAttr("signals", joinToStringSorting(signals, " "));
838 12358 : od.closeTag();
839 :
840 : std::vector<std::string> foes;
841 23006 : for (MSDriveWay* dw : myFoes) {
842 16827 : foes.push_back(dw->myID);
843 : }
844 6179 : if (foes.size() > 0) {
845 6144 : od.openTag("foes");
846 12288 : od.writeAttr("driveWays", joinToStringSorting(foes, " "));
847 12288 : od.closeTag();
848 : }
849 6795 : for (auto item : mySidings) {
850 616 : od.openTag("sidings");
851 1232 : od.writeAttr("foe", item.first->getID());
852 3163 : for (auto siding : item.second) {
853 2547 : od.openTag("siding");
854 2547 : od.writeAttr("start", myRoute[siding.start]->getID());
855 2547 : od.writeAttr("end", myRoute[siding.end]->getID());
856 2547 : od.writeAttr("length", siding.length);
857 5094 : od.closeTag();
858 : }
859 1232 : od.closeTag();
860 : }
861 6260 : for (auto item : myDeadlocks) {
862 81 : od.openTag("deadlock");
863 162 : od.writeAttr("foes", joinNamedToStringSorting(item, " "));
864 162 : od.closeTag();
865 : }
866 6179 : }
867 16114 : od.closeTag(); // driveWay
868 :
869 9935 : for (const MSDriveWay* sub : mySubDriveWays) {
870 1878 : sub->writeBlocks(od);
871 : }
872 : #ifdef DRIVEWAY_SANITY_CHECK
873 8057 : std::set<MSDriveWay*> uFoes(myFoes.begin(), myFoes.end());
874 8057 : if (uFoes.size() != myFoes.size()) {
875 0 : WRITE_WARNINGF("Duplicate foes in driveway '%'", getID());
876 :
877 : }
878 : #endif
879 8057 : }
880 :
881 :
882 : void
883 765 : MSDriveWay::writeBlockVehicles(OutputDevice& od) const {
884 1351 : od.openTag(myIsSubDriveway ? "subDriveWay" : "driveWay");
885 765 : od.writeAttr(SUMO_ATTR_ID, myID);
886 2556 : for (const VehicleEvent& ve : myVehicleEvents) {
887 2685 : od.openTag(ve.isEntry ? "entry" : "exit");
888 1791 : od.writeAttr(SUMO_ATTR_ID, ve.id);
889 1791 : od.writeAttr(SUMO_ATTR_TIME, time2string(ve.time));
890 1791 : od.writeAttr("reason", Notifications.getString(ve.reason));
891 3582 : od.closeTag(); // event
892 : }
893 1530 : od.closeTag(); // driveWay
894 :
895 944 : for (const MSDriveWay* sub : mySubDriveWays) {
896 179 : sub->writeBlockVehicles(od);
897 : }
898 765 : }
899 :
900 :
901 : void
902 7966 : MSDriveWay::buildRoute(const MSLink* origin,
903 : MSRouteIterator next, MSRouteIterator end,
904 : LaneVisitedMap& visited,
905 : std::set<MSLink*>& flankSwitches) {
906 7966 : double length = 0;
907 : bool seekForwardSignal = true;
908 : bool seekBidiSwitch = true;
909 : bool foundUnsafeSwitch = false;
910 7966 : MSLane* toLane = origin ? origin->getViaLaneOrLane() : (*next)->getLanes()[0];
911 20918 : const std::string warnID = origin ? "rail signal " + getClickableTLLinkID(origin) : "insertion lane '" + toLane->getID() + "'";
912 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
913 : gDebugFlag4 = DEBUG_COND_DW;
914 : if (gDebugFlag4) std::cout << "buildRoute origin=" << warnID << " vehRoute=" << toString(ConstMSEdgeVector(next, end))
915 : << " visited=" << formatVisitedMap(visited) << "\n";
916 : #endif
917 : while (true) {
918 98601 : if (length > MSGlobals::gMaxRailSignalBlockLength) {
919 : // typical block length in germany on main lines is 3-5km on branch lines up to 7km
920 : // special branches that are used by one train exclusively could also be up to 20km in length
921 : // minimum block size in germany is 37.5m (LZB)
922 : // larger countries (USA, Russia) might see blocks beyond 20km)
923 210 : if (seekForwardSignal && myBlockLengthWarnings.count(myRoute.front()) == 0) {
924 186 : WRITE_WARNINGF("Block after % exceeds maximum length (stopped searching after edge '%' (length=%m).",
925 : warnID, toLane->getEdge().getID(), length);
926 : myBlockLengthWarnings.insert(myRoute.front());
927 : }
928 210 : myAbortedBuild = true;
929 : // length exceeded
930 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
931 : if (gDebugFlag4) {
932 : std::cout << " abort: length=" << length << "\n";
933 : }
934 : #endif
935 7966 : return;
936 : }
937 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
938 : if (gDebugFlag4) {
939 : std::cout << " toLane=" << toLane->getID() << " visited=" << formatVisitedMap(visited) << "\n";
940 : }
941 : #endif
942 98391 : const MSEdge* current = &toLane->getEdge();
943 98391 : if (current->isNormal()) {
944 61534 : myRoute.push_back(current);
945 61534 : if (next != end) {
946 : next++;
947 : }
948 : }
949 98391 : appendMapIndex(visited, toLane);
950 98391 : length += toLane->getLength();
951 98391 : MSLane* bidi = toLane->getBidiLane();
952 98391 : if (seekForwardSignal) {
953 26083 : if (!foundUnsafeSwitch) {
954 26083 : myForward.push_back(toLane);
955 26083 : if (toLane->isNormal()) {
956 17373 : myForwardEdgeCount++;
957 : }
958 26083 : if (myForward.size() == 1) {
959 7966 : myLane = toLane;
960 7966 : if (MSGlobals::gUseMesoSim) {
961 2348 : MESegment* s = MSGlobals::gMesoNet->getSegmentForEdge(myLane->getEdge());
962 2348 : s->addDetector(this, myLane->getIndex());
963 : } else {
964 5618 : toLane->addMoveReminder(this, false);
965 : }
966 : }
967 : }
968 72308 : } else if (bidi == nullptr) {
969 22373 : if (toLane->isInternal() && toLane->getIncomingLanes().front().viaLink->isTurnaround()) {
970 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
971 : if (gDebugFlag4) {
972 : std::cout << " continue bidiSearch beyond turnaround\n";
973 : }
974 : #endif
975 : } else {
976 : seekBidiSwitch = false;
977 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
978 : if (gDebugFlag4) {
979 : std::cout << " noBidi, abort search for bidiSwitch\n";
980 : }
981 : #endif
982 : }
983 : }
984 98391 : if (bidi != nullptr) {
985 64715 : if (!seekForwardSignal && !foundUnsafeSwitch && bidi->isNormal()) {
986 : // look for switch that could protect from oncoming vehicles
987 23200 : for (const MSLink* const link : bidi->getLinkCont()) {
988 13015 : if (link->getDirection() == LinkDirection::TURN) {
989 531 : continue;
990 : }
991 24806 : if (!myBidi.empty() && link->getViaLaneOrLane() != myBidi.back()) {
992 2207 : myCoreSize = (int)myRoute.size() - 1;
993 2207 : MSLink* used = const_cast<MSLink*>(bidi->getLinkTo(myBidi.back()));
994 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
995 : if (gDebugFlag4) {
996 : std::cout << " found unsafe switch " << link->getDescription() << " (used=" << (used == nullptr ? "NULL" : used->getDescription()) << ")\n";
997 : }
998 : #endif
999 : // trains along our route beyond this switch might create deadlock
1000 : foundUnsafeSwitch = true;
1001 : // the switch itself must still be guarded to ensure safety
1002 2207 : if (used != nullptr) {
1003 : // possibly nullptr if there was an intermediate section of unidirectional edges
1004 : flankSwitches.insert(used);
1005 : }
1006 : break;
1007 : }
1008 : }
1009 : }
1010 62508 : if (foundUnsafeSwitch) {
1011 32533 : myBidiExtended.push_back(bidi);
1012 : } else {
1013 32182 : myBidi.push_back(bidi);
1014 : }
1015 : }
1016 98391 : const std::vector<MSLink*>& links = toLane->getLinkCont();
1017 98391 : toLane = nullptr;
1018 104754 : for (const MSLink* const link : links) {
1019 94798 : if ((next != end && &link->getLane()->getEdge() == *next)
1020 188376 : && isRailwayOrShared(link->getViaLaneOrLane()->getPermissions())) {
1021 147489 : toLane = link->getViaLaneOrLane();
1022 90991 : if (link->getTLLogic() != nullptr && link->getTLIndex() >= 0) {
1023 26383 : if (link == origin) {
1024 356 : if (seekForwardSignal) {
1025 0 : WRITE_WARNINGF(TL("Found circular block after % (% edges, length %)"), warnID, toString(myRoute.size()), toString(length));
1026 : }
1027 : //std::cout << getClickableTLLinkID(origin) << " circularBlock2=" << toString(myRoute) << "\n";
1028 356 : myAbortedBuild = true;
1029 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1030 : if (gDebugFlag4) {
1031 : std::cout << " abort: found circle\n";
1032 : }
1033 : #endif
1034 356 : return;
1035 : }
1036 : seekForwardSignal = false;
1037 26027 : myFoundSignal = true;
1038 : seekBidiSwitch = bidi != nullptr;
1039 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1040 : if (gDebugFlag4) {
1041 : std::cout << " found forwardSignal " << link->getTLLogic()->getID() << " seekBidiSwitch=" << seekBidiSwitch << "\n";
1042 : }
1043 : #endif
1044 : }
1045 : //if (links.size() > 1 && !foundUnsafeSwitch) {
1046 90635 : if (isSwitch(link)) {
1047 : // switch on driveway
1048 : //std::cout << "mySwitchDriveWays " << getID() << " link=" << link->getDescription() << "\n";
1049 51635 : mySwitchDriveWays[link].push_back(this);
1050 : }
1051 90635 : if (link->getLane()->getBidiLane() != nullptr && &link->getLane()->getEdge() == current->getBidiEdge()) {
1052 : // reversal on driveway
1053 1380 : myReversalDriveWays[current].push_back(this);
1054 1380 : myReversals.push_back(current);
1055 : }
1056 : break;
1057 : }
1058 : }
1059 98035 : if (toLane == nullptr) {
1060 7400 : if (next != end) {
1061 : // no connection found, jump to next route edge
1062 : toLane = (*next)->getLanes()[0];
1063 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1064 : if (gDebugFlag4) {
1065 : std::cout << " abort: turn-around or jump\n";
1066 : }
1067 : #endif
1068 87 : myFoundJump = true;
1069 87 : return;
1070 : } else {
1071 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1072 : if (gDebugFlag4) {
1073 : std::cout << " abort: no next lane available\n";
1074 : }
1075 : #endif
1076 7313 : myTerminateRoute = true;
1077 7313 : return;
1078 : }
1079 : }
1080 90635 : }
1081 : myBidiEnded = !seekBidiSwitch;
1082 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1083 : if (gDebugFlag4) {
1084 : std::cout << " normalEnd myBidiEnded=" << myBidiEnded << "\n";
1085 : }
1086 : #endif
1087 : }
1088 :
1089 :
1090 : bool
1091 101505 : MSDriveWay::isSwitch(const MSLink* link) {
1092 196633 : for (const MSLink* other : link->getLaneBefore()->getNormalPredecessorLane()->getLinkCont()) {
1093 111208 : if (other->getLane() != link->getLane() && !other->isTurnaround()) {
1094 : return true;
1095 : }
1096 : }
1097 138955 : for (auto ili : link->getLane()->getIncomingLanes()) {
1098 97465 : if (ili.viaLink != link && !ili.viaLink->isTurnaround()) {
1099 : return true;
1100 : }
1101 : }
1102 41490 : const MSLane* bidi = link->getLane()->getBidiLane();
1103 41490 : if (bidi != nullptr) {
1104 62974 : for (const MSLink* other : bidi->getLinkCont()) {
1105 32588 : if (other->getLane() != link->getLaneBefore()->getNormalPredecessorLane()->getBidiLane() && !other->isTurnaround()) {
1106 : return true;
1107 : }
1108 : }
1109 : }
1110 : return false;
1111 : }
1112 :
1113 :
1114 : void
1115 31864 : MSDriveWay::checkFlanks(const MSLink* originLink, const std::vector<const MSLane*>& lanes, const LaneVisitedMap& visited, bool allFoes, std::set<MSLink*>& flankSwitches) const {
1116 : #ifdef DEBUG_CHECK_FLANKS
1117 : std::cout << " checkFlanks lanes=" << toString(lanes) << " allFoes=" << allFoes << "\n";
1118 : #endif
1119 19944 : const MSLink* reverseOriginLink = originLink != nullptr && originLink->getLane()->getBidiLane() != nullptr && originLink->getLaneBefore()->getBidiLane() != nullptr
1120 42568 : ? originLink->getLane()->getBidiLane()->getLinkTo(originLink->getLaneBefore()->getBidiLane())
1121 : : nullptr;
1122 : //std::cout << " originLink=" << originLink->getDescription() << "\n";
1123 10704 : if (reverseOriginLink != nullptr) {
1124 10704 : reverseOriginLink = reverseOriginLink->getCorrespondingExitLink();
1125 : //std::cout << " reverseOriginLink=" << reverseOriginLink->getDescription() << "\n";
1126 : }
1127 125420 : for (int i = 0; i < (int)lanes.size(); i++) {
1128 93556 : const MSLane* lane = lanes[i];
1129 93556 : const MSLane* prev = i > 0 ? lanes[i - 1] : nullptr;
1130 93556 : const MSLane* next = i + 1 < (int)lanes.size() ? lanes[i + 1] : nullptr;
1131 93556 : if (lane->isInternal()) {
1132 32146 : continue;
1133 : }
1134 127616 : for (auto ili : lane->getIncomingLanes()) {
1135 76765 : if (ili.viaLink == originLink
1136 63854 : || ili.viaLink == reverseOriginLink
1137 60841 : || ili.viaLink->getDirection() == LinkDirection::TURN
1138 121853 : || ili.viaLink->getDirection() == LinkDirection::TURN_LEFTHAND) {
1139 10559 : continue;
1140 : }
1141 55647 : if (ili.lane != prev && ili.lane != next) {
1142 : #ifdef DEBUG_CHECK_FLANKS
1143 : std::cout << " add flankSwitch junction=" << ili.viaLink->getJunction()->getID() << " index=" << ili.viaLink->getIndex() << " iLane=" << ili.lane->getID() << " prev=" << Named::getIDSecure(prev) << " targetLane=" << lane->getID() << " next=" << Named::getIDSecure(next) << "\n";
1144 : #endif
1145 : flankSwitches.insert(ili.viaLink);
1146 44073 : } else if (allFoes) {
1147 : // link is part of the driveway, find foes that cross the driveway without entering
1148 11330 : checkCrossingFlanks(ili.viaLink, visited, flankSwitches);
1149 : }
1150 : }
1151 : }
1152 31864 : }
1153 :
1154 :
1155 : void
1156 11330 : MSDriveWay::checkCrossingFlanks(MSLink* dwLink, const LaneVisitedMap& visited, std::set<MSLink*>& flankSwitches) const {
1157 : #ifdef DEBUG_CHECK_FLANKS
1158 : std::cout << " checkCrossingFlanks dwLink=" << dwLink->getDescription() << " visited=" << formatVisitedMap(visited) << "\n";
1159 : #endif
1160 : const MSJunction* junction = dwLink->getJunction();
1161 11330 : if (junction == nullptr) {
1162 : return; // unregulated junction;
1163 : }
1164 11256 : const MSJunctionLogic* logic = junction->getLogic();
1165 11256 : if (logic == nullptr) {
1166 : return; // unregulated junction;
1167 : }
1168 57872 : for (const MSEdge* in : junction->getIncoming()) {
1169 46676 : if (in->isInternal()) {
1170 23855 : continue;
1171 : }
1172 45696 : for (MSLane* inLane : in->getLanes()) {
1173 22875 : const MSLane* inBidi = inLane->getBidiLane();
1174 32168 : if (isRailwayOrShared(inLane->getPermissions()) && visited.count(inLane) == 0 && (inBidi == nullptr || visited.count(inBidi) == 0)) {
1175 7696 : for (MSLink* link : inLane->getLinkCont()) {
1176 3997 : if (link->getIndex() >= 0 && logic->getFoesFor(dwLink->getIndex()).test(link->getIndex())
1177 10410 : && visited.count(link->getLane()) == 0) {
1178 : #ifdef DEBUG_CHECK_FLANKS
1179 : std::cout << " add crossing flankSwitch junction=" << junction->getID() << " index=" << link->getIndex() << "\n";
1180 : #endif
1181 542 : if (link->getViaLane() == nullptr) {
1182 : flankSwitches.insert(link);
1183 : } else {
1184 : flankSwitches.insert(link->getViaLane()->getLinkCont().front());
1185 : }
1186 : }
1187 : }
1188 : }
1189 : }
1190 : }
1191 : }
1192 :
1193 : void
1194 13959 : MSDriveWay::findFlankProtection(MSLink* link, MSLink* origLink, std::vector<const MSLane*>& flank) {
1195 : #ifdef DEBUG_CHECK_FLANKS
1196 : std::cout << " findFlankProtection link=" << link->getDescription() << " origLink=" << origLink->getDescription() << "\n";
1197 : #endif
1198 13959 : if (link->getCorrespondingEntryLink()->getTLLogic() != nullptr && link->getJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
1199 2212 : MSLink* entry = const_cast<MSLink*>(link->getCorrespondingEntryLink());
1200 : // guarded by signal
1201 : #ifdef DEBUG_CHECK_FLANKS
1202 : std::cout << " flank guarded by " << entry->getTLLogic()->getID() << "\n";
1203 : #endif
1204 : // @note, technically it's enough to collect links from foe driveways
1205 : // but this also adds "unused" conflict links which may aid comprehension
1206 2212 : myConflictLinks.push_back(entry);
1207 2212 : addFoes(entry);
1208 : } else {
1209 : const MSLane* lane = link->getLaneBefore();
1210 : std::vector<MSLink*> predLinks;
1211 23446 : for (auto ili : lane->getIncomingLanes()) {
1212 11699 : if (!ili.viaLink->isTurnaround()) {
1213 11312 : predLinks.push_back(ili.viaLink);
1214 : }
1215 : }
1216 11747 : if (predLinks.size() > 1) {
1217 : // this is a switch
1218 : #ifdef DEBUG_ADD_FOES
1219 : std::cout << " predecessors of " << link->getDescription() << " isSwitch\n";
1220 : #endif
1221 663 : for (MSLink* pred : predLinks) {
1222 442 : addSwitchFoes(pred);
1223 : }
1224 11526 : } else if (predLinks.size() == 1) {
1225 10870 : if (isSwitch(link)) {
1226 9476 : addSwitchFoes(link);
1227 : } else {
1228 : // continue upstream via single predecessor
1229 1394 : findFlankProtection(predLinks.front(), origLink, flank);
1230 : }
1231 : }
1232 : // check for insertions
1233 11747 : if (myDepartureDriveways.count(&lane->getEdge()) != 0) {
1234 296 : for (MSDriveWay* foe : myDepartureDriveways[&lane->getEdge()]) {
1235 150 : if (flankConflict(*foe) || crossingConflict(*foe)) {
1236 : #ifdef DEBUG_ADD_FOES
1237 : std::cout << " foe " << foe->getID() << " departs on flank=" << lane->getID() << "\n";
1238 : #endif
1239 110 : myFoes.push_back(foe);
1240 : } else {
1241 : #ifdef DEBUG_ADD_FOES
1242 : std::cout << " cand foe " << foe->getID() << " departs on flank=" << lane->getID() << " rejected\n";
1243 : #endif
1244 : }
1245 : }
1246 : }
1247 11747 : }
1248 13959 : }
1249 :
1250 :
1251 : void
1252 9918 : MSDriveWay::addSwitchFoes(MSLink* link) {
1253 : auto it = mySwitchDriveWays.find(link);
1254 9918 : if (it != mySwitchDriveWays.end()) {
1255 : #ifdef DEBUG_ADD_FOES
1256 : std::cout << " driveway " << myID << " addSwitchFoes for link " << link->getDescription() << "\n";
1257 : #endif
1258 33859 : for (MSDriveWay* foe : it->second) {
1259 30155 : if (foe != this && (flankConflict(*foe) || foe->flankConflict(*this) || crossingConflict(*foe) || foe->crossingConflict(*this))) {
1260 : #ifdef DEBUG_ADD_FOES
1261 : std::cout << " foe=" << foe->myID
1262 : << " fc1=" << flankConflict(*foe) << " fc2=" << foe->flankConflict(*this)
1263 : << " cc1=" << crossingConflict(*foe) << " cc2=" << foe->crossingConflict(*this) << "\n";
1264 : #endif
1265 3737 : myFoes.push_back(foe);
1266 : } else {
1267 : #ifdef DEBUG_ADD_FOES
1268 : std::cout << " cand=" << foe->myID << "\n";
1269 : #endif
1270 : }
1271 : }
1272 : }
1273 9918 : }
1274 :
1275 :
1276 : MSDriveWay*
1277 7966 : MSDriveWay::buildDriveWay(const std::string& id, const MSLink* link, MSRouteIterator first, MSRouteIterator end) {
1278 : // collect lanes and links that are relevant for setting this signal for the current driveWay
1279 : // For each driveway we collect
1280 : // - conflictLanes (signal must be red if any conflict lane is occupied)
1281 : // - conflictLinks (signal must be red if any conflict link is approached by a vehicle
1282 : // - that cannot break in time (arrivalSpeedBraking > 0)
1283 : // - approached by a vehicle with higher switching priority (see #3941)
1284 : // These objects are construct in steps:
1285 : //
1286 : // forwardBlock
1287 : // - search forward recursive from outgoing lane until controlled railSignal link found
1288 : // -> add all found lanes to conflictLanes
1289 : //
1290 : // bidiBlock (if any forwardBlock edge has bidi edge)
1291 : // - search bidi backward recursive until first switch
1292 : // - from switch search backward recursive all other incoming until controlled rail signal link
1293 : // -> add final links to conflictLinks
1294 : //
1295 : // flanks
1296 : // - search backward recursive from flanking switches
1297 : // until controlled railSignal link or protecting switch is found
1298 : // -> add all found lanes to conflictLanes
1299 : // -> add final links to conflictLinks
1300 7966 : MSDriveWay* dw = new MSDriveWay(link, id);
1301 : LaneVisitedMap visited;
1302 : std::vector<const MSLane*> before;
1303 7966 : MSLane* fromBidi = nullptr;
1304 7966 : if (link != nullptr) {
1305 4986 : appendMapIndex(visited, link->getLaneBefore());
1306 4986 : fromBidi = link->getLaneBefore()->getBidiLane();
1307 : }
1308 : std::set<MSLink*> flankSwitches; // list of switches that threaten the driveway and for which protection must be found
1309 :
1310 7966 : if (fromBidi != nullptr) {
1311 2758 : before.push_back(fromBidi);
1312 : }
1313 7966 : dw->buildRoute(link, first, end, visited, flankSwitches);
1314 7966 : dw->myCoreSize = (int)dw->myRoute.size();
1315 7966 : dw->checkFlanks(link, dw->myForward, visited, true, flankSwitches);
1316 7966 : dw->checkFlanks(link, dw->myBidi, visited, false, flankSwitches);
1317 7966 : dw->checkFlanks(link, before, visited, true, flankSwitches);
1318 16489 : for (MSLink* fsLink : flankSwitches) {
1319 : #ifdef DEBUG_ADD_FOES
1320 : if (DEBUG_COND_DW) {
1321 : std::cout << " fsLink=" << fsLink->getDescription() << "\n";
1322 : }
1323 : #endif
1324 8523 : dw->findFlankProtection(fsLink, fsLink, dw->myFlank);
1325 : }
1326 : std::set<MSLink*> flankSwitchesBidiExtended;
1327 7966 : dw->checkFlanks(link, dw->myBidiExtended, visited, false, flankSwitchesBidiExtended);
1328 12008 : for (MSLink* const flink : flankSwitchesBidiExtended) {
1329 : #ifdef DEBUG_ADD_FOES
1330 : if (DEBUG_COND_DW) {
1331 : std::cout << " fsLinkExtended=" << flink->getDescription() << "\n";
1332 : }
1333 : #endif
1334 4042 : dw->findFlankProtection(flink, flink, dw->myBidiExtended);
1335 : }
1336 7966 : MSRailSignal* rs = link ? const_cast<MSRailSignal*>(static_cast<const MSRailSignal*>(link->getTLLogic())) : nullptr;
1337 12952 : const bool movingBlock = (rs && rs->isMovingBlock()) || (!rs && OptionsCont::getOptions().getBool("railsignal-moving-block"));
1338 : #ifdef DEBUG_BUILD_DRIVEWAY
1339 : if (DEBUG_COND_DW) {
1340 : std::cout << SIMTIME << " buildDriveWay " << dw->myID << " link=" << (link == nullptr ? "NULL" : link->getDescription())
1341 : << "\n route=" << toString(dw->myRoute)
1342 : << "\n forward=" << toString(dw->myForward)
1343 : << "\n bidi=" << toString(dw->myBidi)
1344 : << "\n bidiEx=" << toString(dw->myBidiExtended)
1345 : << "\n flank=" << toString(dw->myFlank)
1346 : << "\n flankSwitch=" << MSRailSignal::describeLinks(std::vector<MSLink*>(flankSwitches.begin(), flankSwitches.end()))
1347 : << "\n coreSize=" << dw->myCoreSize
1348 : << "\n";
1349 : }
1350 : #endif
1351 7966 : if (!rs || !rs->isMovingBlock()) {
1352 7911 : dw->myConflictLanes.insert(dw->myConflictLanes.end(), dw->myForward.begin(), dw->myForward.end());
1353 : }
1354 7966 : dw->myConflictLanes.insert(dw->myConflictLanes.end(), dw->myBidi.begin(), dw->myBidi.end());
1355 7966 : dw->myConflictLanes.insert(dw->myConflictLanes.end(), dw->myFlank.begin(), dw->myFlank.end());
1356 7966 : dw->addBidiFoes(rs, false);
1357 7966 : dw->addBidiFoes(rs, true);
1358 : // add driveways that start on the same signal / lane
1359 7966 : dw->addParallelFoes(link, *first);
1360 : // add driveways that reverse along this driveways route
1361 7966 : dw->addReversalFoes(movingBlock);
1362 : // make foes unique and symmetrical
1363 7966 : std::set<MSDriveWay*, ComparatorNumericalIdLess> uniqueFoes(dw->myFoes.begin(), dw->myFoes.end());
1364 : dw->myFoes.clear();
1365 : // check for self-intersecting forward-section in movingBlock mode
1366 7966 : if (movingBlock && uniqueFoes.count(dw) == 0) {
1367 : std::set<const MSJunction*> forwardJunctions;
1368 964 : for (const MSLane* fw : dw->myForward) {
1369 877 : if (fw->isNormal()) {
1370 501 : const MSJunction* fwTo = fw->getEdge().getToJunction();
1371 : if (forwardJunctions.count(fwTo) == 1) {
1372 15 : dw->myFoes.push_back(dw);
1373 : #ifdef DEBUG_ADD_FOES
1374 : if (DEBUG_COND_DW) {
1375 : std::cout << " self-intersecting movingBlock for dw=" << dw->getID() << "\n";
1376 : }
1377 : #endif
1378 15 : break;
1379 : }
1380 : forwardJunctions.insert(fwTo);
1381 : }
1382 : }
1383 : }
1384 7966 : std::set<MSLink*> uniqueCLink(dw->myConflictLinks.begin(), dw->myConflictLinks.end());
1385 7966 : const MSEdge* lastEdge = &dw->myForward.back()->getEdge();
1386 14138 : for (MSDriveWay* foe : uniqueFoes) {
1387 6172 : const MSEdge* foeLastEdge = &foe->myForward.back()->getEdge();
1388 6172 : const bool sameLast = foeLastEdge == lastEdge;
1389 6172 : if (sameLast && !movingBlock) {
1390 2450 : dw->myFoes.push_back(foe);
1391 2450 : if (foe != dw) {
1392 2450 : foe->myFoes.push_back(dw);
1393 : }
1394 : } else {
1395 3722 : if (foe->bidiBlockedByEnd(*dw)) {
1396 : #ifdef DEBUG_ADD_FOES
1397 : if (DEBUG_COND_DW) {
1398 : std::cout << " setting " << dw->getID() << " as foe of " << foe->getID() << "\n";
1399 : }
1400 : #endif
1401 1867 : foe->myFoes.push_back(dw);
1402 1867 : foe->addSidings(dw);
1403 : } else {
1404 1855 : dw->buildSubFoe(foe, movingBlock);
1405 : }
1406 3722 : if (foe != dw) { // check for movingBlock
1407 3699 : if (dw->bidiBlockedByEnd(*foe)) {
1408 : #ifdef DEBUG_ADD_FOES
1409 : if (DEBUG_COND_DW) {
1410 : std::cout << " addFoeCheckSiding " << foe->getID() << "\n";
1411 : }
1412 : #endif
1413 2041 : dw->myFoes.push_back(foe);
1414 2041 : dw->addSidings(foe);
1415 : } else {
1416 1658 : foe->buildSubFoe(dw, movingBlock);
1417 : }
1418 : }
1419 : }
1420 6172 : if (link) {
1421 4521 : foe->addConflictLink(link);
1422 : }
1423 : // ignore links that have the same start junction
1424 6172 : if (foe->myRoute.front()->getFromJunction() != dw->myRoute.front()->getFromJunction()) {
1425 7702 : for (auto ili : foe->myForward.front()->getIncomingLanes()) {
1426 3544 : if (ili.viaLink->getTLLogic() != nullptr) {
1427 : // ignore links that originate on myBidi
1428 3104 : const MSLane* origin = ili.viaLink->getLaneBefore();
1429 3104 : if (std::find(dw->myBidi.begin(), dw->myBidi.end(), origin) == dw->myBidi.end()) {
1430 : uniqueCLink.insert(ili.viaLink);
1431 : }
1432 : }
1433 : }
1434 : }
1435 : }
1436 : dw->myConflictLinks.clear();
1437 7966 : dw->myConflictLinks.insert(dw->myConflictLinks.begin(), uniqueCLink.begin(), uniqueCLink.end());
1438 7966 : myEndingDriveways[lastEdge].push_back(dw);
1439 7966 : if (!movingBlock) {
1440 : // every driveway is it's own foe (also all driveways that depart in the same block)
1441 18339 : for (MSDriveWay* sameEnd : myEndingDriveways[lastEdge]) {
1442 : if (uniqueFoes.count(sameEnd) == 0) {
1443 8048 : dw->myFoes.push_back(sameEnd);
1444 8048 : if (sameEnd != dw) {
1445 207 : sameEnd->myFoes.push_back(dw);
1446 : }
1447 : }
1448 : }
1449 : }
1450 : #ifdef DEBUG_BUILD_DRIVEWAY
1451 : if (DEBUG_COND_DW) {
1452 : std::cout << dw->myID << " finalFoes " << toString(dw->myFoes) << "\n";
1453 : }
1454 : #endif
1455 7966 : return dw;
1456 7966 : }
1457 :
1458 : std::string
1459 3708 : MSDriveWay::getTLLinkID(const MSLink* link) {
1460 7416 : return link->getTLLogic()->getID() + "_" + toString(link->getTLIndex());
1461 : }
1462 :
1463 : std::string
1464 0 : MSDriveWay::getJunctionLinkID(const MSLink* link) {
1465 0 : return link->getJunction()->getID() + "_" + toString(link->getIndex());
1466 : }
1467 :
1468 : std::string
1469 4986 : MSDriveWay::getClickableTLLinkID(const MSLink* link) {
1470 14958 : return "junction '" + link->getTLLogic()->getID() + "', link " + toString(link->getTLIndex());
1471 : }
1472 :
1473 : std::string
1474 0 : MSDriveWay::formatVisitedMap(const LaneVisitedMap& visited) {
1475 : UNUSED_PARAMETER(visited);
1476 : /*
1477 : std::vector<const MSLane*> lanes(visited.size(), nullptr);
1478 : for (auto item : visited) {
1479 : lanes[item.second] = item.first;
1480 : }
1481 : for (auto it = lanes.begin(); it != lanes.end();) {
1482 : if (*it == nullptr) {
1483 : it = lanes.erase(it);
1484 : } else {
1485 : it++;
1486 : }
1487 : }
1488 : return toString(lanes);
1489 : */
1490 0 : return "dummy";
1491 : }
1492 :
1493 :
1494 : void
1495 103377 : MSDriveWay::appendMapIndex(LaneVisitedMap& map, const MSLane* lane) {
1496 : // avoid undefined behavior from evaluation order
1497 103377 : const int tmp = (int)map.size();
1498 103377 : map[lane] = tmp;
1499 103377 : }
1500 :
1501 : bool
1502 8012232 : MSDriveWay::match(MSRouteIterator firstIt, MSRouteIterator endIt) const {
1503 : // @todo optimize: it is sufficient to check for specific edges (after each switch)
1504 8012232 : auto itRoute = firstIt;
1505 : auto itDwRoute = myRoute.begin();
1506 : bool match = true;
1507 56439731 : while (itRoute != endIt && itDwRoute != myRoute.end()) {
1508 48617836 : if (*itRoute != *itDwRoute) {
1509 : match = false;
1510 : #ifdef DEBUG_MATCH
1511 : std::cout << " check dw=" << getID() << " match failed at vehEdge=" << (*itRoute)->getID() << " dwEdge=" << (*itDwRoute)->getID() << "\n";
1512 : #endif
1513 : break;
1514 : }
1515 : itRoute++;
1516 : itDwRoute++;
1517 : }
1518 : // if the vehicle arrives before the end of this driveway,
1519 : // we'd rather build a new driveway to avoid superfluous restrictions
1520 7821895 : if (match && itDwRoute == myRoute.end()
1521 15807467 : && (itRoute == endIt || myAbortedBuild || myBidiEnded || myFoundJump || myIsSubDriveway)) {
1522 : //std::cout << " using dw=" << "\n";
1523 7782580 : if (itRoute != endIt) {
1524 : // check whether the current route requires an extended driveway
1525 142289 : const MSEdge* next = *itRoute;
1526 142289 : const MSEdge* prev = myRoute.back();
1527 478 : if (myFoundJump && prev->getBidiEdge() != next && prev->getBidiEdge() != nullptr
1528 142540 : && prev->isConnectedTo(*next, (SUMOVehicleClass)(SVC_RAIL_CLASSES & prev->getPermissions()))) {
1529 : #ifdef DEBUG_MATCH
1530 : std::cout << " check dw=" << getID() << " prev=" << prev->getID() << " next=" << next->getID() << "\n";
1531 : #endif
1532 : return false;
1533 : }
1534 142272 : if (!myFoundJump && prev->getBidiEdge() == next && prev == &myForward.back()->getEdge()) {
1535 : assert(myIsSubDriveway || myBidiEnded);
1536 : // must not leave driveway via reversal
1537 : #ifdef DEBUG_MATCH
1538 : std::cout << getID() << " back=" << myForward.back()->getID() << " noMatch route " << toString(ConstMSEdgeVector(firstIt, endIt)) << "\n";
1539 : #endif
1540 : return false;
1541 : }
1542 : }
1543 7782557 : return true;
1544 : }
1545 : return false;
1546 : }
1547 :
1548 : void
1549 11443 : MSDriveWay::addFoes(const MSLink* link) {
1550 : #ifdef DEBUG_ADD_FOES
1551 : std::cout << "driveway " << myID << " addFoes for link " << link->getDescription() << "\n";
1552 : #endif
1553 11443 : const MSRailSignal* rs = dynamic_cast<const MSRailSignal*>(link->getTLLogic());
1554 11443 : if (rs != nullptr) {
1555 15821 : for (MSDriveWay* foe : rs->retrieveDriveWays(link->getTLIndex())) {
1556 : #ifdef DEBUG_ADD_FOES
1557 : std::cout << " cand foe=" << foe->myID << " fc1=" << flankConflict(*foe) << " fc2=" << foe->flankConflict(*this) << " cc1=" << crossingConflict(*foe) << " cc2=" << foe->crossingConflict(*this) << "\n";
1558 : #endif
1559 4378 : if (foe != this && (flankConflict(*foe) || foe->flankConflict(*this) || crossingConflict(*foe) || foe->crossingConflict(*this))) {
1560 : #ifdef DEBUG_ADD_FOES
1561 : std::cout << " foe=" << foe->myID << "\n";
1562 : #endif
1563 3304 : myFoes.push_back(foe);
1564 : }
1565 11443 : }
1566 : }
1567 11443 : }
1568 :
1569 :
1570 : void
1571 15932 : MSDriveWay::addBidiFoes(const MSRailSignal* ownSignal, bool extended) {
1572 : #ifdef DEBUG_ADD_FOES
1573 : std::cout << "driveway " << myID << " addBidiFoes extended=" << extended << "\n";
1574 : #endif
1575 15932 : const std::vector<const MSLane*>& bidiLanes = extended ? myBidiExtended : myBidi;
1576 80647 : for (const MSLane* bidi : bidiLanes) {
1577 132540 : for (auto ili : bidi->getIncomingLanes()) {
1578 67825 : const MSRailSignal* rs = dynamic_cast<const MSRailSignal*>(ili.viaLink->getTLLogic());
1579 67825 : if (rs != nullptr && rs != ownSignal &&
1580 67825 : std::find(bidiLanes.begin(), bidiLanes.end(), ili.lane) != bidiLanes.end()) {
1581 4245 : addFoes(ili.viaLink);
1582 : }
1583 : }
1584 64715 : const MSEdge* bidiEdge = &bidi->getEdge();
1585 : if (myDepartureDriveways.count(bidiEdge) != 0) {
1586 3090 : for (MSDriveWay* foe : myDepartureDriveways[bidiEdge]) {
1587 1433 : if (flankConflict(*foe)) {
1588 : #ifdef DEBUG_ADD_FOES
1589 : std::cout << " foe " << foe->getID() << " departs on bidi=" << bidiEdge->getID() << "\n";
1590 : #endif
1591 851 : myFoes.push_back(foe);
1592 : } else {
1593 : #ifdef DEBUG_ADD_FOES
1594 : std::cout << " cand foe " << foe->getID() << " departs on bidi=" << bidiEdge->getID() << " rejected\n";
1595 : #endif
1596 : }
1597 : }
1598 : }
1599 : if (myDepartureDrivewaysEnds.count(bidiEdge) != 0) {
1600 2630 : for (MSDriveWay* foe : myDepartureDrivewaysEnds[bidiEdge]) {
1601 1362 : if (flankConflict(*foe)) {
1602 : #ifdef DEBUG_ADD_FOES
1603 : std::cout << " foe " << foe->getID() << " ends on bidi=" << bidiEdge->getID() << "\n";
1604 : #endif
1605 842 : myFoes.push_back(foe);
1606 : } else {
1607 : #ifdef DEBUG_ADD_FOES
1608 : std::cout << " cand foe " << foe->getID() << " ends on bidi=" << bidiEdge->getID() << " rejected\n";
1609 : #endif
1610 : }
1611 : }
1612 : }
1613 : }
1614 15932 : }
1615 :
1616 :
1617 : void
1618 7966 : MSDriveWay::addParallelFoes(const MSLink* link, const MSEdge* first) {
1619 : #ifdef DEBUG_ADD_FOES
1620 : std::cout << "driveway " << myID << " addParallelFoes\n";
1621 : #endif
1622 7966 : if (link) {
1623 4986 : addFoes(link);
1624 : } else {
1625 : auto it = myDepartureDriveways.find(first);
1626 2980 : if (it != myDepartureDriveways.end()) {
1627 3146 : for (MSDriveWay* foe : it->second) {
1628 : #ifdef DEBUG_ADD_FOES
1629 : std::cout << " foe " << foe->getID() << " departs on first=" << first->getID() << "\n";
1630 : #endif
1631 193 : myFoes.push_back(foe);
1632 : }
1633 : }
1634 : }
1635 7966 : }
1636 :
1637 :
1638 : void
1639 7966 : MSDriveWay::addReversalFoes(bool movingBlock) {
1640 : #ifdef DEBUG_ADD_FOES
1641 : std::cout << "driveway " << myID << " addReversalFoes\n";
1642 : #endif
1643 : std::set<const MSEdge*> forward;
1644 34049 : for (const MSLane* lane : myForward) {
1645 26083 : if (lane->isNormal()) {
1646 17373 : forward.insert(&lane->getEdge());
1647 : }
1648 : }
1649 : int i = 0;
1650 69500 : for (const MSEdge* e : myRoute) {
1651 17387 : if (forward.count(e) != 0 && !movingBlock) {
1652 : // reversals in our own forward can be ignored because each driveway
1653 : // is automatically a foe of itself by default
1654 : continue;
1655 : }
1656 44766 : if (i == myCoreSize) {
1657 : break;
1658 : }
1659 44766 : i++;
1660 : auto it = myReversalDriveWays.find(e);
1661 44766 : if (it != myReversalDriveWays.end()) {
1662 6336 : for (MSDriveWay* foe : it->second) {
1663 : // check whether the foe reverses into our own forward section
1664 : // (it might reverse again or disappear via arrival)
1665 : #ifdef DEBUG_ADD_FOES
1666 : //std::cout << " candidate foe " << foe->getID() << " reverses on edge=" << e->getID() << " forward=" << joinNamedToString(forward, " ") << " foeRoute=" << toString(foe->myRoute) << "\n";
1667 : #endif
1668 11160 : if (forwardRouteConflict(forward, *foe)) {
1669 : std::set<const MSEdge*> foeForward;
1670 1162 : for (const MSLane* lane : foe->myForward) {
1671 973 : if (lane->isNormal()) {
1672 607 : foeForward.insert(&lane->getEdge());
1673 607 : if (lane->getBidiLane() != nullptr) {
1674 596 : foeForward.insert(lane->getEdge().getBidiEdge());
1675 : }
1676 : }
1677 : }
1678 : #ifdef DEBUG_ADD_FOES
1679 : std::cout << " reversal cand=" << foe->getID() << " foeForward " << toString(foeForward) << "\n";
1680 : #endif
1681 378 : if (foe->forwardRouteConflict(foeForward, *this, true)) {
1682 : #ifdef DEBUG_ADD_FOES
1683 : std::cout << " foe " << foe->getID() << " reverses on edge=" << e->getID() << "\n";
1684 : #endif
1685 143 : myFoes.push_back(foe);
1686 : }
1687 5391 : } else if (movingBlock && foe == this) {
1688 : #ifdef DEBUG_ADD_FOES
1689 : std::cout << " dw " << getID() << " reverses on forward edge=" << e->getID() << " (movingBlock)\n";
1690 : #endif
1691 23 : myFoes.push_back(foe);
1692 : }
1693 : }
1694 : }
1695 : }
1696 7966 : }
1697 :
1698 :
1699 : bool
1700 3513 : MSDriveWay::buildSubFoe(MSDriveWay* foe, bool movingBlock) {
1701 : // Subdriveways (Teilfahrstraße) model the resolution of a driving conflict
1702 : // before a vehicle has left the driveway. This is possible when the driveway diverges from the foe
1703 : // driveway at an earlier point (switch or crossing).
1704 : //
1705 : // We already know that the last edge of this driveway doesn't impact the foe (unless the driveway ends within the block).
1706 : // Remove further edges from the end of the driveway (myForward) until the point of conflict is found.
1707 : //
1708 : // For movingBlock the logic is changed:
1709 : // We remove the conflict-free part as before but then keep removing the conflict part until another non-conconflit part is found
1710 3513 : if (myForward.size() < foe->myForward.size() &&
1711 3513 : myForward == std::vector<const MSLane*>(foe->myForward.begin(), foe->myForward.begin() + myForward.size())) {
1712 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1713 : std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " is subpart of foe=" << foe->getID() << "\n";
1714 : #endif
1715 29 : foe->myFoes.push_back(this);
1716 29 : return true;
1717 : }
1718 3484 : int subLast = (int)myForward.size() - 2;
1719 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1720 : if (subLast < 0) {
1721 : std::cout << " " << getID() << " cannot build subDriveWay for foe " << foe->getID() << " because myForward has only a single lane\n";
1722 : }
1723 : #endif
1724 : bool foundConflict = false;
1725 11605 : while (subLast >= 0) {
1726 10866 : const MSLane* lane = myForward[subLast];
1727 10866 : MSDriveWay tmp(myOrigin, "tmp", true);
1728 10866 : tmp.myForward.push_back(lane);
1729 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1730 : std::cout << " subLast=" << subLast << " lane=" << lane->getID() << " fc=" << tmp.flankConflict(*foe) << " cc=" << tmp.crossingConflict(*foe)
1731 : << " bc=" << (std::find(foe->myBidi.begin(), foe->myBidi.end(), lane) != foe->myBidi.end()) << "\n";
1732 : #endif
1733 10866 : const bool bidiConflict = std::find(foe->myBidi.begin(), foe->myBidi.end(), lane) != foe->myBidi.end();
1734 10866 : if (tmp.flankConflict(*foe) || tmp.crossingConflict(*foe) || bidiConflict) {
1735 : foundConflict = true;
1736 2762 : if (!movingBlock || bidiConflict) {
1737 : break;
1738 : }
1739 8104 : } else if (foundConflict) {
1740 : break;
1741 : }
1742 8121 : subLast--;
1743 10866 : }
1744 3484 : if (subLast < 0) {
1745 739 : if (&myForward.back()->getEdge() == myRoute.back() && foe->forwardEndOnRoute(this)) {
1746 : // driveway ends in the middle of the block and only the final edge overlaps with the foe driveWay
1747 14 : foe->myFoes.push_back(this);
1748 725 : } else if (foe->myTerminateRoute) {
1749 597 : if (bidiBlockedByEnd(*foe) && bidiBlockedBy(*this) && foe->forwardEndOnRoute(this)) {
1750 89 : foe->myFoes.push_back(this);
1751 : // foe will get the sidings
1752 89 : addSidings(foe, true);
1753 : }
1754 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1755 : std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " terminates\n";
1756 : #endif
1757 128 : } else if (myTerminateRoute && myBidi.size() <= myForward.size()) {
1758 0 : foe->myFoes.push_back(this);
1759 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1760 : std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " terminates, foe=" << foe->getID() << "\n";
1761 : #endif
1762 0 : return true;
1763 : } else if (foe->myReversals.size() % 2 == 1) {
1764 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1765 : std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " has " << foe->myReversals.size() << " reversals\n";
1766 : #endif
1767 : } else {
1768 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1769 : std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " failed\n";
1770 : #endif
1771 : #ifdef SUBDRIVEWAY_WARN_NOCONFLICT
1772 : WRITE_WARNINGF("No point of conflict found between driveway '%' and driveway '%' when creating sub-driveway", getID(), foe->getID());
1773 : #endif
1774 : }
1775 739 : return false;
1776 : }
1777 2745 : int subSize = subLast + 1;
1778 3487 : for (MSDriveWay* cand : mySubDriveWays) {
1779 1401 : if ((int)cand->myForward.size() == subSize) {
1780 : // can re-use existing sub-driveway
1781 659 : foe->myFoes.push_back(cand);
1782 659 : cand->myFoes.push_back(foe);
1783 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1784 : std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " useExisting=" << cand->getID() << "\n";
1785 : #endif
1786 659 : return true;
1787 : }
1788 : }
1789 2086 : std::vector<const MSLane*> forward(myForward.begin(), myForward.begin() + subSize);
1790 : std::vector<const MSEdge*> route;
1791 9596 : for (const MSLane* lane : forward) {
1792 7510 : if (lane->isNormal()) {
1793 4470 : route.push_back(&lane->getEdge());
1794 : }
1795 : }
1796 2086 : if (route.empty()) {
1797 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1798 : std::cout << SIMTIME << " abort subFoe dw=" << getID() << " foe=" << foe->getID() << " empty subRoute\n";
1799 : #endif
1800 : return false;
1801 : }
1802 2086 : if (myRoute.size() > route.size()) {
1803 : // route continues. make sure the subDriveway does not end with a reversal
1804 2086 : const MSEdge* lastNormal = route.back();
1805 2086 : const MSEdge* nextNormal = myRoute[route.size()];
1806 2086 : if (lastNormal->getBidiEdge() == nextNormal) {
1807 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1808 : std::cout << SIMTIME << " abort subFoe dw=" << getID() << " foe=" << foe->getID()
1809 : << " lastNormal=" << lastNormal->getID() << " nextNormal=" << nextNormal->getID() << " endWithReversal\n";
1810 : #endif
1811 : return false;
1812 : }
1813 : }
1814 4132 : MSDriveWay* sub = new MSDriveWay(myOrigin, getID() + "." + toString(mySubDriveWays.size()));
1815 2066 : sub->myLane = myLane;
1816 2066 : sub->myIsSubDriveway = true;
1817 2066 : sub->myForward = forward;
1818 2066 : sub->myRoute = route;
1819 2066 : sub->myCoreSize = (int)sub->myRoute.size();
1820 2066 : myLane->addMoveReminder(sub, false);
1821 :
1822 : // copy trains that are currently on this driveway (and associated entry events)
1823 2188 : for (SUMOVehicle* veh : myTrains) {
1824 122 : auto itOnSub = std::find(sub->myRoute.begin(), sub->myRoute.end(), veh->getEdge());
1825 122 : if (itOnSub != sub->myRoute.end()) {
1826 : sub->myTrains.insert(veh);
1827 : // non-zero is enough to avoid superfluous activation via activateReminders (and removal)
1828 109 : const double pos = sub->myRoute.front()->getLength();
1829 109 : dynamic_cast<MSBaseVehicle*>(veh)->addReminder(sub, pos);
1830 116 : for (const VehicleEvent& ve : myVehicleEvents) {
1831 7 : if (ve.id == veh->getID()) {
1832 7 : sub->myVehicleEvents.push_back(ve);
1833 : }
1834 : }
1835 : }
1836 : }
1837 :
1838 2066 : foe->myFoes.push_back(sub);
1839 2066 : sub->myFoes.push_back(foe);
1840 2066 : mySubDriveWays.push_back(sub);
1841 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1842 : std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " sub=" << sub->getID() << " route=" << toString(sub->myRoute) << "\n";
1843 : #endif
1844 : return true;
1845 2086 : }
1846 :
1847 : void
1848 3997 : MSDriveWay::addSidings(MSDriveWay* foe, bool addToFoe) {
1849 3997 : const MSEdge* foeEndBidi = foe->myForward.back()->getEdge().getBidiEdge();
1850 : int forwardNormals = 0;
1851 18037 : for (auto lane : foe->myForward) {
1852 14040 : if (lane->isNormal()) {
1853 9363 : forwardNormals++;
1854 : }
1855 : }
1856 3997 : if (forwardNormals == (int)foe->myRoute.size()) {
1857 : #ifdef DEBUG_BUILD_SIDINGS
1858 : std::cout << "checkSiding " << getID() << " foe=" << foe->getID() << " forwardNormals=" << forwardNormals << " frSize=" << foe->myRoute.size() << " aborted\n";
1859 : #endif
1860 773 : return;
1861 : }
1862 : auto foeSearchBeg = foe->myRoute.begin() + forwardNormals;
1863 : auto foeSearchEnd = foe->myRoute.end();
1864 3224 : if (foeEndBidi == nullptr) {
1865 0 : throw ProcessError("checkSiding " + getID() + " foe=" + foe->getID() + " noBidi\n");
1866 : }
1867 : int i;
1868 : std::vector<int> start;
1869 : std::vector<double> length;
1870 21062 : for (i = 0; i < (int)myRoute.size(); i++) {
1871 21062 : if (myRoute[i] == foeEndBidi) {
1872 : break;
1873 : }
1874 : }
1875 3224 : if (i == (int)myRoute.size()) {
1876 0 : throw ProcessError("checkSiding " + getID() + " foe=" + foe->getID() + " foeEndBidi=" + foeEndBidi->getID() + " not on route\n");
1877 : }
1878 3224 : const MSEdge* next = myRoute[i];
1879 : #ifdef DEBUG_BUILD_SIDINGS
1880 : std::cout << "checkSiding " << getID() << " foe=" << foe->getID() << " i=" << i << " next=" << next->getID() << " forwardNormals=" << forwardNormals << " frSize=" << foe->myRoute.size() << " foeSearchBeg=" << (*foeSearchBeg)->getID() << "\n";
1881 : #endif
1882 3224 : i--;
1883 21062 : for (; i >= 0; i--) {
1884 17838 : const MSEdge* cur = myRoute[i];
1885 17838 : if (hasRS(cur, next)) {
1886 5243 : if (std::find(foeSearchBeg, foeSearchEnd, cur->getBidiEdge()) == foeSearchEnd) {
1887 2876 : start.push_back(i);
1888 2876 : length.push_back(0);
1889 : }
1890 : }
1891 17838 : if (!start.empty()) {
1892 4689 : auto itFind = std::find(foeSearchBeg, foeSearchEnd, cur->getBidiEdge());
1893 4689 : if (itFind != foeSearchEnd) {
1894 : #ifdef DEBUG_BUILD_SIDINGS
1895 : std::cout << "endSiding " << getID() << " foe=" << foe->getID() << " i=" << i << " curBidi=" << Named::getIDSecure(cur->getBidiEdge()) << " length=" << toString(length) << "\n";
1896 : #endif
1897 707 : const int firstIndex = i + 1;
1898 707 : if (addToFoe) {
1899 69 : auto& foeSidings = foe->mySidings[this];
1900 : // indices must be mapped onto foe route;
1901 69 : const MSEdge* first = myRoute[firstIndex];
1902 69 : auto itFirst = std::find(foe->myRoute.begin(), foe->myRoute.end(), first);
1903 69 : if (itFirst != foe->myRoute.end()) {
1904 279 : for (int j = 0; j < (int)length.size(); j++) {
1905 221 : const MSEdge* last = myRoute[start[j]];
1906 221 : auto itLast = std::find(itFirst, foe->myRoute.end(), last);
1907 221 : if (itLast != foe->myRoute.end()) {
1908 221 : foeSidings.insert(foeSidings.begin(), Siding((int)(itFirst - foe->myRoute.begin()), (int)(itLast - foe->myRoute.begin()), length[j]));
1909 : }
1910 : }
1911 : }
1912 : } else {
1913 638 : auto& foeSidings = mySidings[foe];
1914 2968 : for (int j = 0; j < (int)length.size(); j++) {
1915 2330 : foeSidings.insert(foeSidings.begin(), Siding(firstIndex, start[j], length[j]));
1916 : }
1917 : }
1918 : start.clear();
1919 : length.clear();
1920 707 : foeSearchBeg = itFind;
1921 : } else {
1922 25300 : for (int j = 0; j < (int)length.size(); j++) {
1923 21318 : length[j] += cur->getLength();
1924 : }
1925 : }
1926 : }
1927 : next = cur;
1928 : }
1929 3224 : }
1930 :
1931 :
1932 : bool
1933 17838 : MSDriveWay::hasRS(const MSEdge* cur, const MSEdge* next) {
1934 17838 : if (cur->getToJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
1935 : // check if there is a controlled link between cur and next
1936 13815 : for (auto lane : cur->getLanes()) {
1937 14221 : for (const MSLink* link : lane->getLinkCont()) {
1938 9935 : if (&link->getLane()->getEdge() == next && link->getTLLogic() != nullptr) {
1939 : return true;
1940 : }
1941 : }
1942 : }
1943 : }
1944 : return false;
1945 : }
1946 :
1947 :
1948 : bool
1949 113 : MSDriveWay::forwardEndOnRoute(const MSDriveWay* foe) const {
1950 113 : const MSEdge* foeForwardEnd = &foe->myForward.back()->getNormalPredecessorLane()->getEdge();
1951 113 : return std::find(myRoute.begin(), myRoute.end(), foeForwardEnd) != myRoute.end();
1952 : }
1953 :
1954 : void
1955 4521 : MSDriveWay::addConflictLink(const MSLink* link) {
1956 4521 : if (link->getTLLogic() != nullptr) {
1957 : // ignore links that originate on myBidi
1958 : // and also links from the same junction as my own link
1959 4521 : const MSLane* origin = link->getLaneBefore();
1960 4521 : if (std::find(myBidi.begin(), myBidi.end(), origin) == myBidi.end()) {
1961 3621 : if (link->getJunction() != myRoute.front()->getFromJunction()) {
1962 2159 : if (std::find(myConflictLinks.begin(), myConflictLinks.end(), link) == myConflictLinks.end()) {
1963 1487 : myConflictLinks.push_back(const_cast<MSLink*>(link));
1964 : }
1965 : }
1966 : }
1967 : }
1968 4521 : }
1969 :
1970 : void
1971 297 : MSDriveWay::addDWDeadlock(const std::vector<const MSDriveWay*>& deadlockFoes) {
1972 : std::set<const MSDriveWay*> filtered;
1973 1515 : for (const MSDriveWay* foe : deadlockFoes) {
1974 1218 : if (std::find(myFoes.begin(), myFoes.end(), foe) == myFoes.end()) {
1975 : filtered.insert(foe);
1976 : }
1977 : }
1978 297 : if (std::find(myDeadlocks.begin(), myDeadlocks.end(), filtered) == myDeadlocks.end()) {
1979 81 : myDeadlocks.push_back(filtered);
1980 : //std::cout << getID() << " deadlockFoes=" << toString(deadlockFoes) << "\n";
1981 : }
1982 297 : }
1983 :
1984 : const MSDriveWay*
1985 53548 : MSDriveWay::getDepartureDriveway(const SUMOVehicle* veh, bool init) {
1986 53548 : const MSEdge* edge = init ? veh->getRoute().getEdges()[veh->getDepartEdge()] : veh->getEdge();
1987 : assert(isRailwayOrShared(edge->getPermissions()));
1988 53548 : if (edge->getFromJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
1989 3486 : for (const MSLane* lane : edge->getLanes()) {
1990 3795 : for (auto ili : lane->getIncomingLanes()) {
1991 2324 : const MSLink* entry = ili.viaLink->getCorrespondingEntryLink();
1992 2324 : const MSRailSignal* rs = dynamic_cast<const MSRailSignal*>(entry->getTLLogic());
1993 1689 : if (rs != nullptr) {
1994 1689 : const MSDriveWay* dw = &const_cast<MSRailSignal*>(rs)->retrieveDriveWayForVeh(entry->getTLIndex(), veh);
1995 1689 : if (&dw->myForward.front()->getEdge() == edge) {
1996 : return dw;
1997 : }
1998 : }
1999 : }
2000 : }
2001 : }
2002 56210 : for (MSDriveWay* dw : myDepartureDriveways[edge]) {
2003 53230 : auto matchStart = init ? veh->getRoute().begin() + veh->getDepartEdge() : veh->getCurrentRouteEdge();
2004 53230 : if (dw->match(matchStart, veh->getRoute().end())) {
2005 50024 : return dw;
2006 : }
2007 : }
2008 5960 : const std::string id = edge->getFromJunction()->getID() + ".d" + toString(myDepartDrivewayIndex[edge->getFromJunction()]++);
2009 2980 : MSDriveWay* dw = buildDriveWay(id, nullptr, veh->getCurrentRouteEdge(), veh->getRoute().end());
2010 2980 : myDepartureDriveways[edge].push_back(dw);
2011 2980 : myDepartureDrivewaysEnds[&dw->myForward.back()->getEdge()].push_back(dw);
2012 : dw->setVehicle(veh->getID());
2013 : return dw;
2014 : }
2015 :
2016 :
2017 : void
2018 1354 : MSDriveWay::writeDepatureBlocks(OutputDevice& od, bool writeVehicles) {
2019 3400 : for (auto item : myDepartureDriveways) {
2020 2046 : const MSEdge* edge = item.first;
2021 2046 : if (item.second.size() > 0) {
2022 4092 : od.openTag("departJunction");
2023 2046 : od.writeAttr(SUMO_ATTR_ID, edge->getFromJunction()->getID());
2024 4283 : for (const MSDriveWay* dw : item.second) {
2025 2237 : if (writeVehicles) {
2026 215 : dw->writeBlockVehicles(od);
2027 : } else {
2028 2022 : dw->writeBlocks(od);
2029 : }
2030 : }
2031 4092 : od.closeTag(); // departJunction
2032 : }
2033 : }
2034 1354 : }
2035 :
2036 : void
2037 476 : MSDriveWay::saveState(OutputDevice& out) {
2038 : // all driveways are in myEndingDriveways which makes it convenient
2039 551 : for (auto item : myEndingDriveways) {
2040 164 : for (MSDriveWay* dw : item.second) {
2041 89 : dw->_saveState(out);
2042 107 : for (MSDriveWay* sub : dw->mySubDriveWays) {
2043 18 : sub->_saveState(out);
2044 : }
2045 : }
2046 : }
2047 476 : }
2048 :
2049 : void
2050 107 : MSDriveWay::_saveState(OutputDevice& out) const {
2051 107 : if (!myTrains.empty() || haveSubTrains()) {
2052 73 : out.openTag(myIsSubDriveway ? SUMO_TAG_SUBDRIVEWAY : SUMO_TAG_DRIVEWAY);
2053 38 : out.writeAttr(SUMO_ATTR_ID, getID());
2054 76 : out.writeAttr(SUMO_ATTR_EDGES, toString(myRoute));
2055 38 : if (!myTrains.empty()) {
2056 76 : out.writeAttr(SUMO_ATTR_VEHICLES, toString(myTrains));
2057 : }
2058 76 : out.closeTag();
2059 : }
2060 107 : }
2061 :
2062 :
2063 : bool
2064 69 : MSDriveWay::haveSubTrains() const {
2065 76 : for (MSDriveWay* sub : mySubDriveWays) {
2066 7 : if (!sub->myTrains.empty()) {
2067 : return true;
2068 : }
2069 : }
2070 : return false;
2071 : }
2072 :
2073 : void
2074 41 : MSDriveWay::loadState(const SUMOSAXAttributes& attrs, int tag) {
2075 41 : if ((int)myDriveWayRouteLookup.size() < myGlobalDriveWayIndex) {
2076 106 : for (auto item : myEndingDriveways) {
2077 168 : for (MSDriveWay* dw : item.second) {
2078 86 : myDriveWayRouteLookup[dw->myRoute] = dw;
2079 : }
2080 : }
2081 : }
2082 41 : MSVehicleControl& c = MSNet::getInstance()->getVehicleControl();
2083 : bool ok;
2084 41 : const std::string id = attrs.get<std::string>(SUMO_ATTR_ID, nullptr, ok);
2085 41 : const std::string edges = attrs.get<std::string>(SUMO_ATTR_EDGES, id.c_str(), ok);
2086 : ConstMSEdgeVector route;
2087 41 : if (attrs.hasAttribute(SUMO_ATTR_EDGES)) {
2088 41 : MSEdge::parseEdgesList(edges, route, id);
2089 : }
2090 : MSDriveWay* dw = nullptr;
2091 41 : if (tag == SUMO_TAG_DRIVEWAY) {
2092 : auto it = myDriveWayRouteLookup.find(route);
2093 38 : if (it == myDriveWayRouteLookup.end()) {
2094 : //WRITE_WARNING(TLF("Unknown driveWay '%' with route '%'", id, edges));
2095 : //return;
2096 0 : throw ProcessError(TLF("Unknown driveWay '%' with route '%'", id, edges));
2097 : }
2098 38 : dw = it->second;
2099 38 : myDriveWayLookup[id] = dw;
2100 : } else {
2101 3 : std::string parentID = id.substr(0, id.rfind('.'));
2102 : auto it = myDriveWayLookup.find(parentID);
2103 3 : if (it == myDriveWayLookup.end()) {
2104 : //WRITE_WARNING(TLF("Unknown parent driveway '%' for subDriveWay '%'", parentID, id));
2105 : //return;
2106 0 : throw ProcessError(TLF("Unknown parent driveway '%' for subDriveWay '%'", parentID, id));
2107 : }
2108 3 : MSDriveWay* parent = it->second;
2109 3 : for (MSDriveWay* sub : parent->mySubDriveWays) {
2110 1 : if (sub->myRoute == route) {
2111 : dw = sub;
2112 : break;
2113 : }
2114 : }
2115 3 : if (dw == nullptr) {
2116 : // missing subdriveways can be ignored. They may have been created
2117 : // as foes for driveways that are not relevant at state loading time
2118 : return;
2119 : }
2120 : }
2121 78 : const std::string vehicles = attrs.getOpt<std::string>(SUMO_ATTR_VEHICLES, id.c_str(), ok, "");
2122 117 : for (const std::string& vehID : StringTokenizer(vehicles).getVector()) {
2123 39 : MSBaseVehicle* veh = dynamic_cast<MSBaseVehicle*>(c.getVehicle(vehID));
2124 39 : if (veh == nullptr) {
2125 0 : throw ProcessError(TLF("Unknown vehicle '%' in driveway '%'", vehID, id));
2126 : }
2127 39 : if (!dw->hasTrain(veh)) {
2128 5 : dw->myTrains.insert(veh);
2129 5 : veh->addReminder(dw);
2130 : }
2131 39 : }
2132 41 : }
2133 :
2134 : const MSDriveWay*
2135 0 : MSDriveWay::retrieveDepartDriveWay(const MSEdge* edge, const std::string& id) {
2136 0 : for (MSDriveWay* dw : myDepartureDriveways[edge]) {
2137 0 : if (dw->getID() == id) {
2138 : return dw;
2139 : }
2140 : }
2141 : return nullptr;
2142 : }
2143 :
2144 :
2145 : bool
2146 1494 : MSDriveWay::hasTrain(SUMOVehicle* veh) const {
2147 1494 : return myTrains.count(veh) != 0;
2148 : }
2149 :
2150 : /****************************************************************************/
|
