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