Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file 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 3115804 : MSVehicle::State::operator=(const State& state) {
153 3115804 : myPos = state.myPos;
154 3115804 : mySpeed = state.mySpeed;
155 3115804 : myPosLat = state.myPosLat;
156 3115804 : myBackPos = state.myBackPos;
157 3115804 : myPreviousSpeed = state.myPreviousSpeed;
158 3115804 : myLastCoveredDist = state.myLastCoveredDist;
159 3115804 : 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 7746951 : MSVehicle::State::State(double pos, double speed, double posLat, double backPos, double previousSpeed) :
175 7746951 : myPos(pos), mySpeed(speed), myPosLat(posLat), myBackPos(backPos), myPreviousSpeed(previousSpeed), myLastCoveredDist(SPEED2DIST(speed)) {}
176 :
177 :
178 :
179 : /* -------------------------------------------------------------------------
180 : * methods of MSVehicle::WaitingTimeCollector
181 : * ----------------------------------------------------------------------- */
182 4631147 : MSVehicle::WaitingTimeCollector::WaitingTimeCollector(SUMOTime memory) : myMemorySize(memory) {}
183 :
184 :
185 : SUMOTime
186 1408008 : MSVehicle::WaitingTimeCollector::cumulatedWaitingTime(SUMOTime memorySpan) const {
187 : assert(memorySpan <= myMemorySize);
188 1408008 : if (memorySpan == -1) {
189 0 : memorySpan = myMemorySize;
190 : }
191 : SUMOTime totalWaitingTime = 0;
192 5885939 : for (const auto& interval : myWaitingIntervals) {
193 4477931 : if (interval.second >= memorySpan) {
194 649281 : if (interval.first >= memorySpan) {
195 : break;
196 : } else {
197 649281 : totalWaitingTime += memorySpan - interval.first;
198 : }
199 : } else {
200 3828650 : totalWaitingTime += interval.second - interval.first;
201 : }
202 : }
203 1408008 : return totalWaitingTime;
204 : }
205 :
206 :
207 : void
208 674121440 : MSVehicle::WaitingTimeCollector::passTime(SUMOTime dt, bool waiting) {
209 : auto i = myWaitingIntervals.begin();
210 : const auto end = myWaitingIntervals.end();
211 674121440 : const bool startNewInterval = i == end || (i->first != 0);
212 1090351649 : while (i != end) {
213 418415101 : i->first += dt;
214 418415101 : if (i->first >= myMemorySize) {
215 : break;
216 : }
217 416230209 : i->second += dt;
218 : i++;
219 : }
220 :
221 : // remove intervals beyond memorySize
222 : auto d = std::distance(i, end);
223 676306332 : while (d > 0) {
224 2184892 : myWaitingIntervals.pop_back();
225 2184892 : d--;
226 : }
227 :
228 674121440 : if (!waiting) {
229 : return;
230 83193228 : } else if (!startNewInterval) {
231 79789113 : myWaitingIntervals.begin()->first = 0;
232 : } else {
233 6808230 : myWaitingIntervals.push_front(std::make_pair(0, dt));
234 : }
235 : return;
236 : }
237 :
238 :
239 : const std::string
240 2485 : MSVehicle::WaitingTimeCollector::getState() const {
241 2485 : std::ostringstream state;
242 2485 : state << myMemorySize << " " << myWaitingIntervals.size();
243 3419 : for (const auto& interval : myWaitingIntervals) {
244 1868 : state << " " << interval.first << " " << interval.second;
245 : }
246 2485 : return state.str();
247 2485 : }
248 :
249 :
250 : void
251 4168 : MSVehicle::WaitingTimeCollector::setState(const std::string& state) {
252 4168 : std::istringstream is(state);
253 : int numIntervals;
254 : SUMOTime begin, end;
255 4168 : is >> myMemorySize >> numIntervals;
256 6938 : while (numIntervals-- > 0) {
257 : is >> begin >> end;
258 2770 : myWaitingIntervals.emplace_back(begin, end);
259 : }
260 4168 : }
261 :
262 :
263 : /* -------------------------------------------------------------------------
264 : * methods of MSVehicle::Influencer::GapControlState
265 : * ----------------------------------------------------------------------- */
266 : void
267 32 : 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 32 : switch (to) {
270 5 : 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 5 : const MSVehicle* msVeh = static_cast<const MSVehicle*>(vehicle);
276 : // std::cout << "GapControlVehStateListener::vehicleStateChanged() vehicle=" << vehicle->getID() << " left the road." << std::endl;
277 5 : if (GapControlState::refVehMap.find(msVeh) != end(GapControlState::refVehMap)) {
278 : // std::cout << "GapControlVehStateListener::deactivating ref vehicle=" << vehicle->getID() << std::endl;
279 5 : GapControlState::refVehMap[msVeh]->deactivate();
280 : }
281 : }
282 5 : break;
283 32 : default:
284 : {};
285 : // do nothing, vehicle still on road
286 : }
287 32 : }
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 59 : MSVehicle::Influencer::GapControlState::GapControlState() :
295 59 : tauOriginal(-1), tauCurrent(-1), tauTarget(-1), addGapCurrent(-1), addGapTarget(-1),
296 59 : remainingDuration(-1), changeRate(-1), maxDecel(-1), referenceVeh(nullptr), active(false), gapAttained(false), prevLeader(nullptr),
297 59 : lastUpdate(-1), timeHeadwayIncrement(0.0), spaceHeadwayIncrement(0.0) {}
298 :
299 :
300 59 : MSVehicle::Influencer::GapControlState::~GapControlState() {
301 59 : deactivate();
302 59 : }
303 :
304 : void
305 59 : MSVehicle::Influencer::GapControlState::init() {
306 59 : if (MSNet::hasInstance()) {
307 59 : if (myVehStateListener == nullptr) {
308 : //std::cout << "GapControlState::init()" << std::endl;
309 59 : myVehStateListener = new GapControlVehStateListener();
310 59 : MSNet::getInstance()->addVehicleStateListener(myVehStateListener);
311 : }
312 : } else {
313 0 : WRITE_ERROR("MSVehicle::Influencer::GapControlState::init(): No MSNet instance found!")
314 : }
315 59 : }
316 :
317 : void
318 32678 : MSVehicle::Influencer::GapControlState::cleanup() {
319 32678 : if (myVehStateListener != nullptr) {
320 59 : MSNet::getInstance()->removeVehicleStateListener(myVehStateListener);
321 59 : delete myVehStateListener;
322 59 : myVehStateListener = nullptr;
323 : }
324 32678 : }
325 :
326 : void
327 59 : MSVehicle::Influencer::GapControlState::activate(double tauOrig, double tauNew, double additionalGap, double dur, double rate, double decel, const MSVehicle* refVeh) {
328 59 : 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 59 : tauOriginal = tauOrig;
334 59 : tauCurrent = tauOrig;
335 59 : tauTarget = tauNew;
336 59 : addGapCurrent = 0.0;
337 59 : addGapTarget = additionalGap;
338 59 : remainingDuration = dur;
339 59 : changeRate = rate;
340 59 : maxDecel = decel;
341 59 : referenceVeh = refVeh;
342 59 : active = true;
343 59 : gapAttained = false;
344 59 : prevLeader = nullptr;
345 59 : lastUpdate = SIMSTEP - DELTA_T;
346 59 : timeHeadwayIncrement = changeRate * TS * (tauTarget - tauOriginal);
347 59 : spaceHeadwayIncrement = changeRate * TS * addGapTarget;
348 :
349 59 : if (referenceVeh != nullptr) {
350 : // Add refVeh to refVehMap
351 13 : GapControlState::refVehMap[referenceVeh] = this;
352 : }
353 : }
354 59 : }
355 :
356 : void
357 118 : MSVehicle::Influencer::GapControlState::deactivate() {
358 118 : active = false;
359 118 : if (referenceVeh != nullptr) {
360 : // Remove corresponding refVehMapEntry if appropriate
361 13 : GapControlState::refVehMap.erase(referenceVeh);
362 13 : referenceVeh = nullptr;
363 : }
364 118 : }
365 :
366 :
367 : /* -------------------------------------------------------------------------
368 : * methods of MSVehicle::Influencer
369 : * ----------------------------------------------------------------------- */
370 3554 : MSVehicle::Influencer::Influencer() :
371 : myGapControlState(nullptr),
372 3554 : myOriginalSpeed(-1),
373 3554 : myLatDist(0),
374 3554 : mySpeedAdaptationStarted(true),
375 3554 : myConsiderSafeVelocity(true),
376 3554 : myConsiderSpeedLimit(true),
377 3554 : myConsiderMaxAcceleration(true),
378 3554 : myConsiderMaxDeceleration(true),
379 3554 : myRespectJunctionPriority(true),
380 3554 : myEmergencyBrakeRedLight(true),
381 3554 : myRespectJunctionLeaderPriority(true),
382 3554 : myLastRemoteAccess(-TIME2STEPS(20)),
383 3554 : myStrategicLC(LC_NOCONFLICT),
384 3554 : myCooperativeLC(LC_NOCONFLICT),
385 3554 : mySpeedGainLC(LC_NOCONFLICT),
386 3554 : myRightDriveLC(LC_NOCONFLICT),
387 3554 : mySublaneLC(LC_NOCONFLICT),
388 3554 : myTraciLaneChangePriority(LCP_URGENT),
389 3554 : myTraCISignals(-1)
390 3554 : {}
391 :
392 :
393 10662 : MSVehicle::Influencer::~Influencer() {}
394 :
395 : void
396 59 : MSVehicle::Influencer::init() {
397 59 : GapControlState::init();
398 59 : }
399 :
400 : void
401 32678 : MSVehicle::Influencer::cleanup() {
402 32678 : GapControlState::cleanup();
403 32678 : }
404 :
405 : void
406 36037 : MSVehicle::Influencer::setSpeedTimeLine(const std::vector<std::pair<SUMOTime, double> >& speedTimeLine) {
407 36037 : mySpeedAdaptationStarted = true;
408 36037 : mySpeedTimeLine = speedTimeLine;
409 36037 : }
410 :
411 : void
412 59 : MSVehicle::Influencer::activateGapController(double originalTau, double newTimeHeadway, double newSpaceHeadway, double duration, double changeRate, double maxDecel, MSVehicle* refVeh) {
413 59 : if (myGapControlState == nullptr) {
414 59 : myGapControlState = std::make_shared<GapControlState>();
415 59 : init(); // only does things on first call
416 : }
417 59 : myGapControlState->activate(originalTau, newTimeHeadway, newSpaceHeadway, duration, changeRate, maxDecel, refVeh);
418 59 : }
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 7399 : MSVehicle::Influencer::setLaneTimeLine(const std::vector<std::pair<SUMOTime, int> >& laneTimeLine) {
429 7399 : myLaneTimeLine = laneTimeLine;
430 7399 : }
431 :
432 :
433 : void
434 9095 : MSVehicle::Influencer::adaptLaneTimeLine(int indexShift) {
435 19301 : for (auto& item : myLaneTimeLine) {
436 10206 : item.second += indexShift;
437 : }
438 9095 : }
439 :
440 :
441 : void
442 1487 : MSVehicle::Influencer::setSublaneChange(double latDist) {
443 1487 : myLatDist = latDist;
444 1487 : }
445 :
446 : int
447 74 : MSVehicle::Influencer::getSpeedMode() const {
448 74 : return (1 * myConsiderSafeVelocity +
449 74 : 2 * myConsiderMaxAcceleration +
450 74 : 4 * myConsiderMaxDeceleration +
451 74 : 8 * myRespectJunctionPriority +
452 74 : 16 * myEmergencyBrakeRedLight +
453 74 : 32 * !myRespectJunctionLeaderPriority + // inverted!
454 74 : 64 * !myConsiderSpeedLimit // inverted!
455 74 : );
456 : }
457 :
458 :
459 : int
460 1476 : MSVehicle::Influencer::getLaneChangeMode() const {
461 1476 : return (1 * myStrategicLC +
462 1476 : 4 * myCooperativeLC +
463 1476 : 16 * mySpeedGainLC +
464 1476 : 64 * myRightDriveLC +
465 1476 : 256 * myTraciLaneChangePriority +
466 1476 : 1024 * mySublaneLC);
467 : }
468 :
469 : SUMOTime
470 71 : MSVehicle::Influencer::getLaneTimeLineDuration() {
471 : SUMOTime duration = -1;
472 213 : for (std::vector<std::pair<SUMOTime, int>>::iterator i = myLaneTimeLine.begin(); i != myLaneTimeLine.end(); ++i) {
473 142 : if (duration < 0) {
474 71 : duration = i->first;
475 : } else {
476 71 : duration -= i->first;
477 : }
478 : }
479 71 : 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 980714 : MSVehicle::Influencer::influenceSpeed(SUMOTime currentTime, double speed, double vSafe, double vMin, double vMax) {
494 : // remove leading commands which are no longer valid
495 982068 : while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
496 : mySpeedTimeLine.erase(mySpeedTimeLine.begin());
497 : }
498 :
499 980714 : if (!(mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first)) {
500 : // Speed advice is active -> compute new speed according to speedTimeLine
501 49877 : if (!mySpeedAdaptationStarted) {
502 0 : mySpeedTimeLine[0].second = speed;
503 0 : mySpeedAdaptationStarted = true;
504 : }
505 49877 : 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 99402 : const double td = MIN2(1.0, STEPS2TIME(currentTime - mySpeedTimeLine[0].first) / MAX2(TS, STEPS2TIME(mySpeedTimeLine[1].first - mySpeedTimeLine[0].first)));
507 :
508 49877 : speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
509 49877 : if (myConsiderSafeVelocity) {
510 : speed = MIN2(speed, vSafe);
511 : }
512 49877 : if (myConsiderMaxAcceleration) {
513 : speed = MIN2(speed, vMax);
514 : }
515 49877 : if (myConsiderMaxDeceleration) {
516 : speed = MAX2(speed, vMin);
517 : }
518 : }
519 980714 : return speed;
520 : }
521 :
522 : double
523 487063 : 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 487063 : if (myGapControlState != nullptr && myGapControlState->active) {
535 : // Determine leader and the speed that would be chosen by the gap controller
536 7778 : const double currentSpeed = veh->getSpeed();
537 7778 : const MSVehicle* msVeh = dynamic_cast<const MSVehicle*>(veh);
538 : assert(msVeh != nullptr);
539 7778 : const double desiredTargetTimeSpacing = myGapControlState->tauTarget * currentSpeed;
540 : std::pair<const MSVehicle*, double> leaderInfo;
541 7778 : 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 438 : double dist = msVeh->getDistanceToPosition(leader->getPositionOnLane(), leader->getLane()) - leader->getLength();
554 438 : if (dist > 100000) {
555 : // Reference vehicle was not found downstream the ego's route
556 : // Maybe, it is behind the ego vehicle
557 42 : 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 438 : leaderInfo = std::make_pair(leader, dist - msVeh->getVehicleType().getMinGap());
570 : }
571 7778 : 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 7778 : 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 7778 : myGapControlState->prevLeader = leaderInfo.first;
594 :
595 : // Calculate desired following speed assuming the alternative headway time
596 7778 : MSCFModel* cfm = (MSCFModel*) & (msVeh->getVehicleType().getCarFollowModel());
597 7778 : const double origTau = cfm->getHeadwayTime();
598 7778 : cfm->setHeadwayTime(myGapControlState->tauCurrent);
599 7778 : gapControlSpeed = MIN2(gapControlSpeed,
600 7778 : cfm->followSpeed(msVeh, currentSpeed, fakeDist, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first));
601 7778 : 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 7778 : 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 7778 : if (myGapControlState->lastUpdate < currentTime) {
620 : #ifdef DEBUG_TRACI
621 : if DEBUG_COND2(veh) {
622 : std::cout << " Updating GapControlState." << std::endl;
623 : }
624 : #endif
625 7778 : if (myGapControlState->tauCurrent == myGapControlState->tauTarget && myGapControlState->addGapCurrent == myGapControlState->addGapTarget) {
626 2992 : 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 4180 : 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 4786 : myGapControlState->tauCurrent = MIN2(myGapControlState->tauCurrent + myGapControlState->timeHeadwayIncrement, myGapControlState->tauTarget);
657 5168 : myGapControlState->addGapCurrent = MIN2(myGapControlState->addGapCurrent + myGapControlState->spaceHeadwayIncrement, myGapControlState->addGapTarget);
658 : }
659 : }
660 7778 : if (myConsiderSafeVelocity) {
661 : gapControlSpeed = MIN2(gapControlSpeed, vSafe);
662 : }
663 7778 : if (myConsiderMaxAcceleration) {
664 : gapControlSpeed = MIN2(gapControlSpeed, vMax);
665 : }
666 7778 : if (myConsiderMaxDeceleration) {
667 : gapControlSpeed = MAX2(gapControlSpeed, vMin);
668 : }
669 : return MIN2(speed, gapControlSpeed);
670 : } else {
671 : return speed;
672 : }
673 : }
674 :
675 : double
676 7084 : MSVehicle::Influencer::getOriginalSpeed() const {
677 7084 : return myOriginalSpeed;
678 : }
679 :
680 : void
681 493651 : MSVehicle::Influencer::setOriginalSpeed(double speed) {
682 493651 : myOriginalSpeed = speed;
683 493651 : }
684 :
685 :
686 : int
687 2446380 : MSVehicle::Influencer::influenceChangeDecision(const SUMOTime currentTime, const MSEdge& currentEdge, const int currentLaneIndex, int state) {
688 : // remove leading commands which are no longer valid
689 2446682 : 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 2446380 : if (myLaneTimeLine.size() >= 2 && currentTime >= myLaneTimeLine[0].first) {
695 177173 : const int destinationLaneIndex = myLaneTimeLine[1].second;
696 177173 : if (destinationLaneIndex < (int)currentEdge.getLanes().size()) {
697 176842 : if (currentLaneIndex > destinationLaneIndex) {
698 : changeRequest = REQUEST_RIGHT;
699 175839 : } else if (currentLaneIndex < destinationLaneIndex) {
700 : changeRequest = REQUEST_LEFT;
701 : } else {
702 : changeRequest = REQUEST_HOLD;
703 : }
704 331 : } else if (currentEdge.getLanes().back()->getOpposite() != nullptr) { // change to opposite direction driving
705 : changeRequest = REQUEST_LEFT;
706 331 : state = state | LCA_TRACI;
707 : }
708 : }
709 : // check whether the current reason shall be canceled / overridden
710 2446380 : if ((state & LCA_WANTS_LANECHANGE_OR_STAY) != 0) {
711 : // flags for the current reason
712 : LaneChangeMode mode = LC_NEVER;
713 1588318 : if ((state & LCA_TRACI) != 0 && myLatDist != 0) {
714 : // security checks
715 2877 : if ((myTraciLaneChangePriority == LCP_ALWAYS)
716 692 : || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
717 2720 : state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
718 : }
719 : // continue sublane change manoeuvre
720 2877 : return state;
721 1585441 : } else if ((state & LCA_STRATEGIC) != 0) {
722 492448 : mode = myStrategicLC;
723 1092993 : } else if ((state & LCA_COOPERATIVE) != 0) {
724 78 : mode = myCooperativeLC;
725 1092915 : } else if ((state & LCA_SPEEDGAIN) != 0) {
726 40923 : mode = mySpeedGainLC;
727 1051992 : } else if ((state & LCA_KEEPRIGHT) != 0) {
728 6498 : mode = myRightDriveLC;
729 1045494 : } else if ((state & LCA_SUBLANE) != 0) {
730 1045492 : 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 1585439 : if (mode == LC_NEVER) {
737 : // cancel all lcModel requests
738 : state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
739 43992 : state &= ~LCA_URGENT;
740 43992 : if (changeRequest == REQUEST_NONE) {
741 : // also remove all reasons except TRACI
742 43348 : state &= ~LCA_CHANGE_REASONS | LCA_TRACI;
743 : }
744 1541449 : } else if (mode == LC_NOCONFLICT && changeRequest != REQUEST_NONE) {
745 9035 : if (
746 9035 : ((state & LCA_LEFT) != 0 && changeRequest != REQUEST_LEFT) ||
747 7929 : ((state & LCA_RIGHT) != 0 && changeRequest != REQUEST_RIGHT) ||
748 6598 : ((state & LCA_STAY) != 0 && changeRequest != REQUEST_HOLD)) {
749 : // cancel conflicting lcModel request
750 : state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
751 2601 : state &= ~LCA_URGENT;
752 : }
753 1532414 : } else if (mode == LC_ALWAYS) {
754 : // ignore any TraCI requests
755 : return state;
756 : }
757 : }
758 : // apply traci requests
759 2436499 : if (changeRequest == REQUEST_NONE) {
760 2266838 : return state;
761 : } else {
762 176655 : state |= LCA_TRACI;
763 : // security checks
764 176655 : if ((myTraciLaneChangePriority == LCP_ALWAYS)
765 174581 : || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
766 2541 : state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
767 : }
768 176655 : if (changeRequest != REQUEST_HOLD && myTraciLaneChangePriority != LCP_OPPORTUNISTIC) {
769 2294 : state |= LCA_URGENT;
770 : }
771 2323 : switch (changeRequest) {
772 : case REQUEST_HOLD:
773 174332 : return state | LCA_STAY;
774 1420 : case REQUEST_LEFT:
775 1420 : return state | LCA_LEFT;
776 903 : case REQUEST_RIGHT:
777 903 : return state | LCA_RIGHT;
778 : default:
779 : throw ProcessError(TL("should not happen"));
780 : }
781 : }
782 : }
783 :
784 :
785 : double
786 442 : MSVehicle::Influencer::changeRequestRemainingSeconds(const SUMOTime currentTime) const {
787 : assert(myLaneTimeLine.size() >= 2);
788 : assert(currentTime >= myLaneTimeLine[0].first);
789 442 : return STEPS2TIME(myLaneTimeLine[1].first - currentTime);
790 : }
791 :
792 :
793 : void
794 5566 : MSVehicle::Influencer::setSpeedMode(int speedMode) {
795 5566 : myConsiderSafeVelocity = ((speedMode & 1) != 0);
796 5566 : myConsiderMaxAcceleration = ((speedMode & 2) != 0);
797 5566 : myConsiderMaxDeceleration = ((speedMode & 4) != 0);
798 5566 : myRespectJunctionPriority = ((speedMode & 8) != 0);
799 5566 : myEmergencyBrakeRedLight = ((speedMode & 16) != 0);
800 5566 : myRespectJunctionLeaderPriority = ((speedMode & 32) == 0); // inverted!
801 5566 : myConsiderSpeedLimit = ((speedMode & 64) == 0); // inverted!
802 5566 : }
803 :
804 :
805 : void
806 18690 : MSVehicle::Influencer::setLaneChangeMode(int value) {
807 18690 : myStrategicLC = (LaneChangeMode)(value & (1 + 2));
808 18690 : myCooperativeLC = (LaneChangeMode)((value & (4 + 8)) >> 2);
809 18690 : mySpeedGainLC = (LaneChangeMode)((value & (16 + 32)) >> 4);
810 18690 : myRightDriveLC = (LaneChangeMode)((value & (64 + 128)) >> 6);
811 18690 : myTraciLaneChangePriority = (TraciLaneChangePriority)((value & (256 + 512)) >> 8);
812 18690 : mySublaneLC = (LaneChangeMode)((value & (1024 + 2048)) >> 10);
813 18690 : }
814 :
815 :
816 : void
817 7800 : MSVehicle::Influencer::setRemoteControlled(Position xyPos, MSLane* l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector& route, SUMOTime t) {
818 7800 : myRemoteXYPos = xyPos;
819 7800 : myRemoteLane = l;
820 7800 : myRemotePos = pos;
821 7800 : myRemotePosLat = posLat;
822 7800 : myRemoteAngle = angle;
823 7800 : myRemoteEdgeOffset = edgeOffset;
824 7800 : myRemoteRoute = route;
825 7800 : myLastRemoteAccess = t;
826 7800 : }
827 :
828 :
829 : bool
830 1011002 : MSVehicle::Influencer::isRemoteControlled() const {
831 1011002 : return myLastRemoteAccess == MSNet::getInstance()->getCurrentTimeStep();
832 : }
833 :
834 :
835 : bool
836 492355 : MSVehicle::Influencer::isRemoteAffected(SUMOTime t) const {
837 492355 : return myLastRemoteAccess >= t - TIME2STEPS(10);
838 : }
839 :
840 :
841 : void
842 487063 : MSVehicle::Influencer::updateRemoteControlRoute(MSVehicle* v) {
843 487063 : if (myRemoteRoute.size() != 0 && myRemoteRoute != v->getRoute().getEdges()) {
844 : // only replace route at this time if the vehicle is moving with the flow
845 62 : 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 10 : v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
851 10 : v->updateBestLanes();
852 : }
853 : }
854 487063 : }
855 :
856 :
857 : void
858 7775 : MSVehicle::Influencer::postProcessRemoteControl(MSVehicle* v) {
859 7775 : const bool wasOnRoad = v->isOnRoad();
860 7775 : const bool withinLane = myRemoteLane != nullptr && fabs(myRemotePosLat) < 0.5 * (myRemoteLane->getWidth() + v->getVehicleType().getWidth());
861 7775 : const bool keepLane = wasOnRoad && v->getLane() == myRemoteLane;
862 7775 : if (v->isOnRoad() && !(keepLane && withinLane)) {
863 150 : if (myRemoteLane != nullptr && &v->getLane()->getEdge() == &myRemoteLane->getEdge()) {
864 : // correct odometer which gets incremented via onRemovalFromNet->leaveLane
865 71 : v->myOdometer -= v->getLane()->getLength();
866 : }
867 150 : v->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT);
868 150 : v->getMutableLane()->removeVehicle(v, MSMoveReminder::NOTIFICATION_TELEPORT, false);
869 : }
870 7775 : 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 67 : const_cast<SUMOVehicleParameter&>(v->getParameter()).stops.clear();
883 134 : v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
884 : myRemoteRoute.clear();
885 : }
886 7775 : v->myCurrEdge = v->getRoute().begin() + myRemoteEdgeOffset;
887 7775 : if (myRemoteLane != nullptr && myRemotePos > myRemoteLane->getLength()) {
888 0 : myRemotePos = myRemoteLane->getLength();
889 : }
890 7775 : if (myRemoteLane != nullptr && withinLane) {
891 7593 : if (keepLane) {
892 : // TODO this handles only the case when the new vehicle is completely on the edge
893 7415 : const bool needFurtherUpdate = v->myState.myPos < v->getVehicleType().getLength() && myRemotePos >= v->getVehicleType().getLength();
894 7415 : v->myState.myPos = myRemotePos;
895 7415 : v->myState.myPosLat = myRemotePosLat;
896 7415 : if (needFurtherUpdate) {
897 5 : v->myState.myBackPos = v->updateFurtherLanes(v->myFurtherLanes, v->myFurtherLanesPosLat, std::vector<MSLane*>());
898 : }
899 : } else {
900 178 : MSMoveReminder::Notification notify = v->getDeparture() == NOT_YET_DEPARTED
901 178 : ? MSMoveReminder::NOTIFICATION_DEPARTED
902 : : MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED;
903 178 : if (!v->isOnRoad()) {
904 178 : MSVehicleTransfer::getInstance()->remove(v); // TODO may need optimization, this is linear in the number of vehicles in transfer
905 : }
906 178 : myRemoteLane->forceVehicleInsertion(v, myRemotePos, notify, myRemotePosLat);
907 178 : v->updateBestLanes();
908 : }
909 7593 : if (!wasOnRoad) {
910 65 : v->drawOutsideNetwork(false);
911 : }
912 : //std::cout << "on road network p=" << myRemoteXYPos << " a=" << myRemoteAngle << " l=" << Named::getIDSecure(myRemoteLane) << " pos=" << myRemotePos << " posLat=" << myRemotePosLat << "\n";
913 7593 : myRemoteLane->requireCollisionCheck();
914 : } else {
915 182 : if (v->getDeparture() == NOT_YET_DEPARTED) {
916 5 : v->onDepart();
917 : }
918 182 : v->drawOutsideNetwork(true);
919 : // see updateState
920 182 : double vNext = v->processTraCISpeedControl(
921 182 : v->getMaxSpeed(), v->getSpeed());
922 182 : v->setBrakingSignals(vNext);
923 182 : v->myState.myPreviousSpeed = v->getSpeed();
924 182 : v->myAcceleration = SPEED2ACCEL(vNext - v->getSpeed());
925 182 : v->myState.mySpeed = vNext;
926 182 : 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 7775 : v->setRemoteState(myRemoteXYPos);
931 7775 : v->setAngle(GeomHelper::fromNaviDegree(myRemoteAngle));
932 7775 : }
933 :
934 :
935 : double
936 7752 : MSVehicle::Influencer::implicitSpeedRemote(const MSVehicle* veh, double oldSpeed) {
937 7752 : if (veh->getPosition() == Position::INVALID) {
938 10 : return oldSpeed;
939 : }
940 7742 : double dist = veh->getPosition().distanceTo2D(myRemoteXYPos);
941 7742 : 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 7600 : const double distAlongRoute = veh->getDistanceToPosition(myRemotePos, myRemoteLane);
947 7600 : 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 7742 : const double minSpeed = myConsiderMaxDeceleration ?
953 5034 : veh->getCarFollowModel().minNextSpeedEmergency(oldSpeed, veh) : 0;
954 7742 : const double maxSpeed = (myRemoteLane != nullptr
955 7742 : ? myRemoteLane->getVehicleMaxSpeed(veh)
956 142 : : (veh->getLane() != nullptr
957 142 : ? veh->getLane()->getVehicleMaxSpeed(veh)
958 5 : : veh->getMaxSpeed()));
959 7742 : return MIN2(maxSpeed, MAX2(minSpeed, DIST2SPEED(dist)));
960 : }
961 :
962 :
963 : double
964 7580 : MSVehicle::Influencer::implicitDeltaPosRemote(const MSVehicle* veh) {
965 : double dist = 0;
966 7580 : if (myRemoteLane == nullptr) {
967 6 : 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 7574 : dist = veh->getDistanceToPosition(myRemotePos, myRemoteLane);
975 : }
976 7580 : if (dist == std::numeric_limits<double>::max()) {
977 : return 0;
978 : } else {
979 7339 : if (DIST2SPEED(dist) > veh->getMaxSpeed() * 1.1) {
980 48 : 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 16 : dist = MIN2(dist, SPEED2DIST(veh->getMaxSpeed() * 2));
984 : }
985 7339 : return dist;
986 : }
987 : }
988 :
989 :
990 : /* -------------------------------------------------------------------------
991 : * MSVehicle-methods
992 : * ----------------------------------------------------------------------- */
993 4631147 : MSVehicle::MSVehicle(SUMOVehicleParameter* pars, ConstMSRoutePtr route,
994 4631147 : MSVehicleType* type, const double speedFactor) :
995 : MSBaseVehicle(pars, route, type, speedFactor),
996 4631147 : myWaitingTime(0),
997 4631147 : myWaitingTimeCollector(),
998 4631147 : myTimeLoss(0),
999 4631147 : myState(0, 0, 0, 0, 0),
1000 4631147 : myDriverState(nullptr),
1001 4631147 : myActionStep(true),
1002 4631147 : myLastActionTime(0),
1003 4631147 : myLane(nullptr),
1004 4631147 : myLaneChangeModel(nullptr),
1005 4631147 : myLastBestLanesEdge(nullptr),
1006 4631147 : myLastBestLanesInternalLane(nullptr),
1007 4631147 : myAcceleration(0),
1008 : myNextTurn(0., nullptr),
1009 4631147 : mySignals(0),
1010 4631147 : myAmOnNet(false),
1011 4631147 : myAmIdling(false),
1012 4631147 : myHaveToWaitOnNextLink(false),
1013 4631147 : myAngle(0),
1014 4631147 : myStopDist(std::numeric_limits<double>::max()),
1015 4631147 : myCollisionImmunity(-1),
1016 4631147 : myCachedPosition(Position::INVALID),
1017 4631147 : myJunctionEntryTime(SUMOTime_MAX),
1018 4631147 : myJunctionEntryTimeNeverYield(SUMOTime_MAX),
1019 4631147 : myJunctionConflictEntryTime(SUMOTime_MAX),
1020 4631147 : myTimeSinceStartup(TIME2STEPS(3600 * 24)),
1021 4631147 : myHaveStoppedFor(nullptr),
1022 13893441 : myInfluencer(nullptr) {
1023 4631147 : myCFVariables = type->getCarFollowModel().createVehicleVariables();
1024 4631147 : myNextDriveItem = myLFLinkLanes.begin();
1025 4631147 : }
1026 :
1027 :
1028 8764888 : MSVehicle::~MSVehicle() {
1029 4631076 : cleanupFurtherLanes();
1030 4631076 : delete myLaneChangeModel;
1031 4631076 : if (myType->isVehicleSpecific()) {
1032 322 : MSNet::getInstance()->getVehicleControl().removeVType(myType);
1033 : }
1034 4631076 : delete myInfluencer;
1035 4631076 : delete myCFVariables;
1036 13395964 : }
1037 :
1038 :
1039 : void
1040 4631638 : MSVehicle::cleanupFurtherLanes() {
1041 4634437 : for (MSLane* further : myFurtherLanes) {
1042 2799 : further->resetPartialOccupation(this);
1043 2799 : if (further->getBidiLane() != nullptr
1044 2799 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
1045 0 : further->getBidiLane()->resetPartialOccupation(this);
1046 : }
1047 : }
1048 4631638 : if (myLaneChangeModel != nullptr) {
1049 4631604 : removeApproachingInformation(myLFLinkLanes);
1050 4631604 : myLaneChangeModel->cleanupShadowLane();
1051 4631604 : myLaneChangeModel->cleanupTargetLane();
1052 : // still needed when calling resetPartialOccupation (getShadowLane) and when removing
1053 : // approach information from parallel links
1054 : }
1055 : myFurtherLanes.clear();
1056 : myFurtherLanesPosLat.clear();
1057 4631638 : }
1058 :
1059 :
1060 : void
1061 2949457 : MSVehicle::onRemovalFromNet(const MSMoveReminder::Notification reason) {
1062 : #ifdef DEBUG_ACTIONSTEPS
1063 : if (DEBUG_COND) {
1064 : std::cout << SIMTIME << " Removing vehicle '" << getID() << "' (reason: " << toString(reason) << ")" << std::endl;
1065 : }
1066 : #endif
1067 2949457 : MSVehicleTransfer::getInstance()->remove(this);
1068 2949457 : removeApproachingInformation(myLFLinkLanes);
1069 2949457 : leaveLane(reason);
1070 2949457 : if (reason == MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION) {
1071 562 : cleanupFurtherLanes();
1072 : }
1073 2949457 : }
1074 :
1075 :
1076 : void
1077 4631147 : MSVehicle::initDevices() {
1078 4631147 : MSBaseVehicle::initDevices();
1079 4631135 : myLaneChangeModel = MSAbstractLaneChangeModel::build(myType->getLaneChangeModel(), *this);
1080 4631113 : myDriverState = static_cast<MSDevice_DriverState*>(getDevice(typeid(MSDevice_DriverState)));
1081 4631113 : myFrictionDevice = static_cast<MSDevice_Friction*>(getDevice(typeid(MSDevice_Friction)));
1082 4631113 : }
1083 :
1084 :
1085 : // ------------ interaction with the route
1086 : bool
1087 2401929041 : MSVehicle::hasValidRouteStart(std::string& msg) {
1088 : // note: not a const method because getDepartLane may call updateBestLanes
1089 2401929041 : if (!(*myCurrEdge)->isTazConnector()) {
1090 2401482401 : if (myParameter->departLaneProcedure == DepartLaneDefinition::GIVEN) {
1091 50471381 : if ((*myCurrEdge)->getDepartLane(*this) == nullptr) {
1092 122 : msg = "Invalid departlane definition for vehicle '" + getID() + "'.";
1093 61 : if (myParameter->departLane >= (int)(*myCurrEdge)->getLanes().size()) {
1094 11 : myRouteValidity |= ROUTE_START_INVALID_LANE;
1095 : } else {
1096 50 : myRouteValidity |= ROUTE_START_INVALID_PERMISSIONS;
1097 : }
1098 61 : return false;
1099 : }
1100 : } else {
1101 2351011020 : if ((*myCurrEdge)->allowedLanes(getVClass()) == nullptr) {
1102 156 : msg = "Vehicle '" + getID() + "' is not allowed to depart on any lane of edge '" + (*myCurrEdge)->getID() + "'.";
1103 78 : myRouteValidity |= ROUTE_START_INVALID_PERMISSIONS;
1104 78 : return false;
1105 : }
1106 : }
1107 2401482262 : if (myParameter->departSpeedProcedure == DepartSpeedDefinition::GIVEN && myParameter->departSpeed > myType->getMaxSpeed() + SPEED_EPS) {
1108 38 : msg = "Departure speed for vehicle '" + getID() + "' is too high for the vehicle type '" + myType->getID() + "'.";
1109 19 : myRouteValidity |= ROUTE_START_INVALID_LANE;
1110 19 : return false;
1111 : }
1112 : }
1113 2401928883 : myRouteValidity &= ~(ROUTE_START_INVALID_LANE | ROUTE_START_INVALID_PERMISSIONS);
1114 2401928883 : return true;
1115 : }
1116 :
1117 :
1118 : bool
1119 685896189 : MSVehicle::hasArrived() const {
1120 685896189 : return hasArrivedInternal(false);
1121 : }
1122 :
1123 :
1124 : bool
1125 1380340300 : MSVehicle::hasArrivedInternal(bool oppositeTransformed) const {
1126 2231827692 : return ((myCurrEdge == myRoute->end() - 1 || (myParameter->arrivalEdge >= 0 && getRoutePosition() >= myParameter->arrivalEdge))
1127 528889293 : && (myStops.empty() || myStops.front().edge != myCurrEdge || myStops.front().getSpeed() > 0)
1128 987912362 : && ((myLaneChangeModel->isOpposite() && !oppositeTransformed) ? myLane->getLength() - myState.myPos : myState.myPos) > MIN2(myLane->getLength(), myArrivalPos) - POSITION_EPS
1129 1390265618 : && !isRemoteControlled());
1130 : }
1131 :
1132 :
1133 : bool
1134 1716266 : MSVehicle::replaceRoute(ConstMSRoutePtr newRoute, const std::string& info, bool onInit, int offset, bool addRouteStops, bool removeStops, std::string* msgReturn) {
1135 3432532 : if (MSBaseVehicle::replaceRoute(newRoute, info, onInit, offset, addRouteStops, removeStops, msgReturn)) {
1136 : // update best lanes (after stops were added)
1137 1716248 : myLastBestLanesEdge = nullptr;
1138 1716248 : myLastBestLanesInternalLane = nullptr;
1139 1716248 : updateBestLanes(true, onInit ? (*myCurrEdge)->getLanes().front() : 0);
1140 : assert(!removeStops || haveValidStopEdges());
1141 1716248 : if (myStops.size() == 0) {
1142 1683481 : myStopDist = std::numeric_limits<double>::max();
1143 : }
1144 1716248 : return true;
1145 : }
1146 : return false;
1147 : }
1148 :
1149 :
1150 : // ------------ Interaction with move reminders
1151 : void
1152 674229535 : MSVehicle::workOnMoveReminders(double oldPos, double newPos, double newSpeed) {
1153 : // This erasure-idiom works for all stl-sequence-containers
1154 : // See Meyers: Effective STL, Item 9
1155 1777262157 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
1156 : // XXX: calling notifyMove with newSpeed seems not the best choice. For the ballistic update, the average speed is calculated and used
1157 : // although a higher order quadrature-formula might be more adequate.
1158 : // For the euler case (where the speed is considered constant for each time step) it is conceivable that
1159 : // the current calculations may lead to systematic errors for large time steps (compared to reality). Refs. #2579
1160 2206065244 : if (!rem->first->notifyMove(*this, oldPos + rem->second, newPos + rem->second, MAX2(0., newSpeed))) {
1161 : #ifdef _DEBUG
1162 : if (myTraceMoveReminders) {
1163 : traceMoveReminder("notifyMove", rem->first, rem->second, false);
1164 : }
1165 : #endif
1166 : rem = myMoveReminders.erase(rem);
1167 : } else {
1168 : #ifdef _DEBUG
1169 : if (myTraceMoveReminders) {
1170 : traceMoveReminder("notifyMove", rem->first, rem->second, true);
1171 : }
1172 : #endif
1173 : ++rem;
1174 : }
1175 : }
1176 674229535 : if (myEnergyParams != nullptr) {
1177 : // TODO make the vehicle energy params a derived class which is a move reminder
1178 129075290 : myEnergyParams->setDynamicValues(isStopped() ? getNextStop().duration : -1, isParking(), getWaitingTime(), getAngle());
1179 : }
1180 674229535 : }
1181 :
1182 :
1183 : void
1184 105279 : MSVehicle::workOnIdleReminders() {
1185 105279 : updateWaitingTime(0.); // cf issue 2233
1186 :
1187 : // vehicle move reminders
1188 131439 : for (const auto& rem : myMoveReminders) {
1189 26160 : rem.first->notifyIdle(*this);
1190 : }
1191 :
1192 : // lane move reminders - for aggregated values
1193 243928 : for (MSMoveReminder* rem : getLane()->getMoveReminders()) {
1194 138649 : rem->notifyIdle(*this);
1195 : }
1196 105279 : }
1197 :
1198 : // XXX: consider renaming...
1199 : void
1200 17705151 : MSVehicle::adaptLaneEntering2MoveReminder(const MSLane& enteredLane) {
1201 : // save the old work reminders, patching the position information
1202 : // add the information about the new offset to the old lane reminders
1203 17705151 : const double oldLaneLength = myLane->getLength();
1204 50838768 : for (auto& rem : myMoveReminders) {
1205 33133617 : rem.second += oldLaneLength;
1206 : #ifdef _DEBUG
1207 : // if (rem->first==0) std::cout << "Null reminder (?!)" << std::endl;
1208 : // std::cout << "Adapted MoveReminder on lane " << ((rem->first->getLane()==0) ? "NULL" : rem->first->getLane()->getID()) <<" position to " << rem->second << std::endl;
1209 : if (myTraceMoveReminders) {
1210 : traceMoveReminder("adaptedPos", rem.first, rem.second, true);
1211 : }
1212 : #endif
1213 : }
1214 29595778 : for (MSMoveReminder* const rem : enteredLane.getMoveReminders()) {
1215 11890627 : addReminder(rem);
1216 : }
1217 17705151 : }
1218 :
1219 :
1220 : // ------------ Other getter methods
1221 : double
1222 156679454 : MSVehicle::getSlope() const {
1223 156679454 : if (isParking() && getStops().begin()->parkingarea != nullptr) {
1224 3781 : return getStops().begin()->parkingarea->getVehicleSlope(*this);
1225 : }
1226 156675673 : if (myLane == nullptr) {
1227 : return 0;
1228 : }
1229 156675673 : const double posLat = myState.myPosLat; // @todo get rid of the '-'
1230 156675673 : Position p1 = getPosition();
1231 156675673 : Position p2 = getBackPosition();
1232 : if (p2 == Position::INVALID) {
1233 : // Handle special case of vehicle's back reaching out of the network
1234 6 : if (myFurtherLanes.size() > 0) {
1235 6 : p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1236 : if (p2 == Position::INVALID) {
1237 : // unsuitable lane geometry
1238 0 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1239 : }
1240 : } else {
1241 0 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1242 : }
1243 : }
1244 156675673 : return (p1 != p2 ? RAD2DEG(p2.slopeTo2D(p1)) : myLane->getShape().slopeDegreeAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1245 : }
1246 :
1247 :
1248 : Position
1249 890110055 : MSVehicle::getPosition(const double offset) const {
1250 890110055 : if (myLane == nullptr) {
1251 : // when called in the context of GUI-Drawing, the simulation step is already incremented
1252 174 : if (myInfluencer != nullptr && myInfluencer->isRemoteAffected(MSNet::getInstance()->getCurrentTimeStep())) {
1253 50 : return myCachedPosition;
1254 : } else {
1255 124 : return Position::INVALID;
1256 : }
1257 : }
1258 890109881 : if (isParking()) {
1259 1358374 : if (myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() > getNextStopParameter()->started) {
1260 150 : return myCachedPosition;
1261 : }
1262 1358224 : if (myStops.begin()->parkingarea != nullptr) {
1263 19472 : return myStops.begin()->parkingarea->getVehiclePosition(*this);
1264 : } else {
1265 : // position beside the road
1266 1338752 : PositionVector shp = myLane->getEdge().getLanes()[0]->getShape();
1267 2677384 : shp.move2side(SUMO_const_laneWidth * (MSGlobals::gLefthand ? -1 : 1));
1268 1338752 : return shp.positionAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane() + offset));
1269 1338752 : }
1270 : }
1271 888751507 : const bool changingLanes = myLaneChangeModel->isChangingLanes();
1272 1767658418 : const double posLat = (MSGlobals::gLefthand ? 1 : -1) * getLateralPositionOnLane();
1273 888751507 : if (offset == 0. && !changingLanes) {
1274 : if (myCachedPosition == Position::INVALID) {
1275 678701269 : myCachedPosition = validatePosition(myLane->geometryPositionAtOffset(myState.myPos, posLat));
1276 678701269 : if (MSNet::getInstance()->hasElevation() && MSGlobals::gSublane) {
1277 58610 : interpolateLateralZ(myCachedPosition, myState.myPos, posLat);
1278 : }
1279 : }
1280 882174257 : return myCachedPosition;
1281 : }
1282 6577250 : Position result = validatePosition(myLane->geometryPositionAtOffset(getPositionOnLane() + offset, posLat), offset);
1283 6577250 : interpolateLateralZ(result, getPositionOnLane() + offset, posLat);
1284 6577250 : return result;
1285 : }
1286 :
1287 :
1288 : void
1289 6917095 : MSVehicle::interpolateLateralZ(Position& pos, double offset, double posLat) const {
1290 6917095 : const MSLane* shadow = myLaneChangeModel->getShadowLane();
1291 6917095 : if (shadow != nullptr && pos != Position::INVALID) {
1292 : // ignore negative offset
1293 : const Position shadowPos = shadow->geometryPositionAtOffset(MAX2(0.0, offset));
1294 50594 : if (shadowPos != Position::INVALID && pos.z() != shadowPos.z()) {
1295 133 : const double centerDist = (myLane->getWidth() + shadow->getWidth()) * 0.5;
1296 133 : double relOffset = fabs(posLat) / centerDist;
1297 133 : double newZ = (1 - relOffset) * pos.z() + relOffset * shadowPos.z();
1298 : pos.setz(newZ);
1299 : }
1300 : }
1301 6917095 : }
1302 :
1303 :
1304 : double
1305 86846 : MSVehicle::getDistanceToLeaveJunction() const {
1306 86846 : double result = getLength() - getPositionOnLane();
1307 86846 : if (myLane->isNormal()) {
1308 : return MAX2(0.0, result);
1309 : }
1310 337 : const MSLane* lane = myLane;
1311 674 : while (lane->isInternal()) {
1312 337 : result += lane->getLength();
1313 337 : lane = lane->getCanonicalSuccessorLane();
1314 : }
1315 : return result;
1316 : }
1317 :
1318 :
1319 : Position
1320 105032 : MSVehicle::getPositionAlongBestLanes(double offset) const {
1321 : assert(MSGlobals::gUsingInternalLanes);
1322 105032 : if (!isOnRoad()) {
1323 0 : return Position::INVALID;
1324 : }
1325 105032 : const std::vector<MSLane*>& bestLanes = getBestLanesContinuation();
1326 : auto nextBestLane = bestLanes.begin();
1327 105032 : const bool opposite = myLaneChangeModel->isOpposite();
1328 105032 : double pos = opposite ? myLane->getLength() - myState.myPos : myState.myPos;
1329 105032 : const MSLane* lane = opposite ? myLane->getParallelOpposite() : getLane();
1330 : assert(lane != 0);
1331 : bool success = true;
1332 :
1333 310185 : while (offset > 0) {
1334 : // take into account lengths along internal lanes
1335 313563 : while (lane->isInternal() && offset > 0) {
1336 108410 : if (offset > lane->getLength() - pos) {
1337 3569 : offset -= lane->getLength() - pos;
1338 3569 : lane = lane->getLinkCont()[0]->getViaLaneOrLane();
1339 : pos = 0.;
1340 3569 : if (lane == nullptr) {
1341 : success = false;
1342 : offset = 0.;
1343 : }
1344 : } else {
1345 104841 : pos += offset;
1346 : offset = 0;
1347 : }
1348 : }
1349 : // set nextBestLane to next non-internal lane
1350 210288 : while (nextBestLane != bestLanes.end() && *nextBestLane == nullptr) {
1351 : ++nextBestLane;
1352 : }
1353 205153 : if (offset > 0) {
1354 : assert(!lane->isInternal());
1355 : assert(lane == *nextBestLane);
1356 100312 : if (offset > lane->getLength() - pos) {
1357 100129 : offset -= lane->getLength() - pos;
1358 : ++nextBestLane;
1359 : assert(nextBestLane == bestLanes.end() || *nextBestLane != 0);
1360 100129 : if (nextBestLane == bestLanes.end()) {
1361 : success = false;
1362 : offset = 0.;
1363 : } else {
1364 100129 : const MSLink* link = lane->getLinkTo(*nextBestLane);
1365 : assert(link != nullptr);
1366 : lane = link->getViaLaneOrLane();
1367 : pos = 0.;
1368 : }
1369 : } else {
1370 183 : pos += offset;
1371 : offset = 0;
1372 : }
1373 : }
1374 :
1375 : }
1376 :
1377 105032 : if (success) {
1378 105032 : return lane->geometryPositionAtOffset(pos, -getLateralPositionOnLane());
1379 : } else {
1380 0 : return Position::INVALID;
1381 : }
1382 : }
1383 :
1384 :
1385 : double
1386 705242 : MSVehicle::getMaxSpeedOnLane() const {
1387 705242 : if (myLane != nullptr) {
1388 705242 : return myLane->getVehicleMaxSpeed(this);
1389 : }
1390 0 : return myType->getMaxSpeed();
1391 : }
1392 :
1393 :
1394 : Position
1395 685278519 : MSVehicle::validatePosition(Position result, double offset) const {
1396 : int furtherIndex = 0;
1397 685278519 : double lastLength = getPositionOnLane();
1398 685278519 : while (result == Position::INVALID) {
1399 751310 : if (furtherIndex >= (int)myFurtherLanes.size()) {
1400 : //WRITE_WARNINGF(TL("Could not compute position for vehicle '%', time=%."), getID(), time2string(MSNet::getInstance()->getCurrentTimeStep()));
1401 : break;
1402 : }
1403 : //std::cout << SIMTIME << " veh=" << getID() << " lane=" << myLane->getID() << " pos=" << getPositionOnLane() << " posLat=" << getLateralPositionOnLane() << " offset=" << offset << " result=" << result << " i=" << furtherIndex << " further=" << myFurtherLanes.size() << "\n";
1404 179926 : MSLane* further = myFurtherLanes[furtherIndex];
1405 179926 : offset += lastLength;
1406 179926 : result = further->geometryPositionAtOffset(further->getLength() + offset, -getLateralPositionOnLane());
1407 : lastLength = further->getLength();
1408 179926 : furtherIndex++;
1409 : //std::cout << SIMTIME << " newResult=" << result << "\n";
1410 : }
1411 685278519 : return result;
1412 : }
1413 :
1414 :
1415 : ConstMSEdgeVector::const_iterator
1416 2642097 : MSVehicle::getRerouteOrigin() const {
1417 : // too close to the next junction, so avoid an emergency brake here
1418 2642097 : if (myLane != nullptr && (myCurrEdge + 1) != myRoute->end() && !isRailway(getVClass())) {
1419 633215 : if (myLane->isInternal()) {
1420 : return myCurrEdge + 1;
1421 : }
1422 626201 : if (myState.myPos > myLane->getLength() - getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.)) {
1423 : return myCurrEdge + 1;
1424 : }
1425 623711 : if (myLane->getEdge().hasChangeProhibitions(getVClass(), myLane->getIndex())) {
1426 : return myCurrEdge + 1;
1427 : }
1428 : }
1429 2632481 : return myCurrEdge;
1430 : }
1431 :
1432 : void
1433 5230097 : MSVehicle::setAngle(double angle, bool straightenFurther) {
1434 : #ifdef DEBUG_FURTHER
1435 : if (DEBUG_COND) {
1436 : std::cout << SIMTIME << " veh '" << getID() << " setAngle(" << angle << ") straightenFurther=" << straightenFurther << std::endl;
1437 : }
1438 : #endif
1439 5230097 : myAngle = angle;
1440 5230097 : MSLane* next = myLane;
1441 5230097 : if (straightenFurther && myFurtherLanesPosLat.size() > 0) {
1442 195235 : for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
1443 100185 : MSLane* further = myFurtherLanes[i];
1444 100185 : const MSLink* link = further->getLinkTo(next);
1445 100185 : if (link != nullptr) {
1446 99823 : myFurtherLanesPosLat[i] = getLateralPositionOnLane() - link->getLateralShift();
1447 : next = further;
1448 : } else {
1449 : break;
1450 : }
1451 : }
1452 : }
1453 5230097 : }
1454 :
1455 :
1456 : void
1457 406831 : MSVehicle::setActionStepLength(double actionStepLength, bool resetOffset) {
1458 406831 : SUMOTime actionStepLengthMillisecs = SUMOVehicleParserHelper::processActionStepLength(actionStepLength);
1459 : SUMOTime previousActionStepLength = getActionStepLength();
1460 : const bool newActionStepLength = actionStepLengthMillisecs != previousActionStepLength;
1461 406831 : if (newActionStepLength) {
1462 7 : getSingularType().setActionStepLength(actionStepLengthMillisecs, resetOffset);
1463 7 : if (!resetOffset) {
1464 1 : updateActionOffset(previousActionStepLength, actionStepLengthMillisecs);
1465 : }
1466 : }
1467 406825 : if (resetOffset) {
1468 6 : resetActionOffset();
1469 : }
1470 406831 : }
1471 :
1472 :
1473 : bool
1474 291967152 : MSVehicle::congested() const {
1475 291967152 : return myState.mySpeed < (60.0 / 3.6) || myLane->getSpeedLimit() < (60.1 / 3.6);
1476 : }
1477 :
1478 :
1479 : double
1480 681371520 : MSVehicle::computeAngle() const {
1481 : Position p1;
1482 681371520 : const double posLat = -myState.myPosLat; // @todo get rid of the '-'
1483 681371520 : const double lefthandSign = (MSGlobals::gLefthand ? -1 : 1);
1484 :
1485 : // if parking manoeuvre is happening then rotate vehicle on each step
1486 681371520 : if (MSGlobals::gModelParkingManoeuver && !manoeuvreIsComplete()) {
1487 450 : return getAngle() + myManoeuvre.getGUIIncrement();
1488 : }
1489 :
1490 681371070 : if (isParking()) {
1491 27368 : if (myStops.begin()->parkingarea != nullptr) {
1492 15528 : return myStops.begin()->parkingarea->getVehicleAngle(*this);
1493 : } else {
1494 11840 : return myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane()));
1495 : }
1496 : }
1497 681343702 : if (myLaneChangeModel->isChangingLanes()) {
1498 : // cannot use getPosition() because it already includes the offset to the side and thus messes up the angle
1499 1124853 : p1 = myLane->geometryPositionAtOffset(myState.myPos, lefthandSign * posLat);
1500 12 : if (p1 == Position::INVALID && myLane->getShape().length2D() == 0. && myLane->isInternal()) {
1501 : // workaround: extrapolate the preceding lane shape
1502 12 : MSLane* predecessorLane = myLane->getCanonicalPredecessorLane();
1503 12 : p1 = predecessorLane->geometryPositionAtOffset(predecessorLane->getLength() + myState.myPos, lefthandSign * posLat);
1504 : }
1505 : } else {
1506 680218849 : p1 = getPosition();
1507 : }
1508 :
1509 : Position p2;
1510 681343702 : if (getVehicleType().getParameter().locomotiveLength > 0) {
1511 : // articulated vehicle should use the heading of the first part
1512 1701068 : const double locoLength = MIN2(getVehicleType().getParameter().locomotiveLength, getLength());
1513 1701068 : p2 = getPosition(-locoLength);
1514 : } else {
1515 679642634 : p2 = getBackPosition();
1516 : }
1517 : if (p2 == Position::INVALID) {
1518 : // Handle special case of vehicle's back reaching out of the network
1519 946 : if (myFurtherLanes.size() > 0) {
1520 226 : p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1521 : if (p2 == Position::INVALID) {
1522 : // unsuitable lane geometry
1523 178 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1524 : }
1525 : } else {
1526 720 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1527 : }
1528 : }
1529 : double result = (p1 != p2 ? p2.angleTo2D(p1) :
1530 87508 : myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1531 :
1532 681343702 : result += lefthandSign * myLaneChangeModel->calcAngleOffset();
1533 :
1534 : #ifdef DEBUG_FURTHER
1535 : if (DEBUG_COND) {
1536 : std::cout << SIMTIME << " computeAngle veh=" << getID() << " p1=" << p1 << " p2=" << p2 << " angle=" << RAD2DEG(result) << " naviDegree=" << GeomHelper::naviDegree(result) << "\n";
1537 : }
1538 : #endif
1539 681343702 : return result;
1540 : }
1541 :
1542 :
1543 : const Position
1544 843007860 : MSVehicle::getBackPosition() const {
1545 843007860 : const double posLat = MSGlobals::gLefthand ? myState.myPosLat : -myState.myPosLat;
1546 : Position result;
1547 843007860 : if (myState.myPos >= myType->getLength()) {
1548 : // vehicle is fully on the new lane
1549 827309513 : result = myLane->geometryPositionAtOffset(myState.myPos - myType->getLength(), posLat);
1550 : } else {
1551 15698347 : if (myLaneChangeModel->isChangingLanes() && myFurtherLanes.size() > 0 && myLaneChangeModel->getShadowLane(myFurtherLanes.back()) == nullptr) {
1552 : // special case where the target lane has no predecessor
1553 : #ifdef DEBUG_FURTHER
1554 : if (DEBUG_COND) {
1555 : std::cout << " getBackPosition veh=" << getID() << " specialCase using myLane=" << myLane->getID() << " pos=0 posLat=" << myState.myPosLat << " result=" << myLane->geometryPositionAtOffset(0, posLat) << "\n";
1556 : }
1557 : #endif
1558 1701 : result = myLane->geometryPositionAtOffset(0, posLat);
1559 : } else {
1560 : #ifdef DEBUG_FURTHER
1561 : if (DEBUG_COND) {
1562 : std::cout << " getBackPosition veh=" << getID() << " myLane=" << myLane->getID() << " further=" << toString(myFurtherLanes) << " myFurtherLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
1563 : }
1564 : #endif
1565 15696646 : if (myFurtherLanes.size() > 0 && !myLaneChangeModel->isChangingLanes()) {
1566 : // truncate to 0 if vehicle starts on an edge that is shorter than its length
1567 14813582 : const double backPos = MAX2(0.0, getBackPositionOnLane(myFurtherLanes.back()));
1568 29329111 : result = myFurtherLanes.back()->geometryPositionAtOffset(backPos, -myFurtherLanesPosLat.back() * (MSGlobals::gLefthand ? -1 : 1));
1569 : } else {
1570 883064 : result = myLane->geometryPositionAtOffset(0, posLat);
1571 : }
1572 : }
1573 : }
1574 843007860 : if (MSNet::getInstance()->hasElevation() && MSGlobals::gSublane) {
1575 281235 : interpolateLateralZ(result, myState.myPos - myType->getLength(), posLat);
1576 : }
1577 843007860 : return result;
1578 : }
1579 :
1580 :
1581 : bool
1582 446926 : MSVehicle::willStop() const {
1583 446926 : return !isStopped() && !myStops.empty() && myLane != nullptr && &myStops.front().lane->getEdge() == &myLane->getEdge();
1584 : }
1585 :
1586 : bool
1587 355576601 : MSVehicle::isStoppedOnLane() const {
1588 355576601 : return isStopped() && myStops.front().lane == myLane;
1589 : }
1590 :
1591 : bool
1592 30704713 : MSVehicle::keepStopping(bool afterProcessing) const {
1593 30704713 : if (isStopped()) {
1594 : // when coming out of vehicleTransfer we must shift the time forward
1595 36625378 : return (myStops.front().duration - (afterProcessing ? DELTA_T : 0) > 0 || isStoppedTriggered() || myStops.front().pars.collision
1596 30509703 : || myStops.front().pars.breakDown || (myStops.front().getSpeed() > 0
1597 36250 : && (myState.myPos < MIN2(myStops.front().pars.endPos, myStops.front().lane->getLength() - POSITION_EPS))
1598 30422 : && (myStops.front().pars.parking == ParkingType::ONROAD || getSpeed() >= SUMO_const_haltingSpeed)));
1599 : } else {
1600 : return false;
1601 : }
1602 : }
1603 :
1604 :
1605 : SUMOTime
1606 15939 : MSVehicle::remainingStopDuration() const {
1607 15939 : if (isStopped()) {
1608 15939 : return myStops.front().duration;
1609 : }
1610 : return 0;
1611 : }
1612 :
1613 :
1614 : SUMOTime
1615 656092435 : MSVehicle::collisionStopTime() const {
1616 656092435 : return (myStops.empty() || !myStops.front().pars.collision) ? myCollisionImmunity : MAX2((SUMOTime)0, myStops.front().duration);
1617 : }
1618 :
1619 :
1620 : bool
1621 656004144 : MSVehicle::brokeDown() const {
1622 656004144 : return isStopped() && !myStops.empty() && myStops.front().pars.breakDown;
1623 : }
1624 :
1625 :
1626 : bool
1627 109907 : MSVehicle::ignoreCollision() const {
1628 109907 : return myCollisionImmunity > 0;
1629 : }
1630 :
1631 :
1632 : double
1633 615268823 : MSVehicle::processNextStop(double currentVelocity) {
1634 615268823 : if (myStops.empty()) {
1635 : // no stops; pass
1636 : return currentVelocity;
1637 : }
1638 :
1639 : #ifdef DEBUG_STOPS
1640 : if (DEBUG_COND) {
1641 : std::cout << "\nPROCESS_NEXT_STOP\n" << SIMTIME << " vehicle '" << getID() << "'" << std::endl;
1642 : }
1643 : #endif
1644 :
1645 : MSStop& stop = myStops.front();
1646 37346307 : const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1647 37346307 : if (stop.reached) {
1648 23149146 : stop.duration -= getActionStepLength();
1649 :
1650 : #ifdef DEBUG_STOPS
1651 : if (DEBUG_COND) {
1652 : std::cout << SIMTIME << " vehicle '" << getID() << "' reached stop.\n"
1653 : << "Remaining duration: " << STEPS2TIME(stop.duration) << std::endl;
1654 : if (stop.getSpeed() > 0) {
1655 : std::cout << " waypointSpeed=" << stop.getSpeed() << " vehPos=" << myState.myPos << " endPos=" << stop.pars.endPos << "\n";
1656 : }
1657 : }
1658 : #endif
1659 23149146 : if (stop.duration <= 0 && stop.pars.join != "") {
1660 : // join this train (part) to another one
1661 36425 : MSVehicle* joinVeh = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(stop.pars.join));
1662 968 : if (joinVeh && joinVeh->hasDeparted() && (joinVeh->joinTrainPart(this) || joinVeh->joinTrainPartFront(this))) {
1663 36 : stop.joinTriggered = false;
1664 36 : if (myAmRegisteredAsWaiting) {
1665 21 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1666 21 : myAmRegisteredAsWaiting = false;
1667 : }
1668 : // avoid collision warning before this vehicle is removed (joinVeh was already made longer)
1669 36 : myCollisionImmunity = TIME2STEPS(100);
1670 : // mark this vehicle as arrived
1671 36 : myArrivalPos = getPositionOnLane();
1672 36 : const_cast<SUMOVehicleParameter*>(myParameter)->arrivalEdge = getRoutePosition();
1673 : // handle transportables that want to continue in the other vehicle
1674 36 : if (myPersonDevice != nullptr) {
1675 3 : myPersonDevice->transferAtSplitOrJoin(joinVeh);
1676 : }
1677 36 : if (myContainerDevice != nullptr) {
1678 3 : myContainerDevice->transferAtSplitOrJoin(joinVeh);
1679 : }
1680 : }
1681 : }
1682 23149146 : boardTransportables(stop);
1683 21826707 : if (time > stop.endBoarding) {
1684 : // for taxi: cancel customers
1685 223201 : MSDevice_Taxi* taxiDevice = static_cast<MSDevice_Taxi*>(getDevice(typeid(MSDevice_Taxi)));
1686 : if (taxiDevice != nullptr) {
1687 : // may invalidate stops including the current reference
1688 64 : taxiDevice->cancelCurrentCustomers();
1689 64 : resumeFromStopping();
1690 64 : return currentVelocity;
1691 : }
1692 : }
1693 21826643 : if (!keepStopping() && isOnRoad()) {
1694 : #ifdef DEBUG_STOPS
1695 : if (DEBUG_COND) {
1696 : std::cout << SIMTIME << " vehicle '" << getID() << "' resumes from stopping." << std::endl;
1697 : }
1698 : #endif
1699 42331 : resumeFromStopping();
1700 42331 : if (isRail() && hasStops()) {
1701 : // stay on the current lane in case of a double stop
1702 2782 : const MSStop& nextStop = getNextStop();
1703 2782 : if (nextStop.edge == myCurrEdge) {
1704 1033 : const double stopSpeed = getCarFollowModel().stopSpeed(this, getSpeed(), nextStop.pars.endPos - myState.myPos);
1705 : //std::cout << SIMTIME << " veh=" << getID() << " resumedFromStopping currentVelocity=" << currentVelocity << " stopSpeed=" << stopSpeed << "\n";
1706 1033 : return stopSpeed;
1707 : }
1708 : }
1709 : } else {
1710 21784312 : if (stop.triggered) {
1711 2988156 : if (getVehicleType().getPersonCapacity() == getPersonNumber()) {
1712 30 : WRITE_WARNINGF(TL("Vehicle '%' ignores triggered stop on lane '%' due to capacity constraints."), getID(), stop.lane->getID());
1713 10 : stop.triggered = false;
1714 2988146 : } else if (!myAmRegisteredAsWaiting && stop.duration <= DELTA_T) {
1715 : // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1716 3207 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1717 3207 : myAmRegisteredAsWaiting = true;
1718 : #ifdef DEBUG_STOPS
1719 : if (DEBUG_COND) {
1720 : std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for person." << std::endl;
1721 : }
1722 : #endif
1723 : }
1724 : }
1725 21784312 : if (stop.containerTriggered) {
1726 39493 : if (getVehicleType().getContainerCapacity() == getContainerNumber()) {
1727 1311 : WRITE_WARNINGF(TL("Vehicle '%' ignores container triggered stop on lane '%' due to capacity constraints."), getID(), stop.lane->getID());
1728 437 : stop.containerTriggered = false;
1729 39056 : } else if (stop.containerTriggered && !myAmRegisteredAsWaiting && stop.duration <= DELTA_T) {
1730 : // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1731 92 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1732 92 : myAmRegisteredAsWaiting = true;
1733 : #ifdef DEBUG_STOPS
1734 : if (DEBUG_COND) {
1735 : std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for container." << std::endl;
1736 : }
1737 : #endif
1738 : }
1739 : }
1740 : // joining only takes place after stop duration is over
1741 21784312 : if (stop.joinTriggered && !myAmRegisteredAsWaiting
1742 6602 : && stop.duration <= (stop.pars.extension >= 0 ? -stop.pars.extension : 0)) {
1743 91 : if (stop.pars.extension >= 0) {
1744 84 : WRITE_WARNINGF(TL("Vehicle '%' aborts joining after extension of %s at time %."), getID(), STEPS2TIME(stop.pars.extension), time2string(SIMSTEP));
1745 28 : stop.joinTriggered = false;
1746 : } else {
1747 : // keep stopping indefinitely but ensure that simulation terminates
1748 63 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1749 63 : myAmRegisteredAsWaiting = true;
1750 : }
1751 : }
1752 21784312 : if (stop.getSpeed() > 0) {
1753 : //waypoint mode
1754 242771 : if (stop.duration == 0) {
1755 266 : return stop.getSpeed();
1756 : } else {
1757 : // stop for 'until' (computed in planMove)
1758 : return currentVelocity;
1759 : }
1760 : } else {
1761 : // brake
1762 21541541 : if (MSGlobals::gSemiImplicitEulerUpdate || stop.getSpeed() > 0) {
1763 21275169 : return 0;
1764 : } else {
1765 : // ballistic:
1766 266372 : return getSpeed() - getCarFollowModel().getMaxDecel();
1767 : }
1768 : }
1769 : }
1770 : } else {
1771 :
1772 : #ifdef DEBUG_STOPS
1773 : if (DEBUG_COND) {
1774 : std::cout << SIMTIME << " vehicle '" << getID() << "' hasn't reached next stop." << std::endl;
1775 : }
1776 : #endif
1777 : //std::cout << SIMTIME << " myStopDist=" << myStopDist << " bGap=" << getBrakeGap(myLane->getVehicleMaxSpeed(this)) << "\n";
1778 14252859 : if (stop.pars.onDemand && !stop.skipOnDemand && myStopDist <= getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this))) {
1779 562 : MSNet* const net = MSNet::getInstance();
1780 44 : const bool noExits = ((myPersonDevice == nullptr || !myPersonDevice->anyLeavingAtStop(stop))
1781 572 : && (myContainerDevice == nullptr || !myContainerDevice->anyLeavingAtStop(stop)));
1782 83 : const bool noEntries = ((!net->hasPersons() || !net->getPersonControl().hasAnyWaiting(stop.getEdge(), this))
1783 611 : && (!net->hasContainers() || !net->getContainerControl().hasAnyWaiting(stop.getEdge(), this)));
1784 562 : if (noExits && noEntries) {
1785 : //std::cout << " skipOnDemand\n";
1786 494 : stop.skipOnDemand = true;
1787 : }
1788 : }
1789 : // is the next stop on the current lane?
1790 14197161 : if (stop.edge == myCurrEdge) {
1791 : // get the stopping position
1792 5838889 : bool useStoppingPlace = stop.busstop != nullptr || stop.containerstop != nullptr || stop.parkingarea != nullptr;
1793 : bool fitsOnStoppingPlace = true;
1794 5838889 : if (!stop.skipOnDemand) { // no need to check available space if we skip it anyway
1795 5836951 : if (stop.busstop != nullptr) {
1796 1657050 : fitsOnStoppingPlace &= stop.busstop->fits(myState.myPos, *this);
1797 : }
1798 5836951 : if (stop.containerstop != nullptr) {
1799 21788 : fitsOnStoppingPlace &= stop.containerstop->fits(myState.myPos, *this);
1800 : }
1801 : // if the stop is a parking area we check if there is a free position on the area
1802 5836951 : if (stop.parkingarea != nullptr) {
1803 656149 : fitsOnStoppingPlace &= myState.myPos > stop.parkingarea->getBeginLanePosition();
1804 656149 : if (stop.parkingarea->getOccupancy() >= stop.parkingarea->getCapacity()) {
1805 : fitsOnStoppingPlace = false;
1806 : // trigger potential parkingZoneReroute
1807 424480 : MSParkingArea* oldParkingArea = stop.parkingarea;
1808 454994 : for (MSMoveReminder* rem : myLane->getMoveReminders()) {
1809 30514 : if (rem->isParkingRerouter()) {
1810 9290 : rem->notifyEnter(*this, MSMoveReminder::NOTIFICATION_PARKING_REROUTE, myLane);
1811 : }
1812 : }
1813 424480 : if (myStops.empty() || myStops.front().parkingarea != oldParkingArea) {
1814 : // rerouted, keep driving
1815 : return currentVelocity;
1816 : }
1817 231669 : } else if (stop.parkingarea->getOccupancyIncludingReservations(this) >= stop.parkingarea->getCapacity()) {
1818 : fitsOnStoppingPlace = false;
1819 104867 : } else if (stop.parkingarea->parkOnRoad() && stop.parkingarea->getLotIndex(this) < 0) {
1820 : fitsOnStoppingPlace = false;
1821 : }
1822 : }
1823 : }
1824 5836910 : const double targetPos = myState.myPos + myStopDist + (stop.getSpeed() > 0 ? (stop.pars.startPos - stop.pars.endPos) : 0);
1825 5836910 : const double reachedThreshold = (useStoppingPlace ? targetPos - STOPPING_PLACE_OFFSET : stop.getReachedThreshold()) - NUMERICAL_EPS;
1826 : #ifdef DEBUG_STOPS
1827 : if (DEBUG_COND) {
1828 : std::cout << " pos=" << myState.pos() << " speed=" << currentVelocity << " targetPos=" << targetPos << " fits=" << fitsOnStoppingPlace
1829 : << " reachedThresh=" << reachedThreshold
1830 : << " myLane=" << Named::getIDSecure(myLane)
1831 : << " stopLane=" << Named::getIDSecure(stop.lane)
1832 : << "\n";
1833 : }
1834 : #endif
1835 5836910 : const bool posReached = myState.pos() >= reachedThreshold && currentVelocity <= stop.getSpeed() + SUMO_const_haltingSpeed && myLane == stop.lane;
1836 5836910 : if (posReached && !fitsOnStoppingPlace && MSStopOut::active()) {
1837 4505 : MSStopOut::getInstance()->stopBlocked(this, time);
1838 : }
1839 5836910 : if (fitsOnStoppingPlace && posReached && (!MSGlobals::gModelParkingManoeuver || myManoeuvre.entryManoeuvreIsComplete(this))) {
1840 : // ok, we may stop (have reached the stop) and either we are not modelling maneuvering or have completed entry
1841 53274 : stop.reached = true;
1842 53274 : if (!stop.startedFromState) {
1843 53063 : stop.pars.started = time;
1844 : }
1845 : #ifdef DEBUG_STOPS
1846 : if (DEBUG_COND) {
1847 : std::cout << SIMTIME << " vehicle '" << getID() << "' reached next stop." << std::endl;
1848 : }
1849 : #endif
1850 53274 : if (MSStopOut::active()) {
1851 4942 : MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), time);
1852 : }
1853 53274 : myLane->getEdge().addWaiting(this);
1854 53274 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::STARTING_STOP);
1855 53274 : MSNet::getInstance()->getVehicleControl().registerStopStarted();
1856 : // compute stopping time
1857 53274 : stop.duration = stop.getMinDuration(time);
1858 53274 : stop.endBoarding = stop.pars.extension >= 0 ? time + stop.duration + stop.pars.extension : SUMOTime_MAX;
1859 53274 : MSDevice_Taxi* taxiDevice = static_cast<MSDevice_Taxi*>(getDevice(typeid(MSDevice_Taxi)));
1860 2705 : if (taxiDevice != nullptr && stop.pars.extension >= 0) {
1861 : // earliestPickupTime is set with waitUntil
1862 84 : stop.endBoarding = MAX2(time, stop.pars.waitUntil) + stop.pars.extension;
1863 : }
1864 53274 : if (stop.getSpeed() > 0) {
1865 : // ignore duration parameter in waypoint mode unless 'until' or 'ended' are set
1866 3257 : if (stop.getUntil() > time) {
1867 334 : stop.duration = stop.getUntil() - time;
1868 : } else {
1869 2923 : stop.duration = 0;
1870 : }
1871 : }
1872 53274 : if (stop.busstop != nullptr) {
1873 : // let the bus stop know the vehicle
1874 17981 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1875 : }
1876 53274 : if (stop.containerstop != nullptr) {
1877 : // let the container stop know the vehicle
1878 569 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1879 : }
1880 53274 : if (stop.parkingarea != nullptr && stop.getSpeed() <= 0) {
1881 : // let the parking area know the vehicle
1882 8822 : stop.parkingarea->enter(this);
1883 : }
1884 53274 : if (stop.chargingStation != nullptr) {
1885 : // let the container stop know the vehicle
1886 3259 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1887 : }
1888 :
1889 53274 : if (stop.pars.tripId != "") {
1890 2570 : ((SUMOVehicleParameter&)getParameter()).setParameter("tripId", stop.pars.tripId);
1891 : }
1892 53274 : if (stop.pars.line != "") {
1893 1289 : ((SUMOVehicleParameter&)getParameter()).line = stop.pars.line;
1894 : }
1895 53274 : if (stop.pars.split != "") {
1896 : // split the train
1897 1064 : MSVehicle* splitVeh = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(stop.pars.split));
1898 24 : if (splitVeh == nullptr) {
1899 3120 : WRITE_WARNINGF(TL("Vehicle '%' to split from vehicle '%' is not known. time=%."), stop.pars.split, getID(), SIMTIME)
1900 : } else {
1901 24 : MSNet::getInstance()->getInsertionControl().add(splitVeh);
1902 24 : splitVeh->getRoute().getEdges()[0]->removeWaiting(splitVeh);
1903 24 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1904 24 : const double newLength = MAX2(myType->getLength() - splitVeh->getVehicleType().getLength(),
1905 24 : myType->getParameter().locomotiveLength);
1906 24 : getSingularType().setLength(newLength);
1907 : // handle transportables that want to continue in the split part
1908 24 : if (myPersonDevice != nullptr) {
1909 0 : myPersonDevice->transferAtSplitOrJoin(splitVeh);
1910 : }
1911 24 : if (myContainerDevice != nullptr) {
1912 6 : myContainerDevice->transferAtSplitOrJoin(splitVeh);
1913 : }
1914 24 : if (splitVeh->getParameter().departPosProcedure == DepartPosDefinition::SPLIT_FRONT) {
1915 3 : const double backShift = splitVeh->getLength() + getVehicleType().getMinGap();
1916 3 : myState.myPos -= backShift;
1917 3 : myState.myBackPos -= backShift;
1918 : }
1919 : }
1920 : }
1921 :
1922 53274 : boardTransportables(stop);
1923 53270 : if (stop.pars.posLat != INVALID_DOUBLE) {
1924 232 : myState.myPosLat = stop.pars.posLat;
1925 : }
1926 : }
1927 : }
1928 : }
1929 : return currentVelocity;
1930 : }
1931 :
1932 :
1933 : void
1934 23202420 : MSVehicle::boardTransportables(MSStop& stop) {
1935 23202420 : if (stop.skipOnDemand) {
1936 : return;
1937 : }
1938 : // we have reached the stop
1939 : // any waiting persons may board now
1940 22975353 : const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1941 22975353 : MSNet* const net = MSNet::getInstance();
1942 22975353 : const bool boarded = (time <= stop.endBoarding
1943 22973270 : && net->hasPersons()
1944 1202875 : && net->getPersonControl().loadAnyWaiting(&myLane->getEdge(), this, stop.timeToBoardNextPerson, stop.duration)
1945 22980040 : && stop.numExpectedPerson == 0);
1946 : // load containers
1947 22975353 : const bool loaded = (time <= stop.endBoarding
1948 22973270 : && net->hasContainers()
1949 1424861 : && net->getContainerControl().loadAnyWaiting(&myLane->getEdge(), this, stop.timeToLoadNextContainer, stop.duration)
1950 22975928 : && stop.numExpectedContainer == 0);
1951 :
1952 : bool unregister = false;
1953 21652910 : if (time > stop.endBoarding) {
1954 2083 : stop.triggered = false;
1955 2083 : stop.containerTriggered = false;
1956 2083 : if (myAmRegisteredAsWaiting) {
1957 : unregister = true;
1958 332 : myAmRegisteredAsWaiting = false;
1959 : }
1960 : }
1961 21652910 : if (boarded) {
1962 : // the triggering condition has been fulfilled. Maybe we want to wait a bit longer for additional riders (car pooling)
1963 4556 : if (myAmRegisteredAsWaiting) {
1964 : unregister = true;
1965 : }
1966 4556 : stop.triggered = false;
1967 4556 : myAmRegisteredAsWaiting = false;
1968 : }
1969 21652910 : if (loaded) {
1970 : // the triggering condition has been fulfilled
1971 555 : if (myAmRegisteredAsWaiting) {
1972 : unregister = true;
1973 : }
1974 555 : stop.containerTriggered = false;
1975 555 : myAmRegisteredAsWaiting = false;
1976 : }
1977 :
1978 21652910 : if (unregister) {
1979 417 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1980 : #ifdef DEBUG_STOPS
1981 : if (DEBUG_COND) {
1982 : std::cout << SIMTIME << " vehicle '" << getID() << "' unregisters as waiting for transportable." << std::endl;
1983 : }
1984 : #endif
1985 : }
1986 : }
1987 :
1988 : bool
1989 920 : MSVehicle::joinTrainPart(MSVehicle* veh) {
1990 : // check if veh is close enough to be joined to the rear of this vehicle
1991 920 : MSLane* backLane = myFurtherLanes.size() == 0 ? myLane : myFurtherLanes.back();
1992 920 : double gap = getBackPositionOnLane() - veh->getPositionOnLane();
1993 1142 : if (isStopped() && myStops.begin()->duration <= DELTA_T && myStops.begin()->joinTriggered && backLane == veh->getLane()
1994 950 : && gap >= 0 && gap <= getVehicleType().getMinGap() + 1) {
1995 15 : const double newLength = myType->getLength() + veh->getVehicleType().getLength();
1996 15 : getSingularType().setLength(newLength);
1997 15 : myStops.begin()->joinTriggered = false;
1998 15 : if (myAmRegisteredAsWaiting) {
1999 0 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2000 0 : myAmRegisteredAsWaiting = false;
2001 : }
2002 : return true;
2003 : } else {
2004 905 : return false;
2005 : }
2006 : }
2007 :
2008 :
2009 : bool
2010 905 : MSVehicle::joinTrainPartFront(MSVehicle* veh) {
2011 : // check if veh is close enough to be joined to the front of this vehicle
2012 905 : MSLane* backLane = veh->myFurtherLanes.size() == 0 ? veh->myLane : veh->myFurtherLanes.back();
2013 905 : double gap = veh->getBackPositionOnLane(backLane) - getPositionOnLane();
2014 1112 : if (isStopped() && myStops.begin()->duration <= DELTA_T && myStops.begin()->joinTriggered && backLane == getLane()
2015 929 : && gap >= 0 && gap <= getVehicleType().getMinGap() + 1) {
2016 : double skippedLaneLengths = 0;
2017 24 : if (veh->myFurtherLanes.size() > 0) {
2018 9 : skippedLaneLengths += getLane()->getLength();
2019 : // this vehicle must be moved to the lane of veh
2020 : // ensure that lane and furtherLanes of veh match our route
2021 9 : int routeIndex = getRoutePosition();
2022 9 : if (myLane->isInternal()) {
2023 0 : routeIndex++;
2024 : }
2025 27 : for (int i = (int)veh->myFurtherLanes.size() - 1; i >= 0; i--) {
2026 18 : MSEdge* edge = &veh->myFurtherLanes[i]->getEdge();
2027 18 : if (edge->isInternal()) {
2028 9 : continue;
2029 : }
2030 9 : if (!edge->isInternal() && edge != myRoute->getEdges()[routeIndex]) {
2031 0 : std::string warn = TL("Cannot join vehicle '%' to vehicle '%' due to incompatible routes. time=%.");
2032 0 : WRITE_WARNINGF(warn, veh->getID(), getID(), time2string(SIMSTEP));
2033 : return false;
2034 : }
2035 9 : routeIndex++;
2036 : }
2037 9 : if (veh->getCurrentEdge()->getNormalSuccessor() != myRoute->getEdges()[routeIndex]) {
2038 3 : std::string warn = TL("Cannot join vehicle '%' to vehicle '%' due to incompatible routes. time=%.");
2039 9 : WRITE_WARNINGF(warn, veh->getID(), getID(), time2string(SIMSTEP));
2040 : return false;
2041 : }
2042 12 : for (int i = (int)veh->myFurtherLanes.size() - 2; i >= 0; i--) {
2043 6 : skippedLaneLengths += veh->myFurtherLanes[i]->getLength();
2044 : }
2045 : }
2046 :
2047 21 : const double newLength = myType->getLength() + veh->getVehicleType().getLength();
2048 21 : getSingularType().setLength(newLength);
2049 : // lane will be advanced just as for regular movement
2050 21 : myState.myPos = skippedLaneLengths + veh->getPositionOnLane();
2051 21 : myStops.begin()->joinTriggered = false;
2052 21 : if (myAmRegisteredAsWaiting) {
2053 6 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2054 6 : myAmRegisteredAsWaiting = false;
2055 : }
2056 21 : return true;
2057 : } else {
2058 881 : return false;
2059 : }
2060 : }
2061 :
2062 : double
2063 8921693 : MSVehicle::getBrakeGap(bool delayed) const {
2064 8921693 : return getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), delayed ? getCarFollowModel().getHeadwayTime() : 0);
2065 : }
2066 :
2067 :
2068 : bool
2069 675338425 : MSVehicle::checkActionStep(const SUMOTime t) {
2070 675338425 : myActionStep = isActionStep(t);
2071 675338425 : if (myActionStep) {
2072 603822217 : myLastActionTime = t;
2073 : }
2074 675338425 : return myActionStep;
2075 : }
2076 :
2077 :
2078 : void
2079 1605 : MSVehicle::resetActionOffset(const SUMOTime timeUntilNextAction) {
2080 1605 : myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + timeUntilNextAction;
2081 1605 : }
2082 :
2083 :
2084 : void
2085 1 : MSVehicle::updateActionOffset(const SUMOTime oldActionStepLength, const SUMOTime newActionStepLength) {
2086 1 : SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2087 1 : SUMOTime timeSinceLastAction = now - myLastActionTime;
2088 1 : if (timeSinceLastAction == 0) {
2089 : // Action was scheduled now, may be delayed be new action step length
2090 : timeSinceLastAction = oldActionStepLength;
2091 : }
2092 1 : if (timeSinceLastAction >= newActionStepLength) {
2093 : // Action point required in this step
2094 0 : myLastActionTime = now;
2095 : } else {
2096 1 : SUMOTime timeUntilNextAction = newActionStepLength - timeSinceLastAction;
2097 1 : resetActionOffset(timeUntilNextAction);
2098 : }
2099 1 : }
2100 :
2101 :
2102 :
2103 : void
2104 675338425 : MSVehicle::planMove(const SUMOTime t, const MSLeaderInfo& ahead, const double lengthsInFront) {
2105 : #ifdef DEBUG_PLAN_MOVE
2106 : if (DEBUG_COND) {
2107 : std::cout
2108 : << "\nPLAN_MOVE\n"
2109 : << SIMTIME
2110 : << std::setprecision(gPrecision)
2111 : << " veh=" << getID()
2112 : << " lane=" << myLane->getID()
2113 : << " pos=" << getPositionOnLane()
2114 : << " posLat=" << getLateralPositionOnLane()
2115 : << " speed=" << getSpeed()
2116 : << "\n";
2117 : }
2118 : #endif
2119 : // Update the driver state
2120 675338425 : if (hasDriverState()) {
2121 406817 : myDriverState->update();
2122 813634 : setActionStepLength(myDriverState->getDriverState()->getActionStepLength(), false);
2123 : }
2124 :
2125 675338425 : if (!checkActionStep(t)) {
2126 : #ifdef DEBUG_ACTIONSTEPS
2127 : if (DEBUG_COND) {
2128 : std::cout << STEPS2TIME(t) << " vehicle '" << getID() << "' skips action." << std::endl;
2129 : }
2130 : #endif
2131 : // During non-action passed drive items still need to be removed
2132 : // @todo rather work with updating myCurrentDriveItem (refs #3714)
2133 71516208 : removePassedDriveItems();
2134 71516208 : return;
2135 : } else {
2136 : #ifdef DEBUG_ACTIONSTEPS
2137 : if (DEBUG_COND) {
2138 : std::cout << STEPS2TIME(t) << " vehicle = '" << getID() << "' takes action." << std::endl;
2139 : }
2140 : #endif
2141 603822217 : myLFLinkLanesPrev = myLFLinkLanes;
2142 603822217 : if (myInfluencer != nullptr) {
2143 487063 : myInfluencer->updateRemoteControlRoute(this);
2144 : }
2145 603822217 : planMoveInternal(t, ahead, myLFLinkLanes, myStopDist, myNextTurn);
2146 : #ifdef DEBUG_PLAN_MOVE
2147 : if (DEBUG_COND) {
2148 : DriveItemVector::iterator i;
2149 : for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
2150 : std::cout
2151 : << " vPass=" << (*i).myVLinkPass
2152 : << " vWait=" << (*i).myVLinkWait
2153 : << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
2154 : << " request=" << (*i).mySetRequest
2155 : << "\n";
2156 : }
2157 : }
2158 : #endif
2159 603822217 : checkRewindLinkLanes(lengthsInFront, myLFLinkLanes);
2160 603822217 : myNextDriveItem = myLFLinkLanes.begin();
2161 : // ideally would only do this with the call inside planMoveInternal - but that needs a const method
2162 : // so this is a kludge here - nuisance as it adds an extra check in a busy loop
2163 603822217 : if (MSGlobals::gModelParkingManoeuver) {
2164 2971 : if (getManoeuvreType() == MSVehicle::MANOEUVRE_EXIT && manoeuvreIsComplete()) {
2165 30 : setManoeuvreType(MSVehicle::MANOEUVRE_NONE);
2166 : }
2167 : }
2168 : }
2169 603822217 : myLaneChangeModel->resetChanged();
2170 : }
2171 :
2172 :
2173 : bool
2174 167633330 : MSVehicle::brakeForOverlap(const MSLink* link, const MSLane* lane) const {
2175 : // @review needed
2176 : //const double futurePosLat = getLateralPositionOnLane() + link->getLateralShift();
2177 : //const double overlap = getLateralOverlap(futurePosLat, link->getViaLaneOrLane());
2178 : //const double edgeWidth = link->getViaLaneOrLane()->getEdge().getWidth();
2179 167633330 : const double futurePosLat = getLateralPositionOnLane() + (
2180 167633330 : lane != myLane && lane->isInternal() ? lane->getIncomingLanes()[0].viaLink->getLateralShift() : 0);
2181 167633330 : const double overlap = getLateralOverlap(futurePosLat, lane);
2182 : const double edgeWidth = lane->getEdge().getWidth();
2183 : const bool result = (overlap > POSITION_EPS
2184 : // do not get stuck on narrow edges
2185 2870707 : && getVehicleType().getWidth() <= edgeWidth
2186 2866422 : && link->getViaLane() == nullptr
2187 : // this is the exit link of a junction. The normal edge should support the shadow
2188 1382250 : && ((myLaneChangeModel->getShadowLane(link->getLane()) == nullptr)
2189 : // the internal lane after an internal junction has no parallel lane. make sure there is no shadow before continuing
2190 1013920 : || (lane->getEdge().isInternal() && lane->getIncomingLanes()[0].lane->getEdge().isInternal()))
2191 : // ignore situations where the shadow lane is part of a double-connection with the current lane
2192 382996 : && (myLaneChangeModel->getShadowLane() == nullptr
2193 217211 : || myLaneChangeModel->getShadowLane()->getLinkCont().size() == 0
2194 204052 : || myLaneChangeModel->getShadowLane()->getLinkCont().front()->getLane() != link->getLane())
2195 : // emergency vehicles may do some crazy stuff
2196 167954054 : && !myLaneChangeModel->hasBlueLight());
2197 :
2198 : #ifdef DEBUG_PLAN_MOVE
2199 : if (DEBUG_COND) {
2200 : std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getDescription() << " lane=" << lane->getID()
2201 : << " shift=" << link->getLateralShift()
2202 : << " fpLat=" << futurePosLat << " overlap=" << overlap << " w=" << getVehicleType().getWidth() << " result=" << result << "\n";
2203 : }
2204 : #endif
2205 167633330 : return result;
2206 : }
2207 :
2208 :
2209 :
2210 : void
2211 603822217 : MSVehicle::planMoveInternal(const SUMOTime t, MSLeaderInfo ahead, DriveItemVector& lfLinks, double& newStopDist, std::pair<double, const MSLink*>& nextTurn) const {
2212 : lfLinks.clear();
2213 603822217 : newStopDist = std::numeric_limits<double>::max();
2214 : //
2215 : const MSCFModel& cfModel = getCarFollowModel();
2216 603822217 : const double vehicleLength = getVehicleType().getLength();
2217 603822217 : const double maxV = cfModel.maxNextSpeed(myState.mySpeed, this);
2218 603822217 : const double maxVD = MAX2(getMaxSpeed(), MIN2(maxV, getDesiredMaxSpeed()));
2219 603822217 : const bool opposite = myLaneChangeModel->isOpposite();
2220 : // maxVD is possibly higher than vType-maxSpeed and in this case laneMaxV may be higher as well
2221 603822217 : double laneMaxV = myLane->getVehicleMaxSpeed(this, maxVD);
2222 603822217 : const double vMinComfortable = cfModel.minNextSpeed(getSpeed(), this);
2223 : double lateralShift = 0;
2224 603822217 : if (isRail()) {
2225 : // speed limits must hold for the whole length of the train
2226 1557294 : for (MSLane* l : myFurtherLanes) {
2227 339449 : laneMaxV = MIN2(laneMaxV, l->getVehicleMaxSpeed(this, maxVD));
2228 : #ifdef DEBUG_PLAN_MOVE
2229 : if (DEBUG_COND) {
2230 : std::cout << " laneMaxV=" << laneMaxV << " lane=" << l->getID() << "\n";
2231 : }
2232 : #endif
2233 : }
2234 : }
2235 : // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
2236 : laneMaxV = MAX2(laneMaxV, vMinComfortable);
2237 604309248 : if (myInfluencer && !myInfluencer->considerSpeedLimit()) {
2238 : laneMaxV = std::numeric_limits<double>::max();
2239 : }
2240 : // v is the initial maximum velocity of this vehicle in this step
2241 603822217 : double v = cfModel.maximumLaneSpeedCF(this, maxV, laneMaxV);
2242 : // if we are modelling parking then we dawdle until the manoeuvre is complete - by setting a very low max speed
2243 : // in practice this only applies to exit manoeuvre because entry manoeuvre just delays setting stop.reached - when the vehicle is virtually stopped
2244 603822217 : if (MSGlobals::gModelParkingManoeuver && !manoeuvreIsComplete()) {
2245 420 : v = NUMERICAL_EPS_SPEED;
2246 : }
2247 :
2248 603822217 : if (myInfluencer != nullptr) {
2249 487063 : const double vMin = MAX2(0., cfModel.minNextSpeed(myState.mySpeed, this));
2250 : #ifdef DEBUG_TRACI
2251 : if (DEBUG_COND) {
2252 : std::cout << SIMTIME << " veh=" << getID() << " speedBeforeTraci=" << v;
2253 : }
2254 : #endif
2255 487063 : v = myInfluencer->influenceSpeed(t, v, v, vMin, maxV);
2256 : #ifdef DEBUG_TRACI
2257 : if (DEBUG_COND) {
2258 : std::cout << " influencedSpeed=" << v;
2259 : }
2260 : #endif
2261 487063 : v = myInfluencer->gapControlSpeed(t, this, v, v, vMin, maxV);
2262 : #ifdef DEBUG_TRACI
2263 : if (DEBUG_COND) {
2264 : std::cout << " gapControlSpeed=" << v << "\n";
2265 : }
2266 : #endif
2267 : }
2268 : // all links within dist are taken into account (potentially)
2269 603822217 : const double dist = SPEED2DIST(maxV) + cfModel.brakeGap(maxV);
2270 :
2271 603822217 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation();
2272 : #ifdef DEBUG_PLAN_MOVE
2273 : if (DEBUG_COND) {
2274 : std::cout << " dist=" << dist << " bestLaneConts=" << toString(bestLaneConts)
2275 : << "\n maxV=" << maxV << " laneMaxV=" << laneMaxV << " v=" << v << "\n";
2276 : }
2277 : #endif
2278 : assert(bestLaneConts.size() > 0);
2279 : bool hadNonInternal = false;
2280 : // the distance already "seen"; in the following always up to the end of the current "lane"
2281 603822217 : double seen = opposite ? myState.myPos : myLane->getLength() - myState.myPos;
2282 603822217 : nextTurn.first = seen;
2283 603822217 : nextTurn.second = nullptr;
2284 603822217 : bool encounteredTurn = (MSGlobals::gLateralResolution <= 0); // next turn is only needed for sublane
2285 : double seenNonInternal = 0;
2286 603822217 : double seenInternal = myLane->isInternal() ? seen : 0;
2287 603822217 : double vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(seen, v, cfModel.getMaxAccel()), laneMaxV); // upper bound
2288 : int view = 0;
2289 : DriveProcessItem* lastLink = nullptr;
2290 : bool slowedDownForMinor = false; // whether the vehicle already had to slow down on approach to a minor link
2291 : double mustSeeBeforeReversal = 0;
2292 : // iterator over subsequent lanes and fill lfLinks until stopping distance or stopped
2293 603822217 : const MSLane* lane = opposite ? myLane->getParallelOpposite() : myLane;
2294 : assert(lane != 0);
2295 603822217 : const MSLane* leaderLane = myLane;
2296 603822217 : bool foundRailSignal = !isRail();
2297 : bool planningToStop = false;
2298 : #ifdef PARALLEL_STOPWATCH
2299 : myLane->getStopWatch()[0].start();
2300 : #endif
2301 :
2302 : // optionally slow down to match arrival time
2303 603822217 : const double sfp = getVehicleType().getParameter().speedFactorPremature;
2304 603811449 : if (v > vMinComfortable && hasStops() && myStops.front().pars.arrival >= 0 && sfp > 0
2305 4279 : && v > myLane->getSpeedLimit() * sfp
2306 603825253 : && !myStops.front().reached) {
2307 2786 : const double vSlowDown = slowDownForSchedule(vMinComfortable);
2308 5403 : v = MIN2(v, vSlowDown);
2309 : }
2310 : auto stopIt = myStops.begin();
2311 : while (true) {
2312 : // check leader on lane
2313 : // leader is given for the first edge only
2314 1144152115 : if (opposite &&
2315 : (leaderLane->getVehicleNumberWithPartials() > 1
2316 100730 : || (leaderLane != myLane && leaderLane->getVehicleNumber() > 0))) {
2317 401260 : ahead.clear();
2318 : // find opposite-driving leader that must be respected on the currently looked at lane
2319 : // (only looking at one lane at a time)
2320 401260 : const double backOffset = leaderLane == myLane ? getPositionOnLane() : leaderLane->getLength();
2321 401260 : const double gapOffset = leaderLane == myLane ? 0 : seen - leaderLane->getLength();
2322 401260 : const MSLeaderDistanceInfo cands = leaderLane->getFollowersOnConsecutive(this, backOffset, true, backOffset, MSLane::MinorLinkMode::FOLLOW_NEVER);
2323 401260 : MSLeaderDistanceInfo oppositeLeaders(leaderLane->getWidth(), this, 0.);
2324 401260 : const double minTimeToLeaveLane = MSGlobals::gSublane ? MAX2(TS, (0.5 * myLane->getWidth() - getLateralPositionOnLane()) / getVehicleType().getMaxSpeedLat()) : TS;
2325 1065250 : for (int i = 0; i < cands.numSublanes(); i++) {
2326 663990 : CLeaderDist cand = cands[i];
2327 663990 : if (cand.first != 0) {
2328 567760 : if ((cand.first->myLaneChangeModel->isOpposite() && cand.first->getLaneChangeModel().getShadowLane() != leaderLane)
2329 568235 : || (!cand.first->myLaneChangeModel->isOpposite() && cand.first->getLaneChangeModel().getShadowLane() == leaderLane)) {
2330 : // respect leaders that also drive in the opposite direction (fully or with some overlap)
2331 358276 : oppositeLeaders.addLeader(cand.first, cand.second + gapOffset - getVehicleType().getMinGap() + cand.first->getVehicleType().getMinGap() - cand.first->getVehicleType().getLength());
2332 : } else {
2333 : // avoid frontal collision
2334 384894 : const bool assumeStopped = cand.first->isStopped() || cand.first->getWaitingSeconds() > 1;
2335 209484 : const double predMaxDist = cand.first->getSpeed() + (assumeStopped ? 0 : cand.first->getCarFollowModel().getMaxAccel()) * minTimeToLeaveLane;
2336 209484 : if (cand.second >= 0 && (cand.second - v * minTimeToLeaveLane - predMaxDist < 0 || assumeStopped)) {
2337 48135 : oppositeLeaders.addLeader(cand.first, cand.second + gapOffset - predMaxDist - getVehicleType().getMinGap());
2338 : }
2339 : }
2340 : }
2341 : }
2342 : #ifdef DEBUG_PLAN_MOVE
2343 : if (DEBUG_COND) {
2344 : std::cout << " leaderLane=" << leaderLane->getID() << " gapOffset=" << gapOffset << " minTimeToLeaveLane=" << minTimeToLeaveLane
2345 : << " cands=" << cands.toString() << " oppositeLeaders=" << oppositeLeaders.toString() << "\n";
2346 : }
2347 : #endif
2348 401260 : adaptToLeaderDistance(oppositeLeaders, 0, seen, lastLink, v, vLinkPass);
2349 401260 : } else {
2350 1143750855 : if (MSGlobals::gLateralResolution > 0 && myLaneChangeModel->getShadowLane() == nullptr) {
2351 189695949 : const double rightOL = getRightSideOnLane(lane) + lateralShift;
2352 189695949 : const double leftOL = getLeftSideOnLane(lane) + lateralShift;
2353 : const bool outsideLeft = leftOL > lane->getWidth();
2354 : #ifdef DEBUG_PLAN_MOVE
2355 : if (DEBUG_COND) {
2356 : std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
2357 : }
2358 : #endif
2359 189695949 : if (rightOL < 0 || outsideLeft) {
2360 1216473 : MSLeaderInfo outsideLeaders(lane->getWidth());
2361 : // if ego is driving outside lane bounds we must consider
2362 : // potential leaders that are also outside bounds
2363 : int sublaneOffset = 0;
2364 1216473 : if (outsideLeft) {
2365 543542 : sublaneOffset = MIN2(-1, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution));
2366 : } else {
2367 672931 : sublaneOffset = MAX2(1, (int)ceil(-rightOL / MSGlobals::gLateralResolution));
2368 : }
2369 1216473 : outsideLeaders.setSublaneOffset(sublaneOffset);
2370 : #ifdef DEBUG_PLAN_MOVE
2371 : if (DEBUG_COND) {
2372 : std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
2373 : }
2374 : #endif
2375 5619701 : for (const MSVehicle* cand : lane->getVehiclesSecure()) {
2376 1799611 : if ((lane != myLane || cand->getPositionOnLane() > getPositionOnLane())
2377 5178663 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
2378 3378799 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
2379 218850 : outsideLeaders.addLeader(cand, true);
2380 : #ifdef DEBUG_PLAN_MOVE
2381 : if (DEBUG_COND) {
2382 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << outsideLeaders.toString() << "\n";
2383 : }
2384 : #endif
2385 : }
2386 : }
2387 1216473 : lane->releaseVehicles();
2388 1216473 : if (outsideLeaders.hasVehicles()) {
2389 36213 : adaptToLeaders(outsideLeaders, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2390 : }
2391 1216473 : }
2392 : }
2393 1143750855 : adaptToLeaders(ahead, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2394 : }
2395 1144152115 : if (lastLink != nullptr) {
2396 1014545729 : lastLink->myVLinkWait = MIN2(lastLink->myVLinkWait, v);
2397 : }
2398 : #ifdef DEBUG_PLAN_MOVE
2399 : if (DEBUG_COND) {
2400 : std::cout << "\nv = " << v << "\n";
2401 :
2402 : }
2403 : #endif
2404 : // XXX efficiently adapt to shadow leaders using neighAhead by iteration over the whole edge in parallel (lanechanger-style)
2405 1144152115 : if (myLaneChangeModel->getShadowLane() != nullptr) {
2406 : // also slow down for leaders on the shadowLane relative to the current lane
2407 4821342 : const MSLane* shadowLane = myLaneChangeModel->getShadowLane(leaderLane);
2408 : if (shadowLane != nullptr
2409 4821342 : && (MSGlobals::gLateralResolution > 0 || getLateralOverlap() > POSITION_EPS
2410 : // continous lane change cannot be stopped so we must adapt to the leader on the target lane
2411 190992 : || myLaneChangeModel->getLaneChangeCompletion() < 0.5)) {
2412 4306145 : if ((&shadowLane->getEdge() == &leaderLane->getEdge() || myLaneChangeModel->isOpposite())) {
2413 4264765 : double latOffset = getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge();
2414 4264765 : if (myLaneChangeModel->isOpposite()) {
2415 : // ego posLat is added when retrieving sublanes but it
2416 : // should be negated (subtract twice to compensate)
2417 138465 : latOffset = ((myLane->getWidth() + shadowLane->getWidth()) * 0.5
2418 138465 : - 2 * getLateralPositionOnLane());
2419 :
2420 : }
2421 4264765 : MSLeaderInfo shadowLeaders = shadowLane->getLastVehicleInformation(this, latOffset, lane->getLength() - seen);
2422 : #ifdef DEBUG_PLAN_MOVE
2423 : if (DEBUG_COND && myLaneChangeModel->isOpposite()) {
2424 : std::cout << SIMTIME << " opposite veh=" << getID() << " shadowLane=" << shadowLane->getID() << " latOffset=" << latOffset << " shadowLeaders=" << shadowLeaders.toString() << "\n";
2425 : }
2426 : #endif
2427 4264765 : if (myLaneChangeModel->isOpposite()) {
2428 : // ignore oncoming vehicles on the shadow lane
2429 138465 : shadowLeaders.removeOpposite(shadowLane);
2430 : }
2431 4264765 : const double turningDifference = MAX2(0.0, leaderLane->getLength() - shadowLane->getLength());
2432 4264765 : adaptToLeaders(shadowLeaders, latOffset, seen - turningDifference, lastLink, shadowLane, v, vLinkPass);
2433 4306145 : } else if (shadowLane == myLaneChangeModel->getShadowLane() && leaderLane == myLane) {
2434 : // check for leader vehicles driving in the opposite direction on the opposite-direction shadow lane
2435 : // (and thus in the same direction as ego)
2436 30397 : MSLeaderDistanceInfo shadowLeaders = shadowLane->getFollowersOnConsecutive(this, myLane->getOppositePos(getPositionOnLane()), true);
2437 : const double latOffset = 0;
2438 : #ifdef DEBUG_PLAN_MOVE
2439 : if (DEBUG_COND) {
2440 : std::cout << SIMTIME << " opposite shadows veh=" << getID() << " shadowLane=" << shadowLane->getID()
2441 : << " latOffset=" << latOffset << " shadowLeaders=" << shadowLeaders.toString() << "\n";
2442 : }
2443 : #endif
2444 30397 : shadowLeaders.fixOppositeGaps(true);
2445 : #ifdef DEBUG_PLAN_MOVE
2446 : if (DEBUG_COND) {
2447 : std::cout << " shadowLeadersFixed=" << shadowLeaders.toString() << "\n";
2448 : }
2449 : #endif
2450 30397 : adaptToLeaderDistance(shadowLeaders, latOffset, seen, lastLink, v, vLinkPass);
2451 30397 : }
2452 : }
2453 : }
2454 : // adapt to pedestrians on the same lane
2455 1144152115 : if (lane->getEdge().getPersons().size() > 0 && lane->hasPedestrians()) {
2456 193669 : const double relativePos = lane->getLength() - seen;
2457 : #ifdef DEBUG_PLAN_MOVE
2458 : if (DEBUG_COND) {
2459 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2460 : }
2461 : #endif
2462 193669 : const double stopTime = MAX2(1.0, ceil(getSpeed() / cfModel.getMaxDecel()));
2463 193669 : PersonDist leader = lane->nextBlocking(relativePos,
2464 193669 : getRightSideOnLane(lane), getRightSideOnLane(lane) + getVehicleType().getWidth(), stopTime);
2465 193669 : if (leader.first != 0) {
2466 21338 : const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2467 30106 : v = MIN2(v, stopSpeed);
2468 : #ifdef DEBUG_PLAN_MOVE
2469 : if (DEBUG_COND) {
2470 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2471 : }
2472 : #endif
2473 : }
2474 : }
2475 1144152115 : if (lane->getBidiLane() != nullptr) {
2476 : // adapt to pedestrians on the bidi lane
2477 3753304 : const MSLane* bidiLane = lane->getBidiLane();
2478 3753304 : if (bidiLane->getEdge().getPersons().size() > 0 && bidiLane->hasPedestrians()) {
2479 1028 : const double relativePos = seen;
2480 : #ifdef DEBUG_PLAN_MOVE
2481 : if (DEBUG_COND) {
2482 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2483 : }
2484 : #endif
2485 1028 : const double stopTime = ceil(getSpeed() / cfModel.getMaxDecel());
2486 1028 : const double leftSideOnLane = bidiLane->getWidth() - getRightSideOnLane(lane);
2487 1028 : PersonDist leader = bidiLane->nextBlocking(relativePos,
2488 1028 : leftSideOnLane - getVehicleType().getWidth(), leftSideOnLane, stopTime, true);
2489 1028 : if (leader.first != 0) {
2490 267 : const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2491 516 : v = MIN2(v, stopSpeed);
2492 : #ifdef DEBUG_PLAN_MOVE
2493 : if (DEBUG_COND) {
2494 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2495 : }
2496 : #endif
2497 : }
2498 : }
2499 : }
2500 : // adapt to vehicles blocked from (urgent) lane-changing
2501 1144152115 : if (!opposite && lane->getEdge().hasLaneChanger()) {
2502 559120109 : v = MIN2(v, myLaneChangeModel->getCooperativeHelpSpeed(lane, seen));
2503 : }
2504 :
2505 : // process all stops and waypoints on the current edge
2506 : bool foundRealStop = false;
2507 : while (stopIt != myStops.end()
2508 49158075 : && ((&stopIt->lane->getEdge() == &lane->getEdge())
2509 23502099 : || (stopIt->isOpposite && stopIt->lane->getEdge().getOppositeEdge() == &lane->getEdge()))
2510 : // ignore stops that occur later in a looped route
2511 1185895138 : && stopIt->edge == myCurrEdge + view) {
2512 25609636 : double stopDist = std::numeric_limits<double>::max();
2513 : const MSStop& stop = *stopIt;
2514 : const bool isFirstStop = stopIt == myStops.begin();
2515 : stopIt++;
2516 25609636 : if (!stop.reached || (stop.getSpeed() > 0 && keepStopping())) {
2517 : // we are approaching a stop on the edge; must not drive further
2518 10826841 : bool isWaypoint = stop.getSpeed() > 0;
2519 10826841 : double endPos = stop.getEndPos(*this) + NUMERICAL_EPS;
2520 10826841 : if (stop.parkingarea != nullptr) {
2521 : // leave enough space so parking vehicles can exit
2522 1568595 : const double brakePos = getBrakeGap() + lane->getLength() - seen;
2523 1568595 : endPos = stop.parkingarea->getLastFreePosWithReservation(t, *this, brakePos);
2524 9258246 : } else if (isWaypoint && !stop.reached) {
2525 101847 : endPos = stop.pars.startPos;
2526 : }
2527 10826841 : stopDist = seen + endPos - lane->getLength();
2528 : #ifdef DEBUG_STOPS
2529 : if (DEBUG_COND) {
2530 : std::cout << SIMTIME << " veh=" << getID() << " stopDist=" << stopDist << " stopLane=" << stop.lane->getID() << " stopEndPos=" << endPos << "\n";
2531 : }
2532 : #endif
2533 : // regular stops are not emergencies
2534 : double stopSpeed = laneMaxV;
2535 10826841 : if (isWaypoint) {
2536 : bool waypointWithStop = false;
2537 117202 : if (stop.getUntil() > t) {
2538 : // check if we have to slow down or even stop
2539 : SUMOTime time2end = 0;
2540 4051 : if (stop.reached) {
2541 800 : time2end = TIME2STEPS((stop.pars.endPos - myState.myPos) / stop.getSpeed());
2542 : } else {
2543 3437 : time2end = TIME2STEPS(
2544 : // time to reach waypoint start
2545 : stopDist / ((getSpeed() + stop.getSpeed()) / 2)
2546 : // time to reach waypoint end
2547 : + (stop.pars.endPos - stop.pars.startPos) / stop.getSpeed());
2548 : }
2549 4051 : if (stop.getUntil() > t + time2end) {
2550 : // we need to stop
2551 : double distToEnd = stopDist;
2552 3915 : if (!stop.reached) {
2553 3191 : distToEnd += stop.pars.endPos - stop.pars.startPos;
2554 : }
2555 3915 : stopSpeed = MAX2(cfModel.stopSpeed(this, getSpeed(), distToEnd), vMinComfortable);
2556 : waypointWithStop = true;
2557 : }
2558 : }
2559 117202 : if (stop.reached) {
2560 14602 : stopSpeed = MIN2(stop.getSpeed(), stopSpeed);
2561 14602 : if (myState.myPos >= stop.pars.endPos && !waypointWithStop) {
2562 263 : stopDist = std::numeric_limits<double>::max();
2563 : }
2564 : } else {
2565 102600 : stopSpeed = MIN2(MAX2(cfModel.freeSpeed(this, getSpeed(), stopDist, stop.getSpeed()), vMinComfortable), stopSpeed);
2566 102600 : if (!stop.reached) {
2567 102600 : stopDist += stop.pars.endPos - stop.pars.startPos;
2568 : }
2569 102600 : if (lastLink != nullptr) {
2570 64827 : lastLink->adaptLeaveSpeed(cfModel.freeSpeed(this, vLinkPass, endPos, stop.getSpeed(), false, MSCFModel::CalcReason::FUTURE));
2571 : }
2572 : }
2573 : } else {
2574 10709639 : stopSpeed = MAX2(cfModel.stopSpeed(this, getSpeed(), stopDist), vMinComfortable);
2575 10709639 : if (lastLink != nullptr) {
2576 4739723 : lastLink->adaptLeaveSpeed(cfModel.stopSpeed(this, vLinkPass, endPos, MSCFModel::CalcReason::FUTURE));
2577 : }
2578 : }
2579 10826841 : v = MIN2(v, stopSpeed);
2580 10826841 : if (lane->isInternal()) {
2581 6636 : std::vector<MSLink*>::const_iterator exitLink = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2582 : assert(!lane->isLinkEnd(exitLink));
2583 : bool dummySetRequest;
2584 : double dummyVLinkWait;
2585 6636 : checkLinkLeaderCurrentAndParallel(*exitLink, lane, seen, lastLink, v, vLinkPass, dummyVLinkWait, dummySetRequest);
2586 : }
2587 :
2588 : #ifdef DEBUG_PLAN_MOVE
2589 : if (DEBUG_COND) {
2590 : std::cout << "\n" << SIMTIME << " next stop: distance = " << stopDist << " requires stopSpeed = " << stopSpeed << "\n";
2591 :
2592 : }
2593 : #endif
2594 10826841 : if (isFirstStop) {
2595 9909102 : newStopDist = stopDist;
2596 : // if the vehicle is going to stop we don't need to look further
2597 : // (except for trains that make use of further link-approach registration for safety purposes)
2598 9909102 : if (!isWaypoint) {
2599 : planningToStop = true;
2600 9826469 : if (!isRail()) {
2601 9525750 : lfLinks.emplace_back(v, stopDist);
2602 : foundRealStop = true;
2603 9525750 : break;
2604 : }
2605 : }
2606 : }
2607 : }
2608 : }
2609 : if (foundRealStop) {
2610 : break;
2611 : }
2612 :
2613 : // move to next lane
2614 : // get the next link used
2615 1134626365 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2616 1134626365 : if (lane->isLinkEnd(link) && myLaneChangeModel->hasBlueLight() && myCurrEdge != myRoute->end() - 1) {
2617 : // emergency vehicle is on the wrong lane. Obtain the link that it would use from the correct turning lane
2618 : const int currentIndex = lane->getIndex();
2619 : const MSLane* bestJump = nullptr;
2620 167993 : for (const LaneQ& preb : getBestLanes()) {
2621 110928 : if (preb.allowsContinuation &&
2622 : (bestJump == nullptr
2623 3216 : || abs(currentIndex - preb.lane->getIndex()) < abs(currentIndex - bestJump->getIndex()))) {
2624 58070 : bestJump = preb.lane;
2625 : }
2626 : }
2627 57065 : if (bestJump != nullptr) {
2628 57065 : const MSEdge* nextEdge = *(myCurrEdge + 1);
2629 105950 : for (auto cand_it = bestJump->getLinkCont().begin(); cand_it != bestJump->getLinkCont().end(); cand_it++) {
2630 102609 : if (&(*cand_it)->getLane()->getEdge() == nextEdge) {
2631 : link = cand_it;
2632 : break;
2633 : }
2634 : }
2635 : }
2636 : }
2637 :
2638 : // Check whether this is a turn (to save info about the next upcoming turn)
2639 1134626365 : if (!encounteredTurn) {
2640 184266200 : if (!lane->isLinkEnd(link) && lane->getLinkCont().size() > 1) {
2641 17851997 : LinkDirection linkDir = (*link)->getDirection();
2642 17851997 : switch (linkDir) {
2643 : case LinkDirection::STRAIGHT:
2644 : case LinkDirection::NODIR:
2645 : break;
2646 6725553 : default:
2647 6725553 : nextTurn.first = seen;
2648 6725553 : nextTurn.second = *link;
2649 : encounteredTurn = true;
2650 : #ifdef DEBUG_NEXT_TURN
2651 : if (DEBUG_COND) {
2652 : std::cout << SIMTIME << " veh '" << getID() << "' nextTurn: " << toString(linkDir)
2653 : << " at " << nextTurn.first << "m." << std::endl;
2654 : }
2655 : #endif
2656 : }
2657 : }
2658 : }
2659 :
2660 : // check whether the vehicle is on its final edge
2661 1950077933 : if (myCurrEdge + view + 1 == myRoute->end()
2662 1134626365 : || (myParameter->arrivalEdge >= 0 && getRoutePosition() + view == myParameter->arrivalEdge)) {
2663 319174797 : const double arrivalSpeed = (myParameter->arrivalSpeedProcedure == ArrivalSpeedDefinition::GIVEN ?
2664 : myParameter->arrivalSpeed : laneMaxV);
2665 : // subtract the arrival speed from the remaining distance so we get one additional driving step with arrival speed
2666 : // XXX: This does not work for ballistic update refs #2579
2667 319174797 : const double distToArrival = seen + myArrivalPos - lane->getLength() - SPEED2DIST(arrivalSpeed);
2668 319174797 : const double va = MAX2(NUMERICAL_EPS, cfModel.freeSpeed(this, getSpeed(), distToArrival, arrivalSpeed));
2669 319174797 : v = MIN2(v, va);
2670 319174797 : if (lastLink != nullptr) {
2671 : lastLink->adaptLeaveSpeed(va);
2672 : }
2673 319174797 : lfLinks.push_back(DriveProcessItem(v, seen, lane->getEdge().isFringe() ? 1000 : 0));
2674 319174797 : break;
2675 : }
2676 : // check whether the lane or the shadowLane is a dead end (allow some leeway on intersections)
2677 : if (lane->isLinkEnd(link)
2678 805820216 : || (MSGlobals::gSublane && brakeForOverlap(*link, lane))
2679 1621000212 : || (opposite && (*link)->getViaLaneOrLane()->getParallelOpposite() == nullptr
2680 212657 : && !myLaneChangeModel->hasBlueLight())) {
2681 10154261 : double va = cfModel.stopSpeed(this, getSpeed(), seen);
2682 10154261 : if (lastLink != nullptr) {
2683 : lastLink->adaptLeaveSpeed(va);
2684 : }
2685 10154261 : if (myLaneChangeModel->getCommittedSpeed() > 0) {
2686 335560 : v = MIN2(myLaneChangeModel->getCommittedSpeed(), v);
2687 : } else {
2688 19112266 : v = MIN2(va, v);
2689 : }
2690 : #ifdef DEBUG_PLAN_MOVE
2691 : if (DEBUG_COND) {
2692 : std::cout << " braking for link end lane=" << lane->getID() << " seen=" << seen
2693 : << " overlap=" << getLateralOverlap() << " va=" << va << " committed=" << myLaneChangeModel->getCommittedSpeed() << " v=" << v << "\n";
2694 :
2695 : }
2696 : #endif
2697 10154261 : if (lane->isLinkEnd(link)) {
2698 9631352 : lfLinks.emplace_back(v, seen);
2699 : break;
2700 : }
2701 : }
2702 805820216 : lateralShift += (*link)->getLateralShift();
2703 805820216 : const bool yellowOrRed = (*link)->haveRed() || (*link)->haveYellow();
2704 : // We distinguish 3 cases when determining the point at which a vehicle stops:
2705 : // - allway_stop: the vehicle should stop close to the stop line but may stop at larger distance
2706 : // - red/yellow light: here the vehicle 'knows' that it will have priority eventually and does not need to stop on a precise spot
2707 : // - other types of minor links: the vehicle needs to stop as close to the junction as necessary
2708 : // to minimize the time window for passing the junction. If the
2709 : // vehicle 'decides' to accelerate and cannot enter the junction in
2710 : // the next step, new foes may appear and cause a collision (see #1096)
2711 : // - major links: stopping point is irrelevant
2712 : double laneStopOffset;
2713 805820216 : const double majorStopOffset = MAX2(getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP, DIST_TO_STOPLINE_EXPECT_PRIORITY), lane->getVehicleStopOffset(this));
2714 : // override low desired decel at yellow and red
2715 805820216 : const double stopDecel = yellowOrRed && !isRail() ? MAX2(MIN2(MSGlobals::gTLSYellowMinDecel, cfModel.getEmergencyDecel()), cfModel.getMaxDecel()) : cfModel.getMaxDecel();
2716 805820216 : const double brakeDist = cfModel.brakeGap(myState.mySpeed, stopDecel, 0);
2717 805820216 : const bool canBrakeBeforeLaneEnd = seen >= brakeDist;
2718 805820216 : const bool canBrakeBeforeStopLine = seen - lane->getVehicleStopOffset(this) >= brakeDist;
2719 805820216 : if (yellowOrRed) {
2720 : // Wait at red traffic light with full distance if possible
2721 : laneStopOffset = majorStopOffset;
2722 750711821 : } else if ((*link)->havePriority()) {
2723 : // On priority link, we should never stop below visibility distance
2724 711941177 : laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, majorStopOffset);
2725 : } else {
2726 38770644 : double minorStopOffset = MAX2(lane->getVehicleStopOffset(this),
2727 38770644 : getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP) - (*link)->getDistToFoePedCrossing());
2728 : #ifdef DEBUG_PLAN_MOVE
2729 : if (DEBUG_COND) {
2730 : std::cout << " minorStopOffset=" << minorStopOffset << " distToFoePedCrossing=" << (*link)->getDistToFoePedCrossing() << "\n";
2731 : }
2732 : #endif
2733 38770644 : if ((*link)->getState() == LINKSTATE_ALLWAY_STOP) {
2734 1361478 : minorStopOffset = MAX2(minorStopOffset, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP, 0));
2735 : } else {
2736 37409166 : minorStopOffset = MAX2(minorStopOffset, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP_MINOR, 0));
2737 : }
2738 : // On minor link, we should likewise never stop below visibility distance
2739 38770644 : laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, minorStopOffset);
2740 : }
2741 : #ifdef DEBUG_PLAN_MOVE
2742 : if (DEBUG_COND) {
2743 : std::cout << SIMTIME << " veh=" << getID() << " desired stopOffset on lane '" << lane->getID() << "' is " << laneStopOffset << "\n";
2744 : }
2745 : #endif
2746 805820216 : if (canBrakeBeforeLaneEnd) {
2747 : // avoid emergency braking if possible
2748 778997261 : laneStopOffset = MIN2(laneStopOffset, seen - brakeDist);
2749 : }
2750 : laneStopOffset = MAX2(POSITION_EPS, laneStopOffset);
2751 805820216 : double stopDist = MAX2(0., seen - laneStopOffset);
2752 55108395 : if (yellowOrRed && getDevice(typeid(MSDevice_GLOSA)) != nullptr
2753 508 : && static_cast<MSDevice_GLOSA*>(getDevice(typeid(MSDevice_GLOSA)))->getOverrideSafety()
2754 805820216 : && static_cast<MSDevice_GLOSA*>(getDevice(typeid(MSDevice_GLOSA)))->isSpeedAdviceActive()) {
2755 : stopDist = std::numeric_limits<double>::max();
2756 : }
2757 805820216 : if (newStopDist != std::numeric_limits<double>::max()) {
2758 : stopDist = MAX2(stopDist, newStopDist);
2759 : }
2760 : #ifdef DEBUG_PLAN_MOVE
2761 : if (DEBUG_COND) {
2762 : std::cout << SIMTIME << " veh=" << getID() << " effective stopOffset on lane '" << lane->getID()
2763 : << "' is " << laneStopOffset << " (-> stopDist=" << stopDist << ")" << std::endl;
2764 : }
2765 : #endif
2766 805820216 : if (isRail()
2767 805820216 : && !lane->isInternal()) {
2768 : // check for train direction reversal
2769 2928671 : if (lane->getBidiLane() != nullptr
2770 2928671 : && (*link)->getLane()->getBidiLane() == lane) {
2771 629988 : double vMustReverse = getCarFollowModel().stopSpeed(this, getSpeed(), seen - POSITION_EPS);
2772 629988 : if (seen < 1) {
2773 2261 : mustSeeBeforeReversal = 2 * seen + getLength();
2774 : }
2775 1219331 : v = MIN2(v, vMustReverse);
2776 : }
2777 : // signal that is passed in the current step does not count
2778 5857342 : foundRailSignal |= ((*link)->getTLLogic() != nullptr
2779 641171 : && (*link)->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL
2780 3480649 : && seen > SPEED2DIST(v));
2781 : }
2782 :
2783 805820216 : bool canReverseEventually = false;
2784 805820216 : const double vReverse = checkReversal(canReverseEventually, laneMaxV, seen);
2785 805820216 : v = MIN2(v, vReverse);
2786 : #ifdef DEBUG_PLAN_MOVE
2787 : if (DEBUG_COND) {
2788 : std::cout << SIMTIME << " veh=" << getID() << " canReverseEventually=" << canReverseEventually << " v=" << v << "\n";
2789 : }
2790 : #endif
2791 :
2792 : // check whether we need to slow down in order to finish a continuous lane change
2793 805820216 : if (myLaneChangeModel->isChangingLanes()) {
2794 : if ( // slow down to finish lane change before a turn lane
2795 160591 : ((*link)->getDirection() == LinkDirection::LEFT || (*link)->getDirection() == LinkDirection::RIGHT) ||
2796 : // slow down to finish lane change before the shadow lane ends
2797 137201 : (myLaneChangeModel->getShadowLane() != nullptr &&
2798 137201 : (*link)->getViaLaneOrLane()->getParallelLane(myLaneChangeModel->getShadowDirection()) == nullptr)) {
2799 : // XXX maybe this is too harsh. Vehicles could cut some corners here
2800 46405 : const double timeRemaining = STEPS2TIME(myLaneChangeModel->remainingTime());
2801 : assert(timeRemaining != 0);
2802 : // XXX: Euler-logic (#860), but I couldn't identify problems from this yet (Leo). Refs. #2575
2803 46405 : const double va = MAX2(cfModel.stopSpeed(this, getSpeed(), seen - POSITION_EPS),
2804 46405 : (seen - POSITION_EPS) / timeRemaining);
2805 : #ifdef DEBUG_PLAN_MOVE
2806 : if (DEBUG_COND) {
2807 : std::cout << SIMTIME << " veh=" << getID() << " slowing down to finish continuous change before"
2808 : << " link=" << (*link)->getViaLaneOrLane()->getID()
2809 : << " timeRemaining=" << timeRemaining
2810 : << " v=" << v
2811 : << " va=" << va
2812 : << std::endl;
2813 : }
2814 : #endif
2815 92609 : v = MIN2(va, v);
2816 : }
2817 : }
2818 :
2819 : // - always issue a request to leave the intersection we are currently on
2820 805820216 : const bool leavingCurrentIntersection = myLane->getEdge().isInternal() && lastLink == nullptr;
2821 : // - do not issue a request to enter an intersection after we already slowed down for an earlier one
2822 805820216 : const bool abortRequestAfterMinor = slowedDownForMinor && (*link)->getInternalLaneBefore() == nullptr;
2823 : // - even if red, if we cannot break we should issue a request
2824 805820216 : bool setRequest = (v > NUMERICAL_EPS_SPEED && !abortRequestAfterMinor) || (leavingCurrentIntersection);
2825 :
2826 805820216 : double stopSpeed = cfModel.stopSpeed(this, getSpeed(), stopDist, stopDecel, MSCFModel::CalcReason::CURRENT_WAIT);
2827 805820216 : double vLinkWait = MIN2(v, stopSpeed);
2828 : #ifdef DEBUG_PLAN_MOVE
2829 : if (DEBUG_COND) {
2830 : std::cout
2831 : << " stopDist=" << stopDist
2832 : << " stopDecel=" << stopDecel
2833 : << " vLinkWait=" << vLinkWait
2834 : << " brakeDist=" << brakeDist
2835 : << " seen=" << seen
2836 : << " leaveIntersection=" << leavingCurrentIntersection
2837 : << " setRequest=" << setRequest
2838 : //<< std::setprecision(16)
2839 : //<< " v=" << v
2840 : //<< " speedEps=" << NUMERICAL_EPS_SPEED
2841 : //<< std::setprecision(gPrecision)
2842 : << "\n";
2843 : }
2844 : #endif
2845 :
2846 805820216 : if (yellowOrRed && canBrakeBeforeStopLine && !ignoreRed(*link, canBrakeBeforeStopLine) && seen >= mustSeeBeforeReversal) {
2847 55053281 : if (lane->isInternal()) {
2848 42239 : checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2849 : }
2850 : // arrivalSpeed / arrivalTime when braking for red light is only relevent for rail signal switching
2851 55053281 : const SUMOTime arrivalTime = getArrivalTime(t, seen, v, vLinkPass);
2852 : // the vehicle is able to brake in front of a yellow/red traffic light
2853 55053281 : lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, false, arrivalTime, vLinkWait, 0, seen, -1));
2854 : //lfLinks.push_back(DriveProcessItem(0, vLinkWait, vLinkWait, false, 0, 0, stopDist));
2855 55053281 : break;
2856 : }
2857 :
2858 750766935 : const MSLink* entryLink = (*link)->getCorrespondingEntryLink();
2859 750766935 : if (entryLink->haveRed() && ignoreRed(*link, canBrakeBeforeStopLine) && STEPS2TIME(t - entryLink->getLastStateChange()) > 2) {
2860 : // restrict speed when ignoring a red light
2861 101346 : const double redSpeed = MIN2(v, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_RED_SPEED, v));
2862 101346 : const double va = MAX2(redSpeed, cfModel.freeSpeed(this, getSpeed(), seen, redSpeed));
2863 202214 : v = MIN2(va, v);
2864 : #ifdef DEBUG_PLAN_MOVE
2865 : if (DEBUG_COND) std::cout
2866 : << " ignoreRed spent=" << STEPS2TIME(t - (*link)->getLastStateChange())
2867 : << " redSpeed=" << redSpeed
2868 : << " va=" << va
2869 : << " v=" << v
2870 : << "\n";
2871 : #endif
2872 : }
2873 :
2874 750766935 : checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2875 :
2876 750766935 : if (lastLink != nullptr) {
2877 : lastLink->adaptLeaveSpeed(laneMaxV);
2878 : }
2879 750766935 : double arrivalSpeed = vLinkPass;
2880 : // vehicles should decelerate when approaching a minor link
2881 : // - unless they are close enough to have clear visibility of all relevant foe lanes and may start to accelerate again
2882 : // - and unless they are so close that stopping is impossible (i.e. when a green light turns to yellow when close to the junction)
2883 :
2884 : // whether the vehicle/driver is close enough to the link to see all possible foes #2123
2885 750766935 : const double visibilityDistance = (*link)->getFoeVisibilityDistance();
2886 750766935 : const double determinedFoePresence = seen <= visibilityDistance;
2887 : // // VARIANT: account for time needed to recognize whether relevant vehicles are on the foe lanes. (Leo)
2888 : // double foeRecognitionTime = 0.0;
2889 : // double determinedFoePresence = seen < visibilityDistance - myState.mySpeed*foeRecognitionTime;
2890 :
2891 : #ifdef DEBUG_PLAN_MOVE
2892 : if (DEBUG_COND) {
2893 : std::cout << " approaching link=" << (*link)->getViaLaneOrLane()->getID() << " prio=" << (*link)->havePriority() << " seen=" << seen << " visibilityDistance=" << visibilityDistance << " brakeDist=" << brakeDist << "\n";
2894 : }
2895 : #endif
2896 :
2897 750766935 : const bool couldBrakeForMinor = !(*link)->havePriority() && brakeDist < seen && !(*link)->lastWasContMajor();
2898 38308289 : if (couldBrakeForMinor && !determinedFoePresence) {
2899 : // vehicle decelerates just enough to be able to stop if necessary and then accelerates
2900 35763754 : double maxSpeedAtVisibilityDist = cfModel.maximumSafeStopSpeed(visibilityDistance, cfModel.getMaxDecel(), myState.mySpeed, false, 0., false);
2901 : // XXX: estimateSpeedAfterDistance does not use euler-logic (thus returns a lower value than possible here...)
2902 35763754 : double maxArrivalSpeed = cfModel.estimateSpeedAfterDistance(visibilityDistance, maxSpeedAtVisibilityDist, cfModel.getMaxAccel());
2903 35763754 : arrivalSpeed = MIN2(vLinkPass, maxArrivalSpeed);
2904 : slowedDownForMinor = true;
2905 : #ifdef DEBUG_PLAN_MOVE
2906 : if (DEBUG_COND) {
2907 : std::cout << " slowedDownForMinor maxSpeedAtVisDist=" << maxSpeedAtVisibilityDist << " maxArrivalSpeed=" << maxArrivalSpeed << " arrivalSpeed=" << arrivalSpeed << "\n";
2908 : }
2909 : #endif
2910 715003181 : } else if ((*link)->getState() == LINKSTATE_EQUAL && myWaitingTime > 0) {
2911 : // check for deadlock (circular yielding)
2912 : //std::cout << SIMTIME << " veh=" << getID() << " check rbl-deadlock\n";
2913 2913 : std::pair<const SUMOVehicle*, const MSLink*> blocker = (*link)->getFirstApproachingFoe(*link);
2914 : //std::cout << " blocker=" << Named::getIDSecure(blocker.first) << "\n";
2915 : int n = 100;
2916 5744 : while (blocker.second != nullptr && blocker.second != *link && n > 0) {
2917 2831 : blocker = blocker.second->getFirstApproachingFoe(*link);
2918 2831 : n--;
2919 : //std::cout << " blocker=" << Named::getIDSecure(blocker.first) << "\n";
2920 : }
2921 2913 : if (n == 0) {
2922 0 : WRITE_WARNINGF(TL("Suspicious right_before_left junction '%'."), lane->getEdge().getToJunction()->getID());
2923 : }
2924 : //std::cout << " blockerLink=" << blocker.second << " link=" << *link << "\n";
2925 2913 : if (blocker.second == *link) {
2926 488 : const double threshold = (*link)->getDirection() == LinkDirection::STRAIGHT ? 0.25 : 0.75;
2927 488 : if (RandHelper::rand(getRNG()) < threshold) {
2928 : //std::cout << " abort request, threshold=" << threshold << "\n";
2929 310 : setRequest = false;
2930 : }
2931 : }
2932 : }
2933 :
2934 750766935 : const SUMOTime arrivalTime = getArrivalTime(t, seen, v, arrivalSpeed);
2935 750766935 : if (couldBrakeForMinor && determinedFoePresence && (*link)->getLane()->getEdge().isRoundabout()) {
2936 839653 : const bool wasOpened = (*link)->opened(arrivalTime, arrivalSpeed, arrivalSpeed,
2937 839653 : getLength(), getImpatience(),
2938 : getCarFollowModel().getMaxDecel(),
2939 839653 : getWaitingTime(), getLateralPositionOnLane(),
2940 : nullptr, false, this);
2941 839653 : if (!wasOpened) {
2942 : slowedDownForMinor = true;
2943 : }
2944 : #ifdef DEBUG_PLAN_MOVE
2945 : if (DEBUG_COND) {
2946 : std::cout << " slowedDownForMinor at roundabout=" << (!wasOpened) << "\n";
2947 : }
2948 : #endif
2949 : }
2950 :
2951 : // compute arrival speed and arrival time if vehicle starts braking now
2952 : // if stopping is possible, arrivalTime can be arbitrarily large. A small value keeps fractional times (impatience) meaningful
2953 : double arrivalSpeedBraking = 0;
2954 750766935 : const double bGap = cfModel.brakeGap(v);
2955 750766935 : if (seen < bGap && !isStopped() && !planningToStop) { // XXX: should this use the current speed (at least for the ballistic case)? (Leo) Refs. #2575
2956 : // vehicle cannot come to a complete stop in time
2957 53413357 : if (MSGlobals::gSemiImplicitEulerUpdate) {
2958 50746308 : arrivalSpeedBraking = cfModel.getMinimalArrivalSpeedEuler(seen, v);
2959 : // due to discrete/continuous mismatch (when using Euler update) we have to ensure that braking actually helps
2960 : arrivalSpeedBraking = MIN2(arrivalSpeedBraking, arrivalSpeed);
2961 : } else {
2962 2667049 : arrivalSpeedBraking = cfModel.getMinimalArrivalSpeed(seen, myState.mySpeed);
2963 : }
2964 : }
2965 :
2966 : // estimate leave speed for passing time computation
2967 : // l=linkLength, a=accel, t=continuousTime, v=vLeave
2968 : // l=v*t + 0.5*a*t^2, solve for t and multiply with a, then add v
2969 1115837429 : const double estimatedLeaveSpeed = MIN2((*link)->getViaLaneOrLane()->getVehicleMaxSpeed(this, maxVD),
2970 750766935 : getCarFollowModel().estimateSpeedAfterDistance((*link)->getLength(), arrivalSpeed, getVehicleType().getCarFollowModel().getMaxAccel()));
2971 750766935 : lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, setRequest,
2972 : arrivalTime, arrivalSpeed,
2973 : arrivalSpeedBraking,
2974 : seen, estimatedLeaveSpeed));
2975 750766935 : if ((*link)->getViaLane() == nullptr) {
2976 : hadNonInternal = true;
2977 : ++view;
2978 : }
2979 : #ifdef DEBUG_PLAN_MOVE
2980 : if (DEBUG_COND) {
2981 : std::cout << " checkAbort setRequest=" << setRequest << " v=" << v << " seen=" << seen << " dist=" << dist
2982 : << " seenNonInternal=" << seenNonInternal
2983 : << " seenInternal=" << seenInternal << " length=" << vehicleLength << "\n";
2984 : }
2985 : #endif
2986 : // we need to look ahead far enough to see available space for checkRewindLinkLanes
2987 774192547 : if ((!setRequest || v <= 0 || seen > dist) && hadNonInternal && seenNonInternal > MAX2(vehicleLength * CRLL_LOOK_AHEAD, vehicleLength + seenInternal) && foundRailSignal) {
2988 : break;
2989 : }
2990 : // get the following lane
2991 : lane = (*link)->getViaLaneOrLane();
2992 540540507 : laneMaxV = lane->getVehicleMaxSpeed(this, maxVD);
2993 540922067 : if (myInfluencer && !myInfluencer->considerSpeedLimit()) {
2994 : laneMaxV = std::numeric_limits<double>::max();
2995 : }
2996 : // the link was passed
2997 : // compute the velocity to use when the link is not blocked by other vehicles
2998 : // the vehicle shall be not faster when reaching the next lane than allowed
2999 : // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
3000 540540507 : const double va = MAX2(cfModel.freeSpeed(this, getSpeed(), seen, laneMaxV), vMinComfortable);
3001 1071771665 : v = MIN2(va, v);
3002 : #ifdef DEBUG_PLAN_MOVE
3003 : if (DEBUG_COND) {
3004 : std::cout << " laneMaxV=" << laneMaxV << " freeSpeed=" << va << " v=" << v << "\n";
3005 : }
3006 : #endif
3007 540540507 : if (lane->getEdge().isInternal()) {
3008 233305223 : seenInternal += lane->getLength();
3009 : } else {
3010 307235284 : seenNonInternal += lane->getLength();
3011 : }
3012 : // do not restrict results to the current vehicle to allow caching for the current time step
3013 540540507 : leaderLane = opposite ? lane->getParallelOpposite() : lane;
3014 540540507 : if (leaderLane == nullptr) {
3015 :
3016 : break;
3017 : }
3018 1080659796 : ahead = opposite ? MSLeaderInfo(leaderLane->getWidth()) : leaderLane->getLastVehicleInformation(nullptr, 0);
3019 540329898 : seen += lane->getLength();
3020 1080659796 : vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(lane->getLength(), v, cfModel.getMaxAccel()), laneMaxV); // upper bound
3021 : lastLink = &lfLinks.back();
3022 540329898 : }
3023 :
3024 : //#ifdef DEBUG_PLAN_MOVE
3025 : // if(DEBUG_COND){
3026 : // std::cout << "planMoveInternal found safe speed v = " << v << std::endl;
3027 : // }
3028 : //#endif
3029 :
3030 : #ifdef PARALLEL_STOPWATCH
3031 : myLane->getStopWatch()[0].stop();
3032 : #endif
3033 603822217 : }
3034 :
3035 :
3036 : double
3037 2786 : MSVehicle::slowDownForSchedule(double vMinComfortable) const {
3038 2786 : const double sfp = getVehicleType().getParameter().speedFactorPremature;
3039 : const MSStop& stop = myStops.front();
3040 2786 : std::pair<double, double> timeDist = estimateTimeToNextStop();
3041 2786 : double arrivalDelay = SIMTIME + timeDist.first - STEPS2TIME(stop.pars.arrival);
3042 2786 : double t = STEPS2TIME(stop.pars.arrival - SIMSTEP);
3043 5572 : if (stop.pars.hasParameter(toString(SUMO_ATTR_FLEX_ARRIVAL))) {
3044 150 : SUMOTime flexStart = string2time(stop.pars.getParameter(toString(SUMO_ATTR_FLEX_ARRIVAL)));
3045 75 : arrivalDelay += STEPS2TIME(stop.pars.arrival - flexStart);
3046 75 : t = STEPS2TIME(flexStart - SIMSTEP);
3047 2711 : } else if (stop.pars.started >= 0 && MSGlobals::gUseStopStarted) {
3048 200 : arrivalDelay += STEPS2TIME(stop.pars.arrival - stop.pars.started);
3049 200 : t = STEPS2TIME(stop.pars.started - SIMSTEP);
3050 : }
3051 2786 : if (arrivalDelay < 0 && sfp < getChosenSpeedFactor()) {
3052 : // we can slow down to better match the schedule (and increase energy efficiency)
3053 2721 : const double vSlowDownMin = MAX2(myLane->getSpeedLimit() * sfp, vMinComfortable);
3054 2721 : const double s = timeDist.second;
3055 : const double b = getCarFollowModel().getMaxDecel();
3056 : // x = speed for arriving in t seconds
3057 : // u = time at full speed
3058 : // u * x + (t - u) * 0.5 * x = s
3059 : // t - u = x / b
3060 : // eliminate u, solve x
3061 2721 : const double radicand = 4 * t * t * b * b - 8 * s * b;
3062 2721 : const double x = radicand >= 0 ? t * b - sqrt(radicand) * 0.5 : vSlowDownMin;
3063 2721 : double vSlowDown = x < vSlowDownMin ? vSlowDownMin : x;
3064 : #ifdef DEBUG_PLAN_MOVE
3065 : if (DEBUG_COND) {
3066 : std::cout << SIMTIME << " veh=" << getID() << " ad=" << arrivalDelay << " t=" << t << " vsm=" << vSlowDownMin
3067 : << " r=" << radicand << " vs=" << vSlowDown << "\n";
3068 : }
3069 : #endif
3070 2721 : return vSlowDown;
3071 65 : } else if (arrivalDelay > 0 && sfp > getChosenSpeedFactor()) {
3072 : // in principle we could up to catch up with the schedule
3073 : // but at this point we can only lower the speed, the
3074 : // information would have to be used when computing getVehicleMaxSpeed
3075 : }
3076 65 : return getMaxSpeed();
3077 : }
3078 :
3079 : SUMOTime
3080 805820216 : MSVehicle::getArrivalTime(SUMOTime t, double seen, double v, double arrivalSpeed) const {
3081 : const MSCFModel& cfModel = getCarFollowModel();
3082 : SUMOTime arrivalTime;
3083 805820216 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3084 : // @note intuitively it would make sense to compare arrivalSpeed with getSpeed() instead of v
3085 : // however, due to the current position update rule (ticket #860) the vehicle moves with v in this step
3086 : // subtract DELTA_T because t is the time at the end of this step and the movement is not carried out yet
3087 749776118 : arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, v, arrivalSpeed);
3088 : } else {
3089 56044098 : arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, myState.mySpeed, arrivalSpeed);
3090 : }
3091 805820216 : if (isStopped()) {
3092 2340212 : arrivalTime += MAX2((SUMOTime)0, myStops.front().duration);
3093 : }
3094 805820216 : return arrivalTime;
3095 : }
3096 :
3097 :
3098 : void
3099 1148884152 : MSVehicle::adaptToLeaders(const MSLeaderInfo& ahead, double latOffset,
3100 : const double seen, DriveProcessItem* const lastLink,
3101 : const MSLane* const lane, double& v, double& vLinkPass) const {
3102 : int rightmost;
3103 : int leftmost;
3104 1148884152 : ahead.getSubLanes(this, latOffset, rightmost, leftmost);
3105 : #ifdef DEBUG_PLAN_MOVE
3106 : if (DEBUG_COND) std::cout << SIMTIME
3107 : << "\nADAPT_TO_LEADERS\nveh=" << getID()
3108 : << " lane=" << lane->getID()
3109 : << " latOffset=" << latOffset
3110 : << " rm=" << rightmost
3111 : << " lm=" << leftmost
3112 : << " shift=" << ahead.getSublaneOffset()
3113 : << " ahead=" << ahead.toString()
3114 : << "\n";
3115 : #endif
3116 : /*
3117 : if (myLaneChangeModel->getCommittedSpeed() > 0) {
3118 : v = MIN2(v, myLaneChangeModel->getCommittedSpeed());
3119 : vLinkPass = MIN2(vLinkPass, myLaneChangeModel->getCommittedSpeed());
3120 : #ifdef DEBUG_PLAN_MOVE
3121 : if (DEBUG_COND) std::cout << " hasCommitted=" << myLaneChangeModel->getCommittedSpeed() << "\n";
3122 : #endif
3123 : return;
3124 : }
3125 : */
3126 2828910439 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
3127 1680026287 : const MSVehicle* pred = ahead[sublane];
3128 1680026287 : if (pred != nullptr && pred != this) {
3129 : // @todo avoid multiple adaptations to the same leader
3130 1233729928 : const double predBack = pred->getBackPositionOnLane(lane);
3131 : double gap = (lastLink == nullptr
3132 1760258850 : ? predBack - myState.myPos - getVehicleType().getMinGap()
3133 526528922 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3134 : bool oncoming = false;
3135 1233729928 : if (myLaneChangeModel->isOpposite()) {
3136 26680 : if (pred->getLaneChangeModel().isOpposite() || lane == pred->getLaneChangeModel().getShadowLane()) {
3137 : // ego might and leader are driving against lane
3138 : gap = (lastLink == nullptr
3139 0 : ? myState.myPos - predBack - getVehicleType().getMinGap()
3140 0 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3141 : } else {
3142 : // ego and leader are driving in the same direction as lane (shadowlane for ego)
3143 : gap = (lastLink == nullptr
3144 27377 : ? predBack - (myLane->getLength() - myState.myPos) - getVehicleType().getMinGap()
3145 697 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3146 : }
3147 1233703248 : } else if (pred->getLaneChangeModel().isOpposite() && pred->getLaneChangeModel().getShadowLane() != lane) {
3148 : // must react to stopped / dangerous oncoming vehicles
3149 191803 : gap += -pred->getVehicleType().getLength() + getVehicleType().getMinGap() - MAX2(getVehicleType().getMinGap(), pred->getVehicleType().getMinGap());
3150 : // try to avoid collision in the next second
3151 191803 : const double predMaxDist = pred->getSpeed() + pred->getCarFollowModel().getMaxAccel();
3152 : #ifdef DEBUG_PLAN_MOVE
3153 : if (DEBUG_COND) {
3154 : std::cout << " fixedGap=" << gap << " predMaxDist=" << predMaxDist << "\n";
3155 : }
3156 : #endif
3157 191803 : if (gap < predMaxDist + getSpeed() || pred->getLane() == lane->getBidiLane()) {
3158 23361 : gap -= predMaxDist;
3159 : }
3160 1233511445 : } else if (pred->getLane() == lane->getBidiLane()) {
3161 43542 : gap -= pred->getVehicleType().getLengthWithGap();
3162 : oncoming = true;
3163 : }
3164 : #ifdef DEBUG_PLAN_MOVE
3165 : if (DEBUG_COND) {
3166 : 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";
3167 : }
3168 : #endif
3169 43542 : if (oncoming && gap >= 0) {
3170 43542 : adaptToOncomingLeader(std::make_pair(pred, gap), lastLink, v, vLinkPass);
3171 : } else {
3172 1233686386 : adaptToLeader(std::make_pair(pred, gap), seen, lastLink, v, vLinkPass);
3173 : }
3174 : }
3175 : }
3176 1148884152 : }
3177 :
3178 : void
3179 431657 : MSVehicle::adaptToLeaderDistance(const MSLeaderDistanceInfo& ahead, double latOffset,
3180 : double seen,
3181 : DriveProcessItem* const lastLink,
3182 : double& v, double& vLinkPass) const {
3183 : int rightmost;
3184 : int leftmost;
3185 431657 : ahead.getSubLanes(this, latOffset, rightmost, leftmost);
3186 : #ifdef DEBUG_PLAN_MOVE
3187 : if (DEBUG_COND) std::cout << SIMTIME
3188 : << "\nADAPT_TO_LEADERS_DISTANCE\nveh=" << getID()
3189 : << " latOffset=" << latOffset
3190 : << " rm=" << rightmost
3191 : << " lm=" << leftmost
3192 : << " ahead=" << ahead.toString()
3193 : << "\n";
3194 : #endif
3195 1058014 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
3196 626357 : CLeaderDist predDist = ahead[sublane];
3197 626357 : const MSVehicle* pred = predDist.first;
3198 626357 : if (pred != nullptr && pred != this) {
3199 : #ifdef DEBUG_PLAN_MOVE
3200 : if (DEBUG_COND) {
3201 : std::cout << " pred=" << pred->getID() << " predLane=" << pred->getLane()->getID() << " predPos=" << pred->getPositionOnLane() << " gap=" << predDist.second << "\n";
3202 : }
3203 : #endif
3204 400184 : adaptToLeader(predDist, seen, lastLink, v, vLinkPass);
3205 : }
3206 : }
3207 431657 : }
3208 :
3209 :
3210 : void
3211 1234086570 : MSVehicle::adaptToLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3212 : double seen,
3213 : DriveProcessItem* const lastLink,
3214 : double& v, double& vLinkPass) const {
3215 1234086570 : if (leaderInfo.first != 0) {
3216 1234086570 : if (ignoreFoe(leaderInfo.first)) {
3217 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3218 : if (DEBUG_COND) {
3219 : std::cout << " foe ignored\n";
3220 : }
3221 : #endif
3222 : return;
3223 : }
3224 : const MSCFModel& cfModel = getCarFollowModel();
3225 : double vsafeLeader = 0;
3226 1234085750 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
3227 : vsafeLeader = -std::numeric_limits<double>::max();
3228 : }
3229 : bool backOnRoute = true;
3230 1234085750 : if (leaderInfo.second < 0 && lastLink != nullptr && lastLink->myLink != nullptr) {
3231 : backOnRoute = false;
3232 : // this can either be
3233 : // a) a merging situation (leader back is is not our route) or
3234 : // b) a minGap violation / collision
3235 : MSLane* current = lastLink->myLink->getViaLaneOrLane();
3236 230838 : if (leaderInfo.first->getBackLane() == current) {
3237 : backOnRoute = true;
3238 : } else {
3239 533668 : for (MSLane* lane : getBestLanesContinuation()) {
3240 488780 : if (lane == current) {
3241 : break;
3242 : }
3243 348758 : if (leaderInfo.first->getBackLane() == lane) {
3244 : backOnRoute = true;
3245 : }
3246 : }
3247 : }
3248 : #ifdef DEBUG_PLAN_MOVE
3249 : if (DEBUG_COND) {
3250 : std::cout << SIMTIME << " current=" << current->getID() << " leaderBackLane=" << leaderInfo.first->getBackLane()->getID() << " backOnRoute=" << backOnRoute << "\n";
3251 : }
3252 : #endif
3253 184910 : if (!backOnRoute) {
3254 112744 : double stopDist = seen - current->getLength() - POSITION_EPS;
3255 112744 : if (lastLink->myLink->getInternalLaneBefore() != nullptr) {
3256 : // do not drive onto the junction conflict area
3257 93922 : stopDist -= lastLink->myLink->getInternalLaneBefore()->getLength();
3258 : }
3259 112744 : vsafeLeader = cfModel.stopSpeed(this, getSpeed(), stopDist);
3260 : }
3261 : }
3262 158672 : if (backOnRoute) {
3263 1233973006 : vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3264 : }
3265 1234085750 : if (lastLink != nullptr) {
3266 526519098 : const double futureVSafe = cfModel.followSpeed(this, lastLink->accelV, leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first, MSCFModel::CalcReason::FUTURE);
3267 : lastLink->adaptLeaveSpeed(futureVSafe);
3268 : #ifdef DEBUG_PLAN_MOVE
3269 : if (DEBUG_COND) {
3270 : std::cout << " vlinkpass=" << lastLink->myVLinkPass << " futureVSafe=" << futureVSafe << "\n";
3271 : }
3272 : #endif
3273 : }
3274 1234085750 : v = MIN2(v, vsafeLeader);
3275 2120602696 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3276 : #ifdef DEBUG_PLAN_MOVE
3277 : if (DEBUG_COND) std::cout
3278 : << SIMTIME
3279 : //std::cout << std::setprecision(10);
3280 : << " veh=" << getID()
3281 : << " lead=" << leaderInfo.first->getID()
3282 : << " leadSpeed=" << leaderInfo.first->getSpeed()
3283 : << " gap=" << leaderInfo.second
3284 : << " leadLane=" << leaderInfo.first->getLane()->getID()
3285 : << " predPos=" << leaderInfo.first->getPositionOnLane()
3286 : << " myLane=" << myLane->getID()
3287 : << " v=" << v
3288 : << " vSafeLeader=" << vsafeLeader
3289 : << " vLinkPass=" << vLinkPass
3290 : << "\n";
3291 : #endif
3292 : }
3293 : }
3294 :
3295 :
3296 : void
3297 17373319 : MSVehicle::adaptToJunctionLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3298 : const double seen, DriveProcessItem* const lastLink,
3299 : const MSLane* const lane, double& v, double& vLinkPass,
3300 : double distToCrossing) const {
3301 17373319 : if (leaderInfo.first != 0) {
3302 17373319 : if (ignoreFoe(leaderInfo.first)) {
3303 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3304 : if (DEBUG_COND) {
3305 : std::cout << " junction foe ignored\n";
3306 : }
3307 : #endif
3308 : return;
3309 : }
3310 : const MSCFModel& cfModel = getCarFollowModel();
3311 : double vsafeLeader = 0;
3312 17373283 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
3313 : vsafeLeader = -std::numeric_limits<double>::max();
3314 : }
3315 17373283 : if (leaderInfo.second >= 0) {
3316 14318258 : if (hasDeparted()) {
3317 14313116 : vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3318 : } else {
3319 : // called in the context of MSLane::isInsertionSuccess
3320 5142 : vsafeLeader = cfModel.insertionFollowSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3321 : }
3322 3055025 : } else if (leaderInfo.first != this) {
3323 : // the leading, in-lapping vehicle is occupying the complete next lane
3324 : // stop before entering this lane
3325 2654531 : vsafeLeader = cfModel.stopSpeed(this, getSpeed(), seen - lane->getLength() - POSITION_EPS);
3326 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3327 : if (DEBUG_COND) {
3328 : std::cout << SIMTIME << " veh=" << getID() << " stopping before junction: lane=" << lane->getID() << " seen=" << seen
3329 : << " laneLength=" << lane->getLength()
3330 : << " stopDist=" << seen - lane->getLength() - POSITION_EPS
3331 : << " vsafeLeader=" << vsafeLeader
3332 : << " distToCrossing=" << distToCrossing
3333 : << "\n";
3334 : }
3335 : #endif
3336 : }
3337 17373283 : if (distToCrossing >= 0) {
3338 : // can the leader still stop in the way?
3339 5273628 : const double vStop = cfModel.stopSpeed(this, getSpeed(), distToCrossing - getVehicleType().getMinGap());
3340 5273628 : if (leaderInfo.first == this) {
3341 : // braking for pedestrian
3342 391811 : const double vStopCrossing = cfModel.stopSpeed(this, getSpeed(), distToCrossing);
3343 : vsafeLeader = vStopCrossing;
3344 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3345 : if (DEBUG_COND) {
3346 : std::cout << " breaking for pedestrian distToCrossing=" << distToCrossing << " vStopCrossing=" << vStopCrossing << "\n";
3347 : }
3348 : #endif
3349 391811 : if (lastLink != nullptr) {
3350 : lastLink->adaptStopSpeed(vsafeLeader);
3351 : }
3352 4881817 : } else if (leaderInfo.second == -std::numeric_limits<double>::max()) {
3353 : // drive up to the crossing point and stop
3354 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3355 : if (DEBUG_COND) {
3356 : std::cout << " stop at crossing point for critical leader vStop=" << vStop << "\n";
3357 : };
3358 : #endif
3359 : vsafeLeader = MAX2(vsafeLeader, vStop);
3360 : } else {
3361 4825779 : const double leaderDistToCrossing = distToCrossing - leaderInfo.second;
3362 : // estimate the time at which the leader has gone past the crossing point
3363 4825779 : const double leaderPastCPTime = leaderDistToCrossing / MAX2(leaderInfo.first->getSpeed(), SUMO_const_haltingSpeed);
3364 : // reach distToCrossing after that time
3365 : // avgSpeed * leaderPastCPTime = distToCrossing
3366 : // ballistic: avgSpeed = (getSpeed + vFinal) / 2
3367 4825779 : const double vFinal = MAX2(getSpeed(), 2 * (distToCrossing - getVehicleType().getMinGap()) / leaderPastCPTime - getSpeed());
3368 4825779 : const double v2 = getSpeed() + ACCEL2SPEED((vFinal - getSpeed()) / leaderPastCPTime);
3369 : vsafeLeader = MAX2(vsafeLeader, MIN2(v2, vStop));
3370 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3371 : if (DEBUG_COND) {
3372 : std::cout << " driving up to the crossing point (distToCrossing=" << distToCrossing << ")"
3373 : << " leaderPastCPTime=" << leaderPastCPTime
3374 : << " vFinal=" << vFinal
3375 : << " v2=" << v2
3376 : << " vStop=" << vStop
3377 : << " vsafeLeader=" << vsafeLeader << "\n";
3378 : }
3379 : #endif
3380 : }
3381 : }
3382 16981472 : if (lastLink != nullptr) {
3383 : lastLink->adaptLeaveSpeed(vsafeLeader);
3384 : }
3385 17373283 : v = MIN2(v, vsafeLeader);
3386 32441722 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3387 : #ifdef DEBUG_PLAN_MOVE
3388 : if (DEBUG_COND) std::cout
3389 : << SIMTIME
3390 : //std::cout << std::setprecision(10);
3391 : << " veh=" << getID()
3392 : << " lead=" << leaderInfo.first->getID()
3393 : << " leadSpeed=" << leaderInfo.first->getSpeed()
3394 : << " gap=" << leaderInfo.second
3395 : << " leadLane=" << leaderInfo.first->getLane()->getID()
3396 : << " predPos=" << leaderInfo.first->getPositionOnLane()
3397 : << " seen=" << seen
3398 : << " lane=" << lane->getID()
3399 : << " myLane=" << myLane->getID()
3400 : << " dTC=" << distToCrossing
3401 : << " v=" << v
3402 : << " vSafeLeader=" << vsafeLeader
3403 : << " vLinkPass=" << vLinkPass
3404 : << "\n";
3405 : #endif
3406 : }
3407 : }
3408 :
3409 :
3410 : void
3411 43542 : MSVehicle::adaptToOncomingLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3412 : DriveProcessItem* const lastLink,
3413 : double& v, double& vLinkPass) const {
3414 43542 : if (leaderInfo.first != 0) {
3415 43542 : if (ignoreFoe(leaderInfo.first)) {
3416 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3417 : if (DEBUG_COND) {
3418 : std::cout << " oncoming foe ignored\n";
3419 : }
3420 : #endif
3421 : return;
3422 : }
3423 : const MSCFModel& cfModel = getCarFollowModel();
3424 : const MSVehicle* lead = leaderInfo.first;
3425 : const MSCFModel& cfModelL = lead->getCarFollowModel();
3426 : // assume the leader reacts symmetrically (neither stopping instantly nor ignoring ego)
3427 43423 : const double leaderBrakeGap = cfModelL.brakeGap(lead->getSpeed(), cfModelL.getMaxDecel(), 0);
3428 43423 : const double egoBrakeGap = cfModel.brakeGap(getSpeed(), cfModel.getMaxDecel(), 0);
3429 43423 : const double gapSum = leaderBrakeGap + egoBrakeGap;
3430 : // ensure that both vehicles can leave an intersection if they are currently on it
3431 43423 : double egoExit = getDistanceToLeaveJunction();
3432 43423 : const double leaderExit = lead->getDistanceToLeaveJunction();
3433 : double gap = leaderInfo.second;
3434 43423 : if (egoExit + leaderExit < gap) {
3435 38482 : gap -= egoExit + leaderExit;
3436 : } else {
3437 : egoExit = 0;
3438 : }
3439 : // split any distance in excess of brakeGaps evenly
3440 43423 : const double freeGap = MAX2(0.0, gap - gapSum);
3441 : const double splitGap = MIN2(gap, gapSum);
3442 : // assume remaining distance is allocated in proportion to braking distance
3443 43423 : const double gapRatio = gapSum > 0 ? egoBrakeGap / gapSum : 0.5;
3444 43423 : const double vsafeLeader = cfModel.stopSpeed(this, getSpeed(), splitGap * gapRatio + egoExit + 0.5 * freeGap);
3445 43423 : if (lastLink != nullptr) {
3446 29855 : const double futureVSafe = cfModel.stopSpeed(this, lastLink->accelV, leaderInfo.second, MSCFModel::CalcReason::FUTURE);
3447 : lastLink->adaptLeaveSpeed(futureVSafe);
3448 : #ifdef DEBUG_PLAN_MOVE
3449 : if (DEBUG_COND) {
3450 : std::cout << " vlinkpass=" << lastLink->myVLinkPass << " futureVSafe=" << futureVSafe << "\n";
3451 : }
3452 : #endif
3453 : }
3454 43423 : v = MIN2(v, vsafeLeader);
3455 85556 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3456 : #ifdef DEBUG_PLAN_MOVE
3457 : if (DEBUG_COND) std::cout
3458 : << SIMTIME
3459 : //std::cout << std::setprecision(10);
3460 : << " veh=" << getID()
3461 : << " oncomingLead=" << lead->getID()
3462 : << " leadSpeed=" << lead->getSpeed()
3463 : << " gap=" << leaderInfo.second
3464 : << " gap2=" << gap
3465 : << " gapRatio=" << gapRatio
3466 : << " leadLane=" << lead->getLane()->getID()
3467 : << " predPos=" << lead->getPositionOnLane()
3468 : << " myLane=" << myLane->getID()
3469 : << " v=" << v
3470 : << " vSafeLeader=" << vsafeLeader
3471 : << " vLinkPass=" << vLinkPass
3472 : << "\n";
3473 : #endif
3474 : }
3475 : }
3476 :
3477 :
3478 : void
3479 750815810 : MSVehicle::checkLinkLeaderCurrentAndParallel(const MSLink* link, const MSLane* lane, double seen,
3480 : DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest) const {
3481 750815810 : if (MSGlobals::gUsingInternalLanes && (myInfluencer == nullptr || myInfluencer->getRespectJunctionLeaderPriority())) {
3482 : // we want to pass the link but need to check for foes on internal lanes
3483 750710447 : checkLinkLeader(link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
3484 750710447 : if (myLaneChangeModel->getShadowLane() != nullptr) {
3485 2966204 : const MSLink* const parallelLink = link->getParallelLink(myLaneChangeModel->getShadowDirection());
3486 2966204 : if (parallelLink != nullptr) {
3487 2085359 : checkLinkLeader(parallelLink, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest, true);
3488 : }
3489 : }
3490 : }
3491 :
3492 750815810 : }
3493 :
3494 : void
3495 753000020 : MSVehicle::checkLinkLeader(const MSLink* link, const MSLane* lane, double seen,
3496 : DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest,
3497 : bool isShadowLink) const {
3498 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3499 : if (DEBUG_COND) {
3500 : gDebugFlag1 = true; // See MSLink::getLeaderInfo
3501 : }
3502 : #endif
3503 753000020 : const MSLink::LinkLeaders linkLeaders = link->getLeaderInfo(this, seen, nullptr, isShadowLink);
3504 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3505 : if (DEBUG_COND) {
3506 : gDebugFlag1 = false; // See MSLink::getLeaderInfo
3507 : }
3508 : #endif
3509 771235409 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
3510 : // the vehicle to enter the junction first has priority
3511 18235389 : const MSVehicle* leader = (*it).vehAndGap.first;
3512 18235389 : if (leader == nullptr) {
3513 : // leader is a pedestrian. Passing 'this' as a dummy.
3514 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3515 : if (DEBUG_COND) {
3516 : std::cout << SIMTIME << " veh=" << getID() << " is blocked on link to " << link->getViaLaneOrLane()->getID() << " by pedestrian. dist=" << it->distToCrossing << "\n";
3517 : }
3518 : #endif
3519 401190 : if (getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) > 0
3520 401190 : && getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) >= RandHelper::rand(getRNG())) {
3521 : #ifdef DEBUG_PLAN_MOVE
3522 : if (DEBUG_COND) {
3523 : std::cout << SIMTIME << " veh=" << getID() << " is ignoring pedestrian (jmIgnoreJunctionFoeProb)\n";
3524 : }
3525 : #endif
3526 696 : continue;
3527 : }
3528 400494 : adaptToJunctionLeader(std::make_pair(this, -1), seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
3529 : // if blocked by a pedestrian for too long we must yield our request
3530 400494 : if (v < SUMO_const_haltingSpeed && getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME)) {
3531 69939 : setRequest = false;
3532 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3533 : if (DEBUG_COND) {
3534 : std::cout << " aborting request\n";
3535 : }
3536 : #endif
3537 : }
3538 17834199 : } else if (isLeader(link, leader, (*it).vehAndGap.second) || (*it).inTheWay()) {
3539 17784532 : if (getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) > 0
3540 17784532 : && getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) >= RandHelper::rand(getRNG())) {
3541 : #ifdef DEBUG_PLAN_MOVE
3542 : if (DEBUG_COND) {
3543 : std::cout << SIMTIME << " veh=" << getID() << " is ignoring linkLeader=" << leader->getID() << " (jmIgnoreJunctionFoeProb)\n";
3544 : }
3545 : #endif
3546 2179 : continue;
3547 : }
3548 24356249 : if (MSGlobals::gLateralResolution > 0 &&
3549 : // sibling link (XXX: could also be partial occupator where this check fails)
3550 6573896 : &leader->getLane()->getEdge() == &lane->getEdge()) {
3551 : // check for sublane obstruction (trivial for sibling link leaders)
3552 : const MSLane* conflictLane = link->getInternalLaneBefore();
3553 832319 : MSLeaderInfo linkLeadersAhead = MSLeaderInfo(conflictLane->getWidth());
3554 832319 : linkLeadersAhead.addLeader(leader, false, 0); // assume sibling lane has the same geometry as the leader lane
3555 832319 : const double latOffset = isShadowLink ? (getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge()) : 0;
3556 : // leader is neither on lane nor conflictLane (the conflict is only established geometrically)
3557 832319 : adaptToLeaders(linkLeadersAhead, latOffset, seen, lastLink, leader->getLane(), v, vLinkPass);
3558 : #ifdef DEBUG_PLAN_MOVE
3559 : if (DEBUG_COND) {
3560 : std::cout << SIMTIME << " veh=" << getID()
3561 : << " siblingFoe link=" << link->getViaLaneOrLane()->getID()
3562 : << " isShadowLink=" << isShadowLink
3563 : << " lane=" << lane->getID()
3564 : << " foe=" << leader->getID()
3565 : << " foeLane=" << leader->getLane()->getID()
3566 : << " latOffset=" << latOffset
3567 : << " latOffsetFoe=" << leader->getLatOffset(lane)
3568 : << " linkLeadersAhead=" << linkLeadersAhead.toString()
3569 : << "\n";
3570 : }
3571 : #endif
3572 832319 : } else {
3573 : #ifdef DEBUG_PLAN_MOVE
3574 : if (DEBUG_COND) {
3575 : std::cout << SIMTIME << " veh=" << getID() << " linkLeader=" << leader->getID() << " gap=" << it->vehAndGap.second
3576 : << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
3577 : << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
3578 : << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
3579 : << "\n";
3580 : }
3581 : #endif
3582 16950034 : adaptToJunctionLeader(it->vehAndGap, seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
3583 : }
3584 17782353 : if (lastLink != nullptr) {
3585 : // we are not yet on the junction with this linkLeader.
3586 : // at least we can drive up to the previous link and stop there
3587 34205870 : v = MAX2(v, lastLink->myVLinkWait);
3588 : }
3589 : // if blocked by a leader from the same or next lane we must yield our request
3590 : // also, if blocked by a stopped or blocked leader
3591 17782353 : if (v < SUMO_const_haltingSpeed
3592 : //&& leader->getSpeed() < SUMO_const_haltingSpeed
3593 17782353 : && (leader->getLane()->getLogicalPredecessorLane() == myLane->getLogicalPredecessorLane()
3594 9800437 : || leader->getLane()->getLogicalPredecessorLane() == myLane
3595 7843549 : || leader->isStopped()
3596 7762912 : || leader->getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME))) {
3597 3868609 : setRequest = false;
3598 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3599 : if (DEBUG_COND) {
3600 : std::cout << " aborting request\n";
3601 : }
3602 : #endif
3603 3868609 : if (lastLink != nullptr && leader->getLane()->getLogicalPredecessorLane() == myLane) {
3604 : // we are not yet on the junction so must abort that request as well
3605 : // (or maybe we are already on the junction and the leader is a partial occupator beyond)
3606 1944084 : lastLink->mySetRequest = false;
3607 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3608 : if (DEBUG_COND) {
3609 : std::cout << " aborting previous request\n";
3610 : }
3611 : #endif
3612 : }
3613 : }
3614 : }
3615 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3616 : else {
3617 : if (DEBUG_COND) {
3618 : std::cout << SIMTIME << " veh=" << getID() << " ignoring leader " << leader->getID() << " gap=" << (*it).vehAndGap.second << " dtC=" << (*it).distToCrossing
3619 : << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
3620 : << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
3621 : << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
3622 : << "\n";
3623 : }
3624 : }
3625 : #endif
3626 : }
3627 : // if this is the link between two internal lanes we may have to slow down for pedestrians
3628 753000020 : vLinkWait = MIN2(vLinkWait, v);
3629 753000020 : }
3630 :
3631 :
3632 : double
3633 99218956 : MSVehicle::getDeltaPos(const double accel) const {
3634 99218956 : double vNext = myState.mySpeed + ACCEL2SPEED(accel);
3635 99218956 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3636 : // apply implicit Euler positional update
3637 0 : return SPEED2DIST(MAX2(vNext, 0.));
3638 : } else {
3639 : // apply ballistic update
3640 99218956 : if (vNext >= 0) {
3641 : // assume constant acceleration during this time step
3642 98610125 : return SPEED2DIST(myState.mySpeed + 0.5 * ACCEL2SPEED(accel));
3643 : } else {
3644 : // negative vNext indicates a stop within the middle of time step
3645 : // The corresponding stop time is s = mySpeed/deceleration \in [0,dt], and the
3646 : // covered distance is therefore deltaPos = mySpeed*s - 0.5*deceleration*s^2.
3647 : // Here, deceleration = (myState.mySpeed - vNext)/dt is the constant deceleration
3648 : // until the vehicle stops.
3649 608831 : return -SPEED2DIST(0.5 * myState.mySpeed * myState.mySpeed / ACCEL2SPEED(accel));
3650 : }
3651 : }
3652 : }
3653 :
3654 : void
3655 603822217 : MSVehicle::processLinkApproaches(double& vSafe, double& vSafeMin, double& vSafeMinDist) {
3656 :
3657 : // Speed limit due to zipper merging
3658 : double vSafeZipper = std::numeric_limits<double>::max();
3659 :
3660 603822217 : myHaveToWaitOnNextLink = false;
3661 : bool canBrakeVSafeMin = false;
3662 :
3663 : // Get safe velocities from DriveProcessItems.
3664 : assert(myLFLinkLanes.size() != 0 || isRemoteControlled());
3665 1202752563 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
3666 1033965357 : MSLink* const link = dpi.myLink;
3667 :
3668 : #ifdef DEBUG_EXEC_MOVE
3669 : if (DEBUG_COND) {
3670 : std::cout
3671 : << SIMTIME
3672 : << " veh=" << getID()
3673 : << " link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID())
3674 : << " req=" << dpi.mySetRequest
3675 : << " vP=" << dpi.myVLinkPass
3676 : << " vW=" << dpi.myVLinkWait
3677 : << " d=" << dpi.myDistance
3678 : << "\n";
3679 : gDebugFlag1 = true; // See MSLink_DEBUG_OPENED
3680 : }
3681 : #endif
3682 :
3683 : // the vehicle must change the lane on one of the next lanes (XXX: refs to code further below???, Leo)
3684 1033965357 : if (link != nullptr && dpi.mySetRequest) {
3685 :
3686 : const LinkState ls = link->getState();
3687 : // vehicles should brake when running onto a yellow light if the distance allows to halt in front
3688 : const bool yellow = link->haveYellow();
3689 618728977 : const bool canBrake = (dpi.myDistance > getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.)
3690 618728977 : || (MSGlobals::gSemiImplicitEulerUpdate && myState.mySpeed < ACCEL2SPEED(getCarFollowModel().getMaxDecel())));
3691 : assert(link->getLaneBefore() != nullptr);
3692 618728977 : const bool beyondStopLine = dpi.myDistance < link->getLaneBefore()->getVehicleStopOffset(this);
3693 618728977 : const bool ignoreRedLink = ignoreRed(link, canBrake) || beyondStopLine;
3694 618728977 : if (yellow && canBrake && !ignoreRedLink) {
3695 1 : vSafe = dpi.myVLinkWait;
3696 1 : myHaveToWaitOnNextLink = true;
3697 : #ifdef DEBUG_CHECKREWINDLINKLANES
3698 : if (DEBUG_COND) {
3699 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (yellow)\n";
3700 : }
3701 : #endif
3702 19798631 : break;
3703 : }
3704 618728976 : const bool influencerPrio = (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority());
3705 : MSLink::BlockingFoes collectFoes;
3706 618728976 : bool opened = (yellow || influencerPrio
3707 1855882178 : || link->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
3708 618576601 : getVehicleType().getLength(),
3709 591776270 : canBrake ? getImpatience() : 1,
3710 : getCarFollowModel().getMaxDecel(),
3711 618576601 : getWaitingTimeFor(link), getLateralPositionOnLane(),
3712 : ls == LINKSTATE_ZIPPER ? &collectFoes : nullptr,
3713 618576601 : ignoreRedLink, this, dpi.myDistance));
3714 613502505 : if (opened && myLaneChangeModel->getShadowLane() != nullptr) {
3715 1873041 : const MSLink* const parallelLink = dpi.myLink->getParallelLink(myLaneChangeModel->getShadowDirection());
3716 1873041 : if (parallelLink != nullptr) {
3717 1173934 : const double shadowLatPos = getLateralPositionOnLane() - myLaneChangeModel->getShadowDirection() * 0.5 * (
3718 1173934 : myLane->getWidth() + myLaneChangeModel->getShadowLane()->getWidth());
3719 2347067 : opened = yellow || influencerPrio || (opened && parallelLink->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
3720 1173133 : getVehicleType().getLength(), getImpatience(),
3721 : getCarFollowModel().getMaxDecel(),
3722 : getWaitingTimeFor(link), shadowLatPos, nullptr,
3723 1173133 : ignoreRedLink, this, dpi.myDistance));
3724 : #ifdef DEBUG_EXEC_MOVE
3725 : if (DEBUG_COND) {
3726 : std::cout << SIMTIME
3727 : << " veh=" << getID()
3728 : << " shadowLane=" << myLaneChangeModel->getShadowLane()->getID()
3729 : << " shadowDir=" << myLaneChangeModel->getShadowDirection()
3730 : << " parallelLink=" << (parallelLink == 0 ? "NULL" : parallelLink->getViaLaneOrLane()->getID())
3731 : << " opened=" << opened
3732 : << "\n";
3733 : }
3734 : #endif
3735 : }
3736 : }
3737 : // vehicles should decelerate when approaching a minor link
3738 : #ifdef DEBUG_EXEC_MOVE
3739 : if (DEBUG_COND) {
3740 : std::cout << SIMTIME
3741 : << " opened=" << opened
3742 : << " influencerPrio=" << influencerPrio
3743 : << " linkPrio=" << link->havePriority()
3744 : << " lastContMajor=" << link->lastWasContMajor()
3745 : << " isCont=" << link->isCont()
3746 : << " ignoreRed=" << ignoreRedLink
3747 : << "\n";
3748 : }
3749 : #endif
3750 : double visibilityDistance = link->getFoeVisibilityDistance();
3751 618728976 : bool determinedFoePresence = dpi.myDistance <= visibilityDistance;
3752 618728976 : if (opened && !influencerPrio && !link->havePriority() && !link->lastWasContMajor() && !link->isCont() && !ignoreRedLink) {
3753 16662892 : if (!determinedFoePresence && (canBrake || !yellow)) {
3754 15746413 : vSafe = dpi.myVLinkWait;
3755 15746413 : myHaveToWaitOnNextLink = true;
3756 : #ifdef DEBUG_CHECKREWINDLINKLANES
3757 : if (DEBUG_COND) {
3758 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (minor)\n";
3759 : }
3760 : #endif
3761 15746413 : break;
3762 : } else {
3763 : // past the point of no return. we need to drive fast enough
3764 : // to make it across the link. However, minor slowdowns
3765 : // should be permissible to follow leading traffic safely
3766 : // basically, this code prevents dawdling
3767 : // (it's harder to do this later using
3768 : // SUMO_ATTR_JM_SIGMA_MINOR because we don't know whether the
3769 : // vehicle is already too close to stop at that part of the code)
3770 : //
3771 : // XXX: There is a problem in subsecond simulation: If we cannot
3772 : // make it across the minor link in one step, new traffic
3773 : // could appear on a major foe link and cause a collision. Refs. #1845, #2123
3774 916479 : vSafeMinDist = dpi.myDistance; // distance that must be covered
3775 916479 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3776 1667464 : vSafeMin = MIN3((double)DIST2SPEED(vSafeMinDist + POSITION_EPS), dpi.myVLinkPass, getCarFollowModel().maxNextSafeMin(getSpeed(), this));
3777 : } else {
3778 165494 : vSafeMin = MIN3((double)DIST2SPEED(2 * vSafeMinDist + NUMERICAL_EPS) - getSpeed(), dpi.myVLinkPass, getCarFollowModel().maxNextSafeMin(getSpeed(), this));
3779 : }
3780 : canBrakeVSafeMin = canBrake;
3781 : #ifdef DEBUG_EXEC_MOVE
3782 : if (DEBUG_COND) {
3783 : std::cout << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << " canBrake=" << canBrake << "\n";
3784 : }
3785 : #endif
3786 : }
3787 : }
3788 : // have waited; may pass if opened...
3789 602982563 : if (opened) {
3790 597735312 : vSafe = dpi.myVLinkPass;
3791 597735312 : if (vSafe < getCarFollowModel().getMaxDecel() && vSafe <= dpi.myVLinkWait && vSafe < getCarFollowModel().maxNextSpeed(getSpeed(), this)) {
3792 : // this vehicle is probably not gonna drive across the next junction (heuristic)
3793 52293535 : myHaveToWaitOnNextLink = true;
3794 : #ifdef DEBUG_CHECKREWINDLINKLANES
3795 : if (DEBUG_COND) {
3796 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (very slow)\n";
3797 : }
3798 : #endif
3799 : }
3800 597735312 : if (link->mustStop() && determinedFoePresence && myHaveStoppedFor == nullptr) {
3801 19921 : myHaveStoppedFor = link;
3802 : }
3803 5247251 : } else if (link->getState() == LINKSTATE_ZIPPER) {
3804 1194797 : vSafeZipper = MIN2(vSafeZipper,
3805 1194797 : link->getZipperSpeed(this, dpi.myDistance, dpi.myVLinkPass, dpi.myArrivalTime, &collectFoes));
3806 : } else if (!canBrake
3807 : // always brake hard for traffic lights (since an emergency stop is necessary anyway)
3808 1746 : && link->getTLLogic() == nullptr
3809 : // cannot brake even with emergency deceleration
3810 4053339 : && dpi.myDistance < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0.)) {
3811 : #ifdef DEBUG_EXEC_MOVE
3812 : if (DEBUG_COND) {
3813 : std::cout << SIMTIME << " too fast to brake for closed link\n";
3814 : }
3815 : #endif
3816 237 : vSafe = dpi.myVLinkPass;
3817 : } else {
3818 4052217 : vSafe = dpi.myVLinkWait;
3819 4052217 : myHaveToWaitOnNextLink = true;
3820 : #ifdef DEBUG_CHECKREWINDLINKLANES
3821 : if (DEBUG_COND) {
3822 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (closed)\n";
3823 : }
3824 : #endif
3825 : #ifdef DEBUG_EXEC_MOVE
3826 : if (DEBUG_COND) {
3827 : std::cout << SIMTIME << " braking for closed link=" << link->getViaLaneOrLane()->getID() << "\n";
3828 : }
3829 : #endif
3830 4052217 : break;
3831 : }
3832 618728976 : } else {
3833 415236380 : if (link != nullptr && link->getInternalLaneBefore() != nullptr && myLane->isInternal() && link->getJunction() == myLane->getEdge().getToJunction()) {
3834 : // blocked on the junction. yield request so other vehicles may
3835 : // become junction leader
3836 : #ifdef DEBUG_EXEC_MOVE
3837 : if (DEBUG_COND) {
3838 : std::cout << SIMTIME << " resetting junctionEntryTime at junction '" << link->getJunction()->getID() << "' beause of non-request exitLink\n";
3839 : }
3840 : #endif
3841 230822 : myJunctionEntryTime = SUMOTime_MAX;
3842 230822 : myJunctionConflictEntryTime = SUMOTime_MAX;
3843 : }
3844 : // we have: i->link == 0 || !i->setRequest
3845 415236380 : vSafe = dpi.myVLinkWait;
3846 415236380 : if (vSafe < getSpeed()) {
3847 60930996 : myHaveToWaitOnNextLink = true;
3848 : #ifdef DEBUG_CHECKREWINDLINKLANES
3849 : if (DEBUG_COND) {
3850 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, braking) vSafe=" << vSafe << "\n";
3851 : }
3852 : #endif
3853 354305384 : } else if (vSafe < SUMO_const_haltingSpeed) {
3854 72418552 : myHaveToWaitOnNextLink = true;
3855 : #ifdef DEBUG_CHECKREWINDLINKLANES
3856 : if (DEBUG_COND) {
3857 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, stopping)\n";
3858 : }
3859 : #endif
3860 : }
3861 322335627 : if (link == nullptr && myLFLinkLanes.size() == 1
3862 262055581 : && getBestLanesContinuation().size() > 1
3863 183561 : && getBestLanesContinuation()[1]->hadPermissionChanges()
3864 415367509 : && myLane->getFirstAnyVehicle() == this) {
3865 : // temporal lane closing without notification, visible to the
3866 : // vehicle at the front of the queue
3867 34027 : updateBestLanes(true);
3868 : //std::cout << SIMTIME << " veh=" << getID() << " updated bestLanes=" << toString(getBestLanesContinuation()) << "\n";
3869 : }
3870 : break;
3871 : }
3872 : }
3873 :
3874 : //#ifdef DEBUG_EXEC_MOVE
3875 : // if (DEBUG_COND) {
3876 : // std::cout << "\nvCurrent = " << toString(getSpeed(), 24) << "" << std::endl;
3877 : // std::cout << "vSafe = " << toString(vSafe, 24) << "" << std::endl;
3878 : // std::cout << "vSafeMin = " << toString(vSafeMin, 24) << "" << std::endl;
3879 : // std::cout << "vSafeMinDist = " << toString(vSafeMinDist, 24) << "" << std::endl;
3880 : //
3881 : // double gap = getLeader().second;
3882 : // std::cout << "gap = " << toString(gap, 24) << std::endl;
3883 : // std::cout << "vSafeStoppedLeader = " << toString(getCarFollowModel().stopSpeed(this, getSpeed(), gap, MSCFModel::CalcReason::FUTURE), 24)
3884 : // << "\n" << std::endl;
3885 : // }
3886 : //#endif
3887 :
3888 603822217 : if ((MSGlobals::gSemiImplicitEulerUpdate && vSafe + NUMERICAL_EPS < vSafeMin)
3889 603617875 : || (!MSGlobals::gSemiImplicitEulerUpdate && (vSafe + NUMERICAL_EPS < vSafeMin && vSafeMin != 0))) { // this might be good for the euler case as well
3890 : // XXX: (Leo) This often called stopSpeed with vSafeMinDist==0 (for the ballistic update), since vSafe can become negative
3891 : // 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
3892 : #ifdef DEBUG_EXEC_MOVE
3893 : if (DEBUG_COND) {
3894 : std::cout << "vSafeMin Problem? vSafe=" << vSafe << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << std::endl;
3895 : }
3896 : #endif
3897 244420 : if (canBrakeVSafeMin && vSafe < getSpeed()) {
3898 : // cannot drive across a link so we need to stop before it
3899 125822 : vSafe = MIN2(vSafe, MAX2(getCarFollowModel().minNextSpeed(getSpeed(), this),
3900 62911 : getCarFollowModel().stopSpeed(this, getSpeed(), vSafeMinDist)));
3901 62911 : vSafeMin = 0;
3902 62911 : myHaveToWaitOnNextLink = true;
3903 : #ifdef DEBUG_CHECKREWINDLINKLANES
3904 : if (DEBUG_COND) {
3905 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (vSafe=" << vSafe << " < vSafeMin=" << vSafeMin << ")\n";
3906 : }
3907 : #endif
3908 : } else {
3909 : // if the link is yellow or visibility distance is large
3910 : // then we might not make it across the link in one step anyway..
3911 : // Possibly, the lane after the intersection has a lower speed limit so
3912 : // we really need to drive slower already
3913 : // -> keep driving without dawdling
3914 181509 : vSafeMin = vSafe;
3915 : }
3916 : }
3917 :
3918 : // vehicles inside a roundabout should maintain their requests
3919 603822217 : if (myLane->getEdge().isRoundabout()) {
3920 2579654 : myHaveToWaitOnNextLink = false;
3921 : }
3922 :
3923 603822217 : vSafe = MIN2(vSafe, vSafeZipper);
3924 603822217 : }
3925 :
3926 :
3927 : double
3928 674016161 : MSVehicle::processTraCISpeedControl(double vSafe, double vNext) {
3929 674016161 : if (myInfluencer != nullptr) {
3930 493651 : myInfluencer->setOriginalSpeed(vNext);
3931 : #ifdef DEBUG_TRACI
3932 : if DEBUG_COND2(this) {
3933 : std::cout << SIMTIME << " MSVehicle::processTraCISpeedControl() for vehicle '" << getID() << "'"
3934 : << " vSafe=" << vSafe << " (init)vNext=" << vNext << " keepStopping=" << keepStopping();
3935 : }
3936 : #endif
3937 493651 : if (myInfluencer->isRemoteControlled()) {
3938 7752 : vNext = myInfluencer->implicitSpeedRemote(this, myState.mySpeed);
3939 : }
3940 493651 : const double vMax = getVehicleType().getCarFollowModel().maxNextSpeed(myState.mySpeed, this);
3941 493651 : double vMin = getVehicleType().getCarFollowModel().minNextSpeed(myState.mySpeed, this);
3942 493651 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3943 : vMin = MAX2(0., vMin);
3944 : }
3945 493651 : vNext = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), vNext, vSafe, vMin, vMax);
3946 493651 : if (keepStopping() && myStops.front().getSpeed() == 0) {
3947 : // avoid driving while stopped (unless it's actually a waypoint
3948 3744 : vNext = myInfluencer->getOriginalSpeed();
3949 : }
3950 : #ifdef DEBUG_TRACI
3951 : if DEBUG_COND2(this) {
3952 : std::cout << " (processed)vNext=" << vNext << std::endl;
3953 : }
3954 : #endif
3955 : }
3956 674016161 : return vNext;
3957 : }
3958 :
3959 :
3960 : void
3961 71516208 : MSVehicle::removePassedDriveItems() {
3962 : #ifdef DEBUG_ACTIONSTEPS
3963 : if (DEBUG_COND) {
3964 : std::cout << SIMTIME << " veh=" << getID() << " removePassedDriveItems()\n"
3965 : << " Current items: ";
3966 : for (auto& j : myLFLinkLanes) {
3967 : if (j.myLink == 0) {
3968 : std::cout << "\n Stop at distance " << j.myDistance;
3969 : } else {
3970 : const MSLane* to = j.myLink->getViaLaneOrLane();
3971 : const MSLane* from = j.myLink->getLaneBefore();
3972 : std::cout << "\n Link at distance " << j.myDistance << ": '"
3973 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
3974 : }
3975 : }
3976 : std::cout << "\n myNextDriveItem: ";
3977 : if (myLFLinkLanes.size() != 0) {
3978 : if (myNextDriveItem->myLink == 0) {
3979 : std::cout << "\n Stop at distance " << myNextDriveItem->myDistance;
3980 : } else {
3981 : const MSLane* to = myNextDriveItem->myLink->getViaLaneOrLane();
3982 : const MSLane* from = myNextDriveItem->myLink->getLaneBefore();
3983 : std::cout << "\n Link at distance " << myNextDriveItem->myDistance << ": '"
3984 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
3985 : }
3986 : }
3987 : std::cout << std::endl;
3988 : }
3989 : #endif
3990 71842381 : for (auto j = myLFLinkLanes.begin(); j != myNextDriveItem; ++j) {
3991 : #ifdef DEBUG_ACTIONSTEPS
3992 : if (DEBUG_COND) {
3993 : std::cout << " Removing item: ";
3994 : if (j->myLink == 0) {
3995 : std::cout << "Stop at distance " << j->myDistance;
3996 : } else {
3997 : const MSLane* to = j->myLink->getViaLaneOrLane();
3998 : const MSLane* from = j->myLink->getLaneBefore();
3999 : std::cout << "Link at distance " << j->myDistance << ": '"
4000 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4001 : }
4002 : std::cout << std::endl;
4003 : }
4004 : #endif
4005 326173 : if (j->myLink != nullptr) {
4006 326105 : j->myLink->removeApproaching(this);
4007 : }
4008 : }
4009 71516208 : myLFLinkLanes.erase(myLFLinkLanes.begin(), myNextDriveItem);
4010 71516208 : myNextDriveItem = myLFLinkLanes.begin();
4011 71516208 : }
4012 :
4013 :
4014 : void
4015 1109663 : MSVehicle::updateDriveItems() {
4016 : #ifdef DEBUG_ACTIONSTEPS
4017 : if (DEBUG_COND) {
4018 : std::cout << SIMTIME << " updateDriveItems(), veh='" << getID() << "' (lane: '" << getLane()->getID() << "')\nCurrent drive items:" << std::endl;
4019 : for (const auto& dpi : myLFLinkLanes) {
4020 : std::cout
4021 : << " vPass=" << dpi.myVLinkPass
4022 : << " vWait=" << dpi.myVLinkWait
4023 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4024 : << " request=" << dpi.mySetRequest
4025 : << "\n";
4026 : }
4027 : std::cout << " myNextDriveItem's linked lane: " << (myNextDriveItem->myLink == 0 ? "NULL" : myNextDriveItem->myLink->getViaLaneOrLane()->getID()) << std::endl;
4028 : }
4029 : #endif
4030 1109663 : if (myLFLinkLanes.size() == 0) {
4031 : // nothing to update
4032 : return;
4033 : }
4034 : const MSLink* nextPlannedLink = nullptr;
4035 : // auto i = myLFLinkLanes.begin();
4036 1109663 : auto i = myNextDriveItem;
4037 2219324 : while (i != myLFLinkLanes.end() && nextPlannedLink == nullptr) {
4038 1109661 : nextPlannedLink = i->myLink;
4039 : ++i;
4040 : }
4041 :
4042 1109663 : if (nextPlannedLink == nullptr) {
4043 : // No link for upcoming item -> no need for an update
4044 : #ifdef DEBUG_ACTIONSTEPS
4045 : if (DEBUG_COND) {
4046 : std::cout << "Found no link-related drive item." << std::endl;
4047 : }
4048 : #endif
4049 : return;
4050 : }
4051 :
4052 550754 : if (getLane() == nextPlannedLink->getLaneBefore()) {
4053 : // Current lane approaches the stored next link, i.e. no LC happend and no update is required.
4054 : #ifdef DEBUG_ACTIONSTEPS
4055 : if (DEBUG_COND) {
4056 : std::cout << "Continuing on planned lane sequence, no update required." << std::endl;
4057 : }
4058 : #endif
4059 : return;
4060 : }
4061 : // Lane must have been changed, determine the change direction
4062 540961 : const MSLink* parallelLink = nextPlannedLink->getParallelLink(1);
4063 540961 : if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
4064 : // lcDir = 1;
4065 : } else {
4066 273475 : parallelLink = nextPlannedLink->getParallelLink(-1);
4067 273475 : if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
4068 : // lcDir = -1;
4069 : } else {
4070 : // If the vehicle's current lane is not the approaching lane for the next
4071 : // drive process item's link, it is expected to lead to a parallel link,
4072 : // XXX: What if the lc was an overtaking maneuver and there is no upcoming link?
4073 : // Then a stop item should be scheduled! -> TODO!
4074 : //assert(false);
4075 72442 : return;
4076 : }
4077 : }
4078 : #ifdef DEBUG_ACTIONSTEPS
4079 : if (DEBUG_COND) {
4080 : std::cout << "Changed lane. Drive items will be updated along the current lane continuation." << std::endl;
4081 : }
4082 : #endif
4083 : // Trace link sequence along current best lanes and transfer drive items to the corresponding links
4084 : // DriveItemVector::iterator driveItemIt = myLFLinkLanes.begin();
4085 468519 : DriveItemVector::iterator driveItemIt = myNextDriveItem;
4086 : // In the loop below, lane holds the currently considered lane on the vehicles continuation (including internal lanes)
4087 468519 : const MSLane* lane = myLane;
4088 : assert(myLane == parallelLink->getLaneBefore());
4089 : // *lit is a pointer to the next lane in best continuations for the current lane (always non-internal)
4090 468519 : std::vector<MSLane*>::const_iterator bestLaneIt = getBestLanesContinuation().begin() + 1;
4091 : // Pointer to the new link for the current drive process item
4092 : MSLink* newLink = nullptr;
4093 1740990 : while (driveItemIt != myLFLinkLanes.end()) {
4094 1301547 : if (driveItemIt->myLink == nullptr) {
4095 : // Items not related to a specific link are not updated
4096 : // (XXX: when a stop item corresponded to a dead end, which is overcome by the LC that made
4097 : // the update necessary, this may slow down the vehicle's continuation on the new lane...)
4098 : ++driveItemIt;
4099 165097 : continue;
4100 : }
4101 : // Continuation links for current best lanes are less than for the former drive items (myLFLinkLanes)
4102 : // We just remove the leftover link-items, as they cannot be mapped to new links.
4103 1136450 : if (bestLaneIt == getBestLanesContinuation().end()) {
4104 : #ifdef DEBUG_ACTIONSTEPS
4105 : if (DEBUG_COND) {
4106 : std::cout << "Reached end of the new continuation sequence. Erasing leftover link-items." << std::endl;
4107 : }
4108 : #endif
4109 87935 : while (driveItemIt != myLFLinkLanes.end()) {
4110 58859 : if (driveItemIt->myLink == nullptr) {
4111 : ++driveItemIt;
4112 13112 : continue;
4113 : } else {
4114 45747 : driveItemIt->myLink->removeApproaching(this);
4115 : driveItemIt = myLFLinkLanes.erase(driveItemIt);
4116 : }
4117 : }
4118 : break;
4119 : }
4120 : // Do the actual link-remapping for the item. And un/register approaching information on the corresponding links
4121 1107374 : const MSLane* const target = *bestLaneIt;
4122 : assert(!target->isInternal());
4123 : newLink = nullptr;
4124 1230929 : for (MSLink* const link : lane->getLinkCont()) {
4125 1230929 : if (link->getLane() == target) {
4126 : newLink = link;
4127 : break;
4128 : }
4129 : }
4130 :
4131 1107374 : if (newLink == driveItemIt->myLink) {
4132 : // new continuation merged into previous - stop update
4133 : #ifdef DEBUG_ACTIONSTEPS
4134 : if (DEBUG_COND) {
4135 : std::cout << "Old and new continuation sequences merge at link\n"
4136 : << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'"
4137 : << "\nNo update beyond merge required." << std::endl;
4138 : }
4139 : #endif
4140 : break;
4141 : }
4142 :
4143 : #ifdef DEBUG_ACTIONSTEPS
4144 : if (DEBUG_COND) {
4145 : std::cout << "lane=" << lane->getID() << "\nUpdating link\n '" << driveItemIt->myLink->getLaneBefore()->getID() << "'->'" << driveItemIt->myLink->getViaLaneOrLane()->getID() << "'"
4146 : << "==> " << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'" << std::endl;
4147 : }
4148 : #endif
4149 1107374 : newLink->setApproaching(this, driveItemIt->myLink->getApproaching(this));
4150 1107374 : driveItemIt->myLink->removeApproaching(this);
4151 1107374 : driveItemIt->myLink = newLink;
4152 : lane = newLink->getViaLaneOrLane();
4153 : ++driveItemIt;
4154 1107374 : if (!lane->isInternal()) {
4155 : ++bestLaneIt;
4156 : }
4157 : }
4158 : #ifdef DEBUG_ACTIONSTEPS
4159 : if (DEBUG_COND) {
4160 : std::cout << "Updated drive items:" << std::endl;
4161 : for (const auto& dpi : myLFLinkLanes) {
4162 : std::cout
4163 : << " vPass=" << dpi.myVLinkPass
4164 : << " vWait=" << dpi.myVLinkWait
4165 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4166 : << " request=" << dpi.mySetRequest
4167 : << "\n";
4168 : }
4169 : }
4170 : #endif
4171 : }
4172 :
4173 :
4174 : void
4175 674016161 : MSVehicle::setBrakingSignals(double vNext) {
4176 : // To avoid casual blinking brake lights at high speeds due to dawdling of the
4177 : // leading vehicle, we don't show brake lights when the deceleration could be caused
4178 : // by frictional forces and air resistance (i.e. proportional to v^2, coefficient could be adapted further)
4179 674016161 : double pseudoFriction = (0.05 + 0.005 * getSpeed()) * getSpeed();
4180 674016161 : bool brakelightsOn = vNext < getSpeed() - ACCEL2SPEED(pseudoFriction);
4181 :
4182 674016161 : if (vNext <= SUMO_const_haltingSpeed) {
4183 : brakelightsOn = true;
4184 : }
4185 674016161 : if (brakelightsOn && !isStopped()) {
4186 : switchOnSignal(VEH_SIGNAL_BRAKELIGHT);
4187 : } else {
4188 : switchOffSignal(VEH_SIGNAL_BRAKELIGHT);
4189 : }
4190 674016161 : }
4191 :
4192 :
4193 : void
4194 674121440 : MSVehicle::updateWaitingTime(double vNext) {
4195 674121440 : if (vNext <= SUMO_const_haltingSpeed && (!isStopped() || isIdling()) && myAcceleration <= accelThresholdForWaiting()) {
4196 83193228 : myWaitingTime += DELTA_T;
4197 83193228 : myWaitingTimeCollector.passTime(DELTA_T, true);
4198 : } else {
4199 590928212 : myWaitingTime = 0;
4200 590928212 : myWaitingTimeCollector.passTime(DELTA_T, false);
4201 590928212 : if (hasInfluencer()) {
4202 270823 : getInfluencer().setExtraImpatience(0);
4203 : }
4204 : }
4205 674121440 : }
4206 :
4207 :
4208 : void
4209 674015979 : MSVehicle::updateTimeLoss(double vNext) {
4210 : // update time loss (depends on the updated edge)
4211 674015979 : if (!isStopped()) {
4212 660482839 : const double vmax = myLane->getVehicleMaxSpeed(this);
4213 660482839 : if (vmax > 0) {
4214 660475456 : myTimeLoss += TS * (vmax - vNext) / vmax;
4215 : }
4216 : }
4217 674015979 : }
4218 :
4219 :
4220 : double
4221 1480049751 : MSVehicle::checkReversal(bool& canReverse, double speedThreshold, double seen) const {
4222 49306846 : const bool stopOk = (myStops.empty() || myStops.front().edge != myCurrEdge
4223 1502314337 : || (myStops.front().getSpeed() > 0 && myState.myPos > myStops.front().pars.endPos - 2 * POSITION_EPS));
4224 : #ifdef DEBUG_REVERSE_BIDI
4225 : if (DEBUG_COND) std::cout << SIMTIME << " checkReversal lane=" << myLane->getID()
4226 : << " pos=" << myState.myPos
4227 : << " speed=" << std::setprecision(6) << getPreviousSpeed() << std::setprecision(gPrecision)
4228 : << " speedThreshold=" << speedThreshold
4229 : << " seen=" << seen
4230 : << " isRail=" << isRail()
4231 : << " speedOk=" << (getPreviousSpeed() <= speedThreshold)
4232 : << " posOK=" << (myState.myPos <= myLane->getLength())
4233 : << " normal=" << !myLane->isInternal()
4234 : << " routeOK=" << ((myCurrEdge + 1) != myRoute->end())
4235 : << " bidi=" << (myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1))
4236 : << " stopOk=" << stopOk
4237 : << "\n";
4238 : #endif
4239 1480049751 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4240 6600921 : && getPreviousSpeed() <= speedThreshold
4241 5633448 : && myState.myPos <= myLane->getLength()
4242 5632455 : && !myLane->isInternal()
4243 5567220 : && (myCurrEdge + 1) != myRoute->end()
4244 5478536 : && myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1)
4245 : // ensure there are no further stops on this edge
4246 1480896347 : && stopOk
4247 : ) {
4248 : //if (isSelected()) std::cout << " check1 passed\n";
4249 :
4250 : // ensure that the vehicle is fully on bidi edges that allow reversal
4251 177786 : const int neededFutureRoute = 1 + (int)(MSGlobals::gUsingInternalLanes
4252 : ? myFurtherLanes.size()
4253 504 : : ceil((double)myFurtherLanes.size() / 2.0));
4254 177786 : const int remainingRoute = int(myRoute->end() - myCurrEdge) - 1;
4255 177786 : if (remainingRoute < neededFutureRoute) {
4256 : #ifdef DEBUG_REVERSE_BIDI
4257 : if (DEBUG_COND) {
4258 : std::cout << " fail: remainingEdges=" << ((int)(myRoute->end() - myCurrEdge)) << " further=" << myFurtherLanes.size() << "\n";
4259 : }
4260 : #endif
4261 3567 : return getMaxSpeed();
4262 : }
4263 : //if (isSelected()) std::cout << " check2 passed\n";
4264 :
4265 : // ensure that the turn-around connection exists from the current edge to its bidi-edge
4266 174219 : const MSEdgeVector& succ = myLane->getEdge().getSuccessors();
4267 174219 : if (std::find(succ.begin(), succ.end(), myLane->getEdge().getBidiEdge()) == succ.end()) {
4268 : #ifdef DEBUG_REVERSE_BIDI
4269 : if (DEBUG_COND) {
4270 : std::cout << " noTurn (bidi=" << myLane->getEdge().getBidiEdge()->getID() << " succ=" << toString(succ) << "\n";
4271 : }
4272 : #endif
4273 909 : return getMaxSpeed();
4274 : }
4275 : //if (isSelected()) std::cout << " check3 passed\n";
4276 :
4277 : // ensure that the vehicle front will not move past a stop on the bidi edge of the current edge
4278 173310 : if (!myStops.empty() && myStops.front().edge == (myCurrEdge + 1)) {
4279 160006 : const double stopPos = myStops.front().getEndPos(*this);
4280 160006 : const double brakeDist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
4281 160006 : const double newPos = myLane->getLength() - (getBackPositionOnLane() + brakeDist);
4282 160006 : if (newPos > stopPos) {
4283 : #ifdef DEBUG_REVERSE_BIDI
4284 : if (DEBUG_COND) {
4285 : std::cout << " reversal would go past stop on " << myLane->getBidiLane()->getID() << "\n";
4286 : }
4287 : #endif
4288 158332 : if (seen > MAX2(brakeDist, 1.0)) {
4289 157202 : return getMaxSpeed();
4290 : } else {
4291 : #ifdef DEBUG_REVERSE_BIDI
4292 : if (DEBUG_COND) {
4293 : std::cout << " train is too long, skipping stop at " << stopPos << " cannot be avoided\n";
4294 : }
4295 : #endif
4296 : }
4297 : }
4298 : }
4299 : //if (isSelected()) std::cout << " check4 passed\n";
4300 :
4301 : // ensure that bidi-edges exist for all further edges
4302 : // and that no stops will be skipped when reversing
4303 : // and that the train will not be on top of a red rail signal after reversal
4304 16108 : const MSLane* bidi = myLane->getBidiLane();
4305 : int view = 2;
4306 32828 : for (MSLane* further : myFurtherLanes) {
4307 18277 : if (!further->getEdge().isInternal()) {
4308 9709 : if (further->getEdge().getBidiEdge() != *(myCurrEdge + view)) {
4309 : #ifdef DEBUG_REVERSE_BIDI
4310 : if (DEBUG_COND) {
4311 : std::cout << " noBidi view=" << view << " further=" << further->getID() << " furtherBidi=" << Named::getIDSecure(further->getEdge().getBidiEdge()) << " future=" << (*(myCurrEdge + view))->getID() << "\n";
4312 : }
4313 : #endif
4314 1398 : return getMaxSpeed();
4315 : }
4316 8311 : const MSLane* nextBidi = further->getBidiLane();
4317 8311 : const MSLink* toNext = bidi->getLinkTo(nextBidi);
4318 8311 : if (toNext == nullptr) {
4319 : // can only happen if the route is invalid
4320 0 : return getMaxSpeed();
4321 : }
4322 8311 : if (toNext->haveRed()) {
4323 : #ifdef DEBUG_REVERSE_BIDI
4324 : if (DEBUG_COND) {
4325 : std::cout << " do not reverse on a red signal\n";
4326 : }
4327 : #endif
4328 0 : return getMaxSpeed();
4329 : }
4330 : bidi = nextBidi;
4331 8311 : if (!myStops.empty() && myStops.front().edge == (myCurrEdge + view)) {
4332 453 : const double brakeDist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
4333 453 : const double stopPos = myStops.front().getEndPos(*this);
4334 453 : const double newPos = further->getLength() - (getBackPositionOnLane(further) + brakeDist);
4335 453 : if (newPos > stopPos) {
4336 : #ifdef DEBUG_REVERSE_BIDI
4337 : if (DEBUG_COND) {
4338 : std::cout << " reversal would go past stop on further-opposite lane " << further->getBidiLane()->getID() << "\n";
4339 : }
4340 : #endif
4341 171 : if (seen > MAX2(brakeDist, 1.0)) {
4342 159 : canReverse = false;
4343 159 : return getMaxSpeed();
4344 : } else {
4345 : #ifdef DEBUG_REVERSE_BIDI
4346 : if (DEBUG_COND) {
4347 : std::cout << " train is too long, skipping stop at " << stopPos << " cannot be avoided\n";
4348 : }
4349 : #endif
4350 : }
4351 : }
4352 : }
4353 8152 : view++;
4354 : }
4355 : }
4356 : // reverse as soon as comfortably possible
4357 14551 : const double vMinComfortable = getCarFollowModel().minNextSpeed(getSpeed(), this);
4358 : #ifdef DEBUG_REVERSE_BIDI
4359 : if (DEBUG_COND) {
4360 : std::cout << SIMTIME << " seen=" << seen << " vReverseOK=" << vMinComfortable << "\n";
4361 : }
4362 : #endif
4363 14551 : canReverse = true;
4364 14551 : return vMinComfortable;
4365 : }
4366 1479871965 : return getMaxSpeed();
4367 : }
4368 :
4369 :
4370 : void
4371 674229535 : MSVehicle::processLaneAdvances(std::vector<MSLane*>& passedLanes, std::string& emergencyReason) {
4372 687827976 : for (std::vector<MSLane*>::reverse_iterator i = myFurtherLanes.rbegin(); i != myFurtherLanes.rend(); ++i) {
4373 13598441 : passedLanes.push_back(*i);
4374 : }
4375 674229535 : if (passedLanes.size() == 0 || passedLanes.back() != myLane) {
4376 674229535 : passedLanes.push_back(myLane);
4377 : }
4378 : // let trains reverse direction
4379 674229535 : bool reverseTrain = false;
4380 674229535 : checkReversal(reverseTrain);
4381 674229535 : if (reverseTrain) {
4382 : // Train is 'reversing' so toggle the logical state
4383 772 : myAmReversed = !myAmReversed;
4384 : // add some slack to ensure that the back of train does appear looped
4385 772 : myState.myPos += 2 * (myLane->getLength() - myState.myPos) + myType->getLength() + NUMERICAL_EPS;
4386 772 : myState.mySpeed = 0;
4387 : #ifdef DEBUG_REVERSE_BIDI
4388 : if (DEBUG_COND) {
4389 : std::cout << SIMTIME << " reversing train=" << getID() << " newPos=" << myState.myPos << "\n";
4390 : }
4391 : #endif
4392 : }
4393 : // move on lane(s)
4394 674229535 : if (myState.myPos > myLane->getLength()) {
4395 : // The vehicle has moved at least to the next lane (maybe it passed even more than one)
4396 17853010 : if (myCurrEdge != myRoute->end() - 1) {
4397 15113343 : MSLane* approachedLane = myLane;
4398 : // move the vehicle forward
4399 15113343 : myNextDriveItem = myLFLinkLanes.begin();
4400 32789595 : while (myNextDriveItem != myLFLinkLanes.end() && approachedLane != nullptr && myState.myPos > approachedLane->getLength()) {
4401 17691563 : const MSLink* link = myNextDriveItem->myLink;
4402 17691563 : const double linkDist = myNextDriveItem->myDistance;
4403 : ++myNextDriveItem;
4404 : // check whether the vehicle was allowed to enter lane
4405 : // otherwise it is decelerated and we do not need to test for it's
4406 : // approach on the following lanes when a lane changing is performed
4407 : // proceed to the next lane
4408 17691563 : if (approachedLane->mustCheckJunctionCollisions()) {
4409 : // vehicle moves past approachedLane within a single step, collision checking must still be done
4410 60518 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(approachedLane);
4411 : }
4412 17691563 : if (link != nullptr) {
4413 17688720 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4414 39794 : && !myLane->isInternal()
4415 21056 : && myLane->getBidiLane() != nullptr
4416 11913 : && link->getLane()->getBidiLane() == myLane
4417 17689489 : && !reverseTrain) {
4418 : emergencyReason = " because it must reverse direction";
4419 : approachedLane = nullptr;
4420 : break;
4421 : }
4422 17688717 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4423 39791 : && myState.myPos < myLane->getLength() + NUMERICAL_EPS
4424 17688942 : && hasStops() && getNextStop().edge == myCurrEdge) {
4425 : // avoid skipping stop due to numerical instability
4426 : // this is a special case for rail vehicles because they
4427 : // continue myLFLinkLanes past stops
4428 197 : approachedLane = myLane;
4429 197 : myState.myPos = myLane->getLength();
4430 197 : break;
4431 : }
4432 17688520 : approachedLane = link->getViaLaneOrLane();
4433 17688520 : if (myInfluencer == nullptr || myInfluencer->getEmergencyBrakeRedLight()) {
4434 17686916 : bool beyondStopLine = linkDist < link->getLaneBefore()->getVehicleStopOffset(this);
4435 17686916 : if (link->haveRed() && !ignoreRed(link, false) && !beyondStopLine && !reverseTrain) {
4436 : emergencyReason = " because of a red traffic light";
4437 : break;
4438 : }
4439 : }
4440 17688465 : if (reverseTrain && approachedLane->isInternal()) {
4441 : // avoid getting stuck on a slow turn-around internal lane
4442 842 : myState.myPos += approachedLane->getLength();
4443 : }
4444 2843 : } else if (myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
4445 : // avoid warning due to numerical instability
4446 165 : approachedLane = myLane;
4447 165 : myState.myPos = myLane->getLength();
4448 2678 : } else if (reverseTrain) {
4449 0 : approachedLane = (*(myCurrEdge + 1))->getLanes()[0];
4450 0 : link = myLane->getLinkTo(approachedLane);
4451 : assert(link != 0);
4452 0 : while (link->getViaLane() != nullptr) {
4453 0 : link = link->getViaLane()->getLinkCont()[0];
4454 : }
4455 : --myNextDriveItem;
4456 : } else {
4457 : emergencyReason = " because there is no connection to the next edge";
4458 : approachedLane = nullptr;
4459 : break;
4460 : }
4461 17688630 : if (approachedLane != myLane && approachedLane != nullptr) {
4462 17688465 : leaveLane(MSMoveReminder::NOTIFICATION_JUNCTION, approachedLane);
4463 17688465 : myState.myPos -= myLane->getLength();
4464 : assert(myState.myPos > 0);
4465 17688465 : enterLaneAtMove(approachedLane);
4466 17688465 : if (link->isEntryLink()) {
4467 6788576 : myJunctionEntryTime = MSNet::getInstance()->getCurrentTimeStep();
4468 6788576 : myJunctionEntryTimeNeverYield = myJunctionEntryTime;
4469 6788576 : myHaveStoppedFor = nullptr;
4470 : }
4471 17688465 : if (link->isConflictEntryLink()) {
4472 6788144 : myJunctionConflictEntryTime = MSNet::getInstance()->getCurrentTimeStep();
4473 : // renew yielded request
4474 6788144 : myJunctionEntryTime = myJunctionEntryTimeNeverYield;
4475 : }
4476 17688465 : if (link->isExitLink()) {
4477 : // passed junction, reset for approaching the next one
4478 6728904 : myJunctionEntryTime = SUMOTime_MAX;
4479 6728904 : myJunctionEntryTimeNeverYield = SUMOTime_MAX;
4480 6728904 : myJunctionConflictEntryTime = SUMOTime_MAX;
4481 : }
4482 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
4483 : if (DEBUG_COND) {
4484 : std::cout << "Update junctionTimes link=" << link->getViaLaneOrLane()->getID()
4485 : << " entry=" << link->isEntryLink() << " conflict=" << link->isConflictEntryLink() << " exit=" << link->isExitLink()
4486 : << " ET=" << myJunctionEntryTime
4487 : << " ETN=" << myJunctionEntryTimeNeverYield
4488 : << " CET=" << myJunctionConflictEntryTime
4489 : << "\n";
4490 : }
4491 : #endif
4492 17688465 : if (hasArrivedInternal()) {
4493 : break;
4494 : }
4495 17676843 : if (myLaneChangeModel->isChangingLanes()) {
4496 6570 : if (link->getDirection() == LinkDirection::LEFT || link->getDirection() == LinkDirection::RIGHT) {
4497 : // abort lane change
4498 0 : WRITE_WARNING("Vehicle '" + getID() + "' could not finish continuous lane change (turn lane) time=" +
4499 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
4500 0 : myLaneChangeModel->endLaneChangeManeuver();
4501 : }
4502 : }
4503 17676843 : if (approachedLane->getEdge().isVaporizing()) {
4504 756 : leaveLane(MSMoveReminder::NOTIFICATION_VAPORIZED_VAPORIZER);
4505 : break;
4506 : }
4507 17676087 : passedLanes.push_back(approachedLane);
4508 : }
4509 : }
4510 : // NOTE: Passed drive items will be erased in the next simstep's planMove()
4511 :
4512 : #ifdef DEBUG_ACTIONSTEPS
4513 : if (DEBUG_COND && myNextDriveItem != myLFLinkLanes.begin()) {
4514 : std::cout << "Updated drive items:" << std::endl;
4515 : for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
4516 : std::cout
4517 : << " vPass=" << (*i).myVLinkPass
4518 : << " vWait=" << (*i).myVLinkWait
4519 : << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
4520 : << " request=" << (*i).mySetRequest
4521 : << "\n";
4522 : }
4523 : }
4524 : #endif
4525 2739667 : } else if (!hasArrivedInternal() && myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
4526 : // avoid warning due to numerical instability when stopping at the end of the route
4527 64 : myState.myPos = myLane->getLength();
4528 : }
4529 :
4530 : }
4531 674229535 : }
4532 :
4533 :
4534 :
4535 : bool
4536 675338425 : MSVehicle::executeMove() {
4537 : #ifdef DEBUG_EXEC_MOVE
4538 : if (DEBUG_COND) {
4539 : std::cout << "\nEXECUTE_MOVE\n"
4540 : << SIMTIME
4541 : << " veh=" << getID()
4542 : << " speed=" << getSpeed() // toString(getSpeed(), 24)
4543 : << std::endl;
4544 : }
4545 : #endif
4546 :
4547 :
4548 : // Maximum safe velocity
4549 675338425 : double vSafe = std::numeric_limits<double>::max();
4550 : // Minimum safe velocity (lower bound).
4551 675338425 : double vSafeMin = -std::numeric_limits<double>::max();
4552 : // The distance to a link, which should either be crossed this step
4553 : // or in front of which we need to stop.
4554 675338425 : double vSafeMinDist = 0;
4555 :
4556 675338425 : if (myActionStep) {
4557 : // Actuate control (i.e. choose bounds for safe speed in current simstep (euler), resp. after current sim step (ballistic))
4558 603822217 : processLinkApproaches(vSafe, vSafeMin, vSafeMinDist);
4559 : #ifdef DEBUG_ACTIONSTEPS
4560 : if (DEBUG_COND) {
4561 : std::cout << SIMTIME << " vehicle '" << getID() << "'\n"
4562 : " vsafe from processLinkApproaches(): vsafe " << vSafe << std::endl;
4563 : }
4564 : #endif
4565 : } else {
4566 : // Continue with current acceleration
4567 71516208 : vSafe = getSpeed() + ACCEL2SPEED(myAcceleration);
4568 : #ifdef DEBUG_ACTIONSTEPS
4569 : if (DEBUG_COND) {
4570 : std::cout << SIMTIME << " vehicle '" << getID() << "' skips processLinkApproaches()\n"
4571 : " continues with constant accel " << myAcceleration << "...\n"
4572 : << "speed: " << getSpeed() << " -> " << vSafe << std::endl;
4573 : }
4574 : #endif
4575 : }
4576 :
4577 :
4578 : //#ifdef DEBUG_EXEC_MOVE
4579 : // if (DEBUG_COND) {
4580 : // std::cout << "vSafe = " << toString(vSafe,12) << "\n" << std::endl;
4581 : // }
4582 : //#endif
4583 :
4584 : // Determine vNext = speed after current sim step (ballistic), resp. in current simstep (euler)
4585 : // Call to finalizeSpeed applies speed reduction due to dawdling / lane changing but ensures minimum safe speed
4586 675338425 : double vNext = vSafe;
4587 675338425 : const double rawAccel = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
4588 675338425 : if (vNext <= SUMO_const_haltingSpeed * TS && myWaitingTime > MSGlobals::gStartupWaitThreshold && rawAccel <= accelThresholdForWaiting() && myActionStep) {
4589 67746231 : myTimeSinceStartup = 0;
4590 607592194 : } else if (isStopped()) {
4591 : // do not apply startupDelay for waypoints
4592 14844652 : if (getCarFollowModel().startupDelayStopped() && getNextStop().pars.speed <= 0) {
4593 13772 : myTimeSinceStartup = DELTA_T;
4594 : } else {
4595 : // do not apply startupDelay but signal that a stop has taken place
4596 14830880 : myTimeSinceStartup = getCarFollowModel().getStartupDelay() + DELTA_T;
4597 : }
4598 : } else {
4599 : // identify potential startup (before other effects reduce the speed again)
4600 592747542 : myTimeSinceStartup += DELTA_T;
4601 : }
4602 675338425 : if (myActionStep) {
4603 603822217 : vNext = getCarFollowModel().finalizeSpeed(this, vSafe);
4604 602499771 : if (vNext > 0) {
4605 561652640 : vNext = MAX2(vNext, vSafeMin);
4606 : }
4607 : }
4608 : // (Leo) to avoid tiny oscillations (< 1e-10) of vNext in a standing vehicle column (observed for ballistic update), we cap off vNext
4609 : // (We assure to do this only for vNext<<NUMERICAL_EPS since otherwise this would nullify the workaround for #2995
4610 : // (Jakob) We also need to make sure to reach a stop at the start of the next edge
4611 674015979 : if (fabs(vNext) < NUMERICAL_EPS_SPEED && (myStopDist > POSITION_EPS || (hasStops() && myCurrEdge == getNextStop().edge))) {
4612 : vNext = 0.;
4613 : }
4614 : #ifdef DEBUG_EXEC_MOVE
4615 : if (DEBUG_COND) {
4616 : std::cout << SIMTIME << " finalizeSpeed vSafe=" << vSafe << " vSafeMin=" << (vSafeMin == -std::numeric_limits<double>::max() ? "-Inf" : toString(vSafeMin))
4617 : << " vNext=" << vNext << " (i.e. accel=" << SPEED2ACCEL(vNext - getSpeed()) << ")" << std::endl;
4618 : }
4619 : #endif
4620 :
4621 : // vNext may be higher than vSafe without implying a bug:
4622 : // - when approaching a green light that suddenly switches to yellow
4623 : // - when using unregulated junctions
4624 : // - when using tau < step-size
4625 : // - when using unsafe car following models
4626 : // - when using TraCI and some speedMode / laneChangeMode settings
4627 : //if (vNext > vSafe + NUMERICAL_EPS) {
4628 : // WRITE_WARNING("vehicle '" + getID() + "' cannot brake hard enough to reach safe speed "
4629 : // + toString(vSafe, 4) + ", moving at " + toString(vNext, 4) + " instead. time="
4630 : // + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
4631 : //}
4632 :
4633 674015979 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4634 : vNext = MAX2(vNext, 0.);
4635 : } else {
4636 : // (Leo) Ballistic: negative vNext can be used to indicate a stop within next step.
4637 : }
4638 :
4639 : // Check for speed advices from the traci client
4640 674015979 : vNext = processTraCISpeedControl(vSafe, vNext);
4641 :
4642 : // the acceleration of a vehicle equipped with the elecHybrid device is restricted by the maximal power of the electric drive as well
4643 674015979 : MSDevice_ElecHybrid* elecHybridOfVehicle = dynamic_cast<MSDevice_ElecHybrid*>(getDevice(typeid(MSDevice_ElecHybrid)));
4644 1135 : if (elecHybridOfVehicle != nullptr) {
4645 : // this is the consumption given by the car following model-computed acceleration
4646 1135 : elecHybridOfVehicle->setConsum(elecHybridOfVehicle->consumption(*this, (vNext - this->getSpeed()) / TS, vNext));
4647 : // but the maximum power of the electric motor may be lower
4648 : // it needs to be converted from [W] to [Wh/s] (3600s / 1h) so that TS can be taken into account
4649 1135 : double maxPower = getEmissionParameters()->getDoubleOptional(SUMO_ATTR_MAXIMUMPOWER, 100000.) / 3600;
4650 1135 : if (elecHybridOfVehicle->getConsum() / TS > maxPower) {
4651 : // no, we cannot accelerate that fast, recompute the maximum possible acceleration
4652 70 : double accel = elecHybridOfVehicle->acceleration(*this, maxPower, this->getSpeed());
4653 : // and update the speed of the vehicle
4654 70 : vNext = MIN2(vNext, this->getSpeed() + accel * TS);
4655 : vNext = MAX2(vNext, 0.);
4656 : // and set the vehicle consumption to reflect this
4657 70 : elecHybridOfVehicle->setConsum(elecHybridOfVehicle->consumption(*this, (vNext - this->getSpeed()) / TS, vNext));
4658 : }
4659 : }
4660 :
4661 674015979 : setBrakingSignals(vNext);
4662 :
4663 : // update position and speed
4664 674015979 : int oldLaneOffset = myLane->getEdge().getNumLanes() - myLane->getIndex();
4665 : const MSLane* oldLaneMaybeOpposite = myLane;
4666 674015979 : if (myLaneChangeModel->isOpposite()) {
4667 : // transform to the forward-direction lane, move and then transform back
4668 409887 : myState.myPos = myLane->getOppositePos(myState.myPos);
4669 409887 : myLane = myLane->getParallelOpposite();
4670 : }
4671 674015979 : updateState(vNext);
4672 674015979 : updateWaitingTime(vNext);
4673 :
4674 : // Lanes, which the vehicle touched at some moment of the executed simstep
4675 : std::vector<MSLane*> passedLanes;
4676 : // remember previous lane (myLane is updated in processLaneAdvances)
4677 674015979 : const MSLane* oldLane = myLane;
4678 : // Reason for a possible emergency stop
4679 : std::string emergencyReason;
4680 674015979 : processLaneAdvances(passedLanes, emergencyReason);
4681 :
4682 674015979 : updateTimeLoss(vNext);
4683 674015979 : myCollisionImmunity = MAX2((SUMOTime) - 1, myCollisionImmunity - DELTA_T);
4684 :
4685 674015979 : if (!hasArrivedInternal() && !myLane->getEdge().isVaporizing()) {
4686 671112891 : if (myState.myPos > myLane->getLength()) {
4687 324 : if (emergencyReason == "") {
4688 70 : emergencyReason = TL(" for unknown reasons");
4689 : }
4690 1296 : WRITE_WARNINGF(TL("Vehicle '%' performs emergency stop at the end of lane '%'% (decel=%, offset=%), time=%."),
4691 : getID(), myLane->getID(), emergencyReason, myAcceleration - myState.mySpeed,
4692 : myState.myPos - myLane->getLength(), time2string(SIMSTEP));
4693 324 : MSNet::getInstance()->getVehicleControl().registerEmergencyStop();
4694 324 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::EMERGENCYSTOP);
4695 324 : myState.myPos = myLane->getLength();
4696 324 : myState.mySpeed = 0;
4697 324 : myAcceleration = 0;
4698 : }
4699 671112891 : const MSLane* oldBackLane = getBackLane();
4700 671112891 : if (myLaneChangeModel->isOpposite()) {
4701 : passedLanes.clear(); // ignore back occupation
4702 : }
4703 : #ifdef DEBUG_ACTIONSTEPS
4704 : if (DEBUG_COND) {
4705 : std::cout << SIMTIME << " veh '" << getID() << "' updates further lanes." << std::endl;
4706 : }
4707 : #endif
4708 671112891 : myState.myBackPos = updateFurtherLanes(myFurtherLanes, myFurtherLanesPosLat, passedLanes);
4709 671112891 : if (passedLanes.size() > 1 && isRail()) {
4710 731173 : for (auto pi = passedLanes.rbegin(); pi != passedLanes.rend(); ++pi) {
4711 554534 : MSLane* pLane = *pi;
4712 554534 : if (pLane != myLane && std::find(myFurtherLanes.begin(), myFurtherLanes.end(), pLane) == myFurtherLanes.end()) {
4713 39487 : leaveLaneBack(MSMoveReminder::NOTIFICATION_JUNCTION, *pi);
4714 : }
4715 : }
4716 : }
4717 : // bestLanes need to be updated before lane changing starts. NOTE: This call is also a presumption for updateDriveItems()
4718 671112891 : updateBestLanes();
4719 671112891 : if (myLane != oldLane || oldBackLane != getBackLane()) {
4720 21868449 : if (myLaneChangeModel->getShadowLane() != nullptr || getLateralOverlap() > POSITION_EPS) {
4721 : // shadow lane must be updated if the front or back lane changed
4722 : // either if we already have a shadowLane or if there is lateral overlap
4723 414328 : myLaneChangeModel->updateShadowLane();
4724 : }
4725 21868449 : if (MSGlobals::gLateralResolution > 0 && !myLaneChangeModel->isOpposite()) {
4726 : // The vehicles target lane must be also be updated if the front or back lane changed
4727 4284157 : myLaneChangeModel->updateTargetLane();
4728 : }
4729 : }
4730 671112891 : setBlinkerInformation(); // needs updated bestLanes
4731 : //change the blue light only for emergency vehicles SUMOVehicleClass
4732 671112891 : if (myType->getVehicleClass() == SVC_EMERGENCY) {
4733 79153 : setEmergencyBlueLight(MSNet::getInstance()->getCurrentTimeStep());
4734 : }
4735 : // must be done before angle computation
4736 : // State needs to be reset for all vehicles before the next call to MSEdgeControl::changeLanes
4737 671112891 : if (myActionStep) {
4738 : // check (#2681): Can this be skipped?
4739 599617904 : myLaneChangeModel->prepareStep();
4740 : } else {
4741 71494987 : myLaneChangeModel->resetSpeedLat();
4742 : #ifdef DEBUG_ACTIONSTEPS
4743 : if (DEBUG_COND) {
4744 : std::cout << SIMTIME << " veh '" << getID() << "' skips LCM->prepareStep()." << std::endl;
4745 : }
4746 : #endif
4747 : }
4748 671112891 : myLaneChangeModel->setPreviousAngleOffset(myLaneChangeModel->getAngleOffset());
4749 671112891 : myAngle = computeAngle();
4750 : }
4751 :
4752 : #ifdef DEBUG_EXEC_MOVE
4753 : if (DEBUG_COND) {
4754 : std::cout << SIMTIME << " executeMove finished veh=" << getID() << " lane=" << myLane->getID() << " myPos=" << getPositionOnLane() << " myPosLat=" << getLateralPositionOnLane() << "\n";
4755 : gDebugFlag1 = false; // See MSLink_DEBUG_OPENED
4756 : }
4757 : #endif
4758 674015979 : if (myLaneChangeModel->isOpposite()) {
4759 : // transform back to the opposite-direction lane
4760 : MSLane* newOpposite = nullptr;
4761 409887 : const MSEdge* newOppositeEdge = myLane->getEdge().getOppositeEdge();
4762 409887 : if (newOppositeEdge != nullptr) {
4763 409837 : newOpposite = newOppositeEdge->getLanes()[newOppositeEdge->getNumLanes() - MAX2(1, oldLaneOffset)];
4764 : #ifdef DEBUG_EXEC_MOVE
4765 : if (DEBUG_COND) {
4766 : std::cout << SIMTIME << " newOppositeEdge=" << newOppositeEdge->getID() << " oldLaneOffset=" << oldLaneOffset << " leftMost=" << newOppositeEdge->getNumLanes() - 1 << " newOpposite=" << Named::getIDSecure(newOpposite) << "\n";
4767 : }
4768 : #endif
4769 : }
4770 409837 : if (newOpposite == nullptr) {
4771 50 : if (!myLaneChangeModel->hasBlueLight()) {
4772 : // unusual overtaking at junctions is ok for emergency vehicles
4773 0 : WRITE_WARNINGF(TL("Unexpected end of opposite lane for vehicle '%' at lane '%', time=%."),
4774 : getID(), myLane->getID(), time2string(SIMSTEP));
4775 : }
4776 50 : myLaneChangeModel->changedToOpposite();
4777 50 : if (myState.myPos < getLength()) {
4778 : // further lanes is always cleared during opposite driving
4779 50 : MSLane* oldOpposite = oldLane->getOpposite();
4780 50 : if (oldOpposite != nullptr) {
4781 50 : myFurtherLanes.push_back(oldOpposite);
4782 50 : myFurtherLanesPosLat.push_back(0);
4783 : // small value since the lane is going in the other direction
4784 50 : myState.myBackPos = getLength() - myState.myPos;
4785 50 : myAngle = computeAngle();
4786 : } else {
4787 : SOFT_ASSERT(false);
4788 : }
4789 : }
4790 : } else {
4791 409837 : myState.myPos = myLane->getOppositePos(myState.myPos);
4792 409837 : myLane = newOpposite;
4793 : oldLane = oldLaneMaybeOpposite;
4794 : //std::cout << SIMTIME << " updated myLane=" << Named::getIDSecure(myLane) << " oldLane=" << oldLane->getID() << "\n";
4795 409837 : myCachedPosition = Position::INVALID;
4796 409837 : myLaneChangeModel->updateShadowLane();
4797 : }
4798 : }
4799 674015979 : workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
4800 : // Return whether the vehicle did move to another lane
4801 1348031958 : return myLane != oldLane;
4802 674015979 : }
4803 :
4804 : void
4805 213556 : MSVehicle::executeFractionalMove(double dist) {
4806 213556 : myState.myPos += dist;
4807 213556 : myState.myLastCoveredDist = dist;
4808 213556 : myCachedPosition = Position::INVALID;
4809 :
4810 213556 : const std::vector<const MSLane*> lanes = getUpcomingLanesUntil(dist);
4811 213556 : const SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
4812 443117 : for (int i = 0; i < (int)lanes.size(); i++) {
4813 229561 : MSLink* link = nullptr;
4814 229561 : if (i + 1 < (int)lanes.size()) {
4815 16005 : const MSLane* const to = lanes[i + 1];
4816 16005 : const bool internal = to->isInternal();
4817 16010 : for (MSLink* const l : lanes[i]->getLinkCont()) {
4818 16010 : if ((internal && l->getViaLane() == to) || (!internal && l->getLane() == to)) {
4819 16005 : link = l;
4820 16005 : break;
4821 : }
4822 : }
4823 : }
4824 229561 : myLFLinkLanes.emplace_back(link, getSpeed(), getSpeed(), true, t, getSpeed(), 0, 0, dist);
4825 : }
4826 : // minimum execute move:
4827 : std::vector<MSLane*> passedLanes;
4828 : // Reason for a possible emergency stop
4829 213556 : if (lanes.size() > 1) {
4830 4005 : myLane->removeVehicle(this, MSMoveReminder::NOTIFICATION_JUNCTION, false);
4831 : }
4832 : std::string emergencyReason;
4833 213556 : processLaneAdvances(passedLanes, emergencyReason);
4834 : #ifdef DEBUG_EXTRAPOLATE_DEPARTPOS
4835 : if (DEBUG_COND) {
4836 : std::cout << SIMTIME << " veh=" << getID() << " executeFractionalMove dist=" << dist
4837 : << " passedLanes=" << toString(passedLanes) << " lanes=" << toString(lanes)
4838 : << " finalPos=" << myState.myPos
4839 : << " speed=" << getSpeed()
4840 : << " myFurtherLanes=" << toString(myFurtherLanes)
4841 : << "\n";
4842 : }
4843 : #endif
4844 213556 : workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
4845 213556 : if (lanes.size() > 1) {
4846 4010 : for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
4847 : #ifdef DEBUG_FURTHER
4848 : if (DEBUG_COND) {
4849 : std::cout << SIMTIME << " leaveLane \n";
4850 : }
4851 : #endif
4852 5 : (*i)->resetPartialOccupation(this);
4853 : }
4854 : myFurtherLanes.clear();
4855 : myFurtherLanesPosLat.clear();
4856 4005 : myLane->forceVehicleInsertion(this, getPositionOnLane(), MSMoveReminder::NOTIFICATION_JUNCTION, getLateralPositionOnLane());
4857 : }
4858 213556 : }
4859 :
4860 :
4861 : void
4862 682367737 : MSVehicle::updateState(double vNext, bool parking) {
4863 : // update position and speed
4864 : double deltaPos; // positional change
4865 682367737 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4866 : // euler
4867 583148781 : deltaPos = SPEED2DIST(vNext);
4868 : } else {
4869 : // ballistic
4870 99218956 : deltaPos = getDeltaPos(SPEED2ACCEL(vNext - myState.mySpeed));
4871 : }
4872 :
4873 : // the *mean* acceleration during the next step (probably most appropriate for emission calculation)
4874 : // NOTE: for the ballistic update vNext may be negative, indicating a stop.
4875 682367737 : myAcceleration = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
4876 :
4877 : #ifdef DEBUG_EXEC_MOVE
4878 : if (DEBUG_COND) {
4879 : std::cout << SIMTIME << " updateState() for veh '" << getID() << "': deltaPos=" << deltaPos
4880 : << " pos=" << myState.myPos << " newPos=" << myState.myPos + deltaPos << std::endl;
4881 : }
4882 : #endif
4883 682367737 : double decelPlus = -myAcceleration - getCarFollowModel().getMaxDecel() - NUMERICAL_EPS;
4884 682367737 : if (decelPlus > 0) {
4885 420418 : const double previousAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
4886 420418 : if (myAcceleration + NUMERICAL_EPS < previousAcceleration) {
4887 : // vehicle brakes beyond wished maximum deceleration (only warn at the start of the braking manoeuvre)
4888 289365 : decelPlus += 2 * NUMERICAL_EPS;
4889 289365 : const double emergencyFraction = decelPlus / MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel());
4890 289365 : if (emergencyFraction >= MSGlobals::gEmergencyDecelWarningThreshold) {
4891 93750 : WRITE_WARNINGF(TL("Vehicle '%' performs emergency braking on lane '%' with decel=%, wished=%, severity=%, time=%."),
4892 : //+ " decelPlus=" + toString(decelPlus)
4893 : //+ " prevAccel=" + toString(previousAcceleration)
4894 : //+ " reserve=" + toString(MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel()))
4895 : getID(), myLane->getID(), -myAcceleration, getCarFollowModel().getMaxDecel(), emergencyFraction, time2string(SIMSTEP));
4896 31250 : MSNet::getInstance()->getVehicleControl().registerEmergencyBraking();
4897 : }
4898 : }
4899 : }
4900 :
4901 682367737 : myState.myPreviousSpeed = myState.mySpeed;
4902 682367737 : myState.mySpeed = MAX2(vNext, 0.);
4903 :
4904 682367737 : if (isRemoteControlled()) {
4905 7580 : deltaPos = myInfluencer->implicitDeltaPosRemote(this);
4906 : }
4907 :
4908 682367737 : myState.myPos += deltaPos;
4909 682367737 : myState.myLastCoveredDist = deltaPos;
4910 682367737 : myNextTurn.first -= deltaPos;
4911 :
4912 682367737 : if (!parking) {
4913 674015979 : myCachedPosition = Position::INVALID;
4914 : }
4915 682367737 : }
4916 :
4917 : void
4918 8351758 : MSVehicle::updateParkingState() {
4919 8351758 : updateState(0, true);
4920 : // deboard while parked
4921 8351758 : if (myPersonDevice != nullptr) {
4922 586576 : myPersonDevice->notifyMove(*this, getPositionOnLane(), getPositionOnLane(), 0);
4923 : }
4924 8351758 : if (myContainerDevice != nullptr) {
4925 59887 : myContainerDevice->notifyMove(*this, getPositionOnLane(), getPositionOnLane(), 0);
4926 : }
4927 16400639 : for (MSVehicleDevice* const dev : myDevices) {
4928 8048881 : dev->notifyParking();
4929 : }
4930 8351758 : }
4931 :
4932 :
4933 : void
4934 30643 : MSVehicle::replaceVehicleType(const MSVehicleType* type) {
4935 30643 : MSBaseVehicle::replaceVehicleType(type);
4936 30643 : delete myCFVariables;
4937 30643 : myCFVariables = type->getCarFollowModel().createVehicleVariables();
4938 30643 : }
4939 :
4940 :
4941 : const MSLane*
4942 1327912837 : MSVehicle::getBackLane() const {
4943 1327912837 : if (myFurtherLanes.size() > 0) {
4944 16002675 : return myFurtherLanes.back();
4945 : } else {
4946 1311910162 : return myLane;
4947 : }
4948 : }
4949 :
4950 :
4951 : double
4952 676499696 : MSVehicle::updateFurtherLanes(std::vector<MSLane*>& furtherLanes, std::vector<double>& furtherLanesPosLat,
4953 : const std::vector<MSLane*>& passedLanes) {
4954 : #ifdef DEBUG_SETFURTHER
4955 : if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID()
4956 : << " updateFurtherLanes oldFurther=" << toString(furtherLanes)
4957 : << " oldFurtherPosLat=" << toString(furtherLanesPosLat)
4958 : << " passed=" << toString(passedLanes)
4959 : << "\n";
4960 : #endif
4961 690134316 : for (MSLane* further : furtherLanes) {
4962 13634620 : further->resetPartialOccupation(this);
4963 13634620 : if (further->getBidiLane() != nullptr
4964 13634620 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
4965 25347 : further->getBidiLane()->resetPartialOccupation(this);
4966 : }
4967 : }
4968 :
4969 : std::vector<MSLane*> newFurther;
4970 : std::vector<double> newFurtherPosLat;
4971 676499696 : double backPosOnPreviousLane = myState.myPos - getLength();
4972 : bool widthShift = myFurtherLanesPosLat.size() > myFurtherLanes.size();
4973 676499696 : if (passedLanes.size() > 1) {
4974 : // There are candidates for further lanes. (passedLanes[-1] is the current lane, or current shadow lane in context of updateShadowLanes())
4975 : std::vector<MSLane*>::const_iterator fi = furtherLanes.begin();
4976 : std::vector<double>::const_iterator fpi = furtherLanesPosLat.begin();
4977 38810022 : for (auto pi = passedLanes.rbegin() + 1; pi != passedLanes.rend() && backPosOnPreviousLane < 0; ++pi) {
4978 : // As long as vehicle back reaches into passed lane, add it to the further lanes
4979 13568532 : MSLane* further = *pi;
4980 13568532 : newFurther.push_back(further);
4981 13568532 : backPosOnPreviousLane += further->setPartialOccupation(this);
4982 13568532 : if (further->getBidiLane() != nullptr
4983 13568532 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
4984 24841 : further->getBidiLane()->setPartialOccupation(this);
4985 : }
4986 13568532 : if (fi != furtherLanes.end() && further == *fi) {
4987 : // Lateral position on this lane is already known. Assume constant and use old value.
4988 4791673 : newFurtherPosLat.push_back(*fpi);
4989 : ++fi;
4990 : ++fpi;
4991 : } else {
4992 : // The lane *pi was not in furtherLanes before.
4993 : // If it is downstream, we assume as lateral position the current position
4994 : // If it is a new lane upstream (can appear as shadow further in case of LC-maneuvering, e.g.)
4995 : // we assign the last known lateral position.
4996 8776859 : if (newFurtherPosLat.size() == 0) {
4997 8187796 : if (widthShift) {
4998 1440415 : newFurtherPosLat.push_back(myFurtherLanesPosLat.back());
4999 : } else {
5000 6747381 : newFurtherPosLat.push_back(myState.myPosLat);
5001 : }
5002 : } else {
5003 589063 : newFurtherPosLat.push_back(newFurtherPosLat.back());
5004 : }
5005 : }
5006 : #ifdef DEBUG_SETFURTHER
5007 : if (DEBUG_COND) {
5008 : std::cout << SIMTIME << " updateFurtherLanes \n"
5009 : << " further lane '" << further->getID() << "' backPosOnPreviousLane=" << backPosOnPreviousLane
5010 : << std::endl;
5011 : }
5012 : #endif
5013 : }
5014 25241490 : furtherLanes = newFurther;
5015 25241490 : furtherLanesPosLat = newFurtherPosLat;
5016 : } else {
5017 : furtherLanes.clear();
5018 : furtherLanesPosLat.clear();
5019 : }
5020 : #ifdef DEBUG_SETFURTHER
5021 : if (DEBUG_COND) std::cout
5022 : << " newFurther=" << toString(furtherLanes)
5023 : << " newFurtherPosLat=" << toString(furtherLanesPosLat)
5024 : << " newBackPos=" << backPosOnPreviousLane
5025 : << "\n";
5026 : #endif
5027 676499696 : return backPosOnPreviousLane;
5028 676499696 : }
5029 :
5030 :
5031 : double
5032 33706565328 : MSVehicle::getBackPositionOnLane(const MSLane* lane, bool calledByGetPosition) const {
5033 : #ifdef DEBUG_FURTHER
5034 : if (DEBUG_COND) {
5035 : std::cout << SIMTIME
5036 : << " getBackPositionOnLane veh=" << getID()
5037 : << " lane=" << Named::getIDSecure(lane)
5038 : << " cbgP=" << calledByGetPosition
5039 : << " pos=" << myState.myPos
5040 : << " backPos=" << myState.myBackPos
5041 : << " myLane=" << myLane->getID()
5042 : << " myLaneBidi=" << Named::getIDSecure(myLane->getBidiLane())
5043 : << " further=" << toString(myFurtherLanes)
5044 : << " furtherPosLat=" << toString(myFurtherLanesPosLat)
5045 : << "\n shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
5046 : << " shadowFurther=" << toString(myLaneChangeModel->getShadowFurtherLanes())
5047 : << " shadowFurtherPosLat=" << toString(myLaneChangeModel->getShadowFurtherLanesPosLat())
5048 : << "\n targetLane=" << Named::getIDSecure(myLaneChangeModel->getTargetLane())
5049 : << " furtherTargets=" << toString(myLaneChangeModel->getFurtherTargetLanes())
5050 : << std::endl;
5051 : }
5052 : #endif
5053 33706565328 : if (lane == myLane
5054 7956607486 : || lane == myLaneChangeModel->getShadowLane()
5055 38189651003 : || lane == myLaneChangeModel->getTargetLane()) {
5056 29225051516 : if (myLaneChangeModel->isOpposite()) {
5057 231571582 : if (lane == myLaneChangeModel->getShadowLane()) {
5058 198916049 : return lane->getLength() - myState.myPos - myType->getLength();
5059 : } else {
5060 37597946 : return myState.myPos + (calledByGetPosition ? -1 : 1) * myType->getLength();
5061 : }
5062 28993479934 : } else if (&lane->getEdge() != &myLane->getEdge()) {
5063 20747013 : return lane->getLength() - myState.myPos + (calledByGetPosition ? -1 : 1) * myType->getLength();
5064 : } else {
5065 : // account for parallel lanes of different lengths in the most conservative manner (i.e. while turning)
5066 57946175718 : return myState.myPos - myType->getLength() + MIN2(0.0, lane->getLength() - myLane->getLength());
5067 : }
5068 4481513812 : } else if (lane == myLane->getBidiLane()) {
5069 3691207 : return lane->getLength() - myState.myPos + myType->getLength() * (calledByGetPosition ? -1 : 1);
5070 4479237117 : } else if (myFurtherLanes.size() > 0 && lane == myFurtherLanes.back()) {
5071 4432537589 : return myState.myBackPos;
5072 46699528 : } else if ((myLaneChangeModel->getShadowFurtherLanes().size() > 0 && lane == myLaneChangeModel->getShadowFurtherLanes().back())
5073 47146056 : || (myLaneChangeModel->getFurtherTargetLanes().size() > 0 && lane == myLaneChangeModel->getFurtherTargetLanes().back())) {
5074 : assert(myFurtherLanes.size() > 0);
5075 19345442 : if (lane->getLength() == myFurtherLanes.back()->getLength()) {
5076 18846025 : return myState.myBackPos;
5077 : } else {
5078 : // interpolate
5079 : //if (DEBUG_COND) {
5080 : //if (myFurtherLanes.back()->getLength() != lane->getLength()) {
5081 : // std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " further=" << myFurtherLanes.back()->getID()
5082 : // << " len=" << lane->getLength() << " fLen=" << myFurtherLanes.back()->getLength()
5083 : // << " backPos=" << myState.myBackPos << " result=" << myState.myBackPos / myFurtherLanes.back()->getLength() * lane->getLength() << "\n";
5084 : //}
5085 499417 : return myState.myBackPos / myFurtherLanes.back()->getLength() * lane->getLength();
5086 : }
5087 : } else {
5088 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherLanes=" << toString(myFurtherLanes) << "\n";
5089 27354086 : double leftLength = myType->getLength() - myState.myPos;
5090 :
5091 : std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin();
5092 29409927 : while (leftLength > 0 && i != myFurtherLanes.end()) {
5093 29384738 : leftLength -= (*i)->getLength();
5094 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5095 29384738 : if (*i == lane) {
5096 27042994 : return -leftLength;
5097 2341744 : } else if (*i == lane->getBidiLane()) {
5098 285903 : return lane->getLength() + leftLength - (calledByGetPosition ? 2 * myType->getLength() : 0);
5099 : }
5100 : ++i;
5101 : }
5102 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
5103 25189 : leftLength = myType->getLength() - myState.myPos;
5104 25189 : i = myLaneChangeModel->getShadowFurtherLanes().begin();
5105 25189 : while (leftLength > 0 && i != myLaneChangeModel->getShadowFurtherLanes().end()) {
5106 25185 : leftLength -= (*i)->getLength();
5107 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5108 25185 : if (*i == lane) {
5109 25185 : return -leftLength;
5110 : }
5111 : ++i;
5112 : }
5113 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherTargetLanes=" << toString(myLaneChangeModel->getFurtherTargetLanes()) << "\n";
5114 : leftLength = myType->getLength() - myState.myPos;
5115 : i = getFurtherLanes().begin();
5116 4 : const std::vector<MSLane*> furtherTargetLanes = myLaneChangeModel->getFurtherTargetLanes();
5117 : auto j = furtherTargetLanes.begin();
5118 5 : while (leftLength > 0 && j != furtherTargetLanes.end()) {
5119 2 : leftLength -= (*i)->getLength();
5120 : // if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5121 2 : if (*j == lane) {
5122 1 : return -leftLength;
5123 : }
5124 : ++i;
5125 : ++j;
5126 : }
5127 21 : WRITE_WARNING("Request backPos of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane)
5128 : + "' time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".")
5129 : SOFT_ASSERT(false);
5130 3 : return myState.myBackPos;
5131 4 : }
5132 : }
5133 :
5134 :
5135 : double
5136 27601257030 : MSVehicle::getPositionOnLane(const MSLane* lane) const {
5137 27601257030 : return getBackPositionOnLane(lane, true) + myType->getLength();
5138 : }
5139 :
5140 :
5141 : bool
5142 406408342 : MSVehicle::isFrontOnLane(const MSLane* lane) const {
5143 406408342 : return lane == myLane || lane == myLaneChangeModel->getShadowLane() || lane == myLane->getBidiLane();
5144 : }
5145 :
5146 :
5147 : void
5148 603822217 : MSVehicle::checkRewindLinkLanes(const double lengthsInFront, DriveItemVector& lfLinks) const {
5149 603822217 : if (MSGlobals::gUsingInternalLanes && !myLane->getEdge().isRoundabout() && !myLaneChangeModel->isOpposite()) {
5150 600711566 : double seenSpace = -lengthsInFront;
5151 : #ifdef DEBUG_CHECKREWINDLINKLANES
5152 : if (DEBUG_COND) {
5153 : std::cout << "\nCHECK_REWIND_LINKLANES\n" << " veh=" << getID() << " lengthsInFront=" << lengthsInFront << "\n";
5154 : };
5155 : #endif
5156 600711566 : bool foundStopped = false;
5157 : // compute available space until a stopped vehicle is found
5158 : // this is the sum of non-interal lane length minus in-between vehicle lengths
5159 1737473659 : for (int i = 0; i < (int)lfLinks.size(); ++i) {
5160 : // skip unset links
5161 1136762093 : DriveProcessItem& item = lfLinks[i];
5162 : #ifdef DEBUG_CHECKREWINDLINKLANES
5163 : if (DEBUG_COND) std::cout << SIMTIME
5164 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5165 : << " foundStopped=" << foundStopped;
5166 : #endif
5167 1136762093 : if (item.myLink == nullptr || foundStopped) {
5168 384322562 : if (!foundStopped) {
5169 328066712 : item.availableSpace += seenSpace;
5170 : } else {
5171 56255850 : item.availableSpace = seenSpace;
5172 : }
5173 : #ifdef DEBUG_CHECKREWINDLINKLANES
5174 : if (DEBUG_COND) {
5175 : std::cout << " avail=" << item.availableSpace << "\n";
5176 : }
5177 : #endif
5178 384322562 : continue;
5179 : }
5180 : // get the next lane, determine whether it is an internal lane
5181 : const MSLane* approachedLane = item.myLink->getViaLane();
5182 752439531 : if (approachedLane != nullptr) {
5183 404495495 : if (keepClear(item.myLink)) {
5184 123528563 : seenSpace = seenSpace - approachedLane->getBruttoVehLenSum();
5185 123528563 : if (approachedLane == myLane) {
5186 51001 : seenSpace += getVehicleType().getLengthWithGap();
5187 : }
5188 : } else {
5189 280966932 : seenSpace = seenSpace + approachedLane->getSpaceTillLastStanding(this, foundStopped);// - approachedLane->getBruttoVehLenSum() + approachedLane->getLength();
5190 : }
5191 404495495 : item.availableSpace = seenSpace;
5192 : #ifdef DEBUG_CHECKREWINDLINKLANES
5193 : if (DEBUG_COND) std::cout
5194 : << " approached=" << approachedLane->getID()
5195 : << " approachedBrutto=" << approachedLane->getBruttoVehLenSum()
5196 : << " avail=" << item.availableSpace
5197 : << " seenSpace=" << seenSpace
5198 : << " hadStoppedVehicle=" << item.hadStoppedVehicle
5199 : << " lengthsInFront=" << lengthsInFront
5200 : << "\n";
5201 : #endif
5202 404495495 : continue;
5203 : }
5204 : approachedLane = item.myLink->getLane();
5205 347944036 : const MSVehicle* last = approachedLane->getLastAnyVehicle();
5206 347944036 : if (last == nullptr || last == this) {
5207 53999306 : if (approachedLane->getLength() > getVehicleType().getLength()
5208 53999306 : || keepClear(item.myLink)) {
5209 51874379 : seenSpace += approachedLane->getLength();
5210 : }
5211 53999306 : item.availableSpace = seenSpace;
5212 : #ifdef DEBUG_CHECKREWINDLINKLANES
5213 : if (DEBUG_COND) {
5214 : std::cout << " last=" << Named::getIDSecure(last) << " laneLength=" << approachedLane->getLength() << " avail=" << item.availableSpace << "\n";
5215 : }
5216 : #endif
5217 : } else {
5218 293944730 : bool foundStopped2 = false;
5219 293944730 : double spaceTillLastStanding = approachedLane->getSpaceTillLastStanding(this, foundStopped2);
5220 293944730 : if (approachedLane->getBidiLane() != nullptr) {
5221 79769 : const MSVehicle* oncomingVeh = approachedLane->getBidiLane()->getFirstFullVehicle();
5222 79769 : if (oncomingVeh) {
5223 27331 : const double oncomingGap = approachedLane->getLength() - oncomingVeh->getPositionOnLane();
5224 27331 : const double oncomingBGap = oncomingVeh->getBrakeGap(true);
5225 : // oncoming movement until ego enters the junction
5226 27331 : const double oncomingMove = STEPS2TIME(item.myArrivalTime - SIMSTEP) * oncomingVeh->getSpeed();
5227 27331 : const double spaceTillOncoming = oncomingGap - oncomingBGap - oncomingMove;
5228 : spaceTillLastStanding = MIN2(spaceTillLastStanding, spaceTillOncoming);
5229 27331 : if (spaceTillOncoming <= getVehicleType().getLengthWithGap()) {
5230 22542 : foundStopped = true;
5231 : }
5232 : #ifdef DEBUG_CHECKREWINDLINKLANES
5233 : if (DEBUG_COND) {
5234 : std::cout << " oVeh=" << oncomingVeh->getID()
5235 : << " oGap=" << oncomingGap
5236 : << " bGap=" << oncomingBGap
5237 : << " mGap=" << oncomingMove
5238 : << " sto=" << spaceTillOncoming;
5239 : }
5240 : #endif
5241 : }
5242 : }
5243 293944730 : seenSpace += spaceTillLastStanding;
5244 293944730 : if (foundStopped2) {
5245 18785400 : foundStopped = true;
5246 18785400 : item.hadStoppedVehicle = true;
5247 : }
5248 293944730 : item.availableSpace = seenSpace;
5249 293944730 : if (last->myHaveToWaitOnNextLink || last->isStopped()) {
5250 40976989 : foundStopped = true;
5251 40976989 : item.hadStoppedVehicle = true;
5252 : }
5253 : #ifdef DEBUG_CHECKREWINDLINKLANES
5254 : if (DEBUG_COND) std::cout
5255 : << " approached=" << approachedLane->getID()
5256 : << " last=" << last->getID()
5257 : << " lastHasToWait=" << last->myHaveToWaitOnNextLink
5258 : << " lastBrakeLight=" << last->signalSet(VEH_SIGNAL_BRAKELIGHT)
5259 : << " lastBrakeGap=" << last->getCarFollowModel().brakeGap(last->getSpeed())
5260 : << " lastGap=" << (last->getBackPositionOnLane(approachedLane) + last->getCarFollowModel().brakeGap(last->getSpeed()) - last->getSpeed() * last->getCarFollowModel().getHeadwayTime()
5261 : // gap of last up to the next intersection
5262 : - last->getVehicleType().getMinGap())
5263 : << " stls=" << spaceTillLastStanding
5264 : << " avail=" << item.availableSpace
5265 : << " seenSpace=" << seenSpace
5266 : << " foundStopped=" << foundStopped
5267 : << " foundStopped2=" << foundStopped2
5268 : << "\n";
5269 : #endif
5270 : }
5271 : }
5272 :
5273 : // check which links allow continuation and add pass available to the previous item
5274 1136762093 : for (int i = ((int)lfLinks.size() - 1); i > 0; --i) {
5275 536050527 : DriveProcessItem& item = lfLinks[i - 1];
5276 536050527 : DriveProcessItem& nextItem = lfLinks[i];
5277 536050527 : const bool canLeaveJunction = item.myLink->getViaLane() == nullptr || nextItem.myLink == nullptr || nextItem.mySetRequest;
5278 : const bool opened = (item.myLink != nullptr
5279 536050527 : && (canLeaveJunction || (
5280 : // indirect bicycle turn
5281 24634890 : nextItem.myLink != nullptr && nextItem.myLink->isInternalJunctionLink() && nextItem.myLink->haveRed()))
5282 511429676 : && (
5283 511429676 : item.myLink->havePriority()
5284 24988532 : || i == 1 // the upcoming link (item 0) is checked in executeMove anyway. No need to use outdata approachData here
5285 4364305 : || (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority())
5286 4336745 : || item.myLink->opened(item.myArrivalTime, item.myArrivalSpeed,
5287 4336745 : item.getLeaveSpeed(), getVehicleType().getLength(),
5288 4336745 : getImpatience(), getCarFollowModel().getMaxDecel(), getWaitingTime(), getLateralPositionOnLane(), nullptr, false, this)));
5289 536050527 : bool allowsContinuation = (item.myLink == nullptr || item.myLink->isCont() || opened) && !item.hadStoppedVehicle;
5290 : #ifdef DEBUG_CHECKREWINDLINKLANES
5291 : if (DEBUG_COND) std::cout
5292 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5293 : << " canLeave=" << canLeaveJunction
5294 : << " opened=" << opened
5295 : << " allowsContinuation=" << allowsContinuation
5296 : << " foundStopped=" << foundStopped
5297 : << "\n";
5298 : #endif
5299 536050527 : if (!opened && item.myLink != nullptr) {
5300 25194081 : foundStopped = true;
5301 25194081 : if (i > 1) {
5302 3969555 : DriveProcessItem& item2 = lfLinks[i - 2];
5303 3969555 : if (item2.myLink != nullptr && item2.myLink->isCont()) {
5304 : allowsContinuation = true;
5305 : }
5306 : }
5307 : }
5308 533650093 : if (allowsContinuation) {
5309 471737097 : item.availableSpace = nextItem.availableSpace;
5310 : #ifdef DEBUG_CHECKREWINDLINKLANES
5311 : if (DEBUG_COND) std::cout
5312 : << " link=" << (item.myLink == nullptr ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5313 : << " copy nextAvail=" << nextItem.availableSpace
5314 : << "\n";
5315 : #endif
5316 : }
5317 : }
5318 :
5319 : // find removalBegin
5320 : int removalBegin = -1;
5321 745667585 : for (int i = 0; foundStopped && i < (int)lfLinks.size() && removalBegin < 0; ++i) {
5322 : // skip unset links
5323 144956019 : const DriveProcessItem& item = lfLinks[i];
5324 144956019 : if (item.myLink == nullptr) {
5325 8466874 : continue;
5326 : }
5327 : /*
5328 : double impatienceCorrection = MAX2(0., double(double(myWaitingTime)));
5329 : if (seenSpace<getVehicleType().getLengthWithGap()-impatienceCorrection/10.&&nextSeenNonInternal!=0) {
5330 : removalBegin = lastLinkToInternal;
5331 : }
5332 : */
5333 :
5334 136489145 : const double leftSpace = item.availableSpace - getVehicleType().getLengthWithGap();
5335 : #ifdef DEBUG_CHECKREWINDLINKLANES
5336 : if (DEBUG_COND) std::cout
5337 : << SIMTIME
5338 : << " veh=" << getID()
5339 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5340 : << " avail=" << item.availableSpace
5341 : << " leftSpace=" << leftSpace
5342 : << "\n";
5343 : #endif
5344 136489145 : if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
5345 : double impatienceCorrection = 0;
5346 : /*
5347 : if(item.myLink->getState()==LINKSTATE_MINOR) {
5348 : impatienceCorrection = MAX2(0., STEPS2TIME(myWaitingTime));
5349 : }
5350 : */
5351 : // may ignore keepClear rules
5352 78632245 : if (leftSpace < -impatienceCorrection / 10. && keepClear(item.myLink)) {
5353 : removalBegin = i;
5354 : }
5355 : //removalBegin = i;
5356 : }
5357 : }
5358 : // abort requests
5359 600711566 : if (removalBegin != -1 && !(removalBegin == 0 && myLane->getEdge().isInternal())) {
5360 28807573 : const double brakeGap = getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.);
5361 98279740 : while (removalBegin < (int)(lfLinks.size())) {
5362 73934295 : DriveProcessItem& dpi = lfLinks[removalBegin];
5363 73934295 : if (dpi.myLink == nullptr) {
5364 : break;
5365 : }
5366 69472167 : dpi.myVLinkPass = dpi.myVLinkWait;
5367 : #ifdef DEBUG_CHECKREWINDLINKLANES
5368 : if (DEBUG_COND) {
5369 : std::cout << " removalBegin=" << removalBegin << " brakeGap=" << brakeGap << " dist=" << dpi.myDistance << " speed=" << myState.mySpeed << " a2s=" << ACCEL2SPEED(getCarFollowModel().getMaxDecel()) << "\n";
5370 : }
5371 : #endif
5372 69472167 : if (dpi.myDistance >= brakeGap + POSITION_EPS) {
5373 : // always leave junctions after requesting to enter
5374 69462726 : if (!dpi.myLink->isExitLink() || !lfLinks[removalBegin - 1].mySetRequest) {
5375 69454960 : dpi.mySetRequest = false;
5376 : }
5377 : }
5378 69472167 : ++removalBegin;
5379 : }
5380 : }
5381 : }
5382 603822217 : }
5383 :
5384 :
5385 : void
5386 675338425 : MSVehicle::setApproachingForAllLinks() {
5387 675338425 : if (!myActionStep) {
5388 : return;
5389 : }
5390 603822217 : removeApproachingInformation(myLFLinkLanesPrev);
5391 1747974332 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5392 1144152115 : if (dpi.myLink != nullptr) {
5393 805820216 : if (dpi.myLink->getState() == LINKSTATE_ALLWAY_STOP) {
5394 2722956 : dpi.myArrivalTime += (SUMOTime)RandHelper::rand((int)2, getRNG()); // tie braker
5395 : }
5396 805820216 : dpi.myLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
5397 805820216 : dpi.mySetRequest, dpi.myArrivalSpeedBraking, getWaitingTimeFor(dpi.myLink), dpi.myDistance, getLateralPositionOnLane());
5398 : }
5399 : }
5400 603822217 : if (isRail()) {
5401 7462859 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5402 6245014 : if (dpi.myLink != nullptr && dpi.myLink->getTLLogic() != nullptr && dpi.myLink->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL) {
5403 551978 : MSRailSignalControl::getInstance().notifyApproach(dpi.myLink);
5404 : }
5405 : }
5406 : }
5407 603822217 : if (myLaneChangeModel->getShadowLane() != nullptr) {
5408 : // register on all shadow links
5409 7330205 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
5410 4821342 : if (dpi.myLink != nullptr) {
5411 3254561 : MSLink* parallelLink = dpi.myLink->getParallelLink(myLaneChangeModel->getShadowDirection());
5412 3254561 : if (parallelLink == nullptr && getLaneChangeModel().isOpposite() && dpi.myLink->isEntryLink()) {
5413 : // register on opposite direction entry link to warn foes at minor side road
5414 169901 : parallelLink = dpi.myLink->getOppositeDirectionLink();
5415 : }
5416 3254561 : if (parallelLink != nullptr) {
5417 2353408 : const double latOffset = getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge();
5418 2353408 : parallelLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
5419 2353408 : dpi.mySetRequest, dpi.myArrivalSpeedBraking, getWaitingTimeFor(dpi.myLink), dpi.myDistance,
5420 : latOffset);
5421 2353408 : myLaneChangeModel->setShadowApproachingInformation(parallelLink);
5422 : }
5423 : }
5424 : }
5425 : }
5426 : #ifdef DEBUG_PLAN_MOVE
5427 : if (DEBUG_COND) {
5428 : std::cout << SIMTIME
5429 : << " veh=" << getID()
5430 : << " after checkRewindLinkLanes\n";
5431 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5432 : std::cout
5433 : << " vPass=" << dpi.myVLinkPass
5434 : << " vWait=" << dpi.myVLinkWait
5435 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
5436 : << " request=" << dpi.mySetRequest
5437 : << " atime=" << dpi.myArrivalTime
5438 : << "\n";
5439 : }
5440 : }
5441 : #endif
5442 : }
5443 :
5444 :
5445 : void
5446 1674 : MSVehicle::registerInsertionApproach(MSLink* link, double dist) {
5447 : DriveProcessItem dpi(0, dist);
5448 1674 : dpi.myLink = link;
5449 1674 : const double arrivalSpeedBraking = getCarFollowModel().getMinimalArrivalSpeedEuler(dist, getSpeed());
5450 1674 : link->setApproaching(this, SUMOTime_MAX, 0, 0, false, arrivalSpeedBraking, 0, dpi.myDistance, 0);
5451 : // ensure cleanup in the next step
5452 1674 : myLFLinkLanes.push_back(dpi);
5453 1674 : MSRailSignalControl::getInstance().notifyApproach(link);
5454 1674 : }
5455 :
5456 :
5457 : void
5458 17705151 : MSVehicle::enterLaneAtMove(MSLane* enteredLane, bool onTeleporting) {
5459 17705151 : myAmOnNet = !onTeleporting;
5460 : // vaporizing edge?
5461 : /*
5462 : if (enteredLane->getEdge().isVaporizing()) {
5463 : // yep, let's do the vaporization...
5464 : myLane = enteredLane;
5465 : return true;
5466 : }
5467 : */
5468 : // Adjust MoveReminder offset to the next lane
5469 17705151 : adaptLaneEntering2MoveReminder(*enteredLane);
5470 : // set the entered lane as the current lane
5471 17705151 : MSLane* oldLane = myLane;
5472 17705151 : myLane = enteredLane;
5473 17705151 : myLastBestLanesEdge = nullptr;
5474 :
5475 : // internal edges are not a part of the route...
5476 17705151 : if (!enteredLane->getEdge().isInternal()) {
5477 : ++myCurrEdge;
5478 : assert(myLaneChangeModel->isOpposite() || haveValidStopEdges());
5479 : }
5480 17705151 : if (myInfluencer != nullptr) {
5481 9095 : myInfluencer->adaptLaneTimeLine(myLane->getIndex() - oldLane->getIndex());
5482 : }
5483 17705151 : if (!onTeleporting) {
5484 17688465 : activateReminders(MSMoveReminder::NOTIFICATION_JUNCTION, enteredLane);
5485 17688465 : if (MSGlobals::gLateralResolution > 0) {
5486 3726466 : myFurtherLanesPosLat.push_back(myState.myPosLat);
5487 : // transform lateral position when the lane width changes
5488 : assert(oldLane != nullptr);
5489 3726466 : const MSLink* const link = oldLane->getLinkTo(myLane);
5490 3726466 : if (link != nullptr) {
5491 3726424 : myState.myPosLat += link->getLateralShift();
5492 : } else {
5493 42 : myState.myPosLat += (oldLane->getCenterOnEdge() - myLane->getCanonicalPredecessorLane()->getRightSideOnEdge()) / 2;
5494 : }
5495 13961999 : } else if (fabs(myState.myPosLat) > NUMERICAL_EPS) {
5496 199343 : const double overlap = MAX2(0.0, getLateralOverlap(myState.myPosLat, oldLane));
5497 199343 : const double range = (oldLane->getWidth() - getVehicleType().getWidth()) * 0.5 + overlap;
5498 199343 : const double range2 = (myLane->getWidth() - getVehicleType().getWidth()) * 0.5 + overlap;
5499 199343 : myState.myPosLat *= range2 / range;
5500 : }
5501 17688465 : if (myLane->getBidiLane() != nullptr && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5502 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
5503 : // (unless the lane is shared with cars)
5504 9636 : myLane->getBidiLane()->setPartialOccupation(this);
5505 : }
5506 : } else {
5507 : // normal move() isn't called so reset position here. must be done
5508 : // before calling reminders
5509 16686 : myState.myPos = 0;
5510 16686 : myCachedPosition = Position::INVALID;
5511 16686 : activateReminders(MSMoveReminder::NOTIFICATION_TELEPORT, enteredLane);
5512 : }
5513 : // update via
5514 17705151 : if (myParameter->via.size() > 0 && myLane->getEdge().getID() == myParameter->via.front()) {
5515 5725 : myParameter->via.erase(myParameter->via.begin());
5516 : }
5517 17705151 : }
5518 :
5519 :
5520 : void
5521 1066606 : MSVehicle::enterLaneAtLaneChange(MSLane* enteredLane) {
5522 1066606 : myAmOnNet = true;
5523 1066606 : myLane = enteredLane;
5524 1066606 : myCachedPosition = Position::INVALID;
5525 : // need to update myCurrentLaneInBestLanes
5526 1066606 : updateBestLanes();
5527 : // switch to and activate the new lane's reminders
5528 : // keep OldLaneReminders
5529 1261257 : for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
5530 194651 : addReminder(*rem);
5531 : }
5532 1066606 : activateReminders(MSMoveReminder::NOTIFICATION_LANE_CHANGE, enteredLane);
5533 1066606 : MSLane* lane = myLane;
5534 1066606 : double leftLength = getVehicleType().getLength() - myState.myPos;
5535 : int deleteFurther = 0;
5536 : #ifdef DEBUG_SETFURTHER
5537 : if (DEBUG_COND) {
5538 : std::cout << SIMTIME << " enterLaneAtLaneChange entered=" << Named::getIDSecure(enteredLane) << " oldFurther=" << toString(myFurtherLanes) << "\n";
5539 : }
5540 : #endif
5541 1066606 : if (myLane->getBidiLane() != nullptr && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5542 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
5543 : // (unless the lane is shared with cars)
5544 8478 : myLane->getBidiLane()->setPartialOccupation(this);
5545 : }
5546 1146722 : for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
5547 80116 : if (lane != nullptr) {
5548 77065 : lane = lane->getLogicalPredecessorLane(myFurtherLanes[i]->getEdge());
5549 : }
5550 : #ifdef DEBUG_SETFURTHER
5551 : if (DEBUG_COND) {
5552 : std::cout << " enterLaneAtLaneChange i=" << i << " lane=" << Named::getIDSecure(lane) << " leftLength=" << leftLength << "\n";
5553 : }
5554 : #endif
5555 80116 : if (leftLength > 0) {
5556 79598 : if (lane != nullptr) {
5557 29900 : myFurtherLanes[i]->resetPartialOccupation(this);
5558 29900 : if (myFurtherLanes[i]->getBidiLane() != nullptr
5559 29900 : && (!isRailway(getVClass()) || (myFurtherLanes[i]->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5560 60 : myFurtherLanes[i]->getBidiLane()->resetPartialOccupation(this);
5561 : }
5562 : // lane changing onto longer lanes may reduce the number of
5563 : // remaining further lanes
5564 29900 : myFurtherLanes[i] = lane;
5565 29900 : myFurtherLanesPosLat[i] = myState.myPosLat;
5566 29900 : leftLength -= lane->setPartialOccupation(this);
5567 29900 : if (lane->getBidiLane() != nullptr
5568 29900 : && (!isRailway(getVClass()) || (lane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5569 561 : lane->getBidiLane()->setPartialOccupation(this);
5570 : }
5571 29900 : myState.myBackPos = -leftLength;
5572 : #ifdef DEBUG_SETFURTHER
5573 : if (DEBUG_COND) {
5574 : std::cout << SIMTIME << " newBackPos=" << myState.myBackPos << "\n";
5575 : }
5576 : #endif
5577 : } else {
5578 : // keep the old values, but ensure there is no shadow
5579 49698 : if (myLaneChangeModel->isChangingLanes()) {
5580 0 : myLaneChangeModel->setNoShadowPartialOccupator(myFurtherLanes[i]);
5581 : }
5582 49698 : if (myState.myBackPos < 0) {
5583 57 : myState.myBackPos += myFurtherLanes[i]->getLength();
5584 : }
5585 : #ifdef DEBUG_SETFURTHER
5586 : if (DEBUG_COND) {
5587 : std::cout << SIMTIME << " i=" << i << " further=" << myFurtherLanes[i]->getID() << " newBackPos=" << myState.myBackPos << "\n";
5588 : }
5589 : #endif
5590 : }
5591 : } else {
5592 518 : myFurtherLanes[i]->resetPartialOccupation(this);
5593 518 : if (myFurtherLanes[i]->getBidiLane() != nullptr
5594 518 : && (!isRailway(getVClass()) || (myFurtherLanes[i]->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5595 0 : myFurtherLanes[i]->getBidiLane()->resetPartialOccupation(this);
5596 : }
5597 518 : deleteFurther++;
5598 : }
5599 : }
5600 1066606 : if (deleteFurther > 0) {
5601 : #ifdef DEBUG_SETFURTHER
5602 : if (DEBUG_COND) {
5603 : std::cout << SIMTIME << " veh=" << getID() << " shortening myFurtherLanes by " << deleteFurther << "\n";
5604 : }
5605 : #endif
5606 503 : myFurtherLanes.erase(myFurtherLanes.end() - deleteFurther, myFurtherLanes.end());
5607 503 : myFurtherLanesPosLat.erase(myFurtherLanesPosLat.end() - deleteFurther, myFurtherLanesPosLat.end());
5608 : }
5609 : #ifdef DEBUG_SETFURTHER
5610 : if (DEBUG_COND) {
5611 : std::cout << SIMTIME << " enterLaneAtLaneChange new furtherLanes=" << toString(myFurtherLanes)
5612 : << " furterLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
5613 : }
5614 : #endif
5615 1066606 : myAngle = computeAngle();
5616 1066606 : }
5617 :
5618 :
5619 : void
5620 3116243 : MSVehicle::computeFurtherLanes(MSLane* enteredLane, double pos, bool collision) {
5621 : // build the list of lanes the vehicle is lapping into
5622 3116243 : if (!myLaneChangeModel->isOpposite()) {
5623 3094611 : double leftLength = myType->getLength() - pos;
5624 3094611 : MSLane* clane = enteredLane;
5625 3094611 : int routeIndex = getRoutePosition();
5626 3194494 : while (leftLength > 0) {
5627 219136 : if (routeIndex > 0 && clane->getEdge().isNormal()) {
5628 : // get predecessor lane that corresponds to prior route
5629 4413 : routeIndex--;
5630 4413 : const MSEdge* fromRouteEdge = myRoute->getEdges()[routeIndex];
5631 : MSLane* target = clane;
5632 4413 : clane = nullptr;
5633 5785 : for (auto ili : target->getIncomingLanes()) {
5634 5778 : if (ili.lane->getEdge().getNormalBefore() == fromRouteEdge) {
5635 4406 : clane = ili.lane;
5636 4406 : break;
5637 : }
5638 : }
5639 : } else {
5640 214723 : clane = clane->getLogicalPredecessorLane();
5641 : }
5642 129734 : if (clane == nullptr || clane == myLane || clane == myLane->getBidiLane()
5643 348862 : || (clane->isInternal() && (
5644 106707 : clane->getLinkCont()[0]->getDirection() == LinkDirection::TURN
5645 76864 : || clane->getLinkCont()[0]->getDirection() == LinkDirection::TURN_LEFTHAND))) {
5646 : break;
5647 : }
5648 99883 : if (!collision || std::find(myFurtherLanes.begin(), myFurtherLanes.end(), clane) == myFurtherLanes.end()) {
5649 99445 : myFurtherLanes.push_back(clane);
5650 99445 : myFurtherLanesPosLat.push_back(myState.myPosLat);
5651 99445 : clane->setPartialOccupation(this);
5652 99445 : if (clane->getBidiLane() != nullptr
5653 99445 : && (!isRailway(getVClass()) || (clane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5654 5 : clane->getBidiLane()->setPartialOccupation(this);
5655 : }
5656 : }
5657 99883 : leftLength -= clane->getLength();
5658 : }
5659 3094611 : myState.myBackPos = -leftLength;
5660 : #ifdef DEBUG_SETFURTHER
5661 : if (DEBUG_COND) {
5662 : std::cout << SIMTIME << " computeFurtherLanes veh=" << getID() << " pos=" << pos << " myFurtherLanes=" << toString(myFurtherLanes) << " backPos=" << myState.myBackPos << "\n";
5663 : }
5664 : #endif
5665 : } else {
5666 : // clear partial occupation
5667 22039 : for (MSLane* further : myFurtherLanes) {
5668 : #ifdef DEBUG_SETFURTHER
5669 : if (DEBUG_COND) {
5670 : std::cout << SIMTIME << " opposite: resetPartialOccupation " << further->getID() << " \n";
5671 : }
5672 : #endif
5673 407 : further->resetPartialOccupation(this);
5674 407 : if (further->getBidiLane() != nullptr
5675 407 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5676 0 : further->getBidiLane()->resetPartialOccupation(this);
5677 : }
5678 : }
5679 : myFurtherLanes.clear();
5680 : myFurtherLanesPosLat.clear();
5681 : }
5682 3116243 : }
5683 :
5684 :
5685 : void
5686 3115804 : MSVehicle::enterLaneAtInsertion(MSLane* enteredLane, double pos, double speed, double posLat, MSMoveReminder::Notification notification) {
5687 3115804 : myState = State(pos, speed, posLat, pos - getVehicleType().getLength(), hasDeparted() ? myState.myPreviousSpeed : speed);
5688 3115804 : if (myDeparture == NOT_YET_DEPARTED) {
5689 3044694 : onDepart();
5690 : }
5691 3115804 : myCachedPosition = Position::INVALID;
5692 : assert(myState.myPos >= 0);
5693 : assert(myState.mySpeed >= 0);
5694 3115804 : myLane = enteredLane;
5695 3115804 : myAmOnNet = true;
5696 : // schedule action for the next timestep
5697 3115804 : myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + DELTA_T;
5698 3115804 : if (notification != MSMoveReminder::NOTIFICATION_TELEPORT) {
5699 3105337 : if (notification == MSMoveReminder::NOTIFICATION_PARKING && myInfluencer != nullptr) {
5700 13 : drawOutsideNetwork(false);
5701 : }
5702 : // set and activate the new lane's reminders, teleports already did that at enterLaneAtMove
5703 6510569 : for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
5704 3405232 : addReminder(*rem);
5705 : }
5706 3105337 : activateReminders(notification, enteredLane);
5707 : } else {
5708 10467 : myLastBestLanesEdge = nullptr;
5709 10467 : myLastBestLanesInternalLane = nullptr;
5710 10467 : myLaneChangeModel->resetState();
5711 11423 : while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()
5712 10978 : && myStops.front().pars.endPos < pos) {
5713 0 : WRITE_WARNINGF(TL("Vehicle '%' skips stop on lane '%' time=%."), getID(), myStops.front().lane->getID(),
5714 : time2string(MSNet::getInstance()->getCurrentTimeStep()));
5715 0 : myStops.pop_front();
5716 : }
5717 : // avoid startup-effects after teleport
5718 10467 : myTimeSinceStartup = getCarFollowModel().getStartupDelay() + DELTA_T;
5719 :
5720 : }
5721 3115802 : computeFurtherLanes(enteredLane, pos);
5722 3115802 : if (MSGlobals::gLateralResolution > 0) {
5723 498603 : myLaneChangeModel->updateShadowLane();
5724 498603 : myLaneChangeModel->updateTargetLane();
5725 2617199 : } else if (MSGlobals::gLaneChangeDuration > 0) {
5726 34035 : myLaneChangeModel->updateShadowLane();
5727 : }
5728 3115802 : if (notification != MSMoveReminder::NOTIFICATION_LOAD_STATE) {
5729 3114217 : myAngle = computeAngle();
5730 3114217 : if (myLaneChangeModel->isOpposite()) {
5731 21632 : myAngle += M_PI;
5732 : }
5733 : }
5734 3115802 : if (MSNet::getInstance()->hasPersons()) {
5735 65351 : for (MSLane* further : myFurtherLanes) {
5736 747 : if (further->mustCheckJunctionCollisions()) {
5737 4 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(further);
5738 : }
5739 : }
5740 : }
5741 3115802 : }
5742 :
5743 :
5744 : void
5745 21771851 : MSVehicle::leaveLane(const MSMoveReminder::Notification reason, const MSLane* approachedLane) {
5746 60787119 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
5747 39015268 : if (rem->first->notifyLeave(*this, myState.myPos + rem->second, reason, approachedLane)) {
5748 : #ifdef _DEBUG
5749 : if (myTraceMoveReminders) {
5750 : traceMoveReminder("notifyLeave", rem->first, rem->second, true);
5751 : }
5752 : #endif
5753 : ++rem;
5754 : } else {
5755 : #ifdef _DEBUG
5756 : if (myTraceMoveReminders) {
5757 : traceMoveReminder("notifyLeave", rem->first, rem->second, false);
5758 : }
5759 : #endif
5760 : rem = myMoveReminders.erase(rem);
5761 : }
5762 : }
5763 21771851 : if ((reason == MSMoveReminder::NOTIFICATION_JUNCTION
5764 21771851 : || reason == MSMoveReminder::NOTIFICATION_TELEPORT
5765 4072329 : || reason == MSMoveReminder::NOTIFICATION_TELEPORT_CONTINUATION)
5766 17705411 : && myLane != nullptr) {
5767 17705381 : myOdometer += getLane()->getLength();
5768 : }
5769 21771821 : if (myLane != nullptr && myLane->getBidiLane() != nullptr && myAmOnNet
5770 21812780 : && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5771 18653 : myLane->getBidiLane()->resetPartialOccupation(this);
5772 : }
5773 21771851 : if (reason != MSMoveReminder::NOTIFICATION_JUNCTION && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
5774 : // @note. In case of lane change, myFurtherLanes and partial occupation
5775 : // are handled in enterLaneAtLaneChange()
5776 3003028 : for (MSLane* further : myFurtherLanes) {
5777 : #ifdef DEBUG_FURTHER
5778 : if (DEBUG_COND) {
5779 : std::cout << SIMTIME << " leaveLane \n";
5780 : }
5781 : #endif
5782 29305 : further->resetPartialOccupation(this);
5783 29305 : if (further->getBidiLane() != nullptr
5784 29305 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5785 0 : further->getBidiLane()->resetPartialOccupation(this);
5786 : }
5787 : }
5788 : myFurtherLanes.clear();
5789 : myFurtherLanesPosLat.clear();
5790 : }
5791 2973723 : if (reason >= MSMoveReminder::NOTIFICATION_TELEPORT) {
5792 2973723 : myAmOnNet = false;
5793 2973723 : myWaitingTime = 0;
5794 : }
5795 21771851 : if (reason != MSMoveReminder::NOTIFICATION_PARKING && resumeFromStopping()) {
5796 542 : myStopDist = std::numeric_limits<double>::max();
5797 542 : if (myPastStops.back().speed <= 0) {
5798 972 : WRITE_WARNINGF(TL("Vehicle '%' aborts stop."), getID());
5799 : }
5800 : }
5801 21771851 : if (reason != MSMoveReminder::NOTIFICATION_PARKING && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
5802 20647621 : while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()) {
5803 884 : if (myStops.front().getSpeed() <= 0) {
5804 1701 : WRITE_WARNINGF(TL("Vehicle '%' skips stop on lane '%' time=%."), getID(), myStops.front().lane->getID(),
5805 : time2string(MSNet::getInstance()->getCurrentTimeStep()))
5806 567 : if (MSStopOut::active()) {
5807 : // clean up if stopBlocked was called
5808 6 : MSStopOut::getInstance()->stopNotStarted(this);
5809 : }
5810 567 : myStops.pop_front();
5811 : } else {
5812 : MSStop& stop = myStops.front();
5813 : // passed waypoint at the end of the lane
5814 317 : if (!stop.reached) {
5815 317 : if (MSStopOut::active()) {
5816 12 : MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), MSNet::getInstance()->getCurrentTimeStep());
5817 : }
5818 317 : stop.reached = true;
5819 : // enter stopping place so leaveFrom works as expected
5820 317 : if (stop.busstop != nullptr) {
5821 : // let the bus stop know the vehicle
5822 25 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5823 : }
5824 317 : if (stop.containerstop != nullptr) {
5825 : // let the container stop know the vehicle
5826 13 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5827 : }
5828 : // do not enter parkingarea!
5829 317 : if (stop.chargingStation != nullptr) {
5830 : // let the container stop know the vehicle
5831 121 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5832 : }
5833 : }
5834 317 : resumeFromStopping();
5835 : }
5836 884 : myStopDist = std::numeric_limits<double>::max();
5837 : }
5838 : }
5839 21771851 : }
5840 :
5841 :
5842 : void
5843 39487 : MSVehicle::leaveLaneBack(const MSMoveReminder::Notification reason, const MSLane* leftLane) {
5844 161921 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
5845 122434 : if (rem->first->notifyLeaveBack(*this, reason, leftLane)) {
5846 : #ifdef _DEBUG
5847 : if (myTraceMoveReminders) {
5848 : traceMoveReminder("notifyLeaveBack", rem->first, rem->second, true);
5849 : }
5850 : #endif
5851 : ++rem;
5852 : } else {
5853 : #ifdef _DEBUG
5854 : if (myTraceMoveReminders) {
5855 : traceMoveReminder("notifyLeaveBack", rem->first, rem->second, false);
5856 : }
5857 : #endif
5858 : rem = myMoveReminders.erase(rem);
5859 : }
5860 : }
5861 : #ifdef DEBUG_MOVEREMINDERS
5862 : if (DEBUG_COND) {
5863 : std::cout << SIMTIME << " veh=" << getID() << " myReminders:";
5864 : for (auto rem : myMoveReminders) {
5865 : std::cout << rem.first->getDescription() << " ";
5866 : }
5867 : std::cout << "\n";
5868 : }
5869 : #endif
5870 39487 : }
5871 :
5872 :
5873 : MSAbstractLaneChangeModel&
5874 10146741017 : MSVehicle::getLaneChangeModel() {
5875 10146741017 : return *myLaneChangeModel;
5876 : }
5877 :
5878 :
5879 : const MSAbstractLaneChangeModel&
5880 4782385514 : MSVehicle::getLaneChangeModel() const {
5881 4782385514 : return *myLaneChangeModel;
5882 : }
5883 :
5884 : bool
5885 520933 : MSVehicle::isOppositeLane(const MSLane* lane) const {
5886 520933 : return (lane->isInternal()
5887 520933 : ? & (lane->getLinkCont()[0]->getLane()->getEdge()) != *(myCurrEdge + 1)
5888 519140 : : &lane->getEdge() != *myCurrEdge);
5889 : }
5890 :
5891 : const std::vector<MSVehicle::LaneQ>&
5892 496200122 : MSVehicle::getBestLanes() const {
5893 496200122 : return *myBestLanes.begin();
5894 : }
5895 :
5896 :
5897 : void
5898 1003923781 : MSVehicle::updateBestLanes(bool forceRebuild, const MSLane* startLane) {
5899 : #ifdef DEBUG_BESTLANES
5900 : if (DEBUG_COND) {
5901 : std::cout << SIMTIME << " updateBestLanes veh=" << getID() << " force=" << forceRebuild << " startLane1=" << Named::getIDSecure(startLane) << " myLane=" << Named::getIDSecure(myLane) << "\n";
5902 : }
5903 : #endif
5904 1003923781 : if (startLane == nullptr) {
5905 958250605 : startLane = myLane;
5906 : }
5907 : assert(startLane != 0);
5908 1003923781 : if (myLaneChangeModel->isOpposite()) {
5909 : // depending on the calling context, startLane might be the forward lane
5910 : // or the reverse-direction lane. In the latter case we need to
5911 : // transform it to the forward lane.
5912 520933 : if (isOppositeLane(startLane)) {
5913 : // use leftmost lane of forward edge
5914 110870 : startLane = startLane->getEdge().getOppositeEdge()->getLanes().back();
5915 : assert(startLane != 0);
5916 : #ifdef DEBUG_BESTLANES
5917 : if (DEBUG_COND) {
5918 : std::cout << " startLaneIsOpposite newStartLane=" << startLane->getID() << "\n";
5919 : }
5920 : #endif
5921 : }
5922 : }
5923 1003923781 : if (forceRebuild) {
5924 1892544 : myLastBestLanesEdge = nullptr;
5925 1892544 : myLastBestLanesInternalLane = nullptr;
5926 : }
5927 1003923781 : if (myBestLanes.size() > 0 && !forceRebuild && myLastBestLanesEdge == &startLane->getEdge()) {
5928 976429202 : updateOccupancyAndCurrentBestLane(startLane);
5929 : #ifdef DEBUG_BESTLANES
5930 : if (DEBUG_COND) {
5931 : std::cout << " only updateOccupancyAndCurrentBestLane\n";
5932 : }
5933 : #endif
5934 976429202 : return;
5935 : }
5936 27494579 : if (startLane->getEdge().isInternal()) {
5937 12442717 : if (myBestLanes.size() == 0 || forceRebuild) {
5938 : // rebuilt from previous non-internal lane (may backtrack twice if behind an internal junction)
5939 2299 : updateBestLanes(true, startLane->getLogicalPredecessorLane());
5940 : }
5941 12442717 : if (myLastBestLanesInternalLane == startLane && !forceRebuild) {
5942 : #ifdef DEBUG_BESTLANES
5943 : if (DEBUG_COND) {
5944 : std::cout << " nothing to do on internal\n";
5945 : }
5946 : #endif
5947 : return;
5948 : }
5949 : // adapt best lanes to fit the current internal edge:
5950 : // keep the entries that are reachable from this edge
5951 4445184 : const MSEdge* nextEdge = startLane->getNextNormal();
5952 : assert(!nextEdge->isInternal());
5953 8759695 : for (std::vector<std::vector<LaneQ> >::iterator it = myBestLanes.begin(); it != myBestLanes.end();) {
5954 : std::vector<LaneQ>& lanes = *it;
5955 : assert(lanes.size() > 0);
5956 8759695 : if (&(lanes[0].lane->getEdge()) == nextEdge) {
5957 : // keep those lanes which are successors of internal lanes from the edge of startLane
5958 4445184 : std::vector<LaneQ> oldLanes = lanes;
5959 : lanes.clear();
5960 : const std::vector<MSLane*>& sourceLanes = startLane->getEdge().getLanes();
5961 10190164 : for (std::vector<MSLane*>::const_iterator it_source = sourceLanes.begin(); it_source != sourceLanes.end(); ++it_source) {
5962 9635344 : for (std::vector<LaneQ>::iterator it_lane = oldLanes.begin(); it_lane != oldLanes.end(); ++it_lane) {
5963 9635344 : if ((*it_source)->getLinkCont()[0]->getLane() == (*it_lane).lane) {
5964 5744980 : lanes.push_back(*it_lane);
5965 : break;
5966 : }
5967 : }
5968 : }
5969 : assert(lanes.size() == startLane->getEdge().getLanes().size());
5970 : // patch invalid bestLaneOffset and updated myCurrentLaneInBestLanes
5971 10190164 : for (int i = 0; i < (int)lanes.size(); ++i) {
5972 5744980 : if (i + lanes[i].bestLaneOffset < 0) {
5973 106093 : lanes[i].bestLaneOffset = -i;
5974 : }
5975 5744980 : if (i + lanes[i].bestLaneOffset >= (int)lanes.size()) {
5976 23501 : lanes[i].bestLaneOffset = (int)lanes.size() - i - 1;
5977 : }
5978 : assert(i + lanes[i].bestLaneOffset >= 0);
5979 : assert(i + lanes[i].bestLaneOffset < (int)lanes.size());
5980 5744980 : if (lanes[i].bestContinuations[0] != 0) {
5981 : // patch length of bestContinuation to match expectations (only once)
5982 5566456 : lanes[i].bestContinuations.insert(lanes[i].bestContinuations.begin(), (MSLane*)nullptr);
5983 : }
5984 5744980 : if (startLane->getLinkCont()[0]->getLane() == lanes[i].lane) {
5985 4487782 : myCurrentLaneInBestLanes = lanes.begin() + i;
5986 : }
5987 : assert(&(lanes[i].lane->getEdge()) == nextEdge);
5988 : }
5989 4445184 : myLastBestLanesInternalLane = startLane;
5990 4445184 : updateOccupancyAndCurrentBestLane(startLane);
5991 : #ifdef DEBUG_BESTLANES
5992 : if (DEBUG_COND) {
5993 : std::cout << " updated for internal\n";
5994 : }
5995 : #endif
5996 : return;
5997 4445184 : } else {
5998 : // remove passed edges
5999 4314511 : it = myBestLanes.erase(it);
6000 : }
6001 : }
6002 : assert(false); // should always find the next edge
6003 : }
6004 : // start rebuilding
6005 15051862 : myLastBestLanesInternalLane = nullptr;
6006 15051862 : myLastBestLanesEdge = &startLane->getEdge();
6007 : myBestLanes.clear();
6008 :
6009 : // get information about the next stop
6010 15051862 : MSRouteIterator nextStopEdge = myRoute->end();
6011 : const MSLane* nextStopLane = nullptr;
6012 : double nextStopPos = 0;
6013 : bool nextStopIsWaypoint = false;
6014 15051862 : if (!myStops.empty()) {
6015 : const MSStop& nextStop = myStops.front();
6016 238700 : nextStopLane = nextStop.lane;
6017 238700 : if (nextStop.isOpposite) {
6018 : // target leftmost lane in forward direction
6019 340 : nextStopLane = nextStopLane->getEdge().getOppositeEdge()->getLanes().back();
6020 : }
6021 238700 : nextStopEdge = nextStop.edge;
6022 238700 : nextStopPos = nextStop.pars.startPos;
6023 238700 : nextStopIsWaypoint = nextStop.getSpeed() > 0;
6024 : }
6025 : // myArrivalTime = -1 in the context of validating departSpeed with departLane=best
6026 15051862 : if (myParameter->arrivalLaneProcedure >= ArrivalLaneDefinition::GIVEN && nextStopEdge == myRoute->end() && myArrivalLane >= 0) {
6027 271294 : nextStopEdge = (myRoute->end() - 1);
6028 271294 : nextStopLane = (*nextStopEdge)->getLanes()[myArrivalLane];
6029 271294 : nextStopPos = myArrivalPos;
6030 : }
6031 15051862 : if (nextStopEdge != myRoute->end()) {
6032 : // make sure that the "wrong" lanes get a penalty. (penalty needs to be
6033 : // large enough to overcome a magic threshold in MSLaneChangeModel::DK2004.cpp:383)
6034 509994 : nextStopPos = MAX2(POSITION_EPS, MIN2((double)nextStopPos, (double)(nextStopLane->getLength() - 2 * POSITION_EPS)));
6035 509994 : if (nextStopLane->isInternal()) {
6036 : // switch to the correct lane before entering the intersection
6037 167 : nextStopPos = (*nextStopEdge)->getLength();
6038 : }
6039 : }
6040 :
6041 : // go forward along the next lanes;
6042 : // trains do not have to deal with lane-changing for stops but their best
6043 : // lanes lookahead is needed for rail signal control
6044 15051862 : const bool continueAfterStop = nextStopIsWaypoint || isRailway(getVClass());
6045 : int seen = 0;
6046 : double seenLength = 0;
6047 : bool progress = true;
6048 : // bestLanes must cover the braking distance even when at the very end of the current lane to avoid unecessary slow down
6049 30103724 : const double maxBrakeDist = startLane->getLength() + getCarFollowModel().getHeadwayTime() * getMaxSpeed() + getCarFollowModel().brakeGap(getMaxSpeed()) + getVehicleType().getMinGap();
6050 15051862 : const double lookahead = getLaneChangeModel().getStrategicLookahead();
6051 77436701 : for (MSRouteIterator ce = myCurrEdge; progress;) {
6052 : std::vector<LaneQ> currentLanes;
6053 : const std::vector<MSLane*>* allowed = nullptr;
6054 : const MSEdge* nextEdge = nullptr;
6055 62384839 : if (ce != myRoute->end() && ce + 1 != myRoute->end()) {
6056 51487287 : nextEdge = *(ce + 1);
6057 51487287 : allowed = (*ce)->allowedLanes(*nextEdge, myType->getVehicleClass());
6058 : }
6059 62384839 : const std::vector<MSLane*>& lanes = (*ce)->getLanes();
6060 157304304 : for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
6061 : LaneQ q;
6062 94919465 : MSLane* cl = *i;
6063 94919465 : q.lane = cl;
6064 94919465 : q.bestContinuations.push_back(cl);
6065 94919465 : q.bestLaneOffset = 0;
6066 94919465 : q.length = cl->allowsVehicleClass(myType->getVehicleClass()) ? (*ce)->getLength() : 0;
6067 94919465 : q.currentLength = q.length;
6068 : // if all lanes are forbidden (i.e. due to a dynamic closing) we want to express no preference
6069 94919465 : q.allowsContinuation = allowed == nullptr || std::find(allowed->begin(), allowed->end(), cl) != allowed->end();
6070 94919465 : q.occupation = 0;
6071 94919465 : q.nextOccupation = 0;
6072 94919465 : currentLanes.push_back(q);
6073 : }
6074 : //
6075 : if (nextStopEdge == ce
6076 : // already past the stop edge
6077 62384839 : && !(ce == myCurrEdge && myLane != nullptr && myLane->isInternal())) {
6078 502846 : if (!nextStopLane->isInternal() && !continueAfterStop) {
6079 : progress = false;
6080 : }
6081 502846 : const MSLane* normalStopLane = nextStopLane->getNormalPredecessorLane();
6082 1622275 : for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
6083 1119429 : if (nextStopLane != nullptr && normalStopLane != (*q).lane) {
6084 616583 : (*q).allowsContinuation = false;
6085 616583 : (*q).length = nextStopPos;
6086 616583 : (*q).currentLength = (*q).length;
6087 : }
6088 : }
6089 : }
6090 :
6091 62384839 : myBestLanes.push_back(currentLanes);
6092 62384839 : ++seen;
6093 62384839 : seenLength += currentLanes[0].lane->getLength();
6094 : ++ce;
6095 62384839 : if (lookahead >= 0) {
6096 45 : progress &= (seen <= 2 || seenLength < lookahead); // custom (but we need to look at least one edge ahead)
6097 : } else {
6098 83189632 : progress &= (seen <= 4 || seenLength < MAX2(maxBrakeDist, 3000.0)); // motorway
6099 67147498 : progress &= (seen <= 8 || seenLength < MAX2(maxBrakeDist, 200.0) || isRailway(getVClass())); // urban
6100 : }
6101 62384839 : progress &= ce != myRoute->end();
6102 : /*
6103 : if(progress) {
6104 : progress &= (currentLanes.size()!=1||(*ce)->getLanes().size()!=1);
6105 : }
6106 : */
6107 62384839 : }
6108 :
6109 : // we are examining the last lane explicitly
6110 15051862 : if (myBestLanes.size() != 0) {
6111 : double bestLength = -1;
6112 : // minimum and maximum lane index with best length
6113 : int bestThisIndex = 0;
6114 : int bestThisMaxIndex = 0;
6115 : int index = 0;
6116 : std::vector<LaneQ>& last = myBestLanes.back();
6117 39098149 : for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
6118 24046287 : if ((*j).length > bestLength) {
6119 : bestLength = (*j).length;
6120 : bestThisIndex = index;
6121 : bestThisMaxIndex = index;
6122 5944694 : } else if ((*j).length == bestLength) {
6123 : bestThisMaxIndex = index;
6124 : }
6125 : }
6126 : index = 0;
6127 : bool requiredChangeRightForbidden = false;
6128 : int requireChangeToLeftForbidden = -1;
6129 39098149 : for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
6130 24046287 : if ((*j).length < bestLength) {
6131 3464602 : if (abs(bestThisIndex - index) < abs(bestThisMaxIndex - index)) {
6132 123814 : (*j).bestLaneOffset = bestThisIndex - index;
6133 : } else {
6134 3340788 : (*j).bestLaneOffset = bestThisMaxIndex - index;
6135 : }
6136 3464602 : if ((*j).bestLaneOffset < 0 && (!(*j).lane->allowsChangingRight(getVClass())
6137 299601 : || !(*j).lane->getParallelLane(-1, false)->allowsVehicleClass(getVClass())
6138 295022 : || requiredChangeRightForbidden)) {
6139 : // this lane and all further lanes to the left cannot be used
6140 : requiredChangeRightForbidden = true;
6141 4589 : (*j).length = 0;
6142 3460013 : } else if ((*j).bestLaneOffset > 0 && (!(*j).lane->allowsChangingLeft(getVClass())
6143 3164960 : || !(*j).lane->getParallelLane(1, false)->allowsVehicleClass(getVClass()))) {
6144 : // this lane and all previous lanes to the right cannot be used
6145 32085 : requireChangeToLeftForbidden = (*j).lane->getIndex();
6146 : }
6147 : }
6148 : }
6149 15084002 : for (int i = requireChangeToLeftForbidden; i >= 0; i--) {
6150 32140 : if (last[i].bestLaneOffset > 0) {
6151 32100 : last[i].length = 0;
6152 : }
6153 : }
6154 : #ifdef DEBUG_BESTLANES
6155 : if (DEBUG_COND) {
6156 : std::cout << " last edge=" << last.front().lane->getEdge().getID() << " (bestIndex=" << bestThisIndex << " bestMaxIndex=" << bestThisMaxIndex << "):\n";
6157 : std::vector<LaneQ>& laneQs = myBestLanes.back();
6158 : for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
6159 : std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << "\n";
6160 : }
6161 : }
6162 : #endif
6163 : }
6164 : // go backward through the lanes
6165 : // track back best lane and compute the best prior lane(s)
6166 62384839 : for (std::vector<std::vector<LaneQ> >::reverse_iterator i = myBestLanes.rbegin() + 1; i != myBestLanes.rend(); ++i) {
6167 : std::vector<LaneQ>& nextLanes = (*(i - 1));
6168 : std::vector<LaneQ>& clanes = (*i);
6169 47332977 : MSEdge* const cE = &clanes[0].lane->getEdge();
6170 : int index = 0;
6171 : double bestConnectedLength = -1;
6172 : double bestLength = -1;
6173 117608236 : for (const LaneQ& j : nextLanes) {
6174 140550518 : if (j.lane->isApproachedFrom(cE) && bestConnectedLength < j.length) {
6175 : bestConnectedLength = j.length;
6176 : }
6177 70275259 : if (bestLength < j.length) {
6178 : bestLength = j.length;
6179 : }
6180 : }
6181 : // compute index of the best lane (highest length and least offset from the best next lane)
6182 : int bestThisIndex = 0;
6183 : int bestThisMaxIndex = 0;
6184 47332977 : if (bestConnectedLength > 0) {
6185 : index = 0;
6186 118185841 : for (LaneQ& j : clanes) {
6187 : const LaneQ* bestConnectedNext = nullptr;
6188 70860452 : if (j.allowsContinuation) {
6189 169837357 : for (const LaneQ& m : nextLanes) {
6190 117262365 : if ((m.lane->allowsVehicleClass(getVClass()) || m.lane->hadPermissionChanges())
6191 108082915 : && m.lane->isApproachedFrom(cE, j.lane)) {
6192 63556401 : if (betterContinuation(bestConnectedNext, m)) {
6193 : bestConnectedNext = &m;
6194 : }
6195 : }
6196 : }
6197 61805226 : if (bestConnectedNext != nullptr) {
6198 61805222 : if (bestConnectedNext->length == bestConnectedLength && abs(bestConnectedNext->bestLaneOffset) < 2) {
6199 60204719 : j.length += bestLength;
6200 : } else {
6201 1600503 : j.length += bestConnectedNext->length;
6202 : }
6203 61805222 : j.bestLaneOffset = bestConnectedNext->bestLaneOffset;
6204 : }
6205 : }
6206 61805222 : if (bestConnectedNext != nullptr && (bestConnectedNext->allowsContinuation || bestConnectedNext->length > 0)) {
6207 61779961 : copy(bestConnectedNext->bestContinuations.begin(), bestConnectedNext->bestContinuations.end(), back_inserter(j.bestContinuations));
6208 : } else {
6209 9080491 : j.allowsContinuation = false;
6210 : }
6211 70860452 : if (clanes[bestThisIndex].length < j.length
6212 64279330 : || (clanes[bestThisIndex].length == j.length && abs(clanes[bestThisIndex].bestLaneOffset) > abs(j.bestLaneOffset))
6213 195466088 : || (clanes[bestThisIndex].length == j.length && abs(clanes[bestThisIndex].bestLaneOffset) == abs(j.bestLaneOffset) &&
6214 60456138 : nextLinkPriority(clanes[bestThisIndex].bestContinuations) < nextLinkPriority(j.bestContinuations))
6215 : ) {
6216 : bestThisIndex = index;
6217 : bestThisMaxIndex = index;
6218 64137487 : } else if (clanes[bestThisIndex].length == j.length
6219 60444254 : && abs(clanes[bestThisIndex].bestLaneOffset) == abs(j.bestLaneOffset)
6220 124581614 : && nextLinkPriority(clanes[bestThisIndex].bestContinuations) == nextLinkPriority(j.bestContinuations)) {
6221 : bestThisMaxIndex = index;
6222 : }
6223 70860452 : index++;
6224 : }
6225 :
6226 : //vehicle with elecHybrid device prefers running under an overhead wire
6227 47325389 : if (getDevice(typeid(MSDevice_ElecHybrid)) != nullptr) {
6228 : index = 0;
6229 221 : for (const LaneQ& j : clanes) {
6230 159 : std::string overheadWireSegmentID = MSNet::getInstance()->getStoppingPlaceID(j.lane, j.currentLength / 2., SUMO_TAG_OVERHEAD_WIRE_SEGMENT);
6231 159 : if (overheadWireSegmentID != "") {
6232 : bestThisIndex = index;
6233 : bestThisMaxIndex = index;
6234 : }
6235 159 : index++;
6236 : }
6237 : }
6238 :
6239 : } else {
6240 : // only needed in case of disconnected routes
6241 : int bestNextIndex = 0;
6242 7588 : int bestDistToNeeded = (int) clanes.size();
6243 : index = 0;
6244 20314 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6245 12726 : if ((*j).allowsContinuation) {
6246 : int nextIndex = 0;
6247 28086 : for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
6248 15360 : if ((*m).lane->isApproachedFrom(cE, (*j).lane)) {
6249 5675 : if (bestDistToNeeded > abs((*m).bestLaneOffset)) {
6250 : bestDistToNeeded = abs((*m).bestLaneOffset);
6251 : bestThisIndex = index;
6252 : bestThisMaxIndex = index;
6253 : bestNextIndex = nextIndex;
6254 : }
6255 : }
6256 : }
6257 : }
6258 : }
6259 7588 : clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
6260 7588 : copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
6261 :
6262 : }
6263 : // set bestLaneOffset for all lanes
6264 : index = 0;
6265 : bool requiredChangeRightForbidden = false;
6266 : int requireChangeToLeftForbidden = -1;
6267 118206155 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6268 70873178 : if ((*j).length < clanes[bestThisIndex].length
6269 60219397 : || ((*j).length == clanes[bestThisIndex].length && abs((*j).bestLaneOffset) > abs(clanes[bestThisIndex].bestLaneOffset))
6270 131092371 : || (nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)
6271 : ) {
6272 10829320 : if (abs(bestThisIndex - index) < abs(bestThisMaxIndex - index)) {
6273 737692 : (*j).bestLaneOffset = bestThisIndex - index;
6274 : } else {
6275 10091628 : (*j).bestLaneOffset = bestThisMaxIndex - index;
6276 : }
6277 10829320 : if ((nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)) {
6278 : // try to move away from the lower-priority lane before it ends
6279 9275826 : (*j).length = (*j).currentLength;
6280 : }
6281 10829320 : if ((*j).bestLaneOffset < 0 && (!(*j).lane->allowsChangingRight(getVClass())
6282 3383592 : || !(*j).lane->getParallelLane(-1, false)->allowsVehicleClass(getVClass())
6283 3368067 : || requiredChangeRightForbidden)) {
6284 : // this lane and all further lanes to the left cannot be used
6285 : requiredChangeRightForbidden = true;
6286 32861 : if ((*j).length == (*j).currentLength) {
6287 29837 : (*j).length = 0;
6288 : }
6289 10796459 : } else if ((*j).bestLaneOffset > 0 && (!(*j).lane->allowsChangingLeft(getVClass())
6290 7388821 : || !(*j).lane->getParallelLane(1, false)->allowsVehicleClass(getVClass()))) {
6291 : // this lane and all previous lanes to the right cannot be used
6292 95446 : requireChangeToLeftForbidden = (*j).lane->getIndex();
6293 : }
6294 : } else {
6295 60043858 : (*j).bestLaneOffset = 0;
6296 : }
6297 : }
6298 47437635 : for (int idx = requireChangeToLeftForbidden; idx >= 0; idx--) {
6299 104658 : if (clanes[idx].length == clanes[idx].currentLength) {
6300 98984 : clanes[idx].length = 0;
6301 : };
6302 : }
6303 :
6304 : //vehicle with elecHybrid device prefers running under an overhead wire
6305 47332977 : if (static_cast<MSDevice_ElecHybrid*>(getDevice(typeid(MSDevice_ElecHybrid))) != 0) {
6306 : index = 0;
6307 62 : std::string overheadWireID = MSNet::getInstance()->getStoppingPlaceID(clanes[bestThisIndex].lane, (clanes[bestThisIndex].currentLength) / 2, SUMO_TAG_OVERHEAD_WIRE_SEGMENT);
6308 62 : if (overheadWireID != "") {
6309 211 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6310 153 : (*j).bestLaneOffset = bestThisIndex - index;
6311 : }
6312 : }
6313 : }
6314 :
6315 : #ifdef DEBUG_BESTLANES
6316 : if (DEBUG_COND) {
6317 : std::cout << " edge=" << cE->getID() << " (bestIndex=" << bestThisIndex << " bestMaxIndex=" << bestThisMaxIndex << "):\n";
6318 : std::vector<LaneQ>& laneQs = clanes;
6319 : for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
6320 : std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << " allowCont=" << (*j).allowsContinuation << "\n";
6321 : }
6322 : }
6323 : #endif
6324 :
6325 : }
6326 15051862 : updateOccupancyAndCurrentBestLane(startLane);
6327 : #ifdef DEBUG_BESTLANES
6328 : if (DEBUG_COND) {
6329 : std::cout << SIMTIME << " veh=" << getID() << " bestCont=" << toString(getBestLanesContinuation()) << "\n";
6330 : }
6331 : #endif
6332 : }
6333 :
6334 : void
6335 228 : MSVehicle::updateLaneBruttoSum() {
6336 228 : if (myLane != nullptr) {
6337 228 : myLane->markRecalculateBruttoSum();
6338 : }
6339 228 : }
6340 :
6341 : bool
6342 63556401 : MSVehicle::betterContinuation(const LaneQ* bestConnectedNext, const LaneQ& m) const {
6343 63556401 : if (bestConnectedNext == nullptr) {
6344 : return true;
6345 1751179 : } else if (m.lane->getBidiLane() != nullptr && bestConnectedNext->lane->getBidiLane() == nullptr) {
6346 : return false;
6347 1750441 : } else if (bestConnectedNext->lane->getBidiLane() != nullptr && m.lane->getBidiLane() == nullptr) {
6348 : return true;
6349 1750441 : } else if (bestConnectedNext->length < m.length) {
6350 : return true;
6351 1499637 : } else if (bestConnectedNext->length == m.length) {
6352 1126875 : if (abs(bestConnectedNext->bestLaneOffset) > abs(m.bestLaneOffset)) {
6353 : return true;
6354 : }
6355 1029074 : const double contRight = getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_CONTRIGHT, 1);
6356 : if (contRight < 1
6357 : // if we don't check for adjacency, the rightmost line will get
6358 : // multiple chances to be better which leads to an uninituitve distribution
6359 1006 : && (m.lane->getIndex() - bestConnectedNext->lane->getIndex()) == 1
6360 1029853 : && RandHelper::rand(getRNG()) > contRight) {
6361 : return true;
6362 : }
6363 : }
6364 : return false;
6365 : }
6366 :
6367 :
6368 : int
6369 383897556 : MSVehicle::nextLinkPriority(const std::vector<MSLane*>& conts) {
6370 383897556 : if (conts.size() < 2) {
6371 : return -1;
6372 : } else {
6373 351617093 : const MSLink* const link = conts[0]->getLinkTo(conts[1]);
6374 351617093 : if (link != nullptr) {
6375 351611945 : return link->havePriority() ? 1 : 0;
6376 : } else {
6377 : // disconnected route
6378 : return -1;
6379 : }
6380 : }
6381 : }
6382 :
6383 :
6384 : void
6385 995926248 : MSVehicle::updateOccupancyAndCurrentBestLane(const MSLane* startLane) {
6386 : std::vector<LaneQ>& currLanes = *myBestLanes.begin();
6387 : std::vector<LaneQ>::iterator i;
6388 2774134768 : for (i = currLanes.begin(); i != currLanes.end(); ++i) {
6389 : double nextOccupation = 0;
6390 4190468964 : for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
6391 2412260444 : nextOccupation += (*j)->getBruttoVehLenSum();
6392 : }
6393 1778208520 : (*i).nextOccupation = nextOccupation;
6394 : #ifdef DEBUG_BESTLANES
6395 : if (DEBUG_COND) {
6396 : std::cout << " lane=" << (*i).lane->getID() << " nextOccupation=" << nextOccupation << "\n";
6397 : }
6398 : #endif
6399 1778208520 : if ((*i).lane == startLane) {
6400 991481035 : myCurrentLaneInBestLanes = i;
6401 : }
6402 : }
6403 995926248 : }
6404 :
6405 :
6406 : const std::vector<MSLane*>&
6407 1595487804 : MSVehicle::getBestLanesContinuation() const {
6408 1595487804 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6409 : return myEmptyLaneVector;
6410 : }
6411 1595487804 : return (*myCurrentLaneInBestLanes).bestContinuations;
6412 : }
6413 :
6414 :
6415 : const std::vector<MSLane*>&
6416 63948727 : MSVehicle::getBestLanesContinuation(const MSLane* const l) const {
6417 : const MSLane* lane = l;
6418 : // XXX: shouldn't this be a "while" to cover more than one internal lane? (Leo) Refs. #2575
6419 63948727 : if (lane->getEdge().isInternal()) {
6420 : // internal edges are not kept inside the bestLanes structure
6421 4583393 : lane = lane->getLinkCont()[0]->getLane();
6422 : }
6423 63948727 : if (myBestLanes.size() == 0) {
6424 : return myEmptyLaneVector;
6425 : }
6426 104995914 : for (std::vector<LaneQ>::const_iterator i = myBestLanes[0].begin(); i != myBestLanes[0].end(); ++i) {
6427 104983744 : if ((*i).lane == lane) {
6428 63936557 : return (*i).bestContinuations;
6429 : }
6430 : }
6431 : return myEmptyLaneVector;
6432 : }
6433 :
6434 : const std::vector<const MSLane*>
6435 279233 : MSVehicle::getUpcomingLanesUntil(double distance) const {
6436 : std::vector<const MSLane*> lanes;
6437 :
6438 279233 : if (distance <= 0. || hasArrived()) {
6439 : // WRITE_WARNINGF(TL("MSVehicle::getUpcomingLanesUntil(): distance ('%') should be greater than 0."), distance);
6440 : return lanes;
6441 : }
6442 :
6443 279025 : if (!myLaneChangeModel->isOpposite()) {
6444 275699 : distance += getPositionOnLane();
6445 : } else {
6446 3326 : distance += myLane->getOppositePos(getPositionOnLane());
6447 : }
6448 279025 : MSLane* lane = myLaneChangeModel->isOpposite() ? myLane->getParallelOpposite() : myLane;
6449 283848 : while (lane->isInternal() && (distance > 0.)) { // include initial internal lanes
6450 4823 : lanes.insert(lanes.end(), lane);
6451 4823 : distance -= lane->getLength();
6452 8265 : lane = lane->getLinkCont().front()->getViaLaneOrLane();
6453 : }
6454 :
6455 279025 : const std::vector<MSLane*>& contLanes = getBestLanesContinuation();
6456 279025 : if (contLanes.empty()) {
6457 : return lanes;
6458 : }
6459 : auto contLanesIt = contLanes.begin();
6460 279025 : MSRouteIterator routeIt = myCurrEdge; // keep track of covered edges in myRoute
6461 591768 : while (distance > 0.) {
6462 320565 : MSLane* l = nullptr;
6463 320565 : if (contLanesIt != contLanes.end()) {
6464 307400 : l = *contLanesIt;
6465 : if (l != nullptr) {
6466 : assert(l->getEdge().getID() == (*routeIt)->getLanes().front()->getEdge().getID());
6467 : }
6468 : ++contLanesIt;
6469 307400 : if (l != nullptr || myLane->isInternal()) {
6470 : ++routeIt;
6471 : }
6472 307400 : if (l == nullptr) {
6473 3438 : continue;
6474 : }
6475 13165 : } else if (routeIt != myRoute->end()) { // bestLanes didn't get us far enough
6476 : // choose left-most lane as default (avoid sidewalks, bike lanes etc)
6477 6232 : l = (*routeIt)->getLanes().back();
6478 : ++routeIt;
6479 : } else { // the search distance goes beyond our route
6480 : break;
6481 : }
6482 :
6483 : assert(l != nullptr);
6484 :
6485 : // insert internal lanes if applicable
6486 310194 : const MSLane* internalLane = lanes.size() > 0 ? lanes.back()->getInternalFollowingLane(l) : nullptr;
6487 350407 : while ((internalLane != nullptr) && internalLane->isInternal() && (distance > 0.)) {
6488 40213 : lanes.insert(lanes.end(), internalLane);
6489 40213 : distance -= internalLane->getLength();
6490 64654 : internalLane = internalLane->getLinkCont().front()->getViaLaneOrLane();
6491 : }
6492 310194 : if (distance <= 0.) {
6493 : break;
6494 : }
6495 :
6496 309305 : lanes.insert(lanes.end(), l);
6497 309305 : distance -= l->getLength();
6498 : }
6499 :
6500 : return lanes;
6501 0 : }
6502 :
6503 : const std::vector<const MSLane*>
6504 6665 : MSVehicle::getPastLanesUntil(double distance) const {
6505 : std::vector<const MSLane*> lanes;
6506 :
6507 6665 : if (distance <= 0.) {
6508 : // WRITE_WARNINGF(TL("MSVehicle::getPastLanesUntil(): distance ('%') should be greater than 0."), distance);
6509 : return lanes;
6510 : }
6511 :
6512 6557 : MSRouteIterator routeIt = myCurrEdge;
6513 6557 : if (!myLaneChangeModel->isOpposite()) {
6514 6533 : distance += myLane->getLength() - getPositionOnLane();
6515 : } else {
6516 24 : distance += myLane->getParallelOpposite()->getLength() - myLane->getOppositePos(getPositionOnLane());
6517 : }
6518 6557 : MSLane* lane = myLaneChangeModel->isOpposite() ? myLane->getParallelOpposite() : myLane;
6519 6578 : while (lane->isInternal() && (distance > 0.)) { // include initial internal lanes
6520 21 : lanes.insert(lanes.end(), lane);
6521 21 : distance -= lane->getLength();
6522 21 : lane = lane->getLogicalPredecessorLane();
6523 : }
6524 :
6525 10051 : while (distance > 0.) {
6526 : // choose left-most lane as default (avoid sidewalks, bike lanes etc)
6527 8741 : MSLane* l = (*routeIt)->getLanes().back();
6528 :
6529 : // insert internal lanes if applicable
6530 8741 : const MSEdge* internalEdge = lanes.size() > 0 ? (*routeIt)->getInternalFollowingEdge(&(lanes.back()->getEdge()), getVClass()) : nullptr;
6531 8762 : const MSLane* internalLane = internalEdge != nullptr ? internalEdge->getLanes().front() : nullptr;
6532 : std::vector<const MSLane*> internalLanes;
6533 10939 : while ((internalLane != nullptr) && internalLane->isInternal()) { // collect all internal successor lanes
6534 2198 : internalLanes.insert(internalLanes.begin(), internalLane);
6535 4382 : internalLane = internalLane->getLinkCont().front()->getViaLaneOrLane();
6536 : }
6537 10939 : for (auto it = internalLanes.begin(); (it != internalLanes.end()) && (distance > 0.); ++it) { // check remaining distance in correct order
6538 2198 : lanes.insert(lanes.end(), *it);
6539 2198 : distance -= (*it)->getLength();
6540 : }
6541 8741 : if (distance <= 0.) {
6542 : break;
6543 : }
6544 :
6545 8725 : lanes.insert(lanes.end(), l);
6546 8725 : distance -= l->getLength();
6547 :
6548 : // NOTE: we're going backwards with the (bi-directional) Iterator
6549 : // TODO: consider make reverse_iterator() when moving on to C++14 or later
6550 8725 : if (routeIt != myRoute->begin()) {
6551 : --routeIt;
6552 : } else { // we went backwards to begin() and already processed the first and final element
6553 : break;
6554 : }
6555 8741 : }
6556 :
6557 : return lanes;
6558 0 : }
6559 :
6560 :
6561 : const std::vector<MSLane*>
6562 6329 : MSVehicle::getUpstreamOppositeLanes() const {
6563 6329 : const std::vector<const MSLane*> routeLanes = getPastLanesUntil(myLane->getMaximumBrakeDist());
6564 : std::vector<MSLane*> result;
6565 15341 : for (const MSLane* lane : routeLanes) {
6566 9783 : MSLane* opposite = lane->getOpposite();
6567 9783 : if (opposite != nullptr) {
6568 9012 : result.push_back(opposite);
6569 : } else {
6570 : break;
6571 : }
6572 : }
6573 6329 : return result;
6574 6329 : }
6575 :
6576 :
6577 : int
6578 295904622 : MSVehicle::getBestLaneOffset() const {
6579 295904622 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6580 : return 0;
6581 : } else {
6582 295349950 : return (*myCurrentLaneInBestLanes).bestLaneOffset;
6583 : }
6584 : }
6585 :
6586 : double
6587 19955 : MSVehicle::getBestLaneDist() const {
6588 19955 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6589 : return -1;
6590 : } else {
6591 19955 : return (*myCurrentLaneInBestLanes).length;
6592 : }
6593 : }
6594 :
6595 :
6596 :
6597 : void
6598 625398919 : MSVehicle::adaptBestLanesOccupation(int laneIndex, double density) {
6599 : std::vector<MSVehicle::LaneQ>& preb = myBestLanes.front();
6600 : assert(laneIndex < (int)preb.size());
6601 625398919 : preb[laneIndex].occupation = density + preb[laneIndex].nextOccupation;
6602 625398919 : }
6603 :
6604 :
6605 : void
6606 62371 : MSVehicle::fixPosition() {
6607 62371 : if (MSGlobals::gLaneChangeDuration > 0 && !myLaneChangeModel->isChangingLanes()) {
6608 33628 : myState.myPosLat = 0;
6609 : }
6610 62371 : }
6611 :
6612 : std::pair<const MSLane*, double>
6613 251 : MSVehicle::getLanePosAfterDist(double distance) const {
6614 251 : if (distance == 0) {
6615 227 : return std::make_pair(myLane, getPositionOnLane());
6616 : }
6617 24 : const std::vector<const MSLane*> lanes = getUpcomingLanesUntil(distance);
6618 24 : distance += getPositionOnLane();
6619 24 : for (const MSLane* lane : lanes) {
6620 24 : if (lane->getLength() > distance) {
6621 : return std::make_pair(lane, distance);
6622 : }
6623 0 : distance -= lane->getLength();
6624 : }
6625 0 : return std::make_pair(nullptr, -1);
6626 24 : }
6627 :
6628 :
6629 : double
6630 16620 : MSVehicle::getDistanceToPosition(double destPos, const MSLane* destLane) const {
6631 16620 : if (isOnRoad() && destLane != nullptr) {
6632 16574 : return myRoute->getDistanceBetween(getPositionOnLane(), destPos, myLane, destLane);
6633 : }
6634 : return std::numeric_limits<double>::max();
6635 : }
6636 :
6637 :
6638 : std::pair<const MSVehicle* const, double>
6639 76406405 : MSVehicle::getLeader(double dist, bool considerCrossingFoes) const {
6640 76406405 : if (myLane == nullptr) {
6641 0 : return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
6642 : }
6643 76406405 : if (dist == 0) {
6644 2474 : dist = getCarFollowModel().brakeGap(getSpeed()) + getVehicleType().getMinGap();
6645 : }
6646 : const MSVehicle* lead = nullptr;
6647 76406405 : const MSLane* lane = myLane; // ensure lane does not change between getVehiclesSecure and releaseVehicles;
6648 76406405 : const MSLane::VehCont& vehs = lane->getVehiclesSecure();
6649 : // vehicle might be outside the road network
6650 76406405 : MSLane::VehCont::const_iterator it = std::find(vehs.begin(), vehs.end(), this);
6651 76406405 : if (it != vehs.end() && it + 1 != vehs.end()) {
6652 72694746 : lead = *(it + 1);
6653 : }
6654 72694746 : if (lead != nullptr) {
6655 : std::pair<const MSVehicle* const, double> result(
6656 72694746 : lead, lead->getBackPositionOnLane(myLane) - getPositionOnLane() - getVehicleType().getMinGap());
6657 72694746 : lane->releaseVehicles();
6658 72694746 : return result;
6659 : }
6660 3711659 : const double seen = myLane->getLength() - getPositionOnLane();
6661 3711659 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(myLane);
6662 3711659 : std::pair<const MSVehicle* const, double> result = myLane->getLeaderOnConsecutive(dist, seen, getSpeed(), *this, bestLaneConts, considerCrossingFoes);
6663 3711656 : lane->releaseVehicles();
6664 3711656 : return result;
6665 : }
6666 :
6667 :
6668 : std::pair<const MSVehicle* const, double>
6669 2419144 : MSVehicle::getFollower(double dist) const {
6670 2419144 : if (myLane == nullptr) {
6671 0 : return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
6672 : }
6673 2419144 : if (dist == 0) {
6674 748173 : dist = getCarFollowModel().brakeGap(myLane->getEdge().getSpeedLimit() * 2, 4.5, 0);
6675 : }
6676 2419144 : return myLane->getFollower(this, getPositionOnLane(), dist, MSLane::MinorLinkMode::FOLLOW_NEVER);
6677 : }
6678 :
6679 :
6680 : double
6681 0 : MSVehicle::getTimeGapOnLane() const {
6682 : // calling getLeader with 0 would induce a dist calculation but we only want to look for the leaders on the current lane
6683 0 : std::pair<const MSVehicle* const, double> leaderInfo = getLeader(-1);
6684 0 : if (leaderInfo.first == nullptr || getSpeed() == 0) {
6685 0 : return -1;
6686 : }
6687 0 : return (leaderInfo.second + getVehicleType().getMinGap()) / getSpeed();
6688 : }
6689 :
6690 :
6691 : void
6692 1333508 : MSVehicle::addTransportable(MSTransportable* transportable) {
6693 1333508 : MSBaseVehicle::addTransportable(transportable);
6694 11065 : if (myStops.size() > 0 && myStops.front().reached) {
6695 7882 : if (transportable->isPerson()) {
6696 7303 : if (myStops.front().triggered && myStops.front().numExpectedPerson > 0) {
6697 1508 : myStops.front().numExpectedPerson -= (int)myStops.front().pars.awaitedPersons.count(transportable->getID());
6698 : }
6699 : } else {
6700 579 : if (myStops.front().pars.containerTriggered && myStops.front().numExpectedContainer > 0) {
6701 20 : myStops.front().numExpectedContainer -= (int)myStops.front().pars.awaitedContainers.count(transportable->getID());
6702 : }
6703 : }
6704 : }
6705 11065 : }
6706 :
6707 :
6708 : void
6709 671112891 : MSVehicle::setBlinkerInformation() {
6710 : switchOffSignal(VEH_SIGNAL_BLINKER_RIGHT | VEH_SIGNAL_BLINKER_LEFT);
6711 671112891 : int state = myLaneChangeModel->getOwnState();
6712 : // do not set blinker for sublane changes or when blocked from changing to the right
6713 671112891 : const bool blinkerManoeuvre = (((state & LCA_SUBLANE) == 0) && (
6714 582097492 : (state & LCA_KEEPRIGHT) == 0 || (state & LCA_BLOCKED) == 0));
6715 : Signalling left = VEH_SIGNAL_BLINKER_LEFT;
6716 : Signalling right = VEH_SIGNAL_BLINKER_RIGHT;
6717 671112891 : if (MSGlobals::gLefthand) {
6718 : // lane indices increase from left to right
6719 : std::swap(left, right);
6720 : }
6721 671112891 : if ((state & LCA_LEFT) != 0 && blinkerManoeuvre) {
6722 19315213 : switchOnSignal(left);
6723 651797678 : } else if ((state & LCA_RIGHT) != 0 && blinkerManoeuvre) {
6724 5483656 : switchOnSignal(right);
6725 646314022 : } else if (myLaneChangeModel->isChangingLanes()) {
6726 248224 : if (myLaneChangeModel->getLaneChangeDirection() == 1) {
6727 159156 : switchOnSignal(left);
6728 : } else {
6729 89068 : switchOnSignal(right);
6730 : }
6731 : } else {
6732 646065798 : const MSLane* lane = getLane();
6733 646065798 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, 1, *lane, getBestLanesContinuation());
6734 646065798 : if (link != lane->getLinkCont().end() && lane->getLength() - getPositionOnLane() < lane->getVehicleMaxSpeed(this) * (double) 7.) {
6735 152877386 : switch ((*link)->getDirection()) {
6736 : case LinkDirection::TURN:
6737 : case LinkDirection::LEFT:
6738 : case LinkDirection::PARTLEFT:
6739 : switchOnSignal(VEH_SIGNAL_BLINKER_LEFT);
6740 : break;
6741 : case LinkDirection::RIGHT:
6742 : case LinkDirection::PARTRIGHT:
6743 : switchOnSignal(VEH_SIGNAL_BLINKER_RIGHT);
6744 : break;
6745 : default:
6746 : break;
6747 : }
6748 : }
6749 : }
6750 : // stopping related signals
6751 671112891 : if (hasStops()
6752 671112891 : && (myStops.begin()->reached ||
6753 14179209 : (myStopDist < (myLane->getLength() - getPositionOnLane())
6754 4074751 : && myStopDist < getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this), getCarFollowModel().getMaxDecel(), 3)))) {
6755 16592047 : if (myStops.begin()->lane->getIndex() > 0 && myStops.begin()->lane->getParallelLane(-1)->allowsVehicleClass(getVClass())) {
6756 : // not stopping on the right. Activate emergency blinkers
6757 : switchOnSignal(VEH_SIGNAL_BLINKER_LEFT | VEH_SIGNAL_BLINKER_RIGHT);
6758 16366598 : } else if (!myStops.begin()->reached && (myStops.begin()->pars.parking == ParkingType::OFFROAD)) {
6759 : // signal upcoming parking stop on the current lane when within braking distance (~2 seconds before braking)
6760 1495914 : switchOnSignal(MSGlobals::gLefthand ? VEH_SIGNAL_BLINKER_LEFT : VEH_SIGNAL_BLINKER_RIGHT);
6761 : }
6762 : }
6763 671112891 : if (myInfluencer != nullptr && myInfluencer->getSignals() >= 0) {
6764 15 : mySignals = myInfluencer->getSignals();
6765 : myInfluencer->setSignals(-1); // overwrite computed signals only once
6766 : }
6767 671112891 : }
6768 :
6769 : void
6770 79153 : MSVehicle::setEmergencyBlueLight(SUMOTime currentTime) {
6771 :
6772 : //TODO look if timestep ist SIMSTEP
6773 79153 : if (currentTime % 1000 == 0) {
6774 25429 : if (signalSet(VEH_SIGNAL_EMERGENCY_BLUE)) {
6775 : switchOffSignal(VEH_SIGNAL_EMERGENCY_BLUE);
6776 : } else {
6777 : switchOnSignal(VEH_SIGNAL_EMERGENCY_BLUE);
6778 : }
6779 : }
6780 79153 : }
6781 :
6782 :
6783 : int
6784 21332549 : MSVehicle::getLaneIndex() const {
6785 21332549 : return myLane == nullptr ? -1 : myLane->getIndex();
6786 : }
6787 :
6788 :
6789 : void
6790 13086835 : MSVehicle::setTentativeLaneAndPosition(MSLane* lane, double pos, double posLat) {
6791 13086835 : myLane = lane;
6792 13086835 : myState.myPos = pos;
6793 13086835 : myState.myPosLat = posLat;
6794 13086835 : myState.myBackPos = pos - getVehicleType().getLength();
6795 13086835 : }
6796 :
6797 :
6798 : double
6799 379742052 : MSVehicle::getRightSideOnLane() const {
6800 379742052 : return myState.myPosLat + 0.5 * myLane->getWidth() - 0.5 * getVehicleType().getWidth();
6801 : }
6802 :
6803 :
6804 : double
6805 374439633 : MSVehicle::getLeftSideOnLane() const {
6806 374439633 : return myState.myPosLat + 0.5 * myLane->getWidth() + 0.5 * getVehicleType().getWidth();
6807 : }
6808 :
6809 :
6810 : double
6811 300421462 : MSVehicle::getRightSideOnLane(const MSLane* lane) const {
6812 300421462 : return myState.myPosLat + 0.5 * lane->getWidth() - 0.5 * getVehicleType().getWidth();
6813 : }
6814 :
6815 :
6816 : double
6817 299931374 : MSVehicle::getLeftSideOnLane(const MSLane* lane) const {
6818 299931374 : return myState.myPosLat + 0.5 * lane->getWidth() + 0.5 * getVehicleType().getWidth();
6819 : }
6820 :
6821 :
6822 : double
6823 239283494 : MSVehicle::getRightSideOnEdge(const MSLane* lane) const {
6824 239283494 : return getCenterOnEdge(lane) - 0.5 * getVehicleType().getWidth();
6825 : }
6826 :
6827 :
6828 : double
6829 29481991 : MSVehicle::getLeftSideOnEdge(const MSLane* lane) const {
6830 29481991 : return getCenterOnEdge(lane) + 0.5 * getVehicleType().getWidth();
6831 : }
6832 :
6833 :
6834 : double
6835 703783305 : MSVehicle::getCenterOnEdge(const MSLane* lane) const {
6836 703783305 : if (lane == nullptr || &lane->getEdge() == &myLane->getEdge()) {
6837 703354371 : return myLane->getRightSideOnEdge() + myState.myPosLat + 0.5 * myLane->getWidth();
6838 428934 : } else if (lane == myLaneChangeModel->getShadowLane()) {
6839 13687 : if (myLaneChangeModel->isOpposite() && &lane->getEdge() != &myLane->getEdge()) {
6840 13686 : return lane->getRightSideOnEdge() + lane->getWidth() - myState.myPosLat + 0.5 * myLane->getWidth();
6841 : }
6842 1 : if (myLaneChangeModel->getShadowDirection() == -1) {
6843 0 : return lane->getRightSideOnEdge() + lane->getWidth() + myState.myPosLat + 0.5 * myLane->getWidth();
6844 : } else {
6845 1 : return lane->getRightSideOnEdge() - myLane->getWidth() + myState.myPosLat + 0.5 * myLane->getWidth();
6846 : }
6847 415247 : } else if (lane == myLane->getBidiLane()) {
6848 1320 : return lane->getRightSideOnEdge() - myState.myPosLat + 0.5 * lane->getWidth();
6849 : } else {
6850 : assert(myFurtherLanes.size() == myFurtherLanesPosLat.size());
6851 485040 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6852 462124 : if (myFurtherLanes[i] == lane) {
6853 : #ifdef DEBUG_FURTHER
6854 : if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat=" << myFurtherLanesPosLat[i]
6855 : << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
6856 : << "\n";
6857 : #endif
6858 390955 : return lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
6859 71169 : } else if (myFurtherLanes[i]->getBidiLane() == lane) {
6860 : #ifdef DEBUG_FURTHER
6861 : if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat(bidi)=" << myFurtherLanesPosLat[i]
6862 : << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
6863 : << "\n";
6864 : #endif
6865 56 : return lane->getRightSideOnEdge() - myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
6866 : }
6867 : }
6868 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
6869 22916 : const std::vector<MSLane*>& shadowFurther = myLaneChangeModel->getShadowFurtherLanes();
6870 23160 : for (int i = 0; i < (int)shadowFurther.size(); ++i) {
6871 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
6872 23160 : if (shadowFurther[i] == lane) {
6873 : assert(myLaneChangeModel->getShadowLane() != 0);
6874 22916 : return (lane->getRightSideOnEdge() + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] + 0.5 * lane->getWidth()
6875 22916 : + (myLane->getCenterOnEdge() - myLaneChangeModel->getShadowLane()->getCenterOnEdge()));
6876 : }
6877 : }
6878 : assert(false);
6879 0 : throw ProcessError("Request lateral pos of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
6880 : }
6881 : }
6882 :
6883 :
6884 : double
6885 3307565600 : MSVehicle::getLatOffset(const MSLane* lane) const {
6886 : assert(lane != 0);
6887 3307565600 : if (&lane->getEdge() == &myLane->getEdge()) {
6888 3263083279 : return myLane->getRightSideOnEdge() - lane->getRightSideOnEdge();
6889 44482321 : } else if (myLane->getParallelOpposite() == lane) {
6890 2167932 : return (myLane->getWidth() + lane->getWidth()) * 0.5 - 2 * getLateralPositionOnLane();
6891 42314389 : } else if (myLane->getBidiLane() == lane) {
6892 173671 : return -2 * getLateralPositionOnLane();
6893 : } else {
6894 : // Check whether the lane is a further lane for the vehicle
6895 47702188 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6896 46885285 : if (myFurtherLanes[i] == lane) {
6897 : #ifdef DEBUG_FURTHER
6898 : if (DEBUG_COND) {
6899 : std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherLat=" << myFurtherLanesPosLat[i] << "\n";
6900 : }
6901 : #endif
6902 41296828 : return myFurtherLanesPosLat[i] - myState.myPosLat;
6903 5588457 : } else if (myFurtherLanes[i]->getBidiLane() == lane) {
6904 : #ifdef DEBUG_FURTHER
6905 : if (DEBUG_COND) {
6906 : std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherBidiLat=" << myFurtherLanesPosLat[i] << "\n";
6907 : }
6908 : #endif
6909 26987 : return -2 * (myFurtherLanesPosLat[i] - myState.myPosLat);
6910 : }
6911 : }
6912 : #ifdef DEBUG_FURTHER
6913 : if (DEBUG_COND) {
6914 : std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
6915 : }
6916 : #endif
6917 : // Check whether the lane is a "shadow further lane" for the vehicle
6918 816903 : const std::vector<MSLane*>& shadowFurther = myLaneChangeModel->getShadowFurtherLanes();
6919 827442 : for (int i = 0; i < (int)shadowFurther.size(); ++i) {
6920 825019 : if (shadowFurther[i] == lane) {
6921 : #ifdef DEBUG_FURTHER
6922 : if (DEBUG_COND) std::cout << " getLatOffset veh=" << getID()
6923 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
6924 : << " lane=" << lane->getID()
6925 : << " i=" << i
6926 : << " posLat=" << myState.myPosLat
6927 : << " shadowPosLat=" << getLatOffset(myLaneChangeModel->getShadowLane())
6928 : << " shadowFurtherLat=" << myLaneChangeModel->getShadowFurtherLanesPosLat()[i]
6929 : << "\n";
6930 : #endif
6931 814480 : return getLatOffset(myLaneChangeModel->getShadowLane()) + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] - myState.myPosLat;
6932 : }
6933 : }
6934 : // Check whether the vehicle issued a maneuverReservation on the lane.
6935 : const std::vector<MSLane*>& furtherTargets = myLaneChangeModel->getFurtherTargetLanes();
6936 2873 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6937 : // Further target lanes are just neighboring lanes of the vehicle's further lanes, @see MSAbstractLaneChangeModel::updateTargetLane()
6938 2870 : MSLane* targetLane = furtherTargets[i];
6939 2870 : if (targetLane == lane) {
6940 2420 : const double targetDir = myLaneChangeModel->getManeuverDist() < 0 ? -1. : 1.;
6941 2420 : const double latOffset = myFurtherLanesPosLat[i] - myState.myPosLat + targetDir * 0.5 * (myFurtherLanes[i]->getWidth() + targetLane->getWidth());
6942 : #ifdef DEBUG_TARGET_LANE
6943 : if (DEBUG_COND) {
6944 : std::cout << " getLatOffset veh=" << getID()
6945 : << " wrt targetLane=" << Named::getIDSecure(myLaneChangeModel->getTargetLane())
6946 : << "\n i=" << i
6947 : << " posLat=" << myState.myPosLat
6948 : << " furtherPosLat=" << myFurtherLanesPosLat[i]
6949 : << " maneuverDist=" << myLaneChangeModel->getManeuverDist()
6950 : << " targetDir=" << targetDir
6951 : << " latOffset=" << latOffset
6952 : << std::endl;
6953 : }
6954 : #endif
6955 2420 : return latOffset;
6956 : }
6957 : }
6958 : assert(false);
6959 18 : throw ProcessError("Request lateral offset of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
6960 : }
6961 : }
6962 :
6963 :
6964 : double
6965 35267831 : MSVehicle::lateralDistanceToLane(const int offset) const {
6966 : // compute the distance when changing to the neighboring lane
6967 : // (ensure we do not lap into the line behind neighLane since there might be unseen blockers)
6968 : assert(offset == 0 || offset == 1 || offset == -1);
6969 : assert(myLane != nullptr);
6970 : assert(myLane->getParallelLane(offset) != nullptr || myLane->getParallelOpposite() != nullptr);
6971 35267831 : const double halfCurrentLaneWidth = 0.5 * myLane->getWidth();
6972 35267831 : const double halfVehWidth = 0.5 * (getWidth() + NUMERICAL_EPS);
6973 35267831 : const double latPos = getLateralPositionOnLane();
6974 35267831 : const double oppositeSign = getLaneChangeModel().isOpposite() ? -1 : 1;
6975 35267831 : double leftLimit = halfCurrentLaneWidth - halfVehWidth - oppositeSign * latPos;
6976 35267831 : double rightLimit = -halfCurrentLaneWidth + halfVehWidth - oppositeSign * latPos;
6977 : double latLaneDist = 0; // minimum distance to move the vehicle fully onto the new lane
6978 35267831 : if (offset == 0) {
6979 10 : if (latPos + halfVehWidth > halfCurrentLaneWidth) {
6980 : // correct overlapping left
6981 5 : latLaneDist = halfCurrentLaneWidth - latPos - halfVehWidth;
6982 5 : } else if (latPos - halfVehWidth < -halfCurrentLaneWidth) {
6983 : // correct overlapping right
6984 5 : latLaneDist = -halfCurrentLaneWidth - latPos + halfVehWidth;
6985 : }
6986 10 : latLaneDist *= oppositeSign;
6987 35267821 : } else if (offset == -1) {
6988 16015953 : latLaneDist = rightLimit - (getWidth() + NUMERICAL_EPS);
6989 19251868 : } else if (offset == 1) {
6990 19251868 : latLaneDist = leftLimit + (getWidth() + NUMERICAL_EPS);
6991 : }
6992 : #ifdef DEBUG_ACTIONSTEPS
6993 : if (DEBUG_COND) {
6994 : std::cout << SIMTIME
6995 : << " veh=" << getID()
6996 : << " halfCurrentLaneWidth=" << halfCurrentLaneWidth
6997 : << " halfVehWidth=" << halfVehWidth
6998 : << " latPos=" << latPos
6999 : << " latLaneDist=" << latLaneDist
7000 : << " leftLimit=" << leftLimit
7001 : << " rightLimit=" << rightLimit
7002 : << "\n";
7003 : }
7004 : #endif
7005 35267831 : return latLaneDist;
7006 : }
7007 :
7008 :
7009 : double
7010 4909668917 : MSVehicle::getLateralOverlap(double posLat, const MSLane* lane) const {
7011 4909668917 : return (fabs(posLat) + 0.5 * getVehicleType().getWidth()
7012 4909668917 : - 0.5 * lane->getWidth());
7013 : }
7014 :
7015 : double
7016 0 : MSVehicle::getLateralOverlap(const MSLane* lane) const {
7017 0 : return getLateralOverlap(getLateralPositionOnLane(), lane);
7018 : }
7019 :
7020 : double
7021 4725319190 : MSVehicle::getLateralOverlap() const {
7022 4725319190 : return getLateralOverlap(getLateralPositionOnLane(), myLane);
7023 : }
7024 :
7025 :
7026 : void
7027 611403278 : MSVehicle::removeApproachingInformation(const DriveItemVector& lfLinks) const {
7028 1758403633 : for (const DriveProcessItem& dpi : lfLinks) {
7029 1147000355 : if (dpi.myLink != nullptr) {
7030 805528315 : dpi.myLink->removeApproaching(this);
7031 : }
7032 : }
7033 : // unregister on all shadow links
7034 611403278 : myLaneChangeModel->removeShadowApproachingInformation();
7035 611403278 : }
7036 :
7037 :
7038 : bool
7039 798097 : MSVehicle::unsafeLinkAhead(const MSLane* lane, double zipperDist) const {
7040 : // the following links are unsafe:
7041 : // - zipper links if they are close enough and have approaching vehicles in the relevant time range
7042 : // - unprioritized links if the vehicle is currently approaching a prioritzed link and unable to stop in time
7043 798097 : double seen = myLane->getLength() - getPositionOnLane();
7044 798097 : const double dist = MAX2(zipperDist, getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0));
7045 798097 : if (seen < dist) {
7046 73883 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(lane);
7047 : int view = 1;
7048 73883 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
7049 : DriveItemVector::const_iterator di = myLFLinkLanes.begin();
7050 123471 : while (!lane->isLinkEnd(link) && seen <= dist) {
7051 73778 : if ((!lane->isInternal()
7052 48232 : && (((*link)->getState() == LINKSTATE_ZIPPER && seen < (*link)->getFoeVisibilityDistance())
7053 26038 : || !(*link)->havePriority()))
7054 99621 : || (lane->isInternal() && zipperDist > 0)) {
7055 : // find the drive item corresponding to this link
7056 : bool found = false;
7057 51388 : while (di != myLFLinkLanes.end() && !found) {
7058 26629 : if ((*di).myLink != nullptr) {
7059 : const MSLane* diPredLane = (*di).myLink->getLaneBefore();
7060 26627 : if (diPredLane != nullptr) {
7061 26627 : if (&diPredLane->getEdge() == &lane->getEdge()) {
7062 : found = true;
7063 : }
7064 : }
7065 : }
7066 26629 : if (!found) {
7067 : di++;
7068 : }
7069 : }
7070 24759 : if (found) {
7071 24757 : const SUMOTime leaveTime = (*link)->getLeaveTime((*di).myArrivalTime, (*di).myArrivalSpeed,
7072 24757 : (*di).getLeaveSpeed(), getVehicleType().getLength());
7073 24757 : const MSLink* entry = (*link)->getCorrespondingEntryLink();
7074 : //if (DEBUG_COND) {
7075 : // 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";
7076 : //}
7077 24757 : if (entry->hasApproachingFoe((*di).myArrivalTime, leaveTime, (*di).myArrivalSpeed, getCarFollowModel().getMaxDecel())) {
7078 : //std::cout << SIMTIME << " veh=" << getID() << " aborting changeTo=" << Named::getIDSecure(bestLaneConts.front()) << " linkState=" << toString((*link)->getState()) << " seen=" << seen << " dist=" << dist << "\n";
7079 : return true;
7080 : }
7081 : }
7082 : // no drive item is found if the vehicle aborts its request within dist
7083 : }
7084 49588 : lane = (*link)->getViaLaneOrLane();
7085 49588 : if (!lane->getEdge().isInternal()) {
7086 27565 : view++;
7087 : }
7088 49588 : seen += lane->getLength();
7089 49588 : link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
7090 : }
7091 : }
7092 : return false;
7093 : }
7094 :
7095 :
7096 : PositionVector
7097 6433807 : MSVehicle::getBoundingBox(double offset) const {
7098 6433807 : PositionVector centerLine;
7099 6433807 : Position pos = getPosition();
7100 6433807 : centerLine.push_back(pos);
7101 6433807 : switch (myType->getGuiShape()) {
7102 1730 : case SUMOVehicleShape::BUS_FLEXIBLE:
7103 : case SUMOVehicleShape::RAIL:
7104 : case SUMOVehicleShape::RAIL_CAR:
7105 : case SUMOVehicleShape::RAIL_CARGO:
7106 : case SUMOVehicleShape::TRUCK_SEMITRAILER:
7107 : case SUMOVehicleShape::TRUCK_1TRAILER: {
7108 4460 : for (MSLane* lane : myFurtherLanes) {
7109 2730 : centerLine.push_back(lane->getShape().back());
7110 : }
7111 : break;
7112 : }
7113 : default:
7114 : break;
7115 : }
7116 6433807 : double l = getLength();
7117 6433807 : Position backPos = getBackPosition();
7118 6433807 : if (pos.distanceTo2D(backPos) > l + NUMERICAL_EPS) {
7119 : // getBackPosition may not match the visual back in networks without internal lanes
7120 329431 : double a = getAngle() + M_PI; // angle pointing backwards
7121 329431 : backPos = pos + Position(l * cos(a), l * sin(a));
7122 : }
7123 6433807 : centerLine.push_back(backPos);
7124 6433807 : if (offset != 0) {
7125 6543 : centerLine.extrapolate2D(offset);
7126 : }
7127 : PositionVector result = centerLine;
7128 12863324 : result.move2side(MAX2(0.0, 0.5 * myType->getWidth() + offset));
7129 12863324 : centerLine.move2side(MIN2(0.0, -0.5 * myType->getWidth() - offset));
7130 6433807 : result.append(centerLine.reverse(), POSITION_EPS);
7131 6433807 : return result;
7132 6433807 : }
7133 :
7134 :
7135 : PositionVector
7136 73693 : MSVehicle::getBoundingPoly(double offset) const {
7137 73693 : switch (myType->getGuiShape()) {
7138 73197 : case SUMOVehicleShape::PASSENGER:
7139 : case SUMOVehicleShape::PASSENGER_SEDAN:
7140 : case SUMOVehicleShape::PASSENGER_HATCHBACK:
7141 : case SUMOVehicleShape::PASSENGER_WAGON:
7142 : case SUMOVehicleShape::PASSENGER_VAN: {
7143 : // box with corners cut off
7144 73197 : PositionVector result;
7145 73197 : PositionVector centerLine;
7146 73197 : centerLine.push_back(getPosition());
7147 73197 : centerLine.push_back(getBackPosition());
7148 73197 : if (offset != 0) {
7149 1600 : centerLine.extrapolate2D(offset);
7150 : }
7151 : PositionVector line1 = centerLine;
7152 : PositionVector line2 = centerLine;
7153 146394 : line1.move2side(MAX2(0.0, 0.3 * myType->getWidth() + offset));
7154 146394 : line2.move2side(MAX2(0.0, 0.5 * myType->getWidth() + offset));
7155 73197 : line2.scaleRelative(0.8);
7156 73197 : result.push_back(line1[0]);
7157 73197 : result.push_back(line2[0]);
7158 73197 : result.push_back(line2[1]);
7159 73197 : result.push_back(line1[1]);
7160 146394 : line1.move2side(MIN2(0.0, -0.6 * myType->getWidth() - offset));
7161 146394 : line2.move2side(MIN2(0.0, -1.0 * myType->getWidth() - offset));
7162 73197 : result.push_back(line1[1]);
7163 73197 : result.push_back(line2[1]);
7164 73197 : result.push_back(line2[0]);
7165 73197 : result.push_back(line1[0]);
7166 : return result;
7167 73197 : }
7168 496 : default:
7169 496 : return getBoundingBox();
7170 : }
7171 : }
7172 :
7173 :
7174 : bool
7175 546400 : MSVehicle::onFurtherEdge(const MSEdge* edge) const {
7176 634846 : for (std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
7177 178629 : if (&(*i)->getEdge() == edge) {
7178 : return true;
7179 : }
7180 : }
7181 : return false;
7182 : }
7183 :
7184 :
7185 : bool
7186 7588607931 : MSVehicle::isBidiOn(const MSLane* lane) const {
7187 7589688707 : return lane->getBidiLane() != nullptr && (
7188 1080776 : myLane == lane->getBidiLane()
7189 546400 : || onFurtherEdge(&lane->getBidiLane()->getEdge()));
7190 : }
7191 :
7192 :
7193 : bool
7194 15 : MSVehicle::rerouteParkingArea(const std::string& parkingAreaID, std::string& errorMsg) {
7195 : // this function is based on MSTriggeredRerouter::rerouteParkingArea in order to keep
7196 : // consistency in the behaviour.
7197 :
7198 : // get vehicle params
7199 15 : MSParkingArea* destParkArea = getNextParkingArea();
7200 15 : const MSRoute& route = getRoute();
7201 15 : const MSEdge* lastEdge = route.getLastEdge();
7202 :
7203 15 : if (destParkArea == nullptr) {
7204 : // not driving towards a parking area
7205 0 : errorMsg = "Vehicle " + getID() + " is not driving to a parking area so it cannot be rerouted.";
7206 0 : return false;
7207 : }
7208 :
7209 : // if the current route ends at the parking area, the new route will also and at the new area
7210 15 : bool newDestination = (&destParkArea->getLane().getEdge() == route.getLastEdge()
7211 5 : && getArrivalPos() >= destParkArea->getBeginLanePosition()
7212 20 : && getArrivalPos() <= destParkArea->getEndLanePosition());
7213 :
7214 : // retrieve info on the new parking area
7215 15 : MSParkingArea* newParkingArea = (MSParkingArea*) MSNet::getInstance()->getStoppingPlace(
7216 : parkingAreaID, SumoXMLTag::SUMO_TAG_PARKING_AREA);
7217 :
7218 15 : if (newParkingArea == nullptr) {
7219 0 : errorMsg = "Parking area ID " + toString(parkingAreaID) + " not found in the network.";
7220 0 : return false;
7221 : }
7222 :
7223 15 : const MSEdge* newEdge = &(newParkingArea->getLane().getEdge());
7224 15 : SUMOAbstractRouter<MSEdge, SUMOVehicle>& router = getRouterTT();
7225 :
7226 : // Compute the route from the current edge to the parking area edge
7227 : ConstMSEdgeVector edgesToPark;
7228 15 : router.compute(getEdge(), newEdge, this, MSNet::getInstance()->getCurrentTimeStep(), edgesToPark);
7229 :
7230 : // Compute the route from the parking area edge to the end of the route
7231 : ConstMSEdgeVector edgesFromPark;
7232 15 : if (!newDestination) {
7233 10 : router.compute(newEdge, lastEdge, this, MSNet::getInstance()->getCurrentTimeStep(), edgesFromPark);
7234 : } else {
7235 : // adapt plans of any riders
7236 10 : for (MSTransportable* p : getPersons()) {
7237 5 : p->rerouteParkingArea(getNextParkingArea(), newParkingArea);
7238 : }
7239 : }
7240 :
7241 : // we have a new destination, let's replace the vehicle route
7242 15 : ConstMSEdgeVector edges = edgesToPark;
7243 15 : if (edgesFromPark.size() > 0) {
7244 10 : edges.insert(edges.end(), edgesFromPark.begin() + 1, edgesFromPark.end());
7245 : }
7246 :
7247 15 : if (newDestination) {
7248 5 : SUMOVehicleParameter* newParameter = new SUMOVehicleParameter();
7249 5 : *newParameter = getParameter();
7250 5 : newParameter->arrivalPosProcedure = ArrivalPosDefinition::GIVEN;
7251 5 : newParameter->arrivalPos = newParkingArea->getEndLanePosition();
7252 5 : replaceParameter(newParameter);
7253 : }
7254 15 : const double routeCost = router.recomputeCosts(edges, this, MSNet::getInstance()->getCurrentTimeStep());
7255 15 : ConstMSEdgeVector prevEdges(myCurrEdge, myRoute->end());
7256 15 : const double savings = router.recomputeCosts(prevEdges, this, MSNet::getInstance()->getCurrentTimeStep());
7257 15 : if (replaceParkingArea(newParkingArea, errorMsg)) {
7258 15 : const bool onInit = myLane == nullptr;
7259 30 : replaceRouteEdges(edges, routeCost, savings, "TraCI:" + toString(SUMO_TAG_PARKING_AREA_REROUTE), onInit, false, false);
7260 : } else {
7261 0 : WRITE_WARNING("Vehicle '" + getID() + "' could not reroute to new parkingArea '" + newParkingArea->getID()
7262 : + "' reason=" + errorMsg + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
7263 0 : return false;
7264 : }
7265 15 : return true;
7266 15 : }
7267 :
7268 :
7269 : bool
7270 45120 : MSVehicle::addTraciStop(SUMOVehicleParameter::Stop stop, std::string& errorMsg) {
7271 45120 : const int numStops = (int)myStops.size();
7272 45120 : const bool result = MSBaseVehicle::addTraciStop(stop, errorMsg);
7273 45120 : if (myLane != nullptr && numStops != (int)myStops.size()) {
7274 43542 : updateBestLanes(true);
7275 : }
7276 45120 : return result;
7277 : }
7278 :
7279 :
7280 : bool
7281 2945 : MSVehicle::handleCollisionStop(MSStop& stop, const double distToStop) {
7282 2945 : if (myCurrEdge == stop.edge && distToStop + POSITION_EPS < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0)) {
7283 1307 : if (distToStop < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)) {
7284 1013 : double vNew = getCarFollowModel().maximumSafeStopSpeed(distToStop, getCarFollowModel().getMaxDecel(), getSpeed(), false, 0);
7285 : //std::cout << SIMTIME << " veh=" << getID() << " v=" << myState.mySpeed << " distToStop=" << distToStop
7286 : // << " vMinNex=" << getCarFollowModel().minNextSpeed(getSpeed(), this)
7287 : // << " bg1=" << getCarFollowModel().brakeGap(myState.mySpeed)
7288 : // << " bg2=" << getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)
7289 : // << " vNew=" << vNew
7290 : // << "\n";
7291 1013 : myState.mySpeed = MIN2(myState.mySpeed, vNew + ACCEL2SPEED(getCarFollowModel().getEmergencyDecel()));
7292 1013 : myState.myPos = MIN2(myState.myPos, stop.pars.endPos);
7293 1013 : myCachedPosition = Position::INVALID;
7294 1013 : if (myState.myPos < myType->getLength()) {
7295 441 : computeFurtherLanes(myLane, myState.myPos, true);
7296 441 : myAngle = computeAngle();
7297 441 : if (myLaneChangeModel->isOpposite()) {
7298 0 : myAngle += M_PI;
7299 : }
7300 : }
7301 : }
7302 : }
7303 2945 : return true;
7304 : }
7305 :
7306 :
7307 : bool
7308 21802402 : MSVehicle::resumeFromStopping() {
7309 21802402 : if (isStopped()) {
7310 46097 : if (myAmRegisteredAsWaiting) {
7311 114 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
7312 114 : myAmRegisteredAsWaiting = false;
7313 : }
7314 : MSStop& stop = myStops.front();
7315 : // we have waited long enough and fulfilled any passenger-requirements
7316 46097 : if (stop.busstop != nullptr) {
7317 : // inform bus stop about leaving it
7318 17832 : stop.busstop->leaveFrom(this);
7319 : }
7320 : // we have waited long enough and fulfilled any container-requirements
7321 46097 : if (stop.containerstop != nullptr) {
7322 : // inform container stop about leaving it
7323 542 : stop.containerstop->leaveFrom(this);
7324 : }
7325 46097 : if (stop.parkingarea != nullptr && stop.getSpeed() <= 0) {
7326 : // inform parking area about leaving it
7327 7518 : stop.parkingarea->leaveFrom(this);
7328 : }
7329 46097 : if (stop.chargingStation != nullptr) {
7330 : // inform charging station about leaving it
7331 3301 : stop.chargingStation->leaveFrom(this);
7332 : }
7333 : // the current stop is no longer valid
7334 46097 : myLane->getEdge().removeWaiting(this);
7335 : // MSStopOut needs to know whether the stop had a loaded 'ended' value so we call this before replacing the value
7336 46097 : if (stop.pars.started == -1) {
7337 : // waypoint edge was passed in a single step
7338 317 : stop.pars.started = MSNet::getInstance()->getCurrentTimeStep();
7339 : }
7340 46097 : if (MSStopOut::active()) {
7341 3624 : MSStopOut::getInstance()->stopEnded(this, stop.pars, stop.lane->getID());
7342 : }
7343 46097 : stop.pars.ended = MSNet::getInstance()->getCurrentTimeStep();
7344 101450 : for (const auto& rem : myMoveReminders) {
7345 55353 : rem.first->notifyStopEnded();
7346 : }
7347 46097 : if (stop.pars.collision && MSLane::getCollisionAction() == MSLane::COLLISION_ACTION_WARN) {
7348 458 : myCollisionImmunity = TIME2STEPS(5); // leave the conflict area
7349 : }
7350 46097 : if (stop.pars.posLat != INVALID_DOUBLE && MSGlobals::gLateralResolution <= 0) {
7351 : // reset lateral position to default
7352 208 : myState.myPosLat = 0;
7353 : }
7354 46097 : myPastStops.push_back(stop.pars);
7355 46097 : myPastStops.back().routeIndex = (int)(stop.edge - myRoute->begin());
7356 46097 : myStops.pop_front();
7357 46097 : myStopDist = std::numeric_limits<double>::max();
7358 : // do not count the stopping time towards gridlock time.
7359 : // Other outputs use an independent counter and are not affected.
7360 46097 : myWaitingTime = 0;
7361 : // maybe the next stop is on the same edge; let's rebuild best lanes
7362 46097 : updateBestLanes(true);
7363 : // continue as wished...
7364 46097 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::ENDING_STOP);
7365 46097 : MSNet::getInstance()->getVehicleControl().registerStopEnded();
7366 46097 : return true;
7367 : }
7368 : return false;
7369 : }
7370 :
7371 :
7372 : MSVehicle::Influencer&
7373 4166860 : MSVehicle::getInfluencer() {
7374 4166860 : if (myInfluencer == nullptr) {
7375 3554 : myInfluencer = new Influencer();
7376 : }
7377 4166860 : return *myInfluencer;
7378 : }
7379 :
7380 : MSVehicle::BaseInfluencer&
7381 24 : MSVehicle::getBaseInfluencer() {
7382 24 : return getInfluencer();
7383 : }
7384 :
7385 :
7386 : const MSVehicle::Influencer*
7387 0 : MSVehicle::getInfluencer() const {
7388 0 : return myInfluencer;
7389 : }
7390 :
7391 : const MSVehicle::BaseInfluencer*
7392 235427 : MSVehicle::getBaseInfluencer() const {
7393 235427 : return myInfluencer;
7394 : }
7395 :
7396 :
7397 : double
7398 4090 : MSVehicle::getSpeedWithoutTraciInfluence() const {
7399 4090 : if (myInfluencer != nullptr && myInfluencer->getOriginalSpeed() >= 0) {
7400 : // influencer original speed is -1 on initialization
7401 1666 : return myInfluencer->getOriginalSpeed();
7402 : }
7403 2424 : return myState.mySpeed;
7404 : }
7405 :
7406 :
7407 : int
7408 891183714 : MSVehicle::influenceChangeDecision(int state) {
7409 891183714 : if (hasInfluencer()) {
7410 2446380 : state = getInfluencer().influenceChangeDecision(
7411 : MSNet::getInstance()->getCurrentTimeStep(),
7412 2446380 : myLane->getEdge(),
7413 : getLaneIndex(),
7414 : state);
7415 : }
7416 891183714 : return state;
7417 : }
7418 :
7419 :
7420 : void
7421 7775 : MSVehicle::setRemoteState(Position xyPos) {
7422 7775 : myCachedPosition = xyPos;
7423 7775 : }
7424 :
7425 :
7426 : bool
7427 763223479 : MSVehicle::isRemoteControlled() const {
7428 763223479 : return myInfluencer != nullptr && myInfluencer->isRemoteControlled();
7429 : }
7430 :
7431 :
7432 : bool
7433 21148 : MSVehicle::wasRemoteControlled(SUMOTime lookBack) const {
7434 21148 : return myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() + lookBack >= MSNet::getInstance()->getCurrentTimeStep();
7435 : }
7436 :
7437 :
7438 : bool
7439 485255017 : MSVehicle::keepClear(const MSLink* link) const {
7440 485255017 : if (link->hasFoes() && link->keepClear() /* && item.myLink->willHaveBlockedFoe()*/) {
7441 153931264 : const double keepClearTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_KEEPCLEAR_TIME, -1);
7442 : //std::cout << SIMTIME << " veh=" << getID() << " keepClearTime=" << keepClearTime << " accWait=" << getAccumulatedWaitingSeconds() << " keepClear=" << (keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime) << "\n";
7443 155332462 : return keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime;
7444 : } else {
7445 : return false;
7446 : }
7447 : }
7448 :
7449 :
7450 : bool
7451 680109359 : MSVehicle::ignoreRed(const MSLink* link, bool canBrake) const {
7452 680109359 : if ((myInfluencer != nullptr && !myInfluencer->getEmergencyBrakeRedLight())) {
7453 : return true;
7454 : }
7455 679831265 : const double ignoreRedTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
7456 : #ifdef DEBUG_IGNORE_RED
7457 : if (DEBUG_COND) {
7458 : std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID() << " state=" << toString(link->getState()) << "\n";
7459 : }
7460 : #endif
7461 679831265 : if (ignoreRedTime < 0) {
7462 679825866 : const double ignoreYellowTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, 0);
7463 679825866 : if (ignoreYellowTime > 0 && link->haveYellow()) {
7464 : assert(link->getTLLogic() != 0);
7465 52 : const double yellowDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
7466 : // when activating ignoreYellow behavior, vehicles will drive if they cannot brake
7467 92 : return !canBrake || ignoreYellowTime > yellowDuration;
7468 : } else {
7469 : return false;
7470 : }
7471 5399 : } else if (link->haveYellow()) {
7472 : // always drive at yellow when ignoring red
7473 : return true;
7474 5243 : } else if (link->haveRed()) {
7475 : assert(link->getTLLogic() != 0);
7476 3832 : const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
7477 : #ifdef DEBUG_IGNORE_RED
7478 : if (DEBUG_COND) {
7479 : std::cout
7480 : // << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID()
7481 : << " ignoreRedTime=" << ignoreRedTime
7482 : << " spentRed=" << redDuration
7483 : << " canBrake=" << canBrake << "\n";
7484 : }
7485 : #endif
7486 : // when activating ignoreRed behavior, vehicles will always drive if they cannot brake
7487 6356 : return !canBrake || ignoreRedTime > redDuration;
7488 : } else {
7489 : return false;
7490 : }
7491 : }
7492 :
7493 : bool
7494 1251503431 : MSVehicle::ignoreFoe(const SUMOTrafficObject* foe) const {
7495 1251503431 : if (!getParameter().wasSet(VEHPARS_CFMODEL_PARAMS_SET)) {
7496 : return false;
7497 : }
7498 2768 : for (const std::string& typeID : StringTokenizer(getParameter().getParameter(toString(SUMO_ATTR_CF_IGNORE_TYPES), "")).getVector()) {
7499 398 : if (typeID == foe->getVehicleType().getID()) {
7500 : return true;
7501 : }
7502 1384 : }
7503 2381 : for (const std::string& id : StringTokenizer(getParameter().getParameter(toString(SUMO_ATTR_CF_IGNORE_IDS), "")).getVector()) {
7504 986 : if (id == foe->getID()) {
7505 : return true;
7506 : }
7507 986 : }
7508 409 : return false;
7509 : }
7510 :
7511 : bool
7512 524164485 : MSVehicle::passingMinor() const {
7513 : // either on an internal lane that was entered via minor link
7514 : // or on approach to minor link below visibility distance
7515 524164485 : if (myLane == nullptr) {
7516 : return false;
7517 : }
7518 524164485 : if (myLane->getEdge().isInternal()) {
7519 8344593 : return !myLane->getIncomingLanes().front().viaLink->havePriority();
7520 515819892 : } else if (myLFLinkLanes.size() > 0 && myLFLinkLanes.front().myLink != nullptr) {
7521 : MSLink* link = myLFLinkLanes.front().myLink;
7522 262017453 : return !link->havePriority() && myLFLinkLanes.front().myDistance <= link->getFoeVisibilityDistance();
7523 : }
7524 : return false;
7525 : }
7526 :
7527 : bool
7528 19774257 : MSVehicle::isLeader(const MSLink* link, const MSVehicle* veh, const double gap) const {
7529 : assert(link->fromInternalLane());
7530 19774257 : if (veh == nullptr) {
7531 : return false;
7532 : }
7533 19774257 : if (!myLane->isInternal() || myLane->getEdge().getToJunction() != link->getJunction()) {
7534 : // if this vehicle is not yet on the junction, every vehicle is a leader
7535 : return true;
7536 : }
7537 1720014 : if (veh->getLaneChangeModel().hasBlueLight()) {
7538 : // blue light device automatically gets right of way
7539 : return true;
7540 : }
7541 1719746 : const MSLane* foeLane = veh->getLane();
7542 1719746 : if (foeLane->isInternal()) {
7543 1306148 : if (foeLane->getEdge().getFromJunction() == link->getJunction()) {
7544 1295883 : SUMOTime egoET = myJunctionConflictEntryTime;
7545 1295883 : SUMOTime foeET = veh->myJunctionEntryTime;
7546 : // check relationship between link and foeLane
7547 1295883 : if (foeLane->getNormalPredecessorLane() == link->getInternalLaneBefore()->getNormalPredecessorLane()) {
7548 : // we are entering the junction from the same lane
7549 403439 : egoET = myJunctionEntryTimeNeverYield;
7550 403439 : foeET = veh->myJunctionEntryTimeNeverYield;
7551 403439 : if (link->isExitLinkAfterInternalJunction() && link->getInternalLaneBefore()->getLogicalPredecessorLane()->getEntryLink()->isIndirect()) {
7552 80411 : egoET = myJunctionConflictEntryTime;
7553 : }
7554 : } else {
7555 892444 : const MSLink* foeLink = foeLane->getIncomingLanes()[0].viaLink;
7556 892444 : const MSJunctionLogic* logic = link->getJunction()->getLogic();
7557 : assert(logic != nullptr);
7558 : // determine who has right of way
7559 : bool response; // ego response to foe
7560 : bool response2; // foe response to ego
7561 : // attempt 1: tlLinkState
7562 892444 : const MSLink* entry = link->getCorrespondingEntryLink();
7563 892444 : const MSLink* foeEntry = foeLink->getCorrespondingEntryLink();
7564 892444 : if (entry->haveRed() || foeEntry->haveRed()) {
7565 : // ensure that vehicles which are stuck on the intersection may exit
7566 111934 : if (!foeEntry->haveRed() && veh->getSpeed() > SUMO_const_haltingSpeed && gap < 0) {
7567 : // foe might be oncoming, don't drive unless foe can still brake safely
7568 11235 : const double foeNextSpeed = veh->getSpeed() + ACCEL2SPEED(veh->getCarFollowModel().getMaxAccel());
7569 11235 : const double foeBrakeGap = veh->getCarFollowModel().brakeGap(
7570 11235 : foeNextSpeed, veh->getCarFollowModel().getMaxDecel(), veh->getCarFollowModel().getHeadwayTime());
7571 : // the minGap was subtracted from gap in MSLink::getLeaderInfo (enlarging the negative gap)
7572 : // so the -2* makes it point in the right direction
7573 11235 : const double foeGap = -gap - veh->getLength() - 2 * getVehicleType().getMinGap();
7574 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7575 : if (DEBUG_COND) {
7576 : std::cout << " foeGap=" << foeGap << " foeBGap=" << foeBrakeGap << "\n";
7577 :
7578 : }
7579 : #endif
7580 11235 : if (foeGap < foeBrakeGap) {
7581 : response = true;
7582 : response2 = false;
7583 : } else {
7584 : response = false;
7585 : response2 = true;
7586 : }
7587 : } else {
7588 : // let conflict entry time decide
7589 : response = true;
7590 : response2 = true;
7591 : }
7592 780510 : } else if (entry->havePriority() != foeEntry->havePriority()) {
7593 562008 : response = !entry->havePriority();
7594 562008 : response2 = !foeEntry->havePriority();
7595 218502 : } else if (entry->haveYellow() && foeEntry->haveYellow()) {
7596 : // let the faster vehicle keep moving
7597 5178 : response = veh->getSpeed() >= getSpeed();
7598 5178 : response2 = getSpeed() >= veh->getSpeed();
7599 : } else {
7600 : // fallback if pedestrian crossings are involved
7601 213324 : response = logic->getResponseFor(link->getIndex()).test(foeLink->getIndex());
7602 213324 : response2 = logic->getResponseFor(foeLink->getIndex()).test(link->getIndex());
7603 : }
7604 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7605 : if (DEBUG_COND) {
7606 : std::cout << SIMTIME
7607 : << " foeLane=" << foeLane->getID()
7608 : << " foeLink=" << foeLink->getViaLaneOrLane()->getID()
7609 : << " linkIndex=" << link->getIndex()
7610 : << " foeLinkIndex=" << foeLink->getIndex()
7611 : << " entryState=" << toString(entry->getState())
7612 : << " entryState2=" << toString(foeEntry->getState())
7613 : << " response=" << response
7614 : << " response2=" << response2
7615 : << "\n";
7616 : }
7617 : #endif
7618 892444 : if (!response) {
7619 : // if we have right of way over the foe, entryTime does not matter
7620 81718 : foeET = veh->myJunctionConflictEntryTime;
7621 81718 : egoET = myJunctionEntryTime;
7622 810726 : } else if (response && response2) {
7623 : // in a mutual conflict scenario, use entry time to avoid deadlock
7624 135506 : foeET = veh->myJunctionConflictEntryTime;
7625 135506 : egoET = myJunctionConflictEntryTime;
7626 : }
7627 : }
7628 1295883 : if (egoET == foeET) {
7629 : // try to use speed as tie braker
7630 126284 : if (getSpeed() == veh->getSpeed()) {
7631 : // use ID as tie braker
7632 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7633 : if (DEBUG_COND) {
7634 : std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
7635 : << " foeIsLeaderByID=" << (getID() < veh->getID()) << "\n";
7636 : }
7637 : #endif
7638 60344 : return getID() < veh->getID();
7639 : } else {
7640 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7641 : if (DEBUG_COND) {
7642 : std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
7643 : << " foeIsLeaderBySpeed=" << (getSpeed() < veh->getSpeed())
7644 : << " v=" << getSpeed() << " foeV=" << veh->getSpeed()
7645 : << "\n";
7646 : }
7647 : #endif
7648 65940 : return getSpeed() < veh->getSpeed();
7649 : }
7650 : } else {
7651 : // leader was on the junction first
7652 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7653 : if (DEBUG_COND) {
7654 : std::cout << SIMTIME << " veh=" << getID() << " egoET " << egoET << " with foe " << veh->getID()
7655 : << " foeET=" << foeET << " isLeader=" << (egoET > foeET) << "\n";
7656 : }
7657 : #endif
7658 1169599 : return egoET > foeET;
7659 : }
7660 : } else {
7661 : // vehicle can only be partially on the junction. Must be a leader
7662 : return true;
7663 : }
7664 : } else {
7665 : // vehicle can only be partially on the junction. Must be a leader
7666 : return true;
7667 : }
7668 : }
7669 :
7670 : void
7671 2485 : MSVehicle::saveState(OutputDevice& out) {
7672 2485 : MSBaseVehicle::saveState(out);
7673 : // here starts the vehicle internal part (see loading)
7674 : std::vector<std::string> internals;
7675 2485 : internals.push_back(toString(myParameter->parametersSet));
7676 2485 : internals.push_back(toString(myDeparture));
7677 2485 : internals.push_back(toString(distance(myRoute->begin(), myCurrEdge)));
7678 2485 : internals.push_back(toString(myDepartPos));
7679 2485 : internals.push_back(toString(myWaitingTime));
7680 2485 : internals.push_back(toString(myTimeLoss));
7681 2485 : internals.push_back(toString(myLastActionTime));
7682 2485 : internals.push_back(toString(isStopped()));
7683 2485 : internals.push_back(toString(myPastStops.size()));
7684 2485 : out.writeAttr(SUMO_ATTR_STATE, internals);
7685 2485 : out.writeAttr(SUMO_ATTR_POSITION, std::vector<double> { myState.myPos, myState.myBackPos, myState.myLastCoveredDist });
7686 2485 : out.writeAttr(SUMO_ATTR_SPEED, std::vector<double> { myState.mySpeed, myState.myPreviousSpeed });
7687 2485 : out.writeAttr(SUMO_ATTR_ANGLE, GeomHelper::naviDegree(myAngle));
7688 2485 : out.writeAttr(SUMO_ATTR_POSITION_LAT, myState.myPosLat);
7689 2485 : out.writeAttr(SUMO_ATTR_WAITINGTIME, myWaitingTimeCollector.getState());
7690 2485 : myLaneChangeModel->saveState(out);
7691 : // save past stops
7692 5452 : for (SUMOVehicleParameter::Stop stop : myPastStops) {
7693 2967 : stop.write(out, false);
7694 : // do not write started and ended twice
7695 2967 : if ((stop.parametersSet & STOP_STARTED_SET) == 0) {
7696 5924 : out.writeAttr(SUMO_ATTR_STARTED, time2string(stop.started));
7697 : }
7698 2967 : if ((stop.parametersSet & STOP_ENDED_SET) == 0) {
7699 5924 : out.writeAttr(SUMO_ATTR_ENDED, time2string(stop.ended));
7700 : }
7701 2967 : stop.writeParams(out);
7702 2967 : out.closeTag();
7703 2967 : }
7704 : // save upcoming stops
7705 2953 : for (MSStop& stop : myStops) {
7706 468 : stop.write(out);
7707 : }
7708 : // save parameters and device states
7709 2485 : myParameter->writeParams(out);
7710 6274 : for (MSVehicleDevice* const dev : myDevices) {
7711 3789 : dev->saveState(out);
7712 : }
7713 2485 : out.closeTag();
7714 2485 : }
7715 :
7716 : void
7717 4168 : MSVehicle::loadState(const SUMOSAXAttributes& attrs, const SUMOTime offset) {
7718 4168 : if (!attrs.hasAttribute(SUMO_ATTR_POSITION)) {
7719 0 : throw ProcessError(TL("Error: Invalid vehicles in state (may be a meso state)!"));
7720 : }
7721 : int routeOffset;
7722 : bool stopped;
7723 : int pastStops;
7724 :
7725 4168 : std::istringstream bis(attrs.getString(SUMO_ATTR_STATE));
7726 4168 : bis >> myParameter->parametersSet;
7727 4168 : bis >> myDeparture;
7728 4168 : bis >> routeOffset;
7729 4168 : bis >> myDepartPos;
7730 4168 : bis >> myWaitingTime;
7731 4168 : bis >> myTimeLoss;
7732 4168 : bis >> myLastActionTime;
7733 : bis >> stopped;
7734 4168 : bis >> pastStops;
7735 :
7736 4168 : if (attrs.hasAttribute(SUMO_ATTR_ARRIVALPOS_RANDOMIZED)) {
7737 : bool ok;
7738 4 : myArrivalPos = attrs.get<double>(SUMO_ATTR_ARRIVALPOS_RANDOMIZED, getID().c_str(), ok);
7739 : }
7740 : // load stops
7741 : myStops.clear();
7742 4168 : addStops(!MSGlobals::gCheckRoutes, &myCurrEdge, false);
7743 :
7744 4168 : if (hasDeparted()) {
7745 1789 : myCurrEdge = myRoute->begin() + routeOffset;
7746 1789 : myDeparture -= offset;
7747 : // fix stops
7748 4730 : while (pastStops > 0) {
7749 2941 : myPastStops.push_back(myStops.front().pars);
7750 2941 : myPastStops.back().routeIndex = (int)(myStops.front().edge - myRoute->begin());
7751 2941 : myStops.pop_front();
7752 2941 : pastStops--;
7753 : }
7754 : // see MSBaseVehicle constructor
7755 1789 : if (myParameter->wasSet(VEHPARS_FORCE_REROUTE)) {
7756 1390 : calculateArrivalParams(true);
7757 : }
7758 : // a (tentative lane is needed for calling hasArrivedInternal
7759 1789 : myLane = (*myCurrEdge)->getLanes()[0];
7760 : }
7761 4168 : if (getActionStepLength() == DELTA_T && !isActionStep(SIMSTEP)) {
7762 1 : myLastActionTime -= (myLastActionTime - SIMSTEP) % DELTA_T;
7763 3 : WRITE_WARNINGF(TL("Action steps are out of sync for loaded vehicle '%'."), getID());
7764 : }
7765 4168 : std::istringstream pis(attrs.getString(SUMO_ATTR_POSITION));
7766 4168 : pis >> myState.myPos >> myState.myBackPos >> myState.myLastCoveredDist;
7767 4168 : std::istringstream sis(attrs.getString(SUMO_ATTR_SPEED));
7768 4168 : sis >> myState.mySpeed >> myState.myPreviousSpeed;
7769 4168 : myAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
7770 4168 : myAngle = GeomHelper::fromNaviDegree(attrs.getFloat(SUMO_ATTR_ANGLE));
7771 4168 : myState.myPosLat = attrs.getFloat(SUMO_ATTR_POSITION_LAT);
7772 4168 : std::istringstream dis(attrs.getString(SUMO_ATTR_DISTANCE));
7773 4168 : dis >> myOdometer >> myNumberReroutes;
7774 4168 : myWaitingTimeCollector.setState(attrs.getString(SUMO_ATTR_WAITINGTIME));
7775 4168 : if (stopped) {
7776 211 : myStops.front().startedFromState = true;
7777 211 : myStopDist = 0;
7778 : }
7779 4168 : myLaneChangeModel->loadState(attrs);
7780 : // no need to reset myCachedPosition here since state loading happens directly after creation
7781 4168 : }
7782 :
7783 : void
7784 37 : MSVehicle::loadPreviousApproaching(MSLink* link, bool setRequest,
7785 : SUMOTime arrivalTime, double arrivalSpeed,
7786 : double arrivalSpeedBraking,
7787 : double dist, double leaveSpeed) {
7788 : // ensure that approach information is reset on the next call to setApproachingForAllLinks
7789 37 : myLFLinkLanes.push_back(DriveProcessItem(link, 0, 0, setRequest,
7790 : arrivalTime, arrivalSpeed, arrivalSpeedBraking, dist, leaveSpeed));
7791 :
7792 37 : }
7793 :
7794 :
7795 : std::shared_ptr<MSSimpleDriverState>
7796 2438302 : MSVehicle::getDriverState() const {
7797 2438302 : return myDriverState->getDriverState();
7798 : }
7799 :
7800 :
7801 : double
7802 596730443 : MSVehicle::getFriction() const {
7803 596730443 : return myFrictionDevice == nullptr ? 1. : myFrictionDevice->getMeasuredFriction();
7804 : }
7805 :
7806 :
7807 : void
7808 241 : MSVehicle::setPreviousSpeed(double prevSpeed, double prevAcceleration) {
7809 241 : myState.mySpeed = MAX2(0., prevSpeed);
7810 : // also retcon acceleration
7811 241 : if (prevAcceleration != std::numeric_limits<double>::min()) {
7812 8 : myAcceleration = prevAcceleration;
7813 : } else {
7814 233 : myAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
7815 : }
7816 241 : }
7817 :
7818 :
7819 : double
7820 1774818140 : MSVehicle::getCurrentApparentDecel() const {
7821 : //return MAX2(-myAcceleration, getCarFollowModel().getApparentDecel());
7822 1774818140 : return getCarFollowModel().getApparentDecel();
7823 : }
7824 :
7825 : /****************************************************************************/
7826 : bool
7827 32 : MSVehicle::setExitManoeuvre() {
7828 32 : return (myManoeuvre.configureExitManoeuvre(this));
7829 : }
7830 :
7831 : /* -------------------------------------------------------------------------
7832 : * methods of MSVehicle::manoeuvre
7833 : * ----------------------------------------------------------------------- */
7834 :
7835 4631147 : MSVehicle::Manoeuvre::Manoeuvre() : myManoeuvreStop(""), myManoeuvreStartTime(0), myManoeuvreCompleteTime(0), myManoeuvreType(MSVehicle::MANOEUVRE_NONE), myGUIIncrement(0) {}
7836 :
7837 :
7838 0 : MSVehicle::Manoeuvre::Manoeuvre(const Manoeuvre& manoeuvre) {
7839 0 : myManoeuvreStop = manoeuvre.myManoeuvreStop;
7840 0 : myManoeuvreStartTime = manoeuvre.myManoeuvreStartTime;
7841 0 : myManoeuvreCompleteTime = manoeuvre.myManoeuvreCompleteTime;
7842 0 : myManoeuvreType = manoeuvre.myManoeuvreType;
7843 0 : myGUIIncrement = manoeuvre.myGUIIncrement;
7844 0 : }
7845 :
7846 :
7847 : MSVehicle::Manoeuvre&
7848 0 : MSVehicle::Manoeuvre::operator=(const Manoeuvre& manoeuvre) {
7849 0 : myManoeuvreStop = manoeuvre.myManoeuvreStop;
7850 0 : myManoeuvreStartTime = manoeuvre.myManoeuvreStartTime;
7851 0 : myManoeuvreCompleteTime = manoeuvre.myManoeuvreCompleteTime;
7852 0 : myManoeuvreType = manoeuvre.myManoeuvreType;
7853 0 : myGUIIncrement = manoeuvre.myGUIIncrement;
7854 0 : return *this;
7855 : }
7856 :
7857 :
7858 : bool
7859 0 : MSVehicle::Manoeuvre::operator!=(const Manoeuvre& manoeuvre) {
7860 0 : return (myManoeuvreStop != manoeuvre.myManoeuvreStop ||
7861 0 : myManoeuvreStartTime != manoeuvre.myManoeuvreStartTime ||
7862 0 : myManoeuvreCompleteTime != manoeuvre.myManoeuvreCompleteTime ||
7863 0 : myManoeuvreType != manoeuvre.myManoeuvreType ||
7864 0 : myGUIIncrement != manoeuvre.myGUIIncrement
7865 0 : );
7866 : }
7867 :
7868 :
7869 : double
7870 450 : MSVehicle::Manoeuvre::getGUIIncrement() const {
7871 450 : return (myGUIIncrement);
7872 : }
7873 :
7874 :
7875 : MSVehicle::ManoeuvreType
7876 2971 : MSVehicle::Manoeuvre::getManoeuvreType() const {
7877 2971 : return (myManoeuvreType);
7878 : }
7879 :
7880 :
7881 : MSVehicle::ManoeuvreType
7882 2971 : MSVehicle::getManoeuvreType() const {
7883 2971 : return (myManoeuvre.getManoeuvreType());
7884 : }
7885 :
7886 :
7887 : void
7888 30 : MSVehicle::setManoeuvreType(const MSVehicle::ManoeuvreType mType) {
7889 30 : myManoeuvre.setManoeuvreType(mType);
7890 30 : }
7891 :
7892 :
7893 : void
7894 30 : MSVehicle::Manoeuvre::setManoeuvreType(const MSVehicle::ManoeuvreType mType) {
7895 30 : myManoeuvreType = mType;
7896 30 : }
7897 :
7898 :
7899 : bool
7900 30 : MSVehicle::Manoeuvre::configureEntryManoeuvre(MSVehicle* veh) {
7901 30 : if (!veh->hasStops()) {
7902 : return false; // should never happen - checked before call
7903 : }
7904 :
7905 30 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
7906 30 : const MSStop& stop = veh->getNextStop();
7907 :
7908 30 : int manoeuverAngle = stop.parkingarea->getLastFreeLotAngle();
7909 30 : double GUIAngle = stop.parkingarea->getLastFreeLotGUIAngle();
7910 30 : if (abs(GUIAngle) < 0.1) {
7911 : GUIAngle = -0.1; // Wiggle vehicle on parallel entry
7912 : }
7913 30 : myManoeuvreVehicleID = veh->getID();
7914 30 : myManoeuvreStop = stop.parkingarea->getID();
7915 30 : myManoeuvreType = MSVehicle::MANOEUVRE_ENTRY;
7916 30 : myManoeuvreStartTime = currentTime;
7917 30 : myManoeuvreCompleteTime = currentTime + veh->myType->getEntryManoeuvreTime(manoeuverAngle);
7918 30 : myGUIIncrement = GUIAngle / (STEPS2TIME(myManoeuvreCompleteTime - myManoeuvreStartTime) / TS);
7919 :
7920 : #ifdef DEBUG_STOPS
7921 : if (veh->isSelected()) {
7922 : 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 <<
7923 : " endTime=" << myManoeuvreCompleteTime << " manoeuvre time=" << myManoeuvreCompleteTime - currentTime << " parkArea=" << myManoeuvreStop << std::endl;
7924 : }
7925 : #endif
7926 :
7927 30 : return (true);
7928 : }
7929 :
7930 :
7931 : bool
7932 32 : MSVehicle::Manoeuvre::configureExitManoeuvre(MSVehicle* veh) {
7933 : // At the moment we only want to set for parking areas
7934 32 : if (!veh->hasStops()) {
7935 : return true;
7936 : }
7937 32 : if (veh->getNextStop().parkingarea == nullptr) {
7938 : return true;
7939 : }
7940 :
7941 30 : if (myManoeuvreType != MSVehicle::MANOEUVRE_NONE) {
7942 : return (false);
7943 : }
7944 :
7945 30 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
7946 :
7947 30 : int manoeuverAngle = veh->getCurrentParkingArea()->getManoeuverAngle(*veh);
7948 30 : double GUIAngle = veh->getCurrentParkingArea()->getGUIAngle(*veh);
7949 30 : if (abs(GUIAngle) < 0.1) {
7950 : GUIAngle = 0.1; // Wiggle vehicle on parallel exit
7951 : }
7952 :
7953 30 : myManoeuvreVehicleID = veh->getID();
7954 30 : myManoeuvreStop = veh->getCurrentParkingArea()->getID();
7955 30 : myManoeuvreType = MSVehicle::MANOEUVRE_EXIT;
7956 30 : myManoeuvreStartTime = currentTime;
7957 30 : myManoeuvreCompleteTime = currentTime + veh->myType->getExitManoeuvreTime(manoeuverAngle);
7958 30 : myGUIIncrement = -GUIAngle / (STEPS2TIME(myManoeuvreCompleteTime - myManoeuvreStartTime) / TS);
7959 30 : if (veh->remainingStopDuration() > 0) {
7960 20 : myManoeuvreCompleteTime += veh->remainingStopDuration();
7961 : }
7962 :
7963 : #ifdef DEBUG_STOPS
7964 : if (veh->isSelected()) {
7965 : std::cout << "EXIT manoeuvre start: vehicle=" << veh->getID() << " Manoeuvre Angle=" << manoeuverAngle << " increment=" << RAD2DEG(myGUIIncrement) << " currentTime=" << currentTime
7966 : << " endTime=" << myManoeuvreCompleteTime << " manoeuvre time=" << myManoeuvreCompleteTime - currentTime << " parkArea=" << myManoeuvreStop << std::endl;
7967 : }
7968 : #endif
7969 :
7970 : return (true);
7971 : }
7972 :
7973 :
7974 : bool
7975 222 : MSVehicle::Manoeuvre::entryManoeuvreIsComplete(MSVehicle* veh) {
7976 : // At the moment we only want to consider parking areas - need to check because we could be setting up a manoeuvre
7977 222 : if (!veh->hasStops()) {
7978 : return (true);
7979 : }
7980 : MSStop* currentStop = &veh->myStops.front();
7981 222 : if (currentStop->parkingarea == nullptr) {
7982 : return true;
7983 220 : } else if (currentStop->parkingarea->getID() != myManoeuvreStop || MSVehicle::MANOEUVRE_ENTRY != myManoeuvreType) {
7984 30 : if (configureEntryManoeuvre(veh)) {
7985 30 : MSNet::getInstance()->informVehicleStateListener(veh, MSNet::VehicleState::MANEUVERING);
7986 30 : return (false);
7987 : } else { // cannot configure entry so stop trying
7988 : return true;
7989 : }
7990 190 : } else if (MSNet::getInstance()->getCurrentTimeStep() < myManoeuvreCompleteTime) {
7991 : return false;
7992 : } else { // manoeuvre complete
7993 30 : myManoeuvreType = MSVehicle::MANOEUVRE_NONE;
7994 30 : return true;
7995 : }
7996 : }
7997 :
7998 :
7999 : bool
8000 0 : MSVehicle::Manoeuvre::manoeuvreIsComplete(const ManoeuvreType checkType) const {
8001 0 : if (checkType != myManoeuvreType) {
8002 : return true; // we're not maneuvering / wrong manoeuvre
8003 : }
8004 :
8005 0 : if (MSNet::getInstance()->getCurrentTimeStep() < myManoeuvreCompleteTime) {
8006 : return false;
8007 : } else {
8008 : return true;
8009 : }
8010 : }
8011 :
8012 :
8013 : bool
8014 6266 : MSVehicle::Manoeuvre::manoeuvreIsComplete() const {
8015 6266 : return (MSNet::getInstance()->getCurrentTimeStep() >= myManoeuvreCompleteTime);
8016 : }
8017 :
8018 :
8019 : bool
8020 6266 : MSVehicle::manoeuvreIsComplete() const {
8021 6266 : return (myManoeuvre.manoeuvreIsComplete());
8022 : }
8023 :
8024 :
8025 : std::pair<double, double>
8026 7058 : MSVehicle::estimateTimeToNextStop() const {
8027 7058 : if (hasStops()) {
8028 7058 : MSLane* lane = myLane;
8029 7058 : if (lane == nullptr) {
8030 : // not in network
8031 84 : lane = getEdge()->getLanes()[0];
8032 : }
8033 : const MSStop& stop = myStops.front();
8034 : auto it = myCurrEdge + 1;
8035 : // drive to end of current edge
8036 7058 : double dist = (lane->getLength() - getPositionOnLane());
8037 7058 : double travelTime = lane->getEdge().getMinimumTravelTime(this) * dist / lane->getLength();
8038 : // drive until stop edge
8039 8414 : while (it != myRoute->end() && it < stop.edge) {
8040 1356 : travelTime += (*it)->getMinimumTravelTime(this);
8041 1356 : dist += (*it)->getLength();
8042 : it++;
8043 : }
8044 : // drive up to the stop position
8045 7058 : const double stopEdgeDist = stop.pars.endPos - (lane == stop.lane ? lane->getLength() : 0);
8046 7058 : dist += stopEdgeDist;
8047 7058 : travelTime += stop.lane->getEdge().getMinimumTravelTime(this) * (stopEdgeDist / stop.lane->getLength());
8048 : // estimate time loss due to acceleration and deceleration
8049 : // maximum speed is limited by available distance:
8050 : const double a = getCarFollowModel().getMaxAccel();
8051 : const double b = getCarFollowModel().getMaxDecel();
8052 7058 : const double c = getSpeed();
8053 : const double d = dist;
8054 7058 : const double len = getVehicleType().getLength();
8055 7058 : const double vs = MIN2(MAX2(stop.getSpeed(), 0.0), stop.lane->getVehicleMaxSpeed(this));
8056 : // distAccel = (v - c)^2 / (2a)
8057 : // distDecel = (v + vs)*(v - vs) / 2b = (v^2 - vs^2) / (2b)
8058 : // distAccel + distDecel < d
8059 7058 : 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))))
8060 13848 : + pow((a * vs), 2))))) * 0.5) + (c * b)) / (b + a));
8061 7058 : it = myCurrEdge;
8062 : double v0 = c;
8063 7058 : bool v0Stable = getAcceleration() == 0 && v0 > 0;
8064 : double timeLossAccel = 0;
8065 : double timeLossDecel = 0;
8066 : double timeLossLength = 0;
8067 16848 : while (it != myRoute->end() && it <= stop.edge) {
8068 9790 : double v = MIN2(maxVD, (*it)->getVehicleMaxSpeed(this));
8069 9790 : double edgeLength = (it == stop.edge ? stop.pars.endPos : (*it)->getLength()) - (it == myCurrEdge ? getPositionOnLane() : 0);
8070 9790 : if (edgeLength <= len && v0Stable && v0 < v) {
8071 : const double lengthDist = MIN2(len, edgeLength);
8072 20 : const double dTL = lengthDist / v0 - lengthDist / v;
8073 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " el=" << edgeLength << " lDist=" << lengthDist << " newTLL=" << dTL<< "\n";
8074 20 : timeLossLength += dTL;
8075 : }
8076 9790 : if (edgeLength > len) {
8077 8698 : const double dv = v - v0;
8078 8698 : if (dv > 0) {
8079 : // timeLossAccel = timeAccel - timeMaxspeed = dv / a - distAccel / v
8080 6350 : const double dTA = dv / a - dv * (v + v0) / (2 * a * v);
8081 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLA=" << dTA << "\n";
8082 6350 : timeLossAccel += dTA;
8083 : // time loss from vehicle length
8084 2348 : } else if (dv < 0) {
8085 : // timeLossDecel = timeDecel - timeMaxspeed = dv / b - distDecel / v
8086 540 : const double dTD = -dv / b + dv * (v + v0) / (2 * b * v0);
8087 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLD=" << dTD << "\n";
8088 540 : timeLossDecel += dTD;
8089 : }
8090 : v0 = v;
8091 : v0Stable = true;
8092 : }
8093 : it++;
8094 : }
8095 : // final deceleration to stop (may also be acceleration or deceleration to waypoint speed)
8096 : double v = vs;
8097 7058 : const double dv = v - v0;
8098 7058 : if (dv > 0) {
8099 : // timeLossAccel = timeAccel - timeMaxspeed = dv / a - distAccel / v
8100 144 : const double dTA = dv / a - dv * (v + v0) / (2 * a * v);
8101 : //std::cout << " final e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLA=" << dTA << "\n";
8102 144 : timeLossAccel += dTA;
8103 : // time loss from vehicle length
8104 6914 : } else if (dv < 0) {
8105 : // timeLossDecel = timeDecel - timeMaxspeed = dv / b - distDecel / v
8106 6890 : const double dTD = -dv / b + dv * (v + v0) / (2 * b * v0);
8107 : //std::cout << " final e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLD=" << dTD << "\n";
8108 6890 : timeLossDecel += dTD;
8109 : }
8110 7058 : const double result = travelTime + timeLossAccel + timeLossDecel + timeLossLength;
8111 : //std::cout << SIMTIME << " v=" << c << " a=" << a << " b=" << b << " maxVD=" << maxVD << " tt=" << travelTime
8112 : // << " ta=" << timeLossAccel << " td=" << timeLossDecel << " tl=" << timeLossLength << " res=" << result << "\n";
8113 7058 : return {MAX2(0.0, result), dist};
8114 : } else {
8115 0 : return {INVALID_DOUBLE, INVALID_DOUBLE};
8116 : }
8117 : }
8118 :
8119 :
8120 : double
8121 1675 : MSVehicle::getStopDelay() const {
8122 1675 : if (hasStops() && myStops.front().pars.until >= 0) {
8123 : const MSStop& stop = myStops.front();
8124 1392 : SUMOTime estimatedDepart = MSNet::getInstance()->getCurrentTimeStep() - DELTA_T;
8125 1392 : if (stop.reached) {
8126 732 : return STEPS2TIME(estimatedDepart + stop.duration - stop.pars.until);
8127 : }
8128 660 : if (stop.pars.duration > 0) {
8129 588 : estimatedDepart += stop.pars.duration;
8130 : }
8131 660 : estimatedDepart += TIME2STEPS(estimateTimeToNextStop().first);
8132 660 : const double result = MAX2(0.0, STEPS2TIME(estimatedDepart - stop.pars.until));
8133 660 : return result;
8134 : } else {
8135 : // vehicles cannot drive before 'until' so stop delay can never be
8136 : // negative and we can use -1 to signal "undefined"
8137 : return -1;
8138 : }
8139 : }
8140 :
8141 :
8142 : double
8143 5430 : MSVehicle::getStopArrivalDelay() const {
8144 5430 : if (hasStops() && myStops.front().pars.arrival >= 0) {
8145 : const MSStop& stop = myStops.front();
8146 4252 : if (stop.reached) {
8147 1304 : return STEPS2TIME(stop.pars.started - stop.pars.arrival);
8148 : } else {
8149 2948 : return STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) + estimateTimeToNextStop().first - STEPS2TIME(stop.pars.arrival);
8150 : }
8151 : } else {
8152 : // vehicles can arrival earlier than planned so arrival delay can be negative
8153 : return INVALID_DOUBLE;
8154 : }
8155 : }
8156 :
8157 :
8158 : const MSEdge*
8159 2941153521 : MSVehicle::getCurrentEdge() const {
8160 2941153521 : return myLane != nullptr ? &myLane->getEdge() : getEdge();
8161 : }
8162 :
8163 :
8164 : const MSEdge*
8165 3896 : MSVehicle::getNextEdgePtr() const {
8166 3896 : if (myLane == nullptr || (myCurrEdge + 1) == myRoute->end()) {
8167 8 : return nullptr;
8168 : }
8169 3888 : if (myLane->isInternal()) {
8170 568 : return &myLane->getCanonicalSuccessorLane()->getEdge();
8171 : } else {
8172 3320 : const MSEdge* nextNormal = succEdge(1);
8173 3320 : const MSEdge* nextInternal = myLane->getEdge().getInternalFollowingEdge(nextNormal, getVClass());
8174 3320 : return nextInternal ? nextInternal : nextNormal;
8175 : }
8176 : }
8177 :
8178 :
8179 : const MSLane*
8180 1451 : MSVehicle::getPreviousLane(const MSLane* current, int& furtherIndex) const {
8181 1451 : if (furtherIndex < (int)myFurtherLanes.size()) {
8182 1074 : return myFurtherLanes[furtherIndex++];
8183 : } else {
8184 : // try to use route information
8185 377 : int routeIndex = getRoutePosition();
8186 : bool resultInternal;
8187 377 : if (MSGlobals::gUsingInternalLanes && MSNet::getInstance()->hasInternalLinks()) {
8188 0 : if (myLane->isInternal()) {
8189 0 : if (furtherIndex % 2 == 0) {
8190 0 : routeIndex -= (furtherIndex + 0) / 2;
8191 : resultInternal = false;
8192 : } else {
8193 0 : routeIndex -= (furtherIndex + 1) / 2;
8194 : resultInternal = false;
8195 : }
8196 : } else {
8197 0 : if (furtherIndex % 2 != 0) {
8198 0 : routeIndex -= (furtherIndex + 1) / 2;
8199 : resultInternal = false;
8200 : } else {
8201 0 : routeIndex -= (furtherIndex + 2) / 2;
8202 : resultInternal = true;
8203 : }
8204 : }
8205 : } else {
8206 377 : routeIndex -= furtherIndex;
8207 : resultInternal = false;
8208 : }
8209 377 : furtherIndex++;
8210 377 : if (routeIndex >= 0) {
8211 163 : if (resultInternal) {
8212 0 : const MSEdge* prevNormal = myRoute->getEdges()[routeIndex];
8213 0 : for (MSLane* cand : prevNormal->getLanes()) {
8214 0 : for (MSLink* link : cand->getLinkCont()) {
8215 0 : if (link->getLane() == current) {
8216 0 : if (link->getViaLane() != nullptr) {
8217 : return link->getViaLane();
8218 : } else {
8219 0 : return const_cast<MSLane*>(link->getLaneBefore());
8220 : }
8221 : }
8222 : }
8223 : }
8224 : } else {
8225 163 : return myRoute->getEdges()[routeIndex]->getLanes()[0];
8226 : }
8227 : }
8228 : }
8229 : return current;
8230 : }
8231 :
8232 : SUMOTime
8233 1427923358 : MSVehicle::getWaitingTimeFor(const MSLink* link) const {
8234 : // this vehicle currently has the highest priority on the allway_stop
8235 1427923358 : return link == myHaveStoppedFor ? SUMOTime_MAX : getWaitingTime();
8236 : }
8237 :
8238 :
8239 : void
8240 715 : MSVehicle::resetApproachOnReroute() {
8241 : bool diverged = false;
8242 : const ConstMSEdgeVector& route = myRoute->getEdges();
8243 715 : int ri = getRoutePosition();
8244 3122 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
8245 2407 : if (dpi.myLink != nullptr) {
8246 2404 : if (!diverged) {
8247 2161 : const MSEdge* next = route[ri + 1];
8248 2161 : if (&dpi.myLink->getLane()->getEdge() != next) {
8249 : diverged = true;
8250 : } else {
8251 2094 : if (dpi.myLink->getViaLane() == nullptr) {
8252 : ri++;
8253 : }
8254 : }
8255 : }
8256 : if (diverged) {
8257 310 : dpi.myLink->removeApproaching(this);
8258 : }
8259 : }
8260 : }
8261 715 : }
8262 :
8263 : /****************************************************************************/
|
