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 MSVehicle.cpp
15 : /// @author Christian Roessel
16 : /// @author Jakob Erdmann
17 : /// @author Bjoern Hendriks
18 : /// @author Daniel Krajzewicz
19 : /// @author Thimor Bohn
20 : /// @author Friedemann Wesner
21 : /// @author Laura Bieker
22 : /// @author Clemens Honomichl
23 : /// @author Michael Behrisch
24 : /// @author Axel Wegener
25 : /// @author Christoph Sommer
26 : /// @author Leonhard Luecken
27 : /// @author Lara Codeca
28 : /// @author Mirko Barthauer
29 : /// @date Mon, 05 Mar 2001
30 : ///
31 : // Representation of a vehicle in the micro simulation
32 : /****************************************************************************/
33 : #include <config.h>
34 :
35 : #include <iostream>
36 : #include <cassert>
37 : #include <cmath>
38 : #include <cstdlib>
39 : #include <algorithm>
40 : #include <map>
41 : #include <memory>
42 : #include <utils/common/ToString.h>
43 : #include <utils/common/FileHelpers.h>
44 : #include <utils/router/DijkstraRouter.h>
45 : #include <utils/common/MsgHandler.h>
46 : #include <utils/common/RandHelper.h>
47 : #include <utils/common/StringUtils.h>
48 : #include <utils/common/StdDefs.h>
49 : #include <utils/geom/GeomHelper.h>
50 : #include <utils/iodevices/OutputDevice.h>
51 : #include <utils/xml/SUMOSAXAttributes.h>
52 : #include <utils/vehicle/SUMOVehicleParserHelper.h>
53 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
54 : #include <microsim/transportables/MSPerson.h>
55 : #include <microsim/transportables/MSPModel.h>
56 : #include <microsim/devices/MSDevice_Transportable.h>
57 : #include <microsim/devices/MSDevice_DriverState.h>
58 : #include <microsim/devices/MSDevice_Friction.h>
59 : #include <microsim/devices/MSDevice_Taxi.h>
60 : #include <microsim/devices/MSDevice_Vehroutes.h>
61 : #include <microsim/devices/MSDevice_ElecHybrid.h>
62 : #include <microsim/devices/MSDevice_GLOSA.h>
63 : #include <microsim/output/MSStopOut.h>
64 : #include <microsim/trigger/MSChargingStation.h>
65 : #include <microsim/trigger/MSOverheadWire.h>
66 : #include <microsim/traffic_lights/MSTrafficLightLogic.h>
67 : #include <microsim/traffic_lights/MSRailSignalControl.h>
68 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
69 : #include <microsim/transportables/MSTransportableControl.h>
70 : #include <microsim/devices/MSDevice_Transportable.h>
71 : #include "MSEdgeControl.h"
72 : #include "MSVehicleControl.h"
73 : #include "MSInsertionControl.h"
74 : #include "MSVehicleTransfer.h"
75 : #include "MSGlobals.h"
76 : #include "MSJunctionLogic.h"
77 : #include "MSStop.h"
78 : #include "MSStoppingPlace.h"
79 : #include "MSParkingArea.h"
80 : #include "MSMoveReminder.h"
81 : #include "MSLane.h"
82 : #include "MSJunction.h"
83 : #include "MSEdge.h"
84 : #include "MSVehicleType.h"
85 : #include "MSNet.h"
86 : #include "MSRoute.h"
87 : #include "MSLeaderInfo.h"
88 : #include "MSDriverState.h"
89 : #include "MSVehicle.h"
90 :
91 :
92 : //#define DEBUG_PLAN_MOVE
93 : //#define DEBUG_PLAN_MOVE_LEADERINFO
94 : //#define DEBUG_CHECKREWINDLINKLANES
95 : //#define DEBUG_EXEC_MOVE
96 : //#define DEBUG_FURTHER
97 : //#define DEBUG_SETFURTHER
98 : //#define DEBUG_TARGET_LANE
99 : //#define DEBUG_STOPS
100 : //#define DEBUG_BESTLANES
101 : //#define DEBUG_IGNORE_RED
102 : //#define DEBUG_ACTIONSTEPS
103 : //#define DEBUG_NEXT_TURN
104 : //#define DEBUG_TRACI
105 : //#define DEBUG_REVERSE_BIDI
106 : //#define DEBUG_EXTRAPOLATE_DEPARTPOS
107 : //#define DEBUG_REMOTECONTROL
108 : //#define DEBUG_MOVEREMINDERS
109 : //#define DEBUG_COND (getID() == "ego")
110 : //#define DEBUG_COND (true)
111 : #define DEBUG_COND (isSelected())
112 : //#define DEBUG_COND2(obj) (obj->getID() == "ego")
113 : #define DEBUG_COND2(obj) (obj->isSelected())
114 :
115 : //#define PARALLEL_STOPWATCH
116 :
117 :
118 : #define STOPPING_PLACE_OFFSET 0.5
119 :
120 : #define CRLL_LOOK_AHEAD 5
121 :
122 : #define JUNCTION_BLOCKAGE_TIME 5 // s
123 :
124 : // @todo Calibrate with real-world values / make configurable
125 : #define DIST_TO_STOPLINE_EXPECT_PRIORITY 1.0
126 :
127 : #define NUMERICAL_EPS_SPEED (0.1 * NUMERICAL_EPS * TS)
128 :
129 : // ===========================================================================
130 : // static value definitions
131 : // ===========================================================================
132 : std::vector<MSLane*> MSVehicle::myEmptyLaneVector;
133 :
134 :
135 : // ===========================================================================
136 : // method definitions
137 : // ===========================================================================
138 : /* -------------------------------------------------------------------------
139 : * methods of MSVehicle::State
140 : * ----------------------------------------------------------------------- */
141 0 : MSVehicle::State::State(const State& state) {
142 0 : myPos = state.myPos;
143 0 : mySpeed = state.mySpeed;
144 0 : myPosLat = state.myPosLat;
145 0 : myBackPos = state.myBackPos;
146 0 : myPreviousSpeed = state.myPreviousSpeed;
147 0 : myLastCoveredDist = state.myLastCoveredDist;
148 0 : }
149 :
150 :
151 : MSVehicle::State&
152 3798980 : MSVehicle::State::operator=(const State& state) {
153 3798980 : myPos = state.myPos;
154 3798980 : mySpeed = state.mySpeed;
155 3798980 : myPosLat = state.myPosLat;
156 3798980 : myBackPos = state.myBackPos;
157 3798980 : myPreviousSpeed = state.myPreviousSpeed;
158 3798980 : myLastCoveredDist = state.myLastCoveredDist;
159 3798980 : return *this;
160 : }
161 :
162 :
163 : bool
164 0 : MSVehicle::State::operator!=(const State& state) {
165 0 : return (myPos != state.myPos ||
166 0 : mySpeed != state.mySpeed ||
167 0 : myPosLat != state.myPosLat ||
168 0 : myLastCoveredDist != state.myLastCoveredDist ||
169 0 : myPreviousSpeed != state.myPreviousSpeed ||
170 0 : myBackPos != state.myBackPos);
171 : }
172 :
173 :
174 8593076 : MSVehicle::State::State(double pos, double speed, double posLat, double backPos, double previousSpeed) :
175 8593076 : myPos(pos), mySpeed(speed), myPosLat(posLat), myBackPos(backPos), myPreviousSpeed(previousSpeed), myLastCoveredDist(SPEED2DIST(speed)) {}
176 :
177 :
178 :
179 : /* -------------------------------------------------------------------------
180 : * methods of MSVehicle::WaitingTimeCollector
181 : * ----------------------------------------------------------------------- */
182 4794096 : MSVehicle::WaitingTimeCollector::WaitingTimeCollector(SUMOTime memory) : myMemorySize(memory) {}
183 :
184 :
185 : SUMOTime
186 1428927 : MSVehicle::WaitingTimeCollector::cumulatedWaitingTime(SUMOTime memorySpan) const {
187 : assert(memorySpan <= myMemorySize);
188 1428927 : if (memorySpan == -1) {
189 0 : memorySpan = myMemorySize;
190 : }
191 : SUMOTime totalWaitingTime = 0;
192 5944969 : for (const auto& interval : myWaitingIntervals) {
193 4516042 : if (interval.second >= memorySpan) {
194 655960 : if (interval.first >= memorySpan) {
195 : break;
196 : } else {
197 655960 : totalWaitingTime += memorySpan - interval.first;
198 : }
199 : } else {
200 3860082 : totalWaitingTime += interval.second - interval.first;
201 : }
202 : }
203 1428927 : return totalWaitingTime;
204 : }
205 :
206 :
207 : void
208 704272881 : MSVehicle::WaitingTimeCollector::passTime(SUMOTime dt, bool waiting) {
209 : auto i = myWaitingIntervals.begin();
210 : const auto end = myWaitingIntervals.end();
211 704272881 : const bool startNewInterval = i == end || (i->first != 0);
212 1141109554 : while (i != end) {
213 439095102 : i->first += dt;
214 439095102 : if (i->first >= myMemorySize) {
215 : break;
216 : }
217 436836673 : i->second += dt;
218 : i++;
219 : }
220 :
221 : // remove intervals beyond memorySize
222 : auto d = std::distance(i, end);
223 706531310 : while (d > 0) {
224 2258429 : myWaitingIntervals.pop_back();
225 2258429 : d--;
226 : }
227 :
228 704272881 : if (!waiting) {
229 : return;
230 90247620 : } else if (!startNewInterval) {
231 86634007 : myWaitingIntervals.begin()->first = 0;
232 : } else {
233 7227226 : myWaitingIntervals.push_front(std::make_pair(0, dt));
234 : }
235 : return;
236 : }
237 :
238 :
239 : const std::string
240 2456 : MSVehicle::WaitingTimeCollector::getState() const {
241 2456 : std::ostringstream state;
242 2456 : state << myMemorySize << " " << myWaitingIntervals.size();
243 3335 : for (const auto& interval : myWaitingIntervals) {
244 1758 : state << " " << interval.first << " " << interval.second;
245 : }
246 2456 : return state.str();
247 2456 : }
248 :
249 :
250 : void
251 3539 : MSVehicle::WaitingTimeCollector::setState(const std::string& state) {
252 3539 : std::istringstream is(state);
253 : int numIntervals;
254 : SUMOTime begin, end;
255 3539 : is >> myMemorySize >> numIntervals;
256 5585 : while (numIntervals-- > 0) {
257 : is >> begin >> end;
258 2046 : myWaitingIntervals.emplace_back(begin, end);
259 : }
260 3539 : }
261 :
262 :
263 : /* -------------------------------------------------------------------------
264 : * methods of MSVehicle::Influencer::GapControlState
265 : * ----------------------------------------------------------------------- */
266 : void
267 30 : MSVehicle::Influencer::GapControlVehStateListener::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
268 : // std::cout << "GapControlVehStateListener::vehicleStateChanged() vehicle=" << vehicle->getID() << ", to=" << to << std::endl;
269 30 : switch (to) {
270 4 : case MSNet::VehicleState::STARTING_TELEPORT:
271 : case MSNet::VehicleState::ARRIVED:
272 : case MSNet::VehicleState::STARTING_PARKING: {
273 : // Vehicle left road
274 : // Look up reference vehicle in refVehMap and in case deactivate corresponding gap control
275 4 : const MSVehicle* msVeh = static_cast<const MSVehicle*>(vehicle);
276 : // std::cout << "GapControlVehStateListener::vehicleStateChanged() vehicle=" << vehicle->getID() << " left the road." << std::endl;
277 4 : if (GapControlState::refVehMap.find(msVeh) != end(GapControlState::refVehMap)) {
278 : // std::cout << "GapControlVehStateListener::deactivating ref vehicle=" << vehicle->getID() << std::endl;
279 4 : GapControlState::refVehMap[msVeh]->deactivate();
280 : }
281 : }
282 4 : break;
283 30 : default:
284 : {};
285 : // do nothing, vehicle still on road
286 : }
287 30 : }
288 :
289 : std::map<const MSVehicle*, MSVehicle::Influencer::GapControlState*>
290 : MSVehicle::Influencer::GapControlState::refVehMap;
291 :
292 : MSVehicle::Influencer::GapControlVehStateListener* MSVehicle::Influencer::GapControlState::myVehStateListener(nullptr);
293 :
294 58 : MSVehicle::Influencer::GapControlState::GapControlState() :
295 58 : tauOriginal(-1), tauCurrent(-1), tauTarget(-1), addGapCurrent(-1), addGapTarget(-1),
296 58 : remainingDuration(-1), changeRate(-1), maxDecel(-1), referenceVeh(nullptr), active(false), gapAttained(false), prevLeader(nullptr),
297 58 : lastUpdate(-1), timeHeadwayIncrement(0.0), spaceHeadwayIncrement(0.0) {}
298 :
299 :
300 58 : MSVehicle::Influencer::GapControlState::~GapControlState() {
301 58 : deactivate();
302 58 : }
303 :
304 : void
305 58 : MSVehicle::Influencer::GapControlState::init() {
306 58 : if (MSNet::hasInstance()) {
307 58 : if (myVehStateListener == nullptr) {
308 : //std::cout << "GapControlState::init()" << std::endl;
309 58 : myVehStateListener = new GapControlVehStateListener();
310 58 : MSNet::getInstance()->addVehicleStateListener(myVehStateListener);
311 : }
312 : } else {
313 0 : WRITE_ERROR("MSVehicle::Influencer::GapControlState::init(): No MSNet instance found!")
314 : }
315 58 : }
316 :
317 : void
318 34661 : MSVehicle::Influencer::GapControlState::cleanup() {
319 34661 : if (myVehStateListener != nullptr) {
320 58 : MSNet::getInstance()->removeVehicleStateListener(myVehStateListener);
321 58 : delete myVehStateListener;
322 58 : myVehStateListener = nullptr;
323 : }
324 34661 : }
325 :
326 : void
327 58 : MSVehicle::Influencer::GapControlState::activate(double tauOrig, double tauNew, double additionalGap, double dur, double rate, double decel, const MSVehicle* refVeh) {
328 58 : if (MSGlobals::gUseMesoSim) {
329 0 : WRITE_ERROR(TL("No gap control available for meso."))
330 : } else {
331 : // always deactivate control before activating (triggers clean-up of refVehMap)
332 : // std::cout << "activate gap control with refVeh=" << (refVeh==nullptr? "NULL" : refVeh->getID()) << std::endl;
333 58 : tauOriginal = tauOrig;
334 58 : tauCurrent = tauOrig;
335 58 : tauTarget = tauNew;
336 58 : addGapCurrent = 0.0;
337 58 : addGapTarget = additionalGap;
338 58 : remainingDuration = dur;
339 58 : changeRate = rate;
340 58 : maxDecel = decel;
341 58 : referenceVeh = refVeh;
342 58 : active = true;
343 58 : gapAttained = false;
344 58 : prevLeader = nullptr;
345 58 : lastUpdate = SIMSTEP - DELTA_T;
346 58 : timeHeadwayIncrement = changeRate * TS * (tauTarget - tauOriginal);
347 58 : spaceHeadwayIncrement = changeRate * TS * addGapTarget;
348 :
349 58 : if (referenceVeh != nullptr) {
350 : // Add refVeh to refVehMap
351 12 : GapControlState::refVehMap[referenceVeh] = this;
352 : }
353 : }
354 58 : }
355 :
356 : void
357 116 : MSVehicle::Influencer::GapControlState::deactivate() {
358 116 : active = false;
359 116 : if (referenceVeh != nullptr) {
360 : // Remove corresponding refVehMapEntry if appropriate
361 12 : GapControlState::refVehMap.erase(referenceVeh);
362 12 : referenceVeh = nullptr;
363 : }
364 116 : }
365 :
366 :
367 : /* -------------------------------------------------------------------------
368 : * methods of MSVehicle::Influencer
369 : * ----------------------------------------------------------------------- */
370 3483 : MSVehicle::Influencer::Influencer() :
371 : myGapControlState(nullptr),
372 3483 : myOriginalSpeed(-1),
373 3483 : myLatDist(0),
374 3483 : mySpeedAdaptationStarted(true),
375 3483 : myConsiderSafeVelocity(true),
376 3483 : myConsiderSpeedLimit(true),
377 3483 : myConsiderMaxAcceleration(true),
378 3483 : myConsiderMaxDeceleration(true),
379 3483 : myRespectJunctionPriority(true),
380 3483 : myEmergencyBrakeRedLight(true),
381 3483 : myRespectJunctionLeaderPriority(true),
382 3483 : myLastRemoteAccess(-TIME2STEPS(20)),
383 3483 : myStrategicLC(LC_NOCONFLICT),
384 3483 : myCooperativeLC(LC_NOCONFLICT),
385 3483 : mySpeedGainLC(LC_NOCONFLICT),
386 3483 : myRightDriveLC(LC_NOCONFLICT),
387 3483 : mySublaneLC(LC_NOCONFLICT),
388 3483 : myTraciLaneChangePriority(LCP_URGENT),
389 3483 : myTraCISignals(-1)
390 3483 : {}
391 :
392 :
393 10449 : MSVehicle::Influencer::~Influencer() {}
394 :
395 : void
396 58 : MSVehicle::Influencer::init() {
397 58 : GapControlState::init();
398 58 : }
399 :
400 : void
401 34661 : MSVehicle::Influencer::cleanup() {
402 34661 : GapControlState::cleanup();
403 34661 : }
404 :
405 : void
406 42904 : MSVehicle::Influencer::setSpeedTimeLine(const std::vector<std::pair<SUMOTime, double> >& speedTimeLine) {
407 42904 : mySpeedAdaptationStarted = true;
408 42904 : mySpeedTimeLine = speedTimeLine;
409 42904 : }
410 :
411 : void
412 58 : MSVehicle::Influencer::activateGapController(double originalTau, double newTimeHeadway, double newSpaceHeadway, double duration, double changeRate, double maxDecel, MSVehicle* refVeh) {
413 58 : if (myGapControlState == nullptr) {
414 58 : myGapControlState = std::make_shared<GapControlState>();
415 58 : init(); // only does things on first call
416 : }
417 58 : myGapControlState->activate(originalTau, newTimeHeadway, newSpaceHeadway, duration, changeRate, maxDecel, refVeh);
418 58 : }
419 :
420 : void
421 10 : MSVehicle::Influencer::deactivateGapController() {
422 10 : if (myGapControlState != nullptr && myGapControlState->active) {
423 10 : myGapControlState->deactivate();
424 : }
425 10 : }
426 :
427 : void
428 7801 : MSVehicle::Influencer::setLaneTimeLine(const std::vector<std::pair<SUMOTime, int> >& laneTimeLine) {
429 7801 : myLaneTimeLine = laneTimeLine;
430 7801 : }
431 :
432 :
433 : void
434 9045 : MSVehicle::Influencer::adaptLaneTimeLine(int indexShift) {
435 19211 : for (auto& item : myLaneTimeLine) {
436 10166 : item.second += indexShift;
437 : }
438 9045 : }
439 :
440 :
441 : void
442 1268 : MSVehicle::Influencer::setSublaneChange(double latDist) {
443 1268 : myLatDist = latDist;
444 1268 : }
445 :
446 : int
447 70 : MSVehicle::Influencer::getSpeedMode() const {
448 70 : return (1 * myConsiderSafeVelocity +
449 70 : 2 * myConsiderMaxAcceleration +
450 70 : 4 * myConsiderMaxDeceleration +
451 70 : 8 * myRespectJunctionPriority +
452 70 : 16 * myEmergencyBrakeRedLight +
453 70 : 32 * !myRespectJunctionLeaderPriority + // inverted!
454 70 : 64 * !myConsiderSpeedLimit // inverted!
455 70 : );
456 : }
457 :
458 :
459 : int
460 1471 : MSVehicle::Influencer::getLaneChangeMode() const {
461 1471 : return (1 * myStrategicLC +
462 1471 : 4 * myCooperativeLC +
463 1471 : 16 * mySpeedGainLC +
464 1471 : 64 * myRightDriveLC +
465 1471 : 256 * myTraciLaneChangePriority +
466 1471 : 1024 * mySublaneLC);
467 : }
468 :
469 : SUMOTime
470 60 : MSVehicle::Influencer::getLaneTimeLineDuration() {
471 : SUMOTime duration = -1;
472 180 : for (std::vector<std::pair<SUMOTime, int>>::iterator i = myLaneTimeLine.begin(); i != myLaneTimeLine.end(); ++i) {
473 120 : if (duration < 0) {
474 60 : duration = i->first;
475 : } else {
476 60 : duration -= i->first;
477 : }
478 : }
479 60 : return -duration;
480 : }
481 :
482 : SUMOTime
483 0 : MSVehicle::Influencer::getLaneTimeLineEnd() {
484 0 : if (!myLaneTimeLine.empty()) {
485 0 : return myLaneTimeLine.back().first;
486 : } else {
487 : return -1;
488 : }
489 : }
490 :
491 :
492 : double
493 992422 : MSVehicle::Influencer::influenceSpeed(SUMOTime currentTime, double speed, double vSafe, double vMin, double vMax) {
494 : // remove leading commands which are no longer valid
495 993822 : while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
496 : mySpeedTimeLine.erase(mySpeedTimeLine.begin());
497 : }
498 :
499 992422 : if (!(mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first)) {
500 : // Speed advice is active -> compute new speed according to speedTimeLine
501 53685 : if (!mySpeedAdaptationStarted) {
502 0 : mySpeedTimeLine[0].second = speed;
503 0 : mySpeedAdaptationStarted = true;
504 : }
505 53685 : currentTime += DELTA_T; // start slowing down in the step in which this command was issued (the input value of currentTime still reflects the previous step)
506 107029 : const double td = MIN2(1.0, STEPS2TIME(currentTime - mySpeedTimeLine[0].first) / MAX2(TS, STEPS2TIME(mySpeedTimeLine[1].first - mySpeedTimeLine[0].first)));
507 :
508 53685 : speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
509 53685 : if (myConsiderSafeVelocity) {
510 : speed = MIN2(speed, vSafe);
511 : }
512 53685 : if (myConsiderMaxAcceleration) {
513 : speed = MIN2(speed, vMax);
514 : }
515 53685 : if (myConsiderMaxDeceleration) {
516 : speed = MAX2(speed, vMin);
517 : }
518 : }
519 992422 : return speed;
520 : }
521 :
522 : double
523 493536 : MSVehicle::Influencer::gapControlSpeed(SUMOTime currentTime, const SUMOVehicle* veh, double speed, double vSafe, double vMin, double vMax) {
524 : #ifdef DEBUG_TRACI
525 : if DEBUG_COND2(veh) {
526 : std::cout << currentTime << " Influencer::gapControlSpeed(): speed=" << speed
527 : << ", vSafe=" << vSafe
528 : << ", vMin=" << vMin
529 : << ", vMax=" << vMax
530 : << std::endl;
531 : }
532 : #endif
533 : double gapControlSpeed = speed;
534 493536 : if (myGapControlState != nullptr && myGapControlState->active) {
535 : // Determine leader and the speed that would be chosen by the gap controller
536 7772 : const double currentSpeed = veh->getSpeed();
537 7772 : const MSVehicle* msVeh = dynamic_cast<const MSVehicle*>(veh);
538 : assert(msVeh != nullptr);
539 7772 : const double desiredTargetTimeSpacing = myGapControlState->tauTarget * currentSpeed;
540 : std::pair<const MSVehicle*, double> leaderInfo;
541 7772 : if (myGapControlState->referenceVeh == nullptr) {
542 : // No reference vehicle specified -> use current leader as reference
543 7340 : const double brakeGap = msVeh->getBrakeGap(true);
544 14680 : leaderInfo = msVeh->getLeader(MAX2(desiredTargetTimeSpacing, myGapControlState->addGapCurrent) + MAX2(brakeGap, 20.0));
545 : #ifdef DEBUG_TRACI
546 : if DEBUG_COND2(veh) {
547 : std::cout << " --- no refVeh; myGapControlState->addGapCurrent: " << myGapControlState->addGapCurrent << ", brakeGap: " << brakeGap << " in simstep: " << SIMSTEP << std::endl;
548 : }
549 : #endif
550 : } else {
551 : // Control gap wrt reference vehicle
552 : const MSVehicle* leader = myGapControlState->referenceVeh;
553 432 : double dist = msVeh->getDistanceToPosition(leader->getPositionOnLane(), leader->getLane()) - leader->getLength();
554 432 : if (dist > 100000) {
555 : // Reference vehicle was not found downstream the ego's route
556 : // Maybe, it is behind the ego vehicle
557 40 : dist = - leader->getDistanceToPosition(msVeh->getPositionOnLane(), msVeh->getLane()) - leader->getLength();
558 : #ifdef DEBUG_TRACI
559 : if DEBUG_COND2(veh) {
560 : if (dist < -100000) {
561 : // also the ego vehicle is not ahead of the reference vehicle -> no CF-relation
562 : std::cout << " Ego and reference vehicle are not in CF relation..." << std::endl;
563 : } else {
564 : std::cout << " Reference vehicle is behind ego..." << std::endl;
565 : }
566 : }
567 : #endif
568 : }
569 432 : leaderInfo = std::make_pair(leader, dist - msVeh->getVehicleType().getMinGap());
570 : }
571 7772 : const double fakeDist = MAX2(0.0, leaderInfo.second - myGapControlState->addGapCurrent);
572 : #ifdef DEBUG_TRACI
573 : if DEBUG_COND2(veh) {
574 : const double desiredCurrentSpacing = myGapControlState->tauCurrent * currentSpeed;
575 : std::cout << " Gap control active:"
576 : << " currentSpeed=" << currentSpeed
577 : << ", desiredTargetTimeSpacing=" << desiredTargetTimeSpacing
578 : << ", desiredCurrentSpacing=" << desiredCurrentSpacing
579 : << ", leader=" << (leaderInfo.first == nullptr ? "NULL" : leaderInfo.first->getID())
580 : << ", dist=" << leaderInfo.second
581 : << ", fakeDist=" << fakeDist
582 : << ",\n tauOriginal=" << myGapControlState->tauOriginal
583 : << ", tauTarget=" << myGapControlState->tauTarget
584 : << ", tauCurrent=" << myGapControlState->tauCurrent
585 : << std::endl;
586 : }
587 : #endif
588 7772 : if (leaderInfo.first != nullptr) {
589 : if (myGapControlState->prevLeader != nullptr && myGapControlState->prevLeader != leaderInfo.first) {
590 : // TODO: The leader changed. What to do?
591 : }
592 : // Remember leader
593 7772 : myGapControlState->prevLeader = leaderInfo.first;
594 :
595 : // Calculate desired following speed assuming the alternative headway time
596 7772 : MSCFModel* cfm = (MSCFModel*) & (msVeh->getVehicleType().getCarFollowModel());
597 7772 : const double origTau = cfm->getHeadwayTime();
598 7772 : cfm->setHeadwayTime(myGapControlState->tauCurrent);
599 7772 : gapControlSpeed = MIN2(gapControlSpeed,
600 7772 : cfm->followSpeed(msVeh, currentSpeed, fakeDist, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first));
601 7772 : cfm->setHeadwayTime(origTau);
602 : #ifdef DEBUG_TRACI
603 : if DEBUG_COND2(veh) {
604 : std::cout << " -> gapControlSpeed=" << gapControlSpeed;
605 : if (myGapControlState->maxDecel > 0) {
606 : std::cout << ", with maxDecel bound: " << MAX2(gapControlSpeed, currentSpeed - TS * myGapControlState->maxDecel);
607 : }
608 : std::cout << std::endl;
609 : }
610 : #endif
611 7772 : if (myGapControlState->maxDecel > 0) {
612 2568 : gapControlSpeed = MAX2(gapControlSpeed, currentSpeed - TS * myGapControlState->maxDecel);
613 : }
614 : }
615 :
616 : // Update gap controller
617 : // Check (1) if the gap control has established the desired gap,
618 : // and (2) if it has maintained active for the given duration afterwards
619 7772 : if (myGapControlState->lastUpdate < currentTime) {
620 : #ifdef DEBUG_TRACI
621 : if DEBUG_COND2(veh) {
622 : std::cout << " Updating GapControlState." << std::endl;
623 : }
624 : #endif
625 7772 : if (myGapControlState->tauCurrent == myGapControlState->tauTarget && myGapControlState->addGapCurrent == myGapControlState->addGapTarget) {
626 2990 : if (!myGapControlState->gapAttained) {
627 : // Check if the desired gap was established (add the POSITION_EPS to avoid infinite asymptotic behavior without having established the gap)
628 4176 : myGapControlState->gapAttained = leaderInfo.first == nullptr || leaderInfo.second > MAX2(desiredTargetTimeSpacing, myGapControlState->addGapTarget) - POSITION_EPS;
629 : #ifdef DEBUG_TRACI
630 : if DEBUG_COND2(veh) {
631 : if (myGapControlState->gapAttained) {
632 : std::cout << " Target gap was established." << std::endl;
633 : }
634 : }
635 : #endif
636 : } else {
637 : // Count down remaining time if desired gap was established
638 924 : myGapControlState->remainingDuration -= TS;
639 : #ifdef DEBUG_TRACI
640 : if DEBUG_COND2(veh) {
641 : std::cout << " Gap control remaining duration: " << myGapControlState->remainingDuration << std::endl;
642 : }
643 : #endif
644 924 : if (myGapControlState->remainingDuration <= 0) {
645 : #ifdef DEBUG_TRACI
646 : if DEBUG_COND2(veh) {
647 : std::cout << " Gap control duration expired, deactivating control." << std::endl;
648 : }
649 : #endif
650 : // switch off gap control
651 44 : myGapControlState->deactivate();
652 : }
653 : }
654 : } else {
655 : // Adjust current headway values
656 4782 : myGapControlState->tauCurrent = MIN2(myGapControlState->tauCurrent + myGapControlState->timeHeadwayIncrement, myGapControlState->tauTarget);
657 5160 : myGapControlState->addGapCurrent = MIN2(myGapControlState->addGapCurrent + myGapControlState->spaceHeadwayIncrement, myGapControlState->addGapTarget);
658 : }
659 : }
660 7772 : if (myConsiderSafeVelocity) {
661 : gapControlSpeed = MIN2(gapControlSpeed, vSafe);
662 : }
663 7772 : if (myConsiderMaxAcceleration) {
664 : gapControlSpeed = MIN2(gapControlSpeed, vMax);
665 : }
666 7772 : if (myConsiderMaxDeceleration) {
667 : gapControlSpeed = MAX2(gapControlSpeed, vMin);
668 : }
669 : return MIN2(speed, gapControlSpeed);
670 : } else {
671 : return speed;
672 : }
673 : }
674 :
675 : double
676 7138 : MSVehicle::Influencer::getOriginalSpeed() const {
677 7138 : return myOriginalSpeed;
678 : }
679 :
680 : void
681 498886 : MSVehicle::Influencer::setOriginalSpeed(double speed) {
682 498886 : myOriginalSpeed = speed;
683 498886 : }
684 :
685 :
686 : int
687 2827156 : MSVehicle::Influencer::influenceChangeDecision(const SUMOTime currentTime, const MSEdge& currentEdge, const int currentLaneIndex, int state) {
688 : // remove leading commands which are no longer valid
689 2827408 : while (myLaneTimeLine.size() == 1 || (myLaneTimeLine.size() > 1 && currentTime > myLaneTimeLine[1].first)) {
690 : myLaneTimeLine.erase(myLaneTimeLine.begin());
691 : }
692 : ChangeRequest changeRequest = REQUEST_NONE;
693 : // do nothing if the time line does not apply for the current time
694 2827156 : if (myLaneTimeLine.size() >= 2 && currentTime >= myLaneTimeLine[0].first) {
695 173425 : const int destinationLaneIndex = myLaneTimeLine[1].second;
696 173425 : if (destinationLaneIndex < (int)currentEdge.getLanes().size()) {
697 173133 : if (currentLaneIndex > destinationLaneIndex) {
698 : changeRequest = REQUEST_RIGHT;
699 172258 : } else if (currentLaneIndex < destinationLaneIndex) {
700 : changeRequest = REQUEST_LEFT;
701 : } else {
702 : changeRequest = REQUEST_HOLD;
703 : }
704 292 : } else if (currentEdge.getLanes().back()->getOpposite() != nullptr) { // change to opposite direction driving
705 : changeRequest = REQUEST_LEFT;
706 292 : state = state | LCA_TRACI;
707 : }
708 : }
709 : // check whether the current reason shall be canceled / overridden
710 2827156 : if ((state & LCA_WANTS_LANECHANGE_OR_STAY) != 0) {
711 : // flags for the current reason
712 : LaneChangeMode mode = LC_NEVER;
713 1601931 : if ((state & LCA_TRACI) != 0 && myLatDist != 0) {
714 : // security checks
715 2380 : if ((myTraciLaneChangePriority == LCP_ALWAYS)
716 552 : || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
717 2252 : state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
718 : }
719 : // continue sublane change manoeuvre
720 2380 : return state;
721 1599551 : } else if ((state & LCA_STRATEGIC) != 0) {
722 485744 : mode = myStrategicLC;
723 1113807 : } else if ((state & LCA_COOPERATIVE) != 0) {
724 90 : mode = myCooperativeLC;
725 1113717 : } else if ((state & LCA_SPEEDGAIN) != 0) {
726 41556 : mode = mySpeedGainLC;
727 1072161 : } else if ((state & LCA_KEEPRIGHT) != 0) {
728 6182 : mode = myRightDriveLC;
729 1065979 : } else if ((state & LCA_SUBLANE) != 0) {
730 1065977 : mode = mySublaneLC;
731 2 : } else if ((state & LCA_TRACI) != 0) {
732 : mode = LC_NEVER;
733 : } else {
734 0 : WRITE_WARNINGF(TL("Lane change model did not provide a reason for changing (state=%, time=%\n"), toString(state), time2string(currentTime));
735 : }
736 1599549 : if (mode == LC_NEVER) {
737 : // cancel all lcModel requests
738 : state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
739 42021 : state &= ~LCA_URGENT;
740 42021 : if (changeRequest == REQUEST_NONE) {
741 : // also remove all reasons except TRACI
742 41554 : state &= ~LCA_CHANGE_REASONS | LCA_TRACI;
743 : }
744 1557530 : } else if (mode == LC_NOCONFLICT && changeRequest != REQUEST_NONE) {
745 5623 : if (
746 5623 : ((state & LCA_LEFT) != 0 && changeRequest != REQUEST_LEFT) ||
747 5391 : ((state & LCA_RIGHT) != 0 && changeRequest != REQUEST_RIGHT) ||
748 4943 : ((state & LCA_STAY) != 0 && changeRequest != REQUEST_HOLD)) {
749 : // cancel conflicting lcModel request
750 : state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
751 827 : state &= ~LCA_URGENT;
752 : }
753 1551907 : } else if (mode == LC_ALWAYS) {
754 : // ignore any TraCI requests
755 : return state;
756 : }
757 : }
758 : // apply traci requests
759 2819575 : if (changeRequest == REQUEST_NONE) {
760 2651731 : return state;
761 : } else {
762 173035 : state |= LCA_TRACI;
763 : // security checks
764 173035 : if ((myTraciLaneChangePriority == LCP_ALWAYS)
765 171179 : || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
766 2250 : state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
767 : }
768 173035 : if (changeRequest != REQUEST_HOLD && myTraciLaneChangePriority != LCP_OPPORTUNISTIC) {
769 2094 : state |= LCA_URGENT;
770 : }
771 2123 : switch (changeRequest) {
772 : case REQUEST_HOLD:
773 170912 : return state | LCA_STAY;
774 1328 : case REQUEST_LEFT:
775 1328 : return state | LCA_LEFT;
776 795 : case REQUEST_RIGHT:
777 795 : return state | LCA_RIGHT;
778 : default:
779 : throw ProcessError(TL("should not happen"));
780 : }
781 : }
782 : }
783 :
784 :
785 : double
786 364 : MSVehicle::Influencer::changeRequestRemainingSeconds(const SUMOTime currentTime) const {
787 : assert(myLaneTimeLine.size() >= 2);
788 : assert(currentTime >= myLaneTimeLine[0].first);
789 364 : return STEPS2TIME(myLaneTimeLine[1].first - currentTime);
790 : }
791 :
792 :
793 : void
794 5062 : MSVehicle::Influencer::setSpeedMode(int speedMode) {
795 5062 : myConsiderSafeVelocity = ((speedMode & 1) != 0);
796 5062 : myConsiderMaxAcceleration = ((speedMode & 2) != 0);
797 5062 : myConsiderMaxDeceleration = ((speedMode & 4) != 0);
798 5062 : myRespectJunctionPriority = ((speedMode & 8) != 0);
799 5062 : myEmergencyBrakeRedLight = ((speedMode & 16) != 0);
800 5062 : myRespectJunctionLeaderPriority = ((speedMode & 32) == 0); // inverted!
801 5062 : myConsiderSpeedLimit = ((speedMode & 64) == 0); // inverted!
802 5062 : }
803 :
804 :
805 : void
806 18667 : MSVehicle::Influencer::setLaneChangeMode(int value) {
807 18667 : myStrategicLC = (LaneChangeMode)(value & (1 + 2));
808 18667 : myCooperativeLC = (LaneChangeMode)((value & (4 + 8)) >> 2);
809 18667 : mySpeedGainLC = (LaneChangeMode)((value & (16 + 32)) >> 4);
810 18667 : myRightDriveLC = (LaneChangeMode)((value & (64 + 128)) >> 6);
811 18667 : myTraciLaneChangePriority = (TraciLaneChangePriority)((value & (256 + 512)) >> 8);
812 18667 : mySublaneLC = (LaneChangeMode)((value & (1024 + 2048)) >> 10);
813 18667 : }
814 :
815 :
816 : void
817 6788 : MSVehicle::Influencer::setRemoteControlled(Position xyPos, MSLane* l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector& route, SUMOTime t) {
818 6788 : myRemoteXYPos = xyPos;
819 6788 : myRemoteLane = l;
820 6788 : myRemotePos = pos;
821 6788 : myRemotePosLat = posLat;
822 6788 : myRemoteAngle = angle;
823 6788 : myRemoteEdgeOffset = edgeOffset;
824 6788 : myRemoteRoute = route;
825 6788 : myLastRemoteAccess = t;
826 6788 : }
827 :
828 :
829 : bool
830 1021146 : MSVehicle::Influencer::isRemoteControlled() const {
831 1021146 : return myLastRemoteAccess == MSNet::getInstance()->getCurrentTimeStep();
832 : }
833 :
834 :
835 : bool
836 485454 : MSVehicle::Influencer::isRemoteAffected(SUMOTime t) const {
837 485454 : return myLastRemoteAccess >= t - TIME2STEPS(10);
838 : }
839 :
840 :
841 : void
842 493536 : MSVehicle::Influencer::updateRemoteControlRoute(MSVehicle* v) {
843 493536 : if (myRemoteRoute.size() != 0 && myRemoteRoute != v->getRoute().getEdges()) {
844 : // only replace route at this time if the vehicle is moving with the flow
845 54 : const bool isForward = v->getLane() != 0 && &v->getLane()->getEdge() == myRemoteRoute[0];
846 : #ifdef DEBUG_REMOTECONTROL
847 : std::cout << SIMSTEP << " updateRemoteControlRoute veh=" << v->getID() << " old=" << toString(v->getRoute().getEdges()) << " new=" << toString(myRemoteRoute) << " fwd=" << isForward << "\n";
848 : #endif
849 : if (isForward) {
850 8 : v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
851 8 : v->updateBestLanes();
852 : }
853 : }
854 493536 : }
855 :
856 :
857 : void
858 6768 : MSVehicle::Influencer::postProcessRemoteControl(MSVehicle* v) {
859 6768 : const bool wasOnRoad = v->isOnRoad();
860 6768 : const bool withinLane = myRemoteLane != nullptr && fabs(myRemotePosLat) < 0.5 * (myRemoteLane->getWidth() + v->getVehicleType().getWidth());
861 6768 : const bool keepLane = wasOnRoad && v->getLane() == myRemoteLane;
862 6768 : if (v->isOnRoad() && !(keepLane && withinLane)) {
863 128 : if (myRemoteLane != nullptr && &v->getLane()->getEdge() == &myRemoteLane->getEdge()) {
864 : // correct odometer which gets incremented via onRemovalFromNet->leaveLane
865 59 : v->myOdometer -= v->getLane()->getLength();
866 : }
867 128 : v->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT);
868 128 : v->getMutableLane()->removeVehicle(v, MSMoveReminder::NOTIFICATION_TELEPORT, false);
869 : }
870 6768 : if (myRemoteRoute.size() != 0 && myRemoteRoute != v->getRoute().getEdges()) {
871 : // needed for the insertion step
872 : #ifdef DEBUG_REMOTECONTROL
873 : std::cout << SIMSTEP << " postProcessRemoteControl veh=" << v->getID()
874 : << "\n oldLane=" << Named::getIDSecure(v->getLane())
875 : << " oldRoute=" << toString(v->getRoute().getEdges())
876 : << "\n newLane=" << Named::getIDSecure(myRemoteLane)
877 : << " newRoute=" << toString(myRemoteRoute)
878 : << " newRouteEdge=" << myRemoteRoute[myRemoteEdgeOffset]->getID()
879 : << "\n";
880 : #endif
881 : // clear any prior stops because they cannot apply to the new route
882 57 : const_cast<SUMOVehicleParameter&>(v->getParameter()).stops.clear();
883 114 : v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
884 : myRemoteRoute.clear();
885 : }
886 6768 : v->myCurrEdge = v->getRoute().begin() + myRemoteEdgeOffset;
887 6768 : if (myRemoteLane != nullptr && myRemotePos > myRemoteLane->getLength()) {
888 0 : myRemotePos = myRemoteLane->getLength();
889 : }
890 6768 : if (myRemoteLane != nullptr && withinLane) {
891 6625 : if (keepLane) {
892 : // TODO this handles only the case when the new vehicle is completely on the edge
893 6474 : const bool needFurtherUpdate = v->myState.myPos < v->getVehicleType().getLength() && myRemotePos >= v->getVehicleType().getLength();
894 6474 : v->myState.myPos = myRemotePos;
895 6474 : v->myState.myPosLat = myRemotePosLat;
896 6474 : if (needFurtherUpdate) {
897 4 : v->myState.myBackPos = v->updateFurtherLanes(v->myFurtherLanes, v->myFurtherLanesPosLat, std::vector<MSLane*>());
898 : }
899 : } else {
900 151 : MSMoveReminder::Notification notify = v->getDeparture() == NOT_YET_DEPARTED
901 151 : ? MSMoveReminder::NOTIFICATION_DEPARTED
902 : : MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED;
903 151 : if (!v->isOnRoad()) {
904 151 : MSVehicleTransfer::getInstance()->remove(v); // TODO may need optimization, this is linear in the number of vehicles in transfer
905 : }
906 151 : myRemoteLane->forceVehicleInsertion(v, myRemotePos, notify, myRemotePosLat);
907 151 : v->updateBestLanes();
908 : }
909 6625 : if (!wasOnRoad) {
910 52 : v->drawOutsideNetwork(false);
911 : }
912 : //std::cout << "on road network p=" << myRemoteXYPos << " a=" << myRemoteAngle << " l=" << Named::getIDSecure(myRemoteLane) << " pos=" << myRemotePos << " posLat=" << myRemotePosLat << "\n";
913 6625 : myRemoteLane->requireCollisionCheck();
914 : } else {
915 143 : if (v->getDeparture() == NOT_YET_DEPARTED) {
916 3 : v->onDepart();
917 : }
918 143 : v->drawOutsideNetwork(true);
919 : // see updateState
920 143 : double vNext = v->processTraCISpeedControl(
921 143 : v->getMaxSpeed(), v->getSpeed());
922 143 : v->setBrakingSignals(vNext);
923 143 : v->myState.myPreviousSpeed = v->getSpeed();
924 143 : v->myAcceleration = SPEED2ACCEL(vNext - v->getSpeed());
925 143 : v->myState.mySpeed = vNext;
926 143 : v->updateWaitingTime(vNext);
927 : //std::cout << "outside network p=" << myRemoteXYPos << " a=" << myRemoteAngle << " l=" << Named::getIDSecure(myRemoteLane) << "\n";
928 : }
929 : // ensure that the position is correct (i.e. when the lanePosition is ambiguous at corners)
930 6768 : v->setRemoteState(myRemoteXYPos);
931 6768 : v->setAngle(GeomHelper::fromNaviDegree(myRemoteAngle));
932 6768 : }
933 :
934 :
935 : double
936 6749 : MSVehicle::Influencer::implicitSpeedRemote(const MSVehicle* veh, double oldSpeed) {
937 6749 : if (veh->getPosition() == Position::INVALID) {
938 7 : return oldSpeed;
939 : }
940 6742 : double dist = veh->getPosition().distanceTo2D(myRemoteXYPos);
941 6742 : if (myRemoteLane != nullptr) {
942 : // if the vehicles is frequently placed on a new edge, the route may
943 : // consist only of a single edge. In this case the new edge may not be
944 : // on the route so distAlongRoute will be double::max.
945 : // In this case we still want a sensible speed value
946 6629 : const double distAlongRoute = veh->getDistanceToPosition(myRemotePos, myRemoteLane);
947 6629 : if (distAlongRoute != std::numeric_limits<double>::max()) {
948 : dist = distAlongRoute;
949 : }
950 : }
951 : //std::cout << SIMTIME << " veh=" << veh->getID() << " oldPos=" << veh->getPosition() << " traciPos=" << myRemoteXYPos << " dist=" << dist << "\n";
952 6742 : const double minSpeed = myConsiderMaxDeceleration ?
953 4034 : veh->getCarFollowModel().minNextSpeedEmergency(oldSpeed, veh) : 0;
954 6742 : const double maxSpeed = (myRemoteLane != nullptr
955 6742 : ? myRemoteLane->getVehicleMaxSpeed(veh)
956 113 : : (veh->getLane() != nullptr
957 113 : ? veh->getLane()->getVehicleMaxSpeed(veh)
958 3 : : veh->getMaxSpeed()));
959 6742 : return MIN2(maxSpeed, MAX2(minSpeed, DIST2SPEED(dist)));
960 : }
961 :
962 :
963 : double
964 6614 : MSVehicle::Influencer::implicitDeltaPosRemote(const MSVehicle* veh) {
965 : double dist = 0;
966 6614 : if (myRemoteLane == nullptr) {
967 5 : dist = veh->getPosition().distanceTo2D(myRemoteXYPos);
968 : } else {
969 : // if the vehicles is frequently placed on a new edge, the route may
970 : // consist only of a single edge. In this case the new edge may not be
971 : // on the route so getDistanceToPosition will return double::max.
972 : // In this case we would rather not move the vehicle in executeMove
973 : // (updateState) as it would result in emergency braking
974 6609 : dist = veh->getDistanceToPosition(myRemotePos, myRemoteLane);
975 : }
976 6614 : if (dist == std::numeric_limits<double>::max()) {
977 : return 0;
978 : } else {
979 6414 : if (DIST2SPEED(dist) > veh->getMaxSpeed() * 1.1) {
980 42 : WRITE_WARNINGF(TL("Vehicle '%' moved by TraCI from % to % (dist %) with implied speed of % (exceeding maximum speed %). time=%."),
981 : veh->getID(), veh->getPosition(), myRemoteXYPos, dist, DIST2SPEED(dist), veh->getMaxSpeed(), time2string(SIMSTEP));
982 : // some sanity check here
983 14 : dist = MIN2(dist, SPEED2DIST(veh->getMaxSpeed() * 2));
984 : }
985 6414 : return dist;
986 : }
987 : }
988 :
989 :
990 : /* -------------------------------------------------------------------------
991 : * MSVehicle-methods
992 : * ----------------------------------------------------------------------- */
993 4794096 : MSVehicle::MSVehicle(SUMOVehicleParameter* pars, ConstMSRoutePtr route,
994 4794096 : MSVehicleType* type, const double speedFactor) :
995 : MSBaseVehicle(pars, route, type, speedFactor),
996 4794096 : myWaitingTime(0),
997 4794096 : myWaitingTimeCollector(),
998 4794096 : myTimeLoss(0),
999 4794096 : myState(0, 0, 0, 0, 0),
1000 4794096 : myDriverState(nullptr),
1001 4794096 : myActionStep(true),
1002 4794096 : myLastActionTime(0),
1003 4794096 : myLane(nullptr),
1004 4794096 : myLaneChangeModel(nullptr),
1005 4794096 : myLastBestLanesEdge(nullptr),
1006 4794096 : myLastBestLanesInternalLane(nullptr),
1007 4794096 : myAcceleration(0),
1008 : myNextTurn(0., nullptr),
1009 4794096 : mySignals(0),
1010 4794096 : myAmOnNet(false),
1011 4794096 : myAmIdling(false),
1012 4794096 : myHaveToWaitOnNextLink(false),
1013 4794096 : myAngle(0),
1014 4794096 : myStopDist(std::numeric_limits<double>::max()),
1015 4794096 : myStopSpeed(std::numeric_limits<double>::max()),
1016 4794096 : myCollisionImmunity(-1),
1017 4794096 : myCachedPosition(Position::INVALID),
1018 4794096 : myJunctionEntryTime(SUMOTime_MAX),
1019 4794096 : myJunctionEntryTimeNeverYield(SUMOTime_MAX),
1020 4794096 : myJunctionConflictEntryTime(SUMOTime_MAX),
1021 4794096 : myTimeSinceStartup(TIME2STEPS(3600 * 24)),
1022 4794096 : myHaveStoppedFor(nullptr),
1023 14382288 : myInfluencer(nullptr) {
1024 4794096 : myCFVariables = type->getCarFollowModel().createVehicleVariables();
1025 4794096 : myNextDriveItem = myLFLinkLanes.begin();
1026 4794096 : }
1027 :
1028 :
1029 8950310 : MSVehicle::~MSVehicle() {
1030 4794022 : cleanupParkingReservation();
1031 4794022 : cleanupFurtherLanes();
1032 4794022 : delete myLaneChangeModel;
1033 4794022 : if (myType->isVehicleSpecific()) {
1034 320 : MSNet::getInstance()->getVehicleControl().removeVType(myType);
1035 : }
1036 4794022 : delete myInfluencer;
1037 4794022 : delete myCFVariables;
1038 13744332 : }
1039 :
1040 :
1041 : void
1042 4794584 : MSVehicle::cleanupFurtherLanes() {
1043 4797082 : for (MSLane* further : myFurtherLanes) {
1044 2498 : further->resetPartialOccupation(this);
1045 2498 : if (further->getBidiLane() != nullptr
1046 2498 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
1047 0 : further->getBidiLane()->resetPartialOccupation(this);
1048 : }
1049 : }
1050 4794584 : if (myLaneChangeModel != nullptr) {
1051 4794550 : removeApproachingInformation(myLFLinkLanes);
1052 4794550 : myLaneChangeModel->cleanupShadowLane();
1053 4794550 : myLaneChangeModel->cleanupTargetLane();
1054 : // still needed when calling resetPartialOccupation (getShadowLane) and when removing
1055 : // approach information from parallel links
1056 : }
1057 : myFurtherLanes.clear();
1058 : myFurtherLanesPosLat.clear();
1059 4794584 : }
1060 :
1061 :
1062 : void
1063 3626749 : MSVehicle::onRemovalFromNet(const MSMoveReminder::Notification reason) {
1064 : #ifdef DEBUG_ACTIONSTEPS
1065 : if (DEBUG_COND) {
1066 : std::cout << SIMTIME << " Removing vehicle '" << getID() << "' (reason: " << toString(reason) << ")" << std::endl;
1067 : }
1068 : #endif
1069 3626749 : MSVehicleTransfer::getInstance()->remove(this);
1070 3626749 : removeApproachingInformation(myLFLinkLanes);
1071 3626749 : leaveLane(reason);
1072 3626749 : if (reason == MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION) {
1073 562 : cleanupFurtherLanes();
1074 : }
1075 3626749 : }
1076 :
1077 :
1078 : void
1079 4794096 : MSVehicle::initDevices() {
1080 4794096 : MSBaseVehicle::initDevices();
1081 4794084 : myLaneChangeModel = MSAbstractLaneChangeModel::build(myType->getLaneChangeModel(), *this);
1082 4794062 : myDriverState = static_cast<MSDevice_DriverState*>(getDevice(typeid(MSDevice_DriverState)));
1083 4794062 : myFrictionDevice = static_cast<MSDevice_Friction*>(getDevice(typeid(MSDevice_Friction)));
1084 4794062 : }
1085 :
1086 :
1087 : // ------------ interaction with the route
1088 : bool
1089 7053333334 : MSVehicle::hasValidRouteStart(std::string& msg) {
1090 : // note: not a const method because getDepartLane may call updateBestLanes
1091 7053333334 : if (!(*myCurrEdge)->isTazConnector()) {
1092 7052893144 : if (myParameter->departLaneProcedure == DepartLaneDefinition::GIVEN) {
1093 50430865 : if ((*myCurrEdge)->getDepartLane(*this) == nullptr) {
1094 122 : msg = "Invalid departlane definition for vehicle '" + getID() + "'.";
1095 61 : if (myParameter->departLane >= (int)(*myCurrEdge)->getLanes().size()) {
1096 11 : myRouteValidity |= ROUTE_START_INVALID_LANE;
1097 : } else {
1098 50 : myRouteValidity |= ROUTE_START_INVALID_PERMISSIONS;
1099 : }
1100 61 : return false;
1101 : }
1102 : } else {
1103 7002462279 : if ((*myCurrEdge)->allowedLanes(getVClass(), ignoreTransientPermissions()) == nullptr) {
1104 144 : msg = "Vehicle '" + getID() + "' is not allowed to depart on any lane of edge '" + (*myCurrEdge)->getID() + "'.";
1105 72 : myRouteValidity |= ROUTE_START_INVALID_PERMISSIONS;
1106 72 : return false;
1107 : }
1108 : }
1109 7052893011 : if (myParameter->departSpeedProcedure == DepartSpeedDefinition::GIVEN && myParameter->departSpeed > myType->getMaxSpeed() + SPEED_EPS) {
1110 38 : msg = "Departure speed for vehicle '" + getID() + "' is too high for the vehicle type '" + myType->getID() + "'.";
1111 19 : myRouteValidity |= ROUTE_START_INVALID_LANE;
1112 19 : return false;
1113 : }
1114 : }
1115 7053333182 : myRouteValidity &= ~(ROUTE_START_INVALID_LANE | ROUTE_START_INVALID_PERMISSIONS);
1116 7053333182 : return true;
1117 : }
1118 :
1119 :
1120 : bool
1121 718739148 : MSVehicle::hasArrived() const {
1122 718739148 : return hasArrivedInternal(false);
1123 : }
1124 :
1125 :
1126 : bool
1127 1446752940 : MSVehicle::hasArrivedInternal(bool oppositeTransformed) const {
1128 2349820073 : return ((myCurrEdge == myRoute->end() - 1 || (myParameter->arrivalEdge >= 0 && getRoutePosition() >= myParameter->arrivalEdge))
1129 543734896 : && (myStops.empty() || myStops.front().edge != myCurrEdge || myStops.front().getSpeed() > 0)
1130 1018518210 : && ((myLaneChangeModel->isOpposite() && !oppositeTransformed) ? myLane->getLength() - myState.myPos : myState.myPos) > MIN2(myLane->getLength(), myArrivalPos) - POSITION_EPS
1131 1459230831 : && !isRemoteControlled());
1132 : }
1133 :
1134 :
1135 : bool
1136 1732308 : MSVehicle::replaceRoute(ConstMSRoutePtr newRoute, const std::string& info, bool onInit, int offset, bool addRouteStops, bool removeStops, std::string* msgReturn) {
1137 3464616 : if (MSBaseVehicle::replaceRoute(newRoute, info, onInit, offset, addRouteStops, removeStops, msgReturn)) {
1138 : // update best lanes (after stops were added)
1139 1732290 : myLastBestLanesEdge = nullptr;
1140 1732290 : myLastBestLanesInternalLane = nullptr;
1141 1732290 : updateBestLanes(true, onInit ? (*myCurrEdge)->getLanes().front() : 0);
1142 : assert(!removeStops || haveValidStopEdges());
1143 1732290 : if (myStops.size() == 0) {
1144 1688255 : myStopDist = std::numeric_limits<double>::max();
1145 : }
1146 1732290 : return true;
1147 : }
1148 : return false;
1149 : }
1150 :
1151 :
1152 : // ------------ Interaction with move reminders
1153 : void
1154 704369264 : MSVehicle::workOnMoveReminders(double oldPos, double newPos, double newSpeed) {
1155 : // This erasure-idiom works for all stl-sequence-containers
1156 : // See Meyers: Effective STL, Item 9
1157 1866393418 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
1158 : // XXX: calling notifyMove with newSpeed seems not the best choice. For the ballistic update, the average speed is calculated and used
1159 : // although a higher order quadrature-formula might be more adequate.
1160 : // For the euler case (where the speed is considered constant for each time step) it is conceivable that
1161 : // the current calculations may lead to systematic errors for large time steps (compared to reality). Refs. #2579
1162 2324048308 : if (!rem->first->notifyMove(*this, oldPos + rem->second, newPos + rem->second, MAX2(0., newSpeed))) {
1163 : #ifdef _DEBUG
1164 : if (myTraceMoveReminders) {
1165 : traceMoveReminder("notifyMove", rem->first, rem->second, false);
1166 : }
1167 : #endif
1168 : rem = myMoveReminders.erase(rem);
1169 : } else {
1170 : #ifdef _DEBUG
1171 : if (myTraceMoveReminders) {
1172 : traceMoveReminder("notifyMove", rem->first, rem->second, true);
1173 : }
1174 : #endif
1175 : ++rem;
1176 : }
1177 : }
1178 704369264 : if (myEnergyParams != nullptr) {
1179 : // TODO make the vehicle energy params a derived class which is a move reminder
1180 139368109 : myEnergyParams->setDynamicValues(isStopped() ? getNextStop().duration : -1, isParking(), getWaitingTime(), getAngle());
1181 : }
1182 704369264 : }
1183 :
1184 :
1185 : void
1186 118709 : MSVehicle::workOnIdleReminders() {
1187 118709 : updateWaitingTime(0.); // cf issue 2233
1188 :
1189 : // vehicle move reminders
1190 145241 : for (const auto& rem : myMoveReminders) {
1191 26532 : rem.first->notifyIdle(*this);
1192 : }
1193 :
1194 : // lane move reminders - for aggregated values
1195 282068 : for (MSMoveReminder* rem : getLane()->getMoveReminders()) {
1196 163359 : rem->notifyIdle(*this);
1197 : }
1198 118709 : }
1199 :
1200 : // XXX: consider renaming...
1201 : void
1202 20511726 : MSVehicle::adaptLaneEntering2MoveReminder(const MSLane& enteredLane) {
1203 : // save the old work reminders, patching the position information
1204 : // add the information about the new offset to the old lane reminders
1205 20511726 : const double oldLaneLength = myLane->getLength();
1206 57903612 : for (auto& rem : myMoveReminders) {
1207 37391886 : rem.second += oldLaneLength;
1208 : #ifdef _DEBUG
1209 : // if (rem->first==0) std::cout << "Null reminder (?!)" << std::endl;
1210 : // std::cout << "Adapted MoveReminder on lane " << ((rem->first->getLane()==0) ? "NULL" : rem->first->getLane()->getID()) <<" position to " << rem->second << std::endl;
1211 : if (myTraceMoveReminders) {
1212 : traceMoveReminder("adaptedPos", rem.first, rem.second, true);
1213 : }
1214 : #endif
1215 : }
1216 35127711 : for (MSMoveReminder* const rem : enteredLane.getMoveReminders()) {
1217 14615985 : addReminder(rem);
1218 : }
1219 20511726 : }
1220 :
1221 :
1222 : // ------------ Other getter methods
1223 : double
1224 163187021 : MSVehicle::getSlope() const {
1225 163187021 : if (isParking() && getStops().begin()->parkingarea != nullptr) {
1226 3881 : return getStops().begin()->parkingarea->getVehicleSlope(*this);
1227 : }
1228 163183140 : if (myLane == nullptr) {
1229 : return 0;
1230 : }
1231 163183140 : const double posLat = myState.myPosLat; // @todo get rid of the '-'
1232 163183140 : Position p1 = getPosition();
1233 163183140 : Position p2 = getBackPosition();
1234 : if (p2 == Position::INVALID) {
1235 : // Handle special case of vehicle's back reaching out of the network
1236 6 : if (myFurtherLanes.size() > 0) {
1237 6 : p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1238 : if (p2 == Position::INVALID) {
1239 : // unsuitable lane geometry
1240 0 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1241 : }
1242 : } else {
1243 0 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1244 : }
1245 : }
1246 163183140 : return (p1 != p2 ? RAD2DEG(p2.slopeTo2D(p1)) : myLane->getShape().slopeDegreeAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1247 : }
1248 :
1249 :
1250 : Position
1251 929653700 : MSVehicle::getPosition(const double offset) const {
1252 929653700 : if (myLane == nullptr) {
1253 : // when called in the context of GUI-Drawing, the simulation step is already incremented
1254 133 : if (myInfluencer != nullptr && myInfluencer->isRemoteAffected(MSNet::getInstance()->getCurrentTimeStep())) {
1255 30 : return myCachedPosition;
1256 : } else {
1257 103 : return Position::INVALID;
1258 : }
1259 : }
1260 929653567 : if (isParking()) {
1261 1149902 : if (myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() > getNextStopParameter()->started) {
1262 122 : return myCachedPosition;
1263 : }
1264 1149780 : if (myStops.begin()->parkingarea != nullptr) {
1265 21225 : return myStops.begin()->parkingarea->getVehiclePosition(*this);
1266 : } else {
1267 : // position beside the road
1268 1128555 : PositionVector shp = myLane->getEdge().getLanes()[0]->getShape();
1269 2256990 : shp.move2side(SUMO_const_laneWidth * (MSGlobals::gLefthand ? -1 : 1));
1270 1128555 : return shp.positionAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane() + offset));
1271 1128555 : }
1272 : }
1273 928503665 : const bool changingLanes = myLaneChangeModel->isChangingLanes();
1274 1846797122 : const double posLat = (MSGlobals::gLefthand ? 1 : -1) * getLateralPositionOnLane();
1275 928503665 : if (offset == 0. && !changingLanes) {
1276 : if (myCachedPosition == Position::INVALID) {
1277 708831263 : myCachedPosition = validatePosition(myLane->geometryPositionAtOffset(myState.myPos, posLat));
1278 708831263 : if (MSNet::getInstance()->hasElevation() && MSGlobals::gSublane) {
1279 60962 : interpolateLateralZ(myCachedPosition, myState.myPos, posLat);
1280 : }
1281 : }
1282 921870467 : return myCachedPosition;
1283 : }
1284 6633198 : Position result = validatePosition(myLane->geometryPositionAtOffset(getPositionOnLane() + offset, posLat), offset);
1285 6633198 : interpolateLateralZ(result, getPositionOnLane() + offset, posLat);
1286 6633198 : return result;
1287 : }
1288 :
1289 :
1290 : void
1291 6978038 : MSVehicle::interpolateLateralZ(Position& pos, double offset, double posLat) const {
1292 6978038 : const MSLane* shadow = myLaneChangeModel->getShadowLane();
1293 6978038 : if (shadow != nullptr && pos != Position::INVALID) {
1294 : // ignore negative offset
1295 : const Position shadowPos = shadow->geometryPositionAtOffset(MAX2(0.0, offset));
1296 58859 : if (shadowPos != Position::INVALID && pos.z() != shadowPos.z()) {
1297 325 : const double centerDist = (myLane->getWidth() + shadow->getWidth()) * 0.5;
1298 325 : double relOffset = fabs(posLat) / centerDist;
1299 325 : double newZ = (1 - relOffset) * pos.z() + relOffset * shadowPos.z();
1300 : pos.setz(newZ);
1301 : }
1302 : }
1303 6978038 : }
1304 :
1305 :
1306 : double
1307 93474 : MSVehicle::getDistanceToLeaveJunction() const {
1308 93474 : double result = getLength() - getPositionOnLane();
1309 93474 : if (myLane->isNormal()) {
1310 : return MAX2(0.0, result);
1311 : }
1312 376 : const MSLane* lane = myLane;
1313 752 : while (lane->isInternal()) {
1314 376 : result += lane->getLength();
1315 376 : lane = lane->getCanonicalSuccessorLane();
1316 : }
1317 : return result;
1318 : }
1319 :
1320 :
1321 : Position
1322 104873 : MSVehicle::getPositionAlongBestLanes(double offset) const {
1323 : assert(MSGlobals::gUsingInternalLanes);
1324 104873 : if (!isOnRoad()) {
1325 0 : return Position::INVALID;
1326 : }
1327 104873 : const std::vector<MSLane*>& bestLanes = getBestLanesContinuation();
1328 : auto nextBestLane = bestLanes.begin();
1329 104873 : const bool opposite = myLaneChangeModel->isOpposite();
1330 104873 : double pos = opposite ? myLane->getLength() - myState.myPos : myState.myPos;
1331 104873 : const MSLane* lane = opposite ? myLane->getParallelOpposite() : getLane();
1332 : assert(lane != 0);
1333 : bool success = true;
1334 :
1335 309716 : while (offset > 0) {
1336 : // take into account lengths along internal lanes
1337 313086 : while (lane->isInternal() && offset > 0) {
1338 108243 : if (offset > lane->getLength() - pos) {
1339 3561 : offset -= lane->getLength() - pos;
1340 3561 : lane = lane->getLinkCont()[0]->getViaLaneOrLane();
1341 : pos = 0.;
1342 3561 : if (lane == nullptr) {
1343 : success = false;
1344 : offset = 0.;
1345 : }
1346 : } else {
1347 104682 : pos += offset;
1348 : offset = 0;
1349 : }
1350 : }
1351 : // set nextBestLane to next non-internal lane
1352 209970 : while (nextBestLane != bestLanes.end() && *nextBestLane == nullptr) {
1353 : ++nextBestLane;
1354 : }
1355 204843 : if (offset > 0) {
1356 : assert(!lane->isInternal());
1357 : assert(lane == *nextBestLane);
1358 100161 : if (offset > lane->getLength() - pos) {
1359 99978 : offset -= lane->getLength() - pos;
1360 : ++nextBestLane;
1361 : assert(nextBestLane == bestLanes.end() || *nextBestLane != 0);
1362 99978 : if (nextBestLane == bestLanes.end()) {
1363 : success = false;
1364 : offset = 0.;
1365 : } else {
1366 99978 : const MSLink* link = lane->getLinkTo(*nextBestLane);
1367 : assert(link != nullptr);
1368 : lane = link->getViaLaneOrLane();
1369 : pos = 0.;
1370 : }
1371 : } else {
1372 183 : pos += offset;
1373 : offset = 0;
1374 : }
1375 : }
1376 :
1377 : }
1378 :
1379 104873 : if (success) {
1380 104873 : return lane->geometryPositionAtOffset(pos, -getLateralPositionOnLane());
1381 : } else {
1382 0 : return Position::INVALID;
1383 : }
1384 : }
1385 :
1386 :
1387 : double
1388 716852 : MSVehicle::getMaxSpeedOnLane() const {
1389 716852 : if (myLane != nullptr) {
1390 716852 : return myLane->getVehicleMaxSpeed(this);
1391 : }
1392 0 : return myType->getMaxSpeed();
1393 : }
1394 :
1395 :
1396 : Position
1397 715464461 : MSVehicle::validatePosition(Position result, double offset) const {
1398 : int furtherIndex = 0;
1399 715464461 : double lastLength = getPositionOnLane();
1400 715464461 : while (result == Position::INVALID) {
1401 269638 : if (furtherIndex >= (int)myFurtherLanes.size()) {
1402 : //WRITE_WARNINGF(TL("Could not compute position for vehicle '%', time=%."), getID(), time2string(MSNet::getInstance()->getCurrentTimeStep()));
1403 : break;
1404 : }
1405 : //std::cout << SIMTIME << " veh=" << getID() << " lane=" << myLane->getID() << " pos=" << getPositionOnLane() << " posLat=" << getLateralPositionOnLane() << " offset=" << offset << " result=" << result << " i=" << furtherIndex << " further=" << myFurtherLanes.size() << "\n";
1406 190872 : MSLane* further = myFurtherLanes[furtherIndex];
1407 190872 : offset += lastLength;
1408 190872 : result = further->geometryPositionAtOffset(further->getLength() + offset, -getLateralPositionOnLane());
1409 : lastLength = further->getLength();
1410 190872 : furtherIndex++;
1411 : //std::cout << SIMTIME << " newResult=" << result << "\n";
1412 : }
1413 715464461 : return result;
1414 : }
1415 :
1416 :
1417 : ConstMSEdgeVector::const_iterator
1418 282891 : MSVehicle::getRerouteOrigin() const {
1419 : // too close to the next junction, so avoid an emergency brake here
1420 282891 : if (myLane != nullptr && (myCurrEdge + 1) != myRoute->end() && !isRailway(getVClass())) {
1421 220774 : if (myLane->isInternal()) {
1422 : return myCurrEdge + 1;
1423 : }
1424 213687 : if (myState.myPos > myLane->getLength() - getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.)) {
1425 : return myCurrEdge + 1;
1426 : }
1427 211291 : if (myLane->getEdge().hasChangeProhibitions(getVClass(), myLane->getIndex())) {
1428 : return myCurrEdge + 1;
1429 : }
1430 : }
1431 273296 : return myCurrEdge;
1432 : }
1433 :
1434 : void
1435 5191537 : MSVehicle::setAngle(double angle, bool straightenFurther) {
1436 : #ifdef DEBUG_FURTHER
1437 : if (DEBUG_COND) {
1438 : std::cout << SIMTIME << " veh '" << getID() << " setAngle(" << angle << ") straightenFurther=" << straightenFurther << std::endl;
1439 : }
1440 : #endif
1441 5191537 : myAngle = angle;
1442 5191537 : MSLane* next = myLane;
1443 5191537 : if (straightenFurther && myFurtherLanesPosLat.size() > 0) {
1444 197359 : for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
1445 101368 : MSLane* further = myFurtherLanes[i];
1446 101368 : const MSLink* link = further->getLinkTo(next);
1447 101368 : if (link != nullptr) {
1448 100937 : myFurtherLanesPosLat[i] = getLateralPositionOnLane() - link->getLateralShift();
1449 : next = further;
1450 : } else {
1451 : break;
1452 : }
1453 : }
1454 : }
1455 5191537 : }
1456 :
1457 :
1458 : void
1459 451815 : MSVehicle::setActionStepLength(double actionStepLength, bool resetOffset) {
1460 451815 : SUMOTime actionStepLengthMillisecs = SUMOVehicleParserHelper::processActionStepLength(actionStepLength);
1461 : SUMOTime previousActionStepLength = getActionStepLength();
1462 : const bool newActionStepLength = actionStepLengthMillisecs != previousActionStepLength;
1463 451815 : if (newActionStepLength) {
1464 7 : getSingularType().setActionStepLength(actionStepLengthMillisecs, resetOffset);
1465 7 : if (!resetOffset) {
1466 1 : updateActionOffset(previousActionStepLength, actionStepLengthMillisecs);
1467 : }
1468 : }
1469 451809 : if (resetOffset) {
1470 6 : resetActionOffset();
1471 : }
1472 451815 : }
1473 :
1474 :
1475 : bool
1476 300265858 : MSVehicle::congested() const {
1477 300265858 : return myState.mySpeed < (60.0 / 3.6) || myLane->getSpeedLimit() < (60.1 / 3.6);
1478 : }
1479 :
1480 :
1481 : double
1482 711498322 : MSVehicle::computeAngle() const {
1483 : Position p1;
1484 711498322 : const double posLat = -myState.myPosLat; // @todo get rid of the '-'
1485 711498322 : const double lefthandSign = (MSGlobals::gLefthand ? -1 : 1);
1486 :
1487 : // if parking manoeuvre is happening then rotate vehicle on each step
1488 711498322 : if (MSGlobals::gModelParkingManoeuver && !manoeuvreIsComplete()) {
1489 450 : return getAngle() + myManoeuvre.getGUIIncrement();
1490 : }
1491 :
1492 711497872 : if (isParking()) {
1493 27610 : if (myStops.begin()->parkingarea != nullptr) {
1494 15644 : return myStops.begin()->parkingarea->getVehicleAngle(*this);
1495 : } else {
1496 11966 : return myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane()));
1497 : }
1498 : }
1499 711470262 : if (myLaneChangeModel->isChangingLanes()) {
1500 : // cannot use getPosition() because it already includes the offset to the side and thus messes up the angle
1501 1126847 : p1 = myLane->geometryPositionAtOffset(myState.myPos, lefthandSign * posLat);
1502 12 : if (p1 == Position::INVALID && myLane->getShape().length2D() == 0. && myLane->isInternal()) {
1503 : // workaround: extrapolate the preceding lane shape
1504 12 : MSLane* predecessorLane = myLane->getCanonicalPredecessorLane();
1505 12 : p1 = predecessorLane->geometryPositionAtOffset(predecessorLane->getLength() + myState.myPos, lefthandSign * posLat);
1506 : }
1507 : } else {
1508 710343415 : p1 = getPosition();
1509 : }
1510 :
1511 : Position p2;
1512 711470262 : if (getVehicleType().getParameter().locomotiveLength > 0) {
1513 : // articulated vehicle should use the heading of the first part
1514 1745044 : const double locoLength = MIN2(getVehicleType().getParameter().locomotiveLength, getLength());
1515 1745044 : p2 = getPosition(-locoLength);
1516 : } else {
1517 709725218 : p2 = getBackPosition();
1518 : }
1519 : if (p2 == Position::INVALID) {
1520 : // Handle special case of vehicle's back reaching out of the network
1521 857 : if (myFurtherLanes.size() > 0) {
1522 137 : p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1523 : if (p2 == Position::INVALID) {
1524 : // unsuitable lane geometry
1525 89 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1526 : }
1527 : } else {
1528 720 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1529 : }
1530 : }
1531 : double result = (p1 != p2 ? p2.angleTo2D(p1) :
1532 89777 : myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1533 :
1534 711470262 : result += lefthandSign * myLaneChangeModel->calcAngleOffset();
1535 :
1536 : #ifdef DEBUG_FURTHER
1537 : if (DEBUG_COND) {
1538 : std::cout << SIMTIME << " computeAngle veh=" << getID() << " p1=" << p1 << " p2=" << p2 << " angle=" << RAD2DEG(result) << " naviDegree=" << GeomHelper::naviDegree(result) << "\n";
1539 : }
1540 : #endif
1541 711470262 : return result;
1542 : }
1543 :
1544 :
1545 : const Position
1546 879893332 : MSVehicle::getBackPosition() const {
1547 879893332 : const double posLat = MSGlobals::gLefthand ? myState.myPosLat : -myState.myPosLat;
1548 : Position result;
1549 879893332 : if (myState.myPos >= myType->getLength()) {
1550 : // vehicle is fully on the new lane
1551 861896910 : result = myLane->geometryPositionAtOffset(myState.myPos - myType->getLength(), posLat);
1552 : } else {
1553 17996422 : if (myLaneChangeModel->isChangingLanes() && myFurtherLanes.size() > 0 && myLaneChangeModel->getShadowLane(myFurtherLanes.back()) == nullptr) {
1554 : // special case where the target lane has no predecessor
1555 : #ifdef DEBUG_FURTHER
1556 : if (DEBUG_COND) {
1557 : std::cout << " getBackPosition veh=" << getID() << " specialCase using myLane=" << myLane->getID() << " pos=0 posLat=" << myState.myPosLat << " result=" << myLane->geometryPositionAtOffset(0, posLat) << "\n";
1558 : }
1559 : #endif
1560 1692 : result = myLane->geometryPositionAtOffset(0, posLat);
1561 : } else {
1562 : #ifdef DEBUG_FURTHER
1563 : if (DEBUG_COND) {
1564 : std::cout << " getBackPosition veh=" << getID() << " myLane=" << myLane->getID() << " further=" << toString(myFurtherLanes) << " myFurtherLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
1565 : }
1566 : #endif
1567 17994730 : if (myFurtherLanes.size() > 0 && !myLaneChangeModel->isChangingLanes()) {
1568 : // truncate to 0 if vehicle starts on an edge that is shorter than its length
1569 17583717 : const double backPos = MAX2(0.0, getBackPositionOnLane(myFurtherLanes.back()));
1570 34868666 : result = myFurtherLanes.back()->geometryPositionAtOffset(backPos, -myFurtherLanesPosLat.back() * (MSGlobals::gLefthand ? -1 : 1));
1571 : } else {
1572 411013 : result = myLane->geometryPositionAtOffset(0, posLat);
1573 : }
1574 : }
1575 : }
1576 879893332 : if (MSNet::getInstance()->hasElevation() && MSGlobals::gSublane) {
1577 283878 : interpolateLateralZ(result, myState.myPos - myType->getLength(), posLat);
1578 : }
1579 879893332 : return result;
1580 : }
1581 :
1582 :
1583 : bool
1584 384810 : MSVehicle::willStop() const {
1585 384810 : return !isStopped() && !myStops.empty() && myLane != nullptr && &myStops.front().lane->getEdge() == &myLane->getEdge();
1586 : }
1587 :
1588 : bool
1589 374366618 : MSVehicle::isStoppedOnLane() const {
1590 374366618 : return isStopped() && myStops.front().lane == myLane;
1591 : }
1592 :
1593 : bool
1594 30633031 : MSVehicle::keepStopping(bool afterProcessing) const {
1595 30633031 : if (isStopped()) {
1596 : // when coming out of vehicleTransfer we must shift the time forward
1597 36513889 : return (myStops.front().duration - (afterProcessing ? DELTA_T : 0) > 0 || isStoppedTriggered() || myStops.front().pars.collision
1598 30461851 : || myStops.front().pars.breakDown || (myStops.front().getSpeed() > 0
1599 37286 : && (myState.myPos < MIN2(myStops.front().pars.endPos, myStops.front().lane->getLength() - POSITION_EPS))
1600 31140 : && (myStops.front().pars.parking == ParkingType::ONROAD || getSpeed() >= SUMO_const_haltingSpeed)));
1601 : } else {
1602 : return false;
1603 : }
1604 : }
1605 :
1606 :
1607 : SUMOTime
1608 15280 : MSVehicle::remainingStopDuration() const {
1609 15280 : if (isStopped()) {
1610 15280 : return myStops.front().duration;
1611 : }
1612 : return 0;
1613 : }
1614 :
1615 :
1616 : SUMOTime
1617 682972920 : MSVehicle::collisionStopTime() const {
1618 682972920 : return (myStops.empty() || !myStops.front().pars.collision) ? myCollisionImmunity : MAX2((SUMOTime)0, myStops.front().duration);
1619 : }
1620 :
1621 :
1622 : bool
1623 682751734 : MSVehicle::brokeDown() const {
1624 682751734 : return isStopped() && !myStops.empty() && myStops.front().pars.breakDown;
1625 : }
1626 :
1627 :
1628 : bool
1629 240650 : MSVehicle::ignoreCollision() const {
1630 240650 : return myCollisionImmunity > 0;
1631 : }
1632 :
1633 :
1634 : double
1635 644756348 : MSVehicle::processNextStop(double currentVelocity) {
1636 644756348 : if (myStops.empty()) {
1637 : // no stops; pass
1638 : return currentVelocity;
1639 : }
1640 :
1641 : #ifdef DEBUG_STOPS
1642 : if (DEBUG_COND) {
1643 : std::cout << "\nPROCESS_NEXT_STOP\n" << SIMTIME << " vehicle '" << getID() << "'" << std::endl;
1644 : }
1645 : #endif
1646 :
1647 : MSStop& stop = myStops.front();
1648 38572712 : const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1649 38572712 : if (stop.reached) {
1650 23137060 : stop.duration -= getActionStepLength();
1651 :
1652 : #ifdef DEBUG_STOPS
1653 : if (DEBUG_COND) {
1654 : std::cout << SIMTIME << " vehicle '" << getID() << "' reached stop.\n"
1655 : << "Remaining duration: " << STEPS2TIME(stop.duration) << std::endl;
1656 : if (stop.getSpeed() > 0) {
1657 : std::cout << " waypointSpeed=" << stop.getSpeed() << " vehPos=" << myState.myPos << " endPos=" << stop.pars.endPos << "\n";
1658 : }
1659 : }
1660 : #endif
1661 23137060 : if (stop.duration <= 0 && stop.pars.join != "") {
1662 : // join this train (part) to another one
1663 37459 : MSVehicle* joinVeh = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(stop.pars.join));
1664 969 : if (joinVeh && joinVeh->hasDeparted() && (joinVeh->joinTrainPart(this) || joinVeh->joinTrainPartFront(this))) {
1665 36 : stop.joinTriggered = false;
1666 36 : if (myAmRegisteredAsWaiting) {
1667 21 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1668 21 : myAmRegisteredAsWaiting = false;
1669 : }
1670 : // avoid collision warning before this vehicle is removed (joinVeh was already made longer)
1671 36 : myCollisionImmunity = TIME2STEPS(100);
1672 : // mark this vehicle as arrived
1673 36 : myArrivalPos = getPositionOnLane();
1674 36 : const_cast<SUMOVehicleParameter*>(myParameter)->arrivalEdge = getRoutePosition();
1675 : // handle transportables that want to continue in the other vehicle
1676 36 : if (myPersonDevice != nullptr) {
1677 3 : myPersonDevice->transferAtSplitOrJoin(joinVeh);
1678 : }
1679 36 : if (myContainerDevice != nullptr) {
1680 3 : myContainerDevice->transferAtSplitOrJoin(joinVeh);
1681 : }
1682 : }
1683 : }
1684 23137060 : boardTransportables(stop);
1685 22027735 : if (time > stop.endBoarding) {
1686 : // for taxi: cancel customers
1687 133261 : MSDevice_Taxi* taxiDevice = static_cast<MSDevice_Taxi*>(getDevice(typeid(MSDevice_Taxi)));
1688 : if (taxiDevice != nullptr) {
1689 : // may invalidate stops including the current reference
1690 64 : taxiDevice->cancelCurrentCustomers();
1691 64 : resumeFromStopping();
1692 64 : return currentVelocity;
1693 : }
1694 : }
1695 22027671 : if (!keepStopping() && isOnRoad()) {
1696 : #ifdef DEBUG_STOPS
1697 : if (DEBUG_COND) {
1698 : std::cout << SIMTIME << " vehicle '" << getID() << "' resumes from stopping." << std::endl;
1699 : }
1700 : #endif
1701 43568 : resumeFromStopping();
1702 43568 : if (isRail() && hasStops()) {
1703 : // stay on the current lane in case of a double stop
1704 2794 : const MSStop& nextStop = getNextStop();
1705 2794 : if (nextStop.edge == myCurrEdge) {
1706 1033 : const double stopSpeed = getCarFollowModel().stopSpeed(this, getSpeed(), nextStop.pars.endPos - myState.myPos);
1707 : //std::cout << SIMTIME << " veh=" << getID() << " resumedFromStopping currentVelocity=" << currentVelocity << " stopSpeed=" << stopSpeed << "\n";
1708 1033 : return stopSpeed;
1709 : }
1710 : }
1711 : } else {
1712 21984103 : if (stop.triggered) {
1713 3102929 : if (getVehicleType().getPersonCapacity() == getPersonNumber()) {
1714 30 : WRITE_WARNINGF(TL("Vehicle '%' ignores triggered stop on lane '%' due to capacity constraints."), getID(), stop.lane->getID());
1715 10 : stop.triggered = false;
1716 3102919 : } else if (!myAmRegisteredAsWaiting && stop.duration <= DELTA_T) {
1717 : // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1718 4116 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1719 4116 : myAmRegisteredAsWaiting = true;
1720 : #ifdef DEBUG_STOPS
1721 : if (DEBUG_COND) {
1722 : std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for person." << std::endl;
1723 : }
1724 : #endif
1725 : }
1726 : }
1727 21984103 : if (stop.containerTriggered) {
1728 39500 : if (getVehicleType().getContainerCapacity() == getContainerNumber()) {
1729 1332 : WRITE_WARNINGF(TL("Vehicle '%' ignores container triggered stop on lane '%' due to capacity constraints."), getID(), stop.lane->getID());
1730 444 : stop.containerTriggered = false;
1731 39056 : } else if (stop.containerTriggered && !myAmRegisteredAsWaiting && stop.duration <= DELTA_T) {
1732 : // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1733 92 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1734 92 : myAmRegisteredAsWaiting = true;
1735 : #ifdef DEBUG_STOPS
1736 : if (DEBUG_COND) {
1737 : std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for container." << std::endl;
1738 : }
1739 : #endif
1740 : }
1741 : }
1742 : // joining only takes place after stop duration is over
1743 21984103 : if (stop.joinTriggered && !myAmRegisteredAsWaiting
1744 7163 : && stop.duration <= (stop.pars.extension >= 0 ? -stop.pars.extension : 0)) {
1745 99 : if (stop.pars.extension >= 0) {
1746 105 : WRITE_WARNINGF(TL("Vehicle '%' aborts joining after extension of %s at time %."), getID(), STEPS2TIME(stop.pars.extension), time2string(SIMSTEP));
1747 35 : stop.joinTriggered = false;
1748 : } else {
1749 : // keep stopping indefinitely but ensure that simulation terminates
1750 64 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1751 64 : myAmRegisteredAsWaiting = true;
1752 : }
1753 : }
1754 21984103 : if (stop.getSpeed() > 0) {
1755 : //waypoint mode
1756 152797 : if (stop.duration == 0) {
1757 264 : return stop.getSpeed();
1758 : } else {
1759 : // stop for 'until' (computed in planMove)
1760 : return currentVelocity;
1761 : }
1762 : } else {
1763 : // brake
1764 21831306 : if (MSGlobals::gSemiImplicitEulerUpdate || stop.getSpeed() > 0) {
1765 21562704 : return 0;
1766 : } else {
1767 : // ballistic:
1768 268602 : return getSpeed() - getCarFollowModel().getMaxDecel();
1769 : }
1770 : }
1771 : }
1772 : } else {
1773 :
1774 : #ifdef DEBUG_STOPS
1775 : if (DEBUG_COND) {
1776 : std::cout << SIMTIME << " vehicle '" << getID() << "' hasn't reached next stop." << std::endl;
1777 : }
1778 : #endif
1779 : //std::cout << SIMTIME << " myStopDist=" << myStopDist << " bGap=" << getBrakeGap(myLane->getVehicleMaxSpeed(this)) << "\n";
1780 15499317 : if (stop.pars.onDemand && !stop.skipOnDemand && myStopDist <= getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this))) {
1781 577 : MSNet* const net = MSNet::getInstance();
1782 44 : const bool noExits = ((myPersonDevice == nullptr || !myPersonDevice->anyLeavingAtStop(stop))
1783 587 : && (myContainerDevice == nullptr || !myContainerDevice->anyLeavingAtStop(stop)));
1784 83 : const bool noEntries = ((!net->hasPersons() || !net->getPersonControl().hasAnyWaiting(stop.getEdge(), this))
1785 626 : && (!net->hasContainers() || !net->getContainerControl().hasAnyWaiting(stop.getEdge(), this)));
1786 577 : if (noExits && noEntries) {
1787 : //std::cout << " skipOnDemand\n";
1788 509 : stop.skipOnDemand = true;
1789 : // bestLanes must be extended past this stop
1790 509 : updateBestLanes(true);
1791 : }
1792 : }
1793 : // is the next stop on the current lane?
1794 15435652 : if (stop.edge == myCurrEdge) {
1795 : // get the stopping position
1796 5513449 : bool useStoppingPlace = stop.busstop != nullptr || stop.containerstop != nullptr || stop.parkingarea != nullptr;
1797 : bool fitsOnStoppingPlace = true;
1798 5513449 : if (!stop.skipOnDemand) { // no need to check available space if we skip it anyway
1799 5511461 : if (stop.busstop != nullptr) {
1800 1632847 : fitsOnStoppingPlace &= stop.busstop->fits(myState.myPos, *this);
1801 : }
1802 5511461 : if (stop.containerstop != nullptr) {
1803 21798 : fitsOnStoppingPlace &= stop.containerstop->fits(myState.myPos, *this);
1804 : }
1805 : // if the stop is a parking area we check if there is a free position on the area
1806 5511461 : if (stop.parkingarea != nullptr) {
1807 687930 : fitsOnStoppingPlace &= myState.myPos > stop.parkingarea->getBeginLanePosition();
1808 687930 : if (stop.parkingarea->getOccupancy() >= stop.parkingarea->getCapacity()) {
1809 : fitsOnStoppingPlace = false;
1810 : // trigger potential parkingZoneReroute
1811 434980 : MSParkingArea* oldParkingArea = stop.parkingarea;
1812 475849 : for (MSMoveReminder* rem : myLane->getMoveReminders()) {
1813 40869 : if (rem->isParkingRerouter()) {
1814 19645 : rem->notifyEnter(*this, MSMoveReminder::NOTIFICATION_PARKING_REROUTE, myLane);
1815 : }
1816 : }
1817 434980 : if (myStops.empty() || myStops.front().parkingarea != oldParkingArea) {
1818 : // rerouted, keep driving
1819 : return currentVelocity;
1820 : }
1821 252950 : } else if (stop.parkingarea->getOccupancyIncludingReservations(this) >= stop.parkingarea->getCapacity()) {
1822 : fitsOnStoppingPlace = false;
1823 127690 : } else if (stop.parkingarea->parkOnRoad() && stop.parkingarea->getLotIndex(this) < 0) {
1824 : fitsOnStoppingPlace = false;
1825 : }
1826 : }
1827 : }
1828 5511754 : const double targetPos = myState.myPos + myStopDist + (stop.getSpeed() > 0 ? (stop.pars.startPos - stop.pars.endPos) : 0);
1829 5511754 : double reachedThreshold = (useStoppingPlace ? targetPos - STOPPING_PLACE_OFFSET : stop.getReachedThreshold()) - NUMERICAL_EPS;
1830 5511754 : if (stop.busstop != nullptr && stop.getSpeed() <= 0 && getWaitingTime() > DELTA_T && myLane == stop.lane) {
1831 : // count (long) busStop as reached when fully within and jammed before the designated spot
1832 783119 : reachedThreshold = MIN2(reachedThreshold, stop.pars.startPos + getLength());
1833 : }
1834 : #ifdef DEBUG_STOPS
1835 : if (DEBUG_COND) {
1836 : std::cout << " pos=" << myState.pos() << " speed=" << currentVelocity << " targetPos=" << targetPos << " fits=" << fitsOnStoppingPlace
1837 : << " reachedThresh=" << reachedThreshold
1838 : << " myLane=" << Named::getIDSecure(myLane)
1839 : << " stopLane=" << Named::getIDSecure(stop.lane)
1840 : << "\n";
1841 : }
1842 : #endif
1843 5511754 : const bool posReached = myState.pos() >= reachedThreshold && currentVelocity <= stop.getSpeed() + SUMO_const_haltingSpeed && myLane == stop.lane;
1844 5511754 : if (posReached && !fitsOnStoppingPlace && MSStopOut::active()) {
1845 5637 : MSStopOut::getInstance()->stopBlocked(this, time);
1846 : }
1847 5511754 : if (fitsOnStoppingPlace && posReached && (!MSGlobals::gModelParkingManoeuver || myManoeuvre.entryManoeuvreIsComplete(this))) {
1848 : // ok, we may stop (have reached the stop) and either we are not modelling maneuvering or have completed entry
1849 55516 : stop.reached = true;
1850 55516 : if (!stop.startedFromState) {
1851 55307 : stop.pars.started = time;
1852 : }
1853 : #ifdef DEBUG_STOPS
1854 : if (DEBUG_COND) {
1855 : std::cout << SIMTIME << " vehicle '" << getID() << "' reached next stop." << std::endl;
1856 : }
1857 : #endif
1858 55516 : if (MSStopOut::active()) {
1859 5381 : MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), time);
1860 : }
1861 55516 : myLane->getEdge().addWaiting(this);
1862 55516 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::STARTING_STOP);
1863 55516 : MSNet::getInstance()->getVehicleControl().registerStopStarted();
1864 : // compute stopping time
1865 55516 : stop.duration = stop.getMinDuration(time);
1866 55516 : stop.endBoarding = stop.pars.extension >= 0 ? time + stop.duration + stop.pars.extension : SUMOTime_MAX;
1867 55516 : MSDevice_Taxi* taxiDevice = static_cast<MSDevice_Taxi*>(getDevice(typeid(MSDevice_Taxi)));
1868 3740 : if (taxiDevice != nullptr && stop.pars.extension >= 0) {
1869 : // earliestPickupTime is set with waitUntil
1870 84 : stop.endBoarding = MAX2(time, stop.pars.waitUntil) + stop.pars.extension;
1871 : }
1872 55516 : if (stop.getSpeed() > 0) {
1873 : // ignore duration parameter in waypoint mode unless 'until' or 'ended' are set
1874 3435 : if (stop.getUntil() > time) {
1875 334 : stop.duration = stop.getUntil() - time;
1876 : } else {
1877 3101 : stop.duration = 0;
1878 : }
1879 : }
1880 55516 : if (stop.busstop != nullptr) {
1881 : // let the bus stop know the vehicle
1882 18239 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1883 : }
1884 55516 : if (stop.containerstop != nullptr) {
1885 : // let the container stop know the vehicle
1886 571 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1887 : }
1888 55516 : if (stop.parkingarea != nullptr && stop.getSpeed() <= 0) {
1889 : // let the parking area know the vehicle
1890 9825 : stop.parkingarea->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1891 : }
1892 55516 : if (stop.chargingStation != nullptr) {
1893 : // let the container stop know the vehicle
1894 3504 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1895 : }
1896 :
1897 55516 : if (stop.pars.tripId != "") {
1898 2920 : ((SUMOVehicleParameter&)getParameter()).setParameter("tripId", stop.pars.tripId);
1899 : }
1900 55516 : if (stop.pars.line != "") {
1901 1464 : ((SUMOVehicleParameter&)getParameter()).line = stop.pars.line;
1902 : }
1903 55516 : if (stop.pars.split != "") {
1904 : // split the train
1905 1239 : MSVehicle* splitVeh = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(stop.pars.split));
1906 24 : if (splitVeh == nullptr) {
1907 3645 : WRITE_WARNINGF(TL("Vehicle '%' to split from vehicle '%' is not known. time=%."), stop.pars.split, getID(), SIMTIME)
1908 : } else {
1909 24 : MSNet::getInstance()->getInsertionControl().add(splitVeh);
1910 24 : splitVeh->getRoute().getEdges()[0]->removeWaiting(splitVeh);
1911 24 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1912 24 : const double newLength = MAX2(myType->getLength() - splitVeh->getVehicleType().getLength(),
1913 24 : myType->getParameter().locomotiveLength);
1914 24 : getSingularType().setLength(newLength);
1915 : // handle transportables that want to continue in the split part
1916 24 : if (myPersonDevice != nullptr) {
1917 0 : myPersonDevice->transferAtSplitOrJoin(splitVeh);
1918 : }
1919 24 : if (myContainerDevice != nullptr) {
1920 6 : myContainerDevice->transferAtSplitOrJoin(splitVeh);
1921 : }
1922 24 : if (splitVeh->getParameter().departPosProcedure == DepartPosDefinition::SPLIT_FRONT) {
1923 3 : const double backShift = splitVeh->getLength() + getVehicleType().getMinGap();
1924 3 : myState.myPos -= backShift;
1925 3 : myState.myBackPos -= backShift;
1926 : }
1927 : }
1928 : }
1929 :
1930 55516 : boardTransportables(stop);
1931 55512 : if (stop.pars.posLat != INVALID_DOUBLE) {
1932 232 : myState.myPosLat = stop.pars.posLat;
1933 : }
1934 : }
1935 : }
1936 : }
1937 : return currentVelocity;
1938 : }
1939 :
1940 :
1941 : void
1942 23192576 : MSVehicle::boardTransportables(MSStop& stop) {
1943 23192576 : if (stop.skipOnDemand) {
1944 : return;
1945 : }
1946 : // we have reached the stop
1947 : // any waiting persons may board now
1948 23055845 : const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1949 23055845 : MSNet* const net = MSNet::getInstance();
1950 23055845 : const bool boarded = (time <= stop.endBoarding
1951 23053769 : && net->hasPersons()
1952 1476747 : && net->getPersonControl().loadAnyWaiting(&myLane->getEdge(), this, stop.timeToBoardNextPerson, stop.duration)
1953 23060653 : && stop.numExpectedPerson == 0);
1954 : // load containers
1955 23055845 : const bool loaded = (time <= stop.endBoarding
1956 23053769 : && net->hasContainers()
1957 1211747 : && net->getContainerControl().loadAnyWaiting(&myLane->getEdge(), this, stop.timeToLoadNextContainer, stop.duration)
1958 23056420 : && stop.numExpectedContainer == 0);
1959 :
1960 : bool unregister = false;
1961 21946516 : if (time > stop.endBoarding) {
1962 2076 : stop.triggered = false;
1963 2076 : stop.containerTriggered = false;
1964 2076 : if (myAmRegisteredAsWaiting) {
1965 : unregister = true;
1966 325 : myAmRegisteredAsWaiting = false;
1967 : }
1968 : }
1969 21946516 : if (boarded) {
1970 : // the triggering condition has been fulfilled. Maybe we want to wait a bit longer for additional riders (car pooling)
1971 4677 : if (myAmRegisteredAsWaiting) {
1972 : unregister = true;
1973 : }
1974 4677 : stop.triggered = false;
1975 4677 : myAmRegisteredAsWaiting = false;
1976 : }
1977 21946516 : if (loaded) {
1978 : // the triggering condition has been fulfilled
1979 555 : if (myAmRegisteredAsWaiting) {
1980 : unregister = true;
1981 : }
1982 555 : stop.containerTriggered = false;
1983 555 : myAmRegisteredAsWaiting = false;
1984 : }
1985 :
1986 21946516 : if (unregister) {
1987 409 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1988 : #ifdef DEBUG_STOPS
1989 : if (DEBUG_COND) {
1990 : std::cout << SIMTIME << " vehicle '" << getID() << "' unregisters as waiting for transportable." << std::endl;
1991 : }
1992 : #endif
1993 : }
1994 : }
1995 :
1996 : bool
1997 921 : MSVehicle::joinTrainPart(MSVehicle* veh) {
1998 : // check if veh is close enough to be joined to the rear of this vehicle
1999 921 : MSLane* backLane = myFurtherLanes.size() == 0 ? myLane : myFurtherLanes.back();
2000 921 : double gap = getBackPositionOnLane() - veh->getPositionOnLane();
2001 1143 : if (isStopped() && myStops.begin()->duration <= DELTA_T && myStops.begin()->joinTriggered && backLane == veh->getLane()
2002 951 : && gap >= 0 && gap <= getVehicleType().getMinGap() + 1) {
2003 15 : const double newLength = myType->getLength() + veh->getVehicleType().getLength();
2004 15 : getSingularType().setLength(newLength);
2005 15 : myStops.begin()->joinTriggered = false;
2006 15 : if (myAmRegisteredAsWaiting) {
2007 0 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2008 0 : myAmRegisteredAsWaiting = false;
2009 : }
2010 : return true;
2011 : } else {
2012 906 : return false;
2013 : }
2014 : }
2015 :
2016 :
2017 : bool
2018 906 : MSVehicle::joinTrainPartFront(MSVehicle* veh) {
2019 : // check if veh is close enough to be joined to the front of this vehicle
2020 906 : MSLane* backLane = veh->myFurtherLanes.size() == 0 ? veh->myLane : veh->myFurtherLanes.back();
2021 906 : double gap = veh->getBackPositionOnLane(backLane) - getPositionOnLane();
2022 1113 : if (isStopped() && myStops.begin()->duration <= DELTA_T && myStops.begin()->joinTriggered && backLane == getLane()
2023 930 : && gap >= 0 && gap <= getVehicleType().getMinGap() + 1) {
2024 : double skippedLaneLengths = 0;
2025 24 : if (veh->myFurtherLanes.size() > 0) {
2026 9 : skippedLaneLengths += getLane()->getLength();
2027 : // this vehicle must be moved to the lane of veh
2028 : // ensure that lane and furtherLanes of veh match our route
2029 9 : int routeIndex = getRoutePosition();
2030 9 : if (myLane->isInternal()) {
2031 0 : routeIndex++;
2032 : }
2033 27 : for (int i = (int)veh->myFurtherLanes.size() - 1; i >= 0; i--) {
2034 18 : MSEdge* edge = &veh->myFurtherLanes[i]->getEdge();
2035 18 : if (edge->isInternal()) {
2036 9 : continue;
2037 : }
2038 9 : if (!edge->isInternal() && edge != myRoute->getEdges()[routeIndex]) {
2039 0 : std::string warn = TL("Cannot join vehicle '%' to vehicle '%' due to incompatible routes. time=%.");
2040 0 : WRITE_WARNINGF(warn, veh->getID(), getID(), time2string(SIMSTEP));
2041 : return false;
2042 : }
2043 9 : routeIndex++;
2044 : }
2045 9 : if (veh->getCurrentEdge()->getNormalSuccessor() != myRoute->getEdges()[routeIndex]) {
2046 3 : std::string warn = TL("Cannot join vehicle '%' to vehicle '%' due to incompatible routes. time=%.");
2047 9 : WRITE_WARNINGF(warn, veh->getID(), getID(), time2string(SIMSTEP));
2048 : return false;
2049 : }
2050 12 : for (int i = (int)veh->myFurtherLanes.size() - 2; i >= 0; i--) {
2051 6 : skippedLaneLengths += veh->myFurtherLanes[i]->getLength();
2052 : }
2053 : }
2054 :
2055 21 : const double newLength = myType->getLength() + veh->getVehicleType().getLength();
2056 21 : getSingularType().setLength(newLength);
2057 : // lane will be advanced just as for regular movement
2058 21 : myState.myPos = skippedLaneLengths + veh->getPositionOnLane();
2059 21 : myStops.begin()->joinTriggered = false;
2060 21 : if (myAmRegisteredAsWaiting) {
2061 7 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2062 7 : myAmRegisteredAsWaiting = false;
2063 : }
2064 21 : return true;
2065 : } else {
2066 882 : return false;
2067 : }
2068 : }
2069 :
2070 : double
2071 8626567 : MSVehicle::getBrakeGap(bool delayed) const {
2072 8626567 : return getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), delayed ? getCarFollowModel().getHeadwayTime() : 0);
2073 : }
2074 :
2075 :
2076 : bool
2077 705263362 : MSVehicle::checkActionStep(const SUMOTime t) {
2078 705263362 : myActionStep = isActionStep(t);
2079 705263362 : if (myActionStep) {
2080 633588686 : myLastActionTime = t;
2081 : }
2082 705263362 : return myActionStep;
2083 : }
2084 :
2085 :
2086 : void
2087 1360 : MSVehicle::resetActionOffset(const SUMOTime timeUntilNextAction) {
2088 1360 : myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + timeUntilNextAction;
2089 1360 : }
2090 :
2091 :
2092 : void
2093 1 : MSVehicle::updateActionOffset(const SUMOTime oldActionStepLength, const SUMOTime newActionStepLength) {
2094 1 : SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2095 1 : SUMOTime timeSinceLastAction = now - myLastActionTime;
2096 1 : if (timeSinceLastAction == 0) {
2097 : // Action was scheduled now, may be delayed be new action step length
2098 : timeSinceLastAction = oldActionStepLength;
2099 : }
2100 1 : if (timeSinceLastAction >= newActionStepLength) {
2101 : // Action point required in this step
2102 0 : myLastActionTime = now;
2103 : } else {
2104 1 : SUMOTime timeUntilNextAction = newActionStepLength - timeSinceLastAction;
2105 1 : resetActionOffset(timeUntilNextAction);
2106 : }
2107 1 : }
2108 :
2109 :
2110 :
2111 : void
2112 705263362 : MSVehicle::planMove(const SUMOTime t, const MSLeaderInfo& ahead, const double lengthsInFront) {
2113 : #ifdef DEBUG_PLAN_MOVE
2114 : if (DEBUG_COND) {
2115 : std::cout
2116 : << "\nPLAN_MOVE\n"
2117 : << SIMTIME
2118 : << std::setprecision(gPrecision)
2119 : << " veh=" << getID()
2120 : << " lane=" << myLane->getID()
2121 : << " pos=" << getPositionOnLane()
2122 : << " posLat=" << getLateralPositionOnLane()
2123 : << " speed=" << getSpeed()
2124 : << "\n";
2125 : }
2126 : #endif
2127 : // Update the driver state
2128 705263362 : if (hasDriverState()) {
2129 451801 : myDriverState->update();
2130 903602 : setActionStepLength(myDriverState->getDriverState()->getActionStepLength(), false);
2131 : }
2132 :
2133 705263362 : myStopSpeed = getCarFollowModel().maxNextSpeed(myStopSpeed, this);
2134 705263362 : if (!checkActionStep(t)) {
2135 : #ifdef DEBUG_ACTIONSTEPS
2136 : if (DEBUG_COND) {
2137 : std::cout << STEPS2TIME(t) << " vehicle '" << getID() << "' skips action." << std::endl;
2138 : }
2139 : #endif
2140 : // During non-action passed drive items still need to be removed
2141 : // @todo rather work with updating myCurrentDriveItem (refs #3714)
2142 71674676 : removePassedDriveItems();
2143 71674676 : return;
2144 : } else {
2145 : #ifdef DEBUG_ACTIONSTEPS
2146 : if (DEBUG_COND) {
2147 : std::cout << STEPS2TIME(t) << " vehicle = '" << getID() << "' takes action." << std::endl;
2148 : }
2149 : #endif
2150 633588686 : myLFLinkLanesPrev = myLFLinkLanes;
2151 633588686 : if (myInfluencer != nullptr) {
2152 493536 : myInfluencer->updateRemoteControlRoute(this);
2153 : }
2154 633588686 : planMoveInternal(t, ahead, myLFLinkLanes, myStopDist, myStopSpeed, myNextTurn);
2155 : #ifdef DEBUG_PLAN_MOVE
2156 : if (DEBUG_COND) {
2157 : DriveItemVector::iterator i;
2158 : for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
2159 : std::cout
2160 : << " vPass=" << (*i).myVLinkPass
2161 : << " vWait=" << (*i).myVLinkWait
2162 : << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
2163 : << " request=" << (*i).mySetRequest
2164 : << "\n";
2165 : }
2166 : }
2167 : #endif
2168 633588686 : checkRewindLinkLanes(lengthsInFront, myLFLinkLanes);
2169 633588686 : myNextDriveItem = myLFLinkLanes.begin();
2170 : // ideally would only do this with the call inside planMoveInternal - but that needs a const method
2171 : // so this is a kludge here - nuisance as it adds an extra check in a busy loop
2172 633588686 : if (MSGlobals::gModelParkingManoeuver) {
2173 2971 : if (getManoeuvreType() == MSVehicle::MANOEUVRE_EXIT && manoeuvreIsComplete()) {
2174 30 : setManoeuvreType(MSVehicle::MANOEUVRE_NONE);
2175 : }
2176 : }
2177 : }
2178 633588686 : myLaneChangeModel->resetChanged();
2179 : }
2180 :
2181 :
2182 : bool
2183 170387301 : MSVehicle::brakeForOverlap(const MSLink* link, const MSLane* lane) const {
2184 : // @review needed
2185 : //const double futurePosLat = getLateralPositionOnLane() + link->getLateralShift();
2186 : //const double overlap = getLateralOverlap(futurePosLat, link->getViaLaneOrLane());
2187 : //const double edgeWidth = link->getViaLaneOrLane()->getEdge().getWidth();
2188 170387301 : const double futurePosLat = getLateralPositionOnLane() + (
2189 170387301 : lane != myLane && lane->isInternal() ? lane->getIncomingLanes()[0].viaLink->getLateralShift() : 0);
2190 170387301 : const double overlap = getLateralOverlap(futurePosLat, lane);
2191 : const double edgeWidth = lane->getEdge().getWidth();
2192 : const bool result = (overlap > POSITION_EPS
2193 : // do not get stuck on narrow edges
2194 3079282 : && getVehicleType().getWidth() <= edgeWidth
2195 3074997 : && link->getViaLane() == nullptr
2196 : // this is the exit link of a junction. The normal edge should support the shadow
2197 1494249 : && ((myLaneChangeModel->getShadowLane(link->getLane()) == nullptr)
2198 : // the shadow lane must be permitted
2199 1116220 : || !myLaneChangeModel->getShadowLane(link->getLane())->allowsVehicleClass(getVClass())
2200 : // the internal lane after an internal junction has no parallel lane. make sure there is no shadow before continuing
2201 1055758 : || (lane->getEdge().isInternal() && lane->getIncomingLanes()[0].lane->getEdge().isInternal()))
2202 : // ignore situations where the shadow lane is part of a double-connection with the current lane
2203 454390 : && (myLaneChangeModel->getShadowLane() == nullptr
2204 250851 : || myLaneChangeModel->getShadowLane()->getLinkCont().size() == 0
2205 233363 : || myLaneChangeModel->getShadowLane()->getLinkCont().front()->getLane() != link->getLane())
2206 : // emergency vehicles may do some crazy stuff
2207 170779989 : && !myLaneChangeModel->hasBlueLight());
2208 :
2209 : #ifdef DEBUG_PLAN_MOVE
2210 : if (DEBUG_COND) {
2211 : std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getDescription() << " lane=" << lane->getID()
2212 : << " linkLane=" << link->getLane()->getID()
2213 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
2214 : << " shift=" << link->getLateralShift()
2215 : << " fpLat=" << futurePosLat << " overlap=" << overlap << " w=" << getVehicleType().getWidth()
2216 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane(link->getLane()))
2217 : << " result=" << result << "\n";
2218 : }
2219 : #endif
2220 170387301 : return result;
2221 : }
2222 :
2223 :
2224 :
2225 : void
2226 633588686 : MSVehicle::planMoveInternal(const SUMOTime t, MSLeaderInfo ahead, DriveItemVector& lfLinks, double& newStopDist, double& newStopSpeed, std::pair<double, const MSLink*>& nextTurn) const {
2227 : lfLinks.clear();
2228 633588686 : newStopDist = std::numeric_limits<double>::max();
2229 : //
2230 : const MSCFModel& cfModel = getCarFollowModel();
2231 633588686 : const double vehicleLength = getVehicleType().getLength();
2232 633588686 : const double maxV = cfModel.maxNextSpeed(myState.mySpeed, this);
2233 633588686 : const double maxVD = MAX2(getMaxSpeed(), MIN2(maxV, getDesiredMaxSpeed()));
2234 633588686 : const bool opposite = myLaneChangeModel->isOpposite();
2235 : // maxVD is possibly higher than vType-maxSpeed and in this case laneMaxV may be higher as well
2236 633588686 : double laneMaxV = myLane->getVehicleMaxSpeed(this, maxVD);
2237 633588686 : const double vMinComfortable = cfModel.minNextSpeed(getSpeed(), this);
2238 : double lateralShift = 0;
2239 633588686 : if (isRail()) {
2240 : // speed limits must hold for the whole length of the train
2241 1602294 : for (MSLane* l : myFurtherLanes) {
2242 347435 : laneMaxV = MIN2(laneMaxV, l->getVehicleMaxSpeed(this, maxVD));
2243 : #ifdef DEBUG_PLAN_MOVE
2244 : if (DEBUG_COND) {
2245 : std::cout << " laneMaxV=" << laneMaxV << " lane=" << l->getID() << "\n";
2246 : }
2247 : #endif
2248 : }
2249 : }
2250 : // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
2251 : laneMaxV = MAX2(laneMaxV, vMinComfortable);
2252 634082190 : if (myInfluencer && !myInfluencer->considerSpeedLimit()) {
2253 : laneMaxV = std::numeric_limits<double>::max();
2254 : }
2255 : // v is the initial maximum velocity of this vehicle in this step
2256 633588686 : double v = cfModel.maximumLaneSpeedCF(this, maxV, laneMaxV);
2257 : // if we are modelling parking then we dawdle until the manoeuvre is complete - by setting a very low max speed
2258 : // in practice this only applies to exit manoeuvre because entry manoeuvre just delays setting stop.reached - when the vehicle is virtually stopped
2259 633588686 : if (MSGlobals::gModelParkingManoeuver && !manoeuvreIsComplete()) {
2260 420 : v = NUMERICAL_EPS_SPEED;
2261 : }
2262 :
2263 633588686 : if (myInfluencer != nullptr) {
2264 493536 : const double vMin = MAX2(0., cfModel.minNextSpeed(myState.mySpeed, this));
2265 : #ifdef DEBUG_TRACI
2266 : if (DEBUG_COND) {
2267 : std::cout << SIMTIME << " veh=" << getID() << " speedBeforeTraci=" << v;
2268 : }
2269 : #endif
2270 493536 : v = myInfluencer->influenceSpeed(t, v, v, vMin, maxV);
2271 : #ifdef DEBUG_TRACI
2272 : if (DEBUG_COND) {
2273 : std::cout << " influencedSpeed=" << v;
2274 : }
2275 : #endif
2276 493536 : v = myInfluencer->gapControlSpeed(t, this, v, v, vMin, maxV);
2277 : #ifdef DEBUG_TRACI
2278 : if (DEBUG_COND) {
2279 : std::cout << " gapControlSpeed=" << v << "\n";
2280 : }
2281 : #endif
2282 : }
2283 : // all links within dist are taken into account (potentially)
2284 633588686 : const double dist = SPEED2DIST(maxV) + cfModel.brakeGap(maxV);
2285 :
2286 633588686 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation();
2287 : #ifdef DEBUG_PLAN_MOVE
2288 : if (DEBUG_COND) {
2289 : std::cout << " dist=" << dist << " bestLaneConts=" << toString(bestLaneConts)
2290 : << "\n maxV=" << maxV << " laneMaxV=" << laneMaxV << " v=" << v << "\n";
2291 : }
2292 : #endif
2293 : assert(bestLaneConts.size() > 0);
2294 : bool hadNonInternal = false;
2295 : // the distance already "seen"; in the following always up to the end of the current "lane"
2296 633588686 : double seen = opposite ? myState.myPos : myLane->getLength() - myState.myPos;
2297 633588686 : nextTurn.first = seen;
2298 633588686 : nextTurn.second = nullptr;
2299 633588686 : bool encounteredTurn = (MSGlobals::gLateralResolution <= 0); // next turn is only needed for sublane
2300 : double seenNonInternal = 0;
2301 633588686 : double seenInternal = myLane->isInternal() ? seen : 0;
2302 633588686 : double vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(seen, v, cfModel.getMaxAccel()), laneMaxV); // upper bound
2303 : int view = 0;
2304 : DriveProcessItem* lastLink = nullptr;
2305 : bool slowedDownForMinor = false; // whether the vehicle already had to slow down on approach to a minor link
2306 : double mustSeeBeforeReversal = 0;
2307 : // iterator over subsequent lanes and fill lfLinks until stopping distance or stopped
2308 633588686 : const MSLane* lane = opposite ? myLane->getParallelOpposite() : myLane;
2309 : assert(lane != 0);
2310 633588686 : const MSLane* leaderLane = myLane;
2311 633588686 : bool foundRailSignal = !isRail();
2312 : bool planningToStop = false;
2313 : #ifdef PARALLEL_STOPWATCH
2314 : myLane->getStopWatch()[0].start();
2315 : #endif
2316 :
2317 : // optionally slow down to match arrival time
2318 633588686 : const double sfp = getVehicleType().getParameter().speedFactorPremature;
2319 633578413 : if (v > vMinComfortable && hasStops() && myStops.front().pars.arrival >= 0 && sfp > 0
2320 4279 : && v > myLane->getSpeedLimit() * sfp
2321 633591722 : && !myStops.front().reached) {
2322 2786 : const double vSlowDown = slowDownForSchedule(vMinComfortable);
2323 5403 : v = MIN2(v, vSlowDown);
2324 : }
2325 : auto stopIt = myStops.begin();
2326 : while (true) {
2327 : // check leader on lane
2328 : // leader is given for the first edge only
2329 1211411136 : if (opposite &&
2330 : (leaderLane->getVehicleNumberWithPartials() > 1
2331 100999 : || (leaderLane != myLane && leaderLane->getVehicleNumber() > 0))) {
2332 396367 : ahead.clear();
2333 : // find opposite-driving leader that must be respected on the currently looked at lane
2334 : // (only looking at one lane at a time)
2335 396367 : const double backOffset = leaderLane == myLane ? getPositionOnLane() : leaderLane->getLength();
2336 396367 : const double gapOffset = leaderLane == myLane ? 0 : seen - leaderLane->getLength();
2337 396367 : const MSLeaderDistanceInfo cands = leaderLane->getFollowersOnConsecutive(this, backOffset, true, backOffset, MSLane::MinorLinkMode::FOLLOW_NEVER);
2338 396367 : MSLeaderDistanceInfo oppositeLeaders(leaderLane->getWidth(), this, 0.);
2339 396367 : const double minTimeToLeaveLane = MSGlobals::gSublane ? MAX2(TS, (0.5 * myLane->getWidth() - getLateralPositionOnLane()) / getVehicleType().getMaxSpeedLat()) : TS;
2340 1053397 : for (int i = 0; i < cands.numSublanes(); i++) {
2341 657030 : CLeaderDist cand = cands[i];
2342 657030 : if (cand.first != 0) {
2343 560349 : if ((cand.first->myLaneChangeModel->isOpposite() && cand.first->getLaneChangeModel().getShadowLane() != leaderLane)
2344 560820 : || (!cand.first->myLaneChangeModel->isOpposite() && cand.first->getLaneChangeModel().getShadowLane() == leaderLane)) {
2345 : // respect leaders that also drive in the opposite direction (fully or with some overlap)
2346 354420 : oppositeLeaders.addLeader(cand.first, cand.second + gapOffset - getVehicleType().getMinGap() + cand.first->getVehicleType().getMinGap() - cand.first->getVehicleType().getLength());
2347 : } else {
2348 : // avoid frontal collision
2349 379024 : const bool assumeStopped = cand.first->isStopped() || cand.first->getWaitingSeconds() > 1;
2350 205929 : const double predMaxDist = cand.first->getSpeed() + (assumeStopped ? 0 : cand.first->getCarFollowModel().getMaxAccel()) * minTimeToLeaveLane;
2351 205929 : if (cand.second >= 0 && (cand.second - v * minTimeToLeaveLane - predMaxDist < 0 || assumeStopped)) {
2352 44643 : oppositeLeaders.addLeader(cand.first, cand.second + gapOffset - predMaxDist - getVehicleType().getMinGap());
2353 : }
2354 : }
2355 : }
2356 : }
2357 : #ifdef DEBUG_PLAN_MOVE
2358 : if (DEBUG_COND) {
2359 : std::cout << " leaderLane=" << leaderLane->getID() << " gapOffset=" << gapOffset << " minTimeToLeaveLane=" << minTimeToLeaveLane
2360 : << " cands=" << cands.toString() << " oppositeLeaders=" << oppositeLeaders.toString() << "\n";
2361 : }
2362 : #endif
2363 396367 : adaptToLeaderDistance(oppositeLeaders, 0, seen, lastLink, v, vLinkPass);
2364 396367 : } else {
2365 1211014769 : if (MSGlobals::gLateralResolution > 0 && myLaneChangeModel->getShadowLane() == nullptr) {
2366 192766523 : const double rightOL = getRightSideOnLane(lane) + lateralShift;
2367 192766523 : const double leftOL = getLeftSideOnLane(lane) + lateralShift;
2368 : const bool outsideLeft = leftOL > lane->getWidth();
2369 : #ifdef DEBUG_PLAN_MOVE
2370 : if (DEBUG_COND) {
2371 : std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
2372 : }
2373 : #endif
2374 192766523 : if (rightOL < 0 || outsideLeft) {
2375 1265573 : MSLeaderInfo outsideLeaders(lane->getWidth());
2376 : // if ego is driving outside lane bounds we must consider
2377 : // potential leaders that are also outside bounds
2378 : int sublaneOffset = 0;
2379 1265573 : if (outsideLeft) {
2380 531270 : sublaneOffset = MIN2(-1, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution));
2381 : } else {
2382 734303 : sublaneOffset = MAX2(1, (int)ceil(-rightOL / MSGlobals::gLateralResolution));
2383 : }
2384 1265573 : outsideLeaders.setSublaneOffset(sublaneOffset);
2385 : #ifdef DEBUG_PLAN_MOVE
2386 : if (DEBUG_COND) {
2387 : std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
2388 : }
2389 : #endif
2390 5328355 : for (const MSVehicle* cand : lane->getVehiclesSecure()) {
2391 1488168 : if ((lane != myLane || cand->getPositionOnLane() > getPositionOnLane())
2392 4698527 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
2393 3210197 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
2394 97506 : outsideLeaders.addLeader(cand, true);
2395 : #ifdef DEBUG_PLAN_MOVE
2396 : if (DEBUG_COND) {
2397 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << outsideLeaders.toString() << "\n";
2398 : }
2399 : #endif
2400 : }
2401 : }
2402 1265573 : lane->releaseVehicles();
2403 1265573 : if (outsideLeaders.hasVehicles()) {
2404 26311 : adaptToLeaders(outsideLeaders, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2405 : }
2406 1265573 : }
2407 : }
2408 1211014769 : adaptToLeaders(ahead, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2409 : }
2410 1211411136 : if (lastLink != nullptr) {
2411 1084077790 : lastLink->myVLinkWait = MIN2(lastLink->myVLinkWait, v);
2412 : }
2413 : #ifdef DEBUG_PLAN_MOVE
2414 : if (DEBUG_COND) {
2415 : std::cout << "\nv = " << v << "\n";
2416 :
2417 : }
2418 : #endif
2419 : // XXX efficiently adapt to shadow leaders using neighAhead by iteration over the whole edge in parallel (lanechanger-style)
2420 1211411136 : if (myLaneChangeModel->getShadowLane() != nullptr) {
2421 : // also slow down for leaders on the shadowLane relative to the current lane
2422 5004883 : const MSLane* shadowLane = myLaneChangeModel->getShadowLane(leaderLane);
2423 : if (shadowLane != nullptr
2424 5004883 : && (MSGlobals::gLateralResolution > 0 || getLateralOverlap() > POSITION_EPS
2425 : // continous lane change cannot be stopped so we must adapt to the leader on the target lane
2426 191105 : || myLaneChangeModel->getLaneChangeCompletion() < 0.5)) {
2427 4458586 : if ((&shadowLane->getEdge() == &leaderLane->getEdge() || myLaneChangeModel->isOpposite())) {
2428 4416249 : double latOffset = getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge();
2429 4416249 : if (myLaneChangeModel->isOpposite()) {
2430 : // ego posLat is added when retrieving sublanes but it
2431 : // should be negated (subtract twice to compensate)
2432 138083 : latOffset = ((myLane->getWidth() + shadowLane->getWidth()) * 0.5
2433 138083 : - 2 * getLateralPositionOnLane());
2434 :
2435 : }
2436 4416249 : MSLeaderInfo shadowLeaders = shadowLane->getLastVehicleInformation(this, latOffset, lane->getLength() - seen);
2437 : #ifdef DEBUG_PLAN_MOVE
2438 : if (DEBUG_COND && myLaneChangeModel->isOpposite()) {
2439 : std::cout << SIMTIME << " opposite veh=" << getID() << " shadowLane=" << shadowLane->getID() << " latOffset=" << latOffset << " shadowLeaders=" << shadowLeaders.toString() << "\n";
2440 : }
2441 : #endif
2442 4416249 : if (myLaneChangeModel->isOpposite()) {
2443 : // ignore oncoming vehicles on the shadow lane
2444 138083 : shadowLeaders.removeOpposite(shadowLane);
2445 : }
2446 4416249 : const double turningDifference = MAX2(0.0, leaderLane->getLength() - shadowLane->getLength());
2447 4416249 : adaptToLeaders(shadowLeaders, latOffset, seen - turningDifference, lastLink, shadowLane, v, vLinkPass);
2448 4458586 : } else if (shadowLane == myLaneChangeModel->getShadowLane() && leaderLane == myLane) {
2449 : // check for leader vehicles driving in the opposite direction on the opposite-direction shadow lane
2450 : // (and thus in the same direction as ego)
2451 30474 : MSLeaderDistanceInfo shadowLeaders = shadowLane->getFollowersOnConsecutive(this, myLane->getOppositePos(getPositionOnLane()), true);
2452 : const double latOffset = 0;
2453 : #ifdef DEBUG_PLAN_MOVE
2454 : if (DEBUG_COND) {
2455 : std::cout << SIMTIME << " opposite shadows veh=" << getID() << " shadowLane=" << shadowLane->getID()
2456 : << " latOffset=" << latOffset << " shadowLeaders=" << shadowLeaders.toString() << "\n";
2457 : }
2458 : #endif
2459 30474 : shadowLeaders.fixOppositeGaps(true);
2460 : #ifdef DEBUG_PLAN_MOVE
2461 : if (DEBUG_COND) {
2462 : std::cout << " shadowLeadersFixed=" << shadowLeaders.toString() << "\n";
2463 : }
2464 : #endif
2465 30474 : adaptToLeaderDistance(shadowLeaders, latOffset, seen, lastLink, v, vLinkPass);
2466 30474 : }
2467 : }
2468 : }
2469 : // adapt to pedestrians on the same lane
2470 1211411136 : if (lane->getEdge().getPersons().size() > 0 && lane->hasPedestrians()) {
2471 193489 : const double relativePos = lane->getLength() - seen;
2472 : #ifdef DEBUG_PLAN_MOVE
2473 : if (DEBUG_COND) {
2474 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2475 : }
2476 : #endif
2477 193489 : const double stopTime = MAX2(1.0, ceil(getSpeed() / cfModel.getMaxDecel()));
2478 193489 : PersonDist leader = lane->nextBlocking(relativePos,
2479 193489 : getRightSideOnLane(lane), getRightSideOnLane(lane) + getVehicleType().getWidth(), stopTime);
2480 193489 : if (leader.first != 0) {
2481 20816 : const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2482 29481 : v = MIN2(v, stopSpeed);
2483 : #ifdef DEBUG_PLAN_MOVE
2484 : if (DEBUG_COND) {
2485 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2486 : }
2487 : #endif
2488 : }
2489 : }
2490 1211411136 : if (lane->getBidiLane() != nullptr) {
2491 : // adapt to pedestrians on the bidi lane
2492 3795434 : const MSLane* bidiLane = lane->getBidiLane();
2493 3795434 : if (bidiLane->getEdge().getPersons().size() > 0 && bidiLane->hasPedestrians()) {
2494 1028 : const double relativePos = seen;
2495 : #ifdef DEBUG_PLAN_MOVE
2496 : if (DEBUG_COND) {
2497 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2498 : }
2499 : #endif
2500 1028 : const double stopTime = ceil(getSpeed() / cfModel.getMaxDecel());
2501 1028 : const double leftSideOnLane = bidiLane->getWidth() - getRightSideOnLane(lane);
2502 1028 : PersonDist leader = bidiLane->nextBlocking(relativePos,
2503 1028 : leftSideOnLane - getVehicleType().getWidth(), leftSideOnLane, stopTime, true);
2504 1028 : if (leader.first != 0) {
2505 267 : const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2506 516 : v = MIN2(v, stopSpeed);
2507 : #ifdef DEBUG_PLAN_MOVE
2508 : if (DEBUG_COND) {
2509 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2510 : }
2511 : #endif
2512 : }
2513 : }
2514 : }
2515 : // adapt to vehicles blocked from (urgent) lane-changing
2516 1211411136 : if (!opposite && lane->getEdge().hasLaneChanger()) {
2517 596197499 : const double vHelp = myLaneChangeModel->getCooperativeHelpSpeed(lane, seen);
2518 : #ifdef DEBUG_PLAN_MOVE
2519 : if (DEBUG_COND && vHelp < v) {
2520 : std::cout << SIMTIME << " applying cooperativeHelpSpeed v=" << vHelp << "\n";
2521 : }
2522 : #endif
2523 596251909 : v = MIN2(v, vHelp);
2524 : }
2525 :
2526 : // process all stops and waypoints on the current edge
2527 : bool foundRealStop = false;
2528 : while (stopIt != myStops.end()
2529 53008688 : && ((&stopIt->lane->getEdge() == &lane->getEdge())
2530 26787594 : || (stopIt->isOpposite && stopIt->lane->getEdge().getOppositeEdge() == &lane->getEdge()))
2531 : // ignore stops that occur later in a looped route
2532 1253994715 : && stopIt->edge == myCurrEdge + view) {
2533 26165899 : double stopDist = std::numeric_limits<double>::max();
2534 : const MSStop& stop = *stopIt;
2535 : const bool isFirstStop = stopIt == myStops.begin();
2536 : stopIt++;
2537 26165899 : if (!stop.reached || (stop.getSpeed() > 0 && keepStopping())) {
2538 : // we are approaching a stop on the edge; must not drive further
2539 11116868 : bool isWaypoint = stop.getSpeed() > 0;
2540 11116868 : double endPos = stop.getEndPos(*this) + NUMERICAL_EPS;
2541 11116868 : if (stop.parkingarea != nullptr) {
2542 : // leave enough space so parking vehicles can exit
2543 1669002 : const double brakePos = getBrakeGap() + lane->getLength() - seen;
2544 1669002 : endPos = stop.parkingarea->getLastFreePosWithReservation(t, *this, brakePos);
2545 9447866 : } else if (isWaypoint && !stop.reached) {
2546 105654 : endPos = stop.pars.startPos;
2547 : }
2548 11116868 : stopDist = seen + endPos - lane->getLength();
2549 : #ifdef DEBUG_STOPS
2550 : if (DEBUG_COND) {
2551 : std::cout << SIMTIME << " veh=" << getID() << " stopDist=" << stopDist << " stopLane=" << stop.lane->getID() << " stopEndPos=" << endPos << "\n";
2552 : }
2553 : #endif
2554 : double stopSpeed = laneMaxV;
2555 11116868 : if (isWaypoint) {
2556 : bool waypointWithStop = false;
2557 121403 : if (stop.getUntil() > t) {
2558 : // check if we have to slow down or even stop
2559 : SUMOTime time2end = 0;
2560 4051 : if (stop.reached) {
2561 800 : time2end = TIME2STEPS((stop.pars.endPos - myState.myPos) / stop.getSpeed());
2562 : } else {
2563 3437 : time2end = TIME2STEPS(
2564 : // time to reach waypoint start
2565 : stopDist / ((getSpeed() + stop.getSpeed()) / 2)
2566 : // time to reach waypoint end
2567 : + (stop.pars.endPos - stop.pars.startPos) / stop.getSpeed());
2568 : }
2569 4051 : if (stop.getUntil() > t + time2end) {
2570 : // we need to stop
2571 : double distToEnd = stopDist;
2572 3915 : if (!stop.reached) {
2573 3191 : distToEnd += stop.pars.endPos - stop.pars.startPos;
2574 : }
2575 3915 : stopSpeed = MAX2(cfModel.stopSpeed(this, getSpeed(), distToEnd), vMinComfortable);
2576 : waypointWithStop = true;
2577 : }
2578 : }
2579 121403 : if (stop.reached) {
2580 14976 : stopSpeed = MIN2(stop.getSpeed(), stopSpeed);
2581 14976 : if (myState.myPos >= stop.pars.endPos && !waypointWithStop) {
2582 281 : stopDist = std::numeric_limits<double>::max();
2583 : }
2584 : } else {
2585 106427 : stopSpeed = MIN2(MAX2(cfModel.freeSpeed(this, getSpeed(), stopDist, stop.getSpeed()), vMinComfortable), stopSpeed);
2586 106427 : if (!stop.reached) {
2587 106427 : stopDist += stop.pars.endPos - stop.pars.startPos;
2588 : }
2589 106427 : if (lastLink != nullptr) {
2590 67988 : lastLink->adaptLeaveSpeed(cfModel.freeSpeed(this, vLinkPass, endPos, stop.getSpeed(), false, MSCFModel::CalcReason::FUTURE));
2591 : }
2592 : }
2593 : } else {
2594 10995465 : stopSpeed = cfModel.stopSpeed(this, getSpeed(), stopDist);
2595 10995465 : if (!instantStopping()) {
2596 : // regular stops are not emergencies
2597 : stopSpeed = MAX2(stopSpeed, vMinComfortable);
2598 20 : } else if (myInfluencer && !myInfluencer->hasSpeedTimeLine(SIMSTEP)) {
2599 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
2600 20 : speedTimeLine.push_back(std::make_pair(SIMSTEP, getSpeed()));
2601 20 : speedTimeLine.push_back(std::make_pair(SIMSTEP + DELTA_T, stopSpeed));
2602 20 : myInfluencer->setSpeedTimeLine(speedTimeLine);
2603 20 : }
2604 10995465 : if (lastLink != nullptr) {
2605 5355136 : lastLink->adaptLeaveSpeed(cfModel.stopSpeed(this, vLinkPass, endPos, MSCFModel::CalcReason::FUTURE));
2606 : }
2607 : }
2608 11116868 : newStopSpeed = MIN2(newStopSpeed, stopSpeed);
2609 11116868 : v = MIN2(v, stopSpeed);
2610 11116868 : if (lane->isInternal()) {
2611 6731 : std::vector<MSLink*>::const_iterator exitLink = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2612 : assert(!lane->isLinkEnd(exitLink));
2613 : bool dummySetRequest;
2614 : double dummyVLinkWait;
2615 6731 : checkLinkLeaderCurrentAndParallel(*exitLink, lane, seen, lastLink, v, vLinkPass, dummyVLinkWait, dummySetRequest);
2616 : }
2617 :
2618 : #ifdef DEBUG_PLAN_MOVE
2619 : if (DEBUG_COND) {
2620 : std::cout << "\n" << SIMTIME << " next stop: distance = " << stopDist << " requires stopSpeed = " << stopSpeed << "\n";
2621 :
2622 : }
2623 : #endif
2624 11116868 : if (isFirstStop) {
2625 10198207 : newStopDist = stopDist;
2626 : // if the vehicle is going to stop we don't need to look further
2627 : // (except for trains that make use of further link-approach registration for safety purposes)
2628 10198207 : if (!isWaypoint) {
2629 : planningToStop = true;
2630 10111381 : if (!isRail()) {
2631 9806575 : lfLinks.emplace_back(v, stopDist);
2632 : foundRealStop = true;
2633 : break;
2634 : }
2635 : }
2636 : }
2637 : }
2638 : }
2639 : if (foundRealStop) {
2640 : break;
2641 : }
2642 :
2643 : // move to next lane
2644 : // get the next link used
2645 1201604561 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2646 1201604561 : if (lane->isLinkEnd(link) && myLaneChangeModel->hasBlueLight() && myCurrEdge != myRoute->end() - 1) {
2647 : // emergency vehicle is on the wrong lane. Obtain the link that it would use from the correct turning lane
2648 : const int currentIndex = lane->getIndex();
2649 : const MSLane* bestJump = nullptr;
2650 194472 : for (const LaneQ& preb : getBestLanes()) {
2651 127903 : if (preb.allowsContinuation &&
2652 : (bestJump == nullptr
2653 3214 : || abs(currentIndex - preb.lane->getIndex()) < abs(currentIndex - bestJump->getIndex()))) {
2654 67575 : bestJump = preb.lane;
2655 : }
2656 : }
2657 66569 : if (bestJump != nullptr) {
2658 66569 : const MSEdge* nextEdge = *(myCurrEdge + 1);
2659 122912 : for (auto cand_it = bestJump->getLinkCont().begin(); cand_it != bestJump->getLinkCont().end(); cand_it++) {
2660 117530 : if (&(*cand_it)->getLane()->getEdge() == nextEdge) {
2661 : link = cand_it;
2662 : break;
2663 : }
2664 : }
2665 : }
2666 : }
2667 :
2668 : // Check whether this is a turn (to save info about the next upcoming turn)
2669 1201604561 : if (!encounteredTurn) {
2670 187720837 : if (!lane->isLinkEnd(link) && lane->getLinkCont().size() > 1) {
2671 18483863 : LinkDirection linkDir = (*link)->getDirection();
2672 18483863 : switch (linkDir) {
2673 : case LinkDirection::STRAIGHT:
2674 : case LinkDirection::NODIR:
2675 : break;
2676 6805059 : default:
2677 6805059 : nextTurn.first = seen;
2678 6805059 : nextTurn.second = *link;
2679 : encounteredTurn = true;
2680 : #ifdef DEBUG_NEXT_TURN
2681 : if (DEBUG_COND) {
2682 : std::cout << SIMTIME << " veh '" << getID() << "' nextTurn: " << toString(linkDir)
2683 : << " at " << nextTurn.first << "m." << std::endl;
2684 : }
2685 : #endif
2686 : }
2687 : }
2688 : }
2689 :
2690 : // check whether the vehicle is on its final edge
2691 2071290341 : if (myCurrEdge + view + 1 == myRoute->end()
2692 1201604561 : || (myParameter->arrivalEdge >= 0 && getRoutePosition() + view == myParameter->arrivalEdge)) {
2693 331918781 : const double arrivalSpeed = (myParameter->arrivalSpeedProcedure == ArrivalSpeedDefinition::GIVEN ?
2694 : myParameter->arrivalSpeed : laneMaxV);
2695 : // subtract the arrival speed from the remaining distance so we get one additional driving step with arrival speed
2696 : // XXX: This does not work for ballistic update refs #2579
2697 331918781 : const double distToArrival = seen + myArrivalPos - lane->getLength() - SPEED2DIST(arrivalSpeed);
2698 331918781 : const double va = MAX2(NUMERICAL_EPS, cfModel.freeSpeed(this, getSpeed(), distToArrival, arrivalSpeed));
2699 331918781 : v = MIN2(v, va);
2700 331918781 : if (lastLink != nullptr) {
2701 : lastLink->adaptLeaveSpeed(va);
2702 : }
2703 331918781 : lfLinks.push_back(DriveProcessItem(v, seen, lane->getEdge().isFringe() ? 1000 : 0));
2704 331918781 : break;
2705 : }
2706 : // check whether the lane or the shadowLane is a dead end (allow some leeway on intersections)
2707 : if (lane->isLinkEnd(link)
2708 859096633 : || (MSGlobals::gSublane && brakeForOverlap(*link, lane))
2709 1728441715 : || (opposite && (*link)->getViaLaneOrLane()->getParallelOpposite() == nullptr
2710 212760 : && !myLaneChangeModel->hasBlueLight())) {
2711 11181314 : double va = cfModel.stopSpeed(this, getSpeed(), seen);
2712 11181314 : if (lastLink != nullptr) {
2713 : lastLink->adaptLeaveSpeed(va);
2714 : }
2715 11181314 : if (myLaneChangeModel->getCommittedSpeed() > 0) {
2716 362609 : v = MIN2(myLaneChangeModel->getCommittedSpeed(), v);
2717 : } else {
2718 21023926 : v = MIN2(va, v);
2719 : }
2720 : #ifdef DEBUG_PLAN_MOVE
2721 : if (DEBUG_COND) {
2722 : std::cout << " braking for link end lane=" << lane->getID() << " seen=" << seen
2723 : << " overlap=" << getLateralOverlap() << " va=" << va << " committed=" << myLaneChangeModel->getCommittedSpeed() << " v=" << v << "\n";
2724 :
2725 : }
2726 : #endif
2727 11181314 : if (lane->isLinkEnd(link)) {
2728 10589147 : lfLinks.emplace_back(v, seen);
2729 : break;
2730 : }
2731 : }
2732 859096633 : lateralShift += (*link)->getLateralShift();
2733 859096633 : const bool yellowOrRed = (*link)->haveRed() || (*link)->haveYellow();
2734 : // We distinguish 3 cases when determining the point at which a vehicle stops:
2735 : // - allway_stop: the vehicle should stop close to the stop line but may stop at larger distance
2736 : // - red/yellow light: here the vehicle 'knows' that it will have priority eventually and does not need to stop on a precise spot
2737 : // - other types of minor links: the vehicle needs to stop as close to the junction as necessary
2738 : // to minimize the time window for passing the junction. If the
2739 : // vehicle 'decides' to accelerate and cannot enter the junction in
2740 : // the next step, new foes may appear and cause a collision (see #1096)
2741 : // - major links: stopping point is irrelevant
2742 : double laneStopOffset;
2743 859096633 : const double majorStopOffset = MAX2(getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP, DIST_TO_STOPLINE_EXPECT_PRIORITY), lane->getVehicleStopOffset(this));
2744 : // override low desired decel at yellow and red
2745 859096633 : const double stopDecel = yellowOrRed && !isRail() ? MAX2(MIN2(MSGlobals::gTLSYellowMinDecel, cfModel.getEmergencyDecel()), cfModel.getMaxDecel()) : cfModel.getMaxDecel();
2746 859096633 : const double brakeDist = cfModel.brakeGap(myState.mySpeed, stopDecel, 0);
2747 859096633 : const bool canBrakeBeforeLaneEnd = seen >= brakeDist;
2748 859096633 : const bool canBrakeBeforeStopLine = seen - lane->getVehicleStopOffset(this) >= brakeDist;
2749 859096633 : if (yellowOrRed) {
2750 : // Wait at red traffic light with full distance if possible
2751 : laneStopOffset = majorStopOffset;
2752 798078928 : } else if ((*link)->havePriority()) {
2753 : // On priority link, we should never stop below visibility distance
2754 756546156 : laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, majorStopOffset);
2755 : } else {
2756 41532772 : double minorStopOffset = MAX2(lane->getVehicleStopOffset(this),
2757 41532772 : getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP) - (*link)->getDistToFoePedCrossing());
2758 : #ifdef DEBUG_PLAN_MOVE
2759 : if (DEBUG_COND) {
2760 : std::cout << " minorStopOffset=" << minorStopOffset << " distToFoePedCrossing=" << (*link)->getDistToFoePedCrossing() << "\n";
2761 : }
2762 : #endif
2763 41532772 : if ((*link)->getState() == LINKSTATE_ALLWAY_STOP) {
2764 1425292 : minorStopOffset = MAX2(minorStopOffset, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP, 0));
2765 : } else {
2766 40107480 : minorStopOffset = MAX2(minorStopOffset, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP_MINOR, 0));
2767 : }
2768 : // On minor link, we should likewise never stop below visibility distance
2769 41532772 : laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, minorStopOffset);
2770 : }
2771 : #ifdef DEBUG_PLAN_MOVE
2772 : if (DEBUG_COND) {
2773 : std::cout << SIMTIME << " veh=" << getID() << " desired stopOffset on lane '" << lane->getID() << "' is " << laneStopOffset << "\n";
2774 : }
2775 : #endif
2776 859096633 : if (canBrakeBeforeLaneEnd) {
2777 : // avoid emergency braking if possible
2778 830841525 : laneStopOffset = MIN2(laneStopOffset, seen - brakeDist);
2779 : }
2780 : laneStopOffset = MAX2(POSITION_EPS, laneStopOffset);
2781 859096633 : double stopDist = MAX2(0., seen - laneStopOffset);
2782 61017705 : if (yellowOrRed && getDevice(typeid(MSDevice_GLOSA)) != nullptr
2783 508 : && static_cast<MSDevice_GLOSA*>(getDevice(typeid(MSDevice_GLOSA)))->getOverrideSafety()
2784 859096633 : && static_cast<MSDevice_GLOSA*>(getDevice(typeid(MSDevice_GLOSA)))->isSpeedAdviceActive()) {
2785 : stopDist = std::numeric_limits<double>::max();
2786 : }
2787 859096633 : if (newStopDist != std::numeric_limits<double>::max()) {
2788 : stopDist = MAX2(stopDist, newStopDist);
2789 : }
2790 : #ifdef DEBUG_PLAN_MOVE
2791 : if (DEBUG_COND) {
2792 : std::cout << SIMTIME << " veh=" << getID() << " effective stopOffset on lane '" << lane->getID()
2793 : << "' is " << laneStopOffset << " (-> stopDist=" << stopDist << ")" << std::endl;
2794 : }
2795 : #endif
2796 859096633 : if (isRail()
2797 859096633 : && !lane->isInternal()) {
2798 : // check for train direction reversal
2799 2986148 : if (lane->getBidiLane() != nullptr
2800 2986148 : && (*link)->getLane()->getBidiLane() == lane) {
2801 629981 : double vMustReverse = getCarFollowModel().stopSpeed(this, getSpeed(), seen - POSITION_EPS);
2802 629981 : if (seen < 1) {
2803 2261 : mustSeeBeforeReversal = 2 * seen + getLength();
2804 : }
2805 1219258 : v = MIN2(v, vMustReverse);
2806 : }
2807 : // signal that is passed in the current step does not count
2808 5972296 : foundRailSignal |= ((*link)->getTLLogic() != nullptr
2809 656307 : && (*link)->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL
2810 3541900 : && seen > SPEED2DIST(v));
2811 : }
2812 :
2813 859096633 : bool canReverseEventually = false;
2814 859096633 : const double vReverse = checkReversal(canReverseEventually, laneMaxV, seen);
2815 859096633 : v = MIN2(v, vReverse);
2816 : #ifdef DEBUG_PLAN_MOVE
2817 : if (DEBUG_COND) {
2818 : std::cout << SIMTIME << " veh=" << getID() << " canReverseEventually=" << canReverseEventually << " v=" << v << "\n";
2819 : }
2820 : #endif
2821 :
2822 : // check whether we need to slow down in order to finish a continuous lane change
2823 859096633 : if (myLaneChangeModel->isChangingLanes()) {
2824 : if ( // slow down to finish lane change before a turn lane
2825 161227 : ((*link)->getDirection() == LinkDirection::LEFT || (*link)->getDirection() == LinkDirection::RIGHT) ||
2826 : // slow down to finish lane change before the shadow lane ends
2827 137738 : (myLaneChangeModel->getShadowLane() != nullptr &&
2828 137738 : (*link)->getViaLaneOrLane()->getParallelLane(myLaneChangeModel->getShadowDirection()) == nullptr)) {
2829 : // XXX maybe this is too harsh. Vehicles could cut some corners here
2830 46945 : const double timeRemaining = STEPS2TIME(myLaneChangeModel->remainingTime());
2831 : assert(timeRemaining != 0);
2832 : // XXX: Euler-logic (#860), but I couldn't identify problems from this yet (Leo). Refs. #2575
2833 46945 : const double va = MAX2(cfModel.stopSpeed(this, getSpeed(), seen - POSITION_EPS),
2834 46945 : (seen - POSITION_EPS) / timeRemaining);
2835 : #ifdef DEBUG_PLAN_MOVE
2836 : if (DEBUG_COND) {
2837 : std::cout << SIMTIME << " veh=" << getID() << " slowing down to finish continuous change before"
2838 : << " link=" << (*link)->getViaLaneOrLane()->getID()
2839 : << " timeRemaining=" << timeRemaining
2840 : << " v=" << v
2841 : << " va=" << va
2842 : << std::endl;
2843 : }
2844 : #endif
2845 93689 : v = MIN2(va, v);
2846 : }
2847 : }
2848 :
2849 : // - always issue a request to leave the intersection we are currently on
2850 859096633 : const bool leavingCurrentIntersection = myLane->getEdge().isInternal() && lastLink == nullptr;
2851 : // - do not issue a request to enter an intersection after we already slowed down for an earlier one
2852 859096633 : const bool abortRequestAfterMinor = slowedDownForMinor && (*link)->getInternalLaneBefore() == nullptr;
2853 : // - even if red, if we cannot break we should issue a request
2854 859096633 : bool setRequest = (v > NUMERICAL_EPS_SPEED && !abortRequestAfterMinor) || (leavingCurrentIntersection);
2855 :
2856 859096633 : double stopSpeed = cfModel.stopSpeed(this, getSpeed(), stopDist, stopDecel, MSCFModel::CalcReason::CURRENT_WAIT);
2857 859096633 : double vLinkWait = MIN2(v, stopSpeed);
2858 : #ifdef DEBUG_PLAN_MOVE
2859 : if (DEBUG_COND) {
2860 : std::cout
2861 : << " stopDist=" << stopDist
2862 : << " stopDecel=" << stopDecel
2863 : << " vLinkWait=" << vLinkWait
2864 : << " brakeDist=" << brakeDist
2865 : << " seen=" << seen
2866 : << " leaveIntersection=" << leavingCurrentIntersection
2867 : << " setRequest=" << setRequest
2868 : //<< std::setprecision(16)
2869 : //<< " v=" << v
2870 : //<< " speedEps=" << NUMERICAL_EPS_SPEED
2871 : //<< std::setprecision(gPrecision)
2872 : << "\n";
2873 : }
2874 : #endif
2875 :
2876 859096633 : if (yellowOrRed && canBrakeBeforeStopLine && !ignoreRed(*link, canBrakeBeforeStopLine) && seen >= mustSeeBeforeReversal) {
2877 60955880 : if (lane->isInternal()) {
2878 42351 : checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2879 : }
2880 : // arrivalSpeed / arrivalTime when braking for red light is only relevent for rail signal switching
2881 60955880 : const SUMOTime arrivalTime = getArrivalTime(t, seen, v, vLinkPass);
2882 : // the vehicle is able to brake in front of a yellow/red traffic light
2883 60955880 : lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, false, arrivalTime, vLinkWait, 0, seen, -1));
2884 : //lfLinks.push_back(DriveProcessItem(0, vLinkWait, vLinkWait, false, 0, 0, stopDist));
2885 60955880 : break;
2886 : }
2887 :
2888 798140753 : const MSLink* entryLink = (*link)->getCorrespondingEntryLink();
2889 798140753 : if (entryLink->haveRed() && ignoreRed(*link, canBrakeBeforeStopLine) && STEPS2TIME(t - entryLink->getLastStateChange()) > 2) {
2890 : // restrict speed when ignoring a red light
2891 118482 : const double redSpeed = MIN2(v, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_RED_SPEED, v));
2892 118482 : const double va = MAX2(redSpeed, cfModel.freeSpeed(this, getSpeed(), seen, redSpeed));
2893 236516 : v = MIN2(va, v);
2894 : #ifdef DEBUG_PLAN_MOVE
2895 : if (DEBUG_COND) std::cout
2896 : << " ignoreRed spent=" << STEPS2TIME(t - (*link)->getLastStateChange())
2897 : << " redSpeed=" << redSpeed
2898 : << " va=" << va
2899 : << " v=" << v
2900 : << "\n";
2901 : #endif
2902 : }
2903 :
2904 798140753 : checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2905 :
2906 798140753 : if (lastLink != nullptr) {
2907 : lastLink->adaptLeaveSpeed(laneMaxV);
2908 : }
2909 798140753 : double arrivalSpeed = vLinkPass;
2910 : // vehicles should decelerate when approaching a minor link
2911 : // - unless they are close enough to have clear visibility of all relevant foe lanes and may start to accelerate again
2912 : // - and unless they are so close that stopping is impossible (i.e. when a green light turns to yellow when close to the junction)
2913 :
2914 : // whether the vehicle/driver is close enough to the link to see all possible foes #2123
2915 798140753 : const double visibilityDistance = (*link)->getFoeVisibilityDistance();
2916 798140753 : const double determinedFoePresence = seen <= visibilityDistance;
2917 : // // VARIANT: account for time needed to recognize whether relevant vehicles are on the foe lanes. (Leo)
2918 : // double foeRecognitionTime = 0.0;
2919 : // double determinedFoePresence = seen < visibilityDistance - myState.mySpeed*foeRecognitionTime;
2920 :
2921 : #ifdef DEBUG_PLAN_MOVE
2922 : if (DEBUG_COND) {
2923 : std::cout << " approaching link=" << (*link)->getViaLaneOrLane()->getID() << " prio=" << (*link)->havePriority() << " seen=" << seen << " visibilityDistance=" << visibilityDistance << " brakeDist=" << brakeDist << "\n";
2924 : }
2925 : #endif
2926 :
2927 798140753 : const bool couldBrakeForMinor = !(*link)->havePriority() && brakeDist < seen && !(*link)->lastWasContMajor();
2928 40975876 : if (couldBrakeForMinor && !determinedFoePresence) {
2929 : // vehicle decelerates just enough to be able to stop if necessary and then accelerates
2930 38274964 : double maxSpeedAtVisibilityDist = cfModel.maximumSafeStopSpeed(visibilityDistance, cfModel.getMaxDecel(), myState.mySpeed, false, 0., false);
2931 : // XXX: estimateSpeedAfterDistance does not use euler-logic (thus returns a lower value than possible here...)
2932 38274964 : double maxArrivalSpeed = cfModel.estimateSpeedAfterDistance(visibilityDistance, maxSpeedAtVisibilityDist, cfModel.getMaxAccel());
2933 38274964 : arrivalSpeed = MIN2(vLinkPass, maxArrivalSpeed);
2934 : slowedDownForMinor = true;
2935 : #ifdef DEBUG_PLAN_MOVE
2936 : if (DEBUG_COND) {
2937 : std::cout << " slowedDownForMinor maxSpeedAtVisDist=" << maxSpeedAtVisibilityDist << " maxArrivalSpeed=" << maxArrivalSpeed << " arrivalSpeed=" << arrivalSpeed << "\n";
2938 : }
2939 : #endif
2940 759865789 : } else if ((*link)->getState() == LINKSTATE_EQUAL && myWaitingTime > 0) {
2941 : // check for deadlock (circular yielding)
2942 : //std::cout << SIMTIME << " veh=" << getID() << " check rbl-deadlock\n";
2943 2866 : std::pair<const SUMOVehicle*, const MSLink*> blocker = (*link)->getFirstApproachingFoe(*link);
2944 : //std::cout << " blocker=" << Named::getIDSecure(blocker.first) << "\n";
2945 : int n = 100;
2946 5679 : while (blocker.second != nullptr && blocker.second != *link && n > 0) {
2947 2813 : blocker = blocker.second->getFirstApproachingFoe(*link);
2948 2813 : n--;
2949 : //std::cout << " blocker=" << Named::getIDSecure(blocker.first) << "\n";
2950 : }
2951 2866 : if (n == 0) {
2952 0 : WRITE_WARNINGF(TL("Suspicious right_before_left junction '%'."), lane->getEdge().getToJunction()->getID());
2953 : }
2954 : //std::cout << " blockerLink=" << blocker.second << " link=" << *link << "\n";
2955 2866 : if (blocker.second == *link) {
2956 488 : const double threshold = (*link)->getDirection() == LinkDirection::STRAIGHT ? 0.25 : 0.75;
2957 488 : if (RandHelper::rand(getRNG()) < threshold) {
2958 : //std::cout << " abort request, threshold=" << threshold << "\n";
2959 310 : setRequest = false;
2960 : }
2961 : }
2962 : }
2963 :
2964 798140753 : const SUMOTime arrivalTime = getArrivalTime(t, seen, v, arrivalSpeed);
2965 798140753 : if (couldBrakeForMinor && determinedFoePresence && (*link)->getLane()->getEdge().isRoundabout()) {
2966 880239 : const bool wasOpened = (*link)->opened(arrivalTime, arrivalSpeed, arrivalSpeed,
2967 880239 : getLength(), getImpatience(),
2968 : getCarFollowModel().getMaxDecel(),
2969 880239 : getWaitingTime(), getLateralPositionOnLane(),
2970 : nullptr, false, this);
2971 880239 : if (!wasOpened) {
2972 : slowedDownForMinor = true;
2973 : }
2974 : #ifdef DEBUG_PLAN_MOVE
2975 : if (DEBUG_COND) {
2976 : std::cout << " slowedDownForMinor at roundabout=" << (!wasOpened) << "\n";
2977 : }
2978 : #endif
2979 : }
2980 :
2981 : // compute arrival speed and arrival time if vehicle starts braking now
2982 : // if stopping is possible, arrivalTime can be arbitrarily large. A small value keeps fractional times (impatience) meaningful
2983 : double arrivalSpeedBraking = 0;
2984 798140753 : const double bGap = cfModel.brakeGap(v);
2985 798140753 : if (seen < bGap && !isStopped() && !planningToStop) { // XXX: should this use the current speed (at least for the ballistic case)? (Leo) Refs. #2575
2986 : // vehicle cannot come to a complete stop in time
2987 58730874 : if (MSGlobals::gSemiImplicitEulerUpdate) {
2988 56056654 : arrivalSpeedBraking = cfModel.getMinimalArrivalSpeedEuler(seen, v);
2989 : // due to discrete/continuous mismatch (when using Euler update) we have to ensure that braking actually helps
2990 : arrivalSpeedBraking = MIN2(arrivalSpeedBraking, arrivalSpeed);
2991 : } else {
2992 2674220 : arrivalSpeedBraking = cfModel.getMinimalArrivalSpeed(seen, myState.mySpeed);
2993 : }
2994 : }
2995 :
2996 : // estimate leave speed for passing time computation
2997 : // l=linkLength, a=accel, t=continuousTime, v=vLeave
2998 : // l=v*t + 0.5*a*t^2, solve for t and multiply with a, then add v
2999 1183435816 : const double estimatedLeaveSpeed = MIN2((*link)->getViaLaneOrLane()->getVehicleMaxSpeed(this, maxVD),
3000 798140753 : getCarFollowModel().estimateSpeedAfterDistance((*link)->getLength(), arrivalSpeed, getVehicleType().getCarFollowModel().getMaxAccel()));
3001 798140753 : lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, setRequest,
3002 : arrivalTime, arrivalSpeed,
3003 : arrivalSpeedBraking,
3004 : seen, estimatedLeaveSpeed));
3005 798140753 : if ((*link)->getViaLane() == nullptr) {
3006 : hadNonInternal = true;
3007 : ++view;
3008 : }
3009 : #ifdef DEBUG_PLAN_MOVE
3010 : if (DEBUG_COND) {
3011 : std::cout << " checkAbort setRequest=" << setRequest << " v=" << v << " seen=" << seen << " dist=" << dist
3012 : << " seenNonInternal=" << seenNonInternal
3013 : << " seenInternal=" << seenInternal << " length=" << vehicleLength << "\n";
3014 : }
3015 : #endif
3016 : // we need to look ahead far enough to see available space for checkRewindLinkLanes
3017 824089739 : if ((!setRequest || v <= 0 || seen > dist) && hadNonInternal && seenNonInternal > MAX2(vehicleLength * CRLL_LOOK_AHEAD, vehicleLength + seenInternal) && foundRailSignal) {
3018 : break;
3019 : }
3020 : // get the following lane
3021 : lane = (*link)->getViaLaneOrLane();
3022 578033139 : laneMaxV = lane->getVehicleMaxSpeed(this, maxVD);
3023 578431300 : if (myInfluencer && !myInfluencer->considerSpeedLimit()) {
3024 : laneMaxV = std::numeric_limits<double>::max();
3025 : }
3026 : // the link was passed
3027 : // compute the velocity to use when the link is not blocked by other vehicles
3028 : // the vehicle shall be not faster when reaching the next lane than allowed
3029 : // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
3030 578033139 : const double va = MAX2(cfModel.freeSpeed(this, getSpeed(), seen, laneMaxV), vMinComfortable - NUMERICAL_EPS);
3031 1144570077 : v = MIN2(va, v);
3032 : #ifdef DEBUG_PLAN_MOVE
3033 : if (DEBUG_COND) {
3034 : std::cout << " laneMaxV=" << laneMaxV << " freeSpeed=" << va << " v=" << v << "\n";
3035 : }
3036 : #endif
3037 578033139 : if (lane->getEdge().isInternal()) {
3038 250798444 : seenInternal += lane->getLength();
3039 : } else {
3040 327234695 : seenNonInternal += lane->getLength();
3041 : }
3042 : // do not restrict results to the current vehicle to allow caching for the current time step
3043 578033139 : leaderLane = opposite ? lane->getParallelOpposite() : lane;
3044 578033139 : if (leaderLane == nullptr) {
3045 :
3046 : break;
3047 : }
3048 1155644900 : ahead = opposite ? MSLeaderInfo(leaderLane->getWidth()) : leaderLane->getLastVehicleInformation(nullptr, 0);
3049 577822450 : seen += lane->getLength();
3050 1155644900 : vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(lane->getLength(), v, cfModel.getMaxAccel()), laneMaxV); // upper bound
3051 : lastLink = &lfLinks.back();
3052 577822450 : }
3053 :
3054 : //#ifdef DEBUG_PLAN_MOVE
3055 : // if(DEBUG_COND){
3056 : // std::cout << "planMoveInternal found safe speed v = " << v << std::endl;
3057 : // }
3058 : //#endif
3059 :
3060 : #ifdef PARALLEL_STOPWATCH
3061 : myLane->getStopWatch()[0].stop();
3062 : #endif
3063 633588686 : }
3064 :
3065 :
3066 : double
3067 2786 : MSVehicle::slowDownForSchedule(double vMinComfortable) const {
3068 2786 : const double sfp = getVehicleType().getParameter().speedFactorPremature;
3069 : const MSStop& stop = myStops.front();
3070 2786 : std::pair<double, double> timeDist = estimateTimeToNextStop();
3071 2786 : double arrivalDelay = SIMTIME + timeDist.first - STEPS2TIME(stop.pars.arrival);
3072 2786 : double t = STEPS2TIME(stop.pars.arrival - SIMSTEP);
3073 5572 : if (stop.pars.hasParameter(toString(SUMO_ATTR_FLEX_ARRIVAL))) {
3074 150 : SUMOTime flexStart = string2time(stop.pars.getParameter(toString(SUMO_ATTR_FLEX_ARRIVAL)));
3075 75 : arrivalDelay += STEPS2TIME(stop.pars.arrival - flexStart);
3076 75 : t = STEPS2TIME(flexStart - SIMSTEP);
3077 2711 : } else if (stop.pars.started >= 0 && MSGlobals::gUseStopStarted) {
3078 200 : arrivalDelay += STEPS2TIME(stop.pars.arrival - stop.pars.started);
3079 200 : t = STEPS2TIME(stop.pars.started - SIMSTEP);
3080 : }
3081 2786 : if (arrivalDelay < 0 && sfp < getChosenSpeedFactor()) {
3082 : // we can slow down to better match the schedule (and increase energy efficiency)
3083 2721 : const double vSlowDownMin = MAX2(myLane->getSpeedLimit() * sfp, vMinComfortable);
3084 2721 : const double s = timeDist.second;
3085 : const double b = getCarFollowModel().getMaxDecel();
3086 : // x = speed for arriving in t seconds
3087 : // u = time at full speed
3088 : // u * x + (t - u) * 0.5 * x = s
3089 : // t - u = x / b
3090 : // eliminate u, solve x
3091 2721 : const double radicand = 4 * t * t * b * b - 8 * s * b;
3092 2721 : const double x = radicand >= 0 ? t * b - sqrt(radicand) * 0.5 : vSlowDownMin;
3093 2721 : double vSlowDown = x < vSlowDownMin ? vSlowDownMin : x;
3094 : #ifdef DEBUG_PLAN_MOVE
3095 : if (DEBUG_COND) {
3096 : std::cout << SIMTIME << " veh=" << getID() << " ad=" << arrivalDelay << " t=" << t << " vsm=" << vSlowDownMin
3097 : << " r=" << radicand << " vs=" << vSlowDown << "\n";
3098 : }
3099 : #endif
3100 2721 : return vSlowDown;
3101 65 : } else if (arrivalDelay > 0 && sfp > getChosenSpeedFactor()) {
3102 : // in principle we could up to catch up with the schedule
3103 : // but at this point we can only lower the speed, the
3104 : // information would have to be used when computing getVehicleMaxSpeed
3105 : }
3106 65 : return getMaxSpeed();
3107 : }
3108 :
3109 : SUMOTime
3110 859096633 : MSVehicle::getArrivalTime(SUMOTime t, double seen, double v, double arrivalSpeed) const {
3111 : const MSCFModel& cfModel = getCarFollowModel();
3112 : SUMOTime arrivalTime;
3113 859096633 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3114 : // @note intuitively it would make sense to compare arrivalSpeed with getSpeed() instead of v
3115 : // however, due to the current position update rule (ticket #860) the vehicle moves with v in this step
3116 : // subtract DELTA_T because t is the time at the end of this step and the movement is not carried out yet
3117 802683548 : arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, v, arrivalSpeed);
3118 : } else {
3119 56413085 : arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, myState.mySpeed, arrivalSpeed);
3120 : }
3121 859096633 : if (isStopped()) {
3122 2351297 : arrivalTime += MAX2((SUMOTime)0, myStops.front().duration);
3123 : }
3124 859096633 : return arrivalTime;
3125 : }
3126 :
3127 :
3128 : void
3129 1216388827 : MSVehicle::adaptToLeaders(const MSLeaderInfo& ahead, double latOffset,
3130 : const double seen, DriveProcessItem* const lastLink,
3131 : const MSLane* const lane, double& v, double& vLinkPass) const {
3132 : int rightmost;
3133 : int leftmost;
3134 1216388827 : ahead.getSubLanes(this, latOffset, rightmost, leftmost);
3135 : #ifdef DEBUG_PLAN_MOVE
3136 : if (DEBUG_COND) std::cout << SIMTIME
3137 : << "\nADAPT_TO_LEADERS\nveh=" << getID()
3138 : << " lane=" << lane->getID()
3139 : << " latOffset=" << latOffset
3140 : << " rm=" << rightmost
3141 : << " lm=" << leftmost
3142 : << " shift=" << ahead.getSublaneOffset()
3143 : << " ahead=" << ahead.toString()
3144 : << "\n";
3145 : #endif
3146 : /*
3147 : if (myLaneChangeModel->getCommittedSpeed() > 0) {
3148 : v = MIN2(v, myLaneChangeModel->getCommittedSpeed());
3149 : vLinkPass = MIN2(vLinkPass, myLaneChangeModel->getCommittedSpeed());
3150 : #ifdef DEBUG_PLAN_MOVE
3151 : if (DEBUG_COND) std::cout << " hasCommitted=" << myLaneChangeModel->getCommittedSpeed() << "\n";
3152 : #endif
3153 : return;
3154 : }
3155 : */
3156 2972816625 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
3157 1756427798 : const MSVehicle* pred = ahead[sublane];
3158 1756427798 : if (pred != nullptr && pred != this) {
3159 : // @todo avoid multiple adaptations to the same leader
3160 1289989335 : const double predBack = pred->getBackPositionOnLane(lane);
3161 : double gap = (lastLink == nullptr
3162 1848657368 : ? predBack - myState.myPos - getVehicleType().getMinGap()
3163 558668033 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3164 : bool oncoming = false;
3165 1289989335 : if (myLaneChangeModel->isOpposite()) {
3166 26897 : if (pred->getLaneChangeModel().isOpposite() || lane == pred->getLaneChangeModel().getShadowLane()) {
3167 : // ego might and leader are driving against lane
3168 : gap = (lastLink == nullptr
3169 0 : ? myState.myPos - predBack - getVehicleType().getMinGap()
3170 0 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3171 : } else {
3172 : // ego and leader are driving in the same direction as lane (shadowlane for ego)
3173 : gap = (lastLink == nullptr
3174 27598 : ? predBack - (myLane->getLength() - myState.myPos) - getVehicleType().getMinGap()
3175 701 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3176 : }
3177 1289962438 : } else if (pred->getLaneChangeModel().isOpposite() && pred->getLaneChangeModel().getShadowLane() != lane) {
3178 : // must react to stopped / dangerous oncoming vehicles
3179 188349 : gap += -pred->getVehicleType().getLength() + getVehicleType().getMinGap() - MAX2(getVehicleType().getMinGap(), pred->getVehicleType().getMinGap());
3180 : // try to avoid collision in the next second
3181 188349 : const double predMaxDist = pred->getSpeed() + pred->getCarFollowModel().getMaxAccel();
3182 : #ifdef DEBUG_PLAN_MOVE
3183 : if (DEBUG_COND) {
3184 : std::cout << " fixedGap=" << gap << " predMaxDist=" << predMaxDist << "\n";
3185 : }
3186 : #endif
3187 188349 : if (gap < predMaxDist + getSpeed() || pred->getLane() == lane->getBidiLane()) {
3188 20602 : gap -= predMaxDist;
3189 : }
3190 1289774089 : } else if (pred->getLane() == lane->getBidiLane()) {
3191 46867 : gap -= pred->getVehicleType().getLengthWithGap();
3192 : oncoming = true;
3193 : }
3194 : #ifdef DEBUG_PLAN_MOVE
3195 : if (DEBUG_COND) {
3196 : std::cout << " pred=" << pred->getID() << " predLane=" << pred->getLane()->getID() << " predPos=" << pred->getPositionOnLane() << " gap=" << gap << " predBack=" << predBack << " seen=" << seen << " lane=" << lane->getID() << " myLane=" << myLane->getID() << " lastLink=" << (lastLink == nullptr ? "NULL" : lastLink->myLink->getDescription()) << " oncoming=" << oncoming << "\n";
3197 : }
3198 : #endif
3199 46867 : if (oncoming && gap >= 0) {
3200 46856 : adaptToOncomingLeader(std::make_pair(pred, gap), lastLink, v, vLinkPass);
3201 : } else {
3202 1289942479 : adaptToLeader(std::make_pair(pred, gap), seen, lastLink, v, vLinkPass);
3203 : }
3204 : }
3205 : }
3206 1216388827 : }
3207 :
3208 : void
3209 426841 : MSVehicle::adaptToLeaderDistance(const MSLeaderDistanceInfo& ahead, double latOffset,
3210 : double seen,
3211 : DriveProcessItem* const lastLink,
3212 : double& v, double& vLinkPass) const {
3213 : int rightmost;
3214 : int leftmost;
3215 426841 : ahead.getSubLanes(this, latOffset, rightmost, leftmost);
3216 : #ifdef DEBUG_PLAN_MOVE
3217 : if (DEBUG_COND) std::cout << SIMTIME
3218 : << "\nADAPT_TO_LEADERS_DISTANCE\nveh=" << getID()
3219 : << " latOffset=" << latOffset
3220 : << " rm=" << rightmost
3221 : << " lm=" << leftmost
3222 : << " ahead=" << ahead.toString()
3223 : << "\n";
3224 : #endif
3225 1046288 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
3226 619447 : CLeaderDist predDist = ahead[sublane];
3227 619447 : const MSVehicle* pred = predDist.first;
3228 619447 : if (pred != nullptr && pred != this) {
3229 : #ifdef DEBUG_PLAN_MOVE
3230 : if (DEBUG_COND) {
3231 : std::cout << " pred=" << pred->getID() << " predLane=" << pred->getLane()->getID() << " predPos=" << pred->getPositionOnLane() << " gap=" << predDist.second << "\n";
3232 : }
3233 : #endif
3234 393073 : adaptToLeader(predDist, seen, lastLink, v, vLinkPass);
3235 : }
3236 : }
3237 426841 : }
3238 :
3239 :
3240 : void
3241 1290335552 : MSVehicle::adaptToLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3242 : double seen,
3243 : DriveProcessItem* const lastLink,
3244 : double& v, double& vLinkPass) const {
3245 1290335552 : if (leaderInfo.first != 0) {
3246 1290335552 : if (ignoreFoe(leaderInfo.first)) {
3247 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3248 : if (DEBUG_COND) {
3249 : std::cout << " foe ignored\n";
3250 : }
3251 : #endif
3252 : return;
3253 : }
3254 : const MSCFModel& cfModel = getCarFollowModel();
3255 : double vsafeLeader = 0;
3256 1290334732 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
3257 : vsafeLeader = -std::numeric_limits<double>::max();
3258 : }
3259 : bool backOnRoute = true;
3260 1290334732 : if (leaderInfo.second < 0 && lastLink != nullptr && lastLink->myLink != nullptr) {
3261 : backOnRoute = false;
3262 : // this can either be
3263 : // a) a merging situation (leader back is is not our route) or
3264 : // b) a minGap violation / collision
3265 : MSLane* current = lastLink->myLink->getViaLaneOrLane();
3266 302895 : if (leaderInfo.first->getBackLane() == current) {
3267 : backOnRoute = true;
3268 : } else {
3269 674087 : for (MSLane* lane : getBestLanesContinuation()) {
3270 597450 : if (lane == current) {
3271 : break;
3272 : }
3273 444915 : if (leaderInfo.first->getBackLane() == lane) {
3274 : backOnRoute = true;
3275 : }
3276 : }
3277 : }
3278 : #ifdef DEBUG_PLAN_MOVE
3279 : if (DEBUG_COND) {
3280 : std::cout << SIMTIME << " current=" << current->getID() << " leaderBackLane=" << leaderInfo.first->getBackLane()->getID() << " backOnRoute=" << backOnRoute << "\n";
3281 : }
3282 : #endif
3283 229172 : if (!backOnRoute) {
3284 119826 : double stopDist = seen - current->getLength() - POSITION_EPS;
3285 119826 : if (lastLink->myLink->getInternalLaneBefore() != nullptr) {
3286 : // do not drive onto the junction conflict area
3287 101775 : stopDist -= lastLink->myLink->getInternalLaneBefore()->getLength();
3288 : }
3289 119826 : vsafeLeader = cfModel.stopSpeed(this, getSpeed(), stopDist);
3290 : }
3291 : }
3292 193549 : if (backOnRoute) {
3293 1290214906 : vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3294 : }
3295 1290334732 : if (lastLink != nullptr) {
3296 558660811 : const double futureVSafe = cfModel.followSpeed(this, lastLink->accelV, leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first, MSCFModel::CalcReason::FUTURE);
3297 : lastLink->adaptLeaveSpeed(futureVSafe);
3298 : #ifdef DEBUG_PLAN_MOVE
3299 : if (DEBUG_COND) {
3300 : std::cout << " vlinkpass=" << lastLink->myVLinkPass << " futureVSafe=" << futureVSafe << "\n";
3301 : }
3302 : #endif
3303 : }
3304 1290334732 : v = MIN2(v, vsafeLeader);
3305 2222071624 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3306 : #ifdef DEBUG_PLAN_MOVE
3307 : if (DEBUG_COND) std::cout
3308 : << SIMTIME
3309 : //std::cout << std::setprecision(10);
3310 : << " veh=" << getID()
3311 : << " lead=" << leaderInfo.first->getID()
3312 : << " leadSpeed=" << leaderInfo.first->getSpeed()
3313 : << " gap=" << leaderInfo.second
3314 : << " leadLane=" << leaderInfo.first->getLane()->getID()
3315 : << " predPos=" << leaderInfo.first->getPositionOnLane()
3316 : << " myLane=" << myLane->getID()
3317 : << " v=" << v
3318 : << " vSafeLeader=" << vsafeLeader
3319 : << " vLinkPass=" << vLinkPass
3320 : << "\n";
3321 : #endif
3322 : }
3323 : }
3324 :
3325 :
3326 : void
3327 18130109 : MSVehicle::adaptToJunctionLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3328 : const double seen, DriveProcessItem* const lastLink,
3329 : const MSLane* const lane, double& v, double& vLinkPass,
3330 : double distToCrossing) const {
3331 18130109 : if (leaderInfo.first != 0) {
3332 18130109 : if (ignoreFoe(leaderInfo.first)) {
3333 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3334 : if (DEBUG_COND) {
3335 : std::cout << " junction foe ignored\n";
3336 : }
3337 : #endif
3338 : return;
3339 : }
3340 : const MSCFModel& cfModel = getCarFollowModel();
3341 : double vsafeLeader = 0;
3342 18130073 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
3343 : vsafeLeader = -std::numeric_limits<double>::max();
3344 : }
3345 18130073 : if (leaderInfo.second >= 0) {
3346 14997333 : if (hasDeparted()) {
3347 14992820 : vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3348 : } else {
3349 : // called in the context of MSLane::isInsertionSuccess
3350 4513 : vsafeLeader = cfModel.insertionFollowSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3351 : }
3352 3132740 : } else if (leaderInfo.first != this) {
3353 : // the leading, in-lapping vehicle is occupying the complete next lane
3354 : // stop before entering this lane
3355 2710626 : vsafeLeader = cfModel.stopSpeed(this, getSpeed(), seen - lane->getLength() - POSITION_EPS);
3356 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3357 : if (DEBUG_COND) {
3358 : std::cout << SIMTIME << " veh=" << getID() << " stopping before junction: lane=" << lane->getID() << " seen=" << seen
3359 : << " laneLength=" << lane->getLength()
3360 : << " stopDist=" << seen - lane->getLength() - POSITION_EPS
3361 : << " vsafeLeader=" << vsafeLeader
3362 : << " distToCrossing=" << distToCrossing
3363 : << "\n";
3364 : }
3365 : #endif
3366 : }
3367 18130073 : if (distToCrossing >= 0) {
3368 : // can the leader still stop in the way?
3369 5492554 : const double vStop = cfModel.stopSpeed(this, getSpeed(), distToCrossing - getVehicleType().getMinGap());
3370 5492554 : if (leaderInfo.first == this) {
3371 : // braking for pedestrian
3372 410795 : const double vStopCrossing = cfModel.stopSpeed(this, getSpeed(), distToCrossing);
3373 : vsafeLeader = vStopCrossing;
3374 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3375 : if (DEBUG_COND) {
3376 : std::cout << " breaking for pedestrian distToCrossing=" << distToCrossing << " vStopCrossing=" << vStopCrossing << "\n";
3377 : }
3378 : #endif
3379 410795 : if (lastLink != nullptr) {
3380 : lastLink->adaptStopSpeed(vsafeLeader);
3381 : }
3382 5081759 : } else if (leaderInfo.second == -std::numeric_limits<double>::max()) {
3383 : // drive up to the crossing point and stop
3384 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3385 : if (DEBUG_COND) {
3386 : std::cout << " stop at crossing point for critical leader vStop=" << vStop << "\n";
3387 : };
3388 : #endif
3389 : vsafeLeader = MAX2(vsafeLeader, vStop);
3390 : } else {
3391 5022187 : const double leaderDistToCrossing = distToCrossing - leaderInfo.second;
3392 : // estimate the time at which the leader has gone past the crossing point
3393 5022187 : const double leaderPastCPTime = leaderDistToCrossing / MAX2(leaderInfo.first->getSpeed(), SUMO_const_haltingSpeed);
3394 : // reach distToCrossing after that time
3395 : // avgSpeed * leaderPastCPTime = distToCrossing
3396 : // ballistic: avgSpeed = (getSpeed + vFinal) / 2
3397 5022187 : const double vFinal = MAX2(getSpeed(), 2 * (distToCrossing - getVehicleType().getMinGap()) / leaderPastCPTime - getSpeed());
3398 5022187 : const double v2 = getSpeed() + ACCEL2SPEED((vFinal - getSpeed()) / leaderPastCPTime);
3399 : vsafeLeader = MAX2(vsafeLeader, MIN2(v2, vStop));
3400 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3401 : if (DEBUG_COND) {
3402 : std::cout << " driving up to the crossing point (distToCrossing=" << distToCrossing << ")"
3403 : << " leaderPastCPTime=" << leaderPastCPTime
3404 : << " vFinal=" << vFinal
3405 : << " v2=" << v2
3406 : << " vStop=" << vStop
3407 : << " vsafeLeader=" << vsafeLeader << "\n";
3408 : }
3409 : #endif
3410 : }
3411 : }
3412 17719278 : if (lastLink != nullptr) {
3413 : lastLink->adaptLeaveSpeed(vsafeLeader);
3414 : }
3415 18130073 : v = MIN2(v, vsafeLeader);
3416 33809466 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3417 : #ifdef DEBUG_PLAN_MOVE
3418 : if (DEBUG_COND) std::cout
3419 : << SIMTIME
3420 : //std::cout << std::setprecision(10);
3421 : << " veh=" << getID()
3422 : << " lead=" << leaderInfo.first->getID()
3423 : << " leadSpeed=" << leaderInfo.first->getSpeed()
3424 : << " gap=" << leaderInfo.second
3425 : << " leadLane=" << leaderInfo.first->getLane()->getID()
3426 : << " predPos=" << leaderInfo.first->getPositionOnLane()
3427 : << " seen=" << seen
3428 : << " lane=" << lane->getID()
3429 : << " myLane=" << myLane->getID()
3430 : << " dTC=" << distToCrossing
3431 : << " v=" << v
3432 : << " vSafeLeader=" << vsafeLeader
3433 : << " vLinkPass=" << vLinkPass
3434 : << "\n";
3435 : #endif
3436 : }
3437 : }
3438 :
3439 :
3440 : void
3441 46856 : MSVehicle::adaptToOncomingLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3442 : DriveProcessItem* const lastLink,
3443 : double& v, double& vLinkPass) const {
3444 46856 : if (leaderInfo.first != 0) {
3445 46856 : if (ignoreFoe(leaderInfo.first)) {
3446 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3447 : if (DEBUG_COND) {
3448 : std::cout << " oncoming foe ignored\n";
3449 : }
3450 : #endif
3451 : return;
3452 : }
3453 : const MSCFModel& cfModel = getCarFollowModel();
3454 : const MSVehicle* lead = leaderInfo.first;
3455 : const MSCFModel& cfModelL = lead->getCarFollowModel();
3456 : // assume the leader reacts symmetrically (neither stopping instantly nor ignoring ego)
3457 46737 : const double leaderBrakeGap = cfModelL.brakeGap(lead->getSpeed(), cfModelL.getMaxDecel(), 0);
3458 46737 : const double egoBrakeGap = cfModel.brakeGap(getSpeed(), cfModel.getMaxDecel(), 0);
3459 46737 : const double gapSum = leaderBrakeGap + egoBrakeGap;
3460 : // ensure that both vehicles can leave an intersection if they are currently on it
3461 46737 : double egoExit = getDistanceToLeaveJunction();
3462 46737 : const double leaderExit = lead->getDistanceToLeaveJunction();
3463 : double gap = leaderInfo.second;
3464 46737 : if (egoExit + leaderExit < gap) {
3465 42973 : gap -= egoExit + leaderExit;
3466 : } else {
3467 : egoExit = 0;
3468 : }
3469 : // split any distance in excess of brakeGaps evenly
3470 46737 : const double freeGap = MAX2(0.0, gap - gapSum);
3471 : const double splitGap = MIN2(gap, gapSum);
3472 : // assume remaining distance is allocated in proportion to braking distance
3473 46737 : const double gapRatio = gapSum > 0 ? egoBrakeGap / gapSum : 0.5;
3474 46737 : const double vsafeLeader = cfModel.stopSpeed(this, getSpeed(), splitGap * gapRatio + egoExit + 0.5 * freeGap);
3475 46737 : if (lastLink != nullptr) {
3476 27133 : const double futureVSafe = cfModel.stopSpeed(this, lastLink->accelV, leaderInfo.second, MSCFModel::CalcReason::FUTURE);
3477 : lastLink->adaptLeaveSpeed(futureVSafe);
3478 : #ifdef DEBUG_PLAN_MOVE
3479 : if (DEBUG_COND) {
3480 : std::cout << " vlinkpass=" << lastLink->myVLinkPass << " futureVSafe=" << futureVSafe << "\n";
3481 : }
3482 : #endif
3483 : }
3484 46737 : v = MIN2(v, vsafeLeader);
3485 92274 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3486 : #ifdef DEBUG_PLAN_MOVE
3487 : if (DEBUG_COND) std::cout
3488 : << SIMTIME
3489 : //std::cout << std::setprecision(10);
3490 : << " veh=" << getID()
3491 : << " oncomingLead=" << lead->getID()
3492 : << " leadSpeed=" << lead->getSpeed()
3493 : << " gap=" << leaderInfo.second
3494 : << " gap2=" << gap
3495 : << " gapRatio=" << gapRatio
3496 : << " leadLane=" << lead->getLane()->getID()
3497 : << " predPos=" << lead->getPositionOnLane()
3498 : << " myLane=" << myLane->getID()
3499 : << " v=" << v
3500 : << " vSafeLeader=" << vsafeLeader
3501 : << " vLinkPass=" << vLinkPass
3502 : << "\n";
3503 : #endif
3504 : }
3505 : }
3506 :
3507 :
3508 : void
3509 798189835 : MSVehicle::checkLinkLeaderCurrentAndParallel(const MSLink* link, const MSLane* lane, double seen,
3510 : DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest) const {
3511 798189835 : if (MSGlobals::gUsingInternalLanes && (myInfluencer == nullptr || myInfluencer->getRespectJunctionLeaderPriority())) {
3512 : // we want to pass the link but need to check for foes on internal lanes
3513 798083875 : checkLinkLeader(link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
3514 798083875 : if (myLaneChangeModel->getShadowLane() != nullptr) {
3515 3139414 : const MSLink* const parallelLink = link->getParallelLink(myLaneChangeModel->getShadowDirection());
3516 3139414 : if (parallelLink != nullptr) {
3517 2177879 : checkLinkLeader(parallelLink, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest, true);
3518 : }
3519 : }
3520 : }
3521 :
3522 798189835 : }
3523 :
3524 : void
3525 800510952 : MSVehicle::checkLinkLeader(const MSLink* link, const MSLane* lane, double seen,
3526 : DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest,
3527 : bool isShadowLink) const {
3528 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3529 : if (DEBUG_COND) {
3530 : gDebugFlag1 = true; // See MSLink::getLeaderInfo
3531 : }
3532 : #endif
3533 800510952 : const MSLink::LinkLeaders linkLeaders = link->getLeaderInfo(this, seen, nullptr, isShadowLink);
3534 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3535 : if (DEBUG_COND) {
3536 : gDebugFlag1 = false; // See MSLink::getLeaderInfo
3537 : }
3538 : #endif
3539 819593529 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
3540 : // the vehicle to enter the junction first has priority
3541 19082577 : const MSVehicle* leader = (*it).vehAndGap.first;
3542 19082577 : if (leader == nullptr) {
3543 : // leader is a pedestrian. Passing 'this' as a dummy.
3544 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3545 : if (DEBUG_COND) {
3546 : std::cout << SIMTIME << " veh=" << getID() << " is blocked on link to " << link->getViaLaneOrLane()->getID() << " by pedestrian. dist=" << it->distToCrossing << "\n";
3547 : }
3548 : #endif
3549 422810 : if (getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) > 0
3550 422810 : && getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) >= RandHelper::rand(getRNG())) {
3551 : #ifdef DEBUG_PLAN_MOVE
3552 : if (DEBUG_COND) {
3553 : std::cout << SIMTIME << " veh=" << getID() << " is ignoring pedestrian (jmIgnoreJunctionFoeProb)\n";
3554 : }
3555 : #endif
3556 696 : continue;
3557 : }
3558 422114 : adaptToJunctionLeader(std::make_pair(this, -1), seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
3559 : // if blocked by a pedestrian for too long we must yield our request
3560 422114 : if (v < SUMO_const_haltingSpeed && getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME)) {
3561 75596 : setRequest = false;
3562 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3563 : if (DEBUG_COND) {
3564 : std::cout << " aborting request\n";
3565 : }
3566 : #endif
3567 : }
3568 18659767 : } else if (isLeader(link, leader, (*it).vehAndGap.second) || (*it).inTheWay()) {
3569 18610829 : if (getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) > 0
3570 18610829 : && getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) >= RandHelper::rand(getRNG())) {
3571 : #ifdef DEBUG_PLAN_MOVE
3572 : if (DEBUG_COND) {
3573 : std::cout << SIMTIME << " veh=" << getID() << " is ignoring linkLeader=" << leader->getID() << " (jmIgnoreJunctionFoeProb)\n";
3574 : }
3575 : #endif
3576 2183 : continue;
3577 : }
3578 25421889 : if (MSGlobals::gLateralResolution > 0 &&
3579 : // sibling link (XXX: could also be partial occupator where this check fails)
3580 6813243 : &leader->getLane()->getEdge() == &lane->getEdge()) {
3581 : // check for sublane obstruction (trivial for sibling link leaders)
3582 : const MSLane* conflictLane = link->getInternalLaneBefore();
3583 931498 : MSLeaderInfo linkLeadersAhead = MSLeaderInfo(conflictLane->getWidth());
3584 931498 : linkLeadersAhead.addLeader(leader, false, 0); // assume sibling lane has the same geometry as the leader lane
3585 931498 : const double latOffset = isShadowLink ? (getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge()) : 0;
3586 : // leader is neither on lane nor conflictLane (the conflict is only established geometrically)
3587 931498 : adaptToLeaders(linkLeadersAhead, latOffset, seen, lastLink, leader->getLane(), v, vLinkPass);
3588 : #ifdef DEBUG_PLAN_MOVE
3589 : if (DEBUG_COND) {
3590 : std::cout << SIMTIME << " veh=" << getID()
3591 : << " siblingFoe link=" << link->getViaLaneOrLane()->getID()
3592 : << " isShadowLink=" << isShadowLink
3593 : << " lane=" << lane->getID()
3594 : << " foe=" << leader->getID()
3595 : << " foeLane=" << leader->getLane()->getID()
3596 : << " latOffset=" << latOffset
3597 : << " latOffsetFoe=" << leader->getLatOffset(lane)
3598 : << " linkLeadersAhead=" << linkLeadersAhead.toString()
3599 : << "\n";
3600 : }
3601 : #endif
3602 931498 : } else {
3603 : #ifdef DEBUG_PLAN_MOVE
3604 : if (DEBUG_COND) {
3605 : std::cout << SIMTIME << " veh=" << getID() << " linkLeader=" << leader->getID() << " gap=" << it->vehAndGap.second
3606 : << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
3607 : << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
3608 : << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
3609 : << "\n";
3610 : }
3611 : #endif
3612 17677148 : adaptToJunctionLeader(it->vehAndGap, seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
3613 : }
3614 18608646 : if (lastLink != nullptr) {
3615 : // we are not yet on the junction with this linkLeader.
3616 : // at least we can drive up to the previous link and stop there
3617 35790463 : v = MAX2(v, lastLink->myVLinkWait);
3618 : }
3619 : // if blocked by a leader from the same or next lane we must yield our request
3620 : // also, if blocked by a stopped or blocked leader
3621 18608646 : if (v < SUMO_const_haltingSpeed
3622 : //&& leader->getSpeed() < SUMO_const_haltingSpeed
3623 18608646 : && (leader->getLane()->getLogicalPredecessorLane() == myLane->getLogicalPredecessorLane()
3624 10163382 : || leader->getLane()->getLogicalPredecessorLane() == myLane
3625 7972199 : || leader->isStopped()
3626 7893728 : || leader->getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME))) {
3627 4178732 : setRequest = false;
3628 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3629 : if (DEBUG_COND) {
3630 : std::cout << " aborting request\n";
3631 : }
3632 : #endif
3633 4178732 : if (lastLink != nullptr && leader->getLane()->getLogicalPredecessorLane() == myLane) {
3634 : // we are not yet on the junction so must abort that request as well
3635 : // (or maybe we are already on the junction and the leader is a partial occupator beyond)
3636 2179603 : lastLink->mySetRequest = false;
3637 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3638 : if (DEBUG_COND) {
3639 : std::cout << " aborting previous request\n";
3640 : }
3641 : #endif
3642 : }
3643 : }
3644 : }
3645 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3646 : else {
3647 : if (DEBUG_COND) {
3648 : std::cout << SIMTIME << " veh=" << getID() << " ignoring leader " << leader->getID() << " gap=" << (*it).vehAndGap.second << " dtC=" << (*it).distToCrossing
3649 : << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
3650 : << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
3651 : << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
3652 : << "\n";
3653 : }
3654 : }
3655 : #endif
3656 : }
3657 : // if this is the link between two internal lanes we may have to slow down for pedestrians
3658 800510952 : vLinkWait = MIN2(vLinkWait, v);
3659 800510952 : }
3660 :
3661 :
3662 : double
3663 99268801 : MSVehicle::getDeltaPos(const double accel) const {
3664 99268801 : double vNext = myState.mySpeed + ACCEL2SPEED(accel);
3665 99268801 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3666 : // apply implicit Euler positional update
3667 0 : return SPEED2DIST(MAX2(vNext, 0.));
3668 : } else {
3669 : // apply ballistic update
3670 99268801 : if (vNext >= 0) {
3671 : // assume constant acceleration during this time step
3672 98645803 : return SPEED2DIST(myState.mySpeed + 0.5 * ACCEL2SPEED(accel));
3673 : } else {
3674 : // negative vNext indicates a stop within the middle of time step
3675 : // The corresponding stop time is s = mySpeed/deceleration \in [0,dt], and the
3676 : // covered distance is therefore deltaPos = mySpeed*s - 0.5*deceleration*s^2.
3677 : // Here, deceleration = (myState.mySpeed - vNext)/dt is the constant deceleration
3678 : // until the vehicle stops.
3679 622998 : return -SPEED2DIST(0.5 * myState.mySpeed * myState.mySpeed / ACCEL2SPEED(accel));
3680 : }
3681 : }
3682 : }
3683 :
3684 : void
3685 633588686 : MSVehicle::processLinkApproaches(double& vSafe, double& vSafeMin, double& vSafeMinDist) {
3686 :
3687 : // Speed limit due to zipper merging
3688 : double vSafeZipper = std::numeric_limits<double>::max();
3689 :
3690 633588686 : myHaveToWaitOnNextLink = false;
3691 : bool canBrakeVSafeMin = false;
3692 :
3693 : // Get safe velocities from DriveProcessItems.
3694 : assert(myLFLinkLanes.size() != 0 || isRemoteControlled());
3695 1270164892 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
3696 1093536349 : MSLink* const link = dpi.myLink;
3697 :
3698 : #ifdef DEBUG_EXEC_MOVE
3699 : if (DEBUG_COND) {
3700 : std::cout
3701 : << SIMTIME
3702 : << " veh=" << getID()
3703 : << " link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID())
3704 : << " req=" << dpi.mySetRequest
3705 : << " vP=" << dpi.myVLinkPass
3706 : << " vW=" << dpi.myVLinkWait
3707 : << " d=" << dpi.myDistance
3708 : << "\n";
3709 : gDebugFlag1 = true; // See MSLink_DEBUG_OPENED
3710 : }
3711 : #endif
3712 :
3713 : // the vehicle must change the lane on one of the next lanes (XXX: refs to code further below???, Leo)
3714 1093536349 : if (link != nullptr && dpi.mySetRequest) {
3715 :
3716 : const LinkState ls = link->getState();
3717 : // vehicles should brake when running onto a yellow light if the distance allows to halt in front
3718 : const bool yellow = link->haveYellow();
3719 657214315 : const bool canBrake = (dpi.myDistance > getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.)
3720 657214315 : || (MSGlobals::gSemiImplicitEulerUpdate && myState.mySpeed < ACCEL2SPEED(getCarFollowModel().getMaxDecel())));
3721 : assert(link->getLaneBefore() != nullptr);
3722 657214315 : const bool beyondStopLine = dpi.myDistance < link->getLaneBefore()->getVehicleStopOffset(this);
3723 657214315 : const bool ignoreRedLink = ignoreRed(link, canBrake) || beyondStopLine;
3724 657214315 : if (yellow && canBrake && !ignoreRedLink) {
3725 10 : vSafe = dpi.myVLinkWait;
3726 10 : myHaveToWaitOnNextLink = true;
3727 : #ifdef DEBUG_CHECKREWINDLINKLANES
3728 : if (DEBUG_COND) {
3729 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (yellow)\n";
3730 : }
3731 : #endif
3732 20638109 : break;
3733 : }
3734 657214305 : const bool influencerPrio = (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority());
3735 : MSLink::BlockingFoes collectFoes;
3736 657214305 : bool opened = (yellow || influencerPrio
3737 1971317615 : || link->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
3738 657051655 : getVehicleType().getLength(),
3739 628821097 : canBrake ? getImpatience() : 1,
3740 : getCarFollowModel().getMaxDecel(),
3741 657051655 : getWaitingTimeFor(link), getLateralPositionOnLane(),
3742 : ls == LINKSTATE_ZIPPER ? &collectFoes : nullptr,
3743 657051655 : ignoreRedLink, this, dpi.myDistance));
3744 651564477 : if (opened && myLaneChangeModel->getShadowLane() != nullptr) {
3745 1909919 : const MSLink* const parallelLink = dpi.myLink->getParallelLink(myLaneChangeModel->getShadowDirection());
3746 1909919 : if (parallelLink != nullptr) {
3747 1179189 : const double shadowLatPos = getLateralPositionOnLane() - myLaneChangeModel->getShadowDirection() * 0.5 * (
3748 1179189 : myLane->getWidth() + myLaneChangeModel->getShadowLane()->getWidth());
3749 2357609 : opened = yellow || influencerPrio || (opened && parallelLink->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
3750 1178420 : getVehicleType().getLength(), getImpatience(),
3751 : getCarFollowModel().getMaxDecel(),
3752 : getWaitingTimeFor(link), shadowLatPos, nullptr,
3753 1178420 : ignoreRedLink, this, dpi.myDistance));
3754 : #ifdef DEBUG_EXEC_MOVE
3755 : if (DEBUG_COND) {
3756 : std::cout << SIMTIME
3757 : << " veh=" << getID()
3758 : << " shadowLane=" << myLaneChangeModel->getShadowLane()->getID()
3759 : << " shadowDir=" << myLaneChangeModel->getShadowDirection()
3760 : << " parallelLink=" << (parallelLink == 0 ? "NULL" : parallelLink->getViaLaneOrLane()->getID())
3761 : << " opened=" << opened
3762 : << "\n";
3763 : }
3764 : #endif
3765 : }
3766 : }
3767 : // vehicles should decelerate when approaching a minor link
3768 : #ifdef DEBUG_EXEC_MOVE
3769 : if (DEBUG_COND) {
3770 : std::cout << SIMTIME
3771 : << " opened=" << opened
3772 : << " influencerPrio=" << influencerPrio
3773 : << " linkPrio=" << link->havePriority()
3774 : << " lastContMajor=" << link->lastWasContMajor()
3775 : << " isCont=" << link->isCont()
3776 : << " ignoreRed=" << ignoreRedLink
3777 : << "\n";
3778 : }
3779 : #endif
3780 : double visibilityDistance = link->getFoeVisibilityDistance();
3781 657214305 : bool determinedFoePresence = dpi.myDistance <= visibilityDistance;
3782 657214305 : if (opened && !influencerPrio && !link->havePriority() && !link->lastWasContMajor() && !link->isCont() && !ignoreRedLink) {
3783 17257294 : if (!determinedFoePresence && (canBrake || !yellow)) {
3784 16295288 : vSafe = dpi.myVLinkWait;
3785 16295288 : myHaveToWaitOnNextLink = true;
3786 : #ifdef DEBUG_CHECKREWINDLINKLANES
3787 : if (DEBUG_COND) {
3788 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (minor)\n";
3789 : }
3790 : #endif
3791 16295288 : break;
3792 : } else {
3793 : // past the point of no return. we need to drive fast enough
3794 : // to make it across the link. However, minor slowdowns
3795 : // should be permissible to follow leading traffic safely
3796 : // basically, this code prevents dawdling
3797 : // (it's harder to do this later using
3798 : // SUMO_ATTR_JM_SIGMA_MINOR because we don't know whether the
3799 : // vehicle is already too close to stop at that part of the code)
3800 : //
3801 : // XXX: There is a problem in subsecond simulation: If we cannot
3802 : // make it across the minor link in one step, new traffic
3803 : // could appear on a major foe link and cause a collision. Refs. #1845, #2123
3804 962006 : vSafeMinDist = dpi.myDistance; // distance that must be covered
3805 962006 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3806 1759896 : vSafeMin = MIN3((double)DIST2SPEED(vSafeMinDist + POSITION_EPS), dpi.myVLinkPass, getCarFollowModel().maxNextSafeMin(getSpeed(), this));
3807 : } else {
3808 164116 : vSafeMin = MIN3((double)DIST2SPEED(2 * vSafeMinDist + NUMERICAL_EPS) - getSpeed(), dpi.myVLinkPass, getCarFollowModel().maxNextSafeMin(getSpeed(), this));
3809 : }
3810 : canBrakeVSafeMin = canBrake;
3811 : #ifdef DEBUG_EXEC_MOVE
3812 : if (DEBUG_COND) {
3813 : std::cout << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << " canBrake=" << canBrake << "\n";
3814 : }
3815 : #endif
3816 : }
3817 : }
3818 : // have waited; may pass if opened...
3819 640919017 : if (opened) {
3820 635248227 : vSafe = dpi.myVLinkPass;
3821 635248227 : if (vSafe < getCarFollowModel().getMaxDecel() && vSafe <= dpi.myVLinkWait && vSafe < getCarFollowModel().maxNextSpeed(getSpeed(), this)) {
3822 : // this vehicle is probably not gonna drive across the next junction (heuristic)
3823 55571403 : myHaveToWaitOnNextLink = true;
3824 : #ifdef DEBUG_CHECKREWINDLINKLANES
3825 : if (DEBUG_COND) {
3826 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (very slow)\n";
3827 : }
3828 : #endif
3829 : }
3830 635248227 : if (link->mustStop() && determinedFoePresence && myHaveStoppedFor == nullptr) {
3831 20820 : myHaveStoppedFor = link;
3832 : }
3833 5670790 : } else if (link->getState() == LINKSTATE_ZIPPER) {
3834 1327741 : vSafeZipper = MIN2(vSafeZipper,
3835 1327741 : link->getZipperSpeed(this, dpi.myDistance, dpi.myVLinkPass, dpi.myArrivalTime, &collectFoes));
3836 : } else if (!canBrake
3837 : // always brake hard for traffic lights (since an emergency stop is necessary anyway)
3838 1736 : && link->getTLLogic() == nullptr
3839 : // cannot brake even with emergency deceleration
3840 4343940 : && dpi.myDistance < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0.)) {
3841 : #ifdef DEBUG_EXEC_MOVE
3842 : if (DEBUG_COND) {
3843 : std::cout << SIMTIME << " too fast to brake for closed link\n";
3844 : }
3845 : #endif
3846 238 : vSafe = dpi.myVLinkPass;
3847 : } else {
3848 4342811 : vSafe = dpi.myVLinkWait;
3849 4342811 : myHaveToWaitOnNextLink = true;
3850 : #ifdef DEBUG_CHECKREWINDLINKLANES
3851 : if (DEBUG_COND) {
3852 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (closed)\n";
3853 : }
3854 : #endif
3855 : #ifdef DEBUG_EXEC_MOVE
3856 : if (DEBUG_COND) {
3857 : std::cout << SIMTIME << " braking for closed link=" << link->getViaLaneOrLane()->getID() << "\n";
3858 : }
3859 : #endif
3860 4342811 : break;
3861 : }
3862 636576206 : if (myLane->isInternal() && myJunctionEntryTime == SUMOTime_MAX) {
3863 : // request was renewed, restoring entry time
3864 : // @note: using myJunctionEntryTimeNeverYield could lead to inconsistencies with other vehicles already on the junction
3865 80366 : myJunctionEntryTime = SIMSTEP;;
3866 : }
3867 657214305 : } else {
3868 436322034 : if (link != nullptr && link->getInternalLaneBefore() != nullptr && myLane->isInternal() && link->getJunction() == myLane->getEdge().getToJunction()) {
3869 : // blocked on the junction. yield request so other vehicles may
3870 : // become junction leader
3871 : #ifdef DEBUG_EXEC_MOVE
3872 : if (DEBUG_COND) {
3873 : std::cout << SIMTIME << " resetting junctionEntryTime at junction '" << link->getJunction()->getID() << "' beause of non-request exitLink\n";
3874 : }
3875 : #endif
3876 244740 : myJunctionEntryTime = SUMOTime_MAX;
3877 244740 : myJunctionConflictEntryTime = SUMOTime_MAX;
3878 : }
3879 : // we have: i->link == 0 || !i->setRequest
3880 436322034 : vSafe = dpi.myVLinkWait;
3881 436322034 : if (link != nullptr || myStopDist < (myLane->getLength() - getPositionOnLane())) {
3882 105955461 : if (vSafe < getSpeed()) {
3883 16118374 : myHaveToWaitOnNextLink = true;
3884 : #ifdef DEBUG_CHECKREWINDLINKLANES
3885 : if (DEBUG_COND) {
3886 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, braking) vSafe=" << vSafe << "\n";
3887 : }
3888 : #endif
3889 89837087 : } else if (vSafe < SUMO_const_haltingSpeed) {
3890 62546070 : myHaveToWaitOnNextLink = true;
3891 : #ifdef DEBUG_CHECKREWINDLINKLANES
3892 : if (DEBUG_COND) {
3893 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, stopping)\n";
3894 : }
3895 : #endif
3896 : }
3897 : }
3898 334764365 : if (link == nullptr && myLFLinkLanes.size() == 1
3899 268486651 : && getBestLanesContinuation().size() > 1
3900 269041 : && getBestLanesContinuation()[1]->hadPermissionChanges()
3901 436452929 : && myLane->getFirstAnyVehicle() == this) {
3902 : // temporal lane closing without notification, visible to the
3903 : // vehicle at the front of the queue
3904 34246 : updateBestLanes(true);
3905 : //std::cout << SIMTIME << " veh=" << getID() << " updated bestLanes=" << toString(getBestLanesContinuation()) << "\n";
3906 : }
3907 : break;
3908 : }
3909 : }
3910 :
3911 : //#ifdef DEBUG_EXEC_MOVE
3912 : // if (DEBUG_COND) {
3913 : // std::cout << "\nvCurrent = " << toString(getSpeed(), 24) << "" << std::endl;
3914 : // std::cout << "vSafe = " << toString(vSafe, 24) << "" << std::endl;
3915 : // std::cout << "vSafeMin = " << toString(vSafeMin, 24) << "" << std::endl;
3916 : // std::cout << "vSafeMinDist = " << toString(vSafeMinDist, 24) << "" << std::endl;
3917 : //
3918 : // double gap = getLeader().second;
3919 : // std::cout << "gap = " << toString(gap, 24) << std::endl;
3920 : // std::cout << "vSafeStoppedLeader = " << toString(getCarFollowModel().stopSpeed(this, getSpeed(), gap, MSCFModel::CalcReason::FUTURE), 24)
3921 : // << "\n" << std::endl;
3922 : // }
3923 : //#endif
3924 :
3925 633588686 : if ((MSGlobals::gSemiImplicitEulerUpdate && vSafe + NUMERICAL_EPS < vSafeMin)
3926 633368063 : || (!MSGlobals::gSemiImplicitEulerUpdate && (vSafe + NUMERICAL_EPS < vSafeMin && vSafeMin != 0))) { // this might be good for the euler case as well
3927 : // XXX: (Leo) This often called stopSpeed with vSafeMinDist==0 (for the ballistic update), since vSafe can become negative
3928 : // For the Euler update the term '+ NUMERICAL_EPS' prevented a call here... Recheck, consider of -INVALID_SPEED instead of 0 to indicate absence of vSafeMin restrictions. Refs. #2577
3929 : #ifdef DEBUG_EXEC_MOVE
3930 : if (DEBUG_COND) {
3931 : std::cout << "vSafeMin Problem? vSafe=" << vSafe << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << std::endl;
3932 : }
3933 : #endif
3934 260819 : if (canBrakeVSafeMin && vSafe < getSpeed()) {
3935 : // cannot drive across a link so we need to stop before it
3936 126660 : vSafe = MIN2(vSafe, MAX2(getCarFollowModel().minNextSpeed(getSpeed(), this),
3937 63330 : getCarFollowModel().stopSpeed(this, getSpeed(), vSafeMinDist)));
3938 63330 : vSafeMin = 0;
3939 63330 : myHaveToWaitOnNextLink = true;
3940 : #ifdef DEBUG_CHECKREWINDLINKLANES
3941 : if (DEBUG_COND) {
3942 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (vSafe=" << vSafe << " < vSafeMin=" << vSafeMin << ")\n";
3943 : }
3944 : #endif
3945 : } else {
3946 : // if the link is yellow or visibility distance is large
3947 : // then we might not make it across the link in one step anyway..
3948 : // Possibly, the lane after the intersection has a lower speed limit so
3949 : // we really need to drive slower already
3950 : // -> keep driving without dawdling
3951 197489 : vSafeMin = vSafe;
3952 : }
3953 : }
3954 :
3955 : // vehicles inside a roundabout should maintain their requests
3956 633588686 : if (myLane->getEdge().isRoundabout()) {
3957 2659768 : myHaveToWaitOnNextLink = false;
3958 : }
3959 :
3960 633588686 : vSafe = MIN2(vSafe, vSafeZipper);
3961 633588686 : }
3962 :
3963 :
3964 : double
3965 704154172 : MSVehicle::processTraCISpeedControl(double vSafe, double vNext) {
3966 704154172 : if (myInfluencer != nullptr) {
3967 498886 : myInfluencer->setOriginalSpeed(vNext);
3968 : #ifdef DEBUG_TRACI
3969 : if DEBUG_COND2(this) {
3970 : std::cout << SIMTIME << " MSVehicle::processTraCISpeedControl() for vehicle '" << getID() << "'"
3971 : << " vSafe=" << vSafe << " (init)vNext=" << vNext << " keepStopping=" << keepStopping();
3972 : }
3973 : #endif
3974 498886 : if (myInfluencer->isRemoteControlled()) {
3975 6749 : vNext = myInfluencer->implicitSpeedRemote(this, myState.mySpeed);
3976 : }
3977 498886 : const double vMax = getVehicleType().getCarFollowModel().maxNextSpeed(myState.mySpeed, this);
3978 498886 : double vMin = getVehicleType().getCarFollowModel().minNextSpeed(myState.mySpeed, this);
3979 498886 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3980 : vMin = MAX2(0., vMin);
3981 : }
3982 498886 : vNext = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), vNext, vSafe, vMin, vMax);
3983 498886 : if (keepStopping() && myStops.front().getSpeed() == 0) {
3984 : // avoid driving while stopped (unless it's actually a waypoint
3985 3818 : vNext = myInfluencer->getOriginalSpeed();
3986 : }
3987 : #ifdef DEBUG_TRACI
3988 : if DEBUG_COND2(this) {
3989 : std::cout << " (processed)vNext=" << vNext << std::endl;
3990 : }
3991 : #endif
3992 : }
3993 704154172 : return vNext;
3994 : }
3995 :
3996 :
3997 : void
3998 71674676 : MSVehicle::removePassedDriveItems() {
3999 : #ifdef DEBUG_ACTIONSTEPS
4000 : if (DEBUG_COND) {
4001 : std::cout << SIMTIME << " veh=" << getID() << " removePassedDriveItems()\n"
4002 : << " Current items: ";
4003 : for (auto& j : myLFLinkLanes) {
4004 : if (j.myLink == 0) {
4005 : std::cout << "\n Stop at distance " << j.myDistance;
4006 : } else {
4007 : const MSLane* to = j.myLink->getViaLaneOrLane();
4008 : const MSLane* from = j.myLink->getLaneBefore();
4009 : std::cout << "\n Link at distance " << j.myDistance << ": '"
4010 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4011 : }
4012 : }
4013 : std::cout << "\n myNextDriveItem: ";
4014 : if (myLFLinkLanes.size() != 0) {
4015 : if (myNextDriveItem->myLink == 0) {
4016 : std::cout << "\n Stop at distance " << myNextDriveItem->myDistance;
4017 : } else {
4018 : const MSLane* to = myNextDriveItem->myLink->getViaLaneOrLane();
4019 : const MSLane* from = myNextDriveItem->myLink->getLaneBefore();
4020 : std::cout << "\n Link at distance " << myNextDriveItem->myDistance << ": '"
4021 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4022 : }
4023 : }
4024 : std::cout << std::endl;
4025 : }
4026 : #endif
4027 71999777 : for (auto j = myLFLinkLanes.begin(); j != myNextDriveItem; ++j) {
4028 : #ifdef DEBUG_ACTIONSTEPS
4029 : if (DEBUG_COND) {
4030 : std::cout << " Removing item: ";
4031 : if (j->myLink == 0) {
4032 : std::cout << "Stop at distance " << j->myDistance;
4033 : } else {
4034 : const MSLane* to = j->myLink->getViaLaneOrLane();
4035 : const MSLane* from = j->myLink->getLaneBefore();
4036 : std::cout << "Link at distance " << j->myDistance << ": '"
4037 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4038 : }
4039 : std::cout << std::endl;
4040 : }
4041 : #endif
4042 325101 : if (j->myLink != nullptr) {
4043 325028 : j->myLink->removeApproaching(this);
4044 : }
4045 : }
4046 71674676 : myLFLinkLanes.erase(myLFLinkLanes.begin(), myNextDriveItem);
4047 71674676 : myNextDriveItem = myLFLinkLanes.begin();
4048 71674676 : }
4049 :
4050 :
4051 : void
4052 1126098 : MSVehicle::updateDriveItems() {
4053 : #ifdef DEBUG_ACTIONSTEPS
4054 : if (DEBUG_COND) {
4055 : std::cout << SIMTIME << " updateDriveItems(), veh='" << getID() << "' (lane: '" << getLane()->getID() << "')\nCurrent drive items:" << std::endl;
4056 : for (const auto& dpi : myLFLinkLanes) {
4057 : std::cout
4058 : << " vPass=" << dpi.myVLinkPass
4059 : << " vWait=" << dpi.myVLinkWait
4060 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4061 : << " request=" << dpi.mySetRequest
4062 : << "\n";
4063 : }
4064 : std::cout << " myNextDriveItem's linked lane: " << (myNextDriveItem->myLink == 0 ? "NULL" : myNextDriveItem->myLink->getViaLaneOrLane()->getID()) << std::endl;
4065 : }
4066 : #endif
4067 1126098 : if (myLFLinkLanes.size() == 0) {
4068 : // nothing to update
4069 : return;
4070 : }
4071 : const MSLink* nextPlannedLink = nullptr;
4072 : // auto i = myLFLinkLanes.begin();
4073 1126098 : auto i = myNextDriveItem;
4074 2252150 : while (i != myLFLinkLanes.end() && nextPlannedLink == nullptr) {
4075 1126052 : nextPlannedLink = i->myLink;
4076 : ++i;
4077 : }
4078 :
4079 1126098 : if (nextPlannedLink == nullptr) {
4080 : // No link for upcoming item -> no need for an update
4081 : #ifdef DEBUG_ACTIONSTEPS
4082 : if (DEBUG_COND) {
4083 : std::cout << "Found no link-related drive item." << std::endl;
4084 : }
4085 : #endif
4086 : return;
4087 : }
4088 :
4089 544452 : if (getLane() == nextPlannedLink->getLaneBefore()) {
4090 : // Current lane approaches the stored next link, i.e. no LC happend and no update is required.
4091 : #ifdef DEBUG_ACTIONSTEPS
4092 : if (DEBUG_COND) {
4093 : std::cout << "Continuing on planned lane sequence, no update required." << std::endl;
4094 : }
4095 : #endif
4096 : return;
4097 : }
4098 : // Lane must have been changed, determine the change direction
4099 534513 : const MSLink* parallelLink = nextPlannedLink->getParallelLink(1);
4100 534513 : if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
4101 : // lcDir = 1;
4102 : } else {
4103 264143 : parallelLink = nextPlannedLink->getParallelLink(-1);
4104 264143 : if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
4105 : // lcDir = -1;
4106 : } else {
4107 : // If the vehicle's current lane is not the approaching lane for the next
4108 : // drive process item's link, it is expected to lead to a parallel link,
4109 : // XXX: What if the lc was an overtaking maneuver and there is no upcoming link?
4110 : // Then a stop item should be scheduled! -> TODO!
4111 : //assert(false);
4112 72222 : return;
4113 : }
4114 : }
4115 : #ifdef DEBUG_ACTIONSTEPS
4116 : if (DEBUG_COND) {
4117 : std::cout << "Changed lane. Drive items will be updated along the current lane continuation." << std::endl;
4118 : }
4119 : #endif
4120 : // Trace link sequence along current best lanes and transfer drive items to the corresponding links
4121 : // DriveItemVector::iterator driveItemIt = myLFLinkLanes.begin();
4122 462291 : DriveItemVector::iterator driveItemIt = myNextDriveItem;
4123 : // In the loop below, lane holds the currently considered lane on the vehicles continuation (including internal lanes)
4124 462291 : const MSLane* lane = myLane;
4125 : assert(myLane == parallelLink->getLaneBefore());
4126 : // *lit is a pointer to the next lane in best continuations for the current lane (always non-internal)
4127 462291 : std::vector<MSLane*>::const_iterator bestLaneIt = getBestLanesContinuation().begin() + 1;
4128 : // Pointer to the new link for the current drive process item
4129 : MSLink* newLink = nullptr;
4130 1725371 : while (driveItemIt != myLFLinkLanes.end()) {
4131 1291882 : if (driveItemIt->myLink == nullptr) {
4132 : // Items not related to a specific link are not updated
4133 : // (XXX: when a stop item corresponded to a dead end, which is overcome by the LC that made
4134 : // the update necessary, this may slow down the vehicle's continuation on the new lane...)
4135 : ++driveItemIt;
4136 158668 : continue;
4137 : }
4138 : // Continuation links for current best lanes are less than for the former drive items (myLFLinkLanes)
4139 : // We just remove the leftover link-items, as they cannot be mapped to new links.
4140 1133214 : if (bestLaneIt == getBestLanesContinuation().end()) {
4141 : #ifdef DEBUG_ACTIONSTEPS
4142 : if (DEBUG_COND) {
4143 : std::cout << "Reached end of the new continuation sequence. Erasing leftover link-items." << std::endl;
4144 : }
4145 : #endif
4146 91221 : while (driveItemIt != myLFLinkLanes.end()) {
4147 62419 : if (driveItemIt->myLink == nullptr) {
4148 : ++driveItemIt;
4149 14240 : continue;
4150 : } else {
4151 48179 : driveItemIt->myLink->removeApproaching(this);
4152 : driveItemIt = myLFLinkLanes.erase(driveItemIt);
4153 : }
4154 : }
4155 : break;
4156 : }
4157 : // Do the actual link-remapping for the item. And un/register approaching information on the corresponding links
4158 1104412 : const MSLane* const target = *bestLaneIt;
4159 : assert(!target->isInternal());
4160 : newLink = nullptr;
4161 1220699 : for (MSLink* const link : lane->getLinkCont()) {
4162 1220699 : if (link->getLane() == target) {
4163 : newLink = link;
4164 : break;
4165 : }
4166 : }
4167 :
4168 1104412 : if (newLink == driveItemIt->myLink) {
4169 : // new continuation merged into previous - stop update
4170 : #ifdef DEBUG_ACTIONSTEPS
4171 : if (DEBUG_COND) {
4172 : std::cout << "Old and new continuation sequences merge at link\n"
4173 : << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'"
4174 : << "\nNo update beyond merge required." << std::endl;
4175 : }
4176 : #endif
4177 : break;
4178 : }
4179 :
4180 : #ifdef DEBUG_ACTIONSTEPS
4181 : if (DEBUG_COND) {
4182 : std::cout << "lane=" << lane->getID() << "\nUpdating link\n '" << driveItemIt->myLink->getLaneBefore()->getID() << "'->'" << driveItemIt->myLink->getViaLaneOrLane()->getID() << "'"
4183 : << "==> " << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'" << std::endl;
4184 : }
4185 : #endif
4186 1104412 : newLink->setApproaching(this, driveItemIt->myLink->getApproaching(this));
4187 1104412 : driveItemIt->myLink->removeApproaching(this);
4188 1104412 : driveItemIt->myLink = newLink;
4189 : lane = newLink->getViaLaneOrLane();
4190 : ++driveItemIt;
4191 1104412 : if (!lane->isInternal()) {
4192 : ++bestLaneIt;
4193 : }
4194 : }
4195 : #ifdef DEBUG_ACTIONSTEPS
4196 : if (DEBUG_COND) {
4197 : std::cout << "Updated drive items:" << std::endl;
4198 : for (const auto& dpi : myLFLinkLanes) {
4199 : std::cout
4200 : << " vPass=" << dpi.myVLinkPass
4201 : << " vWait=" << dpi.myVLinkWait
4202 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4203 : << " request=" << dpi.mySetRequest
4204 : << "\n";
4205 : }
4206 : }
4207 : #endif
4208 : }
4209 :
4210 :
4211 : void
4212 704154172 : MSVehicle::setBrakingSignals(double vNext) {
4213 : // To avoid casual blinking brake lights at high speeds due to dawdling of the
4214 : // leading vehicle, we don't show brake lights when the deceleration could be caused
4215 : // by frictional forces and air resistance (i.e. proportional to v^2, coefficient could be adapted further)
4216 704154172 : double pseudoFriction = (0.05 + 0.005 * getSpeed()) * getSpeed();
4217 704154172 : bool brakelightsOn = vNext < getSpeed() - ACCEL2SPEED(pseudoFriction);
4218 :
4219 704154172 : if (vNext <= SUMO_const_haltingSpeed) {
4220 : brakelightsOn = true;
4221 : }
4222 704154172 : if (brakelightsOn && !isStopped()) {
4223 : switchOnSignal(VEH_SIGNAL_BRAKELIGHT);
4224 : } else {
4225 : switchOffSignal(VEH_SIGNAL_BRAKELIGHT);
4226 : }
4227 704154172 : }
4228 :
4229 :
4230 : void
4231 704272881 : MSVehicle::updateWaitingTime(double vNext) {
4232 704272881 : if (vNext <= SUMO_const_haltingSpeed && (!isStopped() || isIdling()) && myAcceleration <= accelThresholdForWaiting()) {
4233 90247620 : myWaitingTime += DELTA_T;
4234 90247620 : myWaitingTimeCollector.passTime(DELTA_T, true);
4235 : } else {
4236 614025261 : myWaitingTime = 0;
4237 614025261 : myWaitingTimeCollector.passTime(DELTA_T, false);
4238 614025261 : if (hasInfluencer()) {
4239 272672 : getInfluencer().setExtraImpatience(0);
4240 : }
4241 : }
4242 704272881 : }
4243 :
4244 :
4245 : void
4246 704154029 : MSVehicle::updateTimeLoss(double vNext) {
4247 : // update time loss (depends on the updated edge)
4248 704154029 : if (!isStopped()) {
4249 : // some cfModels (i.e. EIDM may drive faster than predicted by maxNextSpeed)
4250 693650020 : const double vmax = MIN2(myLane->getVehicleMaxSpeed(this), MAX2(myStopSpeed, vNext));
4251 690140159 : if (vmax > 0) {
4252 689859508 : myTimeLoss += TS * (vmax - vNext) / vmax;
4253 : }
4254 : }
4255 704154029 : }
4256 :
4257 :
4258 : double
4259 1563465897 : MSVehicle::checkReversal(bool& canReverse, double speedThreshold, double seen) const {
4260 54336260 : const bool stopOk = (myStops.empty() || myStops.front().edge != myCurrEdge
4261 1585911810 : || (myStops.front().getSpeed() > 0 && myState.myPos > myStops.front().pars.endPos - 2 * POSITION_EPS));
4262 : #ifdef DEBUG_REVERSE_BIDI
4263 : if (DEBUG_COND) std::cout << SIMTIME << " checkReversal lane=" << myLane->getID()
4264 : << " pos=" << myState.myPos
4265 : << " speed=" << std::setprecision(6) << getPreviousSpeed() << std::setprecision(gPrecision)
4266 : << " speedThreshold=" << speedThreshold
4267 : << " seen=" << seen
4268 : << " isRail=" << isRail()
4269 : << " speedOk=" << (getPreviousSpeed() <= speedThreshold)
4270 : << " posOK=" << (myState.myPos <= myLane->getLength())
4271 : << " normal=" << !myLane->isInternal()
4272 : << " routeOK=" << ((myCurrEdge + 1) != myRoute->end())
4273 : << " bidi=" << (myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1))
4274 : << " stopOk=" << stopOk
4275 : << "\n";
4276 : #endif
4277 1563465897 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4278 6719092 : && getPreviousSpeed() <= speedThreshold
4279 5715050 : && myState.myPos <= myLane->getLength()
4280 5714051 : && !myLane->isInternal()
4281 5648083 : && (myCurrEdge + 1) != myRoute->end()
4282 5559397 : && myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1)
4283 : // ensure there are no further stops on this edge
4284 1564312498 : && stopOk
4285 : ) {
4286 : //if (isSelected()) std::cout << " check1 passed\n";
4287 :
4288 : // ensure that the vehicle is fully on bidi edges that allow reversal
4289 177789 : const int neededFutureRoute = 1 + (int)(MSGlobals::gUsingInternalLanes
4290 : ? myFurtherLanes.size()
4291 504 : : ceil((double)myFurtherLanes.size() / 2.0));
4292 177789 : const int remainingRoute = int(myRoute->end() - myCurrEdge) - 1;
4293 177789 : if (remainingRoute < neededFutureRoute) {
4294 : #ifdef DEBUG_REVERSE_BIDI
4295 : if (DEBUG_COND) {
4296 : std::cout << " fail: remainingEdges=" << ((int)(myRoute->end() - myCurrEdge)) << " further=" << myFurtherLanes.size() << "\n";
4297 : }
4298 : #endif
4299 3570 : return getMaxSpeed();
4300 : }
4301 : //if (isSelected()) std::cout << " check2 passed\n";
4302 :
4303 : // ensure that the turn-around connection exists from the current edge to its bidi-edge
4304 174219 : const MSEdgeVector& succ = myLane->getEdge().getSuccessors();
4305 174219 : if (std::find(succ.begin(), succ.end(), myLane->getEdge().getBidiEdge()) == succ.end()) {
4306 : #ifdef DEBUG_REVERSE_BIDI
4307 : if (DEBUG_COND) {
4308 : std::cout << " noTurn (bidi=" << myLane->getEdge().getBidiEdge()->getID() << " succ=" << toString(succ) << "\n";
4309 : }
4310 : #endif
4311 909 : return getMaxSpeed();
4312 : }
4313 : //if (isSelected()) std::cout << " check3 passed\n";
4314 :
4315 : // ensure that the vehicle front will not move past a stop on the bidi edge of the current edge
4316 173310 : if (!myStops.empty() && myStops.front().edge == (myCurrEdge + 1)) {
4317 160006 : const double stopPos = myStops.front().getEndPos(*this);
4318 160006 : const double brakeDist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
4319 160006 : const double newPos = myLane->getLength() - (getBackPositionOnLane() + brakeDist);
4320 160006 : if (newPos > stopPos) {
4321 : #ifdef DEBUG_REVERSE_BIDI
4322 : if (DEBUG_COND) {
4323 : std::cout << " reversal would go past stop on " << myLane->getBidiLane()->getID() << "\n";
4324 : }
4325 : #endif
4326 158332 : if (seen > MAX2(brakeDist, 1.0)) {
4327 157202 : return getMaxSpeed();
4328 : } else {
4329 : #ifdef DEBUG_REVERSE_BIDI
4330 : if (DEBUG_COND) {
4331 : std::cout << " train is too long, skipping stop at " << stopPos << " cannot be avoided\n";
4332 : }
4333 : #endif
4334 : }
4335 : }
4336 : }
4337 : //if (isSelected()) std::cout << " check4 passed\n";
4338 :
4339 : // ensure that bidi-edges exist for all further edges
4340 : // and that no stops will be skipped when reversing
4341 : // and that the train will not be on top of a red rail signal after reversal
4342 16108 : const MSLane* bidi = myLane->getBidiLane();
4343 : int view = 2;
4344 32828 : for (MSLane* further : myFurtherLanes) {
4345 18277 : if (!further->getEdge().isInternal()) {
4346 9709 : if (further->getEdge().getBidiEdge() != *(myCurrEdge + view)) {
4347 : #ifdef DEBUG_REVERSE_BIDI
4348 : if (DEBUG_COND) {
4349 : std::cout << " noBidi view=" << view << " further=" << further->getID() << " furtherBidi=" << Named::getIDSecure(further->getEdge().getBidiEdge()) << " future=" << (*(myCurrEdge + view))->getID() << "\n";
4350 : }
4351 : #endif
4352 1398 : return getMaxSpeed();
4353 : }
4354 8311 : const MSLane* nextBidi = further->getBidiLane();
4355 8311 : const MSLink* toNext = bidi->getLinkTo(nextBidi);
4356 8311 : if (toNext == nullptr) {
4357 : // can only happen if the route is invalid
4358 0 : return getMaxSpeed();
4359 : }
4360 8311 : if (toNext->haveRed()) {
4361 : #ifdef DEBUG_REVERSE_BIDI
4362 : if (DEBUG_COND) {
4363 : std::cout << " do not reverse on a red signal\n";
4364 : }
4365 : #endif
4366 0 : return getMaxSpeed();
4367 : }
4368 : bidi = nextBidi;
4369 8311 : if (!myStops.empty() && myStops.front().edge == (myCurrEdge + view)) {
4370 453 : const double brakeDist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
4371 453 : const double stopPos = myStops.front().getEndPos(*this);
4372 453 : const double newPos = further->getLength() - (getBackPositionOnLane(further) + brakeDist);
4373 453 : if (newPos > stopPos) {
4374 : #ifdef DEBUG_REVERSE_BIDI
4375 : if (DEBUG_COND) {
4376 : std::cout << " reversal would go past stop on further-opposite lane " << further->getBidiLane()->getID() << "\n";
4377 : }
4378 : #endif
4379 171 : if (seen > MAX2(brakeDist, 1.0)) {
4380 159 : canReverse = false;
4381 159 : return getMaxSpeed();
4382 : } else {
4383 : #ifdef DEBUG_REVERSE_BIDI
4384 : if (DEBUG_COND) {
4385 : std::cout << " train is too long, skipping stop at " << stopPos << " cannot be avoided\n";
4386 : }
4387 : #endif
4388 : }
4389 : }
4390 : }
4391 8152 : view++;
4392 : }
4393 : }
4394 : // reverse as soon as comfortably possible
4395 14551 : const double vMinComfortable = getCarFollowModel().minNextSpeed(getSpeed(), this);
4396 : #ifdef DEBUG_REVERSE_BIDI
4397 : if (DEBUG_COND) {
4398 : std::cout << SIMTIME << " seen=" << seen << " vReverseOK=" << vMinComfortable << "\n";
4399 : }
4400 : #endif
4401 14551 : canReverse = true;
4402 14551 : return vMinComfortable;
4403 : }
4404 1563288108 : return getMaxSpeed();
4405 : }
4406 :
4407 :
4408 : void
4409 704369264 : MSVehicle::processLaneAdvances(std::vector<MSLane*>& passedLanes, std::string& emergencyReason) {
4410 720656582 : for (std::vector<MSLane*>::reverse_iterator i = myFurtherLanes.rbegin(); i != myFurtherLanes.rend(); ++i) {
4411 16287318 : passedLanes.push_back(*i);
4412 : }
4413 704369264 : if (passedLanes.size() == 0 || passedLanes.back() != myLane) {
4414 704369264 : passedLanes.push_back(myLane);
4415 : }
4416 : // let trains reverse direction
4417 704369264 : bool reverseTrain = false;
4418 704369264 : checkReversal(reverseTrain);
4419 704369264 : if (reverseTrain) {
4420 : // Train is 'reversing' so toggle the logical state
4421 772 : myAmReversed = !myAmReversed;
4422 : // add some slack to ensure that the back of train does appear looped
4423 772 : myState.myPos += 2 * (myLane->getLength() - myState.myPos) + myType->getLength() + NUMERICAL_EPS;
4424 772 : myState.mySpeed = 0;
4425 : #ifdef DEBUG_REVERSE_BIDI
4426 : if (DEBUG_COND) {
4427 : std::cout << SIMTIME << " reversing train=" << getID() << " newPos=" << myState.myPos << "\n";
4428 : }
4429 : #endif
4430 : }
4431 : // move on lane(s)
4432 704369264 : if (myState.myPos > myLane->getLength()) {
4433 : // The vehicle has moved at least to the next lane (maybe it passed even more than one)
4434 21203042 : if (myCurrEdge != myRoute->end() - 1) {
4435 17837513 : MSLane* approachedLane = myLane;
4436 : // move the vehicle forward
4437 17837513 : myNextDriveItem = myLFLinkLanes.begin();
4438 38311081 : while (myNextDriveItem != myLFLinkLanes.end() && approachedLane != nullptr && myState.myPos > approachedLane->getLength()) {
4439 20498601 : const MSLink* link = myNextDriveItem->myLink;
4440 20498601 : const double linkDist = myNextDriveItem->myDistance;
4441 : ++myNextDriveItem;
4442 : // check whether the vehicle was allowed to enter lane
4443 : // otherwise it is decelerated and we do not need to test for it's
4444 : // approach on the following lanes when a lane changing is performed
4445 : // proceed to the next lane
4446 20498601 : if (approachedLane->mustCheckJunctionCollisions()) {
4447 : // vehicle moves past approachedLane within a single step, collision checking must still be done
4448 61415 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(approachedLane);
4449 : }
4450 20498601 : if (link != nullptr) {
4451 20494494 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4452 40664 : && !myLane->isInternal()
4453 21541 : && myLane->getBidiLane() != nullptr
4454 12073 : && link->getLane()->getBidiLane() == myLane
4455 20495263 : && !reverseTrain) {
4456 : emergencyReason = " because it must reverse direction";
4457 : approachedLane = nullptr;
4458 : break;
4459 : }
4460 20494491 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4461 40661 : && myState.myPos < myLane->getLength() + NUMERICAL_EPS
4462 20494696 : && hasStops() && getNextStop().edge == myCurrEdge) {
4463 : // avoid skipping stop due to numerical instability
4464 : // this is a special case for rail vehicles because they
4465 : // continue myLFLinkLanes past stops
4466 199 : approachedLane = myLane;
4467 199 : myState.myPos = myLane->getLength();
4468 199 : break;
4469 : }
4470 20494292 : approachedLane = link->getViaLaneOrLane();
4471 20494292 : if (myInfluencer == nullptr || myInfluencer->getEmergencyBrakeRedLight()) {
4472 20492695 : bool beyondStopLine = linkDist < link->getLaneBefore()->getVehicleStopOffset(this);
4473 20492695 : if (link->haveRed() && !ignoreRed(link, false) && !beyondStopLine && !reverseTrain) {
4474 : emergencyReason = " because of a red traffic light";
4475 : break;
4476 : }
4477 : }
4478 20494234 : if (reverseTrain && approachedLane->isInternal()) {
4479 : // avoid getting stuck on a slow turn-around internal lane
4480 842 : myState.myPos += approachedLane->getLength();
4481 : }
4482 4107 : } else if (myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
4483 : // avoid warning due to numerical instability
4484 164 : approachedLane = myLane;
4485 164 : myState.myPos = myLane->getLength();
4486 3943 : } else if (reverseTrain) {
4487 0 : approachedLane = (*(myCurrEdge + 1))->getLanes()[0];
4488 0 : link = myLane->getLinkTo(approachedLane);
4489 : assert(link != 0);
4490 0 : while (link->getViaLane() != nullptr) {
4491 0 : link = link->getViaLane()->getLinkCont()[0];
4492 : }
4493 : --myNextDriveItem;
4494 : } else {
4495 : emergencyReason = " because there is no connection to the next edge";
4496 : approachedLane = nullptr;
4497 : break;
4498 : }
4499 20494398 : if (approachedLane != myLane && approachedLane != nullptr) {
4500 20494234 : leaveLane(MSMoveReminder::NOTIFICATION_JUNCTION, approachedLane);
4501 20494234 : myState.myPos -= myLane->getLength();
4502 : assert(myState.myPos > 0);
4503 20494234 : enterLaneAtMove(approachedLane);
4504 20494234 : if (link->isEntryLink()) {
4505 8176024 : myJunctionEntryTime = MSNet::getInstance()->getCurrentTimeStep();
4506 8176024 : myJunctionEntryTimeNeverYield = myJunctionEntryTime;
4507 8176024 : myHaveStoppedFor = nullptr;
4508 : }
4509 20494234 : if (link->isConflictEntryLink()) {
4510 8175409 : myJunctionConflictEntryTime = MSNet::getInstance()->getCurrentTimeStep();
4511 : // renew yielded request
4512 8175409 : myJunctionEntryTime = myJunctionEntryTimeNeverYield;
4513 : }
4514 20494234 : if (link->isExitLink()) {
4515 : // passed junction, reset for approaching the next one
4516 8115559 : myJunctionEntryTime = SUMOTime_MAX;
4517 8115559 : myJunctionEntryTimeNeverYield = SUMOTime_MAX;
4518 8115559 : myJunctionConflictEntryTime = SUMOTime_MAX;
4519 : }
4520 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
4521 : if (DEBUG_COND) {
4522 : std::cout << "Update junctionTimes link=" << link->getViaLaneOrLane()->getID()
4523 : << " entry=" << link->isEntryLink() << " conflict=" << link->isConflictEntryLink() << " exit=" << link->isExitLink()
4524 : << " ET=" << myJunctionEntryTime
4525 : << " ETN=" << myJunctionEntryTimeNeverYield
4526 : << " CET=" << myJunctionConflictEntryTime
4527 : << "\n";
4528 : }
4529 : #endif
4530 20494234 : if (hasArrivedInternal()) {
4531 : break;
4532 : }
4533 20474160 : if (myLaneChangeModel->isChangingLanes()) {
4534 6570 : if (link->getDirection() == LinkDirection::LEFT || link->getDirection() == LinkDirection::RIGHT) {
4535 : // abort lane change
4536 0 : WRITE_WARNING("Vehicle '" + getID() + "' could not finish continuous lane change (turn lane) time=" +
4537 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
4538 0 : myLaneChangeModel->endLaneChangeManeuver();
4539 : }
4540 : }
4541 20474160 : if (approachedLane->getEdge().isVaporizing()) {
4542 756 : leaveLane(MSMoveReminder::NOTIFICATION_VAPORIZED_VAPORIZER);
4543 : break;
4544 : }
4545 20473404 : passedLanes.push_back(approachedLane);
4546 : }
4547 : }
4548 : // NOTE: Passed drive items will be erased in the next simstep's planMove()
4549 :
4550 : #ifdef DEBUG_ACTIONSTEPS
4551 : if (DEBUG_COND && myNextDriveItem != myLFLinkLanes.begin()) {
4552 : std::cout << "Updated drive items:" << std::endl;
4553 : for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
4554 : std::cout
4555 : << " vPass=" << (*i).myVLinkPass
4556 : << " vWait=" << (*i).myVLinkWait
4557 : << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
4558 : << " request=" << (*i).mySetRequest
4559 : << "\n";
4560 : }
4561 : }
4562 : #endif
4563 3365529 : } else if (!hasArrivedInternal() && myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
4564 : // avoid warning due to numerical instability when stopping at the end of the route
4565 59 : myState.myPos = myLane->getLength();
4566 : }
4567 :
4568 : }
4569 704369264 : }
4570 :
4571 :
4572 :
4573 : bool
4574 705263362 : MSVehicle::executeMove() {
4575 : #ifdef DEBUG_EXEC_MOVE
4576 : if (DEBUG_COND) {
4577 : std::cout << "\nEXECUTE_MOVE\n"
4578 : << SIMTIME
4579 : << " veh=" << getID()
4580 : << " speed=" << getSpeed() // toString(getSpeed(), 24)
4581 : << std::endl;
4582 : }
4583 : #endif
4584 :
4585 :
4586 : // Maximum safe velocity
4587 705263362 : double vSafe = std::numeric_limits<double>::max();
4588 : // Minimum safe velocity (lower bound).
4589 705263362 : double vSafeMin = -std::numeric_limits<double>::max();
4590 : // The distance to a link, which should either be crossed this step
4591 : // or in front of which we need to stop.
4592 705263362 : double vSafeMinDist = 0;
4593 :
4594 705263362 : if (myActionStep) {
4595 : // Actuate control (i.e. choose bounds for safe speed in current simstep (euler), resp. after current sim step (ballistic))
4596 633588686 : processLinkApproaches(vSafe, vSafeMin, vSafeMinDist);
4597 : #ifdef DEBUG_ACTIONSTEPS
4598 : if (DEBUG_COND) {
4599 : std::cout << SIMTIME << " vehicle '" << getID() << "'\n"
4600 : " vsafe from processLinkApproaches(): vsafe " << vSafe << std::endl;
4601 : }
4602 : #endif
4603 : } else {
4604 : // Continue with current acceleration
4605 71674676 : vSafe = getSpeed() + ACCEL2SPEED(myAcceleration);
4606 : #ifdef DEBUG_ACTIONSTEPS
4607 : if (DEBUG_COND) {
4608 : std::cout << SIMTIME << " vehicle '" << getID() << "' skips processLinkApproaches()\n"
4609 : " continues with constant accel " << myAcceleration << "...\n"
4610 : << "speed: " << getSpeed() << " -> " << vSafe << std::endl;
4611 : }
4612 : #endif
4613 : }
4614 :
4615 :
4616 : //#ifdef DEBUG_EXEC_MOVE
4617 : // if (DEBUG_COND) {
4618 : // std::cout << "vSafe = " << toString(vSafe,12) << "\n" << std::endl;
4619 : // }
4620 : //#endif
4621 :
4622 : // Determine vNext = speed after current sim step (ballistic), resp. in current simstep (euler)
4623 : // Call to finalizeSpeed applies speed reduction due to dawdling / lane changing but ensures minimum safe speed
4624 705263362 : double vNext = vSafe;
4625 705263362 : const double rawAccel = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
4626 705263362 : if (vNext <= SUMO_const_haltingSpeed * TS && myWaitingTime > MSGlobals::gStartupWaitThreshold && rawAccel <= accelThresholdForWaiting() && myActionStep) {
4627 73813099 : myTimeSinceStartup = 0;
4628 631450263 : } else if (isStopped()) {
4629 : // do not apply startupDelay for waypoints
4630 15111272 : if (getCarFollowModel().startupDelayStopped() && getNextStop().pars.speed <= 0) {
4631 13772 : myTimeSinceStartup = DELTA_T;
4632 : } else {
4633 : // do not apply startupDelay but signal that a stop has taken place
4634 15097500 : myTimeSinceStartup = getCarFollowModel().getStartupDelay() + DELTA_T;
4635 : }
4636 : } else {
4637 : // identify potential startup (before other effects reduce the speed again)
4638 616338991 : myTimeSinceStartup += DELTA_T;
4639 : }
4640 705263362 : if (myActionStep) {
4641 633588686 : vNext = getCarFollowModel().finalizeSpeed(this, vSafe);
4642 632479353 : if (vNext > 0) {
4643 590406638 : vNext = MAX2(vNext, vSafeMin);
4644 : }
4645 : }
4646 : // (Leo) to avoid tiny oscillations (< 1e-10) of vNext in a standing vehicle column (observed for ballistic update), we cap off vNext
4647 : // (We assure to do this only for vNext<<NUMERICAL_EPS since otherwise this would nullify the workaround for #2995
4648 : // (Jakob) We also need to make sure to reach a stop at the start of the next edge
4649 704154029 : if (fabs(vNext) < NUMERICAL_EPS_SPEED && (myStopDist > POSITION_EPS || (hasStops() && myCurrEdge == getNextStop().edge))) {
4650 : vNext = 0.;
4651 : }
4652 : #ifdef DEBUG_EXEC_MOVE
4653 : if (DEBUG_COND) {
4654 : std::cout << SIMTIME << " finalizeSpeed vSafe=" << vSafe << " vSafeMin=" << (vSafeMin == -std::numeric_limits<double>::max() ? "-Inf" : toString(vSafeMin))
4655 : << " vNext=" << vNext << " (i.e. accel=" << SPEED2ACCEL(vNext - getSpeed()) << ")" << std::endl;
4656 : }
4657 : #endif
4658 :
4659 : // vNext may be higher than vSafe without implying a bug:
4660 : // - when approaching a green light that suddenly switches to yellow
4661 : // - when using unregulated junctions
4662 : // - when using tau < step-size
4663 : // - when using unsafe car following models
4664 : // - when using TraCI and some speedMode / laneChangeMode settings
4665 : //if (vNext > vSafe + NUMERICAL_EPS) {
4666 : // WRITE_WARNING("vehicle '" + getID() + "' cannot brake hard enough to reach safe speed "
4667 : // + toString(vSafe, 4) + ", moving at " + toString(vNext, 4) + " instead. time="
4668 : // + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
4669 : //}
4670 :
4671 704154029 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4672 : vNext = MAX2(vNext, 0.);
4673 : } else {
4674 : // (Leo) Ballistic: negative vNext can be used to indicate a stop within next step.
4675 : }
4676 :
4677 : // Check for speed advices from the traci client
4678 704154029 : vNext = processTraCISpeedControl(vSafe, vNext);
4679 :
4680 : // the acceleration of a vehicle equipped with the elecHybrid device is restricted by the maximal power of the electric drive as well
4681 704154029 : MSDevice_ElecHybrid* elecHybridOfVehicle = dynamic_cast<MSDevice_ElecHybrid*>(getDevice(typeid(MSDevice_ElecHybrid)));
4682 981 : if (elecHybridOfVehicle != nullptr) {
4683 : // this is the consumption given by the car following model-computed acceleration
4684 981 : elecHybridOfVehicle->setConsum(elecHybridOfVehicle->consumption(*this, (vNext - this->getSpeed()) / TS, vNext));
4685 : // but the maximum power of the electric motor may be lower
4686 : // it needs to be converted from [W] to [Wh/s] (3600s / 1h) so that TS can be taken into account
4687 981 : double maxPower = getEmissionParameters()->getDoubleOptional(SUMO_ATTR_MAXIMUMPOWER, 100000.) / 3600;
4688 981 : if (elecHybridOfVehicle->getConsum() / TS > maxPower) {
4689 : // no, we cannot accelerate that fast, recompute the maximum possible acceleration
4690 70 : double accel = elecHybridOfVehicle->acceleration(*this, maxPower, this->getSpeed());
4691 : // and update the speed of the vehicle
4692 70 : vNext = MIN2(vNext, this->getSpeed() + accel * TS);
4693 : vNext = MAX2(vNext, 0.);
4694 : // and set the vehicle consumption to reflect this
4695 70 : elecHybridOfVehicle->setConsum(elecHybridOfVehicle->consumption(*this, (vNext - this->getSpeed()) / TS, vNext));
4696 : }
4697 : }
4698 :
4699 704154029 : setBrakingSignals(vNext);
4700 :
4701 : // update position and speed
4702 704154029 : int oldLaneOffset = myLane->getEdge().getNumLanes() - myLane->getIndex();
4703 : const MSLane* oldLaneMaybeOpposite = myLane;
4704 704154029 : if (myLaneChangeModel->isOpposite()) {
4705 : // transform to the forward-direction lane, move and then transform back
4706 405226 : myState.myPos = myLane->getOppositePos(myState.myPos);
4707 405226 : myLane = myLane->getParallelOpposite();
4708 : }
4709 704154029 : updateState(vNext);
4710 704154029 : updateWaitingTime(vNext);
4711 :
4712 : // Lanes, which the vehicle touched at some moment of the executed simstep
4713 : std::vector<MSLane*> passedLanes;
4714 : // remember previous lane (myLane is updated in processLaneAdvances)
4715 704154029 : const MSLane* oldLane = myLane;
4716 : // Reason for a possible emergency stop
4717 : std::string emergencyReason;
4718 704154029 : processLaneAdvances(passedLanes, emergencyReason);
4719 :
4720 704154029 : updateTimeLoss(vNext);
4721 704154029 : myCollisionImmunity = MAX2((SUMOTime) - 1, myCollisionImmunity - DELTA_T);
4722 :
4723 704154029 : if (!hasArrivedInternal() && !myLane->getEdge().isVaporizing()) {
4724 700575192 : if (myState.myPos > myLane->getLength()) {
4725 345 : if (emergencyReason == "") {
4726 31 : emergencyReason = TL(" for unknown reasons");
4727 : }
4728 1380 : WRITE_WARNINGF(TL("Vehicle '%' performs emergency stop at the end of lane '%'% (decel=%, offset=%), time=%."),
4729 : getID(), myLane->getID(), emergencyReason, myAcceleration - myState.mySpeed,
4730 : myState.myPos - myLane->getLength(), time2string(SIMSTEP));
4731 345 : MSNet::getInstance()->getVehicleControl().registerEmergencyStop();
4732 345 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::EMERGENCYSTOP);
4733 345 : myState.myPos = myLane->getLength();
4734 345 : myState.mySpeed = 0;
4735 345 : myAcceleration = 0;
4736 : }
4737 700575192 : const MSLane* oldBackLane = getBackLane();
4738 700575192 : if (myLaneChangeModel->isOpposite()) {
4739 : passedLanes.clear(); // ignore back occupation
4740 : }
4741 : #ifdef DEBUG_ACTIONSTEPS
4742 : if (DEBUG_COND) {
4743 : std::cout << SIMTIME << " veh '" << getID() << "' updates further lanes." << std::endl;
4744 : }
4745 : #endif
4746 700575192 : myState.myBackPos = updateFurtherLanes(myFurtherLanes, myFurtherLanesPosLat, passedLanes);
4747 700575192 : if (passedLanes.size() > 1 && isRail()) {
4748 750763 : for (auto pi = passedLanes.rbegin(); pi != passedLanes.rend(); ++pi) {
4749 568750 : MSLane* pLane = *pi;
4750 568750 : if (pLane != myLane && std::find(myFurtherLanes.begin(), myFurtherLanes.end(), pLane) == myFurtherLanes.end()) {
4751 40349 : leaveLaneBack(MSMoveReminder::NOTIFICATION_JUNCTION, *pi);
4752 : }
4753 : }
4754 : }
4755 : // bestLanes need to be updated before lane changing starts. NOTE: This call is also a presumption for updateDriveItems()
4756 700575192 : updateBestLanes();
4757 700575192 : if (myLane != oldLane || oldBackLane != getBackLane()) {
4758 26172898 : if (myLaneChangeModel->getShadowLane() != nullptr || getLateralOverlap() > POSITION_EPS) {
4759 : // shadow lane must be updated if the front or back lane changed
4760 : // either if we already have a shadowLane or if there is lateral overlap
4761 722228 : myLaneChangeModel->updateShadowLane();
4762 : }
4763 26172898 : if (MSGlobals::gLateralResolution > 0 && !myLaneChangeModel->isOpposite()) {
4764 : // The vehicles target lane must be also be updated if the front or back lane changed
4765 4324700 : myLaneChangeModel->updateTargetLane();
4766 : }
4767 : }
4768 700575192 : setBlinkerInformation(); // needs updated bestLanes
4769 : //change the blue light only for emergency vehicles SUMOVehicleClass
4770 700575192 : if (myType->getVehicleClass() == SVC_EMERGENCY) {
4771 85904 : setEmergencyBlueLight(MSNet::getInstance()->getCurrentTimeStep());
4772 : }
4773 : // must be done before angle computation
4774 : // State needs to be reset for all vehicles before the next call to MSEdgeControl::changeLanes
4775 700575192 : if (myActionStep) {
4776 : // check (#2681): Can this be skipped?
4777 628921743 : myLaneChangeModel->prepareStep();
4778 : } else {
4779 71653449 : myLaneChangeModel->resetSpeedLat();
4780 : #ifdef DEBUG_ACTIONSTEPS
4781 : if (DEBUG_COND) {
4782 : std::cout << SIMTIME << " veh '" << getID() << "' skips LCM->prepareStep()." << std::endl;
4783 : }
4784 : #endif
4785 : }
4786 700575192 : myLaneChangeModel->setPreviousAngleOffset(myLaneChangeModel->getAngleOffset());
4787 700575192 : myAngle = computeAngle();
4788 : }
4789 :
4790 : #ifdef DEBUG_EXEC_MOVE
4791 : if (DEBUG_COND) {
4792 : std::cout << SIMTIME << " executeMove finished veh=" << getID() << " lane=" << myLane->getID() << " myPos=" << getPositionOnLane() << " myPosLat=" << getLateralPositionOnLane() << "\n";
4793 : gDebugFlag1 = false; // See MSLink_DEBUG_OPENED
4794 : }
4795 : #endif
4796 704154029 : if (myLaneChangeModel->isOpposite()) {
4797 : // transform back to the opposite-direction lane
4798 : MSLane* newOpposite = nullptr;
4799 405226 : const MSEdge* newOppositeEdge = myLane->getEdge().getOppositeEdge();
4800 405226 : if (newOppositeEdge != nullptr) {
4801 405176 : newOpposite = newOppositeEdge->getLanes()[newOppositeEdge->getNumLanes() - MAX2(1, oldLaneOffset)];
4802 : #ifdef DEBUG_EXEC_MOVE
4803 : if (DEBUG_COND) {
4804 : std::cout << SIMTIME << " newOppositeEdge=" << newOppositeEdge->getID() << " oldLaneOffset=" << oldLaneOffset << " leftMost=" << newOppositeEdge->getNumLanes() - 1 << " newOpposite=" << Named::getIDSecure(newOpposite) << "\n";
4805 : }
4806 : #endif
4807 : }
4808 405176 : if (newOpposite == nullptr) {
4809 50 : if (!myLaneChangeModel->hasBlueLight()) {
4810 : // unusual overtaking at junctions is ok for emergency vehicles
4811 0 : WRITE_WARNINGF(TL("Unexpected end of opposite lane for vehicle '%' at lane '%', time=%."),
4812 : getID(), myLane->getID(), time2string(SIMSTEP));
4813 : }
4814 50 : myLaneChangeModel->changedToOpposite();
4815 50 : if (myState.myPos < getLength()) {
4816 : // further lanes is always cleared during opposite driving
4817 50 : MSLane* oldOpposite = oldLane->getOpposite();
4818 50 : if (oldOpposite != nullptr) {
4819 50 : myFurtherLanes.push_back(oldOpposite);
4820 50 : myFurtherLanesPosLat.push_back(0);
4821 : // small value since the lane is going in the other direction
4822 50 : myState.myBackPos = getLength() - myState.myPos;
4823 50 : myAngle = computeAngle();
4824 : } else {
4825 : SOFT_ASSERT(false);
4826 : }
4827 : }
4828 : } else {
4829 405176 : myState.myPos = myLane->getOppositePos(myState.myPos);
4830 405176 : myLane = newOpposite;
4831 : oldLane = oldLaneMaybeOpposite;
4832 : //std::cout << SIMTIME << " updated myLane=" << Named::getIDSecure(myLane) << " oldLane=" << oldLane->getID() << "\n";
4833 405176 : myCachedPosition = Position::INVALID;
4834 405176 : myLaneChangeModel->updateShadowLane();
4835 : }
4836 : }
4837 704154029 : workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
4838 : // Return whether the vehicle did move to another lane
4839 1408308058 : return myLane != oldLane;
4840 704154029 : }
4841 :
4842 : void
4843 215235 : MSVehicle::executeFractionalMove(double dist) {
4844 215235 : myState.myPos += dist;
4845 215235 : myState.myLastCoveredDist = dist;
4846 215235 : myCachedPosition = Position::INVALID;
4847 :
4848 215235 : const std::vector<const MSLane*> lanes = getUpcomingLanesUntil(dist);
4849 215235 : const SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
4850 446475 : for (int i = 0; i < (int)lanes.size(); i++) {
4851 231240 : MSLink* link = nullptr;
4852 231240 : if (i + 1 < (int)lanes.size()) {
4853 16005 : const MSLane* const to = lanes[i + 1];
4854 16005 : const bool internal = to->isInternal();
4855 16010 : for (MSLink* const l : lanes[i]->getLinkCont()) {
4856 16010 : if ((internal && l->getViaLane() == to) || (!internal && l->getLane() == to)) {
4857 16005 : link = l;
4858 16005 : break;
4859 : }
4860 : }
4861 : }
4862 231240 : myLFLinkLanes.emplace_back(link, getSpeed(), getSpeed(), true, t, getSpeed(), 0, 0, dist);
4863 : }
4864 : // minimum execute move:
4865 : std::vector<MSLane*> passedLanes;
4866 : // Reason for a possible emergency stop
4867 215235 : if (lanes.size() > 1) {
4868 4005 : myLane->removeVehicle(this, MSMoveReminder::NOTIFICATION_JUNCTION, false);
4869 : }
4870 : std::string emergencyReason;
4871 215235 : processLaneAdvances(passedLanes, emergencyReason);
4872 : #ifdef DEBUG_EXTRAPOLATE_DEPARTPOS
4873 : if (DEBUG_COND) {
4874 : std::cout << SIMTIME << " veh=" << getID() << " executeFractionalMove dist=" << dist
4875 : << " passedLanes=" << toString(passedLanes) << " lanes=" << toString(lanes)
4876 : << " finalPos=" << myState.myPos
4877 : << " speed=" << getSpeed()
4878 : << " myFurtherLanes=" << toString(myFurtherLanes)
4879 : << "\n";
4880 : }
4881 : #endif
4882 215235 : workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
4883 215235 : if (lanes.size() > 1) {
4884 4010 : for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
4885 : #ifdef DEBUG_FURTHER
4886 : if (DEBUG_COND) {
4887 : std::cout << SIMTIME << " leaveLane \n";
4888 : }
4889 : #endif
4890 5 : (*i)->resetPartialOccupation(this);
4891 : }
4892 : myFurtherLanes.clear();
4893 : myFurtherLanesPosLat.clear();
4894 4005 : myLane->forceVehicleInsertion(this, getPositionOnLane(), MSMoveReminder::NOTIFICATION_JUNCTION, getLateralPositionOnLane());
4895 : }
4896 215235 : }
4897 :
4898 :
4899 : void
4900 712227091 : MSVehicle::updateState(double vNext, bool parking) {
4901 : // update position and speed
4902 : double deltaPos; // positional change
4903 712227091 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4904 : // euler
4905 612958290 : deltaPos = SPEED2DIST(vNext);
4906 : } else {
4907 : // ballistic
4908 99268801 : deltaPos = getDeltaPos(SPEED2ACCEL(vNext - myState.mySpeed));
4909 : }
4910 :
4911 : // the *mean* acceleration during the next step (probably most appropriate for emission calculation)
4912 : // NOTE: for the ballistic update vNext may be negative, indicating a stop.
4913 712227091 : myAcceleration = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
4914 :
4915 : #ifdef DEBUG_EXEC_MOVE
4916 : if (DEBUG_COND) {
4917 : std::cout << SIMTIME << " updateState() for veh '" << getID() << "': deltaPos=" << deltaPos
4918 : << " pos=" << myState.myPos << " newPos=" << myState.myPos + deltaPos << std::endl;
4919 : }
4920 : #endif
4921 712227091 : double decelPlus = -myAcceleration - getCarFollowModel().getMaxDecel() - NUMERICAL_EPS;
4922 712227091 : if (decelPlus > 0) {
4923 426688 : const double previousAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
4924 426688 : if (myAcceleration + NUMERICAL_EPS < previousAcceleration) {
4925 : // vehicle brakes beyond wished maximum deceleration (only warn at the start of the braking manoeuvre)
4926 292241 : decelPlus += 2 * NUMERICAL_EPS;
4927 292241 : const double emergencyFraction = decelPlus / MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel());
4928 292241 : if (emergencyFraction >= MSGlobals::gEmergencyDecelWarningThreshold) {
4929 94821 : WRITE_WARNINGF(TL("Vehicle '%' performs emergency braking on lane '%' with decel=%, wished=%, severity=%, time=%."),
4930 : //+ " decelPlus=" + toString(decelPlus)
4931 : //+ " prevAccel=" + toString(previousAcceleration)
4932 : //+ " reserve=" + toString(MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel()))
4933 : getID(), myLane->getID(), -myAcceleration, getCarFollowModel().getMaxDecel(), emergencyFraction, time2string(SIMSTEP));
4934 31607 : MSNet::getInstance()->getVehicleControl().registerEmergencyBraking();
4935 : }
4936 : }
4937 : }
4938 :
4939 712227091 : myState.myPreviousSpeed = myState.mySpeed;
4940 712227091 : myState.mySpeed = MAX2(vNext, 0.);
4941 :
4942 712227091 : if (isRemoteControlled()) {
4943 6614 : deltaPos = myInfluencer->implicitDeltaPosRemote(this);
4944 : }
4945 :
4946 712227091 : myState.myPos += deltaPos;
4947 712227091 : myState.myLastCoveredDist = deltaPos;
4948 712227091 : myNextTurn.first -= deltaPos;
4949 :
4950 712227091 : if (!parking) {
4951 704154029 : myCachedPosition = Position::INVALID;
4952 : }
4953 712227091 : }
4954 :
4955 : void
4956 8073062 : MSVehicle::updateParkingState() {
4957 8073062 : updateState(0, true);
4958 : // deboard while parked
4959 8073062 : if (myPersonDevice != nullptr) {
4960 586852 : myPersonDevice->notifyMove(*this, getPositionOnLane(), getPositionOnLane(), 0);
4961 : }
4962 8073062 : if (myContainerDevice != nullptr) {
4963 59887 : myContainerDevice->notifyMove(*this, getPositionOnLane(), getPositionOnLane(), 0);
4964 : }
4965 16156256 : for (MSVehicleDevice* const dev : myDevices) {
4966 8083194 : dev->notifyParking();
4967 : }
4968 8073062 : }
4969 :
4970 :
4971 : void
4972 30666 : MSVehicle::replaceVehicleType(const MSVehicleType* type) {
4973 30666 : MSBaseVehicle::replaceVehicleType(type);
4974 30666 : delete myCFVariables;
4975 30666 : myCFVariables = type->getCarFollowModel().createVehicleVariables();
4976 30666 : }
4977 :
4978 :
4979 : const MSLane*
4980 1384302951 : MSVehicle::getBackLane() const {
4981 1384302951 : if (myFurtherLanes.size() > 0) {
4982 19314848 : return myFurtherLanes.back();
4983 : } else {
4984 1364988103 : return myLane;
4985 : }
4986 : }
4987 :
4988 :
4989 : double
4990 705930105 : MSVehicle::updateFurtherLanes(std::vector<MSLane*>& furtherLanes, std::vector<double>& furtherLanesPosLat,
4991 : const std::vector<MSLane*>& passedLanes) {
4992 : #ifdef DEBUG_SETFURTHER
4993 : if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID()
4994 : << " updateFurtherLanes oldFurther=" << toString(furtherLanes)
4995 : << " oldFurtherPosLat=" << toString(furtherLanesPosLat)
4996 : << " passed=" << toString(passedLanes)
4997 : << "\n";
4998 : #endif
4999 722244622 : for (MSLane* further : furtherLanes) {
5000 16314517 : further->resetPartialOccupation(this);
5001 16314517 : if (further->getBidiLane() != nullptr
5002 16314517 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5003 25720 : further->getBidiLane()->resetPartialOccupation(this);
5004 : }
5005 : }
5006 :
5007 : std::vector<MSLane*> newFurther;
5008 : std::vector<double> newFurtherPosLat;
5009 705930105 : double backPosOnPreviousLane = myState.myPos - getLength();
5010 : bool widthShift = myFurtherLanesPosLat.size() > myFurtherLanes.size();
5011 705930105 : if (passedLanes.size() > 1) {
5012 : // There are candidates for further lanes. (passedLanes[-1] is the current lane, or current shadow lane in context of updateShadowLanes())
5013 : std::vector<MSLane*>::const_iterator fi = furtherLanes.begin();
5014 : std::vector<double>::const_iterator fpi = furtherLanesPosLat.begin();
5015 46347553 : for (auto pi = passedLanes.rbegin() + 1; pi != passedLanes.rend() && backPosOnPreviousLane < 0; ++pi) {
5016 : // As long as vehicle back reaches into passed lane, add it to the further lanes
5017 16249903 : MSLane* further = *pi;
5018 16249903 : newFurther.push_back(further);
5019 16249903 : backPosOnPreviousLane += further->setPartialOccupation(this);
5020 16249903 : if (further->getBidiLane() != nullptr
5021 16249903 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5022 25235 : further->getBidiLane()->setPartialOccupation(this);
5023 : }
5024 16249903 : if (fi != furtherLanes.end() && further == *fi) {
5025 : // Lateral position on this lane is already known. Assume constant and use old value.
5026 5460945 : newFurtherPosLat.push_back(*fpi);
5027 : ++fi;
5028 : ++fpi;
5029 : } else {
5030 : // The lane *pi was not in furtherLanes before.
5031 : // If it is downstream, we assume as lateral position the current position
5032 : // If it is a new lane upstream (can appear as shadow further in case of LC-maneuvering, e.g.)
5033 : // we assign the last known lateral position.
5034 10788958 : if (newFurtherPosLat.size() == 0) {
5035 10176982 : if (widthShift) {
5036 1461781 : newFurtherPosLat.push_back(myFurtherLanesPosLat.back());
5037 : } else {
5038 8715201 : newFurtherPosLat.push_back(myState.myPosLat);
5039 : }
5040 : } else {
5041 611976 : newFurtherPosLat.push_back(newFurtherPosLat.back());
5042 : }
5043 : }
5044 : #ifdef DEBUG_SETFURTHER
5045 : if (DEBUG_COND) {
5046 : std::cout << SIMTIME << " updateFurtherLanes \n"
5047 : << " further lane '" << further->getID() << "' backPosOnPreviousLane=" << backPosOnPreviousLane
5048 : << std::endl;
5049 : }
5050 : #endif
5051 : }
5052 30097650 : furtherLanes = newFurther;
5053 30097650 : furtherLanesPosLat = newFurtherPosLat;
5054 : } else {
5055 : furtherLanes.clear();
5056 : furtherLanesPosLat.clear();
5057 : }
5058 : #ifdef DEBUG_SETFURTHER
5059 : if (DEBUG_COND) std::cout
5060 : << " newFurther=" << toString(furtherLanes)
5061 : << " newFurtherPosLat=" << toString(furtherLanesPosLat)
5062 : << " newBackPos=" << backPosOnPreviousLane
5063 : << "\n";
5064 : #endif
5065 705930105 : return backPosOnPreviousLane;
5066 705930105 : }
5067 :
5068 :
5069 : double
5070 34895069427 : MSVehicle::getBackPositionOnLane(const MSLane* lane, bool calledByGetPosition) const {
5071 : #ifdef DEBUG_FURTHER
5072 : if (DEBUG_COND) {
5073 : std::cout << SIMTIME
5074 : << " getBackPositionOnLane veh=" << getID()
5075 : << " lane=" << Named::getIDSecure(lane)
5076 : << " cbgP=" << calledByGetPosition
5077 : << " pos=" << myState.myPos
5078 : << " backPos=" << myState.myBackPos
5079 : << " myLane=" << myLane->getID()
5080 : << " myLaneBidi=" << Named::getIDSecure(myLane->getBidiLane())
5081 : << " further=" << toString(myFurtherLanes)
5082 : << " furtherPosLat=" << toString(myFurtherLanesPosLat)
5083 : << "\n shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
5084 : << " shadowFurther=" << toString(myLaneChangeModel->getShadowFurtherLanes())
5085 : << " shadowFurtherPosLat=" << toString(myLaneChangeModel->getShadowFurtherLanesPosLat())
5086 : << "\n targetLane=" << Named::getIDSecure(myLaneChangeModel->getTargetLane())
5087 : << " furtherTargets=" << toString(myLaneChangeModel->getFurtherTargetLanes())
5088 : << std::endl;
5089 : }
5090 : #endif
5091 34895069427 : if (lane == myLane
5092 8631682090 : || lane == myLaneChangeModel->getShadowLane()
5093 39998639806 : || lane == myLaneChangeModel->getTargetLane()) {
5094 29793079308 : if (myLaneChangeModel->isOpposite()) {
5095 231304434 : if (lane == myLaneChangeModel->getShadowLane()) {
5096 199197048 : return lane->getLength() - myState.myPos - myType->getLength();
5097 : } else {
5098 36897046 : return myState.myPos + (calledByGetPosition ? -1 : 1) * myType->getLength();
5099 : }
5100 29561774874 : } else if (&lane->getEdge() != &myLane->getEdge()) {
5101 20834474 : return lane->getLength() - myState.myPos + (calledByGetPosition ? -1 : 1) * myType->getLength();
5102 : } else {
5103 : // account for parallel lanes of different lengths in the most conservative manner (i.e. while turning)
5104 59082583029 : return myState.myPos - myType->getLength() + MIN2(0.0, lane->getLength() - myLane->getLength());
5105 : }
5106 5101990119 : } else if (lane == myLane->getBidiLane()) {
5107 3965068 : return lane->getLength() - myState.myPos + myType->getLength() * (calledByGetPosition ? -1 : 1);
5108 5099529212 : } else if (myFurtherLanes.size() > 0 && lane == myFurtherLanes.back()) {
5109 5050101570 : return myState.myBackPos;
5110 49427642 : } else if ((myLaneChangeModel->getShadowFurtherLanes().size() > 0 && lane == myLaneChangeModel->getShadowFurtherLanes().back())
5111 50005460 : || (myLaneChangeModel->getFurtherTargetLanes().size() > 0 && lane == myLaneChangeModel->getFurtherTargetLanes().back())) {
5112 : assert(myFurtherLanes.size() > 0);
5113 20991227 : if (lane->getLength() == myFurtherLanes.back()->getLength()) {
5114 20327189 : return myState.myBackPos;
5115 : } else {
5116 : // interpolate
5117 : //if (DEBUG_COND) {
5118 : //if (myFurtherLanes.back()->getLength() != lane->getLength()) {
5119 : // std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " further=" << myFurtherLanes.back()->getID()
5120 : // << " len=" << lane->getLength() << " fLen=" << myFurtherLanes.back()->getLength()
5121 : // << " backPos=" << myState.myBackPos << " result=" << myState.myBackPos / myFurtherLanes.back()->getLength() * lane->getLength() << "\n";
5122 : //}
5123 664038 : return myState.myBackPos / myFurtherLanes.back()->getLength() * lane->getLength();
5124 : }
5125 : } else {
5126 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherLanes=" << toString(myFurtherLanes) << "\n";
5127 28436415 : double leftLength = myType->getLength() - myState.myPos;
5128 :
5129 : std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin();
5130 30334295 : while (leftLength > 0 && i != myFurtherLanes.end()) {
5131 30308840 : leftLength -= (*i)->getLength();
5132 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5133 30308840 : if (*i == lane) {
5134 28122534 : return -leftLength;
5135 2186306 : } else if (*i == lane->getBidiLane()) {
5136 288426 : return lane->getLength() + leftLength - (calledByGetPosition ? 2 * myType->getLength() : 0);
5137 : }
5138 : ++i;
5139 : }
5140 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
5141 25455 : leftLength = myType->getLength() - myState.myPos;
5142 25455 : i = myLaneChangeModel->getShadowFurtherLanes().begin();
5143 25455 : while (leftLength > 0 && i != myLaneChangeModel->getShadowFurtherLanes().end()) {
5144 25447 : leftLength -= (*i)->getLength();
5145 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5146 25447 : if (*i == lane) {
5147 25447 : return -leftLength;
5148 : }
5149 : ++i;
5150 : }
5151 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherTargetLanes=" << toString(myLaneChangeModel->getFurtherTargetLanes()) << "\n";
5152 : leftLength = myType->getLength() - myState.myPos;
5153 : i = getFurtherLanes().begin();
5154 8 : const std::vector<MSLane*> furtherTargetLanes = myLaneChangeModel->getFurtherTargetLanes();
5155 : auto j = furtherTargetLanes.begin();
5156 9 : while (leftLength > 0 && j != furtherTargetLanes.end()) {
5157 3 : leftLength -= (*i)->getLength();
5158 : // if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5159 3 : if (*j == lane) {
5160 2 : return -leftLength;
5161 : }
5162 : ++i;
5163 : ++j;
5164 : }
5165 42 : WRITE_WARNING("Request backPos of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane)
5166 : + "' time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".")
5167 : SOFT_ASSERT(false);
5168 6 : return myState.myBackPos;
5169 8 : }
5170 : }
5171 :
5172 :
5173 : double
5174 28132728508 : MSVehicle::getPositionOnLane(const MSLane* lane) const {
5175 28132728508 : return getBackPositionOnLane(lane, true) + myType->getLength();
5176 : }
5177 :
5178 :
5179 : bool
5180 426058602 : MSVehicle::isFrontOnLane(const MSLane* lane) const {
5181 426058602 : return lane == myLane || lane == myLaneChangeModel->getShadowLane() || lane == myLane->getBidiLane();
5182 : }
5183 :
5184 :
5185 : void
5186 633588686 : MSVehicle::checkRewindLinkLanes(const double lengthsInFront, DriveItemVector& lfLinks) const {
5187 633588686 : if (MSGlobals::gUsingInternalLanes && !myLane->getEdge().isRoundabout() && !myLaneChangeModel->isOpposite()) {
5188 630402285 : double seenSpace = -lengthsInFront;
5189 : #ifdef DEBUG_CHECKREWINDLINKLANES
5190 : if (DEBUG_COND) {
5191 : std::cout << "\nCHECK_REWIND_LINKLANES\n" << " veh=" << getID() << " lengthsInFront=" << lengthsInFront << "\n";
5192 : };
5193 : #endif
5194 630402285 : bool foundStopped = false;
5195 : // compute available space until a stopped vehicle is found
5196 : // this is the sum of non-interal lane length minus in-between vehicle lengths
5197 1834007286 : for (int i = 0; i < (int)lfLinks.size(); ++i) {
5198 : // skip unset links
5199 1203605001 : DriveProcessItem& item = lfLinks[i];
5200 : #ifdef DEBUG_CHECKREWINDLINKLANES
5201 : if (DEBUG_COND) std::cout << SIMTIME
5202 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5203 : << " foundStopped=" << foundStopped;
5204 : #endif
5205 1203605001 : if (item.myLink == nullptr || foundStopped) {
5206 396055136 : if (!foundStopped) {
5207 344107232 : item.availableSpace += seenSpace;
5208 : } else {
5209 51947904 : item.availableSpace = seenSpace;
5210 : }
5211 : #ifdef DEBUG_CHECKREWINDLINKLANES
5212 : if (DEBUG_COND) {
5213 : std::cout << " avail=" << item.availableSpace << "\n";
5214 : }
5215 : #endif
5216 396055136 : continue;
5217 : }
5218 : // get the next lane, determine whether it is an internal lane
5219 : const MSLane* approachedLane = item.myLink->getViaLane();
5220 807549865 : if (approachedLane != nullptr) {
5221 440157303 : if (keepClear(item.myLink)) {
5222 133952054 : seenSpace = seenSpace - approachedLane->getBruttoVehLenSum();
5223 133952054 : if (approachedLane == myLane) {
5224 51621 : seenSpace += getVehicleType().getLengthWithGap();
5225 : }
5226 : } else {
5227 306205249 : seenSpace = seenSpace + approachedLane->getSpaceTillLastStanding(this, foundStopped);// - approachedLane->getBruttoVehLenSum() + approachedLane->getLength();
5228 : }
5229 440157303 : item.availableSpace = seenSpace;
5230 : #ifdef DEBUG_CHECKREWINDLINKLANES
5231 : if (DEBUG_COND) std::cout
5232 : << " approached=" << approachedLane->getID()
5233 : << " approachedBrutto=" << approachedLane->getBruttoVehLenSum()
5234 : << " avail=" << item.availableSpace
5235 : << " seenSpace=" << seenSpace
5236 : << " hadStoppedVehicle=" << item.hadStoppedVehicle
5237 : << " lengthsInFront=" << lengthsInFront
5238 : << "\n";
5239 : #endif
5240 440157303 : continue;
5241 : }
5242 : approachedLane = item.myLink->getLane();
5243 367392562 : const MSVehicle* last = approachedLane->getLastAnyVehicle();
5244 367392562 : if (last == nullptr || last == this) {
5245 56354484 : if (approachedLane->getLength() > getVehicleType().getLength()
5246 56354484 : || keepClear(item.myLink)) {
5247 54031058 : seenSpace += approachedLane->getLength();
5248 : }
5249 56354484 : item.availableSpace = seenSpace;
5250 : #ifdef DEBUG_CHECKREWINDLINKLANES
5251 : if (DEBUG_COND) {
5252 : std::cout << " last=" << Named::getIDSecure(last) << " laneLength=" << approachedLane->getLength() << " avail=" << item.availableSpace << "\n";
5253 : }
5254 : #endif
5255 : } else {
5256 311038078 : bool foundStopped2 = false;
5257 311038078 : double spaceTillLastStanding = approachedLane->getSpaceTillLastStanding(this, foundStopped2);
5258 311038078 : if (approachedLane->getBidiLane() != nullptr) {
5259 76021 : const MSVehicle* oncomingVeh = approachedLane->getBidiLane()->getFirstFullVehicle();
5260 76021 : if (oncomingVeh) {
5261 24164 : const double oncomingGap = approachedLane->getLength() - oncomingVeh->getPositionOnLane();
5262 24164 : const double oncomingBGap = oncomingVeh->getBrakeGap(true);
5263 : // oncoming movement until ego enters the junction
5264 24164 : const double oncomingMove = STEPS2TIME(item.myArrivalTime - SIMSTEP) * oncomingVeh->getSpeed();
5265 24164 : const double spaceTillOncoming = oncomingGap - oncomingBGap - oncomingMove;
5266 : spaceTillLastStanding = MIN2(spaceTillLastStanding, spaceTillOncoming);
5267 24164 : if (spaceTillOncoming <= getVehicleType().getLengthWithGap()) {
5268 19465 : foundStopped = true;
5269 : }
5270 : #ifdef DEBUG_CHECKREWINDLINKLANES
5271 : if (DEBUG_COND) {
5272 : std::cout << " oVeh=" << oncomingVeh->getID()
5273 : << " oGap=" << oncomingGap
5274 : << " bGap=" << oncomingBGap
5275 : << " mGap=" << oncomingMove
5276 : << " sto=" << spaceTillOncoming;
5277 : }
5278 : #endif
5279 : }
5280 : }
5281 311038078 : seenSpace += spaceTillLastStanding;
5282 311038078 : if (foundStopped2) {
5283 20541083 : foundStopped = true;
5284 20541083 : item.hadStoppedVehicle = true;
5285 : }
5286 311038078 : item.availableSpace = seenSpace;
5287 311038078 : if (last->myHaveToWaitOnNextLink || last->isStopped()) {
5288 30429309 : foundStopped = true;
5289 30429309 : item.hadStoppedVehicle = true;
5290 : }
5291 : #ifdef DEBUG_CHECKREWINDLINKLANES
5292 : if (DEBUG_COND) std::cout
5293 : << " approached=" << approachedLane->getID()
5294 : << " last=" << last->getID()
5295 : << " lastHasToWait=" << last->myHaveToWaitOnNextLink
5296 : << " lastBrakeLight=" << last->signalSet(VEH_SIGNAL_BRAKELIGHT)
5297 : << " lastBrakeGap=" << last->getCarFollowModel().brakeGap(last->getSpeed())
5298 : << " lastGap=" << (last->getBackPositionOnLane(approachedLane) + last->getCarFollowModel().brakeGap(last->getSpeed()) - last->getSpeed() * last->getCarFollowModel().getHeadwayTime()
5299 : // gap of last up to the next intersection
5300 : - last->getVehicleType().getMinGap())
5301 : << " stls=" << spaceTillLastStanding
5302 : << " avail=" << item.availableSpace
5303 : << " seenSpace=" << seenSpace
5304 : << " foundStopped=" << foundStopped
5305 : << " foundStopped2=" << foundStopped2
5306 : << "\n";
5307 : #endif
5308 : }
5309 : }
5310 :
5311 : // check which links allow continuation and add pass available to the previous item
5312 1203605001 : for (int i = ((int)lfLinks.size() - 1); i > 0; --i) {
5313 573202716 : DriveProcessItem& item = lfLinks[i - 1];
5314 573202716 : DriveProcessItem& nextItem = lfLinks[i];
5315 573202716 : const bool canLeaveJunction = item.myLink->getViaLane() == nullptr || nextItem.myLink == nullptr || nextItem.mySetRequest;
5316 : const bool opened = (item.myLink != nullptr
5317 573202716 : && (canLeaveJunction || (
5318 : // indirect bicycle turn
5319 27053586 : nextItem.myLink != nullptr && nextItem.myLink->isInternalJunctionLink() && nextItem.myLink->haveRed()))
5320 546163192 : && (
5321 546163192 : item.myLink->havePriority()
5322 26722703 : || i == 1 // the upcoming link (item 0) is checked in executeMove anyway. No need to use outdata approachData here
5323 4962937 : || (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority())
5324 4933423 : || item.myLink->opened(item.myArrivalTime, item.myArrivalSpeed,
5325 4933423 : item.getLeaveSpeed(), getVehicleType().getLength(),
5326 4933423 : getImpatience(), getCarFollowModel().getMaxDecel(), getWaitingTime(), getLateralPositionOnLane(), nullptr, false, this)));
5327 573202716 : bool allowsContinuation = (item.myLink == nullptr || item.myLink->isCont() || opened) && !item.hadStoppedVehicle;
5328 : #ifdef DEBUG_CHECKREWINDLINKLANES
5329 : if (DEBUG_COND) std::cout
5330 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5331 : << " canLeave=" << canLeaveJunction
5332 : << " opened=" << opened
5333 : << " allowsContinuation=" << allowsContinuation
5334 : << " foundStopped=" << foundStopped
5335 : << "\n";
5336 : #endif
5337 573202716 : if (!opened && item.myLink != nullptr) {
5338 27764053 : foundStopped = true;
5339 27764053 : if (i > 1) {
5340 4424800 : DriveProcessItem& item2 = lfLinks[i - 2];
5341 4424800 : if (item2.myLink != nullptr && item2.myLink->isCont()) {
5342 : allowsContinuation = true;
5343 : }
5344 : }
5345 : }
5346 570514283 : if (allowsContinuation) {
5347 514420372 : item.availableSpace = nextItem.availableSpace;
5348 : #ifdef DEBUG_CHECKREWINDLINKLANES
5349 : if (DEBUG_COND) std::cout
5350 : << " link=" << (item.myLink == nullptr ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5351 : << " copy nextAvail=" << nextItem.availableSpace
5352 : << "\n";
5353 : #endif
5354 : }
5355 : }
5356 :
5357 : // find removalBegin
5358 : int removalBegin = -1;
5359 758111918 : for (int i = 0; foundStopped && i < (int)lfLinks.size() && removalBegin < 0; ++i) {
5360 : // skip unset links
5361 127709633 : const DriveProcessItem& item = lfLinks[i];
5362 127709633 : if (item.myLink == nullptr) {
5363 6479554 : continue;
5364 : }
5365 : /*
5366 : double impatienceCorrection = MAX2(0., double(double(myWaitingTime)));
5367 : if (seenSpace<getVehicleType().getLengthWithGap()-impatienceCorrection/10.&&nextSeenNonInternal!=0) {
5368 : removalBegin = lastLinkToInternal;
5369 : }
5370 : */
5371 :
5372 121230079 : const double leftSpace = item.availableSpace - getVehicleType().getLengthWithGap();
5373 : #ifdef DEBUG_CHECKREWINDLINKLANES
5374 : if (DEBUG_COND) std::cout
5375 : << SIMTIME
5376 : << " veh=" << getID()
5377 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5378 : << " avail=" << item.availableSpace
5379 : << " leftSpace=" << leftSpace
5380 : << "\n";
5381 : #endif
5382 121230079 : if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
5383 : double impatienceCorrection = 0;
5384 : /*
5385 : if(item.myLink->getState()==LINKSTATE_MINOR) {
5386 : impatienceCorrection = MAX2(0., STEPS2TIME(myWaitingTime));
5387 : }
5388 : */
5389 : // may ignore keepClear rules
5390 76601248 : if (leftSpace < -impatienceCorrection / 10. && keepClear(item.myLink)) {
5391 : removalBegin = i;
5392 : }
5393 : //removalBegin = i;
5394 : }
5395 : }
5396 : // abort requests
5397 630402285 : if (removalBegin != -1 && !(removalBegin == 0 && myLane->getEdge().isInternal())) {
5398 30004077 : const double brakeGap = getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.);
5399 102522835 : while (removalBegin < (int)(lfLinks.size())) {
5400 77311535 : DriveProcessItem& dpi = lfLinks[removalBegin];
5401 77311535 : if (dpi.myLink == nullptr) {
5402 : break;
5403 : }
5404 72518758 : dpi.myVLinkPass = dpi.myVLinkWait;
5405 : #ifdef DEBUG_CHECKREWINDLINKLANES
5406 : if (DEBUG_COND) {
5407 : std::cout << " removalBegin=" << removalBegin << " brakeGap=" << brakeGap << " dist=" << dpi.myDistance << " speed=" << myState.mySpeed << " a2s=" << ACCEL2SPEED(getCarFollowModel().getMaxDecel()) << "\n";
5408 : }
5409 : #endif
5410 72518758 : if (dpi.myDistance >= brakeGap + POSITION_EPS) {
5411 : // always leave junctions after requesting to enter
5412 72510144 : if (!dpi.myLink->isExitLink() || !lfLinks[removalBegin - 1].mySetRequest) {
5413 72502443 : dpi.mySetRequest = false;
5414 : }
5415 : }
5416 72518758 : ++removalBegin;
5417 : }
5418 : }
5419 : }
5420 633588686 : }
5421 :
5422 :
5423 : void
5424 705263362 : MSVehicle::setApproachingForAllLinks() {
5425 705263362 : if (!myActionStep) {
5426 : return;
5427 : }
5428 633588686 : removeApproachingInformation(myLFLinkLanesPrev);
5429 1844999822 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5430 1211411136 : if (dpi.myLink != nullptr) {
5431 859096633 : if (dpi.myLink->getState() == LINKSTATE_ALLWAY_STOP) {
5432 2850584 : dpi.myArrivalTime += (SUMOTime)RandHelper::rand((int)2, getRNG()); // tie braker
5433 : }
5434 859096633 : dpi.myLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
5435 859096633 : dpi.mySetRequest, dpi.myArrivalSpeedBraking, getWaitingTimeFor(dpi.myLink), dpi.myDistance, getLateralPositionOnLane());
5436 : }
5437 : }
5438 633588686 : if (isRail()) {
5439 7616665 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5440 6361806 : if (dpi.myLink != nullptr && dpi.myLink->getTLLogic() != nullptr && dpi.myLink->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL) {
5441 555752 : MSRailSignalControl::getInstance().notifyApproach(dpi.myLink);
5442 : }
5443 : }
5444 : }
5445 633588686 : if (myLaneChangeModel->getShadowLane() != nullptr) {
5446 : // register on all shadow links
5447 7551335 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
5448 5004883 : if (dpi.myLink != nullptr) {
5449 3414805 : MSLink* parallelLink = dpi.myLink->getParallelLink(myLaneChangeModel->getShadowDirection());
5450 3414805 : if (parallelLink == nullptr && getLaneChangeModel().isOpposite() && dpi.myLink->isEntryLink()) {
5451 : // register on opposite direction entry link to warn foes at minor side road
5452 169486 : parallelLink = dpi.myLink->getOppositeDirectionLink();
5453 : }
5454 3414805 : if (parallelLink != nullptr) {
5455 2430269 : const double latOffset = getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge();
5456 2430269 : parallelLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
5457 2430269 : dpi.mySetRequest, dpi.myArrivalSpeedBraking, getWaitingTimeFor(dpi.myLink), dpi.myDistance,
5458 : latOffset);
5459 2430269 : myLaneChangeModel->setShadowApproachingInformation(parallelLink);
5460 : }
5461 : }
5462 : }
5463 : }
5464 : #ifdef DEBUG_PLAN_MOVE
5465 : if (DEBUG_COND) {
5466 : std::cout << SIMTIME
5467 : << " veh=" << getID()
5468 : << " after checkRewindLinkLanes\n";
5469 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5470 : std::cout
5471 : << " vPass=" << dpi.myVLinkPass
5472 : << " vWait=" << dpi.myVLinkWait
5473 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
5474 : << " request=" << dpi.mySetRequest
5475 : << " atime=" << dpi.myArrivalTime
5476 : << "\n";
5477 : }
5478 : }
5479 : #endif
5480 : }
5481 :
5482 :
5483 : void
5484 1730 : MSVehicle::registerInsertionApproach(MSLink* link, double dist) {
5485 : DriveProcessItem dpi(0, dist);
5486 1730 : dpi.myLink = link;
5487 1730 : const double arrivalSpeedBraking = getCarFollowModel().getMinimalArrivalSpeedEuler(dist, getSpeed());
5488 1730 : link->setApproaching(this, SUMOTime_MAX, 0, 0, false, arrivalSpeedBraking, 0, dpi.myDistance, 0);
5489 : // ensure cleanup in the next step
5490 1730 : myLFLinkLanes.push_back(dpi);
5491 1730 : MSRailSignalControl::getInstance().notifyApproach(link);
5492 1730 : }
5493 :
5494 :
5495 : void
5496 20511726 : MSVehicle::enterLaneAtMove(MSLane* enteredLane, bool onTeleporting) {
5497 20511726 : myAmOnNet = !onTeleporting;
5498 : // vaporizing edge?
5499 : /*
5500 : if (enteredLane->getEdge().isVaporizing()) {
5501 : // yep, let's do the vaporization...
5502 : myLane = enteredLane;
5503 : return true;
5504 : }
5505 : */
5506 : // Adjust MoveReminder offset to the next lane
5507 20511726 : adaptLaneEntering2MoveReminder(*enteredLane);
5508 : // set the entered lane as the current lane
5509 20511726 : MSLane* oldLane = myLane;
5510 20511726 : myLane = enteredLane;
5511 20511726 : myLastBestLanesEdge = nullptr;
5512 :
5513 : // internal edges are not a part of the route...
5514 20511726 : if (!enteredLane->getEdge().isInternal()) {
5515 : ++myCurrEdge;
5516 : assert(myLaneChangeModel->isOpposite() || haveValidStopEdges());
5517 : }
5518 20511726 : if (myInfluencer != nullptr) {
5519 9045 : myInfluencer->adaptLaneTimeLine(myLane->getIndex() - oldLane->getIndex());
5520 : }
5521 20511726 : if (!onTeleporting) {
5522 20494234 : activateReminders(MSMoveReminder::NOTIFICATION_JUNCTION, enteredLane);
5523 20494234 : if (MSGlobals::gLateralResolution > 0) {
5524 3764219 : myFurtherLanesPosLat.push_back(myState.myPosLat);
5525 : // transform lateral position when the lane width changes
5526 : assert(oldLane != nullptr);
5527 3764219 : const MSLink* const link = oldLane->getLinkTo(myLane);
5528 3764219 : if (link != nullptr) {
5529 3764177 : myState.myPosLat += link->getLateralShift();
5530 : } else {
5531 42 : myState.myPosLat += (oldLane->getCenterOnEdge() - myLane->getCanonicalPredecessorLane()->getRightSideOnEdge()) / 2;
5532 : }
5533 16730015 : } else if (fabs(myState.myPosLat) > NUMERICAL_EPS) {
5534 358368 : const double overlap = MAX2(0.0, getLateralOverlap(myState.myPosLat, oldLane));
5535 358368 : const double range = (oldLane->getWidth() - getVehicleType().getWidth()) * 0.5 + overlap;
5536 358368 : const double range2 = (myLane->getWidth() - getVehicleType().getWidth()) * 0.5 + overlap;
5537 358368 : myState.myPosLat *= range2 / range;
5538 : }
5539 20494234 : if (myLane->getBidiLane() != nullptr && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5540 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
5541 : // (unless the lane is shared with cars)
5542 9462 : myLane->getBidiLane()->setPartialOccupation(this);
5543 : }
5544 : } else {
5545 : // normal move() isn't called so reset position here. must be done
5546 : // before calling reminders
5547 17492 : myState.myPos = 0;
5548 17492 : myCachedPosition = Position::INVALID;
5549 17492 : activateReminders(MSMoveReminder::NOTIFICATION_TELEPORT, enteredLane);
5550 : }
5551 : // update via
5552 20511726 : if (myParameter->via.size() > 0 && myLane->getEdge().getID() == myParameter->via.front()) {
5553 7153 : myParameter->via.erase(myParameter->via.begin());
5554 : }
5555 20511726 : }
5556 :
5557 :
5558 : void
5559 1081857 : MSVehicle::enterLaneAtLaneChange(MSLane* enteredLane) {
5560 1081857 : myAmOnNet = true;
5561 1081857 : myLane = enteredLane;
5562 1081857 : myCachedPosition = Position::INVALID;
5563 : // need to update myCurrentLaneInBestLanes
5564 1081857 : updateBestLanes();
5565 : // switch to and activate the new lane's reminders
5566 : // keep OldLaneReminders
5567 1283556 : for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
5568 201699 : addReminder(*rem);
5569 : }
5570 1081857 : activateReminders(MSMoveReminder::NOTIFICATION_LANE_CHANGE, enteredLane);
5571 1081857 : MSLane* lane = myLane;
5572 1081857 : double leftLength = getVehicleType().getLength() - myState.myPos;
5573 : int deleteFurther = 0;
5574 : #ifdef DEBUG_SETFURTHER
5575 : if (DEBUG_COND) {
5576 : std::cout << SIMTIME << " enterLaneAtLaneChange entered=" << Named::getIDSecure(enteredLane) << " oldFurther=" << toString(myFurtherLanes) << "\n";
5577 : }
5578 : #endif
5579 1081857 : if (myLane->getBidiLane() != nullptr && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5580 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
5581 : // (unless the lane is shared with cars)
5582 8187 : myLane->getBidiLane()->setPartialOccupation(this);
5583 : }
5584 1165322 : for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
5585 83465 : if (lane != nullptr) {
5586 80422 : lane = lane->getLogicalPredecessorLane(myFurtherLanes[i]->getEdge());
5587 : }
5588 : #ifdef DEBUG_SETFURTHER
5589 : if (DEBUG_COND) {
5590 : std::cout << " enterLaneAtLaneChange i=" << i << " lane=" << Named::getIDSecure(lane) << " leftLength=" << leftLength << "\n";
5591 : }
5592 : #endif
5593 83465 : if (leftLength > 0) {
5594 82908 : if (lane != nullptr) {
5595 33405 : myFurtherLanes[i]->resetPartialOccupation(this);
5596 33405 : if (myFurtherLanes[i]->getBidiLane() != nullptr
5597 33405 : && (!isRailway(getVClass()) || (myFurtherLanes[i]->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5598 60 : myFurtherLanes[i]->getBidiLane()->resetPartialOccupation(this);
5599 : }
5600 : // lane changing onto longer lanes may reduce the number of
5601 : // remaining further lanes
5602 33405 : myFurtherLanes[i] = lane;
5603 33405 : myFurtherLanesPosLat[i] = myState.myPosLat;
5604 33405 : leftLength -= lane->setPartialOccupation(this);
5605 33405 : if (lane->getBidiLane() != nullptr
5606 33405 : && (!isRailway(getVClass()) || (lane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5607 540 : lane->getBidiLane()->setPartialOccupation(this);
5608 : }
5609 33405 : myState.myBackPos = -leftLength;
5610 : #ifdef DEBUG_SETFURTHER
5611 : if (DEBUG_COND) {
5612 : std::cout << SIMTIME << " newBackPos=" << myState.myBackPos << "\n";
5613 : }
5614 : #endif
5615 : } else {
5616 : // keep the old values, but ensure there is no shadow
5617 49503 : if (myLaneChangeModel->isChangingLanes()) {
5618 0 : myLaneChangeModel->setNoShadowPartialOccupator(myFurtherLanes[i]);
5619 : }
5620 49503 : if (myState.myBackPos < 0) {
5621 45 : myState.myBackPos += myFurtherLanes[i]->getLength();
5622 : }
5623 : #ifdef DEBUG_SETFURTHER
5624 : if (DEBUG_COND) {
5625 : std::cout << SIMTIME << " i=" << i << " further=" << myFurtherLanes[i]->getID() << " newBackPos=" << myState.myBackPos << "\n";
5626 : }
5627 : #endif
5628 : }
5629 : } else {
5630 557 : myFurtherLanes[i]->resetPartialOccupation(this);
5631 557 : if (myFurtherLanes[i]->getBidiLane() != nullptr
5632 557 : && (!isRailway(getVClass()) || (myFurtherLanes[i]->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5633 0 : myFurtherLanes[i]->getBidiLane()->resetPartialOccupation(this);
5634 : }
5635 557 : deleteFurther++;
5636 : }
5637 : }
5638 1081857 : if (deleteFurther > 0) {
5639 : #ifdef DEBUG_SETFURTHER
5640 : if (DEBUG_COND) {
5641 : std::cout << SIMTIME << " veh=" << getID() << " shortening myFurtherLanes by " << deleteFurther << "\n";
5642 : }
5643 : #endif
5644 526 : myFurtherLanes.erase(myFurtherLanes.end() - deleteFurther, myFurtherLanes.end());
5645 526 : myFurtherLanesPosLat.erase(myFurtherLanesPosLat.end() - deleteFurther, myFurtherLanesPosLat.end());
5646 : }
5647 : #ifdef DEBUG_SETFURTHER
5648 : if (DEBUG_COND) {
5649 : std::cout << SIMTIME << " enterLaneAtLaneChange new furtherLanes=" << toString(myFurtherLanes)
5650 : << " furterLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
5651 : }
5652 : #endif
5653 1081857 : myAngle = computeAngle();
5654 1081857 : }
5655 :
5656 :
5657 : void
5658 3799396 : MSVehicle::computeFurtherLanes(MSLane* enteredLane, double pos, bool collision) {
5659 : // build the list of lanes the vehicle is lapping into
5660 3799396 : if (!myLaneChangeModel->isOpposite()) {
5661 3777177 : double leftLength = myType->getLength() - pos;
5662 3777177 : MSLane* clane = enteredLane;
5663 3777177 : int routeIndex = getRoutePosition();
5664 3883040 : while (leftLength > 0) {
5665 245415 : if (routeIndex > 0 && clane->getEdge().isNormal()) {
5666 : // get predecessor lane that corresponds to prior route
5667 4390 : routeIndex--;
5668 4390 : const MSEdge* fromRouteEdge = myRoute->getEdges()[routeIndex];
5669 : MSLane* target = clane;
5670 4390 : clane = nullptr;
5671 5774 : for (auto ili : target->getIncomingLanes()) {
5672 5767 : if (ili.lane->getEdge().getNormalBefore() == fromRouteEdge) {
5673 4383 : clane = ili.lane;
5674 4383 : break;
5675 : }
5676 : }
5677 : } else {
5678 241025 : clane = clane->getLogicalPredecessorLane();
5679 : }
5680 155914 : if (clane == nullptr || clane == myLane || clane == myLane->getBidiLane()
5681 401321 : || (clane->isInternal() && (
5682 132819 : clane->getLinkCont()[0]->getDirection() == LinkDirection::TURN
5683 82776 : || clane->getLinkCont()[0]->getDirection() == LinkDirection::TURN_LEFTHAND))) {
5684 : break;
5685 : }
5686 105863 : if (!collision || std::find(myFurtherLanes.begin(), myFurtherLanes.end(), clane) == myFurtherLanes.end()) {
5687 105460 : myFurtherLanes.push_back(clane);
5688 105460 : myFurtherLanesPosLat.push_back(myState.myPosLat);
5689 105460 : clane->setPartialOccupation(this);
5690 105460 : if (clane->getBidiLane() != nullptr
5691 105460 : && (!isRailway(getVClass()) || (clane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5692 5 : clane->getBidiLane()->setPartialOccupation(this);
5693 : }
5694 : }
5695 105863 : leftLength -= clane->getLength();
5696 : }
5697 3777177 : myState.myBackPos = -leftLength;
5698 : #ifdef DEBUG_SETFURTHER
5699 : if (DEBUG_COND) {
5700 : std::cout << SIMTIME << " computeFurtherLanes veh=" << getID() << " pos=" << pos << " myFurtherLanes=" << toString(myFurtherLanes) << " backPos=" << myState.myBackPos << "\n";
5701 : }
5702 : #endif
5703 : } else {
5704 : // clear partial occupation
5705 22649 : for (MSLane* further : myFurtherLanes) {
5706 : #ifdef DEBUG_SETFURTHER
5707 : if (DEBUG_COND) {
5708 : std::cout << SIMTIME << " opposite: resetPartialOccupation " << further->getID() << " \n";
5709 : }
5710 : #endif
5711 430 : further->resetPartialOccupation(this);
5712 430 : if (further->getBidiLane() != nullptr
5713 430 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5714 0 : further->getBidiLane()->resetPartialOccupation(this);
5715 : }
5716 : }
5717 : myFurtherLanes.clear();
5718 : myFurtherLanesPosLat.clear();
5719 : }
5720 3799396 : }
5721 :
5722 :
5723 : void
5724 3798980 : MSVehicle::enterLaneAtInsertion(MSLane* enteredLane, double pos, double speed, double posLat, MSMoveReminder::Notification notification) {
5725 3798980 : myState = State(pos, speed, posLat, pos - getVehicleType().getLength(), hasDeparted() ? myState.myPreviousSpeed : speed);
5726 3798980 : if (myDeparture == NOT_YET_DEPARTED) {
5727 3726202 : onDepart();
5728 : }
5729 3798980 : myCachedPosition = Position::INVALID;
5730 : assert(myState.myPos >= 0);
5731 : assert(myState.mySpeed >= 0);
5732 3798980 : myLane = enteredLane;
5733 3798980 : myAmOnNet = true;
5734 : // schedule action for the next timestep
5735 3798980 : myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + DELTA_T;
5736 3798980 : if (notification != MSMoveReminder::NOTIFICATION_TELEPORT) {
5737 3787785 : if (notification == MSMoveReminder::NOTIFICATION_PARKING && myInfluencer != nullptr) {
5738 12 : drawOutsideNetwork(false);
5739 : }
5740 : // set and activate the new lane's reminders, teleports already did that at enterLaneAtMove
5741 8317389 : for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
5742 4529604 : addReminder(*rem);
5743 : }
5744 3787785 : activateReminders(notification, enteredLane);
5745 : } else {
5746 11195 : myLastBestLanesEdge = nullptr;
5747 11195 : myLastBestLanesInternalLane = nullptr;
5748 11195 : myLaneChangeModel->resetState();
5749 12396 : while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()
5750 11897 : && myStops.front().pars.endPos < pos) {
5751 0 : WRITE_WARNINGF(TL("Vehicle '%' skips stop on lane '%' time=%."), getID(), myStops.front().lane->getID(),
5752 : time2string(MSNet::getInstance()->getCurrentTimeStep()));
5753 0 : cleanupParkingReservation();
5754 0 : myStops.pop_front();
5755 : }
5756 : // avoid startup-effects after teleport
5757 11195 : myTimeSinceStartup = getCarFollowModel().getStartupDelay() + DELTA_T;
5758 11195 : myStopSpeed = std::numeric_limits<double>::max();
5759 : }
5760 3798978 : computeFurtherLanes(enteredLane, pos);
5761 3798978 : if (MSGlobals::gLateralResolution > 0) {
5762 507390 : myLaneChangeModel->updateShadowLane();
5763 507390 : myLaneChangeModel->updateTargetLane();
5764 3291588 : } else if (MSGlobals::gLaneChangeDuration > 0) {
5765 34065 : myLaneChangeModel->updateShadowLane();
5766 : }
5767 3798978 : if (notification != MSMoveReminder::NOTIFICATION_LOAD_STATE) {
5768 3797638 : myAngle = computeAngle();
5769 3797638 : if (myLaneChangeModel->isOpposite()) {
5770 22219 : myAngle += M_PI;
5771 : }
5772 : }
5773 3798978 : if (MSNet::getInstance()->hasPersons()) {
5774 57697 : for (MSLane* further : myFurtherLanes) {
5775 745 : if (further->mustCheckJunctionCollisions()) {
5776 4 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(further);
5777 : }
5778 : }
5779 : }
5780 3798978 : }
5781 :
5782 :
5783 : void
5784 25271633 : MSVehicle::leaveLane(const MSMoveReminder::Notification reason, const MSLane* approachedLane) {
5785 69883773 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
5786 44612140 : if (rem->first->notifyLeave(*this, myState.myPos + rem->second, reason, approachedLane)) {
5787 : #ifdef _DEBUG
5788 : if (myTraceMoveReminders) {
5789 : traceMoveReminder("notifyLeave", rem->first, rem->second, true);
5790 : }
5791 : #endif
5792 : ++rem;
5793 : } else {
5794 : #ifdef _DEBUG
5795 : if (myTraceMoveReminders) {
5796 : traceMoveReminder("notifyLeave", rem->first, rem->second, false);
5797 : }
5798 : #endif
5799 : rem = myMoveReminders.erase(rem);
5800 : }
5801 : }
5802 25271633 : if ((reason == MSMoveReminder::NOTIFICATION_JUNCTION
5803 25271633 : || reason == MSMoveReminder::NOTIFICATION_TELEPORT
5804 4765610 : || reason == MSMoveReminder::NOTIFICATION_TELEPORT_CONTINUATION)
5805 20511956 : && myLane != nullptr) {
5806 20511927 : myOdometer += getLane()->getLength();
5807 : }
5808 25271604 : if (myLane != nullptr && myLane->getBidiLane() != nullptr && myAmOnNet
5809 25312376 : && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5810 18188 : myLane->getBidiLane()->resetPartialOccupation(this);
5811 : }
5812 25271633 : if (reason != MSMoveReminder::NOTIFICATION_JUNCTION && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
5813 : // @note. In case of lane change, myFurtherLanes and partial occupation
5814 : // are handled in enterLaneAtLaneChange()
5815 3688334 : for (MSLane* further : myFurtherLanes) {
5816 : #ifdef DEBUG_FURTHER
5817 : if (DEBUG_COND) {
5818 : std::cout << SIMTIME << " leaveLane \n";
5819 : }
5820 : #endif
5821 37033 : further->resetPartialOccupation(this);
5822 37033 : if (further->getBidiLane() != nullptr
5823 37033 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5824 0 : further->getBidiLane()->resetPartialOccupation(this);
5825 : }
5826 : }
5827 : myFurtherLanes.clear();
5828 : myFurtherLanesPosLat.clear();
5829 : }
5830 3651301 : if (reason >= MSMoveReminder::NOTIFICATION_TELEPORT) {
5831 3651301 : myAmOnNet = false;
5832 3651301 : myWaitingTime = 0;
5833 : }
5834 25271633 : if (reason != MSMoveReminder::NOTIFICATION_PARKING && resumeFromStopping()) {
5835 18 : myStopDist = std::numeric_limits<double>::max();
5836 18 : if (myPastStops.back().speed <= 0) {
5837 54 : WRITE_WARNINGF(TL("Vehicle '%' aborts stop."), getID());
5838 : }
5839 : }
5840 25271633 : if (reason != MSMoveReminder::NOTIFICATION_PARKING && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
5841 24131327 : while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()) {
5842 1461 : if (myStops.front().getSpeed() <= 0) {
5843 3405 : WRITE_WARNINGF(TL("Vehicle '%' skips stop on lane '%' time=%."), getID(), myStops.front().lane->getID(),
5844 : time2string(MSNet::getInstance()->getCurrentTimeStep()));
5845 1135 : cleanupParkingReservation();
5846 1135 : if (MSStopOut::active()) {
5847 : // clean up if stopBlocked was called
5848 17 : MSStopOut::getInstance()->stopNotStarted(this);
5849 : }
5850 1135 : myStops.pop_front();
5851 : } else {
5852 : MSStop& stop = myStops.front();
5853 : // passed waypoint at the end of the lane
5854 326 : if (!stop.reached) {
5855 326 : if (MSStopOut::active()) {
5856 21 : MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), MSNet::getInstance()->getCurrentTimeStep());
5857 : }
5858 326 : stop.reached = true;
5859 : // enter stopping place so leaveFrom works as expected
5860 326 : if (stop.busstop != nullptr) {
5861 : // let the bus stop know the vehicle
5862 25 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5863 : }
5864 326 : if (stop.containerstop != nullptr) {
5865 : // let the container stop know the vehicle
5866 13 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5867 : }
5868 : // do not enter parkingarea!
5869 326 : if (stop.chargingStation != nullptr) {
5870 : // let the container stop know the vehicle
5871 121 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5872 : }
5873 : }
5874 326 : resumeFromStopping();
5875 : }
5876 1461 : myStopDist = std::numeric_limits<double>::max();
5877 : }
5878 : }
5879 25271633 : }
5880 :
5881 :
5882 : void
5883 40349 : MSVehicle::leaveLaneBack(const MSMoveReminder::Notification reason, const MSLane* leftLane) {
5884 172107 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
5885 131758 : if (rem->first->notifyLeaveBack(*this, reason, leftLane)) {
5886 : #ifdef _DEBUG
5887 : if (myTraceMoveReminders) {
5888 : traceMoveReminder("notifyLeaveBack", rem->first, rem->second, true);
5889 : }
5890 : #endif
5891 : ++rem;
5892 : } else {
5893 : #ifdef _DEBUG
5894 : if (myTraceMoveReminders) {
5895 : traceMoveReminder("notifyLeaveBack", rem->first, rem->second, false);
5896 : }
5897 : #endif
5898 : rem = myMoveReminders.erase(rem);
5899 : }
5900 : }
5901 : #ifdef DEBUG_MOVEREMINDERS
5902 : if (DEBUG_COND) {
5903 : std::cout << SIMTIME << " veh=" << getID() << " myReminders:";
5904 : for (auto rem : myMoveReminders) {
5905 : std::cout << rem.first->getDescription() << " ";
5906 : }
5907 : std::cout << "\n";
5908 : }
5909 : #endif
5910 40349 : }
5911 :
5912 :
5913 : MSAbstractLaneChangeModel&
5914 10405210303 : MSVehicle::getLaneChangeModel() {
5915 10405210303 : return *myLaneChangeModel;
5916 : }
5917 :
5918 :
5919 : const MSAbstractLaneChangeModel&
5920 4918255558 : MSVehicle::getLaneChangeModel() const {
5921 4918255558 : return *myLaneChangeModel;
5922 : }
5923 :
5924 : bool
5925 516329 : MSVehicle::isOppositeLane(const MSLane* lane) const {
5926 516329 : return (lane->isInternal()
5927 516329 : ? & (lane->getLinkCont()[0]->getLane()->getEdge()) != *(myCurrEdge + 1)
5928 514575 : : &lane->getEdge() != *myCurrEdge);
5929 : }
5930 :
5931 : const std::vector<MSVehicle::LaneQ>&
5932 508735701 : MSVehicle::getBestLanes() const {
5933 508735701 : return *myBestLanes.begin();
5934 : }
5935 :
5936 :
5937 : void
5938 1046309975 : MSVehicle::updateBestLanes(bool forceRebuild, const MSLane* startLane) {
5939 : #ifdef DEBUG_BESTLANES
5940 : if (DEBUG_COND) {
5941 : std::cout << SIMTIME << " updateBestLanes veh=" << getID() << " force=" << forceRebuild << " startLane1=" << Named::getIDSecure(startLane) << " myLane=" << Named::getIDSecure(myLane) << "\n";
5942 : }
5943 : #endif
5944 1046309975 : if (startLane == nullptr) {
5945 995127381 : startLane = myLane;
5946 : }
5947 : assert(startLane != 0);
5948 1046309975 : if (myLaneChangeModel->isOpposite()) {
5949 : // depending on the calling context, startLane might be the forward lane
5950 : // or the reverse-direction lane. In the latter case we need to
5951 : // transform it to the forward lane.
5952 516329 : if (isOppositeLane(startLane)) {
5953 : // use leftmost lane of forward edge
5954 110927 : startLane = startLane->getEdge().getOppositeEdge()->getLanes().back();
5955 : assert(startLane != 0);
5956 : #ifdef DEBUG_BESTLANES
5957 : if (DEBUG_COND) {
5958 : std::cout << " startLaneIsOpposite newStartLane=" << startLane->getID() << "\n";
5959 : }
5960 : #endif
5961 : }
5962 : }
5963 1046309975 : if (forceRebuild) {
5964 1913279 : myLastBestLanesEdge = nullptr;
5965 1913279 : myLastBestLanesInternalLane = nullptr;
5966 : }
5967 1046309975 : if (myBestLanes.size() > 0 && !forceRebuild && myLastBestLanesEdge == &startLane->getEdge()) {
5968 1014494904 : updateOccupancyAndCurrentBestLane(startLane);
5969 : #ifdef DEBUG_BESTLANES
5970 : if (DEBUG_COND) {
5971 : std::cout << " only updateOccupancyAndCurrentBestLane\n";
5972 : }
5973 : #endif
5974 1014494904 : return;
5975 : }
5976 31815071 : if (startLane->getEdge().isInternal()) {
5977 14962504 : if (myBestLanes.size() == 0 || forceRebuild) {
5978 : // rebuilt from previous non-internal lane (may backtrack twice if behind an internal junction)
5979 2361 : updateBestLanes(true, startLane->getLogicalPredecessorLane());
5980 : }
5981 14962504 : if (myLastBestLanesInternalLane == startLane && !forceRebuild) {
5982 : #ifdef DEBUG_BESTLANES
5983 : if (DEBUG_COND) {
5984 : std::cout << " nothing to do on internal\n";
5985 : }
5986 : #endif
5987 : return;
5988 : }
5989 : // adapt best lanes to fit the current internal edge:
5990 : // keep the entries that are reachable from this edge
5991 5773400 : const MSEdge* nextEdge = startLane->getNextNormal();
5992 : assert(!nextEdge->isInternal());
5993 11409495 : for (std::vector<std::vector<LaneQ> >::iterator it = myBestLanes.begin(); it != myBestLanes.end();) {
5994 : std::vector<LaneQ>& lanes = *it;
5995 : assert(lanes.size() > 0);
5996 11409495 : if (&(lanes[0].lane->getEdge()) == nextEdge) {
5997 : // keep those lanes which are successors of internal lanes from the edge of startLane
5998 5773400 : std::vector<LaneQ> oldLanes = lanes;
5999 : lanes.clear();
6000 : const std::vector<MSLane*>& sourceLanes = startLane->getEdge().getLanes();
6001 12914438 : for (std::vector<MSLane*>::const_iterator it_source = sourceLanes.begin(); it_source != sourceLanes.end(); ++it_source) {
6002 12246793 : for (std::vector<LaneQ>::iterator it_lane = oldLanes.begin(); it_lane != oldLanes.end(); ++it_lane) {
6003 12246793 : if ((*it_source)->getLinkCont()[0]->getLane() == (*it_lane).lane) {
6004 7141038 : lanes.push_back(*it_lane);
6005 : break;
6006 : }
6007 : }
6008 : }
6009 : assert(lanes.size() == startLane->getEdge().getLanes().size());
6010 : // patch invalid bestLaneOffset and updated myCurrentLaneInBestLanes
6011 12914438 : for (int i = 0; i < (int)lanes.size(); ++i) {
6012 7141038 : if (i + lanes[i].bestLaneOffset < 0) {
6013 105704 : lanes[i].bestLaneOffset = -i;
6014 : }
6015 7141038 : if (i + lanes[i].bestLaneOffset >= (int)lanes.size()) {
6016 25105 : lanes[i].bestLaneOffset = (int)lanes.size() - i - 1;
6017 : }
6018 : assert(i + lanes[i].bestLaneOffset >= 0);
6019 : assert(i + lanes[i].bestLaneOffset < (int)lanes.size());
6020 7141038 : if (lanes[i].bestContinuations[0] != 0) {
6021 : // patch length of bestContinuation to match expectations (only once)
6022 6963755 : lanes[i].bestContinuations.insert(lanes[i].bestContinuations.begin(), (MSLane*)nullptr);
6023 : }
6024 7141038 : if (startLane->getLinkCont()[0]->getLane() == lanes[i].lane) {
6025 5817504 : myCurrentLaneInBestLanes = lanes.begin() + i;
6026 : }
6027 : assert(&(lanes[i].lane->getEdge()) == nextEdge);
6028 : }
6029 5773400 : myLastBestLanesInternalLane = startLane;
6030 5773400 : updateOccupancyAndCurrentBestLane(startLane);
6031 : #ifdef DEBUG_BESTLANES
6032 : if (DEBUG_COND) {
6033 : std::cout << " updated for internal\n";
6034 : }
6035 : #endif
6036 : return;
6037 5773400 : } else {
6038 : // remove passed edges
6039 5636095 : it = myBestLanes.erase(it);
6040 : }
6041 : }
6042 : assert(false); // should always find the next edge
6043 : }
6044 : // start rebuilding
6045 16852567 : myLastBestLanesInternalLane = nullptr;
6046 16852567 : myLastBestLanesEdge = &startLane->getEdge();
6047 : myBestLanes.clear();
6048 :
6049 : // get information about the next stop
6050 16852567 : MSRouteIterator nextStopEdge = myRoute->end();
6051 : const MSLane* nextStopLane = nullptr;
6052 : double nextStopPos = 0;
6053 : bool nextStopIsWaypoint = false;
6054 16852567 : if (!myStops.empty()) {
6055 : const MSStop& nextStop = myStops.front();
6056 256575 : nextStopLane = nextStop.lane;
6057 256575 : if (nextStop.isOpposite) {
6058 : // target leftmost lane in forward direction
6059 340 : nextStopLane = nextStopLane->getEdge().getOppositeEdge()->getLanes().back();
6060 : }
6061 256575 : nextStopEdge = nextStop.edge;
6062 256575 : nextStopPos = nextStop.pars.startPos;
6063 256575 : nextStopIsWaypoint = nextStop.getSpeed() > 0;
6064 : }
6065 : // myArrivalTime = -1 in the context of validating departSpeed with departLane=best
6066 16852567 : if (myParameter->arrivalLaneProcedure >= ArrivalLaneDefinition::GIVEN && nextStopEdge == myRoute->end() && myArrivalLane >= 0) {
6067 339027 : nextStopEdge = (myRoute->end() - 1);
6068 339027 : nextStopLane = (*nextStopEdge)->getLanes()[myArrivalLane];
6069 339027 : nextStopPos = myArrivalPos;
6070 : }
6071 16852567 : if (nextStopEdge != myRoute->end()) {
6072 : // make sure that the "wrong" lanes get a penalty. (penalty needs to be
6073 : // large enough to overcome a magic threshold in MSLaneChangeModel::DK2004.cpp:383)
6074 595602 : nextStopPos = MAX2(POSITION_EPS, MIN2((double)nextStopPos, (double)(nextStopLane->getLength() - 2 * POSITION_EPS)));
6075 595602 : if (nextStopLane->isInternal()) {
6076 : // switch to the correct lane before entering the intersection
6077 171 : nextStopPos = (*nextStopEdge)->getLength();
6078 : }
6079 : }
6080 :
6081 : // go forward along the next lanes;
6082 : // trains do not have to deal with lane-changing for stops but their best
6083 : // lanes lookahead is needed for rail signal control
6084 16852567 : const bool continueAfterStop = nextStopIsWaypoint || isRailway(getVClass());
6085 : int seen = 0;
6086 : double seenLength = 0;
6087 : bool progress = true;
6088 : // bestLanes must cover the braking distance even when at the very end of the current lane to avoid unecessary slow down
6089 33705134 : const double maxBrakeDist = startLane->getLength() + getCarFollowModel().getHeadwayTime() * getMaxSpeed() + getCarFollowModel().brakeGap(getMaxSpeed()) + getVehicleType().getMinGap();
6090 16852567 : const double lookahead = getLaneChangeModel().getStrategicLookahead();
6091 83071484 : for (MSRouteIterator ce = myCurrEdge; progress;) {
6092 : std::vector<LaneQ> currentLanes;
6093 : const std::vector<MSLane*>* allowed = nullptr;
6094 : const MSEdge* nextEdge = nullptr;
6095 66218917 : if (ce != myRoute->end() && ce + 1 != myRoute->end()) {
6096 53564751 : nextEdge = *(ce + 1);
6097 53564751 : allowed = (*ce)->allowedLanes(*nextEdge, myType->getVehicleClass());
6098 : }
6099 66218917 : const std::vector<MSLane*>& lanes = (*ce)->getLanes();
6100 168223574 : for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
6101 : LaneQ q;
6102 102004657 : MSLane* cl = *i;
6103 102004657 : q.lane = cl;
6104 102004657 : q.bestContinuations.push_back(cl);
6105 102004657 : q.bestLaneOffset = 0;
6106 102004657 : q.length = cl->allowsVehicleClass(myType->getVehicleClass()) ? (*ce)->getLength() : 0;
6107 102004657 : q.currentLength = q.length;
6108 : // if all lanes are forbidden (i.e. due to a dynamic closing) we want to express no preference
6109 102004657 : q.allowsContinuation = allowed == nullptr || std::find(allowed->begin(), allowed->end(), cl) != allowed->end();
6110 102004657 : q.occupation = 0;
6111 102004657 : q.nextOccupation = 0;
6112 102004657 : currentLanes.push_back(q);
6113 : }
6114 : //
6115 : if (nextStopEdge == ce
6116 : // already past the stop edge
6117 66218917 : && !(ce == myCurrEdge && myLane != nullptr && myLane->isInternal())) {
6118 588027 : if (!nextStopLane->isInternal() && !continueAfterStop) {
6119 : progress = false;
6120 : }
6121 588027 : const MSLane* normalStopLane = nextStopLane->getNormalPredecessorLane();
6122 1867782 : for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
6123 1279755 : if (nextStopLane != nullptr && normalStopLane != (*q).lane) {
6124 691728 : (*q).allowsContinuation = false;
6125 691728 : (*q).length = nextStopPos;
6126 691728 : (*q).currentLength = (*q).length;
6127 : }
6128 : }
6129 : }
6130 :
6131 66218917 : myBestLanes.push_back(currentLanes);
6132 66218917 : ++seen;
6133 66218917 : seenLength += currentLanes[0].lane->getLength();
6134 : ++ce;
6135 66218917 : if (lookahead >= 0) {
6136 45 : progress &= (seen <= 2 || seenLength < lookahead); // custom (but we need to look at least one edge ahead)
6137 : } else {
6138 87346359 : progress &= (seen <= 4 || seenLength < MAX2(maxBrakeDist, 3000.0)); // motorway
6139 71016564 : progress &= (seen <= 8 || seenLength < MAX2(maxBrakeDist, 200.0) || isRailway(getVClass())); // urban
6140 : }
6141 66218917 : progress &= ce != myRoute->end();
6142 : /*
6143 : if(progress) {
6144 : progress &= (currentLanes.size()!=1||(*ce)->getLanes().size()!=1);
6145 : }
6146 : */
6147 66218917 : }
6148 :
6149 : // we are examining the last lane explicitly
6150 16852567 : if (myBestLanes.size() != 0) {
6151 : double bestLength = -1;
6152 : // minimum and maximum lane index with best length
6153 : int bestThisIndex = 0;
6154 : int bestThisMaxIndex = 0;
6155 : int index = 0;
6156 : std::vector<LaneQ>& last = myBestLanes.back();
6157 44166759 : for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
6158 27314192 : if ((*j).length > bestLength) {
6159 : bestLength = (*j).length;
6160 : bestThisIndex = index;
6161 : bestThisMaxIndex = index;
6162 6082648 : } else if ((*j).length == bestLength) {
6163 : bestThisMaxIndex = index;
6164 : }
6165 : }
6166 : index = 0;
6167 : bool requiredChangeRightForbidden = false;
6168 : int requireChangeToLeftForbidden = -1;
6169 44166759 : for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
6170 27314192 : if ((*j).length < bestLength) {
6171 4808284 : if (abs(bestThisIndex - index) < abs(bestThisMaxIndex - index)) {
6172 125378 : (*j).bestLaneOffset = bestThisIndex - index;
6173 : } else {
6174 4682906 : (*j).bestLaneOffset = bestThisMaxIndex - index;
6175 : }
6176 4808284 : if (!(*j).allowsContinuation) {
6177 596870 : if ((*j).bestLaneOffset < 0 && (!(*j).lane->allowsChangingRight(getVClass())
6178 280798 : || !(*j).lane->getParallelLane(-1, false)->allowsVehicleClass(getVClass())
6179 278122 : || requiredChangeRightForbidden)) {
6180 : // this lane and all further lanes to the left cannot be used
6181 : requiredChangeRightForbidden = true;
6182 2686 : (*j).length = 0;
6183 594184 : } else if ((*j).bestLaneOffset > 0 && (!(*j).lane->allowsChangingLeft(getVClass())
6184 316031 : || !(*j).lane->getParallelLane(1, false)->allowsVehicleClass(getVClass()))) {
6185 : // this lane and all previous lanes to the right cannot be used
6186 16254 : requireChangeToLeftForbidden = (*j).lane->getIndex();
6187 : }
6188 : }
6189 : }
6190 : }
6191 16868836 : for (int i = requireChangeToLeftForbidden; i >= 0; i--) {
6192 16269 : if (last[i].bestLaneOffset > 0) {
6193 16269 : last[i].length = 0;
6194 : }
6195 : }
6196 : #ifdef DEBUG_BESTLANES
6197 : if (DEBUG_COND) {
6198 : std::cout << " last edge=" << last.front().lane->getEdge().getID() << " (bestIndex=" << bestThisIndex << " bestMaxIndex=" << bestThisMaxIndex << "):\n";
6199 : std::vector<LaneQ>& laneQs = myBestLanes.back();
6200 : for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
6201 : std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << "\n";
6202 : }
6203 : }
6204 : #endif
6205 : }
6206 : // go backward through the lanes
6207 : // track back best lane and compute the best prior lane(s)
6208 66218917 : for (std::vector<std::vector<LaneQ> >::reverse_iterator i = myBestLanes.rbegin() + 1; i != myBestLanes.rend(); ++i) {
6209 : std::vector<LaneQ>& nextLanes = (*(i - 1));
6210 : std::vector<LaneQ>& clanes = (*i);
6211 49366350 : MSEdge* const cE = &clanes[0].lane->getEdge();
6212 : int index = 0;
6213 : double bestConnectedLength = -1;
6214 : double bestLength = -1;
6215 123380208 : for (const LaneQ& j : nextLanes) {
6216 148027716 : if (j.lane->isApproachedFrom(cE) && bestConnectedLength < j.length) {
6217 : bestConnectedLength = j.length;
6218 : }
6219 74013858 : if (bestLength < j.length) {
6220 : bestLength = j.length;
6221 : }
6222 : }
6223 : // compute index of the best lane (highest length and least offset from the best next lane)
6224 : int bestThisIndex = 0;
6225 : int bestThisMaxIndex = 0;
6226 49366350 : if (bestConnectedLength > 0) {
6227 : index = 0;
6228 124034817 : for (LaneQ& j : clanes) {
6229 : const LaneQ* bestConnectedNext = nullptr;
6230 74676693 : if (j.allowsContinuation) {
6231 176363991 : for (const LaneQ& m : nextLanes) {
6232 122756244 : if ((m.lane->allowsVehicleClass(getVClass()) || m.lane->hadPermissionChanges())
6233 112373781 : && m.lane->isApproachedFrom(cE, j.lane)) {
6234 66146662 : if (betterContinuation(bestConnectedNext, m)) {
6235 : bestConnectedNext = &m;
6236 : }
6237 : }
6238 : }
6239 64041019 : if (bestConnectedNext != nullptr) {
6240 64041015 : if (bestConnectedNext->length == bestConnectedLength && abs(bestConnectedNext->bestLaneOffset) < 2) {
6241 62331614 : j.length += bestLength;
6242 : } else {
6243 1709401 : j.length += bestConnectedNext->length;
6244 : }
6245 64041015 : j.bestLaneOffset = bestConnectedNext->bestLaneOffset;
6246 : }
6247 : }
6248 64041015 : if (bestConnectedNext != nullptr && (bestConnectedNext->allowsContinuation || bestConnectedNext->length > 0)) {
6249 64003046 : copy(bestConnectedNext->bestContinuations.begin(), bestConnectedNext->bestContinuations.end(), back_inserter(j.bestContinuations));
6250 : } else {
6251 10673647 : j.allowsContinuation = false;
6252 : }
6253 74676693 : if (clanes[bestThisIndex].length < j.length
6254 66823717 : || (clanes[bestThisIndex].length == j.length && abs(clanes[bestThisIndex].bestLaneOffset) > abs(j.bestLaneOffset))
6255 204034537 : || (clanes[bestThisIndex].length == j.length && abs(clanes[bestThisIndex].bestLaneOffset) == abs(j.bestLaneOffset) &&
6256 62671315 : nextLinkPriority(clanes[bestThisIndex].bestContinuations) < nextLinkPriority(j.bestContinuations))
6257 : ) {
6258 : bestThisIndex = index;
6259 : bestThisMaxIndex = index;
6260 66674660 : } else if (clanes[bestThisIndex].length == j.length
6261 62659573 : && abs(clanes[bestThisIndex].bestLaneOffset) == abs(j.bestLaneOffset)
6262 129334106 : && nextLinkPriority(clanes[bestThisIndex].bestContinuations) == nextLinkPriority(j.bestContinuations)) {
6263 : bestThisMaxIndex = index;
6264 : }
6265 74676693 : index++;
6266 : }
6267 :
6268 : //vehicle with elecHybrid device prefers running under an overhead wire
6269 49358124 : if (getDevice(typeid(MSDevice_ElecHybrid)) != nullptr) {
6270 : index = 0;
6271 215 : for (const LaneQ& j : clanes) {
6272 155 : std::string overheadWireSegmentID = MSNet::getInstance()->getStoppingPlaceID(j.lane, j.currentLength / 2., SUMO_TAG_OVERHEAD_WIRE_SEGMENT);
6273 155 : if (overheadWireSegmentID != "") {
6274 : bestThisIndex = index;
6275 : bestThisMaxIndex = index;
6276 : }
6277 155 : index++;
6278 : }
6279 : }
6280 :
6281 : } else {
6282 : // only needed in case of disconnected routes
6283 : int bestNextIndex = 0;
6284 8226 : int bestDistToNeeded = (int) clanes.size();
6285 : index = 0;
6286 21998 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6287 13772 : if ((*j).allowsContinuation) {
6288 : int nextIndex = 0;
6289 30400 : for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
6290 16826 : if ((*m).lane->isApproachedFrom(cE, (*j).lane)) {
6291 5895 : if (bestDistToNeeded > abs((*m).bestLaneOffset)) {
6292 : bestDistToNeeded = abs((*m).bestLaneOffset);
6293 : bestThisIndex = index;
6294 : bestThisMaxIndex = index;
6295 : bestNextIndex = nextIndex;
6296 : }
6297 : }
6298 : }
6299 : }
6300 : }
6301 8226 : clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
6302 8226 : copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
6303 :
6304 : }
6305 : // set bestLaneOffset for all lanes
6306 : index = 0;
6307 : bool requiredChangeRightForbidden = false;
6308 : int requireChangeToLeftForbidden = -1;
6309 124056815 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6310 74690465 : if ((*j).length < clanes[bestThisIndex].length
6311 62347304 : || ((*j).length == clanes[bestThisIndex].length && abs((*j).bestLaneOffset) > abs(clanes[bestThisIndex].bestLaneOffset))
6312 137037540 : || (nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)
6313 : ) {
6314 12518703 : if (abs(bestThisIndex - index) < abs(bestThisMaxIndex - index)) {
6315 762950 : (*j).bestLaneOffset = bestThisIndex - index;
6316 : } else {
6317 11755753 : (*j).bestLaneOffset = bestThisMaxIndex - index;
6318 : }
6319 12518703 : if ((nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)) {
6320 : // try to move away from the lower-priority lane before it ends
6321 10868960 : (*j).length = (*j).currentLength;
6322 : }
6323 12518703 : if (!(*j).allowsContinuation) {
6324 10673564 : if ((*j).bestLaneOffset < 0 && (!(*j).lane->allowsChangingRight(getVClass())
6325 2509130 : || !(*j).lane->getParallelLane(-1, false)->allowsVehicleClass(getVClass())
6326 2494966 : || requiredChangeRightForbidden)) {
6327 : // this lane and all further lanes to the left cannot be used
6328 : requiredChangeRightForbidden = true;
6329 28468 : if ((*j).length == (*j).currentLength) {
6330 28468 : (*j).length = 0;
6331 : }
6332 10645096 : } else if ((*j).bestLaneOffset > 0 && (!(*j).lane->allowsChangingLeft(getVClass())
6333 8107122 : || !(*j).lane->getParallelLane(1, false)->allowsVehicleClass(getVClass()))) {
6334 : // this lane and all previous lanes to the right cannot be used
6335 101413 : requireChangeToLeftForbidden = (*j).lane->getIndex();
6336 : }
6337 : }
6338 : } else {
6339 62171762 : (*j).bestLaneOffset = 0;
6340 : }
6341 : }
6342 49485976 : for (int idx = requireChangeToLeftForbidden; idx >= 0; idx--) {
6343 119626 : if (clanes[idx].length == clanes[idx].currentLength) {
6344 119626 : clanes[idx].length = 0;
6345 : };
6346 : }
6347 :
6348 : //vehicle with elecHybrid device prefers running under an overhead wire
6349 49366350 : if (static_cast<MSDevice_ElecHybrid*>(getDevice(typeid(MSDevice_ElecHybrid))) != 0) {
6350 : index = 0;
6351 60 : std::string overheadWireID = MSNet::getInstance()->getStoppingPlaceID(clanes[bestThisIndex].lane, (clanes[bestThisIndex].currentLength) / 2, SUMO_TAG_OVERHEAD_WIRE_SEGMENT);
6352 60 : if (overheadWireID != "") {
6353 205 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6354 149 : (*j).bestLaneOffset = bestThisIndex - index;
6355 : }
6356 : }
6357 : }
6358 :
6359 : #ifdef DEBUG_BESTLANES
6360 : if (DEBUG_COND) {
6361 : std::cout << " edge=" << cE->getID() << " (bestIndex=" << bestThisIndex << " bestMaxIndex=" << bestThisMaxIndex << "):\n";
6362 : std::vector<LaneQ>& laneQs = clanes;
6363 : for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
6364 : std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << " allowCont=" << (*j).allowsContinuation << "\n";
6365 : }
6366 : }
6367 : #endif
6368 :
6369 : }
6370 16852567 : if (myBestLanes.front().front().lane->isInternal()) {
6371 : // route starts on an internal lane
6372 28 : if (myLane != nullptr) {
6373 : startLane = myLane;
6374 : } else {
6375 : // vehicle not yet departed
6376 12 : startLane = myBestLanes.front().front().lane;
6377 : }
6378 : }
6379 16852567 : updateOccupancyAndCurrentBestLane(startLane);
6380 : #ifdef DEBUG_BESTLANES
6381 : if (DEBUG_COND) {
6382 : std::cout << SIMTIME << " veh=" << getID() << " bestCont=" << toString(getBestLanesContinuation()) << "\n";
6383 : }
6384 : #endif
6385 : }
6386 :
6387 : void
6388 249 : MSVehicle::updateLaneBruttoSum() {
6389 249 : if (myLane != nullptr) {
6390 249 : myLane->markRecalculateBruttoSum();
6391 : }
6392 249 : }
6393 :
6394 : bool
6395 66146662 : MSVehicle::betterContinuation(const LaneQ* bestConnectedNext, const LaneQ& m) const {
6396 66146662 : if (bestConnectedNext == nullptr) {
6397 : return true;
6398 2105647 : } else if (m.lane->getBidiLane() != nullptr && bestConnectedNext->lane->getBidiLane() == nullptr) {
6399 : return false;
6400 2104909 : } else if (bestConnectedNext->lane->getBidiLane() != nullptr && m.lane->getBidiLane() == nullptr) {
6401 : return true;
6402 2104909 : } else if (bestConnectedNext->length < m.length) {
6403 : return true;
6404 1751641 : } else if (bestConnectedNext->length == m.length) {
6405 1237408 : if (abs(bestConnectedNext->bestLaneOffset) > abs(m.bestLaneOffset)) {
6406 : return true;
6407 : }
6408 1077612 : const double contRight = getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_CONTRIGHT, 1);
6409 : if (contRight < 1
6410 : // if we don't check for adjacency, the rightmost line will get
6411 : // multiple chances to be better which leads to an uninituitve distribution
6412 1006 : && (m.lane->getIndex() - bestConnectedNext->lane->getIndex()) == 1
6413 1078391 : && RandHelper::rand(getRNG()) > contRight) {
6414 : return true;
6415 : }
6416 : }
6417 : return false;
6418 : }
6419 :
6420 :
6421 : int
6422 400393078 : MSVehicle::nextLinkPriority(const std::vector<MSLane*>& conts) {
6423 400393078 : if (conts.size() < 2) {
6424 : return -1;
6425 : } else {
6426 361087723 : const MSLink* const link = conts[0]->getLinkTo(conts[1]);
6427 361087723 : if (link != nullptr) {
6428 361081331 : return link->havePriority() ? 1 : 0;
6429 : } else {
6430 : // disconnected route
6431 : return -1;
6432 : }
6433 : }
6434 : }
6435 :
6436 :
6437 : void
6438 1037120871 : MSVehicle::updateOccupancyAndCurrentBestLane(const MSLane* startLane) {
6439 : std::vector<LaneQ>& currLanes = *myBestLanes.begin();
6440 : std::vector<LaneQ>::iterator i;
6441 : #ifdef _DEBUG
6442 : bool found = false;
6443 : #endif
6444 2896887068 : for (i = currLanes.begin(); i != currLanes.end(); ++i) {
6445 : double nextOccupation = 0;
6446 4362755756 : for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
6447 2502989559 : nextOccupation += (*j)->getBruttoVehLenSum();
6448 : }
6449 1859766197 : (*i).nextOccupation = nextOccupation;
6450 : #ifdef DEBUG_BESTLANES
6451 : if (DEBUG_COND) {
6452 : std::cout << " lane=" << (*i).lane->getID() << " nextOccupation=" << nextOccupation << "\n";
6453 : }
6454 : #endif
6455 1859766197 : if ((*i).lane == startLane) {
6456 1031347471 : myCurrentLaneInBestLanes = i;
6457 : #ifdef _DEBUG
6458 : found = true;
6459 : #endif
6460 : }
6461 : }
6462 : #ifdef _DEBUG
6463 : assert(found || startLane->isInternal());
6464 : #endif
6465 1037120871 : }
6466 :
6467 :
6468 : const std::vector<MSLane*>&
6469 2043705804 : MSVehicle::getBestLanesContinuation() const {
6470 2043705804 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6471 : return myEmptyLaneVector;
6472 : }
6473 2043705804 : return (*myCurrentLaneInBestLanes).bestContinuations;
6474 : }
6475 :
6476 :
6477 : const std::vector<MSLane*>&
6478 71712398 : MSVehicle::getBestLanesContinuation(const MSLane* const l) const {
6479 : const MSLane* lane = l;
6480 : // XXX: shouldn't this be a "while" to cover more than one internal lane? (Leo) Refs. #2575
6481 71712398 : if (lane->getEdge().isInternal()) {
6482 : // internal edges are not kept inside the bestLanes structure
6483 4839824 : lane = lane->getLinkCont()[0]->getLane();
6484 : }
6485 71712398 : if (myBestLanes.size() == 0) {
6486 : return myEmptyLaneVector;
6487 : }
6488 119684914 : for (std::vector<LaneQ>::const_iterator i = myBestLanes[0].begin(); i != myBestLanes[0].end(); ++i) {
6489 119672810 : if ((*i).lane == lane) {
6490 71700294 : return (*i).bestContinuations;
6491 : }
6492 : }
6493 : return myEmptyLaneVector;
6494 : }
6495 :
6496 : const std::vector<const MSLane*>
6497 287685 : MSVehicle::getUpcomingLanesUntil(double distance) const {
6498 : std::vector<const MSLane*> lanes;
6499 :
6500 287685 : if (distance <= 0. || hasArrived()) {
6501 : // WRITE_WARNINGF(TL("MSVehicle::getUpcomingLanesUntil(): distance ('%') should be greater than 0."), distance);
6502 : return lanes;
6503 : }
6504 :
6505 287477 : if (!myLaneChangeModel->isOpposite()) {
6506 284151 : distance += getPositionOnLane();
6507 : } else {
6508 3326 : distance += myLane->getOppositePos(getPositionOnLane());
6509 : }
6510 287477 : MSLane* lane = myLaneChangeModel->isOpposite() ? myLane->getParallelOpposite() : myLane;
6511 295515 : while (lane->isInternal() && (distance > 0.)) { // include initial internal lanes
6512 8038 : lanes.insert(lanes.end(), lane);
6513 8038 : distance -= lane->getLength();
6514 13521 : lane = lane->getLinkCont().front()->getViaLaneOrLane();
6515 : }
6516 :
6517 287477 : const std::vector<MSLane*>& contLanes = getBestLanesContinuation();
6518 287477 : if (contLanes.empty()) {
6519 : return lanes;
6520 : }
6521 : auto contLanesIt = contLanes.begin();
6522 287477 : MSRouteIterator routeIt = myCurrEdge; // keep track of covered edges in myRoute
6523 615424 : while (distance > 0.) {
6524 335751 : MSLane* l = nullptr;
6525 335751 : if (contLanesIt != contLanes.end()) {
6526 319862 : l = *contLanesIt;
6527 : if (l != nullptr) {
6528 : assert(l->getEdge().getID() == (*routeIt)->getLanes().front()->getEdge().getID());
6529 : }
6530 : ++contLanesIt;
6531 319862 : if (l != nullptr || myLane->isInternal()) {
6532 : ++routeIt;
6533 : }
6534 319862 : if (l == nullptr) {
6535 5479 : continue;
6536 : }
6537 15889 : } else if (routeIt != myRoute->end()) { // bestLanes didn't get us far enough
6538 : // choose left-most lane as default (avoid sidewalks, bike lanes etc)
6539 8974 : l = (*routeIt)->getLanes().back();
6540 : ++routeIt;
6541 : } else { // the search distance goes beyond our route
6542 : break;
6543 : }
6544 :
6545 : assert(l != nullptr);
6546 :
6547 : // insert internal lanes if applicable
6548 323357 : const MSLane* internalLane = lanes.size() > 0 ? lanes.back()->getInternalFollowingLane(l) : nullptr;
6549 367511 : while ((internalLane != nullptr) && internalLane->isInternal() && (distance > 0.)) {
6550 44154 : lanes.insert(lanes.end(), internalLane);
6551 44154 : distance -= internalLane->getLength();
6552 70565 : internalLane = internalLane->getLinkCont().front()->getViaLaneOrLane();
6553 : }
6554 323357 : if (distance <= 0.) {
6555 : break;
6556 : }
6557 :
6558 322468 : lanes.insert(lanes.end(), l);
6559 322468 : distance -= l->getLength();
6560 : }
6561 :
6562 : return lanes;
6563 0 : }
6564 :
6565 : const std::vector<const MSLane*>
6566 6690 : MSVehicle::getPastLanesUntil(double distance) const {
6567 : std::vector<const MSLane*> lanes;
6568 :
6569 6690 : if (distance <= 0.) {
6570 : // WRITE_WARNINGF(TL("MSVehicle::getPastLanesUntil(): distance ('%') should be greater than 0."), distance);
6571 : return lanes;
6572 : }
6573 :
6574 6582 : MSRouteIterator routeIt = myCurrEdge;
6575 6582 : if (!myLaneChangeModel->isOpposite()) {
6576 6558 : distance += myLane->getLength() - getPositionOnLane();
6577 : } else {
6578 24 : distance += myLane->getParallelOpposite()->getLength() - myLane->getOppositePos(getPositionOnLane());
6579 : }
6580 6582 : MSLane* lane = myLaneChangeModel->isOpposite() ? myLane->getParallelOpposite() : myLane;
6581 6603 : while (lane->isInternal() && (distance > 0.)) { // include initial internal lanes
6582 21 : lanes.insert(lanes.end(), lane);
6583 21 : distance -= lane->getLength();
6584 21 : lane = lane->getLogicalPredecessorLane();
6585 : }
6586 :
6587 10126 : while (distance > 0.) {
6588 : // choose left-most lane as default (avoid sidewalks, bike lanes etc)
6589 8792 : MSLane* l = (*routeIt)->getLanes().back();
6590 :
6591 : // insert internal lanes if applicable
6592 8792 : const MSEdge* internalEdge = lanes.size() > 0 ? (*routeIt)->getInternalFollowingEdge(&(lanes.back()->getEdge()), getVClass()) : nullptr;
6593 8813 : const MSLane* internalLane = internalEdge != nullptr ? internalEdge->getLanes().front() : nullptr;
6594 : std::vector<const MSLane*> internalLanes;
6595 11016 : while ((internalLane != nullptr) && internalLane->isInternal()) { // collect all internal successor lanes
6596 2224 : internalLanes.insert(internalLanes.begin(), internalLane);
6597 4434 : internalLane = internalLane->getLinkCont().front()->getViaLaneOrLane();
6598 : }
6599 11016 : for (auto it = internalLanes.begin(); (it != internalLanes.end()) && (distance > 0.); ++it) { // check remaining distance in correct order
6600 2224 : lanes.insert(lanes.end(), *it);
6601 2224 : distance -= (*it)->getLength();
6602 : }
6603 8792 : if (distance <= 0.) {
6604 : break;
6605 : }
6606 :
6607 8776 : lanes.insert(lanes.end(), l);
6608 8776 : distance -= l->getLength();
6609 :
6610 : // NOTE: we're going backwards with the (bi-directional) Iterator
6611 : // TODO: consider make reverse_iterator() when moving on to C++14 or later
6612 8776 : if (routeIt != myRoute->begin()) {
6613 : --routeIt;
6614 : } else { // we went backwards to begin() and already processed the first and final element
6615 : break;
6616 : }
6617 8792 : }
6618 :
6619 : return lanes;
6620 0 : }
6621 :
6622 :
6623 : const std::vector<MSLane*>
6624 6355 : MSVehicle::getUpstreamOppositeLanes() const {
6625 6355 : const std::vector<const MSLane*> routeLanes = getPastLanesUntil(myLane->getMaximumBrakeDist());
6626 : std::vector<MSLane*> result;
6627 15441 : for (const MSLane* lane : routeLanes) {
6628 9859 : MSLane* opposite = lane->getOpposite();
6629 9859 : if (opposite != nullptr) {
6630 9086 : result.push_back(opposite);
6631 : } else {
6632 : break;
6633 : }
6634 : }
6635 6355 : return result;
6636 6355 : }
6637 :
6638 :
6639 : int
6640 307783570 : MSVehicle::getBestLaneOffset() const {
6641 307783570 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6642 : return 0;
6643 : } else {
6644 307470112 : return (*myCurrentLaneInBestLanes).bestLaneOffset;
6645 : }
6646 : }
6647 :
6648 : double
6649 23463 : MSVehicle::getBestLaneDist() const {
6650 23463 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6651 : return -1;
6652 : } else {
6653 23463 : return (*myCurrentLaneInBestLanes).length;
6654 : }
6655 : }
6656 :
6657 :
6658 :
6659 : void
6660 644725807 : MSVehicle::adaptBestLanesOccupation(int laneIndex, double density) {
6661 : std::vector<MSVehicle::LaneQ>& preb = myBestLanes.front();
6662 : assert(laneIndex < (int)preb.size());
6663 644725807 : preb[laneIndex].occupation = density + preb[laneIndex].nextOccupation;
6664 644725807 : }
6665 :
6666 :
6667 : void
6668 63784 : MSVehicle::fixPosition() {
6669 63784 : if (MSGlobals::gLaneChangeDuration > 0 && !myLaneChangeModel->isChangingLanes()) {
6670 33682 : myState.myPosLat = 0;
6671 : }
6672 63784 : }
6673 :
6674 : std::pair<const MSLane*, double>
6675 271 : MSVehicle::getLanePosAfterDist(double distance) const {
6676 271 : if (distance == 0) {
6677 239 : return std::make_pair(myLane, getPositionOnLane());
6678 : }
6679 32 : const std::vector<const MSLane*> lanes = getUpcomingLanesUntil(distance);
6680 32 : distance += getPositionOnLane();
6681 32 : for (const MSLane* lane : lanes) {
6682 32 : if (lane->getLength() > distance) {
6683 : return std::make_pair(lane, distance);
6684 : }
6685 0 : distance -= lane->getLength();
6686 : }
6687 0 : return std::make_pair(nullptr, -1);
6688 32 : }
6689 :
6690 :
6691 : double
6692 14976 : MSVehicle::getDistanceToPosition(double destPos, const MSLane* destLane) const {
6693 14976 : if (isOnRoad() && destLane != nullptr) {
6694 14940 : return myRoute->getDistanceBetween(getPositionOnLane(), destPos, myLane, destLane);
6695 : }
6696 : return std::numeric_limits<double>::max();
6697 : }
6698 :
6699 :
6700 : std::pair<const MSVehicle* const, double>
6701 76410206 : MSVehicle::getLeader(double dist, bool considerCrossingFoes) const {
6702 76410206 : if (myLane == nullptr) {
6703 0 : return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
6704 : }
6705 76410206 : if (dist == 0) {
6706 2474 : dist = getCarFollowModel().brakeGap(getSpeed()) + getVehicleType().getMinGap();
6707 : }
6708 : const MSVehicle* lead = nullptr;
6709 76410206 : const MSLane* lane = myLane; // ensure lane does not change between getVehiclesSecure and releaseVehicles;
6710 76410206 : const MSLane::VehCont& vehs = lane->getVehiclesSecure();
6711 : // vehicle might be outside the road network
6712 76410206 : MSLane::VehCont::const_iterator it = std::find(vehs.begin(), vehs.end(), this);
6713 76410206 : if (it != vehs.end() && it + 1 != vehs.end()) {
6714 72697325 : lead = *(it + 1);
6715 : }
6716 72697325 : if (lead != nullptr) {
6717 : std::pair<const MSVehicle* const, double> result(
6718 72697325 : lead, lead->getBackPositionOnLane(myLane) - getPositionOnLane() - getVehicleType().getMinGap());
6719 72697325 : lane->releaseVehicles();
6720 72697325 : return result;
6721 : }
6722 3712881 : const double seen = myLane->getLength() - getPositionOnLane();
6723 3712881 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(myLane);
6724 3712881 : std::pair<const MSVehicle* const, double> result = myLane->getLeaderOnConsecutive(dist, seen, getSpeed(), *this, bestLaneConts, considerCrossingFoes);
6725 3712877 : lane->releaseVehicles();
6726 3712877 : return result;
6727 : }
6728 :
6729 :
6730 : std::pair<const MSVehicle* const, double>
6731 2271830 : MSVehicle::getFollower(double dist) const {
6732 2271830 : if (myLane == nullptr) {
6733 0 : return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
6734 : }
6735 2271830 : if (dist == 0) {
6736 810923 : dist = getCarFollowModel().brakeGap(myLane->getEdge().getSpeedLimit() * 2, 4.5, 0);
6737 : }
6738 2271830 : return myLane->getFollower(this, getPositionOnLane(), dist, MSLane::MinorLinkMode::FOLLOW_NEVER);
6739 : }
6740 :
6741 :
6742 : double
6743 0 : MSVehicle::getTimeGapOnLane() const {
6744 : // calling getLeader with 0 would induce a dist calculation but we only want to look for the leaders on the current lane
6745 0 : std::pair<const MSVehicle* const, double> leaderInfo = getLeader(-1);
6746 0 : if (leaderInfo.first == nullptr || getSpeed() == 0) {
6747 0 : return -1;
6748 : }
6749 0 : return (leaderInfo.second + getVehicleType().getMinGap()) / getSpeed();
6750 : }
6751 :
6752 :
6753 : void
6754 1120561 : MSVehicle::addTransportable(MSTransportable* transportable) {
6755 1120561 : MSBaseVehicle::addTransportable(transportable);
6756 11232 : if (myStops.size() > 0 && myStops.front().reached) {
6757 8031 : if (transportable->isPerson()) {
6758 7452 : if (myStops.front().triggered && myStops.front().numExpectedPerson > 0) {
6759 1618 : myStops.front().numExpectedPerson -= (int)myStops.front().pars.awaitedPersons.count(transportable->getID());
6760 : }
6761 : } else {
6762 579 : if (myStops.front().pars.containerTriggered && myStops.front().numExpectedContainer > 0) {
6763 20 : myStops.front().numExpectedContainer -= (int)myStops.front().pars.awaitedContainers.count(transportable->getID());
6764 : }
6765 : }
6766 : }
6767 11232 : }
6768 :
6769 :
6770 : void
6771 700575192 : MSVehicle::setBlinkerInformation() {
6772 : switchOffSignal(VEH_SIGNAL_BLINKER_RIGHT | VEH_SIGNAL_BLINKER_LEFT);
6773 700575192 : int state = myLaneChangeModel->getOwnState();
6774 : // do not set blinker for sublane changes or when blocked from changing to the right
6775 700575192 : const bool blinkerManoeuvre = (((state & LCA_SUBLANE) == 0) && (
6776 611057668 : (state & LCA_KEEPRIGHT) == 0 || (state & LCA_BLOCKED) == 0));
6777 : Signalling left = VEH_SIGNAL_BLINKER_LEFT;
6778 : Signalling right = VEH_SIGNAL_BLINKER_RIGHT;
6779 700575192 : if (MSGlobals::gLefthand) {
6780 : // lane indices increase from left to right
6781 : std::swap(left, right);
6782 : }
6783 700575192 : if ((state & LCA_LEFT) != 0 && blinkerManoeuvre) {
6784 19517653 : switchOnSignal(left);
6785 681057539 : } else if ((state & LCA_RIGHT) != 0 && blinkerManoeuvre) {
6786 5632075 : switchOnSignal(right);
6787 675425464 : } else if (myLaneChangeModel->isChangingLanes()) {
6788 248751 : if (myLaneChangeModel->getLaneChangeDirection() == 1) {
6789 159553 : switchOnSignal(left);
6790 : } else {
6791 89198 : switchOnSignal(right);
6792 : }
6793 : } else {
6794 675176713 : const MSLane* lane = getLane();
6795 675176713 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, 1, *lane, getBestLanesContinuation());
6796 675176713 : if (link != lane->getLinkCont().end() && lane->getLength() - getPositionOnLane() < lane->getVehicleMaxSpeed(this) * (double) 7.) {
6797 170596344 : switch ((*link)->getDirection()) {
6798 : case LinkDirection::TURN:
6799 : case LinkDirection::LEFT:
6800 : case LinkDirection::PARTLEFT:
6801 : switchOnSignal(VEH_SIGNAL_BLINKER_LEFT);
6802 : break;
6803 : case LinkDirection::RIGHT:
6804 : case LinkDirection::PARTRIGHT:
6805 : switchOnSignal(VEH_SIGNAL_BLINKER_RIGHT);
6806 : break;
6807 : default:
6808 : break;
6809 : }
6810 : }
6811 : }
6812 : // stopping related signals
6813 700575192 : if (hasStops()
6814 700575192 : && (myStops.begin()->reached ||
6815 15415966 : (myStopDist < (myLane->getLength() - getPositionOnLane())
6816 4257270 : && myStopDist < getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this), getCarFollowModel().getMaxDecel(), 3)))) {
6817 17237540 : if (myStops.begin()->lane->getIndex() > 0 && myStops.begin()->lane->getParallelLane(-1)->allowsVehicleClass(getVClass())) {
6818 : // not stopping on the right. Activate emergency blinkers
6819 : switchOnSignal(VEH_SIGNAL_BLINKER_LEFT | VEH_SIGNAL_BLINKER_RIGHT);
6820 16982812 : } else if (!myStops.begin()->reached && (myStops.begin()->pars.parking == ParkingType::OFFROAD)) {
6821 : // signal upcoming parking stop on the current lane when within braking distance (~2 seconds before braking)
6822 1617393 : switchOnSignal(MSGlobals::gLefthand ? VEH_SIGNAL_BLINKER_LEFT : VEH_SIGNAL_BLINKER_RIGHT);
6823 : }
6824 : }
6825 700575192 : if (myInfluencer != nullptr && myInfluencer->getSignals() >= 0) {
6826 14 : mySignals = myInfluencer->getSignals();
6827 : myInfluencer->setSignals(-1); // overwrite computed signals only once
6828 : }
6829 700575192 : }
6830 :
6831 : void
6832 85904 : MSVehicle::setEmergencyBlueLight(SUMOTime currentTime) {
6833 :
6834 : //TODO look if timestep ist SIMSTEP
6835 85904 : if (currentTime % 1000 == 0) {
6836 26113 : if (signalSet(VEH_SIGNAL_EMERGENCY_BLUE)) {
6837 : switchOffSignal(VEH_SIGNAL_EMERGENCY_BLUE);
6838 : } else {
6839 : switchOnSignal(VEH_SIGNAL_EMERGENCY_BLUE);
6840 : }
6841 : }
6842 85904 : }
6843 :
6844 :
6845 : int
6846 23362442 : MSVehicle::getLaneIndex() const {
6847 23362442 : return myLane == nullptr ? -1 : myLane->getIndex();
6848 : }
6849 :
6850 :
6851 : void
6852 15899286 : MSVehicle::setTentativeLaneAndPosition(MSLane* lane, double pos, double posLat) {
6853 15899286 : myLane = lane;
6854 15899286 : myState.myPos = pos;
6855 15899286 : myState.myPosLat = posLat;
6856 15899286 : myState.myBackPos = pos - getVehicleType().getLength();
6857 15899286 : }
6858 :
6859 :
6860 : double
6861 383734981 : MSVehicle::getRightSideOnLane() const {
6862 383734981 : return myState.myPosLat + 0.5 * myLane->getWidth() - 0.5 * getVehicleType().getWidth();
6863 : }
6864 :
6865 :
6866 : double
6867 378603557 : MSVehicle::getLeftSideOnLane() const {
6868 378603557 : return myState.myPosLat + 0.5 * myLane->getWidth() + 0.5 * getVehicleType().getWidth();
6869 : }
6870 :
6871 :
6872 : double
6873 304127780 : MSVehicle::getRightSideOnLane(const MSLane* lane) const {
6874 304127780 : return myState.myPosLat + 0.5 * lane->getWidth() - 0.5 * getVehicleType().getWidth();
6875 : }
6876 :
6877 :
6878 : double
6879 303639438 : MSVehicle::getLeftSideOnLane(const MSLane* lane) const {
6880 303639438 : return myState.myPosLat + 0.5 * lane->getWidth() + 0.5 * getVehicleType().getWidth();
6881 : }
6882 :
6883 :
6884 : double
6885 244587137 : MSVehicle::getRightSideOnEdge(const MSLane* lane) const {
6886 244587137 : return getCenterOnEdge(lane) - 0.5 * getVehicleType().getWidth();
6887 : }
6888 :
6889 :
6890 : double
6891 31283270 : MSVehicle::getLeftSideOnEdge(const MSLane* lane) const {
6892 31283270 : return getCenterOnEdge(lane) + 0.5 * getVehicleType().getWidth();
6893 : }
6894 :
6895 :
6896 : double
6897 715115555 : MSVehicle::getCenterOnEdge(const MSLane* lane) const {
6898 715115555 : if (lane == nullptr || &lane->getEdge() == &myLane->getEdge()) {
6899 714650686 : return myLane->getRightSideOnEdge() + myState.myPosLat + 0.5 * myLane->getWidth();
6900 464869 : } else if (lane == myLaneChangeModel->getShadowLane()) {
6901 13722 : if (myLaneChangeModel->isOpposite() && &lane->getEdge() != &myLane->getEdge()) {
6902 13715 : return lane->getRightSideOnEdge() + lane->getWidth() - myState.myPosLat + 0.5 * myLane->getWidth();
6903 : }
6904 7 : if (myLaneChangeModel->getShadowDirection() == -1) {
6905 0 : return lane->getRightSideOnEdge() + lane->getWidth() + myState.myPosLat + 0.5 * myLane->getWidth();
6906 : } else {
6907 7 : return lane->getRightSideOnEdge() - myLane->getWidth() + myState.myPosLat + 0.5 * myLane->getWidth();
6908 : }
6909 451147 : } else if (lane == myLane->getBidiLane()) {
6910 1679 : return lane->getRightSideOnEdge() - myState.myPosLat + 0.5 * lane->getWidth();
6911 : } else {
6912 : assert(myFurtherLanes.size() == myFurtherLanesPosLat.size());
6913 525103 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6914 498709 : if (myFurtherLanes[i] == lane) {
6915 : #ifdef DEBUG_FURTHER
6916 : if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat=" << myFurtherLanesPosLat[i]
6917 : << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
6918 : << "\n";
6919 : #endif
6920 423001 : return lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
6921 75708 : } else if (myFurtherLanes[i]->getBidiLane() == lane) {
6922 : #ifdef DEBUG_FURTHER
6923 : if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat(bidi)=" << myFurtherLanesPosLat[i]
6924 : << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
6925 : << "\n";
6926 : #endif
6927 73 : return lane->getRightSideOnEdge() - myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
6928 : }
6929 : }
6930 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
6931 26394 : const std::vector<MSLane*>& shadowFurther = myLaneChangeModel->getShadowFurtherLanes();
6932 26661 : for (int i = 0; i < (int)shadowFurther.size(); ++i) {
6933 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
6934 26661 : if (shadowFurther[i] == lane) {
6935 : assert(myLaneChangeModel->getShadowLane() != 0);
6936 26394 : return (lane->getRightSideOnEdge() + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] + 0.5 * lane->getWidth()
6937 26394 : + (myLane->getCenterOnEdge() - myLaneChangeModel->getShadowLane()->getCenterOnEdge()));
6938 : }
6939 : }
6940 : assert(false);
6941 0 : throw ProcessError("Request lateral pos of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
6942 : }
6943 : }
6944 :
6945 :
6946 : double
6947 3374474213 : MSVehicle::getLatOffset(const MSLane* lane) const {
6948 : assert(lane != 0);
6949 3374474213 : if (&lane->getEdge() == &myLane->getEdge()) {
6950 3324375320 : return myLane->getRightSideOnEdge() - lane->getRightSideOnEdge();
6951 50098893 : } else if (myLane->getParallelOpposite() == lane) {
6952 2160194 : return (myLane->getWidth() + lane->getWidth()) * 0.5 - 2 * getLateralPositionOnLane();
6953 47938699 : } else if (myLane->getBidiLane() == lane) {
6954 179384 : return -2 * getLateralPositionOnLane();
6955 : } else {
6956 : // Check whether the lane is a further lane for the vehicle
6957 53877761 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6958 52911354 : if (myFurtherLanes[i] == lane) {
6959 : #ifdef DEBUG_FURTHER
6960 : if (DEBUG_COND) {
6961 : std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherLat=" << myFurtherLanesPosLat[i] << "\n";
6962 : }
6963 : #endif
6964 46766686 : return myFurtherLanesPosLat[i] - myState.myPosLat;
6965 6144668 : } else if (myFurtherLanes[i]->getBidiLane() == lane) {
6966 : #ifdef DEBUG_FURTHER
6967 : if (DEBUG_COND) {
6968 : std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherBidiLat=" << myFurtherLanesPosLat[i] << "\n";
6969 : }
6970 : #endif
6971 26222 : return -2 * (myFurtherLanesPosLat[i] - myState.myPosLat);
6972 : }
6973 : }
6974 : #ifdef DEBUG_FURTHER
6975 : if (DEBUG_COND) {
6976 : std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
6977 : }
6978 : #endif
6979 : // Check whether the lane is a "shadow further lane" for the vehicle
6980 966407 : const std::vector<MSLane*>& shadowFurther = myLaneChangeModel->getShadowFurtherLanes();
6981 977345 : for (int i = 0; i < (int)shadowFurther.size(); ++i) {
6982 974553 : if (shadowFurther[i] == lane) {
6983 : #ifdef DEBUG_FURTHER
6984 : if (DEBUG_COND) std::cout << " getLatOffset veh=" << getID()
6985 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
6986 : << " lane=" << lane->getID()
6987 : << " i=" << i
6988 : << " posLat=" << myState.myPosLat
6989 : << " shadowPosLat=" << getLatOffset(myLaneChangeModel->getShadowLane())
6990 : << " shadowFurtherLat=" << myLaneChangeModel->getShadowFurtherLanesPosLat()[i]
6991 : << "\n";
6992 : #endif
6993 963615 : return getLatOffset(myLaneChangeModel->getShadowLane()) + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] - myState.myPosLat;
6994 : }
6995 : }
6996 : // Check whether the vehicle issued a maneuverReservation on the lane.
6997 : const std::vector<MSLane*>& furtherTargets = myLaneChangeModel->getFurtherTargetLanes();
6998 3130 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6999 : // Further target lanes are just neighboring lanes of the vehicle's further lanes, @see MSAbstractLaneChangeModel::updateTargetLane()
7000 3126 : MSLane* targetLane = furtherTargets[i];
7001 3126 : if (targetLane == lane) {
7002 2788 : const double targetDir = myLaneChangeModel->getManeuverDist() < 0 ? -1. : 1.;
7003 2788 : const double latOffset = myFurtherLanesPosLat[i] - myState.myPosLat + targetDir * 0.5 * (myFurtherLanes[i]->getWidth() + targetLane->getWidth());
7004 : #ifdef DEBUG_TARGET_LANE
7005 : if (DEBUG_COND) {
7006 : std::cout << " getLatOffset veh=" << getID()
7007 : << " wrt targetLane=" << Named::getIDSecure(myLaneChangeModel->getTargetLane())
7008 : << "\n i=" << i
7009 : << " posLat=" << myState.myPosLat
7010 : << " furtherPosLat=" << myFurtherLanesPosLat[i]
7011 : << " maneuverDist=" << myLaneChangeModel->getManeuverDist()
7012 : << " targetDir=" << targetDir
7013 : << " latOffset=" << latOffset
7014 : << std::endl;
7015 : }
7016 : #endif
7017 2788 : return latOffset;
7018 : }
7019 : }
7020 : assert(false);
7021 24 : throw ProcessError("Request lateral offset of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
7022 : }
7023 : }
7024 :
7025 :
7026 : double
7027 35216924 : MSVehicle::lateralDistanceToLane(const int offset) const {
7028 : // compute the distance when changing to the neighboring lane
7029 : // (ensure we do not lap into the line behind neighLane since there might be unseen blockers)
7030 : assert(offset == 0 || offset == 1 || offset == -1);
7031 : assert(myLane != nullptr);
7032 : assert(myLane->getParallelLane(offset) != nullptr || myLane->getParallelOpposite() != nullptr);
7033 35216924 : const double halfCurrentLaneWidth = 0.5 * myLane->getWidth();
7034 35216924 : const double halfVehWidth = 0.5 * (getWidth() + NUMERICAL_EPS);
7035 35216924 : const double latPos = getLateralPositionOnLane();
7036 35216924 : const double oppositeSign = getLaneChangeModel().isOpposite() ? -1 : 1;
7037 35216924 : double leftLimit = halfCurrentLaneWidth - halfVehWidth - oppositeSign * latPos;
7038 35216924 : double rightLimit = -halfCurrentLaneWidth + halfVehWidth - oppositeSign * latPos;
7039 : double latLaneDist = 0; // minimum distance to move the vehicle fully onto the new lane
7040 35216924 : if (offset == 0) {
7041 8 : if (latPos + halfVehWidth > halfCurrentLaneWidth) {
7042 : // correct overlapping left
7043 4 : latLaneDist = halfCurrentLaneWidth - latPos - halfVehWidth;
7044 4 : } else if (latPos - halfVehWidth < -halfCurrentLaneWidth) {
7045 : // correct overlapping right
7046 4 : latLaneDist = -halfCurrentLaneWidth - latPos + halfVehWidth;
7047 : }
7048 8 : latLaneDist *= oppositeSign;
7049 35216916 : } else if (offset == -1) {
7050 15945107 : latLaneDist = rightLimit - (getWidth() + NUMERICAL_EPS);
7051 19271809 : } else if (offset == 1) {
7052 19271809 : latLaneDist = leftLimit + (getWidth() + NUMERICAL_EPS);
7053 : }
7054 : #ifdef DEBUG_ACTIONSTEPS
7055 : if (DEBUG_COND) {
7056 : std::cout << SIMTIME
7057 : << " veh=" << getID()
7058 : << " halfCurrentLaneWidth=" << halfCurrentLaneWidth
7059 : << " halfVehWidth=" << halfVehWidth
7060 : << " latPos=" << latPos
7061 : << " latLaneDist=" << latLaneDist
7062 : << " leftLimit=" << leftLimit
7063 : << " rightLimit=" << rightLimit
7064 : << "\n";
7065 : }
7066 : #endif
7067 35216924 : return latLaneDist;
7068 : }
7069 :
7070 :
7071 : double
7072 5104882532 : MSVehicle::getLateralOverlap(double posLat, const MSLane* lane) const {
7073 5104882532 : return (fabs(posLat) + 0.5 * getVehicleType().getWidth()
7074 5104882532 : - 0.5 * lane->getWidth());
7075 : }
7076 :
7077 : double
7078 0 : MSVehicle::getLateralOverlap(const MSLane* lane) const {
7079 0 : return getLateralOverlap(getLateralPositionOnLane(), lane);
7080 : }
7081 :
7082 : double
7083 4915536483 : MSVehicle::getLateralOverlap() const {
7084 4915536483 : return getLateralOverlap(getLateralPositionOnLane(), myLane);
7085 : }
7086 :
7087 :
7088 : void
7089 642009985 : MSVehicle::removeApproachingInformation(const DriveItemVector& lfLinks) const {
7090 1856956814 : for (const DriveProcessItem& dpi : lfLinks) {
7091 1214946829 : if (dpi.myLink != nullptr) {
7092 858813291 : dpi.myLink->removeApproaching(this);
7093 : }
7094 : }
7095 : // unregister on all shadow links
7096 642009985 : myLaneChangeModel->removeShadowApproachingInformation();
7097 642009985 : }
7098 :
7099 :
7100 : bool
7101 843060 : MSVehicle::unsafeLinkAhead(const MSLane* lane, double zipperDist) const {
7102 : // the following links are unsafe:
7103 : // - zipper links if they are close enough and have approaching vehicles in the relevant time range
7104 : // - unprioritized links if the vehicle is currently approaching a prioritzed link and unable to stop in time
7105 843060 : double seen = myLane->getLength() - getPositionOnLane();
7106 843060 : const double dist = MAX2(zipperDist, getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0));
7107 843060 : if (seen < dist) {
7108 70964 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(lane);
7109 : int view = 1;
7110 70964 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
7111 : DriveItemVector::const_iterator di = myLFLinkLanes.begin();
7112 117812 : while (!lane->isLinkEnd(link) && seen <= dist) {
7113 71526 : if ((!lane->isInternal()
7114 49141 : && (((*link)->getState() == LINKSTATE_ZIPPER && seen < (*link)->getFoeVisibilityDistance())
7115 26495 : || !(*link)->havePriority()))
7116 97828 : || (lane->isInternal() && zipperDist > 0)) {
7117 : // find the drive item corresponding to this link
7118 : bool found = false;
7119 52280 : while (di != myLFLinkLanes.end() && !found) {
7120 27074 : if ((*di).myLink != nullptr) {
7121 : const MSLane* diPredLane = (*di).myLink->getLaneBefore();
7122 27074 : if (diPredLane != nullptr) {
7123 27074 : if (&diPredLane->getEdge() == &lane->getEdge()) {
7124 : found = true;
7125 : }
7126 : }
7127 : }
7128 27074 : if (!found) {
7129 : di++;
7130 : }
7131 : }
7132 25206 : if (found) {
7133 25206 : const SUMOTime leaveTime = (*link)->getLeaveTime((*di).myArrivalTime, (*di).myArrivalSpeed,
7134 25206 : (*di).getLeaveSpeed(), getVehicleType().getLength());
7135 25206 : const MSLink* entry = (*link)->getCorrespondingEntryLink();
7136 : //if (DEBUG_COND) {
7137 : // std::cout << SIMTIME << " veh=" << getID() << " changeTo=" << Named::getIDSecure(bestLaneConts.front()) << " linkState=" << toString((*link)->getState()) << " seen=" << seen << " dist=" << dist << " zipperDist=" << zipperDist << " aT=" << STEPS2TIME((*di).myArrivalTime) << " lT=" << STEPS2TIME(leaveTime) << "\n";
7138 : //}
7139 25206 : if (entry->hasApproachingFoe((*di).myArrivalTime, leaveTime, (*di).myArrivalSpeed, getCarFollowModel().getMaxDecel())) {
7140 : //std::cout << SIMTIME << " veh=" << getID() << " aborting changeTo=" << Named::getIDSecure(bestLaneConts.front()) << " linkState=" << toString((*link)->getState()) << " seen=" << seen << " dist=" << dist << "\n";
7141 : return true;
7142 : }
7143 : }
7144 : // no drive item is found if the vehicle aborts its request within dist
7145 : }
7146 46848 : lane = (*link)->getViaLaneOrLane();
7147 46848 : if (!lane->getEdge().isInternal()) {
7148 24389 : view++;
7149 : }
7150 46848 : seen += lane->getLength();
7151 46848 : link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
7152 : }
7153 : }
7154 : return false;
7155 : }
7156 :
7157 :
7158 : PositionVector
7159 6726136 : MSVehicle::getBoundingBox(double offset) const {
7160 6726136 : PositionVector centerLine;
7161 6726136 : Position pos = getPosition();
7162 6726136 : centerLine.push_back(pos);
7163 6726136 : switch (myType->getGuiShape()) {
7164 1772 : case SUMOVehicleShape::BUS_FLEXIBLE:
7165 : case SUMOVehicleShape::RAIL:
7166 : case SUMOVehicleShape::RAIL_CAR:
7167 : case SUMOVehicleShape::RAIL_CARGO:
7168 : case SUMOVehicleShape::TRUCK_SEMITRAILER:
7169 : case SUMOVehicleShape::TRUCK_1TRAILER: {
7170 4554 : for (MSLane* lane : myFurtherLanes) {
7171 2782 : centerLine.push_back(lane->getShape().back());
7172 : }
7173 : break;
7174 : }
7175 : default:
7176 : break;
7177 : }
7178 6726136 : double l = getLength();
7179 6726136 : Position backPos = getBackPosition();
7180 6726136 : if (pos.distanceTo2D(backPos) > l + NUMERICAL_EPS) {
7181 : // getBackPosition may not match the visual back in networks without internal lanes
7182 358064 : double a = getAngle() + M_PI; // angle pointing backwards
7183 358064 : backPos = pos + Position(l * cos(a), l * sin(a));
7184 : }
7185 6726136 : centerLine.push_back(backPos);
7186 6726136 : if (offset != 0) {
7187 6543 : centerLine.extrapolate2D(offset);
7188 : }
7189 : PositionVector result = centerLine;
7190 13447982 : result.move2side(MAX2(0.0, 0.5 * myType->getWidth() + offset));
7191 13447982 : centerLine.move2side(MIN2(0.0, -0.5 * myType->getWidth() - offset));
7192 6726136 : result.append(centerLine.reverse(), POSITION_EPS);
7193 6726136 : return result;
7194 6726136 : }
7195 :
7196 :
7197 : PositionVector
7198 72798 : MSVehicle::getBoundingPoly(double offset) const {
7199 72798 : switch (myType->getGuiShape()) {
7200 72318 : case SUMOVehicleShape::PASSENGER:
7201 : case SUMOVehicleShape::PASSENGER_SEDAN:
7202 : case SUMOVehicleShape::PASSENGER_HATCHBACK:
7203 : case SUMOVehicleShape::PASSENGER_WAGON:
7204 : case SUMOVehicleShape::PASSENGER_VAN: {
7205 : // box with corners cut off
7206 72318 : PositionVector result;
7207 72318 : PositionVector centerLine;
7208 72318 : centerLine.push_back(getPosition());
7209 72318 : centerLine.push_back(getBackPosition());
7210 72318 : if (offset != 0) {
7211 1600 : centerLine.extrapolate2D(offset);
7212 : }
7213 : PositionVector line1 = centerLine;
7214 : PositionVector line2 = centerLine;
7215 144636 : line1.move2side(MAX2(0.0, 0.3 * myType->getWidth() + offset));
7216 144636 : line2.move2side(MAX2(0.0, 0.5 * myType->getWidth() + offset));
7217 72318 : line2.scaleRelative(0.8);
7218 72318 : result.push_back(line1[0]);
7219 72318 : result.push_back(line2[0]);
7220 72318 : result.push_back(line2[1]);
7221 72318 : result.push_back(line1[1]);
7222 144636 : line1.move2side(MIN2(0.0, -0.6 * myType->getWidth() - offset));
7223 144636 : line2.move2side(MIN2(0.0, -1.0 * myType->getWidth() - offset));
7224 72318 : result.push_back(line1[1]);
7225 72318 : result.push_back(line2[1]);
7226 72318 : result.push_back(line2[0]);
7227 72318 : result.push_back(line1[0]);
7228 : return result;
7229 72318 : }
7230 480 : default:
7231 480 : return getBoundingBox();
7232 : }
7233 : }
7234 :
7235 :
7236 : bool
7237 562186 : MSVehicle::onFurtherEdge(const MSEdge* edge) const {
7238 647989 : for (std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
7239 179293 : if (&(*i)->getEdge() == edge) {
7240 : return true;
7241 : }
7242 : }
7243 : return false;
7244 : }
7245 :
7246 :
7247 : bool
7248 7694709466 : MSVehicle::isBidiOn(const MSLane* lane) const {
7249 7695829505 : return lane->getBidiLane() != nullptr && (
7250 1120039 : myLane == lane->getBidiLane()
7251 562186 : || onFurtherEdge(&lane->getBidiLane()->getEdge()));
7252 : }
7253 :
7254 :
7255 : bool
7256 16 : MSVehicle::rerouteParkingArea(const std::string& parkingAreaID, std::string& errorMsg) {
7257 : // this function is based on MSTriggeredRerouter::rerouteParkingArea in order to keep
7258 : // consistency in the behaviour.
7259 :
7260 : // get vehicle params
7261 16 : MSParkingArea* destParkArea = getNextParkingArea();
7262 16 : const MSRoute& route = getRoute();
7263 16 : const MSEdge* lastEdge = route.getLastEdge();
7264 :
7265 16 : if (destParkArea == nullptr) {
7266 : // not driving towards a parking area
7267 0 : errorMsg = "Vehicle " + getID() + " is not driving to a parking area so it cannot be rerouted.";
7268 0 : return false;
7269 : }
7270 :
7271 : // if the current route ends at the parking area, the new route will also and at the new area
7272 16 : bool newDestination = (&destParkArea->getLane().getEdge() == route.getLastEdge()
7273 8 : && getArrivalPos() >= destParkArea->getBeginLanePosition()
7274 24 : && getArrivalPos() <= destParkArea->getEndLanePosition());
7275 :
7276 : // retrieve info on the new parking area
7277 16 : MSParkingArea* newParkingArea = (MSParkingArea*) MSNet::getInstance()->getStoppingPlace(
7278 : parkingAreaID, SumoXMLTag::SUMO_TAG_PARKING_AREA);
7279 :
7280 16 : if (newParkingArea == nullptr) {
7281 0 : errorMsg = "Parking area ID " + toString(parkingAreaID) + " not found in the network.";
7282 0 : return false;
7283 : }
7284 :
7285 16 : const MSEdge* newEdge = &(newParkingArea->getLane().getEdge());
7286 16 : SUMOAbstractRouter<MSEdge, SUMOVehicle>& router = getRouterTT();
7287 :
7288 : // Compute the route from the current edge to the parking area edge
7289 : ConstMSEdgeVector edgesToPark;
7290 16 : router.compute(getEdge(), getPositionOnLane(), newEdge, newParkingArea->getEndLanePosition(), this, MSNet::getInstance()->getCurrentTimeStep(), edgesToPark);
7291 :
7292 : // Compute the route from the parking area edge to the end of the route
7293 : ConstMSEdgeVector edgesFromPark;
7294 16 : if (!newDestination) {
7295 12 : router.compute(newEdge, lastEdge, this, MSNet::getInstance()->getCurrentTimeStep(), edgesFromPark);
7296 : } else {
7297 : // adapt plans of any riders
7298 8 : for (MSTransportable* p : getPersons()) {
7299 4 : p->rerouteParkingArea(getNextParkingArea(), newParkingArea);
7300 : }
7301 : }
7302 :
7303 : // we have a new destination, let's replace the vehicle route
7304 16 : ConstMSEdgeVector edges = edgesToPark;
7305 16 : if (edgesFromPark.size() > 0) {
7306 12 : edges.insert(edges.end(), edgesFromPark.begin() + 1, edgesFromPark.end());
7307 : }
7308 :
7309 16 : if (newDestination && getParameter().arrivalPosProcedure != ArrivalPosDefinition::DEFAULT) {
7310 4 : SUMOVehicleParameter* newParameter = new SUMOVehicleParameter();
7311 4 : *newParameter = getParameter();
7312 4 : newParameter->arrivalPosProcedure = ArrivalPosDefinition::GIVEN;
7313 4 : newParameter->arrivalPos = newParkingArea->getEndLanePosition();
7314 4 : replaceParameter(newParameter);
7315 : }
7316 16 : const double routeCost = router.recomputeCosts(edges, this, MSNet::getInstance()->getCurrentTimeStep());
7317 16 : ConstMSEdgeVector prevEdges(myCurrEdge, myRoute->end());
7318 16 : const double savings = router.recomputeCosts(prevEdges, this, MSNet::getInstance()->getCurrentTimeStep());
7319 16 : if (replaceParkingArea(newParkingArea, errorMsg)) {
7320 16 : const bool onInit = myLane == nullptr;
7321 32 : replaceRouteEdges(edges, routeCost, savings, "TraCI:" + toString(SUMO_TAG_PARKING_AREA_REROUTE), onInit, false, false);
7322 : } else {
7323 0 : WRITE_WARNING("Vehicle '" + getID() + "' could not reroute to new parkingArea '" + newParkingArea->getID()
7324 : + "' reason=" + errorMsg + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
7325 0 : return false;
7326 : }
7327 16 : return true;
7328 16 : }
7329 :
7330 :
7331 : bool
7332 45833 : MSVehicle::addTraciStop(SUMOVehicleParameter::Stop stop, std::string& errorMsg) {
7333 45833 : const int numStops = (int)myStops.size();
7334 45833 : const bool result = MSBaseVehicle::addTraciStop(stop, errorMsg);
7335 45833 : if (myLane != nullptr && numStops != (int)myStops.size()) {
7336 44263 : updateBestLanes(true);
7337 : }
7338 45833 : return result;
7339 : }
7340 :
7341 :
7342 : bool
7343 3130 : MSVehicle::handleCollisionStop(MSStop& stop, const double distToStop) {
7344 3130 : if (myCurrEdge == stop.edge && distToStop + POSITION_EPS < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0)) {
7345 1431 : if (distToStop < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)) {
7346 1001 : double vNew = getCarFollowModel().maximumSafeStopSpeed(distToStop, getCarFollowModel().getMaxDecel(), getSpeed(), false, 0);
7347 : //std::cout << SIMTIME << " veh=" << getID() << " v=" << myState.mySpeed << " distToStop=" << distToStop
7348 : // << " vMinNex=" << getCarFollowModel().minNextSpeed(getSpeed(), this)
7349 : // << " bg1=" << getCarFollowModel().brakeGap(myState.mySpeed)
7350 : // << " bg2=" << getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)
7351 : // << " vNew=" << vNew
7352 : // << "\n";
7353 1001 : myState.mySpeed = MIN2(myState.mySpeed, vNew + ACCEL2SPEED(getCarFollowModel().getEmergencyDecel()));
7354 1001 : myState.myPos = MIN2(myState.myPos, stop.pars.endPos);
7355 1001 : myCachedPosition = Position::INVALID;
7356 1001 : if (myState.myPos < myType->getLength()) {
7357 418 : computeFurtherLanes(myLane, myState.myPos, true);
7358 418 : myAngle = computeAngle();
7359 418 : if (myLaneChangeModel->isOpposite()) {
7360 0 : myAngle += M_PI;
7361 : }
7362 : }
7363 : }
7364 : }
7365 3130 : return true;
7366 : }
7367 :
7368 :
7369 : bool
7370 25303924 : MSVehicle::resumeFromStopping() {
7371 25303924 : if (isStopped()) {
7372 48089 : if (myAmRegisteredAsWaiting) {
7373 768 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
7374 768 : myAmRegisteredAsWaiting = false;
7375 : }
7376 : MSStop& stop = myStops.front();
7377 : // we have waited long enough and fulfilled any passenger-requirements
7378 48089 : if (stop.busstop != nullptr) {
7379 : // inform bus stop about leaving it
7380 18072 : stop.busstop->leaveFrom(this);
7381 : }
7382 : // we have waited long enough and fulfilled any container-requirements
7383 48089 : if (stop.containerstop != nullptr) {
7384 : // inform container stop about leaving it
7385 530 : stop.containerstop->leaveFrom(this);
7386 : }
7387 48089 : if (stop.parkingarea != nullptr && stop.getSpeed() <= 0) {
7388 : // inform parking area about leaving it
7389 8348 : stop.parkingarea->leaveFrom(this);
7390 : }
7391 48089 : if (stop.chargingStation != nullptr) {
7392 : // inform charging station about leaving it
7393 3528 : stop.chargingStation->leaveFrom(this);
7394 : }
7395 : // the current stop is no longer valid
7396 48089 : myLane->getEdge().removeWaiting(this);
7397 : // MSStopOut needs to know whether the stop had a loaded 'ended' value so we call this before replacing the value
7398 48089 : if (stop.pars.started == -1) {
7399 : // waypoint edge was passed in a single step
7400 326 : stop.pars.started = MSNet::getInstance()->getCurrentTimeStep();
7401 : }
7402 48089 : if (MSStopOut::active()) {
7403 3966 : MSStopOut::getInstance()->stopEnded(this, stop.pars, stop.lane->getID());
7404 : }
7405 48089 : stop.pars.ended = MSNet::getInstance()->getCurrentTimeStep();
7406 108474 : for (const auto& rem : myMoveReminders) {
7407 60385 : rem.first->notifyStopEnded();
7408 : }
7409 48089 : if (stop.pars.collision && MSLane::getCollisionAction() == MSLane::COLLISION_ACTION_WARN) {
7410 425 : myCollisionImmunity = TIME2STEPS(5); // leave the conflict area
7411 : }
7412 48089 : if (stop.pars.posLat != INVALID_DOUBLE && MSGlobals::gLateralResolution <= 0) {
7413 : // reset lateral position to default
7414 194 : myState.myPosLat = 0;
7415 : }
7416 48089 : const bool wasWaypoint = stop.getSpeed() > 0;
7417 48089 : myPastStops.push_back(stop.pars);
7418 48089 : myPastStops.back().routeIndex = (int)(stop.edge - myRoute->begin());
7419 48089 : myStops.pop_front();
7420 48089 : myStopDist = std::numeric_limits<double>::max();
7421 : // do not count the stopping time towards gridlock time.
7422 : // Other outputs use an independent counter and are not affected.
7423 48089 : myWaitingTime = 0;
7424 48089 : myStopSpeed = getCarFollowModel().maxNextSpeed(getSpeed(), this);
7425 : // maybe the next stop is on the same edge; let's rebuild best lanes
7426 48089 : updateBestLanes(true);
7427 : // continue as wished...
7428 48089 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::ENDING_STOP);
7429 48089 : MSNet::getInstance()->getVehicleControl().registerStopEnded();
7430 48089 : return !wasWaypoint;
7431 : }
7432 : return false;
7433 : }
7434 :
7435 :
7436 : MSVehicle::Influencer&
7437 4563132 : MSVehicle::getInfluencer() {
7438 4563132 : if (myInfluencer == nullptr) {
7439 3483 : myInfluencer = new Influencer();
7440 : }
7441 4563132 : return *myInfluencer;
7442 : }
7443 :
7444 : MSVehicle::BaseInfluencer&
7445 24 : MSVehicle::getBaseInfluencer() {
7446 24 : return getInfluencer();
7447 : }
7448 :
7449 :
7450 : const MSVehicle::Influencer*
7451 0 : MSVehicle::getInfluencer() const {
7452 0 : return myInfluencer;
7453 : }
7454 :
7455 : const MSVehicle::BaseInfluencer*
7456 234560 : MSVehicle::getBaseInfluencer() const {
7457 234560 : return myInfluencer;
7458 : }
7459 :
7460 :
7461 : double
7462 4080 : MSVehicle::getSpeedWithoutTraciInfluence() const {
7463 4080 : if (myInfluencer != nullptr && myInfluencer->getOriginalSpeed() >= 0) {
7464 : // influencer original speed is -1 on initialization
7465 1656 : return myInfluencer->getOriginalSpeed();
7466 : }
7467 2424 : return myState.mySpeed;
7468 : }
7469 :
7470 :
7471 : int
7472 989595316 : MSVehicle::influenceChangeDecision(int state) {
7473 989595316 : if (hasInfluencer()) {
7474 2827156 : state = getInfluencer().influenceChangeDecision(
7475 : MSNet::getInstance()->getCurrentTimeStep(),
7476 2827156 : myLane->getEdge(),
7477 : getLaneIndex(),
7478 : state);
7479 : }
7480 989595316 : return state;
7481 : }
7482 :
7483 :
7484 : void
7485 6768 : MSVehicle::setRemoteState(Position xyPos) {
7486 6768 : myCachedPosition = xyPos;
7487 6768 : }
7488 :
7489 :
7490 : bool
7491 795727896 : MSVehicle::isRemoteControlled() const {
7492 795727896 : return myInfluencer != nullptr && myInfluencer->isRemoteControlled();
7493 : }
7494 :
7495 :
7496 : bool
7497 20995 : MSVehicle::wasRemoteControlled(SUMOTime lookBack) const {
7498 20995 : return myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() + lookBack >= MSNet::getInstance()->getCurrentTimeStep();
7499 : }
7500 :
7501 :
7502 : bool
7503 519084326 : MSVehicle::keepClear(const MSLink* link) const {
7504 519084326 : if (link->hasFoes() && link->keepClear() /* && item.myLink->willHaveBlockedFoe()*/) {
7505 165601978 : const double keepClearTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_KEEPCLEAR_TIME, -1);
7506 : //std::cout << SIMTIME << " veh=" << getID() << " keepClearTime=" << keepClearTime << " accWait=" << getAccumulatedWaitingSeconds() << " keepClear=" << (keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime) << "\n";
7507 167024327 : return keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime;
7508 : } else {
7509 : return false;
7510 : }
7511 : }
7512 :
7513 :
7514 : bool
7515 724979929 : MSVehicle::ignoreRed(const MSLink* link, bool canBrake) const {
7516 724979929 : if ((myInfluencer != nullptr && !myInfluencer->getEmergencyBrakeRedLight())) {
7517 : return true;
7518 : }
7519 724672164 : const double ignoreRedTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
7520 : #ifdef DEBUG_IGNORE_RED
7521 : if (DEBUG_COND) {
7522 : std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID() << " state=" << toString(link->getState()) << "\n";
7523 : }
7524 : #endif
7525 724672164 : if (ignoreRedTime < 0) {
7526 724666765 : const double ignoreYellowTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, 0);
7527 724666765 : if (ignoreYellowTime > 0 && link->haveYellow()) {
7528 : assert(link->getTLLogic() != 0);
7529 52 : const double yellowDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
7530 : // when activating ignoreYellow behavior, vehicles will drive if they cannot brake
7531 92 : return !canBrake || ignoreYellowTime > yellowDuration;
7532 : } else {
7533 : return false;
7534 : }
7535 5399 : } else if (link->haveYellow()) {
7536 : // always drive at yellow when ignoring red
7537 : return true;
7538 5243 : } else if (link->haveRed()) {
7539 : assert(link->getTLLogic() != 0);
7540 3832 : const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
7541 : #ifdef DEBUG_IGNORE_RED
7542 : if (DEBUG_COND) {
7543 : std::cout
7544 : // << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID()
7545 : << " ignoreRedTime=" << ignoreRedTime
7546 : << " spentRed=" << redDuration
7547 : << " canBrake=" << canBrake << "\n";
7548 : }
7549 : #endif
7550 : // when activating ignoreRed behavior, vehicles will always drive if they cannot brake
7551 6356 : return !canBrake || ignoreRedTime > redDuration;
7552 : } else {
7553 : return false;
7554 : }
7555 : }
7556 :
7557 : bool
7558 1308512517 : MSVehicle::ignoreFoe(const SUMOTrafficObject* foe) const {
7559 1308512517 : if (!getParameter().wasSet(VEHPARS_CFMODEL_PARAMS_SET)) {
7560 : return false;
7561 : }
7562 2768 : for (const std::string& typeID : StringTokenizer(getParameter().getParameter(toString(SUMO_ATTR_CF_IGNORE_TYPES), "")).getVector()) {
7563 398 : if (typeID == foe->getVehicleType().getID()) {
7564 : return true;
7565 : }
7566 1384 : }
7567 2381 : for (const std::string& id : StringTokenizer(getParameter().getParameter(toString(SUMO_ATTR_CF_IGNORE_IDS), "")).getVector()) {
7568 986 : if (id == foe->getID()) {
7569 : return true;
7570 : }
7571 986 : }
7572 409 : return false;
7573 : }
7574 :
7575 : bool
7576 553919141 : MSVehicle::passingMinor() const {
7577 : // either on an internal lane that was entered via minor link
7578 : // or on approach to minor link below visibility distance
7579 553919141 : if (myLane == nullptr) {
7580 : return false;
7581 : }
7582 553919141 : if (myLane->getEdge().isInternal()) {
7583 10707191 : return !myLane->getIncomingLanes().front().viaLink->havePriority();
7584 543211950 : } else if (myLFLinkLanes.size() > 0 && myLFLinkLanes.front().myLink != nullptr) {
7585 : MSLink* link = myLFLinkLanes.front().myLink;
7586 282503587 : return !link->havePriority() && myLFLinkLanes.front().myDistance <= link->getFoeVisibilityDistance();
7587 : }
7588 : return false;
7589 : }
7590 :
7591 : bool
7592 20709331 : MSVehicle::isLeader(const MSLink* link, const MSVehicle* veh, const double gap) const {
7593 : assert(link->fromInternalLane());
7594 20709331 : if (veh == nullptr) {
7595 : return false;
7596 : }
7597 20709331 : if (!myLane->isInternal() || myLane->getEdge().getToJunction() != link->getJunction()) {
7598 : // if this vehicle is not yet on the junction, every vehicle is a leader
7599 : return true;
7600 : }
7601 1890699 : if (veh->getLaneChangeModel().hasBlueLight()) {
7602 : // blue light device automatically gets right of way
7603 : return true;
7604 : }
7605 1890431 : const MSLane* foeLane = veh->getLane();
7606 1890431 : if (foeLane->isInternal()) {
7607 1403914 : if (foeLane->getEdge().getFromJunction() == link->getJunction()) {
7608 1387617 : SUMOTime egoET = myJunctionConflictEntryTime;
7609 1387617 : SUMOTime foeET = veh->myJunctionEntryTime;
7610 : // check relationship between link and foeLane
7611 1387617 : if (foeLane->getNormalPredecessorLane() == link->getInternalLaneBefore()->getNormalPredecessorLane()) {
7612 : // we are entering the junction from the same lane
7613 454520 : egoET = myJunctionEntryTimeNeverYield;
7614 454520 : foeET = veh->myJunctionEntryTimeNeverYield;
7615 454520 : if (link->isExitLinkAfterInternalJunction() && link->getInternalLaneBefore()->getLogicalPredecessorLane()->getEntryLink()->isIndirect()) {
7616 91633 : egoET = myJunctionConflictEntryTime;
7617 : }
7618 : } else {
7619 933097 : const MSLink* foeLink = foeLane->getIncomingLanes()[0].viaLink;
7620 933097 : const MSJunctionLogic* logic = link->getJunction()->getLogic();
7621 : assert(logic != nullptr);
7622 : // determine who has right of way
7623 : bool response; // ego response to foe
7624 : bool response2; // foe response to ego
7625 : // attempt 1: tlLinkState
7626 933097 : const MSLink* entry = link->getCorrespondingEntryLink();
7627 933097 : const MSLink* foeEntry = foeLink->getCorrespondingEntryLink();
7628 933097 : if (entry->haveRed() || foeEntry->haveRed()) {
7629 : // ensure that vehicles which are stuck on the intersection may exit
7630 115641 : if (!foeEntry->haveRed() && veh->getSpeed() > SUMO_const_haltingSpeed && gap < 0) {
7631 : // foe might be oncoming, don't drive unless foe can still brake safely
7632 11478 : const double foeNextSpeed = veh->getSpeed() + ACCEL2SPEED(veh->getCarFollowModel().getMaxAccel());
7633 11478 : const double foeBrakeGap = veh->getCarFollowModel().brakeGap(
7634 11478 : foeNextSpeed, veh->getCarFollowModel().getMaxDecel(), veh->getCarFollowModel().getHeadwayTime());
7635 : // the minGap was subtracted from gap in MSLink::getLeaderInfo (enlarging the negative gap)
7636 : // so the -2* makes it point in the right direction
7637 11478 : const double foeGap = -gap - veh->getLength() - 2 * getVehicleType().getMinGap();
7638 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7639 : if (DEBUG_COND) {
7640 : std::cout << " foeGap=" << foeGap << " foeBGap=" << foeBrakeGap << "\n";
7641 :
7642 : }
7643 : #endif
7644 11478 : if (foeGap < foeBrakeGap) {
7645 : response = true;
7646 : response2 = false;
7647 : } else {
7648 : response = false;
7649 : response2 = true;
7650 : }
7651 : } else {
7652 : // let conflict entry time decide
7653 : response = true;
7654 : response2 = true;
7655 : }
7656 817456 : } else if (entry->havePriority() != foeEntry->havePriority()) {
7657 604898 : response = !entry->havePriority();
7658 604898 : response2 = !foeEntry->havePriority();
7659 212558 : } else if (entry->haveYellow() && foeEntry->haveYellow()) {
7660 : // let the faster vehicle keep moving
7661 5200 : response = veh->getSpeed() >= getSpeed();
7662 5200 : response2 = getSpeed() >= veh->getSpeed();
7663 : } else {
7664 : // fallback if pedestrian crossings are involved
7665 207358 : response = logic->getResponseFor(link->getIndex()).test(foeLink->getIndex());
7666 207358 : response2 = logic->getResponseFor(foeLink->getIndex()).test(link->getIndex());
7667 : }
7668 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7669 : if (DEBUG_COND) {
7670 : std::cout << SIMTIME
7671 : << " foeLane=" << foeLane->getID()
7672 : << " foeLink=" << foeLink->getViaLaneOrLane()->getID()
7673 : << " linkIndex=" << link->getIndex()
7674 : << " foeLinkIndex=" << foeLink->getIndex()
7675 : << " entryState=" << toString(entry->getState())
7676 : << " entryState2=" << toString(foeEntry->getState())
7677 : << " response=" << response
7678 : << " response2=" << response2
7679 : << "\n";
7680 : }
7681 : #endif
7682 933097 : if (!response) {
7683 : // if we have right of way over the foe, entryTime does not matter
7684 77686 : foeET = veh->myJunctionConflictEntryTime;
7685 77686 : egoET = myJunctionEntryTime;
7686 855411 : } else if (response && response2) {
7687 : // in a mutual conflict scenario, use entry time to avoid deadlock
7688 127551 : foeET = veh->myJunctionConflictEntryTime;
7689 127551 : egoET = myJunctionConflictEntryTime;
7690 : }
7691 : }
7692 1387617 : if (egoET == foeET) {
7693 : // try to use speed as tie braker
7694 106309 : if (getSpeed() == veh->getSpeed()) {
7695 : // use ID as tie braker
7696 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7697 : if (DEBUG_COND) {
7698 : std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
7699 : << " foeIsLeaderByID=" << (getID() < veh->getID()) << "\n";
7700 : }
7701 : #endif
7702 57092 : return getID() < veh->getID();
7703 : } else {
7704 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7705 : if (DEBUG_COND) {
7706 : std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
7707 : << " foeIsLeaderBySpeed=" << (getSpeed() < veh->getSpeed())
7708 : << " v=" << getSpeed() << " foeV=" << veh->getSpeed()
7709 : << "\n";
7710 : }
7711 : #endif
7712 49217 : return getSpeed() < veh->getSpeed();
7713 : }
7714 : } else {
7715 : // leader was on the junction first
7716 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7717 : if (DEBUG_COND) {
7718 : std::cout << SIMTIME << " veh=" << getID() << " egoET " << egoET << " with foe " << veh->getID()
7719 : << " foeET=" << foeET << " isLeader=" << (egoET > foeET) << "\n";
7720 : }
7721 : #endif
7722 1281308 : return egoET > foeET;
7723 : }
7724 : } else {
7725 : // vehicle can only be partially on the junction. Must be a leader
7726 : return true;
7727 : }
7728 : } else {
7729 : // vehicle can only be partially on the junction. Must be a leader
7730 : return true;
7731 : }
7732 : }
7733 :
7734 : void
7735 2456 : MSVehicle::saveState(OutputDevice& out) {
7736 2456 : MSBaseVehicle::saveState(out);
7737 : // here starts the vehicle internal part (see loading)
7738 : std::vector<std::string> internals;
7739 2456 : internals.push_back(toString(myParameter->parametersSet));
7740 2456 : internals.push_back(toString(myDeparture));
7741 2456 : internals.push_back(toString(distance(myRoute->begin(), myCurrEdge)));
7742 2456 : internals.push_back(toString(myDepartPos));
7743 2456 : internals.push_back(toString(myWaitingTime));
7744 2456 : internals.push_back(toString(myTimeLoss));
7745 2456 : internals.push_back(toString(myLastActionTime));
7746 2456 : internals.push_back(toString(isStopped()));
7747 2456 : internals.push_back(toString(myPastStops.size()));
7748 2456 : out.writeAttr(SUMO_ATTR_STATE, internals);
7749 2456 : out.writeAttr(SUMO_ATTR_POSITION, std::vector<double> { myState.myPos, myState.myBackPos, myState.myLastCoveredDist });
7750 2456 : out.writeAttr(SUMO_ATTR_SPEED, std::vector<double> { myState.mySpeed, myState.myPreviousSpeed });
7751 2456 : out.writeAttr(SUMO_ATTR_ANGLE, GeomHelper::naviDegree(myAngle));
7752 2456 : out.writeAttr(SUMO_ATTR_POSITION_LAT, myState.myPosLat);
7753 2456 : out.writeAttr(SUMO_ATTR_WAITINGTIME, myWaitingTimeCollector.getState());
7754 2456 : myLaneChangeModel->saveState(out);
7755 : // save past stops
7756 5408 : for (SUMOVehicleParameter::Stop stop : myPastStops) {
7757 2952 : stop.write(out, false);
7758 : // do not write started and ended twice
7759 2952 : if ((stop.parametersSet & STOP_STARTED_SET) == 0) {
7760 2947 : out.writeAttr(SUMO_ATTR_STARTED, time2string(stop.started));
7761 : }
7762 2952 : if ((stop.parametersSet & STOP_ENDED_SET) == 0) {
7763 2947 : out.writeAttr(SUMO_ATTR_ENDED, time2string(stop.ended));
7764 : }
7765 2952 : stop.writeParams(out);
7766 2952 : out.closeTag();
7767 2952 : }
7768 : // save upcoming stops
7769 2926 : for (MSStop& stop : myStops) {
7770 470 : stop.write(out);
7771 : }
7772 : // save parameters and device states
7773 2456 : myParameter->writeParams(out);
7774 6223 : for (MSVehicleDevice* const dev : myDevices) {
7775 3767 : dev->saveState(out);
7776 : }
7777 2456 : out.closeTag();
7778 2456 : }
7779 :
7780 : void
7781 3539 : MSVehicle::loadState(const SUMOSAXAttributes& attrs, const SUMOTime offset) {
7782 3539 : if (!attrs.hasAttribute(SUMO_ATTR_POSITION)) {
7783 0 : throw ProcessError(TL("Error: Invalid vehicles in state (may be a meso state)!"));
7784 : }
7785 : int routeOffset;
7786 : bool stopped;
7787 : int pastStops;
7788 :
7789 3539 : std::istringstream bis(attrs.getString(SUMO_ATTR_STATE));
7790 3539 : bis >> myParameter->parametersSet;
7791 3539 : bis >> myDeparture;
7792 3539 : bis >> routeOffset;
7793 3539 : bis >> myDepartPos;
7794 3539 : bis >> myWaitingTime;
7795 3539 : bis >> myTimeLoss;
7796 3539 : bis >> myLastActionTime;
7797 : bis >> stopped;
7798 3539 : bis >> pastStops;
7799 :
7800 3539 : if (attrs.hasAttribute(SUMO_ATTR_ARRIVALPOS_RANDOMIZED)) {
7801 : bool ok;
7802 4 : myArrivalPos = attrs.get<double>(SUMO_ATTR_ARRIVALPOS_RANDOMIZED, getID().c_str(), ok);
7803 : }
7804 : // load stops
7805 : myStops.clear();
7806 3539 : addStops(!MSGlobals::gCheckRoutes, &myCurrEdge, false);
7807 :
7808 3539 : if (hasDeparted()) {
7809 1541 : myCurrEdge = myRoute->begin() + routeOffset;
7810 1541 : myDeparture -= offset;
7811 : // fix stops
7812 4467 : while (pastStops > 0) {
7813 2926 : myPastStops.push_back(myStops.front().pars);
7814 2926 : myPastStops.back().routeIndex = (int)(myStops.front().edge - myRoute->begin());
7815 2926 : myStops.pop_front();
7816 2926 : pastStops--;
7817 : }
7818 : // see MSBaseVehicle constructor
7819 1541 : if (myParameter->wasSet(VEHPARS_FORCE_REROUTE)) {
7820 1155 : calculateArrivalParams(true);
7821 : }
7822 : // a (tentative lane is needed for calling hasArrivedInternal
7823 1541 : myLane = (*myCurrEdge)->getLanes()[0];
7824 : }
7825 3539 : if (getActionStepLength() == DELTA_T && !isActionStep(SIMSTEP)) {
7826 1 : myLastActionTime -= (myLastActionTime - SIMSTEP) % DELTA_T;
7827 3 : WRITE_WARNINGF(TL("Action steps are out of sync for loaded vehicle '%'."), getID());
7828 : }
7829 3539 : std::istringstream pis(attrs.getString(SUMO_ATTR_POSITION));
7830 3539 : pis >> myState.myPos >> myState.myBackPos >> myState.myLastCoveredDist;
7831 3539 : std::istringstream sis(attrs.getString(SUMO_ATTR_SPEED));
7832 3539 : sis >> myState.mySpeed >> myState.myPreviousSpeed;
7833 3539 : myAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
7834 3539 : myAngle = GeomHelper::fromNaviDegree(attrs.getFloat(SUMO_ATTR_ANGLE));
7835 3539 : myState.myPosLat = attrs.getFloat(SUMO_ATTR_POSITION_LAT);
7836 3539 : std::istringstream dis(attrs.getString(SUMO_ATTR_DISTANCE));
7837 3539 : dis >> myOdometer >> myNumberReroutes;
7838 3539 : myWaitingTimeCollector.setState(attrs.getString(SUMO_ATTR_WAITINGTIME));
7839 3539 : if (stopped) {
7840 209 : myStops.front().startedFromState = true;
7841 209 : myStopDist = 0;
7842 : }
7843 3539 : myLaneChangeModel->loadState(attrs);
7844 : // no need to reset myCachedPosition here since state loading happens directly after creation
7845 3539 : }
7846 :
7847 : void
7848 27 : MSVehicle::loadPreviousApproaching(MSLink* link, bool setRequest,
7849 : SUMOTime arrivalTime, double arrivalSpeed,
7850 : double arrivalSpeedBraking,
7851 : double dist, double leaveSpeed) {
7852 : // ensure that approach information is reset on the next call to setApproachingForAllLinks
7853 27 : myLFLinkLanes.push_back(DriveProcessItem(link, 0, 0, setRequest,
7854 : arrivalTime, arrivalSpeed, arrivalSpeedBraking, dist, leaveSpeed));
7855 :
7856 27 : }
7857 :
7858 :
7859 : std::shared_ptr<MSSimpleDriverState>
7860 2567594 : MSVehicle::getDriverState() const {
7861 2567594 : return myDriverState->getDriverState();
7862 : }
7863 :
7864 :
7865 : double
7866 626710259 : MSVehicle::getFriction() const {
7867 626710259 : return myFrictionDevice == nullptr ? 1. : myFrictionDevice->getMeasuredFriction();
7868 : }
7869 :
7870 :
7871 : void
7872 196 : MSVehicle::setPreviousSpeed(double prevSpeed, double prevAcceleration) {
7873 196 : myState.mySpeed = MAX2(0., prevSpeed);
7874 : // also retcon acceleration
7875 196 : if (prevAcceleration != std::numeric_limits<double>::min()) {
7876 8 : myAcceleration = prevAcceleration;
7877 : } else {
7878 188 : myAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
7879 : }
7880 196 : }
7881 :
7882 :
7883 : double
7884 1863880822 : MSVehicle::getCurrentApparentDecel() const {
7885 : //return MAX2(-myAcceleration, getCarFollowModel().getApparentDecel());
7886 1863880822 : return getCarFollowModel().getApparentDecel();
7887 : }
7888 :
7889 : /****************************************************************************/
7890 : bool
7891 32 : MSVehicle::setExitManoeuvre() {
7892 32 : return (myManoeuvre.configureExitManoeuvre(this));
7893 : }
7894 :
7895 : /* -------------------------------------------------------------------------
7896 : * methods of MSVehicle::manoeuvre
7897 : * ----------------------------------------------------------------------- */
7898 :
7899 4794096 : MSVehicle::Manoeuvre::Manoeuvre() : myManoeuvreStop(""), myManoeuvreStartTime(0), myManoeuvreCompleteTime(0), myManoeuvreType(MSVehicle::MANOEUVRE_NONE), myGUIIncrement(0) {}
7900 :
7901 :
7902 0 : MSVehicle::Manoeuvre::Manoeuvre(const Manoeuvre& manoeuvre) {
7903 0 : myManoeuvreStop = manoeuvre.myManoeuvreStop;
7904 0 : myManoeuvreStartTime = manoeuvre.myManoeuvreStartTime;
7905 0 : myManoeuvreCompleteTime = manoeuvre.myManoeuvreCompleteTime;
7906 0 : myManoeuvreType = manoeuvre.myManoeuvreType;
7907 0 : myGUIIncrement = manoeuvre.myGUIIncrement;
7908 0 : }
7909 :
7910 :
7911 : MSVehicle::Manoeuvre&
7912 0 : MSVehicle::Manoeuvre::operator=(const Manoeuvre& manoeuvre) {
7913 0 : myManoeuvreStop = manoeuvre.myManoeuvreStop;
7914 0 : myManoeuvreStartTime = manoeuvre.myManoeuvreStartTime;
7915 0 : myManoeuvreCompleteTime = manoeuvre.myManoeuvreCompleteTime;
7916 0 : myManoeuvreType = manoeuvre.myManoeuvreType;
7917 0 : myGUIIncrement = manoeuvre.myGUIIncrement;
7918 0 : return *this;
7919 : }
7920 :
7921 :
7922 : bool
7923 0 : MSVehicle::Manoeuvre::operator!=(const Manoeuvre& manoeuvre) {
7924 0 : return (myManoeuvreStop != manoeuvre.myManoeuvreStop ||
7925 0 : myManoeuvreStartTime != manoeuvre.myManoeuvreStartTime ||
7926 0 : myManoeuvreCompleteTime != manoeuvre.myManoeuvreCompleteTime ||
7927 0 : myManoeuvreType != manoeuvre.myManoeuvreType ||
7928 0 : myGUIIncrement != manoeuvre.myGUIIncrement
7929 0 : );
7930 : }
7931 :
7932 :
7933 : double
7934 450 : MSVehicle::Manoeuvre::getGUIIncrement() const {
7935 450 : return (myGUIIncrement);
7936 : }
7937 :
7938 :
7939 : MSVehicle::ManoeuvreType
7940 2971 : MSVehicle::Manoeuvre::getManoeuvreType() const {
7941 2971 : return (myManoeuvreType);
7942 : }
7943 :
7944 :
7945 : MSVehicle::ManoeuvreType
7946 2971 : MSVehicle::getManoeuvreType() const {
7947 2971 : return (myManoeuvre.getManoeuvreType());
7948 : }
7949 :
7950 :
7951 : void
7952 30 : MSVehicle::setManoeuvreType(const MSVehicle::ManoeuvreType mType) {
7953 30 : myManoeuvre.setManoeuvreType(mType);
7954 30 : }
7955 :
7956 :
7957 : void
7958 30 : MSVehicle::Manoeuvre::setManoeuvreType(const MSVehicle::ManoeuvreType mType) {
7959 30 : myManoeuvreType = mType;
7960 30 : }
7961 :
7962 :
7963 : bool
7964 30 : MSVehicle::Manoeuvre::configureEntryManoeuvre(MSVehicle* veh) {
7965 30 : if (!veh->hasStops()) {
7966 : return false; // should never happen - checked before call
7967 : }
7968 :
7969 30 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
7970 30 : const MSStop& stop = veh->getNextStop();
7971 :
7972 30 : int manoeuverAngle = stop.parkingarea->getLastFreeLotAngle();
7973 30 : double GUIAngle = stop.parkingarea->getLastFreeLotGUIAngle();
7974 30 : if (abs(GUIAngle) < 0.1) {
7975 : GUIAngle = -0.1; // Wiggle vehicle on parallel entry
7976 : }
7977 30 : myManoeuvreVehicleID = veh->getID();
7978 30 : myManoeuvreStop = stop.parkingarea->getID();
7979 30 : myManoeuvreType = MSVehicle::MANOEUVRE_ENTRY;
7980 30 : myManoeuvreStartTime = currentTime;
7981 30 : myManoeuvreCompleteTime = currentTime + veh->myType->getEntryManoeuvreTime(manoeuverAngle);
7982 30 : myGUIIncrement = GUIAngle / (STEPS2TIME(myManoeuvreCompleteTime - myManoeuvreStartTime) / TS);
7983 :
7984 : #ifdef DEBUG_STOPS
7985 : if (veh->isSelected()) {
7986 : std::cout << "ENTRY manoeuvre start: vehicle=" << veh->getID() << " Manoeuvre Angle=" << manoeuverAngle << " Rotation angle=" << RAD2DEG(GUIAngle) << " Road Angle" << RAD2DEG(veh->getAngle()) << " increment=" << RAD2DEG(myGUIIncrement) << " currentTime=" << currentTime <<
7987 : " endTime=" << myManoeuvreCompleteTime << " manoeuvre time=" << myManoeuvreCompleteTime - currentTime << " parkArea=" << myManoeuvreStop << std::endl;
7988 : }
7989 : #endif
7990 :
7991 30 : return (true);
7992 : }
7993 :
7994 :
7995 : bool
7996 32 : MSVehicle::Manoeuvre::configureExitManoeuvre(MSVehicle* veh) {
7997 : // At the moment we only want to set for parking areas
7998 32 : if (!veh->hasStops()) {
7999 : return true;
8000 : }
8001 32 : if (veh->getNextStop().parkingarea == nullptr) {
8002 : return true;
8003 : }
8004 :
8005 30 : if (myManoeuvreType != MSVehicle::MANOEUVRE_NONE) {
8006 : return (false);
8007 : }
8008 :
8009 30 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
8010 :
8011 30 : int manoeuverAngle = veh->getCurrentParkingArea()->getManoeuverAngle(*veh);
8012 30 : double GUIAngle = veh->getCurrentParkingArea()->getGUIAngle(*veh);
8013 30 : if (abs(GUIAngle) < 0.1) {
8014 : GUIAngle = 0.1; // Wiggle vehicle on parallel exit
8015 : }
8016 :
8017 30 : myManoeuvreVehicleID = veh->getID();
8018 30 : myManoeuvreStop = veh->getCurrentParkingArea()->getID();
8019 30 : myManoeuvreType = MSVehicle::MANOEUVRE_EXIT;
8020 30 : myManoeuvreStartTime = currentTime;
8021 30 : myManoeuvreCompleteTime = currentTime + veh->myType->getExitManoeuvreTime(manoeuverAngle);
8022 30 : myGUIIncrement = -GUIAngle / (STEPS2TIME(myManoeuvreCompleteTime - myManoeuvreStartTime) / TS);
8023 30 : if (veh->remainingStopDuration() > 0) {
8024 20 : myManoeuvreCompleteTime += veh->remainingStopDuration();
8025 : }
8026 :
8027 : #ifdef DEBUG_STOPS
8028 : if (veh->isSelected()) {
8029 : std::cout << "EXIT manoeuvre start: vehicle=" << veh->getID() << " Manoeuvre Angle=" << manoeuverAngle << " increment=" << RAD2DEG(myGUIIncrement) << " currentTime=" << currentTime
8030 : << " endTime=" << myManoeuvreCompleteTime << " manoeuvre time=" << myManoeuvreCompleteTime - currentTime << " parkArea=" << myManoeuvreStop << std::endl;
8031 : }
8032 : #endif
8033 :
8034 : return (true);
8035 : }
8036 :
8037 :
8038 : bool
8039 222 : MSVehicle::Manoeuvre::entryManoeuvreIsComplete(MSVehicle* veh) {
8040 : // At the moment we only want to consider parking areas - need to check because we could be setting up a manoeuvre
8041 222 : if (!veh->hasStops()) {
8042 : return (true);
8043 : }
8044 : MSStop* currentStop = &veh->myStops.front();
8045 222 : if (currentStop->parkingarea == nullptr) {
8046 : return true;
8047 220 : } else if (currentStop->parkingarea->getID() != myManoeuvreStop || MSVehicle::MANOEUVRE_ENTRY != myManoeuvreType) {
8048 30 : if (configureEntryManoeuvre(veh)) {
8049 30 : MSNet::getInstance()->informVehicleStateListener(veh, MSNet::VehicleState::MANEUVERING);
8050 30 : return (false);
8051 : } else { // cannot configure entry so stop trying
8052 : return true;
8053 : }
8054 190 : } else if (MSNet::getInstance()->getCurrentTimeStep() < myManoeuvreCompleteTime) {
8055 : return false;
8056 : } else { // manoeuvre complete
8057 30 : myManoeuvreType = MSVehicle::MANOEUVRE_NONE;
8058 30 : return true;
8059 : }
8060 : }
8061 :
8062 :
8063 : bool
8064 0 : MSVehicle::Manoeuvre::manoeuvreIsComplete(const ManoeuvreType checkType) const {
8065 0 : if (checkType != myManoeuvreType) {
8066 : return true; // we're not maneuvering / wrong manoeuvre
8067 : }
8068 :
8069 0 : if (MSNet::getInstance()->getCurrentTimeStep() < myManoeuvreCompleteTime) {
8070 : return false;
8071 : } else {
8072 : return true;
8073 : }
8074 : }
8075 :
8076 :
8077 : bool
8078 6266 : MSVehicle::Manoeuvre::manoeuvreIsComplete() const {
8079 6266 : return (MSNet::getInstance()->getCurrentTimeStep() >= myManoeuvreCompleteTime);
8080 : }
8081 :
8082 :
8083 : bool
8084 6266 : MSVehicle::manoeuvreIsComplete() const {
8085 6266 : return (myManoeuvre.manoeuvreIsComplete());
8086 : }
8087 :
8088 :
8089 : std::pair<double, double>
8090 7378 : MSVehicle::estimateTimeToNextStop() const {
8091 7378 : if (hasStops()) {
8092 7378 : MSLane* lane = myLane;
8093 7378 : if (lane == nullptr) {
8094 : // not in network
8095 84 : lane = getEdge()->getLanes()[0];
8096 : }
8097 : const MSStop& stop = myStops.front();
8098 : auto it = myCurrEdge + 1;
8099 : // drive to end of current edge
8100 7378 : double dist = (lane->getLength() - getPositionOnLane());
8101 7378 : double travelTime = lane->getEdge().getMinimumTravelTime(this) * dist / lane->getLength();
8102 : // drive until stop edge
8103 8734 : while (it != myRoute->end() && it < stop.edge) {
8104 1356 : travelTime += (*it)->getMinimumTravelTime(this);
8105 1356 : dist += (*it)->getLength();
8106 : it++;
8107 : }
8108 : // drive up to the stop position
8109 7378 : const double stopEdgeDist = stop.pars.endPos - (lane == stop.lane ? lane->getLength() : 0);
8110 7378 : dist += stopEdgeDist;
8111 7378 : travelTime += stop.lane->getEdge().getMinimumTravelTime(this) * (stopEdgeDist / stop.lane->getLength());
8112 : // estimate time loss due to acceleration and deceleration
8113 : // maximum speed is limited by available distance:
8114 : const double a = getCarFollowModel().getMaxAccel();
8115 : const double b = getCarFollowModel().getMaxDecel();
8116 7378 : const double c = getSpeed();
8117 : const double d = dist;
8118 7378 : const double len = getVehicleType().getLength();
8119 7378 : const double vs = MIN2(MAX2(stop.getSpeed(), 0.0), stop.lane->getVehicleMaxSpeed(this));
8120 : // distAccel = (v - c)^2 / (2a)
8121 : // distDecel = (v + vs)*(v - vs) / 2b = (v^2 - vs^2) / (2b)
8122 : // distAccel + distDecel < d
8123 7378 : const double maxVD = MAX2(c, ((sqrt(MAX2(0.0, pow(2 * c * b, 2) + (4 * ((b * ((a * (2 * d * (b + a) + (vs * vs) - (c * c))) - (b * (c * c))))
8124 14468 : + pow((a * vs), 2))))) * 0.5) + (c * b)) / (b + a));
8125 7378 : it = myCurrEdge;
8126 : double v0 = c;
8127 7378 : bool v0Stable = getAcceleration() == 0 && v0 > 0;
8128 : double timeLossAccel = 0;
8129 : double timeLossDecel = 0;
8130 : double timeLossLength = 0;
8131 17548 : while (it != myRoute->end() && it <= stop.edge) {
8132 10170 : double v = MIN2(maxVD, (*it)->getVehicleMaxSpeed(this));
8133 10170 : double edgeLength = (it == stop.edge ? stop.pars.endPos : (*it)->getLength()) - (it == myCurrEdge ? getPositionOnLane() : 0);
8134 10170 : if (edgeLength <= len && v0Stable && v0 < v) {
8135 : const double lengthDist = MIN2(len, edgeLength);
8136 20 : const double dTL = lengthDist / v0 - lengthDist / v;
8137 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " el=" << edgeLength << " lDist=" << lengthDist << " newTLL=" << dTL<< "\n";
8138 20 : timeLossLength += dTL;
8139 : }
8140 10170 : if (edgeLength > len) {
8141 9034 : const double dv = v - v0;
8142 9034 : if (dv > 0) {
8143 : // timeLossAccel = timeAccel - timeMaxspeed = dv / a - distAccel / v
8144 6476 : const double dTA = dv / a - dv * (v + v0) / (2 * a * v);
8145 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLA=" << dTA << "\n";
8146 6476 : timeLossAccel += dTA;
8147 : // time loss from vehicle length
8148 2558 : } else if (dv < 0) {
8149 : // timeLossDecel = timeDecel - timeMaxspeed = dv / b - distDecel / v
8150 540 : const double dTD = -dv / b + dv * (v + v0) / (2 * b * v0);
8151 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLD=" << dTD << "\n";
8152 540 : timeLossDecel += dTD;
8153 : }
8154 : v0 = v;
8155 : v0Stable = true;
8156 : }
8157 : it++;
8158 : }
8159 : // final deceleration to stop (may also be acceleration or deceleration to waypoint speed)
8160 : double v = vs;
8161 7378 : const double dv = v - v0;
8162 7378 : if (dv > 0) {
8163 : // timeLossAccel = timeAccel - timeMaxspeed = dv / a - distAccel / v
8164 144 : const double dTA = dv / a - dv * (v + v0) / (2 * a * v);
8165 : //std::cout << " final e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLA=" << dTA << "\n";
8166 144 : timeLossAccel += dTA;
8167 : // time loss from vehicle length
8168 7234 : } else if (dv < 0) {
8169 : // timeLossDecel = timeDecel - timeMaxspeed = dv / b - distDecel / v
8170 7206 : const double dTD = -dv / b + dv * (v + v0) / (2 * b * v0);
8171 : //std::cout << " final e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLD=" << dTD << "\n";
8172 7206 : timeLossDecel += dTD;
8173 : }
8174 7378 : const double result = travelTime + timeLossAccel + timeLossDecel + timeLossLength;
8175 : //std::cout << SIMTIME << " v=" << c << " a=" << a << " b=" << b << " maxVD=" << maxVD << " tt=" << travelTime
8176 : // << " ta=" << timeLossAccel << " td=" << timeLossDecel << " tl=" << timeLossLength << " res=" << result << "\n";
8177 7378 : return {MAX2(0.0, result), dist};
8178 : } else {
8179 0 : return {INVALID_DOUBLE, INVALID_DOUBLE};
8180 : }
8181 : }
8182 :
8183 :
8184 : double
8185 2459 : MSVehicle::getStopDelay() const {
8186 2459 : if (hasStops() && myStops.front().pars.until >= 0) {
8187 : const MSStop& stop = myStops.front();
8188 1612 : SUMOTime estimatedDepart = MSNet::getInstance()->getCurrentTimeStep() - DELTA_T;
8189 1612 : if (stop.reached) {
8190 802 : return STEPS2TIME(estimatedDepart + stop.duration - stop.pars.until);
8191 : }
8192 810 : if (stop.pars.duration > 0) {
8193 608 : estimatedDepart += stop.pars.duration;
8194 : }
8195 810 : estimatedDepart += TIME2STEPS(estimateTimeToNextStop().first);
8196 810 : const double result = MAX2(0.0, STEPS2TIME(estimatedDepart - stop.pars.until));
8197 810 : return result;
8198 : } else {
8199 : // vehicles cannot drive before 'until' so stop delay can never be
8200 : // negative and we can use -1 to signal "undefined"
8201 : return -1;
8202 : }
8203 : }
8204 :
8205 :
8206 : double
8207 5450 : MSVehicle::getStopArrivalDelay() const {
8208 5450 : if (hasStops() && myStops.front().pars.arrival >= 0) {
8209 : const MSStop& stop = myStops.front();
8210 4272 : if (stop.reached) {
8211 1304 : return STEPS2TIME(stop.pars.started - stop.pars.arrival);
8212 : } else {
8213 2968 : return STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) + estimateTimeToNextStop().first - STEPS2TIME(stop.pars.arrival);
8214 : }
8215 : } else {
8216 : // vehicles can arrival earlier than planned so arrival delay can be negative
8217 : return INVALID_DOUBLE;
8218 : }
8219 : }
8220 :
8221 :
8222 : const MSEdge*
8223 3098798686 : MSVehicle::getCurrentEdge() const {
8224 3098798686 : return myLane != nullptr ? &myLane->getEdge() : getEdge();
8225 : }
8226 :
8227 :
8228 : const MSEdge*
8229 3932 : MSVehicle::getNextEdgePtr() const {
8230 3932 : if (myLane == nullptr || (myCurrEdge + 1) == myRoute->end()) {
8231 8 : return nullptr;
8232 : }
8233 3924 : if (myLane->isInternal()) {
8234 568 : return &myLane->getCanonicalSuccessorLane()->getEdge();
8235 : } else {
8236 3356 : const MSEdge* nextNormal = succEdge(1);
8237 3356 : const MSEdge* nextInternal = myLane->getEdge().getInternalFollowingEdge(nextNormal, getVClass());
8238 3356 : return nextInternal ? nextInternal : nextNormal;
8239 : }
8240 : }
8241 :
8242 :
8243 : const MSLane*
8244 1596 : MSVehicle::getPreviousLane(const MSLane* current, int& furtherIndex) const {
8245 1596 : if (furtherIndex < (int)myFurtherLanes.size()) {
8246 1219 : return myFurtherLanes[furtherIndex++];
8247 : } else {
8248 : // try to use route information
8249 377 : int routeIndex = getRoutePosition();
8250 : bool resultInternal;
8251 377 : if (MSGlobals::gUsingInternalLanes && MSNet::getInstance()->hasInternalLinks()) {
8252 0 : if (myLane->isInternal()) {
8253 0 : if (furtherIndex % 2 == 0) {
8254 0 : routeIndex -= (furtherIndex + 0) / 2;
8255 : resultInternal = false;
8256 : } else {
8257 0 : routeIndex -= (furtherIndex + 1) / 2;
8258 : resultInternal = false;
8259 : }
8260 : } else {
8261 0 : if (furtherIndex % 2 != 0) {
8262 0 : routeIndex -= (furtherIndex + 1) / 2;
8263 : resultInternal = false;
8264 : } else {
8265 0 : routeIndex -= (furtherIndex + 2) / 2;
8266 : resultInternal = true;
8267 : }
8268 : }
8269 : } else {
8270 377 : routeIndex -= furtherIndex;
8271 : resultInternal = false;
8272 : }
8273 377 : furtherIndex++;
8274 377 : if (routeIndex >= 0) {
8275 163 : if (resultInternal) {
8276 0 : const MSEdge* prevNormal = myRoute->getEdges()[routeIndex];
8277 0 : for (MSLane* cand : prevNormal->getLanes()) {
8278 0 : for (MSLink* link : cand->getLinkCont()) {
8279 0 : if (link->getLane() == current) {
8280 0 : if (link->getViaLane() != nullptr) {
8281 : return link->getViaLane();
8282 : } else {
8283 0 : return const_cast<MSLane*>(link->getLaneBefore());
8284 : }
8285 : }
8286 : }
8287 : }
8288 : } else {
8289 163 : return myRoute->getEdges()[routeIndex]->getLanes()[0];
8290 : }
8291 : }
8292 : }
8293 : return current;
8294 : }
8295 :
8296 : SUMOTime
8297 1519756977 : MSVehicle::getWaitingTimeFor(const MSLink* link) const {
8298 : // this vehicle currently has the highest priority on the allway_stop
8299 1519756977 : return link == myHaveStoppedFor ? SUMOTime_MAX : getWaitingTime();
8300 : }
8301 :
8302 :
8303 : void
8304 652 : MSVehicle::resetApproachOnReroute() {
8305 : bool diverged = false;
8306 : const ConstMSEdgeVector& route = myRoute->getEdges();
8307 652 : int ri = getRoutePosition();
8308 2884 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
8309 2232 : if (dpi.myLink != nullptr) {
8310 2229 : if (!diverged) {
8311 1996 : const MSEdge* next = route[ri + 1];
8312 1996 : if (&dpi.myLink->getLane()->getEdge() != next) {
8313 : diverged = true;
8314 : } else {
8315 1930 : if (dpi.myLink->getViaLane() == nullptr) {
8316 : ri++;
8317 : }
8318 : }
8319 : }
8320 : if (diverged) {
8321 299 : dpi.myLink->removeApproaching(this);
8322 : }
8323 : }
8324 : }
8325 652 : }
8326 :
8327 :
8328 : bool
8329 11018341 : MSVehicle::instantStopping() const {
8330 11018341 : return myInfluencer && !myInfluencer->considerMaxDeceleration();
8331 : }
8332 :
8333 : /****************************************************************************/
|
