Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 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_MOVEREMINDER
50 : //#define DEBUG_MATCH
51 :
52 : #define DEBUG_HELPER(obj) ((obj) != nullptr && (obj)->isSelected())
53 : //#define DEBUG_HELPER(obj) ((obj)->getID() == "")
54 : //#define DEBUG_HELPER(obj) (true)
55 :
56 : #define DEBUG_DW_ID ""
57 : #define DEBUG_COND_DW(obj) (obj->getID() == DEBUG_DW_ID || DEBUG_DW_ID == std::string("ALL"))
58 : #define DEBUG_COND_DW2 (getID() == DEBUG_DW_ID || DEBUG_DW_ID == std::string("ALL"))
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 : double MSDriveWay::myMovingBlockMaxDist(1e10);
67 : std::map<const MSLink*, std::vector<MSDriveWay*> > MSDriveWay::mySwitchDriveWays;
68 : std::map<const MSEdge*, std::vector<MSDriveWay*> > MSDriveWay::myReversalDriveWays;
69 : std::map<const MSEdge*, std::vector<MSDriveWay*>, ComparatorNumericalIdLess> MSDriveWay::myDepartureDriveways;
70 : std::map<const MSJunction*, int> MSDriveWay::myDepartDrivewayIndex;
71 : std::map<const MSEdge*, std::vector<MSDriveWay*> > MSDriveWay::myDepartureDrivewaysEnds;
72 : std::map<const MSEdge*, std::vector<MSDriveWay*>, ComparatorNumericalIdLess> MSDriveWay::myEndingDriveways;
73 : std::map<ConstMSEdgeVector, MSDriveWay*> MSDriveWay::myDriveWayRouteLookup;
74 : std::map<std::string, MSDriveWay*> MSDriveWay::myDriveWayLookup;
75 :
76 : // ---------------------------------------------------------------------------
77 : // static initialisation methods
78 : // ---------------------------------------------------------------------------
79 : void
80 41523 : MSDriveWay::init() {
81 41523 : myWriteVehicles = OptionsCont::getOptions().isSet("railsignal-vehicle-output");
82 41523 : myMovingBlockMaxDist = OptionsCont::getOptions().getFloat("railsignal.moving-block.max-dist");
83 41523 : }
84 :
85 : // ===========================================================================
86 : // MSDriveWay method definitions
87 : // ===========================================================================
88 :
89 :
90 22502 : MSDriveWay::MSDriveWay(const MSLink* origin, const std::string& id, bool temporary) :
91 45004 : MSMoveReminder("DriveWay_" + (temporary ? "tmp" : id)),
92 : Named(id),
93 10390 : myNumericalID(temporary ? -1 : myGlobalDriveWayIndex++),
94 22502 : myOrigin(origin),
95 22502 : myActive(nullptr),
96 22502 : myCoreSize(0),
97 22502 : myForwardEdgeCount(0),
98 22502 : myFoundSignal(false),
99 22502 : myFoundJump(false),
100 22502 : myTerminateRoute(false),
101 22502 : myAbortedBuild(false),
102 22502 : myBidiEnded(false),
103 77896 : myIsSubDriveway(false)
104 22502 : {}
105 :
106 :
107 32892 : MSDriveWay::~MSDriveWay() {
108 24681 : for (const MSDriveWay* sub : mySubDriveWays) {
109 2179 : delete sub;
110 : }
111 : mySubDriveWays.clear();
112 100398 : }
113 :
114 : void
115 41182 : MSDriveWay::cleanup() {
116 41182 : myGlobalDriveWayIndex = 0;
117 : myBlockLengthWarnings.clear();
118 41182 : myWriteVehicles = false;
119 :
120 44078 : for (auto item : myDepartureDriveways) {
121 6025 : for (MSDriveWay* dw : item.second) {
122 3129 : delete dw;
123 : }
124 : }
125 : MSDriveWay::mySwitchDriveWays.clear();
126 : MSDriveWay::myReversalDriveWays.clear();
127 : MSDriveWay::myDepartureDriveways.clear();
128 : MSDriveWay::myDepartDrivewayIndex.clear();
129 : MSDriveWay::myDepartureDrivewaysEnds.clear();
130 : MSDriveWay::myEndingDriveways.clear();
131 41182 : }
132 :
133 : void
134 168 : MSDriveWay::clearState() {
135 193 : for (auto item : myEndingDriveways) {
136 50 : for (MSDriveWay* dw : item.second) {
137 : dw->myTrains.clear();
138 : }
139 : }
140 168 : }
141 :
142 :
143 : bool
144 33232 : MSDriveWay::notifyEnter(SUMOTrafficObject& veh, Notification reason, const MSLane* enteredLane) {
145 : #ifdef DEBUG_MOVEREMINDER
146 : std::cout << SIMTIME << " notifyEnter " << getDescription() << " veh=" << veh.getID() << " lane=" << (MSGlobals::gUseMesoSim ? veh.getEdge()->getID() : Named::getIDSecure(enteredLane)) << " reason=" << reason << "\n";
147 : #endif
148 66464 : if (veh.isVehicle() && (enteredLane == myLane || (MSGlobals::gUseMesoSim && veh.getEdge() == &myLane->getEdge()))
149 65067 : && (reason == NOTIFICATION_DEPARTED || reason == NOTIFICATION_JUNCTION || reason == NOTIFICATION_PARKING)) {
150 31750 : SUMOVehicle& sveh = dynamic_cast<SUMOVehicle&>(veh);
151 31750 : MSRouteIterator firstIt = std::find(sveh.getCurrentRouteEdge(), sveh.getRoute().end(), myLane->getNextNormal());
152 31750 : if (match(firstIt, sveh.getRoute().end())) {
153 19602 : if (myTrains.count(&sveh) == 0) {
154 19558 : enterDriveWay(sveh, reason);
155 : }
156 19580 : return true;
157 : }
158 1482 : } else if (reason == NOTIFICATION_REROUTE) {
159 : assert(veh.isVehicle());
160 1427 : SUMOVehicle& sveh = dynamic_cast<SUMOVehicle&>(veh);
161 : assert(myTrains.count(&sveh) == 0);
162 1427 : int movedPast = matchesPastRoute(sveh);
163 : // vehicle must still be one the drivway
164 1427 : if (movedPast >= 0 && movedPast < myForwardEdgeCount) {
165 717 : enterDriveWay(sveh, reason);
166 717 : return true;
167 : }
168 : }
169 : return false;
170 : }
171 :
172 :
173 : bool
174 111650 : MSDriveWay::notifyLeave(SUMOTrafficObject& veh, double /*lastPos*/, Notification reason, const MSLane* enteredLane) {
175 : UNUSED_PARAMETER(enteredLane);
176 : #ifdef DEBUG_MOVEREMINDER
177 : std::cout << SIMTIME << " notifyLeave " << getDescription() << " veh=" << veh.getID() << " lane=" << Named::getIDSecure(enteredLane) << " reason=" << toString(reason) << "\n";
178 : #endif
179 111650 : if (veh.isVehicle()) {
180 : // leaving network with departure, teleport etc
181 111650 : if (reason != MSMoveReminder::NOTIFICATION_JUNCTION && reason != MSMoveReminder::NOTIFICATION_SEGMENT) {
182 6137 : myTrains.erase(&dynamic_cast<SUMOVehicle&>(veh));
183 6137 : if (myWriteVehicles) {
184 626 : myVehicleEvents.push_back(VehicleEvent(SIMSTEP, false, veh.getID(), reason));
185 : }
186 6137 : return false;
187 105513 : } else if (MSGlobals::gUseMesoSim && reason != MSMoveReminder::NOTIFICATION_SEGMENT) {
188 : // notifyLeave is called before moving the route iterator
189 7242 : const MSLane* leftLane = (*(dynamic_cast<SUMOVehicle&>(veh).getCurrentRouteEdge()))->getLanes().front();
190 7242 : return notifyLeaveBack(veh, reason, leftLane);
191 : } else {
192 : return true;
193 : }
194 : } else {
195 : return false;
196 : }
197 : }
198 :
199 :
200 : bool
201 77130 : MSDriveWay::notifyLeaveBack(SUMOTrafficObject& veh, Notification reason, const MSLane* leftLane) {
202 : #ifdef DEBUG_MOVEREMINDER
203 : std::cout << SIMTIME << " notifyLeaveBack " << getDescription() << " veh=" << veh.getID() << " lane=" << Named::getIDSecure(leftLane) << " reason=" << toString(reason) << "\n";
204 : #endif
205 77130 : if (veh.isVehicle()) {
206 77130 : if (leftLane == myForward.back() && (veh.getBackLane() != leftLane->getBidiLane() || MSGlobals::gUseMesoSim)) {
207 13484 : myTrains.erase(&dynamic_cast<SUMOVehicle&>(veh));
208 13484 : if (myWriteVehicles) {
209 1286 : myVehicleEvents.push_back(VehicleEvent(SIMSTEP, false, veh.getID(), reason));
210 : }
211 13484 : return false;
212 : } else {
213 63646 : return true;
214 : }
215 : } else {
216 : return false;
217 : }
218 : }
219 :
220 :
221 : bool
222 1437 : MSDriveWay::notifyReroute(SUMOTrafficObject& veh) {
223 : #ifdef DEBUG_MOVEREMINDER
224 : std::cout << SIMTIME << " notifyReroute " << getDescription() << " veh=" << veh.getID() << "\n";
225 : #endif
226 : assert(veh.isVehicle());
227 1437 : SUMOVehicle* sveh = dynamic_cast<SUMOVehicle*>(&veh);
228 : assert(myTrains.count(sveh) != 0);
229 1437 : if (matchesPastRoute(*sveh) >= 0) {
230 : //std::cout << SIMTIME << " notifyReroute " << getDescription() << " veh=" << veh.getID() << " valid\n";
231 : return true;
232 : }
233 : // no match found, remove
234 : myTrains.erase(sveh);
235 701 : if (myWriteVehicles) {
236 30 : myVehicleEvents.push_back(VehicleEvent(SIMSTEP, false, veh.getID(), NOTIFICATION_REROUTE));
237 : }
238 : //std::cout << SIMTIME << " notifyReroute " << getDescription() << " veh=" << veh.getID() << " invalid\n";
239 : return false;
240 : }
241 :
242 :
243 : int
244 2864 : MSDriveWay::matchesPastRoute(SUMOVehicle& sveh) const {
245 : // look backwards along the route to find the driveway lane
246 2864 : const ConstMSEdgeVector& routeEdges = sveh.getRoute().getEdges();
247 18751 : for (int i = sveh.getRoutePosition(); i >= 0; i--) {
248 18335 : if (routeEdges[i] == myLane->getNextNormal()) {
249 : MSRouteIterator firstIt = routeEdges.begin() + i;
250 2448 : if (match(firstIt, sveh.getRoute().end())) {
251 : // driveway is still valid after rerouting
252 : //std::cout << SIMTIME << " notifyReroute " << getDescription() << " veh=" << veh.getID() << " valid\n";
253 1552 : return sveh.getRoutePosition() - i;
254 : }
255 896 : break;
256 : }
257 : }
258 : return -1;
259 : }
260 :
261 :
262 : void
263 20275 : MSDriveWay::enterDriveWay(SUMOVehicle& sveh, Notification reason) {
264 20275 : myTrains.insert(&sveh);
265 20275 : if (myOrigin != nullptr) {
266 13995 : MSRailSignalControl::getInstance().notifyApproach(myOrigin);
267 : }
268 65924 : for (const MSDriveWay* foe : myFoes) {
269 45649 : if (foe->myOrigin != nullptr) {
270 36427 : MSRailSignalControl::getInstance().notifyApproach(foe->myOrigin);
271 : }
272 : }
273 20275 : if (myWriteVehicles) {
274 1930 : myVehicleEvents.push_back(VehicleEvent(SIMSTEP, true, sveh.getID(), reason));
275 : }
276 20275 : }
277 :
278 : bool
279 600648 : MSDriveWay::reserve(const Approaching& closest, MSEdgeVector& occupied) {
280 600648 : if (foeDriveWayOccupied(true, closest.first, occupied)) {
281 : return false;
282 : }
283 496124 : for (MSLink* foeLink : myConflictLinks) {
284 136501 : if (hasLinkConflict(closest, foeLink)) {
285 : #ifdef DEBUG_SIGNALSTATE
286 : if (gDebugFlag4 || DEBUG_HELPER(closest.first)) {
287 : std::cout << getID() << " linkConflict with " << getTLLinkID(foeLink) << "\n";
288 : }
289 : #endif
290 : return false;
291 : }
292 : }
293 359623 : myActive = closest.first;
294 359623 : return true;
295 : }
296 :
297 :
298 : bool
299 136501 : MSDriveWay::hasLinkConflict(const Approaching& veh, const MSLink* foeLink) const {
300 : #ifdef DEBUG_SIGNALSTATE_PRIORITY
301 : if (gDebugFlag4) {
302 : std::cout << " checkLinkConflict foeLink=" << getTLLinkID(foeLink) << " ego=" << Named::getIDSecure(veh.first) << "\n";
303 : }
304 : #endif
305 136501 : if (foeLink->getApproaching().size() > 0) {
306 45416 : Approaching foe = foeLink->getClosest();
307 : #ifdef DEBUG_SIGNALSTATE_PRIORITY
308 : if (gDebugFlag4) {
309 : std::cout << " approaching foe=" << foe.first->getID() << "\n";
310 : }
311 : #endif
312 45416 : if (foe.first == veh.first) {
313 45416 : return false;
314 : }
315 : const MSTrafficLightLogic* foeTLL = foeLink->getTLLogic();
316 : assert(foeTLL != nullptr);
317 40605 : const MSRailSignal* constFoeRS = dynamic_cast<const MSRailSignal*>(foeTLL);
318 : MSRailSignal* foeRS = const_cast<MSRailSignal*>(constFoeRS);
319 40605 : if (foeRS != nullptr) {
320 40605 : const MSDriveWay& foeDriveWay = foeRS->retrieveDriveWayForVeh(foeLink->getTLIndex(), foe.first);
321 : MSEdgeVector occupied;
322 65558 : if (foeDriveWay.foeDriveWayOccupied(false, foe.first, occupied) ||
323 41312 : !foeRS->constraintsAllow(foe.first) ||
324 32437 : !overlap(foeDriveWay) ||
325 56683 : !isFoeOrSubFoe(&foeDriveWay) ||
326 12658 : canUseSiding(veh.first, &foeDriveWay).first) {
327 : #ifdef DEBUG_SIGNALSTATE_PRIORITY
328 : if (gDebugFlag4) {
329 : if (foeDriveWay.foeDriveWayOccupied(false, foe.first, occupied)) {
330 : std::cout << " foe blocked\n";
331 : } else if (!foeRS->constraintsAllow(foe.first)) {
332 : std::cout << " foe constrained\n";
333 : } else if (!overlap(foeDriveWay)) {
334 : std::cout << " no overlap with foeDW=" << foeDriveWay.getID() << "\n";
335 : } else if (!isFoeOrSubFoe(&foeDriveWay)) {
336 : std::cout << " foeDW=" << foeDriveWay.getID() << " is not a foe to " << getID() << "\n";
337 : } else if (canUseSiding(veh.first, &foeDriveWay).first) {
338 : std::cout << " use siding\n";
339 : }
340 : }
341 : #endif
342 28085 : return false;
343 : }
344 : #ifdef DEBUG_SIGNALSTATE_PRIORITY
345 : if (gDebugFlag4) {
346 : std::cout
347 : << " aSB=" << veh.second.arrivalSpeedBraking << " foeASB=" << foe.second.arrivalSpeedBraking
348 : << " aT=" << veh.second.arrivalTime << " foeAT=" << foe.second.arrivalTime
349 : << " aS=" << veh.first->getSpeed() << " foeS=" << foe.first->getSpeed()
350 : << " aD=" << veh.second.dist << " foeD=" << foe.second.dist
351 : << " aW=" << veh.first->getWaitingTime() << " foeW=" << foe.first->getWaitingTime()
352 : << " aN=" << veh.first->getNumericalID() << " foeN=" << foe.first->getNumericalID()
353 : << "\n";
354 : }
355 : #endif
356 12520 : const bool yield = mustYield(veh, foe);
357 12520 : if (MSRailSignal::storeVehicles()) {
358 540 : MSRailSignal::rivalVehicles().push_back(foe.first);
359 540 : if (yield) {
360 315 : MSRailSignal::priorityVehicles().push_back(foe.first);
361 : }
362 : }
363 12520 : return yield;
364 40605 : }
365 : }
366 : return false;
367 : }
368 :
369 :
370 : bool
371 26191 : MSDriveWay::isFoeOrSubFoe(const MSDriveWay* foe) const {
372 26191 : if (std::find(myFoes.begin(), myFoes.end(), foe) != myFoes.end()) {
373 : return true;
374 : }
375 18035 : for (const MSDriveWay* sub : foe->mySubDriveWays) {
376 10113 : if (isFoeOrSubFoe(sub)) {
377 : return true;
378 : }
379 : }
380 : return false;
381 : }
382 :
383 :
384 : bool
385 12520 : MSDriveWay::mustYield(const Approaching& veh, const Approaching& foe) {
386 12520 : if (foe.second.arrivalSpeedBraking == veh.second.arrivalSpeedBraking) {
387 8070 : if (foe.second.arrivalTime == veh.second.arrivalTime) {
388 616 : if (foe.first->getSpeed() == veh.first->getSpeed()) {
389 592 : if (foe.second.dist == veh.second.dist) {
390 556 : if (foe.first->getWaitingTime() == veh.first->getWaitingTime()) {
391 538 : return foe.first->getNumericalID() < veh.first->getNumericalID();
392 : } else {
393 18 : return foe.first->getWaitingTime() > veh.first->getWaitingTime();
394 : }
395 : } else {
396 36 : return foe.second.dist < veh.second.dist;
397 : }
398 : } else {
399 24 : return foe.first->getSpeed() > veh.first->getSpeed();
400 : }
401 : } else {
402 7454 : return foe.second.arrivalTime < veh.second.arrivalTime;
403 : }
404 : } else {
405 4450 : return foe.second.arrivalSpeedBraking > veh.second.arrivalSpeedBraking;
406 : }
407 : }
408 :
409 :
410 : bool
411 16035 : MSDriveWay::conflictLaneOccupied(bool store, const SUMOVehicle* ego) const {
412 210513 : for (const MSLane* lane : myConflictLanes) {
413 : if (!lane->isEmpty()) {
414 4410 : std::string joinVehicle = "";
415 4410 : if (ego != nullptr && !MSGlobals::gUseMesoSim) {
416 0 : const SUMOVehicleParameter::Stop* stop = ego->getNextStopParameter();
417 0 : if (stop != nullptr) {
418 0 : joinVehicle = stop->join;
419 : }
420 : }
421 : #ifdef DEBUG_SIGNALSTATE
422 : if (gDebugFlag4) {
423 : std::cout << SIMTIME << " conflictLane " << lane->getID() << " occupied ego=" << Named::getIDSecure(ego) << " vehNumber=" << lane->getVehicleNumber() << "\n";
424 : if (joinVehicle != "") {
425 : std::cout << " joinVehicle=" << joinVehicle << " occupant=" << toString(lane->getVehiclesSecure()) << "\n";
426 : lane->releaseVehicles();
427 : }
428 : }
429 : #endif
430 4410 : if (lane->getVehicleNumberWithPartials() == 1) {
431 4410 : MSVehicle* foe = lane->getLastAnyVehicle();
432 4410 : if (joinVehicle != "") {
433 0 : if (foe->getID() == joinVehicle && foe->isStopped()) {
434 : #ifdef DEBUG_SIGNALSTATE
435 : if (gDebugFlag4) {
436 : std::cout << " ignore join-target '" << joinVehicle << "\n";
437 : }
438 : #endif
439 0 : continue;
440 : }
441 : }
442 4410 : if (ego != nullptr) {
443 0 : if (foe == ego && std::find(myForward.begin(), myForward.end(), lane) == myForward.end()) {
444 : #ifdef DEBUG_SIGNALSTATE
445 : if (gDebugFlag4) {
446 : std::cout << " ignore ego as oncoming '" << ego->getID() << "\n";
447 : }
448 : #endif
449 0 : continue;
450 : }
451 0 : if (foe->isStopped() && foe->getNextStopParameter()->join == ego->getID()) {
452 : #ifdef DEBUG_SIGNALSTATE
453 : if (gDebugFlag4) {
454 : std::cout << " ignore " << foe->getID() << " for which ego is join-target\n";
455 : }
456 : #endif
457 0 : continue;
458 : }
459 : }
460 : }
461 4410 : if (MSRailSignal::storeVehicles() && store) {
462 4410 : MSRailSignal::blockingVehicles().push_back(lane->getLastAnyVehicle());
463 : }
464 : return true;
465 : }
466 : }
467 11625 : return false;
468 : }
469 :
470 :
471 : bool
472 8076336 : MSDriveWay::foeDriveWayApproached() const {
473 16260700 : for (const MSDriveWay* foeDW : myFoes) {
474 16199099 : if (foeDW->myOrigin != nullptr && foeDW->myOrigin->getApproaching().size() > 0) {
475 : #ifdef DEBUG_SIGNALSTATE
476 : if (gDebugFlag4) {
477 : std::cout << SIMTIME << " foeLink=" << foeDW->myOrigin->getDescription() << " approachedBy=" << foeDW->myOrigin->getApproaching().begin()->first->getID() << "\n";
478 : }
479 : #endif
480 : return true;
481 : }
482 : }
483 : return false;
484 : }
485 :
486 :
487 : bool
488 9354605 : MSDriveWay::foeDriveWayOccupied(bool store, const SUMOVehicle* ego, MSEdgeVector& occupied) const {
489 27390544 : for (const MSDriveWay* foeDW : myFoes) {
490 18928174 : if (!foeDW->myTrains.empty()) {
491 : #ifdef DEBUG_SIGNALSTATE
492 : if (gDebugFlag4 || DEBUG_COND_DW2 || DEBUG_HELPER(ego)) {
493 : std::cout << SIMTIME << " " << getID() << " foeDriveWay " << foeDW->getID() << " occupied ego=" << Named::getIDSecure(ego) << " foeVeh=" << toString(foeDW->myTrains) << "\n";
494 : }
495 : #endif
496 918619 : if (foeDW->myTrains.size() == 1) {
497 916769 : SUMOVehicle* foe = *foeDW->myTrains.begin();
498 916769 : if (foe == ego) {
499 : #ifdef DEBUG_SIGNALSTATE
500 : if (gDebugFlag4 || DEBUG_HELPER(ego)) {
501 : std::cout << " ignore ego as foe '" << Named::getIDSecure(ego) << "\n";
502 : }
503 : #endif
504 26472 : continue;
505 : }
506 895548 : if (hasJoin(ego, foe)) {
507 36 : continue;
508 : }
509 : }
510 897362 : std::pair<bool, const MSDriveWay*> useSiding = canUseSiding(ego, foeDW);
511 : #ifdef DEBUG_SIGNALSTATE
512 : if (gDebugFlag4 || DEBUG_COND_DW2 || DEBUG_HELPER(ego)) {
513 : auto it = mySidings.find(foeDW);
514 : int numSidings = 0;
515 : if (it != mySidings.end()) {
516 : numSidings = it->second.size();
517 : }
518 : std::cout << " useSiding=" << useSiding.first << " sidingFoe=" << Named::getIDSecure(useSiding.second) << " numSidings=" << numSidings << "\n";
519 : }
520 : #endif
521 897362 : if (useSiding.first) {
522 5215 : continue;
523 : } else {
524 892147 : if (MSRailSignal::storeVehicles() && store) {
525 740 : for (SUMOVehicle* foe : foeDW->myTrains) {
526 370 : MSRailSignal::blockingVehicles().push_back(foe);
527 : }
528 370 : MSRailSignal::blockingDriveWays().push_back(foeDW);
529 : }
530 1787159 : for (const SUMOVehicle* foe : foeDW->myTrains) {
531 895012 : occupied.push_back(const_cast<MSEdge*>(foe->getEdge()));
532 895012 : MSEdge* bidi = const_cast<MSEdge*>(foe->getEdge()->getBidiEdge());
533 895012 : if (bidi != nullptr) {
534 481302 : occupied.push_back(bidi);
535 : }
536 : /// @todo: if foe occupies more than one edge we should add all of them to the occupied vector
537 : }
538 298223 : if (ego != nullptr && MSGlobals::gTimeToTeleportRSDeadlock > 0
539 1135785 : && (ego->getWaitingTime() > ego->getVehicleType().getCarFollowModel().getStartupDelay() || !ego->isOnRoad())) {
540 : // if there is an occupied siding, it becomes part of the waitRelation
541 137140 : SUMOVehicle* foe = *(useSiding.second == nullptr ? foeDW : useSiding.second)->myTrains.begin();
542 137140 : const MSRailSignal* rs = myOrigin != nullptr ? dynamic_cast<const MSRailSignal*>(myOrigin->getTLLogic()) : nullptr;
543 137140 : MSRailSignalControl::getInstance().addWaitRelation(ego, rs, foe);
544 : }
545 892147 : return true;
546 : }
547 18009555 : } else if (foeDW != this && isDepartDriveway() && !foeDW->isDepartDriveway()) {
548 9280 : if (foeDW->myOrigin->getApproaching().size() > 0) {
549 1643 : Approaching foeA = foeDW->myOrigin->getClosest();
550 1643 : const SUMOVehicle* foe = foeA.first;
551 1643 : if (foeA.second.dist < foe->getBrakeGap(true)) {
552 131 : MSRouteIterator firstIt = std::find(foe->getCurrentRouteEdge(), foe->getRoute().end(), foeDW->myRoute.front());
553 131 : if (firstIt != foe->getRoute().end()) {
554 131 : if (foeDW->match(firstIt, foe->getRoute().end())) {
555 88 : bool useSiding = canUseSiding(ego, foeDW).first;
556 : #ifdef DEBUG_SIGNALSTATE
557 : if (gDebugFlag4 || DEBUG_COND_DW2 || DEBUG_HELPER(ego)) {
558 : std::cout << SIMTIME << " " << getID() << " blocked by " << foeDW->getID() << " (approached by " << foe->getID() << ") useSiding=" << useSiding << "\n";
559 : }
560 : #endif
561 88 : if (useSiding) {
562 : //std::cout << SIMTIME << " " << getID() << " ego=" << ego->getID() << " foeDW=" << foeDW->getID() << " myFoes=" << toString(myFoes) << "\n";
563 0 : continue;
564 : } else {
565 88 : return true;
566 : }
567 : }
568 : }
569 : }
570 : }
571 : }
572 : }
573 8463385 : for (const std::set<const MSDriveWay*>& dlFoes : myDeadlocks) {
574 : bool allOccupied = true;
575 10108 : for (const MSDriveWay* dlFoe : dlFoes) {
576 7452 : if (dlFoe->myTrains.empty()) {
577 : allOccupied = false;
578 : //std::cout << SIMTIME << " " << getID() << " ego=" << Named::getIDSecure(ego) << " deadlockCheck clear " << dlFoe->getID() << "\n";
579 : break;
580 : }
581 : }
582 3671 : if (allOccupied) {
583 : #ifdef DEBUG_SIGNALSTATE
584 : if (gDebugFlag4 || DEBUG_COND_DW2 || DEBUG_HELPER(ego)) {
585 : std::cout << SIMTIME << " " << getID() << " ego=" << Named::getIDSecure(ego) << " deadlockCheck " << joinNamedToString(dlFoes, " ") << "\n";
586 : }
587 : #endif
588 8645 : for (const MSDriveWay* dlFoe : dlFoes) {
589 5989 : MSRailSignal::blockingDriveWays().push_back(dlFoe);
590 : }
591 : return true;
592 : }
593 : }
594 : return false;
595 : }
596 :
597 :
598 : bool
599 895548 : MSDriveWay::hasJoin(const SUMOVehicle* ego, const SUMOVehicle* foe) {
600 895548 : if (ego != nullptr && !MSGlobals::gUseMesoSim) {
601 234698 : std::string joinVehicle = "";
602 234698 : const SUMOVehicleParameter::Stop* stop = ego->getNextStopParameter();
603 234698 : if (stop != nullptr) {
604 92612 : joinVehicle = stop->join;
605 : }
606 234698 : if (joinVehicle == "" && !ego->hasDeparted() && ego->getStops().size() > 1) {
607 : // check one more stop
608 7441 : auto it = ego->getStops().begin();
609 : std::advance(it, 1);
610 7441 : joinVehicle = it->pars.join;
611 : }
612 234698 : if (joinVehicle != "") {
613 : #ifdef DEBUG_SIGNALSTATE
614 : if (gDebugFlag4 || DEBUG_COND_DW(ego) || DEBUG_COND_DW(foe)) {
615 : std::cout << " joinVehicle=" << joinVehicle << "\n";
616 : }
617 : #endif
618 345 : if (foe->getID() == joinVehicle && foe->isStopped()) {
619 : #ifdef DEBUG_SIGNALSTATE
620 : if (gDebugFlag4 || DEBUG_COND_DW(ego) || DEBUG_COND_DW(foe)) {
621 : std::cout << " ignore join-target '" << joinVehicle << "\n";
622 : }
623 : #endif
624 : return true;
625 : }
626 : }
627 :
628 234674 : if (foe->isStopped() && foe->getNextStopParameter()->join == ego->getID()) {
629 : #ifdef DEBUG_SIGNALSTATE
630 : if (gDebugFlag4 || DEBUG_COND_DW(ego) || DEBUG_COND_DW(foe)) {
631 : std::cout << " ignore " << foe->getID() << " for which ego is join-target\n";
632 : }
633 : #endif
634 : return true;
635 : }
636 : }
637 : return false;
638 : }
639 :
640 :
641 : std::pair<bool, const MSDriveWay*>
642 913852 : MSDriveWay::canUseSiding(const SUMOVehicle* ego, const MSDriveWay* foe, bool recurse) const {
643 : auto it = mySidings.find(foe);
644 913852 : if (it != mySidings.end()) {
645 14568 : for (auto siding : it->second) {
646 : // assume siding is usuable when computing state for unapproached signal (ego == nullptr)
647 13776 : if (ego == nullptr || siding.length >= ego->getLength()) {
648 : // if the siding is already "reserved" by another vehicle we cannot use it here
649 10425 : const MSEdge* sidingEnd = myRoute[siding.end];
650 24336 : for (MSDriveWay* sidingApproach : myEndingDriveways[sidingEnd]) {
651 18677 : if (!sidingApproach->myTrains.empty()) {
652 : // possibly the foe vehicle can use the other part of the siding
653 5072 : if (recurse) {
654 : const SUMOVehicle* foeVeh = nullptr;
655 3744 : if (!foe->myTrains.empty()) {
656 3730 : foeVeh = *foe->myTrains.begin();
657 14 : } else if (foe->myOrigin != nullptr && foe->myOrigin->getApproaching().size() > 0) {
658 14 : foeVeh = foe->myOrigin->getClosest().first;
659 : }
660 3744 : if (foeVeh == nullptr) {
661 0 : WRITE_WARNINGF("Invalid call to canUseSiding dw=% foe=% ego=% time=%", getID(), foe->getID(), Named::getIDSecure(ego), time2string(SIMSTEP));
662 0 : continue;
663 : }
664 3744 : if (foe->canUseSiding(foeVeh, this, false).first) {
665 306 : continue;
666 : }
667 : }
668 : // possibly the foe vehicle
669 : // @todo: in principle it might still be possible to continue if vehicle that approaches the siding can safely leave the situation
670 : #ifdef DEBUG_SIGNALSTATE
671 : if (gDebugFlag4 || DEBUG_COND_DW2 || DEBUG_HELPER(ego)) {
672 : std::cout << SIMTIME << " " << getID() << " ego=" << Named::getIDSecure(ego) << " foe=" << foe->getID()
673 : << " foeVeh=" << toString(foe->myTrains)
674 : << " sidingEnd=" << sidingEnd->getID() << " sidingApproach=" << sidingApproach->getID() << " approaching=" << toString(sidingApproach->myTrains) << "\n";
675 : }
676 : #endif
677 4766 : return std::make_pair(false, sidingApproach);
678 : }
679 : }
680 : //std::cout << SIMTIME << " " << getID() << " ego=" << Named::getIDSecure(ego) << " foe=" << foe->getID()
681 : // << " foeVeh=" << toString(foe->myTrains)
682 : // << " sidingEnd=" << sidingEnd->getID() << "usable\n";
683 5659 : return std::make_pair(true, nullptr);
684 : }
685 : }
686 : }
687 903427 : return std::make_pair(false, nullptr);
688 : }
689 :
690 : bool
691 20097 : MSDriveWay::overlap(const MSDriveWay& other) const {
692 32552 : for (int i = 0; i < myCoreSize; i++) {
693 372946 : for (int j = 0; j < other.myCoreSize; j++) {
694 360491 : const MSEdge* edge = myRoute[i];
695 360491 : const MSEdge* edge2 = other.myRoute[j];
696 : if (edge->getToJunction() == edge2->getToJunction()
697 342867 : || edge->getToJunction() == edge2->getFromJunction()
698 701500 : || edge->getFromJunction() == edge2->getFromJunction()) {
699 : // XXX might be rail_crossing with parallel tracks
700 : return true;
701 : }
702 : }
703 : }
704 : return false;
705 : }
706 :
707 :
708 : bool
709 74562 : MSDriveWay::flankConflict(const MSDriveWay& other) const {
710 247331 : for (const MSLane* lane : myForward) {
711 969712 : for (const MSLane* lane2 : other.myForward) {
712 791550 : if (lane == lane2) {
713 : return true;
714 : }
715 : }
716 2530792 : for (const MSLane* lane2 : other.myBidi) {
717 2355330 : if (lane == lane2) {
718 5282 : if (bidiBlockedBy(other)) {
719 : // it's only a deadlock if both trains block symmetrically
720 : return true;
721 : }
722 : }
723 : }
724 6064405 : for (const MSLane* lane2 : other.myBidiExtended) {
725 5891636 : if (lane == lane2) {
726 20261 : if (bidiBlockedBy(other)) {
727 : // it's only a deadlock if both trains block symmetrically
728 : return true;
729 : }
730 : }
731 : }
732 : }
733 : return false;
734 : }
735 :
736 :
737 : bool
738 64574 : MSDriveWay::crossingConflict(const MSDriveWay& other) const {
739 224636 : for (const MSLane* lane : myForward) {
740 728221 : for (const MSLane* lane2 : other.myForward) {
741 568159 : if (lane->isNormal() && lane2->isNormal() && lane->getEdge().getToJunction() == lane2->getEdge().getToJunction()) {
742 : return true;
743 : }
744 : }
745 : }
746 57049 : if (myOrigin != nullptr && other.myOrigin != nullptr
747 49861 : && myOrigin->getJunction() == other.myOrigin->getJunction()
748 : //&& myForward.front()->isInternal() && other.myForward.front()->isInternal()
749 1241 : && myOrigin->getJunction()->getLogic() != nullptr
750 63846 : && myOrigin->getJunction()->getLogic()->getFoesFor(myOrigin->getIndex()).test(other.myOrigin->getIndex())) {
751 : // switch/crossing is also a rail_signal (direct control)
752 90 : if (!(myForward.front()->isInternal() && other.myForward.front()->isInternal())) {
753 27 : return false;
754 : }
755 : return true;
756 : }
757 62515 : if (other.myOrigin != nullptr && other.myForward.front()->isInternal()) {
758 119255 : for (int i = 0; i < (int)myForward.size() - 1; i++) {
759 77384 : const MSLane* lane = myForward[i];
760 77384 : if (lane->getToJunction() == other.myOrigin->getJunction()) {
761 42 : const MSLane* next = myForward[i + 1];
762 42 : const MSLink* link = lane->getLinkTo(next);
763 42 : if (link && link->getTLLogic() == nullptr) {
764 : // switch/crossing is also a rail_signal (direct control) but own link is uncontrolled
765 42 : if (lane->getToJunction()->getLogic() != nullptr
766 42 : && lane->getToJunction()->getLogic()->getFoesFor(link->getIndex()).test(other.myOrigin->getIndex())) {
767 : // and links are in conflict
768 : return true;
769 : }
770 : }
771 : }
772 : }
773 : }
774 : return false;
775 : }
776 :
777 :
778 : bool
779 25882 : MSDriveWay::bidiBlockedBy(const MSDriveWay& other) const {
780 417432 : for (const MSLane* lane : myBidi) {
781 1376040 : for (const MSLane* lane2 : other.myForward) {
782 984490 : if (lane == lane2) {
783 : return true;
784 : }
785 : }
786 : }
787 442357 : for (const MSLane* lane : myBidiExtended) {
788 1434689 : for (const MSLane* lane2 : other.myForward) {
789 1014979 : if (lane == lane2) {
790 2257 : if (overlap(other)) {
791 : return true;
792 : }
793 : }
794 : }
795 : }
796 : return false;
797 : }
798 :
799 :
800 : bool
801 8153 : MSDriveWay::bidiBlockedByEnd(const MSDriveWay& other) const {
802 8153 : const MSLane* end = other.myForward.back();
803 65509 : for (const MSLane* lane : myBidi) {
804 60121 : if (lane == end) {
805 : return true;
806 : }
807 : }
808 41886 : for (const MSLane* lane : myBidiExtended) {
809 37979 : if (lane == end) {
810 1481 : if (overlap(other)) {
811 : return true;
812 : }
813 : }
814 : }
815 : return false;
816 : }
817 :
818 : bool
819 5769 : MSDriveWay::forwardRouteConflict(std::set<const MSEdge*> forward, const MSDriveWay& other, bool secondCheck) {
820 : int i = 0;
821 62373 : for (const MSEdge* edge2 : other.myRoute) {
822 61571 : if (i == other.myCoreSize) {
823 : return false;
824 : }
825 61571 : i++;
826 61571 : if (edge2 == myForward.front()->getNextNormal() && !secondCheck) {
827 : // foe should not pass from behind through our own forward section
828 : return false;
829 : }
830 : if (forward.count(edge2->getBidiEdge()) != 0) {
831 : return true;
832 : }
833 : }
834 : return false;
835 : }
836 :
837 : void
838 8303 : MSDriveWay::writeBlocks(OutputDevice& od) const {
839 14649 : od.openTag(myIsSubDriveway ? SUMO_TAG_SUBDRIVEWAY : SUMO_TAG_DRIVEWAY);
840 8303 : od.writeAttr(SUMO_ATTR_ID, myID);
841 8303 : od.writeAttr(SUMO_ATTR_VEHICLE, myFirstVehicle);
842 8303 : od.writeAttr(SUMO_ATTR_EDGES, toString(myRoute));
843 8303 : if (myCoreSize != (int)myRoute.size()) {
844 0 : od.writeAttr("core", myCoreSize);
845 : }
846 8303 : od.openTag("forward");
847 8303 : od.writeAttr(SUMO_ATTR_LANES, toString(myForward));
848 8303 : od.closeTag();
849 8303 : if (!myIsSubDriveway) {
850 6346 : od.openTag("bidi");
851 6346 : od.writeAttr(SUMO_ATTR_LANES, toString(myBidi));
852 6346 : if (myBidiExtended.size() > 0) {
853 1951 : od.lf();
854 1951 : od << " ";
855 3902 : od.writeAttr("deadlockCheck", toString(myBidiExtended));
856 : }
857 6346 : od.closeTag();
858 6346 : od.openTag("flank");
859 6346 : od.writeAttr(SUMO_ATTR_LANES, toString(myFlank));
860 6346 : od.closeTag();
861 :
862 12692 : od.openTag("conflictLinks");
863 :
864 : std::vector<std::string> signals;
865 10092 : for (MSLink* link : myConflictLinks) {
866 7492 : signals.push_back(getTLLinkID(link));
867 : }
868 6346 : od.writeAttr("signals", joinToStringSorting(signals, " "));
869 12692 : od.closeTag();
870 :
871 : std::vector<std::string> foes;
872 23265 : for (MSDriveWay* dw : myFoes) {
873 16919 : foes.push_back(dw->myID);
874 : }
875 6346 : if (foes.size() > 0) {
876 6205 : od.openTag("foes");
877 6205 : od.writeAttr("driveWays", joinToStringSorting(foes, " "));
878 12410 : od.closeTag();
879 : }
880 6962 : for (auto item : mySidings) {
881 616 : od.openTag("sidings");
882 616 : od.writeAttr("foe", item.first->getID());
883 3163 : for (auto siding : item.second) {
884 2547 : od.openTag("siding");
885 2547 : od.writeAttr("start", myRoute[siding.start]->getID());
886 2547 : od.writeAttr("end", myRoute[siding.end]->getID());
887 2547 : od.writeAttr("length", siding.length);
888 5094 : od.closeTag();
889 : }
890 1232 : od.closeTag();
891 : }
892 6427 : for (auto item : myDeadlocks) {
893 81 : od.openTag("deadlock");
894 81 : od.writeAttr("foes", joinNamedToStringSorting(item, " "));
895 162 : od.closeTag();
896 : }
897 6346 : }
898 16606 : od.closeTag(); // driveWay
899 :
900 10260 : for (const MSDriveWay* sub : mySubDriveWays) {
901 1957 : sub->writeBlocks(od);
902 : }
903 : #ifdef DRIVEWAY_SANITY_CHECK
904 8303 : std::set<MSDriveWay*> uFoes(myFoes.begin(), myFoes.end());
905 8303 : if (uFoes.size() != myFoes.size()) {
906 0 : WRITE_WARNINGF("Duplicate foes in driveway '%'", getID());
907 :
908 : }
909 : #endif
910 8303 : }
911 :
912 :
913 : void
914 841 : MSDriveWay::writeBlockVehicles(OutputDevice& od) const {
915 1494 : od.openTag(myIsSubDriveway ? "subDriveWay" : "driveWay");
916 841 : od.writeAttr(SUMO_ATTR_ID, myID);
917 2784 : for (const VehicleEvent& ve : myVehicleEvents) {
918 2914 : od.openTag(ve.isEntry ? "entry" : "exit");
919 1943 : od.writeAttr(SUMO_ATTR_ID, ve.id);
920 1943 : od.writeAttr(SUMO_ATTR_TIME, time2string(ve.time));
921 1943 : od.writeAttr("reason", Notifications.getString(ve.reason));
922 3886 : od.closeTag(); // event
923 : }
924 1682 : od.closeTag(); // driveWay
925 :
926 1029 : for (const MSDriveWay* sub : mySubDriveWays) {
927 188 : sub->writeBlockVehicles(od);
928 : }
929 841 : }
930 :
931 :
932 : void
933 8211 : MSDriveWay::buildRoute(const MSLink* origin,
934 : MSRouteIterator next, MSRouteIterator end,
935 : LaneVisitedMap& visited,
936 : std::set<MSLink*, MSLink::ComparatorNumericalLaneIdLess>& flankSwitches) {
937 8211 : double length = 0;
938 : bool seekForwardSignal = true;
939 : bool seekBidiSwitch = true;
940 : bool foundUnsafeSwitch = false;
941 8211 : MSLane* toLane = origin ? origin->getViaLaneOrLane() : (*next)->getLanes()[0];
942 21504 : const std::string warnID = origin ? "rail signal " + getClickableTLLinkID(origin) : "insertion lane '" + toLane->getID() + "'";
943 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
944 : if (gDebugFlag4) std::cout << "buildRoute origin=" << warnID << " vehRoute=" << toString(ConstMSEdgeVector(next, end))
945 : << " visited=" << formatVisitedMap(visited) << "\n";
946 : #endif
947 : while (true) {
948 99635 : if (length > MSGlobals::gMaxRailSignalBlockLength) {
949 : // typical block length in germany on main lines is 3-5km on branch lines up to 7km
950 : // special branches that are used by one train exclusively could also be up to 20km in length
951 : // minimum block size in germany is 37.5m (LZB)
952 : // larger countries (USA, Russia) might see blocks beyond 20km)
953 222 : if (seekForwardSignal && myBlockLengthWarnings.count(myRoute.front()) == 0) {
954 222 : WRITE_WARNINGF("Block after % exceeds maximum length (stopped searching after edge '%' (length=%m).",
955 : warnID, toLane->getEdge().getID(), length);
956 : myBlockLengthWarnings.insert(myRoute.front());
957 : }
958 222 : myAbortedBuild = true;
959 : // length exceeded
960 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
961 : if (gDebugFlag4) {
962 : std::cout << " abort: length=" << length << "\n";
963 : }
964 : #endif
965 8211 : return;
966 : }
967 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
968 : if (gDebugFlag4) {
969 : std::cout << " toLane=" << toLane->getID() << " visited=" << formatVisitedMap(visited) << "\n";
970 : }
971 : #endif
972 99413 : const MSEdge* current = &toLane->getEdge();
973 99413 : if (current->isNormal()) {
974 62281 : myRoute.push_back(current);
975 62281 : if (next != end) {
976 : next++;
977 : }
978 : }
979 99413 : appendMapIndex(visited, toLane);
980 99413 : length += toLane->getLength();
981 99413 : MSLane* bidi = toLane->getBidiLane();
982 99413 : if (seekForwardSignal) {
983 27694 : if (!foundUnsafeSwitch) {
984 27694 : myForward.push_back(toLane);
985 27694 : if (toLane->isNormal()) {
986 18339 : myForwardEdgeCount++;
987 : }
988 27694 : if (myForward.size() == 1) {
989 8211 : myLane = toLane;
990 8211 : if (MSGlobals::gUseMesoSim) {
991 2412 : MESegment* s = MSGlobals::gMesoNet->getSegmentForEdge(myLane->getEdge());
992 2412 : s->addDetector(this, myLane->getIndex());
993 : } else {
994 5799 : toLane->addMoveReminder(this, false);
995 : }
996 : }
997 : }
998 71719 : } else if (bidi == nullptr) {
999 22332 : if (toLane->isInternal() && toLane->getIncomingLanes().front().viaLink->isTurnaround()) {
1000 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1001 : if (gDebugFlag4) {
1002 : std::cout << " continue bidiSearch beyond turnaround\n";
1003 : }
1004 : #endif
1005 : } else {
1006 : seekBidiSwitch = false;
1007 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1008 : if (gDebugFlag4) {
1009 : std::cout << " noBidi, abort search for bidiSwitch\n";
1010 : }
1011 : #endif
1012 : }
1013 : }
1014 99413 : if (bidi != nullptr) {
1015 64790 : if (!seekForwardSignal && !foundUnsafeSwitch && bidi->isNormal()) {
1016 : // look for switch that could protect from oncoming vehicles
1017 22992 : for (const MSLink* const link : bidi->getLinkCont()) {
1018 12903 : if (link->getDirection() == LinkDirection::TURN) {
1019 531 : continue;
1020 : }
1021 24586 : if (!myBidi.empty() && link->getViaLaneOrLane() != myBidi.back()) {
1022 2195 : myCoreSize = (int)myRoute.size() - 1;
1023 2195 : MSLink* used = const_cast<MSLink*>(bidi->getLinkTo(myBidi.back()));
1024 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1025 : if (gDebugFlag4) {
1026 : std::cout << " found unsafe switch " << link->getDescription() << " (used=" << (used == nullptr ? "NULL" : used->getDescription()) << ")\n";
1027 : }
1028 : #endif
1029 : // trains along our route beyond this switch might create deadlock
1030 : foundUnsafeSwitch = true;
1031 : // the switch itself must still be guarded to ensure safety
1032 2195 : if (used != nullptr) {
1033 : // possibly nullptr if there was an intermediate section of unidirectional edges
1034 : flankSwitches.insert(used);
1035 : }
1036 : break;
1037 : }
1038 : }
1039 : }
1040 62595 : if (foundUnsafeSwitch) {
1041 32185 : myBidiExtended.push_back(bidi);
1042 : } else {
1043 32605 : myBidi.push_back(bidi);
1044 : }
1045 : }
1046 99413 : const std::vector<MSLink*>& links = toLane->getLinkCont();
1047 99413 : toLane = nullptr;
1048 105832 : for (const MSLink* const link : links) {
1049 95606 : if ((next != end && &link->getLane()->getEdge() == *next)
1050 190010 : && isRailwayOrShared(link->getViaLaneOrLane()->getPermissions())) {
1051 148829 : toLane = link->getViaLaneOrLane();
1052 91780 : if (link->getTLLogic() != nullptr && link->getTLIndex() >= 0 && link->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL) {
1053 26401 : if (link == origin) {
1054 356 : if (seekForwardSignal) {
1055 0 : WRITE_WARNINGF(TL("Found circular block after % (% edges, length %)"), warnID, toString(myRoute.size()), toString(length));
1056 : }
1057 : //std::cout << getClickableTLLinkID(origin) << " circularBlock2=" << toString(myRoute) << "\n";
1058 356 : myAbortedBuild = true;
1059 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1060 : if (gDebugFlag4) {
1061 : std::cout << " abort: found circle\n";
1062 : }
1063 : #endif
1064 356 : return;
1065 : }
1066 : seekForwardSignal = false;
1067 26045 : myFoundSignal = true;
1068 : seekBidiSwitch = bidi != nullptr;
1069 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1070 : if (gDebugFlag4) {
1071 : std::cout << " found forwardSignal " << link->getTLLogic()->getID() << " seekBidiSwitch=" << seekBidiSwitch << "\n";
1072 : }
1073 : #endif
1074 : }
1075 : //if (links.size() > 1 && !foundUnsafeSwitch) {
1076 91424 : if (isSwitch(link)) {
1077 : // switch on driveway
1078 : //std::cout << "mySwitchDriveWays " << getID() << " link=" << link->getDescription() << "\n";
1079 52044 : mySwitchDriveWays[link].push_back(this);
1080 : }
1081 91424 : if (link->getLane()->getBidiLane() != nullptr && &link->getLane()->getEdge() == current->getBidiEdge()) {
1082 : // reversal on driveway
1083 1380 : myReversalDriveWays[current].push_back(this);
1084 1380 : myReversals.push_back(current);
1085 : }
1086 : break;
1087 : }
1088 : }
1089 99057 : if (toLane == nullptr) {
1090 7633 : if (next != end) {
1091 : // no connection found, jump to next route edge
1092 : toLane = (*next)->getLanes()[0];
1093 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1094 : if (gDebugFlag4) {
1095 : std::cout << " abort: turn-around or jump\n";
1096 : }
1097 : #endif
1098 87 : myFoundJump = true;
1099 87 : return;
1100 : } else {
1101 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1102 : if (gDebugFlag4) {
1103 : std::cout << " abort: no next lane available\n";
1104 : }
1105 : #endif
1106 7546 : myTerminateRoute = true;
1107 7546 : return;
1108 : }
1109 : }
1110 91424 : }
1111 : myBidiEnded = !seekBidiSwitch;
1112 : #ifdef DEBUG_DRIVEWAY_BUILDROUTE
1113 : if (gDebugFlag4) {
1114 : std::cout << " normalEnd myBidiEnded=" << myBidiEnded << "\n";
1115 : }
1116 : #endif
1117 : }
1118 :
1119 :
1120 : bool
1121 102464 : MSDriveWay::isSwitch(const MSLink* link) {
1122 198478 : for (const MSLink* other : link->getLaneBefore()->getNormalPredecessorLane()->getLinkCont()) {
1123 112235 : if (other->getLane() != link->getLane() && !other->isTurnaround()) {
1124 : return true;
1125 : }
1126 : }
1127 140289 : for (auto ili : link->getLane()->getIncomingLanes()) {
1128 98397 : if (ili.viaLink != link && !ili.viaLink->isTurnaround()) {
1129 : return true;
1130 : }
1131 : }
1132 41892 : const MSLane* bidi = link->getLane()->getBidiLane();
1133 41892 : if (bidi != nullptr) {
1134 63081 : for (const MSLink* other : bidi->getLinkCont()) {
1135 32642 : if (other->getLane() != link->getLaneBefore()->getNormalPredecessorLane()->getBidiLane() && !other->isTurnaround()) {
1136 : return true;
1137 : }
1138 : }
1139 : }
1140 : return false;
1141 : }
1142 :
1143 :
1144 : void
1145 32844 : MSDriveWay::checkFlanks(const MSLink* originLink, const std::vector<const MSLane*>& lanes, const LaneVisitedMap& visited,
1146 : bool allFoes, bool movingBlock, std::set<MSLink*, MSLink::ComparatorNumericalLaneIdLess>& flankSwitches) const {
1147 : #ifdef DEBUG_CHECK_FLANKS
1148 : if (gDebugFlag4) std::cout << " checkFlanks lanes=" << toString(lanes) << " allFoes=" << allFoes << "\n";
1149 : #endif
1150 20328 : const MSLink* reverseOriginLink = originLink != nullptr && originLink->getLane()->getBidiLane() != nullptr && originLink->getLaneBefore()->getBidiLane() != nullptr
1151 43588 : ? originLink->getLane()->getBidiLane()->getLinkTo(originLink->getLaneBefore()->getBidiLane())
1152 : : nullptr;
1153 : //std::cout << " originLink=" << originLink->getDescription() << "\n";
1154 10744 : if (reverseOriginLink != nullptr) {
1155 10744 : reverseOriginLink = reverseOriginLink->getCorrespondingExitLink();
1156 : //std::cout << " reverseOriginLink=" << reverseOriginLink->getDescription() << "\n";
1157 : }
1158 128096 : for (int i = 0; i < (int)lanes.size(); i++) {
1159 95252 : const MSLane* lane = lanes[i];
1160 95252 : const MSLane* prev = i > 0 ? lanes[i - 1] : nullptr;
1161 95252 : const MSLane* next = i + 1 < (int)lanes.size() ? lanes[i + 1] : nullptr;
1162 95252 : if (lane->isInternal()) {
1163 32810 : continue;
1164 : }
1165 129668 : for (auto ili : lane->getIncomingLanes()) {
1166 77965 : if (ili.viaLink == originLink
1167 64815 : || ili.viaLink == reverseOriginLink
1168 61792 : || ili.viaLink->getDirection() == LinkDirection::TURN
1169 56594 : || ili.viaLink->getDirection() == LinkDirection::TURN_LEFTHAND
1170 123820 : || (originLink == nullptr && i == 0 && movingBlock)) {
1171 10739 : continue;
1172 : }
1173 56487 : if (ili.lane != prev && ili.lane != next) {
1174 : #ifdef DEBUG_CHECK_FLANKS
1175 : if (gDebugFlag4) 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";
1176 : #endif
1177 : flankSwitches.insert(ili.viaLink);
1178 44791 : } else if (allFoes) {
1179 : // link is part of the driveway, find foes that cross the driveway without entering
1180 12088 : checkCrossingFlanks(ili.viaLink, visited, flankSwitches);
1181 : }
1182 : }
1183 : }
1184 32844 : }
1185 :
1186 :
1187 : void
1188 12088 : MSDriveWay::checkCrossingFlanks(MSLink* dwLink, const LaneVisitedMap& visited, std::set<MSLink*, MSLink::ComparatorNumericalLaneIdLess>& flankSwitches) const {
1189 : #ifdef DEBUG_CHECK_FLANKS
1190 : if (gDebugFlag4) std::cout << " checkCrossingFlanks dwLink=" << dwLink->getDescription() << " visited=" << formatVisitedMap(visited) << "\n";
1191 : #endif
1192 : const MSJunction* junction = dwLink->getJunction();
1193 12088 : if (junction == nullptr) {
1194 : return; // unregulated junction;
1195 : }
1196 12014 : const MSJunctionLogic* logic = junction->getLogic();
1197 12014 : if (logic == nullptr) {
1198 : return; // unregulated junction;
1199 : }
1200 65728 : for (const MSEdge* in : junction->getIncoming()) {
1201 53786 : if (in->isInternal()) {
1202 27129 : continue;
1203 : }
1204 53728 : for (MSLane* inLane : in->getLanes()) {
1205 27071 : const MSLane* inBidi = inLane->getBidiLane();
1206 37090 : if (isRailwayOrShared(inLane->getPermissions()) && visited.count(inLane) == 0 && (inBidi == nullptr || visited.count(inBidi) == 0)) {
1207 8584 : for (MSLink* link : inLane->getLinkCont()) {
1208 4481 : if (link->getIndex() >= 0 && logic->getFoesFor(dwLink->getIndex()).test(link->getIndex())
1209 11485 : && visited.count(link->getLane()) == 0) {
1210 : #ifdef DEBUG_CHECK_FLANKS
1211 : if (gDebugFlag4) std::cout << " add crossing flankSwitch junction=" << junction->getID() << " index=" << link->getIndex() << "\n";
1212 : #endif
1213 630 : if (link->getViaLane() == nullptr) {
1214 : flankSwitches.insert(link);
1215 : } else {
1216 : flankSwitches.insert(link->getViaLane()->getLinkCont().front());
1217 : }
1218 : }
1219 : }
1220 : }
1221 : }
1222 : }
1223 : }
1224 :
1225 : void
1226 14162 : MSDriveWay::findFlankProtection(MSLink* link, MSLink* origLink, std::vector<const MSLane*>& flank) {
1227 : #ifdef DEBUG_CHECK_FLANKS
1228 : if (gDebugFlag4) std::cout << " findFlankProtection link=" << link->getDescription() << " origLink=" << origLink->getDescription() << "\n";
1229 : #endif
1230 14162 : if (link->getCorrespondingEntryLink()->getTLLogic() != nullptr && link->getJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
1231 2225 : MSLink* entry = const_cast<MSLink*>(link->getCorrespondingEntryLink());
1232 : // guarded by signal
1233 : #ifdef DEBUG_CHECK_FLANKS
1234 : if (gDebugFlag4) std::cout << " flank guarded by " << entry->getTLLogic()->getID() << "\n";
1235 : #endif
1236 : // @note, technically it's enough to collect links from foe driveways
1237 : // but this also adds "unused" conflict links which may aid comprehension
1238 2225 : myConflictLinks.push_back(entry);
1239 2225 : addFoes(entry);
1240 : } else {
1241 : const MSLane* lane = link->getLaneBefore();
1242 : std::vector<MSLink*> predLinks;
1243 23834 : for (auto ili : lane->getIncomingLanes()) {
1244 11897 : if (!ili.viaLink->isTurnaround()) {
1245 11498 : predLinks.push_back(ili.viaLink);
1246 : }
1247 : }
1248 11937 : if (predLinks.size() > 1) {
1249 : // this is a switch
1250 : #ifdef DEBUG_ADD_FOES
1251 : if (gDebugFlag4) std::cout << " predecessors of " << link->getDescription() << " isSwitch\n";
1252 : #endif
1253 687 : for (MSLink* pred : predLinks) {
1254 458 : addSwitchFoes(pred);
1255 : }
1256 11708 : } else if (predLinks.size() == 1) {
1257 11040 : if (isSwitch(link)) {
1258 9625 : addSwitchFoes(link);
1259 : } else {
1260 : // continue upstream via single predecessor
1261 1415 : findFlankProtection(predLinks.front(), origLink, flank);
1262 : }
1263 : }
1264 : // check for insertions
1265 11937 : if (myDepartureDriveways.count(&lane->getEdge()) != 0) {
1266 298 : for (MSDriveWay* foe : myDepartureDriveways[&lane->getEdge()]) {
1267 151 : if (flankConflict(*foe) || crossingConflict(*foe)) {
1268 : #ifdef DEBUG_ADD_FOES
1269 : if (gDebugFlag4) std::cout << " foe " << foe->getID() << " departs on flank=" << lane->getID() << "\n";
1270 : #endif
1271 111 : myFoes.push_back(foe);
1272 : } else {
1273 : #ifdef DEBUG_ADD_FOES
1274 : if (gDebugFlag4) std::cout << " cand foe " << foe->getID() << " departs on flank=" << lane->getID() << " rejected\n";
1275 : #endif
1276 : }
1277 : }
1278 : }
1279 11937 : }
1280 14162 : }
1281 :
1282 :
1283 : void
1284 10083 : MSDriveWay::addSwitchFoes(MSLink* link) {
1285 : auto it = mySwitchDriveWays.find(link);
1286 10083 : if (it != mySwitchDriveWays.end()) {
1287 : #ifdef DEBUG_ADD_FOES
1288 : if (gDebugFlag4) std::cout << " driveway " << myID << " addSwitchFoes for link " << link->getDescription() << "\n";
1289 : #endif
1290 33971 : for (MSDriveWay* foe : it->second) {
1291 30231 : if (foe != this && (flankConflict(*foe) || foe->flankConflict(*this) || crossingConflict(*foe) || foe->crossingConflict(*this))) {
1292 : #ifdef DEBUG_ADD_FOES
1293 : if (gDebugFlag4) std::cout << " foe=" << foe->myID
1294 : << " fc1=" << flankConflict(*foe) << " fc2=" << foe->flankConflict(*this)
1295 : << " cc1=" << crossingConflict(*foe) << " cc2=" << foe->crossingConflict(*this) << "\n";
1296 : #endif
1297 3792 : myFoes.push_back(foe);
1298 : } else {
1299 : #ifdef DEBUG_ADD_FOES
1300 : if (gDebugFlag4) std::cout << " cand=" << foe->myID << "\n";
1301 : #endif
1302 : }
1303 : }
1304 : }
1305 10083 : }
1306 :
1307 :
1308 : MSDriveWay*
1309 8211 : MSDriveWay::buildDriveWay(const std::string& id, const MSLink* link, MSRouteIterator first, MSRouteIterator end) {
1310 : // collect lanes and links that are relevant for setting this signal for the current driveWay
1311 : // For each driveway we collect
1312 : // - conflictLanes (signal must be red if any conflict lane is occupied)
1313 : // - conflictLinks (signal must be red if any conflict link is approached by a vehicle
1314 : // - that cannot break in time (arrivalSpeedBraking > 0)
1315 : // - approached by a vehicle with higher switching priority (see #3941)
1316 : // These objects are construct in steps:
1317 : //
1318 : // forwardBlock
1319 : // - search forward recursive from outgoing lane until controlled railSignal link found
1320 : // -> add all found lanes to conflictLanes
1321 : //
1322 : // bidiBlock (if any forwardBlock edge has bidi edge)
1323 : // - search bidi backward recursive until first switch
1324 : // - from switch search backward recursive all other incoming until controlled rail signal link
1325 : // -> add final links to conflictLinks
1326 : //
1327 : // flanks
1328 : // - search backward recursive from flanking switches
1329 : // until controlled railSignal link or protecting switch is found
1330 : // -> add all found lanes to conflictLanes
1331 : // -> add final links to conflictLinks
1332 8211 : MSDriveWay* dw = new MSDriveWay(link, id);
1333 : LaneVisitedMap visited;
1334 : std::vector<const MSLane*> before;
1335 8211 : MSLane* fromBidi = nullptr;
1336 8211 : if (link != nullptr) {
1337 5082 : appendMapIndex(visited, link->getLaneBefore());
1338 5082 : fromBidi = link->getLaneBefore()->getBidiLane();
1339 : }
1340 : std::set<MSLink*, MSLink::ComparatorNumericalLaneIdLess> flankSwitches; // list of switches that threaten the driveway and for which protection must be found
1341 :
1342 8211 : if (fromBidi != nullptr) {
1343 2768 : before.push_back(fromBidi);
1344 : }
1345 : #ifdef DEBUG_BUILD_DRIVEWAY
1346 : gDebugFlag4 = DEBUG_COND_DW(dw);
1347 : #endif
1348 8211 : dw->buildRoute(link, first, end, visited, flankSwitches);
1349 8211 : dw->myCoreSize = (int)dw->myRoute.size();
1350 :
1351 8211 : MSRailSignal* rs = link ? const_cast<MSRailSignal*>(static_cast<const MSRailSignal*>(link->getTLLogic())) : nullptr;
1352 8211 : const bool movingBlock = (rs && rs->isMovingBlock()) || (!rs &&
1353 11340 : (OptionsCont::getOptions().getBool("railsignal-moving-block")
1354 3025 : || MSRailSignalControl::isMovingBlock((*first)->getPermissions())));
1355 :
1356 8211 : dw->checkFlanks(link, dw->myForward, visited, true, movingBlock, flankSwitches);
1357 8211 : dw->checkFlanks(link, dw->myBidi, visited, false, movingBlock, flankSwitches);
1358 8211 : dw->checkFlanks(link, before, visited, true, movingBlock, flankSwitches);
1359 16948 : for (MSLink* fsLink : flankSwitches) {
1360 : #ifdef DEBUG_ADD_FOES
1361 : if (DEBUG_COND_DW(dw)) {
1362 : std::cout << " fsLink=" << fsLink->getDescription() << "\n";
1363 : }
1364 : #endif
1365 8737 : dw->findFlankProtection(fsLink, fsLink, dw->myFlank);
1366 : }
1367 : std::set<MSLink*, MSLink::ComparatorNumericalLaneIdLess> flankSwitchesBidiExtended;
1368 8211 : dw->checkFlanks(link, dw->myBidiExtended, visited, false, movingBlock, flankSwitchesBidiExtended);
1369 12221 : for (MSLink* const flink : flankSwitchesBidiExtended) {
1370 : #ifdef DEBUG_ADD_FOES
1371 : if (DEBUG_COND_DW(dw)) {
1372 : std::cout << " fsLinkExtended=" << flink->getDescription() << "\n";
1373 : }
1374 : #endif
1375 4010 : dw->findFlankProtection(flink, flink, dw->myBidiExtended);
1376 : }
1377 : #ifdef DEBUG_BUILD_DRIVEWAY
1378 : if (DEBUG_COND_DW(dw)) {
1379 : std::cout << SIMTIME << " buildDriveWay " << dw->myID << " link=" << (link == nullptr ? "NULL" : link->getDescription())
1380 : << "\n route=" << toString(dw->myRoute)
1381 : << "\n forward=" << toString(dw->myForward)
1382 : << "\n bidi=" << toString(dw->myBidi)
1383 : << "\n bidiEx=" << toString(dw->myBidiExtended)
1384 : << "\n flank=" << toString(dw->myFlank)
1385 : << "\n flankSwitch=" << MSRailSignal::describeLinks(std::vector<MSLink*>(flankSwitches.begin(), flankSwitches.end()))
1386 : << "\n coreSize=" << dw->myCoreSize
1387 : << "\n";
1388 : }
1389 : #endif
1390 8211 : if (!rs || !rs->isMovingBlock()) {
1391 8105 : dw->myConflictLanes.insert(dw->myConflictLanes.end(), dw->myForward.begin(), dw->myForward.end());
1392 : }
1393 8211 : dw->myConflictLanes.insert(dw->myConflictLanes.end(), dw->myBidi.begin(), dw->myBidi.end());
1394 8211 : dw->myConflictLanes.insert(dw->myConflictLanes.end(), dw->myFlank.begin(), dw->myFlank.end());
1395 8211 : dw->addBidiFoes(rs, false);
1396 8211 : dw->addBidiFoes(rs, true);
1397 8211 : if (!movingBlock) {
1398 : // add driveways that start on the same signal / lane
1399 7923 : dw->addParallelFoes(link, *first);
1400 : }
1401 : // add driveways that reverse along this driveways route
1402 8211 : dw->addReversalFoes(movingBlock);
1403 : // make foes unique and symmetrical
1404 8211 : std::set<MSDriveWay*, ComparatorNumericalIdLess> uniqueFoes(dw->myFoes.begin(), dw->myFoes.end());
1405 : dw->myFoes.clear();
1406 : // check for self-intersecting forward-section in movingBlock mode
1407 8211 : if (movingBlock && uniqueFoes.count(dw) == 0) {
1408 : std::set<const MSJunction*> forwardJunctions;
1409 2698 : for (const MSLane* fw : dw->myForward) {
1410 2454 : if (fw->isNormal()) {
1411 1379 : const MSJunction* fwTo = fw->getEdge().getToJunction();
1412 : if (forwardJunctions.count(fwTo) == 1) {
1413 21 : dw->myFoes.push_back(dw);
1414 : #ifdef DEBUG_ADD_FOES
1415 : if (DEBUG_COND_DW(dw)) {
1416 : std::cout << " self-intersecting movingBlock for dw=" << dw->getID() << "\n";
1417 : }
1418 : #endif
1419 21 : break;
1420 : }
1421 : forwardJunctions.insert(fwTo);
1422 : }
1423 : }
1424 : }
1425 8211 : std::set<MSLink*, MSLink::ComparatorNumericalLaneIdLess> uniqueCLink(dw->myConflictLinks.begin(), dw->myConflictLinks.end());
1426 8211 : const MSEdge* lastEdge = &dw->myForward.back()->getEdge();
1427 14456 : for (MSDriveWay* foe : uniqueFoes) {
1428 6245 : const MSEdge* foeLastEdge = &foe->myForward.back()->getEdge();
1429 6245 : const bool sameLast = foeLastEdge == lastEdge;
1430 6245 : if (sameLast && !movingBlock) {
1431 2456 : dw->myFoes.push_back(foe);
1432 2456 : if (foe != dw) {
1433 2456 : foe->myFoes.push_back(dw);
1434 : }
1435 : } else {
1436 3789 : if (foe->bidiBlockedByEnd(*dw)) {
1437 : #ifdef DEBUG_ADD_FOES
1438 : if (DEBUG_COND_DW(dw)) {
1439 : std::cout << " setting " << dw->getID() << " as foe of " << foe->getID() << "\n";
1440 : }
1441 : #endif
1442 1864 : foe->myFoes.push_back(dw);
1443 1864 : foe->addSidings(dw);
1444 : } else {
1445 1925 : dw->buildSubFoe(foe, movingBlock);
1446 : }
1447 3789 : if (foe != dw) { // check for movingBlock
1448 3766 : if (dw->bidiBlockedByEnd(*foe)) {
1449 : #ifdef DEBUG_ADD_FOES
1450 : if (DEBUG_COND_DW(dw)) {
1451 : std::cout << " addFoeCheckSiding " << foe->getID() << "\n";
1452 : }
1453 : #endif
1454 2043 : dw->myFoes.push_back(foe);
1455 2043 : dw->addSidings(foe);
1456 : } else {
1457 1723 : foe->buildSubFoe(dw, movingBlock);
1458 : }
1459 : }
1460 : }
1461 6245 : if (link) {
1462 4569 : foe->addConflictLink(link);
1463 : }
1464 : // ignore links that have the same start junction
1465 6245 : if (foe->myRoute.front()->getFromJunction() != dw->myRoute.front()->getFromJunction()) {
1466 7784 : for (auto ili : foe->myForward.front()->getIncomingLanes()) {
1467 3580 : if (ili.viaLink->getTLLogic() != nullptr) {
1468 : // ignore links that originate on myBidi
1469 3133 : const MSLane* origin = ili.viaLink->getLaneBefore();
1470 3133 : if (std::find(dw->myBidi.begin(), dw->myBidi.end(), origin) == dw->myBidi.end()) {
1471 : uniqueCLink.insert(ili.viaLink);
1472 : }
1473 : }
1474 : }
1475 : }
1476 : }
1477 : dw->myConflictLinks.clear();
1478 8211 : dw->myConflictLinks.insert(dw->myConflictLinks.begin(), uniqueCLink.begin(), uniqueCLink.end());
1479 8211 : myEndingDriveways[lastEdge].push_back(dw);
1480 8211 : if (!movingBlock) {
1481 : // every driveway is it's own foe (also all driveways that depart in the same block)
1482 18509 : for (MSDriveWay* sameEnd : myEndingDriveways[lastEdge]) {
1483 : if (uniqueFoes.count(sameEnd) == 0) {
1484 8130 : dw->myFoes.push_back(sameEnd);
1485 8130 : if (sameEnd != dw) {
1486 207 : sameEnd->myFoes.push_back(dw);
1487 : }
1488 : }
1489 : }
1490 : }
1491 : #ifdef DEBUG_BUILD_DRIVEWAY
1492 : if (DEBUG_COND_DW(dw)) {
1493 : std::cout << dw->myID << " mb=" << movingBlock << " finalFoes " << toString(dw->myFoes) << "\n";
1494 : }
1495 : gDebugFlag4 = false;
1496 : #endif
1497 8211 : return dw;
1498 8211 : }
1499 :
1500 : std::string
1501 3746 : MSDriveWay::getTLLinkID(const MSLink* link) {
1502 7492 : return link->getTLLogic()->getID() + "_" + toString(link->getTLIndex());
1503 : }
1504 :
1505 : std::string
1506 0 : MSDriveWay::getJunctionLinkID(const MSLink* link) {
1507 0 : return link->getJunction()->getID() + "_" + toString(link->getIndex());
1508 : }
1509 :
1510 : std::string
1511 5082 : MSDriveWay::getClickableTLLinkID(const MSLink* link) {
1512 15246 : return "junction '" + link->getTLLogic()->getID() + "', link " + toString(link->getTLIndex());
1513 : }
1514 :
1515 : std::string
1516 0 : MSDriveWay::formatVisitedMap(const LaneVisitedMap& visited) {
1517 : UNUSED_PARAMETER(visited);
1518 : /*
1519 : std::vector<const MSLane*> lanes(visited.size(), nullptr);
1520 : for (auto item : visited) {
1521 : lanes[item.second] = item.first;
1522 : }
1523 : for (auto it = lanes.begin(); it != lanes.end();) {
1524 : if (*it == nullptr) {
1525 : it = lanes.erase(it);
1526 : } else {
1527 : it++;
1528 : }
1529 : }
1530 : return toString(lanes);
1531 : */
1532 0 : return "dummy";
1533 : }
1534 :
1535 :
1536 : void
1537 104495 : MSDriveWay::appendMapIndex(LaneVisitedMap& map, const MSLane* lane) {
1538 : // avoid undefined behavior from evaluation order
1539 104495 : const int tmp = (int)map.size();
1540 104495 : map[lane] = tmp;
1541 104495 : }
1542 :
1543 : bool
1544 8981078 : MSDriveWay::match(MSRouteIterator firstIt, MSRouteIterator endIt) const {
1545 : // @todo optimize: it is sufficient to check for specific edges (after each switch)
1546 8981078 : auto itRoute = firstIt;
1547 : auto itDwRoute = myRoute.begin();
1548 : bool match = true;
1549 62319993 : while (itRoute != endIt && itDwRoute != myRoute.end()) {
1550 53529743 : if (*itRoute != *itDwRoute) {
1551 : match = false;
1552 : #ifdef DEBUG_MATCH
1553 : std::cout << " check dw=" << getID() << " match failed at vehEdge=" << (*itRoute)->getID() << " dwEdge=" << (*itDwRoute)->getID() << "\n";
1554 : #endif
1555 : break;
1556 : }
1557 : itRoute++;
1558 : itDwRoute++;
1559 : }
1560 : // if the vehicle arrives before the end of this driveway,
1561 : // we'd rather build a new driveway to avoid superfluous restrictions
1562 8790250 : if (match && itDwRoute == myRoute.end()
1563 17744605 : && (itRoute == endIt || myAbortedBuild || myBidiEnded || myFoundJump || myIsSubDriveway)) {
1564 : //std::cout << " using dw=" << "\n";
1565 8750901 : if (itRoute != endIt) {
1566 : // check whether the current route requires an extended driveway
1567 142390 : const MSEdge* next = *itRoute;
1568 142390 : const MSEdge* prev = myRoute.back();
1569 478 : if (myFoundJump && prev->getBidiEdge() != next && prev->getBidiEdge() != nullptr
1570 142641 : && prev->isConnectedTo(*next, (SUMOVehicleClass)(SVC_RAIL_CLASSES & prev->getPermissions()))) {
1571 : #ifdef DEBUG_MATCH
1572 : std::cout << " check dw=" << getID() << " prev=" << prev->getID() << " next=" << next->getID() << "\n";
1573 : #endif
1574 : return false;
1575 : }
1576 142373 : if (!myFoundJump && prev->getBidiEdge() == next && prev == &myForward.back()->getEdge()) {
1577 : assert(myIsSubDriveway || myBidiEnded);
1578 : // must not leave driveway via reversal
1579 : #ifdef DEBUG_MATCH
1580 : std::cout << getID() << " back=" << myForward.back()->getID() << " noMatch route " << toString(ConstMSEdgeVector(firstIt, endIt)) << "\n";
1581 : #endif
1582 : return false;
1583 : }
1584 142367 : if (myForward.back()->isInternal() && myForward.back()->getNextNormal() != (*itRoute)) {
1585 : // driveway is part of a direct-control conflict and continues elsewhere
1586 : #ifdef DEBUG_MATCH
1587 : std::cout << getID() << " back=" << myForward.back()->getID() << " noMatch route " << toString(ConstMSEdgeVector(firstIt, itRoute)) << " (direct control)\n";
1588 : #endif
1589 : return false;
1590 : }
1591 : }
1592 8750876 : return true;
1593 : }
1594 : return false;
1595 : }
1596 :
1597 : void
1598 11456 : MSDriveWay::addFoes(const MSLink* link) {
1599 : #ifdef DEBUG_ADD_FOES
1600 : if (gDebugFlag4) std::cout << "driveway " << myID << " addFoes for link " << link->getDescription() << "\n";
1601 : #endif
1602 11456 : const MSRailSignal* rs = dynamic_cast<const MSRailSignal*>(link->getTLLogic());
1603 11456 : if (rs != nullptr) {
1604 15834 : for (MSDriveWay* foe : rs->retrieveDriveWays(link->getTLIndex())) {
1605 : #ifdef DEBUG_ADD_FOES
1606 : if (gDebugFlag4) std::cout << " cand foe=" << foe->myID << " fc1=" << flankConflict(*foe) << " fc2=" << foe->flankConflict(*this) << " cc1=" << crossingConflict(*foe) << " cc2=" << foe->crossingConflict(*this) << "\n";
1607 : #endif
1608 4378 : if (foe != this && (flankConflict(*foe) || foe->flankConflict(*this) || crossingConflict(*foe) || foe->crossingConflict(*this))) {
1609 : #ifdef DEBUG_ADD_FOES
1610 : if (gDebugFlag4) std::cout << " foe=" << foe->myID << "\n";
1611 : #endif
1612 3322 : myFoes.push_back(foe);
1613 : }
1614 11456 : }
1615 : }
1616 11456 : }
1617 :
1618 :
1619 : void
1620 16422 : MSDriveWay::addBidiFoes(const MSRailSignal* ownSignal, bool extended) {
1621 : #ifdef DEBUG_ADD_FOES
1622 : if (gDebugFlag4) std::cout << "driveway " << myID << " addBidiFoes extended=" << extended << "\n";
1623 : #endif
1624 16422 : const std::vector<const MSLane*>& bidiLanes = extended ? myBidiExtended : myBidi;
1625 81212 : for (const MSLane* bidi : bidiLanes) {
1626 132674 : for (auto ili : bidi->getIncomingLanes()) {
1627 67884 : const MSRailSignal* rs = dynamic_cast<const MSRailSignal*>(ili.viaLink->getTLLogic());
1628 67884 : if (rs != nullptr && rs != ownSignal &&
1629 67884 : std::find(bidiLanes.begin(), bidiLanes.end(), ili.lane) != bidiLanes.end()) {
1630 4255 : addFoes(ili.viaLink);
1631 : }
1632 : }
1633 64790 : const MSEdge* bidiEdge = &bidi->getEdge();
1634 : if (myDepartureDriveways.count(bidiEdge) != 0) {
1635 3086 : for (MSDriveWay* foe : myDepartureDriveways[bidiEdge]) {
1636 1429 : if (flankConflict(*foe)) {
1637 : #ifdef DEBUG_ADD_FOES
1638 : if (gDebugFlag4) std::cout << " foe " << foe->getID() << " departs on bidi=" << bidiEdge->getID() << "\n";
1639 : #endif
1640 851 : myFoes.push_back(foe);
1641 : } else {
1642 : #ifdef DEBUG_ADD_FOES
1643 : if (gDebugFlag4) std::cout << " cand foe " << foe->getID() << " departs on bidi=" << bidiEdge->getID() << " rejected\n";
1644 : #endif
1645 : }
1646 : }
1647 : }
1648 : if (myDepartureDrivewaysEnds.count(bidiEdge) != 0) {
1649 2641 : for (MSDriveWay* foe : myDepartureDrivewaysEnds[bidiEdge]) {
1650 1366 : if (flankConflict(*foe)) {
1651 : #ifdef DEBUG_ADD_FOES
1652 : if (gDebugFlag4) std::cout << " foe " << foe->getID() << " ends on bidi=" << bidiEdge->getID() << "\n";
1653 : #endif
1654 849 : myFoes.push_back(foe);
1655 : } else {
1656 : #ifdef DEBUG_ADD_FOES
1657 : if (gDebugFlag4) std::cout << " cand foe " << foe->getID() << " ends on bidi=" << bidiEdge->getID() << " rejected\n";
1658 : #endif
1659 : }
1660 : }
1661 : }
1662 : }
1663 16422 : }
1664 :
1665 :
1666 : void
1667 7923 : MSDriveWay::addParallelFoes(const MSLink* link, const MSEdge* first) {
1668 : #ifdef DEBUG_ADD_FOES
1669 : if (gDebugFlag4) std::cout << "driveway " << myID << " addParallelFoes\n";
1670 : #endif
1671 7923 : if (link) {
1672 4976 : addFoes(link);
1673 : } else {
1674 : auto it = myDepartureDriveways.find(first);
1675 2947 : if (it != myDepartureDriveways.end()) {
1676 3107 : for (MSDriveWay* foe : it->second) {
1677 : #ifdef DEBUG_ADD_FOES
1678 : if (gDebugFlag4) std::cout << " foe " << foe->getID() << " departs on first=" << first->getID() << "\n";
1679 : #endif
1680 187 : myFoes.push_back(foe);
1681 : }
1682 : }
1683 : }
1684 7923 : }
1685 :
1686 :
1687 : void
1688 8211 : MSDriveWay::addReversalFoes(bool movingBlock) {
1689 : #ifdef DEBUG_ADD_FOES
1690 : if (gDebugFlag4) std::cout << "driveway " << myID << " addReversalFoes\n";
1691 : #endif
1692 : std::set<const MSEdge*> forward;
1693 35905 : for (const MSLane* lane : myForward) {
1694 27694 : if (lane->isNormal()) {
1695 18339 : forward.insert(&lane->getEdge());
1696 : }
1697 : }
1698 : int i = 0;
1699 70492 : for (const MSEdge* e : myRoute) {
1700 18353 : if (forward.count(e) != 0 && !movingBlock) {
1701 : // reversals in our own forward can be ignored because each driveway
1702 : // is automatically a foe of itself by default
1703 : continue;
1704 : }
1705 45455 : if (i == myCoreSize) {
1706 : break;
1707 : }
1708 45455 : i++;
1709 : auto it = myReversalDriveWays.find(e);
1710 45455 : if (it != myReversalDriveWays.end()) {
1711 6336 : for (MSDriveWay* foe : it->second) {
1712 : // check whether the foe reverses into our own forward section
1713 : // (it might reverse again or disappear via arrival)
1714 : #ifdef DEBUG_ADD_FOES
1715 : //std::cout << " candidate foe " << foe->getID() << " reverses on edge=" << e->getID() << " forward=" << joinNamedToString(forward, " ") << " foeRoute=" << toString(foe->myRoute) << "\n";
1716 : #endif
1717 11160 : if (forwardRouteConflict(forward, *foe)) {
1718 : std::set<const MSEdge*> foeForward;
1719 1162 : for (const MSLane* lane : foe->myForward) {
1720 973 : if (lane->isNormal()) {
1721 607 : foeForward.insert(&lane->getEdge());
1722 607 : if (lane->getBidiLane() != nullptr) {
1723 596 : foeForward.insert(lane->getEdge().getBidiEdge());
1724 : }
1725 : }
1726 : }
1727 : #ifdef DEBUG_ADD_FOES
1728 : if (gDebugFlag4) std::cout << " reversal cand=" << foe->getID() << " foeForward " << toString(foeForward) << "\n";
1729 : #endif
1730 378 : if (foe->forwardRouteConflict(foeForward, *this, true)) {
1731 : #ifdef DEBUG_ADD_FOES
1732 : if (gDebugFlag4) std::cout << " foe " << foe->getID() << " reverses on edge=" << e->getID() << "\n";
1733 : #endif
1734 143 : myFoes.push_back(foe);
1735 : }
1736 5391 : } else if (movingBlock && foe == this) {
1737 : #ifdef DEBUG_ADD_FOES
1738 : if (gDebugFlag4) std::cout << " dw " << getID() << " reverses on forward edge=" << e->getID() << " (movingBlock)\n";
1739 : #endif
1740 23 : myFoes.push_back(foe);
1741 : }
1742 : }
1743 : }
1744 : }
1745 8211 : }
1746 :
1747 :
1748 : bool
1749 3648 : MSDriveWay::buildSubFoe(MSDriveWay* foe, bool movingBlock) {
1750 : // Subdriveways (Teilfahrstraße) model the resolution of a driving conflict
1751 : // before a vehicle has left the driveway. This is possible when the driveway diverges from the foe
1752 : // driveway at an earlier point (switch or crossing).
1753 : //
1754 : // We already know that the last edge of this driveway doesn't impact the foe (unless the driveway ends within the block).
1755 : // Remove further edges from the end of the driveway (myForward) until the point of conflict is found.
1756 : //
1757 : // For movingBlock the logic is changed:
1758 : // We remove the conflict-free part as before but then keep removing the conflict part until another non-conconflit part is found
1759 3648 : if (myForward.size() < foe->myForward.size() &&
1760 3648 : myForward == std::vector<const MSLane*>(foe->myForward.begin(), foe->myForward.begin() + myForward.size())) {
1761 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1762 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " is subpart of foe=" << foe->getID() << "\n";
1763 : #endif
1764 29 : foe->myFoes.push_back(this);
1765 29 : return true;
1766 : }
1767 3619 : int subLast = (int)myForward.size() - 2;
1768 3619 : if (movingBlock && myForward.back() == foe->myForward.back()) {
1769 69 : subLast++;
1770 : }
1771 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1772 : if (subLast < 0) {
1773 : if (gDebugFlag4) std::cout << " " << getID() << " cannot build subDriveWay for foe " << foe->getID() << " because myForward has only a single lane\n";
1774 : }
1775 : #endif
1776 : bool foundConflict = false;
1777 : bool flankC = false;
1778 : bool zipperC = false;
1779 : bool crossC = false;
1780 12854 : while (subLast >= 0) {
1781 12112 : const MSLane* lane = myForward[subLast];
1782 12112 : const MSLink* tmpOrigin = subLast > 0 ? myForward[subLast - 1]->getLinkTo(lane) : myOrigin;
1783 12112 : MSDriveWay tmp(tmpOrigin, "tmp", true);
1784 12112 : tmp.myForward.push_back(lane);
1785 12112 : flankC = tmp.flankConflict(*foe);
1786 12112 : const bool bidiConflict = std::find(foe->myBidi.begin(), foe->myBidi.end(), lane) != foe->myBidi.end();
1787 12112 : crossC = tmp.crossingConflict(*foe);
1788 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1789 : if (gDebugFlag4) std::cout << " subLast=" << subLast << " lane=" << lane->getID() << " fc=" << flankC << " cc=" << crossC << " bc=" << bidiConflict << "\n";
1790 : #endif
1791 12112 : if (flankC || crossC || bidiConflict) {
1792 : foundConflict = true;
1793 3188 : if (!movingBlock || bidiConflict) {
1794 : break;
1795 : }
1796 350 : if (((flankC && lane->getFromJunction()->getType() == SumoXMLNodeType::ZIPPER)
1797 52 : || (!flankC && lane->getToJunction()->getType() == SumoXMLNodeType::ZIPPER))
1798 432 : && (isDepartDriveway()
1799 18 : || getForwardDistance(flankC ? subLast - 1 : subLast) > myMovingBlockMaxDist)) {
1800 : zipperC = true;
1801 : foundConflict = false;
1802 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1803 : if (gDebugFlag4) std::cout << " ignored movingBlock zipperConflict\n";
1804 : #endif
1805 18 : if (!flankC && crossC) {
1806 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1807 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " movingBlock-save\n";
1808 : #endif
1809 : return false;
1810 : }
1811 : }
1812 393 : if (!flankC && crossC) {
1813 : break;
1814 : }
1815 8924 : } else if (foundConflict) {
1816 : break;
1817 : }
1818 9235 : subLast--;
1819 12112 : }
1820 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1821 : if (gDebugFlag4) std::cout << " subLastFinal=" << subLast << " movingBlock=" << movingBlock << " zipperC=" << zipperC << "\n";
1822 : #endif
1823 3610 : if (subLast < 0) {
1824 742 : if (movingBlock && zipperC) {
1825 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1826 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " movingBlock-save\n";
1827 : #endif
1828 : return false;
1829 742 : } else if (&myForward.back()->getEdge() == myRoute.back() && foe->forwardEndOnRoute(this)) {
1830 : // driveway ends in the middle of the block and only the final edge overlaps with the foe driveWay
1831 16 : foe->myFoes.push_back(this);
1832 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1833 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " foe endsOnForward\n";
1834 : #endif
1835 726 : } else if (foe->myTerminateRoute) {
1836 598 : if (bidiBlockedByEnd(*foe) && bidiBlockedBy(*this) && foe->forwardEndOnRoute(this)) {
1837 89 : foe->myFoes.push_back(this);
1838 : // foe will get the sidings
1839 89 : addSidings(foe, true);
1840 : }
1841 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1842 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " terminates\n";
1843 : #endif
1844 128 : } else if (myTerminateRoute && myBidi.size() <= myForward.size()) {
1845 0 : foe->myFoes.push_back(this);
1846 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1847 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " terminates, foe=" << foe->getID() << "\n";
1848 : #endif
1849 0 : return true;
1850 : } else if (foe->myReversals.size() % 2 == 1) {
1851 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1852 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " has " << foe->myReversals.size() << " reversals\n";
1853 : #endif
1854 : } else {
1855 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1856 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " failed\n";
1857 : #endif
1858 : #ifdef SUBDRIVEWAY_WARN_NOCONFLICT
1859 : WRITE_WARNINGF("No point of conflict found between driveway '%' and driveway '%' when creating sub-driveway", getID(), foe->getID());
1860 : #endif
1861 : }
1862 742 : return false;
1863 : }
1864 2868 : int subSize = subLast + 1;
1865 3637 : for (MSDriveWay* cand : mySubDriveWays) {
1866 1436 : if ((int)cand->myForward.size() == subSize) {
1867 : // can re-use existing sub-driveway
1868 667 : foe->myFoes.push_back(cand);
1869 667 : cand->myFoes.push_back(foe);
1870 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1871 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " useExisting=" << cand->getID() << "\n";
1872 : #endif
1873 667 : return true;
1874 : }
1875 : }
1876 2201 : std::vector<const MSLane*> forward(myForward.begin(), myForward.begin() + subSize);
1877 : std::vector<const MSEdge*> route;
1878 11187 : for (const MSLane* lane : forward) {
1879 8986 : if (lane->isNormal()) {
1880 5207 : route.push_back(&lane->getEdge());
1881 : }
1882 : }
1883 2201 : if (route.empty()) {
1884 36 : if (subSize == 1 && crossC
1885 34 : && forward.front()->getFromJunction() == foe->myForward.front()->getFromJunction()
1886 70 : && forward.front()->getFromJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
1887 : assert(myForward.front()->isInternal());
1888 : // sub-driveway ends after a single internal lane but since the route cannot be empty we add the next edge
1889 34 : route.push_back(foe->myForward.front()->getEdge().getNormalSuccessor());
1890 : } else {
1891 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1892 : if (gDebugFlag4) std::cout << SIMTIME << " abort subFoe dw=" << getID() << " foe=" << foe->getID() << " empty subRoute\n";
1893 : #endif
1894 2 : return false;
1895 : }
1896 : }
1897 2199 : if (myRoute.size() > route.size()) {
1898 : // route continues. make sure the subDriveway does not end with a reversal
1899 2192 : const MSEdge* lastNormal = route.back();
1900 2192 : const MSEdge* nextNormal = myRoute[route.size()];
1901 2192 : if (lastNormal->getBidiEdge() == nextNormal) {
1902 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1903 : if (gDebugFlag4) std::cout << SIMTIME << " abort subFoe dw=" << getID() << " foe=" << foe->getID()
1904 : << " lastNormal=" << lastNormal->getID() << " nextNormal=" << nextNormal->getID() << " endWithReversal\n";
1905 : #endif
1906 : return false;
1907 : }
1908 : }
1909 4358 : MSDriveWay* sub = new MSDriveWay(myOrigin, getID() + "." + toString(mySubDriveWays.size()));
1910 2179 : sub->myLane = myLane;
1911 2179 : sub->myIsSubDriveway = true;
1912 2179 : sub->myForward = forward;
1913 2179 : sub->myRoute = route;
1914 2179 : sub->myCoreSize = (int)sub->myRoute.size();
1915 2179 : myLane->addMoveReminder(sub, false);
1916 :
1917 : // copy trains that are currently on this driveway (and associated entry events)
1918 2312 : for (SUMOVehicle* veh : myTrains) {
1919 133 : auto itOnSub = std::find(sub->myRoute.begin(), sub->myRoute.end(), veh->getEdge());
1920 133 : if (itOnSub != sub->myRoute.end()) {
1921 : sub->myTrains.insert(veh);
1922 : // non-zero is enough to avoid superfluous activation via activateReminders (and removal)
1923 116 : const double pos = sub->myRoute.front()->getLength();
1924 116 : dynamic_cast<MSBaseVehicle*>(veh)->addReminder(sub, pos);
1925 123 : for (const VehicleEvent& ve : myVehicleEvents) {
1926 7 : if (ve.id == veh->getID()) {
1927 7 : sub->myVehicleEvents.push_back(ve);
1928 : }
1929 : }
1930 : }
1931 : }
1932 :
1933 2179 : foe->myFoes.push_back(sub);
1934 2179 : sub->myFoes.push_back(foe);
1935 2179 : mySubDriveWays.push_back(sub);
1936 : #ifdef DEBUG_BUILD_SUBDRIVEWAY
1937 : if (gDebugFlag4) std::cout << SIMTIME << " buildSubFoe dw=" << getID() << " foe=" << foe->getID() << " sub=" << sub->getID() << " route=" << toString(sub->myRoute) << "\n";
1938 : #endif
1939 : return true;
1940 2201 : }
1941 :
1942 :
1943 : double
1944 18 : MSDriveWay::getForwardDistance(int lastIndex) const {
1945 : assert(lastIndex < (int)myForward.size());
1946 : double result = 0;
1947 54 : for (int i = 0; i <= lastIndex; i++) {
1948 36 : result += myForward[i]->getLength();
1949 : }
1950 18 : return result;
1951 : }
1952 :
1953 :
1954 : void
1955 3996 : MSDriveWay::addSidings(MSDriveWay* foe, bool addToFoe) {
1956 3996 : const MSEdge* foeEndBidi = foe->myForward.back()->getEdge().getBidiEdge();
1957 : int forwardNormals = 0;
1958 18071 : for (auto lane : foe->myForward) {
1959 14075 : if (lane->isNormal()) {
1960 9382 : forwardNormals++;
1961 : }
1962 : }
1963 3996 : if (forwardNormals == (int)foe->myRoute.size()) {
1964 : #ifdef DEBUG_BUILD_SIDINGS
1965 : if (gDebugFlag4) std::cout << "checkSiding " << getID() << " foe=" << foe->getID() << " forwardNormals=" << forwardNormals << " frSize=" << foe->myRoute.size() << " aborted\n";
1966 : #endif
1967 777 : return;
1968 : }
1969 : auto foeSearchBeg = foe->myRoute.begin() + forwardNormals;
1970 : auto foeSearchEnd = foe->myRoute.end();
1971 3219 : if (foeEndBidi == nullptr) {
1972 0 : throw ProcessError("checkSiding " + getID() + " foe=" + foe->getID() + " noBidi\n");
1973 : }
1974 : int i;
1975 : std::vector<int> start;
1976 : std::vector<double> length;
1977 21047 : for (i = 0; i < (int)myRoute.size(); i++) {
1978 21047 : if (myRoute[i] == foeEndBidi) {
1979 : break;
1980 : }
1981 : }
1982 3219 : if (i == (int)myRoute.size()) {
1983 0 : throw ProcessError("checkSiding " + getID() + " foe=" + foe->getID() + " foeEndBidi=" + foeEndBidi->getID() + " not on route\n");
1984 : }
1985 3219 : const MSEdge* next = myRoute[i];
1986 : #ifdef DEBUG_BUILD_SIDINGS
1987 : if (gDebugFlag4) std::cout << "checkSiding " << getID() << " foe=" << foe->getID() << " i=" << i << " next=" << next->getID() << " forwardNormals=" << forwardNormals << " frSize=" << foe->myRoute.size() << " foeSearchBeg=" << (*foeSearchBeg)->getID() << "\n";
1988 : #endif
1989 3219 : i--;
1990 21047 : for (; i >= 0; i--) {
1991 17828 : const MSEdge* cur = myRoute[i];
1992 17828 : if (hasRS(cur, next)) {
1993 5243 : if (std::find(foeSearchBeg, foeSearchEnd, cur->getBidiEdge()) == foeSearchEnd) {
1994 2876 : start.push_back(i);
1995 2876 : length.push_back(0);
1996 : }
1997 : }
1998 17828 : if (!start.empty()) {
1999 4689 : auto itFind = std::find(foeSearchBeg, foeSearchEnd, cur->getBidiEdge());
2000 4689 : if (itFind != foeSearchEnd) {
2001 : #ifdef DEBUG_BUILD_SIDINGS
2002 : if (gDebugFlag4) std::cout << "endSiding " << getID() << " foe=" << foe->getID() << " i=" << i << " curBidi=" << Named::getIDSecure(cur->getBidiEdge()) << " length=" << toString(length) << "\n";
2003 : #endif
2004 707 : const int firstIndex = i + 1;
2005 707 : if (addToFoe) {
2006 69 : auto& foeSidings = foe->mySidings[this];
2007 : // indices must be mapped onto foe route;
2008 69 : const MSEdge* first = myRoute[firstIndex];
2009 69 : auto itFirst = std::find(foe->myRoute.begin(), foe->myRoute.end(), first);
2010 69 : if (itFirst != foe->myRoute.end()) {
2011 279 : for (int j = 0; j < (int)length.size(); j++) {
2012 221 : const MSEdge* last = myRoute[start[j]];
2013 221 : auto itLast = std::find(itFirst, foe->myRoute.end(), last);
2014 221 : if (itLast != foe->myRoute.end()) {
2015 221 : foeSidings.insert(foeSidings.begin(), Siding((int)(itFirst - foe->myRoute.begin()), (int)(itLast - foe->myRoute.begin()), length[j]));
2016 : }
2017 : }
2018 : }
2019 : } else {
2020 638 : auto& foeSidings = mySidings[foe];
2021 2968 : for (int j = 0; j < (int)length.size(); j++) {
2022 2330 : foeSidings.insert(foeSidings.begin(), Siding(firstIndex, start[j], length[j]));
2023 : }
2024 : }
2025 : start.clear();
2026 : length.clear();
2027 707 : foeSearchBeg = itFind;
2028 : } else {
2029 25300 : for (int j = 0; j < (int)length.size(); j++) {
2030 21318 : length[j] += cur->getLength();
2031 : }
2032 : }
2033 : }
2034 : next = cur;
2035 : }
2036 3219 : }
2037 :
2038 :
2039 : bool
2040 17828 : MSDriveWay::hasRS(const MSEdge* cur, const MSEdge* next) {
2041 17828 : if (cur->getToJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
2042 : // check if there is a controlled link between cur and next
2043 13815 : for (auto lane : cur->getLanes()) {
2044 14221 : for (const MSLink* link : lane->getLinkCont()) {
2045 9935 : if (&link->getLane()->getEdge() == next && link->getTLLogic() != nullptr) {
2046 : return true;
2047 : }
2048 : }
2049 : }
2050 : }
2051 : return false;
2052 : }
2053 :
2054 :
2055 : bool
2056 119 : MSDriveWay::forwardEndOnRoute(const MSDriveWay* foe) const {
2057 119 : const MSEdge* foeForwardEnd = &foe->myForward.back()->getNormalPredecessorLane()->getEdge();
2058 119 : return std::find(myRoute.begin(), myRoute.end(), foeForwardEnd) != myRoute.end();
2059 : }
2060 :
2061 : void
2062 4569 : MSDriveWay::addConflictLink(const MSLink* link) {
2063 4569 : if (link->getTLLogic() != nullptr) {
2064 : // ignore links that originate on myBidi
2065 : // and also links from the same junction as my own link
2066 4569 : const MSLane* origin = link->getLaneBefore();
2067 4569 : if (std::find(myBidi.begin(), myBidi.end(), origin) == myBidi.end()) {
2068 3654 : if (link->getJunction() != myRoute.front()->getFromJunction()) {
2069 2184 : if (std::find(myConflictLinks.begin(), myConflictLinks.end(), link) == myConflictLinks.end()) {
2070 1508 : myConflictLinks.push_back(const_cast<MSLink*>(link));
2071 : }
2072 : }
2073 : }
2074 : }
2075 4569 : }
2076 :
2077 : void
2078 306 : MSDriveWay::addDWDeadlock(const std::vector<const MSDriveWay*>& deadlockFoes) {
2079 : std::set<const MSDriveWay*> filtered;
2080 1569 : for (const MSDriveWay* foe : deadlockFoes) {
2081 1263 : if (std::find(myFoes.begin(), myFoes.end(), foe) == myFoes.end()) {
2082 : filtered.insert(foe);
2083 : }
2084 : }
2085 306 : if (std::find(myDeadlocks.begin(), myDeadlocks.end(), filtered) == myDeadlocks.end()) {
2086 81 : myDeadlocks.push_back(filtered);
2087 : //std::cout << getID() << " deadlockFoes=" << toString(deadlockFoes) << "\n";
2088 : }
2089 306 : }
2090 :
2091 : const MSDriveWay*
2092 62139 : MSDriveWay::getDepartureDriveway(const SUMOVehicle* veh, bool init) {
2093 62139 : const MSEdge* edge = init ? veh->getRoute().getEdges()[veh->getDepartEdge()] : veh->getEdge();
2094 : assert(isRailwayOrShared(edge->getPermissions()));
2095 62139 : if (edge->getFromJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
2096 3490 : for (const MSLane* lane : edge->getLanes()) {
2097 3799 : for (auto ili : lane->getIncomingLanes()) {
2098 2328 : const MSLink* entry = ili.viaLink->getCorrespondingEntryLink();
2099 2328 : const MSRailSignal* rs = dynamic_cast<const MSRailSignal*>(entry->getTLLogic());
2100 1693 : if (rs != nullptr) {
2101 1693 : const MSDriveWay* dw = &const_cast<MSRailSignal*>(rs)->retrieveDriveWayForVeh(entry->getTLIndex(), veh);
2102 1693 : if (&dw->myForward.front()->getEdge() == edge) {
2103 : return dw;
2104 : }
2105 : }
2106 : }
2107 : }
2108 : }
2109 64918 : for (MSDriveWay* dw : myDepartureDriveways[edge]) {
2110 61789 : auto matchStart = init ? veh->getRoute().begin() + veh->getDepartEdge() : veh->getCurrentRouteEdge();
2111 61789 : if (dw->match(matchStart, veh->getRoute().end())) {
2112 58462 : return dw;
2113 : }
2114 : }
2115 6258 : const std::string id = edge->getFromJunction()->getID() + ".d" + toString(myDepartDrivewayIndex[edge->getFromJunction()]++);
2116 3129 : MSDriveWay* dw = buildDriveWay(id, nullptr, veh->getCurrentRouteEdge(), veh->getRoute().end());
2117 3129 : myDepartureDriveways[edge].push_back(dw);
2118 3129 : myDepartureDrivewaysEnds[&dw->myForward.back()->getEdge()].push_back(dw);
2119 : dw->setVehicle(veh->getID());
2120 : return dw;
2121 : }
2122 :
2123 :
2124 : void
2125 1441 : MSDriveWay::writeDepatureBlocks(OutputDevice& od, bool writeVehicles) {
2126 3598 : for (auto item : myDepartureDriveways) {
2127 2157 : const MSEdge* edge = item.first;
2128 2157 : if (item.second.size() > 0) {
2129 4314 : od.openTag("departJunction");
2130 2157 : od.writeAttr(SUMO_ATTR_ID, edge->getFromJunction()->getID());
2131 4539 : for (const MSDriveWay* dw : item.second) {
2132 2382 : if (writeVehicles) {
2133 258 : dw->writeBlockVehicles(od);
2134 : } else {
2135 2124 : dw->writeBlocks(od);
2136 : }
2137 : }
2138 4314 : od.closeTag(); // departJunction
2139 : }
2140 : }
2141 1441 : }
2142 :
2143 : void
2144 476 : MSDriveWay::saveState(OutputDevice& out) {
2145 : // all driveways are in myEndingDriveways which makes it convenient
2146 551 : for (auto item : myEndingDriveways) {
2147 164 : for (MSDriveWay* dw : item.second) {
2148 89 : dw->_saveState(out);
2149 107 : for (MSDriveWay* sub : dw->mySubDriveWays) {
2150 18 : sub->_saveState(out);
2151 : }
2152 : }
2153 : }
2154 476 : }
2155 :
2156 : void
2157 107 : MSDriveWay::_saveState(OutputDevice& out) const {
2158 107 : if (!myTrains.empty() || haveSubTrains()) {
2159 73 : out.openTag(myIsSubDriveway ? SUMO_TAG_SUBDRIVEWAY : SUMO_TAG_DRIVEWAY);
2160 38 : out.writeAttr(SUMO_ATTR_ID, getID());
2161 38 : out.writeAttr(SUMO_ATTR_EDGES, toString(myRoute));
2162 38 : if (!myTrains.empty()) {
2163 : std::vector<std::string> trainIDs;
2164 76 : for (SUMOVehicle* veh : myTrains) {
2165 38 : trainIDs.push_back(veh->getID());
2166 : }
2167 38 : out.writeAttr(SUMO_ATTR_VEHICLES, toString(trainIDs));
2168 38 : }
2169 76 : out.closeTag();
2170 : }
2171 107 : }
2172 :
2173 :
2174 : bool
2175 69 : MSDriveWay::haveSubTrains() const {
2176 76 : for (MSDriveWay* sub : mySubDriveWays) {
2177 7 : if (!sub->myTrains.empty()) {
2178 : return true;
2179 : }
2180 : }
2181 : return false;
2182 : }
2183 :
2184 : void
2185 38 : MSDriveWay::loadState(const SUMOSAXAttributes& attrs, int tag) {
2186 38 : if ((int)myDriveWayRouteLookup.size() < myGlobalDriveWayIndex) {
2187 100 : for (auto item : myEndingDriveways) {
2188 158 : for (MSDriveWay* dw : item.second) {
2189 81 : myDriveWayRouteLookup[dw->myRoute] = dw;
2190 : }
2191 : }
2192 : }
2193 38 : MSVehicleControl& c = MSNet::getInstance()->getVehicleControl();
2194 : bool ok;
2195 38 : const std::string id = attrs.get<std::string>(SUMO_ATTR_ID, nullptr, ok);
2196 38 : const std::string edges = attrs.get<std::string>(SUMO_ATTR_EDGES, id.c_str(), ok);
2197 : ConstMSEdgeVector route;
2198 38 : if (attrs.hasAttribute(SUMO_ATTR_EDGES)) {
2199 38 : MSEdge::parseEdgesList(edges, route, id);
2200 : }
2201 : MSDriveWay* dw = nullptr;
2202 38 : if (tag == SUMO_TAG_DRIVEWAY) {
2203 : auto it = myDriveWayRouteLookup.find(route);
2204 35 : if (it == myDriveWayRouteLookup.end()) {
2205 : //WRITE_WARNING(TLF("Unknown driveWay '%' with route '%'", id, edges));
2206 : //return;
2207 0 : throw ProcessError(TLF("Unknown driveWay '%' with route '%'", id, edges));
2208 : }
2209 35 : dw = it->second;
2210 35 : myDriveWayLookup[id] = dw;
2211 : } else {
2212 3 : std::string parentID = id.substr(0, id.rfind('.'));
2213 : auto it = myDriveWayLookup.find(parentID);
2214 3 : if (it == myDriveWayLookup.end()) {
2215 : //WRITE_WARNING(TLF("Unknown parent driveway '%' for subDriveWay '%'", parentID, id));
2216 : //return;
2217 0 : throw ProcessError(TLF("Unknown parent driveway '%' for subDriveWay '%'", parentID, id));
2218 : }
2219 3 : MSDriveWay* parent = it->second;
2220 3 : for (MSDriveWay* sub : parent->mySubDriveWays) {
2221 1 : if (sub->myRoute == route) {
2222 : dw = sub;
2223 : break;
2224 : }
2225 : }
2226 3 : if (dw == nullptr) {
2227 : // missing subdriveways can be ignored. They may have been created
2228 : // as foes for driveways that are not relevant at state loading time
2229 : return;
2230 : }
2231 : }
2232 72 : const std::string vehicles = attrs.getOpt<std::string>(SUMO_ATTR_VEHICLES, id.c_str(), ok, "");
2233 108 : for (const std::string& vehID : StringTokenizer(vehicles).getVector()) {
2234 36 : MSBaseVehicle* veh = dynamic_cast<MSBaseVehicle*>(c.getVehicle(vehID));
2235 36 : if (veh == nullptr) {
2236 0 : throw ProcessError(TLF("Unknown vehicle '%' in driveway '%'", vehID, id));
2237 : }
2238 36 : if (!dw->hasTrain(veh)) {
2239 4 : dw->myTrains.insert(veh);
2240 4 : veh->addReminder(dw);
2241 : }
2242 36 : }
2243 38 : }
2244 :
2245 : const MSDriveWay*
2246 0 : MSDriveWay::retrieveDepartDriveWay(const MSEdge* edge, const std::string& id) {
2247 0 : for (MSDriveWay* dw : myDepartureDriveways[edge]) {
2248 0 : if (dw->getID() == id) {
2249 : return dw;
2250 : }
2251 : }
2252 : return nullptr;
2253 : }
2254 :
2255 :
2256 : bool
2257 1493 : MSDriveWay::hasTrain(SUMOVehicle* veh) const {
2258 1493 : return myTrains.count(veh) != 0;
2259 : }
2260 :
2261 : /****************************************************************************/
|
