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