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 3569988 : MSVehicle::State::operator=(const State& state) {
153 3569988 : myPos = state.myPos;
154 3569988 : mySpeed = state.mySpeed;
155 3569988 : myPosLat = state.myPosLat;
156 3569988 : myBackPos = state.myBackPos;
157 3569988 : myPreviousSpeed = state.myPreviousSpeed;
158 3569988 : myLastCoveredDist = state.myLastCoveredDist;
159 3569988 : 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 8130678 : MSVehicle::State::State(double pos, double speed, double posLat, double backPos, double previousSpeed) :
175 8130678 : myPos(pos), mySpeed(speed), myPosLat(posLat), myBackPos(backPos), myPreviousSpeed(previousSpeed), myLastCoveredDist(SPEED2DIST(speed)) {}
176 :
177 :
178 :
179 : /* -------------------------------------------------------------------------
180 : * methods of MSVehicle::WaitingTimeCollector
181 : * ----------------------------------------------------------------------- */
182 4560690 : MSVehicle::WaitingTimeCollector::WaitingTimeCollector(SUMOTime memory) : myMemorySize(memory) {}
183 :
184 :
185 : SUMOTime
186 1428411 : MSVehicle::WaitingTimeCollector::cumulatedWaitingTime(SUMOTime memorySpan) const {
187 : assert(memorySpan <= myMemorySize);
188 1428411 : if (memorySpan == -1) {
189 0 : memorySpan = myMemorySize;
190 : }
191 : SUMOTime totalWaitingTime = 0;
192 5941986 : for (const auto& interval : myWaitingIntervals) {
193 4513575 : if (interval.second >= memorySpan) {
194 655960 : if (interval.first >= memorySpan) {
195 : break;
196 : } else {
197 655960 : totalWaitingTime += memorySpan - interval.first;
198 : }
199 : } else {
200 3857615 : totalWaitingTime += interval.second - interval.first;
201 : }
202 : }
203 1428411 : return totalWaitingTime;
204 : }
205 :
206 :
207 : void
208 705173132 : MSVehicle::WaitingTimeCollector::passTime(SUMOTime dt, bool waiting) {
209 : auto i = myWaitingIntervals.begin();
210 : const auto end = myWaitingIntervals.end();
211 705173132 : const bool startNewInterval = i == end || (i->first != 0);
212 1161766725 : while (i != end) {
213 458968979 : i->first += dt;
214 458968979 : if (i->first >= myMemorySize) {
215 : break;
216 : }
217 456593593 : i->second += dt;
218 : i++;
219 : }
220 :
221 : // remove intervals beyond memorySize
222 : auto d = std::distance(i, end);
223 707548518 : while (d > 0) {
224 2375386 : myWaitingIntervals.pop_back();
225 2375386 : d--;
226 : }
227 :
228 705173132 : if (!waiting) {
229 : return;
230 95554829 : } else if (!startNewInterval) {
231 91772754 : myWaitingIntervals.begin()->first = 0;
232 : } else {
233 7564150 : myWaitingIntervals.push_front(std::make_pair(0, dt));
234 : }
235 : return;
236 : }
237 :
238 :
239 : const std::string
240 2624 : MSVehicle::WaitingTimeCollector::getState() const {
241 2624 : std::ostringstream state;
242 2624 : state << myMemorySize << " " << myWaitingIntervals.size();
243 3544 : for (const auto& interval : myWaitingIntervals) {
244 1840 : state << " " << interval.first << " " << interval.second;
245 : }
246 2624 : return state.str();
247 2624 : }
248 :
249 :
250 : void
251 3523 : MSVehicle::WaitingTimeCollector::setState(const std::string& state) {
252 3523 : std::istringstream is(state);
253 : int numIntervals;
254 : SUMOTime begin, end;
255 3523 : is >> myMemorySize >> numIntervals;
256 5273 : while (numIntervals-- > 0) {
257 : is >> begin >> end;
258 1750 : myWaitingIntervals.emplace_back(begin, end);
259 : }
260 3523 : }
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 35390 : MSVehicle::Influencer::GapControlState::cleanup() {
319 35390 : if (myVehStateListener != nullptr) {
320 58 : MSNet::getInstance()->removeVehicleStateListener(myVehStateListener);
321 58 : delete myVehStateListener;
322 58 : myVehStateListener = nullptr;
323 : }
324 35390 : }
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 3491 : MSVehicle::Influencer::Influencer() :
371 : myGapControlState(nullptr),
372 3491 : myOriginalSpeed(-1),
373 3491 : myLatDist(0),
374 3491 : mySpeedAdaptationStarted(true),
375 3491 : myConsiderSafeVelocity(true),
376 3491 : myConsiderSpeedLimit(true),
377 3491 : myConsiderMaxAcceleration(true),
378 3491 : myConsiderMaxDeceleration(true),
379 3491 : myRespectJunctionPriority(true),
380 3491 : myEmergencyBrakeRedLight(true),
381 3491 : myRespectJunctionLeaderPriority(true),
382 3491 : myLastRemoteAccess(-TIME2STEPS(20)),
383 3491 : myStrategicLC(LC_NOCONFLICT),
384 3491 : myCooperativeLC(LC_NOCONFLICT),
385 3491 : mySpeedGainLC(LC_NOCONFLICT),
386 3491 : myRightDriveLC(LC_NOCONFLICT),
387 3491 : mySublaneLC(LC_NOCONFLICT),
388 3491 : myTraciLaneChangePriority(LCP_URGENT),
389 3491 : myTraCISignals(-1)
390 3491 : {}
391 :
392 :
393 10473 : MSVehicle::Influencer::~Influencer() {}
394 :
395 : void
396 58 : MSVehicle::Influencer::init() {
397 58 : GapControlState::init();
398 58 : }
399 :
400 : void
401 35390 : MSVehicle::Influencer::cleanup() {
402 35390 : GapControlState::cleanup();
403 35390 : }
404 :
405 : void
406 43291 : MSVehicle::Influencer::setSpeedTimeLine(const std::vector<std::pair<SUMOTime, double> >& speedTimeLine) {
407 43291 : mySpeedAdaptationStarted = true;
408 43291 : mySpeedTimeLine = speedTimeLine;
409 43291 : }
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 7611 : MSVehicle::Influencer::setLaneTimeLine(const std::vector<std::pair<SUMOTime, int> >& laneTimeLine) {
429 7611 : myLaneTimeLine = laneTimeLine;
430 7611 : }
431 :
432 :
433 : void
434 9053 : MSVehicle::Influencer::adaptLaneTimeLine(int indexShift) {
435 19215 : for (auto& item : myLaneTimeLine) {
436 10162 : item.second += indexShift;
437 : }
438 9053 : }
439 :
440 :
441 : void
442 1268 : MSVehicle::Influencer::setSublaneChange(double latDist) {
443 1268 : myLatDist = latDist;
444 1268 : }
445 :
446 : int
447 68 : MSVehicle::Influencer::getSpeedMode() const {
448 68 : return (1 * myConsiderSafeVelocity +
449 68 : 2 * myConsiderMaxAcceleration +
450 68 : 4 * myConsiderMaxDeceleration +
451 68 : 8 * myRespectJunctionPriority +
452 68 : 16 * myEmergencyBrakeRedLight +
453 68 : 32 * !myRespectJunctionLeaderPriority + // inverted!
454 68 : 64 * !myConsiderSpeedLimit // inverted!
455 68 : );
456 : }
457 :
458 :
459 : int
460 1469 : MSVehicle::Influencer::getLaneChangeMode() const {
461 1469 : return (1 * myStrategicLC +
462 1469 : 4 * myCooperativeLC +
463 1469 : 16 * mySpeedGainLC +
464 1469 : 64 * myRightDriveLC +
465 1469 : 256 * myTraciLaneChangePriority +
466 1469 : 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 991174 : MSVehicle::Influencer::influenceSpeed(SUMOTime currentTime, double speed, double vSafe, double vMin, double vMax) {
494 : // remove leading commands which are no longer valid
495 992570 : while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
496 : mySpeedTimeLine.erase(mySpeedTimeLine.begin());
497 : }
498 :
499 991174 : if (!(mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first)) {
500 : // Speed advice is active -> compute new speed according to speedTimeLine
501 53559 : if (!mySpeedAdaptationStarted) {
502 0 : mySpeedTimeLine[0].second = speed;
503 0 : mySpeedAdaptationStarted = true;
504 : }
505 53559 : 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 106767 : const double td = MIN2(1.0, STEPS2TIME(currentTime - mySpeedTimeLine[0].first) / MAX2(TS, STEPS2TIME(mySpeedTimeLine[1].first - mySpeedTimeLine[0].first)));
507 :
508 53559 : speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
509 53559 : if (myConsiderSafeVelocity) {
510 : speed = MIN2(speed, vSafe);
511 : }
512 53559 : if (myConsiderMaxAcceleration) {
513 : speed = MIN2(speed, vMax);
514 : }
515 53559 : if (myConsiderMaxDeceleration) {
516 : speed = MAX2(speed, vMin);
517 : }
518 : }
519 991174 : return speed;
520 : }
521 :
522 : double
523 492912 : 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 492912 : 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 7135 : MSVehicle::Influencer::getOriginalSpeed() const {
677 7135 : return myOriginalSpeed;
678 : }
679 :
680 : void
681 498262 : MSVehicle::Influencer::setOriginalSpeed(double speed) {
682 498262 : myOriginalSpeed = speed;
683 498262 : }
684 :
685 :
686 : int
687 2827546 : MSVehicle::Influencer::influenceChangeDecision(const SUMOTime currentTime, const MSEdge& currentEdge, const int currentLaneIndex, int state) {
688 : // remove leading commands which are no longer valid
689 2827798 : 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 2827546 : 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 2827546 : if ((state & LCA_WANTS_LANECHANGE_OR_STAY) != 0) {
711 : // flags for the current reason
712 : LaneChangeMode mode = LC_NEVER;
713 1601445 : 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 1599065 : } else if ((state & LCA_STRATEGIC) != 0) {
722 485563 : mode = myStrategicLC;
723 1113502 : } else if ((state & LCA_COOPERATIVE) != 0) {
724 32 : mode = myCooperativeLC;
725 1113470 : } else if ((state & LCA_SPEEDGAIN) != 0) {
726 42168 : mode = mySpeedGainLC;
727 1071302 : } else if ((state & LCA_KEEPRIGHT) != 0) {
728 6062 : mode = myRightDriveLC;
729 1065240 : } else if ((state & LCA_SUBLANE) != 0) {
730 1065238 : 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 1599063 : if (mode == LC_NEVER) {
737 : // cancel all lcModel requests
738 : state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
739 42249 : state &= ~LCA_URGENT;
740 42249 : if (changeRequest == REQUEST_NONE) {
741 : // also remove all reasons except TRACI
742 41782 : state &= ~LCA_CHANGE_REASONS | LCA_TRACI;
743 : }
744 1556816 : } 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 1551193 : } else if (mode == LC_ALWAYS) {
754 : // ignore any TraCI requests
755 : return state;
756 : }
757 : }
758 : // apply traci requests
759 2819965 : if (changeRequest == REQUEST_NONE) {
760 2652121 : 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 5622 : MSVehicle::Influencer::setSpeedMode(int speedMode) {
795 5622 : myConsiderSafeVelocity = ((speedMode & 1) != 0);
796 5622 : myConsiderMaxAcceleration = ((speedMode & 2) != 0);
797 5622 : myConsiderMaxDeceleration = ((speedMode & 4) != 0);
798 5622 : myRespectJunctionPriority = ((speedMode & 8) != 0);
799 5622 : myEmergencyBrakeRedLight = ((speedMode & 16) != 0);
800 5622 : myRespectJunctionLeaderPriority = ((speedMode & 32) == 0); // inverted!
801 5622 : myConsiderSpeedLimit = ((speedMode & 64) == 0); // inverted!
802 5622 : }
803 :
804 :
805 : void
806 18657 : MSVehicle::Influencer::setLaneChangeMode(int value) {
807 18657 : myStrategicLC = (LaneChangeMode)(value & (1 + 2));
808 18657 : myCooperativeLC = (LaneChangeMode)((value & (4 + 8)) >> 2);
809 18657 : mySpeedGainLC = (LaneChangeMode)((value & (16 + 32)) >> 4);
810 18657 : myRightDriveLC = (LaneChangeMode)((value & (64 + 128)) >> 6);
811 18657 : myTraciLaneChangePriority = (TraciLaneChangePriority)((value & (256 + 512)) >> 8);
812 18657 : mySublaneLC = (LaneChangeMode)((value & (1024 + 2048)) >> 10);
813 18657 : }
814 :
815 :
816 : void
817 7350 : MSVehicle::Influencer::setRemoteControlled(Position xyPos, MSLane* l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector& route, SUMOTime t) {
818 7350 : myRemoteXYPos = xyPos;
819 7350 : myRemoteLane = l;
820 7350 : myRemotePos = pos;
821 7350 : myRemotePosLat = posLat;
822 7350 : myRemoteAngle = angle;
823 7350 : myRemoteEdgeOffset = edgeOffset;
824 7350 : myRemoteRoute = route;
825 7350 : myLastRemoteAccess = t;
826 7350 : }
827 :
828 :
829 : bool
830 1020153 : MSVehicle::Influencer::isRemoteControlled() const {
831 1020153 : return myLastRemoteAccess == MSNet::getInstance()->getCurrentTimeStep();
832 : }
833 :
834 :
835 : bool
836 483915 : MSVehicle::Influencer::isRemoteAffected(SUMOTime t) const {
837 483915 : return myLastRemoteAccess >= t - TIME2STEPS(10);
838 : }
839 :
840 :
841 : void
842 492912 : MSVehicle::Influencer::updateRemoteControlRoute(MSVehicle* v) {
843 492912 : if (myRemoteRoute.size() != 0 && myRemoteRoute != v->getRoute().getEdges()) {
844 : // only replace route at this time if the vehicle is moving with the flow
845 78 : 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 12 : v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
851 12 : v->updateBestLanes();
852 : }
853 : }
854 492912 : }
855 :
856 :
857 : void
858 7330 : MSVehicle::Influencer::postProcessRemoteControl(MSVehicle* v) {
859 7330 : const bool wasOnRoad = v->isOnRoad();
860 7330 : const bool withinLane = myRemoteLane != nullptr && fabs(myRemotePosLat) < 0.5 * (myRemoteLane->getWidth() + v->getVehicleType().getWidth());
861 7330 : const bool keepLane = wasOnRoad && v->getLane() == myRemoteLane;
862 7330 : if (v->isOnRoad() && !(keepLane && withinLane)) {
863 149 : if (myRemoteLane != nullptr && &v->getLane()->getEdge() == &myRemoteLane->getEdge()) {
864 : // correct odometer which gets incremented via onRemovalFromNet->leaveLane
865 60 : v->myOdometer -= v->getLane()->getLength();
866 : }
867 149 : v->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT);
868 149 : v->getMutableLane()->removeVehicle(v, MSMoveReminder::NOTIFICATION_TELEPORT, false);
869 : }
870 7330 : 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 78 : const_cast<SUMOVehicleParameter&>(v->getParameter()).stops.clear();
883 156 : v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
884 : myRemoteRoute.clear();
885 : }
886 7330 : v->myCurrEdge = v->getRoute().begin() + myRemoteEdgeOffset;
887 7330 : if (myRemoteLane != nullptr && myRemotePos > myRemoteLane->getLength()) {
888 0 : myRemotePos = myRemoteLane->getLength();
889 : }
890 7330 : if (myRemoteLane != nullptr && withinLane) {
891 7188 : if (keepLane) {
892 : // TODO this handles only the case when the new vehicle is completely on the edge
893 7016 : const bool needFurtherUpdate = v->myState.myPos < v->getVehicleType().getLength() && myRemotePos >= v->getVehicleType().getLength();
894 7016 : v->myState.myPos = myRemotePos;
895 7016 : v->myState.myPosLat = myRemotePosLat;
896 7016 : if (needFurtherUpdate) {
897 4 : v->myState.myBackPos = v->updateFurtherLanes(v->myFurtherLanes, v->myFurtherLanesPosLat, std::vector<MSLane*>());
898 : }
899 : } else {
900 172 : MSMoveReminder::Notification notify = v->getDeparture() == NOT_YET_DEPARTED
901 172 : ? MSMoveReminder::NOTIFICATION_DEPARTED
902 : : MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED;
903 172 : if (!v->isOnRoad()) {
904 172 : MSVehicleTransfer::getInstance()->remove(v); // TODO may need optimization, this is linear in the number of vehicles in transfer
905 : }
906 172 : myRemoteLane->forceVehicleInsertion(v, myRemotePos, notify, myRemotePosLat);
907 172 : v->updateBestLanes();
908 : }
909 7188 : if (!wasOnRoad) {
910 49 : v->drawOutsideNetwork(false);
911 : }
912 : //std::cout << "on road network p=" << myRemoteXYPos << " a=" << myRemoteAngle << " l=" << Named::getIDSecure(myRemoteLane) << " pos=" << myRemotePos << " posLat=" << myRemotePosLat << "\n";
913 7188 : myRemoteLane->requireCollisionCheck();
914 : } else {
915 142 : if (v->getDeparture() == NOT_YET_DEPARTED) {
916 4 : v->onDepart();
917 : }
918 142 : v->drawOutsideNetwork(true);
919 : // see updateState
920 142 : double vNext = v->processTraCISpeedControl(
921 142 : v->getMaxSpeed(), v->getSpeed());
922 142 : v->setBrakingSignals(vNext);
923 142 : v->myState.myPreviousSpeed = v->getSpeed();
924 142 : v->myAcceleration = SPEED2ACCEL(vNext - v->getSpeed());
925 142 : v->myState.mySpeed = vNext;
926 142 : 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 7330 : v->setRemoteState(myRemoteXYPos);
931 7330 : v->setAngle(GeomHelper::fromNaviDegree(myRemoteAngle));
932 7330 : }
933 :
934 :
935 : double
936 7311 : MSVehicle::Influencer::implicitSpeedRemote(const MSVehicle* veh, double oldSpeed) {
937 7311 : if (veh->getPosition() == Position::INVALID) {
938 8 : return oldSpeed;
939 : }
940 7303 : double dist = veh->getPosition().distanceTo2D(myRemoteXYPos);
941 7303 : 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 7189 : const double distAlongRoute = veh->getDistanceToPosition(myRemotePos, myRemoteLane);
947 7189 : 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 7303 : const double minSpeed = myConsiderMaxDeceleration ?
953 4035 : veh->getCarFollowModel().minNextSpeedEmergency(oldSpeed, veh) : 0;
954 7303 : const double maxSpeed = (myRemoteLane != nullptr
955 7303 : ? myRemoteLane->getVehicleMaxSpeed(veh)
956 114 : : (veh->getLane() != nullptr
957 114 : ? veh->getLane()->getVehicleMaxSpeed(veh)
958 4 : : veh->getMaxSpeed()));
959 7303 : return MIN2(maxSpeed, MAX2(minSpeed, DIST2SPEED(dist)));
960 : }
961 :
962 :
963 : double
964 7177 : MSVehicle::Influencer::implicitDeltaPosRemote(const MSVehicle* veh) {
965 : double dist = 0;
966 7177 : 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 7172 : dist = veh->getDistanceToPosition(myRemotePos, myRemoteLane);
975 : }
976 7177 : if (dist == std::numeric_limits<double>::max()) {
977 : return 0;
978 : } else {
979 6957 : 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 6957 : return dist;
986 : }
987 : }
988 :
989 :
990 : /* -------------------------------------------------------------------------
991 : * MSVehicle-methods
992 : * ----------------------------------------------------------------------- */
993 4560690 : MSVehicle::MSVehicle(SUMOVehicleParameter* pars, ConstMSRoutePtr route,
994 4560690 : MSVehicleType* type, const double speedFactor) :
995 : MSBaseVehicle(pars, route, type, speedFactor),
996 4560690 : myWaitingTime(0),
997 4560690 : myWaitingTimeCollector(),
998 4560690 : myTimeLoss(0),
999 4560690 : myState(0, 0, 0, 0, 0),
1000 4560690 : myDriverState(nullptr),
1001 4560690 : myActionStep(true),
1002 4560690 : myLastActionTime(0),
1003 4560690 : myLane(nullptr),
1004 4560690 : myLaneChangeModel(nullptr),
1005 4560690 : myLastBestLanesEdge(nullptr),
1006 4560690 : myLastBestLanesInternalLane(nullptr),
1007 4560690 : myAcceleration(0),
1008 : myNextTurn(0., nullptr),
1009 4560690 : mySignals(0),
1010 4560690 : myAmOnNet(false),
1011 4560690 : myAmIdling(false),
1012 4560690 : myHaveToWaitOnNextLink(false),
1013 4560690 : myAngle(0),
1014 4560690 : myRawAngle(0),
1015 4560690 : myLastAngle(INVALID_DOUBLE),
1016 4560690 : myStopDist(std::numeric_limits<double>::max()),
1017 4560690 : myStopSpeed(std::numeric_limits<double>::max()),
1018 4560690 : myCollisionImmunity(-1),
1019 4560690 : myCachedPosition(Position::INVALID),
1020 4560690 : myJunctionEntryTime(SUMOTime_MAX),
1021 4560690 : myJunctionEntryTimeNeverYield(SUMOTime_MAX),
1022 4560690 : myJunctionConflictEntryTime(SUMOTime_MAX),
1023 4560690 : myTimeSinceStartup(TIME2STEPS(3600 * 24)),
1024 4560690 : myHaveStoppedFor(nullptr),
1025 13682070 : myInfluencer(nullptr) {
1026 4560690 : myCFVariables = type->getCarFollowModel().createVehicleVariables();
1027 4560690 : myNextDriveItem = myLFLinkLanes.begin();
1028 4560690 : }
1029 :
1030 :
1031 8462334 : MSVehicle::~MSVehicle() {
1032 4560609 : cleanupParkingReservation();
1033 4560609 : cleanupFurtherLanes();
1034 4560609 : delete myLaneChangeModel;
1035 4560609 : if (myType->isVehicleSpecific()) {
1036 314 : MSNet::getInstance()->getVehicleControl().removeVType(myType);
1037 : }
1038 4560609 : delete myInfluencer;
1039 4560609 : delete myCFVariables;
1040 13022943 : }
1041 :
1042 :
1043 : void
1044 5220053 : MSVehicle::cleanupFurtherLanes() {
1045 5222498 : for (MSLane* further : myFurtherLanes) {
1046 2445 : further->resetPartialOccupation(this);
1047 2445 : if (further->getBidiLane() != nullptr
1048 2445 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
1049 0 : further->getBidiLane()->resetPartialOccupation(this);
1050 : }
1051 : }
1052 5220053 : if (myLaneChangeModel != nullptr) {
1053 5220016 : removeApproachingInformation(myLFLinkLanes);
1054 5220016 : myLaneChangeModel->cleanupShadowLane();
1055 5220016 : myLaneChangeModel->cleanupTargetLane();
1056 : // still needed when calling resetPartialOccupation (getShadowLane) and when removing
1057 : // approach information from parallel links
1058 : }
1059 : myFurtherLanes.clear();
1060 : myFurtherLanesPosLat.clear();
1061 5220053 : }
1062 :
1063 :
1064 : void
1065 3395080 : MSVehicle::onRemovalFromNet(const MSMoveReminder::Notification reason) {
1066 : #ifdef DEBUG_ACTIONSTEPS
1067 : if (DEBUG_COND) {
1068 : std::cout << SIMTIME << " Removing vehicle '" << getID() << "' (reason: " << toString(reason) << ")" << std::endl;
1069 : }
1070 : #endif
1071 3395080 : MSVehicleTransfer::getInstance()->remove(this);
1072 3395080 : removeApproachingInformation(myLFLinkLanes);
1073 3395080 : leaveLane(reason);
1074 3395080 : if (reason == MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION) {
1075 562 : cleanupFurtherLanes();
1076 : }
1077 3395080 : }
1078 :
1079 :
1080 : void
1081 4560690 : MSVehicle::initDevices() {
1082 4560690 : MSBaseVehicle::initDevices();
1083 4560678 : myLaneChangeModel = MSAbstractLaneChangeModel::build(myType->getLaneChangeModel(), *this);
1084 4560656 : myDriverState = static_cast<MSDevice_DriverState*>(getDevice(typeid(MSDevice_DriverState)));
1085 4560656 : myFrictionDevice = static_cast<MSDevice_Friction*>(getDevice(typeid(MSDevice_Friction)));
1086 4560656 : }
1087 :
1088 :
1089 : // ------------ interaction with the route
1090 : bool
1091 2232159741 : MSVehicle::hasValidRouteStart(std::string& msg) {
1092 : // note: not a const method because getDepartLane may call updateBestLanes
1093 2232159741 : if (!(*myCurrEdge)->isTazConnector()) {
1094 2231831383 : if (myParameter->departLaneProcedure == DepartLaneDefinition::GIVEN
1095 2231831383 : || (myParameter->departLaneProcedure == DepartLaneDefinition::DEFAULT && MSEdge::getDefaultDepartLaneDefinition() == DepartLaneDefinition::GIVEN)) {
1096 60601902 : if ((*myCurrEdge)->getDepartLane(*this) == nullptr) {
1097 132 : msg = "Invalid departLane definition for vehicle '" + getID() + "'.";
1098 66 : if (myParameter->departLane >= (int)(*myCurrEdge)->getLanes().size()) {
1099 11 : myRouteValidity |= ROUTE_START_INVALID_LANE;
1100 : } else {
1101 55 : myRouteValidity |= ROUTE_START_INVALID_PERMISSIONS;
1102 : }
1103 66 : return false;
1104 : }
1105 : } else {
1106 2171229481 : if ((*myCurrEdge)->allowedLanes(getVClass(), ignoreTransientPermissions()) == nullptr) {
1107 144 : msg = "Vehicle '" + getID() + "' is not allowed to depart on any lane of edge '" + (*myCurrEdge)->getID() + "'.";
1108 72 : myRouteValidity |= ROUTE_START_INVALID_PERMISSIONS;
1109 72 : return false;
1110 : }
1111 : }
1112 2231831245 : if (myParameter->departSpeedProcedure == DepartSpeedDefinition::GIVEN && myParameter->departSpeed > myType->getMaxSpeed() + SPEED_EPS) {
1113 38 : msg = "Departure speed for vehicle '" + getID() + "' is too high for the vehicle type '" + myType->getID() + "'.";
1114 19 : myRouteValidity |= ROUTE_START_INVALID_LANE;
1115 19 : return false;
1116 : }
1117 : }
1118 2232159584 : myRouteValidity &= ~(ROUTE_START_INVALID_LANE | ROUTE_START_INVALID_PERMISSIONS);
1119 2232159584 : return true;
1120 : }
1121 :
1122 :
1123 : bool
1124 718539903 : MSVehicle::hasArrived() const {
1125 718539903 : return hasArrivedInternal(false);
1126 : }
1127 :
1128 :
1129 : bool
1130 1446487000 : MSVehicle::hasArrivedInternal(bool oppositeTransformed) const {
1131 2356852415 : return ((myCurrEdge == myRoute->end() - 1 || (myParameter->arrivalEdge >= 0 && getRoutePosition() >= myParameter->arrivalEdge))
1132 536170858 : && (myStops.empty() || myStops.front().edge != myCurrEdge || myStops.front().getSpeed() > 0)
1133 1002958168 : && ((myLaneChangeModel->isOpposite() && !oppositeTransformed) ? myLane->getLength() - myState.myPos : myState.myPos) > MIN2(myLane->getLength(), myArrivalPos) - POSITION_EPS
1134 1457976665 : && !isRemoteControlled());
1135 : }
1136 :
1137 :
1138 : bool
1139 1535339 : MSVehicle::replaceRoute(ConstMSRoutePtr newRoute, const std::string& info, bool onInit, int offset, bool addRouteStops, bool removeStops, std::string* msgReturn) {
1140 3070678 : if (MSBaseVehicle::replaceRoute(newRoute, info, onInit, offset, addRouteStops, removeStops, msgReturn)) {
1141 : // update best lanes (after stops were added)
1142 1535322 : myLastBestLanesEdge = nullptr;
1143 1535322 : myLastBestLanesInternalLane = nullptr;
1144 1535322 : updateBestLanes(true, onInit ? (*myCurrEdge)->getLanes().front() : 0);
1145 : assert(!removeStops || haveValidStopEdges());
1146 1535322 : if (myStops.size() == 0) {
1147 1491008 : myStopDist = std::numeric_limits<double>::max();
1148 : }
1149 1535322 : return true;
1150 : }
1151 : return false;
1152 : }
1153 :
1154 :
1155 : // ------------ Interaction with move reminders
1156 : void
1157 705326155 : MSVehicle::workOnMoveReminders(double oldPos, double newPos, double newSpeed) {
1158 : // This erasure-idiom works for all stl-sequence-containers
1159 : // See Meyers: Effective STL, Item 9
1160 1874413822 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
1161 : // XXX: calling notifyMove with newSpeed seems not the best choice. For the ballistic update, the average speed is calculated and used
1162 : // although a higher order quadrature-formula might be more adequate.
1163 : // For the euler case (where the speed is considered constant for each time step) it is conceivable that
1164 : // the current calculations may lead to systematic errors for large time steps (compared to reality). Refs. #2579
1165 2338175336 : if (!rem->first->notifyMove(*this, oldPos + rem->second, newPos + rem->second, MAX2(0., newSpeed))) {
1166 : #ifdef _DEBUG
1167 : if (myTraceMoveReminders) {
1168 : traceMoveReminder("notifyMove", rem->first, rem->second, false);
1169 : }
1170 : #endif
1171 : rem = myMoveReminders.erase(rem);
1172 : } else {
1173 : #ifdef _DEBUG
1174 : if (myTraceMoveReminders) {
1175 : traceMoveReminder("notifyMove", rem->first, rem->second, true);
1176 : }
1177 : #endif
1178 : ++rem;
1179 : }
1180 : }
1181 705326154 : if (myEnergyParams != nullptr) {
1182 : // TODO make the vehicle energy params a derived class which is a move reminder
1183 141628043 : myEnergyParams->setDynamicValues(isStopped() ? getNextStop().duration : -1, isParking(), getWaitingTime(), getAngleDiff());
1184 : }
1185 705326154 : }
1186 :
1187 :
1188 : void
1189 69299 : MSVehicle::workOnIdleReminders() {
1190 69299 : updateWaitingTime(0.); // cf issue 2233
1191 :
1192 : // vehicle move reminders
1193 83009 : for (const auto& rem : myMoveReminders) {
1194 13710 : rem.first->notifyIdle(*this);
1195 : }
1196 :
1197 : // lane move reminders - for aggregated values
1198 171238 : for (MSMoveReminder* rem : getLane()->getMoveReminders()) {
1199 101939 : rem->notifyIdle(*this);
1200 : }
1201 69299 : }
1202 :
1203 : // XXX: consider renaming...
1204 : void
1205 19711077 : MSVehicle::adaptLaneEntering2MoveReminder(const MSLane& enteredLane) {
1206 : // save the old work reminders, patching the position information
1207 : // add the information about the new offset to the old lane reminders
1208 19711077 : const double oldLaneLength = myLane->getLength();
1209 55840076 : for (auto& rem : myMoveReminders) {
1210 36128999 : rem.second += oldLaneLength;
1211 : #ifdef _DEBUG
1212 : // if (rem->first==0) std::cout << "Null reminder (?!)" << std::endl;
1213 : // std::cout << "Adapted MoveReminder on lane " << ((rem->first->getLane()==0) ? "NULL" : rem->first->getLane()->getID()) <<" position to " << rem->second << std::endl;
1214 : if (myTraceMoveReminders) {
1215 : traceMoveReminder("adaptedPos", rem.first, rem.second, true);
1216 : }
1217 : #endif
1218 : }
1219 33075811 : for (MSMoveReminder* const rem : enteredLane.getMoveReminders()) {
1220 13364734 : addReminder(rem);
1221 : }
1222 19711077 : }
1223 :
1224 :
1225 : // ------------ Other getter methods
1226 : double
1227 165039603 : MSVehicle::getSlope() const {
1228 165039603 : if (isParking() && getStops().begin()->parkingarea != nullptr) {
1229 3881 : return getStops().begin()->parkingarea->getVehicleSlope(*this);
1230 : }
1231 165035722 : if (myLane == nullptr) {
1232 : return 0;
1233 : }
1234 165035722 : if (MSGlobals::gSlopeCentered) {
1235 : MSLane* centerLane = myLane;
1236 248 : double centerPos = getPositionOnLane() - getLength() / 2;
1237 : int furtherIndex = 0;
1238 280 : while (centerPos < 0 && furtherIndex < (int)myFurtherLanes.size()) {
1239 32 : centerLane = myFurtherLanes[furtherIndex];
1240 32 : centerPos += centerLane->getLength();
1241 32 : furtherIndex++;
1242 : }
1243 248 : return centerLane->getShape().slopeDegreeAtOffset(centerLane->interpolateLanePosToGeometryPos(centerPos));
1244 : }
1245 165035474 : const double posLat = myState.myPosLat; // @todo get rid of the '-'
1246 165035474 : Position p1 = getPosition();
1247 165035474 : Position p2 = getBackPosition();
1248 : if (p2 == Position::INVALID) {
1249 : // Handle special case of vehicle's back reaching out of the network
1250 6 : if (myFurtherLanes.size() > 0) {
1251 6 : p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1252 : if (p2 == Position::INVALID) {
1253 : // unsuitable lane geometry
1254 0 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1255 : }
1256 : } else {
1257 0 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1258 : }
1259 : }
1260 165035474 : return (p1 != p2 ? RAD2DEG(p2.slopeTo2D(p1)) : myLane->getShape().slopeDegreeAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1261 : }
1262 :
1263 :
1264 : Position
1265 943453188 : MSVehicle::getPosition(const double offset) const {
1266 943453188 : if (myLane == nullptr) {
1267 : // when called in the context of GUI-Drawing, the simulation step is already incremented
1268 145 : if (myInfluencer != nullptr && myInfluencer->isRemoteAffected(MSNet::getInstance()->getCurrentTimeStep())) {
1269 40 : return myCachedPosition;
1270 : } else {
1271 105 : return Position::INVALID;
1272 : }
1273 : }
1274 943453043 : if (isParking()) {
1275 4136469 : if (myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() > getNextStopParameter()->started) {
1276 120 : return myCachedPosition;
1277 : }
1278 4136349 : if (myStops.begin()->parkingarea != nullptr) {
1279 24030 : return myStops.begin()->parkingarea->getVehiclePosition(*this);
1280 : } else {
1281 : // position beside the road
1282 4112319 : PositionVector shp = myLane->getEdge().getLanes()[0]->getShape();
1283 8224518 : shp.move2side(SUMO_const_laneWidth * (MSGlobals::gLefthand ? -1 : 1));
1284 4112319 : return shp.positionAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane() + offset));
1285 4112319 : }
1286 : }
1287 939316574 : const bool changingLanes = myLaneChangeModel->isChangingLanes();
1288 1868495627 : const double posLat = (MSGlobals::gLefthand ? 1 : -1) * getLateralPositionOnLane();
1289 939316574 : if (offset == 0. && !changingLanes) {
1290 : if (myCachedPosition == Position::INVALID) {
1291 709895807 : myCachedPosition = validatePosition(myLane->geometryPositionAtOffset(myState.myPos, posLat));
1292 709895807 : if (MSNet::getInstance()->hasElevation() && MSGlobals::gSublane) {
1293 61155 : interpolateLateralZ(myCachedPosition, myState.myPos, posLat);
1294 : }
1295 : }
1296 932602067 : return myCachedPosition;
1297 : }
1298 6714507 : Position result = validatePosition(myLane->geometryPositionAtOffset(getPositionOnLane() + offset, posLat), offset);
1299 6714507 : interpolateLateralZ(result, getPositionOnLane() + offset, posLat);
1300 6714507 : return result;
1301 : }
1302 :
1303 :
1304 : void
1305 7060946 : MSVehicle::interpolateLateralZ(Position& pos, double offset, double posLat) const {
1306 7060946 : const MSLane* shadow = myLaneChangeModel->getShadowLane();
1307 7060946 : if (shadow != nullptr && pos != Position::INVALID) {
1308 : // ignore negative offset
1309 : const Position shadowPos = shadow->geometryPositionAtOffset(MAX2(0.0, offset));
1310 59301 : if (shadowPos != Position::INVALID && pos.z() != shadowPos.z()) {
1311 320 : const double centerDist = (myLane->getWidth() + shadow->getWidth()) * 0.5;
1312 320 : double relOffset = fabs(posLat) / centerDist;
1313 320 : double newZ = (1 - relOffset) * pos.z() + relOffset * shadowPos.z();
1314 : pos.setz(newZ);
1315 : }
1316 : }
1317 7060946 : }
1318 :
1319 :
1320 : double
1321 296340 : MSVehicle::getDistanceToLeaveJunction() const {
1322 296340 : double result = getLength() - getPositionOnLane();
1323 296340 : if (myLane->isNormal()) {
1324 : return MAX2(0.0, result);
1325 : }
1326 2915 : const MSLane* lane = myLane;
1327 5830 : while (lane->isInternal()) {
1328 2915 : result += lane->getLength();
1329 2915 : lane = lane->getCanonicalSuccessorLane();
1330 : }
1331 : return result;
1332 : }
1333 :
1334 :
1335 : Position
1336 104680 : MSVehicle::getPositionAlongBestLanes(double offset) const {
1337 : assert(MSGlobals::gUsingInternalLanes);
1338 104680 : if (!isOnRoad()) {
1339 0 : return Position::INVALID;
1340 : }
1341 104680 : const std::vector<MSLane*>& bestLanes = getBestLanesContinuation();
1342 : auto nextBestLane = bestLanes.begin();
1343 104680 : const bool opposite = myLaneChangeModel->isOpposite();
1344 104680 : double pos = opposite ? myLane->getLength() - myState.myPos : myState.myPos;
1345 104680 : const MSLane* lane = opposite ? myLane->getParallelOpposite() : getLane();
1346 : assert(lane != 0);
1347 : bool success = true;
1348 :
1349 309135 : while (offset > 0) {
1350 : // take into account lengths along internal lanes
1351 312505 : while (lane->isInternal() && offset > 0) {
1352 108050 : if (offset > lane->getLength() - pos) {
1353 3561 : offset -= lane->getLength() - pos;
1354 3561 : lane = lane->getLinkCont()[0]->getViaLaneOrLane();
1355 : pos = 0.;
1356 3561 : if (lane == nullptr) {
1357 : success = false;
1358 : offset = 0.;
1359 : }
1360 : } else {
1361 104489 : pos += offset;
1362 : offset = 0;
1363 : }
1364 : }
1365 : // set nextBestLane to next non-internal lane
1366 209584 : while (nextBestLane != bestLanes.end() && *nextBestLane == nullptr) {
1367 : ++nextBestLane;
1368 : }
1369 204455 : if (offset > 0) {
1370 : assert(!lane->isInternal());
1371 : assert(lane == *nextBestLane);
1372 99966 : if (offset > lane->getLength() - pos) {
1373 99783 : offset -= lane->getLength() - pos;
1374 : ++nextBestLane;
1375 : assert(nextBestLane == bestLanes.end() || *nextBestLane != 0);
1376 99783 : if (nextBestLane == bestLanes.end()) {
1377 : success = false;
1378 : offset = 0.;
1379 : } else {
1380 99783 : const MSLink* link = lane->getLinkTo(*nextBestLane);
1381 : assert(link != nullptr);
1382 : lane = link->getViaLaneOrLane();
1383 : pos = 0.;
1384 : }
1385 : } else {
1386 183 : pos += offset;
1387 : offset = 0;
1388 : }
1389 : }
1390 :
1391 : }
1392 :
1393 104680 : if (success) {
1394 104680 : return lane->geometryPositionAtOffset(pos, -getLateralPositionOnLane());
1395 : } else {
1396 0 : return Position::INVALID;
1397 : }
1398 : }
1399 :
1400 :
1401 : double
1402 708624 : MSVehicle::getMaxSpeedOnLane() const {
1403 708624 : if (myLane != nullptr) {
1404 708624 : return myLane->getVehicleMaxSpeed(this);
1405 : }
1406 0 : return myType->getMaxSpeed();
1407 : }
1408 :
1409 :
1410 : Position
1411 716610314 : MSVehicle::validatePosition(Position result, double offset) const {
1412 : int furtherIndex = 0;
1413 716610314 : double lastLength = getPositionOnLane();
1414 716610314 : while (result == Position::INVALID) {
1415 343572 : if (furtherIndex >= (int)myFurtherLanes.size()) {
1416 : //WRITE_WARNINGF(TL("Could not compute position for vehicle '%', time=%."), getID(), time2string(MSNet::getInstance()->getCurrentTimeStep()));
1417 : break;
1418 : }
1419 : //std::cout << SIMTIME << " veh=" << getID() << " lane=" << myLane->getID() << " pos=" << getPositionOnLane() << " posLat=" << getLateralPositionOnLane() << " offset=" << offset << " result=" << result << " i=" << furtherIndex << " further=" << myFurtherLanes.size() << "\n";
1420 191416 : MSLane* further = myFurtherLanes[furtherIndex];
1421 191416 : offset += lastLength;
1422 191416 : result = further->geometryPositionAtOffset(further->getLength() + offset, -getLateralPositionOnLane());
1423 : lastLength = further->getLength();
1424 191416 : furtherIndex++;
1425 : //std::cout << SIMTIME << " newResult=" << result << "\n";
1426 : }
1427 716610314 : return result;
1428 : }
1429 :
1430 :
1431 : ConstMSEdgeVector::const_iterator
1432 283645 : MSVehicle::getRerouteOrigin() const {
1433 : // too close to the next junction, so avoid an emergency brake here
1434 283645 : if (myLane != nullptr && (myCurrEdge + 1) != myRoute->end() && !isRailway(getVClass())) {
1435 221105 : if (myLane->isInternal()) {
1436 : return myCurrEdge + 1;
1437 : }
1438 214038 : if (myState.myPos > myLane->getLength() - getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.)) {
1439 : return myCurrEdge + 1;
1440 : }
1441 211687 : if (myLane->getEdge().hasChangeProhibitions(getVClass(), myLane->getIndex())) {
1442 : return myCurrEdge + 1;
1443 : }
1444 : }
1445 274115 : return myCurrEdge;
1446 : }
1447 :
1448 :
1449 : double
1450 141628685 : MSVehicle::getAngleDiff() const {
1451 141628685 : return myLastAngle == INVALID_DOUBLE ? 0. : GeomHelper::angleDiff(myLastAngle, myAngle);
1452 : }
1453 :
1454 : double
1455 642 : MSVehicle::getCurveRadius() const {
1456 642 : const double angleDiff = getAngleDiff();
1457 : return angleDiff == 0
1458 642 : ? std::numeric_limits<double>::max()
1459 0 : : SPEED2DIST(getSpeed()) / fabs(angleDiff);
1460 : }
1461 :
1462 :
1463 : void
1464 5345009 : MSVehicle::setAngle(double angle, bool straightenFurther) {
1465 : #ifdef DEBUG_FURTHER
1466 : if (DEBUG_COND) {
1467 : std::cout << SIMTIME << " veh '" << getID() << " setAngle(" << angle << ") straightenFurther=" << straightenFurther << std::endl;
1468 : }
1469 : #endif
1470 5345009 : myAngle = angle;
1471 5345009 : MSLane* next = myLane;
1472 5345009 : if (straightenFurther && myFurtherLanesPosLat.size() > 0) {
1473 205454 : for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
1474 105539 : MSLane* further = myFurtherLanes[i];
1475 105539 : const MSLink* link = further->getLinkTo(next);
1476 105539 : if (link != nullptr) {
1477 105047 : myFurtherLanesPosLat[i] = getLateralPositionOnLane() - link->getLateralShift();
1478 : next = further;
1479 : } else {
1480 : break;
1481 : }
1482 : }
1483 : }
1484 5345009 : }
1485 :
1486 :
1487 : void
1488 451741 : MSVehicle::setActionStepLength(double actionStepLength, bool resetOffset) {
1489 451741 : SUMOTime actionStepLengthMillisecs = SUMOVehicleParserHelper::processActionStepLength(actionStepLength);
1490 : SUMOTime previousActionStepLength = getActionStepLength();
1491 : const bool newActionStepLength = actionStepLengthMillisecs != previousActionStepLength;
1492 451741 : if (newActionStepLength) {
1493 7 : getSingularType().setActionStepLength(actionStepLengthMillisecs, resetOffset);
1494 7 : if (!resetOffset) {
1495 1 : updateActionOffset(previousActionStepLength, actionStepLengthMillisecs);
1496 : }
1497 : }
1498 451735 : if (resetOffset) {
1499 6 : resetActionOffset();
1500 : }
1501 451741 : }
1502 :
1503 :
1504 : bool
1505 299536686 : MSVehicle::congested() const {
1506 299536686 : return myState.mySpeed < (60.0 / 3.6) || myLane->getSpeedLimit() < (60.1 / 3.6);
1507 : }
1508 :
1509 :
1510 : double
1511 712633743 : MSVehicle::computeAngle() const {
1512 : Position p1;
1513 712633743 : const double posLat = -myState.myPosLat; // @todo get rid of the '-'
1514 712633743 : const double lefthandSign = (MSGlobals::gLefthand ? -1 : 1);
1515 :
1516 : // if parking manoeuvre is happening then rotate vehicle on each step
1517 712633743 : if (MSGlobals::gModelParkingManoeuver && !manoeuvreIsComplete()) {
1518 450 : return getAngle() + myManoeuvre.getGUIIncrement();
1519 : }
1520 :
1521 712633293 : if (isParking()) {
1522 29130 : if (myStops.begin()->parkingarea != nullptr) {
1523 15806 : return myStops.begin()->parkingarea->getVehicleAngle(*this);
1524 : } else {
1525 13324 : return myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane()));
1526 : }
1527 : }
1528 712604163 : if (myLaneChangeModel->isChangingLanes()) {
1529 : // cannot use getPosition() because it already includes the offset to the side and thus messes up the angle
1530 1146742 : p1 = myLane->geometryPositionAtOffset(myState.myPos, lefthandSign * posLat);
1531 9 : if (p1 == Position::INVALID && myLane->getShape().length2D() == 0. && myLane->isInternal()) {
1532 : // workaround: extrapolate the preceding lane shape
1533 9 : MSLane* predecessorLane = myLane->getCanonicalPredecessorLane();
1534 9 : p1 = predecessorLane->geometryPositionAtOffset(predecessorLane->getLength() + myState.myPos, lefthandSign * posLat);
1535 : }
1536 : } else {
1537 711457421 : p1 = getPosition();
1538 : }
1539 :
1540 : Position p2;
1541 712604163 : if (getVehicleType().getParameter().locomotiveLength > 0) {
1542 : // articulated vehicle should use the heading of the first part
1543 1826364 : const double locoLength = MIN2(getVehicleType().getParameter().locomotiveLength, getLength());
1544 1826364 : p2 = getPosition(-locoLength);
1545 : } else {
1546 710777799 : p2 = getBackPosition();
1547 : }
1548 : if (p2 == Position::INVALID) {
1549 : // Handle special case of vehicle's back reaching out of the network
1550 1104 : if (myFurtherLanes.size() > 0) {
1551 183 : p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1552 : if (p2 == Position::INVALID) {
1553 : // unsuitable lane geometry
1554 138 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1555 : }
1556 : } else {
1557 921 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1558 : }
1559 : }
1560 : double result = (p1 != p2 ? p2.angleTo2D(p1) :
1561 100160 : myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1562 :
1563 712604163 : result += lefthandSign * myLaneChangeModel->calcAngleOffset();
1564 :
1565 : #ifdef DEBUG_FURTHER
1566 : if (DEBUG_COND) {
1567 : std::cout << SIMTIME << " computeAngle veh=" << getID() << " p1=" << p1 << " p2=" << p2 << " angle=" << RAD2DEG(result) << " naviDegree=" << GeomHelper::naviDegree(result) << "\n";
1568 : }
1569 : #endif
1570 712604163 : return result;
1571 : }
1572 :
1573 :
1574 : const Position
1575 882700870 : MSVehicle::getBackPosition() const {
1576 882700870 : const double posLat = MSGlobals::gLefthand ? myState.myPosLat : -myState.myPosLat;
1577 : Position result;
1578 882700870 : if (myState.myPos >= myType->getLength()) {
1579 : // vehicle is fully on the new lane
1580 864942633 : result = myLane->geometryPositionAtOffset(myState.myPos - myType->getLength(), posLat);
1581 : } else {
1582 17758237 : if (myLaneChangeModel->isChangingLanes() && myFurtherLanes.size() > 0 && myLaneChangeModel->getShadowLane(myFurtherLanes.back()) == nullptr) {
1583 : // special case where the target lane has no predecessor
1584 : #ifdef DEBUG_FURTHER
1585 : if (DEBUG_COND) {
1586 : std::cout << " getBackPosition veh=" << getID() << " specialCase using myLane=" << myLane->getID() << " pos=0 posLat=" << myState.myPosLat << " result=" << myLane->geometryPositionAtOffset(0, posLat) << "\n";
1587 : }
1588 : #endif
1589 1880 : result = myLane->geometryPositionAtOffset(0, posLat);
1590 : } else {
1591 : #ifdef DEBUG_FURTHER
1592 : if (DEBUG_COND) {
1593 : std::cout << " getBackPosition veh=" << getID() << " myLane=" << myLane->getID() << " further=" << toString(myFurtherLanes) << " myFurtherLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
1594 : }
1595 : #endif
1596 17756357 : if (myFurtherLanes.size() > 0 && !myLaneChangeModel->isChangingLanes()) {
1597 : // truncate to 0 if vehicle starts on an edge that is shorter than its length
1598 17230456 : const double backPos = MAX2(0.0, getBackPositionOnLane(myFurtherLanes.back()));
1599 34161851 : result = myFurtherLanes.back()->geometryPositionAtOffset(backPos, -myFurtherLanesPosLat.back() * (MSGlobals::gLefthand ? -1 : 1));
1600 : } else {
1601 525901 : result = myLane->geometryPositionAtOffset(0, posLat);
1602 : }
1603 : }
1604 : }
1605 882700870 : if (MSNet::getInstance()->hasElevation() && MSGlobals::gSublane) {
1606 285284 : interpolateLateralZ(result, myState.myPos - myType->getLength(), posLat);
1607 : }
1608 882700870 : return result;
1609 : }
1610 :
1611 :
1612 : bool
1613 429076 : MSVehicle::willStop() const {
1614 429076 : return !isStopped() && !myStops.empty() && myLane != nullptr && &myStops.front().lane->getEdge() == &myLane->getEdge();
1615 : }
1616 :
1617 : bool
1618 371889842 : MSVehicle::isStoppedOnLane() const {
1619 371889842 : return isStopped() && myStops.front().lane == myLane;
1620 : }
1621 :
1622 : bool
1623 31296233 : MSVehicle::keepStopping(bool afterProcessing) const {
1624 31296233 : if (isStopped()) {
1625 : // when coming out of vehicleTransfer we must shift the time forward
1626 37354985 : return (myStops.front().duration - (afterProcessing ? DELTA_T : 0) > 0 || isStoppedTriggered() || myStops.front().pars.collision
1627 31028931 : || myStops.front().pars.breakDown || (myStops.front().getSpeed() > 0
1628 35771 : && (myState.myPos < MIN2(myStops.front().pars.endPos, myStops.front().lane->getLength() - POSITION_EPS))
1629 29918 : && (myStops.front().pars.parking == ParkingType::ONROAD || getSpeed() >= SUMO_const_haltingSpeed)));
1630 : } else {
1631 : return false;
1632 : }
1633 : }
1634 :
1635 :
1636 : SUMOTime
1637 16160 : MSVehicle::remainingStopDuration() const {
1638 16160 : if (isStopped()) {
1639 16160 : return myStops.front().duration;
1640 : }
1641 : return 0;
1642 : }
1643 :
1644 :
1645 : SUMOTime
1646 684921620 : MSVehicle::collisionStopTime() const {
1647 684921620 : return (myStops.empty() || !myStops.front().pars.collision) ? myCollisionImmunity : MAX2((SUMOTime)0, myStops.front().duration);
1648 : }
1649 :
1650 :
1651 : bool
1652 684770294 : MSVehicle::brokeDown() const {
1653 684770294 : return isStopped() && !myStops.empty() && myStops.front().pars.breakDown;
1654 : }
1655 :
1656 :
1657 : bool
1658 186237 : MSVehicle::ignoreCollision() const {
1659 186237 : return myCollisionImmunity > 0;
1660 : }
1661 :
1662 :
1663 : double
1664 648912045 : MSVehicle::processNextStop(double currentVelocity) {
1665 648912045 : if (myStops.empty()) {
1666 : // no stops; pass
1667 : return currentVelocity;
1668 : }
1669 :
1670 : #ifdef DEBUG_STOPS
1671 : if (DEBUG_COND) {
1672 : std::cout << "\nPROCESS_NEXT_STOP\n" << SIMTIME << " vehicle '" << getID() << "'" << std::endl;
1673 : }
1674 : #endif
1675 :
1676 : MSStop& stop = myStops.front();
1677 41969386 : const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1678 41969386 : if (stop.reached) {
1679 26510487 : stop.duration -= getActionStepLength();
1680 26510487 : if (getSpeed() > 0) {
1681 : // re-enter stopping places to correct waiting position (except for parkingArea since it's place-based)
1682 4139872 : if (stop.busstop != nullptr) {
1683 : // let the bus stop know the vehicle
1684 13110 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1685 : }
1686 4139872 : if (stop.containerstop != nullptr) {
1687 : // let the container stop know the vehicle
1688 4094207 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1689 : }
1690 4139872 : if (stop.chargingStation != nullptr) {
1691 : // let the container stop know the vehicle
1692 3057 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1693 : }
1694 4139872 : if (stop.getSpeed() <= 0) {
1695 4122702 : stop.entryPos = getPositionOnLane();
1696 : }
1697 : }
1698 :
1699 : #ifdef DEBUG_STOPS
1700 : if (DEBUG_COND) {
1701 : std::cout << SIMTIME << " vehicle '" << getID() << "' reached stop.\n"
1702 : << "Remaining duration: " << STEPS2TIME(stop.duration) << std::endl;
1703 : if (stop.getSpeed() > 0) {
1704 : std::cout << " waypointSpeed=" << stop.getSpeed() << " vehPos=" << myState.myPos << " endPos=" << stop.pars.endPos << "\n";
1705 : }
1706 : }
1707 : #endif
1708 26510487 : if (stop.duration <= 0 && stop.pars.join != "") {
1709 : // join this train (part) to another one
1710 37477 : MSVehicle* joinVeh = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(stop.pars.join));
1711 968 : if (joinVeh && joinVeh->hasDeparted() && (joinVeh->joinTrainPart(this) || joinVeh->joinTrainPartFront(this))) {
1712 36 : stop.joinTriggered = false;
1713 36 : if (myAmRegisteredAsWaiting) {
1714 21 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1715 21 : myAmRegisteredAsWaiting = false;
1716 : }
1717 : // avoid collision warning before this vehicle is removed (joinVeh was already made longer)
1718 36 : myCollisionImmunity = TIME2STEPS(100);
1719 : // mark this vehicle as arrived
1720 36 : myArrivalPos = getPositionOnLane();
1721 36 : const_cast<SUMOVehicleParameter*>(myParameter)->arrivalEdge = getRoutePosition();
1722 : // handle transportables that want to continue in the other vehicle
1723 36 : if (myPersonDevice != nullptr) {
1724 3 : myPersonDevice->transferAtSplitOrJoin(joinVeh);
1725 : }
1726 36 : if (myContainerDevice != nullptr) {
1727 3 : myContainerDevice->transferAtSplitOrJoin(joinVeh);
1728 : }
1729 : }
1730 : }
1731 26510487 : boardTransportables(stop);
1732 22416686 : if (time > stop.endBoarding) {
1733 : // for taxi: cancel customers
1734 198308 : MSDevice_Taxi* taxiDevice = static_cast<MSDevice_Taxi*>(getDevice(typeid(MSDevice_Taxi)));
1735 : if (taxiDevice != nullptr) {
1736 : // may invalidate stops including the current reference
1737 64 : taxiDevice->cancelCurrentCustomers();
1738 64 : resumeFromStopping();
1739 64 : return currentVelocity;
1740 : }
1741 : }
1742 22416622 : if (!keepStopping() && isOnRoad()) {
1743 : #ifdef DEBUG_STOPS
1744 : if (DEBUG_COND) {
1745 : std::cout << SIMTIME << " vehicle '" << getID() << "' resumes from stopping." << std::endl;
1746 : }
1747 : #endif
1748 44880 : resumeFromStopping();
1749 44880 : if (isRail() && hasStops()) {
1750 : // stay on the current lane in case of a double stop
1751 3124 : const MSStop& nextStop = getNextStop();
1752 3124 : if (nextStop.edge == myCurrEdge) {
1753 1079 : const double stopSpeed = getCarFollowModel().stopSpeed(this, getSpeed(), nextStop.pars.endPos - myState.myPos);
1754 : //std::cout << SIMTIME << " veh=" << getID() << " resumedFromStopping currentVelocity=" << currentVelocity << " stopSpeed=" << stopSpeed << "\n";
1755 1079 : return stopSpeed;
1756 : }
1757 : }
1758 : } else {
1759 22371742 : if (stop.triggered) {
1760 3239482 : if (getVehicleType().getPersonCapacity() == getPersonNumber()) {
1761 30 : WRITE_WARNINGF(TL("Vehicle '%' ignores triggered stop on lane '%' due to capacity constraints."), getID(), stop.lane->getID());
1762 10 : stop.triggered = false;
1763 3239472 : } else if (!myAmRegisteredAsWaiting && stop.duration <= DELTA_T) {
1764 : // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1765 4346 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1766 4346 : myAmRegisteredAsWaiting = true;
1767 : #ifdef DEBUG_STOPS
1768 : if (DEBUG_COND) {
1769 : std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for person." << std::endl;
1770 : }
1771 : #endif
1772 : }
1773 : }
1774 22371742 : if (stop.containerTriggered) {
1775 39500 : if (getVehicleType().getContainerCapacity() == getContainerNumber()) {
1776 1332 : WRITE_WARNINGF(TL("Vehicle '%' ignores container triggered stop on lane '%' due to capacity constraints."), getID(), stop.lane->getID());
1777 444 : stop.containerTriggered = false;
1778 39056 : } else if (stop.containerTriggered && !myAmRegisteredAsWaiting && stop.duration <= DELTA_T) {
1779 : // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1780 92 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1781 92 : myAmRegisteredAsWaiting = true;
1782 : #ifdef DEBUG_STOPS
1783 : if (DEBUG_COND) {
1784 : std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for container." << std::endl;
1785 : }
1786 : #endif
1787 : }
1788 : }
1789 : // joining only takes place after stop duration is over
1790 22371742 : if (stop.joinTriggered && !myAmRegisteredAsWaiting
1791 7198 : && stop.duration <= (stop.pars.extension >= 0 ? -stop.pars.extension : 0)) {
1792 100 : if (stop.pars.extension >= 0) {
1793 108 : WRITE_WARNINGF(TL("Vehicle '%' aborts joining after extension of %s at time %."), getID(), STEPS2TIME(stop.pars.extension), time2string(SIMSTEP));
1794 36 : stop.joinTriggered = false;
1795 : } else {
1796 : // keep stopping indefinitely but ensure that simulation terminates
1797 64 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1798 64 : myAmRegisteredAsWaiting = true;
1799 : }
1800 : }
1801 22371742 : if (stop.getSpeed() > 0) {
1802 : //waypoint mode
1803 219665 : if (stop.duration == 0) {
1804 243 : return stop.getSpeed();
1805 : } else {
1806 : // stop for 'until' (computed in planMove)
1807 : return currentVelocity;
1808 : }
1809 : } else {
1810 : // brake
1811 22152077 : if (MSGlobals::gSemiImplicitEulerUpdate || stop.getSpeed() > 0) {
1812 21882305 : return 0;
1813 : } else {
1814 : // ballistic:
1815 269772 : return getSpeed() - getCarFollowModel().getMaxDecel();
1816 : }
1817 : }
1818 : }
1819 : } else {
1820 :
1821 : #ifdef DEBUG_STOPS
1822 : if (DEBUG_COND) {
1823 : std::cout << SIMTIME << " vehicle '" << getID() << "' hasn't reached next stop." << std::endl;
1824 : }
1825 : #endif
1826 : //std::cout << SIMTIME << " myStopDist=" << myStopDist << " bGap=" << getBrakeGap(myLane->getVehicleMaxSpeed(this)) << "\n";
1827 15521427 : if (stop.pars.onDemand && !stop.skipOnDemand && myStopDist <= getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this))) {
1828 577 : MSNet* const net = MSNet::getInstance();
1829 44 : const bool noExits = ((myPersonDevice == nullptr || !myPersonDevice->anyLeavingAtStop(stop))
1830 587 : && (myContainerDevice == nullptr || !myContainerDevice->anyLeavingAtStop(stop)));
1831 83 : const bool noEntries = ((!net->hasPersons() || !net->getPersonControl().hasAnyWaiting(stop.getEdge(), this))
1832 626 : && (!net->hasContainers() || !net->getContainerControl().hasAnyWaiting(stop.getEdge(), this)));
1833 577 : if (noExits && noEntries) {
1834 : //std::cout << " skipOnDemand\n";
1835 509 : stop.skipOnDemand = true;
1836 : // bestLanes must be extended past this stop
1837 509 : updateBestLanes(true);
1838 : }
1839 : }
1840 : // is the next stop on the current lane?
1841 15458899 : if (stop.edge == myCurrEdge) {
1842 : // get the stopping position
1843 5607422 : bool useStoppingPlace = stop.busstop != nullptr || stop.containerstop != nullptr || stop.parkingarea != nullptr;
1844 : bool fitsOnStoppingPlace = true;
1845 5607422 : if (!stop.skipOnDemand) { // no need to check available space if we skip it anyway
1846 5601630 : if (stop.busstop != nullptr) {
1847 1725616 : fitsOnStoppingPlace &= stop.busstop->fits(myState.myPos, *this);
1848 : }
1849 5601630 : if (stop.containerstop != nullptr) {
1850 21791 : fitsOnStoppingPlace &= stop.containerstop->fits(myState.myPos, *this);
1851 : }
1852 : // if the stop is a parking area we check if there is a free position on the area
1853 5601630 : if (stop.parkingarea != nullptr) {
1854 681265 : fitsOnStoppingPlace &= myState.myPos > stop.parkingarea->getBeginLanePosition();
1855 681265 : if (stop.parkingarea->getOccupancy() >= stop.parkingarea->getCapacity()) {
1856 : fitsOnStoppingPlace = false;
1857 : // trigger potential parkingZoneReroute
1858 439779 : MSParkingArea* oldParkingArea = stop.parkingarea;
1859 480891 : for (MSMoveReminder* rem : myLane->getMoveReminders()) {
1860 41112 : if (rem->isParkingRerouter()) {
1861 19884 : rem->notifyEnter(*this, MSMoveReminder::NOTIFICATION_PARKING_REROUTE, myLane);
1862 : }
1863 : }
1864 439779 : if (myStops.empty() || myStops.front().parkingarea != oldParkingArea) {
1865 : // rerouted, keep driving
1866 : return currentVelocity;
1867 : }
1868 241486 : } else if (stop.parkingarea->getOccupancyIncludingReservations(this) >= stop.parkingarea->getCapacity()) {
1869 : fitsOnStoppingPlace = false;
1870 116707 : } else if (stop.parkingarea->parkOnRoad() && stop.parkingarea->getLotIndex(this) < 0) {
1871 : fitsOnStoppingPlace = false;
1872 : }
1873 : }
1874 : }
1875 5605668 : const double targetPos = myState.myPos + myStopDist + (stop.getSpeed() > 0 ? (stop.pars.startPos - stop.pars.endPos) : 0);
1876 5605668 : double reachedThreshold = (useStoppingPlace ? targetPos - STOPPING_PLACE_OFFSET : stop.getReachedThreshold()) - NUMERICAL_EPS;
1877 5605668 : if (stop.busstop != nullptr && stop.getSpeed() <= 0 && getWaitingTime() > DELTA_T && myLane == stop.lane) {
1878 : // count (long) busStop as reached when fully within and jammed before the designated spot
1879 826406 : reachedThreshold = MIN2(reachedThreshold, stop.pars.startPos + getLength());
1880 : }
1881 5605668 : const bool posReached = myState.pos() >= reachedThreshold && currentVelocity <= stop.getSpeed() + SUMO_const_haltingSpeed && myLane == stop.lane;
1882 : #ifdef DEBUG_STOPS
1883 : if (DEBUG_COND) {
1884 : std::cout << " pos=" << myState.pos() << " speed=" << currentVelocity << " targetPos=" << targetPos << " fits=" << fitsOnStoppingPlace
1885 : << " reachedThresh=" << reachedThreshold
1886 : << " posReached=" << posReached
1887 : << " myLane=" << Named::getIDSecure(myLane)
1888 : << " stopLane=" << Named::getIDSecure(stop.lane)
1889 : << "\n";
1890 : }
1891 : #endif
1892 5605668 : if (posReached && !fitsOnStoppingPlace && MSStopOut::active()) {
1893 6052 : MSStopOut::getInstance()->stopBlocked(this, time);
1894 : }
1895 5605668 : if (fitsOnStoppingPlace && posReached && (!MSGlobals::gModelParkingManoeuver || myManoeuvre.entryManoeuvreIsComplete(this))) {
1896 : // ok, we may stop (have reached the stop) and either we are not modelling maneuvering or have completed entry
1897 57112 : stop.reached = true;
1898 57112 : if (!stop.startedFromState) {
1899 56878 : stop.pars.started = time;
1900 : }
1901 : #ifdef DEBUG_STOPS
1902 : if (DEBUG_COND) {
1903 : std::cout << SIMTIME << " vehicle '" << getID() << "' reached next stop." << std::endl;
1904 : }
1905 : #endif
1906 57112 : if (MSStopOut::active()) {
1907 5682 : MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), time);
1908 : }
1909 57112 : myLane->getEdge().addWaiting(this);
1910 57112 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::STARTING_STOP);
1911 57112 : MSNet::getInstance()->getVehicleControl().registerStopStarted();
1912 : // compute stopping time
1913 57112 : stop.duration = stop.getMinDuration(time);
1914 57112 : stop.endBoarding = stop.pars.extension >= 0 ? time + stop.duration + stop.pars.extension : SUMOTime_MAX;
1915 57112 : MSDevice_Taxi* taxiDevice = static_cast<MSDevice_Taxi*>(getDevice(typeid(MSDevice_Taxi)));
1916 4157 : if (taxiDevice != nullptr && stop.pars.extension >= 0) {
1917 : // earliestPickupTime is set with waitUntil
1918 84 : stop.endBoarding = MAX2(time, stop.pars.waitUntil) + stop.pars.extension;
1919 : }
1920 57112 : if (stop.getSpeed() > 0) {
1921 : // ignore duration parameter in waypoint mode unless 'until' or 'ended' are set
1922 3429 : if (stop.getUntil() > time) {
1923 348 : stop.duration = stop.getUntil() - time;
1924 : } else {
1925 3081 : stop.duration = 0;
1926 : }
1927 : } else {
1928 53683 : stop.entryPos = getPositionOnLane();
1929 : }
1930 57112 : if (stop.busstop != nullptr) {
1931 : // let the bus stop know the vehicle
1932 18940 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1933 : }
1934 57112 : if (stop.containerstop != nullptr) {
1935 : // let the container stop know the vehicle
1936 571 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1937 : }
1938 57112 : if (stop.parkingarea != nullptr && stop.getSpeed() <= 0) {
1939 : // let the parking area know the vehicle
1940 9914 : stop.parkingarea->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1941 : }
1942 57112 : if (stop.chargingStation != nullptr) {
1943 : // let the container stop know the vehicle
1944 3567 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1945 : }
1946 :
1947 57112 : if (stop.pars.tripId != "") {
1948 2922 : ((SUMOVehicleParameter&)getParameter()).setParameter("tripId", stop.pars.tripId);
1949 : }
1950 57112 : if (stop.pars.line != "") {
1951 1464 : ((SUMOVehicleParameter&)getParameter()).line = stop.pars.line;
1952 : }
1953 57112 : if (stop.pars.split != "") {
1954 : // split the train
1955 1240 : MSVehicle* splitVeh = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(stop.pars.split));
1956 24 : if (splitVeh == nullptr) {
1957 3648 : WRITE_WARNINGF(TL("Vehicle '%' to split from vehicle '%' is not known. time=%."), stop.pars.split, getID(), SIMTIME)
1958 : } else {
1959 24 : MSNet::getInstance()->getInsertionControl().add(splitVeh);
1960 24 : splitVeh->getRoute().getEdges()[0]->removeWaiting(splitVeh);
1961 24 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1962 24 : const double newLength = MAX2(myType->getLength() - splitVeh->getVehicleType().getLength(),
1963 24 : myType->getParameter().locomotiveLength);
1964 24 : getSingularType().setLength(newLength);
1965 : // handle transportables that want to continue in the split part
1966 24 : if (myPersonDevice != nullptr) {
1967 0 : myPersonDevice->transferAtSplitOrJoin(splitVeh);
1968 : }
1969 24 : if (myContainerDevice != nullptr) {
1970 6 : myContainerDevice->transferAtSplitOrJoin(splitVeh);
1971 : }
1972 24 : if (splitVeh->getParameter().departPosProcedure == DepartPosDefinition::SPLIT_FRONT) {
1973 3 : const double backShift = splitVeh->getLength() + getVehicleType().getMinGap();
1974 3 : myState.myPos -= backShift;
1975 3 : myState.myBackPos -= backShift;
1976 : }
1977 : }
1978 : }
1979 :
1980 57112 : boardTransportables(stop);
1981 57108 : if (stop.pars.posLat != INVALID_DOUBLE) {
1982 231 : myState.myPosLat = stop.pars.posLat;
1983 : }
1984 : }
1985 : }
1986 : }
1987 : return currentVelocity;
1988 : }
1989 :
1990 :
1991 : void
1992 26567599 : MSVehicle::boardTransportables(MSStop& stop) {
1993 26567599 : if (stop.skipOnDemand) {
1994 : return;
1995 : }
1996 : // we have reached the stop
1997 : // any waiting persons may board now
1998 26364391 : const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1999 26364391 : MSNet* const net = MSNet::getInstance();
2000 26364391 : const bool boarded = (time <= stop.endBoarding
2001 26362806 : && net->hasPersons()
2002 1540089 : && net->getPersonControl().loadAnyWaiting(&myLane->getEdge(), this, stop.timeToBoardNextPerson, stop.duration)
2003 26372069 : && stop.numExpectedPerson == 0);
2004 : // load containers
2005 26364391 : const bool loaded = (time <= stop.endBoarding
2006 26362806 : && net->hasContainers()
2007 4196239 : && net->getContainerControl().loadAnyWaiting(&myLane->getEdge(), this, stop.timeToLoadNextContainer, stop.duration)
2008 26364966 : && stop.numExpectedContainer == 0);
2009 :
2010 : bool unregister = false;
2011 22270586 : if (time > stop.endBoarding) {
2012 1585 : stop.triggered = false;
2013 1585 : stop.containerTriggered = false;
2014 1585 : if (myAmRegisteredAsWaiting) {
2015 : unregister = true;
2016 328 : myAmRegisteredAsWaiting = false;
2017 : }
2018 : }
2019 22270586 : if (boarded) {
2020 : // the triggering condition has been fulfilled. Maybe we want to wait a bit longer for additional riders (car pooling)
2021 7539 : if (myAmRegisteredAsWaiting) {
2022 : unregister = true;
2023 : }
2024 7539 : stop.triggered = false;
2025 7539 : myAmRegisteredAsWaiting = false;
2026 : }
2027 22270586 : if (loaded) {
2028 : // the triggering condition has been fulfilled
2029 555 : if (myAmRegisteredAsWaiting) {
2030 : unregister = true;
2031 : }
2032 555 : stop.containerTriggered = false;
2033 555 : myAmRegisteredAsWaiting = false;
2034 : }
2035 :
2036 22270586 : if (unregister) {
2037 412 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2038 : #ifdef DEBUG_STOPS
2039 : if (DEBUG_COND) {
2040 : std::cout << SIMTIME << " vehicle '" << getID() << "' unregisters as waiting for transportable." << std::endl;
2041 : }
2042 : #endif
2043 : }
2044 : }
2045 :
2046 : bool
2047 920 : MSVehicle::joinTrainPart(MSVehicle* veh) {
2048 : // check if veh is close enough to be joined to the rear of this vehicle
2049 920 : MSLane* backLane = myFurtherLanes.size() == 0 ? myLane : myFurtherLanes.back();
2050 920 : double gap = getBackPositionOnLane() - veh->getPositionOnLane();
2051 1141 : if (isStopped() && myStops.begin()->duration <= DELTA_T && myStops.begin()->joinTriggered && backLane == veh->getLane()
2052 950 : && gap >= 0 && gap <= getVehicleType().getMinGap() + 1) {
2053 15 : const double newLength = myType->getLength() + veh->getVehicleType().getLength();
2054 15 : getSingularType().setLength(newLength);
2055 15 : myStops.begin()->joinTriggered = false;
2056 15 : if (myAmRegisteredAsWaiting) {
2057 0 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2058 0 : myAmRegisteredAsWaiting = false;
2059 : }
2060 : return true;
2061 : } else {
2062 905 : return false;
2063 : }
2064 : }
2065 :
2066 :
2067 : bool
2068 905 : MSVehicle::joinTrainPartFront(MSVehicle* veh) {
2069 : // check if veh is close enough to be joined to the front of this vehicle
2070 905 : MSLane* backLane = veh->myFurtherLanes.size() == 0 ? veh->myLane : veh->myFurtherLanes.back();
2071 905 : double gap = veh->getBackPositionOnLane(backLane) - getPositionOnLane();
2072 1111 : if (isStopped() && myStops.begin()->duration <= DELTA_T && myStops.begin()->joinTriggered && backLane == getLane()
2073 929 : && gap >= 0 && gap <= getVehicleType().getMinGap() + 1) {
2074 : double skippedLaneLengths = 0;
2075 24 : if (veh->myFurtherLanes.size() > 0) {
2076 9 : skippedLaneLengths += getLane()->getLength();
2077 : // this vehicle must be moved to the lane of veh
2078 : // ensure that lane and furtherLanes of veh match our route
2079 9 : int routeIndex = getRoutePosition();
2080 9 : if (myLane->isInternal()) {
2081 0 : routeIndex++;
2082 : }
2083 27 : for (int i = (int)veh->myFurtherLanes.size() - 1; i >= 0; i--) {
2084 18 : MSEdge* edge = &veh->myFurtherLanes[i]->getEdge();
2085 18 : if (edge->isInternal()) {
2086 9 : continue;
2087 : }
2088 9 : if (!edge->isInternal() && edge != myRoute->getEdges()[routeIndex]) {
2089 0 : std::string warn = TL("Cannot join vehicle '%' to vehicle '%' due to incompatible routes. time=%.");
2090 0 : WRITE_WARNINGF(warn, veh->getID(), getID(), time2string(SIMSTEP));
2091 : return false;
2092 : }
2093 9 : routeIndex++;
2094 : }
2095 9 : if (veh->getCurrentEdge()->getNormalSuccessor() != myRoute->getEdges()[routeIndex]) {
2096 3 : std::string warn = TL("Cannot join vehicle '%' to vehicle '%' due to incompatible routes. time=%.");
2097 9 : WRITE_WARNINGF(warn, veh->getID(), getID(), time2string(SIMSTEP));
2098 : return false;
2099 : }
2100 12 : for (int i = (int)veh->myFurtherLanes.size() - 2; i >= 0; i--) {
2101 6 : skippedLaneLengths += veh->myFurtherLanes[i]->getLength();
2102 : }
2103 : }
2104 :
2105 21 : const double newLength = myType->getLength() + veh->getVehicleType().getLength();
2106 21 : getSingularType().setLength(newLength);
2107 : // lane will be advanced just as for regular movement
2108 21 : myState.myPos = skippedLaneLengths + veh->getPositionOnLane();
2109 21 : myStops.begin()->joinTriggered = false;
2110 21 : if (myAmRegisteredAsWaiting) {
2111 7 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2112 7 : myAmRegisteredAsWaiting = false;
2113 : }
2114 21 : return true;
2115 : } else {
2116 881 : return false;
2117 : }
2118 : }
2119 :
2120 : double
2121 8813757 : MSVehicle::getBrakeGap(bool delayed) const {
2122 8813757 : return getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), delayed ? getCarFollowModel().getHeadwayTime() : 0);
2123 : }
2124 :
2125 :
2126 : bool
2127 709197496 : MSVehicle::checkActionStep(const SUMOTime t) {
2128 709197496 : myActionStep = isActionStep(t);
2129 709197496 : if (myActionStep) {
2130 637469645 : myLastActionTime = t;
2131 : }
2132 709197496 : return myActionStep;
2133 : }
2134 :
2135 :
2136 : void
2137 1477 : MSVehicle::resetActionOffset(const SUMOTime timeUntilNextAction) {
2138 1477 : myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + timeUntilNextAction;
2139 1477 : }
2140 :
2141 :
2142 : void
2143 1 : MSVehicle::updateActionOffset(const SUMOTime oldActionStepLength, const SUMOTime newActionStepLength) {
2144 1 : SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2145 1 : SUMOTime timeSinceLastAction = now - myLastActionTime;
2146 1 : if (timeSinceLastAction == 0) {
2147 : // Action was scheduled now, may be delayed be new action step length
2148 : timeSinceLastAction = oldActionStepLength;
2149 : }
2150 1 : if (timeSinceLastAction >= newActionStepLength) {
2151 : // Action point required in this step
2152 0 : myLastActionTime = now;
2153 : } else {
2154 1 : SUMOTime timeUntilNextAction = newActionStepLength - timeSinceLastAction;
2155 1 : resetActionOffset(timeUntilNextAction);
2156 : }
2157 1 : }
2158 :
2159 :
2160 :
2161 : void
2162 709197496 : MSVehicle::planMove(const SUMOTime t, const MSLeaderInfo& ahead, const double lengthsInFront) {
2163 : #ifdef DEBUG_PLAN_MOVE
2164 : if (DEBUG_COND) {
2165 : std::cout
2166 : << "\nPLAN_MOVE\n"
2167 : << SIMTIME
2168 : << std::setprecision(gPrecision)
2169 : << " veh=" << getID()
2170 : << " lane=" << myLane->getID()
2171 : << " pos=" << getPositionOnLane()
2172 : << " posLat=" << getLateralPositionOnLane()
2173 : << " speed=" << getSpeed()
2174 : << "\n";
2175 : }
2176 : #endif
2177 : // Update the driver state
2178 709197496 : if (hasDriverState()) {
2179 451727 : myDriverState->update();
2180 903454 : setActionStepLength(myDriverState->getDriverState()->getActionStepLength(), false);
2181 : }
2182 :
2183 709197496 : myStopSpeed = getCarFollowModel().maxNextSpeed(myStopSpeed, this);
2184 709197496 : if (!checkActionStep(t)) {
2185 : #ifdef DEBUG_ACTIONSTEPS
2186 : if (DEBUG_COND) {
2187 : std::cout << STEPS2TIME(t) << " vehicle '" << getID() << "' skips action." << std::endl;
2188 : }
2189 : #endif
2190 : // During non-action passed drive items still need to be removed
2191 : // @todo rather work with updating myCurrentDriveItem (refs #3714)
2192 71727851 : removePassedDriveItems();
2193 71727851 : return;
2194 : } else {
2195 : #ifdef DEBUG_ACTIONSTEPS
2196 : if (DEBUG_COND) {
2197 : std::cout << STEPS2TIME(t) << " vehicle = '" << getID() << "' takes action." << std::endl;
2198 : }
2199 : #endif
2200 : myLFLinkLanesPrev.swap(myLFLinkLanes);
2201 637469645 : if (myInfluencer != nullptr) {
2202 492912 : myInfluencer->updateRemoteControlRoute(this);
2203 : }
2204 637469645 : planMoveInternal(t, ahead, myLFLinkLanes, myStopDist, myStopSpeed, myNextTurn);
2205 : #ifdef DEBUG_PLAN_MOVE
2206 : if (DEBUG_COND) {
2207 : DriveItemVector::iterator i;
2208 : for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
2209 : std::cout
2210 : << " vPass=" << (*i).myVLinkPass
2211 : << " vWait=" << (*i).myVLinkWait
2212 : << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
2213 : << " request=" << (*i).mySetRequest
2214 : << "\n";
2215 : }
2216 : }
2217 : #endif
2218 637469645 : checkRewindLinkLanes(lengthsInFront, myLFLinkLanes);
2219 637469645 : myNextDriveItem = myLFLinkLanes.begin();
2220 : // ideally would only do this with the call inside planMoveInternal - but that needs a const method
2221 : // so this is a kludge here - nuisance as it adds an extra check in a busy loop
2222 637469645 : if (MSGlobals::gModelParkingManoeuver) {
2223 2971 : if (getManoeuvreType() == MSVehicle::MANOEUVRE_EXIT && manoeuvreIsComplete()) {
2224 30 : setManoeuvreType(MSVehicle::MANOEUVRE_NONE);
2225 : }
2226 : }
2227 : }
2228 637469645 : myLaneChangeModel->resetChanged();
2229 : }
2230 :
2231 :
2232 : bool
2233 177682421 : MSVehicle::brakeForOverlap(const MSLink* link, const MSLane* lane) const {
2234 : // @review needed
2235 : //const double futurePosLat = getLateralPositionOnLane() + link->getLateralShift();
2236 : //const double overlap = getLateralOverlap(futurePosLat, link->getViaLaneOrLane());
2237 : //const double edgeWidth = link->getViaLaneOrLane()->getEdge().getWidth();
2238 177682421 : const double futurePosLat = getLateralPositionOnLane() + (
2239 177682421 : lane != myLane && lane->isInternal() ? lane->getIncomingLanes()[0].viaLink->getLateralShift() : 0);
2240 177682421 : const double overlap = getLateralOverlap(futurePosLat, lane);
2241 : const double edgeWidth = lane->getEdge().getWidth();
2242 : const bool result = (overlap > POSITION_EPS
2243 : // do not get stuck on narrow edges
2244 3149233 : && getVehicleType().getWidth() <= edgeWidth
2245 3143628 : && link->getViaLane() == nullptr
2246 : // this is the exit link of a junction. The normal edge should support the shadow
2247 1506458 : && ((myLaneChangeModel->getShadowLane(link->getLane()) == nullptr)
2248 : // the shadow lane must be permitted
2249 1115228 : || !myLaneChangeModel->getShadowLane(link->getLane())->allowsVehicleClass(getVClass())
2250 : // the internal lane after an internal junction has no parallel lane. make sure there is no shadow before continuing
2251 1054781 : || (lane->getEdge().isInternal() && lane->getIncomingLanes()[0].lane->getEdge().isInternal()))
2252 : // ignore situations where the shadow lane is part of a double-connection with the current lane
2253 472270 : && (myLaneChangeModel->getShadowLane() == nullptr
2254 267899 : || myLaneChangeModel->getShadowLane()->getLinkCont().size() == 0
2255 249807 : || myLaneChangeModel->getShadowLane()->getLinkCont().front()->getLane() != link->getLane())
2256 : // emergency vehicles may do some crazy stuff
2257 178092984 : && !myLaneChangeModel->hasBlueLight());
2258 :
2259 : #ifdef DEBUG_PLAN_MOVE
2260 : if (DEBUG_COND) {
2261 : std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getDescription() << " lane=" << lane->getID()
2262 : << " linkLane=" << link->getLane()->getID()
2263 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
2264 : << " shift=" << link->getLateralShift()
2265 : << " fpLat=" << futurePosLat << " overlap=" << overlap << " w=" << getVehicleType().getWidth()
2266 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane(link->getLane()))
2267 : << " result=" << result << "\n";
2268 : }
2269 : #endif
2270 177682421 : return result;
2271 : }
2272 :
2273 :
2274 :
2275 : void
2276 637469645 : MSVehicle::planMoveInternal(const SUMOTime t, MSLeaderInfo ahead, DriveItemVector& lfLinks, double& newStopDist, double& newStopSpeed, std::pair<double, const MSLink*>& nextTurn) const {
2277 : lfLinks.clear();
2278 637469645 : newStopDist = std::numeric_limits<double>::max();
2279 : //
2280 : const MSCFModel& cfModel = getCarFollowModel();
2281 637469645 : const double vehicleLength = getVehicleType().getLength();
2282 637469645 : const double maxV = cfModel.maxNextSpeed(myState.mySpeed, this);
2283 637469645 : const double maxVD = MAX2(getMaxSpeed(), MIN2(maxV, getDesiredMaxSpeed()));
2284 637469645 : const bool opposite = myLaneChangeModel->isOpposite();
2285 : // maxVD is possibly higher than vType-maxSpeed and in this case laneMaxV may be higher as well
2286 637469645 : double laneMaxV = myLane->getVehicleMaxSpeed(this, maxVD);
2287 637469645 : const double vMinComfortable = cfModel.minNextSpeed(getSpeed(), this);
2288 : double lateralShift = 0;
2289 637469645 : if (isRail()) {
2290 : // speed limits must hold for the whole length of the train
2291 1761696 : for (MSLane* l : myFurtherLanes) {
2292 398726 : laneMaxV = MIN2(laneMaxV, l->getVehicleMaxSpeed(this, maxVD));
2293 : #ifdef DEBUG_PLAN_MOVE
2294 : if (DEBUG_COND) {
2295 : std::cout << " laneMaxV=" << laneMaxV << " lane=" << l->getID() << "\n";
2296 : }
2297 : #endif
2298 : }
2299 : }
2300 : // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
2301 : laneMaxV = MAX2(laneMaxV, vMinComfortable);
2302 637962525 : if (myInfluencer && !myInfluencer->considerSpeedLimit()) {
2303 : laneMaxV = std::numeric_limits<double>::max();
2304 : }
2305 : // v is the initial maximum velocity of this vehicle in this step
2306 637469645 : double v = cfModel.maximumLaneSpeedCF(this, maxV, laneMaxV);
2307 : // if we are modelling parking then we dawdle until the manoeuvre is complete - by setting a very low max speed
2308 : // in practice this only applies to exit manoeuvre because entry manoeuvre just delays setting stop.reached - when the vehicle is virtually stopped
2309 637469645 : if (MSGlobals::gModelParkingManoeuver && !manoeuvreIsComplete()) {
2310 420 : v = NUMERICAL_EPS_SPEED;
2311 : }
2312 :
2313 637469645 : if (myInfluencer != nullptr) {
2314 492912 : const double vMin = MAX2(0., cfModel.minNextSpeed(myState.mySpeed, this));
2315 : #ifdef DEBUG_TRACI
2316 : if (DEBUG_COND) {
2317 : std::cout << SIMTIME << " veh=" << getID() << " speedBeforeTraci=" << v;
2318 : }
2319 : #endif
2320 492912 : v = myInfluencer->influenceSpeed(t, v, v, vMin, maxV);
2321 : #ifdef DEBUG_TRACI
2322 : if (DEBUG_COND) {
2323 : std::cout << " influencedSpeed=" << v;
2324 : }
2325 : #endif
2326 492912 : v = myInfluencer->gapControlSpeed(t, this, v, v, vMin, maxV);
2327 : #ifdef DEBUG_TRACI
2328 : if (DEBUG_COND) {
2329 : std::cout << " gapControlSpeed=" << v << "\n";
2330 : }
2331 : #endif
2332 : }
2333 : // all links within dist are taken into account (potentially)
2334 637469645 : const double dist = SPEED2DIST(maxV) + cfModel.brakeGap(maxV);
2335 :
2336 637469645 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation();
2337 : #ifdef DEBUG_PLAN_MOVE
2338 : if (DEBUG_COND) {
2339 : std::cout << " dist=" << dist << " bestLaneConts=" << toString(bestLaneConts)
2340 : << "\n maxV=" << maxV << " laneMaxV=" << laneMaxV << " v=" << v << "\n";
2341 : }
2342 : #endif
2343 : assert(bestLaneConts.size() > 0);
2344 : bool hadNonInternal = false;
2345 : // the distance already "seen"; in the following always up to the end of the current "lane"
2346 637469645 : double seen = opposite ? myState.myPos : myLane->getLength() - myState.myPos;
2347 637469645 : nextTurn.first = seen;
2348 637469645 : nextTurn.second = nullptr;
2349 637469645 : bool encounteredTurn = (MSGlobals::gLateralResolution <= 0); // next turn is only needed for sublane
2350 : double seenNonInternal = 0;
2351 637469645 : double seenInternal = myLane->isInternal() ? seen : 0;
2352 637469645 : double vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(seen, v, cfModel.getMaxAccel()), laneMaxV); // upper bound
2353 : int view = 0;
2354 : DriveProcessItem* lastLink = nullptr;
2355 : bool slowedDownForMinor = false; // whether the vehicle already had to slow down on approach to a minor link
2356 : double mustSeeBeforeReversal = 0;
2357 : // iterator over subsequent lanes and fill lfLinks until stopping distance or stopped
2358 637469645 : const MSLane* lane = opposite ? myLane->getParallelOpposite() : myLane;
2359 : assert(lane != 0);
2360 637469645 : const MSLane* leaderLane = myLane;
2361 637469645 : bool foundRailSignal = !isRail();
2362 : bool planningToStop = false;
2363 : #ifdef PARALLEL_STOPWATCH
2364 : myLane->getStopWatch()[0].start();
2365 : #endif
2366 :
2367 : // optionally slow down to match arrival time
2368 637469645 : const double sfp = getVehicleType().getParameter().speedFactorPremature;
2369 637459414 : if (v > vMinComfortable && hasStops() && myStops.front().pars.arrival >= 0 && sfp > 0
2370 4279 : && v > myLane->getSpeedLimit() * sfp
2371 637472681 : && !myStops.front().reached) {
2372 2786 : const double vSlowDown = slowDownForSchedule(vMinComfortable);
2373 5403 : v = MIN2(v, vSlowDown);
2374 : }
2375 : auto stopIt = myStops.begin();
2376 : while (true) {
2377 : // check leader on lane
2378 : // leader is given for the first edge only
2379 1233694413 : if (opposite &&
2380 : (leaderLane->getVehicleNumberWithPartials() > 1
2381 104965 : || (leaderLane != myLane && leaderLane->getVehicleNumber() > 0))) {
2382 394427 : ahead.clear();
2383 : // find opposite-driving leader that must be respected on the currently looked at lane
2384 : // (only looking at one lane at a time)
2385 394427 : const double backOffset = leaderLane == myLane ? getPositionOnLane() : leaderLane->getLength();
2386 394427 : const double gapOffset = leaderLane == myLane ? 0 : seen - leaderLane->getLength();
2387 394427 : const MSLeaderDistanceInfo cands = leaderLane->getFollowersOnConsecutive(this, backOffset, true, backOffset, MSLane::MinorLinkMode::FOLLOW_NEVER);
2388 394427 : MSLeaderDistanceInfo oppositeLeaders(leaderLane->getWidth(), this, 0.);
2389 394427 : const double minTimeToLeaveLane = MSGlobals::gSublane ? MAX2(TS, (0.5 * myLane->getWidth() - getLateralPositionOnLane()) / getVehicleType().getMaxSpeedLat()) : TS;
2390 1040721 : for (int i = 0; i < cands.numSublanes(); i++) {
2391 646294 : CLeaderDist cand = cands[i];
2392 646294 : if (cand.first != 0) {
2393 529039 : if ((cand.first->myLaneChangeModel->isOpposite() && cand.first->getLaneChangeModel().getShadowLane() != leaderLane)
2394 529523 : || (!cand.first->myLaneChangeModel->isOpposite() && cand.first->getLaneChangeModel().getShadowLane() == leaderLane)) {
2395 : // respect leaders that also drive in the opposite direction (fully or with some overlap)
2396 344400 : oppositeLeaders.addLeader(cand.first, cand.second + gapOffset - getVehicleType().getMinGap() + cand.first->getVehicleType().getMinGap() - cand.first->getVehicleType().getLength());
2397 : } else {
2398 : // avoid frontal collision
2399 336006 : const bool assumeStopped = cand.first->isStopped() || cand.first->getWaitingSeconds() > 1;
2400 184639 : const double predMaxDist = cand.first->getSpeed() + (assumeStopped ? 0 : cand.first->getCarFollowModel().getMaxAccel()) * minTimeToLeaveLane;
2401 184639 : if (cand.second >= 0 && (cand.second - v * minTimeToLeaveLane - predMaxDist < 0 || assumeStopped)) {
2402 45106 : oppositeLeaders.addLeader(cand.first, cand.second + gapOffset - predMaxDist - getVehicleType().getMinGap());
2403 : }
2404 : }
2405 : }
2406 : }
2407 : #ifdef DEBUG_PLAN_MOVE
2408 : if (DEBUG_COND) {
2409 : std::cout << " leaderLane=" << leaderLane->getID() << " gapOffset=" << gapOffset << " minTimeToLeaveLane=" << minTimeToLeaveLane
2410 : << " cands=" << cands.toString() << " oppositeLeaders=" << oppositeLeaders.toString() << "\n";
2411 : }
2412 : #endif
2413 394427 : adaptToLeaderDistance(oppositeLeaders, 0, seen, lastLink, v, vLinkPass);
2414 394427 : } else {
2415 1233299986 : if (MSGlobals::gLateralResolution > 0 && myLaneChangeModel->getShadowLane() == nullptr) {
2416 200502138 : const double rightOL = getRightSideOnLane(lane) + lateralShift;
2417 200502138 : const double leftOL = getLeftSideOnLane(lane) + lateralShift;
2418 : const bool outsideLeft = leftOL > lane->getWidth();
2419 : #ifdef DEBUG_PLAN_MOVE
2420 : if (DEBUG_COND) {
2421 : std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
2422 : }
2423 : #endif
2424 200502138 : if (rightOL < 0 || outsideLeft) {
2425 1336150 : MSLeaderInfo outsideLeaders(lane->getWidth());
2426 : // if ego is driving outside lane bounds we must consider
2427 : // potential leaders that are also outside bounds
2428 : int sublaneOffset = 0;
2429 1336150 : if (outsideLeft) {
2430 526970 : sublaneOffset = MIN2(-1, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution));
2431 : } else {
2432 809180 : sublaneOffset = MAX2(1, (int)ceil(-rightOL / MSGlobals::gLateralResolution));
2433 : }
2434 1336150 : outsideLeaders.setSublaneOffset(sublaneOffset);
2435 : #ifdef DEBUG_PLAN_MOVE
2436 : if (DEBUG_COND) {
2437 : std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
2438 : }
2439 : #endif
2440 5783623 : for (const MSVehicle* cand : lane->getVehiclesSecure()) {
2441 1659914 : if ((lane != myLane || cand->getPositionOnLane() > getPositionOnLane())
2442 5161648 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
2443 3501689 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
2444 111910 : outsideLeaders.addLeader(cand, true);
2445 : #ifdef DEBUG_PLAN_MOVE
2446 : if (DEBUG_COND) {
2447 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << outsideLeaders.toString() << "\n";
2448 : }
2449 : #endif
2450 : }
2451 : }
2452 1336150 : lane->releaseVehicles();
2453 1336150 : if (outsideLeaders.hasVehicles()) {
2454 28383 : adaptToLeaders(outsideLeaders, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2455 : }
2456 1336150 : }
2457 : }
2458 1233299986 : adaptToLeaders(ahead, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2459 : }
2460 1233694413 : if (lastLink != nullptr) {
2461 1122315543 : lastLink->myVLinkWait = MIN2(lastLink->myVLinkWait, v);
2462 : }
2463 : #ifdef DEBUG_PLAN_MOVE
2464 : if (DEBUG_COND) {
2465 : std::cout << "\nv = " << v << "\n";
2466 :
2467 : }
2468 : #endif
2469 : // XXX efficiently adapt to shadow leaders using neighAhead by iteration over the whole edge in parallel (lanechanger-style)
2470 1233694413 : if (myLaneChangeModel->getShadowLane() != nullptr) {
2471 : // also slow down for leaders on the shadowLane relative to the current lane
2472 5084113 : const MSLane* shadowLane = myLaneChangeModel->getShadowLane(leaderLane);
2473 : if (shadowLane != nullptr
2474 5084113 : && (MSGlobals::gLateralResolution > 0 || getLateralOverlap() > POSITION_EPS
2475 : // continous lane change cannot be stopped so we must adapt to the leader on the target lane
2476 191102 : || myLaneChangeModel->getLaneChangeCompletion() < 0.5)) {
2477 4499369 : if ((&shadowLane->getEdge() == &leaderLane->getEdge() || myLaneChangeModel->isOpposite())) {
2478 4452337 : double latOffset = getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge();
2479 4452337 : if (myLaneChangeModel->isOpposite()) {
2480 : // ego posLat is added when retrieving sublanes but it
2481 : // should be negated (subtract twice to compensate)
2482 136109 : latOffset = ((myLane->getWidth() + shadowLane->getWidth()) * 0.5
2483 136109 : - 2 * getLateralPositionOnLane());
2484 :
2485 : }
2486 4452337 : MSLeaderInfo shadowLeaders = shadowLane->getLastVehicleInformation(this, latOffset, lane->getLength() - seen);
2487 : #ifdef DEBUG_PLAN_MOVE
2488 : if (DEBUG_COND && myLaneChangeModel->isOpposite()) {
2489 : std::cout << SIMTIME << " opposite veh=" << getID() << " shadowLane=" << shadowLane->getID() << " latOffset=" << latOffset << " shadowLeaders=" << shadowLeaders.toString() << "\n";
2490 : }
2491 : #endif
2492 4452337 : if (myLaneChangeModel->isOpposite()) {
2493 : // ignore oncoming vehicles on the shadow lane
2494 136109 : shadowLeaders.removeOpposite(shadowLane);
2495 : }
2496 4452337 : const double turningDifference = MAX2(0.0, leaderLane->getLength() - shadowLane->getLength());
2497 4452337 : adaptToLeaders(shadowLeaders, latOffset, seen - turningDifference, lastLink, shadowLane, v, vLinkPass);
2498 4499369 : } else if (shadowLane == myLaneChangeModel->getShadowLane() && leaderLane == myLane) {
2499 : // check for leader vehicles driving in the opposite direction on the opposite-direction shadow lane
2500 : // (and thus in the same direction as ego)
2501 35317 : MSLeaderDistanceInfo shadowLeaders = shadowLane->getFollowersOnConsecutive(this, myLane->getOppositePos(getPositionOnLane()), true);
2502 : const double latOffset = 0;
2503 : #ifdef DEBUG_PLAN_MOVE
2504 : if (DEBUG_COND) {
2505 : std::cout << SIMTIME << " opposite shadows veh=" << getID() << " shadowLane=" << shadowLane->getID()
2506 : << " latOffset=" << latOffset << " shadowLeaders=" << shadowLeaders.toString() << "\n";
2507 : }
2508 : #endif
2509 35317 : shadowLeaders.fixOppositeGaps(true);
2510 : #ifdef DEBUG_PLAN_MOVE
2511 : if (DEBUG_COND) {
2512 : std::cout << " shadowLeadersFixed=" << shadowLeaders.toString() << "\n";
2513 : }
2514 : #endif
2515 35317 : adaptToLeaderDistance(shadowLeaders, latOffset, seen, lastLink, v, vLinkPass);
2516 35317 : }
2517 : }
2518 : }
2519 : // adapt to pedestrians on the same lane
2520 1233694413 : if (lane->getEdge().getPersons().size() > 0 && lane->hasPedestrians()) {
2521 192784 : const double relativePos = lane->getLength() - seen;
2522 : #ifdef DEBUG_PLAN_MOVE
2523 : if (DEBUG_COND) {
2524 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2525 : }
2526 : #endif
2527 192784 : const double stopTime = MAX2(1.0, ceil(getSpeed() / cfModel.getMaxDecel()));
2528 192784 : PersonDist leader = lane->nextBlocking(relativePos,
2529 192784 : getRightSideOnLane(lane), getRightSideOnLane(lane) + getVehicleType().getWidth(), stopTime);
2530 192784 : if (leader.first != 0) {
2531 21158 : const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2532 29904 : v = MIN2(v, stopSpeed);
2533 : #ifdef DEBUG_PLAN_MOVE
2534 : if (DEBUG_COND) {
2535 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2536 : }
2537 : #endif
2538 : }
2539 : }
2540 1233694413 : if (lane->getBidiLane() != nullptr) {
2541 : // adapt to pedestrians on the bidi lane
2542 4715294 : const MSLane* bidiLane = lane->getBidiLane();
2543 4715294 : if (bidiLane->getEdge().getPersons().size() > 0 && bidiLane->hasPedestrians()) {
2544 1028 : const double relativePos = seen;
2545 : #ifdef DEBUG_PLAN_MOVE
2546 : if (DEBUG_COND) {
2547 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2548 : }
2549 : #endif
2550 1028 : const double stopTime = ceil(getSpeed() / cfModel.getMaxDecel());
2551 1028 : const double leftSideOnLane = bidiLane->getWidth() - getRightSideOnLane(lane);
2552 1028 : PersonDist leader = bidiLane->nextBlocking(relativePos,
2553 1028 : leftSideOnLane - getVehicleType().getWidth(), leftSideOnLane, stopTime, true);
2554 1028 : if (leader.first != 0) {
2555 267 : const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2556 524 : v = MIN2(v, stopSpeed);
2557 : #ifdef DEBUG_PLAN_MOVE
2558 : if (DEBUG_COND) {
2559 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2560 : }
2561 : #endif
2562 : }
2563 : }
2564 : }
2565 : // adapt to vehicles blocked from (urgent) lane-changing
2566 1233694413 : if (!opposite && lane->getEdge().hasLaneChanger()) {
2567 596451655 : const double vHelp = myLaneChangeModel->getCooperativeHelpSpeed(lane, seen);
2568 : #ifdef DEBUG_PLAN_MOVE
2569 : if (DEBUG_COND && vHelp < v) {
2570 : std::cout << SIMTIME << " applying cooperativeHelpSpeed v=" << vHelp << "\n";
2571 : }
2572 : #endif
2573 596569589 : v = MIN2(v, vHelp);
2574 : }
2575 :
2576 : // process all stops and waypoints on the current edge
2577 : bool foundRealStop = false;
2578 : while (stopIt != myStops.end()
2579 67641687 : && ((&stopIt->lane->getEdge() == &lane->getEdge())
2580 34033745 : || (stopIt->isOpposite && stopIt->lane->getEdge().getOppositeEdge() == &lane->getEdge()))
2581 : // ignore stops that occur later in a looped route
2582 1290951432 : && stopIt->edge == myCurrEdge + view) {
2583 33552742 : double stopDist = std::numeric_limits<double>::max();
2584 : const MSStop& stop = *stopIt;
2585 : const bool isFirstStop = stopIt == myStops.begin();
2586 : stopIt++;
2587 33552742 : if (!stop.reached || (stop.getSpeed() > 0 && keepStopping())) {
2588 : // we are approaching a stop on the edge; must not drive further
2589 15404598 : bool isWaypoint = stop.getSpeed() > 0;
2590 15404598 : double endPos = stop.getEndPos(*this) + NUMERICAL_EPS;
2591 15404598 : if (stop.parkingarea != nullptr) {
2592 : // leave enough space so parking vehicles can exit
2593 1648211 : const double brakePos = getBrakeGap() + lane->getLength() - seen;
2594 1648211 : endPos = stop.parkingarea->getLastFreePosWithReservation(t, *this, brakePos);
2595 13756387 : } else if (isWaypoint && !stop.reached) {
2596 107861 : endPos = stop.pars.startPos;
2597 : }
2598 15404598 : stopDist = seen + endPos - lane->getLength();
2599 : #ifdef DEBUG_STOPS
2600 : if (DEBUG_COND) {
2601 : std::cout << SIMTIME << " veh=" << getID() << " stopDist=" << stopDist << " stopLane=" << stop.lane->getID() << " stopEndPos=" << endPos << "\n";
2602 : }
2603 : #endif
2604 : double stopSpeed = laneMaxV;
2605 15404598 : if (isWaypoint) {
2606 : bool waypointWithStop = false;
2607 123407 : if (stop.getUntil() > t) {
2608 : // check if we have to slow down or even stop
2609 : SUMOTime time2end = 0;
2610 3691 : if (stop.reached) {
2611 702 : time2end = TIME2STEPS((stop.pars.endPos - myState.myPos) / stop.getSpeed());
2612 : } else {
2613 3267 : time2end = TIME2STEPS(
2614 : // time to reach waypoint start
2615 : stopDist / ((getSpeed() + stop.getSpeed()) / 2)
2616 : // time to reach waypoint end
2617 : + (stop.pars.endPos - stop.pars.startPos) / stop.getSpeed());
2618 : }
2619 3691 : if (stop.getUntil() > t + time2end) {
2620 : // we need to stop
2621 : double distToEnd = stopDist;
2622 3398 : if (!stop.reached) {
2623 2783 : distToEnd += stop.pars.endPos - stop.pars.startPos;
2624 : }
2625 3398 : stopSpeed = MAX2(cfModel.stopSpeed(this, getSpeed(), distToEnd), vMinComfortable);
2626 : waypointWithStop = true;
2627 3398 : if (stopSpeed <= SUMO_const_haltingSpeed) {
2628 531 : const_cast<MSStop&>(stop).waypointWithStop = true;
2629 : }
2630 : }
2631 : }
2632 123407 : if (stop.reached) {
2633 14802 : stopSpeed = MIN2(stop.getSpeed(), stopSpeed);
2634 14802 : if (myState.myPos >= stop.pars.endPos && !waypointWithStop) {
2635 278 : stopDist = std::numeric_limits<double>::max();
2636 : }
2637 : } else {
2638 108605 : stopSpeed = MIN2(MAX2(cfModel.freeSpeed(this, getSpeed(), stopDist, stop.getSpeed()), vMinComfortable), stopSpeed);
2639 108605 : if (!stop.reached) {
2640 108605 : stopDist += stop.pars.endPos - stop.pars.startPos;
2641 : }
2642 108605 : if (lastLink != nullptr) {
2643 66583 : lastLink->adaptLeaveSpeed(cfModel.freeSpeed(this, vLinkPass, endPos, stop.getSpeed(), false, MSCFModel::CalcReason::FUTURE));
2644 : }
2645 : }
2646 : } else {
2647 15281191 : stopSpeed = cfModel.stopSpeed(this, getSpeed(), stopDist);
2648 15281191 : if (!instantStopping()) {
2649 : // regular stops are not emergencies
2650 : stopSpeed = MAX2(stopSpeed, vMinComfortable);
2651 20 : } else if (myInfluencer && !myInfluencer->hasSpeedTimeLine(SIMSTEP)) {
2652 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
2653 20 : speedTimeLine.push_back(std::make_pair(SIMSTEP, getSpeed()));
2654 20 : speedTimeLine.push_back(std::make_pair(SIMSTEP + DELTA_T, stopSpeed));
2655 20 : myInfluencer->setSpeedTimeLine(speedTimeLine);
2656 20 : }
2657 15281191 : if (lastLink != nullptr) {
2658 9545385 : lastLink->adaptLeaveSpeed(cfModel.stopSpeed(this, vLinkPass, endPos, MSCFModel::CalcReason::FUTURE));
2659 : }
2660 : }
2661 15404598 : if (stopSpeed < getSpeed() && getSpeed() > SUMO_const_haltingSpeed) {
2662 : // only discount braking-for-stop timeLoss if we are actually braking
2663 616399 : newStopSpeed = MIN2(newStopSpeed, stopSpeed);
2664 15096312 : } else if (getSpeed() < SUMO_const_haltingSpeed) {
2665 : // blocked from entering a stop
2666 7723688 : newStopSpeed = std::numeric_limits<double>::max();
2667 : }
2668 15404598 : v = MIN2(v, stopSpeed);
2669 15404598 : if (lane->isInternal()) {
2670 6980 : std::vector<MSLink*>::const_iterator exitLink = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2671 : assert(!lane->isLinkEnd(exitLink));
2672 : bool dummySetRequest;
2673 : double dummyVLinkWait;
2674 6980 : checkLinkLeaderCurrentAndParallel(*exitLink, lane, seen, lastLink, v, vLinkPass, dummyVLinkWait, dummySetRequest);
2675 : }
2676 :
2677 : #ifdef DEBUG_PLAN_MOVE
2678 : if (DEBUG_COND) {
2679 : std::cout << "\n" << SIMTIME << " next stop: distance = " << stopDist << " requires stopSpeed = " << stopSpeed << "\n";
2680 :
2681 : }
2682 : #endif
2683 15404598 : if (isFirstStop) {
2684 10318381 : newStopDist = stopDist;
2685 : // if the vehicle is going to stop we don't need to look further
2686 : // (except for trains that make use of further link-approach registration for safety purposes)
2687 10318381 : if (!isWaypoint) {
2688 : planningToStop = true;
2689 10230404 : if (!isRail()) {
2690 9906826 : lfLinks.emplace_back(v, stopDist);
2691 : foundRealStop = true;
2692 : break;
2693 : }
2694 : }
2695 : }
2696 : }
2697 : }
2698 : if (foundRealStop) {
2699 : break;
2700 : }
2701 :
2702 : // move to next lane
2703 : // get the next link used
2704 1223787587 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2705 1223787587 : if (lane->isLinkEnd(link) && myLaneChangeModel->hasBlueLight() && myCurrEdge != myRoute->end() - 1) {
2706 : // emergency vehicle is on the wrong lane. Obtain the link that it would use from the correct turning lane
2707 : const int currentIndex = lane->getIndex();
2708 : const MSLane* bestJump = nullptr;
2709 193386 : for (const LaneQ& preb : getBestLanes()) {
2710 127203 : if (preb.allowsContinuation &&
2711 : (bestJump == nullptr
2712 3218 : || abs(currentIndex - preb.lane->getIndex()) < abs(currentIndex - bestJump->getIndex()))) {
2713 67188 : bestJump = preb.lane;
2714 : }
2715 : }
2716 66183 : if (bestJump != nullptr) {
2717 66183 : const MSEdge* nextEdge = *(myCurrEdge + 1);
2718 122208 : for (auto cand_it = bestJump->getLinkCont().begin(); cand_it != bestJump->getLinkCont().end(); cand_it++) {
2719 116898 : if (&(*cand_it)->getLane()->getEdge() == nextEdge) {
2720 : link = cand_it;
2721 : break;
2722 : }
2723 : }
2724 : }
2725 : }
2726 :
2727 : // Check whether this is a turn (to save info about the next upcoming turn)
2728 1223787587 : if (!encounteredTurn) {
2729 194398262 : if (!lane->isLinkEnd(link) && lane->getLinkCont().size() > 1) {
2730 19396165 : LinkDirection linkDir = (*link)->getDirection();
2731 19396165 : switch (linkDir) {
2732 : case LinkDirection::STRAIGHT:
2733 : case LinkDirection::NODIR:
2734 : break;
2735 7377767 : default:
2736 7377767 : nextTurn.first = seen;
2737 7377767 : nextTurn.second = *link;
2738 : encounteredTurn = true;
2739 : #ifdef DEBUG_NEXT_TURN
2740 : if (DEBUG_COND) {
2741 : std::cout << SIMTIME << " veh '" << getID() << "' nextTurn: " << toString(linkDir)
2742 : << " at " << nextTurn.first << "m." << std::endl;
2743 : }
2744 : #endif
2745 : }
2746 : }
2747 : }
2748 :
2749 : // check whether the vehicle is on its final edge
2750 2113987203 : if (myCurrEdge + view + 1 == myRoute->end()
2751 1223787587 : || (myParameter->arrivalEdge >= 0 && getRoutePosition() + view == myParameter->arrivalEdge)) {
2752 333587971 : const double arrivalSpeed = (myParameter->arrivalSpeedProcedure == ArrivalSpeedDefinition::GIVEN ?
2753 : myParameter->arrivalSpeed : laneMaxV);
2754 : // subtract the arrival speed from the remaining distance so we get one additional driving step with arrival speed
2755 : // XXX: This does not work for ballistic update refs #2579
2756 333587971 : const double distToArrival = seen + myArrivalPos - lane->getLength() - SPEED2DIST(arrivalSpeed);
2757 333587971 : const double va = MAX2(NUMERICAL_EPS, cfModel.freeSpeed(this, getSpeed(), distToArrival, arrivalSpeed));
2758 333587971 : v = MIN2(v, va);
2759 333587971 : if (lastLink != nullptr) {
2760 : lastLink->adaptLeaveSpeed(va);
2761 : }
2762 333587971 : lfLinks.push_back(DriveProcessItem(v, seen, lane->getEdge().isFringe() ? 1000 : 0));
2763 333587971 : break;
2764 : }
2765 : // check whether the lane or the shadowLane is a dead end (allow some leeway on intersections)
2766 : if (lane->isLinkEnd(link)
2767 881092707 : || (MSGlobals::gSublane && brakeForOverlap(*link, lane))
2768 1770935066 : || (opposite && (*link)->getViaLaneOrLane()->getParallelOpposite() == nullptr
2769 209911 : && !myLaneChangeModel->hasBlueLight())) {
2770 9714192 : double va = cfModel.stopSpeed(this, getSpeed(), seen);
2771 9714192 : if (lastLink != nullptr) {
2772 : lastLink->adaptLeaveSpeed(va);
2773 : }
2774 9714192 : if (myLaneChangeModel->getCommittedSpeed() > 0) {
2775 378178 : v = MIN2(myLaneChangeModel->getCommittedSpeed(), v);
2776 : } else {
2777 18101626 : v = MIN2(va, v);
2778 : }
2779 : #ifdef DEBUG_PLAN_MOVE
2780 : if (DEBUG_COND) {
2781 : std::cout << " braking for link end lane=" << lane->getID() << " seen=" << seen
2782 : << " overlap=" << getLateralOverlap() << " va=" << va << " committed=" << myLaneChangeModel->getCommittedSpeed() << " v=" << v << "\n";
2783 :
2784 : }
2785 : #endif
2786 9714192 : if (lane->isLinkEnd(link)) {
2787 9106909 : lfLinks.emplace_back(v, seen);
2788 : break;
2789 : }
2790 : }
2791 881092707 : lateralShift += (*link)->getLateralShift();
2792 881092707 : const bool yellowOrRed = (*link)->haveRed() || (*link)->haveYellow();
2793 : // We distinguish 3 cases when determining the point at which a vehicle stops:
2794 : // - allway_stop: the vehicle should stop close to the stop line but may stop at larger distance
2795 : // - red/yellow light: here the vehicle 'knows' that it will have priority eventually and does not need to stop on a precise spot
2796 : // - other types of minor links: the vehicle needs to stop as close to the junction as necessary
2797 : // to minimize the time window for passing the junction. If the
2798 : // vehicle 'decides' to accelerate and cannot enter the junction in
2799 : // the next step, new foes may appear and cause a collision (see #1096)
2800 : // - major links: stopping point is irrelevant
2801 : double laneStopOffset;
2802 881092707 : const double majorStopOffset = MAX2(getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP, DIST_TO_STOPLINE_EXPECT_PRIORITY), lane->getVehicleStopOffset(this));
2803 : // override low desired decel at yellow and red
2804 881092707 : const double stopDecel = yellowOrRed && !isRail() ? MAX2(MIN2(MSGlobals::gTLSYellowMinDecel, cfModel.getEmergencyDecel()), cfModel.getMaxDecel()) : cfModel.getMaxDecel();
2805 881092707 : const double brakeDist = cfModel.brakeGap(myState.mySpeed, stopDecel, 0);
2806 881092707 : const bool canBrakeBeforeLaneEnd = seen >= brakeDist;
2807 881092707 : const bool canBrakeBeforeStopLine = seen - lane->getVehicleStopOffset(this) >= brakeDist;
2808 881092707 : if (yellowOrRed) {
2809 : // Wait at red traffic light with full distance if possible
2810 : laneStopOffset = majorStopOffset;
2811 819305512 : } else if ((*link)->havePriority()) {
2812 : // On priority link, we should never stop below visibility distance
2813 775241762 : laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, majorStopOffset);
2814 : } else {
2815 44063750 : double minorStopOffset = MAX2(lane->getVehicleStopOffset(this),
2816 44063750 : getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP) - (*link)->getDistToFoePedCrossing());
2817 : #ifdef DEBUG_PLAN_MOVE
2818 : if (DEBUG_COND) {
2819 : std::cout << " minorStopOffset=" << minorStopOffset << " distToFoePedCrossing=" << (*link)->getDistToFoePedCrossing() << "\n";
2820 : }
2821 : #endif
2822 44063750 : if ((*link)->getState() == LINKSTATE_ALLWAY_STOP) {
2823 1426424 : minorStopOffset = MAX2(minorStopOffset, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP, 0));
2824 : } else {
2825 42637326 : minorStopOffset = MAX2(minorStopOffset, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP_MINOR, 0));
2826 : }
2827 : // On minor link, we should likewise never stop below visibility distance
2828 44063750 : laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, minorStopOffset);
2829 : }
2830 : #ifdef DEBUG_PLAN_MOVE
2831 : if (DEBUG_COND) {
2832 : std::cout << SIMTIME << " veh=" << getID() << " desired stopOffset on lane '" << lane->getID() << "' is " << laneStopOffset << "\n";
2833 : }
2834 : #endif
2835 881092707 : if (canBrakeBeforeLaneEnd) {
2836 : // avoid emergency braking if possible
2837 853355689 : laneStopOffset = MIN2(laneStopOffset, seen - brakeDist);
2838 : }
2839 : laneStopOffset = MAX2(POSITION_EPS, laneStopOffset);
2840 881092707 : double stopDist = MAX2(0., seen - laneStopOffset);
2841 61787195 : if (yellowOrRed && getDevice(typeid(MSDevice_GLOSA)) != nullptr
2842 604 : && static_cast<MSDevice_GLOSA*>(getDevice(typeid(MSDevice_GLOSA)))->getOverrideSafety()
2843 881092707 : && static_cast<MSDevice_GLOSA*>(getDevice(typeid(MSDevice_GLOSA)))->isSpeedAdviceActive()) {
2844 : stopDist = std::numeric_limits<double>::max();
2845 : }
2846 881092707 : if (newStopDist != std::numeric_limits<double>::max()) {
2847 : stopDist = MAX2(stopDist, newStopDist);
2848 : }
2849 : #ifdef DEBUG_PLAN_MOVE
2850 : if (DEBUG_COND) {
2851 : std::cout << SIMTIME << " veh=" << getID() << " effective stopOffset on lane '" << lane->getID()
2852 : << "' is " << laneStopOffset << " (-> stopDist=" << stopDist << ")" << std::endl;
2853 : }
2854 : #endif
2855 881092707 : if (isRail()
2856 881092707 : && !lane->isInternal()) {
2857 : // check for train direction reversal
2858 3211288 : if (lane->getBidiLane() != nullptr
2859 3211288 : && (*link)->getLane()->getBidiLane() == lane) {
2860 631456 : double vMustReverse = getCarFollowModel().stopSpeed(this, getSpeed(), seen - POSITION_EPS);
2861 631456 : if (seen < 1) {
2862 2277 : mustSeeBeforeReversal = 2 * seen + getLength();
2863 : }
2864 1221824 : v = MIN2(v, vMustReverse);
2865 : }
2866 : // signal that is passed in the current step does not count
2867 6422576 : foundRailSignal |= ((*link)->getTLLogic() != nullptr
2868 744856 : && (*link)->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL
2869 3904057 : && seen > SPEED2DIST(v));
2870 : }
2871 :
2872 881092707 : bool canReverseEventually = false;
2873 881092707 : const double vReverse = checkReversal(canReverseEventually, laneMaxV, seen);
2874 881092707 : v = MIN2(v, vReverse);
2875 : #ifdef DEBUG_PLAN_MOVE
2876 : if (DEBUG_COND) {
2877 : std::cout << SIMTIME << " veh=" << getID() << " canReverseEventually=" << canReverseEventually << " v=" << v << "\n";
2878 : }
2879 : #endif
2880 :
2881 : // check whether we need to slow down in order to finish a continuous lane change
2882 881092707 : if (myLaneChangeModel->isChangingLanes()) {
2883 : if ( // slow down to finish lane change before a turn lane
2884 179873 : ((*link)->getDirection() == LinkDirection::LEFT || (*link)->getDirection() == LinkDirection::RIGHT) ||
2885 : // slow down to finish lane change before the shadow lane ends
2886 146840 : (myLaneChangeModel->getShadowLane() != nullptr &&
2887 146840 : (*link)->getViaLaneOrLane()->getParallelLane(myLaneChangeModel->getShadowDirection()) == nullptr)) {
2888 : // XXX maybe this is too harsh. Vehicles could cut some corners here
2889 54613 : const double timeRemaining = STEPS2TIME(myLaneChangeModel->remainingTime());
2890 : assert(timeRemaining != 0);
2891 : // XXX: Euler-logic (#860), but I couldn't identify problems from this yet (Leo). Refs. #2575
2892 54613 : const double va = MAX2(cfModel.stopSpeed(this, getSpeed(), seen - POSITION_EPS),
2893 54613 : (seen - POSITION_EPS) / timeRemaining);
2894 : #ifdef DEBUG_PLAN_MOVE
2895 : if (DEBUG_COND) {
2896 : std::cout << SIMTIME << " veh=" << getID() << " slowing down to finish continuous change before"
2897 : << " link=" << (*link)->getViaLaneOrLane()->getID()
2898 : << " timeRemaining=" << timeRemaining
2899 : << " v=" << v
2900 : << " va=" << va
2901 : << std::endl;
2902 : }
2903 : #endif
2904 108730 : v = MIN2(va, v);
2905 : }
2906 : }
2907 :
2908 : // - always issue a request to leave the intersection we are currently on
2909 881092707 : const bool leavingCurrentIntersection = myLane->getEdge().isInternal() && lastLink == nullptr;
2910 : // - do not issue a request to enter an intersection after we already slowed down for an earlier one
2911 881092707 : const bool abortRequestAfterMinor = slowedDownForMinor && (*link)->getInternalLaneBefore() == nullptr;
2912 : // - even if red, if we cannot break we should issue a request
2913 881092707 : bool setRequest = (v > NUMERICAL_EPS_SPEED && !abortRequestAfterMinor) || (leavingCurrentIntersection);
2914 :
2915 881092707 : double stopSpeed = cfModel.stopSpeed(this, getSpeed(), stopDist, stopDecel, MSCFModel::CalcReason::CURRENT_WAIT);
2916 881092707 : double vLinkWait = MIN2(v, stopSpeed);
2917 : #ifdef DEBUG_PLAN_MOVE
2918 : if (DEBUG_COND) {
2919 : std::cout
2920 : << " stopDist=" << stopDist
2921 : << " stopDecel=" << stopDecel
2922 : << " vLinkWait=" << vLinkWait
2923 : << " brakeDist=" << brakeDist
2924 : << " seen=" << seen
2925 : << " leaveIntersection=" << leavingCurrentIntersection
2926 : << " setRequest=" << setRequest
2927 : //<< std::setprecision(16)
2928 : //<< " v=" << v
2929 : //<< " speedEps=" << NUMERICAL_EPS_SPEED
2930 : //<< std::setprecision(gPrecision)
2931 : << "\n";
2932 : }
2933 : #endif
2934 :
2935 881092707 : if (yellowOrRed && canBrakeBeforeStopLine && !ignoreRed(*link, canBrakeBeforeStopLine) && seen >= mustSeeBeforeReversal) {
2936 61725321 : if (lane->isInternal()) {
2937 43055 : checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2938 : }
2939 : // arrivalSpeed / arrivalTime when braking for red light is only relevent for rail signal switching
2940 61725321 : const SUMOTime arrivalTime = getArrivalTime(t, seen, v, vLinkPass);
2941 : // the vehicle is able to brake in front of a yellow/red traffic light
2942 61725321 : lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, false, arrivalTime, vLinkWait, 0, seen, -1));
2943 : //lfLinks.push_back(DriveProcessItem(0, vLinkWait, vLinkWait, false, 0, 0, stopDist));
2944 61725321 : break;
2945 : }
2946 :
2947 819367386 : const MSLink* entryLink = (*link)->getCorrespondingEntryLink();
2948 819367386 : if (entryLink->haveRed() && ignoreRed(*link, canBrakeBeforeStopLine) && STEPS2TIME(t - entryLink->getLastStateChange()) > 2) {
2949 : // restrict speed when ignoring a red light
2950 117832 : const double redSpeed = MIN2(v, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_RED_SPEED, v));
2951 117832 : const double va = MAX2(redSpeed, cfModel.freeSpeed(this, getSpeed(), seen, redSpeed));
2952 235229 : v = MIN2(va, v);
2953 : #ifdef DEBUG_PLAN_MOVE
2954 : if (DEBUG_COND) std::cout
2955 : << " ignoreRed spent=" << STEPS2TIME(t - (*link)->getLastStateChange())
2956 : << " redSpeed=" << redSpeed
2957 : << " va=" << va
2958 : << " v=" << v
2959 : << "\n";
2960 : #endif
2961 : }
2962 :
2963 819367386 : checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2964 :
2965 819367386 : if (lastLink != nullptr) {
2966 : lastLink->adaptLeaveSpeed(laneMaxV);
2967 : }
2968 819367386 : double arrivalSpeed = vLinkPass;
2969 : // vehicles should decelerate when approaching a minor link
2970 : // - unless they are close enough to have clear visibility of all relevant foe lanes and may start to accelerate again
2971 : // - and unless they are so close that stopping is impossible (i.e. when a green light turns to yellow when close to the junction)
2972 :
2973 : // whether the vehicle/driver is close enough to the link to see all possible foes #2123
2974 819367386 : const double visibilityDistance = (*link)->getFoeVisibilityDistance();
2975 819367386 : const double determinedFoePresence = seen <= visibilityDistance;
2976 : // // VARIANT: account for time needed to recognize whether relevant vehicles are on the foe lanes. (Leo)
2977 : // double foeRecognitionTime = 0.0;
2978 : // double determinedFoePresence = seen < visibilityDistance - myState.mySpeed*foeRecognitionTime;
2979 :
2980 : #ifdef DEBUG_PLAN_MOVE
2981 : if (DEBUG_COND) {
2982 : std::cout << " approaching link=" << (*link)->getViaLaneOrLane()->getID() << " prio=" << (*link)->havePriority() << " seen=" << seen << " visibilityDistance=" << visibilityDistance << " brakeDist=" << brakeDist << "\n";
2983 : }
2984 : #endif
2985 :
2986 819367386 : const bool couldBrakeForMinor = !(*link)->havePriority() && brakeDist < seen && !(*link)->lastWasContMajor();
2987 43535841 : if (couldBrakeForMinor && !determinedFoePresence) {
2988 : // vehicle decelerates just enough to be able to stop if necessary and then accelerates
2989 40746426 : double maxSpeedAtVisibilityDist = cfModel.maximumSafeStopSpeed(visibilityDistance, cfModel.getMaxDecel(), myState.mySpeed, false, 0., false);
2990 : // XXX: estimateSpeedAfterDistance does not use euler-logic (thus returns a lower value than possible here...)
2991 40746426 : double maxArrivalSpeed = cfModel.estimateSpeedAfterDistance(visibilityDistance, maxSpeedAtVisibilityDist, cfModel.getMaxAccel());
2992 40746426 : arrivalSpeed = MIN2(vLinkPass, maxArrivalSpeed);
2993 : slowedDownForMinor = true;
2994 : #ifdef DEBUG_PLAN_MOVE
2995 : if (DEBUG_COND) {
2996 : std::cout << " slowedDownForMinor maxSpeedAtVisDist=" << maxSpeedAtVisibilityDist << " maxArrivalSpeed=" << maxArrivalSpeed << " arrivalSpeed=" << arrivalSpeed << "\n";
2997 : }
2998 : #endif
2999 778620960 : } else if ((*link)->getState() == LINKSTATE_EQUAL && myWaitingTime > 0) {
3000 : // check for deadlock (circular yielding)
3001 : //std::cout << SIMTIME << " veh=" << getID() << " check rbl-deadlock\n";
3002 2851 : std::pair<const SUMOVehicle*, const MSLink*> blocker = (*link)->getFirstApproachingFoe(*link);
3003 : //std::cout << " blocker=" << Named::getIDSecure(blocker.first) << "\n";
3004 : int n = 100;
3005 5817 : while (blocker.second != nullptr && blocker.second != *link && n > 0) {
3006 2966 : blocker = blocker.second->getFirstApproachingFoe(*link);
3007 2966 : n--;
3008 : //std::cout << " blocker=" << Named::getIDSecure(blocker.first) << "\n";
3009 : }
3010 2851 : if (n == 0) {
3011 0 : WRITE_WARNINGF(TL("Suspicious right_before_left junction '%'."), lane->getEdge().getToJunction()->getID());
3012 : }
3013 : //std::cout << " blockerLink=" << blocker.second << " link=" << *link << "\n";
3014 2851 : if (blocker.second == *link) {
3015 520 : const double threshold = (*link)->getDirection() == LinkDirection::STRAIGHT ? 0.25 : 0.75;
3016 520 : if (RandHelper::rand(getRNG()) < threshold) {
3017 : //std::cout << " abort request, threshold=" << threshold << "\n";
3018 317 : setRequest = false;
3019 : }
3020 : }
3021 : }
3022 :
3023 819367386 : const SUMOTime arrivalTime = getArrivalTime(t, seen, v, arrivalSpeed);
3024 819367386 : if (couldBrakeForMinor && determinedFoePresence && (*link)->getLane()->getEdge().isRoundabout()) {
3025 885354 : const bool wasOpened = (*link)->opened(arrivalTime, arrivalSpeed, arrivalSpeed,
3026 885354 : getLength(), getImpatience(),
3027 : getCarFollowModel().getMaxDecel(),
3028 885354 : getWaitingTime(), getLateralPositionOnLane(),
3029 : nullptr, false, this);
3030 885354 : if (!wasOpened) {
3031 : slowedDownForMinor = true;
3032 : }
3033 : #ifdef DEBUG_PLAN_MOVE
3034 : if (DEBUG_COND) {
3035 : std::cout << " slowedDownForMinor at roundabout=" << (!wasOpened) << "\n";
3036 : }
3037 : #endif
3038 : }
3039 :
3040 : // compute arrival speed and arrival time if vehicle starts braking now
3041 : // if stopping is possible, arrivalTime can be arbitrarily large. A small value keeps fractional times (impatience) meaningful
3042 : double arrivalSpeedBraking = 0;
3043 819367386 : const double bGap = cfModel.brakeGap(v);
3044 819367386 : if (seen < bGap && !isStopped() && !planningToStop) { // XXX: should this use the current speed (at least for the ballistic case)? (Leo) Refs. #2575
3045 : // vehicle cannot come to a complete stop in time
3046 57333614 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3047 54600530 : arrivalSpeedBraking = cfModel.getMinimalArrivalSpeedEuler(seen, v);
3048 : // due to discrete/continuous mismatch (when using Euler update) we have to ensure that braking actually helps
3049 : arrivalSpeedBraking = MIN2(arrivalSpeedBraking, arrivalSpeed);
3050 : } else {
3051 2733084 : arrivalSpeedBraking = cfModel.getMinimalArrivalSpeed(seen, myState.mySpeed);
3052 : }
3053 : }
3054 :
3055 : // estimate leave speed for passing time computation
3056 : // l=linkLength, a=accel, t=continuousTime, v=vLeave
3057 : // l=v*t + 0.5*a*t^2, solve for t and multiply with a, then add v
3058 1211997407 : const double estimatedLeaveSpeed = MIN2((*link)->getViaLaneOrLane()->getVehicleMaxSpeed(this, maxVD),
3059 819367386 : getCarFollowModel().estimateSpeedAfterDistance((*link)->getLength(), arrivalSpeed, getVehicleType().getCarFollowModel().getMaxAccel()));
3060 819367386 : lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, setRequest,
3061 : arrivalTime, arrivalSpeed,
3062 : arrivalSpeedBraking,
3063 : seen, estimatedLeaveSpeed));
3064 819367386 : if ((*link)->getViaLane() == nullptr) {
3065 : hadNonInternal = true;
3066 : ++view;
3067 : }
3068 : #ifdef DEBUG_PLAN_MOVE
3069 : if (DEBUG_COND) {
3070 : std::cout << " checkAbort setRequest=" << setRequest << " v=" << v << " seen=" << seen << " dist=" << dist
3071 : << " seenNonInternal=" << seenNonInternal
3072 : << " seenInternal=" << seenInternal << " length=" << vehicleLength << "\n";
3073 : }
3074 : #endif
3075 : // we need to look ahead far enough to see available space for checkRewindLinkLanes
3076 845400258 : if ((!setRequest || v <= 0 || seen > dist) && hadNonInternal && seenNonInternal > MAX2(vehicleLength * CRLL_LOOK_AHEAD, vehicleLength + seenInternal) && foundRailSignal) {
3077 : break;
3078 : }
3079 : // get the following lane
3080 : lane = (*link)->getViaLaneOrLane();
3081 596432862 : laneMaxV = lane->getVehicleMaxSpeed(this, maxVD);
3082 596832839 : if (myInfluencer && !myInfluencer->considerSpeedLimit()) {
3083 : laneMaxV = std::numeric_limits<double>::max();
3084 : }
3085 : // the link was passed
3086 : // compute the velocity to use when the link is not blocked by other vehicles
3087 : // the vehicle shall be not faster when reaching the next lane than allowed
3088 : // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
3089 596432862 : const double va = MAX2(cfModel.freeSpeed(this, getSpeed(), seen, laneMaxV), vMinComfortable - NUMERICAL_EPS);
3090 1183087960 : v = MIN2(va, v);
3091 : #ifdef DEBUG_PLAN_MOVE
3092 : if (DEBUG_COND) {
3093 : std::cout << " laneMaxV=" << laneMaxV << " freeSpeed=" << va << " v=" << v << "\n";
3094 : }
3095 : #endif
3096 596432862 : if (lane->getEdge().isInternal()) {
3097 260956501 : seenInternal += lane->getLength();
3098 : } else {
3099 335476361 : seenNonInternal += lane->getLength();
3100 : }
3101 : // do not restrict results to the current vehicle to allow caching for the current time step
3102 596432862 : leaderLane = opposite ? lane->getParallelOpposite() : lane;
3103 596432862 : if (leaderLane == nullptr) {
3104 :
3105 : break;
3106 : }
3107 1192449536 : ahead = opposite ? MSLeaderInfo(leaderLane->getWidth()) : leaderLane->getLastVehicleInformation(nullptr, 0);
3108 596224768 : seen += lane->getLength();
3109 1192449536 : vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(lane->getLength(), v, cfModel.getMaxAccel()), laneMaxV); // upper bound
3110 : lastLink = &lfLinks.back();
3111 596224768 : }
3112 :
3113 : //#ifdef DEBUG_PLAN_MOVE
3114 : // if(DEBUG_COND){
3115 : // std::cout << "planMoveInternal found safe speed v = " << v << std::endl;
3116 : // }
3117 : //#endif
3118 :
3119 : #ifdef PARALLEL_STOPWATCH
3120 : myLane->getStopWatch()[0].stop();
3121 : #endif
3122 637469645 : }
3123 :
3124 :
3125 : double
3126 2786 : MSVehicle::slowDownForSchedule(double vMinComfortable) const {
3127 2786 : const double sfp = getVehicleType().getParameter().speedFactorPremature;
3128 : const MSStop& stop = myStops.front();
3129 2786 : std::pair<double, double> timeDist = estimateTimeToNextStop();
3130 2786 : double arrivalDelay = SIMTIME + timeDist.first - STEPS2TIME(stop.pars.arrival);
3131 2786 : double t = STEPS2TIME(stop.pars.arrival - SIMSTEP);
3132 5572 : if (stop.pars.hasParameter(toString(SUMO_ATTR_FLEX_ARRIVAL))) {
3133 150 : SUMOTime flexStart = string2time(stop.pars.getParameter(toString(SUMO_ATTR_FLEX_ARRIVAL)));
3134 75 : arrivalDelay += STEPS2TIME(stop.pars.arrival - flexStart);
3135 75 : t = STEPS2TIME(flexStart - SIMSTEP);
3136 2711 : } else if (stop.pars.started >= 0 && MSGlobals::gUseStopStarted) {
3137 200 : arrivalDelay += STEPS2TIME(stop.pars.arrival - stop.pars.started);
3138 200 : t = STEPS2TIME(stop.pars.started - SIMSTEP);
3139 : }
3140 2786 : if (arrivalDelay < 0 && sfp < getChosenSpeedFactor()) {
3141 : // we can slow down to better match the schedule (and increase energy efficiency)
3142 2721 : const double vSlowDownMin = MAX2(myLane->getSpeedLimit() * sfp, vMinComfortable);
3143 2721 : const double s = timeDist.second;
3144 : const double b = getCarFollowModel().getMaxDecel();
3145 : // x = speed for arriving in t seconds
3146 : // u = time at full speed
3147 : // u * x + (t - u) * 0.5 * x = s
3148 : // t - u = x / b
3149 : // eliminate u, solve x
3150 2721 : const double radicand = 4 * t * t * b * b - 8 * s * b;
3151 2721 : const double x = radicand >= 0 ? t * b - sqrt(radicand) * 0.5 : vSlowDownMin;
3152 2721 : double vSlowDown = x < vSlowDownMin ? vSlowDownMin : x;
3153 : #ifdef DEBUG_PLAN_MOVE
3154 : if (DEBUG_COND) {
3155 : std::cout << SIMTIME << " veh=" << getID() << " ad=" << arrivalDelay << " t=" << t << " vsm=" << vSlowDownMin
3156 : << " r=" << radicand << " vs=" << vSlowDown << "\n";
3157 : }
3158 : #endif
3159 2721 : return vSlowDown;
3160 65 : } else if (arrivalDelay > 0 && sfp > getChosenSpeedFactor()) {
3161 : // in principle we could up to catch up with the schedule
3162 : // but at this point we can only lower the speed, the
3163 : // information would have to be used when computing getVehicleMaxSpeed
3164 : }
3165 65 : return getMaxSpeed();
3166 : }
3167 :
3168 : SUMOTime
3169 881092707 : MSVehicle::getArrivalTime(SUMOTime t, double seen, double v, double arrivalSpeed) const {
3170 : const MSCFModel& cfModel = getCarFollowModel();
3171 : SUMOTime arrivalTime;
3172 881092707 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3173 : // @note intuitively it would make sense to compare arrivalSpeed with getSpeed() instead of v
3174 : // however, due to the current position update rule (ticket #860) the vehicle moves with v in this step
3175 : // subtract DELTA_T because t is the time at the end of this step and the movement is not carried out yet
3176 824082338 : arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, v, arrivalSpeed);
3177 : } else {
3178 57010369 : arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, myState.mySpeed, arrivalSpeed);
3179 : }
3180 881092707 : if (isStopped()) {
3181 12723396 : arrivalTime += MAX2((SUMOTime)0, myStops.front().duration);
3182 : }
3183 881092707 : return arrivalTime;
3184 : }
3185 :
3186 :
3187 : void
3188 1238666938 : MSVehicle::adaptToLeaders(const MSLeaderInfo& ahead, double latOffset,
3189 : const double seen, DriveProcessItem* const lastLink,
3190 : const MSLane* const lane, double& v, double& vLinkPass) const {
3191 : int rightmost;
3192 : int leftmost;
3193 1238666938 : ahead.getSubLanes(this, latOffset, rightmost, leftmost);
3194 : #ifdef DEBUG_PLAN_MOVE
3195 : if (DEBUG_COND) std::cout << SIMTIME
3196 : << "\nADAPT_TO_LEADERS\nveh=" << getID()
3197 : << " lane=" << lane->getID()
3198 : << " latOffset=" << latOffset
3199 : << " rm=" << rightmost
3200 : << " lm=" << leftmost
3201 : << " shift=" << ahead.getSublaneOffset()
3202 : << " ahead=" << ahead.toString()
3203 : << "\n";
3204 : #endif
3205 : /*
3206 : if (myLaneChangeModel->getCommittedSpeed() > 0) {
3207 : v = MIN2(v, myLaneChangeModel->getCommittedSpeed());
3208 : vLinkPass = MIN2(vLinkPass, myLaneChangeModel->getCommittedSpeed());
3209 : #ifdef DEBUG_PLAN_MOVE
3210 : if (DEBUG_COND) std::cout << " hasCommitted=" << myLaneChangeModel->getCommittedSpeed() << "\n";
3211 : #endif
3212 : return;
3213 : }
3214 : */
3215 3040685605 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
3216 1802018667 : const MSVehicle* pred = ahead[sublane];
3217 1802018667 : if (pred != nullptr && pred != this) {
3218 : // @todo avoid multiple adaptations to the same leader
3219 1317561784 : const double predBack = pred->getBackPositionOnLane(lane);
3220 : double gap = (lastLink == nullptr
3221 1893189901 : ? predBack - myState.myPos - getVehicleType().getMinGap()
3222 575628117 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3223 : bool oncoming = false;
3224 1317561784 : if (myLaneChangeModel->isOpposite()) {
3225 26296 : if (pred->getLaneChangeModel().isOpposite() || lane == pred->getLaneChangeModel().getShadowLane()) {
3226 : // ego might and leader are driving against lane
3227 : gap = (lastLink == nullptr
3228 0 : ? myState.myPos - predBack - getVehicleType().getMinGap()
3229 0 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3230 : } else {
3231 : // ego and leader are driving in the same direction as lane (shadowlane for ego)
3232 : gap = (lastLink == nullptr
3233 26996 : ? predBack - (myLane->getLength() - myState.myPos) - getVehicleType().getMinGap()
3234 700 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3235 : }
3236 1317535488 : } else if (pred->getLaneChangeModel().isOpposite() && pred->getLaneChangeModel().getShadowLane() != lane) {
3237 : // must react to stopped / dangerous oncoming vehicles
3238 184628 : gap += -pred->getVehicleType().getLength() + getVehicleType().getMinGap() - MAX2(getVehicleType().getMinGap(), pred->getVehicleType().getMinGap());
3239 : // try to avoid collision in the next second
3240 184628 : const double predMaxDist = pred->getSpeed() + pred->getCarFollowModel().getMaxAccel();
3241 : #ifdef DEBUG_PLAN_MOVE
3242 : if (DEBUG_COND) {
3243 : std::cout << " fixedGap=" << gap << " predMaxDist=" << predMaxDist << "\n";
3244 : }
3245 : #endif
3246 184628 : if (gap < predMaxDist + getSpeed() || pred->getLane() == lane->getBidiLane()) {
3247 20613 : gap -= predMaxDist;
3248 : }
3249 1317350860 : } else if (pred->getLane() == lane->getBidiLane()) {
3250 168547 : gap -= pred->getVehicleType().getLengthWithGap();
3251 : oncoming = true;
3252 : }
3253 : #ifdef DEBUG_PLAN_MOVE
3254 : if (DEBUG_COND) {
3255 : 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";
3256 : }
3257 : #endif
3258 168547 : if (oncoming && gap >= 0) {
3259 148218 : adaptToOncomingLeader(std::make_pair(pred, gap), lastLink, v, vLinkPass);
3260 : } else {
3261 1317413566 : adaptToLeader(std::make_pair(pred, gap), seen, lastLink, v, vLinkPass);
3262 : }
3263 : }
3264 : }
3265 1238666938 : }
3266 :
3267 : void
3268 429744 : MSVehicle::adaptToLeaderDistance(const MSLeaderDistanceInfo& ahead, double latOffset,
3269 : double seen,
3270 : DriveProcessItem* const lastLink,
3271 : double& v, double& vLinkPass) const {
3272 : int rightmost;
3273 : int leftmost;
3274 429744 : ahead.getSubLanes(this, latOffset, rightmost, leftmost);
3275 : #ifdef DEBUG_PLAN_MOVE
3276 : if (DEBUG_COND) std::cout << SIMTIME
3277 : << "\nADAPT_TO_LEADERS_DISTANCE\nveh=" << getID()
3278 : << " latOffset=" << latOffset
3279 : << " rm=" << rightmost
3280 : << " lm=" << leftmost
3281 : << " ahead=" << ahead.toString()
3282 : << "\n";
3283 : #endif
3284 1055028 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
3285 625284 : CLeaderDist predDist = ahead[sublane];
3286 625284 : const MSVehicle* pred = predDist.first;
3287 625284 : if (pred != nullptr && pred != this) {
3288 : #ifdef DEBUG_PLAN_MOVE
3289 : if (DEBUG_COND) {
3290 : std::cout << " pred=" << pred->getID() << " predLane=" << pred->getLane()->getID() << " predPos=" << pred->getPositionOnLane() << " gap=" << predDist.second << "\n";
3291 : }
3292 : #endif
3293 385482 : adaptToLeader(predDist, seen, lastLink, v, vLinkPass);
3294 : }
3295 : }
3296 429744 : }
3297 :
3298 :
3299 : void
3300 1317799048 : MSVehicle::adaptToLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3301 : double seen,
3302 : DriveProcessItem* const lastLink,
3303 : double& v, double& vLinkPass) const {
3304 1317799048 : if (leaderInfo.first != 0) {
3305 1317799048 : if (ignoreFoe(leaderInfo.first)) {
3306 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3307 : if (DEBUG_COND) {
3308 : std::cout << " foe ignored\n";
3309 : }
3310 : #endif
3311 : return;
3312 : }
3313 : const MSCFModel& cfModel = getCarFollowModel();
3314 : double vsafeLeader = 0;
3315 1317798242 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
3316 : vsafeLeader = -std::numeric_limits<double>::max();
3317 : }
3318 : bool backOnRoute = true;
3319 1317798242 : if (leaderInfo.second < 0 && lastLink != nullptr && lastLink->myLink != nullptr) {
3320 : backOnRoute = false;
3321 : // this can either be
3322 : // a) a merging situation (leader back is is not our route) or
3323 : // b) a minGap violation / collision
3324 : MSLane* current = lastLink->myLink->getViaLaneOrLane();
3325 240652 : if (leaderInfo.first->getBackLane() == current) {
3326 : backOnRoute = true;
3327 : } else {
3328 553662 : for (MSLane* lane : getBestLanesContinuation()) {
3329 507939 : if (lane == current) {
3330 : break;
3331 : }
3332 356257 : if (leaderInfo.first->getBackLane() == lane) {
3333 : backOnRoute = true;
3334 : }
3335 : }
3336 : }
3337 : #ifdef DEBUG_PLAN_MOVE
3338 : if (DEBUG_COND) {
3339 : std::cout << SIMTIME << " current=" << current->getID() << " leaderBackLane=" << leaderInfo.first->getBackLane()->getID() << " backOnRoute=" << backOnRoute << "\n";
3340 : }
3341 : #endif
3342 197405 : if (!backOnRoute) {
3343 124366 : double stopDist = seen - current->getLength() - POSITION_EPS;
3344 124366 : if (lastLink->myLink->getInternalLaneBefore() != nullptr) {
3345 : // do not drive onto the junction conflict area
3346 106772 : stopDist -= lastLink->myLink->getInternalLaneBefore()->getLength();
3347 : }
3348 124366 : vsafeLeader = cfModel.stopSpeed(this, getSpeed(), stopDist);
3349 : }
3350 : }
3351 167613 : if (backOnRoute) {
3352 1317673876 : vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3353 : }
3354 1317798242 : if (lastLink != nullptr) {
3355 575578443 : const double futureVSafe = cfModel.followSpeed(this, lastLink->accelV, leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first, MSCFModel::CalcReason::FUTURE);
3356 : lastLink->adaptLeaveSpeed(futureVSafe);
3357 : #ifdef DEBUG_PLAN_MOVE
3358 : if (DEBUG_COND) {
3359 : std::cout << " vlinkpass=" << lastLink->myVLinkPass << " futureVSafe=" << futureVSafe << "\n";
3360 : }
3361 : #endif
3362 : }
3363 1317798242 : v = MIN2(v, vsafeLeader);
3364 2267843969 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3365 : #ifdef DEBUG_PLAN_MOVE
3366 : if (DEBUG_COND) std::cout
3367 : << SIMTIME
3368 : //std::cout << std::setprecision(10);
3369 : << " veh=" << getID()
3370 : << " lead=" << leaderInfo.first->getID()
3371 : << " leadSpeed=" << leaderInfo.first->getSpeed()
3372 : << " gap=" << leaderInfo.second
3373 : << " leadLane=" << leaderInfo.first->getLane()->getID()
3374 : << " predPos=" << leaderInfo.first->getPositionOnLane()
3375 : << " myLane=" << myLane->getID()
3376 : << " v=" << v
3377 : << " vSafeLeader=" << vsafeLeader
3378 : << " vLinkPass=" << vLinkPass
3379 : << "\n";
3380 : #endif
3381 : }
3382 : }
3383 :
3384 :
3385 : void
3386 18636400 : MSVehicle::adaptToJunctionLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3387 : const double seen, DriveProcessItem* const lastLink,
3388 : const MSLane* const lane, double& v, double& vLinkPass,
3389 : double distToCrossing) const {
3390 18636400 : if (leaderInfo.first != 0) {
3391 18636400 : if (ignoreFoe(leaderInfo.first)) {
3392 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3393 : if (DEBUG_COND) {
3394 : std::cout << " junction foe ignored\n";
3395 : }
3396 : #endif
3397 : return;
3398 : }
3399 : const MSCFModel& cfModel = getCarFollowModel();
3400 : double vsafeLeader = 0;
3401 18636389 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
3402 : vsafeLeader = -std::numeric_limits<double>::max();
3403 : }
3404 18636389 : if (leaderInfo.second >= 0) {
3405 15635075 : if (hasDeparted()) {
3406 15630012 : vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3407 : } else {
3408 : // called in the context of MSLane::isInsertionSuccess
3409 5063 : vsafeLeader = cfModel.insertionFollowSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3410 : }
3411 3001314 : } else if (leaderInfo.first != this) {
3412 : // the leading, in-lapping vehicle is occupying the complete next lane
3413 : // stop before entering this lane
3414 2579970 : vsafeLeader = cfModel.stopSpeed(this, getSpeed(), seen - lane->getLength() - POSITION_EPS);
3415 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3416 : if (DEBUG_COND) {
3417 : std::cout << SIMTIME << " veh=" << getID() << " stopping before junction: lane=" << lane->getID() << " seen=" << seen
3418 : << " laneLength=" << lane->getLength()
3419 : << " stopDist=" << seen - lane->getLength() - POSITION_EPS
3420 : << " vsafeLeader=" << vsafeLeader
3421 : << " distToCrossing=" << distToCrossing
3422 : << "\n";
3423 : }
3424 : #endif
3425 : }
3426 18636389 : if (distToCrossing >= 0) {
3427 : // can the leader still stop in the way?
3428 5902100 : const double vStop = cfModel.stopSpeed(this, getSpeed(), distToCrossing - getVehicleType().getMinGap());
3429 5902100 : if (leaderInfo.first == this) {
3430 : // braking for pedestrian
3431 410119 : const double vStopCrossing = cfModel.stopSpeed(this, getSpeed(), distToCrossing);
3432 : vsafeLeader = vStopCrossing;
3433 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3434 : if (DEBUG_COND) {
3435 : std::cout << " breaking for pedestrian distToCrossing=" << distToCrossing << " vStopCrossing=" << vStopCrossing << "\n";
3436 : }
3437 : #endif
3438 410119 : if (lastLink != nullptr) {
3439 : lastLink->adaptStopSpeed(vsafeLeader);
3440 : }
3441 5491981 : } else if (leaderInfo.second == -std::numeric_limits<double>::max()) {
3442 : // drive up to the crossing point and stop
3443 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3444 : if (DEBUG_COND) {
3445 : std::cout << " stop at crossing point for critical leader vStop=" << vStop << "\n";
3446 : };
3447 : #endif
3448 : vsafeLeader = MAX2(vsafeLeader, vStop);
3449 : } else {
3450 5434898 : const double leaderDistToCrossing = distToCrossing - leaderInfo.second;
3451 : // estimate the time at which the leader has gone past the crossing point
3452 5434898 : const double leaderPastCPTime = leaderDistToCrossing / MAX2(leaderInfo.first->getSpeed(), SUMO_const_haltingSpeed);
3453 : // reach distToCrossing after that time
3454 : // avgSpeed * leaderPastCPTime = distToCrossing
3455 : // ballistic: avgSpeed = (getSpeed + vFinal) / 2
3456 5434898 : const double vFinal = MAX2(getSpeed(), 2 * (distToCrossing - getVehicleType().getMinGap()) / leaderPastCPTime - getSpeed());
3457 5434898 : const double v2 = getSpeed() + ACCEL2SPEED((vFinal - getSpeed()) / leaderPastCPTime);
3458 : vsafeLeader = MAX2(vsafeLeader, MIN2(v2, vStop));
3459 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3460 : if (DEBUG_COND) {
3461 : std::cout << " driving up to the crossing point (distToCrossing=" << distToCrossing << ")"
3462 : << " leaderPastCPTime=" << leaderPastCPTime
3463 : << " vFinal=" << vFinal
3464 : << " v2=" << v2
3465 : << " vStop=" << vStop
3466 : << " vsafeLeader=" << vsafeLeader << "\n";
3467 : }
3468 : #endif
3469 : }
3470 : }
3471 18226270 : if (lastLink != nullptr) {
3472 : lastLink->adaptLeaveSpeed(vsafeLeader);
3473 : }
3474 18636389 : v = MIN2(v, vsafeLeader);
3475 34785384 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3476 : #ifdef DEBUG_PLAN_MOVE
3477 : if (DEBUG_COND) std::cout
3478 : << SIMTIME
3479 : //std::cout << std::setprecision(10);
3480 : << " veh=" << getID()
3481 : << " lead=" << leaderInfo.first->getID()
3482 : << " leadSpeed=" << leaderInfo.first->getSpeed()
3483 : << " gap=" << leaderInfo.second
3484 : << " leadLane=" << leaderInfo.first->getLane()->getID()
3485 : << " predPos=" << leaderInfo.first->getPositionOnLane()
3486 : << " seen=" << seen
3487 : << " lane=" << lane->getID()
3488 : << " myLane=" << myLane->getID()
3489 : << " dTC=" << distToCrossing
3490 : << " v=" << v
3491 : << " vSafeLeader=" << vsafeLeader
3492 : << " vLinkPass=" << vLinkPass
3493 : << "\n";
3494 : #endif
3495 : }
3496 : }
3497 :
3498 :
3499 : void
3500 148218 : MSVehicle::adaptToOncomingLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3501 : DriveProcessItem* const lastLink,
3502 : double& v, double& vLinkPass) const {
3503 148218 : if (leaderInfo.first != 0) {
3504 148218 : if (ignoreFoe(leaderInfo.first)) {
3505 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3506 : if (DEBUG_COND) {
3507 : std::cout << " oncoming foe ignored\n";
3508 : }
3509 : #endif
3510 : return;
3511 : }
3512 : const MSCFModel& cfModel = getCarFollowModel();
3513 : const MSVehicle* lead = leaderInfo.first;
3514 : const MSCFModel& cfModelL = lead->getCarFollowModel();
3515 : // assume the leader reacts symmetrically (neither stopping instantly nor ignoring ego)
3516 148170 : const double leaderBrakeGap = cfModelL.brakeGap(lead->getSpeed(), cfModelL.getMaxDecel(), 0);
3517 148170 : const double egoBrakeGap = cfModel.brakeGap(getSpeed(), cfModel.getMaxDecel(), 0);
3518 148170 : const double gapSum = leaderBrakeGap + egoBrakeGap;
3519 : // ensure that both vehicles can leave an intersection if they are currently on it
3520 148170 : double egoExit = getDistanceToLeaveJunction();
3521 148170 : const double leaderExit = lead->getDistanceToLeaveJunction();
3522 : double gap = leaderInfo.second;
3523 148170 : if (egoExit + leaderExit < gap) {
3524 122750 : gap -= egoExit + leaderExit;
3525 : } else {
3526 : egoExit = 0;
3527 : }
3528 : // split any distance in excess of brakeGaps evenly
3529 148170 : const double freeGap = MAX2(0.0, gap - gapSum);
3530 : const double splitGap = MIN2(gap, gapSum);
3531 : // assume remaining distance is allocated in proportion to braking distance
3532 148170 : const double gapRatio = gapSum > 0 ? egoBrakeGap / gapSum : 0.5;
3533 148170 : const double vsafeLeader = cfModel.stopSpeed(this, getSpeed(), splitGap * gapRatio + egoExit + 0.5 * freeGap);
3534 148170 : if (lastLink != nullptr) {
3535 70070 : const double futureVSafe = cfModel.stopSpeed(this, lastLink->accelV, leaderInfo.second, MSCFModel::CalcReason::FUTURE);
3536 : lastLink->adaptLeaveSpeed(futureVSafe);
3537 : #ifdef DEBUG_PLAN_MOVE
3538 : if (DEBUG_COND) {
3539 : std::cout << " vlinkpass=" << lastLink->myVLinkPass << " futureVSafe=" << futureVSafe << "\n";
3540 : }
3541 : #endif
3542 : }
3543 148170 : v = MIN2(v, vsafeLeader);
3544 291555 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3545 : #ifdef DEBUG_PLAN_MOVE
3546 : if (DEBUG_COND) std::cout
3547 : << SIMTIME
3548 : //std::cout << std::setprecision(10);
3549 : << " veh=" << getID()
3550 : << " oncomingLead=" << lead->getID()
3551 : << " leadSpeed=" << lead->getSpeed()
3552 : << " gap=" << leaderInfo.second
3553 : << " gap2=" << gap
3554 : << " gapRatio=" << gapRatio
3555 : << " leadLane=" << lead->getLane()->getID()
3556 : << " predPos=" << lead->getPositionOnLane()
3557 : << " myLane=" << myLane->getID()
3558 : << " v=" << v
3559 : << " vSafeLeader=" << vsafeLeader
3560 : << " vLinkPass=" << vLinkPass
3561 : << "\n";
3562 : #endif
3563 : }
3564 : }
3565 :
3566 :
3567 : void
3568 819417421 : MSVehicle::checkLinkLeaderCurrentAndParallel(const MSLink* link, const MSLane* lane, double seen,
3569 : DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest) const {
3570 819417421 : if (MSGlobals::gUsingInternalLanes && (myInfluencer == nullptr || myInfluencer->getRespectJunctionLeaderPriority())) {
3571 : // we want to pass the link but need to check for foes on internal lanes
3572 819307635 : checkLinkLeader(link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
3573 819307635 : if (myLaneChangeModel->getShadowLane() != nullptr) {
3574 3221502 : const MSLink* const parallelLink = link->getParallelLink(myLaneChangeModel->getShadowDirection());
3575 3221502 : if (parallelLink != nullptr) {
3576 2209772 : checkLinkLeader(parallelLink, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest, true);
3577 : }
3578 : }
3579 : }
3580 :
3581 819417421 : }
3582 :
3583 : void
3584 821762833 : MSVehicle::checkLinkLeader(const MSLink* link, const MSLane* lane, double seen,
3585 : DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest,
3586 : bool isShadowLink) const {
3587 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3588 : if (DEBUG_COND) {
3589 : gDebugFlag1 = true; // See MSLink::getLeaderInfo
3590 : }
3591 : #endif
3592 821762833 : const MSLink::LinkLeaders linkLeaders = link->getLeaderInfo(this, seen, nullptr, isShadowLink);
3593 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3594 : if (DEBUG_COND) {
3595 : gDebugFlag1 = false; // See MSLink::getLeaderInfo
3596 : }
3597 : #endif
3598 841329119 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
3599 : // the vehicle to enter the junction first has priority
3600 19566286 : const MSVehicle* leader = (*it).vehAndGap.first;
3601 19566286 : if (leader == nullptr) {
3602 : // leader is a pedestrian. Passing 'this' as a dummy.
3603 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3604 : if (DEBUG_COND) {
3605 : std::cout << SIMTIME << " veh=" << getID() << " is blocked on link to " << link->getViaLaneOrLane()->getID() << " by pedestrian. dist=" << it->distToCrossing << "\n";
3606 : }
3607 : #endif
3608 422040 : if (getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) > 0
3609 422040 : && getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) >= RandHelper::rand(getRNG())) {
3610 : #ifdef DEBUG_PLAN_MOVE
3611 : if (DEBUG_COND) {
3612 : std::cout << SIMTIME << " veh=" << getID() << " is ignoring pedestrian (jmIgnoreJunctionFoeProb)\n";
3613 : }
3614 : #endif
3615 696 : continue;
3616 : }
3617 421344 : adaptToJunctionLeader(std::make_pair(this, -1), seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
3618 : // if blocked by a pedestrian for too long we must yield our request
3619 421344 : if (v < SUMO_const_haltingSpeed && getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME)) {
3620 75072 : setRequest = false;
3621 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3622 : if (DEBUG_COND) {
3623 : std::cout << " aborting request\n";
3624 : }
3625 : #endif
3626 : }
3627 19144246 : } else if (isLeader(link, leader, (*it).vehAndGap.second) || (*it).inTheWay()) {
3628 19085772 : if (getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) > 0
3629 19085772 : && getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) >= RandHelper::rand(getRNG())) {
3630 : #ifdef DEBUG_PLAN_MOVE
3631 : if (DEBUG_COND) {
3632 : std::cout << SIMTIME << " veh=" << getID() << " is ignoring linkLeader=" << leader->getID() << " (jmIgnoreJunctionFoeProb)\n";
3633 : }
3634 : #endif
3635 2185 : continue;
3636 : }
3637 25941631 : if (MSGlobals::gLateralResolution > 0 &&
3638 : // sibling link (XXX: could also be partial occupator where this check fails)
3639 6858044 : &leader->getLane()->getEdge() == &lane->getEdge()) {
3640 : // check for sublane obstruction (trivial for sibling link leaders)
3641 : const MSLane* conflictLane = link->getInternalLaneBefore();
3642 886232 : MSLeaderInfo linkLeadersAhead = MSLeaderInfo(conflictLane->getWidth());
3643 886232 : linkLeadersAhead.addLeader(leader, false, 0); // assume sibling lane has the same geometry as the leader lane
3644 886232 : const double latOffset = isShadowLink ? (getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge()) : 0;
3645 : // leader is neither on lane nor conflictLane (the conflict is only established geometrically)
3646 886232 : adaptToLeaders(linkLeadersAhead, latOffset, seen, lastLink, leader->getLane(), v, vLinkPass);
3647 : #ifdef DEBUG_PLAN_MOVE
3648 : if (DEBUG_COND) {
3649 : std::cout << SIMTIME << " veh=" << getID()
3650 : << " siblingFoe link=" << link->getViaLaneOrLane()->getID()
3651 : << " isShadowLink=" << isShadowLink
3652 : << " lane=" << lane->getID()
3653 : << " foe=" << leader->getID()
3654 : << " foeLane=" << leader->getLane()->getID()
3655 : << " latOffset=" << latOffset
3656 : << " latOffsetFoe=" << leader->getLatOffset(lane)
3657 : << " linkLeadersAhead=" << linkLeadersAhead.toString()
3658 : << "\n";
3659 : }
3660 : #endif
3661 886232 : } else {
3662 : #ifdef DEBUG_PLAN_MOVE
3663 : if (DEBUG_COND) {
3664 : std::cout << SIMTIME << " veh=" << getID() << " linkLeader=" << leader->getID() << " gap=" << it->vehAndGap.second
3665 : << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
3666 : << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
3667 : << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
3668 : << "\n";
3669 : }
3670 : #endif
3671 18197355 : adaptToJunctionLeader(it->vehAndGap, seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
3672 : }
3673 19083587 : if (lastLink != nullptr) {
3674 : // we are not yet on the junction with this linkLeader.
3675 : // at least we can drive up to the previous link and stop there
3676 36712208 : v = MAX2(v, lastLink->myVLinkWait);
3677 : }
3678 : // if blocked by a leader from the same or next lane we must yield our request
3679 : // also, if blocked by a stopped or blocked leader
3680 19083587 : if (v < SUMO_const_haltingSpeed
3681 : //&& leader->getSpeed() < SUMO_const_haltingSpeed
3682 19083587 : && (leader->getLane()->getLogicalPredecessorLane() == myLane->getLogicalPredecessorLane()
3683 10473601 : || leader->getLane()->getLogicalPredecessorLane() == myLane
3684 8466340 : || leader->isStopped()
3685 8388412 : || leader->getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME))) {
3686 4102254 : setRequest = false;
3687 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3688 : if (DEBUG_COND) {
3689 : std::cout << " aborting request\n";
3690 : }
3691 : #endif
3692 4102254 : if (lastLink != nullptr && leader->getLane()->getLogicalPredecessorLane() == myLane) {
3693 : // we are not yet on the junction so must abort that request as well
3694 : // (or maybe we are already on the junction and the leader is a partial occupator beyond)
3695 1995160 : lastLink->mySetRequest = false;
3696 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3697 : if (DEBUG_COND) {
3698 : std::cout << " aborting previous request\n";
3699 : }
3700 : #endif
3701 : }
3702 : }
3703 : }
3704 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3705 : else {
3706 : if (DEBUG_COND) {
3707 : std::cout << SIMTIME << " veh=" << getID() << " ignoring leader " << leader->getID() << " gap=" << (*it).vehAndGap.second << " dtC=" << (*it).distToCrossing
3708 : << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
3709 : << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
3710 : << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
3711 : << "\n";
3712 : }
3713 : }
3714 : #endif
3715 : }
3716 : // if this is the link between two internal lanes we may have to slow down for pedestrians
3717 821762833 : vLinkWait = MIN2(vLinkWait, v);
3718 821762833 : }
3719 :
3720 :
3721 : double
3722 99760786 : MSVehicle::getDeltaPos(const double accel) const {
3723 99760786 : double vNext = myState.mySpeed + ACCEL2SPEED(accel);
3724 99760786 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3725 : // apply implicit Euler positional update
3726 0 : return SPEED2DIST(MAX2(vNext, 0.));
3727 : } else {
3728 : // apply ballistic update
3729 99760786 : if (vNext >= 0) {
3730 : // assume constant acceleration during this time step
3731 99127553 : return SPEED2DIST(myState.mySpeed + 0.5 * ACCEL2SPEED(accel));
3732 : } else {
3733 : // negative vNext indicates a stop within the middle of time step
3734 : // The corresponding stop time is s = mySpeed/deceleration \in [0,dt], and the
3735 : // covered distance is therefore deltaPos = mySpeed*s - 0.5*deceleration*s^2.
3736 : // Here, deceleration = (myState.mySpeed - vNext)/dt is the constant deceleration
3737 : // until the vehicle stops.
3738 633233 : return -SPEED2DIST(0.5 * myState.mySpeed * myState.mySpeed / ACCEL2SPEED(accel));
3739 : }
3740 : }
3741 : }
3742 :
3743 : void
3744 637469645 : MSVehicle::processLinkApproaches(double& vSafe, double& vSafeMin, double& vSafeMinDist) {
3745 :
3746 : const MSCFModel& cfModel = getCarFollowModel();
3747 : // Speed limit due to zipper merging
3748 : double vSafeZipper = std::numeric_limits<double>::max();
3749 :
3750 637469645 : myHaveToWaitOnNextLink = false;
3751 : bool canBrakeVSafeMin = false;
3752 :
3753 : // Get safe velocities from DriveProcessItems.
3754 : assert(myLFLinkLanes.size() != 0 || isRemoteControlled());
3755 1280760633 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
3756 1105412021 : MSLink* const link = dpi.myLink;
3757 :
3758 : #ifdef DEBUG_EXEC_MOVE
3759 : if (DEBUG_COND) {
3760 : std::cout
3761 : << SIMTIME
3762 : << " veh=" << getID()
3763 : << " link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID())
3764 : << " req=" << dpi.mySetRequest
3765 : << " vP=" << dpi.myVLinkPass
3766 : << " vW=" << dpi.myVLinkWait
3767 : << " d=" << dpi.myDistance
3768 : << "\n";
3769 : gDebugFlag1 = true; // See MSLink_DEBUG_OPENED
3770 : }
3771 : #endif
3772 :
3773 : // the vehicle must change the lane on one of the next lanes (XXX: refs to code further below???, Leo)
3774 1105412021 : if (link != nullptr && dpi.mySetRequest) {
3775 :
3776 : const LinkState ls = link->getState();
3777 : // vehicles should brake when running onto a yellow light if the distance allows to halt in front
3778 : const bool yellow = link->haveYellow();
3779 664485792 : const bool canBrake = (dpi.myDistance > cfModel.brakeGap(myState.mySpeed, cfModel.getMaxDecel(), 0.)
3780 664485792 : || (MSGlobals::gSemiImplicitEulerUpdate && myState.mySpeed < ACCEL2SPEED(cfModel.getMaxDecel())));
3781 : assert(link->getLaneBefore() != nullptr);
3782 664485792 : const bool beyondStopLine = dpi.myDistance < link->getLaneBefore()->getVehicleStopOffset(this);
3783 664485792 : const bool ignoreRedLink = ignoreRed(link, canBrake) || beyondStopLine;
3784 664485792 : if (yellow && canBrake && !ignoreRedLink) {
3785 3 : vSafe = dpi.myVLinkWait;
3786 3 : myHaveToWaitOnNextLink = true;
3787 : #ifdef DEBUG_CHECKREWINDLINKLANES
3788 : if (DEBUG_COND) {
3789 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (yellow)\n";
3790 : }
3791 : #endif
3792 21194804 : break;
3793 : }
3794 664485789 : const bool influencerPrio = (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority());
3795 : MSLink::BlockingFoes collectFoes;
3796 664485789 : bool opened = (yellow || influencerPrio
3797 1993133103 : || link->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
3798 664323657 : getVehicleType().getLength(),
3799 636609684 : canBrake ? getImpatience() : 1,
3800 : cfModel.getMaxDecel(),
3801 664323657 : getWaitingTimeFor(link), getLateralPositionOnLane(),
3802 : ls == LINKSTATE_ZIPPER ? &collectFoes : nullptr,
3803 664323657 : ignoreRedLink, this, dpi.myDistance));
3804 658790690 : if (opened && myLaneChangeModel->getShadowLane() != nullptr) {
3805 1961872 : const MSLink* const parallelLink = dpi.myLink->getParallelLink(myLaneChangeModel->getShadowDirection());
3806 1961872 : if (parallelLink != nullptr) {
3807 1212704 : const double shadowLatPos = getLateralPositionOnLane() - myLaneChangeModel->getShadowDirection() * 0.5 * (
3808 1212704 : myLane->getWidth() + myLaneChangeModel->getShadowLane()->getWidth());
3809 3636604 : opened = yellow || influencerPrio || (opened && parallelLink->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
3810 1211950 : getVehicleType().getLength(),
3811 1144167 : canBrake ? getImpatience() : 1,
3812 : cfModel.getMaxDecel(),
3813 : getWaitingTimeFor(link), shadowLatPos, nullptr,
3814 1211950 : ignoreRedLink, this, dpi.myDistance));
3815 : #ifdef DEBUG_EXEC_MOVE
3816 : if (DEBUG_COND) {
3817 : std::cout << SIMTIME
3818 : << " veh=" << getID()
3819 : << " shadowLane=" << myLaneChangeModel->getShadowLane()->getID()
3820 : << " shadowDir=" << myLaneChangeModel->getShadowDirection()
3821 : << " parallelLink=" << (parallelLink == 0 ? "NULL" : parallelLink->getViaLaneOrLane()->getID())
3822 : << " opened=" << opened
3823 : << "\n";
3824 : }
3825 : #endif
3826 : }
3827 : }
3828 : // vehicles should decelerate when approaching a minor link
3829 : #ifdef DEBUG_EXEC_MOVE
3830 : if (DEBUG_COND) {
3831 : std::cout << SIMTIME
3832 : << " opened=" << opened
3833 : << " influencerPrio=" << influencerPrio
3834 : << " linkPrio=" << link->havePriority()
3835 : << " lastContMajor=" << link->lastWasContMajor()
3836 : << " isCont=" << link->isCont()
3837 : << " ignoreRed=" << ignoreRedLink
3838 : << " canBrake=" << canBrake
3839 : << "\n";
3840 : }
3841 : #endif
3842 : double visibilityDistance = link->getFoeVisibilityDistance();
3843 664485789 : bool determinedFoePresence = dpi.myDistance <= visibilityDistance;
3844 664485789 : if (opened && !influencerPrio && !link->havePriority() && !link->lastWasContMajor() && !link->isCont() && !ignoreRedLink) {
3845 17803721 : if (!determinedFoePresence && (canBrake || !yellow)) {
3846 16806207 : vSafe = dpi.myVLinkWait;
3847 16806207 : myHaveToWaitOnNextLink = true;
3848 : #ifdef DEBUG_CHECKREWINDLINKLANES
3849 : if (DEBUG_COND) {
3850 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (minor)\n";
3851 : }
3852 : #endif
3853 16806207 : break;
3854 : } else {
3855 : // past the point of no return. we need to drive fast enough
3856 : // to make it across the link. However, minor slowdowns
3857 : // should be permissible to follow leading traffic safely
3858 : // basically, this code prevents dawdling
3859 : // (it's harder to do this later using
3860 : // SUMO_ATTR_JM_SIGMA_MINOR because we don't know whether the
3861 : // vehicle is already too close to stop at that part of the code)
3862 : //
3863 : // XXX: There is a problem in subsecond simulation: If we cannot
3864 : // make it across the minor link in one step, new traffic
3865 : // could appear on a major foe link and cause a collision. Refs. #1845, #2123
3866 997514 : vSafeMinDist = dpi.myDistance; // distance that must be covered
3867 997514 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3868 1811568 : vSafeMin = MIN3((double)DIST2SPEED(vSafeMinDist + POSITION_EPS), dpi.myVLinkPass, cfModel.maxNextSafeMin(getSpeed(), this));
3869 : } else {
3870 183460 : vSafeMin = MIN3((double)DIST2SPEED(2 * vSafeMinDist + NUMERICAL_EPS) - getSpeed(), dpi.myVLinkPass, cfModel.maxNextSafeMin(getSpeed(), this));
3871 : }
3872 : canBrakeVSafeMin = canBrake;
3873 : #ifdef DEBUG_EXEC_MOVE
3874 : if (DEBUG_COND) {
3875 : std::cout << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << " canBrake=" << canBrake << "\n";
3876 : }
3877 : #endif
3878 : }
3879 : }
3880 : // have waited; may pass if opened...
3881 647679582 : if (opened) {
3882 641963828 : vSafe = dpi.myVLinkPass;
3883 641963828 : if (vSafe < cfModel.getMaxDecel() && vSafe <= dpi.myVLinkWait && vSafe < cfModel.maxNextSpeed(getSpeed(), this)) {
3884 : // this vehicle is probably not gonna drive across the next junction (heuristic)
3885 56489681 : myHaveToWaitOnNextLink = true;
3886 : #ifdef DEBUG_CHECKREWINDLINKLANES
3887 : if (DEBUG_COND) {
3888 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (very slow)\n";
3889 : }
3890 : #endif
3891 : }
3892 641963828 : if (link->mustStop() && determinedFoePresence && myHaveStoppedFor == nullptr) {
3893 20899 : myHaveStoppedFor = link;
3894 : }
3895 5715754 : } else if (link->getState() == LINKSTATE_ZIPPER) {
3896 1326926 : vSafeZipper = MIN2(vSafeZipper,
3897 1326926 : link->getZipperSpeed(this, dpi.myDistance, dpi.myVLinkPass, dpi.myArrivalTime, &collectFoes));
3898 : } else if (!canBrake
3899 : // always brake hard for traffic lights (since an emergency stop is necessary anyway)
3900 1879 : && link->getTLLogic() == nullptr
3901 : // cannot brake even with emergency deceleration
3902 4389706 : && dpi.myDistance < cfModel.brakeGap(myState.mySpeed, cfModel.getEmergencyDecel(), 0.)) {
3903 : #ifdef DEBUG_EXEC_MOVE
3904 : if (DEBUG_COND) {
3905 : std::cout << SIMTIME << " too fast to brake for closed link\n";
3906 : }
3907 : #endif
3908 234 : vSafe = dpi.myVLinkPass;
3909 : } else {
3910 4388594 : vSafe = dpi.myVLinkWait;
3911 4388594 : myHaveToWaitOnNextLink = true;
3912 : #ifdef DEBUG_CHECKREWINDLINKLANES
3913 : if (DEBUG_COND) {
3914 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (closed)\n";
3915 : }
3916 : #endif
3917 : #ifdef DEBUG_EXEC_MOVE
3918 : if (DEBUG_COND) {
3919 : std::cout << SIMTIME << " braking for closed link=" << link->getViaLaneOrLane()->getID() << "\n";
3920 : }
3921 : #endif
3922 4388594 : break;
3923 : }
3924 643290988 : if (myLane->isInternal() && myJunctionEntryTime == SUMOTime_MAX) {
3925 : // request was renewed, restoring entry time
3926 : // @note: using myJunctionEntryTimeNeverYield could lead to inconsistencies with other vehicles already on the junction
3927 88324 : myJunctionEntryTime = SIMSTEP;;
3928 : }
3929 664485789 : } else {
3930 440926229 : if (link != nullptr && link->getInternalLaneBefore() != nullptr && myLane->isInternal() && link->getJunction() == myLane->getEdge().getToJunction()) {
3931 : // blocked on the junction. yield request so other vehicles may
3932 : // become junction leader
3933 : #ifdef DEBUG_EXEC_MOVE
3934 : if (DEBUG_COND) {
3935 : std::cout << SIMTIME << " resetting junctionEntryTime at junction '" << link->getJunction()->getID() << "' beause of non-request exitLink\n";
3936 : }
3937 : #endif
3938 257854 : myJunctionEntryTime = SUMOTime_MAX;
3939 257854 : myJunctionConflictEntryTime = SUMOTime_MAX;
3940 : }
3941 : // we have: i->link == 0 || !i->setRequest
3942 440926229 : vSafe = dpi.myVLinkWait;
3943 440926229 : if (link != nullptr || myStopDist < (myLane->getLength() - getPositionOnLane())) {
3944 111176347 : if (vSafe < getSpeed()) {
3945 16482139 : myHaveToWaitOnNextLink = true;
3946 : #ifdef DEBUG_CHECKREWINDLINKLANES
3947 : if (DEBUG_COND) {
3948 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, braking) vSafe=" << vSafe << "\n";
3949 : }
3950 : #endif
3951 94694208 : } else if (vSafe < SUMO_const_haltingSpeed) {
3952 67074360 : myHaveToWaitOnNextLink = true;
3953 : #ifdef DEBUG_CHECKREWINDLINKLANES
3954 : if (DEBUG_COND) {
3955 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, stopping)\n";
3956 : }
3957 : #endif
3958 : }
3959 : }
3960 334227229 : if (link == nullptr && myLFLinkLanes.size() == 1
3961 263745369 : && getBestLanesContinuation().size() > 1
3962 1244403 : && getBestLanesContinuation()[1]->hadPermissionChanges()
3963 441062968 : && myLane->getFirstAnyVehicle() == this) {
3964 : // temporal lane closing without notification, visible to the
3965 : // vehicle at the front of the queue
3966 35020 : updateBestLanes(true);
3967 : //std::cout << SIMTIME << " veh=" << getID() << " updated bestLanes=" << toString(getBestLanesContinuation()) << "\n";
3968 : }
3969 : break;
3970 : }
3971 : }
3972 :
3973 : //#ifdef DEBUG_EXEC_MOVE
3974 : // if (DEBUG_COND) {
3975 : // std::cout << "\nvCurrent = " << toString(getSpeed(), 24) << "" << std::endl;
3976 : // std::cout << "vSafe = " << toString(vSafe, 24) << "" << std::endl;
3977 : // std::cout << "vSafeMin = " << toString(vSafeMin, 24) << "" << std::endl;
3978 : // std::cout << "vSafeMinDist = " << toString(vSafeMinDist, 24) << "" << std::endl;
3979 : //
3980 : // double gap = getLeader().second;
3981 : // std::cout << "gap = " << toString(gap, 24) << std::endl;
3982 : // std::cout << "vSafeStoppedLeader = " << toString(getCarFollowModel().stopSpeed(this, getSpeed(), gap, MSCFModel::CalcReason::FUTURE), 24)
3983 : // << "\n" << std::endl;
3984 : // }
3985 : //#endif
3986 :
3987 637469645 : if ((MSGlobals::gSemiImplicitEulerUpdate && vSafe + NUMERICAL_EPS < vSafeMin)
3988 637240408 : || (!MSGlobals::gSemiImplicitEulerUpdate && (vSafe + NUMERICAL_EPS < vSafeMin && vSafeMin != 0))) { // this might be good for the euler case as well
3989 : // XXX: (Leo) This often called stopSpeed with vSafeMinDist==0 (for the ballistic update), since vSafe can become negative
3990 : // 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
3991 : #ifdef DEBUG_EXEC_MOVE
3992 : if (DEBUG_COND) {
3993 : std::cout << "vSafeMin Problem? vSafe=" << vSafe << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << std::endl;
3994 : }
3995 : #endif
3996 275888 : if (canBrakeVSafeMin && vSafe < getSpeed()) {
3997 : // cannot drive across a link so we need to stop before it
3998 129326 : vSafe = MIN2(vSafe, MAX2(getCarFollowModel().minNextSpeed(getSpeed(), this),
3999 64663 : getCarFollowModel().stopSpeed(this, getSpeed(), vSafeMinDist)));
4000 64663 : vSafeMin = 0;
4001 64663 : myHaveToWaitOnNextLink = true;
4002 : #ifdef DEBUG_CHECKREWINDLINKLANES
4003 : if (DEBUG_COND) {
4004 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (vSafe=" << vSafe << " < vSafeMin=" << vSafeMin << ")\n";
4005 : }
4006 : #endif
4007 : } else {
4008 : // if the link is yellow or visibility distance is large
4009 : // then we might not make it across the link in one step anyway..
4010 : // Possibly, the lane after the intersection has a lower speed limit so
4011 : // we really need to drive slower already
4012 : // -> keep driving without dawdling
4013 211225 : vSafeMin = vSafe;
4014 : }
4015 : }
4016 :
4017 : // vehicles inside a roundabout should maintain their requests
4018 637469645 : if (myLane->getEdge().isRoundabout()) {
4019 2707991 : myHaveToWaitOnNextLink = false;
4020 : }
4021 :
4022 637469645 : vSafe = MIN2(vSafe, vSafeZipper);
4023 637469645 : }
4024 :
4025 :
4026 : double
4027 705103833 : MSVehicle::processTraCISpeedControl(double vSafe, double vNext) {
4028 705103833 : if (myInfluencer != nullptr) {
4029 498262 : myInfluencer->setOriginalSpeed(vNext);
4030 : #ifdef DEBUG_TRACI
4031 : if DEBUG_COND2(this) {
4032 : std::cout << SIMTIME << " MSVehicle::processTraCISpeedControl() for vehicle '" << getID() << "'"
4033 : << " vSafe=" << vSafe << " (init)vNext=" << vNext << " keepStopping=" << keepStopping();
4034 : }
4035 : #endif
4036 498262 : if (myInfluencer->isRemoteControlled()) {
4037 7311 : vNext = myInfluencer->implicitSpeedRemote(this, myState.mySpeed);
4038 : }
4039 498262 : const double vMax = getVehicleType().getCarFollowModel().maxNextSpeed(myState.mySpeed, this);
4040 498262 : double vMin = getVehicleType().getCarFollowModel().minNextSpeed(myState.mySpeed, this);
4041 498262 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4042 : vMin = MAX2(0., vMin);
4043 : }
4044 498262 : vNext = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), vNext, vSafe, vMin, vMax);
4045 498262 : if (keepStopping() && myStops.front().getSpeed() == 0) {
4046 : // avoid driving while stopped (unless it's actually a waypoint
4047 3818 : vNext = myInfluencer->getOriginalSpeed();
4048 : }
4049 : #ifdef DEBUG_TRACI
4050 : if DEBUG_COND2(this) {
4051 : std::cout << " (processed)vNext=" << vNext << std::endl;
4052 : }
4053 : #endif
4054 : }
4055 705103833 : return vNext;
4056 : }
4057 :
4058 :
4059 : void
4060 71727851 : MSVehicle::removePassedDriveItems() {
4061 : #ifdef DEBUG_ACTIONSTEPS
4062 : if (DEBUG_COND) {
4063 : std::cout << SIMTIME << " veh=" << getID() << " removePassedDriveItems()\n"
4064 : << " Current items: ";
4065 : for (auto& j : myLFLinkLanes) {
4066 : if (j.myLink == 0) {
4067 : std::cout << "\n Stop at distance " << j.myDistance;
4068 : } else {
4069 : const MSLane* to = j.myLink->getViaLaneOrLane();
4070 : const MSLane* from = j.myLink->getLaneBefore();
4071 : std::cout << "\n Link at distance " << j.myDistance << ": '"
4072 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4073 : }
4074 : }
4075 : std::cout << "\n myNextDriveItem: ";
4076 : if (myLFLinkLanes.size() != 0) {
4077 : if (myNextDriveItem->myLink == 0) {
4078 : std::cout << "\n Stop at distance " << myNextDriveItem->myDistance;
4079 : } else {
4080 : const MSLane* to = myNextDriveItem->myLink->getViaLaneOrLane();
4081 : const MSLane* from = myNextDriveItem->myLink->getLaneBefore();
4082 : std::cout << "\n Link at distance " << myNextDriveItem->myDistance << ": '"
4083 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4084 : }
4085 : }
4086 : std::cout << std::endl;
4087 : }
4088 : #endif
4089 72052616 : for (auto j = myLFLinkLanes.begin(); j != myNextDriveItem; ++j) {
4090 : #ifdef DEBUG_ACTIONSTEPS
4091 : if (DEBUG_COND) {
4092 : std::cout << " Removing item: ";
4093 : if (j->myLink == 0) {
4094 : std::cout << "Stop at distance " << j->myDistance;
4095 : } else {
4096 : const MSLane* to = j->myLink->getViaLaneOrLane();
4097 : const MSLane* from = j->myLink->getLaneBefore();
4098 : std::cout << "Link at distance " << j->myDistance << ": '"
4099 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4100 : }
4101 : std::cout << std::endl;
4102 : }
4103 : #endif
4104 324765 : if (j->myLink != nullptr) {
4105 324695 : j->myLink->removeApproaching(this);
4106 : }
4107 : }
4108 71727851 : myLFLinkLanes.erase(myLFLinkLanes.begin(), myNextDriveItem);
4109 71727851 : myNextDriveItem = myLFLinkLanes.begin();
4110 71727851 : }
4111 :
4112 :
4113 : void
4114 1137238 : MSVehicle::updateDriveItems() {
4115 : #ifdef DEBUG_ACTIONSTEPS
4116 : if (DEBUG_COND) {
4117 : std::cout << SIMTIME << " updateDriveItems(), veh='" << getID() << "' (lane: '" << getLane()->getID() << "')\nCurrent drive items:" << std::endl;
4118 : for (const auto& dpi : myLFLinkLanes) {
4119 : std::cout
4120 : << " vPass=" << dpi.myVLinkPass
4121 : << " vWait=" << dpi.myVLinkWait
4122 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4123 : << " request=" << dpi.mySetRequest
4124 : << "\n";
4125 : }
4126 : std::cout << " myNextDriveItem's linked lane: " << (myNextDriveItem->myLink == 0 ? "NULL" : myNextDriveItem->myLink->getViaLaneOrLane()->getID()) << std::endl;
4127 : }
4128 : #endif
4129 1137238 : if (myLFLinkLanes.size() == 0) {
4130 : // nothing to update
4131 : return;
4132 : }
4133 : const MSLink* nextPlannedLink = nullptr;
4134 : // auto i = myLFLinkLanes.begin();
4135 1137236 : auto i = myNextDriveItem;
4136 2274425 : while (i != myLFLinkLanes.end() && nextPlannedLink == nullptr) {
4137 1137189 : nextPlannedLink = i->myLink;
4138 : ++i;
4139 : }
4140 :
4141 1137236 : if (nextPlannedLink == nullptr) {
4142 : // No link for upcoming item -> no need for an update
4143 : #ifdef DEBUG_ACTIONSTEPS
4144 : if (DEBUG_COND) {
4145 : std::cout << "Found no link-related drive item." << std::endl;
4146 : }
4147 : #endif
4148 : return;
4149 : }
4150 :
4151 555363 : if (getLane() == nextPlannedLink->getLaneBefore()) {
4152 : // Current lane approaches the stored next link, i.e. no LC happend and no update is required.
4153 : #ifdef DEBUG_ACTIONSTEPS
4154 : if (DEBUG_COND) {
4155 : std::cout << "Continuing on planned lane sequence, no update required." << std::endl;
4156 : }
4157 : #endif
4158 : return;
4159 : }
4160 : // Lane must have been changed, determine the change direction
4161 545476 : const MSLink* parallelLink = nextPlannedLink->getParallelLink(1);
4162 545476 : if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
4163 : // lcDir = 1;
4164 : } else {
4165 265166 : parallelLink = nextPlannedLink->getParallelLink(-1);
4166 265166 : if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
4167 : // lcDir = -1;
4168 : } else {
4169 : // If the vehicle's current lane is not the approaching lane for the next
4170 : // drive process item's link, it is expected to lead to a parallel link,
4171 : // XXX: What if the lc was an overtaking maneuver and there is no upcoming link?
4172 : // Then a stop item should be scheduled! -> TODO!
4173 : //assert(false);
4174 73401 : return;
4175 : }
4176 : }
4177 : #ifdef DEBUG_ACTIONSTEPS
4178 : if (DEBUG_COND) {
4179 : std::cout << "Changed lane. Drive items will be updated along the current lane continuation." << std::endl;
4180 : }
4181 : #endif
4182 : // Trace link sequence along current best lanes and transfer drive items to the corresponding links
4183 : // DriveItemVector::iterator driveItemIt = myLFLinkLanes.begin();
4184 472075 : DriveItemVector::iterator driveItemIt = myNextDriveItem;
4185 : // In the loop below, lane holds the currently considered lane on the vehicles continuation (including internal lanes)
4186 472075 : const MSLane* lane = myLane;
4187 : assert(myLane == parallelLink->getLaneBefore());
4188 : // *lit is a pointer to the next lane in best continuations for the current lane (always non-internal)
4189 472075 : std::vector<MSLane*>::const_iterator bestLaneIt = getBestLanesContinuation().begin() + 1;
4190 : // Pointer to the new link for the current drive process item
4191 : MSLink* newLink = nullptr;
4192 1765500 : while (driveItemIt != myLFLinkLanes.end()) {
4193 1322074 : if (driveItemIt->myLink == nullptr) {
4194 : // Items not related to a specific link are not updated
4195 : // (XXX: when a stop item corresponded to a dead end, which is overcome by the LC that made
4196 : // the update necessary, this may slow down the vehicle's continuation on the new lane...)
4197 : ++driveItemIt;
4198 171521 : continue;
4199 : }
4200 : // Continuation links for current best lanes are less than for the former drive items (myLFLinkLanes)
4201 : // We just remove the leftover link-items, as they cannot be mapped to new links.
4202 1150553 : if (bestLaneIt == getBestLanesContinuation().end()) {
4203 : #ifdef DEBUG_ACTIONSTEPS
4204 : if (DEBUG_COND) {
4205 : std::cout << "Reached end of the new continuation sequence. Erasing leftover link-items." << std::endl;
4206 : }
4207 : #endif
4208 90004 : while (driveItemIt != myLFLinkLanes.end()) {
4209 61355 : if (driveItemIt->myLink == nullptr) {
4210 : ++driveItemIt;
4211 14280 : continue;
4212 : } else {
4213 47075 : driveItemIt->myLink->removeApproaching(this);
4214 : driveItemIt = myLFLinkLanes.erase(driveItemIt);
4215 : }
4216 : }
4217 : break;
4218 : }
4219 : // Do the actual link-remapping for the item. And un/register approaching information on the corresponding links
4220 1121904 : const MSLane* const target = *bestLaneIt;
4221 : assert(!target->isInternal());
4222 : newLink = nullptr;
4223 1237744 : for (MSLink* const link : lane->getLinkCont()) {
4224 1237744 : if (link->getLane() == target) {
4225 : newLink = link;
4226 : break;
4227 : }
4228 : }
4229 :
4230 1121904 : if (newLink == driveItemIt->myLink) {
4231 : // new continuation merged into previous - stop update
4232 : #ifdef DEBUG_ACTIONSTEPS
4233 : if (DEBUG_COND) {
4234 : std::cout << "Old and new continuation sequences merge at link\n"
4235 : << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'"
4236 : << "\nNo update beyond merge required." << std::endl;
4237 : }
4238 : #endif
4239 : break;
4240 : }
4241 :
4242 : #ifdef DEBUG_ACTIONSTEPS
4243 : if (DEBUG_COND) {
4244 : std::cout << "lane=" << lane->getID() << "\nUpdating link\n '" << driveItemIt->myLink->getLaneBefore()->getID() << "'->'" << driveItemIt->myLink->getViaLaneOrLane()->getID() << "'"
4245 : << "==> " << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'" << std::endl;
4246 : }
4247 : #endif
4248 1121904 : newLink->setApproaching(this, driveItemIt->myLink->getApproaching(this));
4249 1121904 : driveItemIt->myLink->removeApproaching(this);
4250 1121904 : driveItemIt->myLink = newLink;
4251 : lane = newLink->getViaLaneOrLane();
4252 : ++driveItemIt;
4253 1121904 : if (!lane->isInternal()) {
4254 : ++bestLaneIt;
4255 : }
4256 : }
4257 : #ifdef DEBUG_ACTIONSTEPS
4258 : if (DEBUG_COND) {
4259 : std::cout << "Updated drive items:" << std::endl;
4260 : for (const auto& dpi : myLFLinkLanes) {
4261 : std::cout
4262 : << " vPass=" << dpi.myVLinkPass
4263 : << " vWait=" << dpi.myVLinkWait
4264 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4265 : << " request=" << dpi.mySetRequest
4266 : << "\n";
4267 : }
4268 : }
4269 : #endif
4270 : }
4271 :
4272 :
4273 : void
4274 705103833 : MSVehicle::setBrakingSignals(double vNext) {
4275 : // To avoid casual blinking brake lights at high speeds due to dawdling of the
4276 : // leading vehicle, we don't show brake lights when the deceleration could be caused
4277 : // by frictional forces and air resistance (i.e. proportional to v^2, coefficient could be adapted further)
4278 705103833 : double pseudoFriction = (0.05 + 0.005 * getSpeed()) * getSpeed();
4279 705103833 : bool brakelightsOn = vNext < getSpeed() - ACCEL2SPEED(pseudoFriction);
4280 :
4281 705103833 : if (vNext <= SUMO_const_haltingSpeed) {
4282 : brakelightsOn = true;
4283 : }
4284 705103833 : if (brakelightsOn && !isStopped()) {
4285 : switchOnSignal(VEH_SIGNAL_BRAKELIGHT);
4286 : } else {
4287 : switchOffSignal(VEH_SIGNAL_BRAKELIGHT);
4288 : }
4289 705103833 : }
4290 :
4291 :
4292 : void
4293 705173132 : MSVehicle::updateWaitingTime(double vNext) {
4294 705173132 : if (vNext <= SUMO_const_haltingSpeed && (!isStopped() || isIdling()) && myAcceleration <= accelThresholdForWaiting()) {
4295 95554829 : myWaitingTime += DELTA_T;
4296 95554829 : myWaitingTimeCollector.passTime(DELTA_T, true);
4297 : } else {
4298 609618303 : myWaitingTime = 0;
4299 609618303 : myWaitingTimeCollector.passTime(DELTA_T, false);
4300 609618303 : if (hasInfluencer()) {
4301 272190 : getInfluencer().setExtraImpatience(0);
4302 : }
4303 : }
4304 705173132 : }
4305 :
4306 :
4307 : void
4308 705103691 : MSVehicle::updateTimeLoss(double vNext) {
4309 : // update time loss (depends on the updated edge)
4310 705103691 : if (!isStopped()) {
4311 : // some cfModels (i.e. EIDM may drive faster than predicted by maxNextSpeed)
4312 694713446 : const double vmax = MIN2(myLane->getVehicleMaxSpeed(this), MAX2(myStopSpeed, vNext));
4313 690975097 : if (vmax > 0) {
4314 690966389 : myTimeLoss += TS * (vmax - vNext) / vmax;
4315 : }
4316 : }
4317 705103691 : }
4318 :
4319 :
4320 : double
4321 1586418862 : MSVehicle::checkReversal(bool& canReverse, double speedThreshold, double seen) const {
4322 64733028 : const bool stopOk = (myStops.empty() || myStops.front().edge != myCurrEdge
4323 1619473940 : || (myStops.front().getSpeed() > 0 && myState.myPos > myStops.front().pars.endPos - 2 * POSITION_EPS));
4324 : #ifdef DEBUG_REVERSE_BIDI
4325 : if (DEBUG_COND) std::cout << SIMTIME << " checkReversal lane=" << myLane->getID()
4326 : << " pos=" << myState.myPos
4327 : << " speed=" << std::setprecision(6) << getPreviousSpeed() << std::setprecision(gPrecision)
4328 : << " speedThreshold=" << speedThreshold
4329 : << " seen=" << seen
4330 : << " isRail=" << isRail()
4331 : << " speedOk=" << (getPreviousSpeed() <= speedThreshold)
4332 : << " posOK=" << (myState.myPos <= myLane->getLength())
4333 : << " normal=" << !myLane->isInternal()
4334 : << " routeOK=" << ((myCurrEdge + 1) != myRoute->end())
4335 : << " bidi=" << (myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1))
4336 : << " stopOk=" << stopOk
4337 : << "\n";
4338 : #endif
4339 1586418862 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4340 7216279 : && getPreviousSpeed() <= speedThreshold
4341 6136749 : && myState.myPos <= myLane->getLength()
4342 6135624 : && !myLane->isInternal()
4343 6065885 : && (myCurrEdge + 1) != myRoute->end()
4344 5964557 : && myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1)
4345 : // ensure there are no further stops on this edge
4346 1587268136 : && stopOk
4347 : ) {
4348 : //if (isSelected()) std::cout << " check1 passed\n";
4349 :
4350 : // ensure that the vehicle is fully on bidi edges that allow reversal
4351 180793 : const int neededFutureRoute = 1 + (int)(MSGlobals::gUsingInternalLanes
4352 : ? myFurtherLanes.size()
4353 504 : : ceil((double)myFurtherLanes.size() / 2.0));
4354 180793 : const int remainingRoute = int(myRoute->end() - myCurrEdge) - 1;
4355 180793 : if (remainingRoute < neededFutureRoute) {
4356 : #ifdef DEBUG_REVERSE_BIDI
4357 : if (DEBUG_COND) {
4358 : std::cout << " fail: remainingEdges=" << ((int)(myRoute->end() - myCurrEdge)) << " further=" << myFurtherLanes.size() << "\n";
4359 : }
4360 : #endif
4361 3567 : return getMaxSpeed();
4362 : }
4363 : //if (isSelected()) std::cout << " check2 passed\n";
4364 :
4365 : // ensure that the turn-around connection exists from the current edge to its bidi-edge
4366 177226 : const MSEdgeVector& succ = myLane->getEdge().getSuccessors();
4367 177226 : if (std::find(succ.begin(), succ.end(), myLane->getEdge().getBidiEdge()) == succ.end()) {
4368 : #ifdef DEBUG_REVERSE_BIDI
4369 : if (DEBUG_COND) {
4370 : std::cout << " noTurn (bidi=" << myLane->getEdge().getBidiEdge()->getID() << " succ=" << toString(succ) << "\n";
4371 : }
4372 : #endif
4373 909 : return getMaxSpeed();
4374 : }
4375 : //if (isSelected()) std::cout << " check3 passed\n";
4376 :
4377 : // ensure that the vehicle front will not move past a stop on the bidi edge of the current edge
4378 176317 : if (!myStops.empty() && myStops.front().edge == (myCurrEdge + 1)) {
4379 160006 : const double stopPos = myStops.front().getEndPos(*this);
4380 160006 : const double brakeDist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
4381 160006 : const double newPos = myLane->getLength() - (getBackPositionOnLane() + brakeDist);
4382 160006 : if (newPos > stopPos) {
4383 : #ifdef DEBUG_REVERSE_BIDI
4384 : if (DEBUG_COND) {
4385 : std::cout << " reversal would go past stop on " << myLane->getBidiLane()->getID() << "\n";
4386 : }
4387 : #endif
4388 158332 : if (seen > MAX2(brakeDist, 1.0)) {
4389 157202 : return getMaxSpeed();
4390 : } else {
4391 : #ifdef DEBUG_REVERSE_BIDI
4392 : if (DEBUG_COND) {
4393 : std::cout << " train is too long, skipping stop at " << stopPos << " cannot be avoided\n";
4394 : }
4395 : #endif
4396 : }
4397 : }
4398 : }
4399 : //if (isSelected()) std::cout << " check4 passed\n";
4400 :
4401 : // ensure that bidi-edges exist for all further edges
4402 : // and that no stops will be skipped when reversing
4403 : // and that the train will not be on top of a red rail signal after reversal
4404 19115 : const MSLane* bidi = myLane->getBidiLane();
4405 : int view = 2;
4406 38572 : for (MSLane* further : myFurtherLanes) {
4407 21893 : if (!further->getEdge().isInternal()) {
4408 11393 : if (further->getEdge().getBidiEdge() != *(myCurrEdge + view)) {
4409 : #ifdef DEBUG_REVERSE_BIDI
4410 : if (DEBUG_COND) {
4411 : std::cout << " noBidi view=" << view << " further=" << further->getID() << " furtherBidi=" << Named::getIDSecure(further->getEdge().getBidiEdge()) << " future=" << (*(myCurrEdge + view))->getID() << "\n";
4412 : }
4413 : #endif
4414 2277 : return getMaxSpeed();
4415 : }
4416 9116 : const MSLane* nextBidi = further->getBidiLane();
4417 9116 : const MSLink* toNext = bidi->getLinkTo(nextBidi);
4418 9116 : if (toNext == nullptr) {
4419 : // can only happen if the route is invalid
4420 0 : return getMaxSpeed();
4421 : }
4422 9116 : if (toNext->haveRed()) {
4423 : #ifdef DEBUG_REVERSE_BIDI
4424 : if (DEBUG_COND) {
4425 : std::cout << " do not reverse on a red signal\n";
4426 : }
4427 : #endif
4428 0 : return getMaxSpeed();
4429 : }
4430 : bidi = nextBidi;
4431 9116 : if (!myStops.empty() && myStops.front().edge == (myCurrEdge + view)) {
4432 453 : const double brakeDist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
4433 453 : const double stopPos = myStops.front().getEndPos(*this);
4434 453 : const double newPos = further->getLength() - (getBackPositionOnLane(further) + brakeDist);
4435 453 : if (newPos > stopPos) {
4436 : #ifdef DEBUG_REVERSE_BIDI
4437 : if (DEBUG_COND) {
4438 : std::cout << " reversal would go past stop on further-opposite lane " << further->getBidiLane()->getID() << "\n";
4439 : }
4440 : #endif
4441 171 : if (seen > MAX2(brakeDist, 1.0)) {
4442 159 : canReverse = false;
4443 159 : return getMaxSpeed();
4444 : } else {
4445 : #ifdef DEBUG_REVERSE_BIDI
4446 : if (DEBUG_COND) {
4447 : std::cout << " train is too long, skipping stop at " << stopPos << " cannot be avoided\n";
4448 : }
4449 : #endif
4450 : }
4451 : }
4452 : }
4453 8957 : view++;
4454 : }
4455 : }
4456 : // reverse as soon as comfortably possible
4457 16679 : const double vMinComfortable = getCarFollowModel().minNextSpeed(getSpeed(), this);
4458 : #ifdef DEBUG_REVERSE_BIDI
4459 : if (DEBUG_COND) {
4460 : std::cout << SIMTIME << " seen=" << seen << " vReverseOK=" << vMinComfortable << "\n";
4461 : }
4462 : #endif
4463 16679 : canReverse = true;
4464 16679 : return vMinComfortable;
4465 : }
4466 1586238069 : return getMaxSpeed();
4467 : }
4468 :
4469 :
4470 : void
4471 705326155 : MSVehicle::processLaneAdvances(std::vector<MSLane*>& passedLanes, std::string& emergencyReason) {
4472 721251393 : for (std::vector<MSLane*>::reverse_iterator i = myFurtherLanes.rbegin(); i != myFurtherLanes.rend(); ++i) {
4473 15925238 : passedLanes.push_back(*i);
4474 : }
4475 705326155 : if (passedLanes.size() == 0 || passedLanes.back() != myLane) {
4476 705326155 : passedLanes.push_back(myLane);
4477 : }
4478 : // let trains reverse direction
4479 705326155 : bool reverseTrain = false;
4480 705326155 : checkReversal(reverseTrain);
4481 705326155 : if (reverseTrain) {
4482 : // Train is 'reversing' so toggle the logical state
4483 810 : myAmReversed = !myAmReversed;
4484 : // add some slack to ensure that the back of train does appear looped
4485 810 : myState.myPos += 2 * (myLane->getLength() - myState.myPos) + myType->getLength() + NUMERICAL_EPS;
4486 810 : myState.mySpeed = 0;
4487 : #ifdef DEBUG_REVERSE_BIDI
4488 : if (DEBUG_COND) {
4489 : std::cout << SIMTIME << " reversing train=" << getID() << " newPos=" << myState.myPos << "\n";
4490 : }
4491 : #endif
4492 : }
4493 : // move on lane(s)
4494 705326155 : if (myState.myPos > myLane->getLength()) {
4495 : // The vehicle has moved at least to the next lane (maybe it passed even more than one)
4496 20145221 : if (myCurrEdge != myRoute->end() - 1) {
4497 16994844 : MSLane* approachedLane = myLane;
4498 : // move the vehicle forward
4499 16994844 : myNextDriveItem = myLFLinkLanes.begin();
4500 36673432 : while (myNextDriveItem != myLFLinkLanes.end() && approachedLane != nullptr && myState.myPos > approachedLane->getLength()) {
4501 19697502 : const MSLink* link = myNextDriveItem->myLink;
4502 19697502 : const double linkDist = myNextDriveItem->myDistance;
4503 : ++myNextDriveItem;
4504 : // check whether the vehicle was allowed to enter lane
4505 : // otherwise it is decelerated and we do not need to test for it's
4506 : // approach on the following lanes when a lane changing is performed
4507 : // proceed to the next lane
4508 19697502 : if (approachedLane->mustCheckJunctionCollisions()) {
4509 : // vehicle moves past approachedLane within a single step, collision checking must still be done
4510 66709 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(approachedLane);
4511 : }
4512 19697502 : if (link != nullptr) {
4513 19693300 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4514 46078 : && !myLane->isInternal()
4515 24251 : && myLane->getBidiLane() != nullptr
4516 13042 : && link->getLane()->getBidiLane() == myLane
4517 19694107 : && !reverseTrain) {
4518 : emergencyReason = " because it must reverse direction";
4519 : approachedLane = nullptr;
4520 : break;
4521 : }
4522 19693297 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4523 46075 : && myState.myPos < myLane->getLength() + NUMERICAL_EPS
4524 19693507 : && hasStops() && getNextStop().edge == myCurrEdge) {
4525 : // avoid skipping stop due to numerical instability
4526 : // this is a special case for rail vehicles because they
4527 : // continue myLFLinkLanes past stops
4528 196 : approachedLane = myLane;
4529 196 : myState.myPos = myLane->getLength();
4530 196 : break;
4531 : }
4532 19693101 : approachedLane = link->getViaLaneOrLane();
4533 19693101 : if (myInfluencer == nullptr || myInfluencer->getEmergencyBrakeRedLight()) {
4534 19691496 : bool beyondStopLine = linkDist < link->getLaneBefore()->getVehicleStopOffset(this);
4535 19691496 : if (link->haveRed() && !ignoreRed(link, false) && !beyondStopLine && !reverseTrain) {
4536 : emergencyReason = " because of a red traffic light";
4537 : break;
4538 : }
4539 : }
4540 19693029 : if (reverseTrain && approachedLane->isInternal()) {
4541 : // avoid getting stuck on a slow turn-around internal lane
4542 888 : myState.myPos += approachedLane->getLength();
4543 : }
4544 4202 : } else if (myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
4545 : // avoid warning due to numerical instability
4546 227 : approachedLane = myLane;
4547 227 : myState.myPos = myLane->getLength();
4548 3975 : } else if (reverseTrain) {
4549 0 : approachedLane = (*(myCurrEdge + 1))->getLanes()[0];
4550 0 : link = myLane->getLinkTo(approachedLane);
4551 : assert(link != 0);
4552 0 : while (link->getViaLane() != nullptr) {
4553 0 : link = link->getViaLane()->getLinkCont()[0];
4554 : }
4555 : --myNextDriveItem;
4556 : } else {
4557 : emergencyReason = " because there is no connection to the next edge";
4558 : approachedLane = nullptr;
4559 : break;
4560 : }
4561 19693256 : if (approachedLane != myLane && approachedLane != nullptr) {
4562 19693029 : leaveLane(MSMoveReminder::NOTIFICATION_JUNCTION, approachedLane);
4563 19693029 : myState.myPos -= myLane->getLength();
4564 : assert(myState.myPos > 0);
4565 19693029 : enterLaneAtMove(approachedLane);
4566 19693029 : if (link->isEntryLink()) {
4567 7707594 : myJunctionEntryTime = MSNet::getInstance()->getCurrentTimeStep();
4568 7707594 : myJunctionEntryTimeNeverYield = myJunctionEntryTime;
4569 7707594 : myHaveStoppedFor = nullptr;
4570 : }
4571 19693029 : if (link->isConflictEntryLink()) {
4572 7707009 : myJunctionConflictEntryTime = MSNet::getInstance()->getCurrentTimeStep();
4573 : // renew yielded request
4574 7707009 : myJunctionEntryTime = myJunctionEntryTimeNeverYield;
4575 : }
4576 19693029 : if (link->isExitLink()) {
4577 : // passed junction, reset for approaching the next one
4578 7645221 : myJunctionEntryTime = SUMOTime_MAX;
4579 7645221 : myJunctionEntryTimeNeverYield = SUMOTime_MAX;
4580 7645221 : myJunctionConflictEntryTime = SUMOTime_MAX;
4581 : }
4582 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
4583 : if (DEBUG_COND) {
4584 : std::cout << "Update junctionTimes link=" << link->getViaLaneOrLane()->getID()
4585 : << " entry=" << link->isEntryLink() << " conflict=" << link->isConflictEntryLink() << " exit=" << link->isExitLink()
4586 : << " ET=" << myJunctionEntryTime
4587 : << " ETN=" << myJunctionEntryTimeNeverYield
4588 : << " CET=" << myJunctionConflictEntryTime
4589 : << "\n";
4590 : }
4591 : #endif
4592 19693029 : if (hasArrivedInternal()) {
4593 : break;
4594 : }
4595 19679117 : if (myLaneChangeModel->isChangingLanes()) {
4596 7273 : if (link->getDirection() == LinkDirection::LEFT || link->getDirection() == LinkDirection::RIGHT) {
4597 : // abort lane change
4598 54 : WRITE_WARNINGF("Vehicle '%' could not finish continuous lane change (turn lane) time=%.", getID(), time2string(SIMSTEP));
4599 18 : myLaneChangeModel->endLaneChangeManeuver();
4600 : }
4601 : }
4602 19679117 : if (approachedLane->getEdge().isVaporizing()) {
4603 756 : leaveLane(MSMoveReminder::NOTIFICATION_VAPORIZED_VAPORIZER);
4604 : break;
4605 : }
4606 19678361 : passedLanes.push_back(approachedLane);
4607 : }
4608 : }
4609 : // NOTE: Passed drive items will be erased in the next simstep's planMove()
4610 :
4611 : #ifdef DEBUG_ACTIONSTEPS
4612 : if (DEBUG_COND && myNextDriveItem != myLFLinkLanes.begin()) {
4613 : std::cout << "Updated drive items:" << std::endl;
4614 : for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
4615 : std::cout
4616 : << " vPass=" << (*i).myVLinkPass
4617 : << " vWait=" << (*i).myVLinkWait
4618 : << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
4619 : << " request=" << (*i).mySetRequest
4620 : << "\n";
4621 : }
4622 : }
4623 : #endif
4624 3150377 : } else if (!hasArrivedInternal() && myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
4625 : // avoid warning due to numerical instability when stopping at the end of the route
4626 91 : myState.myPos = myLane->getLength();
4627 : }
4628 :
4629 : }
4630 705326155 : }
4631 :
4632 :
4633 :
4634 : bool
4635 709197496 : MSVehicle::executeMove() {
4636 : #ifdef DEBUG_EXEC_MOVE
4637 : if (DEBUG_COND) {
4638 : std::cout << "\nEXECUTE_MOVE\n"
4639 : << SIMTIME
4640 : << " veh=" << getID()
4641 : << " speed=" << getSpeed() // toString(getSpeed(), 24)
4642 : << std::endl;
4643 : }
4644 : #endif
4645 :
4646 :
4647 : // Maximum safe velocity
4648 709197496 : double vSafe = std::numeric_limits<double>::max();
4649 : // Minimum safe velocity (lower bound).
4650 709197496 : double vSafeMin = -std::numeric_limits<double>::max();
4651 : // The distance to a link, which should either be crossed this step
4652 : // or in front of which we need to stop.
4653 709197496 : double vSafeMinDist = 0;
4654 :
4655 709197496 : if (myActionStep) {
4656 : // Actuate control (i.e. choose bounds for safe speed in current simstep (euler), resp. after current sim step (ballistic))
4657 637469645 : processLinkApproaches(vSafe, vSafeMin, vSafeMinDist);
4658 : #ifdef DEBUG_ACTIONSTEPS
4659 : if (DEBUG_COND) {
4660 : std::cout << SIMTIME << " vehicle '" << getID() << "'\n"
4661 : " vsafe from processLinkApproaches(): vsafe " << vSafe << std::endl;
4662 : }
4663 : #endif
4664 : } else {
4665 : // Continue with current acceleration
4666 71727851 : vSafe = getSpeed() + ACCEL2SPEED(myAcceleration);
4667 : #ifdef DEBUG_ACTIONSTEPS
4668 : if (DEBUG_COND) {
4669 : std::cout << SIMTIME << " vehicle '" << getID() << "' skips processLinkApproaches()\n"
4670 : " continues with constant accel " << myAcceleration << "...\n"
4671 : << "speed: " << getSpeed() << " -> " << vSafe << std::endl;
4672 : }
4673 : #endif
4674 : }
4675 :
4676 :
4677 : //#ifdef DEBUG_EXEC_MOVE
4678 : // if (DEBUG_COND) {
4679 : // std::cout << "vSafe = " << toString(vSafe,12) << "\n" << std::endl;
4680 : // }
4681 : //#endif
4682 :
4683 : // Determine vNext = speed after current sim step (ballistic), resp. in current simstep (euler)
4684 : // Call to finalizeSpeed applies speed reduction due to dawdling / lane changing but ensures minimum safe speed
4685 709197496 : double vNext = vSafe;
4686 : const MSCFModel& cfModel = getCarFollowModel();
4687 709197496 : const double rawAccel = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
4688 709197496 : if (vNext <= SUMO_const_haltingSpeed * TS && myWaitingTime > MSGlobals::gStartupWaitThreshold && rawAccel <= accelThresholdForWaiting() && myActionStep) {
4689 78470399 : myTimeSinceStartup = 0;
4690 630727097 : } else if (isStopped()) {
4691 : // do not apply startupDelay for waypoints
4692 18210275 : if (cfModel.startupDelayStopped() && getNextStop().pars.speed <= 0) {
4693 13772 : myTimeSinceStartup = DELTA_T;
4694 : } else {
4695 : // do not apply startupDelay but signal that a stop has taken place
4696 18196503 : myTimeSinceStartup = cfModel.getStartupDelay() + DELTA_T;
4697 : }
4698 : } else {
4699 : // identify potential startup (before other effects reduce the speed again)
4700 612516822 : myTimeSinceStartup += DELTA_T;
4701 : }
4702 709197496 : if (myActionStep) {
4703 637469645 : vNext = cfModel.finalizeSpeed(this, vSafe);
4704 633375840 : if (vNext > 0) {
4705 587420199 : vNext = MAX2(vNext, vSafeMin);
4706 : }
4707 : }
4708 : // (Leo) to avoid tiny oscillations (< 1e-10) of vNext in a standing vehicle column (observed for ballistic update), we cap off vNext
4709 : // (We assure to do this only for vNext<<NUMERICAL_EPS since otherwise this would nullify the workaround for #2995
4710 : // (Jakob) We also need to make sure to reach a stop at the start of the next edge
4711 705103691 : if (fabs(vNext) < NUMERICAL_EPS_SPEED && (myStopDist > POSITION_EPS || (hasStops() && myCurrEdge == getNextStop().edge))) {
4712 : vNext = 0.;
4713 : }
4714 : #ifdef DEBUG_EXEC_MOVE
4715 : if (DEBUG_COND) {
4716 : std::cout << SIMTIME << " finalizeSpeed vSafe=" << vSafe << " vSafeMin=" << (vSafeMin == -std::numeric_limits<double>::max() ? "-Inf" : toString(vSafeMin))
4717 : << " vNext=" << vNext << " (i.e. accel=" << SPEED2ACCEL(vNext - getSpeed()) << ")" << std::endl;
4718 : }
4719 : #endif
4720 :
4721 : // vNext may be higher than vSafe without implying a bug:
4722 : // - when approaching a green light that suddenly switches to yellow
4723 : // - when using unregulated junctions
4724 : // - when using tau < step-size
4725 : // - when using unsafe car following models
4726 : // - when using TraCI and some speedMode / laneChangeMode settings
4727 : //if (vNext > vSafe + NUMERICAL_EPS) {
4728 : // WRITE_WARNING("vehicle '" + getID() + "' cannot brake hard enough to reach safe speed "
4729 : // + toString(vSafe, 4) + ", moving at " + toString(vNext, 4) + " instead. time="
4730 : // + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
4731 : //}
4732 :
4733 705103691 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4734 : vNext = MAX2(vNext, 0.);
4735 : } else {
4736 : // (Leo) Ballistic: negative vNext can be used to indicate a stop within next step.
4737 : }
4738 :
4739 : // Check for speed advices from the traci client
4740 705103691 : vNext = processTraCISpeedControl(vSafe, vNext);
4741 :
4742 : // the acceleration of a vehicle equipped with the elecHybrid device is restricted by the maximal power of the electric drive as well
4743 705103691 : MSDevice_ElecHybrid* elecHybridOfVehicle = dynamic_cast<MSDevice_ElecHybrid*>(getDevice(typeid(MSDevice_ElecHybrid)));
4744 981 : if (elecHybridOfVehicle != nullptr) {
4745 : // this is the consumption given by the car following model-computed acceleration
4746 981 : elecHybridOfVehicle->setConsum(elecHybridOfVehicle->consumption(*this, (vNext - this->getSpeed()) / TS, vNext));
4747 : // but the maximum power of the electric motor may be lower
4748 : // it needs to be converted from [W] to [Wh/s] (3600s / 1h) so that TS can be taken into account
4749 981 : double maxPower = getEmissionParameters()->getDoubleOptional(SUMO_ATTR_MAXIMUMPOWER, 100000.) / 3600;
4750 981 : if (elecHybridOfVehicle->getConsum() / TS > maxPower) {
4751 : // no, we cannot accelerate that fast, recompute the maximum possible acceleration
4752 70 : double accel = elecHybridOfVehicle->acceleration(*this, maxPower, this->getSpeed());
4753 : // and update the speed of the vehicle
4754 70 : vNext = MIN2(vNext, this->getSpeed() + accel * TS);
4755 : vNext = MAX2(vNext, 0.);
4756 : // and set the vehicle consumption to reflect this
4757 70 : elecHybridOfVehicle->setConsum(elecHybridOfVehicle->consumption(*this, (vNext - this->getSpeed()) / TS, vNext));
4758 : }
4759 : }
4760 :
4761 705103691 : setBrakingSignals(vNext);
4762 :
4763 : // update position and speed
4764 705103691 : int oldLaneOffset = myLane->getEdge().getNumLanes() - myLane->getIndex();
4765 : const MSLane* oldLaneMaybeOpposite = myLane;
4766 705103691 : if (myLaneChangeModel->isOpposite()) {
4767 : // transform to the forward-direction lane, move and then transform back
4768 404235 : myState.myPos = myLane->getOppositePos(myState.myPos);
4769 404235 : myLane = myLane->getParallelOpposite();
4770 : }
4771 705103691 : updateState(vNext);
4772 705103691 : updateWaitingTime(vNext);
4773 :
4774 : // Lanes, which the vehicle touched at some moment of the executed simstep
4775 : std::vector<MSLane*> passedLanes;
4776 : // remember previous lane (myLane is updated in processLaneAdvances)
4777 705103691 : const MSLane* oldLane = myLane;
4778 : // Reason for a possible emergency stop
4779 : std::string emergencyReason;
4780 705103691 : processLaneAdvances(passedLanes, emergencyReason);
4781 :
4782 705103691 : updateTimeLoss(vNext);
4783 705103691 : myCollisionImmunity = MAX2((SUMOTime) - 1, myCollisionImmunity - DELTA_T);
4784 :
4785 705103691 : if (!hasArrivedInternal() && !myLane->getEdge().isVaporizing()) {
4786 701757907 : if (myState.myPos > myLane->getLength()) {
4787 418 : if (emergencyReason == "") {
4788 56 : emergencyReason = TL(" for unknown reasons");
4789 : }
4790 1672 : WRITE_WARNINGF(TL("Vehicle '%' performs emergency stop at the end of lane '%'% (decel=%, offset=%), time=%."),
4791 : getID(), myLane->getID(), emergencyReason, myAcceleration - myState.mySpeed,
4792 : myState.myPos - myLane->getLength(), time2string(SIMSTEP));
4793 418 : MSNet::getInstance()->getVehicleControl().registerEmergencyStop();
4794 418 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::EMERGENCYSTOP);
4795 418 : myState.myPos = myLane->getLength();
4796 418 : myState.mySpeed = 0;
4797 418 : myAcceleration = 0;
4798 : }
4799 701757907 : const MSLane* oldBackLane = getBackLane();
4800 701757907 : if (myLaneChangeModel->isOpposite()) {
4801 : passedLanes.clear(); // ignore back occupation
4802 : }
4803 : #ifdef DEBUG_ACTIONSTEPS
4804 : if (DEBUG_COND) {
4805 : std::cout << SIMTIME << " veh '" << getID() << "' updates further lanes." << std::endl;
4806 : }
4807 : #endif
4808 701757907 : myState.myBackPos = updateFurtherLanes(myFurtherLanes, myFurtherLanesPosLat, passedLanes);
4809 701757907 : if (passedLanes.size() > 1 && isRail()) {
4810 865443 : for (auto pi = passedLanes.rbegin(); pi != passedLanes.rend(); ++pi) {
4811 654459 : MSLane* pLane = *pi;
4812 654459 : if (pLane != myLane && std::find(myFurtherLanes.begin(), myFurtherLanes.end(), pLane) == myFurtherLanes.end()) {
4813 45731 : leaveLaneBack(MSMoveReminder::NOTIFICATION_JUNCTION, *pi);
4814 : }
4815 : }
4816 : }
4817 : // bestLanes need to be updated before lane changing starts. NOTE: This call is also a presumption for updateDriveItems()
4818 701757907 : updateBestLanes();
4819 701757907 : if (myLane != oldLane || oldBackLane != getBackLane()) {
4820 24945146 : if (myLaneChangeModel->getShadowLane() != nullptr || getLateralOverlap() > POSITION_EPS) {
4821 : // shadow lane must be updated if the front or back lane changed
4822 : // either if we already have a shadowLane or if there is lateral overlap
4823 553990 : myLaneChangeModel->updateShadowLane();
4824 : }
4825 24945146 : if (MSGlobals::gLateralResolution > 0 && !myLaneChangeModel->isOpposite()) {
4826 : // The vehicles target lane must be also be updated if the front or back lane changed
4827 4409720 : myLaneChangeModel->updateTargetLane();
4828 : }
4829 : }
4830 701757907 : setBlinkerInformation(); // needs updated bestLanes
4831 : //change the blue light only for emergency vehicles SUMOVehicleClass
4832 701757907 : if (myType->getVehicleClass() == SVC_EMERGENCY) {
4833 85638 : setEmergencyBlueLight(MSNet::getInstance()->getCurrentTimeStep());
4834 : }
4835 : // must be done before angle computation
4836 : // State needs to be reset for all vehicles before the next call to MSEdgeControl::changeLanes
4837 701757907 : if (myActionStep) {
4838 : // check (#2681): Can this be skipped?
4839 630051277 : myLaneChangeModel->prepareStep();
4840 : } else {
4841 71706630 : myLaneChangeModel->resetSpeedLat();
4842 : #ifdef DEBUG_ACTIONSTEPS
4843 : if (DEBUG_COND) {
4844 : std::cout << SIMTIME << " veh '" << getID() << "' skips LCM->prepareStep()." << std::endl;
4845 : }
4846 : #endif
4847 : }
4848 701757907 : myLaneChangeModel->setPreviousAngleOffset(myLaneChangeModel->getAngleOffset());
4849 701757907 : myAngle = computeAngle();
4850 : }
4851 :
4852 : #ifdef DEBUG_EXEC_MOVE
4853 : if (DEBUG_COND) {
4854 : std::cout << SIMTIME << " executeMove finished veh=" << getID() << " lane=" << myLane->getID() << " myPos=" << getPositionOnLane() << " myPosLat=" << getLateralPositionOnLane() << "\n";
4855 : gDebugFlag1 = false; // See MSLink_DEBUG_OPENED
4856 : }
4857 : #endif
4858 705103691 : if (myLaneChangeModel->isOpposite()) {
4859 : // transform back to the opposite-direction lane
4860 : MSLane* newOpposite = nullptr;
4861 404235 : const MSEdge* newOppositeEdge = myLane->getEdge().getOppositeEdge();
4862 404235 : if (newOppositeEdge != nullptr) {
4863 404185 : newOpposite = newOppositeEdge->getLanes()[newOppositeEdge->getNumLanes() - MAX2(1, oldLaneOffset)];
4864 : #ifdef DEBUG_EXEC_MOVE
4865 : if (DEBUG_COND) {
4866 : std::cout << SIMTIME << " newOppositeEdge=" << newOppositeEdge->getID() << " oldLaneOffset=" << oldLaneOffset << " leftMost=" << newOppositeEdge->getNumLanes() - 1 << " newOpposite=" << Named::getIDSecure(newOpposite) << "\n";
4867 : }
4868 : #endif
4869 : }
4870 404185 : if (newOpposite == nullptr) {
4871 50 : if (!myLaneChangeModel->hasBlueLight()) {
4872 : // unusual overtaking at junctions is ok for emergency vehicles
4873 0 : WRITE_WARNINGF(TL("Unexpected end of opposite lane for vehicle '%' at lane '%', time=%."),
4874 : getID(), myLane->getID(), time2string(SIMSTEP));
4875 : }
4876 50 : myLaneChangeModel->changedToOpposite();
4877 50 : if (myState.myPos < getLength()) {
4878 : // further lanes is always cleared during opposite driving
4879 50 : MSLane* oldOpposite = oldLane->getOpposite();
4880 50 : if (oldOpposite != nullptr) {
4881 50 : myFurtherLanes.push_back(oldOpposite);
4882 51 : myFurtherLanesPosLat.push_back(0);
4883 : // small value since the lane is going in the other direction
4884 50 : myState.myBackPos = getLength() - myState.myPos;
4885 50 : myAngle = computeAngle();
4886 : } else {
4887 : SOFT_ASSERT(false);
4888 : }
4889 : }
4890 : } else {
4891 404185 : myState.myPos = myLane->getOppositePos(myState.myPos);
4892 404185 : myLane = newOpposite;
4893 : oldLane = oldLaneMaybeOpposite;
4894 : //std::cout << SIMTIME << " updated myLane=" << Named::getIDSecure(myLane) << " oldLane=" << oldLane->getID() << "\n";
4895 404185 : myCachedPosition = Position::INVALID;
4896 404185 : myLaneChangeModel->updateShadowLane();
4897 : }
4898 : }
4899 : // myAngle was already updated. Update lastAngle so moveRemindes have consisent angleDiff (after finalizeSpeed because it uses the old angles)
4900 705103691 : myLastAngle = myRawAngle;
4901 : // store angle before lane changing
4902 705103691 : myRawAngle = myAngle;
4903 :
4904 705103691 : workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
4905 : // Return whether the vehicle did move to another lane
4906 1410207380 : return myLane != oldLane;
4907 705103691 : }
4908 :
4909 : void
4910 222464 : MSVehicle::executeFractionalMove(double dist) {
4911 222464 : myState.myPos += dist;
4912 222464 : myState.myLastCoveredDist = dist;
4913 222464 : myCachedPosition = Position::INVALID;
4914 :
4915 222464 : const std::vector<const MSLane*> lanes = getUpcomingLanesUntil(dist);
4916 222464 : const SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
4917 460933 : for (int i = 0; i < (int)lanes.size(); i++) {
4918 238469 : MSLink* link = nullptr;
4919 238469 : if (i + 1 < (int)lanes.size()) {
4920 16005 : const MSLane* const to = lanes[i + 1];
4921 16005 : const bool internal = to->isInternal();
4922 16010 : for (MSLink* const l : lanes[i]->getLinkCont()) {
4923 16010 : if ((internal && l->getViaLane() == to) || (!internal && l->getLane() == to)) {
4924 16005 : link = l;
4925 16005 : break;
4926 : }
4927 : }
4928 : }
4929 238469 : myLFLinkLanes.emplace_back(link, getSpeed(), getSpeed(), true, t, getSpeed(), 0, 0, dist);
4930 : }
4931 : // minimum execute move:
4932 : std::vector<MSLane*> passedLanes;
4933 : // Reason for a possible emergency stop
4934 222464 : if (lanes.size() > 1) {
4935 4005 : myLane->removeVehicle(this, MSMoveReminder::NOTIFICATION_JUNCTION, false);
4936 : }
4937 : std::string emergencyReason;
4938 222464 : processLaneAdvances(passedLanes, emergencyReason);
4939 : #ifdef DEBUG_EXTRAPOLATE_DEPARTPOS
4940 : if (DEBUG_COND) {
4941 : std::cout << SIMTIME << " veh=" << getID() << " executeFractionalMove dist=" << dist
4942 : << " passedLanes=" << toString(passedLanes) << " lanes=" << toString(lanes)
4943 : << " finalPos=" << myState.myPos
4944 : << " speed=" << getSpeed()
4945 : << " myFurtherLanes=" << toString(myFurtherLanes)
4946 : << "\n";
4947 : }
4948 : #endif
4949 222464 : workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
4950 222464 : if (lanes.size() > 1) {
4951 4010 : for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
4952 : #ifdef DEBUG_FURTHER
4953 : if (DEBUG_COND) {
4954 : std::cout << SIMTIME << " leaveLane \n";
4955 : }
4956 : #endif
4957 5 : (*i)->resetPartialOccupation(this);
4958 : }
4959 : myFurtherLanes.clear();
4960 : myFurtherLanesPosLat.clear();
4961 4005 : myLane->forceVehicleInsertion(this, getPositionOnLane(), MSMoveReminder::NOTIFICATION_JUNCTION, getLateralPositionOnLane());
4962 : }
4963 222464 : }
4964 :
4965 :
4966 : void
4967 713451025 : MSVehicle::updateState(double vNext, bool parking) {
4968 : // update position and speed
4969 : double deltaPos; // positional change
4970 713451025 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4971 : // euler
4972 613690239 : deltaPos = SPEED2DIST(vNext);
4973 : } else {
4974 : // ballistic
4975 99760786 : deltaPos = getDeltaPos(SPEED2ACCEL(vNext - myState.mySpeed));
4976 : }
4977 :
4978 : // the *mean* acceleration during the next step (probably most appropriate for emission calculation)
4979 : // NOTE: for the ballistic update vNext may be negative, indicating a stop.
4980 713451025 : myAcceleration = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
4981 :
4982 : #ifdef DEBUG_EXEC_MOVE
4983 : if (DEBUG_COND) {
4984 : std::cout << SIMTIME << " updateState() for veh '" << getID() << "': deltaPos=" << deltaPos
4985 : << " pos=" << myState.myPos << " newPos=" << myState.myPos + deltaPos << std::endl;
4986 : }
4987 : #endif
4988 713451025 : double decelPlus = -myAcceleration - getCarFollowModel().getMaxDecel() - NUMERICAL_EPS;
4989 713451025 : if (decelPlus > 0) {
4990 448643 : const double previousAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
4991 448643 : if (myAcceleration + NUMERICAL_EPS < previousAcceleration) {
4992 : // vehicle brakes beyond wished maximum deceleration (only warn at the start of the braking manoeuvre)
4993 319326 : decelPlus += 2 * NUMERICAL_EPS;
4994 319326 : const double emergencyFraction = decelPlus / MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel());
4995 319326 : if (emergencyFraction >= MSGlobals::gEmergencyDecelWarningThreshold) {
4996 94269 : WRITE_WARNINGF(TL("Vehicle '%' performs emergency braking on lane '%' with decel=%, wished=%, severity=%, time=%."),
4997 : //+ " decelPlus=" + toString(decelPlus)
4998 : //+ " prevAccel=" + toString(previousAcceleration)
4999 : //+ " reserve=" + toString(MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel()))
5000 : getID(), myLane->getID(), -myAcceleration, getCarFollowModel().getMaxDecel(), emergencyFraction, time2string(SIMSTEP));
5001 31423 : MSNet::getInstance()->getVehicleControl().registerEmergencyBraking();
5002 : }
5003 : }
5004 : }
5005 :
5006 713451025 : myState.myPreviousSpeed = myState.mySpeed;
5007 713451025 : myState.mySpeed = MAX2(vNext, 0.);
5008 :
5009 713451025 : if (isRemoteControlled()) {
5010 7177 : deltaPos = myInfluencer->implicitDeltaPosRemote(this);
5011 : }
5012 :
5013 713451025 : myState.myPos += deltaPos;
5014 713451025 : myState.myLastCoveredDist = deltaPos;
5015 713451025 : myNextTurn.first -= deltaPos;
5016 :
5017 713451025 : if (!parking) {
5018 705103691 : myCachedPosition = Position::INVALID;
5019 : }
5020 713451025 : }
5021 :
5022 : void
5023 8347334 : MSVehicle::updateParkingState() {
5024 8347334 : updateState(0, true);
5025 : // deboard while parked
5026 8347334 : if (myPersonDevice != nullptr) {
5027 625579 : myPersonDevice->notifyMove(*this, getPositionOnLane(), getPositionOnLane(), 0);
5028 : }
5029 8347334 : if (myContainerDevice != nullptr) {
5030 59887 : myContainerDevice->notifyMove(*this, getPositionOnLane(), getPositionOnLane(), 0);
5031 : }
5032 16862715 : for (MSVehicleDevice* const dev : myDevices) {
5033 8515381 : dev->notifyParking();
5034 : }
5035 8347334 : }
5036 :
5037 :
5038 : void
5039 30660 : MSVehicle::replaceVehicleType(const MSVehicleType* type) {
5040 30660 : MSBaseVehicle::replaceVehicleType(type);
5041 30660 : delete myCFVariables;
5042 30660 : myCFVariables = type->getCarFollowModel().createVehicleVariables();
5043 30660 : }
5044 :
5045 :
5046 : const MSLane*
5047 1387360644 : MSVehicle::getBackLane() const {
5048 1387360644 : if (myFurtherLanes.size() > 0) {
5049 18889193 : return myFurtherLanes.back();
5050 : } else {
5051 1368471451 : return myLane;
5052 : }
5053 : }
5054 :
5055 :
5056 : double
5057 707272732 : MSVehicle::updateFurtherLanes(std::vector<MSLane*>& furtherLanes, std::vector<double>& furtherLanesPosLat,
5058 : const std::vector<MSLane*>& passedLanes) {
5059 : #ifdef DEBUG_SETFURTHER
5060 : if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID()
5061 : << " updateFurtherLanes oldFurther=" << toString(furtherLanes)
5062 : << " oldFurtherPosLat=" << toString(furtherLanesPosLat)
5063 : << " passed=" << toString(passedLanes)
5064 : << "\n";
5065 : #endif
5066 723234832 : for (MSLane* further : furtherLanes) {
5067 15962100 : further->resetPartialOccupation(this);
5068 15962100 : if (further->getBidiLane() != nullptr
5069 15962100 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5070 81059 : further->getBidiLane()->resetPartialOccupation(this);
5071 : }
5072 : }
5073 :
5074 : std::vector<MSLane*> newFurther;
5075 : std::vector<double> newFurtherPosLat;
5076 707272732 : double backPosOnPreviousLane = myState.myPos - getLength();
5077 : bool widthShift = myFurtherLanesPosLat.size() > myFurtherLanes.size();
5078 707272732 : if (passedLanes.size() > 1) {
5079 : // There are candidates for further lanes. (passedLanes[-1] is the current lane, or current shadow lane in context of updateShadowLanes())
5080 : std::vector<MSLane*>::const_iterator fi = furtherLanes.begin();
5081 : std::vector<double>::const_iterator fpi = furtherLanesPosLat.begin();
5082 44890641 : for (auto pi = passedLanes.rbegin() + 1; pi != passedLanes.rend() && backPosOnPreviousLane < 0; ++pi) {
5083 : // As long as vehicle back reaches into passed lane, add it to the further lanes
5084 15893548 : MSLane* further = *pi;
5085 15893548 : newFurther.push_back(further);
5086 15893548 : backPosOnPreviousLane += further->setPartialOccupation(this);
5087 15893548 : if (further->getBidiLane() != nullptr
5088 15893548 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5089 79357 : further->getBidiLane()->setPartialOccupation(this);
5090 : }
5091 15893548 : if (fi != furtherLanes.end() && further == *fi) {
5092 : // Lateral position on this lane is already known. Assume constant and use old value.
5093 5661928 : newFurtherPosLat.push_back(*fpi);
5094 : ++fi;
5095 : ++fpi;
5096 : } else {
5097 : // The lane *pi was not in furtherLanes before.
5098 : // If it is downstream, we assume as lateral position the current position
5099 : // If it is a new lane upstream (can appear as shadow further in case of LC-maneuvering, e.g.)
5100 : // we assign the last known lateral position.
5101 10231620 : if (newFurtherPosLat.size() == 0) {
5102 9611045 : if (widthShift) {
5103 1508729 : newFurtherPosLat.push_back(myFurtherLanesPosLat.back());
5104 : } else {
5105 8102316 : newFurtherPosLat.push_back(myState.myPosLat);
5106 : }
5107 : } else {
5108 620575 : newFurtherPosLat.push_back(newFurtherPosLat.back());
5109 : }
5110 : }
5111 : #ifdef DEBUG_SETFURTHER
5112 : if (DEBUG_COND) {
5113 : std::cout << SIMTIME << " updateFurtherLanes \n"
5114 : << " further lane '" << further->getID() << "' backPosOnPreviousLane=" << backPosOnPreviousLane
5115 : << std::endl;
5116 : }
5117 : #endif
5118 : }
5119 28997093 : furtherLanes = newFurther;
5120 28997093 : furtherLanesPosLat = newFurtherPosLat;
5121 : } else {
5122 : furtherLanes.clear();
5123 : furtherLanesPosLat.clear();
5124 : }
5125 : #ifdef DEBUG_SETFURTHER
5126 : if (DEBUG_COND) std::cout
5127 : << " newFurther=" << toString(furtherLanes)
5128 : << " newFurtherPosLat=" << toString(furtherLanesPosLat)
5129 : << " newBackPos=" << backPosOnPreviousLane
5130 : << "\n";
5131 : #endif
5132 707272732 : return backPosOnPreviousLane;
5133 707272732 : }
5134 :
5135 :
5136 : double
5137 35229951803 : MSVehicle::getBackPositionOnLane(const MSLane* lane, bool calledByGetPosition) const {
5138 : #ifdef DEBUG_FURTHER
5139 : if (DEBUG_COND) {
5140 : std::cout << SIMTIME
5141 : << " getBackPositionOnLane veh=" << getID()
5142 : << " lane=" << Named::getIDSecure(lane)
5143 : << " cbgP=" << calledByGetPosition
5144 : << " pos=" << myState.myPos
5145 : << " backPos=" << myState.myBackPos
5146 : << " myLane=" << myLane->getID()
5147 : << " myLaneBidi=" << Named::getIDSecure(myLane->getBidiLane())
5148 : << " further=" << toString(myFurtherLanes)
5149 : << " furtherPosLat=" << toString(myFurtherLanesPosLat)
5150 : << "\n shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
5151 : << " shadowFurther=" << toString(myLaneChangeModel->getShadowFurtherLanes())
5152 : << " shadowFurtherPosLat=" << toString(myLaneChangeModel->getShadowFurtherLanesPosLat())
5153 : << "\n targetLane=" << Named::getIDSecure(myLaneChangeModel->getTargetLane())
5154 : << " furtherTargets=" << toString(myLaneChangeModel->getFurtherTargetLanes())
5155 : << std::endl;
5156 : }
5157 : #endif
5158 35229951803 : if (lane == myLane
5159 8476048778 : || lane == myLaneChangeModel->getShadowLane()
5160 40317343891 : || lane == myLaneChangeModel->getTargetLane()) {
5161 30144066312 : if (myLaneChangeModel->isOpposite()) {
5162 227325128 : if (lane == myLaneChangeModel->getShadowLane()) {
5163 195331817 : return lane->getLength() - myState.myPos - myType->getLength();
5164 : } else {
5165 36921750 : return myState.myPos + (calledByGetPosition ? -1 : 1) * myType->getLength();
5166 : }
5167 29916741184 : } else if (&lane->getEdge() != &myLane->getEdge()) {
5168 21003263 : return lane->getLength() - myState.myPos + (calledByGetPosition ? -1 : 1) * myType->getLength();
5169 : } else {
5170 : // account for parallel lanes of different lengths in the most conservative manner (i.e. while turning)
5171 59792268385 : return myState.myPos - myType->getLength() + MIN2(0.0, lane->getLength() - myLane->getLength());
5172 : }
5173 5085885491 : } else if (lane == myLane->getBidiLane()) {
5174 19134562 : return lane->getLength() - myState.myPos + myType->getLength() * (calledByGetPosition ? -1 : 1);
5175 5071199844 : } else if (myFurtherLanes.size() > 0 && lane == myFurtherLanes.back()) {
5176 5021947457 : return myState.myBackPos;
5177 49252387 : } else if ((myLaneChangeModel->getShadowFurtherLanes().size() > 0 && lane == myLaneChangeModel->getShadowFurtherLanes().back())
5178 49784381 : || (myLaneChangeModel->getFurtherTargetLanes().size() > 0 && lane == myLaneChangeModel->getFurtherTargetLanes().back())) {
5179 : assert(myFurtherLanes.size() > 0);
5180 17666996 : if (lane->getLength() == myFurtherLanes.back()->getLength()) {
5181 17208282 : return myState.myBackPos;
5182 : } else {
5183 : // interpolate
5184 : //if (DEBUG_COND) {
5185 : //if (myFurtherLanes.back()->getLength() != lane->getLength()) {
5186 : // std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " further=" << myFurtherLanes.back()->getID()
5187 : // << " len=" << lane->getLength() << " fLen=" << myFurtherLanes.back()->getLength()
5188 : // << " backPos=" << myState.myBackPos << " result=" << myState.myBackPos / myFurtherLanes.back()->getLength() * lane->getLength() << "\n";
5189 : //}
5190 458714 : return myState.myBackPos / myFurtherLanes.back()->getLength() * lane->getLength();
5191 : }
5192 : } else {
5193 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherLanes=" << toString(myFurtherLanes) << "\n";
5194 31585391 : double leftLength = myType->getLength() - myState.myPos;
5195 :
5196 : std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin();
5197 33633918 : while (leftLength > 0 && i != myFurtherLanes.end()) {
5198 33600073 : leftLength -= (*i)->getLength();
5199 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5200 33600073 : if (*i == lane) {
5201 29828973 : return -leftLength;
5202 3771100 : } else if (*i == lane->getBidiLane()) {
5203 1722573 : return lane->getLength() + leftLength - (calledByGetPosition ? 2 * myType->getLength() : 0);
5204 : }
5205 : ++i;
5206 : }
5207 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
5208 33845 : leftLength = myType->getLength() - myState.myPos;
5209 33845 : i = myLaneChangeModel->getShadowFurtherLanes().begin();
5210 33845 : while (leftLength > 0 && i != myLaneChangeModel->getShadowFurtherLanes().end()) {
5211 33838 : leftLength -= (*i)->getLength();
5212 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5213 33838 : if (*i == lane) {
5214 29671 : return -leftLength;
5215 4167 : } else if (*i == lane->getBidiLane()) {
5216 4167 : return lane->getLength() + leftLength - (calledByGetPosition ? 2 * myType->getLength() : 0);
5217 : }
5218 : ++i;
5219 : }
5220 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherTargetLanes=" << toString(myLaneChangeModel->getFurtherTargetLanes()) << "\n";
5221 7 : leftLength = myType->getLength() - myState.myPos;
5222 : i = getFurtherLanes().begin();
5223 7 : const std::vector<MSLane*> furtherTargetLanes = myLaneChangeModel->getFurtherTargetLanes();
5224 : auto j = furtherTargetLanes.begin();
5225 7 : while (leftLength > 0 && j != furtherTargetLanes.end()) {
5226 0 : leftLength -= (*i)->getLength();
5227 : // if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5228 0 : if (*j == lane) {
5229 0 : return -leftLength;
5230 0 : } else if (*j == lane->getBidiLane()) {
5231 0 : return lane->getLength() + leftLength - (calledByGetPosition ? 2 * myType->getLength() : 0);
5232 : }
5233 : ++i;
5234 : ++j;
5235 : }
5236 28 : WRITE_WARNINGF("Request backPos of vehicle '%' for invalid lane '%' time=%.",
5237 : getID(), Named::getIDSecure(lane), time2string(SIMSTEP))
5238 : SOFT_ASSERT(false);
5239 7 : return myState.myBackPos;
5240 7 : }
5241 : }
5242 :
5243 :
5244 : double
5245 28447240057 : MSVehicle::getPositionOnLane(const MSLane* lane) const {
5246 28447240057 : return getBackPositionOnLane(lane, true) + myType->getLength();
5247 : }
5248 :
5249 :
5250 : bool
5251 416791845 : MSVehicle::isFrontOnLane(const MSLane* lane) const {
5252 416791845 : return lane == myLane || lane == myLaneChangeModel->getShadowLane() || lane == myLane->getBidiLane();
5253 : }
5254 :
5255 :
5256 : void
5257 637469645 : MSVehicle::checkRewindLinkLanes(const double lengthsInFront, DriveItemVector& lfLinks) const {
5258 637469645 : if (MSGlobals::gUsingInternalLanes && !myLane->getEdge().isRoundabout() && !myLaneChangeModel->isOpposite()) {
5259 634235041 : double seenSpace = -lengthsInFront;
5260 : #ifdef DEBUG_CHECKREWINDLINKLANES
5261 : if (DEBUG_COND) {
5262 : std::cout << "\nCHECK_REWIND_LINKLANES\n" << " veh=" << getID() << " lengthsInFront=" << lengthsInFront << "\n";
5263 : };
5264 : #endif
5265 634235041 : bool foundStopped = false;
5266 : // compute available space until a stopped vehicle is found
5267 : // this is the sum of non-interal lane length minus in-between vehicle lengths
5268 1859942721 : for (int i = 0; i < (int)lfLinks.size(); ++i) {
5269 : // skip unset links
5270 1225707680 : DriveProcessItem& item = lfLinks[i];
5271 : #ifdef DEBUG_CHECKREWINDLINKLANES
5272 : if (DEBUG_COND) std::cout << SIMTIME
5273 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5274 : << " foundStopped=" << foundStopped;
5275 : #endif
5276 1225707680 : if (item.myLink == nullptr || foundStopped) {
5277 401106123 : if (!foundStopped) {
5278 344037497 : item.availableSpace += seenSpace;
5279 : } else {
5280 57068626 : item.availableSpace = seenSpace;
5281 : }
5282 : #ifdef DEBUG_CHECKREWINDLINKLANES
5283 : if (DEBUG_COND) {
5284 : std::cout << " avail=" << item.availableSpace << "\n";
5285 : }
5286 : #endif
5287 401106123 : continue;
5288 : }
5289 : // get the next lane, determine whether it is an internal lane
5290 : const MSLane* approachedLane = item.myLink->getViaLane();
5291 824601557 : if (approachedLane != nullptr) {
5292 450089050 : if (keepClear(item.myLink)) {
5293 142191963 : seenSpace = seenSpace - approachedLane->getBruttoVehLenSum();
5294 142191963 : if (approachedLane == myLane) {
5295 48432 : seenSpace += getVehicleType().getLengthWithGap();
5296 : }
5297 : } else {
5298 307897087 : seenSpace = seenSpace + approachedLane->getSpaceTillLastStanding(this, foundStopped);// - approachedLane->getBruttoVehLenSum() + approachedLane->getLength();
5299 : }
5300 450089050 : item.availableSpace = seenSpace;
5301 : #ifdef DEBUG_CHECKREWINDLINKLANES
5302 : if (DEBUG_COND) std::cout
5303 : << " approached=" << approachedLane->getID()
5304 : << " approachedBrutto=" << approachedLane->getBruttoVehLenSum()
5305 : << " avail=" << item.availableSpace
5306 : << " seenSpace=" << seenSpace
5307 : << " hadStoppedVehicle=" << item.hadStoppedVehicle
5308 : << " lengthsInFront=" << lengthsInFront
5309 : << "\n";
5310 : #endif
5311 450089050 : continue;
5312 : }
5313 : approachedLane = item.myLink->getLane();
5314 374512507 : const MSVehicle* last = approachedLane->getLastAnyVehicle();
5315 374512507 : if (last == nullptr || last == this) {
5316 61502347 : if (approachedLane->getLength() > getVehicleType().getLength()
5317 61502347 : || keepClear(item.myLink)) {
5318 59113536 : seenSpace += approachedLane->getLength();
5319 : }
5320 61502347 : item.availableSpace = seenSpace;
5321 : #ifdef DEBUG_CHECKREWINDLINKLANES
5322 : if (DEBUG_COND) {
5323 : std::cout << " last=" << Named::getIDSecure(last) << " laneLength=" << approachedLane->getLength() << " avail=" << item.availableSpace << "\n";
5324 : }
5325 : #endif
5326 : } else {
5327 313010160 : bool foundStopped2 = false;
5328 313010160 : double spaceTillLastStanding = approachedLane->getSpaceTillLastStanding(this, foundStopped2);
5329 313010160 : if (approachedLane->getBidiLane() != nullptr) {
5330 276363 : const MSVehicle* oncomingVeh = approachedLane->getBidiLane()->getFirstFullVehicle();
5331 276363 : if (oncomingVeh) {
5332 113288 : const double oncomingGap = approachedLane->getLength() - oncomingVeh->getPositionOnLane();
5333 113288 : const double oncomingBGap = oncomingVeh->getBrakeGap(true);
5334 : // oncoming movement until ego enters the junction
5335 113288 : const double oncomingMove = STEPS2TIME(item.myArrivalTime - SIMSTEP) * oncomingVeh->getSpeed();
5336 113288 : const double spaceTillOncoming = oncomingGap - oncomingBGap - oncomingMove;
5337 : spaceTillLastStanding = MIN2(spaceTillLastStanding, spaceTillOncoming);
5338 113288 : if (spaceTillOncoming <= getVehicleType().getLengthWithGap()) {
5339 72971 : foundStopped = true;
5340 : }
5341 : #ifdef DEBUG_CHECKREWINDLINKLANES
5342 : if (DEBUG_COND) {
5343 : std::cout << " oVeh=" << oncomingVeh->getID()
5344 : << " oGap=" << oncomingGap
5345 : << " bGap=" << oncomingBGap
5346 : << " mGap=" << oncomingMove
5347 : << " sto=" << spaceTillOncoming;
5348 : }
5349 : #endif
5350 : }
5351 : }
5352 313010160 : seenSpace += spaceTillLastStanding;
5353 313010160 : if (foundStopped2) {
5354 24162026 : foundStopped = true;
5355 24162026 : item.hadStoppedVehicle = true;
5356 : }
5357 313010160 : item.availableSpace = seenSpace;
5358 313010160 : if (last->myHaveToWaitOnNextLink || last->isStopped()) {
5359 32375314 : foundStopped = true;
5360 32375314 : item.hadStoppedVehicle = true;
5361 : }
5362 : #ifdef DEBUG_CHECKREWINDLINKLANES
5363 : if (DEBUG_COND) std::cout
5364 : << " approached=" << approachedLane->getID()
5365 : << " last=" << last->getID()
5366 : << " lastHasToWait=" << last->myHaveToWaitOnNextLink
5367 : << " lastBrakeLight=" << last->signalSet(VEH_SIGNAL_BRAKELIGHT)
5368 : << " lastBrakeGap=" << last->getCarFollowModel().brakeGap(last->getSpeed())
5369 : << " lastGap=" << (last->getBackPositionOnLane(approachedLane) + last->getCarFollowModel().brakeGap(last->getSpeed()) - last->getSpeed() * last->getCarFollowModel().getHeadwayTime()
5370 : // gap of last up to the next intersection
5371 : - last->getVehicleType().getMinGap())
5372 : << " stls=" << spaceTillLastStanding
5373 : << " avail=" << item.availableSpace
5374 : << " seenSpace=" << seenSpace
5375 : << " foundStopped=" << foundStopped
5376 : << " foundStopped2=" << foundStopped2
5377 : << "\n";
5378 : #endif
5379 : }
5380 : }
5381 :
5382 : // check which links allow continuation and add pass available to the previous item
5383 1225707680 : for (int i = ((int)lfLinks.size() - 1); i > 0; --i) {
5384 591472639 : DriveProcessItem& item = lfLinks[i - 1];
5385 591472639 : DriveProcessItem& nextItem = lfLinks[i];
5386 591472639 : const bool canLeaveJunction = item.myLink->getViaLane() == nullptr || nextItem.myLink == nullptr || nextItem.mySetRequest;
5387 : const bool opened = (item.myLink != nullptr
5388 591472639 : && (canLeaveJunction || (
5389 : // indirect bicycle turn
5390 31830989 : nextItem.myLink != nullptr && nextItem.myLink->isInternalJunctionLink() && nextItem.myLink->haveRed()))
5391 559656311 : && (
5392 559656311 : item.myLink->havePriority()
5393 27839362 : || i == 1 // the upcoming link (item 0) is checked in executeMove anyway. No need to use outdata approachData here
5394 5452975 : || (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority())
5395 5423636 : || item.myLink->opened(item.myArrivalTime, item.myArrivalSpeed,
5396 5423636 : item.getLeaveSpeed(), getVehicleType().getLength(),
5397 5423636 : getImpatience(), getCarFollowModel().getMaxDecel(), getWaitingTime(), getLateralPositionOnLane(), nullptr, false, this)));
5398 591472639 : bool allowsContinuation = (item.myLink == nullptr || item.myLink->isCont() || opened) && !item.hadStoppedVehicle;
5399 : #ifdef DEBUG_CHECKREWINDLINKLANES
5400 : if (DEBUG_COND) std::cout
5401 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5402 : << " canLeave=" << canLeaveJunction
5403 : << " opened=" << opened
5404 : << " allowsContinuation=" << allowsContinuation
5405 : << " foundStopped=" << foundStopped
5406 : << "\n";
5407 : #endif
5408 591472639 : if (!opened && item.myLink != nullptr) {
5409 32560943 : foundStopped = true;
5410 32560943 : if (i > 1) {
5411 4916586 : DriveProcessItem& item2 = lfLinks[i - 2];
5412 4916586 : if (item2.myLink != nullptr && item2.myLink->isCont()) {
5413 : allowsContinuation = true;
5414 : }
5415 : }
5416 : }
5417 588411561 : if (allowsContinuation) {
5418 525029953 : item.availableSpace = nextItem.availableSpace;
5419 : #ifdef DEBUG_CHECKREWINDLINKLANES
5420 : if (DEBUG_COND) std::cout
5421 : << " link=" << (item.myLink == nullptr ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5422 : << " copy nextAvail=" << nextItem.availableSpace
5423 : << "\n";
5424 : #endif
5425 : }
5426 : }
5427 :
5428 : // find removalBegin
5429 : int removalBegin = -1;
5430 776754521 : for (int i = 0; foundStopped && i < (int)lfLinks.size() && removalBegin < 0; ++i) {
5431 : // skip unset links
5432 142519480 : const DriveProcessItem& item = lfLinks[i];
5433 142519480 : if (item.myLink == nullptr) {
5434 7024866 : continue;
5435 : }
5436 : /*
5437 : double impatienceCorrection = MAX2(0., double(double(myWaitingTime)));
5438 : if (seenSpace<getVehicleType().getLengthWithGap()-impatienceCorrection/10.&&nextSeenNonInternal!=0) {
5439 : removalBegin = lastLinkToInternal;
5440 : }
5441 : */
5442 :
5443 135494614 : const double leftSpace = item.availableSpace - getVehicleType().getLengthWithGap();
5444 : #ifdef DEBUG_CHECKREWINDLINKLANES
5445 : if (DEBUG_COND) std::cout
5446 : << SIMTIME
5447 : << " veh=" << getID()
5448 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5449 : << " avail=" << item.availableSpace
5450 : << " leftSpace=" << leftSpace
5451 : << "\n";
5452 : #endif
5453 135494614 : if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
5454 : double impatienceCorrection = 0;
5455 : /*
5456 : if(item.myLink->getState()==LINKSTATE_MINOR) {
5457 : impatienceCorrection = MAX2(0., STEPS2TIME(myWaitingTime));
5458 : }
5459 : */
5460 : // may ignore keepClear rules
5461 87928229 : if (leftSpace < -impatienceCorrection / 10. && keepClear(item.myLink)) {
5462 : removalBegin = i;
5463 : }
5464 : //removalBegin = i;
5465 : }
5466 : }
5467 : // abort requests
5468 634235041 : if (removalBegin != -1 && !(removalBegin == 0 && myLane->getEdge().isInternal())) {
5469 30751417 : const double brakeGap = getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.);
5470 105630015 : while (removalBegin < (int)(lfLinks.size())) {
5471 79882003 : DriveProcessItem& dpi = lfLinks[removalBegin];
5472 79882003 : if (dpi.myLink == nullptr) {
5473 : break;
5474 : }
5475 74878598 : dpi.myVLinkPass = dpi.myVLinkWait;
5476 : #ifdef DEBUG_CHECKREWINDLINKLANES
5477 : if (DEBUG_COND) {
5478 : std::cout << " removalBegin=" << removalBegin << " brakeGap=" << brakeGap << " dist=" << dpi.myDistance << " speed=" << myState.mySpeed << " a2s=" << ACCEL2SPEED(getCarFollowModel().getMaxDecel()) << "\n";
5479 : }
5480 : #endif
5481 74878598 : if (dpi.myDistance >= brakeGap + POSITION_EPS) {
5482 : // always leave junctions after requesting to enter
5483 74870277 : if (!dpi.myLink->isExitLink() || !lfLinks[removalBegin - 1].mySetRequest) {
5484 74863226 : dpi.mySetRequest = false;
5485 : }
5486 : }
5487 74878598 : ++removalBegin;
5488 : }
5489 : }
5490 : }
5491 637469645 : }
5492 :
5493 :
5494 : void
5495 709197496 : MSVehicle::setApproachingForAllLinks() {
5496 709197496 : if (!myActionStep) {
5497 : return;
5498 : }
5499 637469645 : removeApproachingInformation(myLFLinkLanesPrev);
5500 1871164058 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5501 1233694413 : if (dpi.myLink != nullptr) {
5502 881092707 : if (dpi.myLink->getState() == LINKSTATE_ALLWAY_STOP) {
5503 2852848 : dpi.myArrivalTime += (SUMOTime)RandHelper::rand((int)2, getRNG()); // tie braker
5504 : }
5505 881092707 : dpi.myLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
5506 881092707 : dpi.mySetRequest, dpi.myArrivalSpeedBraking, getWaitingTimeFor(dpi.myLink), dpi.myDistance, getLateralPositionOnLane());
5507 : }
5508 : }
5509 637469645 : if (isRail()) {
5510 8160915 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5511 6797945 : if (dpi.myLink != nullptr && dpi.myLink->getTLLogic() != nullptr && dpi.myLink->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL) {
5512 692769 : MSRailSignalControl::getInstance().notifyApproach(dpi.myLink);
5513 : }
5514 : }
5515 : }
5516 637469645 : if (myLaneChangeModel->getShadowLane() != nullptr) {
5517 : // register on all shadow links
5518 7643830 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
5519 5084113 : if (dpi.myLink != nullptr) {
5520 3503772 : MSLink* parallelLink = dpi.myLink->getParallelLink(myLaneChangeModel->getShadowDirection());
5521 3503772 : if (parallelLink == nullptr && getLaneChangeModel().isOpposite() && dpi.myLink->isEntryLink()) {
5522 : // register on opposite direction entry link to warn foes at minor side road
5523 167103 : parallelLink = dpi.myLink->getOppositeDirectionLink();
5524 : }
5525 3503772 : if (parallelLink != nullptr) {
5526 2470986 : const double latOffset = getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge();
5527 2470986 : parallelLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
5528 2470986 : dpi.mySetRequest, dpi.myArrivalSpeedBraking, getWaitingTimeFor(dpi.myLink), dpi.myDistance,
5529 : latOffset);
5530 2470986 : myLaneChangeModel->setShadowApproachingInformation(parallelLink);
5531 : }
5532 : }
5533 : }
5534 : }
5535 : #ifdef DEBUG_PLAN_MOVE
5536 : if (DEBUG_COND) {
5537 : std::cout << SIMTIME
5538 : << " veh=" << getID()
5539 : << " after checkRewindLinkLanes\n";
5540 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5541 : std::cout
5542 : << " vPass=" << dpi.myVLinkPass
5543 : << " vWait=" << dpi.myVLinkWait
5544 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
5545 : << " request=" << dpi.mySetRequest
5546 : << " atime=" << dpi.myArrivalTime
5547 : << "\n";
5548 : }
5549 : }
5550 : #endif
5551 : }
5552 :
5553 :
5554 : void
5555 1860 : MSVehicle::registerInsertionApproach(MSLink* link, double dist) {
5556 : DriveProcessItem dpi(0, dist);
5557 1860 : dpi.myLink = link;
5558 1860 : const double arrivalSpeedBraking = getCarFollowModel().getMinimalArrivalSpeedEuler(dist, getSpeed());
5559 1860 : link->setApproaching(this, SUMOTime_MAX, 0, 0, false, arrivalSpeedBraking, 0, dpi.myDistance, 0);
5560 : // ensure cleanup in the next step
5561 1860 : myLFLinkLanes.push_back(dpi);
5562 1860 : MSRailSignalControl::getInstance().notifyApproach(link);
5563 1860 : }
5564 :
5565 :
5566 : void
5567 19711077 : MSVehicle::enterLaneAtMove(MSLane* enteredLane, bool onTeleporting) {
5568 19711077 : myAmOnNet = !onTeleporting;
5569 : // vaporizing edge?
5570 : /*
5571 : if (enteredLane->getEdge().isVaporizing()) {
5572 : // yep, let's do the vaporization...
5573 : myLane = enteredLane;
5574 : return true;
5575 : }
5576 : */
5577 : // Adjust MoveReminder offset to the next lane
5578 19711077 : adaptLaneEntering2MoveReminder(*enteredLane);
5579 : // set the entered lane as the current lane
5580 19711077 : MSLane* oldLane = myLane;
5581 19711077 : myLane = enteredLane;
5582 19711077 : myLastBestLanesEdge = nullptr;
5583 :
5584 : // internal edges are not a part of the route...
5585 19711077 : if (!enteredLane->getEdge().isInternal()) {
5586 : ++myCurrEdge;
5587 : assert(myLaneChangeModel->isOpposite() || haveValidStopEdges());
5588 : }
5589 19711077 : if (myInfluencer != nullptr) {
5590 9053 : myInfluencer->adaptLaneTimeLine(myLane->getIndex() - oldLane->getIndex());
5591 : }
5592 19711077 : if (!onTeleporting) {
5593 19693029 : activateReminders(MSMoveReminder::NOTIFICATION_JUNCTION, enteredLane);
5594 19693029 : if (MSGlobals::gLateralResolution > 0) {
5595 3822553 : myFurtherLanesPosLat.push_back(myState.myPosLat);
5596 : // transform lateral position when the lane width changes
5597 : assert(oldLane != nullptr);
5598 3822553 : const MSLink* const link = oldLane->getLinkTo(myLane);
5599 3822553 : if (link != nullptr) {
5600 3822511 : myState.myPosLat += link->getLateralShift();
5601 : } else {
5602 42 : myState.myPosLat += (oldLane->getCenterOnEdge() - myLane->getCanonicalPredecessorLane()->getRightSideOnEdge()) / 2;
5603 : }
5604 15870476 : } else if (fabs(myState.myPosLat) > NUMERICAL_EPS) {
5605 243612 : const double overlap = MAX2(0.0, getLateralOverlap(myState.myPosLat, oldLane));
5606 243612 : const double range = (oldLane->getWidth() - getVehicleType().getWidth()) * 0.5 + overlap;
5607 243612 : const double range2 = (myLane->getWidth() - getVehicleType().getWidth()) * 0.5 + overlap;
5608 243612 : myState.myPosLat *= range2 / range;
5609 : }
5610 19693029 : if (myLane->getBidiLane() != nullptr && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5611 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
5612 : // (unless the lane is shared with cars)
5613 27081 : myLane->getBidiLane()->setPartialOccupation(this);
5614 : }
5615 : } else {
5616 : // normal move() isn't called so reset position here. must be done
5617 : // before calling reminders
5618 18048 : myState.myPos = 0;
5619 18048 : myCachedPosition = Position::INVALID;
5620 18048 : activateReminders(MSMoveReminder::NOTIFICATION_TELEPORT, enteredLane);
5621 : }
5622 : // update via
5623 19711077 : if (myParameter->via.size() > 0 && myLane->getEdge().getID() == myParameter->via.front()) {
5624 7271 : myParameter->via.erase(myParameter->via.begin());
5625 : }
5626 19711077 : }
5627 :
5628 :
5629 : void
5630 1093115 : MSVehicle::enterLaneAtLaneChange(MSLane* enteredLane) {
5631 1093115 : myAmOnNet = true;
5632 1093115 : myLane = enteredLane;
5633 1093115 : myCachedPosition = Position::INVALID;
5634 : // need to update myCurrentLaneInBestLanes
5635 1093115 : updateBestLanes();
5636 : // switch to and activate the new lane's reminders
5637 : // keep OldLaneReminders
5638 1293861 : for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
5639 200746 : addReminder(*rem);
5640 : }
5641 1093115 : activateReminders(MSMoveReminder::NOTIFICATION_LANE_CHANGE, enteredLane);
5642 1093115 : MSLane* lane = myLane;
5643 1093115 : double leftLength = getVehicleType().getLength() - myState.myPos;
5644 : int deleteFurther = 0;
5645 : #ifdef DEBUG_SETFURTHER
5646 : if (DEBUG_COND) {
5647 : std::cout << SIMTIME << " enterLaneAtLaneChange entered=" << Named::getIDSecure(enteredLane) << " oldFurther=" << toString(myFurtherLanes) << "\n";
5648 : }
5649 : #endif
5650 1093115 : if (myLane->getBidiLane() != nullptr && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5651 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
5652 : // (unless the lane is shared with cars)
5653 20679 : myLane->getBidiLane()->setPartialOccupation(this);
5654 : }
5655 1180690 : for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
5656 87575 : if (lane != nullptr) {
5657 84359 : lane = lane->getLogicalPredecessorLane(myFurtherLanes[i]->getEdge());
5658 : }
5659 : #ifdef DEBUG_SETFURTHER
5660 : if (DEBUG_COND) {
5661 : std::cout << " enterLaneAtLaneChange i=" << i << " lane=" << Named::getIDSecure(lane) << " leftLength=" << leftLength << "\n";
5662 : }
5663 : #endif
5664 87575 : if (leftLength > 0) {
5665 87002 : if (lane != nullptr) {
5666 35635 : myFurtherLanes[i]->resetPartialOccupation(this);
5667 35635 : if (myFurtherLanes[i]->getBidiLane() != nullptr
5668 35635 : && (!isRailway(getVClass()) || (myFurtherLanes[i]->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5669 89 : myFurtherLanes[i]->getBidiLane()->resetPartialOccupation(this);
5670 : }
5671 : // lane changing onto longer lanes may reduce the number of
5672 : // remaining further lanes
5673 35635 : myFurtherLanes[i] = lane;
5674 35635 : myFurtherLanesPosLat[i] = myState.myPosLat;
5675 35635 : leftLength -= lane->setPartialOccupation(this);
5676 35635 : if (lane->getBidiLane() != nullptr
5677 35635 : && (!isRailway(getVClass()) || (lane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5678 1856 : lane->getBidiLane()->setPartialOccupation(this);
5679 : }
5680 35635 : myState.myBackPos = -leftLength;
5681 : #ifdef DEBUG_SETFURTHER
5682 : if (DEBUG_COND) {
5683 : std::cout << SIMTIME << " newBackPos=" << myState.myBackPos << "\n";
5684 : }
5685 : #endif
5686 : } else {
5687 : // keep the old values, but ensure there is no shadow
5688 51367 : if (myLaneChangeModel->isChangingLanes()) {
5689 15 : myLaneChangeModel->setNoShadowPartialOccupator(myFurtherLanes[i]);
5690 : }
5691 51367 : if (myState.myBackPos < 0) {
5692 322 : myState.myBackPos += myFurtherLanes[i]->getLength();
5693 : }
5694 : #ifdef DEBUG_SETFURTHER
5695 : if (DEBUG_COND) {
5696 : std::cout << SIMTIME << " i=" << i << " further=" << myFurtherLanes[i]->getID() << " newBackPos=" << myState.myBackPos << "\n";
5697 : }
5698 : #endif
5699 : }
5700 : } else {
5701 573 : myFurtherLanes[i]->resetPartialOccupation(this);
5702 573 : if (myFurtherLanes[i]->getBidiLane() != nullptr
5703 573 : && (!isRailway(getVClass()) || (myFurtherLanes[i]->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5704 0 : myFurtherLanes[i]->getBidiLane()->resetPartialOccupation(this);
5705 : }
5706 573 : deleteFurther++;
5707 : }
5708 : }
5709 1093115 : if (deleteFurther > 0) {
5710 : #ifdef DEBUG_SETFURTHER
5711 : if (DEBUG_COND) {
5712 : std::cout << SIMTIME << " veh=" << getID() << " shortening myFurtherLanes by " << deleteFurther << "\n";
5713 : }
5714 : #endif
5715 555 : myFurtherLanes.erase(myFurtherLanes.end() - deleteFurther, myFurtherLanes.end());
5716 555 : myFurtherLanesPosLat.erase(myFurtherLanesPosLat.end() - deleteFurther, myFurtherLanesPosLat.end());
5717 : }
5718 : #ifdef DEBUG_SETFURTHER
5719 : if (DEBUG_COND) {
5720 : std::cout << SIMTIME << " enterLaneAtLaneChange new furtherLanes=" << toString(myFurtherLanes)
5721 : << " furterLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
5722 : }
5723 : #endif
5724 1093115 : myAngle = computeAngle();
5725 1093115 : }
5726 :
5727 :
5728 : void
5729 3570437 : MSVehicle::computeFurtherLanes(MSLane* enteredLane, double pos, bool collision) {
5730 : // build the list of lanes the vehicle is lapping into
5731 3570437 : if (!myLaneChangeModel->isOpposite()) {
5732 3548267 : double leftLength = myType->getLength() - pos;
5733 3548267 : MSLane* clane = enteredLane;
5734 3548267 : int routeIndex = getRoutePosition();
5735 3654871 : while (leftLength > 0) {
5736 238819 : if (routeIndex > 0 && clane->getEdge().isNormal()) {
5737 : // get predecessor lane that corresponds to prior route
5738 4572 : routeIndex--;
5739 4572 : const MSEdge* fromRouteEdge = myRoute->getEdges()[routeIndex];
5740 : MSLane* target = clane;
5741 4572 : clane = nullptr;
5742 6112 : for (auto ili : target->getIncomingLanes()) {
5743 6105 : if (ili.lane->getEdge().getNormalBefore() == fromRouteEdge) {
5744 4565 : clane = ili.lane;
5745 4565 : break;
5746 : }
5747 : }
5748 : } else {
5749 234247 : clane = clane->getLogicalPredecessorLane();
5750 : }
5751 146945 : if (clane == nullptr || clane == myLane || clane == myLane->getBidiLane()
5752 385756 : || (clane->isInternal() && (
5753 123340 : clane->getLinkCont()[0]->getDirection() == LinkDirection::TURN
5754 83007 : || clane->getLinkCont()[0]->getDirection() == LinkDirection::TURN_LEFTHAND))) {
5755 : break;
5756 : }
5757 106604 : if (!collision || std::find(myFurtherLanes.begin(), myFurtherLanes.end(), clane) == myFurtherLanes.end()) {
5758 106154 : myFurtherLanes.push_back(clane);
5759 106154 : myFurtherLanesPosLat.push_back(myState.myPosLat);
5760 106154 : clane->setPartialOccupation(this);
5761 106154 : if (clane->getBidiLane() != nullptr
5762 106154 : && (!isRailway(getVClass()) || (clane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5763 5 : clane->getBidiLane()->setPartialOccupation(this);
5764 : }
5765 : }
5766 106604 : leftLength -= clane->getLength();
5767 : }
5768 3548267 : myState.myBackPos = -leftLength;
5769 : #ifdef DEBUG_SETFURTHER
5770 : if (DEBUG_COND) {
5771 : std::cout << SIMTIME << " computeFurtherLanes veh=" << getID() << " pos=" << pos << " myFurtherLanes=" << toString(myFurtherLanes) << " backPos=" << myState.myBackPos << "\n";
5772 : }
5773 : #endif
5774 : } else {
5775 : // clear partial occupation
5776 22554 : for (MSLane* further : myFurtherLanes) {
5777 : #ifdef DEBUG_SETFURTHER
5778 : if (DEBUG_COND) {
5779 : std::cout << SIMTIME << " opposite: resetPartialOccupation " << further->getID() << " \n";
5780 : }
5781 : #endif
5782 384 : further->resetPartialOccupation(this);
5783 384 : if (further->getBidiLane() != nullptr
5784 384 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5785 0 : further->getBidiLane()->resetPartialOccupation(this);
5786 : }
5787 : }
5788 : myFurtherLanes.clear();
5789 : myFurtherLanesPosLat.clear();
5790 : }
5791 3570437 : }
5792 :
5793 :
5794 : void
5795 3569988 : MSVehicle::enterLaneAtInsertion(MSLane* enteredLane, double pos, double speed, double posLat, MSMoveReminder::Notification notification) {
5796 3569988 : myState = State(pos, speed, posLat, pos - getVehicleType().getLength(), hasDeparted() ? myState.myPreviousSpeed : speed);
5797 3569988 : if (myDeparture == NOT_YET_DEPARTED) {
5798 3496124 : onDepart();
5799 : }
5800 3569988 : if (enteredLane->isInternal() && myJunctionEntryTime == SUMOTime_MAX) {
5801 63 : myJunctionEntryTime = MSNet::getInstance()->getCurrentTimeStep();
5802 63 : myJunctionEntryTimeNeverYield = myJunctionEntryTime;
5803 : assert(enteredLane->getIncomingLanes().size() == 1);
5804 63 : if (enteredLane->getIncomingLanes().front().viaLink->isConflictEntryLink()) {
5805 49 : myJunctionConflictEntryTime = myJunctionEntryTime;
5806 : }
5807 : }
5808 3569988 : myCachedPosition = Position::INVALID;
5809 : assert(myState.myPos >= 0);
5810 : assert(myState.mySpeed >= 0);
5811 3569988 : myLane = enteredLane;
5812 3569988 : myAmOnNet = true;
5813 : // schedule action for the next timestep
5814 3569988 : myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + DELTA_T;
5815 3569988 : if (notification != MSMoveReminder::NOTIFICATION_TELEPORT) {
5816 3558458 : if (notification == MSMoveReminder::NOTIFICATION_PARKING && myInfluencer != nullptr) {
5817 12 : drawOutsideNetwork(false);
5818 : }
5819 : // set and activate the new lane's reminders, teleports already did that at enterLaneAtMove
5820 7552764 : for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
5821 3994306 : addReminder(*rem);
5822 : }
5823 3558458 : activateReminders(notification, enteredLane);
5824 : } else {
5825 11530 : myLastBestLanesEdge = nullptr;
5826 11530 : myLastBestLanesInternalLane = nullptr;
5827 11530 : myLaneChangeModel->resetState();
5828 12667 : while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()
5829 12219 : && myStops.front().pars.endPos < pos) {
5830 0 : WRITE_WARNINGF(TL("Vehicle '%' skips stop on lane '%' time=%."), getID(), myStops.front().lane->getID(),
5831 : time2string(MSNet::getInstance()->getCurrentTimeStep()));
5832 0 : cleanupParkingReservation();
5833 0 : myStops.pop_front();
5834 : }
5835 : // avoid startup-effects after teleport
5836 11530 : myTimeSinceStartup = getCarFollowModel().getStartupDelay() + DELTA_T;
5837 11530 : myStopSpeed = std::numeric_limits<double>::max();
5838 : }
5839 3569988 : computeFurtherLanes(enteredLane, pos);
5840 3569988 : if (MSGlobals::gLateralResolution > 0) {
5841 530555 : myLaneChangeModel->updateShadowLane();
5842 530555 : myLaneChangeModel->updateTargetLane();
5843 3039433 : } else if (MSGlobals::gLaneChangeDuration > 0) {
5844 40414 : myLaneChangeModel->updateShadowLane();
5845 : }
5846 3569988 : if (notification != MSMoveReminder::NOTIFICATION_LOAD_STATE) {
5847 3568531 : myAngle = computeAngle();
5848 3568531 : myRawAngle = myAngle;
5849 3568531 : if (myLaneChangeModel->isOpposite()) {
5850 22170 : myAngle += M_PI;
5851 : }
5852 : }
5853 3569988 : if (MSNet::getInstance()->hasPersons()) {
5854 58717 : for (MSLane* further : myFurtherLanes) {
5855 885 : if (further->mustCheckJunctionCollisions()) {
5856 4 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(further);
5857 : }
5858 : }
5859 : }
5860 3569988 : }
5861 :
5862 :
5863 : void
5864 24250120 : MSVehicle::leaveLane(const MSMoveReminder::Notification reason, const MSLane* approachedLane) {
5865 67094917 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
5866 42844797 : if (rem->first->notifyLeave(*this, myState.myPos + rem->second, reason, approachedLane)) {
5867 : #ifdef _DEBUG
5868 : if (myTraceMoveReminders) {
5869 : traceMoveReminder("notifyLeave", rem->first, rem->second, true);
5870 : }
5871 : #endif
5872 : ++rem;
5873 : } else {
5874 : #ifdef _DEBUG
5875 : if (myTraceMoveReminders) {
5876 : traceMoveReminder("notifyLeave", rem->first, rem->second, false);
5877 : }
5878 : #endif
5879 : rem = myMoveReminders.erase(rem);
5880 : }
5881 : }
5882 24250120 : if ((reason == MSMoveReminder::NOTIFICATION_JUNCTION
5883 24250120 : || reason == MSMoveReminder::NOTIFICATION_TELEPORT
5884 4544921 : || reason == MSMoveReminder::NOTIFICATION_TELEPORT_CONTINUATION)
5885 19711332 : && myLane != nullptr) {
5886 19711303 : myOdometer += getLane()->getLength();
5887 : }
5888 24250091 : if (myLane != nullptr && myLane->getBidiLane() != nullptr && myAmOnNet
5889 24323864 : && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5890 49227 : myLane->getBidiLane()->resetPartialOccupation(this);
5891 : }
5892 24250120 : if (reason != MSMoveReminder::NOTIFICATION_JUNCTION && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
5893 : // @note. In case of lane change, myFurtherLanes and partial occupation
5894 : // are handled in enterLaneAtLaneChange()
5895 3452943 : for (MSLane* further : myFurtherLanes) {
5896 : #ifdef DEBUG_FURTHER
5897 : if (DEBUG_COND) {
5898 : std::cout << SIMTIME << " leaveLane \n";
5899 : }
5900 : #endif
5901 33090 : further->resetPartialOccupation(this);
5902 33090 : if (further->getBidiLane() != nullptr
5903 33090 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5904 40 : further->getBidiLane()->resetPartialOccupation(this);
5905 : }
5906 : }
5907 : myFurtherLanes.clear();
5908 : myFurtherLanesPosLat.clear();
5909 : }
5910 3419853 : if (reason >= MSMoveReminder::NOTIFICATION_TELEPORT) {
5911 3419853 : myAmOnNet = false;
5912 3419853 : myWaitingTime = 0;
5913 : }
5914 24250120 : if (reason != MSMoveReminder::NOTIFICATION_PARKING && resumeFromStopping()) {
5915 18 : myStopDist = std::numeric_limits<double>::max();
5916 18 : if (myPastStops.back().speed <= 0) {
5917 54 : WRITE_WARNINGF(TL("Vehicle '%' aborts stop."), getID());
5918 : }
5919 : }
5920 24250120 : if (reason != MSMoveReminder::NOTIFICATION_PARKING && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
5921 23097786 : while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()) {
5922 1407 : if (myStops.front().getSpeed() <= 0) {
5923 3219 : WRITE_WARNINGF(TL("Vehicle '%' skips stop on lane '%' time=%."), getID(), myStops.front().lane->getID(),
5924 : time2string(MSNet::getInstance()->getCurrentTimeStep()));
5925 1073 : cleanupParkingReservation();
5926 1073 : if (MSStopOut::active()) {
5927 : // clean up if stopBlocked was called
5928 17 : MSStopOut::getInstance()->stopNotStarted(this);
5929 : }
5930 1073 : myStops.pop_front();
5931 : } else {
5932 : MSStop& stop = myStops.front();
5933 : // passed waypoint at the end of the lane
5934 334 : if (!stop.reached) {
5935 334 : if (MSStopOut::active()) {
5936 21 : MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), MSNet::getInstance()->getCurrentTimeStep());
5937 : }
5938 334 : stop.reached = true;
5939 : // enter stopping place so leaveFrom works as expected
5940 334 : if (stop.busstop != nullptr) {
5941 : // let the bus stop know the vehicle
5942 25 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5943 : }
5944 334 : if (stop.containerstop != nullptr) {
5945 : // let the container stop know the vehicle
5946 13 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5947 : }
5948 : // do not enter parkingarea!
5949 334 : if (stop.chargingStation != nullptr) {
5950 : // let the container stop know the vehicle
5951 122 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5952 : }
5953 : }
5954 334 : resumeFromStopping();
5955 : }
5956 1407 : myStopDist = std::numeric_limits<double>::max();
5957 : }
5958 : }
5959 24250120 : }
5960 :
5961 :
5962 : void
5963 45731 : MSVehicle::leaveLaneBack(const MSMoveReminder::Notification reason, const MSLane* leftLane) {
5964 186343 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
5965 140612 : if (rem->first->notifyLeaveBack(*this, reason, leftLane)) {
5966 : #ifdef _DEBUG
5967 : if (myTraceMoveReminders) {
5968 : traceMoveReminder("notifyLeaveBack", rem->first, rem->second, true);
5969 : }
5970 : #endif
5971 : ++rem;
5972 : } else {
5973 : #ifdef _DEBUG
5974 : if (myTraceMoveReminders) {
5975 : traceMoveReminder("notifyLeaveBack", rem->first, rem->second, false);
5976 : }
5977 : #endif
5978 : rem = myMoveReminders.erase(rem);
5979 : }
5980 : }
5981 : #ifdef DEBUG_MOVEREMINDERS
5982 : if (DEBUG_COND) {
5983 : std::cout << SIMTIME << " veh=" << getID() << " myReminders:";
5984 : for (auto rem : myMoveReminders) {
5985 : std::cout << rem.first->getDescription() << " ";
5986 : }
5987 : std::cout << "\n";
5988 : }
5989 : #endif
5990 45731 : }
5991 :
5992 :
5993 : MSAbstractLaneChangeModel&
5994 10487657754 : MSVehicle::getLaneChangeModel() {
5995 10487657754 : return *myLaneChangeModel;
5996 : }
5997 :
5998 :
5999 : const MSAbstractLaneChangeModel&
6000 4979794387 : MSVehicle::getLaneChangeModel() const {
6001 4979794387 : return *myLaneChangeModel;
6002 : }
6003 :
6004 : bool
6005 512858 : MSVehicle::isOppositeLane(const MSLane* lane) const {
6006 512858 : return (lane->isInternal()
6007 512858 : ? & (lane->getLinkCont()[0]->getLane()->getEdge()) != *(myCurrEdge + 1)
6008 510894 : : &lane->getEdge() != *myCurrEdge);
6009 : }
6010 :
6011 : const std::vector<MSVehicle::LaneQ>&
6012 1345955869 : MSVehicle::getBestLanes() const {
6013 1345955869 : return *myBestLanes.begin();
6014 : }
6015 :
6016 :
6017 : void
6018 1880155978 : MSVehicle::updateBestLanes(bool forceRebuild, const MSLane* startLane) {
6019 : #ifdef DEBUG_BESTLANES
6020 : if (DEBUG_COND) {
6021 : std::cout << SIMTIME << " updateBestLanes veh=" << getID() << " force=" << forceRebuild << " startLane1=" << Named::getIDSecure(startLane) << " myLane=" << Named::getIDSecure(myLane) << "\n";
6022 : }
6023 : #endif
6024 1880155978 : if (startLane == nullptr) {
6025 997892664 : startLane = myLane;
6026 : }
6027 : assert(startLane != 0);
6028 1880155978 : if (myLaneChangeModel->isOpposite()) {
6029 : // depending on the calling context, startLane might be the forward lane
6030 : // or the reverse-direction lane. In the latter case we need to
6031 : // transform it to the forward lane.
6032 512858 : if (isOppositeLane(startLane)) {
6033 : // use leftmost lane of forward edge
6034 108384 : startLane = startLane->getEdge().getOppositeEdge()->getLanes().back();
6035 : assert(startLane != 0);
6036 : #ifdef DEBUG_BESTLANES
6037 : if (DEBUG_COND) {
6038 : std::cout << " startLaneIsOpposite newStartLane=" << startLane->getID() << "\n";
6039 : }
6040 : #endif
6041 : }
6042 : }
6043 1880155978 : if (forceRebuild) {
6044 1719371 : myLastBestLanesEdge = nullptr;
6045 1719371 : myLastBestLanesInternalLane = nullptr;
6046 : }
6047 1880155978 : if (myBestLanes.size() > 0 && !forceRebuild && myLastBestLanesEdge == &startLane->getEdge()) {
6048 1849205055 : updateOccupancyAndCurrentBestLane(startLane);
6049 : #ifdef DEBUG_BESTLANES
6050 : if (DEBUG_COND) {
6051 : std::cout << " only updateOccupancyAndCurrentBestLane\n";
6052 : }
6053 : #endif
6054 1849205055 : return;
6055 : }
6056 30950923 : if (startLane->getEdge().isInternal()) {
6057 14684379 : if (myBestLanes.size() == 0 || forceRebuild) {
6058 : // rebuilt from previous non-internal lane (may backtrack twice if behind an internal junction)
6059 2294 : updateBestLanes(true, startLane->getLogicalPredecessorLane());
6060 : }
6061 14684379 : if (myLastBestLanesInternalLane == startLane && !forceRebuild) {
6062 : #ifdef DEBUG_BESTLANES
6063 : if (DEBUG_COND) {
6064 : std::cout << " nothing to do on internal\n";
6065 : }
6066 : #endif
6067 : return;
6068 : }
6069 : // adapt best lanes to fit the current internal edge:
6070 : // keep the entries that are reachable from this edge
6071 5284375 : const MSEdge* nextEdge = startLane->getNextNormal();
6072 : assert(!nextEdge->isInternal());
6073 10418673 : for (std::vector<std::vector<LaneQ> >::iterator it = myBestLanes.begin(); it != myBestLanes.end();) {
6074 : std::vector<LaneQ>& lanes = *it;
6075 : assert(lanes.size() > 0);
6076 10418673 : if (&(lanes[0].lane->getEdge()) == nextEdge) {
6077 : // keep those lanes which are successors of internal lanes from the edge of startLane
6078 5284375 : std::vector<LaneQ> oldLanes = lanes;
6079 : lanes.clear();
6080 : const std::vector<MSLane*>& sourceLanes = startLane->getEdge().getLanes();
6081 11964123 : for (std::vector<MSLane*>::const_iterator it_source = sourceLanes.begin(); it_source != sourceLanes.end(); ++it_source) {
6082 11265894 : for (std::vector<LaneQ>::iterator it_lane = oldLanes.begin(); it_lane != oldLanes.end(); ++it_lane) {
6083 11265894 : if ((*it_source)->getLinkCont()[0]->getLane() == (*it_lane).lane) {
6084 6679748 : lanes.push_back(*it_lane);
6085 : break;
6086 : }
6087 : }
6088 : }
6089 : assert(lanes.size() == startLane->getEdge().getLanes().size());
6090 : // patch invalid bestLaneOffset and updated myCurrentLaneInBestLanes
6091 11964123 : for (int i = 0; i < (int)lanes.size(); ++i) {
6092 6679748 : if (i + lanes[i].bestLaneOffset < 0) {
6093 106656 : lanes[i].bestLaneOffset = -i;
6094 : }
6095 6679748 : if (i + lanes[i].bestLaneOffset >= (int)lanes.size()) {
6096 27343 : lanes[i].bestLaneOffset = (int)lanes.size() - i - 1;
6097 : }
6098 : assert(i + lanes[i].bestLaneOffset >= 0);
6099 : assert(i + lanes[i].bestLaneOffset < (int)lanes.size());
6100 6679748 : if (lanes[i].bestContinuations[0] != 0) {
6101 : // patch length of bestContinuation to match expectations (only once)
6102 6485258 : lanes[i].bestContinuations.insert(lanes[i].bestContinuations.begin(), (MSLane*)nullptr);
6103 : }
6104 6679748 : if (startLane->getLinkCont()[0]->getLane() == lanes[i].lane) {
6105 5328480 : myCurrentLaneInBestLanes = lanes.begin() + i;
6106 : }
6107 : assert(&(lanes[i].lane->getEdge()) == nextEdge);
6108 : }
6109 5284375 : myLastBestLanesInternalLane = startLane;
6110 5284375 : updateOccupancyAndCurrentBestLane(startLane);
6111 : #ifdef DEBUG_BESTLANES
6112 : if (DEBUG_COND) {
6113 : std::cout << " updated for internal\n";
6114 : }
6115 : #endif
6116 : return;
6117 5284375 : } else {
6118 : // remove passed edges
6119 5134298 : it = myBestLanes.erase(it);
6120 : }
6121 : }
6122 : assert(false); // should always find the next edge
6123 : }
6124 : // start rebuilding
6125 16266544 : myLastBestLanesInternalLane = nullptr;
6126 16266544 : myLastBestLanesEdge = &startLane->getEdge();
6127 : myBestLanes.clear();
6128 :
6129 : // get information about the next stop
6130 16266544 : MSRouteIterator nextStopEdge = myRoute->end();
6131 : const MSLane* nextStopLane = nullptr;
6132 : double nextStopPos = 0;
6133 16266544 : if (!myStops.empty()) {
6134 : const MSStop& nextStop = myStops.front();
6135 262274 : nextStopLane = nextStop.lane;
6136 262274 : if (nextStop.isOpposite) {
6137 : // target leftmost lane in forward direction
6138 340 : nextStopLane = nextStopLane->getEdge().getOppositeEdge()->getLanes().back();
6139 : }
6140 262274 : nextStopEdge = nextStop.edge;
6141 262274 : nextStopPos = nextStop.pars.startPos;
6142 : }
6143 : // myArrivalTime = -1 in the context of validating departSpeed with departLane=best
6144 16266544 : if (myParameter->arrivalLaneProcedure >= ArrivalLaneDefinition::GIVEN && nextStopEdge == myRoute->end() && myArrivalLane >= 0) {
6145 339226 : nextStopEdge = (myRoute->end() - 1);
6146 339226 : nextStopLane = (*nextStopEdge)->getLanes()[myArrivalLane];
6147 339226 : nextStopPos = myArrivalPos;
6148 : }
6149 16266544 : if (nextStopEdge != myRoute->end()) {
6150 : // make sure that the "wrong" lanes get a penalty. (penalty needs to be
6151 : // large enough to overcome a magic threshold in MSLaneChangeModel::DK2004.cpp:383)
6152 601500 : nextStopPos = MAX2(POSITION_EPS, MIN2((double)nextStopPos, (double)(nextStopLane->getLength() - 2 * POSITION_EPS)));
6153 601500 : if (nextStopLane->isInternal()) {
6154 : // switch to the correct lane before entering the intersection
6155 171 : nextStopPos = (*nextStopEdge)->getLength();
6156 : }
6157 : }
6158 :
6159 : // go forward along the next lanes; always look past stops to ensure that we
6160 : // know where to go once the stop ends
6161 : // trains do not have to deal with lane-changing for stops but their best
6162 : // lanes lookahead is needed for rail signal control
6163 : int seen = 0;
6164 : double seenLength = 0;
6165 : bool progress = true;
6166 : // bestLanes must cover the braking distance even when at the very end of the current lane to avoid unecessary slow down
6167 32533088 : const double maxBrakeDist = startLane->getLength() + getCarFollowModel().getHeadwayTime() * getMaxSpeed() + getCarFollowModel().brakeGap(getMaxSpeed()) + getVehicleType().getMinGap();
6168 16266544 : const double lookahead = getLaneChangeModel().getStrategicLookahead();
6169 82427150 : for (MSRouteIterator ce = myCurrEdge; progress;) {
6170 : std::vector<LaneQ> currentLanes;
6171 : const std::vector<MSLane*>* allowed = nullptr;
6172 : const MSEdge* nextEdge = nullptr;
6173 66160606 : if (ce != myRoute->end() && ce + 1 != myRoute->end()) {
6174 54096093 : nextEdge = *(ce + 1);
6175 54096093 : allowed = (*ce)->allowedLanes(*nextEdge, myType->getVehicleClass());
6176 : }
6177 66160606 : const std::vector<MSLane*>& lanes = (*ce)->getLanes();
6178 166590665 : for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
6179 : LaneQ q;
6180 100430059 : MSLane* cl = *i;
6181 100430059 : q.lane = cl;
6182 100430059 : q.bestContinuations.push_back(cl);
6183 100430059 : q.bestLaneOffset = 0;
6184 100430059 : q.length = cl->allowsVehicleClass(myType->getVehicleClass()) ? (*ce)->getLength() : 0;
6185 100430059 : q.currentLength = q.length;
6186 : // if all lanes are forbidden (i.e. due to a dynamic closing) we want to express no preference
6187 100430059 : q.allowsContinuation = allowed == nullptr || std::find(allowed->begin(), allowed->end(), cl) != allowed->end();
6188 100430059 : q.occupation = 0;
6189 100430059 : q.nextOccupation = 0;
6190 100430059 : currentLanes.push_back(q);
6191 : }
6192 : //
6193 : if (nextStopEdge == ce
6194 : // already past the stop edge
6195 66160606 : && !(ce == myCurrEdge && myLane != nullptr && myLane->isInternal())) {
6196 594060 : const MSLane* normalStopLane = nextStopLane->getNormalPredecessorLane();
6197 1882603 : for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
6198 1288543 : if (nextStopLane != nullptr && normalStopLane != (*q).lane) {
6199 694483 : (*q).allowsContinuation = false;
6200 694483 : (*q).length = nextStopPos;
6201 694483 : (*q).currentLength = (*q).length;
6202 : }
6203 : }
6204 : }
6205 :
6206 66160606 : myBestLanes.push_back(currentLanes);
6207 66160606 : ++seen;
6208 66160606 : seenLength += currentLanes[0].lane->getLength();
6209 : ++ce;
6210 66160606 : if (lookahead >= 0) {
6211 45 : progress &= (seen <= 2 || seenLength < lookahead); // custom (but we need to look at least one edge ahead)
6212 : } else {
6213 88049660 : progress &= (seen <= 4 || seenLength < MAX2(maxBrakeDist, 3000.0)); // motorway
6214 71118033 : progress &= (seen <= 8 || seenLength < MAX2(maxBrakeDist, 200.0) || isRailway(getVClass())); // urban
6215 : }
6216 66160606 : progress &= ce != myRoute->end();
6217 : /*
6218 : if(progress) {
6219 : progress &= (currentLanes.size()!=1||(*ce)->getLanes().size()!=1);
6220 : }
6221 : */
6222 66160606 : }
6223 :
6224 : // we are examining the last lane explicitly
6225 16266544 : if (myBestLanes.size() != 0) {
6226 : double bestLength = -1;
6227 : // minimum and maximum lane index with best length
6228 : int bestThisIndex = 0;
6229 : int bestThisMaxIndex = 0;
6230 : int index = 0;
6231 : std::vector<LaneQ>& last = myBestLanes.back();
6232 42220931 : for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
6233 25954387 : if ((*j).length > bestLength) {
6234 : bestLength = (*j).length;
6235 : bestThisIndex = index;
6236 : bestThisMaxIndex = index;
6237 6126517 : } else if ((*j).length == bestLength) {
6238 : bestThisMaxIndex = index;
6239 : }
6240 : }
6241 : index = 0;
6242 : bool requiredChangeRightForbidden = false;
6243 : int requireChangeToLeftForbidden = -1;
6244 42220931 : for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
6245 25954387 : if ((*j).length < bestLength) {
6246 3963348 : if (abs(bestThisIndex - index) < abs(bestThisMaxIndex - index)) {
6247 146883 : (*j).bestLaneOffset = bestThisIndex - index;
6248 : } else {
6249 3816465 : (*j).bestLaneOffset = bestThisMaxIndex - index;
6250 : }
6251 3963348 : if (!(*j).allowsContinuation) {
6252 565752 : if ((*j).bestLaneOffset < 0 && (!(*j).lane->allowsChangingRight(getVClass())
6253 252820 : || !(*j).lane->getParallelLane(-1, false)->allowsVehicleClass(getVClass())
6254 250167 : || requiredChangeRightForbidden)) {
6255 : // this lane and all further lanes to the left cannot be used
6256 : requiredChangeRightForbidden = true;
6257 2653 : (*j).length = 0;
6258 563099 : } else if ((*j).bestLaneOffset > 0 && (!(*j).lane->allowsChangingLeft(getVClass())
6259 312906 : || !(*j).lane->getParallelLane(1, false)->allowsVehicleClass(getVClass()))) {
6260 : // this lane and all previous lanes to the right cannot be used
6261 6393 : requireChangeToLeftForbidden = (*j).lane->getIndex();
6262 : }
6263 : }
6264 : }
6265 : }
6266 16272947 : for (int i = requireChangeToLeftForbidden; i >= 0; i--) {
6267 6403 : if (last[i].bestLaneOffset > 0) {
6268 6403 : last[i].length = 0;
6269 : }
6270 : }
6271 : #ifdef DEBUG_BESTLANES
6272 : if (DEBUG_COND) {
6273 : std::cout << " last edge=" << last.front().lane->getEdge().getID() << " (bestIndex=" << bestThisIndex << " bestMaxIndex=" << bestThisMaxIndex << "):\n";
6274 : std::vector<LaneQ>& laneQs = myBestLanes.back();
6275 : for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
6276 : std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << "\n";
6277 : }
6278 : }
6279 : #endif
6280 : }
6281 : // go backward through the lanes
6282 : // track back best lane and compute the best prior lane(s)
6283 66160606 : for (std::vector<std::vector<LaneQ> >::reverse_iterator i = myBestLanes.rbegin() + 1; i != myBestLanes.rend(); ++i) {
6284 : std::vector<LaneQ>& nextLanes = (*(i - 1));
6285 : std::vector<LaneQ>& clanes = (*i);
6286 49894062 : MSEdge* const cE = &clanes[0].lane->getEdge();
6287 : int index = 0;
6288 : double bestConnectedLength = -1;
6289 : double bestLength = -1;
6290 123646917 : for (const LaneQ& j : nextLanes) {
6291 147505710 : if (j.lane->isApproachedFrom(cE) && bestConnectedLength < j.length) {
6292 : bestConnectedLength = j.length;
6293 : }
6294 73752855 : if (bestLength < j.length) {
6295 : bestLength = j.length;
6296 : }
6297 : }
6298 : // compute index of the best lane (highest length and least offset from the best next lane)
6299 : int bestThisIndex = 0;
6300 : int bestThisMaxIndex = 0;
6301 49894062 : if (bestConnectedLength > 0) {
6302 : index = 0;
6303 124334113 : for (LaneQ& j : clanes) {
6304 : const LaneQ* bestConnectedNext = nullptr;
6305 74452672 : if (j.allowsContinuation) {
6306 175988774 : for (const LaneQ& m : nextLanes) {
6307 120355750 : if ((m.lane->allowsVehicleClass(getVClass()) || m.lane->hadPermissionChanges())
6308 111444103 : && m.lane->isApproachedFrom(j.lane, getVClass())) {
6309 66675218 : if (betterContinuation(bestConnectedNext, m)) {
6310 : bestConnectedNext = &m;
6311 : }
6312 : }
6313 : }
6314 64601734 : if (bestConnectedNext != nullptr) {
6315 64601726 : if (bestConnectedNext->length == bestConnectedLength && abs(bestConnectedNext->bestLaneOffset) < 2) {
6316 62840864 : j.length += bestLength;
6317 : } else {
6318 1760862 : j.length += bestConnectedNext->length;
6319 : }
6320 64601726 : j.bestLaneOffset = bestConnectedNext->bestLaneOffset;
6321 : }
6322 : }
6323 64601726 : if (bestConnectedNext != nullptr && (bestConnectedNext->allowsContinuation || bestConnectedNext->length > 0)) {
6324 64563727 : copy(bestConnectedNext->bestContinuations.begin(), bestConnectedNext->bestContinuations.end(), back_inserter(j.bestContinuations));
6325 : } else {
6326 9888945 : j.allowsContinuation = false;
6327 : }
6328 74452672 : if (clanes[bestThisIndex].length < j.length
6329 67318809 : || (clanes[bestThisIndex].length == j.length && abs(clanes[bestThisIndex].bestLaneOffset) > abs(j.bestLaneOffset))
6330 204850736 : || (clanes[bestThisIndex].length == j.length && abs(clanes[bestThisIndex].bestLaneOffset) == abs(j.bestLaneOffset) &&
6331 63223597 : nextLinkPriority(clanes[bestThisIndex].bestContinuations) < nextLinkPriority(j.bestContinuations))
6332 : ) {
6333 : bestThisIndex = index;
6334 : bestThisMaxIndex = index;
6335 67162628 : } else if (clanes[bestThisIndex].length == j.length
6336 63211890 : && abs(clanes[bestThisIndex].bestLaneOffset) == abs(j.bestLaneOffset)
6337 130374386 : && nextLinkPriority(clanes[bestThisIndex].bestContinuations) == nextLinkPriority(j.bestContinuations)) {
6338 : bestThisMaxIndex = index;
6339 : }
6340 74452672 : index++;
6341 : }
6342 :
6343 : //vehicle with elecHybrid device prefers running under an overhead wire
6344 49881441 : if (getDevice(typeid(MSDevice_ElecHybrid)) != nullptr) {
6345 : index = 0;
6346 491 : for (const LaneQ& j : clanes) {
6347 339 : std::string overheadWireSegmentID = MSNet::getInstance()->getStoppingPlaceID(j.lane, j.currentLength / 2., SUMO_TAG_OVERHEAD_WIRE_SEGMENT);
6348 339 : if (overheadWireSegmentID != "") {
6349 : bestThisIndex = index;
6350 : bestThisMaxIndex = index;
6351 : }
6352 339 : index++;
6353 : }
6354 : }
6355 :
6356 : } else {
6357 : // only needed in case of disconnected routes
6358 : int bestNextIndex = 0;
6359 12621 : int bestDistToNeeded = (int) clanes.size();
6360 : index = 0;
6361 35621 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6362 23000 : if ((*j).allowsContinuation) {
6363 : int nextIndex = 0;
6364 56890 : for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
6365 34640 : if ((*m).lane->isApproachedFrom((*j).lane, getVClass())) {
6366 5023 : if (bestDistToNeeded > abs((*m).bestLaneOffset)) {
6367 : bestDistToNeeded = abs((*m).bestLaneOffset);
6368 : bestThisIndex = index;
6369 : bestThisMaxIndex = index;
6370 : bestNextIndex = nextIndex;
6371 : }
6372 : }
6373 : }
6374 : }
6375 : }
6376 12621 : clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
6377 12621 : copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
6378 :
6379 : }
6380 : // set bestLaneOffset for all lanes
6381 : index = 0;
6382 : bool requiredChangeRightForbidden = false;
6383 : int requireChangeToLeftForbidden = -1;
6384 124369734 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6385 74475672 : if ((*j).length < clanes[bestThisIndex].length
6386 62881539 : || ((*j).length == clanes[bestThisIndex].length && abs((*j).bestLaneOffset) > abs(clanes[bestThisIndex].bestLaneOffset))
6387 137356979 : || (nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)
6388 : ) {
6389 11771038 : if (abs(bestThisIndex - index) < abs(bestThisMaxIndex - index)) {
6390 703087 : (*j).bestLaneOffset = bestThisIndex - index;
6391 : } else {
6392 11067951 : (*j).bestLaneOffset = bestThisMaxIndex - index;
6393 : }
6394 11771038 : if ((nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)) {
6395 : // try to move away from the lower-priority lane before it ends
6396 10055734 : (*j).length = (*j).currentLength;
6397 : }
6398 11771038 : if (!(*j).allowsContinuation) {
6399 9874686 : if ((*j).bestLaneOffset < 0 && (!(*j).lane->allowsChangingRight(getVClass())
6400 2506343 : || !(*j).lane->getParallelLane(-1, false)->allowsVehicleClass(getVClass())
6401 2492157 : || requiredChangeRightForbidden)) {
6402 : // this lane and all further lanes to the left cannot be used
6403 : requiredChangeRightForbidden = true;
6404 28500 : if ((*j).length == (*j).currentLength) {
6405 28500 : (*j).length = 0;
6406 : }
6407 9846186 : } else if ((*j).bestLaneOffset > 0 && (!(*j).lane->allowsChangingLeft(getVClass())
6408 7310971 : || !(*j).lane->getParallelLane(1, false)->allowsVehicleClass(getVClass()))) {
6409 : // this lane and all previous lanes to the right cannot be used
6410 114512 : requireChangeToLeftForbidden = (*j).lane->getIndex();
6411 : }
6412 : }
6413 : } else {
6414 62704634 : (*j).bestLaneOffset = 0;
6415 : }
6416 : }
6417 50026837 : for (int idx = requireChangeToLeftForbidden; idx >= 0; idx--) {
6418 132775 : if (clanes[idx].length == clanes[idx].currentLength) {
6419 132775 : clanes[idx].length = 0;
6420 : };
6421 : }
6422 :
6423 : //vehicle with elecHybrid device prefers running under an overhead wire
6424 49894062 : if (static_cast<MSDevice_ElecHybrid*>(getDevice(typeid(MSDevice_ElecHybrid))) != 0) {
6425 : index = 0;
6426 152 : std::string overheadWireID = MSNet::getInstance()->getStoppingPlaceID(clanes[bestThisIndex].lane, (clanes[bestThisIndex].currentLength) / 2, SUMO_TAG_OVERHEAD_WIRE_SEGMENT);
6427 152 : if (overheadWireID != "") {
6428 373 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6429 261 : (*j).bestLaneOffset = bestThisIndex - index;
6430 : }
6431 : }
6432 : }
6433 :
6434 : #ifdef DEBUG_BESTLANES
6435 : if (DEBUG_COND) {
6436 : std::cout << " edge=" << cE->getID() << " (bestIndex=" << bestThisIndex << " bestMaxIndex=" << bestThisMaxIndex << "):\n";
6437 : std::vector<LaneQ>& laneQs = clanes;
6438 : for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
6439 : std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << " allowCont=" << (*j).allowsContinuation << "\n";
6440 : }
6441 : }
6442 : #endif
6443 :
6444 : }
6445 16266544 : if (myBestLanes.front().front().lane->isInternal()) {
6446 : // route starts on an internal lane
6447 36 : if (myLane != nullptr) {
6448 : startLane = myLane;
6449 : } else {
6450 : // vehicle not yet departed
6451 12 : startLane = myBestLanes.front().front().lane;
6452 : }
6453 : }
6454 16266544 : updateOccupancyAndCurrentBestLane(startLane);
6455 : #ifdef DEBUG_BESTLANES
6456 : if (DEBUG_COND) {
6457 : std::cout << SIMTIME << " veh=" << getID() << " bestCont=" << toString(getBestLanesContinuation()) << "\n";
6458 : }
6459 : #endif
6460 : }
6461 :
6462 : void
6463 236 : MSVehicle::updateLaneBruttoSum() {
6464 236 : if (myLane != nullptr) {
6465 236 : myLane->markRecalculateBruttoSum();
6466 : }
6467 236 : }
6468 :
6469 : bool
6470 66675218 : MSVehicle::betterContinuation(const LaneQ* bestConnectedNext, const LaneQ& m) const {
6471 66675218 : if (bestConnectedNext == nullptr) {
6472 : return true;
6473 2073492 : } else if (m.lane->getBidiLane() != nullptr && bestConnectedNext->lane->getBidiLane() == nullptr) {
6474 : return false;
6475 2072700 : } else if (bestConnectedNext->lane->getBidiLane() != nullptr && m.lane->getBidiLane() == nullptr) {
6476 : return true;
6477 2072700 : } else if (bestConnectedNext->length < m.length) {
6478 : return true;
6479 1706799 : } else if (bestConnectedNext->length == m.length) {
6480 1178833 : if (abs(bestConnectedNext->bestLaneOffset) > abs(m.bestLaneOffset)) {
6481 : return true;
6482 : }
6483 1012632 : const double contRight = getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_CONTRIGHT, 1);
6484 : if (contRight < 1
6485 : // if we don't check for adjacency, the rightmost line will get
6486 : // multiple chances to be better which leads to an uninituitve distribution
6487 1006 : && (m.lane->getIndex() - bestConnectedNext->lane->getIndex()) == 1
6488 1013411 : && RandHelper::rand(getRNG()) > contRight) {
6489 : return true;
6490 : }
6491 : }
6492 : return false;
6493 : }
6494 :
6495 :
6496 : int
6497 402175400 : MSVehicle::nextLinkPriority(const std::vector<MSLane*>& conts) {
6498 402175400 : if (conts.size() < 2) {
6499 : return -1;
6500 : } else {
6501 366485202 : const MSLink* const link = conts[0]->getLinkTo(conts[1]);
6502 366485202 : if (link != nullptr) {
6503 366463451 : return link->havePriority() ? 1 : 0;
6504 : } else {
6505 : // disconnected route
6506 : return -1;
6507 : }
6508 : }
6509 : }
6510 :
6511 :
6512 : void
6513 1870755974 : MSVehicle::updateOccupancyAndCurrentBestLane(const MSLane* startLane) {
6514 : std::vector<LaneQ>& currLanes = *myBestLanes.begin();
6515 : std::vector<LaneQ>::iterator i;
6516 : #ifdef _DEBUG
6517 : bool found = false;
6518 : #endif
6519 5340905819 : for (i = currLanes.begin(); i != currLanes.end(); ++i) {
6520 : double nextOccupation = 0;
6521 7663618212 : for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
6522 4193468367 : nextOccupation += (*j)->getBruttoVehLenSum();
6523 : }
6524 3470149845 : (*i).nextOccupation = nextOccupation;
6525 : #ifdef DEBUG_BESTLANES
6526 : if (DEBUG_COND) {
6527 : std::cout << " lane=" << (*i).lane->getID() << " nextOccupation=" << nextOccupation << "\n";
6528 : }
6529 : #endif
6530 3470149845 : if ((*i).lane == startLane) {
6531 1865471599 : myCurrentLaneInBestLanes = i;
6532 : #ifdef _DEBUG
6533 : found = true;
6534 : #endif
6535 : }
6536 : }
6537 : #ifdef _DEBUG
6538 : assert(found || startLane->isInternal());
6539 : #endif
6540 1870755974 : }
6541 :
6542 :
6543 : const std::vector<MSLane*>&
6544 2055673677 : MSVehicle::getBestLanesContinuation() const {
6545 2055673677 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6546 : return myEmptyLaneVector;
6547 : }
6548 2055673677 : return (*myCurrentLaneInBestLanes).bestContinuations;
6549 : }
6550 :
6551 :
6552 : const std::vector<MSLane*>&
6553 69830593 : MSVehicle::getBestLanesContinuation(const MSLane* const l) const {
6554 : const MSLane* lane = l;
6555 : // XXX: shouldn't this be a "while" to cover more than one internal lane? (Leo) Refs. #2575
6556 69830593 : if (lane->getEdge().isInternal()) {
6557 : // internal edges are not kept inside the bestLanes structure
6558 5449329 : lane = lane->getLinkCont()[0]->getLane();
6559 : }
6560 69830593 : if (myBestLanes.size() == 0) {
6561 : return myEmptyLaneVector;
6562 : }
6563 115122201 : for (std::vector<LaneQ>::const_iterator i = myBestLanes[0].begin(); i != myBestLanes[0].end(); ++i) {
6564 115108960 : if ((*i).lane == lane) {
6565 69817352 : return (*i).bestContinuations;
6566 : }
6567 : }
6568 : return myEmptyLaneVector;
6569 : }
6570 :
6571 : const std::vector<const MSLane*>
6572 294680 : MSVehicle::getUpcomingLanesUntil(double distance) const {
6573 : std::vector<const MSLane*> lanes;
6574 :
6575 294680 : if (distance <= 0. || hasArrived()) {
6576 : // WRITE_WARNINGF(TL("MSVehicle::getUpcomingLanesUntil(): distance ('%') should be greater than 0."), distance);
6577 : return lanes;
6578 : }
6579 :
6580 294472 : if (!myLaneChangeModel->isOpposite()) {
6581 291154 : distance += getPositionOnLane();
6582 : } else {
6583 3318 : distance += myLane->getOppositePos(getPositionOnLane());
6584 : }
6585 294472 : MSLane* lane = myLaneChangeModel->isOpposite() ? myLane->getParallelOpposite() : myLane;
6586 302389 : while (lane->isInternal() && (distance > 0.)) { // include initial internal lanes
6587 7917 : lanes.insert(lanes.end(), lane);
6588 7917 : distance -= lane->getLength();
6589 13328 : lane = lane->getLinkCont().front()->getViaLaneOrLane();
6590 : }
6591 :
6592 294472 : const std::vector<MSLane*>& contLanes = getBestLanesContinuation();
6593 294472 : if (contLanes.empty()) {
6594 : return lanes;
6595 : }
6596 : auto contLanesIt = contLanes.begin();
6597 294472 : MSRouteIterator routeIt = myCurrEdge; // keep track of covered edges in myRoute
6598 629157 : while (distance > 0.) {
6599 342491 : MSLane* l = nullptr;
6600 342491 : if (contLanesIt != contLanes.end()) {
6601 326759 : l = *contLanesIt;
6602 : if (l != nullptr) {
6603 : assert(l->getEdge().getID() == (*routeIt)->getLanes().front()->getEdge().getID());
6604 : }
6605 : ++contLanesIt;
6606 326759 : if (l != nullptr || myLane->isInternal()) {
6607 : ++routeIt;
6608 : }
6609 326759 : if (l == nullptr) {
6610 5407 : continue;
6611 : }
6612 15732 : } else if (routeIt != myRoute->end()) { // bestLanes didn't get us far enough
6613 : // choose left-most lane as default (avoid sidewalks, bike lanes etc)
6614 8815 : l = (*routeIt)->getLanes().back();
6615 : ++routeIt;
6616 : } else { // the search distance goes beyond our route
6617 : break;
6618 : }
6619 :
6620 : assert(l != nullptr);
6621 :
6622 : // insert internal lanes if applicable
6623 330167 : const MSLane* internalLane = lanes.size() > 0 ? lanes.back()->getInternalFollowingLane(l) : nullptr;
6624 374253 : while ((internalLane != nullptr) && internalLane->isInternal() && (distance > 0.)) {
6625 44086 : lanes.insert(lanes.end(), internalLane);
6626 44086 : distance -= internalLane->getLength();
6627 70465 : internalLane = internalLane->getLinkCont().front()->getViaLaneOrLane();
6628 : }
6629 330167 : if (distance <= 0.) {
6630 : break;
6631 : }
6632 :
6633 329278 : lanes.insert(lanes.end(), l);
6634 329278 : distance -= l->getLength();
6635 : }
6636 :
6637 : return lanes;
6638 0 : }
6639 :
6640 : const std::vector<const MSLane*>
6641 6057 : MSVehicle::getPastLanesUntil(double distance) const {
6642 : std::vector<const MSLane*> lanes;
6643 :
6644 6057 : if (distance <= 0.) {
6645 : // WRITE_WARNINGF(TL("MSVehicle::getPastLanesUntil(): distance ('%') should be greater than 0."), distance);
6646 : return lanes;
6647 : }
6648 :
6649 5949 : MSRouteIterator routeIt = myCurrEdge;
6650 5949 : if (!myLaneChangeModel->isOpposite()) {
6651 5925 : distance += myLane->getLength() - getPositionOnLane();
6652 : } else {
6653 24 : distance += myLane->getParallelOpposite()->getLength() - myLane->getOppositePos(getPositionOnLane());
6654 : }
6655 5949 : MSLane* lane = myLaneChangeModel->isOpposite() ? myLane->getParallelOpposite() : myLane;
6656 5970 : while (lane->isInternal() && (distance > 0.)) { // include initial internal lanes
6657 21 : lanes.insert(lanes.end(), lane);
6658 21 : distance -= lane->getLength();
6659 21 : lane = lane->getLogicalPredecessorLane();
6660 : }
6661 :
6662 8043 : while (distance > 0.) {
6663 : // choose left-most lane as default (avoid sidewalks, bike lanes etc)
6664 7462 : MSLane* l = (*routeIt)->getLanes().back();
6665 :
6666 : // insert internal lanes if applicable
6667 7462 : const MSEdge* internalEdge = lanes.size() > 0 ? (*routeIt)->getInternalFollowingEdge(&(lanes.back()->getEdge()), getVClass()) : nullptr;
6668 7483 : const MSLane* internalLane = internalEdge != nullptr ? internalEdge->getLanes().front() : nullptr;
6669 : std::vector<const MSLane*> internalLanes;
6670 8981 : while ((internalLane != nullptr) && internalLane->isInternal()) { // collect all internal successor lanes
6671 1519 : internalLanes.insert(internalLanes.begin(), internalLane);
6672 3032 : internalLane = internalLane->getLinkCont().front()->getViaLaneOrLane();
6673 : }
6674 8981 : for (auto it = internalLanes.begin(); (it != internalLanes.end()) && (distance > 0.); ++it) { // check remaining distance in correct order
6675 1519 : lanes.insert(lanes.end(), *it);
6676 1519 : distance -= (*it)->getLength();
6677 : }
6678 7462 : if (distance <= 0.) {
6679 : break;
6680 : }
6681 :
6682 7446 : lanes.insert(lanes.end(), l);
6683 7446 : distance -= l->getLength();
6684 :
6685 : // NOTE: we're going backwards with the (bi-directional) Iterator
6686 : // TODO: consider make reverse_iterator() when moving on to C++14 or later
6687 7446 : if (routeIt != myRoute->begin()) {
6688 : --routeIt;
6689 : } else { // we went backwards to begin() and already processed the first and final element
6690 : break;
6691 : }
6692 7462 : }
6693 :
6694 : return lanes;
6695 0 : }
6696 :
6697 :
6698 : const std::vector<MSLane*>
6699 5721 : MSVehicle::getUpstreamOppositeLanes() const {
6700 5721 : const std::vector<const MSLane*> routeLanes = getPastLanesUntil(myLane->getMaximumBrakeDist());
6701 : std::vector<MSLane*> result;
6702 12963 : for (const MSLane* lane : routeLanes) {
6703 7923 : MSLane* opposite = lane->getOpposite();
6704 7923 : if (opposite != nullptr) {
6705 7242 : result.push_back(opposite);
6706 : } else {
6707 : break;
6708 : }
6709 : }
6710 5721 : return result;
6711 5721 : }
6712 :
6713 :
6714 : int
6715 310984786 : MSVehicle::getBestLaneOffset() const {
6716 310984786 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6717 : return 0;
6718 : } else {
6719 310658144 : return (*myCurrentLaneInBestLanes).bestLaneOffset;
6720 : }
6721 : }
6722 :
6723 : double
6724 23253 : MSVehicle::getBestLaneDist() const {
6725 23253 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6726 : return -1;
6727 : } else {
6728 23253 : return (*myCurrentLaneInBestLanes).length;
6729 : }
6730 : }
6731 :
6732 :
6733 :
6734 : void
6735 644525081 : MSVehicle::adaptBestLanesOccupation(int laneIndex, double density) {
6736 : std::vector<MSVehicle::LaneQ>& preb = myBestLanes.front();
6737 : assert(laneIndex < (int)preb.size());
6738 644525081 : preb[laneIndex].occupation = density + preb[laneIndex].nextOccupation;
6739 644525081 : }
6740 :
6741 :
6742 : void
6743 71072 : MSVehicle::fixPosition() {
6744 71072 : if (MSGlobals::gLaneChangeDuration > 0 && !myLaneChangeModel->isChangingLanes()) {
6745 39626 : myState.myPosLat = 0;
6746 : }
6747 71072 : }
6748 :
6749 : std::pair<const MSLane*, double>
6750 303 : MSVehicle::getLanePosAfterDist(double distance) const {
6751 303 : if (distance == 0) {
6752 255 : return std::make_pair(myLane, getPositionOnLane());
6753 : }
6754 48 : const std::vector<const MSLane*> lanes = getUpcomingLanesUntil(distance);
6755 48 : distance += getPositionOnLane();
6756 48 : for (const MSLane* lane : lanes) {
6757 48 : if (lane->getLength() > distance) {
6758 : return std::make_pair(lane, distance);
6759 : }
6760 0 : distance -= lane->getLength();
6761 : }
6762 0 : return std::make_pair(nullptr, -1);
6763 48 : }
6764 :
6765 :
6766 : double
6767 16099 : MSVehicle::getDistanceToPosition(double destPos, const MSLane* destLane) const {
6768 16099 : if (isOnRoad() && destLane != nullptr) {
6769 16066 : return myRoute->getDistanceBetween(getPositionOnLane(), destPos, myLane, destLane);
6770 : }
6771 : return std::numeric_limits<double>::max();
6772 : }
6773 :
6774 :
6775 : std::pair<const MSVehicle* const, double>
6776 76422772 : MSVehicle::getLeader(double dist, bool considerCrossingFoes) const {
6777 76422772 : if (myLane == nullptr) {
6778 0 : return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
6779 : }
6780 76422772 : if (dist == 0) {
6781 2460 : dist = getCarFollowModel().brakeGap(getSpeed()) + getVehicleType().getMinGap();
6782 : }
6783 : const MSVehicle* lead = nullptr;
6784 76422772 : const MSLane* lane = myLane; // ensure lane does not change between getVehiclesSecure and releaseVehicles;
6785 76422772 : const MSLane::VehCont& vehs = lane->getVehiclesSecure();
6786 : // vehicle might be outside the road network
6787 76422772 : MSLane::VehCont::const_iterator it = std::find(vehs.begin(), vehs.end(), this);
6788 76422772 : if (it != vehs.end() && it + 1 != vehs.end()) {
6789 72699951 : lead = *(it + 1);
6790 : }
6791 72699951 : if (lead != nullptr) {
6792 : std::pair<const MSVehicle* const, double> result(
6793 72699951 : lead, lead->getBackPositionOnLane(myLane) - getPositionOnLane() - getVehicleType().getMinGap());
6794 72699951 : lane->releaseVehicles();
6795 72699951 : return result;
6796 : }
6797 3722821 : const double seen = myLane->getLength() - getPositionOnLane();
6798 3722821 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(myLane);
6799 3722821 : std::pair<const MSVehicle* const, double> result = myLane->getLeaderOnConsecutive(dist, seen, getSpeed(), *this, bestLaneConts, considerCrossingFoes);
6800 3722821 : lane->releaseVehicles();
6801 3722821 : return result;
6802 : }
6803 :
6804 :
6805 : std::pair<const MSVehicle* const, double>
6806 2271158 : MSVehicle::getFollower(double dist) const {
6807 2271158 : if (myLane == nullptr) {
6808 0 : return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
6809 : }
6810 2271158 : if (dist == 0) {
6811 810953 : dist = getCarFollowModel().brakeGap(myLane->getEdge().getSpeedLimit() * 2, 4.5, 0);
6812 : }
6813 2271158 : return myLane->getFollower(this, getPositionOnLane(), dist, MSLane::MinorLinkMode::FOLLOW_NEVER);
6814 : }
6815 :
6816 :
6817 : double
6818 0 : MSVehicle::getTimeGapOnLane() const {
6819 : // calling getLeader with 0 would induce a dist calculation but we only want to look for the leaders on the current lane
6820 0 : std::pair<const MSVehicle* const, double> leaderInfo = getLeader(-1);
6821 0 : if (leaderInfo.first == nullptr || getSpeed() == 0) {
6822 0 : return -1;
6823 : }
6824 0 : return (leaderInfo.second + getVehicleType().getMinGap()) / getSpeed();
6825 : }
6826 :
6827 :
6828 : void
6829 4135054 : MSVehicle::addTransportable(MSTransportable* transportable) {
6830 4135054 : MSBaseVehicle::addTransportable(transportable);
6831 41249 : if (myStops.size() > 0 && myStops.front().reached) {
6832 37806 : if (transportable->isPerson()) {
6833 37227 : if (myStops.front().triggered && myStops.front().numExpectedPerson > 0) {
6834 1842 : myStops.front().numExpectedPerson -= (int)myStops.front().pars.awaitedPersons.count(transportable->getID());
6835 : }
6836 : } else {
6837 579 : if (myStops.front().pars.containerTriggered && myStops.front().numExpectedContainer > 0) {
6838 20 : myStops.front().numExpectedContainer -= (int)myStops.front().pars.awaitedContainers.count(transportable->getID());
6839 : }
6840 : }
6841 : }
6842 41249 : }
6843 :
6844 :
6845 : void
6846 701757907 : MSVehicle::setBlinkerInformation() {
6847 : switchOffSignal(VEH_SIGNAL_BLINKER_RIGHT | VEH_SIGNAL_BLINKER_LEFT);
6848 701757907 : int state = myLaneChangeModel->getOwnState();
6849 : // do not set blinker for sublane changes or when blocked from changing to the right
6850 701757907 : const bool blinkerManoeuvre = (((state & LCA_SUBLANE) == 0) && (
6851 609889768 : (state & LCA_KEEPRIGHT) == 0 || (state & LCA_BLOCKED) == 0));
6852 : Signalling left = VEH_SIGNAL_BLINKER_LEFT;
6853 : Signalling right = VEH_SIGNAL_BLINKER_RIGHT;
6854 701757907 : if (MSGlobals::gLefthand) {
6855 : // lane indices increase from left to right
6856 : std::swap(left, right);
6857 : }
6858 701757907 : if ((state & LCA_LEFT) != 0 && blinkerManoeuvre) {
6859 19900944 : switchOnSignal(left);
6860 681856963 : } else if ((state & LCA_RIGHT) != 0 && blinkerManoeuvre) {
6861 5608990 : switchOnSignal(right);
6862 676247973 : } else if (myLaneChangeModel->isChangingLanes()) {
6863 254841 : if (myLaneChangeModel->getLaneChangeDirection() == 1) {
6864 165348 : switchOnSignal(left);
6865 : } else {
6866 89493 : switchOnSignal(right);
6867 : }
6868 : } else {
6869 675993132 : const MSLane* lane = getLane();
6870 675993132 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, 1, *lane, getBestLanesContinuation());
6871 675993132 : if (link != lane->getLinkCont().end() && lane->getLength() - getPositionOnLane() < lane->getVehicleMaxSpeed(this) * (double) 7.) {
6872 170606545 : switch ((*link)->getDirection()) {
6873 : case LinkDirection::TURN:
6874 : case LinkDirection::LEFT:
6875 : case LinkDirection::PARTLEFT:
6876 : switchOnSignal(VEH_SIGNAL_BLINKER_LEFT);
6877 : break;
6878 : case LinkDirection::RIGHT:
6879 : case LinkDirection::PARTRIGHT:
6880 : switchOnSignal(VEH_SIGNAL_BLINKER_RIGHT);
6881 : break;
6882 : default:
6883 : break;
6884 : }
6885 : }
6886 : }
6887 : // stopping related signals
6888 701757907 : if (hasStops()
6889 701757907 : && (myStops.begin()->reached ||
6890 15438629 : (myStopDist < (myLane->getLength() - getPositionOnLane())
6891 4215293 : && myStopDist < getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this), getCarFollowModel().getMaxDecel(), 3)))) {
6892 17307820 : if (myStops.begin()->lane->getIndex() > 0 && myStops.begin()->lane->getParallelLane(-1)->allowsVehicleClass(getVClass())) {
6893 : // not stopping on the right. Activate emergency blinkers
6894 : switchOnSignal(VEH_SIGNAL_BLINKER_LEFT | VEH_SIGNAL_BLINKER_RIGHT);
6895 17049307 : } else if (!myStops.begin()->reached && (myStops.begin()->pars.parking == ParkingType::OFFROAD)) {
6896 : // signal upcoming parking stop on the current lane when within braking distance (~2 seconds before braking)
6897 1640264 : switchOnSignal(MSGlobals::gLefthand ? VEH_SIGNAL_BLINKER_LEFT : VEH_SIGNAL_BLINKER_RIGHT);
6898 : }
6899 : }
6900 701757907 : if (myInfluencer != nullptr && myInfluencer->getSignals() >= 0) {
6901 14 : mySignals = myInfluencer->getSignals();
6902 : myInfluencer->setSignals(-1); // overwrite computed signals only once
6903 : }
6904 701757907 : }
6905 :
6906 : void
6907 85638 : MSVehicle::setEmergencyBlueLight(SUMOTime currentTime) {
6908 :
6909 : //TODO look if timestep ist SIMSTEP
6910 85638 : if (currentTime % 1000 == 0) {
6911 26061 : if (signalSet(VEH_SIGNAL_EMERGENCY_BLUE)) {
6912 : switchOffSignal(VEH_SIGNAL_EMERGENCY_BLUE);
6913 : } else {
6914 : switchOnSignal(VEH_SIGNAL_EMERGENCY_BLUE);
6915 : }
6916 : }
6917 85638 : }
6918 :
6919 :
6920 : int
6921 22491021 : MSVehicle::getLaneIndex() const {
6922 22491021 : return myLane == nullptr ? -1 : myLane->getIndex();
6923 : }
6924 :
6925 :
6926 : void
6927 14732948 : MSVehicle::setTentativeLaneAndPosition(MSLane* lane, double pos, double posLat) {
6928 14732948 : myLane = lane;
6929 14732948 : myState.myPos = pos;
6930 14732948 : myState.myPosLat = posLat;
6931 14732948 : myState.myBackPos = pos - getVehicleType().getLength();
6932 14732948 : }
6933 :
6934 :
6935 : double
6936 395354977 : MSVehicle::getRightSideOnLane() const {
6937 395354977 : return myState.myPosLat + 0.5 * myLane->getWidth() - 0.5 * getVehicleType().getWidth();
6938 : }
6939 :
6940 :
6941 : double
6942 389876491 : MSVehicle::getLeftSideOnLane() const {
6943 389876491 : return myState.myPosLat + 0.5 * myLane->getWidth() + 0.5 * getVehicleType().getWidth();
6944 : }
6945 :
6946 :
6947 : double
6948 315178329 : MSVehicle::getRightSideOnLane(const MSLane* lane) const {
6949 315178329 : return myState.myPosLat + 0.5 * lane->getWidth() - 0.5 * getVehicleType().getWidth();
6950 : }
6951 :
6952 :
6953 : double
6954 314691413 : MSVehicle::getLeftSideOnLane(const MSLane* lane) const {
6955 314691413 : return myState.myPosLat + 0.5 * lane->getWidth() + 0.5 * getVehicleType().getWidth();
6956 : }
6957 :
6958 :
6959 : double
6960 251079991 : MSVehicle::getRightSideOnEdge(const MSLane* lane) const {
6961 251079991 : return getCenterOnEdge(lane) - 0.5 * getVehicleType().getWidth();
6962 : }
6963 :
6964 :
6965 : double
6966 33073350 : MSVehicle::getLeftSideOnEdge(const MSLane* lane) const {
6967 33073350 : return getCenterOnEdge(lane) + 0.5 * getVehicleType().getWidth();
6968 : }
6969 :
6970 :
6971 : double
6972 733416109 : MSVehicle::getCenterOnEdge(const MSLane* lane) const {
6973 733416109 : if (lane == nullptr || &lane->getEdge() == &myLane->getEdge()) {
6974 732961269 : return myLane->getRightSideOnEdge() + myState.myPosLat + 0.5 * myLane->getWidth();
6975 454840 : } else if (lane == myLaneChangeModel->getShadowLane()) {
6976 15277 : if (myLaneChangeModel->isOpposite() && &lane->getEdge() != &myLane->getEdge()) {
6977 15198 : return lane->getRightSideOnEdge() + lane->getWidth() - myState.myPosLat + 0.5 * myLane->getWidth();
6978 : }
6979 79 : if (myLaneChangeModel->getShadowDirection() == -1) {
6980 0 : return lane->getRightSideOnEdge() + lane->getWidth() + myState.myPosLat + 0.5 * myLane->getWidth();
6981 : } else {
6982 79 : return lane->getRightSideOnEdge() - myLane->getWidth() + myState.myPosLat + 0.5 * myLane->getWidth();
6983 : }
6984 439563 : } else if (lane == myLane->getBidiLane()) {
6985 18751 : return lane->getRightSideOnEdge() - myState.myPosLat + 0.5 * lane->getWidth();
6986 : } else {
6987 : assert(myFurtherLanes.size() == myFurtherLanesPosLat.size());
6988 491546 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6989 466951 : if (myFurtherLanes[i] == lane) {
6990 : #ifdef DEBUG_FURTHER
6991 : if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat=" << myFurtherLanesPosLat[i]
6992 : << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
6993 : << "\n";
6994 : #endif
6995 395842 : return lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
6996 71109 : } else if (myFurtherLanes[i]->getBidiLane() == lane) {
6997 : #ifdef DEBUG_FURTHER
6998 : if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat(bidi)=" << myFurtherLanesPosLat[i]
6999 : << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
7000 : << "\n";
7001 : #endif
7002 375 : return lane->getRightSideOnEdge() - myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
7003 : }
7004 : }
7005 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
7006 24595 : const std::vector<MSLane*>& shadowFurther = myLaneChangeModel->getShadowFurtherLanes();
7007 24944 : for (int i = 0; i < (int)shadowFurther.size(); ++i) {
7008 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
7009 24944 : if (shadowFurther[i] == lane) {
7010 : assert(myLaneChangeModel->getShadowLane() != 0);
7011 24595 : return (lane->getRightSideOnEdge() + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] + 0.5 * lane->getWidth()
7012 24595 : + (myLane->getCenterOnEdge() - myLaneChangeModel->getShadowLane()->getCenterOnEdge()));
7013 349 : } else if (shadowFurther[i]->getBidiLane() == lane) {
7014 : assert(myLaneChangeModel->getShadowLane() != 0);
7015 0 : return (lane->getRightSideOnEdge() - myLaneChangeModel->getShadowFurtherLanesPosLat()[i] + 0.5 * lane->getWidth()
7016 0 : + (myLane->getCenterOnEdge() - myLaneChangeModel->getShadowLane()->getCenterOnEdge()));
7017 : }
7018 : }
7019 : assert(false);
7020 0 : throw ProcessError("Request lateral pos of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
7021 : }
7022 : }
7023 :
7024 :
7025 : double
7026 3352905034 : MSVehicle::getLatOffset(const MSLane* lane) const {
7027 : assert(lane != 0);
7028 3352905034 : if (&lane->getEdge() == &myLane->getEdge()) {
7029 3301003812 : return myLane->getRightSideOnEdge() - lane->getRightSideOnEdge();
7030 51901222 : } else if (myLane->getParallelOpposite() == lane) {
7031 2124994 : return (myLane->getWidth() + lane->getWidth()) * 0.5 - 2 * getLateralPositionOnLane();
7032 49776228 : } else if (myLane->getBidiLane() == lane) {
7033 789670 : return -2 * getLateralPositionOnLane();
7034 : } else {
7035 : // Check whether the lane is a further lane for the vehicle
7036 55410989 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
7037 54356657 : if (myFurtherLanes[i] == lane) {
7038 : #ifdef DEBUG_FURTHER
7039 : if (DEBUG_COND) {
7040 : std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherLat=" << myFurtherLanesPosLat[i] << "\n";
7041 : }
7042 : #endif
7043 47742861 : return myFurtherLanesPosLat[i] - myState.myPosLat;
7044 6613796 : } else if (myFurtherLanes[i]->getBidiLane() == lane) {
7045 : #ifdef DEBUG_FURTHER
7046 : if (DEBUG_COND) {
7047 : std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherBidiLat=" << myFurtherLanesPosLat[i] << "\n";
7048 : }
7049 : #endif
7050 189365 : return -2 * (myFurtherLanesPosLat[i] - myState.myPosLat);
7051 : }
7052 : }
7053 : #ifdef DEBUG_FURTHER
7054 : if (DEBUG_COND) {
7055 : std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
7056 : }
7057 : #endif
7058 : // Check whether the lane is a "shadow further lane" for the vehicle
7059 1054332 : const std::vector<MSLane*>& shadowFurther = myLaneChangeModel->getShadowFurtherLanes();
7060 1068220 : for (int i = 0; i < (int)shadowFurther.size(); ++i) {
7061 1063888 : if (shadowFurther[i] == lane) {
7062 : #ifdef DEBUG_FURTHER
7063 : if (DEBUG_COND) std::cout << " getLatOffset veh=" << getID()
7064 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
7065 : << " lane=" << lane->getID()
7066 : << " i=" << i
7067 : << " posLat=" << myState.myPosLat
7068 : << " shadowPosLat=" << getLatOffset(myLaneChangeModel->getShadowLane())
7069 : << " shadowFurtherLat=" << myLaneChangeModel->getShadowFurtherLanesPosLat()[i]
7070 : << "\n";
7071 : #endif
7072 1049947 : return getLatOffset(myLaneChangeModel->getShadowLane()) + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] - myState.myPosLat;
7073 13941 : } else if (shadowFurther[i]->getBidiLane() == lane) {
7074 : #ifdef DEBUG_FURTHER
7075 : if (DEBUG_COND) {
7076 : std::cout << " getLatOffset veh=" << getID() << " shadowbidilane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherBidiLat=" << myFurtherLanesPosLat[i] << "\n";
7077 : }
7078 : #endif
7079 53 : return -2 * getLatOffset(myLaneChangeModel->getShadowLane()) + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] - myState.myPosLat;
7080 : }
7081 : }
7082 : // Check whether the vehicle issued a maneuverReservation on the lane.
7083 4332 : const std::vector<MSLane*>& furtherTargets = myLaneChangeModel->getFurtherTargetLanes();
7084 6410 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
7085 : // Further target lanes are just neighboring lanes of the vehicle's further lanes, @see MSAbstractLaneChangeModel::updateTargetLane()
7086 6409 : MSLane* targetLane = furtherTargets[i];
7087 6409 : if (targetLane == lane) {
7088 4331 : const double targetDir = myLaneChangeModel->getManeuverDist() < 0 ? -1. : 1.;
7089 4331 : const double latOffset = myFurtherLanesPosLat[i] - myState.myPosLat + targetDir * 0.5 * (myFurtherLanes[i]->getWidth() + targetLane->getWidth());
7090 : #ifdef DEBUG_TARGET_LANE
7091 : if (DEBUG_COND) {
7092 : std::cout << " getLatOffset veh=" << getID()
7093 : << " wrt targetLane=" << Named::getIDSecure(myLaneChangeModel->getTargetLane())
7094 : << "\n i=" << i
7095 : << " posLat=" << myState.myPosLat
7096 : << " furtherPosLat=" << myFurtherLanesPosLat[i]
7097 : << " maneuverDist=" << myLaneChangeModel->getManeuverDist()
7098 : << " targetDir=" << targetDir
7099 : << " latOffset=" << latOffset
7100 : << std::endl;
7101 : }
7102 : #endif
7103 4331 : return latOffset;
7104 2078 : } else if (targetLane != nullptr && targetLane->getBidiLane() == lane) {
7105 0 : const double targetDir = myLaneChangeModel->getManeuverDist() < 0 ? -1. : 1.;
7106 0 : const double latOffset = myFurtherLanesPosLat[i] - myState.myPosLat + targetDir * 0.5 * (myFurtherLanes[i]->getWidth() + targetLane->getWidth());
7107 : #ifdef DEBUG_FURTHER
7108 : if (DEBUG_COND) {
7109 : std::cout << " getLatOffset veh=" << getID() << " furthertargetbidilane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherBidiLat=" << myFurtherLanesPosLat[i] << "\n";
7110 : }
7111 : #endif
7112 0 : return -2 * latOffset;
7113 : }
7114 : }
7115 : assert(false);
7116 6 : throw ProcessError("Request lateral offset of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
7117 : }
7118 : }
7119 :
7120 :
7121 : double
7122 35845956 : MSVehicle::lateralDistanceToLane(const int offset) const {
7123 : // compute the distance when changing to the neighboring lane
7124 : // (ensure we do not lap into the line behind neighLane since there might be unseen blockers)
7125 : assert(offset == 0 || offset == 1 || offset == -1);
7126 : assert(myLane != nullptr);
7127 : assert(myLane->getParallelLane(offset) != nullptr || myLane->getParallelOpposite() != nullptr);
7128 35845956 : const double halfCurrentLaneWidth = 0.5 * myLane->getWidth();
7129 35845956 : const double halfVehWidth = 0.5 * (getWidth() + NUMERICAL_EPS);
7130 35845956 : const double latPos = getLateralPositionOnLane();
7131 35845956 : const double oppositeSign = getLaneChangeModel().isOpposite() ? -1 : 1;
7132 35845956 : double leftLimit = halfCurrentLaneWidth - halfVehWidth - oppositeSign * latPos;
7133 35845956 : double rightLimit = -halfCurrentLaneWidth + halfVehWidth - oppositeSign * latPos;
7134 : double latLaneDist = 0; // minimum distance to move the vehicle fully onto the new lane
7135 35845956 : if (offset == 0) {
7136 8 : if (latPos + halfVehWidth > halfCurrentLaneWidth) {
7137 : // correct overlapping left
7138 4 : latLaneDist = halfCurrentLaneWidth - latPos - halfVehWidth;
7139 4 : } else if (latPos - halfVehWidth < -halfCurrentLaneWidth) {
7140 : // correct overlapping right
7141 4 : latLaneDist = -halfCurrentLaneWidth - latPos + halfVehWidth;
7142 : }
7143 8 : latLaneDist *= oppositeSign;
7144 35845948 : } else if (offset == -1) {
7145 16193067 : latLaneDist = rightLimit - (getWidth() + NUMERICAL_EPS);
7146 19652881 : } else if (offset == 1) {
7147 19652881 : latLaneDist = leftLimit + (getWidth() + NUMERICAL_EPS);
7148 : }
7149 : #ifdef DEBUG_ACTIONSTEPS
7150 : if (DEBUG_COND) {
7151 : std::cout << SIMTIME
7152 : << " veh=" << getID()
7153 : << " halfCurrentLaneWidth=" << halfCurrentLaneWidth
7154 : << " halfVehWidth=" << halfVehWidth
7155 : << " latPos=" << latPos
7156 : << " latLaneDist=" << latLaneDist
7157 : << " leftLimit=" << leftLimit
7158 : << " rightLimit=" << rightLimit
7159 : << "\n";
7160 : }
7161 : #endif
7162 35845956 : return latLaneDist;
7163 : }
7164 :
7165 :
7166 : double
7167 5185442475 : MSVehicle::getLateralOverlap(double posLat, const MSLane* lane) const {
7168 5185442475 : return (fabs(posLat) + 0.5 * getVehicleType().getWidth()
7169 5185442475 : - 0.5 * lane->getWidth());
7170 : }
7171 :
7172 : double
7173 0 : MSVehicle::getLateralOverlap(const MSLane* lane) const {
7174 0 : return getLateralOverlap(getLateralPositionOnLane(), lane);
7175 : }
7176 :
7177 : double
7178 4988926168 : MSVehicle::getLateralOverlap() const {
7179 4988926168 : return getLateralOverlap(getLateralPositionOnLane(), myLane);
7180 : }
7181 :
7182 :
7183 : void
7184 646084741 : MSVehicle::removeApproachingInformation(const DriveItemVector& lfLinks) const {
7185 1883622877 : for (const DriveProcessItem& dpi : lfLinks) {
7186 1237538136 : if (dpi.myLink != nullptr) {
7187 880870759 : dpi.myLink->removeApproaching(this);
7188 : }
7189 : }
7190 : // unregister on all shadow links
7191 646084741 : myLaneChangeModel->removeShadowApproachingInformation();
7192 646084741 : }
7193 :
7194 :
7195 : bool
7196 842727 : MSVehicle::unsafeLinkAhead(const MSLane* lane, double zipperDist) const {
7197 : // the following links are unsafe:
7198 : // - zipper links if they are close enough and have approaching vehicles in the relevant time range
7199 : // - unprioritized links if the vehicle is currently approaching a prioritzed link and unable to stop in time
7200 842727 : double seen = myLane->getLength() - getPositionOnLane();
7201 842727 : const double dist = MAX2(zipperDist, getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0));
7202 842727 : if (seen < dist) {
7203 71117 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(lane);
7204 : int view = 1;
7205 71117 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
7206 : DriveItemVector::const_iterator di = myLFLinkLanes.begin();
7207 118331 : while (!lane->isLinkEnd(link) && seen <= dist) {
7208 71825 : if ((!lane->isInternal()
7209 49270 : && (((*link)->getState() == LINKSTATE_ZIPPER && seen < (*link)->getFoeVisibilityDistance())
7210 26697 : || !(*link)->havePriority()))
7211 98286 : || (lane->isInternal() && zipperDist > 0)) {
7212 : // find the drive item corresponding to this link
7213 : bool found = false;
7214 52242 : while (di != myLFLinkLanes.end() && !found) {
7215 27066 : if ((*di).myLink != nullptr) {
7216 : const MSLane* diPredLane = (*di).myLink->getLaneBefore();
7217 27062 : if (diPredLane != nullptr) {
7218 27062 : if (&diPredLane->getEdge() == &lane->getEdge()) {
7219 : found = true;
7220 : }
7221 : }
7222 : }
7223 27066 : if (!found) {
7224 : di++;
7225 : }
7226 : }
7227 25176 : if (found) {
7228 25172 : const SUMOTime leaveTime = (*link)->getLeaveTime((*di).myArrivalTime, (*di).myArrivalSpeed,
7229 25172 : (*di).getLeaveSpeed(), getVehicleType().getLength());
7230 25172 : const MSLink* entry = (*link)->getCorrespondingEntryLink();
7231 : //if (DEBUG_COND) {
7232 : // 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";
7233 : //}
7234 25172 : if (entry->hasApproachingFoe((*di).myArrivalTime, leaveTime, (*di).myArrivalSpeed, getCarFollowModel().getMaxDecel())) {
7235 : //std::cout << SIMTIME << " veh=" << getID() << " aborting changeTo=" << Named::getIDSecure(bestLaneConts.front()) << " linkState=" << toString((*link)->getState()) << " seen=" << seen << " dist=" << dist << "\n";
7236 : return true;
7237 : }
7238 : }
7239 : // no drive item is found if the vehicle aborts its request within dist
7240 : }
7241 47214 : lane = (*link)->getViaLaneOrLane();
7242 47214 : if (!lane->getEdge().isInternal()) {
7243 24522 : view++;
7244 : }
7245 47214 : seen += lane->getLength();
7246 47214 : link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
7247 : }
7248 : }
7249 : return false;
7250 : }
7251 :
7252 :
7253 : PositionVector
7254 6641712 : MSVehicle::getBoundingBox(double offset) const {
7255 6641712 : PositionVector centerLine;
7256 6641712 : Position pos = getPosition();
7257 6641712 : centerLine.push_back(pos);
7258 6641712 : switch (myType->getGuiShape()) {
7259 12386 : case SUMOVehicleShape::BUS_FLEXIBLE:
7260 : case SUMOVehicleShape::RAIL:
7261 : case SUMOVehicleShape::RAIL_CAR:
7262 : case SUMOVehicleShape::RAIL_CARGO:
7263 : case SUMOVehicleShape::TRUCK_SEMITRAILER:
7264 : case SUMOVehicleShape::TRUCK_1TRAILER: {
7265 26365 : for (MSLane* lane : myFurtherLanes) {
7266 13979 : centerLine.push_back(lane->getShape().back());
7267 : }
7268 : break;
7269 : }
7270 : default:
7271 : break;
7272 : }
7273 6641712 : double l = getLength();
7274 6641712 : Position backPos = getBackPosition();
7275 6641712 : if (pos.distanceTo2D(backPos) > l + NUMERICAL_EPS) {
7276 : // getBackPosition may not match the visual back in networks without internal lanes
7277 352447 : double a = getAngle() + M_PI; // angle pointing backwards
7278 352447 : backPos = pos + Position(l * cos(a), l * sin(a));
7279 : }
7280 6641712 : centerLine.push_back(backPos);
7281 6641712 : if (offset != 0) {
7282 6543 : centerLine.extrapolate2D(offset);
7283 : }
7284 : PositionVector result = centerLine;
7285 13279134 : result.move2side(MAX2(0.0, 0.5 * myType->getWidth() + offset));
7286 13279134 : centerLine.move2side(MIN2(0.0, -0.5 * myType->getWidth() - offset));
7287 6641712 : result.append(centerLine.reverse(), POSITION_EPS);
7288 6641712 : return result;
7289 6641712 : }
7290 :
7291 :
7292 : PositionVector
7293 66454 : MSVehicle::getBoundingPoly(double offset) const {
7294 66454 : switch (myType->getGuiShape()) {
7295 66044 : case SUMOVehicleShape::PASSENGER:
7296 : case SUMOVehicleShape::PASSENGER_SEDAN:
7297 : case SUMOVehicleShape::PASSENGER_HATCHBACK:
7298 : case SUMOVehicleShape::PASSENGER_WAGON:
7299 : case SUMOVehicleShape::PASSENGER_VAN: {
7300 : // box with corners cut off
7301 66044 : PositionVector result;
7302 66044 : PositionVector centerLine;
7303 66044 : centerLine.push_back(getPosition());
7304 66044 : centerLine.push_back(getBackPosition());
7305 66044 : if (offset != 0) {
7306 1600 : centerLine.extrapolate2D(offset);
7307 : }
7308 : PositionVector line1 = centerLine;
7309 : PositionVector line2 = centerLine;
7310 132088 : line1.move2side(MAX2(0.0, 0.3 * myType->getWidth() + offset));
7311 132088 : line2.move2side(MAX2(0.0, 0.5 * myType->getWidth() + offset));
7312 66044 : line2.scaleRelative(0.8);
7313 66044 : result.push_back(line1[0]);
7314 66044 : result.push_back(line2[0]);
7315 66044 : result.push_back(line2[1]);
7316 66044 : result.push_back(line1[1]);
7317 132088 : line1.move2side(MIN2(0.0, -0.6 * myType->getWidth() - offset));
7318 132088 : line2.move2side(MIN2(0.0, -1.0 * myType->getWidth() - offset));
7319 66044 : result.push_back(line1[1]);
7320 66044 : result.push_back(line2[1]);
7321 66044 : result.push_back(line2[0]);
7322 66044 : result.push_back(line1[0]);
7323 : return result;
7324 66044 : }
7325 410 : default:
7326 410 : return getBoundingBox();
7327 : }
7328 : }
7329 :
7330 :
7331 : bool
7332 5528250 : MSVehicle::onFurtherEdge(const MSEdge* edge) const {
7333 6044896 : for (std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
7334 865826 : if (&(*i)->getEdge() == edge) {
7335 : return true;
7336 : }
7337 : }
7338 : return false;
7339 : }
7340 :
7341 :
7342 : bool
7343 7767924033 : MSVehicle::isBidiOn(const MSLane* lane) const {
7344 7775149063 : return lane->getBidiLane() != nullptr && (
7345 7225030 : myLane == lane->getBidiLane()
7346 5528250 : || onFurtherEdge(&lane->getBidiLane()->getEdge()));
7347 : }
7348 :
7349 :
7350 : bool
7351 16 : MSVehicle::rerouteParkingArea(const std::string& parkingAreaID, std::string& errorMsg) {
7352 : // this function is based on MSTriggeredRerouter::rerouteParkingArea in order to keep
7353 : // consistency in the behaviour.
7354 :
7355 : // get vehicle params
7356 16 : MSParkingArea* destParkArea = getNextParkingArea();
7357 16 : const MSRoute& route = getRoute();
7358 16 : const MSEdge* lastEdge = route.getLastEdge();
7359 :
7360 16 : if (destParkArea == nullptr) {
7361 : // not driving towards a parking area
7362 0 : errorMsg = "Vehicle " + getID() + " is not driving to a parking area so it cannot be rerouted.";
7363 0 : return false;
7364 : }
7365 :
7366 : // if the current route ends at the parking area, the new route will also and at the new area
7367 16 : bool newDestination = (&destParkArea->getLane().getEdge() == route.getLastEdge()
7368 8 : && getArrivalPos() >= destParkArea->getBeginLanePosition()
7369 24 : && getArrivalPos() <= destParkArea->getEndLanePosition());
7370 :
7371 : // retrieve info on the new parking area
7372 16 : MSParkingArea* newParkingArea = (MSParkingArea*) MSNet::getInstance()->getStoppingPlace(
7373 : parkingAreaID, SumoXMLTag::SUMO_TAG_PARKING_AREA);
7374 :
7375 16 : if (newParkingArea == nullptr) {
7376 0 : errorMsg = "Parking area ID " + toString(parkingAreaID) + " not found in the network.";
7377 0 : return false;
7378 : }
7379 :
7380 16 : const MSEdge* newEdge = &(newParkingArea->getLane().getEdge());
7381 16 : SUMOAbstractRouter<MSEdge, SUMOVehicle>& router = getRouterTT();
7382 :
7383 : // Compute the route from the current edge to the parking area edge
7384 : ConstMSEdgeVector edgesToPark;
7385 16 : router.compute(getEdge(), getPositionOnLane(), newEdge, newParkingArea->getEndLanePosition(), this, MSNet::getInstance()->getCurrentTimeStep(), edgesToPark);
7386 :
7387 : // Compute the route from the parking area edge to the end of the route
7388 : ConstMSEdgeVector edgesFromPark;
7389 16 : if (!newDestination) {
7390 12 : router.compute(newEdge, lastEdge, this, MSNet::getInstance()->getCurrentTimeStep(), edgesFromPark);
7391 : } else {
7392 : // adapt plans of any riders
7393 8 : for (MSTransportable* p : getPersons()) {
7394 4 : p->rerouteParkingArea(getNextParkingArea(), newParkingArea);
7395 : }
7396 : }
7397 :
7398 : // we have a new destination, let's replace the vehicle route
7399 16 : ConstMSEdgeVector edges = edgesToPark;
7400 16 : if (edgesFromPark.size() > 0) {
7401 12 : edges.insert(edges.end(), edgesFromPark.begin() + 1, edgesFromPark.end());
7402 : }
7403 :
7404 16 : if (newDestination && getParameter().arrivalPosProcedure != ArrivalPosDefinition::DEFAULT) {
7405 4 : SUMOVehicleParameter* newParameter = new SUMOVehicleParameter();
7406 4 : *newParameter = getParameter();
7407 4 : newParameter->arrivalPosProcedure = ArrivalPosDefinition::GIVEN;
7408 4 : newParameter->arrivalPos = newParkingArea->getEndLanePosition();
7409 4 : replaceParameter(newParameter);
7410 : }
7411 16 : const double routeCost = router.recomputeCosts(edges, this, MSNet::getInstance()->getCurrentTimeStep());
7412 16 : ConstMSEdgeVector prevEdges(myCurrEdge, myRoute->end());
7413 16 : const double savings = router.recomputeCosts(prevEdges, this, MSNet::getInstance()->getCurrentTimeStep());
7414 16 : if (replaceParkingArea(newParkingArea, errorMsg)) {
7415 16 : const bool onInit = myLane == nullptr;
7416 32 : replaceRouteEdges(edges, routeCost, savings, "TraCI:" + toString(SUMO_TAG_PARKING_AREA_REROUTE), onInit, false, false);
7417 : } else {
7418 0 : WRITE_WARNINGF("Vehicle '%' could not reroute to new parkingArea '%' reason=%, time=%.",
7419 : getID(), newParkingArea->getID(), errorMsg, time2string(SIMSTEP));
7420 0 : return false;
7421 : }
7422 16 : return true;
7423 16 : }
7424 :
7425 :
7426 : bool
7427 46837 : MSVehicle::addTraciStop(SUMOVehicleParameter::Stop stop, std::string& errorMsg) {
7428 46837 : const int numStops = (int)myStops.size();
7429 46837 : const bool result = MSBaseVehicle::addTraciStop(stop, errorMsg);
7430 46837 : if (myLane != nullptr && numStops != (int)myStops.size()) {
7431 45200 : updateBestLanes(true);
7432 : }
7433 46837 : return result;
7434 : }
7435 :
7436 :
7437 : bool
7438 3257 : MSVehicle::handleCollisionStop(MSStop& stop, const double distToStop) {
7439 3257 : if (myCurrEdge == stop.edge && distToStop + POSITION_EPS < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0)) {
7440 1454 : if (distToStop < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)) {
7441 1040 : double vNew = getCarFollowModel().maximumSafeStopSpeed(distToStop, getCarFollowModel().getMaxDecel(), getSpeed(), false, 0);
7442 : //std::cout << SIMTIME << " veh=" << getID() << " v=" << myState.mySpeed << " distToStop=" << distToStop
7443 : // << " vMinNex=" << getCarFollowModel().minNextSpeed(getSpeed(), this)
7444 : // << " bg1=" << getCarFollowModel().brakeGap(myState.mySpeed)
7445 : // << " bg2=" << getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)
7446 : // << " vNew=" << vNew
7447 : // << "\n";
7448 1040 : myState.mySpeed = MIN2(myState.mySpeed, vNew + ACCEL2SPEED(getCarFollowModel().getEmergencyDecel()));
7449 1040 : myState.myPos = MIN2(myState.myPos, stop.pars.endPos);
7450 1040 : myCachedPosition = Position::INVALID;
7451 1040 : if (myState.myPos < myType->getLength()) {
7452 449 : computeFurtherLanes(myLane, myState.myPos, true);
7453 449 : myAngle = computeAngle();
7454 449 : if (myLaneChangeModel->isOpposite()) {
7455 0 : myAngle += M_PI;
7456 : }
7457 : }
7458 : }
7459 : }
7460 3257 : return true;
7461 : }
7462 :
7463 :
7464 : bool
7465 24283020 : MSVehicle::resumeFromStopping() {
7466 24283020 : if (isStopped()) {
7467 49572 : if (myAmRegisteredAsWaiting) {
7468 750 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
7469 750 : myAmRegisteredAsWaiting = false;
7470 : }
7471 : MSStop& stop = myStops.front();
7472 : // we have waited long enough and fulfilled any passenger-requirements
7473 49572 : if (stop.busstop != nullptr) {
7474 : // inform bus stop about leaving it
7475 18750 : stop.busstop->leaveFrom(this);
7476 : }
7477 : // we have waited long enough and fulfilled any container-requirements
7478 49572 : if (stop.containerstop != nullptr) {
7479 : // inform container stop about leaving it
7480 530 : stop.containerstop->leaveFrom(this);
7481 : }
7482 49572 : if (stop.parkingarea != nullptr && stop.getSpeed() <= 0) {
7483 : // inform parking area about leaving it
7484 8433 : stop.parkingarea->leaveFrom(this);
7485 : }
7486 49572 : if (stop.chargingStation != nullptr) {
7487 : // inform charging station about leaving it
7488 3583 : stop.chargingStation->leaveFrom(this);
7489 : }
7490 : // the current stop is no longer valid
7491 49572 : myLane->getEdge().removeWaiting(this);
7492 : // MSStopOut needs to know whether the stop had a loaded 'ended' value so we call this before replacing the value
7493 49572 : if (stop.pars.started == -1) {
7494 : // waypoint edge was passed in a single step
7495 334 : stop.pars.started = MSNet::getInstance()->getCurrentTimeStep();
7496 : }
7497 49572 : if (MSStopOut::active()) {
7498 4263 : MSStopOut::getInstance()->stopEnded(this, stop);
7499 : }
7500 49572 : stop.pars.ended = MSNet::getInstance()->getCurrentTimeStep();
7501 112814 : for (const auto& rem : myMoveReminders) {
7502 63242 : rem.first->notifyStopEnded();
7503 : }
7504 49572 : if (stop.pars.collision && MSLane::getCollisionAction() == MSLane::COLLISION_ACTION_WARN) {
7505 393 : myCollisionImmunity = TIME2STEPS(5); // leave the conflict area
7506 : }
7507 49572 : if (stop.pars.posLat != INVALID_DOUBLE && MSGlobals::gLateralResolution <= 0) {
7508 : // reset lateral position to default
7509 195 : myState.myPosLat = 0;
7510 : }
7511 49572 : const bool wasWaypoint = stop.getSpeed() > 0;
7512 49572 : myPastStops.push_back(stop.pars);
7513 49572 : myPastStops.back().routeIndex = (int)(stop.edge - myRoute->begin());
7514 49572 : myStops.pop_front();
7515 49572 : myStopDist = std::numeric_limits<double>::max();
7516 : // do not count the stopping time towards gridlock time.
7517 : // Other outputs use an independent counter and are not affected.
7518 49572 : myWaitingTime = 0;
7519 49572 : myStopSpeed = getCarFollowModel().maxNextSpeed(getSpeed(), this);
7520 : // maybe the next stop is on the same edge; let's rebuild best lanes
7521 49572 : updateBestLanes(true);
7522 : // continue as wished...
7523 49572 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::ENDING_STOP);
7524 49572 : MSNet::getInstance()->getVehicleControl().registerStopEnded();
7525 49572 : return !wasWaypoint;
7526 : }
7527 : return false;
7528 : }
7529 :
7530 :
7531 : MSVehicle::Influencer&
7532 4564669 : MSVehicle::getInfluencer() {
7533 4564669 : if (myInfluencer == nullptr) {
7534 3491 : myInfluencer = new Influencer();
7535 : }
7536 4564669 : return *myInfluencer;
7537 : }
7538 :
7539 : MSVehicle::BaseInfluencer&
7540 24 : MSVehicle::getBaseInfluencer() {
7541 24 : return getInfluencer();
7542 : }
7543 :
7544 :
7545 : const MSVehicle::Influencer*
7546 0 : MSVehicle::getInfluencer() const {
7547 0 : return myInfluencer;
7548 : }
7549 :
7550 : const MSVehicle::BaseInfluencer*
7551 237334 : MSVehicle::getBaseInfluencer() const {
7552 237334 : return myInfluencer;
7553 : }
7554 :
7555 :
7556 : double
7557 4078 : MSVehicle::getSpeedWithoutTraciInfluence() const {
7558 4078 : if (myInfluencer != nullptr && myInfluencer->getOriginalSpeed() >= 0) {
7559 : // influencer original speed is -1 on initialization
7560 1655 : return myInfluencer->getOriginalSpeed();
7561 : }
7562 2423 : return myState.mySpeed;
7563 : }
7564 :
7565 :
7566 : int
7567 998799076 : MSVehicle::influenceChangeDecision(int state) {
7568 998799076 : if (hasInfluencer()) {
7569 2827546 : state = getInfluencer().influenceChangeDecision(
7570 : MSNet::getInstance()->getCurrentTimeStep(),
7571 2827546 : myLane->getEdge(),
7572 : getLaneIndex(),
7573 : state);
7574 : }
7575 998799076 : return state;
7576 : }
7577 :
7578 :
7579 : void
7580 7330 : MSVehicle::setRemoteState(Position xyPos) {
7581 7330 : myCachedPosition = xyPos;
7582 7330 : }
7583 :
7584 :
7585 : bool
7586 797097309 : MSVehicle::isRemoteControlled() const {
7587 797097309 : return myInfluencer != nullptr && myInfluencer->isRemoteControlled();
7588 : }
7589 :
7590 :
7591 : bool
7592 20565 : MSVehicle::wasRemoteControlled(SUMOTime lookBack) const {
7593 20565 : return myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() + lookBack >= MSNet::getInstance()->getCurrentTimeStep();
7594 : }
7595 :
7596 :
7597 : bool
7598 540408475 : MSVehicle::keepClear(const MSLink* link) const {
7599 540408475 : if (link->hasFoes() && link->keepClear() /* && item.myLink->willHaveBlockedFoe()*/) {
7600 174607443 : const double keepClearTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_KEEPCLEAR_TIME, -1);
7601 : //std::cout << SIMTIME << " veh=" << getID() << " keepClearTime=" << keepClearTime << " accWait=" << getAccumulatedWaitingSeconds() << " keepClear=" << (keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime) << "\n";
7602 176029288 : return keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime;
7603 : } else {
7604 : return false;
7605 : }
7606 : }
7607 :
7608 :
7609 : bool
7610 733175647 : MSVehicle::ignoreRed(const MSLink* link, bool canBrake) const {
7611 733175647 : if ((myInfluencer != nullptr && !myInfluencer->getEmergencyBrakeRedLight())) {
7612 : return true;
7613 : }
7614 732868483 : const double ignoreRedTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
7615 : #ifdef DEBUG_IGNORE_RED
7616 : if (DEBUG_COND) {
7617 : std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID() << " state=" << toString(link->getState()) << "\n";
7618 : }
7619 : #endif
7620 732868483 : if (ignoreRedTime < 0) {
7621 732863084 : const double ignoreYellowTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, 0);
7622 732863084 : if (ignoreYellowTime > 0 && link->haveYellow()) {
7623 : assert(link->getTLLogic() != 0);
7624 52 : const double yellowDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
7625 : // when activating ignoreYellow behavior, vehicles will drive if they cannot brake
7626 92 : return !canBrake || ignoreYellowTime > yellowDuration;
7627 : } else {
7628 : return false;
7629 : }
7630 5399 : } else if (link->haveYellow()) {
7631 : // always drive at yellow when ignoring red
7632 : return true;
7633 5243 : } else if (link->haveRed()) {
7634 : assert(link->getTLLogic() != 0);
7635 3832 : const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
7636 : #ifdef DEBUG_IGNORE_RED
7637 : if (DEBUG_COND) {
7638 : std::cout
7639 : // << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID()
7640 : << " ignoreRedTime=" << ignoreRedTime
7641 : << " spentRed=" << redDuration
7642 : << " canBrake=" << canBrake << "\n";
7643 : }
7644 : #endif
7645 : // when activating ignoreRed behavior, vehicles will always drive if they cannot brake
7646 6356 : return !canBrake || ignoreRedTime > redDuration;
7647 : } else {
7648 : return false;
7649 : }
7650 : }
7651 :
7652 : bool
7653 1336583666 : MSVehicle::ignoreFoe(const SUMOTrafficObject* foe) const {
7654 1336583666 : if (!getParameter().wasSet(VEHPARS_CFMODEL_PARAMS_SET)) {
7655 : return false;
7656 : }
7657 2548 : for (const std::string& typeID : StringTokenizer(getParameter().getParameter(toString(SUMO_ATTR_CF_IGNORE_TYPES), "")).getVector()) {
7658 398 : if (typeID == foe->getVehicleType().getID()) {
7659 : return true;
7660 : }
7661 1274 : }
7662 2161 : for (const std::string& id : StringTokenizer(getParameter().getParameter(toString(SUMO_ATTR_CF_IGNORE_IDS), "")).getVector()) {
7663 876 : if (id == foe->getID()) {
7664 : return true;
7665 : }
7666 876 : }
7667 409 : return false;
7668 : }
7669 :
7670 : bool
7671 549012195 : MSVehicle::passingMinor() const {
7672 : // either on an internal lane that was entered via minor link
7673 : // or on approach to minor link below visibility distance
7674 549012195 : if (myLane == nullptr) {
7675 : return false;
7676 : }
7677 549012195 : if (myLane->getEdge().isInternal()) {
7678 10293439 : return !myLane->getIncomingLanes().front().viaLink->havePriority();
7679 538718756 : } else if (myLFLinkLanes.size() > 0 && myLFLinkLanes.front().myLink != nullptr) {
7680 : MSLink* link = myLFLinkLanes.front().myLink;
7681 281599649 : return !link->havePriority() && myLFLinkLanes.front().myDistance <= link->getFoeVisibilityDistance();
7682 : }
7683 : return false;
7684 : }
7685 :
7686 : bool
7687 22418063 : MSVehicle::isLeader(const MSLink* link, const MSVehicle* veh, const double gap) const {
7688 : assert(link->fromInternalLane());
7689 22418063 : if (veh == nullptr) {
7690 : return false;
7691 : }
7692 22418063 : if (!myLane->isInternal() || myLane->getEdge().getToJunction() != link->getJunction()) {
7693 : // if this vehicle is not yet on the junction, every vehicle is a leader
7694 : return true;
7695 : }
7696 2387535 : if (veh->getLaneChangeModel().hasBlueLight()) {
7697 : // blue light device automatically gets right of way
7698 : return true;
7699 : }
7700 2387212 : const MSLane* foeLane = veh->getLane();
7701 2387212 : if (foeLane->isInternal()) {
7702 1796078 : if (foeLane->getEdge().getFromJunction() == link->getJunction()) {
7703 1774089 : SUMOTime egoET = myJunctionConflictEntryTime;
7704 1774089 : SUMOTime foeET = veh->myJunctionEntryTime;
7705 : // check relationship between link and foeLane
7706 1774089 : if (foeLane->getNormalPredecessorLane() == link->getInternalLaneBefore()->getNormalPredecessorLane()) {
7707 : // we are entering the junction from the same lane
7708 592885 : egoET = myJunctionEntryTimeNeverYield;
7709 592885 : foeET = veh->myJunctionEntryTimeNeverYield;
7710 592885 : if (link->isExitLinkAfterInternalJunction() && link->getInternalLaneBefore()->getLogicalPredecessorLane()->getEntryLink()->isIndirect()) {
7711 58433 : egoET = myJunctionConflictEntryTime;
7712 : }
7713 : } else {
7714 1181204 : const MSLink* foeLink = foeLane->getIncomingLanes()[0].viaLink;
7715 1181204 : const MSJunctionLogic* logic = link->getJunction()->getLogic();
7716 : assert(logic != nullptr);
7717 : // determine who has right of way
7718 : bool response; // ego response to foe
7719 : bool response2; // foe response to ego
7720 : // attempt 1: tlLinkState
7721 1181204 : const MSLink* entry = link->getCorrespondingEntryLink();
7722 1181204 : const MSLink* foeEntry = foeLink->getCorrespondingEntryLink();
7723 1181204 : if (entry->haveRed() || foeEntry->haveRed()) {
7724 : // ensure that vehicles which are stuck on the intersection may exit
7725 145614 : if (!foeEntry->haveRed() && veh->getSpeed() > SUMO_const_haltingSpeed && gap < 0) {
7726 : // foe might be oncoming, don't drive unless foe can still brake safely
7727 14108 : const double foeNextSpeed = veh->getSpeed() + ACCEL2SPEED(veh->getCarFollowModel().getMaxAccel());
7728 14108 : const double foeBrakeGap = veh->getCarFollowModel().brakeGap(
7729 14108 : foeNextSpeed, veh->getCarFollowModel().getMaxDecel(), veh->getCarFollowModel().getHeadwayTime());
7730 : // the minGap was subtracted from gap in MSLink::getLeaderInfo (enlarging the negative gap)
7731 : // so the -2* makes it point in the right direction
7732 14108 : const double foeGap = -gap - veh->getLength() - 2 * getVehicleType().getMinGap();
7733 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7734 : if (DEBUG_COND) {
7735 : std::cout << " foeGap=" << foeGap << " foeBGap=" << foeBrakeGap << "\n";
7736 :
7737 : }
7738 : #endif
7739 14108 : if (foeGap < foeBrakeGap) {
7740 : response = true;
7741 : response2 = false;
7742 : } else {
7743 : response = false;
7744 : response2 = true;
7745 : }
7746 : } else {
7747 : // let conflict entry time decide
7748 : response = true;
7749 : response2 = true;
7750 : }
7751 1035590 : } else if (entry->havePriority() != foeEntry->havePriority()) {
7752 773885 : response = !entry->havePriority();
7753 773885 : response2 = !foeEntry->havePriority();
7754 261705 : } else if (entry->haveYellow() && foeEntry->haveYellow()) {
7755 : // let the faster vehicle keep moving
7756 6967 : response = veh->getSpeed() >= getSpeed();
7757 6967 : response2 = getSpeed() >= veh->getSpeed();
7758 : } else {
7759 : // fallback if pedestrian crossings are involved
7760 254738 : response = logic->getResponseFor(link->getIndex()).test(foeLink->getIndex());
7761 254738 : response2 = logic->getResponseFor(foeLink->getIndex()).test(link->getIndex());
7762 : }
7763 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7764 : if (DEBUG_COND) {
7765 : std::cout << SIMTIME
7766 : << " foeLane=" << foeLane->getID()
7767 : << " foeLink=" << foeLink->getViaLaneOrLane()->getID()
7768 : << " linkIndex=" << link->getIndex()
7769 : << " foeLinkIndex=" << foeLink->getIndex()
7770 : << " entryState=" << toString(entry->getState())
7771 : << " entryState2=" << toString(foeEntry->getState())
7772 : << " response=" << response
7773 : << " response2=" << response2
7774 : << "\n";
7775 : }
7776 : #endif
7777 1181204 : if (!response) {
7778 : // if we have right of way over the foe, entryTime does not matter
7779 93139 : foeET = veh->myJunctionConflictEntryTime;
7780 93139 : egoET = myJunctionEntryTime;
7781 1088065 : } else if (response && response2) {
7782 : // in a mutual conflict scenario, use entry time to avoid deadlock
7783 157262 : foeET = veh->myJunctionConflictEntryTime;
7784 157262 : egoET = myJunctionConflictEntryTime;
7785 : }
7786 : }
7787 1774089 : if (egoET == foeET) {
7788 : // try to use speed as tie braker
7789 135170 : if (getSpeed() == veh->getSpeed()) {
7790 : // use ID as tie braker
7791 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7792 : if (DEBUG_COND) {
7793 : std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
7794 : << " foeIsLeaderByID=" << (getID() < veh->getID()) << "\n";
7795 : }
7796 : #endif
7797 67377 : return getID() < veh->getID();
7798 : } else {
7799 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7800 : if (DEBUG_COND) {
7801 : std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
7802 : << " foeIsLeaderBySpeed=" << (getSpeed() < veh->getSpeed())
7803 : << " v=" << getSpeed() << " foeV=" << veh->getSpeed()
7804 : << "\n";
7805 : }
7806 : #endif
7807 67793 : return getSpeed() < veh->getSpeed();
7808 : }
7809 : } else {
7810 : // leader was on the junction first
7811 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7812 : if (DEBUG_COND) {
7813 : std::cout << SIMTIME << " veh=" << getID() << " egoET " << egoET << " with foe " << veh->getID()
7814 : << " foeET=" << foeET << " isLeader=" << (egoET > foeET) << "\n";
7815 : }
7816 : #endif
7817 1638919 : return egoET > foeET;
7818 : }
7819 : } else {
7820 : // vehicle can only be partially on the junction. Must be a leader
7821 : return true;
7822 : }
7823 : } else {
7824 : // vehicle can only be partially on the junction. Must be a leader
7825 : return true;
7826 : }
7827 : }
7828 :
7829 : void
7830 2624 : MSVehicle::saveState(OutputDevice& out) {
7831 2624 : MSBaseVehicle::saveState(out);
7832 : // here starts the vehicle internal part (see loading)
7833 : std::vector<std::string> internals;
7834 2624 : internals.push_back(toString(myParameter->parametersSet));
7835 2624 : internals.push_back(toString(myDeparture));
7836 2624 : internals.push_back(toString(distance(myRoute->begin(), myCurrEdge)));
7837 2624 : internals.push_back(toString(myDepartPos));
7838 2624 : internals.push_back(toString(myWaitingTime));
7839 2624 : internals.push_back(toString(myTimeLoss));
7840 2624 : internals.push_back(toString(myLastActionTime));
7841 2624 : internals.push_back(toString(isStopped()));
7842 2624 : internals.push_back(toString(isStopped() ? myStops.front().duration : 0));
7843 2624 : internals.push_back(toString(myPastStops.size()));
7844 2624 : internals.push_back(toString(myJunctionEntryTime));
7845 2624 : internals.push_back(toString(myJunctionConflictEntryTime));
7846 2624 : internals.push_back(toString(myJunctionEntryTimeNeverYield));
7847 2624 : out.writeAttr(SUMO_ATTR_STATE, internals);
7848 2624 : out.writeAttr(SUMO_ATTR_POSITION, std::vector<double> { myState.myPos, myState.myBackPos, myState.myLastCoveredDist });
7849 2624 : out.writeAttr(SUMO_ATTR_SPEED, std::vector<double> { myState.mySpeed, myState.myPreviousSpeed });
7850 2624 : out.writeAttr(SUMO_ATTR_ANGLE, GeomHelper::naviDegree(myAngle));
7851 2624 : out.writeAttr(SUMO_ATTR_POSITION_LAT, myState.myPosLat);
7852 2624 : out.writeAttr(SUMO_ATTR_WAITINGTIME, myWaitingTimeCollector.getState());
7853 2624 : if (isStopped() && myStops.front().entryPos != getPositionOnLane()) {
7854 1 : out.writeAttr(SUMO_ATTR_ENTRYPOS, myStops.front().entryPos);
7855 : }
7856 2624 : myLaneChangeModel->saveState(out);
7857 : // save past stops
7858 5703 : for (SUMOVehicleParameter::Stop stop : myPastStops) {
7859 3079 : stop.write(out, false);
7860 : // do not write started and ended twice
7861 3079 : if ((stop.parametersSet & STOP_STARTED_SET) == 0) {
7862 3074 : out.writeAttr(SUMO_ATTR_STARTED, time2string(stop.started));
7863 : }
7864 3079 : if ((stop.parametersSet & STOP_ENDED_SET) == 0) {
7865 3074 : out.writeAttr(SUMO_ATTR_ENDED, time2string(stop.ended));
7866 : }
7867 3079 : stop.writeParams(out);
7868 3079 : out.closeTag();
7869 3079 : }
7870 : // save upcoming stops
7871 3113 : for (MSStop& stop : myStops) {
7872 489 : stop.write(out);
7873 : }
7874 : // save parameters and device states
7875 2624 : myParameter->writeParams(out);
7876 6617 : for (MSVehicleDevice* const dev : myDevices) {
7877 3993 : dev->saveState(out);
7878 : }
7879 2624 : if (myCFVariables != nullptr) {
7880 110 : myCFVariables->saveState(out, getCarFollowModel());
7881 : }
7882 2624 : out.closeTag();
7883 2624 : }
7884 :
7885 : void
7886 3523 : MSVehicle::loadState(const SUMOSAXAttributes& attrs, const SUMOTime offset) {
7887 3523 : if (!attrs.hasAttribute(SUMO_ATTR_POSITION)) {
7888 0 : throw ProcessError(TL("Error: Invalid vehicles in state (may be a meso state)!"));
7889 : }
7890 : bool ok;
7891 : int routeOffset;
7892 : bool stopped;
7893 : SUMOTime stopDuration;
7894 : int pastStops;
7895 :
7896 3523 : std::istringstream bis(attrs.getString(SUMO_ATTR_STATE));
7897 3523 : bis >> myParameter->parametersSet;
7898 3523 : bis >> myDeparture;
7899 3523 : bis >> routeOffset;
7900 3523 : bis >> myDepartPos;
7901 3523 : bis >> myWaitingTime;
7902 3523 : bis >> myTimeLoss;
7903 3523 : bis >> myLastActionTime;
7904 : bis >> stopped;
7905 : bis >> stopDuration;
7906 3523 : bis >> pastStops;
7907 3523 : bis >> myJunctionEntryTime;
7908 3523 : bis >> myJunctionConflictEntryTime;
7909 3523 : bis >> myJunctionEntryTimeNeverYield;
7910 :
7911 3523 : if (attrs.hasAttribute(SUMO_ATTR_ARRIVALPOS_RANDOMIZED)) {
7912 4 : myArrivalPos = attrs.get<double>(SUMO_ATTR_ARRIVALPOS_RANDOMIZED, getID().c_str(), ok);
7913 : }
7914 : // load stops
7915 : myStops.clear();
7916 3523 : addStops(!MSGlobals::gCheckRoutes, &myCurrEdge, false);
7917 :
7918 3523 : if (hasDeparted()) {
7919 1678 : myCurrEdge = myRoute->begin() + routeOffset;
7920 1678 : myDeparture -= offset;
7921 : // fix stops
7922 4733 : while (pastStops > 0) {
7923 : SUMOVehicleParameter::Stop& pars = const_cast<SUMOVehicleParameter::Stop&>(myStops.front().pars);
7924 : // assumed these attributes were only added to restore vehroute-output.exit-times
7925 3055 : if (!MSGlobals::gUseStopEnded) {
7926 3055 : pars.parametersSet &= ~STOP_ENDED_SET;
7927 : }
7928 3055 : if (!MSGlobals::gUseStopStarted) {
7929 3055 : pars.parametersSet &= ~STOP_STARTED_SET;
7930 : }
7931 6135 : for (const auto& rem : myMoveReminders) {
7932 3080 : rem.first->notifyStopEnded();
7933 : }
7934 3055 : myPastStops.push_back(myStops.front().pars);
7935 3055 : myPastStops.back().routeIndex = (int)(myStops.front().edge - myRoute->begin());
7936 3055 : myStops.pop_front();
7937 3055 : pastStops--;
7938 : }
7939 : // see MSBaseVehicle constructor
7940 1678 : if (myParameter->wasSet(VEHPARS_FORCE_REROUTE)) {
7941 1153 : calculateArrivalParams(true);
7942 : }
7943 : // a (tentative lane is needed for calling hasArrivedInternal
7944 1678 : myLane = (*myCurrEdge)->getLanes()[0];
7945 : }
7946 3523 : if (getActionStepLength() == DELTA_T && !isActionStep(SIMSTEP)) {
7947 1 : myLastActionTime -= (myLastActionTime - SIMSTEP) % DELTA_T;
7948 3 : WRITE_WARNINGF(TL("Action steps are out of sync for loaded vehicle '%'."), getID());
7949 : }
7950 3523 : std::istringstream pis(attrs.getString(SUMO_ATTR_POSITION));
7951 3523 : pis >> myState.myPos >> myState.myBackPos >> myState.myLastCoveredDist;
7952 3523 : std::istringstream sis(attrs.getString(SUMO_ATTR_SPEED));
7953 3523 : sis >> myState.mySpeed >> myState.myPreviousSpeed;
7954 3523 : myAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
7955 3523 : myAngle = GeomHelper::fromNaviDegree(attrs.getFloat(SUMO_ATTR_ANGLE));
7956 3523 : myRawAngle = myAngle;
7957 3523 : myState.myPosLat = attrs.getFloat(SUMO_ATTR_POSITION_LAT);
7958 3523 : std::istringstream dis(attrs.getString(SUMO_ATTR_DISTANCE));
7959 3523 : dis >> myOdometer >> myNumberReroutes;
7960 3523 : myWaitingTimeCollector.setState(attrs.getString(SUMO_ATTR_WAITINGTIME));
7961 3523 : if (stopped) {
7962 234 : double realPos = getPositionOnLane();
7963 234 : double entryPos = attrs.getOpt<double>(SUMO_ATTR_ENTRYPOS, getID().c_str(), ok, realPos);
7964 234 : myStops.front().startedFromState = true;
7965 234 : if (entryPos != realPos) {
7966 1 : myStops.front().entryPos = entryPos;
7967 : }
7968 234 : myLane = const_cast<MSLane*>(myStops.front().lane);
7969 234 : myStopDist = 0;
7970 234 : myState.myPos = entryPos; // fake position for replication stop entry which happened before the position was updated
7971 234 : processNextStop(getSpeed());
7972 234 : myState.myPos = realPos; // reset fake position
7973 234 : if (myStops.front().pars.parking != ParkingType::ONROAD) {
7974 : // processNextStop is called again during MSVehicleTransfer::loadState
7975 216 : stopDuration += getActionStepLength();
7976 : }
7977 234 : myStops.front().duration = stopDuration;
7978 234 : if (!MSGlobals::gUseStopStarted) {
7979 : SUMOVehicleParameter::Stop& pars = const_cast<SUMOVehicleParameter::Stop&>(myStops.front().pars);
7980 234 : pars.parametersSet &= ~STOP_STARTED_SET;
7981 : }
7982 : }
7983 3523 : myLaneChangeModel->loadState(attrs);
7984 : // no need to reset myCachedPosition here since state loading happens directly after creation
7985 3523 : }
7986 :
7987 : void
7988 32 : MSVehicle::loadPreviousApproaching(MSLink* link, bool setRequest,
7989 : SUMOTime arrivalTime, double arrivalSpeed,
7990 : double arrivalSpeedBraking,
7991 : double dist, double leaveSpeed) {
7992 : // ensure that approach information is reset on the next call to setApproachingForAllLinks
7993 32 : myLFLinkLanes.push_back(DriveProcessItem(link, 0, 0, setRequest,
7994 : arrivalTime, arrivalSpeed, arrivalSpeedBraking, dist, leaveSpeed));
7995 :
7996 32 : }
7997 :
7998 :
7999 : std::shared_ptr<MSSimpleDriverState>
8000 2567279 : MSVehicle::getDriverState() const {
8001 2567279 : return myDriverState->getDriverState();
8002 : }
8003 :
8004 :
8005 : double
8006 627605013 : MSVehicle::getFriction() const {
8007 627605013 : return myFrictionDevice == nullptr ? 1. : myFrictionDevice->getMeasuredFriction();
8008 : }
8009 :
8010 :
8011 : void
8012 196 : MSVehicle::setPreviousSpeed(double prevSpeed, double prevAcceleration) {
8013 196 : myState.mySpeed = MAX2(0., prevSpeed);
8014 : // also retcon acceleration
8015 196 : if (prevAcceleration != std::numeric_limits<double>::min()) {
8016 8 : myAcceleration = prevAcceleration;
8017 : } else {
8018 188 : myAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
8019 : }
8020 196 : }
8021 :
8022 :
8023 : double
8024 1908895166 : MSVehicle::getCurrentApparentDecel() const {
8025 : //return MAX2(-myAcceleration, getCarFollowModel().getApparentDecel());
8026 1908895166 : return getCarFollowModel().getApparentDecel();
8027 : }
8028 :
8029 : /****************************************************************************/
8030 : bool
8031 32 : MSVehicle::setExitManoeuvre() {
8032 32 : return (myManoeuvre.configureExitManoeuvre(this));
8033 : }
8034 :
8035 : /* -------------------------------------------------------------------------
8036 : * methods of MSVehicle::manoeuvre
8037 : * ----------------------------------------------------------------------- */
8038 :
8039 4560690 : MSVehicle::Manoeuvre::Manoeuvre() : myManoeuvreStop(""), myManoeuvreStartTime(0), myManoeuvreCompleteTime(0), myManoeuvreType(MSVehicle::MANOEUVRE_NONE), myGUIIncrement(0) {}
8040 :
8041 :
8042 0 : MSVehicle::Manoeuvre::Manoeuvre(const Manoeuvre& manoeuvre) {
8043 0 : myManoeuvreStop = manoeuvre.myManoeuvreStop;
8044 0 : myManoeuvreStartTime = manoeuvre.myManoeuvreStartTime;
8045 0 : myManoeuvreCompleteTime = manoeuvre.myManoeuvreCompleteTime;
8046 0 : myManoeuvreType = manoeuvre.myManoeuvreType;
8047 0 : myGUIIncrement = manoeuvre.myGUIIncrement;
8048 0 : }
8049 :
8050 :
8051 : MSVehicle::Manoeuvre&
8052 0 : MSVehicle::Manoeuvre::operator=(const Manoeuvre& manoeuvre) {
8053 0 : myManoeuvreStop = manoeuvre.myManoeuvreStop;
8054 0 : myManoeuvreStartTime = manoeuvre.myManoeuvreStartTime;
8055 0 : myManoeuvreCompleteTime = manoeuvre.myManoeuvreCompleteTime;
8056 0 : myManoeuvreType = manoeuvre.myManoeuvreType;
8057 0 : myGUIIncrement = manoeuvre.myGUIIncrement;
8058 0 : return *this;
8059 : }
8060 :
8061 :
8062 : bool
8063 0 : MSVehicle::Manoeuvre::operator!=(const Manoeuvre& manoeuvre) {
8064 0 : return (myManoeuvreStop != manoeuvre.myManoeuvreStop ||
8065 0 : myManoeuvreStartTime != manoeuvre.myManoeuvreStartTime ||
8066 0 : myManoeuvreCompleteTime != manoeuvre.myManoeuvreCompleteTime ||
8067 0 : myManoeuvreType != manoeuvre.myManoeuvreType ||
8068 0 : myGUIIncrement != manoeuvre.myGUIIncrement
8069 0 : );
8070 : }
8071 :
8072 :
8073 : double
8074 450 : MSVehicle::Manoeuvre::getGUIIncrement() const {
8075 450 : return (myGUIIncrement);
8076 : }
8077 :
8078 :
8079 : MSVehicle::ManoeuvreType
8080 2971 : MSVehicle::Manoeuvre::getManoeuvreType() const {
8081 2971 : return (myManoeuvreType);
8082 : }
8083 :
8084 :
8085 : MSVehicle::ManoeuvreType
8086 2971 : MSVehicle::getManoeuvreType() const {
8087 2971 : return (myManoeuvre.getManoeuvreType());
8088 : }
8089 :
8090 :
8091 : void
8092 30 : MSVehicle::setManoeuvreType(const MSVehicle::ManoeuvreType mType) {
8093 30 : myManoeuvre.setManoeuvreType(mType);
8094 30 : }
8095 :
8096 :
8097 : void
8098 30 : MSVehicle::Manoeuvre::setManoeuvreType(const MSVehicle::ManoeuvreType mType) {
8099 30 : myManoeuvreType = mType;
8100 30 : }
8101 :
8102 :
8103 : bool
8104 30 : MSVehicle::Manoeuvre::configureEntryManoeuvre(MSVehicle* veh) {
8105 30 : if (!veh->hasStops()) {
8106 : return false; // should never happen - checked before call
8107 : }
8108 :
8109 30 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
8110 30 : const MSStop& stop = veh->getNextStop();
8111 :
8112 30 : int manoeuverAngle = stop.parkingarea->getLastFreeLotAngle();
8113 30 : double GUIAngle = stop.parkingarea->getLastFreeLotGUIAngle();
8114 30 : if (abs(GUIAngle) < 0.1) {
8115 : GUIAngle = -0.1; // Wiggle vehicle on parallel entry
8116 : }
8117 30 : myManoeuvreVehicleID = veh->getID();
8118 30 : myManoeuvreStop = stop.parkingarea->getID();
8119 30 : myManoeuvreType = MSVehicle::MANOEUVRE_ENTRY;
8120 30 : myManoeuvreStartTime = currentTime;
8121 30 : myManoeuvreCompleteTime = currentTime + veh->myType->getEntryManoeuvreTime(manoeuverAngle);
8122 30 : myGUIIncrement = GUIAngle / (STEPS2TIME(myManoeuvreCompleteTime - myManoeuvreStartTime) / TS);
8123 :
8124 : #ifdef DEBUG_STOPS
8125 : if (veh->isSelected()) {
8126 : 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 <<
8127 : " endTime=" << myManoeuvreCompleteTime << " manoeuvre time=" << myManoeuvreCompleteTime - currentTime << " parkArea=" << myManoeuvreStop << std::endl;
8128 : }
8129 : #endif
8130 :
8131 30 : return (true);
8132 : }
8133 :
8134 :
8135 : bool
8136 32 : MSVehicle::Manoeuvre::configureExitManoeuvre(MSVehicle* veh) {
8137 : // At the moment we only want to set for parking areas
8138 32 : if (!veh->hasStops()) {
8139 : return true;
8140 : }
8141 32 : if (veh->getNextStop().parkingarea == nullptr) {
8142 : return true;
8143 : }
8144 :
8145 30 : if (myManoeuvreType != MSVehicle::MANOEUVRE_NONE) {
8146 : return (false);
8147 : }
8148 :
8149 30 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
8150 :
8151 30 : int manoeuverAngle = veh->getCurrentParkingArea()->getManoeuverAngle(*veh);
8152 30 : double GUIAngle = veh->getCurrentParkingArea()->getGUIAngle(*veh);
8153 30 : if (abs(GUIAngle) < 0.1) {
8154 : GUIAngle = 0.1; // Wiggle vehicle on parallel exit
8155 : }
8156 :
8157 30 : myManoeuvreVehicleID = veh->getID();
8158 30 : myManoeuvreStop = veh->getCurrentParkingArea()->getID();
8159 30 : myManoeuvreType = MSVehicle::MANOEUVRE_EXIT;
8160 30 : myManoeuvreStartTime = currentTime;
8161 30 : myManoeuvreCompleteTime = currentTime + veh->myType->getExitManoeuvreTime(manoeuverAngle);
8162 30 : myGUIIncrement = -GUIAngle / (STEPS2TIME(myManoeuvreCompleteTime - myManoeuvreStartTime) / TS);
8163 30 : if (veh->remainingStopDuration() > 0) {
8164 20 : myManoeuvreCompleteTime += veh->remainingStopDuration();
8165 : }
8166 :
8167 : #ifdef DEBUG_STOPS
8168 : if (veh->isSelected()) {
8169 : std::cout << "EXIT manoeuvre start: vehicle=" << veh->getID() << " Manoeuvre Angle=" << manoeuverAngle << " increment=" << RAD2DEG(myGUIIncrement) << " currentTime=" << currentTime
8170 : << " endTime=" << myManoeuvreCompleteTime << " manoeuvre time=" << myManoeuvreCompleteTime - currentTime << " parkArea=" << myManoeuvreStop << std::endl;
8171 : }
8172 : #endif
8173 :
8174 : return (true);
8175 : }
8176 :
8177 :
8178 : bool
8179 222 : MSVehicle::Manoeuvre::entryManoeuvreIsComplete(MSVehicle* veh) {
8180 : // At the moment we only want to consider parking areas - need to check because we could be setting up a manoeuvre
8181 222 : if (!veh->hasStops()) {
8182 : return (true);
8183 : }
8184 : MSStop* currentStop = &veh->myStops.front();
8185 222 : if (currentStop->parkingarea == nullptr) {
8186 : return true;
8187 220 : } else if (currentStop->parkingarea->getID() != myManoeuvreStop || MSVehicle::MANOEUVRE_ENTRY != myManoeuvreType) {
8188 30 : if (configureEntryManoeuvre(veh)) {
8189 30 : MSNet::getInstance()->informVehicleStateListener(veh, MSNet::VehicleState::MANEUVERING);
8190 30 : return (false);
8191 : } else { // cannot configure entry so stop trying
8192 : return true;
8193 : }
8194 190 : } else if (MSNet::getInstance()->getCurrentTimeStep() < myManoeuvreCompleteTime) {
8195 : return false;
8196 : } else { // manoeuvre complete
8197 30 : myManoeuvreType = MSVehicle::MANOEUVRE_NONE;
8198 30 : return true;
8199 : }
8200 : }
8201 :
8202 :
8203 : bool
8204 0 : MSVehicle::Manoeuvre::manoeuvreIsComplete(const ManoeuvreType checkType) const {
8205 0 : if (checkType != myManoeuvreType) {
8206 : return true; // we're not maneuvering / wrong manoeuvre
8207 : }
8208 :
8209 0 : if (MSNet::getInstance()->getCurrentTimeStep() < myManoeuvreCompleteTime) {
8210 : return false;
8211 : } else {
8212 : return true;
8213 : }
8214 : }
8215 :
8216 :
8217 : bool
8218 6266 : MSVehicle::Manoeuvre::manoeuvreIsComplete() const {
8219 6266 : return (MSNet::getInstance()->getCurrentTimeStep() >= myManoeuvreCompleteTime);
8220 : }
8221 :
8222 :
8223 : bool
8224 6266 : MSVehicle::manoeuvreIsComplete() const {
8225 6266 : return (myManoeuvre.manoeuvreIsComplete());
8226 : }
8227 :
8228 :
8229 : std::pair<double, double>
8230 7440 : MSVehicle::estimateTimeToNextStop() const {
8231 7440 : if (hasStops()) {
8232 7440 : MSLane* lane = myLane;
8233 7440 : if (lane == nullptr) {
8234 : // not in network
8235 84 : lane = getEdge()->getLanes()[0];
8236 : }
8237 : const MSStop& stop = myStops.front();
8238 : auto it = myCurrEdge + 1;
8239 : // drive to end of current edge
8240 7440 : double dist = (lane->getLength() - getPositionOnLane());
8241 7440 : double travelTime = lane->getEdge().getMinimumTravelTime(this) * dist / lane->getLength();
8242 : // drive until stop edge
8243 8804 : while (it != myRoute->end() && it < stop.edge) {
8244 1364 : travelTime += (*it)->getMinimumTravelTime(this);
8245 1364 : dist += (*it)->getLength();
8246 : it++;
8247 : }
8248 : // drive up to the stop position
8249 7440 : const double stopEdgeDist = stop.pars.endPos - (lane == stop.lane ? lane->getLength() : 0);
8250 7440 : dist += stopEdgeDist;
8251 7440 : travelTime += stop.lane->getEdge().getMinimumTravelTime(this) * (stopEdgeDist / stop.lane->getLength());
8252 : // estimate time loss due to acceleration and deceleration
8253 : // maximum speed is limited by available distance:
8254 : const double a = getCarFollowModel().getMaxAccel();
8255 : const double b = getCarFollowModel().getMaxDecel();
8256 7440 : const double c = getSpeed();
8257 : const double d = dist;
8258 7440 : const double len = getVehicleType().getLength();
8259 7440 : const double vs = MIN2(MAX2(stop.getSpeed(), 0.0), stop.lane->getVehicleMaxSpeed(this));
8260 : // distAccel = (v - c)^2 / (2a)
8261 : // distDecel = (v + vs)*(v - vs) / 2b = (v^2 - vs^2) / (2b)
8262 : // distAccel + distDecel < d
8263 7440 : 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))))
8264 14592 : + pow((a * vs), 2))))) * 0.5) + (c * b)) / (b + a));
8265 7440 : it = myCurrEdge;
8266 : double v0 = c;
8267 7440 : bool v0Stable = getAcceleration() == 0 && v0 > 0;
8268 : double timeLossAccel = 0;
8269 : double timeLossDecel = 0;
8270 : double timeLossLength = 0;
8271 17742 : while (it != myRoute->end() && it <= stop.edge) {
8272 10302 : double v = MIN2(maxVD, (*it)->getVehicleMaxSpeed(this));
8273 10302 : double edgeLength = (it == stop.edge ? stop.pars.endPos : (*it)->getLength()) - (it == myCurrEdge ? getPositionOnLane() : 0);
8274 10302 : if (edgeLength <= len && v0Stable && v0 < v) {
8275 : const double lengthDist = MIN2(len, edgeLength);
8276 20 : const double dTL = lengthDist / v0 - lengthDist / v;
8277 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " el=" << edgeLength << " lDist=" << lengthDist << " newTLL=" << dTL<< "\n";
8278 20 : timeLossLength += dTL;
8279 : }
8280 10302 : if (edgeLength > len) {
8281 9166 : const double dv = v - v0;
8282 9166 : if (dv > 0) {
8283 : // timeLossAccel = timeAccel - timeMaxspeed = dv / a - distAccel / v
8284 6504 : const double dTA = dv / a - dv * (v + v0) / (2 * a * v);
8285 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLA=" << dTA << "\n";
8286 6504 : timeLossAccel += dTA;
8287 : // time loss from vehicle length
8288 2662 : } else if (dv < 0) {
8289 : // timeLossDecel = timeDecel - timeMaxspeed = dv / b - distDecel / v
8290 540 : const double dTD = -dv / b + dv * (v + v0) / (2 * b * v0);
8291 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLD=" << dTD << "\n";
8292 540 : timeLossDecel += dTD;
8293 : }
8294 : v0 = v;
8295 : v0Stable = true;
8296 : }
8297 : it++;
8298 : }
8299 : // final deceleration to stop (may also be acceleration or deceleration to waypoint speed)
8300 : double v = vs;
8301 7440 : const double dv = v - v0;
8302 7440 : if (dv > 0) {
8303 : // timeLossAccel = timeAccel - timeMaxspeed = dv / a - distAccel / v
8304 144 : const double dTA = dv / a - dv * (v + v0) / (2 * a * v);
8305 : //std::cout << " final e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLA=" << dTA << "\n";
8306 144 : timeLossAccel += dTA;
8307 : // time loss from vehicle length
8308 7296 : } else if (dv < 0) {
8309 : // timeLossDecel = timeDecel - timeMaxspeed = dv / b - distDecel / v
8310 7268 : const double dTD = -dv / b + dv * (v + v0) / (2 * b * v0);
8311 : //std::cout << " final e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLD=" << dTD << "\n";
8312 7268 : timeLossDecel += dTD;
8313 : }
8314 7440 : const double result = travelTime + timeLossAccel + timeLossDecel + timeLossLength;
8315 : //std::cout << SIMTIME << " v=" << c << " a=" << a << " b=" << b << " maxVD=" << maxVD << " tt=" << travelTime
8316 : // << " ta=" << timeLossAccel << " td=" << timeLossDecel << " tl=" << timeLossLength << " res=" << result << "\n";
8317 7440 : return {MAX2(0.0, result), dist};
8318 : } else {
8319 0 : return {INVALID_DOUBLE, INVALID_DOUBLE};
8320 : }
8321 : }
8322 :
8323 :
8324 : double
8325 2457 : MSVehicle::getStopDelay() const {
8326 2457 : if (hasStops() && myStops.front().pars.until >= 0) {
8327 : const MSStop& stop = myStops.front();
8328 1612 : SUMOTime estimatedDepart = MSNet::getInstance()->getCurrentTimeStep() - DELTA_T;
8329 1612 : if (stop.reached) {
8330 802 : return STEPS2TIME(estimatedDepart + stop.duration - stop.pars.until);
8331 : }
8332 810 : if (stop.pars.duration > 0) {
8333 608 : estimatedDepart += stop.pars.duration;
8334 : }
8335 810 : estimatedDepart += TIME2STEPS(estimateTimeToNextStop().first);
8336 810 : const double result = MAX2(0.0, STEPS2TIME(estimatedDepart - stop.pars.until));
8337 810 : return result;
8338 : } else {
8339 : // vehicles cannot drive before 'until' so stop delay can never be
8340 : // negative and we can use -1 to signal "undefined"
8341 : return -1;
8342 : }
8343 : }
8344 :
8345 :
8346 : double
8347 5510 : MSVehicle::getStopArrivalDelay() const {
8348 5510 : if (hasStops() && myStops.front().pars.arrival >= 0) {
8349 : const MSStop& stop = myStops.front();
8350 4334 : if (stop.reached) {
8351 1304 : return STEPS2TIME(stop.pars.started - stop.pars.arrival);
8352 : } else {
8353 3030 : return STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) + estimateTimeToNextStop().first - STEPS2TIME(stop.pars.arrival);
8354 : }
8355 : } else {
8356 : // vehicles can arrive earlier than planned so arrival delay can be negative
8357 : return INVALID_DOUBLE;
8358 : }
8359 : }
8360 :
8361 :
8362 : const MSEdge*
8363 3136805537 : MSVehicle::getCurrentEdge() const {
8364 3136805537 : return myLane != nullptr ? &myLane->getEdge() : getEdge();
8365 : }
8366 :
8367 :
8368 : const MSEdge*
8369 3932 : MSVehicle::getNextEdgePtr() const {
8370 3932 : if (myLane == nullptr || (myCurrEdge + 1) == myRoute->end()) {
8371 8 : return nullptr;
8372 : }
8373 3924 : if (myLane->isInternal()) {
8374 568 : return &myLane->getCanonicalSuccessorLane()->getEdge();
8375 : } else {
8376 3356 : const MSEdge* nextNormal = succEdge(1);
8377 3356 : const MSEdge* nextInternal = myLane->getEdge().getInternalFollowingEdge(nextNormal, getVClass());
8378 3356 : return nextInternal ? nextInternal : nextNormal;
8379 : }
8380 : }
8381 :
8382 :
8383 : const MSLane*
8384 1592 : MSVehicle::getPreviousLane(const MSLane* current, int& furtherIndex) const {
8385 1592 : if (furtherIndex < (int)myFurtherLanes.size()) {
8386 1215 : return myFurtherLanes[furtherIndex++];
8387 : } else {
8388 : // try to use route information
8389 377 : int routeIndex = getRoutePosition();
8390 : bool resultInternal;
8391 377 : if (MSGlobals::gUsingInternalLanes && MSNet::getInstance()->hasInternalLinks()) {
8392 0 : if (myLane->isInternal()) {
8393 0 : if (furtherIndex % 2 == 0) {
8394 0 : routeIndex -= (furtherIndex + 0) / 2;
8395 : resultInternal = false;
8396 : } else {
8397 0 : routeIndex -= (furtherIndex + 1) / 2;
8398 : resultInternal = false;
8399 : }
8400 : } else {
8401 0 : if (furtherIndex % 2 != 0) {
8402 0 : routeIndex -= (furtherIndex + 1) / 2;
8403 : resultInternal = false;
8404 : } else {
8405 0 : routeIndex -= (furtherIndex + 2) / 2;
8406 : resultInternal = true;
8407 : }
8408 : }
8409 : } else {
8410 377 : routeIndex -= furtherIndex;
8411 : resultInternal = false;
8412 : }
8413 377 : furtherIndex++;
8414 377 : if (routeIndex >= 0) {
8415 163 : if (resultInternal) {
8416 0 : const MSEdge* prevNormal = myRoute->getEdges()[routeIndex];
8417 0 : for (MSLane* cand : prevNormal->getLanes()) {
8418 0 : for (MSLink* link : cand->getLinkCont()) {
8419 0 : if (link->getLane() == current) {
8420 0 : if (link->getViaLane() != nullptr) {
8421 : return link->getViaLane();
8422 : } else {
8423 0 : return const_cast<MSLane*>(link->getLaneBefore());
8424 : }
8425 : }
8426 : }
8427 : }
8428 : } else {
8429 163 : return myRoute->getEdges()[routeIndex]->getLanes()[0];
8430 : }
8431 : }
8432 : }
8433 : return current;
8434 : }
8435 :
8436 : SUMOTime
8437 1549099300 : MSVehicle::getWaitingTimeFor(const MSLink* link) const {
8438 : // this vehicle currently has the highest priority on the allway_stop
8439 1549099300 : return link == myHaveStoppedFor ? SUMOTime_MAX : getWaitingTime();
8440 : }
8441 :
8442 :
8443 : void
8444 694 : MSVehicle::resetApproachOnReroute() {
8445 : bool diverged = false;
8446 : const ConstMSEdgeVector& route = myRoute->getEdges();
8447 694 : int ri = getRoutePosition();
8448 2928 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
8449 2234 : if (dpi.myLink != nullptr) {
8450 2231 : if (!diverged) {
8451 1998 : const MSEdge* next = route[ri + 1];
8452 1998 : if (&dpi.myLink->getLane()->getEdge() != next) {
8453 : diverged = true;
8454 : } else {
8455 1932 : if (dpi.myLink->getViaLane() == nullptr) {
8456 : ri++;
8457 : }
8458 : }
8459 : }
8460 : if (diverged) {
8461 299 : dpi.myLink->removeApproaching(this);
8462 : }
8463 : }
8464 : }
8465 694 : }
8466 :
8467 :
8468 : bool
8469 15305013 : MSVehicle::instantStopping() const {
8470 15305013 : return myInfluencer && !myInfluencer->considerMaxDeceleration();
8471 : }
8472 :
8473 : /****************************************************************************/
|
