Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSVehicle.cpp
15 : /// @author Christian Roessel
16 : /// @author Jakob Erdmann
17 : /// @author Bjoern Hendriks
18 : /// @author Daniel Krajzewicz
19 : /// @author Thimor Bohn
20 : /// @author Friedemann Wesner
21 : /// @author Laura Bieker
22 : /// @author Clemens Honomichl
23 : /// @author Michael Behrisch
24 : /// @author Axel Wegener
25 : /// @author Christoph Sommer
26 : /// @author Leonhard Luecken
27 : /// @author Lara Codeca
28 : /// @author Mirko Barthauer
29 : /// @date Mon, 05 Mar 2001
30 : ///
31 : // Representation of a vehicle in the micro simulation
32 : /****************************************************************************/
33 : #include <config.h>
34 :
35 : #include <iostream>
36 : #include <cassert>
37 : #include <cmath>
38 : #include <cstdlib>
39 : #include <algorithm>
40 : #include <map>
41 : #include <memory>
42 : #include <utils/common/ToString.h>
43 : #include <utils/common/FileHelpers.h>
44 : #include <utils/router/DijkstraRouter.h>
45 : #include <utils/common/MsgHandler.h>
46 : #include <utils/common/RandHelper.h>
47 : #include <utils/common/StringUtils.h>
48 : #include <utils/common/StdDefs.h>
49 : #include <utils/geom/GeomHelper.h>
50 : #include <utils/iodevices/OutputDevice.h>
51 : #include <utils/xml/SUMOSAXAttributes.h>
52 : #include <utils/vehicle/SUMOVehicleParserHelper.h>
53 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
54 : #include <microsim/transportables/MSPerson.h>
55 : #include <microsim/transportables/MSPModel.h>
56 : #include <microsim/devices/MSDevice_Transportable.h>
57 : #include <microsim/devices/MSDevice_DriverState.h>
58 : #include <microsim/devices/MSDevice_Friction.h>
59 : #include <microsim/devices/MSDevice_Taxi.h>
60 : #include <microsim/devices/MSDevice_Vehroutes.h>
61 : #include <microsim/devices/MSDevice_ElecHybrid.h>
62 : #include <microsim/devices/MSDevice_GLOSA.h>
63 : #include <microsim/output/MSStopOut.h>
64 : #include <microsim/trigger/MSChargingStation.h>
65 : #include <microsim/trigger/MSOverheadWire.h>
66 : #include <microsim/traffic_lights/MSTrafficLightLogic.h>
67 : #include <microsim/traffic_lights/MSRailSignalControl.h>
68 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
69 : #include <microsim/transportables/MSTransportableControl.h>
70 : #include <microsim/devices/MSDevice_Transportable.h>
71 : #include "MSEdgeControl.h"
72 : #include "MSVehicleControl.h"
73 : #include "MSInsertionControl.h"
74 : #include "MSVehicleTransfer.h"
75 : #include "MSGlobals.h"
76 : #include "MSJunctionLogic.h"
77 : #include "MSStop.h"
78 : #include "MSStoppingPlace.h"
79 : #include "MSParkingArea.h"
80 : #include "MSMoveReminder.h"
81 : #include "MSLane.h"
82 : #include "MSJunction.h"
83 : #include "MSEdge.h"
84 : #include "MSVehicleType.h"
85 : #include "MSNet.h"
86 : #include "MSRoute.h"
87 : #include "MSLeaderInfo.h"
88 : #include "MSDriverState.h"
89 : #include "MSVehicle.h"
90 :
91 :
92 : //#define DEBUG_PLAN_MOVE
93 : //#define DEBUG_PLAN_MOVE_LEADERINFO
94 : //#define DEBUG_CHECKREWINDLINKLANES
95 : //#define DEBUG_EXEC_MOVE
96 : //#define DEBUG_FURTHER
97 : //#define DEBUG_SETFURTHER
98 : //#define DEBUG_TARGET_LANE
99 : //#define DEBUG_STOPS
100 : //#define DEBUG_BESTLANES
101 : //#define DEBUG_IGNORE_RED
102 : //#define DEBUG_ACTIONSTEPS
103 : //#define DEBUG_NEXT_TURN
104 : //#define DEBUG_TRACI
105 : //#define DEBUG_REVERSE_BIDI
106 : //#define DEBUG_EXTRAPOLATE_DEPARTPOS
107 : //#define DEBUG_REMOTECONTROL
108 : //#define DEBUG_MOVEREMINDERS
109 : //#define DEBUG_COND (getID() == "ego")
110 : //#define DEBUG_COND (true)
111 : #define DEBUG_COND (isSelected())
112 : //#define DEBUG_COND2(obj) (obj->getID() == "ego")
113 : #define DEBUG_COND2(obj) (obj->isSelected())
114 :
115 : //#define PARALLEL_STOPWATCH
116 :
117 :
118 : #define STOPPING_PLACE_OFFSET 0.5
119 :
120 : #define CRLL_LOOK_AHEAD 5
121 :
122 : #define JUNCTION_BLOCKAGE_TIME 5 // s
123 :
124 : // @todo Calibrate with real-world values / make configurable
125 : #define DIST_TO_STOPLINE_EXPECT_PRIORITY 1.0
126 :
127 : #define NUMERICAL_EPS_SPEED (0.1 * NUMERICAL_EPS * TS)
128 :
129 : // ===========================================================================
130 : // static value definitions
131 : // ===========================================================================
132 : std::vector<MSLane*> MSVehicle::myEmptyLaneVector;
133 :
134 :
135 : // ===========================================================================
136 : // method definitions
137 : // ===========================================================================
138 : /* -------------------------------------------------------------------------
139 : * methods of MSVehicle::State
140 : * ----------------------------------------------------------------------- */
141 0 : MSVehicle::State::State(const State& state) {
142 0 : myPos = state.myPos;
143 0 : mySpeed = state.mySpeed;
144 0 : myPosLat = state.myPosLat;
145 0 : myBackPos = state.myBackPos;
146 0 : myPreviousSpeed = state.myPreviousSpeed;
147 0 : myLastCoveredDist = state.myLastCoveredDist;
148 0 : }
149 :
150 :
151 : MSVehicle::State&
152 3534639 : MSVehicle::State::operator=(const State& state) {
153 3534639 : myPos = state.myPos;
154 3534639 : mySpeed = state.mySpeed;
155 3534639 : myPosLat = state.myPosLat;
156 3534639 : myBackPos = state.myBackPos;
157 3534639 : myPreviousSpeed = state.myPreviousSpeed;
158 3534639 : myLastCoveredDist = state.myLastCoveredDist;
159 3534639 : 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 8034785 : MSVehicle::State::State(double pos, double speed, double posLat, double backPos, double previousSpeed) :
175 8034785 : myPos(pos), mySpeed(speed), myPosLat(posLat), myBackPos(backPos), myPreviousSpeed(previousSpeed), myLastCoveredDist(SPEED2DIST(speed)) {}
176 :
177 :
178 :
179 : /* -------------------------------------------------------------------------
180 : * methods of MSVehicle::WaitingTimeCollector
181 : * ----------------------------------------------------------------------- */
182 4500146 : MSVehicle::WaitingTimeCollector::WaitingTimeCollector(SUMOTime memory) : myMemorySize(memory) {}
183 :
184 :
185 : SUMOTime
186 1428750 : MSVehicle::WaitingTimeCollector::cumulatedWaitingTime(SUMOTime memorySpan) const {
187 : assert(memorySpan <= myMemorySize);
188 1428750 : if (memorySpan == -1) {
189 0 : memorySpan = myMemorySize;
190 : }
191 : SUMOTime totalWaitingTime = 0;
192 5944794 : for (const auto& interval : myWaitingIntervals) {
193 4516044 : if (interval.second >= memorySpan) {
194 655960 : if (interval.first >= memorySpan) {
195 : break;
196 : } else {
197 655960 : totalWaitingTime += memorySpan - interval.first;
198 : }
199 : } else {
200 3860084 : totalWaitingTime += interval.second - interval.first;
201 : }
202 : }
203 1428750 : return totalWaitingTime;
204 : }
205 :
206 :
207 : void
208 700712306 : MSVehicle::WaitingTimeCollector::passTime(SUMOTime dt, bool waiting) {
209 : auto i = myWaitingIntervals.begin();
210 : const auto end = myWaitingIntervals.end();
211 700712306 : const bool startNewInterval = i == end || (i->first != 0);
212 1142227121 : while (i != end) {
213 443781559 : i->first += dt;
214 443781559 : if (i->first >= myMemorySize) {
215 : break;
216 : }
217 441514815 : i->second += dt;
218 : i++;
219 : }
220 :
221 : // remove intervals beyond memorySize
222 : auto d = std::distance(i, end);
223 702979050 : while (d > 0) {
224 2266744 : myWaitingIntervals.pop_back();
225 2266744 : d--;
226 : }
227 :
228 700712306 : if (!waiting) {
229 : return;
230 90933101 : } else if (!startNewInterval) {
231 87303060 : myWaitingIntervals.begin()->first = 0;
232 : } else {
233 7260082 : myWaitingIntervals.push_front(std::make_pair(0, dt));
234 : }
235 : return;
236 : }
237 :
238 :
239 : const std::string
240 2499 : MSVehicle::WaitingTimeCollector::getState() const {
241 2499 : std::ostringstream state;
242 2499 : state << myMemorySize << " " << myWaitingIntervals.size();
243 3391 : for (const auto& interval : myWaitingIntervals) {
244 1784 : state << " " << interval.first << " " << interval.second;
245 : }
246 2499 : return state.str();
247 2499 : }
248 :
249 :
250 : void
251 3356 : MSVehicle::WaitingTimeCollector::setState(const std::string& state) {
252 3356 : std::istringstream is(state);
253 : int numIntervals;
254 : SUMOTime begin, end;
255 3356 : is >> myMemorySize >> numIntervals;
256 5028 : while (numIntervals-- > 0) {
257 : is >> begin >> end;
258 1672 : myWaitingIntervals.emplace_back(begin, end);
259 : }
260 3356 : }
261 :
262 :
263 : /* -------------------------------------------------------------------------
264 : * methods of MSVehicle::Influencer::GapControlState
265 : * ----------------------------------------------------------------------- */
266 : void
267 30 : MSVehicle::Influencer::GapControlVehStateListener::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
268 : // std::cout << "GapControlVehStateListener::vehicleStateChanged() vehicle=" << vehicle->getID() << ", to=" << to << std::endl;
269 30 : switch (to) {
270 4 : case MSNet::VehicleState::STARTING_TELEPORT:
271 : case MSNet::VehicleState::ARRIVED:
272 : case MSNet::VehicleState::STARTING_PARKING: {
273 : // Vehicle left road
274 : // Look up reference vehicle in refVehMap and in case deactivate corresponding gap control
275 4 : const MSVehicle* msVeh = static_cast<const MSVehicle*>(vehicle);
276 : // std::cout << "GapControlVehStateListener::vehicleStateChanged() vehicle=" << vehicle->getID() << " left the road." << std::endl;
277 4 : if (GapControlState::refVehMap.find(msVeh) != end(GapControlState::refVehMap)) {
278 : // std::cout << "GapControlVehStateListener::deactivating ref vehicle=" << vehicle->getID() << std::endl;
279 4 : GapControlState::refVehMap[msVeh]->deactivate();
280 : }
281 : }
282 4 : break;
283 30 : default:
284 : {};
285 : // do nothing, vehicle still on road
286 : }
287 30 : }
288 :
289 : std::map<const MSVehicle*, MSVehicle::Influencer::GapControlState*>
290 : MSVehicle::Influencer::GapControlState::refVehMap;
291 :
292 : MSVehicle::Influencer::GapControlVehStateListener* MSVehicle::Influencer::GapControlState::myVehStateListener(nullptr);
293 :
294 58 : MSVehicle::Influencer::GapControlState::GapControlState() :
295 58 : tauOriginal(-1), tauCurrent(-1), tauTarget(-1), addGapCurrent(-1), addGapTarget(-1),
296 58 : remainingDuration(-1), changeRate(-1), maxDecel(-1), referenceVeh(nullptr), active(false), gapAttained(false), prevLeader(nullptr),
297 58 : lastUpdate(-1), timeHeadwayIncrement(0.0), spaceHeadwayIncrement(0.0) {}
298 :
299 :
300 58 : MSVehicle::Influencer::GapControlState::~GapControlState() {
301 58 : deactivate();
302 58 : }
303 :
304 : void
305 58 : MSVehicle::Influencer::GapControlState::init() {
306 58 : if (MSNet::hasInstance()) {
307 58 : if (myVehStateListener == nullptr) {
308 : //std::cout << "GapControlState::init()" << std::endl;
309 58 : myVehStateListener = new GapControlVehStateListener();
310 58 : MSNet::getInstance()->addVehicleStateListener(myVehStateListener);
311 : }
312 : } else {
313 0 : WRITE_ERROR("MSVehicle::Influencer::GapControlState::init(): No MSNet instance found!")
314 : }
315 58 : }
316 :
317 : void
318 35065 : MSVehicle::Influencer::GapControlState::cleanup() {
319 35065 : if (myVehStateListener != nullptr) {
320 58 : MSNet::getInstance()->removeVehicleStateListener(myVehStateListener);
321 58 : delete myVehStateListener;
322 58 : myVehStateListener = nullptr;
323 : }
324 35065 : }
325 :
326 : void
327 58 : MSVehicle::Influencer::GapControlState::activate(double tauOrig, double tauNew, double additionalGap, double dur, double rate, double decel, const MSVehicle* refVeh) {
328 58 : if (MSGlobals::gUseMesoSim) {
329 0 : WRITE_ERROR(TL("No gap control available for meso."))
330 : } else {
331 : // always deactivate control before activating (triggers clean-up of refVehMap)
332 : // std::cout << "activate gap control with refVeh=" << (refVeh==nullptr? "NULL" : refVeh->getID()) << std::endl;
333 58 : tauOriginal = tauOrig;
334 58 : tauCurrent = tauOrig;
335 58 : tauTarget = tauNew;
336 58 : addGapCurrent = 0.0;
337 58 : addGapTarget = additionalGap;
338 58 : remainingDuration = dur;
339 58 : changeRate = rate;
340 58 : maxDecel = decel;
341 58 : referenceVeh = refVeh;
342 58 : active = true;
343 58 : gapAttained = false;
344 58 : prevLeader = nullptr;
345 58 : lastUpdate = SIMSTEP - DELTA_T;
346 58 : timeHeadwayIncrement = changeRate * TS * (tauTarget - tauOriginal);
347 58 : spaceHeadwayIncrement = changeRate * TS * addGapTarget;
348 :
349 58 : if (referenceVeh != nullptr) {
350 : // Add refVeh to refVehMap
351 12 : GapControlState::refVehMap[referenceVeh] = this;
352 : }
353 : }
354 58 : }
355 :
356 : void
357 116 : MSVehicle::Influencer::GapControlState::deactivate() {
358 116 : active = false;
359 116 : if (referenceVeh != nullptr) {
360 : // Remove corresponding refVehMapEntry if appropriate
361 12 : GapControlState::refVehMap.erase(referenceVeh);
362 12 : referenceVeh = nullptr;
363 : }
364 116 : }
365 :
366 :
367 : /* -------------------------------------------------------------------------
368 : * methods of MSVehicle::Influencer
369 : * ----------------------------------------------------------------------- */
370 3448 : MSVehicle::Influencer::Influencer() :
371 : myGapControlState(nullptr),
372 3448 : myOriginalSpeed(-1),
373 3448 : myLatDist(0),
374 3448 : mySpeedAdaptationStarted(true),
375 3448 : myConsiderSafeVelocity(true),
376 3448 : myConsiderSpeedLimit(true),
377 3448 : myConsiderMaxAcceleration(true),
378 3448 : myConsiderMaxDeceleration(true),
379 3448 : myRespectJunctionPriority(true),
380 3448 : myEmergencyBrakeRedLight(true),
381 3448 : myRespectJunctionLeaderPriority(true),
382 3448 : myLastRemoteAccess(-TIME2STEPS(20)),
383 3448 : myStrategicLC(LC_NOCONFLICT),
384 3448 : myCooperativeLC(LC_NOCONFLICT),
385 3448 : mySpeedGainLC(LC_NOCONFLICT),
386 3448 : myRightDriveLC(LC_NOCONFLICT),
387 3448 : mySublaneLC(LC_NOCONFLICT),
388 3448 : myTraciLaneChangePriority(LCP_URGENT),
389 3448 : myTraCISignals(-1)
390 3448 : {}
391 :
392 :
393 10344 : MSVehicle::Influencer::~Influencer() {}
394 :
395 : void
396 58 : MSVehicle::Influencer::init() {
397 58 : GapControlState::init();
398 58 : }
399 :
400 : void
401 35065 : MSVehicle::Influencer::cleanup() {
402 35065 : GapControlState::cleanup();
403 35065 : }
404 :
405 : void
406 42894 : MSVehicle::Influencer::setSpeedTimeLine(const std::vector<std::pair<SUMOTime, double> >& speedTimeLine) {
407 42894 : mySpeedAdaptationStarted = true;
408 42894 : mySpeedTimeLine = speedTimeLine;
409 42894 : }
410 :
411 : void
412 58 : MSVehicle::Influencer::activateGapController(double originalTau, double newTimeHeadway, double newSpaceHeadway, double duration, double changeRate, double maxDecel, MSVehicle* refVeh) {
413 58 : if (myGapControlState == nullptr) {
414 58 : myGapControlState = std::make_shared<GapControlState>();
415 58 : init(); // only does things on first call
416 : }
417 58 : myGapControlState->activate(originalTau, newTimeHeadway, newSpaceHeadway, duration, changeRate, maxDecel, refVeh);
418 58 : }
419 :
420 : void
421 10 : MSVehicle::Influencer::deactivateGapController() {
422 10 : if (myGapControlState != nullptr && myGapControlState->active) {
423 10 : myGapControlState->deactivate();
424 : }
425 10 : }
426 :
427 : void
428 7538 : MSVehicle::Influencer::setLaneTimeLine(const std::vector<std::pair<SUMOTime, int> >& laneTimeLine) {
429 7538 : myLaneTimeLine = laneTimeLine;
430 7538 : }
431 :
432 :
433 : void
434 8975 : MSVehicle::Influencer::adaptLaneTimeLine(int indexShift) {
435 19133 : for (auto& item : myLaneTimeLine) {
436 10158 : item.second += indexShift;
437 : }
438 8975 : }
439 :
440 :
441 : void
442 1268 : MSVehicle::Influencer::setSublaneChange(double latDist) {
443 1268 : myLatDist = latDist;
444 1268 : }
445 :
446 : int
447 68 : MSVehicle::Influencer::getSpeedMode() const {
448 68 : return (1 * myConsiderSafeVelocity +
449 68 : 2 * myConsiderMaxAcceleration +
450 68 : 4 * myConsiderMaxDeceleration +
451 68 : 8 * myRespectJunctionPriority +
452 68 : 16 * myEmergencyBrakeRedLight +
453 68 : 32 * !myRespectJunctionLeaderPriority + // inverted!
454 68 : 64 * !myConsiderSpeedLimit // inverted!
455 68 : );
456 : }
457 :
458 :
459 : int
460 1469 : MSVehicle::Influencer::getLaneChangeMode() const {
461 1469 : return (1 * myStrategicLC +
462 1469 : 4 * myCooperativeLC +
463 1469 : 16 * mySpeedGainLC +
464 1469 : 64 * myRightDriveLC +
465 1469 : 256 * myTraciLaneChangePriority +
466 1469 : 1024 * mySublaneLC);
467 : }
468 :
469 : SUMOTime
470 60 : MSVehicle::Influencer::getLaneTimeLineDuration() {
471 : SUMOTime duration = -1;
472 180 : for (std::vector<std::pair<SUMOTime, int>>::iterator i = myLaneTimeLine.begin(); i != myLaneTimeLine.end(); ++i) {
473 120 : if (duration < 0) {
474 60 : duration = i->first;
475 : } else {
476 60 : duration -= i->first;
477 : }
478 : }
479 60 : return -duration;
480 : }
481 :
482 : SUMOTime
483 0 : MSVehicle::Influencer::getLaneTimeLineEnd() {
484 0 : if (!myLaneTimeLine.empty()) {
485 0 : return myLaneTimeLine.back().first;
486 : } else {
487 : return -1;
488 : }
489 : }
490 :
491 :
492 : double
493 985791 : MSVehicle::Influencer::influenceSpeed(SUMOTime currentTime, double speed, double vSafe, double vMin, double vMax) {
494 : // remove leading commands which are no longer valid
495 987117 : while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
496 : mySpeedTimeLine.erase(mySpeedTimeLine.begin());
497 : }
498 :
499 985791 : if (!(mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first)) {
500 : // Speed advice is active -> compute new speed according to speedTimeLine
501 53658 : if (!mySpeedAdaptationStarted) {
502 0 : mySpeedTimeLine[0].second = speed;
503 0 : mySpeedAdaptationStarted = true;
504 : }
505 53658 : currentTime += DELTA_T; // start slowing down in the step in which this command was issued (the input value of currentTime still reflects the previous step)
506 106968 : const double td = MIN2(1.0, STEPS2TIME(currentTime - mySpeedTimeLine[0].first) / MAX2(TS, STEPS2TIME(mySpeedTimeLine[1].first - mySpeedTimeLine[0].first)));
507 :
508 53658 : speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
509 53658 : if (myConsiderSafeVelocity) {
510 : speed = MIN2(speed, vSafe);
511 : }
512 53658 : if (myConsiderMaxAcceleration) {
513 : speed = MIN2(speed, vMax);
514 : }
515 53658 : if (myConsiderMaxDeceleration) {
516 : speed = MAX2(speed, vMin);
517 : }
518 : }
519 985791 : return speed;
520 : }
521 :
522 : double
523 490226 : MSVehicle::Influencer::gapControlSpeed(SUMOTime currentTime, const SUMOVehicle* veh, double speed, double vSafe, double vMin, double vMax) {
524 : #ifdef DEBUG_TRACI
525 : if DEBUG_COND2(veh) {
526 : std::cout << currentTime << " Influencer::gapControlSpeed(): speed=" << speed
527 : << ", vSafe=" << vSafe
528 : << ", vMin=" << vMin
529 : << ", vMax=" << vMax
530 : << std::endl;
531 : }
532 : #endif
533 : double gapControlSpeed = speed;
534 490226 : if (myGapControlState != nullptr && myGapControlState->active) {
535 : // Determine leader and the speed that would be chosen by the gap controller
536 7772 : const double currentSpeed = veh->getSpeed();
537 7772 : const MSVehicle* msVeh = dynamic_cast<const MSVehicle*>(veh);
538 : assert(msVeh != nullptr);
539 7772 : const double desiredTargetTimeSpacing = myGapControlState->tauTarget * currentSpeed;
540 : std::pair<const MSVehicle*, double> leaderInfo;
541 7772 : if (myGapControlState->referenceVeh == nullptr) {
542 : // No reference vehicle specified -> use current leader as reference
543 7340 : const double brakeGap = msVeh->getBrakeGap(true);
544 14680 : leaderInfo = msVeh->getLeader(MAX2(desiredTargetTimeSpacing, myGapControlState->addGapCurrent) + MAX2(brakeGap, 20.0));
545 : #ifdef DEBUG_TRACI
546 : if DEBUG_COND2(veh) {
547 : std::cout << " --- no refVeh; myGapControlState->addGapCurrent: " << myGapControlState->addGapCurrent << ", brakeGap: " << brakeGap << " in simstep: " << SIMSTEP << std::endl;
548 : }
549 : #endif
550 : } else {
551 : // Control gap wrt reference vehicle
552 : const MSVehicle* leader = myGapControlState->referenceVeh;
553 432 : double dist = msVeh->getDistanceToPosition(leader->getPositionOnLane(), leader->getLane()) - leader->getLength();
554 432 : if (dist > 100000) {
555 : // Reference vehicle was not found downstream the ego's route
556 : // Maybe, it is behind the ego vehicle
557 40 : dist = - leader->getDistanceToPosition(msVeh->getPositionOnLane(), msVeh->getLane()) - leader->getLength();
558 : #ifdef DEBUG_TRACI
559 : if DEBUG_COND2(veh) {
560 : if (dist < -100000) {
561 : // also the ego vehicle is not ahead of the reference vehicle -> no CF-relation
562 : std::cout << " Ego and reference vehicle are not in CF relation..." << std::endl;
563 : } else {
564 : std::cout << " Reference vehicle is behind ego..." << std::endl;
565 : }
566 : }
567 : #endif
568 : }
569 432 : leaderInfo = std::make_pair(leader, dist - msVeh->getVehicleType().getMinGap());
570 : }
571 7772 : const double fakeDist = MAX2(0.0, leaderInfo.second - myGapControlState->addGapCurrent);
572 : #ifdef DEBUG_TRACI
573 : if DEBUG_COND2(veh) {
574 : const double desiredCurrentSpacing = myGapControlState->tauCurrent * currentSpeed;
575 : std::cout << " Gap control active:"
576 : << " currentSpeed=" << currentSpeed
577 : << ", desiredTargetTimeSpacing=" << desiredTargetTimeSpacing
578 : << ", desiredCurrentSpacing=" << desiredCurrentSpacing
579 : << ", leader=" << (leaderInfo.first == nullptr ? "NULL" : leaderInfo.first->getID())
580 : << ", dist=" << leaderInfo.second
581 : << ", fakeDist=" << fakeDist
582 : << ",\n tauOriginal=" << myGapControlState->tauOriginal
583 : << ", tauTarget=" << myGapControlState->tauTarget
584 : << ", tauCurrent=" << myGapControlState->tauCurrent
585 : << std::endl;
586 : }
587 : #endif
588 7772 : if (leaderInfo.first != nullptr) {
589 : if (myGapControlState->prevLeader != nullptr && myGapControlState->prevLeader != leaderInfo.first) {
590 : // TODO: The leader changed. What to do?
591 : }
592 : // Remember leader
593 7772 : myGapControlState->prevLeader = leaderInfo.first;
594 :
595 : // Calculate desired following speed assuming the alternative headway time
596 7772 : MSCFModel* cfm = (MSCFModel*) & (msVeh->getVehicleType().getCarFollowModel());
597 7772 : const double origTau = cfm->getHeadwayTime();
598 7772 : cfm->setHeadwayTime(myGapControlState->tauCurrent);
599 7772 : gapControlSpeed = MIN2(gapControlSpeed,
600 7772 : cfm->followSpeed(msVeh, currentSpeed, fakeDist, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first));
601 7772 : cfm->setHeadwayTime(origTau);
602 : #ifdef DEBUG_TRACI
603 : if DEBUG_COND2(veh) {
604 : std::cout << " -> gapControlSpeed=" << gapControlSpeed;
605 : if (myGapControlState->maxDecel > 0) {
606 : std::cout << ", with maxDecel bound: " << MAX2(gapControlSpeed, currentSpeed - TS * myGapControlState->maxDecel);
607 : }
608 : std::cout << std::endl;
609 : }
610 : #endif
611 7772 : if (myGapControlState->maxDecel > 0) {
612 2568 : gapControlSpeed = MAX2(gapControlSpeed, currentSpeed - TS * myGapControlState->maxDecel);
613 : }
614 : }
615 :
616 : // Update gap controller
617 : // Check (1) if the gap control has established the desired gap,
618 : // and (2) if it has maintained active for the given duration afterwards
619 7772 : if (myGapControlState->lastUpdate < currentTime) {
620 : #ifdef DEBUG_TRACI
621 : if DEBUG_COND2(veh) {
622 : std::cout << " Updating GapControlState." << std::endl;
623 : }
624 : #endif
625 7772 : if (myGapControlState->tauCurrent == myGapControlState->tauTarget && myGapControlState->addGapCurrent == myGapControlState->addGapTarget) {
626 2990 : if (!myGapControlState->gapAttained) {
627 : // Check if the desired gap was established (add the POSITION_EPS to avoid infinite asymptotic behavior without having established the gap)
628 4176 : myGapControlState->gapAttained = leaderInfo.first == nullptr || leaderInfo.second > MAX2(desiredTargetTimeSpacing, myGapControlState->addGapTarget) - POSITION_EPS;
629 : #ifdef DEBUG_TRACI
630 : if DEBUG_COND2(veh) {
631 : if (myGapControlState->gapAttained) {
632 : std::cout << " Target gap was established." << std::endl;
633 : }
634 : }
635 : #endif
636 : } else {
637 : // Count down remaining time if desired gap was established
638 924 : myGapControlState->remainingDuration -= TS;
639 : #ifdef DEBUG_TRACI
640 : if DEBUG_COND2(veh) {
641 : std::cout << " Gap control remaining duration: " << myGapControlState->remainingDuration << std::endl;
642 : }
643 : #endif
644 924 : if (myGapControlState->remainingDuration <= 0) {
645 : #ifdef DEBUG_TRACI
646 : if DEBUG_COND2(veh) {
647 : std::cout << " Gap control duration expired, deactivating control." << std::endl;
648 : }
649 : #endif
650 : // switch off gap control
651 44 : myGapControlState->deactivate();
652 : }
653 : }
654 : } else {
655 : // Adjust current headway values
656 4782 : myGapControlState->tauCurrent = MIN2(myGapControlState->tauCurrent + myGapControlState->timeHeadwayIncrement, myGapControlState->tauTarget);
657 5160 : myGapControlState->addGapCurrent = MIN2(myGapControlState->addGapCurrent + myGapControlState->spaceHeadwayIncrement, myGapControlState->addGapTarget);
658 : }
659 : }
660 7772 : if (myConsiderSafeVelocity) {
661 : gapControlSpeed = MIN2(gapControlSpeed, vSafe);
662 : }
663 7772 : if (myConsiderMaxAcceleration) {
664 : gapControlSpeed = MIN2(gapControlSpeed, vMax);
665 : }
666 7772 : if (myConsiderMaxDeceleration) {
667 : gapControlSpeed = MAX2(gapControlSpeed, vMin);
668 : }
669 : return MIN2(speed, gapControlSpeed);
670 : } else {
671 : return speed;
672 : }
673 : }
674 :
675 : double
676 7136 : MSVehicle::Influencer::getOriginalSpeed() const {
677 7136 : return myOriginalSpeed;
678 : }
679 :
680 : void
681 495565 : MSVehicle::Influencer::setOriginalSpeed(double speed) {
682 495565 : myOriginalSpeed = speed;
683 495565 : }
684 :
685 :
686 : int
687 2814057 : MSVehicle::Influencer::influenceChangeDecision(const SUMOTime currentTime, const MSEdge& currentEdge, const int currentLaneIndex, int state) {
688 : // remove leading commands which are no longer valid
689 2814309 : while (myLaneTimeLine.size() == 1 || (myLaneTimeLine.size() > 1 && currentTime > myLaneTimeLine[1].first)) {
690 : myLaneTimeLine.erase(myLaneTimeLine.begin());
691 : }
692 : ChangeRequest changeRequest = REQUEST_NONE;
693 : // do nothing if the time line does not apply for the current time
694 2814057 : if (myLaneTimeLine.size() >= 2 && currentTime >= myLaneTimeLine[0].first) {
695 173425 : const int destinationLaneIndex = myLaneTimeLine[1].second;
696 173425 : if (destinationLaneIndex < (int)currentEdge.getLanes().size()) {
697 173133 : if (currentLaneIndex > destinationLaneIndex) {
698 : changeRequest = REQUEST_RIGHT;
699 172258 : } else if (currentLaneIndex < destinationLaneIndex) {
700 : changeRequest = REQUEST_LEFT;
701 : } else {
702 : changeRequest = REQUEST_HOLD;
703 : }
704 292 : } else if (currentEdge.getLanes().back()->getOpposite() != nullptr) { // change to opposite direction driving
705 : changeRequest = REQUEST_LEFT;
706 292 : state = state | LCA_TRACI;
707 : }
708 : }
709 : // check whether the current reason shall be canceled / overridden
710 2814057 : if ((state & LCA_WANTS_LANECHANGE_OR_STAY) != 0) {
711 : // flags for the current reason
712 : LaneChangeMode mode = LC_NEVER;
713 1594532 : if ((state & LCA_TRACI) != 0 && myLatDist != 0) {
714 : // security checks
715 2380 : if ((myTraciLaneChangePriority == LCP_ALWAYS)
716 552 : || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
717 2252 : state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
718 : }
719 : // continue sublane change manoeuvre
720 2380 : return state;
721 1592152 : } else if ((state & LCA_STRATEGIC) != 0) {
722 485254 : mode = myStrategicLC;
723 1106898 : } else if ((state & LCA_COOPERATIVE) != 0) {
724 78 : mode = myCooperativeLC;
725 1106820 : } else if ((state & LCA_SPEEDGAIN) != 0) {
726 42193 : mode = mySpeedGainLC;
727 1064627 : } else if ((state & LCA_KEEPRIGHT) != 0) {
728 6640 : mode = myRightDriveLC;
729 1057987 : } else if ((state & LCA_SUBLANE) != 0) {
730 1057985 : mode = mySublaneLC;
731 2 : } else if ((state & LCA_TRACI) != 0) {
732 : mode = LC_NEVER;
733 : } else {
734 0 : WRITE_WARNINGF(TL("Lane change model did not provide a reason for changing (state=%, time=%\n"), toString(state), time2string(currentTime));
735 : }
736 1592150 : if (mode == LC_NEVER) {
737 : // cancel all lcModel requests
738 : state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
739 42814 : state &= ~LCA_URGENT;
740 42814 : if (changeRequest == REQUEST_NONE) {
741 : // also remove all reasons except TRACI
742 42347 : state &= ~LCA_CHANGE_REASONS | LCA_TRACI;
743 : }
744 1549338 : } else if (mode == LC_NOCONFLICT && changeRequest != REQUEST_NONE) {
745 5623 : if (
746 5623 : ((state & LCA_LEFT) != 0 && changeRequest != REQUEST_LEFT) ||
747 5391 : ((state & LCA_RIGHT) != 0 && changeRequest != REQUEST_RIGHT) ||
748 4943 : ((state & LCA_STAY) != 0 && changeRequest != REQUEST_HOLD)) {
749 : // cancel conflicting lcModel request
750 : state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
751 827 : state &= ~LCA_URGENT;
752 : }
753 1543715 : } else if (mode == LC_ALWAYS) {
754 : // ignore any TraCI requests
755 : return state;
756 : }
757 : }
758 : // apply traci requests
759 2806476 : if (changeRequest == REQUEST_NONE) {
760 2638632 : return state;
761 : } else {
762 173035 : state |= LCA_TRACI;
763 : // security checks
764 173035 : if ((myTraciLaneChangePriority == LCP_ALWAYS)
765 171179 : || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
766 2250 : state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
767 : }
768 173035 : if (changeRequest != REQUEST_HOLD && myTraciLaneChangePriority != LCP_OPPORTUNISTIC) {
769 2094 : state |= LCA_URGENT;
770 : }
771 2123 : switch (changeRequest) {
772 : case REQUEST_HOLD:
773 170912 : return state | LCA_STAY;
774 1328 : case REQUEST_LEFT:
775 1328 : return state | LCA_LEFT;
776 795 : case REQUEST_RIGHT:
777 795 : return state | LCA_RIGHT;
778 : default:
779 : throw ProcessError(TL("should not happen"));
780 : }
781 : }
782 : }
783 :
784 :
785 : double
786 364 : MSVehicle::Influencer::changeRequestRemainingSeconds(const SUMOTime currentTime) const {
787 : assert(myLaneTimeLine.size() >= 2);
788 : assert(currentTime >= myLaneTimeLine[0].first);
789 364 : return STEPS2TIME(myLaneTimeLine[1].first - currentTime);
790 : }
791 :
792 :
793 : void
794 5060 : MSVehicle::Influencer::setSpeedMode(int speedMode) {
795 5060 : myConsiderSafeVelocity = ((speedMode & 1) != 0);
796 5060 : myConsiderMaxAcceleration = ((speedMode & 2) != 0);
797 5060 : myConsiderMaxDeceleration = ((speedMode & 4) != 0);
798 5060 : myRespectJunctionPriority = ((speedMode & 8) != 0);
799 5060 : myEmergencyBrakeRedLight = ((speedMode & 16) != 0);
800 5060 : myRespectJunctionLeaderPriority = ((speedMode & 32) == 0); // inverted!
801 5060 : myConsiderSpeedLimit = ((speedMode & 64) == 0); // inverted!
802 5060 : }
803 :
804 :
805 : void
806 18486 : MSVehicle::Influencer::setLaneChangeMode(int value) {
807 18486 : myStrategicLC = (LaneChangeMode)(value & (1 + 2));
808 18486 : myCooperativeLC = (LaneChangeMode)((value & (4 + 8)) >> 2);
809 18486 : mySpeedGainLC = (LaneChangeMode)((value & (16 + 32)) >> 4);
810 18486 : myRightDriveLC = (LaneChangeMode)((value & (64 + 128)) >> 6);
811 18486 : myTraciLaneChangePriority = (TraciLaneChangePriority)((value & (256 + 512)) >> 8);
812 18486 : mySublaneLC = (LaneChangeMode)((value & (1024 + 2048)) >> 10);
813 18486 : }
814 :
815 :
816 : void
817 6794 : MSVehicle::Influencer::setRemoteControlled(Position xyPos, MSLane* l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector& route, SUMOTime t) {
818 6794 : myRemoteXYPos = xyPos;
819 6794 : myRemoteLane = l;
820 6794 : myRemotePos = pos;
821 6794 : myRemotePosLat = posLat;
822 6794 : myRemoteAngle = angle;
823 6794 : myRemoteEdgeOffset = edgeOffset;
824 6794 : myRemoteRoute = route;
825 6794 : myLastRemoteAccess = t;
826 6794 : }
827 :
828 :
829 : bool
830 1014349 : MSVehicle::Influencer::isRemoteControlled() const {
831 1014349 : return myLastRemoteAccess == MSNet::getInstance()->getCurrentTimeStep();
832 : }
833 :
834 :
835 : bool
836 485249 : MSVehicle::Influencer::isRemoteAffected(SUMOTime t) const {
837 485249 : return myLastRemoteAccess >= t - TIME2STEPS(10);
838 : }
839 :
840 :
841 : void
842 490226 : MSVehicle::Influencer::updateRemoteControlRoute(MSVehicle* v) {
843 490226 : if (myRemoteRoute.size() != 0 && myRemoteRoute != v->getRoute().getEdges()) {
844 : // only replace route at this time if the vehicle is moving with the flow
845 54 : const bool isForward = v->getLane() != 0 && &v->getLane()->getEdge() == myRemoteRoute[0];
846 : #ifdef DEBUG_REMOTECONTROL
847 : std::cout << SIMSTEP << " updateRemoteControlRoute veh=" << v->getID() << " old=" << toString(v->getRoute().getEdges()) << " new=" << toString(myRemoteRoute) << " fwd=" << isForward << "\n";
848 : #endif
849 : if (isForward) {
850 8 : v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
851 8 : v->updateBestLanes();
852 : }
853 : }
854 490226 : }
855 :
856 :
857 : void
858 6774 : MSVehicle::Influencer::postProcessRemoteControl(MSVehicle* v) {
859 6774 : const bool wasOnRoad = v->isOnRoad();
860 6774 : const bool withinLane = myRemoteLane != nullptr && fabs(myRemotePosLat) < 0.5 * (myRemoteLane->getWidth() + v->getVehicleType().getWidth());
861 6774 : const bool keepLane = wasOnRoad && v->getLane() == myRemoteLane;
862 6774 : if (v->isOnRoad() && !(keepLane && withinLane)) {
863 129 : if (myRemoteLane != nullptr && &v->getLane()->getEdge() == &myRemoteLane->getEdge()) {
864 : // correct odometer which gets incremented via onRemovalFromNet->leaveLane
865 60 : v->myOdometer -= v->getLane()->getLength();
866 : }
867 129 : v->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT);
868 129 : v->getMutableLane()->removeVehicle(v, MSMoveReminder::NOTIFICATION_TELEPORT, false);
869 : }
870 6774 : if (myRemoteRoute.size() != 0 && myRemoteRoute != v->getRoute().getEdges()) {
871 : // needed for the insertion step
872 : #ifdef DEBUG_REMOTECONTROL
873 : std::cout << SIMSTEP << " postProcessRemoteControl veh=" << v->getID()
874 : << "\n oldLane=" << Named::getIDSecure(v->getLane())
875 : << " oldRoute=" << toString(v->getRoute().getEdges())
876 : << "\n newLane=" << Named::getIDSecure(myRemoteLane)
877 : << " newRoute=" << toString(myRemoteRoute)
878 : << " newRouteEdge=" << myRemoteRoute[myRemoteEdgeOffset]->getID()
879 : << "\n";
880 : #endif
881 : // clear any prior stops because they cannot apply to the new route
882 58 : const_cast<SUMOVehicleParameter&>(v->getParameter()).stops.clear();
883 116 : v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
884 : myRemoteRoute.clear();
885 : }
886 6774 : v->myCurrEdge = v->getRoute().begin() + myRemoteEdgeOffset;
887 6774 : if (myRemoteLane != nullptr && myRemotePos > myRemoteLane->getLength()) {
888 0 : myRemotePos = myRemoteLane->getLength();
889 : }
890 6774 : if (myRemoteLane != nullptr && withinLane) {
891 6628 : if (keepLane) {
892 : // TODO this handles only the case when the new vehicle is completely on the edge
893 6476 : const bool needFurtherUpdate = v->myState.myPos < v->getVehicleType().getLength() && myRemotePos >= v->getVehicleType().getLength();
894 6476 : v->myState.myPos = myRemotePos;
895 6476 : v->myState.myPosLat = myRemotePosLat;
896 6476 : if (needFurtherUpdate) {
897 4 : v->myState.myBackPos = v->updateFurtherLanes(v->myFurtherLanes, v->myFurtherLanesPosLat, std::vector<MSLane*>());
898 : }
899 : } else {
900 152 : MSMoveReminder::Notification notify = v->getDeparture() == NOT_YET_DEPARTED
901 152 : ? MSMoveReminder::NOTIFICATION_DEPARTED
902 : : MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED;
903 152 : if (!v->isOnRoad()) {
904 152 : MSVehicleTransfer::getInstance()->remove(v); // TODO may need optimization, this is linear in the number of vehicles in transfer
905 : }
906 152 : myRemoteLane->forceVehicleInsertion(v, myRemotePos, notify, myRemotePosLat);
907 152 : v->updateBestLanes();
908 : }
909 6628 : if (!wasOnRoad) {
910 53 : v->drawOutsideNetwork(false);
911 : }
912 : //std::cout << "on road network p=" << myRemoteXYPos << " a=" << myRemoteAngle << " l=" << Named::getIDSecure(myRemoteLane) << " pos=" << myRemotePos << " posLat=" << myRemotePosLat << "\n";
913 6628 : myRemoteLane->requireCollisionCheck();
914 : } else {
915 146 : if (v->getDeparture() == NOT_YET_DEPARTED) {
916 4 : v->onDepart();
917 : }
918 146 : v->drawOutsideNetwork(true);
919 : // see updateState
920 146 : double vNext = v->processTraCISpeedControl(
921 146 : v->getMaxSpeed(), v->getSpeed());
922 146 : v->setBrakingSignals(vNext);
923 146 : v->myState.myPreviousSpeed = v->getSpeed();
924 146 : v->myAcceleration = SPEED2ACCEL(vNext - v->getSpeed());
925 146 : v->myState.mySpeed = vNext;
926 146 : v->updateWaitingTime(vNext);
927 : //std::cout << "outside network p=" << myRemoteXYPos << " a=" << myRemoteAngle << " l=" << Named::getIDSecure(myRemoteLane) << "\n";
928 : }
929 : // ensure that the position is correct (i.e. when the lanePosition is ambiguous at corners)
930 6774 : v->setRemoteState(myRemoteXYPos);
931 6774 : v->setAngle(GeomHelper::fromNaviDegree(myRemoteAngle));
932 6774 : }
933 :
934 :
935 : double
936 6755 : MSVehicle::Influencer::implicitSpeedRemote(const MSVehicle* veh, double oldSpeed) {
937 6755 : if (veh->getPosition() == Position::INVALID) {
938 8 : return oldSpeed;
939 : }
940 6747 : double dist = veh->getPosition().distanceTo2D(myRemoteXYPos);
941 6747 : if (myRemoteLane != nullptr) {
942 : // if the vehicles is frequently placed on a new edge, the route may
943 : // consist only of a single edge. In this case the new edge may not be
944 : // on the route so distAlongRoute will be double::max.
945 : // In this case we still want a sensible speed value
946 6633 : const double distAlongRoute = veh->getDistanceToPosition(myRemotePos, myRemoteLane);
947 6633 : if (distAlongRoute != std::numeric_limits<double>::max()) {
948 : dist = distAlongRoute;
949 : }
950 : }
951 : //std::cout << SIMTIME << " veh=" << veh->getID() << " oldPos=" << veh->getPosition() << " traciPos=" << myRemoteXYPos << " dist=" << dist << "\n";
952 6747 : const double minSpeed = myConsiderMaxDeceleration ?
953 4039 : veh->getCarFollowModel().minNextSpeedEmergency(oldSpeed, veh) : 0;
954 6747 : const double maxSpeed = (myRemoteLane != nullptr
955 6747 : ? myRemoteLane->getVehicleMaxSpeed(veh)
956 114 : : (veh->getLane() != nullptr
957 114 : ? veh->getLane()->getVehicleMaxSpeed(veh)
958 4 : : veh->getMaxSpeed()));
959 6747 : return MIN2(maxSpeed, MAX2(minSpeed, DIST2SPEED(dist)));
960 : }
961 :
962 :
963 : double
964 6617 : MSVehicle::Influencer::implicitDeltaPosRemote(const MSVehicle* veh) {
965 : double dist = 0;
966 6617 : if (myRemoteLane == nullptr) {
967 5 : dist = veh->getPosition().distanceTo2D(myRemoteXYPos);
968 : } else {
969 : // if the vehicles is frequently placed on a new edge, the route may
970 : // consist only of a single edge. In this case the new edge may not be
971 : // on the route so getDistanceToPosition will return double::max.
972 : // In this case we would rather not move the vehicle in executeMove
973 : // (updateState) as it would result in emergency braking
974 6612 : dist = veh->getDistanceToPosition(myRemotePos, myRemoteLane);
975 : }
976 6617 : if (dist == std::numeric_limits<double>::max()) {
977 : return 0;
978 : } else {
979 6417 : if (DIST2SPEED(dist) > veh->getMaxSpeed() * 1.1) {
980 42 : WRITE_WARNINGF(TL("Vehicle '%' moved by TraCI from % to % (dist %) with implied speed of % (exceeding maximum speed %). time=%."),
981 : veh->getID(), veh->getPosition(), myRemoteXYPos, dist, DIST2SPEED(dist), veh->getMaxSpeed(), time2string(SIMSTEP));
982 : // some sanity check here
983 14 : dist = MIN2(dist, SPEED2DIST(veh->getMaxSpeed() * 2));
984 : }
985 6417 : return dist;
986 : }
987 : }
988 :
989 :
990 : /* -------------------------------------------------------------------------
991 : * MSVehicle-methods
992 : * ----------------------------------------------------------------------- */
993 4500146 : MSVehicle::MSVehicle(SUMOVehicleParameter* pars, ConstMSRoutePtr route,
994 4500146 : MSVehicleType* type, const double speedFactor) :
995 : MSBaseVehicle(pars, route, type, speedFactor),
996 4500146 : myWaitingTime(0),
997 4500146 : myWaitingTimeCollector(),
998 4500146 : myTimeLoss(0),
999 4500146 : myState(0, 0, 0, 0, 0),
1000 4500146 : myDriverState(nullptr),
1001 4500146 : myActionStep(true),
1002 4500146 : myLastActionTime(0),
1003 4500146 : myLane(nullptr),
1004 4500146 : myLaneChangeModel(nullptr),
1005 4500146 : myLastBestLanesEdge(nullptr),
1006 4500146 : myLastBestLanesInternalLane(nullptr),
1007 4500146 : myAcceleration(0),
1008 : myNextTurn(0., nullptr),
1009 4500146 : mySignals(0),
1010 4500146 : myAmOnNet(false),
1011 4500146 : myAmIdling(false),
1012 4500146 : myHaveToWaitOnNextLink(false),
1013 4500146 : myAngle(0),
1014 4500146 : myStopDist(std::numeric_limits<double>::max()),
1015 4500146 : myStopSpeed(std::numeric_limits<double>::max()),
1016 4500146 : myCollisionImmunity(-1),
1017 4500146 : myCachedPosition(Position::INVALID),
1018 4500146 : myJunctionEntryTime(SUMOTime_MAX),
1019 4500146 : myJunctionEntryTimeNeverYield(SUMOTime_MAX),
1020 4500146 : myJunctionConflictEntryTime(SUMOTime_MAX),
1021 4500146 : myTimeSinceStartup(TIME2STEPS(3600 * 24)),
1022 4500146 : myHaveStoppedFor(nullptr),
1023 13500438 : myInfluencer(nullptr) {
1024 4500146 : myCFVariables = type->getCarFollowModel().createVehicleVariables();
1025 4500146 : myNextDriveItem = myLFLinkLanes.begin();
1026 4500146 : }
1027 :
1028 :
1029 8358604 : MSVehicle::~MSVehicle() {
1030 4500065 : cleanupParkingReservation();
1031 4500065 : cleanupFurtherLanes();
1032 4500065 : delete myLaneChangeModel;
1033 4500065 : if (myType->isVehicleSpecific()) {
1034 303 : MSNet::getInstance()->getVehicleControl().removeVType(myType);
1035 : }
1036 4500065 : delete myInfluencer;
1037 4500065 : delete myCFVariables;
1038 12858669 : }
1039 :
1040 :
1041 : void
1042 4500627 : MSVehicle::cleanupFurtherLanes() {
1043 4503131 : for (MSLane* further : myFurtherLanes) {
1044 2504 : further->resetPartialOccupation(this);
1045 2504 : if (further->getBidiLane() != nullptr
1046 2504 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
1047 0 : further->getBidiLane()->resetPartialOccupation(this);
1048 : }
1049 : }
1050 4500627 : if (myLaneChangeModel != nullptr) {
1051 4500593 : removeApproachingInformation(myLFLinkLanes);
1052 4500593 : myLaneChangeModel->cleanupShadowLane();
1053 4500593 : myLaneChangeModel->cleanupTargetLane();
1054 : // still needed when calling resetPartialOccupation (getShadowLane) and when removing
1055 : // approach information from parallel links
1056 : }
1057 : myFurtherLanes.clear();
1058 : myFurtherLanesPosLat.clear();
1059 4500627 : }
1060 :
1061 :
1062 : void
1063 3361773 : MSVehicle::onRemovalFromNet(const MSMoveReminder::Notification reason) {
1064 : #ifdef DEBUG_ACTIONSTEPS
1065 : if (DEBUG_COND) {
1066 : std::cout << SIMTIME << " Removing vehicle '" << getID() << "' (reason: " << toString(reason) << ")" << std::endl;
1067 : }
1068 : #endif
1069 3361773 : MSVehicleTransfer::getInstance()->remove(this);
1070 3361773 : removeApproachingInformation(myLFLinkLanes);
1071 3361773 : leaveLane(reason);
1072 3361773 : if (reason == MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION) {
1073 562 : cleanupFurtherLanes();
1074 : }
1075 3361773 : }
1076 :
1077 :
1078 : void
1079 4500146 : MSVehicle::initDevices() {
1080 4500146 : MSBaseVehicle::initDevices();
1081 4500134 : myLaneChangeModel = MSAbstractLaneChangeModel::build(myType->getLaneChangeModel(), *this);
1082 4500112 : myDriverState = static_cast<MSDevice_DriverState*>(getDevice(typeid(MSDevice_DriverState)));
1083 4500112 : myFrictionDevice = static_cast<MSDevice_Friction*>(getDevice(typeid(MSDevice_Friction)));
1084 4500112 : }
1085 :
1086 :
1087 : // ------------ interaction with the route
1088 : bool
1089 2230437971 : MSVehicle::hasValidRouteStart(std::string& msg) {
1090 : // note: not a const method because getDepartLane may call updateBestLanes
1091 2230437971 : if (!(*myCurrEdge)->isTazConnector()) {
1092 2230109613 : if (myParameter->departLaneProcedure == DepartLaneDefinition::GIVEN
1093 2230109613 : || (myParameter->departLaneProcedure == DepartLaneDefinition::DEFAULT && MSEdge::getDefaultDepartLaneDefinition() == DepartLaneDefinition::GIVEN)) {
1094 54026924 : if ((*myCurrEdge)->getDepartLane(*this) == nullptr) {
1095 132 : msg = "Invalid departlane definition for vehicle '" + getID() + "'.";
1096 66 : if (myParameter->departLane >= (int)(*myCurrEdge)->getLanes().size()) {
1097 11 : myRouteValidity |= ROUTE_START_INVALID_LANE;
1098 : } else {
1099 55 : myRouteValidity |= ROUTE_START_INVALID_PERMISSIONS;
1100 : }
1101 66 : return false;
1102 : }
1103 : } else {
1104 2176082689 : if ((*myCurrEdge)->allowedLanes(getVClass(), ignoreTransientPermissions()) == nullptr) {
1105 144 : msg = "Vehicle '" + getID() + "' is not allowed to depart on any lane of edge '" + (*myCurrEdge)->getID() + "'.";
1106 72 : myRouteValidity |= ROUTE_START_INVALID_PERMISSIONS;
1107 72 : return false;
1108 : }
1109 : }
1110 2230109475 : if (myParameter->departSpeedProcedure == DepartSpeedDefinition::GIVEN && myParameter->departSpeed > myType->getMaxSpeed() + SPEED_EPS) {
1111 38 : msg = "Departure speed for vehicle '" + getID() + "' is too high for the vehicle type '" + myType->getID() + "'.";
1112 19 : myRouteValidity |= ROUTE_START_INVALID_LANE;
1113 19 : return false;
1114 : }
1115 : }
1116 2230437814 : myRouteValidity &= ~(ROUTE_START_INVALID_LANE | ROUTE_START_INVALID_PERMISSIONS);
1117 2230437814 : return true;
1118 : }
1119 :
1120 :
1121 : bool
1122 713813000 : MSVehicle::hasArrived() const {
1123 713813000 : return hasArrivedInternal(false);
1124 : }
1125 :
1126 :
1127 : bool
1128 1437040157 : MSVehicle::hasArrivedInternal(bool oppositeTransformed) const {
1129 2332869264 : return ((myCurrEdge == myRoute->end() - 1 || (myParameter->arrivalEdge >= 0 && getRoutePosition() >= myParameter->arrivalEdge))
1130 541260323 : && (myStops.empty() || myStops.front().edge != myCurrEdge || myStops.front().getSpeed() > 0)
1131 1013308058 : && ((myLaneChangeModel->isOpposite() && !oppositeTransformed) ? myLane->getLength() - myState.myPos : myState.myPos) > MIN2(myLane->getLength(), myArrivalPos) - POSITION_EPS
1132 1448417921 : && !isRemoteControlled());
1133 : }
1134 :
1135 :
1136 : bool
1137 1566347 : MSVehicle::replaceRoute(ConstMSRoutePtr newRoute, const std::string& info, bool onInit, int offset, bool addRouteStops, bool removeStops, std::string* msgReturn) {
1138 3132694 : if (MSBaseVehicle::replaceRoute(newRoute, info, onInit, offset, addRouteStops, removeStops, msgReturn)) {
1139 : // update best lanes (after stops were added)
1140 1566330 : myLastBestLanesEdge = nullptr;
1141 1566330 : myLastBestLanesInternalLane = nullptr;
1142 1566330 : updateBestLanes(true, onInit ? (*myCurrEdge)->getLanes().front() : 0);
1143 : assert(!removeStops || haveValidStopEdges());
1144 1566330 : if (myStops.size() == 0) {
1145 1521940 : myStopDist = std::numeric_limits<double>::max();
1146 : }
1147 1566330 : return true;
1148 : }
1149 : return false;
1150 : }
1151 :
1152 :
1153 : // ------------ Interaction with move reminders
1154 : void
1155 700861806 : MSVehicle::workOnMoveReminders(double oldPos, double newPos, double newSpeed) {
1156 : // This erasure-idiom works for all stl-sequence-containers
1157 : // See Meyers: Effective STL, Item 9
1158 1852894079 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
1159 : // XXX: calling notifyMove with newSpeed seems not the best choice. For the ballistic update, the average speed is calculated and used
1160 : // although a higher order quadrature-formula might be more adequate.
1161 : // For the euler case (where the speed is considered constant for each time step) it is conceivable that
1162 : // the current calculations may lead to systematic errors for large time steps (compared to reality). Refs. #2579
1163 2304064550 : if (!rem->first->notifyMove(*this, oldPos + rem->second, newPos + rem->second, MAX2(0., newSpeed))) {
1164 : #ifdef _DEBUG
1165 : if (myTraceMoveReminders) {
1166 : traceMoveReminder("notifyMove", rem->first, rem->second, false);
1167 : }
1168 : #endif
1169 : rem = myMoveReminders.erase(rem);
1170 : } else {
1171 : #ifdef _DEBUG
1172 : if (myTraceMoveReminders) {
1173 : traceMoveReminder("notifyMove", rem->first, rem->second, true);
1174 : }
1175 : #endif
1176 : ++rem;
1177 : }
1178 : }
1179 700861804 : if (myEnergyParams != nullptr) {
1180 : // TODO make the vehicle energy params a derived class which is a move reminder
1181 139797599 : myEnergyParams->setDynamicValues(isStopped() ? getNextStop().duration : -1, isParking(), getWaitingTime(), getAngle());
1182 : }
1183 700861804 : }
1184 :
1185 :
1186 : void
1187 66787 : MSVehicle::workOnIdleReminders() {
1188 66787 : updateWaitingTime(0.); // cf issue 2233
1189 :
1190 : // vehicle move reminders
1191 79799 : for (const auto& rem : myMoveReminders) {
1192 13012 : rem.first->notifyIdle(*this);
1193 : }
1194 :
1195 : // lane move reminders - for aggregated values
1196 166692 : for (MSMoveReminder* rem : getLane()->getMoveReminders()) {
1197 99905 : rem->notifyIdle(*this);
1198 : }
1199 66787 : }
1200 :
1201 : // XXX: consider renaming...
1202 : void
1203 19483389 : MSVehicle::adaptLaneEntering2MoveReminder(const MSLane& enteredLane) {
1204 : // save the old work reminders, patching the position information
1205 : // add the information about the new offset to the old lane reminders
1206 19483389 : const double oldLaneLength = myLane->getLength();
1207 55199029 : for (auto& rem : myMoveReminders) {
1208 35715640 : rem.second += oldLaneLength;
1209 : #ifdef _DEBUG
1210 : // if (rem->first==0) std::cout << "Null reminder (?!)" << std::endl;
1211 : // std::cout << "Adapted MoveReminder on lane " << ((rem->first->getLane()==0) ? "NULL" : rem->first->getLane()->getID()) <<" position to " << rem->second << std::endl;
1212 : if (myTraceMoveReminders) {
1213 : traceMoveReminder("adaptedPos", rem.first, rem.second, true);
1214 : }
1215 : #endif
1216 : }
1217 32719186 : for (MSMoveReminder* const rem : enteredLane.getMoveReminders()) {
1218 13235797 : addReminder(rem);
1219 : }
1220 19483389 : }
1221 :
1222 :
1223 : // ------------ Other getter methods
1224 : double
1225 163990770 : MSVehicle::getSlope() const {
1226 163990770 : if (isParking() && getStops().begin()->parkingarea != nullptr) {
1227 3881 : return getStops().begin()->parkingarea->getVehicleSlope(*this);
1228 : }
1229 163986889 : if (myLane == nullptr) {
1230 : return 0;
1231 : }
1232 163986889 : const double posLat = myState.myPosLat; // @todo get rid of the '-'
1233 163986889 : Position p1 = getPosition();
1234 163986889 : Position p2 = getBackPosition();
1235 : if (p2 == Position::INVALID) {
1236 : // Handle special case of vehicle's back reaching out of the network
1237 6 : if (myFurtherLanes.size() > 0) {
1238 6 : p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1239 : if (p2 == Position::INVALID) {
1240 : // unsuitable lane geometry
1241 0 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1242 : }
1243 : } else {
1244 0 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1245 : }
1246 : }
1247 163986889 : return (p1 != p2 ? RAD2DEG(p2.slopeTo2D(p1)) : myLane->getShape().slopeDegreeAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1248 : }
1249 :
1250 :
1251 : Position
1252 933202207 : MSVehicle::getPosition(const double offset) const {
1253 933202207 : if (myLane == nullptr) {
1254 : // when called in the context of GUI-Drawing, the simulation step is already incremented
1255 146 : if (myInfluencer != nullptr && myInfluencer->isRemoteAffected(MSNet::getInstance()->getCurrentTimeStep())) {
1256 40 : return myCachedPosition;
1257 : } else {
1258 106 : return Position::INVALID;
1259 : }
1260 : }
1261 933202061 : if (isParking()) {
1262 2574295 : if (myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() > getNextStopParameter()->started) {
1263 121 : return myCachedPosition;
1264 : }
1265 2574174 : if (myStops.begin()->parkingarea != nullptr) {
1266 22510 : return myStops.begin()->parkingarea->getVehiclePosition(*this);
1267 : } else {
1268 : // position beside the road
1269 2551664 : PositionVector shp = myLane->getEdge().getLanes()[0]->getShape();
1270 5103208 : shp.move2side(SUMO_const_laneWidth * (MSGlobals::gLefthand ? -1 : 1));
1271 2551664 : return shp.positionAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane() + offset));
1272 2551664 : }
1273 : }
1274 930627766 : const bool changingLanes = myLaneChangeModel->isChangingLanes();
1275 1851107906 : const double posLat = (MSGlobals::gLefthand ? 1 : -1) * getLateralPositionOnLane();
1276 930627766 : if (offset == 0. && !changingLanes) {
1277 : if (myCachedPosition == Position::INVALID) {
1278 705378566 : myCachedPosition = validatePosition(myLane->geometryPositionAtOffset(myState.myPos, posLat));
1279 705378566 : if (MSNet::getInstance()->hasElevation() && MSGlobals::gSublane) {
1280 60962 : interpolateLateralZ(myCachedPosition, myState.myPos, posLat);
1281 : }
1282 : }
1283 923905186 : return myCachedPosition;
1284 : }
1285 6722580 : Position result = validatePosition(myLane->geometryPositionAtOffset(getPositionOnLane() + offset, posLat), offset);
1286 6722580 : interpolateLateralZ(result, getPositionOnLane() + offset, posLat);
1287 6722580 : return result;
1288 : }
1289 :
1290 :
1291 : void
1292 7067420 : MSVehicle::interpolateLateralZ(Position& pos, double offset, double posLat) const {
1293 7067420 : const MSLane* shadow = myLaneChangeModel->getShadowLane();
1294 7067420 : if (shadow != nullptr && pos != Position::INVALID) {
1295 : // ignore negative offset
1296 : const Position shadowPos = shadow->geometryPositionAtOffset(MAX2(0.0, offset));
1297 60280 : if (shadowPos != Position::INVALID && pos.z() != shadowPos.z()) {
1298 325 : const double centerDist = (myLane->getWidth() + shadow->getWidth()) * 0.5;
1299 325 : double relOffset = fabs(posLat) / centerDist;
1300 325 : double newZ = (1 - relOffset) * pos.z() + relOffset * shadowPos.z();
1301 : pos.setz(newZ);
1302 : }
1303 : }
1304 7067420 : }
1305 :
1306 :
1307 : double
1308 1214028 : MSVehicle::getDistanceToLeaveJunction() const {
1309 1214028 : double result = getLength() - getPositionOnLane();
1310 1214028 : if (myLane->isNormal()) {
1311 : return MAX2(0.0, result);
1312 : }
1313 4548 : const MSLane* lane = myLane;
1314 9096 : while (lane->isInternal()) {
1315 4548 : result += lane->getLength();
1316 4548 : lane = lane->getCanonicalSuccessorLane();
1317 : }
1318 : return result;
1319 : }
1320 :
1321 :
1322 : Position
1323 103615 : MSVehicle::getPositionAlongBestLanes(double offset) const {
1324 : assert(MSGlobals::gUsingInternalLanes);
1325 103615 : if (!isOnRoad()) {
1326 0 : return Position::INVALID;
1327 : }
1328 103615 : const std::vector<MSLane*>& bestLanes = getBestLanesContinuation();
1329 : auto nextBestLane = bestLanes.begin();
1330 103615 : const bool opposite = myLaneChangeModel->isOpposite();
1331 103615 : double pos = opposite ? myLane->getLength() - myState.myPos : myState.myPos;
1332 103615 : const MSLane* lane = opposite ? myLane->getParallelOpposite() : getLane();
1333 : assert(lane != 0);
1334 : bool success = true;
1335 :
1336 305940 : while (offset > 0) {
1337 : // take into account lengths along internal lanes
1338 309310 : while (lane->isInternal() && offset > 0) {
1339 106985 : if (offset > lane->getLength() - pos) {
1340 3561 : offset -= lane->getLength() - pos;
1341 3561 : lane = lane->getLinkCont()[0]->getViaLaneOrLane();
1342 : pos = 0.;
1343 3561 : if (lane == nullptr) {
1344 : success = false;
1345 : offset = 0.;
1346 : }
1347 : } else {
1348 103424 : pos += offset;
1349 : offset = 0;
1350 : }
1351 : }
1352 : // set nextBestLane to next non-internal lane
1353 207454 : while (nextBestLane != bestLanes.end() && *nextBestLane == nullptr) {
1354 : ++nextBestLane;
1355 : }
1356 202325 : if (offset > 0) {
1357 : assert(!lane->isInternal());
1358 : assert(lane == *nextBestLane);
1359 98901 : if (offset > lane->getLength() - pos) {
1360 98718 : offset -= lane->getLength() - pos;
1361 : ++nextBestLane;
1362 : assert(nextBestLane == bestLanes.end() || *nextBestLane != 0);
1363 98718 : if (nextBestLane == bestLanes.end()) {
1364 : success = false;
1365 : offset = 0.;
1366 : } else {
1367 98718 : const MSLink* link = lane->getLinkTo(*nextBestLane);
1368 : assert(link != nullptr);
1369 : lane = link->getViaLaneOrLane();
1370 : pos = 0.;
1371 : }
1372 : } else {
1373 183 : pos += offset;
1374 : offset = 0;
1375 : }
1376 : }
1377 :
1378 : }
1379 :
1380 103615 : if (success) {
1381 103615 : return lane->geometryPositionAtOffset(pos, -getLateralPositionOnLane());
1382 : } else {
1383 0 : return Position::INVALID;
1384 : }
1385 : }
1386 :
1387 :
1388 : double
1389 712215 : MSVehicle::getMaxSpeedOnLane() const {
1390 712215 : if (myLane != nullptr) {
1391 712215 : return myLane->getVehicleMaxSpeed(this);
1392 : }
1393 0 : return myType->getMaxSpeed();
1394 : }
1395 :
1396 :
1397 : Position
1398 712101146 : MSVehicle::validatePosition(Position result, double offset) const {
1399 : int furtherIndex = 0;
1400 712101146 : double lastLength = getPositionOnLane();
1401 712101146 : while (result == Position::INVALID) {
1402 278541 : if (furtherIndex >= (int)myFurtherLanes.size()) {
1403 : //WRITE_WARNINGF(TL("Could not compute position for vehicle '%', time=%."), getID(), time2string(MSNet::getInstance()->getCurrentTimeStep()));
1404 : break;
1405 : }
1406 : //std::cout << SIMTIME << " veh=" << getID() << " lane=" << myLane->getID() << " pos=" << getPositionOnLane() << " posLat=" << getLateralPositionOnLane() << " offset=" << offset << " result=" << result << " i=" << furtherIndex << " further=" << myFurtherLanes.size() << "\n";
1407 198948 : MSLane* further = myFurtherLanes[furtherIndex];
1408 198948 : offset += lastLength;
1409 198948 : result = further->geometryPositionAtOffset(further->getLength() + offset, -getLateralPositionOnLane());
1410 : lastLength = further->getLength();
1411 198948 : furtherIndex++;
1412 : //std::cout << SIMTIME << " newResult=" << result << "\n";
1413 : }
1414 712101146 : return result;
1415 : }
1416 :
1417 :
1418 : ConstMSEdgeVector::const_iterator
1419 280462 : MSVehicle::getRerouteOrigin() const {
1420 : // too close to the next junction, so avoid an emergency brake here
1421 280462 : if (myLane != nullptr && (myCurrEdge + 1) != myRoute->end() && !isRailway(getVClass())) {
1422 218038 : if (myLane->isInternal()) {
1423 : return myCurrEdge + 1;
1424 : }
1425 211177 : if (myState.myPos > myLane->getLength() - getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.)) {
1426 : return myCurrEdge + 1;
1427 : }
1428 208901 : if (myLane->getEdge().hasChangeProhibitions(getVClass(), myLane->getIndex())) {
1429 : return myCurrEdge + 1;
1430 : }
1431 : }
1432 271213 : return myCurrEdge;
1433 : }
1434 :
1435 : void
1436 5256152 : MSVehicle::setAngle(double angle, bool straightenFurther) {
1437 : #ifdef DEBUG_FURTHER
1438 : if (DEBUG_COND) {
1439 : std::cout << SIMTIME << " veh '" << getID() << " setAngle(" << angle << ") straightenFurther=" << straightenFurther << std::endl;
1440 : }
1441 : #endif
1442 5256152 : myAngle = angle;
1443 5256152 : MSLane* next = myLane;
1444 5256152 : if (straightenFurther && myFurtherLanesPosLat.size() > 0) {
1445 200251 : for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
1446 102811 : MSLane* further = myFurtherLanes[i];
1447 102811 : const MSLink* link = further->getLinkTo(next);
1448 102811 : if (link != nullptr) {
1449 102371 : myFurtherLanesPosLat[i] = getLateralPositionOnLane() - link->getLateralShift();
1450 : next = further;
1451 : } else {
1452 : break;
1453 : }
1454 : }
1455 : }
1456 5256152 : }
1457 :
1458 :
1459 : void
1460 451439 : MSVehicle::setActionStepLength(double actionStepLength, bool resetOffset) {
1461 451439 : SUMOTime actionStepLengthMillisecs = SUMOVehicleParserHelper::processActionStepLength(actionStepLength);
1462 : SUMOTime previousActionStepLength = getActionStepLength();
1463 : const bool newActionStepLength = actionStepLengthMillisecs != previousActionStepLength;
1464 451439 : if (newActionStepLength) {
1465 7 : getSingularType().setActionStepLength(actionStepLengthMillisecs, resetOffset);
1466 7 : if (!resetOffset) {
1467 1 : updateActionOffset(previousActionStepLength, actionStepLengthMillisecs);
1468 : }
1469 : }
1470 451433 : if (resetOffset) {
1471 6 : resetActionOffset();
1472 : }
1473 451439 : }
1474 :
1475 :
1476 : bool
1477 302119914 : MSVehicle::congested() const {
1478 302119914 : return myState.mySpeed < (60.0 / 3.6) || myLane->getSpeedLimit() < (60.1 / 3.6);
1479 : }
1480 :
1481 :
1482 : double
1483 708086006 : MSVehicle::computeAngle() const {
1484 : Position p1;
1485 708086006 : const double posLat = -myState.myPosLat; // @todo get rid of the '-'
1486 708086006 : const double lefthandSign = (MSGlobals::gLefthand ? -1 : 1);
1487 :
1488 : // if parking manoeuvre is happening then rotate vehicle on each step
1489 708086006 : if (MSGlobals::gModelParkingManoeuver && !manoeuvreIsComplete()) {
1490 450 : return getAngle() + myManoeuvre.getGUIIncrement();
1491 : }
1492 :
1493 708085556 : if (isParking()) {
1494 29019 : if (myStops.begin()->parkingarea != nullptr) {
1495 15769 : return myStops.begin()->parkingarea->getVehicleAngle(*this);
1496 : } else {
1497 13250 : return myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane()));
1498 : }
1499 : }
1500 708056537 : if (myLaneChangeModel->isChangingLanes()) {
1501 : // cannot use getPosition() because it already includes the offset to the side and thus messes up the angle
1502 1147029 : p1 = myLane->geometryPositionAtOffset(myState.myPos, lefthandSign * posLat);
1503 9 : if (p1 == Position::INVALID && myLane->getShape().length2D() == 0. && myLane->isInternal()) {
1504 : // workaround: extrapolate the preceding lane shape
1505 9 : MSLane* predecessorLane = myLane->getCanonicalPredecessorLane();
1506 9 : p1 = predecessorLane->geometryPositionAtOffset(predecessorLane->getLength() + myState.myPos, lefthandSign * posLat);
1507 : }
1508 : } else {
1509 706909508 : p1 = getPosition();
1510 : }
1511 :
1512 : Position p2;
1513 708056537 : if (getVehicleType().getParameter().locomotiveLength > 0) {
1514 : // articulated vehicle should use the heading of the first part
1515 1831317 : const double locoLength = MIN2(getVehicleType().getParameter().locomotiveLength, getLength());
1516 1831317 : p2 = getPosition(-locoLength);
1517 : } else {
1518 706225220 : p2 = getBackPosition();
1519 : }
1520 : if (p2 == Position::INVALID) {
1521 : // Handle special case of vehicle's back reaching out of the network
1522 1664 : if (myFurtherLanes.size() > 0) {
1523 117 : p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1524 : if (p2 == Position::INVALID) {
1525 : // unsuitable lane geometry
1526 72 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1527 : }
1528 : } else {
1529 1547 : p2 = myLane->geometryPositionAtOffset(0, posLat);
1530 : }
1531 : }
1532 : double result = (p1 != p2 ? p2.angleTo2D(p1) :
1533 106451 : myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1534 :
1535 708056537 : result += lefthandSign * myLaneChangeModel->calcAngleOffset();
1536 :
1537 : #ifdef DEBUG_FURTHER
1538 : if (DEBUG_COND) {
1539 : std::cout << SIMTIME << " computeAngle veh=" << getID() << " p1=" << p1 << " p2=" << p2 << " angle=" << RAD2DEG(result) << " naviDegree=" << GeomHelper::naviDegree(result) << "\n";
1540 : }
1541 : #endif
1542 708056537 : return result;
1543 : }
1544 :
1545 :
1546 : const Position
1547 877087607 : MSVehicle::getBackPosition() const {
1548 877087607 : const double posLat = MSGlobals::gLefthand ? myState.myPosLat : -myState.myPosLat;
1549 : Position result;
1550 877087607 : if (myState.myPos >= myType->getLength()) {
1551 : // vehicle is fully on the new lane
1552 859557893 : result = myLane->geometryPositionAtOffset(myState.myPos - myType->getLength(), posLat);
1553 : } else {
1554 17529714 : if (myLaneChangeModel->isChangingLanes() && myFurtherLanes.size() > 0 && myLaneChangeModel->getShadowLane(myFurtherLanes.back()) == nullptr) {
1555 : // special case where the target lane has no predecessor
1556 : #ifdef DEBUG_FURTHER
1557 : if (DEBUG_COND) {
1558 : std::cout << " getBackPosition veh=" << getID() << " specialCase using myLane=" << myLane->getID() << " pos=0 posLat=" << myState.myPosLat << " result=" << myLane->geometryPositionAtOffset(0, posLat) << "\n";
1559 : }
1560 : #endif
1561 1909 : result = myLane->geometryPositionAtOffset(0, posLat);
1562 : } else {
1563 : #ifdef DEBUG_FURTHER
1564 : if (DEBUG_COND) {
1565 : std::cout << " getBackPosition veh=" << getID() << " myLane=" << myLane->getID() << " further=" << toString(myFurtherLanes) << " myFurtherLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
1566 : }
1567 : #endif
1568 17527805 : if (myFurtherLanes.size() > 0 && !myLaneChangeModel->isChangingLanes()) {
1569 : // truncate to 0 if vehicle starts on an edge that is shorter than its length
1570 17132615 : const double backPos = MAX2(0.0, getBackPositionOnLane(myFurtherLanes.back()));
1571 33966462 : result = myFurtherLanes.back()->geometryPositionAtOffset(backPos, -myFurtherLanesPosLat.back() * (MSGlobals::gLefthand ? -1 : 1));
1572 : } else {
1573 395190 : result = myLane->geometryPositionAtOffset(0, posLat);
1574 : }
1575 : }
1576 : }
1577 877087607 : if (MSNet::getInstance()->hasElevation() && MSGlobals::gSublane) {
1578 283878 : interpolateLateralZ(result, myState.myPos - myType->getLength(), posLat);
1579 : }
1580 877087607 : return result;
1581 : }
1582 :
1583 :
1584 : bool
1585 447249 : MSVehicle::willStop() const {
1586 447249 : return !isStopped() && !myStops.empty() && myLane != nullptr && &myStops.front().lane->getEdge() == &myLane->getEdge();
1587 : }
1588 :
1589 : bool
1590 371735008 : MSVehicle::isStoppedOnLane() const {
1591 371735008 : return isStopped() && myStops.front().lane == myLane;
1592 : }
1593 :
1594 : bool
1595 31107042 : MSVehicle::keepStopping(bool afterProcessing) const {
1596 31107042 : if (isStopped()) {
1597 : // when coming out of vehicleTransfer we must shift the time forward
1598 37136910 : return (myStops.front().duration - (afterProcessing ? DELTA_T : 0) > 0 || isStoppedTriggered() || myStops.front().pars.collision
1599 30836803 : || myStops.front().pars.breakDown || (myStops.front().getSpeed() > 0
1600 35771 : && (myState.myPos < MIN2(myStops.front().pars.endPos, myStops.front().lane->getLength() - POSITION_EPS))
1601 29918 : && (myStops.front().pars.parking == ParkingType::ONROAD || getSpeed() >= SUMO_const_haltingSpeed)));
1602 : } else {
1603 : return false;
1604 : }
1605 : }
1606 :
1607 :
1608 : SUMOTime
1609 15529 : MSVehicle::remainingStopDuration() const {
1610 15529 : if (isStopped()) {
1611 15529 : return myStops.front().duration;
1612 : }
1613 : return 0;
1614 : }
1615 :
1616 :
1617 : SUMOTime
1618 680751326 : MSVehicle::collisionStopTime() const {
1619 680751326 : return (myStops.empty() || !myStops.front().pars.collision) ? myCollisionImmunity : MAX2((SUMOTime)0, myStops.front().duration);
1620 : }
1621 :
1622 :
1623 : bool
1624 680530139 : MSVehicle::brokeDown() const {
1625 680530139 : return isStopped() && !myStops.empty() && myStops.front().pars.breakDown;
1626 : }
1627 :
1628 :
1629 : bool
1630 261834 : MSVehicle::ignoreCollision() const {
1631 261834 : return myCollisionImmunity > 0;
1632 : }
1633 :
1634 :
1635 : double
1636 642953053 : MSVehicle::processNextStop(double currentVelocity) {
1637 642953053 : if (myStops.empty()) {
1638 : // no stops; pass
1639 : return currentVelocity;
1640 : }
1641 :
1642 : #ifdef DEBUG_STOPS
1643 : if (DEBUG_COND) {
1644 : std::cout << "\nPROCESS_NEXT_STOP\n" << SIMTIME << " vehicle '" << getID() << "'" << std::endl;
1645 : }
1646 : #endif
1647 :
1648 : MSStop& stop = myStops.front();
1649 40206810 : const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1650 40206810 : if (stop.reached) {
1651 24851847 : stop.duration -= getActionStepLength();
1652 24851847 : if (getSpeed() > 0) {
1653 : // re-enter stopping places to correct waiting position (except for parkingArea since it's place-based)
1654 2580197 : if (stop.busstop != nullptr) {
1655 : // let the bus stop know the vehicle
1656 12750 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1657 : }
1658 2580197 : if (stop.containerstop != nullptr) {
1659 : // let the container stop know the vehicle
1660 2535051 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1661 : }
1662 2580197 : if (stop.chargingStation != nullptr) {
1663 : // let the container stop know the vehicle
1664 3045 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1665 : }
1666 2580197 : if (stop.getSpeed() <= 0) {
1667 2563027 : stop.entryPos = getPositionOnLane();
1668 : }
1669 : }
1670 :
1671 : #ifdef DEBUG_STOPS
1672 : if (DEBUG_COND) {
1673 : std::cout << SIMTIME << " vehicle '" << getID() << "' reached stop.\n"
1674 : << "Remaining duration: " << STEPS2TIME(stop.duration) << std::endl;
1675 : if (stop.getSpeed() > 0) {
1676 : std::cout << " waypointSpeed=" << stop.getSpeed() << " vehPos=" << myState.myPos << " endPos=" << stop.pars.endPos << "\n";
1677 : }
1678 : }
1679 : #endif
1680 24851847 : if (stop.duration <= 0 && stop.pars.join != "") {
1681 : // join this train (part) to another one
1682 37477 : MSVehicle* joinVeh = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(stop.pars.join));
1683 968 : if (joinVeh && joinVeh->hasDeparted() && (joinVeh->joinTrainPart(this) || joinVeh->joinTrainPartFront(this))) {
1684 36 : stop.joinTriggered = false;
1685 36 : if (myAmRegisteredAsWaiting) {
1686 21 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1687 21 : myAmRegisteredAsWaiting = false;
1688 : }
1689 : // avoid collision warning before this vehicle is removed (joinVeh was already made longer)
1690 36 : myCollisionImmunity = TIME2STEPS(100);
1691 : // mark this vehicle as arrived
1692 36 : myArrivalPos = getPositionOnLane();
1693 36 : const_cast<SUMOVehicleParameter*>(myParameter)->arrivalEdge = getRoutePosition();
1694 : // handle transportables that want to continue in the other vehicle
1695 36 : if (myPersonDevice != nullptr) {
1696 3 : myPersonDevice->transferAtSplitOrJoin(joinVeh);
1697 : }
1698 36 : if (myContainerDevice != nullptr) {
1699 3 : myContainerDevice->transferAtSplitOrJoin(joinVeh);
1700 : }
1701 : }
1702 : }
1703 24851847 : boardTransportables(stop);
1704 22317202 : if (time > stop.endBoarding) {
1705 : // for taxi: cancel customers
1706 198308 : MSDevice_Taxi* taxiDevice = static_cast<MSDevice_Taxi*>(getDevice(typeid(MSDevice_Taxi)));
1707 : if (taxiDevice != nullptr) {
1708 : // may invalidate stops including the current reference
1709 64 : taxiDevice->cancelCurrentCustomers();
1710 64 : resumeFromStopping();
1711 64 : return currentVelocity;
1712 : }
1713 : }
1714 22317138 : if (!keepStopping() && isOnRoad()) {
1715 : #ifdef DEBUG_STOPS
1716 : if (DEBUG_COND) {
1717 : std::cout << SIMTIME << " vehicle '" << getID() << "' resumes from stopping." << std::endl;
1718 : }
1719 : #endif
1720 44303 : resumeFromStopping();
1721 44303 : if (isRail() && hasStops()) {
1722 : // stay on the current lane in case of a double stop
1723 2860 : const MSStop& nextStop = getNextStop();
1724 2860 : if (nextStop.edge == myCurrEdge) {
1725 1079 : const double stopSpeed = getCarFollowModel().stopSpeed(this, getSpeed(), nextStop.pars.endPos - myState.myPos);
1726 : //std::cout << SIMTIME << " veh=" << getID() << " resumedFromStopping currentVelocity=" << currentVelocity << " stopSpeed=" << stopSpeed << "\n";
1727 1079 : return stopSpeed;
1728 : }
1729 : }
1730 : } else {
1731 22272835 : if (stop.triggered) {
1732 3226582 : if (getVehicleType().getPersonCapacity() == getPersonNumber()) {
1733 30 : WRITE_WARNINGF(TL("Vehicle '%' ignores triggered stop on lane '%' due to capacity constraints."), getID(), stop.lane->getID());
1734 10 : stop.triggered = false;
1735 3226572 : } else if (!myAmRegisteredAsWaiting && stop.duration <= DELTA_T) {
1736 : // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1737 4329 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1738 4329 : myAmRegisteredAsWaiting = true;
1739 : #ifdef DEBUG_STOPS
1740 : if (DEBUG_COND) {
1741 : std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for person." << std::endl;
1742 : }
1743 : #endif
1744 : }
1745 : }
1746 22272835 : if (stop.containerTriggered) {
1747 39500 : if (getVehicleType().getContainerCapacity() == getContainerNumber()) {
1748 1332 : WRITE_WARNINGF(TL("Vehicle '%' ignores container triggered stop on lane '%' due to capacity constraints."), getID(), stop.lane->getID());
1749 444 : stop.containerTriggered = false;
1750 39056 : } else if (stop.containerTriggered && !myAmRegisteredAsWaiting && stop.duration <= DELTA_T) {
1751 : // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1752 92 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1753 92 : myAmRegisteredAsWaiting = true;
1754 : #ifdef DEBUG_STOPS
1755 : if (DEBUG_COND) {
1756 : std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for container." << std::endl;
1757 : }
1758 : #endif
1759 : }
1760 : }
1761 : // joining only takes place after stop duration is over
1762 22272835 : if (stop.joinTriggered && !myAmRegisteredAsWaiting
1763 7197 : && stop.duration <= (stop.pars.extension >= 0 ? -stop.pars.extension : 0)) {
1764 99 : if (stop.pars.extension >= 0) {
1765 108 : WRITE_WARNINGF(TL("Vehicle '%' aborts joining after extension of %s at time %."), getID(), STEPS2TIME(stop.pars.extension), time2string(SIMSTEP));
1766 36 : stop.joinTriggered = false;
1767 : } else {
1768 : // keep stopping indefinitely but ensure that simulation terminates
1769 63 : MSNet::getInstance()->getVehicleControl().registerOneWaiting();
1770 63 : myAmRegisteredAsWaiting = true;
1771 : }
1772 : }
1773 22272835 : if (stop.getSpeed() > 0) {
1774 : //waypoint mode
1775 219665 : if (stop.duration == 0) {
1776 243 : return stop.getSpeed();
1777 : } else {
1778 : // stop for 'until' (computed in planMove)
1779 : return currentVelocity;
1780 : }
1781 : } else {
1782 : // brake
1783 22053170 : if (MSGlobals::gSemiImplicitEulerUpdate || stop.getSpeed() > 0) {
1784 21783553 : return 0;
1785 : } else {
1786 : // ballistic:
1787 269617 : return getSpeed() - getCarFollowModel().getMaxDecel();
1788 : }
1789 : }
1790 : }
1791 : } else {
1792 :
1793 : #ifdef DEBUG_STOPS
1794 : if (DEBUG_COND) {
1795 : std::cout << SIMTIME << " vehicle '" << getID() << "' hasn't reached next stop." << std::endl;
1796 : }
1797 : #endif
1798 : //std::cout << SIMTIME << " myStopDist=" << myStopDist << " bGap=" << getBrakeGap(myLane->getVehicleMaxSpeed(this)) << "\n";
1799 15417491 : if (stop.pars.onDemand && !stop.skipOnDemand && myStopDist <= getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this))) {
1800 577 : MSNet* const net = MSNet::getInstance();
1801 44 : const bool noExits = ((myPersonDevice == nullptr || !myPersonDevice->anyLeavingAtStop(stop))
1802 587 : && (myContainerDevice == nullptr || !myContainerDevice->anyLeavingAtStop(stop)));
1803 83 : const bool noEntries = ((!net->hasPersons() || !net->getPersonControl().hasAnyWaiting(stop.getEdge(), this))
1804 626 : && (!net->hasContainers() || !net->getContainerControl().hasAnyWaiting(stop.getEdge(), this)));
1805 577 : if (noExits && noEntries) {
1806 : //std::cout << " skipOnDemand\n";
1807 509 : stop.skipOnDemand = true;
1808 : // bestLanes must be extended past this stop
1809 509 : updateBestLanes(true);
1810 : }
1811 : }
1812 : // is the next stop on the current lane?
1813 15354963 : if (stop.edge == myCurrEdge) {
1814 : // get the stopping position
1815 5503185 : bool useStoppingPlace = stop.busstop != nullptr || stop.containerstop != nullptr || stop.parkingarea != nullptr;
1816 : bool fitsOnStoppingPlace = true;
1817 5503185 : if (!stop.skipOnDemand) { // no need to check available space if we skip it anyway
1818 5497393 : if (stop.busstop != nullptr) {
1819 1704900 : fitsOnStoppingPlace &= stop.busstop->fits(myState.myPos, *this);
1820 : }
1821 5497393 : if (stop.containerstop != nullptr) {
1822 21791 : fitsOnStoppingPlace &= stop.containerstop->fits(myState.myPos, *this);
1823 : }
1824 : // if the stop is a parking area we check if there is a free position on the area
1825 5497393 : if (stop.parkingarea != nullptr) {
1826 687128 : fitsOnStoppingPlace &= myState.myPos > stop.parkingarea->getBeginLanePosition();
1827 687128 : if (stop.parkingarea->getOccupancy() >= stop.parkingarea->getCapacity()) {
1828 : fitsOnStoppingPlace = false;
1829 : // trigger potential parkingZoneReroute
1830 434927 : MSParkingArea* oldParkingArea = stop.parkingarea;
1831 475993 : for (MSMoveReminder* rem : myLane->getMoveReminders()) {
1832 41066 : if (rem->isParkingRerouter()) {
1833 19838 : rem->notifyEnter(*this, MSMoveReminder::NOTIFICATION_PARKING_REROUTE, myLane);
1834 : }
1835 : }
1836 434927 : if (myStops.empty() || myStops.front().parkingarea != oldParkingArea) {
1837 : // rerouted, keep driving
1838 : return currentVelocity;
1839 : }
1840 252201 : } else if (stop.parkingarea->getOccupancyIncludingReservations(this) >= stop.parkingarea->getCapacity()) {
1841 : fitsOnStoppingPlace = false;
1842 122973 : } else if (stop.parkingarea->parkOnRoad() && stop.parkingarea->getLotIndex(this) < 0) {
1843 : fitsOnStoppingPlace = false;
1844 : }
1845 : }
1846 : }
1847 5501431 : const double targetPos = myState.myPos + myStopDist + (stop.getSpeed() > 0 ? (stop.pars.startPos - stop.pars.endPos) : 0);
1848 5501431 : double reachedThreshold = (useStoppingPlace ? targetPos - STOPPING_PLACE_OFFSET : stop.getReachedThreshold()) - NUMERICAL_EPS;
1849 5501431 : if (stop.busstop != nullptr && stop.getSpeed() <= 0 && getWaitingTime() > DELTA_T && myLane == stop.lane) {
1850 : // count (long) busStop as reached when fully within and jammed before the designated spot
1851 818365 : reachedThreshold = MIN2(reachedThreshold, stop.pars.startPos + getLength());
1852 : }
1853 5501431 : const bool posReached = myState.pos() >= reachedThreshold && currentVelocity <= stop.getSpeed() + SUMO_const_haltingSpeed && myLane == stop.lane;
1854 : #ifdef DEBUG_STOPS
1855 : if (DEBUG_COND) {
1856 : std::cout << " pos=" << myState.pos() << " speed=" << currentVelocity << " targetPos=" << targetPos << " fits=" << fitsOnStoppingPlace
1857 : << " reachedThresh=" << reachedThreshold
1858 : << " posReached=" << posReached
1859 : << " myLane=" << Named::getIDSecure(myLane)
1860 : << " stopLane=" << Named::getIDSecure(stop.lane)
1861 : << "\n";
1862 : }
1863 : #endif
1864 5501431 : if (posReached && !fitsOnStoppingPlace && MSStopOut::active()) {
1865 5736 : MSStopOut::getInstance()->stopBlocked(this, time);
1866 : }
1867 5501431 : if (fitsOnStoppingPlace && posReached && (!MSGlobals::gModelParkingManoeuver || myManoeuvre.entryManoeuvreIsComplete(this))) {
1868 : // ok, we may stop (have reached the stop) and either we are not modelling maneuvering or have completed entry
1869 56305 : stop.reached = true;
1870 56305 : if (!stop.startedFromState) {
1871 56083 : stop.pars.started = time;
1872 : }
1873 : #ifdef DEBUG_STOPS
1874 : if (DEBUG_COND) {
1875 : std::cout << SIMTIME << " vehicle '" << getID() << "' reached next stop." << std::endl;
1876 : }
1877 : #endif
1878 56305 : if (MSStopOut::active()) {
1879 5339 : MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), time);
1880 : }
1881 56305 : myLane->getEdge().addWaiting(this);
1882 56305 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::STARTING_STOP);
1883 56305 : MSNet::getInstance()->getVehicleControl().registerStopStarted();
1884 : // compute stopping time
1885 56305 : stop.duration = stop.getMinDuration(time);
1886 56305 : stop.endBoarding = stop.pars.extension >= 0 ? time + stop.duration + stop.pars.extension : SUMOTime_MAX;
1887 56305 : MSDevice_Taxi* taxiDevice = static_cast<MSDevice_Taxi*>(getDevice(typeid(MSDevice_Taxi)));
1888 4122 : if (taxiDevice != nullptr && stop.pars.extension >= 0) {
1889 : // earliestPickupTime is set with waitUntil
1890 84 : stop.endBoarding = MAX2(time, stop.pars.waitUntil) + stop.pars.extension;
1891 : }
1892 56305 : if (stop.getSpeed() > 0) {
1893 : // ignore duration parameter in waypoint mode unless 'until' or 'ended' are set
1894 3429 : if (stop.getUntil() > time) {
1895 348 : stop.duration = stop.getUntil() - time;
1896 : } else {
1897 3081 : stop.duration = 0;
1898 : }
1899 : } else {
1900 52876 : stop.entryPos = getPositionOnLane();
1901 : }
1902 56305 : if (stop.busstop != nullptr) {
1903 : // let the bus stop know the vehicle
1904 18491 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1905 : }
1906 56305 : if (stop.containerstop != nullptr) {
1907 : // let the container stop know the vehicle
1908 571 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1909 : }
1910 56305 : if (stop.parkingarea != nullptr && stop.getSpeed() <= 0) {
1911 : // let the parking area know the vehicle
1912 9863 : stop.parkingarea->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1913 : }
1914 56305 : if (stop.chargingStation != nullptr) {
1915 : // let the container stop know the vehicle
1916 3503 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
1917 : }
1918 :
1919 56305 : if (stop.pars.tripId != "") {
1920 2922 : ((SUMOVehicleParameter&)getParameter()).setParameter("tripId", stop.pars.tripId);
1921 : }
1922 56305 : if (stop.pars.line != "") {
1923 1464 : ((SUMOVehicleParameter&)getParameter()).line = stop.pars.line;
1924 : }
1925 56305 : if (stop.pars.split != "") {
1926 : // split the train
1927 1240 : MSVehicle* splitVeh = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(stop.pars.split));
1928 24 : if (splitVeh == nullptr) {
1929 3648 : WRITE_WARNINGF(TL("Vehicle '%' to split from vehicle '%' is not known. time=%."), stop.pars.split, getID(), SIMTIME)
1930 : } else {
1931 24 : MSNet::getInstance()->getInsertionControl().add(splitVeh);
1932 24 : splitVeh->getRoute().getEdges()[0]->removeWaiting(splitVeh);
1933 24 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
1934 24 : const double newLength = MAX2(myType->getLength() - splitVeh->getVehicleType().getLength(),
1935 24 : myType->getParameter().locomotiveLength);
1936 24 : getSingularType().setLength(newLength);
1937 : // handle transportables that want to continue in the split part
1938 24 : if (myPersonDevice != nullptr) {
1939 0 : myPersonDevice->transferAtSplitOrJoin(splitVeh);
1940 : }
1941 24 : if (myContainerDevice != nullptr) {
1942 6 : myContainerDevice->transferAtSplitOrJoin(splitVeh);
1943 : }
1944 24 : if (splitVeh->getParameter().departPosProcedure == DepartPosDefinition::SPLIT_FRONT) {
1945 3 : const double backShift = splitVeh->getLength() + getVehicleType().getMinGap();
1946 3 : myState.myPos -= backShift;
1947 3 : myState.myBackPos -= backShift;
1948 : }
1949 : }
1950 : }
1951 :
1952 56305 : boardTransportables(stop);
1953 56301 : if (stop.pars.posLat != INVALID_DOUBLE) {
1954 231 : myState.myPosLat = stop.pars.posLat;
1955 : }
1956 : }
1957 : }
1958 : }
1959 : return currentVelocity;
1960 : }
1961 :
1962 :
1963 : void
1964 24908152 : MSVehicle::boardTransportables(MSStop& stop) {
1965 24908152 : if (stop.skipOnDemand) {
1966 : return;
1967 : }
1968 : // we have reached the stop
1969 : // any waiting persons may board now
1970 24704944 : const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1971 24704944 : MSNet* const net = MSNet::getInstance();
1972 24704944 : const bool boarded = (time <= stop.endBoarding
1973 24703359 : && net->hasPersons()
1974 1526599 : && net->getPersonControl().loadAnyWaiting(&myLane->getEdge(), this, stop.timeToBoardNextPerson, stop.duration)
1975 24709951 : && stop.numExpectedPerson == 0);
1976 : // load containers
1977 24704944 : const bool loaded = (time <= stop.endBoarding
1978 24703359 : && net->hasContainers()
1979 2637083 : && net->getContainerControl().loadAnyWaiting(&myLane->getEdge(), this, stop.timeToLoadNextContainer, stop.duration)
1980 24705519 : && stop.numExpectedContainer == 0);
1981 :
1982 : bool unregister = false;
1983 22170295 : if (time > stop.endBoarding) {
1984 1585 : stop.triggered = false;
1985 1585 : stop.containerTriggered = false;
1986 1585 : if (myAmRegisteredAsWaiting) {
1987 : unregister = true;
1988 328 : myAmRegisteredAsWaiting = false;
1989 : }
1990 : }
1991 22170295 : if (boarded) {
1992 : // the triggering condition has been fulfilled. Maybe we want to wait a bit longer for additional riders (car pooling)
1993 4869 : if (myAmRegisteredAsWaiting) {
1994 : unregister = true;
1995 : }
1996 4869 : stop.triggered = false;
1997 4869 : myAmRegisteredAsWaiting = false;
1998 : }
1999 22170295 : if (loaded) {
2000 : // the triggering condition has been fulfilled
2001 555 : if (myAmRegisteredAsWaiting) {
2002 : unregister = true;
2003 : }
2004 555 : stop.containerTriggered = false;
2005 555 : myAmRegisteredAsWaiting = false;
2006 : }
2007 :
2008 22170295 : if (unregister) {
2009 411 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2010 : #ifdef DEBUG_STOPS
2011 : if (DEBUG_COND) {
2012 : std::cout << SIMTIME << " vehicle '" << getID() << "' unregisters as waiting for transportable." << std::endl;
2013 : }
2014 : #endif
2015 : }
2016 : }
2017 :
2018 : bool
2019 920 : MSVehicle::joinTrainPart(MSVehicle* veh) {
2020 : // check if veh is close enough to be joined to the rear of this vehicle
2021 920 : MSLane* backLane = myFurtherLanes.size() == 0 ? myLane : myFurtherLanes.back();
2022 920 : double gap = getBackPositionOnLane() - veh->getPositionOnLane();
2023 1142 : if (isStopped() && myStops.begin()->duration <= DELTA_T && myStops.begin()->joinTriggered && backLane == veh->getLane()
2024 950 : && gap >= 0 && gap <= getVehicleType().getMinGap() + 1) {
2025 15 : const double newLength = myType->getLength() + veh->getVehicleType().getLength();
2026 15 : getSingularType().setLength(newLength);
2027 15 : myStops.begin()->joinTriggered = false;
2028 15 : if (myAmRegisteredAsWaiting) {
2029 0 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2030 0 : myAmRegisteredAsWaiting = false;
2031 : }
2032 : return true;
2033 : } else {
2034 905 : return false;
2035 : }
2036 : }
2037 :
2038 :
2039 : bool
2040 905 : MSVehicle::joinTrainPartFront(MSVehicle* veh) {
2041 : // check if veh is close enough to be joined to the front of this vehicle
2042 905 : MSLane* backLane = veh->myFurtherLanes.size() == 0 ? veh->myLane : veh->myFurtherLanes.back();
2043 905 : double gap = veh->getBackPositionOnLane(backLane) - getPositionOnLane();
2044 1112 : if (isStopped() && myStops.begin()->duration <= DELTA_T && myStops.begin()->joinTriggered && backLane == getLane()
2045 929 : && gap >= 0 && gap <= getVehicleType().getMinGap() + 1) {
2046 : double skippedLaneLengths = 0;
2047 24 : if (veh->myFurtherLanes.size() > 0) {
2048 9 : skippedLaneLengths += getLane()->getLength();
2049 : // this vehicle must be moved to the lane of veh
2050 : // ensure that lane and furtherLanes of veh match our route
2051 9 : int routeIndex = getRoutePosition();
2052 9 : if (myLane->isInternal()) {
2053 0 : routeIndex++;
2054 : }
2055 27 : for (int i = (int)veh->myFurtherLanes.size() - 1; i >= 0; i--) {
2056 18 : MSEdge* edge = &veh->myFurtherLanes[i]->getEdge();
2057 18 : if (edge->isInternal()) {
2058 9 : continue;
2059 : }
2060 9 : if (!edge->isInternal() && edge != myRoute->getEdges()[routeIndex]) {
2061 0 : std::string warn = TL("Cannot join vehicle '%' to vehicle '%' due to incompatible routes. time=%.");
2062 0 : WRITE_WARNINGF(warn, veh->getID(), getID(), time2string(SIMSTEP));
2063 : return false;
2064 : }
2065 9 : routeIndex++;
2066 : }
2067 9 : if (veh->getCurrentEdge()->getNormalSuccessor() != myRoute->getEdges()[routeIndex]) {
2068 3 : std::string warn = TL("Cannot join vehicle '%' to vehicle '%' due to incompatible routes. time=%.");
2069 9 : WRITE_WARNINGF(warn, veh->getID(), getID(), time2string(SIMSTEP));
2070 : return false;
2071 : }
2072 12 : for (int i = (int)veh->myFurtherLanes.size() - 2; i >= 0; i--) {
2073 6 : skippedLaneLengths += veh->myFurtherLanes[i]->getLength();
2074 : }
2075 : }
2076 :
2077 21 : const double newLength = myType->getLength() + veh->getVehicleType().getLength();
2078 21 : getSingularType().setLength(newLength);
2079 : // lane will be advanced just as for regular movement
2080 21 : myState.myPos = skippedLaneLengths + veh->getPositionOnLane();
2081 21 : myStops.begin()->joinTriggered = false;
2082 21 : if (myAmRegisteredAsWaiting) {
2083 6 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
2084 6 : myAmRegisteredAsWaiting = false;
2085 : }
2086 21 : return true;
2087 : } else {
2088 881 : return false;
2089 : }
2090 : }
2091 :
2092 : double
2093 8952342 : MSVehicle::getBrakeGap(bool delayed) const {
2094 8952342 : return getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), delayed ? getCarFollowModel().getHeadwayTime() : 0);
2095 : }
2096 :
2097 :
2098 : bool
2099 703180022 : MSVehicle::checkActionStep(const SUMOTime t) {
2100 703180022 : myActionStep = isActionStep(t);
2101 703180022 : if (myActionStep) {
2102 631599154 : myLastActionTime = t;
2103 : }
2104 703180022 : return myActionStep;
2105 : }
2106 :
2107 :
2108 : void
2109 1367 : MSVehicle::resetActionOffset(const SUMOTime timeUntilNextAction) {
2110 1367 : myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + timeUntilNextAction;
2111 1367 : }
2112 :
2113 :
2114 : void
2115 1 : MSVehicle::updateActionOffset(const SUMOTime oldActionStepLength, const SUMOTime newActionStepLength) {
2116 1 : SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2117 1 : SUMOTime timeSinceLastAction = now - myLastActionTime;
2118 1 : if (timeSinceLastAction == 0) {
2119 : // Action was scheduled now, may be delayed be new action step length
2120 : timeSinceLastAction = oldActionStepLength;
2121 : }
2122 1 : if (timeSinceLastAction >= newActionStepLength) {
2123 : // Action point required in this step
2124 0 : myLastActionTime = now;
2125 : } else {
2126 1 : SUMOTime timeUntilNextAction = newActionStepLength - timeSinceLastAction;
2127 1 : resetActionOffset(timeUntilNextAction);
2128 : }
2129 1 : }
2130 :
2131 :
2132 :
2133 : void
2134 703180022 : MSVehicle::planMove(const SUMOTime t, const MSLeaderInfo& ahead, const double lengthsInFront) {
2135 : #ifdef DEBUG_PLAN_MOVE
2136 : if (DEBUG_COND) {
2137 : std::cout
2138 : << "\nPLAN_MOVE\n"
2139 : << SIMTIME
2140 : << std::setprecision(gPrecision)
2141 : << " veh=" << getID()
2142 : << " lane=" << myLane->getID()
2143 : << " pos=" << getPositionOnLane()
2144 : << " posLat=" << getLateralPositionOnLane()
2145 : << " speed=" << getSpeed()
2146 : << "\n";
2147 : }
2148 : #endif
2149 : // Update the driver state
2150 703180022 : if (hasDriverState()) {
2151 451425 : myDriverState->update();
2152 902850 : setActionStepLength(myDriverState->getDriverState()->getActionStepLength(), false);
2153 : }
2154 :
2155 703180022 : myStopSpeed = getCarFollowModel().maxNextSpeed(myStopSpeed, this);
2156 703180022 : if (!checkActionStep(t)) {
2157 : #ifdef DEBUG_ACTIONSTEPS
2158 : if (DEBUG_COND) {
2159 : std::cout << STEPS2TIME(t) << " vehicle '" << getID() << "' skips action." << std::endl;
2160 : }
2161 : #endif
2162 : // During non-action passed drive items still need to be removed
2163 : // @todo rather work with updating myCurrentDriveItem (refs #3714)
2164 71580868 : removePassedDriveItems();
2165 71580868 : return;
2166 : } else {
2167 : #ifdef DEBUG_ACTIONSTEPS
2168 : if (DEBUG_COND) {
2169 : std::cout << STEPS2TIME(t) << " vehicle = '" << getID() << "' takes action." << std::endl;
2170 : }
2171 : #endif
2172 : myLFLinkLanesPrev.swap(myLFLinkLanes);
2173 631599154 : if (myInfluencer != nullptr) {
2174 490226 : myInfluencer->updateRemoteControlRoute(this);
2175 : }
2176 631599154 : planMoveInternal(t, ahead, myLFLinkLanes, myStopDist, myStopSpeed, myNextTurn);
2177 : #ifdef DEBUG_PLAN_MOVE
2178 : if (DEBUG_COND) {
2179 : DriveItemVector::iterator i;
2180 : for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
2181 : std::cout
2182 : << " vPass=" << (*i).myVLinkPass
2183 : << " vWait=" << (*i).myVLinkWait
2184 : << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
2185 : << " request=" << (*i).mySetRequest
2186 : << "\n";
2187 : }
2188 : }
2189 : #endif
2190 631599154 : checkRewindLinkLanes(lengthsInFront, myLFLinkLanes);
2191 631599154 : myNextDriveItem = myLFLinkLanes.begin();
2192 : // ideally would only do this with the call inside planMoveInternal - but that needs a const method
2193 : // so this is a kludge here - nuisance as it adds an extra check in a busy loop
2194 631599154 : if (MSGlobals::gModelParkingManoeuver) {
2195 2971 : if (getManoeuvreType() == MSVehicle::MANOEUVRE_EXIT && manoeuvreIsComplete()) {
2196 30 : setManoeuvreType(MSVehicle::MANOEUVRE_NONE);
2197 : }
2198 : }
2199 : }
2200 631599154 : myLaneChangeModel->resetChanged();
2201 : }
2202 :
2203 :
2204 : bool
2205 173475130 : MSVehicle::brakeForOverlap(const MSLink* link, const MSLane* lane) const {
2206 : // @review needed
2207 : //const double futurePosLat = getLateralPositionOnLane() + link->getLateralShift();
2208 : //const double overlap = getLateralOverlap(futurePosLat, link->getViaLaneOrLane());
2209 : //const double edgeWidth = link->getViaLaneOrLane()->getEdge().getWidth();
2210 173475130 : const double futurePosLat = getLateralPositionOnLane() + (
2211 173475130 : lane != myLane && lane->isInternal() ? lane->getIncomingLanes()[0].viaLink->getLateralShift() : 0);
2212 173475130 : const double overlap = getLateralOverlap(futurePosLat, lane);
2213 : const double edgeWidth = lane->getEdge().getWidth();
2214 : const bool result = (overlap > POSITION_EPS
2215 : // do not get stuck on narrow edges
2216 3123177 : && getVehicleType().getWidth() <= edgeWidth
2217 3117572 : && link->getViaLane() == nullptr
2218 : // this is the exit link of a junction. The normal edge should support the shadow
2219 1507620 : && ((myLaneChangeModel->getShadowLane(link->getLane()) == nullptr)
2220 : // the shadow lane must be permitted
2221 1125186 : || !myLaneChangeModel->getShadowLane(link->getLane())->allowsVehicleClass(getVClass())
2222 : // the internal lane after an internal junction has no parallel lane. make sure there is no shadow before continuing
2223 1065041 : || (lane->getEdge().isInternal() && lane->getIncomingLanes()[0].lane->getEdge().isInternal()))
2224 : // ignore situations where the shadow lane is part of a double-connection with the current lane
2225 458253 : && (myLaneChangeModel->getShadowLane() == nullptr
2226 255324 : || myLaneChangeModel->getShadowLane()->getLinkCont().size() == 0
2227 238869 : || myLaneChangeModel->getShadowLane()->getLinkCont().front()->getLane() != link->getLane())
2228 : // emergency vehicles may do some crazy stuff
2229 173871652 : && !myLaneChangeModel->hasBlueLight());
2230 :
2231 : #ifdef DEBUG_PLAN_MOVE
2232 : if (DEBUG_COND) {
2233 : std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getDescription() << " lane=" << lane->getID()
2234 : << " linkLane=" << link->getLane()->getID()
2235 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
2236 : << " shift=" << link->getLateralShift()
2237 : << " fpLat=" << futurePosLat << " overlap=" << overlap << " w=" << getVehicleType().getWidth()
2238 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane(link->getLane()))
2239 : << " result=" << result << "\n";
2240 : }
2241 : #endif
2242 173475130 : return result;
2243 : }
2244 :
2245 :
2246 :
2247 : void
2248 631599154 : MSVehicle::planMoveInternal(const SUMOTime t, MSLeaderInfo ahead, DriveItemVector& lfLinks, double& newStopDist, double& newStopSpeed, std::pair<double, const MSLink*>& nextTurn) const {
2249 : lfLinks.clear();
2250 631599154 : newStopDist = std::numeric_limits<double>::max();
2251 : //
2252 : const MSCFModel& cfModel = getCarFollowModel();
2253 631599154 : const double vehicleLength = getVehicleType().getLength();
2254 631599154 : const double maxV = cfModel.maxNextSpeed(myState.mySpeed, this);
2255 631599154 : const double maxVD = MAX2(getMaxSpeed(), MIN2(maxV, getDesiredMaxSpeed()));
2256 631599154 : const bool opposite = myLaneChangeModel->isOpposite();
2257 : // maxVD is possibly higher than vType-maxSpeed and in this case laneMaxV may be higher as well
2258 631599154 : double laneMaxV = myLane->getVehicleMaxSpeed(this, maxVD);
2259 631599154 : const double vMinComfortable = cfModel.minNextSpeed(getSpeed(), this);
2260 : double lateralShift = 0;
2261 631599154 : if (isRail()) {
2262 : // speed limits must hold for the whole length of the train
2263 1718264 : for (MSLane* l : myFurtherLanes) {
2264 377886 : laneMaxV = MIN2(laneMaxV, l->getVehicleMaxSpeed(this, maxVD));
2265 : #ifdef DEBUG_PLAN_MOVE
2266 : if (DEBUG_COND) {
2267 : std::cout << " laneMaxV=" << laneMaxV << " lane=" << l->getID() << "\n";
2268 : }
2269 : #endif
2270 : }
2271 : }
2272 : // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
2273 : laneMaxV = MAX2(laneMaxV, vMinComfortable);
2274 632089348 : if (myInfluencer && !myInfluencer->considerSpeedLimit()) {
2275 : laneMaxV = std::numeric_limits<double>::max();
2276 : }
2277 : // v is the initial maximum velocity of this vehicle in this step
2278 631599154 : double v = cfModel.maximumLaneSpeedCF(this, maxV, laneMaxV);
2279 : // if we are modelling parking then we dawdle until the manoeuvre is complete - by setting a very low max speed
2280 : // in practice this only applies to exit manoeuvre because entry manoeuvre just delays setting stop.reached - when the vehicle is virtually stopped
2281 631599154 : if (MSGlobals::gModelParkingManoeuver && !manoeuvreIsComplete()) {
2282 420 : v = NUMERICAL_EPS_SPEED;
2283 : }
2284 :
2285 631599154 : if (myInfluencer != nullptr) {
2286 490226 : const double vMin = MAX2(0., cfModel.minNextSpeed(myState.mySpeed, this));
2287 : #ifdef DEBUG_TRACI
2288 : if (DEBUG_COND) {
2289 : std::cout << SIMTIME << " veh=" << getID() << " speedBeforeTraci=" << v;
2290 : }
2291 : #endif
2292 490226 : v = myInfluencer->influenceSpeed(t, v, v, vMin, maxV);
2293 : #ifdef DEBUG_TRACI
2294 : if (DEBUG_COND) {
2295 : std::cout << " influencedSpeed=" << v;
2296 : }
2297 : #endif
2298 490226 : v = myInfluencer->gapControlSpeed(t, this, v, v, vMin, maxV);
2299 : #ifdef DEBUG_TRACI
2300 : if (DEBUG_COND) {
2301 : std::cout << " gapControlSpeed=" << v << "\n";
2302 : }
2303 : #endif
2304 : }
2305 : // all links within dist are taken into account (potentially)
2306 631599154 : const double dist = SPEED2DIST(maxV) + cfModel.brakeGap(maxV);
2307 :
2308 631599154 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation();
2309 : #ifdef DEBUG_PLAN_MOVE
2310 : if (DEBUG_COND) {
2311 : std::cout << " dist=" << dist << " bestLaneConts=" << toString(bestLaneConts)
2312 : << "\n maxV=" << maxV << " laneMaxV=" << laneMaxV << " v=" << v << "\n";
2313 : }
2314 : #endif
2315 : assert(bestLaneConts.size() > 0);
2316 : bool hadNonInternal = false;
2317 : // the distance already "seen"; in the following always up to the end of the current "lane"
2318 631599154 : double seen = opposite ? myState.myPos : myLane->getLength() - myState.myPos;
2319 631599154 : nextTurn.first = seen;
2320 631599154 : nextTurn.second = nullptr;
2321 631599154 : bool encounteredTurn = (MSGlobals::gLateralResolution <= 0); // next turn is only needed for sublane
2322 : double seenNonInternal = 0;
2323 631599154 : double seenInternal = myLane->isInternal() ? seen : 0;
2324 631599154 : double vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(seen, v, cfModel.getMaxAccel()), laneMaxV); // upper bound
2325 : int view = 0;
2326 : DriveProcessItem* lastLink = nullptr;
2327 : bool slowedDownForMinor = false; // whether the vehicle already had to slow down on approach to a minor link
2328 : double mustSeeBeforeReversal = 0;
2329 : // iterator over subsequent lanes and fill lfLinks until stopping distance or stopped
2330 631599154 : const MSLane* lane = opposite ? myLane->getParallelOpposite() : myLane;
2331 : assert(lane != 0);
2332 631599154 : const MSLane* leaderLane = myLane;
2333 631599154 : bool foundRailSignal = !isRail();
2334 : bool planningToStop = false;
2335 : #ifdef PARALLEL_STOPWATCH
2336 : myLane->getStopWatch()[0].start();
2337 : #endif
2338 :
2339 : // optionally slow down to match arrival time
2340 631599154 : const double sfp = getVehicleType().getParameter().speedFactorPremature;
2341 631588896 : if (v > vMinComfortable && hasStops() && myStops.front().pars.arrival >= 0 && sfp > 0
2342 4279 : && v > myLane->getSpeedLimit() * sfp
2343 631602190 : && !myStops.front().reached) {
2344 2786 : const double vSlowDown = slowDownForSchedule(vMinComfortable);
2345 5403 : v = MIN2(v, vSlowDown);
2346 : }
2347 : auto stopIt = myStops.begin();
2348 : while (true) {
2349 : // check leader on lane
2350 : // leader is given for the first edge only
2351 1211969846 : if (opposite &&
2352 : (leaderLane->getVehicleNumberWithPartials() > 1
2353 104042 : || (leaderLane != myLane && leaderLane->getVehicleNumber() > 0))) {
2354 397180 : ahead.clear();
2355 : // find opposite-driving leader that must be respected on the currently looked at lane
2356 : // (only looking at one lane at a time)
2357 397180 : const double backOffset = leaderLane == myLane ? getPositionOnLane() : leaderLane->getLength();
2358 397180 : const double gapOffset = leaderLane == myLane ? 0 : seen - leaderLane->getLength();
2359 397180 : const MSLeaderDistanceInfo cands = leaderLane->getFollowersOnConsecutive(this, backOffset, true, backOffset, MSLane::MinorLinkMode::FOLLOW_NEVER);
2360 397180 : MSLeaderDistanceInfo oppositeLeaders(leaderLane->getWidth(), this, 0.);
2361 397180 : const double minTimeToLeaveLane = MSGlobals::gSublane ? MAX2(TS, (0.5 * myLane->getWidth() - getLateralPositionOnLane()) / getVehicleType().getMaxSpeedLat()) : TS;
2362 1055500 : for (int i = 0; i < cands.numSublanes(); i++) {
2363 658320 : CLeaderDist cand = cands[i];
2364 658320 : if (cand.first != 0) {
2365 561162 : if ((cand.first->myLaneChangeModel->isOpposite() && cand.first->getLaneChangeModel().getShadowLane() != leaderLane)
2366 561633 : || (!cand.first->myLaneChangeModel->isOpposite() && cand.first->getLaneChangeModel().getShadowLane() == leaderLane)) {
2367 : // respect leaders that also drive in the opposite direction (fully or with some overlap)
2368 355139 : oppositeLeaders.addLeader(cand.first, cand.second + gapOffset - getVehicleType().getMinGap() + cand.first->getVehicleType().getMinGap() - cand.first->getVehicleType().getLength());
2369 : } else {
2370 : // avoid frontal collision
2371 379212 : const bool assumeStopped = cand.first->isStopped() || cand.first->getWaitingSeconds() > 1;
2372 206023 : const double predMaxDist = cand.first->getSpeed() + (assumeStopped ? 0 : cand.first->getCarFollowModel().getMaxAccel()) * minTimeToLeaveLane;
2373 206023 : if (cand.second >= 0 && (cand.second - v * minTimeToLeaveLane - predMaxDist < 0 || assumeStopped)) {
2374 44678 : oppositeLeaders.addLeader(cand.first, cand.second + gapOffset - predMaxDist - getVehicleType().getMinGap());
2375 : }
2376 : }
2377 : }
2378 : }
2379 : #ifdef DEBUG_PLAN_MOVE
2380 : if (DEBUG_COND) {
2381 : std::cout << " leaderLane=" << leaderLane->getID() << " gapOffset=" << gapOffset << " minTimeToLeaveLane=" << minTimeToLeaveLane
2382 : << " cands=" << cands.toString() << " oppositeLeaders=" << oppositeLeaders.toString() << "\n";
2383 : }
2384 : #endif
2385 397180 : adaptToLeaderDistance(oppositeLeaders, 0, seen, lastLink, v, vLinkPass);
2386 397180 : } else {
2387 1211572666 : if (MSGlobals::gLateralResolution > 0 && myLaneChangeModel->getShadowLane() == nullptr) {
2388 195983671 : const double rightOL = getRightSideOnLane(lane) + lateralShift;
2389 195983671 : const double leftOL = getLeftSideOnLane(lane) + lateralShift;
2390 : const bool outsideLeft = leftOL > lane->getWidth();
2391 : #ifdef DEBUG_PLAN_MOVE
2392 : if (DEBUG_COND) {
2393 : std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
2394 : }
2395 : #endif
2396 195983671 : if (rightOL < 0 || outsideLeft) {
2397 1269862 : MSLeaderInfo outsideLeaders(lane->getWidth());
2398 : // if ego is driving outside lane bounds we must consider
2399 : // potential leaders that are also outside bounds
2400 : int sublaneOffset = 0;
2401 1269862 : if (outsideLeft) {
2402 530540 : sublaneOffset = MIN2(-1, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution));
2403 : } else {
2404 739322 : sublaneOffset = MAX2(1, (int)ceil(-rightOL / MSGlobals::gLateralResolution));
2405 : }
2406 1269862 : outsideLeaders.setSublaneOffset(sublaneOffset);
2407 : #ifdef DEBUG_PLAN_MOVE
2408 : if (DEBUG_COND) {
2409 : std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
2410 : }
2411 : #endif
2412 5313837 : for (const MSVehicle* cand : lane->getVehiclesSecure()) {
2413 1473508 : if ((lane != myLane || cand->getPositionOnLane() > getPositionOnLane())
2414 4670186 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
2415 3196516 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
2416 97507 : outsideLeaders.addLeader(cand, true);
2417 : #ifdef DEBUG_PLAN_MOVE
2418 : if (DEBUG_COND) {
2419 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << outsideLeaders.toString() << "\n";
2420 : }
2421 : #endif
2422 : }
2423 : }
2424 1269862 : lane->releaseVehicles();
2425 1269862 : if (outsideLeaders.hasVehicles()) {
2426 26312 : adaptToLeaders(outsideLeaders, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2427 : }
2428 1269862 : }
2429 : }
2430 1211572666 : adaptToLeaders(ahead, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2431 : }
2432 1211969846 : if (lastLink != nullptr) {
2433 1091267788 : lastLink->myVLinkWait = MIN2(lastLink->myVLinkWait, v);
2434 : }
2435 : #ifdef DEBUG_PLAN_MOVE
2436 : if (DEBUG_COND) {
2437 : std::cout << "\nv = " << v << "\n";
2438 :
2439 : }
2440 : #endif
2441 : // XXX efficiently adapt to shadow leaders using neighAhead by iteration over the whole edge in parallel (lanechanger-style)
2442 1211969846 : if (myLaneChangeModel->getShadowLane() != nullptr) {
2443 : // also slow down for leaders on the shadowLane relative to the current lane
2444 5065946 : const MSLane* shadowLane = myLaneChangeModel->getShadowLane(leaderLane);
2445 : if (shadowLane != nullptr
2446 5065946 : && (MSGlobals::gLateralResolution > 0 || getLateralOverlap() > POSITION_EPS
2447 : // continous lane change cannot be stopped so we must adapt to the leader on the target lane
2448 191108 : || myLaneChangeModel->getLaneChangeCompletion() < 0.5)) {
2449 4509660 : if ((&shadowLane->getEdge() == &leaderLane->getEdge() || myLaneChangeModel->isOpposite())) {
2450 4467051 : double latOffset = getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge();
2451 4467051 : if (myLaneChangeModel->isOpposite()) {
2452 : // ego posLat is added when retrieving sublanes but it
2453 : // should be negated (subtract twice to compensate)
2454 138623 : latOffset = ((myLane->getWidth() + shadowLane->getWidth()) * 0.5
2455 138623 : - 2 * getLateralPositionOnLane());
2456 :
2457 : }
2458 4467051 : MSLeaderInfo shadowLeaders = shadowLane->getLastVehicleInformation(this, latOffset, lane->getLength() - seen);
2459 : #ifdef DEBUG_PLAN_MOVE
2460 : if (DEBUG_COND && myLaneChangeModel->isOpposite()) {
2461 : std::cout << SIMTIME << " opposite veh=" << getID() << " shadowLane=" << shadowLane->getID() << " latOffset=" << latOffset << " shadowLeaders=" << shadowLeaders.toString() << "\n";
2462 : }
2463 : #endif
2464 4467051 : if (myLaneChangeModel->isOpposite()) {
2465 : // ignore oncoming vehicles on the shadow lane
2466 138623 : shadowLeaders.removeOpposite(shadowLane);
2467 : }
2468 4467051 : const double turningDifference = MAX2(0.0, leaderLane->getLength() - shadowLane->getLength());
2469 4467051 : adaptToLeaders(shadowLeaders, latOffset, seen - turningDifference, lastLink, shadowLane, v, vLinkPass);
2470 4509660 : } else if (shadowLane == myLaneChangeModel->getShadowLane() && leaderLane == myLane) {
2471 : // check for leader vehicles driving in the opposite direction on the opposite-direction shadow lane
2472 : // (and thus in the same direction as ego)
2473 30567 : MSLeaderDistanceInfo shadowLeaders = shadowLane->getFollowersOnConsecutive(this, myLane->getOppositePos(getPositionOnLane()), true);
2474 : const double latOffset = 0;
2475 : #ifdef DEBUG_PLAN_MOVE
2476 : if (DEBUG_COND) {
2477 : std::cout << SIMTIME << " opposite shadows veh=" << getID() << " shadowLane=" << shadowLane->getID()
2478 : << " latOffset=" << latOffset << " shadowLeaders=" << shadowLeaders.toString() << "\n";
2479 : }
2480 : #endif
2481 30567 : shadowLeaders.fixOppositeGaps(true);
2482 : #ifdef DEBUG_PLAN_MOVE
2483 : if (DEBUG_COND) {
2484 : std::cout << " shadowLeadersFixed=" << shadowLeaders.toString() << "\n";
2485 : }
2486 : #endif
2487 30567 : adaptToLeaderDistance(shadowLeaders, latOffset, seen, lastLink, v, vLinkPass);
2488 30567 : }
2489 : }
2490 : }
2491 : // adapt to pedestrians on the same lane
2492 1211969846 : if (lane->getEdge().getPersons().size() > 0 && lane->hasPedestrians()) {
2493 193932 : const double relativePos = lane->getLength() - seen;
2494 : #ifdef DEBUG_PLAN_MOVE
2495 : if (DEBUG_COND) {
2496 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2497 : }
2498 : #endif
2499 193932 : const double stopTime = MAX2(1.0, ceil(getSpeed() / cfModel.getMaxDecel()));
2500 193932 : PersonDist leader = lane->nextBlocking(relativePos,
2501 193932 : getRightSideOnLane(lane), getRightSideOnLane(lane) + getVehicleType().getWidth(), stopTime);
2502 193932 : if (leader.first != 0) {
2503 20843 : const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2504 29576 : v = MIN2(v, stopSpeed);
2505 : #ifdef DEBUG_PLAN_MOVE
2506 : if (DEBUG_COND) {
2507 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2508 : }
2509 : #endif
2510 : }
2511 : }
2512 1211969846 : if (lane->getBidiLane() != nullptr) {
2513 : // adapt to pedestrians on the bidi lane
2514 5734242 : const MSLane* bidiLane = lane->getBidiLane();
2515 5734242 : if (bidiLane->getEdge().getPersons().size() > 0 && bidiLane->hasPedestrians()) {
2516 1028 : const double relativePos = seen;
2517 : #ifdef DEBUG_PLAN_MOVE
2518 : if (DEBUG_COND) {
2519 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2520 : }
2521 : #endif
2522 1028 : const double stopTime = ceil(getSpeed() / cfModel.getMaxDecel());
2523 1028 : const double leftSideOnLane = bidiLane->getWidth() - getRightSideOnLane(lane);
2524 1028 : PersonDist leader = bidiLane->nextBlocking(relativePos,
2525 1028 : leftSideOnLane - getVehicleType().getWidth(), leftSideOnLane, stopTime, true);
2526 1028 : if (leader.first != 0) {
2527 267 : const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2528 524 : v = MIN2(v, stopSpeed);
2529 : #ifdef DEBUG_PLAN_MOVE
2530 : if (DEBUG_COND) {
2531 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2532 : }
2533 : #endif
2534 : }
2535 : }
2536 : }
2537 : // adapt to vehicles blocked from (urgent) lane-changing
2538 1211969846 : if (!opposite && lane->getEdge().hasLaneChanger()) {
2539 592986019 : const double vHelp = myLaneChangeModel->getCooperativeHelpSpeed(lane, seen);
2540 : #ifdef DEBUG_PLAN_MOVE
2541 : if (DEBUG_COND && vHelp < v) {
2542 : std::cout << SIMTIME << " applying cooperativeHelpSpeed v=" << vHelp << "\n";
2543 : }
2544 : #endif
2545 593075632 : v = MIN2(v, vHelp);
2546 : }
2547 :
2548 : // process all stops and waypoints on the current edge
2549 : bool foundRealStop = false;
2550 : while (stopIt != myStops.end()
2551 61198041 : && ((&stopIt->lane->getEdge() == &lane->getEdge())
2552 30861710 : || (stopIt->isOpposite && stopIt->lane->getEdge().getOppositeEdge() == &lane->getEdge()))
2553 : // ignore stops that occur later in a looped route
2554 1262808806 : && stopIt->edge == myCurrEdge + view) {
2555 30281076 : double stopDist = std::numeric_limits<double>::max();
2556 : const MSStop& stop = *stopIt;
2557 : const bool isFirstStop = stopIt == myStops.begin();
2558 : stopIt++;
2559 30281076 : if (!stop.reached || (stop.getSpeed() > 0 && keepStopping())) {
2560 : // we are approaching a stop on the edge; must not drive further
2561 13704631 : bool isWaypoint = stop.getSpeed() > 0;
2562 13704631 : double endPos = stop.getEndPos(*this) + NUMERICAL_EPS;
2563 13704631 : if (stop.parkingarea != nullptr) {
2564 : // leave enough space so parking vehicles can exit
2565 1640295 : const double brakePos = getBrakeGap() + lane->getLength() - seen;
2566 1640295 : endPos = stop.parkingarea->getLastFreePosWithReservation(t, *this, brakePos);
2567 12064336 : } else if (isWaypoint && !stop.reached) {
2568 107831 : endPos = stop.pars.startPos;
2569 : }
2570 13704631 : stopDist = seen + endPos - lane->getLength();
2571 : #ifdef DEBUG_STOPS
2572 : if (DEBUG_COND) {
2573 : std::cout << SIMTIME << " veh=" << getID() << " stopDist=" << stopDist << " stopLane=" << stop.lane->getID() << " stopEndPos=" << endPos << "\n";
2574 : }
2575 : #endif
2576 : double stopSpeed = laneMaxV;
2577 13704631 : if (isWaypoint) {
2578 : bool waypointWithStop = false;
2579 123377 : if (stop.getUntil() > t) {
2580 : // check if we have to slow down or even stop
2581 : SUMOTime time2end = 0;
2582 3691 : if (stop.reached) {
2583 702 : time2end = TIME2STEPS((stop.pars.endPos - myState.myPos) / stop.getSpeed());
2584 : } else {
2585 3267 : time2end = TIME2STEPS(
2586 : // time to reach waypoint start
2587 : stopDist / ((getSpeed() + stop.getSpeed()) / 2)
2588 : // time to reach waypoint end
2589 : + (stop.pars.endPos - stop.pars.startPos) / stop.getSpeed());
2590 : }
2591 3691 : if (stop.getUntil() > t + time2end) {
2592 : // we need to stop
2593 : double distToEnd = stopDist;
2594 3398 : if (!stop.reached) {
2595 2783 : distToEnd += stop.pars.endPos - stop.pars.startPos;
2596 : }
2597 3398 : stopSpeed = MAX2(cfModel.stopSpeed(this, getSpeed(), distToEnd), vMinComfortable);
2598 : waypointWithStop = true;
2599 3398 : if (stopSpeed <= SUMO_const_haltingSpeed) {
2600 531 : const_cast<MSStop&>(stop).waypointWithStop = true;
2601 : }
2602 : }
2603 : }
2604 123377 : if (stop.reached) {
2605 14802 : stopSpeed = MIN2(stop.getSpeed(), stopSpeed);
2606 14802 : if (myState.myPos >= stop.pars.endPos && !waypointWithStop) {
2607 278 : stopDist = std::numeric_limits<double>::max();
2608 : }
2609 : } else {
2610 108575 : stopSpeed = MIN2(MAX2(cfModel.freeSpeed(this, getSpeed(), stopDist, stop.getSpeed()), vMinComfortable), stopSpeed);
2611 108575 : if (!stop.reached) {
2612 108575 : stopDist += stop.pars.endPos - stop.pars.startPos;
2613 : }
2614 108575 : if (lastLink != nullptr) {
2615 66550 : lastLink->adaptLeaveSpeed(cfModel.freeSpeed(this, vLinkPass, endPos, stop.getSpeed(), false, MSCFModel::CalcReason::FUTURE));
2616 : }
2617 : }
2618 : } else {
2619 13581254 : stopSpeed = cfModel.stopSpeed(this, getSpeed(), stopDist);
2620 13581254 : if (!instantStopping()) {
2621 : // regular stops are not emergencies
2622 : stopSpeed = MAX2(stopSpeed, vMinComfortable);
2623 20 : } else if (myInfluencer && !myInfluencer->hasSpeedTimeLine(SIMSTEP)) {
2624 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
2625 20 : speedTimeLine.push_back(std::make_pair(SIMSTEP, getSpeed()));
2626 20 : speedTimeLine.push_back(std::make_pair(SIMSTEP + DELTA_T, stopSpeed));
2627 20 : myInfluencer->setSpeedTimeLine(speedTimeLine);
2628 20 : }
2629 13581254 : if (lastLink != nullptr) {
2630 7950854 : lastLink->adaptLeaveSpeed(cfModel.stopSpeed(this, vLinkPass, endPos, MSCFModel::CalcReason::FUTURE));
2631 : }
2632 : }
2633 13704631 : if (stopSpeed < getSpeed() && getSpeed() > SUMO_const_haltingSpeed) {
2634 : // only discount braking-for-stop timeLoss if we are actually braking
2635 602603 : newStopSpeed = MIN2(newStopSpeed, stopSpeed);
2636 13403243 : } else if (getSpeed() < SUMO_const_haltingSpeed) {
2637 : // blocked from entering a stop
2638 7676905 : newStopSpeed = std::numeric_limits<double>::max();
2639 : }
2640 13704631 : v = MIN2(v, stopSpeed);
2641 13704631 : if (lane->isInternal()) {
2642 7002 : std::vector<MSLink*>::const_iterator exitLink = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2643 : assert(!lane->isLinkEnd(exitLink));
2644 : bool dummySetRequest;
2645 : double dummyVLinkWait;
2646 7002 : checkLinkLeaderCurrentAndParallel(*exitLink, lane, seen, lastLink, v, vLinkPass, dummyVLinkWait, dummySetRequest);
2647 : }
2648 :
2649 : #ifdef DEBUG_PLAN_MOVE
2650 : if (DEBUG_COND) {
2651 : std::cout << "\n" << SIMTIME << " next stop: distance = " << stopDist << " requires stopSpeed = " << stopSpeed << "\n";
2652 :
2653 : }
2654 : #endif
2655 13704631 : if (isFirstStop) {
2656 10179298 : newStopDist = stopDist;
2657 : // if the vehicle is going to stop we don't need to look further
2658 : // (except for trains that make use of further link-approach registration for safety purposes)
2659 10179298 : if (!isWaypoint) {
2660 : planningToStop = true;
2661 10091296 : if (!isRail()) {
2662 9781608 : lfLinks.emplace_back(v, stopDist);
2663 : foundRealStop = true;
2664 : break;
2665 : }
2666 : }
2667 : }
2668 : }
2669 : }
2670 : if (foundRealStop) {
2671 : break;
2672 : }
2673 :
2674 : // move to next lane
2675 : // get the next link used
2676 1202188238 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2677 1202188238 : if (lane->isLinkEnd(link) && myLaneChangeModel->hasBlueLight() && myCurrEdge != myRoute->end() - 1) {
2678 : // emergency vehicle is on the wrong lane. Obtain the link that it would use from the correct turning lane
2679 : const int currentIndex = lane->getIndex();
2680 : const MSLane* bestJump = nullptr;
2681 194113 : for (const LaneQ& preb : getBestLanes()) {
2682 127666 : if (preb.allowsContinuation &&
2683 : (bestJump == nullptr
2684 3219 : || abs(currentIndex - preb.lane->getIndex()) < abs(currentIndex - bestJump->getIndex()))) {
2685 67451 : bestJump = preb.lane;
2686 : }
2687 : }
2688 66447 : if (bestJump != nullptr) {
2689 66447 : const MSEdge* nextEdge = *(myCurrEdge + 1);
2690 122669 : for (auto cand_it = bestJump->getLinkCont().begin(); cand_it != bestJump->getLinkCont().end(); cand_it++) {
2691 117294 : if (&(*cand_it)->getLane()->getEdge() == nextEdge) {
2692 : link = cand_it;
2693 : break;
2694 : }
2695 : }
2696 : }
2697 : }
2698 :
2699 : // Check whether this is a turn (to save info about the next upcoming turn)
2700 1202188238 : if (!encounteredTurn) {
2701 189868154 : if (!lane->isLinkEnd(link) && lane->getLinkCont().size() > 1) {
2702 19669468 : LinkDirection linkDir = (*link)->getDirection();
2703 19669468 : switch (linkDir) {
2704 : case LinkDirection::STRAIGHT:
2705 : case LinkDirection::NODIR:
2706 : break;
2707 7668408 : default:
2708 7668408 : nextTurn.first = seen;
2709 7668408 : nextTurn.second = *link;
2710 : encounteredTurn = true;
2711 : #ifdef DEBUG_NEXT_TURN
2712 : if (DEBUG_COND) {
2713 : std::cout << SIMTIME << " veh '" << getID() << "' nextTurn: " << toString(linkDir)
2714 : << " at " << nextTurn.first << "m." << std::endl;
2715 : }
2716 : #endif
2717 : }
2718 : }
2719 : }
2720 :
2721 : // check whether the vehicle is on its final edge
2722 2070844725 : if (myCurrEdge + view + 1 == myRoute->end()
2723 1202188238 : || (myParameter->arrivalEdge >= 0 && getRoutePosition() + view == myParameter->arrivalEdge)) {
2724 333531751 : const double arrivalSpeed = (myParameter->arrivalSpeedProcedure == ArrivalSpeedDefinition::GIVEN ?
2725 : myParameter->arrivalSpeed : laneMaxV);
2726 : // subtract the arrival speed from the remaining distance so we get one additional driving step with arrival speed
2727 : // XXX: This does not work for ballistic update refs #2579
2728 333531751 : const double distToArrival = seen + myArrivalPos - lane->getLength() - SPEED2DIST(arrivalSpeed);
2729 333531751 : const double va = MAX2(NUMERICAL_EPS, cfModel.freeSpeed(this, getSpeed(), distToArrival, arrivalSpeed));
2730 333531751 : v = MIN2(v, va);
2731 333531751 : if (lastLink != nullptr) {
2732 : lastLink->adaptLeaveSpeed(va);
2733 : }
2734 333531751 : lfLinks.push_back(DriveProcessItem(v, seen, lane->getEdge().isFringe() ? 1000 : 0));
2735 333531751 : break;
2736 : }
2737 : // check whether the lane or the shadowLane is a dead end (allow some leeway on intersections)
2738 : if (lane->isLinkEnd(link)
2739 859596658 : || (MSGlobals::gSublane && brakeForOverlap(*link, lane))
2740 1727909765 : || (opposite && (*link)->getViaLaneOrLane()->getParallelOpposite() == nullptr
2741 212827 : && !myLaneChangeModel->hasBlueLight())) {
2742 9655903 : double va = cfModel.stopSpeed(this, getSpeed(), seen);
2743 9655903 : if (lastLink != nullptr) {
2744 : lastLink->adaptLeaveSpeed(va);
2745 : }
2746 9655903 : if (myLaneChangeModel->getCommittedSpeed() > 0) {
2747 367243 : v = MIN2(myLaneChangeModel->getCommittedSpeed(), v);
2748 : } else {
2749 18033412 : v = MIN2(va, v);
2750 : }
2751 : #ifdef DEBUG_PLAN_MOVE
2752 : if (DEBUG_COND) {
2753 : std::cout << " braking for link end lane=" << lane->getID() << " seen=" << seen
2754 : << " overlap=" << getLateralOverlap() << " va=" << va << " committed=" << myLaneChangeModel->getCommittedSpeed() << " v=" << v << "\n";
2755 :
2756 : }
2757 : #endif
2758 9655903 : if (lane->isLinkEnd(link)) {
2759 9059829 : lfLinks.emplace_back(v, seen);
2760 : break;
2761 : }
2762 : }
2763 859596658 : lateralShift += (*link)->getLateralShift();
2764 859596658 : const bool yellowOrRed = (*link)->haveRed() || (*link)->haveYellow();
2765 : // We distinguish 3 cases when determining the point at which a vehicle stops:
2766 : // - allway_stop: the vehicle should stop close to the stop line but may stop at larger distance
2767 : // - red/yellow light: here the vehicle 'knows' that it will have priority eventually and does not need to stop on a precise spot
2768 : // - other types of minor links: the vehicle needs to stop as close to the junction as necessary
2769 : // to minimize the time window for passing the junction. If the
2770 : // vehicle 'decides' to accelerate and cannot enter the junction in
2771 : // the next step, new foes may appear and cause a collision (see #1096)
2772 : // - major links: stopping point is irrelevant
2773 : double laneStopOffset;
2774 859596658 : const double majorStopOffset = MAX2(getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP, DIST_TO_STOPLINE_EXPECT_PRIORITY), lane->getVehicleStopOffset(this));
2775 : // override low desired decel at yellow and red
2776 859596658 : const double stopDecel = yellowOrRed && !isRail() ? MAX2(MIN2(MSGlobals::gTLSYellowMinDecel, cfModel.getEmergencyDecel()), cfModel.getMaxDecel()) : cfModel.getMaxDecel();
2777 859596658 : const double brakeDist = cfModel.brakeGap(myState.mySpeed, stopDecel, 0);
2778 859596658 : const bool canBrakeBeforeLaneEnd = seen >= brakeDist;
2779 859596658 : const bool canBrakeBeforeStopLine = seen - lane->getVehicleStopOffset(this) >= brakeDist;
2780 859596658 : if (yellowOrRed) {
2781 : // Wait at red traffic light with full distance if possible
2782 : laneStopOffset = majorStopOffset;
2783 798368058 : } else if ((*link)->havePriority()) {
2784 : // On priority link, we should never stop below visibility distance
2785 754616148 : laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, majorStopOffset);
2786 : } else {
2787 43751910 : double minorStopOffset = MAX2(lane->getVehicleStopOffset(this),
2788 43751910 : getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP) - (*link)->getDistToFoePedCrossing());
2789 : #ifdef DEBUG_PLAN_MOVE
2790 : if (DEBUG_COND) {
2791 : std::cout << " minorStopOffset=" << minorStopOffset << " distToFoePedCrossing=" << (*link)->getDistToFoePedCrossing() << "\n";
2792 : }
2793 : #endif
2794 43751910 : if ((*link)->getState() == LINKSTATE_ALLWAY_STOP) {
2795 1425260 : minorStopOffset = MAX2(minorStopOffset, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP, 0));
2796 : } else {
2797 42326650 : minorStopOffset = MAX2(minorStopOffset, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_GAP_MINOR, 0));
2798 : }
2799 : // On minor link, we should likewise never stop below visibility distance
2800 43751910 : laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, minorStopOffset);
2801 : }
2802 : #ifdef DEBUG_PLAN_MOVE
2803 : if (DEBUG_COND) {
2804 : std::cout << SIMTIME << " veh=" << getID() << " desired stopOffset on lane '" << lane->getID() << "' is " << laneStopOffset << "\n";
2805 : }
2806 : #endif
2807 859596658 : if (canBrakeBeforeLaneEnd) {
2808 : // avoid emergency braking if possible
2809 832263525 : laneStopOffset = MIN2(laneStopOffset, seen - brakeDist);
2810 : }
2811 : laneStopOffset = MAX2(POSITION_EPS, laneStopOffset);
2812 859596658 : double stopDist = MAX2(0., seen - laneStopOffset);
2813 61228600 : if (yellowOrRed && getDevice(typeid(MSDevice_GLOSA)) != nullptr
2814 508 : && static_cast<MSDevice_GLOSA*>(getDevice(typeid(MSDevice_GLOSA)))->getOverrideSafety()
2815 859596658 : && static_cast<MSDevice_GLOSA*>(getDevice(typeid(MSDevice_GLOSA)))->isSpeedAdviceActive()) {
2816 : stopDist = std::numeric_limits<double>::max();
2817 : }
2818 859596658 : if (newStopDist != std::numeric_limits<double>::max()) {
2819 : stopDist = MAX2(stopDist, newStopDist);
2820 : }
2821 : #ifdef DEBUG_PLAN_MOVE
2822 : if (DEBUG_COND) {
2823 : std::cout << SIMTIME << " veh=" << getID() << " effective stopOffset on lane '" << lane->getID()
2824 : << "' is " << laneStopOffset << " (-> stopDist=" << stopDist << ")" << std::endl;
2825 : }
2826 : #endif
2827 859596658 : if (isRail()
2828 859596658 : && !lane->isInternal()) {
2829 : // check for train direction reversal
2830 3095072 : if (lane->getBidiLane() != nullptr
2831 3095072 : && (*link)->getLane()->getBidiLane() == lane) {
2832 631456 : double vMustReverse = getCarFollowModel().stopSpeed(this, getSpeed(), seen - POSITION_EPS);
2833 631456 : if (seen < 1) {
2834 2277 : mustSeeBeforeReversal = 2 * seen + getLength();
2835 : }
2836 1221824 : v = MIN2(v, vMustReverse);
2837 : }
2838 : // signal that is passed in the current step does not count
2839 6190144 : foundRailSignal |= ((*link)->getTLLogic() != nullptr
2840 693877 : && (*link)->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL
2841 3688401 : && seen > SPEED2DIST(v));
2842 : }
2843 :
2844 859596658 : bool canReverseEventually = false;
2845 859596658 : const double vReverse = checkReversal(canReverseEventually, laneMaxV, seen);
2846 859596658 : v = MIN2(v, vReverse);
2847 : #ifdef DEBUG_PLAN_MOVE
2848 : if (DEBUG_COND) {
2849 : std::cout << SIMTIME << " veh=" << getID() << " canReverseEventually=" << canReverseEventually << " v=" << v << "\n";
2850 : }
2851 : #endif
2852 :
2853 : // check whether we need to slow down in order to finish a continuous lane change
2854 859596658 : if (myLaneChangeModel->isChangingLanes()) {
2855 : if ( // slow down to finish lane change before a turn lane
2856 179797 : ((*link)->getDirection() == LinkDirection::LEFT || (*link)->getDirection() == LinkDirection::RIGHT) ||
2857 : // slow down to finish lane change before the shadow lane ends
2858 146834 : (myLaneChangeModel->getShadowLane() != nullptr &&
2859 146834 : (*link)->getViaLaneOrLane()->getParallelLane(myLaneChangeModel->getShadowDirection()) == nullptr)) {
2860 : // XXX maybe this is too harsh. Vehicles could cut some corners here
2861 54539 : const double timeRemaining = STEPS2TIME(myLaneChangeModel->remainingTime());
2862 : assert(timeRemaining != 0);
2863 : // XXX: Euler-logic (#860), but I couldn't identify problems from this yet (Leo). Refs. #2575
2864 54539 : const double va = MAX2(cfModel.stopSpeed(this, getSpeed(), seen - POSITION_EPS),
2865 54539 : (seen - POSITION_EPS) / timeRemaining);
2866 : #ifdef DEBUG_PLAN_MOVE
2867 : if (DEBUG_COND) {
2868 : std::cout << SIMTIME << " veh=" << getID() << " slowing down to finish continuous change before"
2869 : << " link=" << (*link)->getViaLaneOrLane()->getID()
2870 : << " timeRemaining=" << timeRemaining
2871 : << " v=" << v
2872 : << " va=" << va
2873 : << std::endl;
2874 : }
2875 : #endif
2876 108595 : v = MIN2(va, v);
2877 : }
2878 : }
2879 :
2880 : // - always issue a request to leave the intersection we are currently on
2881 859596658 : const bool leavingCurrentIntersection = myLane->getEdge().isInternal() && lastLink == nullptr;
2882 : // - do not issue a request to enter an intersection after we already slowed down for an earlier one
2883 859596658 : const bool abortRequestAfterMinor = slowedDownForMinor && (*link)->getInternalLaneBefore() == nullptr;
2884 : // - even if red, if we cannot break we should issue a request
2885 859596658 : bool setRequest = (v > NUMERICAL_EPS_SPEED && !abortRequestAfterMinor) || (leavingCurrentIntersection);
2886 :
2887 859596658 : double stopSpeed = cfModel.stopSpeed(this, getSpeed(), stopDist, stopDecel, MSCFModel::CalcReason::CURRENT_WAIT);
2888 859596658 : double vLinkWait = MIN2(v, stopSpeed);
2889 : #ifdef DEBUG_PLAN_MOVE
2890 : if (DEBUG_COND) {
2891 : std::cout
2892 : << " stopDist=" << stopDist
2893 : << " stopDecel=" << stopDecel
2894 : << " vLinkWait=" << vLinkWait
2895 : << " brakeDist=" << brakeDist
2896 : << " seen=" << seen
2897 : << " leaveIntersection=" << leavingCurrentIntersection
2898 : << " setRequest=" << setRequest
2899 : //<< std::setprecision(16)
2900 : //<< " v=" << v
2901 : //<< " speedEps=" << NUMERICAL_EPS_SPEED
2902 : //<< std::setprecision(gPrecision)
2903 : << "\n";
2904 : }
2905 : #endif
2906 :
2907 859596658 : if (yellowOrRed && canBrakeBeforeStopLine && !ignoreRed(*link, canBrakeBeforeStopLine) && seen >= mustSeeBeforeReversal) {
2908 61166667 : if (lane->isInternal()) {
2909 42351 : checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2910 : }
2911 : // arrivalSpeed / arrivalTime when braking for red light is only relevent for rail signal switching
2912 61166667 : const SUMOTime arrivalTime = getArrivalTime(t, seen, v, vLinkPass);
2913 : // the vehicle is able to brake in front of a yellow/red traffic light
2914 61166667 : lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, false, arrivalTime, vLinkWait, 0, seen, -1));
2915 : //lfLinks.push_back(DriveProcessItem(0, vLinkWait, vLinkWait, false, 0, 0, stopDist));
2916 61166667 : break;
2917 : }
2918 :
2919 798429991 : const MSLink* entryLink = (*link)->getCorrespondingEntryLink();
2920 798429991 : if (entryLink->haveRed() && ignoreRed(*link, canBrakeBeforeStopLine) && STEPS2TIME(t - entryLink->getLastStateChange()) > 2) {
2921 : // restrict speed when ignoring a red light
2922 118299 : const double redSpeed = MIN2(v, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_RED_SPEED, v));
2923 118299 : const double va = MAX2(redSpeed, cfModel.freeSpeed(this, getSpeed(), seen, redSpeed));
2924 236149 : v = MIN2(va, v);
2925 : #ifdef DEBUG_PLAN_MOVE
2926 : if (DEBUG_COND) std::cout
2927 : << " ignoreRed spent=" << STEPS2TIME(t - (*link)->getLastStateChange())
2928 : << " redSpeed=" << redSpeed
2929 : << " va=" << va
2930 : << " v=" << v
2931 : << "\n";
2932 : #endif
2933 : }
2934 :
2935 798429991 : checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2936 :
2937 798429991 : if (lastLink != nullptr) {
2938 : lastLink->adaptLeaveSpeed(laneMaxV);
2939 : }
2940 798429991 : double arrivalSpeed = vLinkPass;
2941 : // vehicles should decelerate when approaching a minor link
2942 : // - unless they are close enough to have clear visibility of all relevant foe lanes and may start to accelerate again
2943 : // - and unless they are so close that stopping is impossible (i.e. when a green light turns to yellow when close to the junction)
2944 :
2945 : // whether the vehicle/driver is close enough to the link to see all possible foes #2123
2946 798429991 : const double visibilityDistance = (*link)->getFoeVisibilityDistance();
2947 798429991 : const double determinedFoePresence = seen <= visibilityDistance;
2948 : // // VARIANT: account for time needed to recognize whether relevant vehicles are on the foe lanes. (Leo)
2949 : // double foeRecognitionTime = 0.0;
2950 : // double determinedFoePresence = seen < visibilityDistance - myState.mySpeed*foeRecognitionTime;
2951 :
2952 : #ifdef DEBUG_PLAN_MOVE
2953 : if (DEBUG_COND) {
2954 : std::cout << " approaching link=" << (*link)->getViaLaneOrLane()->getID() << " prio=" << (*link)->havePriority() << " seen=" << seen << " visibilityDistance=" << visibilityDistance << " brakeDist=" << brakeDist << "\n";
2955 : }
2956 : #endif
2957 :
2958 798429991 : const bool couldBrakeForMinor = !(*link)->havePriority() && brakeDist < seen && !(*link)->lastWasContMajor();
2959 43251112 : if (couldBrakeForMinor && !determinedFoePresence) {
2960 : // vehicle decelerates just enough to be able to stop if necessary and then accelerates
2961 40459648 : double maxSpeedAtVisibilityDist = cfModel.maximumSafeStopSpeed(visibilityDistance, cfModel.getMaxDecel(), myState.mySpeed, false, 0., false);
2962 : // XXX: estimateSpeedAfterDistance does not use euler-logic (thus returns a lower value than possible here...)
2963 40459648 : double maxArrivalSpeed = cfModel.estimateSpeedAfterDistance(visibilityDistance, maxSpeedAtVisibilityDist, cfModel.getMaxAccel());
2964 40459648 : arrivalSpeed = MIN2(vLinkPass, maxArrivalSpeed);
2965 : slowedDownForMinor = true;
2966 : #ifdef DEBUG_PLAN_MOVE
2967 : if (DEBUG_COND) {
2968 : std::cout << " slowedDownForMinor maxSpeedAtVisDist=" << maxSpeedAtVisibilityDist << " maxArrivalSpeed=" << maxArrivalSpeed << " arrivalSpeed=" << arrivalSpeed << "\n";
2969 : }
2970 : #endif
2971 757970343 : } else if ((*link)->getState() == LINKSTATE_EQUAL && myWaitingTime > 0) {
2972 : // check for deadlock (circular yielding)
2973 : //std::cout << SIMTIME << " veh=" << getID() << " check rbl-deadlock\n";
2974 2810 : std::pair<const SUMOVehicle*, const MSLink*> blocker = (*link)->getFirstApproachingFoe(*link);
2975 : //std::cout << " blocker=" << Named::getIDSecure(blocker.first) << "\n";
2976 : int n = 100;
2977 5622 : while (blocker.second != nullptr && blocker.second != *link && n > 0) {
2978 2812 : blocker = blocker.second->getFirstApproachingFoe(*link);
2979 2812 : n--;
2980 : //std::cout << " blocker=" << Named::getIDSecure(blocker.first) << "\n";
2981 : }
2982 2810 : if (n == 0) {
2983 0 : WRITE_WARNINGF(TL("Suspicious right_before_left junction '%'."), lane->getEdge().getToJunction()->getID());
2984 : }
2985 : //std::cout << " blockerLink=" << blocker.second << " link=" << *link << "\n";
2986 2810 : if (blocker.second == *link) {
2987 489 : const double threshold = (*link)->getDirection() == LinkDirection::STRAIGHT ? 0.25 : 0.75;
2988 489 : if (RandHelper::rand(getRNG()) < threshold) {
2989 : //std::cout << " abort request, threshold=" << threshold << "\n";
2990 308 : setRequest = false;
2991 : }
2992 : }
2993 : }
2994 :
2995 798429991 : const SUMOTime arrivalTime = getArrivalTime(t, seen, v, arrivalSpeed);
2996 798429991 : if (couldBrakeForMinor && determinedFoePresence && (*link)->getLane()->getEdge().isRoundabout()) {
2997 880232 : const bool wasOpened = (*link)->opened(arrivalTime, arrivalSpeed, arrivalSpeed,
2998 880232 : getLength(), getImpatience(),
2999 : getCarFollowModel().getMaxDecel(),
3000 880232 : getWaitingTime(), getLateralPositionOnLane(),
3001 : nullptr, false, this);
3002 880232 : if (!wasOpened) {
3003 : slowedDownForMinor = true;
3004 : }
3005 : #ifdef DEBUG_PLAN_MOVE
3006 : if (DEBUG_COND) {
3007 : std::cout << " slowedDownForMinor at roundabout=" << (!wasOpened) << "\n";
3008 : }
3009 : #endif
3010 : }
3011 :
3012 : // compute arrival speed and arrival time if vehicle starts braking now
3013 : // if stopping is possible, arrivalTime can be arbitrarily large. A small value keeps fractional times (impatience) meaningful
3014 : double arrivalSpeedBraking = 0;
3015 798429991 : const double bGap = cfModel.brakeGap(v);
3016 798429991 : if (seen < bGap && !isStopped() && !planningToStop) { // XXX: should this use the current speed (at least for the ballistic case)? (Leo) Refs. #2575
3017 : // vehicle cannot come to a complete stop in time
3018 56616956 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3019 53901523 : arrivalSpeedBraking = cfModel.getMinimalArrivalSpeedEuler(seen, v);
3020 : // due to discrete/continuous mismatch (when using Euler update) we have to ensure that braking actually helps
3021 : arrivalSpeedBraking = MIN2(arrivalSpeedBraking, arrivalSpeed);
3022 : } else {
3023 2715433 : arrivalSpeedBraking = cfModel.getMinimalArrivalSpeed(seen, myState.mySpeed);
3024 : }
3025 : }
3026 :
3027 : // estimate leave speed for passing time computation
3028 : // l=linkLength, a=accel, t=continuousTime, v=vLeave
3029 : // l=v*t + 0.5*a*t^2, solve for t and multiply with a, then add v
3030 1183145343 : const double estimatedLeaveSpeed = MIN2((*link)->getViaLaneOrLane()->getVehicleMaxSpeed(this, maxVD),
3031 798429991 : getCarFollowModel().estimateSpeedAfterDistance((*link)->getLength(), arrivalSpeed, getVehicleType().getCarFollowModel().getMaxAccel()));
3032 798429991 : lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, setRequest,
3033 : arrivalTime, arrivalSpeed,
3034 : arrivalSpeedBraking,
3035 : seen, estimatedLeaveSpeed));
3036 798429991 : if ((*link)->getViaLane() == nullptr) {
3037 : hadNonInternal = true;
3038 : ++view;
3039 : }
3040 : #ifdef DEBUG_PLAN_MOVE
3041 : if (DEBUG_COND) {
3042 : std::cout << " checkAbort setRequest=" << setRequest << " v=" << v << " seen=" << seen << " dist=" << dist
3043 : << " seenNonInternal=" << seenNonInternal
3044 : << " seenInternal=" << seenInternal << " length=" << vehicleLength << "\n";
3045 : }
3046 : #endif
3047 : // we need to look ahead far enough to see available space for checkRewindLinkLanes
3048 824597744 : if ((!setRequest || v <= 0 || seen > dist) && hadNonInternal && seenNonInternal > MAX2(vehicleLength * CRLL_LOOK_AHEAD, vehicleLength + seenInternal) && foundRailSignal) {
3049 : break;
3050 : }
3051 : // get the following lane
3052 : lane = (*link)->getViaLaneOrLane();
3053 580581596 : laneMaxV = lane->getVehicleMaxSpeed(this, maxVD);
3054 580981107 : if (myInfluencer && !myInfluencer->considerSpeedLimit()) {
3055 : laneMaxV = std::numeric_limits<double>::max();
3056 : }
3057 : // the link was passed
3058 : // compute the velocity to use when the link is not blocked by other vehicles
3059 : // the vehicle shall be not faster when reaching the next lane than allowed
3060 : // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
3061 580581596 : const double va = MAX2(cfModel.freeSpeed(this, getSpeed(), seen, laneMaxV), vMinComfortable - NUMERICAL_EPS);
3062 1151393928 : v = MIN2(va, v);
3063 : #ifdef DEBUG_PLAN_MOVE
3064 : if (DEBUG_COND) {
3065 : std::cout << " laneMaxV=" << laneMaxV << " freeSpeed=" << va << " v=" << v << "\n";
3066 : }
3067 : #endif
3068 580581596 : if (lane->getEdge().isInternal()) {
3069 253021040 : seenInternal += lane->getLength();
3070 : } else {
3071 327560556 : seenNonInternal += lane->getLength();
3072 : }
3073 : // do not restrict results to the current vehicle to allow caching for the current time step
3074 580581596 : leaderLane = opposite ? lane->getParallelOpposite() : lane;
3075 580581596 : if (leaderLane == nullptr) {
3076 :
3077 : break;
3078 : }
3079 1160741384 : ahead = opposite ? MSLeaderInfo(leaderLane->getWidth()) : leaderLane->getLastVehicleInformation(nullptr, 0);
3080 580370692 : seen += lane->getLength();
3081 1160741384 : vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(lane->getLength(), v, cfModel.getMaxAccel()), laneMaxV); // upper bound
3082 : lastLink = &lfLinks.back();
3083 580370692 : }
3084 :
3085 : //#ifdef DEBUG_PLAN_MOVE
3086 : // if(DEBUG_COND){
3087 : // std::cout << "planMoveInternal found safe speed v = " << v << std::endl;
3088 : // }
3089 : //#endif
3090 :
3091 : #ifdef PARALLEL_STOPWATCH
3092 : myLane->getStopWatch()[0].stop();
3093 : #endif
3094 631599154 : }
3095 :
3096 :
3097 : double
3098 2786 : MSVehicle::slowDownForSchedule(double vMinComfortable) const {
3099 2786 : const double sfp = getVehicleType().getParameter().speedFactorPremature;
3100 : const MSStop& stop = myStops.front();
3101 2786 : std::pair<double, double> timeDist = estimateTimeToNextStop();
3102 2786 : double arrivalDelay = SIMTIME + timeDist.first - STEPS2TIME(stop.pars.arrival);
3103 2786 : double t = STEPS2TIME(stop.pars.arrival - SIMSTEP);
3104 5572 : if (stop.pars.hasParameter(toString(SUMO_ATTR_FLEX_ARRIVAL))) {
3105 150 : SUMOTime flexStart = string2time(stop.pars.getParameter(toString(SUMO_ATTR_FLEX_ARRIVAL)));
3106 75 : arrivalDelay += STEPS2TIME(stop.pars.arrival - flexStart);
3107 75 : t = STEPS2TIME(flexStart - SIMSTEP);
3108 2711 : } else if (stop.pars.started >= 0 && MSGlobals::gUseStopStarted) {
3109 200 : arrivalDelay += STEPS2TIME(stop.pars.arrival - stop.pars.started);
3110 200 : t = STEPS2TIME(stop.pars.started - SIMSTEP);
3111 : }
3112 2786 : if (arrivalDelay < 0 && sfp < getChosenSpeedFactor()) {
3113 : // we can slow down to better match the schedule (and increase energy efficiency)
3114 2721 : const double vSlowDownMin = MAX2(myLane->getSpeedLimit() * sfp, vMinComfortable);
3115 2721 : const double s = timeDist.second;
3116 : const double b = getCarFollowModel().getMaxDecel();
3117 : // x = speed for arriving in t seconds
3118 : // u = time at full speed
3119 : // u * x + (t - u) * 0.5 * x = s
3120 : // t - u = x / b
3121 : // eliminate u, solve x
3122 2721 : const double radicand = 4 * t * t * b * b - 8 * s * b;
3123 2721 : const double x = radicand >= 0 ? t * b - sqrt(radicand) * 0.5 : vSlowDownMin;
3124 2721 : double vSlowDown = x < vSlowDownMin ? vSlowDownMin : x;
3125 : #ifdef DEBUG_PLAN_MOVE
3126 : if (DEBUG_COND) {
3127 : std::cout << SIMTIME << " veh=" << getID() << " ad=" << arrivalDelay << " t=" << t << " vsm=" << vSlowDownMin
3128 : << " r=" << radicand << " vs=" << vSlowDown << "\n";
3129 : }
3130 : #endif
3131 2721 : return vSlowDown;
3132 65 : } else if (arrivalDelay > 0 && sfp > getChosenSpeedFactor()) {
3133 : // in principle we could up to catch up with the schedule
3134 : // but at this point we can only lower the speed, the
3135 : // information would have to be used when computing getVehicleMaxSpeed
3136 : }
3137 65 : return getMaxSpeed();
3138 : }
3139 :
3140 : SUMOTime
3141 859596658 : MSVehicle::getArrivalTime(SUMOTime t, double seen, double v, double arrivalSpeed) const {
3142 : const MSCFModel& cfModel = getCarFollowModel();
3143 : SUMOTime arrivalTime;
3144 859596658 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3145 : // @note intuitively it would make sense to compare arrivalSpeed with getSpeed() instead of v
3146 : // however, due to the current position update rule (ticket #860) the vehicle moves with v in this step
3147 : // subtract DELTA_T because t is the time at the end of this step and the movement is not carried out yet
3148 802463972 : arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, v, arrivalSpeed);
3149 : } else {
3150 57132686 : arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, myState.mySpeed, arrivalSpeed);
3151 : }
3152 859596658 : if (isStopped()) {
3153 9580944 : arrivalTime += MAX2((SUMOTime)0, myStops.front().duration);
3154 : }
3155 859596658 : return arrivalTime;
3156 : }
3157 :
3158 :
3159 : void
3160 1216986831 : MSVehicle::adaptToLeaders(const MSLeaderInfo& ahead, double latOffset,
3161 : const double seen, DriveProcessItem* const lastLink,
3162 : const MSLane* const lane, double& v, double& vLinkPass) const {
3163 : int rightmost;
3164 : int leftmost;
3165 1216986831 : ahead.getSubLanes(this, latOffset, rightmost, leftmost);
3166 : #ifdef DEBUG_PLAN_MOVE
3167 : if (DEBUG_COND) std::cout << SIMTIME
3168 : << "\nADAPT_TO_LEADERS\nveh=" << getID()
3169 : << " lane=" << lane->getID()
3170 : << " latOffset=" << latOffset
3171 : << " rm=" << rightmost
3172 : << " lm=" << leftmost
3173 : << " shift=" << ahead.getSublaneOffset()
3174 : << " ahead=" << ahead.toString()
3175 : << "\n";
3176 : #endif
3177 : /*
3178 : if (myLaneChangeModel->getCommittedSpeed() > 0) {
3179 : v = MIN2(v, myLaneChangeModel->getCommittedSpeed());
3180 : vLinkPass = MIN2(vLinkPass, myLaneChangeModel->getCommittedSpeed());
3181 : #ifdef DEBUG_PLAN_MOVE
3182 : if (DEBUG_COND) std::cout << " hasCommitted=" << myLaneChangeModel->getCommittedSpeed() << "\n";
3183 : #endif
3184 : return;
3185 : }
3186 : */
3187 2983438493 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
3188 1766451662 : const MSVehicle* pred = ahead[sublane];
3189 1766451662 : if (pred != nullptr && pred != this) {
3190 : // @todo avoid multiple adaptations to the same leader
3191 1293067377 : const double predBack = pred->getBackPositionOnLane(lane);
3192 : double gap = (lastLink == nullptr
3193 1852298744 : ? predBack - myState.myPos - getVehicleType().getMinGap()
3194 559231367 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3195 : bool oncoming = false;
3196 1293067377 : if (myLaneChangeModel->isOpposite()) {
3197 26975 : if (pred->getLaneChangeModel().isOpposite() || lane == pred->getLaneChangeModel().getShadowLane()) {
3198 : // ego might and leader are driving against lane
3199 : gap = (lastLink == nullptr
3200 0 : ? myState.myPos - predBack - getVehicleType().getMinGap()
3201 0 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3202 : } else {
3203 : // ego and leader are driving in the same direction as lane (shadowlane for ego)
3204 : gap = (lastLink == nullptr
3205 27703 : ? predBack - (myLane->getLength() - myState.myPos) - getVehicleType().getMinGap()
3206 728 : : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
3207 : }
3208 1293040402 : } else if (pred->getLaneChangeModel().isOpposite() && pred->getLaneChangeModel().getShadowLane() != lane) {
3209 : // must react to stopped / dangerous oncoming vehicles
3210 188934 : gap += -pred->getVehicleType().getLength() + getVehicleType().getMinGap() - MAX2(getVehicleType().getMinGap(), pred->getVehicleType().getMinGap());
3211 : // try to avoid collision in the next second
3212 188934 : const double predMaxDist = pred->getSpeed() + pred->getCarFollowModel().getMaxAccel();
3213 : #ifdef DEBUG_PLAN_MOVE
3214 : if (DEBUG_COND) {
3215 : std::cout << " fixedGap=" << gap << " predMaxDist=" << predMaxDist << "\n";
3216 : }
3217 : #endif
3218 188934 : if (gap < predMaxDist + getSpeed() || pred->getLane() == lane->getBidiLane()) {
3219 20613 : gap -= predMaxDist;
3220 : }
3221 1292851468 : } else if (pred->getLane() == lane->getBidiLane()) {
3222 675840 : gap -= pred->getVehicleType().getLengthWithGap();
3223 : oncoming = true;
3224 : }
3225 : #ifdef DEBUG_PLAN_MOVE
3226 : if (DEBUG_COND) {
3227 : 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";
3228 : }
3229 : #endif
3230 675840 : if (oncoming && gap >= 0) {
3231 607062 : adaptToOncomingLeader(std::make_pair(pred, gap), lastLink, v, vLinkPass);
3232 : } else {
3233 1292460315 : adaptToLeader(std::make_pair(pred, gap), seen, lastLink, v, vLinkPass);
3234 : }
3235 : }
3236 : }
3237 1216986831 : }
3238 :
3239 : void
3240 427747 : MSVehicle::adaptToLeaderDistance(const MSLeaderDistanceInfo& ahead, double latOffset,
3241 : double seen,
3242 : DriveProcessItem* const lastLink,
3243 : double& v, double& vLinkPass) const {
3244 : int rightmost;
3245 : int leftmost;
3246 427747 : ahead.getSubLanes(this, latOffset, rightmost, leftmost);
3247 : #ifdef DEBUG_PLAN_MOVE
3248 : if (DEBUG_COND) std::cout << SIMTIME
3249 : << "\nADAPT_TO_LEADERS_DISTANCE\nveh=" << getID()
3250 : << " latOffset=" << latOffset
3251 : << " rm=" << rightmost
3252 : << " lm=" << leftmost
3253 : << " ahead=" << ahead.toString()
3254 : << "\n";
3255 : #endif
3256 1048495 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
3257 620748 : CLeaderDist predDist = ahead[sublane];
3258 620748 : const MSVehicle* pred = predDist.first;
3259 620748 : if (pred != nullptr && pred != this) {
3260 : #ifdef DEBUG_PLAN_MOVE
3261 : if (DEBUG_COND) {
3262 : std::cout << " pred=" << pred->getID() << " predLane=" << pred->getLane()->getID() << " predPos=" << pred->getPositionOnLane() << " gap=" << predDist.second << "\n";
3263 : }
3264 : #endif
3265 393809 : adaptToLeader(predDist, seen, lastLink, v, vLinkPass);
3266 : }
3267 : }
3268 427747 : }
3269 :
3270 :
3271 : void
3272 1292854124 : MSVehicle::adaptToLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3273 : double seen,
3274 : DriveProcessItem* const lastLink,
3275 : double& v, double& vLinkPass) const {
3276 1292854124 : if (leaderInfo.first != 0) {
3277 1292854124 : if (ignoreFoe(leaderInfo.first)) {
3278 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3279 : if (DEBUG_COND) {
3280 : std::cout << " foe ignored\n";
3281 : }
3282 : #endif
3283 : return;
3284 : }
3285 : const MSCFModel& cfModel = getCarFollowModel();
3286 : double vsafeLeader = 0;
3287 1292853318 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
3288 : vsafeLeader = -std::numeric_limits<double>::max();
3289 : }
3290 : bool backOnRoute = true;
3291 1292853318 : if (leaderInfo.second < 0 && lastLink != nullptr && lastLink->myLink != nullptr) {
3292 : backOnRoute = false;
3293 : // this can either be
3294 : // a) a merging situation (leader back is is not our route) or
3295 : // b) a minGap violation / collision
3296 : MSLane* current = lastLink->myLink->getViaLaneOrLane();
3297 291921 : if (leaderInfo.first->getBackLane() == current) {
3298 : backOnRoute = true;
3299 : } else {
3300 674764 : for (MSLane* lane : getBestLanesContinuation()) {
3301 593522 : if (lane == current) {
3302 : break;
3303 : }
3304 448223 : if (leaderInfo.first->getBackLane() == lane) {
3305 : backOnRoute = true;
3306 : }
3307 : }
3308 : }
3309 : #ifdef DEBUG_PLAN_MOVE
3310 : if (DEBUG_COND) {
3311 : std::cout << SIMTIME << " current=" << current->getID() << " leaderBackLane=" << leaderInfo.first->getBackLane()->getID() << " backOnRoute=" << backOnRoute << "\n";
3312 : }
3313 : #endif
3314 226541 : if (!backOnRoute) {
3315 120730 : double stopDist = seen - current->getLength() - POSITION_EPS;
3316 120730 : if (lastLink->myLink->getInternalLaneBefore() != nullptr) {
3317 : // do not drive onto the junction conflict area
3318 102616 : stopDist -= lastLink->myLink->getInternalLaneBefore()->getLength();
3319 : }
3320 120730 : vsafeLeader = cfModel.stopSpeed(this, getSpeed(), stopDist);
3321 : }
3322 : }
3323 186110 : if (backOnRoute) {
3324 1292732588 : vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3325 : }
3326 1292853318 : if (lastLink != nullptr) {
3327 559182332 : const double futureVSafe = cfModel.followSpeed(this, lastLink->accelV, leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first, MSCFModel::CalcReason::FUTURE);
3328 : lastLink->adaptLeaveSpeed(futureVSafe);
3329 : #ifdef DEBUG_PLAN_MOVE
3330 : if (DEBUG_COND) {
3331 : std::cout << " vlinkpass=" << lastLink->myVLinkPass << " futureVSafe=" << futureVSafe << "\n";
3332 : }
3333 : #endif
3334 : }
3335 1292853318 : v = MIN2(v, vsafeLeader);
3336 2227286399 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3337 : #ifdef DEBUG_PLAN_MOVE
3338 : if (DEBUG_COND) std::cout
3339 : << SIMTIME
3340 : //std::cout << std::setprecision(10);
3341 : << " veh=" << getID()
3342 : << " lead=" << leaderInfo.first->getID()
3343 : << " leadSpeed=" << leaderInfo.first->getSpeed()
3344 : << " gap=" << leaderInfo.second
3345 : << " leadLane=" << leaderInfo.first->getLane()->getID()
3346 : << " predPos=" << leaderInfo.first->getPositionOnLane()
3347 : << " myLane=" << myLane->getID()
3348 : << " v=" << v
3349 : << " vSafeLeader=" << vsafeLeader
3350 : << " vLinkPass=" << vLinkPass
3351 : << "\n";
3352 : #endif
3353 : }
3354 : }
3355 :
3356 :
3357 : void
3358 18490372 : MSVehicle::adaptToJunctionLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3359 : const double seen, DriveProcessItem* const lastLink,
3360 : const MSLane* const lane, double& v, double& vLinkPass,
3361 : double distToCrossing) const {
3362 18490372 : if (leaderInfo.first != 0) {
3363 18490372 : if (ignoreFoe(leaderInfo.first)) {
3364 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3365 : if (DEBUG_COND) {
3366 : std::cout << " junction foe ignored\n";
3367 : }
3368 : #endif
3369 : return;
3370 : }
3371 : const MSCFModel& cfModel = getCarFollowModel();
3372 : double vsafeLeader = 0;
3373 18490361 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
3374 : vsafeLeader = -std::numeric_limits<double>::max();
3375 : }
3376 18490361 : if (leaderInfo.second >= 0) {
3377 15355360 : if (hasDeparted()) {
3378 15350825 : vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3379 : } else {
3380 : // called in the context of MSLane::isInsertionSuccess
3381 4535 : vsafeLeader = cfModel.insertionFollowSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCurrentApparentDecel(), leaderInfo.first);
3382 : }
3383 3135001 : } else if (leaderInfo.first != this) {
3384 : // the leading, in-lapping vehicle is occupying the complete next lane
3385 : // stop before entering this lane
3386 2712852 : vsafeLeader = cfModel.stopSpeed(this, getSpeed(), seen - lane->getLength() - POSITION_EPS);
3387 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3388 : if (DEBUG_COND) {
3389 : std::cout << SIMTIME << " veh=" << getID() << " stopping before junction: lane=" << lane->getID() << " seen=" << seen
3390 : << " laneLength=" << lane->getLength()
3391 : << " stopDist=" << seen - lane->getLength() - POSITION_EPS
3392 : << " vsafeLeader=" << vsafeLeader
3393 : << " distToCrossing=" << distToCrossing
3394 : << "\n";
3395 : }
3396 : #endif
3397 : }
3398 18490361 : if (distToCrossing >= 0) {
3399 : // can the leader still stop in the way?
3400 5892159 : const double vStop = cfModel.stopSpeed(this, getSpeed(), distToCrossing - getVehicleType().getMinGap());
3401 5892159 : if (leaderInfo.first == this) {
3402 : // braking for pedestrian
3403 410830 : const double vStopCrossing = cfModel.stopSpeed(this, getSpeed(), distToCrossing);
3404 : vsafeLeader = vStopCrossing;
3405 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3406 : if (DEBUG_COND) {
3407 : std::cout << " breaking for pedestrian distToCrossing=" << distToCrossing << " vStopCrossing=" << vStopCrossing << "\n";
3408 : }
3409 : #endif
3410 410830 : if (lastLink != nullptr) {
3411 : lastLink->adaptStopSpeed(vsafeLeader);
3412 : }
3413 5481329 : } else if (leaderInfo.second == -std::numeric_limits<double>::max()) {
3414 : // drive up to the crossing point and stop
3415 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3416 : if (DEBUG_COND) {
3417 : std::cout << " stop at crossing point for critical leader vStop=" << vStop << "\n";
3418 : };
3419 : #endif
3420 : vsafeLeader = MAX2(vsafeLeader, vStop);
3421 : } else {
3422 5430018 : const double leaderDistToCrossing = distToCrossing - leaderInfo.second;
3423 : // estimate the time at which the leader has gone past the crossing point
3424 5430018 : const double leaderPastCPTime = leaderDistToCrossing / MAX2(leaderInfo.first->getSpeed(), SUMO_const_haltingSpeed);
3425 : // reach distToCrossing after that time
3426 : // avgSpeed * leaderPastCPTime = distToCrossing
3427 : // ballistic: avgSpeed = (getSpeed + vFinal) / 2
3428 5430018 : const double vFinal = MAX2(getSpeed(), 2 * (distToCrossing - getVehicleType().getMinGap()) / leaderPastCPTime - getSpeed());
3429 5430018 : const double v2 = getSpeed() + ACCEL2SPEED((vFinal - getSpeed()) / leaderPastCPTime);
3430 : vsafeLeader = MAX2(vsafeLeader, MIN2(v2, vStop));
3431 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3432 : if (DEBUG_COND) {
3433 : std::cout << " driving up to the crossing point (distToCrossing=" << distToCrossing << ")"
3434 : << " leaderPastCPTime=" << leaderPastCPTime
3435 : << " vFinal=" << vFinal
3436 : << " v2=" << v2
3437 : << " vStop=" << vStop
3438 : << " vsafeLeader=" << vsafeLeader << "\n";
3439 : }
3440 : #endif
3441 : }
3442 : }
3443 18079531 : if (lastLink != nullptr) {
3444 : lastLink->adaptLeaveSpeed(vsafeLeader);
3445 : }
3446 18490361 : v = MIN2(v, vsafeLeader);
3447 34502949 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3448 : #ifdef DEBUG_PLAN_MOVE
3449 : if (DEBUG_COND) std::cout
3450 : << SIMTIME
3451 : //std::cout << std::setprecision(10);
3452 : << " veh=" << getID()
3453 : << " lead=" << leaderInfo.first->getID()
3454 : << " leadSpeed=" << leaderInfo.first->getSpeed()
3455 : << " gap=" << leaderInfo.second
3456 : << " leadLane=" << leaderInfo.first->getLane()->getID()
3457 : << " predPos=" << leaderInfo.first->getPositionOnLane()
3458 : << " seen=" << seen
3459 : << " lane=" << lane->getID()
3460 : << " myLane=" << myLane->getID()
3461 : << " dTC=" << distToCrossing
3462 : << " v=" << v
3463 : << " vSafeLeader=" << vsafeLeader
3464 : << " vLinkPass=" << vLinkPass
3465 : << "\n";
3466 : #endif
3467 : }
3468 : }
3469 :
3470 :
3471 : void
3472 607062 : MSVehicle::adaptToOncomingLeader(const std::pair<const MSVehicle*, double> leaderInfo,
3473 : DriveProcessItem* const lastLink,
3474 : double& v, double& vLinkPass) const {
3475 607062 : if (leaderInfo.first != 0) {
3476 607062 : if (ignoreFoe(leaderInfo.first)) {
3477 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3478 : if (DEBUG_COND) {
3479 : std::cout << " oncoming foe ignored\n";
3480 : }
3481 : #endif
3482 : return;
3483 : }
3484 : const MSCFModel& cfModel = getCarFollowModel();
3485 : const MSVehicle* lead = leaderInfo.first;
3486 : const MSCFModel& cfModelL = lead->getCarFollowModel();
3487 : // assume the leader reacts symmetrically (neither stopping instantly nor ignoring ego)
3488 607014 : const double leaderBrakeGap = cfModelL.brakeGap(lead->getSpeed(), cfModelL.getMaxDecel(), 0);
3489 607014 : const double egoBrakeGap = cfModel.brakeGap(getSpeed(), cfModel.getMaxDecel(), 0);
3490 607014 : const double gapSum = leaderBrakeGap + egoBrakeGap;
3491 : // ensure that both vehicles can leave an intersection if they are currently on it
3492 607014 : double egoExit = getDistanceToLeaveJunction();
3493 607014 : const double leaderExit = lead->getDistanceToLeaveJunction();
3494 : double gap = leaderInfo.second;
3495 607014 : if (egoExit + leaderExit < gap) {
3496 506932 : gap -= egoExit + leaderExit;
3497 : } else {
3498 : egoExit = 0;
3499 : }
3500 : // split any distance in excess of brakeGaps evenly
3501 607014 : const double freeGap = MAX2(0.0, gap - gapSum);
3502 : const double splitGap = MIN2(gap, gapSum);
3503 : // assume remaining distance is allocated in proportion to braking distance
3504 607014 : const double gapRatio = gapSum > 0 ? egoBrakeGap / gapSum : 0.5;
3505 607014 : const double vsafeLeader = cfModel.stopSpeed(this, getSpeed(), splitGap * gapRatio + egoExit + 0.5 * freeGap);
3506 607014 : if (lastLink != nullptr) {
3507 69296 : const double futureVSafe = cfModel.stopSpeed(this, lastLink->accelV, leaderInfo.second, MSCFModel::CalcReason::FUTURE);
3508 : lastLink->adaptLeaveSpeed(futureVSafe);
3509 : #ifdef DEBUG_PLAN_MOVE
3510 : if (DEBUG_COND) {
3511 : std::cout << " vlinkpass=" << lastLink->myVLinkPass << " futureVSafe=" << futureVSafe << "\n";
3512 : }
3513 : #endif
3514 : }
3515 607014 : v = MIN2(v, vsafeLeader);
3516 1198155 : vLinkPass = MIN2(vLinkPass, vsafeLeader);
3517 : #ifdef DEBUG_PLAN_MOVE
3518 : if (DEBUG_COND) std::cout
3519 : << SIMTIME
3520 : //std::cout << std::setprecision(10);
3521 : << " veh=" << getID()
3522 : << " oncomingLead=" << lead->getID()
3523 : << " leadSpeed=" << lead->getSpeed()
3524 : << " gap=" << leaderInfo.second
3525 : << " gap2=" << gap
3526 : << " gapRatio=" << gapRatio
3527 : << " leadLane=" << lead->getLane()->getID()
3528 : << " predPos=" << lead->getPositionOnLane()
3529 : << " myLane=" << myLane->getID()
3530 : << " v=" << v
3531 : << " vSafeLeader=" << vsafeLeader
3532 : << " vLinkPass=" << vLinkPass
3533 : << "\n";
3534 : #endif
3535 : }
3536 : }
3537 :
3538 :
3539 : void
3540 798479344 : MSVehicle::checkLinkLeaderCurrentAndParallel(const MSLink* link, const MSLane* lane, double seen,
3541 : DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest) const {
3542 798479344 : if (MSGlobals::gUsingInternalLanes && (myInfluencer == nullptr || myInfluencer->getRespectJunctionLeaderPriority())) {
3543 : // we want to pass the link but need to check for foes on internal lanes
3544 798370190 : checkLinkLeader(link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
3545 798370190 : if (myLaneChangeModel->getShadowLane() != nullptr) {
3546 3182388 : const MSLink* const parallelLink = link->getParallelLink(myLaneChangeModel->getShadowDirection());
3547 3182388 : if (parallelLink != nullptr) {
3548 2202904 : checkLinkLeader(parallelLink, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest, true);
3549 : }
3550 : }
3551 : }
3552 :
3553 798479344 : }
3554 :
3555 : void
3556 800818931 : MSVehicle::checkLinkLeader(const MSLink* link, const MSLane* lane, double seen,
3557 : DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest,
3558 : bool isShadowLink) const {
3559 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3560 : if (DEBUG_COND) {
3561 : gDebugFlag1 = true; // See MSLink::getLeaderInfo
3562 : }
3563 : #endif
3564 800818931 : const MSLink::LinkLeaders linkLeaders = link->getLeaderInfo(this, seen, nullptr, isShadowLink);
3565 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3566 : if (DEBUG_COND) {
3567 : gDebugFlag1 = false; // See MSLink::getLeaderInfo
3568 : }
3569 : #endif
3570 820249909 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
3571 : // the vehicle to enter the junction first has priority
3572 19430978 : const MSVehicle* leader = (*it).vehAndGap.first;
3573 19430978 : if (leader == nullptr) {
3574 : // leader is a pedestrian. Passing 'this' as a dummy.
3575 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3576 : if (DEBUG_COND) {
3577 : std::cout << SIMTIME << " veh=" << getID() << " is blocked on link to " << link->getViaLaneOrLane()->getID() << " by pedestrian. dist=" << it->distToCrossing << "\n";
3578 : }
3579 : #endif
3580 422845 : if (getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) > 0
3581 422845 : && getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) >= RandHelper::rand(getRNG())) {
3582 : #ifdef DEBUG_PLAN_MOVE
3583 : if (DEBUG_COND) {
3584 : std::cout << SIMTIME << " veh=" << getID() << " is ignoring pedestrian (jmIgnoreJunctionFoeProb)\n";
3585 : }
3586 : #endif
3587 696 : continue;
3588 : }
3589 422149 : adaptToJunctionLeader(std::make_pair(this, -1), seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
3590 : // if blocked by a pedestrian for too long we must yield our request
3591 422149 : if (v < SUMO_const_haltingSpeed && getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME)) {
3592 75591 : setRequest = false;
3593 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3594 : if (DEBUG_COND) {
3595 : std::cout << " aborting request\n";
3596 : }
3597 : #endif
3598 : }
3599 19008133 : } else if (isLeader(link, leader, (*it).vehAndGap.second) || (*it).inTheWay()) {
3600 18960355 : if (getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) > 0
3601 18960355 : && getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_JUNCTION_FOE_PROB, 0) >= RandHelper::rand(getRNG())) {
3602 : #ifdef DEBUG_PLAN_MOVE
3603 : if (DEBUG_COND) {
3604 : std::cout << SIMTIME << " veh=" << getID() << " is ignoring linkLeader=" << leader->getID() << " (jmIgnoreJunctionFoeProb)\n";
3605 : }
3606 : #endif
3607 2177 : continue;
3608 : }
3609 25980905 : if (MSGlobals::gLateralResolution > 0 &&
3610 : // sibling link (XXX: could also be partial occupator where this check fails)
3611 7022727 : &leader->getLane()->getEdge() == &lane->getEdge()) {
3612 : // check for sublane obstruction (trivial for sibling link leaders)
3613 : const MSLane* conflictLane = link->getInternalLaneBefore();
3614 920802 : MSLeaderInfo linkLeadersAhead = MSLeaderInfo(conflictLane->getWidth());
3615 920802 : linkLeadersAhead.addLeader(leader, false, 0); // assume sibling lane has the same geometry as the leader lane
3616 920802 : const double latOffset = isShadowLink ? (getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge()) : 0;
3617 : // leader is neither on lane nor conflictLane (the conflict is only established geometrically)
3618 920802 : adaptToLeaders(linkLeadersAhead, latOffset, seen, lastLink, leader->getLane(), v, vLinkPass);
3619 : #ifdef DEBUG_PLAN_MOVE
3620 : if (DEBUG_COND) {
3621 : std::cout << SIMTIME << " veh=" << getID()
3622 : << " siblingFoe link=" << link->getViaLaneOrLane()->getID()
3623 : << " isShadowLink=" << isShadowLink
3624 : << " lane=" << lane->getID()
3625 : << " foe=" << leader->getID()
3626 : << " foeLane=" << leader->getLane()->getID()
3627 : << " latOffset=" << latOffset
3628 : << " latOffsetFoe=" << leader->getLatOffset(lane)
3629 : << " linkLeadersAhead=" << linkLeadersAhead.toString()
3630 : << "\n";
3631 : }
3632 : #endif
3633 920802 : } else {
3634 : #ifdef DEBUG_PLAN_MOVE
3635 : if (DEBUG_COND) {
3636 : std::cout << SIMTIME << " veh=" << getID() << " linkLeader=" << leader->getID() << " gap=" << it->vehAndGap.second
3637 : << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
3638 : << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
3639 : << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
3640 : << "\n";
3641 : }
3642 : #endif
3643 18037376 : adaptToJunctionLeader(it->vehAndGap, seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
3644 : }
3645 18958178 : if (lastLink != nullptr) {
3646 : // we are not yet on the junction with this linkLeader.
3647 : // at least we can drive up to the previous link and stop there
3648 36492383 : v = MAX2(v, lastLink->myVLinkWait);
3649 : }
3650 : // if blocked by a leader from the same or next lane we must yield our request
3651 : // also, if blocked by a stopped or blocked leader
3652 18958178 : if (v < SUMO_const_haltingSpeed
3653 : //&& leader->getSpeed() < SUMO_const_haltingSpeed
3654 18958178 : && (leader->getLane()->getLogicalPredecessorLane() == myLane->getLogicalPredecessorLane()
3655 10495187 : || leader->getLane()->getLogicalPredecessorLane() == myLane
3656 8320184 : || leader->isStopped()
3657 8241685 : || leader->getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME))) {
3658 4456744 : setRequest = false;
3659 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3660 : if (DEBUG_COND) {
3661 : std::cout << " aborting request\n";
3662 : }
3663 : #endif
3664 4456744 : if (lastLink != nullptr && leader->getLane()->getLogicalPredecessorLane() == myLane) {
3665 : // we are not yet on the junction so must abort that request as well
3666 : // (or maybe we are already on the junction and the leader is a partial occupator beyond)
3667 2163248 : lastLink->mySetRequest = false;
3668 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3669 : if (DEBUG_COND) {
3670 : std::cout << " aborting previous request\n";
3671 : }
3672 : #endif
3673 : }
3674 : }
3675 : }
3676 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3677 : else {
3678 : if (DEBUG_COND) {
3679 : std::cout << SIMTIME << " veh=" << getID() << " ignoring leader " << leader->getID() << " gap=" << (*it).vehAndGap.second << " dtC=" << (*it).distToCrossing
3680 : << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
3681 : << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
3682 : << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
3683 : << "\n";
3684 : }
3685 : }
3686 : #endif
3687 : }
3688 : // if this is the link between two internal lanes we may have to slow down for pedestrians
3689 800818931 : vLinkWait = MIN2(vLinkWait, v);
3690 800818931 : }
3691 :
3692 :
3693 : double
3694 99479644 : MSVehicle::getDeltaPos(const double accel) const {
3695 99479644 : double vNext = myState.mySpeed + ACCEL2SPEED(accel);
3696 99479644 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3697 : // apply implicit Euler positional update
3698 0 : return SPEED2DIST(MAX2(vNext, 0.));
3699 : } else {
3700 : // apply ballistic update
3701 99479644 : if (vNext >= 0) {
3702 : // assume constant acceleration during this time step
3703 98842156 : return SPEED2DIST(myState.mySpeed + 0.5 * ACCEL2SPEED(accel));
3704 : } else {
3705 : // negative vNext indicates a stop within the middle of time step
3706 : // The corresponding stop time is s = mySpeed/deceleration \in [0,dt], and the
3707 : // covered distance is therefore deltaPos = mySpeed*s - 0.5*deceleration*s^2.
3708 : // Here, deceleration = (myState.mySpeed - vNext)/dt is the constant deceleration
3709 : // until the vehicle stops.
3710 637488 : return -SPEED2DIST(0.5 * myState.mySpeed * myState.mySpeed / ACCEL2SPEED(accel));
3711 : }
3712 : }
3713 : }
3714 :
3715 : void
3716 631599154 : MSVehicle::processLinkApproaches(double& vSafe, double& vSafeMin, double& vSafeMinDist) {
3717 :
3718 : const MSCFModel& cfModel = getCarFollowModel();
3719 : // Speed limit due to zipper merging
3720 : double vSafeZipper = std::numeric_limits<double>::max();
3721 :
3722 631599154 : myHaveToWaitOnNextLink = false;
3723 : bool canBrakeVSafeMin = false;
3724 :
3725 : // Get safe velocities from DriveProcessItems.
3726 : assert(myLFLinkLanes.size() != 0 || isRemoteControlled());
3727 1264743207 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
3728 1091130946 : MSLink* const link = dpi.myLink;
3729 :
3730 : #ifdef DEBUG_EXEC_MOVE
3731 : if (DEBUG_COND) {
3732 : std::cout
3733 : << SIMTIME
3734 : << " veh=" << getID()
3735 : << " link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID())
3736 : << " req=" << dpi.mySetRequest
3737 : << " vP=" << dpi.myVLinkPass
3738 : << " vW=" << dpi.myVLinkWait
3739 : << " d=" << dpi.myDistance
3740 : << "\n";
3741 : gDebugFlag1 = true; // See MSLink_DEBUG_OPENED
3742 : }
3743 : #endif
3744 :
3745 : // the vehicle must change the lane on one of the next lanes (XXX: refs to code further below???, Leo)
3746 1091130946 : if (link != nullptr && dpi.mySetRequest) {
3747 :
3748 : const LinkState ls = link->getState();
3749 : // vehicles should brake when running onto a yellow light if the distance allows to halt in front
3750 : const bool yellow = link->haveYellow();
3751 654193071 : const bool canBrake = (dpi.myDistance > cfModel.brakeGap(myState.mySpeed, cfModel.getMaxDecel(), 0.)
3752 654193071 : || (MSGlobals::gSemiImplicitEulerUpdate && myState.mySpeed < ACCEL2SPEED(cfModel.getMaxDecel())));
3753 : assert(link->getLaneBefore() != nullptr);
3754 654193071 : const bool beyondStopLine = dpi.myDistance < link->getLaneBefore()->getVehicleStopOffset(this);
3755 654193071 : const bool ignoreRedLink = ignoreRed(link, canBrake) || beyondStopLine;
3756 654193071 : if (yellow && canBrake && !ignoreRedLink) {
3757 10 : vSafe = dpi.myVLinkWait;
3758 10 : myHaveToWaitOnNextLink = true;
3759 : #ifdef DEBUG_CHECKREWINDLINKLANES
3760 : if (DEBUG_COND) {
3761 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (yellow)\n";
3762 : }
3763 : #endif
3764 21049018 : break;
3765 : }
3766 654193061 : const bool influencerPrio = (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority());
3767 : MSLink::BlockingFoes collectFoes;
3768 654193061 : bool opened = (yellow || influencerPrio
3769 1962254219 : || link->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
3770 654030579 : getVehicleType().getLength(),
3771 626719983 : canBrake ? getImpatience() : 1,
3772 : cfModel.getMaxDecel(),
3773 654030579 : getWaitingTimeFor(link), getLateralPositionOnLane(),
3774 : ls == LINKSTATE_ZIPPER ? &collectFoes : nullptr,
3775 654030579 : ignoreRedLink, this, dpi.myDistance));
3776 648532804 : if (opened && myLaneChangeModel->getShadowLane() != nullptr) {
3777 1939719 : const MSLink* const parallelLink = dpi.myLink->getParallelLink(myLaneChangeModel->getShadowDirection());
3778 1939719 : if (parallelLink != nullptr) {
3779 1206080 : const double shadowLatPos = getLateralPositionOnLane() - myLaneChangeModel->getShadowDirection() * 0.5 * (
3780 1206080 : myLane->getWidth() + myLaneChangeModel->getShadowLane()->getWidth());
3781 2411389 : opened = yellow || influencerPrio || (opened && parallelLink->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
3782 1205309 : getVehicleType().getLength(), getImpatience(),
3783 : cfModel.getMaxDecel(),
3784 : getWaitingTimeFor(link), shadowLatPos, nullptr,
3785 1205309 : ignoreRedLink, this, dpi.myDistance));
3786 : #ifdef DEBUG_EXEC_MOVE
3787 : if (DEBUG_COND) {
3788 : std::cout << SIMTIME
3789 : << " veh=" << getID()
3790 : << " shadowLane=" << myLaneChangeModel->getShadowLane()->getID()
3791 : << " shadowDir=" << myLaneChangeModel->getShadowDirection()
3792 : << " parallelLink=" << (parallelLink == 0 ? "NULL" : parallelLink->getViaLaneOrLane()->getID())
3793 : << " opened=" << opened
3794 : << "\n";
3795 : }
3796 : #endif
3797 : }
3798 : }
3799 : // vehicles should decelerate when approaching a minor link
3800 : #ifdef DEBUG_EXEC_MOVE
3801 : if (DEBUG_COND) {
3802 : std::cout << SIMTIME
3803 : << " opened=" << opened
3804 : << " influencerPrio=" << influencerPrio
3805 : << " linkPrio=" << link->havePriority()
3806 : << " lastContMajor=" << link->lastWasContMajor()
3807 : << " isCont=" << link->isCont()
3808 : << " ignoreRed=" << ignoreRedLink
3809 : << "\n";
3810 : }
3811 : #endif
3812 : double visibilityDistance = link->getFoeVisibilityDistance();
3813 654193061 : bool determinedFoePresence = dpi.myDistance <= visibilityDistance;
3814 654193061 : if (opened && !influencerPrio && !link->havePriority() && !link->lastWasContMajor() && !link->isCont() && !ignoreRedLink) {
3815 17681771 : if (!determinedFoePresence && (canBrake || !yellow)) {
3816 16697181 : vSafe = dpi.myVLinkWait;
3817 16697181 : myHaveToWaitOnNextLink = true;
3818 : #ifdef DEBUG_CHECKREWINDLINKLANES
3819 : if (DEBUG_COND) {
3820 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (minor)\n";
3821 : }
3822 : #endif
3823 16697181 : break;
3824 : } else {
3825 : // past the point of no return. we need to drive fast enough
3826 : // to make it across the link. However, minor slowdowns
3827 : // should be permissible to follow leading traffic safely
3828 : // basically, this code prevents dawdling
3829 : // (it's harder to do this later using
3830 : // SUMO_ATTR_JM_SIGMA_MINOR because we don't know whether the
3831 : // vehicle is already too close to stop at that part of the code)
3832 : //
3833 : // XXX: There is a problem in subsecond simulation: If we cannot
3834 : // make it across the minor link in one step, new traffic
3835 : // could appear on a major foe link and cause a collision. Refs. #1845, #2123
3836 984590 : vSafeMinDist = dpi.myDistance; // distance that must be covered
3837 984590 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3838 1798838 : vSafeMin = MIN3((double)DIST2SPEED(vSafeMinDist + POSITION_EPS), dpi.myVLinkPass, cfModel.maxNextSafeMin(getSpeed(), this));
3839 : } else {
3840 170342 : vSafeMin = MIN3((double)DIST2SPEED(2 * vSafeMinDist + NUMERICAL_EPS) - getSpeed(), dpi.myVLinkPass, cfModel.maxNextSafeMin(getSpeed(), this));
3841 : }
3842 : canBrakeVSafeMin = canBrake;
3843 : #ifdef DEBUG_EXEC_MOVE
3844 : if (DEBUG_COND) {
3845 : std::cout << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << " canBrake=" << canBrake << "\n";
3846 : }
3847 : #endif
3848 : }
3849 : }
3850 : // have waited; may pass if opened...
3851 637495880 : if (opened) {
3852 631814595 : vSafe = dpi.myVLinkPass;
3853 631814595 : if (vSafe < cfModel.getMaxDecel() && vSafe <= dpi.myVLinkWait && vSafe < cfModel.maxNextSpeed(getSpeed(), this)) {
3854 : // this vehicle is probably not gonna drive across the next junction (heuristic)
3855 55162317 : myHaveToWaitOnNextLink = true;
3856 : #ifdef DEBUG_CHECKREWINDLINKLANES
3857 : if (DEBUG_COND) {
3858 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (very slow)\n";
3859 : }
3860 : #endif
3861 : }
3862 631814595 : if (link->mustStop() && determinedFoePresence && myHaveStoppedFor == nullptr) {
3863 20820 : myHaveStoppedFor = link;
3864 : }
3865 5681285 : } else if (link->getState() == LINKSTATE_ZIPPER) {
3866 1329220 : vSafeZipper = MIN2(vSafeZipper,
3867 1329220 : link->getZipperSpeed(this, dpi.myDistance, dpi.myVLinkPass, dpi.myArrivalTime, &collectFoes));
3868 : } else if (!canBrake
3869 : // always brake hard for traffic lights (since an emergency stop is necessary anyway)
3870 1858 : && link->getTLLogic() == nullptr
3871 : // cannot brake even with emergency deceleration
3872 4352958 : && dpi.myDistance < cfModel.brakeGap(myState.mySpeed, cfModel.getEmergencyDecel(), 0.)) {
3873 : #ifdef DEBUG_EXEC_MOVE
3874 : if (DEBUG_COND) {
3875 : std::cout << SIMTIME << " too fast to brake for closed link\n";
3876 : }
3877 : #endif
3878 238 : vSafe = dpi.myVLinkPass;
3879 : } else {
3880 4351827 : vSafe = dpi.myVLinkWait;
3881 4351827 : myHaveToWaitOnNextLink = true;
3882 : #ifdef DEBUG_CHECKREWINDLINKLANES
3883 : if (DEBUG_COND) {
3884 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (closed)\n";
3885 : }
3886 : #endif
3887 : #ifdef DEBUG_EXEC_MOVE
3888 : if (DEBUG_COND) {
3889 : std::cout << SIMTIME << " braking for closed link=" << link->getViaLaneOrLane()->getID() << "\n";
3890 : }
3891 : #endif
3892 4351827 : break;
3893 : }
3894 633144053 : if (myLane->isInternal() && myJunctionEntryTime == SUMOTime_MAX) {
3895 : // request was renewed, restoring entry time
3896 : // @note: using myJunctionEntryTimeNeverYield could lead to inconsistencies with other vehicles already on the junction
3897 81637 : myJunctionEntryTime = SIMSTEP;;
3898 : }
3899 654193061 : } else {
3900 436937875 : if (link != nullptr && link->getInternalLaneBefore() != nullptr && myLane->isInternal() && link->getJunction() == myLane->getEdge().getToJunction()) {
3901 : // blocked on the junction. yield request so other vehicles may
3902 : // become junction leader
3903 : #ifdef DEBUG_EXEC_MOVE
3904 : if (DEBUG_COND) {
3905 : std::cout << SIMTIME << " resetting junctionEntryTime at junction '" << link->getJunction()->getID() << "' beause of non-request exitLink\n";
3906 : }
3907 : #endif
3908 242893 : myJunctionEntryTime = SUMOTime_MAX;
3909 242893 : myJunctionConflictEntryTime = SUMOTime_MAX;
3910 : }
3911 : // we have: i->link == 0 || !i->setRequest
3912 436937875 : vSafe = dpi.myVLinkWait;
3913 436937875 : if (link != nullptr || myStopDist < (myLane->getLength() - getPositionOnLane())) {
3914 107005182 : if (vSafe < getSpeed()) {
3915 16219093 : myHaveToWaitOnNextLink = true;
3916 : #ifdef DEBUG_CHECKREWINDLINKLANES
3917 : if (DEBUG_COND) {
3918 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, braking) vSafe=" << vSafe << "\n";
3919 : }
3920 : #endif
3921 90786089 : } else if (vSafe < SUMO_const_haltingSpeed) {
3922 63333774 : myHaveToWaitOnNextLink = true;
3923 : #ifdef DEBUG_CHECKREWINDLINKLANES
3924 : if (DEBUG_COND) {
3925 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, stopping)\n";
3926 : }
3927 : #endif
3928 : }
3929 : }
3930 334312851 : if (link == nullptr && myLFLinkLanes.size() == 1
3931 266134367 : && getBestLanesContinuation().size() > 1
3932 1194533 : && getBestLanesContinuation()[1]->hadPermissionChanges()
3933 437074487 : && myLane->getFirstAnyVehicle() == this) {
3934 : // temporal lane closing without notification, visible to the
3935 : // vehicle at the front of the queue
3936 34895 : updateBestLanes(true);
3937 : //std::cout << SIMTIME << " veh=" << getID() << " updated bestLanes=" << toString(getBestLanesContinuation()) << "\n";
3938 : }
3939 : break;
3940 : }
3941 : }
3942 :
3943 : //#ifdef DEBUG_EXEC_MOVE
3944 : // if (DEBUG_COND) {
3945 : // std::cout << "\nvCurrent = " << toString(getSpeed(), 24) << "" << std::endl;
3946 : // std::cout << "vSafe = " << toString(vSafe, 24) << "" << std::endl;
3947 : // std::cout << "vSafeMin = " << toString(vSafeMin, 24) << "" << std::endl;
3948 : // std::cout << "vSafeMinDist = " << toString(vSafeMinDist, 24) << "" << std::endl;
3949 : //
3950 : // double gap = getLeader().second;
3951 : // std::cout << "gap = " << toString(gap, 24) << std::endl;
3952 : // std::cout << "vSafeStoppedLeader = " << toString(getCarFollowModel().stopSpeed(this, getSpeed(), gap, MSCFModel::CalcReason::FUTURE), 24)
3953 : // << "\n" << std::endl;
3954 : // }
3955 : //#endif
3956 :
3957 631599154 : if ((MSGlobals::gSemiImplicitEulerUpdate && vSafe + NUMERICAL_EPS < vSafeMin)
3958 631375699 : || (!MSGlobals::gSemiImplicitEulerUpdate && (vSafe + NUMERICAL_EPS < vSafeMin && vSafeMin != 0))) { // this might be good for the euler case as well
3959 : // XXX: (Leo) This often called stopSpeed with vSafeMinDist==0 (for the ballistic update), since vSafe can become negative
3960 : // 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
3961 : #ifdef DEBUG_EXEC_MOVE
3962 : if (DEBUG_COND) {
3963 : std::cout << "vSafeMin Problem? vSafe=" << vSafe << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << std::endl;
3964 : }
3965 : #endif
3966 264594 : if (canBrakeVSafeMin && vSafe < getSpeed()) {
3967 : // cannot drive across a link so we need to stop before it
3968 127270 : vSafe = MIN2(vSafe, MAX2(getCarFollowModel().minNextSpeed(getSpeed(), this),
3969 63635 : getCarFollowModel().stopSpeed(this, getSpeed(), vSafeMinDist)));
3970 63635 : vSafeMin = 0;
3971 63635 : myHaveToWaitOnNextLink = true;
3972 : #ifdef DEBUG_CHECKREWINDLINKLANES
3973 : if (DEBUG_COND) {
3974 : std::cout << SIMTIME << " veh=" << getID() << " haveToWait (vSafe=" << vSafe << " < vSafeMin=" << vSafeMin << ")\n";
3975 : }
3976 : #endif
3977 : } else {
3978 : // if the link is yellow or visibility distance is large
3979 : // then we might not make it across the link in one step anyway..
3980 : // Possibly, the lane after the intersection has a lower speed limit so
3981 : // we really need to drive slower already
3982 : // -> keep driving without dawdling
3983 200959 : vSafeMin = vSafe;
3984 : }
3985 : }
3986 :
3987 : // vehicles inside a roundabout should maintain their requests
3988 631599154 : if (myLane->getEdge().isRoundabout()) {
3989 2659765 : myHaveToWaitOnNextLink = false;
3990 : }
3991 :
3992 631599154 : vSafe = MIN2(vSafe, vSafeZipper);
3993 631599154 : }
3994 :
3995 :
3996 : double
3997 700645519 : MSVehicle::processTraCISpeedControl(double vSafe, double vNext) {
3998 700645519 : if (myInfluencer != nullptr) {
3999 495565 : myInfluencer->setOriginalSpeed(vNext);
4000 : #ifdef DEBUG_TRACI
4001 : if DEBUG_COND2(this) {
4002 : std::cout << SIMTIME << " MSVehicle::processTraCISpeedControl() for vehicle '" << getID() << "'"
4003 : << " vSafe=" << vSafe << " (init)vNext=" << vNext << " keepStopping=" << keepStopping();
4004 : }
4005 : #endif
4006 495565 : if (myInfluencer->isRemoteControlled()) {
4007 6755 : vNext = myInfluencer->implicitSpeedRemote(this, myState.mySpeed);
4008 : }
4009 495565 : const double vMax = getVehicleType().getCarFollowModel().maxNextSpeed(myState.mySpeed, this);
4010 495565 : double vMin = getVehicleType().getCarFollowModel().minNextSpeed(myState.mySpeed, this);
4011 495565 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4012 : vMin = MAX2(0., vMin);
4013 : }
4014 495565 : vNext = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), vNext, vSafe, vMin, vMax);
4015 495565 : if (keepStopping() && myStops.front().getSpeed() == 0) {
4016 : // avoid driving while stopped (unless it's actually a waypoint
4017 3819 : vNext = myInfluencer->getOriginalSpeed();
4018 : }
4019 : #ifdef DEBUG_TRACI
4020 : if DEBUG_COND2(this) {
4021 : std::cout << " (processed)vNext=" << vNext << std::endl;
4022 : }
4023 : #endif
4024 : }
4025 700645519 : return vNext;
4026 : }
4027 :
4028 :
4029 : void
4030 71580868 : MSVehicle::removePassedDriveItems() {
4031 : #ifdef DEBUG_ACTIONSTEPS
4032 : if (DEBUG_COND) {
4033 : std::cout << SIMTIME << " veh=" << getID() << " removePassedDriveItems()\n"
4034 : << " Current items: ";
4035 : for (auto& j : myLFLinkLanes) {
4036 : if (j.myLink == 0) {
4037 : std::cout << "\n Stop at distance " << j.myDistance;
4038 : } else {
4039 : const MSLane* to = j.myLink->getViaLaneOrLane();
4040 : const MSLane* from = j.myLink->getLaneBefore();
4041 : std::cout << "\n Link at distance " << j.myDistance << ": '"
4042 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4043 : }
4044 : }
4045 : std::cout << "\n myNextDriveItem: ";
4046 : if (myLFLinkLanes.size() != 0) {
4047 : if (myNextDriveItem->myLink == 0) {
4048 : std::cout << "\n Stop at distance " << myNextDriveItem->myDistance;
4049 : } else {
4050 : const MSLane* to = myNextDriveItem->myLink->getViaLaneOrLane();
4051 : const MSLane* from = myNextDriveItem->myLink->getLaneBefore();
4052 : std::cout << "\n Link at distance " << myNextDriveItem->myDistance << ": '"
4053 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4054 : }
4055 : }
4056 : std::cout << std::endl;
4057 : }
4058 : #endif
4059 71906235 : for (auto j = myLFLinkLanes.begin(); j != myNextDriveItem; ++j) {
4060 : #ifdef DEBUG_ACTIONSTEPS
4061 : if (DEBUG_COND) {
4062 : std::cout << " Removing item: ";
4063 : if (j->myLink == 0) {
4064 : std::cout << "Stop at distance " << j->myDistance;
4065 : } else {
4066 : const MSLane* to = j->myLink->getViaLaneOrLane();
4067 : const MSLane* from = j->myLink->getLaneBefore();
4068 : std::cout << "Link at distance " << j->myDistance << ": '"
4069 : << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
4070 : }
4071 : std::cout << std::endl;
4072 : }
4073 : #endif
4074 325367 : if (j->myLink != nullptr) {
4075 325294 : j->myLink->removeApproaching(this);
4076 : }
4077 : }
4078 71580868 : myLFLinkLanes.erase(myLFLinkLanes.begin(), myNextDriveItem);
4079 71580868 : myNextDriveItem = myLFLinkLanes.begin();
4080 71580868 : }
4081 :
4082 :
4083 : void
4084 1131928 : MSVehicle::updateDriveItems() {
4085 : #ifdef DEBUG_ACTIONSTEPS
4086 : if (DEBUG_COND) {
4087 : std::cout << SIMTIME << " updateDriveItems(), veh='" << getID() << "' (lane: '" << getLane()->getID() << "')\nCurrent drive items:" << std::endl;
4088 : for (const auto& dpi : myLFLinkLanes) {
4089 : std::cout
4090 : << " vPass=" << dpi.myVLinkPass
4091 : << " vWait=" << dpi.myVLinkWait
4092 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4093 : << " request=" << dpi.mySetRequest
4094 : << "\n";
4095 : }
4096 : std::cout << " myNextDriveItem's linked lane: " << (myNextDriveItem->myLink == 0 ? "NULL" : myNextDriveItem->myLink->getViaLaneOrLane()->getID()) << std::endl;
4097 : }
4098 : #endif
4099 1131928 : if (myLFLinkLanes.size() == 0) {
4100 : // nothing to update
4101 : return;
4102 : }
4103 : const MSLink* nextPlannedLink = nullptr;
4104 : // auto i = myLFLinkLanes.begin();
4105 1131926 : auto i = myNextDriveItem;
4106 2263805 : while (i != myLFLinkLanes.end() && nextPlannedLink == nullptr) {
4107 1131879 : nextPlannedLink = i->myLink;
4108 : ++i;
4109 : }
4110 :
4111 1131926 : if (nextPlannedLink == nullptr) {
4112 : // No link for upcoming item -> no need for an update
4113 : #ifdef DEBUG_ACTIONSTEPS
4114 : if (DEBUG_COND) {
4115 : std::cout << "Found no link-related drive item." << std::endl;
4116 : }
4117 : #endif
4118 : return;
4119 : }
4120 :
4121 555079 : if (getLane() == nextPlannedLink->getLaneBefore()) {
4122 : // Current lane approaches the stored next link, i.e. no LC happend and no update is required.
4123 : #ifdef DEBUG_ACTIONSTEPS
4124 : if (DEBUG_COND) {
4125 : std::cout << "Continuing on planned lane sequence, no update required." << std::endl;
4126 : }
4127 : #endif
4128 : return;
4129 : }
4130 : // Lane must have been changed, determine the change direction
4131 544964 : const MSLink* parallelLink = nextPlannedLink->getParallelLink(1);
4132 544964 : if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
4133 : // lcDir = 1;
4134 : } else {
4135 264477 : parallelLink = nextPlannedLink->getParallelLink(-1);
4136 264477 : if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
4137 : // lcDir = -1;
4138 : } else {
4139 : // If the vehicle's current lane is not the approaching lane for the next
4140 : // drive process item's link, it is expected to lead to a parallel link,
4141 : // XXX: What if the lc was an overtaking maneuver and there is no upcoming link?
4142 : // Then a stop item should be scheduled! -> TODO!
4143 : //assert(false);
4144 72888 : return;
4145 : }
4146 : }
4147 : #ifdef DEBUG_ACTIONSTEPS
4148 : if (DEBUG_COND) {
4149 : std::cout << "Changed lane. Drive items will be updated along the current lane continuation." << std::endl;
4150 : }
4151 : #endif
4152 : // Trace link sequence along current best lanes and transfer drive items to the corresponding links
4153 : // DriveItemVector::iterator driveItemIt = myLFLinkLanes.begin();
4154 472076 : DriveItemVector::iterator driveItemIt = myNextDriveItem;
4155 : // In the loop below, lane holds the currently considered lane on the vehicles continuation (including internal lanes)
4156 472076 : const MSLane* lane = myLane;
4157 : assert(myLane == parallelLink->getLaneBefore());
4158 : // *lit is a pointer to the next lane in best continuations for the current lane (always non-internal)
4159 472076 : std::vector<MSLane*>::const_iterator bestLaneIt = getBestLanesContinuation().begin() + 1;
4160 : // Pointer to the new link for the current drive process item
4161 : MSLink* newLink = nullptr;
4162 1764710 : while (driveItemIt != myLFLinkLanes.end()) {
4163 1321527 : if (driveItemIt->myLink == nullptr) {
4164 : // Items not related to a specific link are not updated
4165 : // (XXX: when a stop item corresponded to a dead end, which is overcome by the LC that made
4166 : // the update necessary, this may slow down the vehicle's continuation on the new lane...)
4167 : ++driveItemIt;
4168 168946 : continue;
4169 : }
4170 : // Continuation links for current best lanes are less than for the former drive items (myLFLinkLanes)
4171 : // We just remove the leftover link-items, as they cannot be mapped to new links.
4172 1152581 : if (bestLaneIt == getBestLanesContinuation().end()) {
4173 : #ifdef DEBUG_ACTIONSTEPS
4174 : if (DEBUG_COND) {
4175 : std::cout << "Reached end of the new continuation sequence. Erasing leftover link-items." << std::endl;
4176 : }
4177 : #endif
4178 91419 : while (driveItemIt != myLFLinkLanes.end()) {
4179 62526 : if (driveItemIt->myLink == nullptr) {
4180 : ++driveItemIt;
4181 14252 : continue;
4182 : } else {
4183 48274 : driveItemIt->myLink->removeApproaching(this);
4184 : driveItemIt = myLFLinkLanes.erase(driveItemIt);
4185 : }
4186 : }
4187 : break;
4188 : }
4189 : // Do the actual link-remapping for the item. And un/register approaching information on the corresponding links
4190 1123688 : const MSLane* const target = *bestLaneIt;
4191 : assert(!target->isInternal());
4192 : newLink = nullptr;
4193 1240234 : for (MSLink* const link : lane->getLinkCont()) {
4194 1240234 : if (link->getLane() == target) {
4195 : newLink = link;
4196 : break;
4197 : }
4198 : }
4199 :
4200 1123688 : if (newLink == driveItemIt->myLink) {
4201 : // new continuation merged into previous - stop update
4202 : #ifdef DEBUG_ACTIONSTEPS
4203 : if (DEBUG_COND) {
4204 : std::cout << "Old and new continuation sequences merge at link\n"
4205 : << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'"
4206 : << "\nNo update beyond merge required." << std::endl;
4207 : }
4208 : #endif
4209 : break;
4210 : }
4211 :
4212 : #ifdef DEBUG_ACTIONSTEPS
4213 : if (DEBUG_COND) {
4214 : std::cout << "lane=" << lane->getID() << "\nUpdating link\n '" << driveItemIt->myLink->getLaneBefore()->getID() << "'->'" << driveItemIt->myLink->getViaLaneOrLane()->getID() << "'"
4215 : << "==> " << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'" << std::endl;
4216 : }
4217 : #endif
4218 1123688 : newLink->setApproaching(this, driveItemIt->myLink->getApproaching(this));
4219 1123688 : driveItemIt->myLink->removeApproaching(this);
4220 1123688 : driveItemIt->myLink = newLink;
4221 : lane = newLink->getViaLaneOrLane();
4222 : ++driveItemIt;
4223 1123688 : if (!lane->isInternal()) {
4224 : ++bestLaneIt;
4225 : }
4226 : }
4227 : #ifdef DEBUG_ACTIONSTEPS
4228 : if (DEBUG_COND) {
4229 : std::cout << "Updated drive items:" << std::endl;
4230 : for (const auto& dpi : myLFLinkLanes) {
4231 : std::cout
4232 : << " vPass=" << dpi.myVLinkPass
4233 : << " vWait=" << dpi.myVLinkWait
4234 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4235 : << " request=" << dpi.mySetRequest
4236 : << "\n";
4237 : }
4238 : }
4239 : #endif
4240 : }
4241 :
4242 :
4243 : void
4244 700645519 : MSVehicle::setBrakingSignals(double vNext) {
4245 : // To avoid casual blinking brake lights at high speeds due to dawdling of the
4246 : // leading vehicle, we don't show brake lights when the deceleration could be caused
4247 : // by frictional forces and air resistance (i.e. proportional to v^2, coefficient could be adapted further)
4248 700645519 : double pseudoFriction = (0.05 + 0.005 * getSpeed()) * getSpeed();
4249 700645519 : bool brakelightsOn = vNext < getSpeed() - ACCEL2SPEED(pseudoFriction);
4250 :
4251 700645519 : if (vNext <= SUMO_const_haltingSpeed) {
4252 : brakelightsOn = true;
4253 : }
4254 700645519 : if (brakelightsOn && !isStopped()) {
4255 : switchOnSignal(VEH_SIGNAL_BRAKELIGHT);
4256 : } else {
4257 : switchOffSignal(VEH_SIGNAL_BRAKELIGHT);
4258 : }
4259 700645519 : }
4260 :
4261 :
4262 : void
4263 700712306 : MSVehicle::updateWaitingTime(double vNext) {
4264 700712306 : if (vNext <= SUMO_const_haltingSpeed && (!isStopped() || isIdling()) && myAcceleration <= accelThresholdForWaiting()) {
4265 90933101 : myWaitingTime += DELTA_T;
4266 90933101 : myWaitingTimeCollector.passTime(DELTA_T, true);
4267 : } else {
4268 609779205 : myWaitingTime = 0;
4269 609779205 : myWaitingTimeCollector.passTime(DELTA_T, false);
4270 609779205 : if (hasInfluencer()) {
4271 269900 : getInfluencer().setExtraImpatience(0);
4272 : }
4273 : }
4274 700712306 : }
4275 :
4276 :
4277 : void
4278 700645373 : MSVehicle::updateTimeLoss(double vNext) {
4279 : // update time loss (depends on the updated edge)
4280 700645373 : if (!isStopped()) {
4281 : // some cfModels (i.e. EIDM may drive faster than predicted by maxNextSpeed)
4282 689637149 : const double vmax = MIN2(myLane->getVehicleMaxSpeed(this), MAX2(myStopSpeed, vNext));
4283 686529540 : if (vmax > 0) {
4284 686520730 : myTimeLoss += TS * (vmax - vNext) / vmax;
4285 : }
4286 : }
4287 700645373 : }
4288 :
4289 :
4290 : double
4291 1560458464 : MSVehicle::checkReversal(bool& canReverse, double speedThreshold, double seen) const {
4292 61437846 : const bool stopOk = (myStops.empty() || myStops.front().edge != myCurrEdge
4293 1590229233 : || (myStops.front().getSpeed() > 0 && myState.myPos > myStops.front().pars.endPos - 2 * POSITION_EPS));
4294 : #ifdef DEBUG_REVERSE_BIDI
4295 : if (DEBUG_COND) std::cout << SIMTIME << " checkReversal lane=" << myLane->getID()
4296 : << " pos=" << myState.myPos
4297 : << " speed=" << std::setprecision(6) << getPreviousSpeed() << std::setprecision(gPrecision)
4298 : << " speedThreshold=" << speedThreshold
4299 : << " seen=" << seen
4300 : << " isRail=" << isRail()
4301 : << " speedOk=" << (getPreviousSpeed() <= speedThreshold)
4302 : << " posOK=" << (myState.myPos <= myLane->getLength())
4303 : << " normal=" << !myLane->isInternal()
4304 : << " routeOK=" << ((myCurrEdge + 1) != myRoute->end())
4305 : << " bidi=" << (myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1))
4306 : << " stopOk=" << stopOk
4307 : << "\n";
4308 : #endif
4309 1560458464 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4310 6997981 : && getPreviousSpeed() <= speedThreshold
4311 5959471 : && myState.myPos <= myLane->getLength()
4312 5958373 : && !myLane->isInternal()
4313 5884872 : && (myCurrEdge + 1) != myRoute->end()
4314 5773189 : && myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1)
4315 : // ensure there are no further stops on this edge
4316 1561307738 : && stopOk
4317 : ) {
4318 : //if (isSelected()) std::cout << " check1 passed\n";
4319 :
4320 : // ensure that the vehicle is fully on bidi edges that allow reversal
4321 180793 : const int neededFutureRoute = 1 + (int)(MSGlobals::gUsingInternalLanes
4322 : ? myFurtherLanes.size()
4323 504 : : ceil((double)myFurtherLanes.size() / 2.0));
4324 180793 : const int remainingRoute = int(myRoute->end() - myCurrEdge) - 1;
4325 180793 : if (remainingRoute < neededFutureRoute) {
4326 : #ifdef DEBUG_REVERSE_BIDI
4327 : if (DEBUG_COND) {
4328 : std::cout << " fail: remainingEdges=" << ((int)(myRoute->end() - myCurrEdge)) << " further=" << myFurtherLanes.size() << "\n";
4329 : }
4330 : #endif
4331 3567 : return getMaxSpeed();
4332 : }
4333 : //if (isSelected()) std::cout << " check2 passed\n";
4334 :
4335 : // ensure that the turn-around connection exists from the current edge to its bidi-edge
4336 177226 : const MSEdgeVector& succ = myLane->getEdge().getSuccessors();
4337 177226 : if (std::find(succ.begin(), succ.end(), myLane->getEdge().getBidiEdge()) == succ.end()) {
4338 : #ifdef DEBUG_REVERSE_BIDI
4339 : if (DEBUG_COND) {
4340 : std::cout << " noTurn (bidi=" << myLane->getEdge().getBidiEdge()->getID() << " succ=" << toString(succ) << "\n";
4341 : }
4342 : #endif
4343 909 : return getMaxSpeed();
4344 : }
4345 : //if (isSelected()) std::cout << " check3 passed\n";
4346 :
4347 : // ensure that the vehicle front will not move past a stop on the bidi edge of the current edge
4348 176317 : if (!myStops.empty() && myStops.front().edge == (myCurrEdge + 1)) {
4349 160006 : const double stopPos = myStops.front().getEndPos(*this);
4350 160006 : const double brakeDist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
4351 160006 : const double newPos = myLane->getLength() - (getBackPositionOnLane() + brakeDist);
4352 160006 : if (newPos > stopPos) {
4353 : #ifdef DEBUG_REVERSE_BIDI
4354 : if (DEBUG_COND) {
4355 : std::cout << " reversal would go past stop on " << myLane->getBidiLane()->getID() << "\n";
4356 : }
4357 : #endif
4358 158332 : if (seen > MAX2(brakeDist, 1.0)) {
4359 157202 : return getMaxSpeed();
4360 : } else {
4361 : #ifdef DEBUG_REVERSE_BIDI
4362 : if (DEBUG_COND) {
4363 : std::cout << " train is too long, skipping stop at " << stopPos << " cannot be avoided\n";
4364 : }
4365 : #endif
4366 : }
4367 : }
4368 : }
4369 : //if (isSelected()) std::cout << " check4 passed\n";
4370 :
4371 : // ensure that bidi-edges exist for all further edges
4372 : // and that no stops will be skipped when reversing
4373 : // and that the train will not be on top of a red rail signal after reversal
4374 19115 : const MSLane* bidi = myLane->getBidiLane();
4375 : int view = 2;
4376 38572 : for (MSLane* further : myFurtherLanes) {
4377 21893 : if (!further->getEdge().isInternal()) {
4378 11393 : if (further->getEdge().getBidiEdge() != *(myCurrEdge + view)) {
4379 : #ifdef DEBUG_REVERSE_BIDI
4380 : if (DEBUG_COND) {
4381 : std::cout << " noBidi view=" << view << " further=" << further->getID() << " furtherBidi=" << Named::getIDSecure(further->getEdge().getBidiEdge()) << " future=" << (*(myCurrEdge + view))->getID() << "\n";
4382 : }
4383 : #endif
4384 2277 : return getMaxSpeed();
4385 : }
4386 9116 : const MSLane* nextBidi = further->getBidiLane();
4387 9116 : const MSLink* toNext = bidi->getLinkTo(nextBidi);
4388 9116 : if (toNext == nullptr) {
4389 : // can only happen if the route is invalid
4390 0 : return getMaxSpeed();
4391 : }
4392 9116 : if (toNext->haveRed()) {
4393 : #ifdef DEBUG_REVERSE_BIDI
4394 : if (DEBUG_COND) {
4395 : std::cout << " do not reverse on a red signal\n";
4396 : }
4397 : #endif
4398 0 : return getMaxSpeed();
4399 : }
4400 : bidi = nextBidi;
4401 9116 : if (!myStops.empty() && myStops.front().edge == (myCurrEdge + view)) {
4402 453 : const double brakeDist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
4403 453 : const double stopPos = myStops.front().getEndPos(*this);
4404 453 : const double newPos = further->getLength() - (getBackPositionOnLane(further) + brakeDist);
4405 453 : if (newPos > stopPos) {
4406 : #ifdef DEBUG_REVERSE_BIDI
4407 : if (DEBUG_COND) {
4408 : std::cout << " reversal would go past stop on further-opposite lane " << further->getBidiLane()->getID() << "\n";
4409 : }
4410 : #endif
4411 171 : if (seen > MAX2(brakeDist, 1.0)) {
4412 159 : canReverse = false;
4413 159 : return getMaxSpeed();
4414 : } else {
4415 : #ifdef DEBUG_REVERSE_BIDI
4416 : if (DEBUG_COND) {
4417 : std::cout << " train is too long, skipping stop at " << stopPos << " cannot be avoided\n";
4418 : }
4419 : #endif
4420 : }
4421 : }
4422 : }
4423 8957 : view++;
4424 : }
4425 : }
4426 : // reverse as soon as comfortably possible
4427 16679 : const double vMinComfortable = getCarFollowModel().minNextSpeed(getSpeed(), this);
4428 : #ifdef DEBUG_REVERSE_BIDI
4429 : if (DEBUG_COND) {
4430 : std::cout << SIMTIME << " seen=" << seen << " vReverseOK=" << vMinComfortable << "\n";
4431 : }
4432 : #endif
4433 16679 : canReverse = true;
4434 16679 : return vMinComfortable;
4435 : }
4436 1560277671 : return getMaxSpeed();
4437 : }
4438 :
4439 :
4440 : void
4441 700861806 : MSVehicle::processLaneAdvances(std::vector<MSLane*>& passedLanes, std::string& emergencyReason) {
4442 716627727 : for (std::vector<MSLane*>::reverse_iterator i = myFurtherLanes.rbegin(); i != myFurtherLanes.rend(); ++i) {
4443 15765921 : passedLanes.push_back(*i);
4444 : }
4445 700861806 : if (passedLanes.size() == 0 || passedLanes.back() != myLane) {
4446 700861806 : passedLanes.push_back(myLane);
4447 : }
4448 : // let trains reverse direction
4449 700861806 : bool reverseTrain = false;
4450 700861806 : checkReversal(reverseTrain);
4451 700861806 : if (reverseTrain) {
4452 : // Train is 'reversing' so toggle the logical state
4453 810 : myAmReversed = !myAmReversed;
4454 : // add some slack to ensure that the back of train does appear looped
4455 810 : myState.myPos += 2 * (myLane->getLength() - myState.myPos) + myType->getLength() + NUMERICAL_EPS;
4456 810 : myState.mySpeed = 0;
4457 : #ifdef DEBUG_REVERSE_BIDI
4458 : if (DEBUG_COND) {
4459 : std::cout << SIMTIME << " reversing train=" << getID() << " newPos=" << myState.myPos << "\n";
4460 : }
4461 : #endif
4462 : }
4463 : // move on lane(s)
4464 700861806 : if (myState.myPos > myLane->getLength()) {
4465 : // The vehicle has moved at least to the next lane (maybe it passed even more than one)
4466 19927224 : if (myCurrEdge != myRoute->end() - 1) {
4467 16810164 : MSLane* approachedLane = myLane;
4468 : // move the vehicle forward
4469 16810164 : myNextDriveItem = myLFLinkLanes.begin();
4470 36260496 : while (myNextDriveItem != myLFLinkLanes.end() && approachedLane != nullptr && myState.myPos > approachedLane->getLength()) {
4471 19469161 : const MSLink* link = myNextDriveItem->myLink;
4472 19469161 : const double linkDist = myNextDriveItem->myDistance;
4473 : ++myNextDriveItem;
4474 : // check whether the vehicle was allowed to enter lane
4475 : // otherwise it is decelerated and we do not need to test for it's
4476 : // approach on the following lanes when a lane changing is performed
4477 : // proceed to the next lane
4478 19469161 : if (approachedLane->mustCheckJunctionCollisions()) {
4479 : // vehicle moves past approachedLane within a single step, collision checking must still be done
4480 65401 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(approachedLane);
4481 : }
4482 19469161 : if (link != nullptr) {
4483 19464991 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4484 43435 : && !myLane->isInternal()
4485 22935 : && myLane->getBidiLane() != nullptr
4486 13038 : && link->getLane()->getBidiLane() == myLane
4487 19465798 : && !reverseTrain) {
4488 : emergencyReason = " because it must reverse direction";
4489 : approachedLane = nullptr;
4490 : break;
4491 : }
4492 19464988 : if ((getVClass() & SVC_RAIL_CLASSES) != 0
4493 43432 : && myState.myPos < myLane->getLength() + NUMERICAL_EPS
4494 19465204 : && hasStops() && getNextStop().edge == myCurrEdge) {
4495 : // avoid skipping stop due to numerical instability
4496 : // this is a special case for rail vehicles because they
4497 : // continue myLFLinkLanes past stops
4498 202 : approachedLane = myLane;
4499 202 : myState.myPos = myLane->getLength();
4500 202 : break;
4501 : }
4502 19464786 : approachedLane = link->getViaLaneOrLane();
4503 19464786 : if (myInfluencer == nullptr || myInfluencer->getEmergencyBrakeRedLight()) {
4504 19463195 : bool beyondStopLine = linkDist < link->getLaneBefore()->getVehicleStopOffset(this);
4505 19463195 : if (link->haveRed() && !ignoreRed(link, false) && !beyondStopLine && !reverseTrain) {
4506 : emergencyReason = " because of a red traffic light";
4507 : break;
4508 : }
4509 : }
4510 19464724 : if (reverseTrain && approachedLane->isInternal()) {
4511 : // avoid getting stuck on a slow turn-around internal lane
4512 888 : myState.myPos += approachedLane->getLength();
4513 : }
4514 4170 : } else if (myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
4515 : // avoid warning due to numerical instability
4516 225 : approachedLane = myLane;
4517 225 : myState.myPos = myLane->getLength();
4518 3945 : } else if (reverseTrain) {
4519 0 : approachedLane = (*(myCurrEdge + 1))->getLanes()[0];
4520 0 : link = myLane->getLinkTo(approachedLane);
4521 : assert(link != 0);
4522 0 : while (link->getViaLane() != nullptr) {
4523 0 : link = link->getViaLane()->getLinkCont()[0];
4524 : }
4525 : --myNextDriveItem;
4526 : } else {
4527 : emergencyReason = " because there is no connection to the next edge";
4528 : approachedLane = nullptr;
4529 : break;
4530 : }
4531 19464949 : if (approachedLane != myLane && approachedLane != nullptr) {
4532 19464724 : leaveLane(MSMoveReminder::NOTIFICATION_JUNCTION, approachedLane);
4533 19464724 : myState.myPos -= myLane->getLength();
4534 : assert(myState.myPos > 0);
4535 19464724 : enterLaneAtMove(approachedLane);
4536 19464724 : if (link->isEntryLink()) {
4537 7645326 : myJunctionEntryTime = MSNet::getInstance()->getCurrentTimeStep();
4538 7645326 : myJunctionEntryTimeNeverYield = myJunctionEntryTime;
4539 7645326 : myHaveStoppedFor = nullptr;
4540 : }
4541 19464724 : if (link->isConflictEntryLink()) {
4542 7644711 : myJunctionConflictEntryTime = MSNet::getInstance()->getCurrentTimeStep();
4543 : // renew yielded request
4544 7644711 : myJunctionEntryTime = myJunctionEntryTimeNeverYield;
4545 : }
4546 19464724 : if (link->isExitLink()) {
4547 : // passed junction, reset for approaching the next one
4548 7585818 : myJunctionEntryTime = SUMOTime_MAX;
4549 7585818 : myJunctionEntryTimeNeverYield = SUMOTime_MAX;
4550 7585818 : myJunctionConflictEntryTime = SUMOTime_MAX;
4551 : }
4552 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
4553 : if (DEBUG_COND) {
4554 : std::cout << "Update junctionTimes link=" << link->getViaLaneOrLane()->getID()
4555 : << " entry=" << link->isEntryLink() << " conflict=" << link->isConflictEntryLink() << " exit=" << link->isExitLink()
4556 : << " ET=" << myJunctionEntryTime
4557 : << " ETN=" << myJunctionEntryTimeNeverYield
4558 : << " CET=" << myJunctionConflictEntryTime
4559 : << "\n";
4560 : }
4561 : #endif
4562 19464724 : if (hasArrivedInternal()) {
4563 : break;
4564 : }
4565 19450863 : if (myLaneChangeModel->isChangingLanes()) {
4566 7282 : if (link->getDirection() == LinkDirection::LEFT || link->getDirection() == LinkDirection::RIGHT) {
4567 : // abort lane change
4568 63 : WRITE_WARNINGF("Vehicle '%' could not finish continuous lane change (turn lane) time=%.", getID(), time2string(SIMSTEP));
4569 21 : myLaneChangeModel->endLaneChangeManeuver();
4570 : }
4571 : }
4572 19450863 : if (approachedLane->getEdge().isVaporizing()) {
4573 756 : leaveLane(MSMoveReminder::NOTIFICATION_VAPORIZED_VAPORIZER);
4574 : break;
4575 : }
4576 19450107 : passedLanes.push_back(approachedLane);
4577 : }
4578 : }
4579 : // NOTE: Passed drive items will be erased in the next simstep's planMove()
4580 :
4581 : #ifdef DEBUG_ACTIONSTEPS
4582 : if (DEBUG_COND && myNextDriveItem != myLFLinkLanes.begin()) {
4583 : std::cout << "Updated drive items:" << std::endl;
4584 : for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
4585 : std::cout
4586 : << " vPass=" << (*i).myVLinkPass
4587 : << " vWait=" << (*i).myVLinkWait
4588 : << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
4589 : << " request=" << (*i).mySetRequest
4590 : << "\n";
4591 : }
4592 : }
4593 : #endif
4594 3117060 : } else if (!hasArrivedInternal() && myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
4595 : // avoid warning due to numerical instability when stopping at the end of the route
4596 89 : myState.myPos = myLane->getLength();
4597 : }
4598 :
4599 : }
4600 700861806 : }
4601 :
4602 :
4603 :
4604 : bool
4605 703180022 : MSVehicle::executeMove() {
4606 : #ifdef DEBUG_EXEC_MOVE
4607 : if (DEBUG_COND) {
4608 : std::cout << "\nEXECUTE_MOVE\n"
4609 : << SIMTIME
4610 : << " veh=" << getID()
4611 : << " speed=" << getSpeed() // toString(getSpeed(), 24)
4612 : << std::endl;
4613 : }
4614 : #endif
4615 :
4616 :
4617 : // Maximum safe velocity
4618 703180022 : double vSafe = std::numeric_limits<double>::max();
4619 : // Minimum safe velocity (lower bound).
4620 703180022 : double vSafeMin = -std::numeric_limits<double>::max();
4621 : // The distance to a link, which should either be crossed this step
4622 : // or in front of which we need to stop.
4623 703180022 : double vSafeMinDist = 0;
4624 :
4625 703180022 : if (myActionStep) {
4626 : // Actuate control (i.e. choose bounds for safe speed in current simstep (euler), resp. after current sim step (ballistic))
4627 631599154 : processLinkApproaches(vSafe, vSafeMin, vSafeMinDist);
4628 : #ifdef DEBUG_ACTIONSTEPS
4629 : if (DEBUG_COND) {
4630 : std::cout << SIMTIME << " vehicle '" << getID() << "'\n"
4631 : " vsafe from processLinkApproaches(): vsafe " << vSafe << std::endl;
4632 : }
4633 : #endif
4634 : } else {
4635 : // Continue with current acceleration
4636 71580868 : vSafe = getSpeed() + ACCEL2SPEED(myAcceleration);
4637 : #ifdef DEBUG_ACTIONSTEPS
4638 : if (DEBUG_COND) {
4639 : std::cout << SIMTIME << " vehicle '" << getID() << "' skips processLinkApproaches()\n"
4640 : " continues with constant accel " << myAcceleration << "...\n"
4641 : << "speed: " << getSpeed() << " -> " << vSafe << std::endl;
4642 : }
4643 : #endif
4644 : }
4645 :
4646 :
4647 : //#ifdef DEBUG_EXEC_MOVE
4648 : // if (DEBUG_COND) {
4649 : // std::cout << "vSafe = " << toString(vSafe,12) << "\n" << std::endl;
4650 : // }
4651 : //#endif
4652 :
4653 : // Determine vNext = speed after current sim step (ballistic), resp. in current simstep (euler)
4654 : // Call to finalizeSpeed applies speed reduction due to dawdling / lane changing but ensures minimum safe speed
4655 703180022 : double vNext = vSafe;
4656 : const MSCFModel& cfModel = getCarFollowModel();
4657 703180022 : const double rawAccel = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
4658 703180022 : if (vNext <= SUMO_const_haltingSpeed * TS && myWaitingTime > MSGlobals::gStartupWaitThreshold && rawAccel <= accelThresholdForWaiting() && myActionStep) {
4659 74659681 : myTimeSinceStartup = 0;
4660 628520341 : } else if (isStopped()) {
4661 : // do not apply startupDelay for waypoints
4662 16638596 : if (cfModel.startupDelayStopped() && getNextStop().pars.speed <= 0) {
4663 13772 : myTimeSinceStartup = DELTA_T;
4664 : } else {
4665 : // do not apply startupDelay but signal that a stop has taken place
4666 16624824 : myTimeSinceStartup = cfModel.getStartupDelay() + DELTA_T;
4667 : }
4668 : } else {
4669 : // identify potential startup (before other effects reduce the speed again)
4670 611881745 : myTimeSinceStartup += DELTA_T;
4671 : }
4672 703180022 : if (myActionStep) {
4673 631599154 : vNext = cfModel.finalizeSpeed(this, vSafe);
4674 629064505 : if (vNext > 0) {
4675 586646979 : vNext = MAX2(vNext, vSafeMin);
4676 : }
4677 : }
4678 : // (Leo) to avoid tiny oscillations (< 1e-10) of vNext in a standing vehicle column (observed for ballistic update), we cap off vNext
4679 : // (We assure to do this only for vNext<<NUMERICAL_EPS since otherwise this would nullify the workaround for #2995
4680 : // (Jakob) We also need to make sure to reach a stop at the start of the next edge
4681 700645373 : if (fabs(vNext) < NUMERICAL_EPS_SPEED && (myStopDist > POSITION_EPS || (hasStops() && myCurrEdge == getNextStop().edge))) {
4682 : vNext = 0.;
4683 : }
4684 : #ifdef DEBUG_EXEC_MOVE
4685 : if (DEBUG_COND) {
4686 : std::cout << SIMTIME << " finalizeSpeed vSafe=" << vSafe << " vSafeMin=" << (vSafeMin == -std::numeric_limits<double>::max() ? "-Inf" : toString(vSafeMin))
4687 : << " vNext=" << vNext << " (i.e. accel=" << SPEED2ACCEL(vNext - getSpeed()) << ")" << std::endl;
4688 : }
4689 : #endif
4690 :
4691 : // vNext may be higher than vSafe without implying a bug:
4692 : // - when approaching a green light that suddenly switches to yellow
4693 : // - when using unregulated junctions
4694 : // - when using tau < step-size
4695 : // - when using unsafe car following models
4696 : // - when using TraCI and some speedMode / laneChangeMode settings
4697 : //if (vNext > vSafe + NUMERICAL_EPS) {
4698 : // WRITE_WARNING("vehicle '" + getID() + "' cannot brake hard enough to reach safe speed "
4699 : // + toString(vSafe, 4) + ", moving at " + toString(vNext, 4) + " instead. time="
4700 : // + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
4701 : //}
4702 :
4703 700645373 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4704 : vNext = MAX2(vNext, 0.);
4705 : } else {
4706 : // (Leo) Ballistic: negative vNext can be used to indicate a stop within next step.
4707 : }
4708 :
4709 : // Check for speed advices from the traci client
4710 700645373 : vNext = processTraCISpeedControl(vSafe, vNext);
4711 :
4712 : // the acceleration of a vehicle equipped with the elecHybrid device is restricted by the maximal power of the electric drive as well
4713 700645373 : MSDevice_ElecHybrid* elecHybridOfVehicle = dynamic_cast<MSDevice_ElecHybrid*>(getDevice(typeid(MSDevice_ElecHybrid)));
4714 981 : if (elecHybridOfVehicle != nullptr) {
4715 : // this is the consumption given by the car following model-computed acceleration
4716 981 : elecHybridOfVehicle->setConsum(elecHybridOfVehicle->consumption(*this, (vNext - this->getSpeed()) / TS, vNext));
4717 : // but the maximum power of the electric motor may be lower
4718 : // it needs to be converted from [W] to [Wh/s] (3600s / 1h) so that TS can be taken into account
4719 981 : double maxPower = getEmissionParameters()->getDoubleOptional(SUMO_ATTR_MAXIMUMPOWER, 100000.) / 3600;
4720 981 : if (elecHybridOfVehicle->getConsum() / TS > maxPower) {
4721 : // no, we cannot accelerate that fast, recompute the maximum possible acceleration
4722 70 : double accel = elecHybridOfVehicle->acceleration(*this, maxPower, this->getSpeed());
4723 : // and update the speed of the vehicle
4724 70 : vNext = MIN2(vNext, this->getSpeed() + accel * TS);
4725 : vNext = MAX2(vNext, 0.);
4726 : // and set the vehicle consumption to reflect this
4727 70 : elecHybridOfVehicle->setConsum(elecHybridOfVehicle->consumption(*this, (vNext - this->getSpeed()) / TS, vNext));
4728 : }
4729 : }
4730 :
4731 700645373 : setBrakingSignals(vNext);
4732 :
4733 : // update position and speed
4734 700645373 : int oldLaneOffset = myLane->getEdge().getNumLanes() - myLane->getIndex();
4735 : const MSLane* oldLaneMaybeOpposite = myLane;
4736 700645373 : if (myLaneChangeModel->isOpposite()) {
4737 : // transform to the forward-direction lane, move and then transform back
4738 406591 : myState.myPos = myLane->getOppositePos(myState.myPos);
4739 406591 : myLane = myLane->getParallelOpposite();
4740 : }
4741 700645373 : updateState(vNext);
4742 700645373 : updateWaitingTime(vNext);
4743 :
4744 : // Lanes, which the vehicle touched at some moment of the executed simstep
4745 : std::vector<MSLane*> passedLanes;
4746 : // remember previous lane (myLane is updated in processLaneAdvances)
4747 700645373 : const MSLane* oldLane = myLane;
4748 : // Reason for a possible emergency stop
4749 : std::string emergencyReason;
4750 700645373 : processLaneAdvances(passedLanes, emergencyReason);
4751 :
4752 700645373 : updateTimeLoss(vNext);
4753 700645373 : myCollisionImmunity = MAX2((SUMOTime) - 1, myCollisionImmunity - DELTA_T);
4754 :
4755 700645373 : if (!hasArrivedInternal() && !myLane->getEdge().isVaporizing()) {
4756 697333152 : if (myState.myPos > myLane->getLength()) {
4757 353 : if (emergencyReason == "") {
4758 31 : emergencyReason = TL(" for unknown reasons");
4759 : }
4760 1412 : WRITE_WARNINGF(TL("Vehicle '%' performs emergency stop at the end of lane '%'% (decel=%, offset=%), time=%."),
4761 : getID(), myLane->getID(), emergencyReason, myAcceleration - myState.mySpeed,
4762 : myState.myPos - myLane->getLength(), time2string(SIMSTEP));
4763 353 : MSNet::getInstance()->getVehicleControl().registerEmergencyStop();
4764 353 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::EMERGENCYSTOP);
4765 353 : myState.myPos = myLane->getLength();
4766 353 : myState.mySpeed = 0;
4767 353 : myAcceleration = 0;
4768 : }
4769 697333152 : const MSLane* oldBackLane = getBackLane();
4770 697333152 : if (myLaneChangeModel->isOpposite()) {
4771 : passedLanes.clear(); // ignore back occupation
4772 : }
4773 : #ifdef DEBUG_ACTIONSTEPS
4774 : if (DEBUG_COND) {
4775 : std::cout << SIMTIME << " veh '" << getID() << "' updates further lanes." << std::endl;
4776 : }
4777 : #endif
4778 697333152 : myState.myBackPos = updateFurtherLanes(myFurtherLanes, myFurtherLanesPosLat, passedLanes);
4779 697333152 : if (passedLanes.size() > 1 && isRail()) {
4780 827442 : for (auto pi = passedLanes.rbegin(); pi != passedLanes.rend(); ++pi) {
4781 623714 : MSLane* pLane = *pi;
4782 623714 : if (pLane != myLane && std::find(myFurtherLanes.begin(), myFurtherLanes.end(), pLane) == myFurtherLanes.end()) {
4783 43137 : leaveLaneBack(MSMoveReminder::NOTIFICATION_JUNCTION, *pi);
4784 : }
4785 : }
4786 : }
4787 : // bestLanes need to be updated before lane changing starts. NOTE: This call is also a presumption for updateDriveItems()
4788 697333152 : updateBestLanes();
4789 697333152 : if (myLane != oldLane || oldBackLane != getBackLane()) {
4790 24700752 : if (myLaneChangeModel->getShadowLane() != nullptr || getLateralOverlap() > POSITION_EPS) {
4791 : // shadow lane must be updated if the front or back lane changed
4792 : // either if we already have a shadowLane or if there is lateral overlap
4793 551251 : myLaneChangeModel->updateShadowLane();
4794 : }
4795 24700752 : if (MSGlobals::gLateralResolution > 0 && !myLaneChangeModel->isOpposite()) {
4796 : // The vehicles target lane must be also be updated if the front or back lane changed
4797 4382074 : myLaneChangeModel->updateTargetLane();
4798 : }
4799 : }
4800 697333152 : setBlinkerInformation(); // needs updated bestLanes
4801 : //change the blue light only for emergency vehicles SUMOVehicleClass
4802 697333152 : if (myType->getVehicleClass() == SVC_EMERGENCY) {
4803 85732 : setEmergencyBlueLight(MSNet::getInstance()->getCurrentTimeStep());
4804 : }
4805 : // must be done before angle computation
4806 : // State needs to be reset for all vehicles before the next call to MSEdgeControl::changeLanes
4807 697333152 : if (myActionStep) {
4808 : // check (#2681): Can this be skipped?
4809 625773564 : myLaneChangeModel->prepareStep();
4810 : } else {
4811 71559588 : myLaneChangeModel->resetSpeedLat();
4812 : #ifdef DEBUG_ACTIONSTEPS
4813 : if (DEBUG_COND) {
4814 : std::cout << SIMTIME << " veh '" << getID() << "' skips LCM->prepareStep()." << std::endl;
4815 : }
4816 : #endif
4817 : }
4818 697333152 : myLaneChangeModel->setPreviousAngleOffset(myLaneChangeModel->getAngleOffset());
4819 697333152 : myAngle = computeAngle();
4820 : }
4821 :
4822 : #ifdef DEBUG_EXEC_MOVE
4823 : if (DEBUG_COND) {
4824 : std::cout << SIMTIME << " executeMove finished veh=" << getID() << " lane=" << myLane->getID() << " myPos=" << getPositionOnLane() << " myPosLat=" << getLateralPositionOnLane() << "\n";
4825 : gDebugFlag1 = false; // See MSLink_DEBUG_OPENED
4826 : }
4827 : #endif
4828 700645373 : if (myLaneChangeModel->isOpposite()) {
4829 : // transform back to the opposite-direction lane
4830 : MSLane* newOpposite = nullptr;
4831 406591 : const MSEdge* newOppositeEdge = myLane->getEdge().getOppositeEdge();
4832 406591 : if (newOppositeEdge != nullptr) {
4833 406541 : newOpposite = newOppositeEdge->getLanes()[newOppositeEdge->getNumLanes() - MAX2(1, oldLaneOffset)];
4834 : #ifdef DEBUG_EXEC_MOVE
4835 : if (DEBUG_COND) {
4836 : std::cout << SIMTIME << " newOppositeEdge=" << newOppositeEdge->getID() << " oldLaneOffset=" << oldLaneOffset << " leftMost=" << newOppositeEdge->getNumLanes() - 1 << " newOpposite=" << Named::getIDSecure(newOpposite) << "\n";
4837 : }
4838 : #endif
4839 : }
4840 406541 : if (newOpposite == nullptr) {
4841 50 : if (!myLaneChangeModel->hasBlueLight()) {
4842 : // unusual overtaking at junctions is ok for emergency vehicles
4843 0 : WRITE_WARNINGF(TL("Unexpected end of opposite lane for vehicle '%' at lane '%', time=%."),
4844 : getID(), myLane->getID(), time2string(SIMSTEP));
4845 : }
4846 50 : myLaneChangeModel->changedToOpposite();
4847 50 : if (myState.myPos < getLength()) {
4848 : // further lanes is always cleared during opposite driving
4849 50 : MSLane* oldOpposite = oldLane->getOpposite();
4850 50 : if (oldOpposite != nullptr) {
4851 50 : myFurtherLanes.push_back(oldOpposite);
4852 52 : myFurtherLanesPosLat.push_back(0);
4853 : // small value since the lane is going in the other direction
4854 50 : myState.myBackPos = getLength() - myState.myPos;
4855 50 : myAngle = computeAngle();
4856 : } else {
4857 : SOFT_ASSERT(false);
4858 : }
4859 : }
4860 : } else {
4861 406541 : myState.myPos = myLane->getOppositePos(myState.myPos);
4862 406541 : myLane = newOpposite;
4863 : oldLane = oldLaneMaybeOpposite;
4864 : //std::cout << SIMTIME << " updated myLane=" << Named::getIDSecure(myLane) << " oldLane=" << oldLane->getID() << "\n";
4865 406541 : myCachedPosition = Position::INVALID;
4866 406541 : myLaneChangeModel->updateShadowLane();
4867 : }
4868 : }
4869 700645373 : workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
4870 : // Return whether the vehicle did move to another lane
4871 1401290742 : return myLane != oldLane;
4872 700645373 : }
4873 :
4874 : void
4875 216433 : MSVehicle::executeFractionalMove(double dist) {
4876 216433 : myState.myPos += dist;
4877 216433 : myState.myLastCoveredDist = dist;
4878 216433 : myCachedPosition = Position::INVALID;
4879 :
4880 216433 : const std::vector<const MSLane*> lanes = getUpcomingLanesUntil(dist);
4881 216433 : const SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
4882 448871 : for (int i = 0; i < (int)lanes.size(); i++) {
4883 232438 : MSLink* link = nullptr;
4884 232438 : if (i + 1 < (int)lanes.size()) {
4885 16005 : const MSLane* const to = lanes[i + 1];
4886 16005 : const bool internal = to->isInternal();
4887 16010 : for (MSLink* const l : lanes[i]->getLinkCont()) {
4888 16010 : if ((internal && l->getViaLane() == to) || (!internal && l->getLane() == to)) {
4889 16005 : link = l;
4890 16005 : break;
4891 : }
4892 : }
4893 : }
4894 232438 : myLFLinkLanes.emplace_back(link, getSpeed(), getSpeed(), true, t, getSpeed(), 0, 0, dist);
4895 : }
4896 : // minimum execute move:
4897 : std::vector<MSLane*> passedLanes;
4898 : // Reason for a possible emergency stop
4899 216433 : if (lanes.size() > 1) {
4900 4005 : myLane->removeVehicle(this, MSMoveReminder::NOTIFICATION_JUNCTION, false);
4901 : }
4902 : std::string emergencyReason;
4903 216433 : processLaneAdvances(passedLanes, emergencyReason);
4904 : #ifdef DEBUG_EXTRAPOLATE_DEPARTPOS
4905 : if (DEBUG_COND) {
4906 : std::cout << SIMTIME << " veh=" << getID() << " executeFractionalMove dist=" << dist
4907 : << " passedLanes=" << toString(passedLanes) << " lanes=" << toString(lanes)
4908 : << " finalPos=" << myState.myPos
4909 : << " speed=" << getSpeed()
4910 : << " myFurtherLanes=" << toString(myFurtherLanes)
4911 : << "\n";
4912 : }
4913 : #endif
4914 216433 : workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
4915 216433 : if (lanes.size() > 1) {
4916 4010 : for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
4917 : #ifdef DEBUG_FURTHER
4918 : if (DEBUG_COND) {
4919 : std::cout << SIMTIME << " leaveLane \n";
4920 : }
4921 : #endif
4922 5 : (*i)->resetPartialOccupation(this);
4923 : }
4924 : myFurtherLanes.clear();
4925 : myFurtherLanesPosLat.clear();
4926 4005 : myLane->forceVehicleInsertion(this, getPositionOnLane(), MSMoveReminder::NOTIFICATION_JUNCTION, getLateralPositionOnLane());
4927 : }
4928 216433 : }
4929 :
4930 :
4931 : void
4932 708905778 : MSVehicle::updateState(double vNext, bool parking) {
4933 : // update position and speed
4934 : double deltaPos; // positional change
4935 708905778 : if (MSGlobals::gSemiImplicitEulerUpdate) {
4936 : // euler
4937 609426134 : deltaPos = SPEED2DIST(vNext);
4938 : } else {
4939 : // ballistic
4940 99479644 : deltaPos = getDeltaPos(SPEED2ACCEL(vNext - myState.mySpeed));
4941 : }
4942 :
4943 : // the *mean* acceleration during the next step (probably most appropriate for emission calculation)
4944 : // NOTE: for the ballistic update vNext may be negative, indicating a stop.
4945 708905778 : myAcceleration = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
4946 :
4947 : #ifdef DEBUG_EXEC_MOVE
4948 : if (DEBUG_COND) {
4949 : std::cout << SIMTIME << " updateState() for veh '" << getID() << "': deltaPos=" << deltaPos
4950 : << " pos=" << myState.myPos << " newPos=" << myState.myPos + deltaPos << std::endl;
4951 : }
4952 : #endif
4953 708905778 : double decelPlus = -myAcceleration - getCarFollowModel().getMaxDecel() - NUMERICAL_EPS;
4954 708905778 : if (decelPlus > 0) {
4955 428682 : const double previousAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
4956 428682 : if (myAcceleration + NUMERICAL_EPS < previousAcceleration) {
4957 : // vehicle brakes beyond wished maximum deceleration (only warn at the start of the braking manoeuvre)
4958 289362 : decelPlus += 2 * NUMERICAL_EPS;
4959 289362 : const double emergencyFraction = decelPlus / MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel());
4960 289362 : if (emergencyFraction >= MSGlobals::gEmergencyDecelWarningThreshold) {
4961 94845 : WRITE_WARNINGF(TL("Vehicle '%' performs emergency braking on lane '%' with decel=%, wished=%, severity=%, time=%."),
4962 : //+ " decelPlus=" + toString(decelPlus)
4963 : //+ " prevAccel=" + toString(previousAcceleration)
4964 : //+ " reserve=" + toString(MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel()))
4965 : getID(), myLane->getID(), -myAcceleration, getCarFollowModel().getMaxDecel(), emergencyFraction, time2string(SIMSTEP));
4966 31615 : MSNet::getInstance()->getVehicleControl().registerEmergencyBraking();
4967 : }
4968 : }
4969 : }
4970 :
4971 708905778 : myState.myPreviousSpeed = myState.mySpeed;
4972 708905778 : myState.mySpeed = MAX2(vNext, 0.);
4973 :
4974 708905778 : if (isRemoteControlled()) {
4975 6617 : deltaPos = myInfluencer->implicitDeltaPosRemote(this);
4976 : }
4977 :
4978 708905778 : myState.myPos += deltaPos;
4979 708905778 : myState.myLastCoveredDist = deltaPos;
4980 708905778 : myNextTurn.first -= deltaPos;
4981 :
4982 708905778 : if (!parking) {
4983 700645373 : myCachedPosition = Position::INVALID;
4984 : }
4985 708905778 : }
4986 :
4987 : void
4988 8260405 : MSVehicle::updateParkingState() {
4989 8260405 : updateState(0, true);
4990 : // deboard while parked
4991 8260405 : if (myPersonDevice != nullptr) {
4992 620048 : myPersonDevice->notifyMove(*this, getPositionOnLane(), getPositionOnLane(), 0);
4993 : }
4994 8260405 : if (myContainerDevice != nullptr) {
4995 59887 : myContainerDevice->notifyMove(*this, getPositionOnLane(), getPositionOnLane(), 0);
4996 : }
4997 16674524 : for (MSVehicleDevice* const dev : myDevices) {
4998 8414119 : dev->notifyParking();
4999 : }
5000 8260405 : }
5001 :
5002 :
5003 : void
5004 30623 : MSVehicle::replaceVehicleType(const MSVehicleType* type) {
5005 30623 : MSBaseVehicle::replaceVehicleType(type);
5006 30623 : delete myCFVariables;
5007 30623 : myCFVariables = type->getCarFollowModel().createVehicleVariables();
5008 30623 : }
5009 :
5010 :
5011 : const MSLane*
5012 1378899704 : MSVehicle::getBackLane() const {
5013 1378899704 : if (myFurtherLanes.size() > 0) {
5014 18900204 : return myFurtherLanes.back();
5015 : } else {
5016 1359999500 : return myLane;
5017 : }
5018 : }
5019 :
5020 :
5021 : double
5022 702769277 : MSVehicle::updateFurtherLanes(std::vector<MSLane*>& furtherLanes, std::vector<double>& furtherLanesPosLat,
5023 : const std::vector<MSLane*>& passedLanes) {
5024 : #ifdef DEBUG_SETFURTHER
5025 : if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID()
5026 : << " updateFurtherLanes oldFurther=" << toString(furtherLanes)
5027 : << " oldFurtherPosLat=" << toString(furtherLanesPosLat)
5028 : << " passed=" << toString(passedLanes)
5029 : << "\n";
5030 : #endif
5031 718569569 : for (MSLane* further : furtherLanes) {
5032 15800292 : further->resetPartialOccupation(this);
5033 15800292 : if (further->getBidiLane() != nullptr
5034 15800292 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5035 105874 : further->getBidiLane()->resetPartialOccupation(this);
5036 : }
5037 : }
5038 :
5039 : std::vector<MSLane*> newFurther;
5040 : std::vector<double> newFurtherPosLat;
5041 702769277 : double backPosOnPreviousLane = myState.myPos - getLength();
5042 : bool widthShift = myFurtherLanesPosLat.size() > myFurtherLanes.size();
5043 702769277 : if (passedLanes.size() > 1) {
5044 : // There are candidates for further lanes. (passedLanes[-1] is the current lane, or current shadow lane in context of updateShadowLanes())
5045 : std::vector<MSLane*>::const_iterator fi = furtherLanes.begin();
5046 : std::vector<double>::const_iterator fpi = furtherLanesPosLat.begin();
5047 44472852 : for (auto pi = passedLanes.rbegin() + 1; pi != passedLanes.rend() && backPosOnPreviousLane < 0; ++pi) {
5048 : // As long as vehicle back reaches into passed lane, add it to the further lanes
5049 15733763 : MSLane* further = *pi;
5050 15733763 : newFurther.push_back(further);
5051 15733763 : backPosOnPreviousLane += further->setPartialOccupation(this);
5052 15733763 : if (further->getBidiLane() != nullptr
5053 15733763 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5054 103990 : further->getBidiLane()->setPartialOccupation(this);
5055 : }
5056 15733763 : if (fi != furtherLanes.end() && further == *fi) {
5057 : // Lateral position on this lane is already known. Assume constant and use old value.
5058 5580170 : newFurtherPosLat.push_back(*fpi);
5059 : ++fi;
5060 : ++fpi;
5061 : } else {
5062 : // The lane *pi was not in furtherLanes before.
5063 : // If it is downstream, we assume as lateral position the current position
5064 : // If it is a new lane upstream (can appear as shadow further in case of LC-maneuvering, e.g.)
5065 : // we assign the last known lateral position.
5066 10153593 : if (newFurtherPosLat.size() == 0) {
5067 9542648 : if (widthShift) {
5068 1487595 : newFurtherPosLat.push_back(myFurtherLanesPosLat.back());
5069 : } else {
5070 8055053 : newFurtherPosLat.push_back(myState.myPosLat);
5071 : }
5072 : } else {
5073 610945 : newFurtherPosLat.push_back(newFurtherPosLat.back());
5074 : }
5075 : }
5076 : #ifdef DEBUG_SETFURTHER
5077 : if (DEBUG_COND) {
5078 : std::cout << SIMTIME << " updateFurtherLanes \n"
5079 : << " further lane '" << further->getID() << "' backPosOnPreviousLane=" << backPosOnPreviousLane
5080 : << std::endl;
5081 : }
5082 : #endif
5083 : }
5084 28739089 : furtherLanes = newFurther;
5085 28739089 : furtherLanesPosLat = newFurtherPosLat;
5086 : } else {
5087 : furtherLanes.clear();
5088 : furtherLanesPosLat.clear();
5089 : }
5090 : #ifdef DEBUG_SETFURTHER
5091 : if (DEBUG_COND) std::cout
5092 : << " newFurther=" << toString(furtherLanes)
5093 : << " newFurtherPosLat=" << toString(furtherLanesPosLat)
5094 : << " newBackPos=" << backPosOnPreviousLane
5095 : << "\n";
5096 : #endif
5097 702769277 : return backPosOnPreviousLane;
5098 702769277 : }
5099 :
5100 :
5101 : double
5102 33803284519 : MSVehicle::getBackPositionOnLane(const MSLane* lane, bool calledByGetPosition) const {
5103 : #ifdef DEBUG_FURTHER
5104 : if (DEBUG_COND) {
5105 : std::cout << SIMTIME
5106 : << " getBackPositionOnLane veh=" << getID()
5107 : << " lane=" << Named::getIDSecure(lane)
5108 : << " cbgP=" << calledByGetPosition
5109 : << " pos=" << myState.myPos
5110 : << " backPos=" << myState.myBackPos
5111 : << " myLane=" << myLane->getID()
5112 : << " myLaneBidi=" << Named::getIDSecure(myLane->getBidiLane())
5113 : << " further=" << toString(myFurtherLanes)
5114 : << " furtherPosLat=" << toString(myFurtherLanesPosLat)
5115 : << "\n shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
5116 : << " shadowFurther=" << toString(myLaneChangeModel->getShadowFurtherLanes())
5117 : << " shadowFurtherPosLat=" << toString(myLaneChangeModel->getShadowFurtherLanesPosLat())
5118 : << "\n targetLane=" << Named::getIDSecure(myLaneChangeModel->getTargetLane())
5119 : << " furtherTargets=" << toString(myLaneChangeModel->getFurtherTargetLanes())
5120 : << std::endl;
5121 : }
5122 : #endif
5123 33803284519 : if (lane == myLane
5124 8150120725 : || lane == myLaneChangeModel->getShadowLane()
5125 38460331051 : || lane == myLaneChangeModel->getTargetLane()) {
5126 29147808353 : if (myLaneChangeModel->isOpposite()) {
5127 231392051 : if (lane == myLaneChangeModel->getShadowLane()) {
5128 199248795 : return lane->getLength() - myState.myPos - myType->getLength();
5129 : } else {
5130 36938520 : return myState.myPos + (calledByGetPosition ? -1 : 1) * myType->getLength();
5131 : }
5132 28916416302 : } else if (&lane->getEdge() != &myLane->getEdge()) {
5133 20847462 : return lane->getLength() - myState.myPos + (calledByGetPosition ? -1 : 1) * myType->getLength();
5134 : } else {
5135 : // account for parallel lanes of different lengths in the most conservative manner (i.e. while turning)
5136 57791840534 : return myState.myPos - myType->getLength() + MIN2(0.0, lane->getLength() - myLane->getLength());
5137 : }
5138 4655476166 : } else if (lane == myLane->getBidiLane()) {
5139 48261379 : return lane->getLength() - myState.myPos + myType->getLength() * (calledByGetPosition ? -1 : 1);
5140 4615646989 : } else if (myFurtherLanes.size() > 0 && lane == myFurtherLanes.back()) {
5141 4567142427 : return myState.myBackPos;
5142 48504562 : } else if ((myLaneChangeModel->getShadowFurtherLanes().size() > 0 && lane == myLaneChangeModel->getShadowFurtherLanes().back())
5143 49110688 : || (myLaneChangeModel->getFurtherTargetLanes().size() > 0 && lane == myLaneChangeModel->getFurtherTargetLanes().back())) {
5144 : assert(myFurtherLanes.size() > 0);
5145 19295095 : if (lane->getLength() == myFurtherLanes.back()->getLength()) {
5146 18552523 : return myState.myBackPos;
5147 : } else {
5148 : // interpolate
5149 : //if (DEBUG_COND) {
5150 : //if (myFurtherLanes.back()->getLength() != lane->getLength()) {
5151 : // std::cout << SIMTIME << " veh=" << getID() << " lane=" << lane->getID() << " further=" << myFurtherLanes.back()->getID()
5152 : // << " len=" << lane->getLength() << " fLen=" << myFurtherLanes.back()->getLength()
5153 : // << " backPos=" << myState.myBackPos << " result=" << myState.myBackPos / myFurtherLanes.back()->getLength() * lane->getLength() << "\n";
5154 : //}
5155 742572 : return myState.myBackPos / myFurtherLanes.back()->getLength() * lane->getLength();
5156 : }
5157 : } else {
5158 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherLanes=" << toString(myFurtherLanes) << "\n";
5159 29209467 : double leftLength = myType->getLength() - myState.myPos;
5160 :
5161 : std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin();
5162 31133561 : while (leftLength > 0 && i != myFurtherLanes.end()) {
5163 31081556 : leftLength -= (*i)->getLength();
5164 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5165 31081556 : if (*i == lane) {
5166 28130910 : return -leftLength;
5167 2950646 : } else if (*i == lane->getBidiLane()) {
5168 1026552 : return lane->getLength() + leftLength - (calledByGetPosition ? 2 * myType->getLength() : 0);
5169 : }
5170 : ++i;
5171 : }
5172 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
5173 52005 : leftLength = myType->getLength() - myState.myPos;
5174 52005 : i = myLaneChangeModel->getShadowFurtherLanes().begin();
5175 52005 : while (leftLength > 0 && i != myLaneChangeModel->getShadowFurtherLanes().end()) {
5176 51997 : leftLength -= (*i)->getLength();
5177 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5178 51997 : if (*i == lane) {
5179 23376 : return -leftLength;
5180 28621 : } else if (*i == lane->getBidiLane()) {
5181 28621 : return lane->getLength() + leftLength - (calledByGetPosition ? 2 * myType->getLength() : 0);
5182 : }
5183 : ++i;
5184 : }
5185 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherTargetLanes=" << toString(myLaneChangeModel->getFurtherTargetLanes()) << "\n";
5186 8 : leftLength = myType->getLength() - myState.myPos;
5187 : i = getFurtherLanes().begin();
5188 8 : const std::vector<MSLane*> furtherTargetLanes = myLaneChangeModel->getFurtherTargetLanes();
5189 : auto j = furtherTargetLanes.begin();
5190 9 : while (leftLength > 0 && j != furtherTargetLanes.end()) {
5191 3 : leftLength -= (*i)->getLength();
5192 : // if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5193 3 : if (*j == lane) {
5194 2 : return -leftLength;
5195 1 : } else if (*j == lane->getBidiLane()) {
5196 0 : return lane->getLength() + leftLength - (calledByGetPosition ? 2 * myType->getLength() : 0);
5197 : }
5198 : ++i;
5199 : ++j;
5200 : }
5201 24 : WRITE_WARNINGF("Request backPos of vehicle '%' for invalid lane '%' time=%.",
5202 : getID(), Named::getIDSecure(lane), time2string(SIMSTEP))
5203 : SOFT_ASSERT(false);
5204 6 : return myState.myBackPos;
5205 8 : }
5206 : }
5207 :
5208 :
5209 : double
5210 27400924490 : MSVehicle::getPositionOnLane(const MSLane* lane) const {
5211 27400924490 : return getBackPositionOnLane(lane, true) + myType->getLength();
5212 : }
5213 :
5214 :
5215 : bool
5216 435344074 : MSVehicle::isFrontOnLane(const MSLane* lane) const {
5217 435344074 : return lane == myLane || lane == myLaneChangeModel->getShadowLane() || lane == myLane->getBidiLane();
5218 : }
5219 :
5220 :
5221 : void
5222 631599154 : MSVehicle::checkRewindLinkLanes(const double lengthsInFront, DriveItemVector& lfLinks) const {
5223 631599154 : if (MSGlobals::gUsingInternalLanes && !myLane->getEdge().isRoundabout() && !myLaneChangeModel->isOpposite()) {
5224 628411394 : double seenSpace = -lengthsInFront;
5225 : #ifdef DEBUG_CHECKREWINDLINKLANES
5226 : if (DEBUG_COND) {
5227 : std::cout << "\nCHECK_REWIND_LINKLANES\n" << " veh=" << getID() << " lengthsInFront=" << lengthsInFront << "\n";
5228 : };
5229 : #endif
5230 628411394 : bool foundStopped = false;
5231 : // compute available space until a stopped vehicle is found
5232 : // this is the sum of non-interal lane length minus in-between vehicle lengths
5233 1832568133 : for (int i = 0; i < (int)lfLinks.size(); ++i) {
5234 : // skip unset links
5235 1204156739 : DriveProcessItem& item = lfLinks[i];
5236 : #ifdef DEBUG_CHECKREWINDLINKLANES
5237 : if (DEBUG_COND) std::cout << SIMTIME
5238 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5239 : << " foundStopped=" << foundStopped;
5240 : #endif
5241 1204156739 : if (item.myLink == nullptr || foundStopped) {
5242 396432513 : if (!foundStopped) {
5243 344000451 : item.availableSpace += seenSpace;
5244 : } else {
5245 52432062 : item.availableSpace = seenSpace;
5246 : }
5247 : #ifdef DEBUG_CHECKREWINDLINKLANES
5248 : if (DEBUG_COND) {
5249 : std::cout << " avail=" << item.availableSpace << "\n";
5250 : }
5251 : #endif
5252 396432513 : continue;
5253 : }
5254 : // get the next lane, determine whether it is an internal lane
5255 : const MSLane* approachedLane = item.myLink->getViaLane();
5256 807724226 : if (approachedLane != nullptr) {
5257 440585872 : if (keepClear(item.myLink)) {
5258 139288055 : seenSpace = seenSpace - approachedLane->getBruttoVehLenSum();
5259 139288055 : if (approachedLane == myLane) {
5260 48432 : seenSpace += getVehicleType().getLengthWithGap();
5261 : }
5262 : } else {
5263 301297817 : seenSpace = seenSpace + approachedLane->getSpaceTillLastStanding(this, foundStopped);// - approachedLane->getBruttoVehLenSum() + approachedLane->getLength();
5264 : }
5265 440585872 : item.availableSpace = seenSpace;
5266 : #ifdef DEBUG_CHECKREWINDLINKLANES
5267 : if (DEBUG_COND) std::cout
5268 : << " approached=" << approachedLane->getID()
5269 : << " approachedBrutto=" << approachedLane->getBruttoVehLenSum()
5270 : << " avail=" << item.availableSpace
5271 : << " seenSpace=" << seenSpace
5272 : << " hadStoppedVehicle=" << item.hadStoppedVehicle
5273 : << " lengthsInFront=" << lengthsInFront
5274 : << "\n";
5275 : #endif
5276 440585872 : continue;
5277 : }
5278 : approachedLane = item.myLink->getLane();
5279 367138354 : const MSVehicle* last = approachedLane->getLastAnyVehicle();
5280 367138354 : if (last == nullptr || last == this) {
5281 59469095 : if (approachedLane->getLength() > getVehicleType().getLength()
5282 59469095 : || keepClear(item.myLink)) {
5283 57130652 : seenSpace += approachedLane->getLength();
5284 : }
5285 59469095 : item.availableSpace = seenSpace;
5286 : #ifdef DEBUG_CHECKREWINDLINKLANES
5287 : if (DEBUG_COND) {
5288 : std::cout << " last=" << Named::getIDSecure(last) << " laneLength=" << approachedLane->getLength() << " avail=" << item.availableSpace << "\n";
5289 : }
5290 : #endif
5291 : } else {
5292 307669259 : bool foundStopped2 = false;
5293 307669259 : double spaceTillLastStanding = approachedLane->getSpaceTillLastStanding(this, foundStopped2);
5294 307669259 : if (approachedLane->getBidiLane() != nullptr) {
5295 557688 : const MSVehicle* oncomingVeh = approachedLane->getBidiLane()->getFirstFullVehicle();
5296 557688 : if (oncomingVeh) {
5297 379250 : const double oncomingGap = approachedLane->getLength() - oncomingVeh->getPositionOnLane();
5298 379250 : const double oncomingBGap = oncomingVeh->getBrakeGap(true);
5299 : // oncoming movement until ego enters the junction
5300 379250 : const double oncomingMove = STEPS2TIME(item.myArrivalTime - SIMSTEP) * oncomingVeh->getSpeed();
5301 379250 : const double spaceTillOncoming = oncomingGap - oncomingBGap - oncomingMove;
5302 : spaceTillLastStanding = MIN2(spaceTillLastStanding, spaceTillOncoming);
5303 379250 : if (spaceTillOncoming <= getVehicleType().getLengthWithGap()) {
5304 35036 : foundStopped = true;
5305 : }
5306 : #ifdef DEBUG_CHECKREWINDLINKLANES
5307 : if (DEBUG_COND) {
5308 : std::cout << " oVeh=" << oncomingVeh->getID()
5309 : << " oGap=" << oncomingGap
5310 : << " bGap=" << oncomingBGap
5311 : << " mGap=" << oncomingMove
5312 : << " sto=" << spaceTillOncoming;
5313 : }
5314 : #endif
5315 : }
5316 : }
5317 307669259 : seenSpace += spaceTillLastStanding;
5318 307669259 : if (foundStopped2) {
5319 21130770 : foundStopped = true;
5320 21130770 : item.hadStoppedVehicle = true;
5321 : }
5322 307669259 : item.availableSpace = seenSpace;
5323 307669259 : if (last->myHaveToWaitOnNextLink || last->isStopped()) {
5324 30661455 : foundStopped = true;
5325 30661455 : item.hadStoppedVehicle = true;
5326 : }
5327 : #ifdef DEBUG_CHECKREWINDLINKLANES
5328 : if (DEBUG_COND) std::cout
5329 : << " approached=" << approachedLane->getID()
5330 : << " last=" << last->getID()
5331 : << " lastHasToWait=" << last->myHaveToWaitOnNextLink
5332 : << " lastBrakeLight=" << last->signalSet(VEH_SIGNAL_BRAKELIGHT)
5333 : << " lastBrakeGap=" << last->getCarFollowModel().brakeGap(last->getSpeed())
5334 : << " lastGap=" << (last->getBackPositionOnLane(approachedLane) + last->getCarFollowModel().brakeGap(last->getSpeed()) - last->getSpeed() * last->getCarFollowModel().getHeadwayTime()
5335 : // gap of last up to the next intersection
5336 : - last->getVehicleType().getMinGap())
5337 : << " stls=" << spaceTillLastStanding
5338 : << " avail=" << item.availableSpace
5339 : << " seenSpace=" << seenSpace
5340 : << " foundStopped=" << foundStopped
5341 : << " foundStopped2=" << foundStopped2
5342 : << "\n";
5343 : #endif
5344 : }
5345 : }
5346 :
5347 : // check which links allow continuation and add pass available to the previous item
5348 1204156739 : for (int i = ((int)lfLinks.size() - 1); i > 0; --i) {
5349 575745345 : DriveProcessItem& item = lfLinks[i - 1];
5350 575745345 : DriveProcessItem& nextItem = lfLinks[i];
5351 575745345 : const bool canLeaveJunction = item.myLink->getViaLane() == nullptr || nextItem.myLink == nullptr || nextItem.mySetRequest;
5352 : const bool opened = (item.myLink != nullptr
5353 575745345 : && (canLeaveJunction || (
5354 : // indirect bicycle turn
5355 28003853 : nextItem.myLink != nullptr && nextItem.myLink->isInternalJunctionLink() && nextItem.myLink->haveRed()))
5356 547755554 : && (
5357 547755554 : item.myLink->havePriority()
5358 27523394 : || i == 1 // the upcoming link (item 0) is checked in executeMove anyway. No need to use outdata approachData here
5359 5202773 : || (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority())
5360 5173340 : || item.myLink->opened(item.myArrivalTime, item.myArrivalSpeed,
5361 5173340 : item.getLeaveSpeed(), getVehicleType().getLength(),
5362 5173340 : getImpatience(), getCarFollowModel().getMaxDecel(), getWaitingTime(), getLateralPositionOnLane(), nullptr, false, this)));
5363 575745345 : bool allowsContinuation = (item.myLink == nullptr || item.myLink->isCont() || opened) && !item.hadStoppedVehicle;
5364 : #ifdef DEBUG_CHECKREWINDLINKLANES
5365 : if (DEBUG_COND) std::cout
5366 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5367 : << " canLeave=" << canLeaveJunction
5368 : << " opened=" << opened
5369 : << " allowsContinuation=" << allowsContinuation
5370 : << " foundStopped=" << foundStopped
5371 : << "\n";
5372 : #endif
5373 575745345 : if (!opened && item.myLink != nullptr) {
5374 28719619 : foundStopped = true;
5375 28719619 : if (i > 1) {
5376 4761507 : DriveProcessItem& item2 = lfLinks[i - 2];
5377 4761507 : if (item2.myLink != nullptr && item2.myLink->isCont()) {
5378 : allowsContinuation = true;
5379 : }
5380 : }
5381 : }
5382 572745976 : if (allowsContinuation) {
5383 516150295 : item.availableSpace = nextItem.availableSpace;
5384 : #ifdef DEBUG_CHECKREWINDLINKLANES
5385 : if (DEBUG_COND) std::cout
5386 : << " link=" << (item.myLink == nullptr ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5387 : << " copy nextAvail=" << nextItem.availableSpace
5388 : << "\n";
5389 : #endif
5390 : }
5391 : }
5392 :
5393 : // find removalBegin
5394 : int removalBegin = -1;
5395 757324301 : for (int i = 0; foundStopped && i < (int)lfLinks.size() && removalBegin < 0; ++i) {
5396 : // skip unset links
5397 128912907 : const DriveProcessItem& item = lfLinks[i];
5398 128912907 : if (item.myLink == nullptr) {
5399 6784154 : continue;
5400 : }
5401 : /*
5402 : double impatienceCorrection = MAX2(0., double(double(myWaitingTime)));
5403 : if (seenSpace<getVehicleType().getLengthWithGap()-impatienceCorrection/10.&&nextSeenNonInternal!=0) {
5404 : removalBegin = lastLinkToInternal;
5405 : }
5406 : */
5407 :
5408 122128753 : const double leftSpace = item.availableSpace - getVehicleType().getLengthWithGap();
5409 : #ifdef DEBUG_CHECKREWINDLINKLANES
5410 : if (DEBUG_COND) std::cout
5411 : << SIMTIME
5412 : << " veh=" << getID()
5413 : << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
5414 : << " avail=" << item.availableSpace
5415 : << " leftSpace=" << leftSpace
5416 : << "\n";
5417 : #endif
5418 122128753 : if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
5419 : double impatienceCorrection = 0;
5420 : /*
5421 : if(item.myLink->getState()==LINKSTATE_MINOR) {
5422 : impatienceCorrection = MAX2(0., STEPS2TIME(myWaitingTime));
5423 : }
5424 : */
5425 : // may ignore keepClear rules
5426 76931547 : if (leftSpace < -impatienceCorrection / 10. && keepClear(item.myLink)) {
5427 : removalBegin = i;
5428 : }
5429 : //removalBegin = i;
5430 : }
5431 : }
5432 : // abort requests
5433 628411394 : if (removalBegin != -1 && !(removalBegin == 0 && myLane->getEdge().isInternal())) {
5434 30798432 : const double brakeGap = getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.);
5435 105457307 : while (removalBegin < (int)(lfLinks.size())) {
5436 79604823 : DriveProcessItem& dpi = lfLinks[removalBegin];
5437 79604823 : if (dpi.myLink == nullptr) {
5438 : break;
5439 : }
5440 74658875 : dpi.myVLinkPass = dpi.myVLinkWait;
5441 : #ifdef DEBUG_CHECKREWINDLINKLANES
5442 : if (DEBUG_COND) {
5443 : std::cout << " removalBegin=" << removalBegin << " brakeGap=" << brakeGap << " dist=" << dpi.myDistance << " speed=" << myState.mySpeed << " a2s=" << ACCEL2SPEED(getCarFollowModel().getMaxDecel()) << "\n";
5444 : }
5445 : #endif
5446 74658875 : if (dpi.myDistance >= brakeGap + POSITION_EPS) {
5447 : // always leave junctions after requesting to enter
5448 74650067 : if (!dpi.myLink->isExitLink() || !lfLinks[removalBegin - 1].mySetRequest) {
5449 74642431 : dpi.mySetRequest = false;
5450 : }
5451 : }
5452 74658875 : ++removalBegin;
5453 : }
5454 : }
5455 : }
5456 631599154 : }
5457 :
5458 :
5459 : void
5460 703180022 : MSVehicle::setApproachingForAllLinks() {
5461 703180022 : if (!myActionStep) {
5462 : return;
5463 : }
5464 631599154 : removeApproachingInformation(myLFLinkLanesPrev);
5465 1843569000 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5466 1211969846 : if (dpi.myLink != nullptr) {
5467 859596658 : if (dpi.myLink->getState() == LINKSTATE_ALLWAY_STOP) {
5468 2850520 : dpi.myArrivalTime += (SUMOTime)RandHelper::rand((int)2, getRNG()); // tie braker
5469 : }
5470 859596658 : dpi.myLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
5471 859596658 : dpi.mySetRequest, dpi.myArrivalSpeedBraking, getWaitingTimeFor(dpi.myLink), dpi.myDistance, getLateralPositionOnLane());
5472 : }
5473 : }
5474 631599154 : if (isRail()) {
5475 7956560 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5476 6616182 : if (dpi.myLink != nullptr && dpi.myLink->getTLLogic() != nullptr && dpi.myLink->getTLLogic()->getLogicType() == TrafficLightType::RAIL_SIGNAL) {
5477 593329 : MSRailSignalControl::getInstance().notifyApproach(dpi.myLink);
5478 : }
5479 : }
5480 : }
5481 631599154 : if (myLaneChangeModel->getShadowLane() != nullptr) {
5482 : // register on all shadow links
5483 7634024 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
5484 5065946 : if (dpi.myLink != nullptr) {
5485 3457787 : MSLink* parallelLink = dpi.myLink->getParallelLink(myLaneChangeModel->getShadowDirection());
5486 3457787 : if (parallelLink == nullptr && getLaneChangeModel().isOpposite() && dpi.myLink->isEntryLink()) {
5487 : // register on opposite direction entry link to warn foes at minor side road
5488 169783 : parallelLink = dpi.myLink->getOppositeDirectionLink();
5489 : }
5490 3457787 : if (parallelLink != nullptr) {
5491 2455540 : const double latOffset = getLane()->getRightSideOnEdge() - myLaneChangeModel->getShadowLane()->getRightSideOnEdge();
5492 2455540 : parallelLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
5493 2455540 : dpi.mySetRequest, dpi.myArrivalSpeedBraking, getWaitingTimeFor(dpi.myLink), dpi.myDistance,
5494 : latOffset);
5495 2455540 : myLaneChangeModel->setShadowApproachingInformation(parallelLink);
5496 : }
5497 : }
5498 : }
5499 : }
5500 : #ifdef DEBUG_PLAN_MOVE
5501 : if (DEBUG_COND) {
5502 : std::cout << SIMTIME
5503 : << " veh=" << getID()
5504 : << " after checkRewindLinkLanes\n";
5505 : for (DriveProcessItem& dpi : myLFLinkLanes) {
5506 : std::cout
5507 : << " vPass=" << dpi.myVLinkPass
5508 : << " vWait=" << dpi.myVLinkWait
5509 : << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
5510 : << " request=" << dpi.mySetRequest
5511 : << " atime=" << dpi.myArrivalTime
5512 : << "\n";
5513 : }
5514 : }
5515 : #endif
5516 : }
5517 :
5518 :
5519 : void
5520 1850 : MSVehicle::registerInsertionApproach(MSLink* link, double dist) {
5521 : DriveProcessItem dpi(0, dist);
5522 1850 : dpi.myLink = link;
5523 1850 : const double arrivalSpeedBraking = getCarFollowModel().getMinimalArrivalSpeedEuler(dist, getSpeed());
5524 1850 : link->setApproaching(this, SUMOTime_MAX, 0, 0, false, arrivalSpeedBraking, 0, dpi.myDistance, 0);
5525 : // ensure cleanup in the next step
5526 1850 : myLFLinkLanes.push_back(dpi);
5527 1850 : MSRailSignalControl::getInstance().notifyApproach(link);
5528 1850 : }
5529 :
5530 :
5531 : void
5532 19483389 : MSVehicle::enterLaneAtMove(MSLane* enteredLane, bool onTeleporting) {
5533 19483389 : myAmOnNet = !onTeleporting;
5534 : // vaporizing edge?
5535 : /*
5536 : if (enteredLane->getEdge().isVaporizing()) {
5537 : // yep, let's do the vaporization...
5538 : myLane = enteredLane;
5539 : return true;
5540 : }
5541 : */
5542 : // Adjust MoveReminder offset to the next lane
5543 19483389 : adaptLaneEntering2MoveReminder(*enteredLane);
5544 : // set the entered lane as the current lane
5545 19483389 : MSLane* oldLane = myLane;
5546 19483389 : myLane = enteredLane;
5547 19483389 : myLastBestLanesEdge = nullptr;
5548 :
5549 : // internal edges are not a part of the route...
5550 19483389 : if (!enteredLane->getEdge().isInternal()) {
5551 : ++myCurrEdge;
5552 : assert(myLaneChangeModel->isOpposite() || haveValidStopEdges());
5553 : }
5554 19483389 : if (myInfluencer != nullptr) {
5555 8975 : myInfluencer->adaptLaneTimeLine(myLane->getIndex() - oldLane->getIndex());
5556 : }
5557 19483389 : if (!onTeleporting) {
5558 19464724 : activateReminders(MSMoveReminder::NOTIFICATION_JUNCTION, enteredLane);
5559 19464724 : if (MSGlobals::gLateralResolution > 0) {
5560 3801452 : myFurtherLanesPosLat.push_back(myState.myPosLat);
5561 : // transform lateral position when the lane width changes
5562 : assert(oldLane != nullptr);
5563 3801452 : const MSLink* const link = oldLane->getLinkTo(myLane);
5564 3801452 : if (link != nullptr) {
5565 3801411 : myState.myPosLat += link->getLateralShift();
5566 : } else {
5567 41 : myState.myPosLat += (oldLane->getCenterOnEdge() - myLane->getCanonicalPredecessorLane()->getRightSideOnEdge()) / 2;
5568 : }
5569 15663272 : } else if (fabs(myState.myPosLat) > NUMERICAL_EPS) {
5570 243621 : const double overlap = MAX2(0.0, getLateralOverlap(myState.myPosLat, oldLane));
5571 243621 : const double range = (oldLane->getWidth() - getVehicleType().getWidth()) * 0.5 + overlap;
5572 243621 : const double range2 = (myLane->getWidth() - getVehicleType().getWidth()) * 0.5 + overlap;
5573 243621 : myState.myPosLat *= range2 / range;
5574 : }
5575 19464724 : if (myLane->getBidiLane() != nullptr && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5576 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
5577 : // (unless the lane is shared with cars)
5578 27321 : myLane->getBidiLane()->setPartialOccupation(this);
5579 : }
5580 : } else {
5581 : // normal move() isn't called so reset position here. must be done
5582 : // before calling reminders
5583 18665 : myState.myPos = 0;
5584 18665 : myCachedPosition = Position::INVALID;
5585 18665 : activateReminders(MSMoveReminder::NOTIFICATION_TELEPORT, enteredLane);
5586 : }
5587 : // update via
5588 19483389 : if (myParameter->via.size() > 0 && myLane->getEdge().getID() == myParameter->via.front()) {
5589 7237 : myParameter->via.erase(myParameter->via.begin());
5590 : }
5591 19483389 : }
5592 :
5593 :
5594 : void
5595 1087322 : MSVehicle::enterLaneAtLaneChange(MSLane* enteredLane) {
5596 1087322 : myAmOnNet = true;
5597 1087322 : myLane = enteredLane;
5598 1087322 : myCachedPosition = Position::INVALID;
5599 : // need to update myCurrentLaneInBestLanes
5600 1087322 : updateBestLanes();
5601 : // switch to and activate the new lane's reminders
5602 : // keep OldLaneReminders
5603 1284912 : for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
5604 197590 : addReminder(*rem);
5605 : }
5606 1087322 : activateReminders(MSMoveReminder::NOTIFICATION_LANE_CHANGE, enteredLane);
5607 1087322 : MSLane* lane = myLane;
5608 1087322 : double leftLength = getVehicleType().getLength() - myState.myPos;
5609 : int deleteFurther = 0;
5610 : #ifdef DEBUG_SETFURTHER
5611 : if (DEBUG_COND) {
5612 : std::cout << SIMTIME << " enterLaneAtLaneChange entered=" << Named::getIDSecure(enteredLane) << " oldFurther=" << toString(myFurtherLanes) << "\n";
5613 : }
5614 : #endif
5615 1087322 : if (myLane->getBidiLane() != nullptr && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5616 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
5617 : // (unless the lane is shared with cars)
5618 19978 : myLane->getBidiLane()->setPartialOccupation(this);
5619 : }
5620 1172136 : for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
5621 84814 : if (lane != nullptr) {
5622 81795 : lane = lane->getLogicalPredecessorLane(myFurtherLanes[i]->getEdge());
5623 : }
5624 : #ifdef DEBUG_SETFURTHER
5625 : if (DEBUG_COND) {
5626 : std::cout << " enterLaneAtLaneChange i=" << i << " lane=" << Named::getIDSecure(lane) << " leftLength=" << leftLength << "\n";
5627 : }
5628 : #endif
5629 84814 : if (leftLength > 0) {
5630 84257 : if (lane != nullptr) {
5631 34890 : myFurtherLanes[i]->resetPartialOccupation(this);
5632 34890 : if (myFurtherLanes[i]->getBidiLane() != nullptr
5633 34890 : && (!isRailway(getVClass()) || (myFurtherLanes[i]->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5634 59 : myFurtherLanes[i]->getBidiLane()->resetPartialOccupation(this);
5635 : }
5636 : // lane changing onto longer lanes may reduce the number of
5637 : // remaining further lanes
5638 34890 : myFurtherLanes[i] = lane;
5639 34890 : myFurtherLanesPosLat[i] = myState.myPosLat;
5640 34890 : leftLength -= lane->setPartialOccupation(this);
5641 34890 : if (lane->getBidiLane() != nullptr
5642 34890 : && (!isRailway(getVClass()) || (lane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5643 1967 : lane->getBidiLane()->setPartialOccupation(this);
5644 : }
5645 34890 : myState.myBackPos = -leftLength;
5646 : #ifdef DEBUG_SETFURTHER
5647 : if (DEBUG_COND) {
5648 : std::cout << SIMTIME << " newBackPos=" << myState.myBackPos << "\n";
5649 : }
5650 : #endif
5651 : } else {
5652 : // keep the old values, but ensure there is no shadow
5653 49367 : if (myLaneChangeModel->isChangingLanes()) {
5654 15 : myLaneChangeModel->setNoShadowPartialOccupator(myFurtherLanes[i]);
5655 : }
5656 49367 : if (myState.myBackPos < 0) {
5657 46 : myState.myBackPos += myFurtherLanes[i]->getLength();
5658 : }
5659 : #ifdef DEBUG_SETFURTHER
5660 : if (DEBUG_COND) {
5661 : std::cout << SIMTIME << " i=" << i << " further=" << myFurtherLanes[i]->getID() << " newBackPos=" << myState.myBackPos << "\n";
5662 : }
5663 : #endif
5664 : }
5665 : } else {
5666 557 : myFurtherLanes[i]->resetPartialOccupation(this);
5667 557 : if (myFurtherLanes[i]->getBidiLane() != nullptr
5668 557 : && (!isRailway(getVClass()) || (myFurtherLanes[i]->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5669 0 : myFurtherLanes[i]->getBidiLane()->resetPartialOccupation(this);
5670 : }
5671 557 : deleteFurther++;
5672 : }
5673 : }
5674 1087322 : if (deleteFurther > 0) {
5675 : #ifdef DEBUG_SETFURTHER
5676 : if (DEBUG_COND) {
5677 : std::cout << SIMTIME << " veh=" << getID() << " shortening myFurtherLanes by " << deleteFurther << "\n";
5678 : }
5679 : #endif
5680 526 : myFurtherLanes.erase(myFurtherLanes.end() - deleteFurther, myFurtherLanes.end());
5681 526 : myFurtherLanesPosLat.erase(myFurtherLanesPosLat.end() - deleteFurther, myFurtherLanesPosLat.end());
5682 : }
5683 : #ifdef DEBUG_SETFURTHER
5684 : if (DEBUG_COND) {
5685 : std::cout << SIMTIME << " enterLaneAtLaneChange new furtherLanes=" << toString(myFurtherLanes)
5686 : << " furterLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
5687 : }
5688 : #endif
5689 1087322 : myAngle = computeAngle();
5690 1087322 : }
5691 :
5692 :
5693 : void
5694 3535055 : MSVehicle::computeFurtherLanes(MSLane* enteredLane, double pos, bool collision) {
5695 : // build the list of lanes the vehicle is lapping into
5696 3535055 : if (!myLaneChangeModel->isOpposite()) {
5697 3512654 : double leftLength = myType->getLength() - pos;
5698 3512654 : MSLane* clane = enteredLane;
5699 3512654 : int routeIndex = getRoutePosition();
5700 3616878 : while (leftLength > 0) {
5701 234098 : if (routeIndex > 0 && clane->getEdge().isNormal()) {
5702 : // get predecessor lane that corresponds to prior route
5703 4532 : routeIndex--;
5704 4532 : const MSEdge* fromRouteEdge = myRoute->getEdges()[routeIndex];
5705 : MSLane* target = clane;
5706 4532 : clane = nullptr;
5707 6064 : for (auto ili : target->getIncomingLanes()) {
5708 6057 : if (ili.lane->getEdge().getNormalBefore() == fromRouteEdge) {
5709 4525 : clane = ili.lane;
5710 4525 : break;
5711 : }
5712 : }
5713 : } else {
5714 229566 : clane = clane->getLogicalPredecessorLane();
5715 : }
5716 144567 : if (clane == nullptr || clane == myLane || clane == myLane->getBidiLane()
5717 378657 : || (clane->isInternal() && (
5718 121362 : clane->getLinkCont()[0]->getDirection() == LinkDirection::TURN
5719 81027 : || clane->getLinkCont()[0]->getDirection() == LinkDirection::TURN_LEFTHAND))) {
5720 : break;
5721 : }
5722 104224 : if (!collision || std::find(myFurtherLanes.begin(), myFurtherLanes.end(), clane) == myFurtherLanes.end()) {
5723 103821 : myFurtherLanes.push_back(clane);
5724 103821 : myFurtherLanesPosLat.push_back(myState.myPosLat);
5725 103821 : clane->setPartialOccupation(this);
5726 103821 : if (clane->getBidiLane() != nullptr
5727 103821 : && (!isRailway(getVClass()) || (clane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5728 5 : clane->getBidiLane()->setPartialOccupation(this);
5729 : }
5730 : }
5731 104224 : leftLength -= clane->getLength();
5732 : }
5733 3512654 : myState.myBackPos = -leftLength;
5734 : #ifdef DEBUG_SETFURTHER
5735 : if (DEBUG_COND) {
5736 : std::cout << SIMTIME << " computeFurtherLanes veh=" << getID() << " pos=" << pos << " myFurtherLanes=" << toString(myFurtherLanes) << " backPos=" << myState.myBackPos << "\n";
5737 : }
5738 : #endif
5739 : } else {
5740 : // clear partial occupation
5741 22797 : for (MSLane* further : myFurtherLanes) {
5742 : #ifdef DEBUG_SETFURTHER
5743 : if (DEBUG_COND) {
5744 : std::cout << SIMTIME << " opposite: resetPartialOccupation " << further->getID() << " \n";
5745 : }
5746 : #endif
5747 396 : further->resetPartialOccupation(this);
5748 396 : if (further->getBidiLane() != nullptr
5749 396 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5750 0 : further->getBidiLane()->resetPartialOccupation(this);
5751 : }
5752 : }
5753 : myFurtherLanes.clear();
5754 : myFurtherLanesPosLat.clear();
5755 : }
5756 3535055 : }
5757 :
5758 :
5759 : void
5760 3534639 : MSVehicle::enterLaneAtInsertion(MSLane* enteredLane, double pos, double speed, double posLat, MSMoveReminder::Notification notification) {
5761 3534639 : myState = State(pos, speed, posLat, pos - getVehicleType().getLength(), hasDeparted() ? myState.myPreviousSpeed : speed);
5762 3534639 : if (myDeparture == NOT_YET_DEPARTED) {
5763 3459970 : onDepart();
5764 : }
5765 3534639 : myCachedPosition = Position::INVALID;
5766 : assert(myState.myPos >= 0);
5767 : assert(myState.mySpeed >= 0);
5768 3534639 : myLane = enteredLane;
5769 3534639 : myAmOnNet = true;
5770 : // schedule action for the next timestep
5771 3534639 : myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + DELTA_T;
5772 3534639 : if (notification != MSMoveReminder::NOTIFICATION_TELEPORT) {
5773 3522622 : if (notification == MSMoveReminder::NOTIFICATION_PARKING && myInfluencer != nullptr) {
5774 12 : drawOutsideNetwork(false);
5775 : }
5776 : // set and activate the new lane's reminders, teleports already did that at enterLaneAtMove
5777 7484973 : for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
5778 3962351 : addReminder(*rem);
5779 : }
5780 3522622 : activateReminders(notification, enteredLane);
5781 : } else {
5782 12017 : myLastBestLanesEdge = nullptr;
5783 12017 : myLastBestLanesInternalLane = nullptr;
5784 12017 : myLaneChangeModel->resetState();
5785 13183 : while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()
5786 12699 : && myStops.front().pars.endPos < pos) {
5787 0 : WRITE_WARNINGF(TL("Vehicle '%' skips stop on lane '%' time=%."), getID(), myStops.front().lane->getID(),
5788 : time2string(MSNet::getInstance()->getCurrentTimeStep()));
5789 0 : cleanupParkingReservation();
5790 0 : myStops.pop_front();
5791 : }
5792 : // avoid startup-effects after teleport
5793 12017 : myTimeSinceStartup = getCarFollowModel().getStartupDelay() + DELTA_T;
5794 12017 : myStopSpeed = std::numeric_limits<double>::max();
5795 : }
5796 3534637 : computeFurtherLanes(enteredLane, pos);
5797 3534637 : if (MSGlobals::gLateralResolution > 0) {
5798 514125 : myLaneChangeModel->updateShadowLane();
5799 514125 : myLaneChangeModel->updateTargetLane();
5800 3020512 : } else if (MSGlobals::gLaneChangeDuration > 0) {
5801 40414 : myLaneChangeModel->updateShadowLane();
5802 : }
5803 3534637 : if (notification != MSMoveReminder::NOTIFICATION_LOAD_STATE) {
5804 3533290 : myAngle = computeAngle();
5805 3533290 : if (myLaneChangeModel->isOpposite()) {
5806 22401 : myAngle += M_PI;
5807 : }
5808 : }
5809 3534637 : if (MSNet::getInstance()->hasPersons()) {
5810 58404 : for (MSLane* further : myFurtherLanes) {
5811 876 : if (further->mustCheckJunctionCollisions()) {
5812 4 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(further);
5813 : }
5814 : }
5815 : }
5816 3534637 : }
5817 :
5818 :
5819 : void
5820 23983328 : MSVehicle::leaveLane(const MSMoveReminder::Notification reason, const MSLane* approachedLane) {
5821 66354706 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
5822 42371378 : if (rem->first->notifyLeave(*this, myState.myPos + rem->second, reason, approachedLane)) {
5823 : #ifdef _DEBUG
5824 : if (myTraceMoveReminders) {
5825 : traceMoveReminder("notifyLeave", rem->first, rem->second, true);
5826 : }
5827 : #endif
5828 : ++rem;
5829 : } else {
5830 : #ifdef _DEBUG
5831 : if (myTraceMoveReminders) {
5832 : traceMoveReminder("notifyLeave", rem->first, rem->second, false);
5833 : }
5834 : #endif
5835 : rem = myMoveReminders.erase(rem);
5836 : }
5837 : }
5838 23983328 : if ((reason == MSMoveReminder::NOTIFICATION_JUNCTION
5839 23983328 : || reason == MSMoveReminder::NOTIFICATION_TELEPORT
5840 4505958 : || reason == MSMoveReminder::NOTIFICATION_TELEPORT_CONTINUATION)
5841 19483620 : && myLane != nullptr) {
5842 19483591 : myOdometer += getLane()->getLength();
5843 : }
5844 23983299 : if (myLane != nullptr && myLane->getBidiLane() != nullptr && myAmOnNet
5845 24056597 : && (!isRailway(getVClass()) || (myLane->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5846 48760 : myLane->getBidiLane()->resetPartialOccupation(this);
5847 : }
5848 23983328 : if (reason != MSMoveReminder::NOTIFICATION_JUNCTION && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
5849 : // @note. In case of lane change, myFurtherLanes and partial occupation
5850 : // are handled in enterLaneAtLaneChange()
5851 3419571 : for (MSLane* further : myFurtherLanes) {
5852 : #ifdef DEBUG_FURTHER
5853 : if (DEBUG_COND) {
5854 : std::cout << SIMTIME << " leaveLane \n";
5855 : }
5856 : #endif
5857 32895 : further->resetPartialOccupation(this);
5858 32895 : if (further->getBidiLane() != nullptr
5859 32895 : && (!isRailway(getVClass()) || (further->getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
5860 5 : further->getBidiLane()->resetPartialOccupation(this);
5861 : }
5862 : }
5863 : myFurtherLanes.clear();
5864 : myFurtherLanesPosLat.clear();
5865 : }
5866 3386676 : if (reason >= MSMoveReminder::NOTIFICATION_TELEPORT) {
5867 3386676 : myAmOnNet = false;
5868 3386676 : myWaitingTime = 0;
5869 : }
5870 23983328 : if (reason != MSMoveReminder::NOTIFICATION_PARKING && resumeFromStopping()) {
5871 18 : myStopDist = std::numeric_limits<double>::max();
5872 18 : if (myPastStops.back().speed <= 0) {
5873 54 : WRITE_WARNINGF(TL("Vehicle '%' aborts stop."), getID());
5874 : }
5875 : }
5876 23983328 : if (reason != MSMoveReminder::NOTIFICATION_PARKING && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
5877 22836433 : while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()) {
5878 1455 : if (myStops.front().getSpeed() <= 0) {
5879 3363 : WRITE_WARNINGF(TL("Vehicle '%' skips stop on lane '%' time=%."), getID(), myStops.front().lane->getID(),
5880 : time2string(MSNet::getInstance()->getCurrentTimeStep()));
5881 1121 : cleanupParkingReservation();
5882 1121 : if (MSStopOut::active()) {
5883 : // clean up if stopBlocked was called
5884 17 : MSStopOut::getInstance()->stopNotStarted(this);
5885 : }
5886 1121 : myStops.pop_front();
5887 : } else {
5888 : MSStop& stop = myStops.front();
5889 : // passed waypoint at the end of the lane
5890 334 : if (!stop.reached) {
5891 334 : if (MSStopOut::active()) {
5892 21 : MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), MSNet::getInstance()->getCurrentTimeStep());
5893 : }
5894 334 : stop.reached = true;
5895 : // enter stopping place so leaveFrom works as expected
5896 334 : if (stop.busstop != nullptr) {
5897 : // let the bus stop know the vehicle
5898 25 : stop.busstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5899 : }
5900 334 : if (stop.containerstop != nullptr) {
5901 : // let the container stop know the vehicle
5902 13 : stop.containerstop->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5903 : }
5904 : // do not enter parkingarea!
5905 334 : if (stop.chargingStation != nullptr) {
5906 : // let the container stop know the vehicle
5907 122 : stop.chargingStation->enter(this, stop.pars.parking == ParkingType::OFFROAD);
5908 : }
5909 : }
5910 334 : resumeFromStopping();
5911 : }
5912 1455 : myStopDist = std::numeric_limits<double>::max();
5913 : }
5914 : }
5915 23983328 : }
5916 :
5917 :
5918 : void
5919 43137 : MSVehicle::leaveLaneBack(const MSMoveReminder::Notification reason, const MSLane* leftLane) {
5920 176197 : for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
5921 133060 : if (rem->first->notifyLeaveBack(*this, reason, leftLane)) {
5922 : #ifdef _DEBUG
5923 : if (myTraceMoveReminders) {
5924 : traceMoveReminder("notifyLeaveBack", rem->first, rem->second, true);
5925 : }
5926 : #endif
5927 : ++rem;
5928 : } else {
5929 : #ifdef _DEBUG
5930 : if (myTraceMoveReminders) {
5931 : traceMoveReminder("notifyLeaveBack", rem->first, rem->second, false);
5932 : }
5933 : #endif
5934 : rem = myMoveReminders.erase(rem);
5935 : }
5936 : }
5937 : #ifdef DEBUG_MOVEREMINDERS
5938 : if (DEBUG_COND) {
5939 : std::cout << SIMTIME << " veh=" << getID() << " myReminders:";
5940 : for (auto rem : myMoveReminders) {
5941 : std::cout << rem.first->getDescription() << " ";
5942 : }
5943 : std::cout << "\n";
5944 : }
5945 : #endif
5946 43137 : }
5947 :
5948 :
5949 : MSAbstractLaneChangeModel&
5950 10319873042 : MSVehicle::getLaneChangeModel() {
5951 10319873042 : return *myLaneChangeModel;
5952 : }
5953 :
5954 :
5955 : const MSAbstractLaneChangeModel&
5956 4923768994 : MSVehicle::getLaneChangeModel() const {
5957 4923768994 : return *myLaneChangeModel;
5958 : }
5959 :
5960 : bool
5961 518098 : MSVehicle::isOppositeLane(const MSLane* lane) const {
5962 518098 : return (lane->isInternal()
5963 518098 : ? & (lane->getLinkCont()[0]->getLane()->getEdge()) != *(myCurrEdge + 1)
5964 516362 : : &lane->getEdge() != *myCurrEdge);
5965 : }
5966 :
5967 : const std::vector<MSVehicle::LaneQ>&
5968 1345078871 : MSVehicle::getBestLanes() const {
5969 1345078871 : return *myBestLanes.begin();
5970 : }
5971 :
5972 :
5973 : void
5974 1876096290 : MSVehicle::updateBestLanes(bool forceRebuild, const MSLane* startLane) {
5975 : #ifdef DEBUG_BESTLANES
5976 : if (DEBUG_COND) {
5977 : std::cout << SIMTIME << " updateBestLanes veh=" << getID() << " force=" << forceRebuild << " startLane1=" << Named::getIDSecure(startLane) << " myLane=" << Named::getIDSecure(myLane) << "\n";
5978 : }
5979 : #endif
5980 1876096290 : if (startLane == nullptr) {
5981 993412278 : startLane = myLane;
5982 : }
5983 : assert(startLane != 0);
5984 1876096290 : if (myLaneChangeModel->isOpposite()) {
5985 : // depending on the calling context, startLane might be the forward lane
5986 : // or the reverse-direction lane. In the latter case we need to
5987 : // transform it to the forward lane.
5988 518098 : if (isOppositeLane(startLane)) {
5989 : // use leftmost lane of forward edge
5990 111278 : startLane = startLane->getEdge().getOppositeEdge()->getLanes().back();
5991 : assert(startLane != 0);
5992 : #ifdef DEBUG_BESTLANES
5993 : if (DEBUG_COND) {
5994 : std::cout << " startLaneIsOpposite newStartLane=" << startLane->getID() << "\n";
5995 : }
5996 : #endif
5997 : }
5998 : }
5999 1876096290 : if (forceRebuild) {
6000 1747069 : myLastBestLanesEdge = nullptr;
6001 1747069 : myLastBestLanesInternalLane = nullptr;
6002 : }
6003 1876096290 : if (myBestLanes.size() > 0 && !forceRebuild && myLastBestLanesEdge == &startLane->getEdge()) {
6004 1845265861 : updateOccupancyAndCurrentBestLane(startLane);
6005 : #ifdef DEBUG_BESTLANES
6006 : if (DEBUG_COND) {
6007 : std::cout << " only updateOccupancyAndCurrentBestLane\n";
6008 : }
6009 : #endif
6010 1845265861 : return;
6011 : }
6012 30830429 : if (startLane->getEdge().isInternal()) {
6013 14742585 : if (myBestLanes.size() == 0 || forceRebuild) {
6014 : // rebuilt from previous non-internal lane (may backtrack twice if behind an internal junction)
6015 2252 : updateBestLanes(true, startLane->getLogicalPredecessorLane());
6016 : }
6017 14742585 : if (myLastBestLanesInternalLane == startLane && !forceRebuild) {
6018 : #ifdef DEBUG_BESTLANES
6019 : if (DEBUG_COND) {
6020 : std::cout << " nothing to do on internal\n";
6021 : }
6022 : #endif
6023 : return;
6024 : }
6025 : // adapt best lanes to fit the current internal edge:
6026 : // keep the entries that are reachable from this edge
6027 5256961 : const MSEdge* nextEdge = startLane->getNextNormal();
6028 : assert(!nextEdge->isInternal());
6029 10366069 : for (std::vector<std::vector<LaneQ> >::iterator it = myBestLanes.begin(); it != myBestLanes.end();) {
6030 : std::vector<LaneQ>& lanes = *it;
6031 : assert(lanes.size() > 0);
6032 10366069 : if (&(lanes[0].lane->getEdge()) == nextEdge) {
6033 : // keep those lanes which are successors of internal lanes from the edge of startLane
6034 5256961 : std::vector<LaneQ> oldLanes = lanes;
6035 : lanes.clear();
6036 : const std::vector<MSLane*>& sourceLanes = startLane->getEdge().getLanes();
6037 11900422 : for (std::vector<MSLane*>::const_iterator it_source = sourceLanes.begin(); it_source != sourceLanes.end(); ++it_source) {
6038 11214536 : for (std::vector<LaneQ>::iterator it_lane = oldLanes.begin(); it_lane != oldLanes.end(); ++it_lane) {
6039 11214536 : if ((*it_source)->getLinkCont()[0]->getLane() == (*it_lane).lane) {
6040 6643461 : lanes.push_back(*it_lane);
6041 : break;
6042 : }
6043 : }
6044 : }
6045 : assert(lanes.size() == startLane->getEdge().getLanes().size());
6046 : // patch invalid bestLaneOffset and updated myCurrentLaneInBestLanes
6047 11900422 : for (int i = 0; i < (int)lanes.size(); ++i) {
6048 6643461 : if (i + lanes[i].bestLaneOffset < 0) {
6049 105363 : lanes[i].bestLaneOffset = -i;
6050 : }
6051 6643461 : if (i + lanes[i].bestLaneOffset >= (int)lanes.size()) {
6052 24729 : lanes[i].bestLaneOffset = (int)lanes.size() - i - 1;
6053 : }
6054 : assert(i + lanes[i].bestLaneOffset >= 0);
6055 : assert(i + lanes[i].bestLaneOffset < (int)lanes.size());
6056 6643461 : if (lanes[i].bestContinuations[0] != 0) {
6057 : // patch length of bestContinuation to match expectations (only once)
6058 6453400 : lanes[i].bestContinuations.insert(lanes[i].bestContinuations.begin(), (MSLane*)nullptr);
6059 : }
6060 6643461 : if (startLane->getLinkCont()[0]->getLane() == lanes[i].lane) {
6061 5301032 : myCurrentLaneInBestLanes = lanes.begin() + i;
6062 : }
6063 : assert(&(lanes[i].lane->getEdge()) == nextEdge);
6064 : }
6065 5256961 : myLastBestLanesInternalLane = startLane;
6066 5256961 : updateOccupancyAndCurrentBestLane(startLane);
6067 : #ifdef DEBUG_BESTLANES
6068 : if (DEBUG_COND) {
6069 : std::cout << " updated for internal\n";
6070 : }
6071 : #endif
6072 : return;
6073 5256961 : } else {
6074 : // remove passed edges
6075 5109108 : it = myBestLanes.erase(it);
6076 : }
6077 : }
6078 : assert(false); // should always find the next edge
6079 : }
6080 : // start rebuilding
6081 16087844 : myLastBestLanesInternalLane = nullptr;
6082 16087844 : myLastBestLanesEdge = &startLane->getEdge();
6083 : myBestLanes.clear();
6084 :
6085 : // get information about the next stop
6086 16087844 : MSRouteIterator nextStopEdge = myRoute->end();
6087 : const MSLane* nextStopLane = nullptr;
6088 : double nextStopPos = 0;
6089 16087844 : if (!myStops.empty()) {
6090 : const MSStop& nextStop = myStops.front();
6091 258722 : nextStopLane = nextStop.lane;
6092 258722 : if (nextStop.isOpposite) {
6093 : // target leftmost lane in forward direction
6094 340 : nextStopLane = nextStopLane->getEdge().getOppositeEdge()->getLanes().back();
6095 : }
6096 258722 : nextStopEdge = nextStop.edge;
6097 258722 : nextStopPos = nextStop.pars.startPos;
6098 : }
6099 : // myArrivalTime = -1 in the context of validating departSpeed with departLane=best
6100 16087844 : if (myParameter->arrivalLaneProcedure >= ArrivalLaneDefinition::GIVEN && nextStopEdge == myRoute->end() && myArrivalLane >= 0) {
6101 309469 : nextStopEdge = (myRoute->end() - 1);
6102 309469 : nextStopLane = (*nextStopEdge)->getLanes()[myArrivalLane];
6103 309469 : nextStopPos = myArrivalPos;
6104 : }
6105 16087844 : if (nextStopEdge != myRoute->end()) {
6106 : // make sure that the "wrong" lanes get a penalty. (penalty needs to be
6107 : // large enough to overcome a magic threshold in MSLaneChangeModel::DK2004.cpp:383)
6108 568191 : nextStopPos = MAX2(POSITION_EPS, MIN2((double)nextStopPos, (double)(nextStopLane->getLength() - 2 * POSITION_EPS)));
6109 568191 : if (nextStopLane->isInternal()) {
6110 : // switch to the correct lane before entering the intersection
6111 171 : nextStopPos = (*nextStopEdge)->getLength();
6112 : }
6113 : }
6114 :
6115 : // go forward along the next lanes; always look past stops to ensure that we
6116 : // know where to go once the stop ends
6117 : // trains do not have to deal with lane-changing for stops but their best
6118 : // lanes lookahead is needed for rail signal control
6119 : int seen = 0;
6120 : double seenLength = 0;
6121 : bool progress = true;
6122 : // bestLanes must cover the braking distance even when at the very end of the current lane to avoid unecessary slow down
6123 32175688 : const double maxBrakeDist = startLane->getLength() + getCarFollowModel().getHeadwayTime() * getMaxSpeed() + getCarFollowModel().brakeGap(getMaxSpeed()) + getVehicleType().getMinGap();
6124 16087844 : const double lookahead = getLaneChangeModel().getStrategicLookahead();
6125 81354449 : for (MSRouteIterator ce = myCurrEdge; progress;) {
6126 : std::vector<LaneQ> currentLanes;
6127 : const std::vector<MSLane*>* allowed = nullptr;
6128 : const MSEdge* nextEdge = nullptr;
6129 65266605 : if (ce != myRoute->end() && ce + 1 != myRoute->end()) {
6130 53300575 : nextEdge = *(ce + 1);
6131 53300575 : allowed = (*ce)->allowedLanes(*nextEdge, myType->getVehicleClass());
6132 : }
6133 65266605 : const std::vector<MSLane*>& lanes = (*ce)->getLanes();
6134 165062030 : for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
6135 : LaneQ q;
6136 99795425 : MSLane* cl = *i;
6137 99795425 : q.lane = cl;
6138 99795425 : q.bestContinuations.push_back(cl);
6139 99795425 : q.bestLaneOffset = 0;
6140 99795425 : q.length = cl->allowsVehicleClass(myType->getVehicleClass()) ? (*ce)->getLength() : 0;
6141 99795425 : q.currentLength = q.length;
6142 : // if all lanes are forbidden (i.e. due to a dynamic closing) we want to express no preference
6143 99795425 : q.allowsContinuation = allowed == nullptr || std::find(allowed->begin(), allowed->end(), cl) != allowed->end();
6144 99795425 : q.occupation = 0;
6145 99795425 : q.nextOccupation = 0;
6146 99795425 : currentLanes.push_back(q);
6147 : }
6148 : //
6149 : if (nextStopEdge == ce
6150 : // already past the stop edge
6151 65266605 : && !(ce == myCurrEdge && myLane != nullptr && myLane->isInternal())) {
6152 561147 : const MSLane* normalStopLane = nextStopLane->getNormalPredecessorLane();
6153 1785312 : for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
6154 1224165 : if (nextStopLane != nullptr && normalStopLane != (*q).lane) {
6155 663018 : (*q).allowsContinuation = false;
6156 663018 : (*q).length = nextStopPos;
6157 663018 : (*q).currentLength = (*q).length;
6158 : }
6159 : }
6160 : }
6161 :
6162 65266605 : myBestLanes.push_back(currentLanes);
6163 65266605 : ++seen;
6164 65266605 : seenLength += currentLanes[0].lane->getLength();
6165 : ++ce;
6166 65266605 : if (lookahead >= 0) {
6167 45 : progress &= (seen <= 2 || seenLength < lookahead); // custom (but we need to look at least one edge ahead)
6168 : } else {
6169 86755206 : progress &= (seen <= 4 || seenLength < MAX2(maxBrakeDist, 3000.0)); // motorway
6170 70130749 : progress &= (seen <= 8 || seenLength < MAX2(maxBrakeDist, 200.0) || isRailway(getVClass())); // urban
6171 : }
6172 65266605 : progress &= ce != myRoute->end();
6173 : /*
6174 : if(progress) {
6175 : progress &= (currentLanes.size()!=1||(*ce)->getLanes().size()!=1);
6176 : }
6177 : */
6178 65266605 : }
6179 :
6180 : // we are examining the last lane explicitly
6181 16087844 : if (myBestLanes.size() != 0) {
6182 : double bestLength = -1;
6183 : // minimum and maximum lane index with best length
6184 : int bestThisIndex = 0;
6185 : int bestThisMaxIndex = 0;
6186 : int index = 0;
6187 : std::vector<LaneQ>& last = myBestLanes.back();
6188 41848363 : for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
6189 25760519 : if ((*j).length > bestLength) {
6190 : bestLength = (*j).length;
6191 : bestThisIndex = index;
6192 : bestThisMaxIndex = index;
6193 6131461 : } else if ((*j).length == bestLength) {
6194 : bestThisMaxIndex = index;
6195 : }
6196 : }
6197 : index = 0;
6198 : bool requiredChangeRightForbidden = false;
6199 : int requireChangeToLeftForbidden = -1;
6200 41848363 : for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
6201 25760519 : if ((*j).length < bestLength) {
6202 3930931 : if (abs(bestThisIndex - index) < abs(bestThisMaxIndex - index)) {
6203 145528 : (*j).bestLaneOffset = bestThisIndex - index;
6204 : } else {
6205 3785403 : (*j).bestLaneOffset = bestThisMaxIndex - index;
6206 : }
6207 3930931 : if (!(*j).allowsContinuation) {
6208 535165 : if ((*j).bestLaneOffset < 0 && (!(*j).lane->allowsChangingRight(getVClass())
6209 241054 : || !(*j).lane->getParallelLane(-1, false)->allowsVehicleClass(getVClass())
6210 238399 : || requiredChangeRightForbidden)) {
6211 : // this lane and all further lanes to the left cannot be used
6212 : requiredChangeRightForbidden = true;
6213 2655 : (*j).length = 0;
6214 532510 : } else if ((*j).bestLaneOffset > 0 && (!(*j).lane->allowsChangingLeft(getVClass())
6215 294085 : || !(*j).lane->getParallelLane(1, false)->allowsVehicleClass(getVClass()))) {
6216 : // this lane and all previous lanes to the right cannot be used
6217 6424 : requireChangeToLeftForbidden = (*j).lane->getIndex();
6218 : }
6219 : }
6220 : }
6221 : }
6222 16094278 : for (int i = requireChangeToLeftForbidden; i >= 0; i--) {
6223 6434 : if (last[i].bestLaneOffset > 0) {
6224 6434 : last[i].length = 0;
6225 : }
6226 : }
6227 : #ifdef DEBUG_BESTLANES
6228 : if (DEBUG_COND) {
6229 : std::cout << " last edge=" << last.front().lane->getEdge().getID() << " (bestIndex=" << bestThisIndex << " bestMaxIndex=" << bestThisMaxIndex << "):\n";
6230 : std::vector<LaneQ>& laneQs = myBestLanes.back();
6231 : for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
6232 : std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << "\n";
6233 : }
6234 : }
6235 : #endif
6236 : }
6237 : // go backward through the lanes
6238 : // track back best lane and compute the best prior lane(s)
6239 65266605 : for (std::vector<std::vector<LaneQ> >::reverse_iterator i = myBestLanes.rbegin() + 1; i != myBestLanes.rend(); ++i) {
6240 : std::vector<LaneQ>& nextLanes = (*(i - 1));
6241 : std::vector<LaneQ>& clanes = (*i);
6242 49178761 : MSEdge* const cE = &clanes[0].lane->getEdge();
6243 : int index = 0;
6244 : double bestConnectedLength = -1;
6245 : double bestLength = -1;
6246 122528668 : for (const LaneQ& j : nextLanes) {
6247 146699814 : if (j.lane->isApproachedFrom(cE) && bestConnectedLength < j.length) {
6248 : bestConnectedLength = j.length;
6249 : }
6250 73349907 : if (bestLength < j.length) {
6251 : bestLength = j.length;
6252 : }
6253 : }
6254 : // compute index of the best lane (highest length and least offset from the best next lane)
6255 : int bestThisIndex = 0;
6256 : int bestThisMaxIndex = 0;
6257 49178761 : if (bestConnectedLength > 0) {
6258 : index = 0;
6259 123178132 : for (LaneQ& j : clanes) {
6260 : const LaneQ* bestConnectedNext = nullptr;
6261 74011962 : if (j.allowsContinuation) {
6262 176179583 : for (const LaneQ& m : nextLanes) {
6263 121769763 : if ((m.lane->allowsVehicleClass(getVClass()) || m.lane->hadPermissionChanges())
6264 112171900 : && m.lane->isApproachedFrom(cE, j.lane)) {
6265 66199643 : if (betterContinuation(bestConnectedNext, m)) {
6266 : bestConnectedNext = &m;
6267 : }
6268 : }
6269 : }
6270 64064526 : if (bestConnectedNext != nullptr) {
6271 64064522 : if (bestConnectedNext->length == bestConnectedLength && abs(bestConnectedNext->bestLaneOffset) < 2) {
6272 62344505 : j.length += bestLength;
6273 : } else {
6274 1720017 : j.length += bestConnectedNext->length;
6275 : }
6276 64064522 : j.bestLaneOffset = bestConnectedNext->bestLaneOffset;
6277 : }
6278 : }
6279 64064522 : if (bestConnectedNext != nullptr && (bestConnectedNext->allowsContinuation || bestConnectedNext->length > 0)) {
6280 64026523 : copy(bestConnectedNext->bestContinuations.begin(), bestConnectedNext->bestContinuations.end(), back_inserter(j.bestContinuations));
6281 : } else {
6282 9985439 : j.allowsContinuation = false;
6283 : }
6284 74011962 : if (clanes[bestThisIndex].length < j.length
6285 66912081 : || (clanes[bestThisIndex].length == j.length && abs(clanes[bestThisIndex].bestLaneOffset) > abs(j.bestLaneOffset))
6286 203505994 : || (clanes[bestThisIndex].length == j.length && abs(clanes[bestThisIndex].bestLaneOffset) == abs(j.bestLaneOffset) &&
6287 62719374 : nextLinkPriority(clanes[bestThisIndex].bestContinuations) < nextLinkPriority(j.bestContinuations))
6288 : ) {
6289 : bestThisIndex = index;
6290 : bestThisMaxIndex = index;
6291 66762789 : } else if (clanes[bestThisIndex].length == j.length
6292 62707632 : && abs(clanes[bestThisIndex].bestLaneOffset) == abs(j.bestLaneOffset)
6293 129470294 : && nextLinkPriority(clanes[bestThisIndex].bestContinuations) == nextLinkPriority(j.bestContinuations)) {
6294 : bestThisMaxIndex = index;
6295 : }
6296 74011962 : index++;
6297 : }
6298 :
6299 : //vehicle with elecHybrid device prefers running under an overhead wire
6300 49166170 : if (getDevice(typeid(MSDevice_ElecHybrid)) != nullptr) {
6301 : index = 0;
6302 491 : for (const LaneQ& j : clanes) {
6303 339 : std::string overheadWireSegmentID = MSNet::getInstance()->getStoppingPlaceID(j.lane, j.currentLength / 2., SUMO_TAG_OVERHEAD_WIRE_SEGMENT);
6304 339 : if (overheadWireSegmentID != "") {
6305 : bestThisIndex = index;
6306 : bestThisMaxIndex = index;
6307 : }
6308 339 : index++;
6309 : }
6310 : }
6311 :
6312 : } else {
6313 : // only needed in case of disconnected routes
6314 : int bestNextIndex = 0;
6315 12591 : int bestDistToNeeded = (int) clanes.size();
6316 : index = 0;
6317 35535 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6318 22944 : if ((*j).allowsContinuation) {
6319 : int nextIndex = 0;
6320 56729 : for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
6321 34534 : if ((*m).lane->isApproachedFrom(cE, (*j).lane)) {
6322 5023 : if (bestDistToNeeded > abs((*m).bestLaneOffset)) {
6323 : bestDistToNeeded = abs((*m).bestLaneOffset);
6324 : bestThisIndex = index;
6325 : bestThisMaxIndex = index;
6326 : bestNextIndex = nextIndex;
6327 : }
6328 : }
6329 : }
6330 : }
6331 : }
6332 12591 : clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
6333 12591 : copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
6334 :
6335 : }
6336 : // set bestLaneOffset for all lanes
6337 : index = 0;
6338 : bool requiredChangeRightForbidden = false;
6339 : int requireChangeToLeftForbidden = -1;
6340 123213667 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6341 74034906 : if ((*j).length < clanes[bestThisIndex].length
6342 62384889 : || ((*j).length == clanes[bestThisIndex].length && abs((*j).bestLaneOffset) > abs(clanes[bestThisIndex].bestLaneOffset))
6343 136419568 : || (nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)
6344 : ) {
6345 11826940 : if (abs(bestThisIndex - index) < abs(bestThisMaxIndex - index)) {
6346 771260 : (*j).bestLaneOffset = bestThisIndex - index;
6347 : } else {
6348 11055680 : (*j).bestLaneOffset = bestThisMaxIndex - index;
6349 : }
6350 11826940 : if ((nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)) {
6351 : // try to move away from the lower-priority lane before it ends
6352 10153039 : (*j).length = (*j).currentLength;
6353 : }
6354 11826940 : if (!(*j).allowsContinuation) {
6355 9971571 : if ((*j).bestLaneOffset < 0 && (!(*j).lane->allowsChangingRight(getVClass())
6356 2554754 : || !(*j).lane->getParallelLane(-1, false)->allowsVehicleClass(getVClass())
6357 2540557 : || requiredChangeRightForbidden)) {
6358 : // this lane and all further lanes to the left cannot be used
6359 : requiredChangeRightForbidden = true;
6360 28511 : if ((*j).length == (*j).currentLength) {
6361 28511 : (*j).length = 0;
6362 : }
6363 9943060 : } else if ((*j).bestLaneOffset > 0 && (!(*j).lane->allowsChangingLeft(getVClass())
6364 7359465 : || !(*j).lane->getParallelLane(1, false)->allowsVehicleClass(getVClass()))) {
6365 : // this lane and all previous lanes to the right cannot be used
6366 114136 : requireChangeToLeftForbidden = (*j).lane->getIndex();
6367 : }
6368 : }
6369 : } else {
6370 62207966 : (*j).bestLaneOffset = 0;
6371 : }
6372 : }
6373 49311140 : for (int idx = requireChangeToLeftForbidden; idx >= 0; idx--) {
6374 132379 : if (clanes[idx].length == clanes[idx].currentLength) {
6375 132379 : clanes[idx].length = 0;
6376 : };
6377 : }
6378 :
6379 : //vehicle with elecHybrid device prefers running under an overhead wire
6380 49178761 : if (static_cast<MSDevice_ElecHybrid*>(getDevice(typeid(MSDevice_ElecHybrid))) != 0) {
6381 : index = 0;
6382 152 : std::string overheadWireID = MSNet::getInstance()->getStoppingPlaceID(clanes[bestThisIndex].lane, (clanes[bestThisIndex].currentLength) / 2, SUMO_TAG_OVERHEAD_WIRE_SEGMENT);
6383 152 : if (overheadWireID != "") {
6384 373 : for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
6385 261 : (*j).bestLaneOffset = bestThisIndex - index;
6386 : }
6387 : }
6388 : }
6389 :
6390 : #ifdef DEBUG_BESTLANES
6391 : if (DEBUG_COND) {
6392 : std::cout << " edge=" << cE->getID() << " (bestIndex=" << bestThisIndex << " bestMaxIndex=" << bestThisMaxIndex << "):\n";
6393 : std::vector<LaneQ>& laneQs = clanes;
6394 : for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
6395 : std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << " allowCont=" << (*j).allowsContinuation << "\n";
6396 : }
6397 : }
6398 : #endif
6399 :
6400 : }
6401 16087844 : if (myBestLanes.front().front().lane->isInternal()) {
6402 : // route starts on an internal lane
6403 28 : if (myLane != nullptr) {
6404 : startLane = myLane;
6405 : } else {
6406 : // vehicle not yet departed
6407 12 : startLane = myBestLanes.front().front().lane;
6408 : }
6409 : }
6410 16087844 : updateOccupancyAndCurrentBestLane(startLane);
6411 : #ifdef DEBUG_BESTLANES
6412 : if (DEBUG_COND) {
6413 : std::cout << SIMTIME << " veh=" << getID() << " bestCont=" << toString(getBestLanesContinuation()) << "\n";
6414 : }
6415 : #endif
6416 : }
6417 :
6418 : void
6419 237 : MSVehicle::updateLaneBruttoSum() {
6420 237 : if (myLane != nullptr) {
6421 237 : myLane->markRecalculateBruttoSum();
6422 : }
6423 237 : }
6424 :
6425 : bool
6426 66199643 : MSVehicle::betterContinuation(const LaneQ* bestConnectedNext, const LaneQ& m) const {
6427 66199643 : if (bestConnectedNext == nullptr) {
6428 : return true;
6429 2135121 : } else if (m.lane->getBidiLane() != nullptr && bestConnectedNext->lane->getBidiLane() == nullptr) {
6430 : return false;
6431 2134329 : } else if (bestConnectedNext->lane->getBidiLane() != nullptr && m.lane->getBidiLane() == nullptr) {
6432 : return true;
6433 2134329 : } else if (bestConnectedNext->length < m.length) {
6434 : return true;
6435 1772109 : } else if (bestConnectedNext->length == m.length) {
6436 1247120 : if (abs(bestConnectedNext->bestLaneOffset) > abs(m.bestLaneOffset)) {
6437 : return true;
6438 : }
6439 1080879 : const double contRight = getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_CONTRIGHT, 1);
6440 : if (contRight < 1
6441 : // if we don't check for adjacency, the rightmost line will get
6442 : // multiple chances to be better which leads to an uninituitve distribution
6443 1006 : && (m.lane->getIndex() - bestConnectedNext->lane->getIndex()) == 1
6444 1081658 : && RandHelper::rand(getRNG()) > contRight) {
6445 : return true;
6446 : }
6447 : }
6448 : return false;
6449 : }
6450 :
6451 :
6452 : int
6453 399276962 : MSVehicle::nextLinkPriority(const std::vector<MSLane*>& conts) {
6454 399276962 : if (conts.size() < 2) {
6455 : return -1;
6456 : } else {
6457 363574106 : const MSLink* const link = conts[0]->getLinkTo(conts[1]);
6458 363574106 : if (link != nullptr) {
6459 363552433 : return link->havePriority() ? 1 : 0;
6460 : } else {
6461 : // disconnected route
6462 : return -1;
6463 : }
6464 : }
6465 : }
6466 :
6467 :
6468 : void
6469 1866610666 : MSVehicle::updateOccupancyAndCurrentBestLane(const MSLane* startLane) {
6470 : std::vector<LaneQ>& currLanes = *myBestLanes.begin();
6471 : std::vector<LaneQ>::iterator i;
6472 : #ifdef _DEBUG
6473 : bool found = false;
6474 : #endif
6475 5342396215 : for (i = currLanes.begin(); i != currLanes.end(); ++i) {
6476 : double nextOccupation = 0;
6477 7645139459 : for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
6478 4169353910 : nextOccupation += (*j)->getBruttoVehLenSum();
6479 : }
6480 3475785549 : (*i).nextOccupation = nextOccupation;
6481 : #ifdef DEBUG_BESTLANES
6482 : if (DEBUG_COND) {
6483 : std::cout << " lane=" << (*i).lane->getID() << " nextOccupation=" << nextOccupation << "\n";
6484 : }
6485 : #endif
6486 3475785549 : if ((*i).lane == startLane) {
6487 1861353705 : myCurrentLaneInBestLanes = i;
6488 : #ifdef _DEBUG
6489 : found = true;
6490 : #endif
6491 : }
6492 : }
6493 : #ifdef _DEBUG
6494 : assert(found || startLane->isInternal());
6495 : #endif
6496 1866610666 : }
6497 :
6498 :
6499 : const std::vector<MSLane*>&
6500 2042961199 : MSVehicle::getBestLanesContinuation() const {
6501 2042961199 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6502 : return myEmptyLaneVector;
6503 : }
6504 2042961199 : return (*myCurrentLaneInBestLanes).bestContinuations;
6505 : }
6506 :
6507 :
6508 : const std::vector<MSLane*>&
6509 69363568 : MSVehicle::getBestLanesContinuation(const MSLane* const l) const {
6510 : const MSLane* lane = l;
6511 : // XXX: shouldn't this be a "while" to cover more than one internal lane? (Leo) Refs. #2575
6512 69363568 : if (lane->getEdge().isInternal()) {
6513 : // internal edges are not kept inside the bestLanes structure
6514 5052648 : lane = lane->getLinkCont()[0]->getLane();
6515 : }
6516 69363568 : if (myBestLanes.size() == 0) {
6517 : return myEmptyLaneVector;
6518 : }
6519 114641459 : for (std::vector<LaneQ>::const_iterator i = myBestLanes[0].begin(); i != myBestLanes[0].end(); ++i) {
6520 114629014 : if ((*i).lane == lane) {
6521 69351123 : return (*i).bestContinuations;
6522 : }
6523 : }
6524 : return myEmptyLaneVector;
6525 : }
6526 :
6527 : const std::vector<const MSLane*>
6528 288758 : MSVehicle::getUpcomingLanesUntil(double distance) const {
6529 : std::vector<const MSLane*> lanes;
6530 :
6531 288758 : if (distance <= 0. || hasArrived()) {
6532 : // WRITE_WARNINGF(TL("MSVehicle::getUpcomingLanesUntil(): distance ('%') should be greater than 0."), distance);
6533 : return lanes;
6534 : }
6535 :
6536 288552 : if (!myLaneChangeModel->isOpposite()) {
6537 285226 : distance += getPositionOnLane();
6538 : } else {
6539 3326 : distance += myLane->getOppositePos(getPositionOnLane());
6540 : }
6541 288552 : MSLane* lane = myLaneChangeModel->isOpposite() ? myLane->getParallelOpposite() : myLane;
6542 296580 : while (lane->isInternal() && (distance > 0.)) { // include initial internal lanes
6543 8028 : lanes.insert(lanes.end(), lane);
6544 8028 : distance -= lane->getLength();
6545 13504 : lane = lane->getLinkCont().front()->getViaLaneOrLane();
6546 : }
6547 :
6548 288552 : const std::vector<MSLane*>& contLanes = getBestLanesContinuation();
6549 288552 : if (contLanes.empty()) {
6550 : return lanes;
6551 : }
6552 : auto contLanesIt = contLanes.begin();
6553 288552 : MSRouteIterator routeIt = myCurrEdge; // keep track of covered edges in myRoute
6554 617532 : while (distance > 0.) {
6555 336757 : MSLane* l = nullptr;
6556 336757 : if (contLanesIt != contLanes.end()) {
6557 320904 : l = *contLanesIt;
6558 : if (l != nullptr) {
6559 : assert(l->getEdge().getID() == (*routeIt)->getLanes().front()->getEdge().getID());
6560 : }
6561 : ++contLanesIt;
6562 320904 : if (l != nullptr || myLane->isInternal()) {
6563 : ++routeIt;
6564 : }
6565 320904 : if (l == nullptr) {
6566 5472 : continue;
6567 : }
6568 15853 : } else if (routeIt != myRoute->end()) { // bestLanes didn't get us far enough
6569 : // choose left-most lane as default (avoid sidewalks, bike lanes etc)
6570 8963 : l = (*routeIt)->getLanes().back();
6571 : ++routeIt;
6572 : } else { // the search distance goes beyond our route
6573 : break;
6574 : }
6575 :
6576 : assert(l != nullptr);
6577 :
6578 : // insert internal lanes if applicable
6579 324395 : const MSLane* internalLane = lanes.size() > 0 ? lanes.back()->getInternalFollowingLane(l) : nullptr;
6580 368481 : while ((internalLane != nullptr) && internalLane->isInternal() && (distance > 0.)) {
6581 44086 : lanes.insert(lanes.end(), internalLane);
6582 44086 : distance -= internalLane->getLength();
6583 70464 : internalLane = internalLane->getLinkCont().front()->getViaLaneOrLane();
6584 : }
6585 324395 : if (distance <= 0.) {
6586 : break;
6587 : }
6588 :
6589 323508 : lanes.insert(lanes.end(), l);
6590 323508 : distance -= l->getLength();
6591 : }
6592 :
6593 : return lanes;
6594 0 : }
6595 :
6596 : const std::vector<const MSLane*>
6597 6636 : MSVehicle::getPastLanesUntil(double distance) const {
6598 : std::vector<const MSLane*> lanes;
6599 :
6600 6636 : if (distance <= 0.) {
6601 : // WRITE_WARNINGF(TL("MSVehicle::getPastLanesUntil(): distance ('%') should be greater than 0."), distance);
6602 : return lanes;
6603 : }
6604 :
6605 6528 : MSRouteIterator routeIt = myCurrEdge;
6606 6528 : if (!myLaneChangeModel->isOpposite()) {
6607 6504 : distance += myLane->getLength() - getPositionOnLane();
6608 : } else {
6609 24 : distance += myLane->getParallelOpposite()->getLength() - myLane->getOppositePos(getPositionOnLane());
6610 : }
6611 6528 : MSLane* lane = myLaneChangeModel->isOpposite() ? myLane->getParallelOpposite() : myLane;
6612 6549 : while (lane->isInternal() && (distance > 0.)) { // include initial internal lanes
6613 21 : lanes.insert(lanes.end(), lane);
6614 21 : distance -= lane->getLength();
6615 21 : lane = lane->getLogicalPredecessorLane();
6616 : }
6617 :
6618 9962 : while (distance > 0.) {
6619 : // choose left-most lane as default (avoid sidewalks, bike lanes etc)
6620 8683 : MSLane* l = (*routeIt)->getLanes().back();
6621 :
6622 : // insert internal lanes if applicable
6623 8683 : const MSEdge* internalEdge = lanes.size() > 0 ? (*routeIt)->getInternalFollowingEdge(&(lanes.back()->getEdge()), getVClass()) : nullptr;
6624 8704 : const MSLane* internalLane = internalEdge != nullptr ? internalEdge->getLanes().front() : nullptr;
6625 : std::vector<const MSLane*> internalLanes;
6626 10845 : while ((internalLane != nullptr) && internalLane->isInternal()) { // collect all internal successor lanes
6627 2162 : internalLanes.insert(internalLanes.begin(), internalLane);
6628 4317 : internalLane = internalLane->getLinkCont().front()->getViaLaneOrLane();
6629 : }
6630 10845 : for (auto it = internalLanes.begin(); (it != internalLanes.end()) && (distance > 0.); ++it) { // check remaining distance in correct order
6631 2162 : lanes.insert(lanes.end(), *it);
6632 2162 : distance -= (*it)->getLength();
6633 : }
6634 8683 : if (distance <= 0.) {
6635 : break;
6636 : }
6637 :
6638 8667 : lanes.insert(lanes.end(), l);
6639 8667 : distance -= l->getLength();
6640 :
6641 : // NOTE: we're going backwards with the (bi-directional) Iterator
6642 : // TODO: consider make reverse_iterator() when moving on to C++14 or later
6643 8667 : if (routeIt != myRoute->begin()) {
6644 : --routeIt;
6645 : } else { // we went backwards to begin() and already processed the first and final element
6646 : break;
6647 : }
6648 8683 : }
6649 :
6650 : return lanes;
6651 0 : }
6652 :
6653 :
6654 : const std::vector<MSLane*>
6655 6300 : MSVehicle::getUpstreamOppositeLanes() const {
6656 6300 : const std::vector<const MSLane*> routeLanes = getPastLanesUntil(myLane->getMaximumBrakeDist());
6657 : std::vector<MSLane*> result;
6658 15303 : for (const MSLane* lane : routeLanes) {
6659 9735 : MSLane* opposite = lane->getOpposite();
6660 9735 : if (opposite != nullptr) {
6661 9003 : result.push_back(opposite);
6662 : } else {
6663 : break;
6664 : }
6665 : }
6666 6300 : return result;
6667 6300 : }
6668 :
6669 :
6670 : int
6671 306622592 : MSVehicle::getBestLaneOffset() const {
6672 306622592 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6673 : return 0;
6674 : } else {
6675 306335628 : return (*myCurrentLaneInBestLanes).bestLaneOffset;
6676 : }
6677 : }
6678 :
6679 : double
6680 23437 : MSVehicle::getBestLaneDist() const {
6681 23437 : if (myBestLanes.empty() || myBestLanes[0].empty()) {
6682 : return -1;
6683 : } else {
6684 23437 : return (*myCurrentLaneInBestLanes).length;
6685 : }
6686 : }
6687 :
6688 :
6689 :
6690 : void
6691 649021724 : MSVehicle::adaptBestLanesOccupation(int laneIndex, double density) {
6692 : std::vector<MSVehicle::LaneQ>& preb = myBestLanes.front();
6693 : assert(laneIndex < (int)preb.size());
6694 649021724 : preb[laneIndex].occupation = density + preb[laneIndex].nextOccupation;
6695 649021724 : }
6696 :
6697 :
6698 : void
6699 71362 : MSVehicle::fixPosition() {
6700 71362 : if (MSGlobals::gLaneChangeDuration > 0 && !myLaneChangeModel->isChangingLanes()) {
6701 39636 : myState.myPosLat = 0;
6702 : }
6703 71362 : }
6704 :
6705 : std::pair<const MSLane*, double>
6706 303 : MSVehicle::getLanePosAfterDist(double distance) const {
6707 303 : if (distance == 0) {
6708 255 : return std::make_pair(myLane, getPositionOnLane());
6709 : }
6710 48 : const std::vector<const MSLane*> lanes = getUpcomingLanesUntil(distance);
6711 48 : distance += getPositionOnLane();
6712 48 : for (const MSLane* lane : lanes) {
6713 48 : if (lane->getLength() > distance) {
6714 : return std::make_pair(lane, distance);
6715 : }
6716 0 : distance -= lane->getLength();
6717 : }
6718 0 : return std::make_pair(nullptr, -1);
6719 48 : }
6720 :
6721 :
6722 : double
6723 14983 : MSVehicle::getDistanceToPosition(double destPos, const MSLane* destLane) const {
6724 14983 : if (isOnRoad() && destLane != nullptr) {
6725 14946 : return myRoute->getDistanceBetween(getPositionOnLane(), destPos, myLane, destLane);
6726 : }
6727 : return std::numeric_limits<double>::max();
6728 : }
6729 :
6730 :
6731 : std::pair<const MSVehicle* const, double>
6732 76418041 : MSVehicle::getLeader(double dist, bool considerCrossingFoes) const {
6733 76418041 : if (myLane == nullptr) {
6734 0 : return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
6735 : }
6736 76418041 : if (dist == 0) {
6737 2460 : dist = getCarFollowModel().brakeGap(getSpeed()) + getVehicleType().getMinGap();
6738 : }
6739 : const MSVehicle* lead = nullptr;
6740 76418041 : const MSLane* lane = myLane; // ensure lane does not change between getVehiclesSecure and releaseVehicles;
6741 76418041 : const MSLane::VehCont& vehs = lane->getVehiclesSecure();
6742 : // vehicle might be outside the road network
6743 76418041 : MSLane::VehCont::const_iterator it = std::find(vehs.begin(), vehs.end(), this);
6744 76418041 : if (it != vehs.end() && it + 1 != vehs.end()) {
6745 72696657 : lead = *(it + 1);
6746 : }
6747 72696657 : if (lead != nullptr) {
6748 : std::pair<const MSVehicle* const, double> result(
6749 72696657 : lead, lead->getBackPositionOnLane(myLane) - getPositionOnLane() - getVehicleType().getMinGap());
6750 72696657 : lane->releaseVehicles();
6751 72696657 : return result;
6752 : }
6753 3721384 : const double seen = myLane->getLength() - getPositionOnLane();
6754 3721384 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(myLane);
6755 3721384 : std::pair<const MSVehicle* const, double> result = myLane->getLeaderOnConsecutive(dist, seen, getSpeed(), *this, bestLaneConts, considerCrossingFoes);
6756 3721384 : lane->releaseVehicles();
6757 3721384 : return result;
6758 : }
6759 :
6760 :
6761 : std::pair<const MSVehicle* const, double>
6762 2271398 : MSVehicle::getFollower(double dist) const {
6763 2271398 : if (myLane == nullptr) {
6764 0 : return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
6765 : }
6766 2271398 : if (dist == 0) {
6767 810945 : dist = getCarFollowModel().brakeGap(myLane->getEdge().getSpeedLimit() * 2, 4.5, 0);
6768 : }
6769 2271398 : return myLane->getFollower(this, getPositionOnLane(), dist, MSLane::MinorLinkMode::FOLLOW_NEVER);
6770 : }
6771 :
6772 :
6773 : double
6774 0 : MSVehicle::getTimeGapOnLane() const {
6775 : // calling getLeader with 0 would induce a dist calculation but we only want to look for the leaders on the current lane
6776 0 : std::pair<const MSVehicle* const, double> leaderInfo = getLeader(-1);
6777 0 : if (leaderInfo.first == nullptr || getSpeed() == 0) {
6778 0 : return -1;
6779 : }
6780 0 : return (leaderInfo.second + getVehicleType().getMinGap()) / getSpeed();
6781 : }
6782 :
6783 :
6784 : void
6785 2546114 : MSVehicle::addTransportable(MSTransportable* transportable) {
6786 2546114 : MSBaseVehicle::addTransportable(transportable);
6787 11465 : if (myStops.size() > 0 && myStops.front().reached) {
6788 8260 : if (transportable->isPerson()) {
6789 7681 : if (myStops.front().triggered && myStops.front().numExpectedPerson > 0) {
6790 1829 : myStops.front().numExpectedPerson -= (int)myStops.front().pars.awaitedPersons.count(transportable->getID());
6791 : }
6792 : } else {
6793 579 : if (myStops.front().pars.containerTriggered && myStops.front().numExpectedContainer > 0) {
6794 20 : myStops.front().numExpectedContainer -= (int)myStops.front().pars.awaitedContainers.count(transportable->getID());
6795 : }
6796 : }
6797 : }
6798 11465 : }
6799 :
6800 :
6801 : void
6802 697333152 : MSVehicle::setBlinkerInformation() {
6803 : switchOffSignal(VEH_SIGNAL_BLINKER_RIGHT | VEH_SIGNAL_BLINKER_LEFT);
6804 697333152 : int state = myLaneChangeModel->getOwnState();
6805 : // do not set blinker for sublane changes or when blocked from changing to the right
6806 697333152 : const bool blinkerManoeuvre = (((state & LCA_SUBLANE) == 0) && (
6807 607287976 : (state & LCA_KEEPRIGHT) == 0 || (state & LCA_BLOCKED) == 0));
6808 : Signalling left = VEH_SIGNAL_BLINKER_LEFT;
6809 : Signalling right = VEH_SIGNAL_BLINKER_RIGHT;
6810 697333152 : if (MSGlobals::gLefthand) {
6811 : // lane indices increase from left to right
6812 : std::swap(left, right);
6813 : }
6814 697333152 : if ((state & LCA_LEFT) != 0 && blinkerManoeuvre) {
6815 19956401 : switchOnSignal(left);
6816 677376751 : } else if ((state & LCA_RIGHT) != 0 && blinkerManoeuvre) {
6817 5532640 : switchOnSignal(right);
6818 671844111 : } else if (myLaneChangeModel->isChangingLanes()) {
6819 254899 : if (myLaneChangeModel->getLaneChangeDirection() == 1) {
6820 165385 : switchOnSignal(left);
6821 : } else {
6822 89514 : switchOnSignal(right);
6823 : }
6824 : } else {
6825 671589212 : const MSLane* lane = getLane();
6826 671589212 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, 1, *lane, getBestLanesContinuation());
6827 671589212 : if (link != lane->getLinkCont().end() && lane->getLength() - getPositionOnLane() < lane->getVehicleMaxSpeed(this) * (double) 7.) {
6828 168942564 : switch ((*link)->getDirection()) {
6829 : case LinkDirection::TURN:
6830 : case LinkDirection::LEFT:
6831 : case LinkDirection::PARTLEFT:
6832 : switchOnSignal(VEH_SIGNAL_BLINKER_LEFT);
6833 : break;
6834 : case LinkDirection::RIGHT:
6835 : case LinkDirection::PARTRIGHT:
6836 : switchOnSignal(VEH_SIGNAL_BLINKER_RIGHT);
6837 : break;
6838 : default:
6839 : break;
6840 : }
6841 : }
6842 : }
6843 : // stopping related signals
6844 697333152 : if (hasStops()
6845 697333152 : && (myStops.begin()->reached ||
6846 15335143 : (myStopDist < (myLane->getLength() - getPositionOnLane())
6847 4233919 : && myStopDist < getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this), getCarFollowModel().getMaxDecel(), 3)))) {
6848 17263896 : if (myStops.begin()->lane->getIndex() > 0 && myStops.begin()->lane->getParallelLane(-1)->allowsVehicleClass(getVClass())) {
6849 : // not stopping on the right. Activate emergency blinkers
6850 : switchOnSignal(VEH_SIGNAL_BLINKER_LEFT | VEH_SIGNAL_BLINKER_RIGHT);
6851 17008381 : } else if (!myStops.begin()->reached && (myStops.begin()->pars.parking == ParkingType::OFFROAD)) {
6852 : // signal upcoming parking stop on the current lane when within braking distance (~2 seconds before braking)
6853 1638319 : switchOnSignal(MSGlobals::gLefthand ? VEH_SIGNAL_BLINKER_LEFT : VEH_SIGNAL_BLINKER_RIGHT);
6854 : }
6855 : }
6856 697333152 : if (myInfluencer != nullptr && myInfluencer->getSignals() >= 0) {
6857 14 : mySignals = myInfluencer->getSignals();
6858 : myInfluencer->setSignals(-1); // overwrite computed signals only once
6859 : }
6860 697333152 : }
6861 :
6862 : void
6863 85732 : MSVehicle::setEmergencyBlueLight(SUMOTime currentTime) {
6864 :
6865 : //TODO look if timestep ist SIMSTEP
6866 85732 : if (currentTime % 1000 == 0) {
6867 25939 : if (signalSet(VEH_SIGNAL_EMERGENCY_BLUE)) {
6868 : switchOffSignal(VEH_SIGNAL_EMERGENCY_BLUE);
6869 : } else {
6870 : switchOnSignal(VEH_SIGNAL_EMERGENCY_BLUE);
6871 : }
6872 : }
6873 85732 : }
6874 :
6875 :
6876 : int
6877 22396131 : MSVehicle::getLaneIndex() const {
6878 22396131 : return myLane == nullptr ? -1 : myLane->getIndex();
6879 : }
6880 :
6881 :
6882 : void
6883 14711517 : MSVehicle::setTentativeLaneAndPosition(MSLane* lane, double pos, double posLat) {
6884 14711517 : myLane = lane;
6885 14711517 : myState.myPos = pos;
6886 14711517 : myState.myPosLat = posLat;
6887 14711517 : myState.myBackPos = pos - getVehicleType().getLength();
6888 14711517 : }
6889 :
6890 :
6891 : double
6892 388489129 : MSVehicle::getRightSideOnLane() const {
6893 388489129 : return myState.myPosLat + 0.5 * myLane->getWidth() - 0.5 * getVehicleType().getWidth();
6894 : }
6895 :
6896 :
6897 : double
6898 383358764 : MSVehicle::getLeftSideOnLane() const {
6899 383358764 : return myState.myPosLat + 0.5 * myLane->getWidth() + 0.5 * getVehicleType().getWidth();
6900 : }
6901 :
6902 :
6903 : double
6904 309226355 : MSVehicle::getRightSideOnLane(const MSLane* lane) const {
6905 309226355 : return myState.myPosLat + 0.5 * lane->getWidth() - 0.5 * getVehicleType().getWidth();
6906 : }
6907 :
6908 :
6909 : double
6910 308737143 : MSVehicle::getLeftSideOnLane(const MSLane* lane) const {
6911 308737143 : return myState.myPosLat + 0.5 * lane->getWidth() + 0.5 * getVehicleType().getWidth();
6912 : }
6913 :
6914 :
6915 : double
6916 246978256 : MSVehicle::getRightSideOnEdge(const MSLane* lane) const {
6917 246978256 : return getCenterOnEdge(lane) - 0.5 * getVehicleType().getWidth();
6918 : }
6919 :
6920 :
6921 : double
6922 30919639 : MSVehicle::getLeftSideOnEdge(const MSLane* lane) const {
6923 30919639 : return getCenterOnEdge(lane) + 0.5 * getVehicleType().getWidth();
6924 : }
6925 :
6926 :
6927 : double
6928 722481392 : MSVehicle::getCenterOnEdge(const MSLane* lane) const {
6929 722481392 : if (lane == nullptr || &lane->getEdge() == &myLane->getEdge()) {
6930 722013524 : return myLane->getRightSideOnEdge() + myState.myPosLat + 0.5 * myLane->getWidth();
6931 467868 : } else if (lane == myLaneChangeModel->getShadowLane()) {
6932 13750 : if (myLaneChangeModel->isOpposite() && &lane->getEdge() != &myLane->getEdge()) {
6933 13742 : return lane->getRightSideOnEdge() + lane->getWidth() - myState.myPosLat + 0.5 * myLane->getWidth();
6934 : }
6935 8 : if (myLaneChangeModel->getShadowDirection() == -1) {
6936 0 : return lane->getRightSideOnEdge() + lane->getWidth() + myState.myPosLat + 0.5 * myLane->getWidth();
6937 : } else {
6938 8 : return lane->getRightSideOnEdge() - myLane->getWidth() + myState.myPosLat + 0.5 * myLane->getWidth();
6939 : }
6940 454118 : } else if (lane == myLane->getBidiLane()) {
6941 19605 : return lane->getRightSideOnEdge() - myState.myPosLat + 0.5 * lane->getWidth();
6942 : } else {
6943 : assert(myFurtherLanes.size() == myFurtherLanesPosLat.size());
6944 510245 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6945 483736 : if (myFurtherLanes[i] == lane) {
6946 : #ifdef DEBUG_FURTHER
6947 : if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat=" << myFurtherLanesPosLat[i]
6948 : << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
6949 : << "\n";
6950 : #endif
6951 407928 : return lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
6952 75808 : } else if (myFurtherLanes[i]->getBidiLane() == lane) {
6953 : #ifdef DEBUG_FURTHER
6954 : if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat(bidi)=" << myFurtherLanesPosLat[i]
6955 : << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
6956 : << "\n";
6957 : #endif
6958 76 : return lane->getRightSideOnEdge() - myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
6959 : }
6960 : }
6961 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
6962 26509 : const std::vector<MSLane*>& shadowFurther = myLaneChangeModel->getShadowFurtherLanes();
6963 26762 : for (int i = 0; i < (int)shadowFurther.size(); ++i) {
6964 : //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
6965 26762 : if (shadowFurther[i] == lane) {
6966 : assert(myLaneChangeModel->getShadowLane() != 0);
6967 26509 : return (lane->getRightSideOnEdge() + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] + 0.5 * lane->getWidth()
6968 26509 : + (myLane->getCenterOnEdge() - myLaneChangeModel->getShadowLane()->getCenterOnEdge()));
6969 : }
6970 : }
6971 : assert(false);
6972 0 : throw ProcessError("Request lateral pos of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
6973 : }
6974 : }
6975 :
6976 :
6977 : double
6978 3283531300 : MSVehicle::getLatOffset(const MSLane* lane) const {
6979 : assert(lane != 0);
6980 3283531300 : if (&lane->getEdge() == &myLane->getEdge()) {
6981 3232272044 : return myLane->getRightSideOnEdge() - lane->getRightSideOnEdge();
6982 51259256 : } else if (myLane->getParallelOpposite() == lane) {
6983 2163132 : return (myLane->getWidth() + lane->getWidth()) * 0.5 - 2 * getLateralPositionOnLane();
6984 49096124 : } else if (myLane->getBidiLane() == lane) {
6985 866423 : return -2 * getLateralPositionOnLane();
6986 : } else {
6987 : // Check whether the lane is a further lane for the vehicle
6988 54374975 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
6989 53379521 : if (myFurtherLanes[i] == lane) {
6990 : #ifdef DEBUG_FURTHER
6991 : if (DEBUG_COND) {
6992 : std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherLat=" << myFurtherLanesPosLat[i] << "\n";
6993 : }
6994 : #endif
6995 47202045 : return myFurtherLanesPosLat[i] - myState.myPosLat;
6996 6177476 : } else if (myFurtherLanes[i]->getBidiLane() == lane) {
6997 : #ifdef DEBUG_FURTHER
6998 : if (DEBUG_COND) {
6999 : std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherBidiLat=" << myFurtherLanesPosLat[i] << "\n";
7000 : }
7001 : #endif
7002 32202 : return -2 * (myFurtherLanesPosLat[i] - myState.myPosLat);
7003 : }
7004 : }
7005 : #ifdef DEBUG_FURTHER
7006 : if (DEBUG_COND) {
7007 : std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(myLaneChangeModel->getShadowFurtherLanes()) << "\n";
7008 : }
7009 : #endif
7010 : // Check whether the lane is a "shadow further lane" for the vehicle
7011 995454 : const std::vector<MSLane*>& shadowFurther = myLaneChangeModel->getShadowFurtherLanes();
7012 1006462 : for (int i = 0; i < (int)shadowFurther.size(); ++i) {
7013 1003784 : if (shadowFurther[i] == lane) {
7014 : #ifdef DEBUG_FURTHER
7015 : if (DEBUG_COND) std::cout << " getLatOffset veh=" << getID()
7016 : << " shadowLane=" << Named::getIDSecure(myLaneChangeModel->getShadowLane())
7017 : << " lane=" << lane->getID()
7018 : << " i=" << i
7019 : << " posLat=" << myState.myPosLat
7020 : << " shadowPosLat=" << getLatOffset(myLaneChangeModel->getShadowLane())
7021 : << " shadowFurtherLat=" << myLaneChangeModel->getShadowFurtherLanesPosLat()[i]
7022 : << "\n";
7023 : #endif
7024 992099 : return getLatOffset(myLaneChangeModel->getShadowLane()) + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] - myState.myPosLat;
7025 11685 : } else if (shadowFurther[i]->getBidiLane() == lane) {
7026 : #ifdef DEBUG_FURTHER
7027 : if (DEBUG_COND) {
7028 : std::cout << " getLatOffset veh=" << getID() << " shadowbidilane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherBidiLat=" << myFurtherLanesPosLat[i] << "\n";
7029 : }
7030 : #endif
7031 677 : return -2 * getLatOffset(myLaneChangeModel->getShadowLane()) + myLaneChangeModel->getShadowFurtherLanesPosLat()[i] - myState.myPosLat;
7032 : }
7033 : }
7034 : // Check whether the vehicle issued a maneuverReservation on the lane.
7035 2678 : const std::vector<MSLane*>& furtherTargets = myLaneChangeModel->getFurtherTargetLanes();
7036 2983 : for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
7037 : // Further target lanes are just neighboring lanes of the vehicle's further lanes, @see MSAbstractLaneChangeModel::updateTargetLane()
7038 2981 : MSLane* targetLane = furtherTargets[i];
7039 2981 : if (targetLane == lane) {
7040 2676 : const double targetDir = myLaneChangeModel->getManeuverDist() < 0 ? -1. : 1.;
7041 2676 : const double latOffset = myFurtherLanesPosLat[i] - myState.myPosLat + targetDir * 0.5 * (myFurtherLanes[i]->getWidth() + targetLane->getWidth());
7042 : #ifdef DEBUG_TARGET_LANE
7043 : if (DEBUG_COND) {
7044 : std::cout << " getLatOffset veh=" << getID()
7045 : << " wrt targetLane=" << Named::getIDSecure(myLaneChangeModel->getTargetLane())
7046 : << "\n i=" << i
7047 : << " posLat=" << myState.myPosLat
7048 : << " furtherPosLat=" << myFurtherLanesPosLat[i]
7049 : << " maneuverDist=" << myLaneChangeModel->getManeuverDist()
7050 : << " targetDir=" << targetDir
7051 : << " latOffset=" << latOffset
7052 : << std::endl;
7053 : }
7054 : #endif
7055 2676 : return latOffset;
7056 305 : } else if (targetLane->getBidiLane() == lane) {
7057 0 : const double targetDir = myLaneChangeModel->getManeuverDist() < 0 ? -1. : 1.;
7058 0 : const double latOffset = myFurtherLanesPosLat[i] - myState.myPosLat + targetDir * 0.5 * (myFurtherLanes[i]->getWidth() + targetLane->getWidth());
7059 : #ifdef DEBUG_FURTHER
7060 : if (DEBUG_COND) {
7061 : std::cout << " getLatOffset veh=" << getID() << " furthertargetbidilane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherBidiLat=" << myFurtherLanesPosLat[i] << "\n";
7062 : }
7063 : #endif
7064 0 : return -2 * latOffset;
7065 : }
7066 : }
7067 : assert(false);
7068 12 : throw ProcessError("Request lateral offset of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
7069 : }
7070 : }
7071 :
7072 :
7073 : double
7074 36147590 : MSVehicle::lateralDistanceToLane(const int offset) const {
7075 : // compute the distance when changing to the neighboring lane
7076 : // (ensure we do not lap into the line behind neighLane since there might be unseen blockers)
7077 : assert(offset == 0 || offset == 1 || offset == -1);
7078 : assert(myLane != nullptr);
7079 : assert(myLane->getParallelLane(offset) != nullptr || myLane->getParallelOpposite() != nullptr);
7080 36147590 : const double halfCurrentLaneWidth = 0.5 * myLane->getWidth();
7081 36147590 : const double halfVehWidth = 0.5 * (getWidth() + NUMERICAL_EPS);
7082 36147590 : const double latPos = getLateralPositionOnLane();
7083 36147590 : const double oppositeSign = getLaneChangeModel().isOpposite() ? -1 : 1;
7084 36147590 : double leftLimit = halfCurrentLaneWidth - halfVehWidth - oppositeSign * latPos;
7085 36147590 : double rightLimit = -halfCurrentLaneWidth + halfVehWidth - oppositeSign * latPos;
7086 : double latLaneDist = 0; // minimum distance to move the vehicle fully onto the new lane
7087 36147590 : if (offset == 0) {
7088 8 : if (latPos + halfVehWidth > halfCurrentLaneWidth) {
7089 : // correct overlapping left
7090 4 : latLaneDist = halfCurrentLaneWidth - latPos - halfVehWidth;
7091 4 : } else if (latPos - halfVehWidth < -halfCurrentLaneWidth) {
7092 : // correct overlapping right
7093 4 : latLaneDist = -halfCurrentLaneWidth - latPos + halfVehWidth;
7094 : }
7095 8 : latLaneDist *= oppositeSign;
7096 36147582 : } else if (offset == -1) {
7097 16690889 : latLaneDist = rightLimit - (getWidth() + NUMERICAL_EPS);
7098 19456693 : } else if (offset == 1) {
7099 19456693 : latLaneDist = leftLimit + (getWidth() + NUMERICAL_EPS);
7100 : }
7101 : #ifdef DEBUG_ACTIONSTEPS
7102 : if (DEBUG_COND) {
7103 : std::cout << SIMTIME
7104 : << " veh=" << getID()
7105 : << " halfCurrentLaneWidth=" << halfCurrentLaneWidth
7106 : << " halfVehWidth=" << halfVehWidth
7107 : << " latPos=" << latPos
7108 : << " latLaneDist=" << latLaneDist
7109 : << " leftLimit=" << leftLimit
7110 : << " rightLimit=" << rightLimit
7111 : << "\n";
7112 : }
7113 : #endif
7114 36147590 : return latLaneDist;
7115 : }
7116 :
7117 :
7118 : double
7119 5013373576 : MSVehicle::getLateralOverlap(double posLat, const MSLane* lane) const {
7120 5013373576 : return (fabs(posLat) + 0.5 * getVehicleType().getWidth()
7121 5013373576 : - 0.5 * lane->getWidth());
7122 : }
7123 :
7124 : double
7125 0 : MSVehicle::getLateralOverlap(const MSLane* lane) const {
7126 0 : return getLateralOverlap(getLateralPositionOnLane(), lane);
7127 : }
7128 :
7129 : double
7130 4821134833 : MSVehicle::getLateralOverlap() const {
7131 4821134833 : return getLateralOverlap(getLateralPositionOnLane(), myLane);
7132 : }
7133 :
7134 :
7135 : void
7136 639461520 : MSVehicle::removeApproachingInformation(const DriveItemVector& lfLinks) const {
7137 1854698877 : for (const DriveProcessItem& dpi : lfLinks) {
7138 1215237357 : if (dpi.myLink != nullptr) {
7139 859309116 : dpi.myLink->removeApproaching(this);
7140 : }
7141 : }
7142 : // unregister on all shadow links
7143 639461520 : myLaneChangeModel->removeShadowApproachingInformation();
7144 639461520 : }
7145 :
7146 :
7147 : bool
7148 845465 : MSVehicle::unsafeLinkAhead(const MSLane* lane, double zipperDist) const {
7149 : // the following links are unsafe:
7150 : // - zipper links if they are close enough and have approaching vehicles in the relevant time range
7151 : // - unprioritized links if the vehicle is currently approaching a prioritzed link and unable to stop in time
7152 845465 : double seen = myLane->getLength() - getPositionOnLane();
7153 845465 : const double dist = MAX2(zipperDist, getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0));
7154 845465 : if (seen < dist) {
7155 71118 : const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(lane);
7156 : int view = 1;
7157 71118 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
7158 : DriveItemVector::const_iterator di = myLFLinkLanes.begin();
7159 118175 : while (!lane->isLinkEnd(link) && seen <= dist) {
7160 71730 : if ((!lane->isInternal()
7161 49175 : && (((*link)->getState() == LINKSTATE_ZIPPER && seen < (*link)->getFoeVisibilityDistance())
7162 26560 : || !(*link)->havePriority()))
7163 98028 : || (lane->isInternal() && zipperDist > 0)) {
7164 : // find the drive item corresponding to this link
7165 : bool found = false;
7166 52372 : while (di != myLFLinkLanes.end() && !found) {
7167 27128 : if ((*di).myLink != nullptr) {
7168 : const MSLane* diPredLane = (*di).myLink->getLaneBefore();
7169 27124 : if (diPredLane != nullptr) {
7170 27124 : if (&diPredLane->getEdge() == &lane->getEdge()) {
7171 : found = true;
7172 : }
7173 : }
7174 : }
7175 27128 : if (!found) {
7176 : di++;
7177 : }
7178 : }
7179 25244 : if (found) {
7180 25240 : const SUMOTime leaveTime = (*link)->getLeaveTime((*di).myArrivalTime, (*di).myArrivalSpeed,
7181 25240 : (*di).getLeaveSpeed(), getVehicleType().getLength());
7182 25240 : const MSLink* entry = (*link)->getCorrespondingEntryLink();
7183 : //if (DEBUG_COND) {
7184 : // std::cout << SIMTIME << " veh=" << getID() << " changeTo=" << Named::getIDSecure(bestLaneConts.front()) << " linkState=" << toString((*link)->getState()) << " seen=" << seen << " dist=" << dist << " zipperDist=" << zipperDist << " aT=" << STEPS2TIME((*di).myArrivalTime) << " lT=" << STEPS2TIME(leaveTime) << "\n";
7185 : //}
7186 25240 : if (entry->hasApproachingFoe((*di).myArrivalTime, leaveTime, (*di).myArrivalSpeed, getCarFollowModel().getMaxDecel())) {
7187 : //std::cout << SIMTIME << " veh=" << getID() << " aborting changeTo=" << Named::getIDSecure(bestLaneConts.front()) << " linkState=" << toString((*link)->getState()) << " seen=" << seen << " dist=" << dist << "\n";
7188 : return true;
7189 : }
7190 : }
7191 : // no drive item is found if the vehicle aborts its request within dist
7192 : }
7193 47057 : lane = (*link)->getViaLaneOrLane();
7194 47057 : if (!lane->getEdge().isInternal()) {
7195 24557 : view++;
7196 : }
7197 47057 : seen += lane->getLength();
7198 47057 : link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
7199 : }
7200 : }
7201 : return false;
7202 : }
7203 :
7204 :
7205 : PositionVector
7206 6616679 : MSVehicle::getBoundingBox(double offset) const {
7207 6616679 : PositionVector centerLine;
7208 6616679 : Position pos = getPosition();
7209 6616679 : centerLine.push_back(pos);
7210 6616679 : switch (myType->getGuiShape()) {
7211 26790 : case SUMOVehicleShape::BUS_FLEXIBLE:
7212 : case SUMOVehicleShape::RAIL:
7213 : case SUMOVehicleShape::RAIL_CAR:
7214 : case SUMOVehicleShape::RAIL_CARGO:
7215 : case SUMOVehicleShape::TRUCK_SEMITRAILER:
7216 : case SUMOVehicleShape::TRUCK_1TRAILER: {
7217 56251 : for (MSLane* lane : myFurtherLanes) {
7218 29461 : centerLine.push_back(lane->getShape().back());
7219 : }
7220 : break;
7221 : }
7222 : default:
7223 : break;
7224 : }
7225 6616679 : double l = getLength();
7226 6616679 : Position backPos = getBackPosition();
7227 6616679 : if (pos.distanceTo2D(backPos) > l + NUMERICAL_EPS) {
7228 : // getBackPosition may not match the visual back in networks without internal lanes
7229 360131 : double a = getAngle() + M_PI; // angle pointing backwards
7230 360131 : backPos = pos + Position(l * cos(a), l * sin(a));
7231 : }
7232 6616679 : centerLine.push_back(backPos);
7233 6616679 : if (offset != 0) {
7234 6543 : centerLine.extrapolate2D(offset);
7235 : }
7236 : PositionVector result = centerLine;
7237 13229068 : result.move2side(MAX2(0.0, 0.5 * myType->getWidth() + offset));
7238 13229068 : centerLine.move2side(MIN2(0.0, -0.5 * myType->getWidth() - offset));
7239 6616679 : result.append(centerLine.reverse(), POSITION_EPS);
7240 6616679 : return result;
7241 6616679 : }
7242 :
7243 :
7244 : PositionVector
7245 72789 : MSVehicle::getBoundingPoly(double offset) const {
7246 72789 : switch (myType->getGuiShape()) {
7247 72328 : case SUMOVehicleShape::PASSENGER:
7248 : case SUMOVehicleShape::PASSENGER_SEDAN:
7249 : case SUMOVehicleShape::PASSENGER_HATCHBACK:
7250 : case SUMOVehicleShape::PASSENGER_WAGON:
7251 : case SUMOVehicleShape::PASSENGER_VAN: {
7252 : // box with corners cut off
7253 72328 : PositionVector result;
7254 72328 : PositionVector centerLine;
7255 72328 : centerLine.push_back(getPosition());
7256 72328 : centerLine.push_back(getBackPosition());
7257 72328 : if (offset != 0) {
7258 1600 : centerLine.extrapolate2D(offset);
7259 : }
7260 : PositionVector line1 = centerLine;
7261 : PositionVector line2 = centerLine;
7262 144656 : line1.move2side(MAX2(0.0, 0.3 * myType->getWidth() + offset));
7263 144656 : line2.move2side(MAX2(0.0, 0.5 * myType->getWidth() + offset));
7264 72328 : line2.scaleRelative(0.8);
7265 72328 : result.push_back(line1[0]);
7266 72328 : result.push_back(line2[0]);
7267 72328 : result.push_back(line2[1]);
7268 72328 : result.push_back(line1[1]);
7269 144656 : line1.move2side(MIN2(0.0, -0.6 * myType->getWidth() - offset));
7270 144656 : line2.move2side(MIN2(0.0, -1.0 * myType->getWidth() - offset));
7271 72328 : result.push_back(line1[1]);
7272 72328 : result.push_back(line2[1]);
7273 72328 : result.push_back(line2[0]);
7274 72328 : result.push_back(line1[0]);
7275 : return result;
7276 72328 : }
7277 461 : default:
7278 461 : return getBoundingBox();
7279 : }
7280 : }
7281 :
7282 :
7283 : bool
7284 12692471 : MSVehicle::onFurtherEdge(const MSEdge* edge) const {
7285 13789894 : for (std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
7286 1690755 : if (&(*i)->getEdge() == edge) {
7287 : return true;
7288 : }
7289 : }
7290 : return false;
7291 : }
7292 :
7293 :
7294 : bool
7295 7507294866 : MSVehicle::isBidiOn(const MSLane* lane) const {
7296 7523708027 : return lane->getBidiLane() != nullptr && (
7297 16413161 : myLane == lane->getBidiLane()
7298 12692471 : || onFurtherEdge(&lane->getBidiLane()->getEdge()));
7299 : }
7300 :
7301 :
7302 : bool
7303 16 : MSVehicle::rerouteParkingArea(const std::string& parkingAreaID, std::string& errorMsg) {
7304 : // this function is based on MSTriggeredRerouter::rerouteParkingArea in order to keep
7305 : // consistency in the behaviour.
7306 :
7307 : // get vehicle params
7308 16 : MSParkingArea* destParkArea = getNextParkingArea();
7309 16 : const MSRoute& route = getRoute();
7310 16 : const MSEdge* lastEdge = route.getLastEdge();
7311 :
7312 16 : if (destParkArea == nullptr) {
7313 : // not driving towards a parking area
7314 0 : errorMsg = "Vehicle " + getID() + " is not driving to a parking area so it cannot be rerouted.";
7315 0 : return false;
7316 : }
7317 :
7318 : // if the current route ends at the parking area, the new route will also and at the new area
7319 16 : bool newDestination = (&destParkArea->getLane().getEdge() == route.getLastEdge()
7320 8 : && getArrivalPos() >= destParkArea->getBeginLanePosition()
7321 24 : && getArrivalPos() <= destParkArea->getEndLanePosition());
7322 :
7323 : // retrieve info on the new parking area
7324 16 : MSParkingArea* newParkingArea = (MSParkingArea*) MSNet::getInstance()->getStoppingPlace(
7325 : parkingAreaID, SumoXMLTag::SUMO_TAG_PARKING_AREA);
7326 :
7327 16 : if (newParkingArea == nullptr) {
7328 0 : errorMsg = "Parking area ID " + toString(parkingAreaID) + " not found in the network.";
7329 0 : return false;
7330 : }
7331 :
7332 16 : const MSEdge* newEdge = &(newParkingArea->getLane().getEdge());
7333 16 : SUMOAbstractRouter<MSEdge, SUMOVehicle>& router = getRouterTT();
7334 :
7335 : // Compute the route from the current edge to the parking area edge
7336 : ConstMSEdgeVector edgesToPark;
7337 16 : router.compute(getEdge(), getPositionOnLane(), newEdge, newParkingArea->getEndLanePosition(), this, MSNet::getInstance()->getCurrentTimeStep(), edgesToPark);
7338 :
7339 : // Compute the route from the parking area edge to the end of the route
7340 : ConstMSEdgeVector edgesFromPark;
7341 16 : if (!newDestination) {
7342 12 : router.compute(newEdge, lastEdge, this, MSNet::getInstance()->getCurrentTimeStep(), edgesFromPark);
7343 : } else {
7344 : // adapt plans of any riders
7345 8 : for (MSTransportable* p : getPersons()) {
7346 4 : p->rerouteParkingArea(getNextParkingArea(), newParkingArea);
7347 : }
7348 : }
7349 :
7350 : // we have a new destination, let's replace the vehicle route
7351 16 : ConstMSEdgeVector edges = edgesToPark;
7352 16 : if (edgesFromPark.size() > 0) {
7353 12 : edges.insert(edges.end(), edgesFromPark.begin() + 1, edgesFromPark.end());
7354 : }
7355 :
7356 16 : if (newDestination && getParameter().arrivalPosProcedure != ArrivalPosDefinition::DEFAULT) {
7357 4 : SUMOVehicleParameter* newParameter = new SUMOVehicleParameter();
7358 4 : *newParameter = getParameter();
7359 4 : newParameter->arrivalPosProcedure = ArrivalPosDefinition::GIVEN;
7360 4 : newParameter->arrivalPos = newParkingArea->getEndLanePosition();
7361 4 : replaceParameter(newParameter);
7362 : }
7363 16 : const double routeCost = router.recomputeCosts(edges, this, MSNet::getInstance()->getCurrentTimeStep());
7364 16 : ConstMSEdgeVector prevEdges(myCurrEdge, myRoute->end());
7365 16 : const double savings = router.recomputeCosts(prevEdges, this, MSNet::getInstance()->getCurrentTimeStep());
7366 16 : if (replaceParkingArea(newParkingArea, errorMsg)) {
7367 16 : const bool onInit = myLane == nullptr;
7368 32 : replaceRouteEdges(edges, routeCost, savings, "TraCI:" + toString(SUMO_TAG_PARKING_AREA_REROUTE), onInit, false, false);
7369 : } else {
7370 0 : WRITE_WARNINGF("Vehicle '%' could not reroute to new parkingArea '%' reason=%, time=%.",
7371 : getID(), newParkingArea->getID(), errorMsg, time2string(SIMSTEP));
7372 0 : return false;
7373 : }
7374 16 : return true;
7375 16 : }
7376 :
7377 :
7378 : bool
7379 43968 : MSVehicle::addTraciStop(SUMOVehicleParameter::Stop stop, std::string& errorMsg) {
7380 43968 : const int numStops = (int)myStops.size();
7381 43968 : const bool result = MSBaseVehicle::addTraciStop(stop, errorMsg);
7382 43968 : if (myLane != nullptr && numStops != (int)myStops.size()) {
7383 42307 : updateBestLanes(true);
7384 : }
7385 43968 : return result;
7386 : }
7387 :
7388 :
7389 : bool
7390 3130 : MSVehicle::handleCollisionStop(MSStop& stop, const double distToStop) {
7391 3130 : if (myCurrEdge == stop.edge && distToStop + POSITION_EPS < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0)) {
7392 1431 : if (distToStop < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)) {
7393 1001 : double vNew = getCarFollowModel().maximumSafeStopSpeed(distToStop, getCarFollowModel().getMaxDecel(), getSpeed(), false, 0);
7394 : //std::cout << SIMTIME << " veh=" << getID() << " v=" << myState.mySpeed << " distToStop=" << distToStop
7395 : // << " vMinNex=" << getCarFollowModel().minNextSpeed(getSpeed(), this)
7396 : // << " bg1=" << getCarFollowModel().brakeGap(myState.mySpeed)
7397 : // << " bg2=" << getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)
7398 : // << " vNew=" << vNew
7399 : // << "\n";
7400 1001 : myState.mySpeed = MIN2(myState.mySpeed, vNew + ACCEL2SPEED(getCarFollowModel().getEmergencyDecel()));
7401 1001 : myState.myPos = MIN2(myState.myPos, stop.pars.endPos);
7402 1001 : myCachedPosition = Position::INVALID;
7403 1001 : if (myState.myPos < myType->getLength()) {
7404 418 : computeFurtherLanes(myLane, myState.myPos, true);
7405 418 : myAngle = computeAngle();
7406 418 : if (myLaneChangeModel->isOpposite()) {
7407 0 : myAngle += M_PI;
7408 : }
7409 : }
7410 : }
7411 : }
7412 3130 : return true;
7413 : }
7414 :
7415 :
7416 : bool
7417 24015597 : MSVehicle::resumeFromStopping() {
7418 24015597 : if (isStopped()) {
7419 48829 : if (myAmRegisteredAsWaiting) {
7420 750 : MSNet::getInstance()->getVehicleControl().unregisterOneWaiting();
7421 750 : myAmRegisteredAsWaiting = false;
7422 : }
7423 : MSStop& stop = myStops.front();
7424 : // we have waited long enough and fulfilled any passenger-requirements
7425 48829 : if (stop.busstop != nullptr) {
7426 : // inform bus stop about leaving it
7427 18301 : stop.busstop->leaveFrom(this);
7428 : }
7429 : // we have waited long enough and fulfilled any container-requirements
7430 48829 : if (stop.containerstop != nullptr) {
7431 : // inform container stop about leaving it
7432 530 : stop.containerstop->leaveFrom(this);
7433 : }
7434 48829 : if (stop.parkingarea != nullptr && stop.getSpeed() <= 0) {
7435 : // inform parking area about leaving it
7436 8435 : stop.parkingarea->leaveFrom(this);
7437 : }
7438 48829 : if (stop.chargingStation != nullptr) {
7439 : // inform charging station about leaving it
7440 3519 : stop.chargingStation->leaveFrom(this);
7441 : }
7442 : // the current stop is no longer valid
7443 48829 : myLane->getEdge().removeWaiting(this);
7444 : // MSStopOut needs to know whether the stop had a loaded 'ended' value so we call this before replacing the value
7445 48829 : if (stop.pars.started == -1) {
7446 : // waypoint edge was passed in a single step
7447 334 : stop.pars.started = MSNet::getInstance()->getCurrentTimeStep();
7448 : }
7449 48829 : if (MSStopOut::active()) {
7450 3969 : MSStopOut::getInstance()->stopEnded(this, stop);
7451 : }
7452 48829 : stop.pars.ended = MSNet::getInstance()->getCurrentTimeStep();
7453 110921 : for (const auto& rem : myMoveReminders) {
7454 62092 : rem.first->notifyStopEnded();
7455 : }
7456 48829 : if (stop.pars.collision && MSLane::getCollisionAction() == MSLane::COLLISION_ACTION_WARN) {
7457 425 : myCollisionImmunity = TIME2STEPS(5); // leave the conflict area
7458 : }
7459 48829 : if (stop.pars.posLat != INVALID_DOUBLE && MSGlobals::gLateralResolution <= 0) {
7460 : // reset lateral position to default
7461 195 : myState.myPosLat = 0;
7462 : }
7463 48829 : const bool wasWaypoint = stop.getSpeed() > 0;
7464 48829 : myPastStops.push_back(stop.pars);
7465 48829 : myPastStops.back().routeIndex = (int)(stop.edge - myRoute->begin());
7466 48829 : myStops.pop_front();
7467 48829 : myStopDist = std::numeric_limits<double>::max();
7468 : // do not count the stopping time towards gridlock time.
7469 : // Other outputs use an independent counter and are not affected.
7470 48829 : myWaitingTime = 0;
7471 48829 : myStopSpeed = getCarFollowModel().maxNextSpeed(getSpeed(), this);
7472 : // maybe the next stop is on the same edge; let's rebuild best lanes
7473 48829 : updateBestLanes(true);
7474 : // continue as wished...
7475 48829 : MSNet::getInstance()->informVehicleStateListener(this, MSNet::VehicleState::ENDING_STOP);
7476 48829 : MSNet::getInstance()->getVehicleControl().registerStopEnded();
7477 48829 : return !wasWaypoint;
7478 : }
7479 : return false;
7480 : }
7481 :
7482 :
7483 : MSVehicle::Influencer&
7484 4544543 : MSVehicle::getInfluencer() {
7485 4544543 : if (myInfluencer == nullptr) {
7486 3448 : myInfluencer = new Influencer();
7487 : }
7488 4544543 : return *myInfluencer;
7489 : }
7490 :
7491 : MSVehicle::BaseInfluencer&
7492 24 : MSVehicle::getBaseInfluencer() {
7493 24 : return getInfluencer();
7494 : }
7495 :
7496 :
7497 : const MSVehicle::Influencer*
7498 0 : MSVehicle::getInfluencer() const {
7499 0 : return myInfluencer;
7500 : }
7501 :
7502 : const MSVehicle::BaseInfluencer*
7503 237080 : MSVehicle::getBaseInfluencer() const {
7504 237080 : return myInfluencer;
7505 : }
7506 :
7507 :
7508 : double
7509 4078 : MSVehicle::getSpeedWithoutTraciInfluence() const {
7510 4078 : if (myInfluencer != nullptr && myInfluencer->getOriginalSpeed() >= 0) {
7511 : // influencer original speed is -1 on initialization
7512 1655 : return myInfluencer->getOriginalSpeed();
7513 : }
7514 2423 : return myState.mySpeed;
7515 : }
7516 :
7517 :
7518 : int
7519 997732802 : MSVehicle::influenceChangeDecision(int state) {
7520 997732802 : if (hasInfluencer()) {
7521 2814057 : state = getInfluencer().influenceChangeDecision(
7522 : MSNet::getInstance()->getCurrentTimeStep(),
7523 2814057 : myLane->getEdge(),
7524 : getLaneIndex(),
7525 : state);
7526 : }
7527 997732802 : return state;
7528 : }
7529 :
7530 :
7531 : void
7532 6774 : MSVehicle::setRemoteState(Position xyPos) {
7533 6774 : myCachedPosition = xyPos;
7534 6774 : }
7535 :
7536 :
7537 : bool
7538 792376286 : MSVehicle::isRemoteControlled() const {
7539 792376286 : return myInfluencer != nullptr && myInfluencer->isRemoteControlled();
7540 : }
7541 :
7542 :
7543 : bool
7544 20570 : MSVehicle::wasRemoteControlled(SUMOTime lookBack) const {
7545 20570 : return myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() + lookBack >= MSNet::getInstance()->getCurrentTimeStep();
7546 : }
7547 :
7548 :
7549 : bool
7550 519858247 : MSVehicle::keepClear(const MSLink* link) const {
7551 519858247 : if (link->hasFoes() && link->keepClear() /* && item.myLink->willHaveBlockedFoe()*/) {
7552 171730747 : const double keepClearTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_KEEPCLEAR_TIME, -1);
7553 : //std::cout << SIMTIME << " veh=" << getID() << " keepClearTime=" << keepClearTime << " accWait=" << getAccumulatedWaitingSeconds() << " keepClear=" << (keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime) << "\n";
7554 173152931 : return keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime;
7555 : } else {
7556 : return false;
7557 : }
7558 : }
7559 :
7560 :
7561 : bool
7562 722205130 : MSVehicle::ignoreRed(const MSLink* link, bool canBrake) const {
7563 722205130 : if ((myInfluencer != nullptr && !myInfluencer->getEmergencyBrakeRedLight())) {
7564 : return true;
7565 : }
7566 721897819 : const double ignoreRedTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
7567 : #ifdef DEBUG_IGNORE_RED
7568 : if (DEBUG_COND) {
7569 : std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID() << " state=" << toString(link->getState()) << "\n";
7570 : }
7571 : #endif
7572 721897819 : if (ignoreRedTime < 0) {
7573 721892420 : const double ignoreYellowTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, 0);
7574 721892420 : if (ignoreYellowTime > 0 && link->haveYellow()) {
7575 : assert(link->getTLLogic() != 0);
7576 52 : const double yellowDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
7577 : // when activating ignoreYellow behavior, vehicles will drive if they cannot brake
7578 92 : return !canBrake || ignoreYellowTime > yellowDuration;
7579 : } else {
7580 : return false;
7581 : }
7582 5399 : } else if (link->haveYellow()) {
7583 : // always drive at yellow when ignoring red
7584 : return true;
7585 5243 : } else if (link->haveRed()) {
7586 : assert(link->getTLLogic() != 0);
7587 3832 : const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
7588 : #ifdef DEBUG_IGNORE_RED
7589 : if (DEBUG_COND) {
7590 : std::cout
7591 : // << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID()
7592 : << " ignoreRedTime=" << ignoreRedTime
7593 : << " spentRed=" << redDuration
7594 : << " canBrake=" << canBrake << "\n";
7595 : }
7596 : #endif
7597 : // when activating ignoreRed behavior, vehicles will always drive if they cannot brake
7598 6356 : return !canBrake || ignoreRedTime > redDuration;
7599 : } else {
7600 : return false;
7601 : }
7602 : }
7603 :
7604 : bool
7605 1311951558 : MSVehicle::ignoreFoe(const SUMOTrafficObject* foe) const {
7606 1311951558 : if (!getParameter().wasSet(VEHPARS_CFMODEL_PARAMS_SET)) {
7607 : return false;
7608 : }
7609 2548 : for (const std::string& typeID : StringTokenizer(getParameter().getParameter(toString(SUMO_ATTR_CF_IGNORE_TYPES), "")).getVector()) {
7610 398 : if (typeID == foe->getVehicleType().getID()) {
7611 : return true;
7612 : }
7613 1274 : }
7614 2161 : for (const std::string& id : StringTokenizer(getParameter().getParameter(toString(SUMO_ATTR_CF_IGNORE_IDS), "")).getVector()) {
7615 876 : if (id == foe->getID()) {
7616 : return true;
7617 : }
7618 876 : }
7619 409 : return false;
7620 : }
7621 :
7622 : bool
7623 550212466 : MSVehicle::passingMinor() const {
7624 : // either on an internal lane that was entered via minor link
7625 : // or on approach to minor link below visibility distance
7626 550212466 : if (myLane == nullptr) {
7627 : return false;
7628 : }
7629 550212466 : if (myLane->getEdge().isInternal()) {
7630 10283303 : return !myLane->getIncomingLanes().front().viaLink->havePriority();
7631 539929163 : } else if (myLFLinkLanes.size() > 0 && myLFLinkLanes.front().myLink != nullptr) {
7632 : MSLink* link = myLFLinkLanes.front().myLink;
7633 280403543 : return !link->havePriority() && myLFLinkLanes.front().myDistance <= link->getFoeVisibilityDistance();
7634 : }
7635 : return false;
7636 : }
7637 :
7638 : bool
7639 21148197 : MSVehicle::isLeader(const MSLink* link, const MSVehicle* veh, const double gap) const {
7640 : assert(link->fromInternalLane());
7641 21148197 : if (veh == nullptr) {
7642 : return false;
7643 : }
7644 21148197 : if (!myLane->isInternal() || myLane->getEdge().getToJunction() != link->getJunction()) {
7645 : // if this vehicle is not yet on the junction, every vehicle is a leader
7646 : return true;
7647 : }
7648 1880836 : if (veh->getLaneChangeModel().hasBlueLight()) {
7649 : // blue light device automatically gets right of way
7650 : return true;
7651 : }
7652 1880568 : const MSLane* foeLane = veh->getLane();
7653 1880568 : if (foeLane->isInternal()) {
7654 1385278 : if (foeLane->getEdge().getFromJunction() == link->getJunction()) {
7655 1368970 : SUMOTime egoET = myJunctionConflictEntryTime;
7656 1368970 : SUMOTime foeET = veh->myJunctionEntryTime;
7657 : // check relationship between link and foeLane
7658 1368970 : if (foeLane->getNormalPredecessorLane() == link->getInternalLaneBefore()->getNormalPredecessorLane()) {
7659 : // we are entering the junction from the same lane
7660 435717 : egoET = myJunctionEntryTimeNeverYield;
7661 435717 : foeET = veh->myJunctionEntryTimeNeverYield;
7662 435717 : if (link->isExitLinkAfterInternalJunction() && link->getInternalLaneBefore()->getLogicalPredecessorLane()->getEntryLink()->isIndirect()) {
7663 79524 : egoET = myJunctionConflictEntryTime;
7664 : }
7665 : } else {
7666 933253 : const MSLink* foeLink = foeLane->getIncomingLanes()[0].viaLink;
7667 933253 : const MSJunctionLogic* logic = link->getJunction()->getLogic();
7668 : assert(logic != nullptr);
7669 : // determine who has right of way
7670 : bool response; // ego response to foe
7671 : bool response2; // foe response to ego
7672 : // attempt 1: tlLinkState
7673 933253 : const MSLink* entry = link->getCorrespondingEntryLink();
7674 933253 : const MSLink* foeEntry = foeLink->getCorrespondingEntryLink();
7675 933253 : if (entry->haveRed() || foeEntry->haveRed()) {
7676 : // ensure that vehicles which are stuck on the intersection may exit
7677 116383 : if (!foeEntry->haveRed() && veh->getSpeed() > SUMO_const_haltingSpeed && gap < 0) {
7678 : // foe might be oncoming, don't drive unless foe can still brake safely
7679 11597 : const double foeNextSpeed = veh->getSpeed() + ACCEL2SPEED(veh->getCarFollowModel().getMaxAccel());
7680 11597 : const double foeBrakeGap = veh->getCarFollowModel().brakeGap(
7681 11597 : foeNextSpeed, veh->getCarFollowModel().getMaxDecel(), veh->getCarFollowModel().getHeadwayTime());
7682 : // the minGap was subtracted from gap in MSLink::getLeaderInfo (enlarging the negative gap)
7683 : // so the -2* makes it point in the right direction
7684 11597 : const double foeGap = -gap - veh->getLength() - 2 * getVehicleType().getMinGap();
7685 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7686 : if (DEBUG_COND) {
7687 : std::cout << " foeGap=" << foeGap << " foeBGap=" << foeBrakeGap << "\n";
7688 :
7689 : }
7690 : #endif
7691 11597 : if (foeGap < foeBrakeGap) {
7692 : response = true;
7693 : response2 = false;
7694 : } else {
7695 : response = false;
7696 : response2 = true;
7697 : }
7698 : } else {
7699 : // let conflict entry time decide
7700 : response = true;
7701 : response2 = true;
7702 : }
7703 816870 : } else if (entry->havePriority() != foeEntry->havePriority()) {
7704 610687 : response = !entry->havePriority();
7705 610687 : response2 = !foeEntry->havePriority();
7706 206183 : } else if (entry->haveYellow() && foeEntry->haveYellow()) {
7707 : // let the faster vehicle keep moving
7708 5664 : response = veh->getSpeed() >= getSpeed();
7709 5664 : response2 = getSpeed() >= veh->getSpeed();
7710 : } else {
7711 : // fallback if pedestrian crossings are involved
7712 200519 : response = logic->getResponseFor(link->getIndex()).test(foeLink->getIndex());
7713 200519 : response2 = logic->getResponseFor(foeLink->getIndex()).test(link->getIndex());
7714 : }
7715 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7716 : if (DEBUG_COND) {
7717 : std::cout << SIMTIME
7718 : << " foeLane=" << foeLane->getID()
7719 : << " foeLink=" << foeLink->getViaLaneOrLane()->getID()
7720 : << " linkIndex=" << link->getIndex()
7721 : << " foeLinkIndex=" << foeLink->getIndex()
7722 : << " entryState=" << toString(entry->getState())
7723 : << " entryState2=" << toString(foeEntry->getState())
7724 : << " response=" << response
7725 : << " response2=" << response2
7726 : << "\n";
7727 : }
7728 : #endif
7729 933253 : if (!response) {
7730 : // if we have right of way over the foe, entryTime does not matter
7731 77399 : foeET = veh->myJunctionConflictEntryTime;
7732 77399 : egoET = myJunctionEntryTime;
7733 855854 : } else if (response && response2) {
7734 : // in a mutual conflict scenario, use entry time to avoid deadlock
7735 128327 : foeET = veh->myJunctionConflictEntryTime;
7736 128327 : egoET = myJunctionConflictEntryTime;
7737 : }
7738 : }
7739 1368970 : if (egoET == foeET) {
7740 : // try to use speed as tie braker
7741 104628 : if (getSpeed() == veh->getSpeed()) {
7742 : // use ID as tie braker
7743 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7744 : if (DEBUG_COND) {
7745 : std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
7746 : << " foeIsLeaderByID=" << (getID() < veh->getID()) << "\n";
7747 : }
7748 : #endif
7749 55634 : return getID() < veh->getID();
7750 : } else {
7751 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7752 : if (DEBUG_COND) {
7753 : std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
7754 : << " foeIsLeaderBySpeed=" << (getSpeed() < veh->getSpeed())
7755 : << " v=" << getSpeed() << " foeV=" << veh->getSpeed()
7756 : << "\n";
7757 : }
7758 : #endif
7759 48994 : return getSpeed() < veh->getSpeed();
7760 : }
7761 : } else {
7762 : // leader was on the junction first
7763 : #ifdef DEBUG_PLAN_MOVE_LEADERINFO
7764 : if (DEBUG_COND) {
7765 : std::cout << SIMTIME << " veh=" << getID() << " egoET " << egoET << " with foe " << veh->getID()
7766 : << " foeET=" << foeET << " isLeader=" << (egoET > foeET) << "\n";
7767 : }
7768 : #endif
7769 1264342 : return egoET > foeET;
7770 : }
7771 : } else {
7772 : // vehicle can only be partially on the junction. Must be a leader
7773 : return true;
7774 : }
7775 : } else {
7776 : // vehicle can only be partially on the junction. Must be a leader
7777 : return true;
7778 : }
7779 : }
7780 :
7781 : void
7782 2499 : MSVehicle::saveState(OutputDevice& out) {
7783 2499 : MSBaseVehicle::saveState(out);
7784 : // here starts the vehicle internal part (see loading)
7785 : std::vector<std::string> internals;
7786 2499 : internals.push_back(toString(myParameter->parametersSet));
7787 2499 : internals.push_back(toString(myDeparture));
7788 2499 : internals.push_back(toString(distance(myRoute->begin(), myCurrEdge)));
7789 2499 : internals.push_back(toString(myDepartPos));
7790 2499 : internals.push_back(toString(myWaitingTime));
7791 2499 : internals.push_back(toString(myTimeLoss));
7792 2499 : internals.push_back(toString(myLastActionTime));
7793 2499 : internals.push_back(toString(isStopped()));
7794 2499 : internals.push_back(toString(isStopped() ? myStops.front().duration : 0));
7795 2499 : internals.push_back(toString(myPastStops.size()));
7796 2499 : out.writeAttr(SUMO_ATTR_STATE, internals);
7797 2499 : out.writeAttr(SUMO_ATTR_POSITION, std::vector<double> { myState.myPos, myState.myBackPos, myState.myLastCoveredDist });
7798 2499 : out.writeAttr(SUMO_ATTR_SPEED, std::vector<double> { myState.mySpeed, myState.myPreviousSpeed });
7799 2499 : out.writeAttr(SUMO_ATTR_ANGLE, GeomHelper::naviDegree(myAngle));
7800 2499 : out.writeAttr(SUMO_ATTR_POSITION_LAT, myState.myPosLat);
7801 2499 : out.writeAttr(SUMO_ATTR_WAITINGTIME, myWaitingTimeCollector.getState());
7802 2499 : if (isStopped() && myStops.front().entryPos != getPositionOnLane()) {
7803 1 : out.writeAttr(SUMO_ATTR_ENTRYPOS, myStops.front().entryPos);
7804 : }
7805 2499 : myLaneChangeModel->saveState(out);
7806 : // save past stops
7807 5573 : for (SUMOVehicleParameter::Stop stop : myPastStops) {
7808 3074 : stop.write(out, false);
7809 : // do not write started and ended twice
7810 3074 : if ((stop.parametersSet & STOP_STARTED_SET) == 0) {
7811 3069 : out.writeAttr(SUMO_ATTR_STARTED, time2string(stop.started));
7812 : }
7813 3074 : if ((stop.parametersSet & STOP_ENDED_SET) == 0) {
7814 3069 : out.writeAttr(SUMO_ATTR_ENDED, time2string(stop.ended));
7815 : }
7816 3074 : stop.writeParams(out);
7817 3074 : out.closeTag();
7818 3074 : }
7819 : // save upcoming stops
7820 2990 : for (MSStop& stop : myStops) {
7821 491 : stop.write(out);
7822 : }
7823 : // save parameters and device states
7824 2499 : myParameter->writeParams(out);
7825 6367 : for (MSVehicleDevice* const dev : myDevices) {
7826 3868 : dev->saveState(out);
7827 : }
7828 2499 : out.closeTag();
7829 2499 : }
7830 :
7831 : void
7832 3356 : MSVehicle::loadState(const SUMOSAXAttributes& attrs, const SUMOTime offset) {
7833 3356 : if (!attrs.hasAttribute(SUMO_ATTR_POSITION)) {
7834 0 : throw ProcessError(TL("Error: Invalid vehicles in state (may be a meso state)!"));
7835 : }
7836 : bool ok;
7837 : int routeOffset;
7838 : bool stopped;
7839 : SUMOTime stopDuration;
7840 : int pastStops;
7841 :
7842 3356 : std::istringstream bis(attrs.getString(SUMO_ATTR_STATE));
7843 3356 : bis >> myParameter->parametersSet;
7844 3356 : bis >> myDeparture;
7845 3356 : bis >> routeOffset;
7846 3356 : bis >> myDepartPos;
7847 3356 : bis >> myWaitingTime;
7848 3356 : bis >> myTimeLoss;
7849 3356 : bis >> myLastActionTime;
7850 : bis >> stopped;
7851 : bis >> stopDuration;
7852 3356 : bis >> pastStops;
7853 :
7854 3356 : if (attrs.hasAttribute(SUMO_ATTR_ARRIVALPOS_RANDOMIZED)) {
7855 4 : myArrivalPos = attrs.get<double>(SUMO_ATTR_ARRIVALPOS_RANDOMIZED, getID().c_str(), ok);
7856 : }
7857 : // load stops
7858 : myStops.clear();
7859 3356 : addStops(!MSGlobals::gCheckRoutes, &myCurrEdge, false);
7860 :
7861 3356 : if (hasDeparted()) {
7862 1556 : myCurrEdge = myRoute->begin() + routeOffset;
7863 1556 : myDeparture -= offset;
7864 : // fix stops
7865 4606 : while (pastStops > 0) {
7866 : SUMOVehicleParameter::Stop& pars = const_cast<SUMOVehicleParameter::Stop&>(myStops.front().pars);
7867 : // assumed these attributes were only added to restore vehroute-output.exit-times
7868 3050 : if (!MSGlobals::gUseStopEnded) {
7869 3050 : pars.parametersSet &= ~STOP_ENDED_SET;
7870 : }
7871 3050 : if (!MSGlobals::gUseStopStarted) {
7872 3050 : pars.parametersSet &= ~STOP_STARTED_SET;
7873 : }
7874 6125 : for (const auto& rem : myMoveReminders) {
7875 3075 : rem.first->notifyStopEnded();
7876 : }
7877 3050 : myPastStops.push_back(myStops.front().pars);
7878 3050 : myPastStops.back().routeIndex = (int)(myStops.front().edge - myRoute->begin());
7879 3050 : myStops.pop_front();
7880 3050 : pastStops--;
7881 : }
7882 : // see MSBaseVehicle constructor
7883 1556 : if (myParameter->wasSet(VEHPARS_FORCE_REROUTE)) {
7884 1146 : calculateArrivalParams(true);
7885 : }
7886 : // a (tentative lane is needed for calling hasArrivedInternal
7887 1556 : myLane = (*myCurrEdge)->getLanes()[0];
7888 : }
7889 3356 : if (getActionStepLength() == DELTA_T && !isActionStep(SIMSTEP)) {
7890 1 : myLastActionTime -= (myLastActionTime - SIMSTEP) % DELTA_T;
7891 3 : WRITE_WARNINGF(TL("Action steps are out of sync for loaded vehicle '%'."), getID());
7892 : }
7893 3356 : std::istringstream pis(attrs.getString(SUMO_ATTR_POSITION));
7894 3356 : pis >> myState.myPos >> myState.myBackPos >> myState.myLastCoveredDist;
7895 3356 : std::istringstream sis(attrs.getString(SUMO_ATTR_SPEED));
7896 3356 : sis >> myState.mySpeed >> myState.myPreviousSpeed;
7897 3356 : myAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
7898 3356 : myAngle = GeomHelper::fromNaviDegree(attrs.getFloat(SUMO_ATTR_ANGLE));
7899 3356 : myState.myPosLat = attrs.getFloat(SUMO_ATTR_POSITION_LAT);
7900 3356 : std::istringstream dis(attrs.getString(SUMO_ATTR_DISTANCE));
7901 3356 : dis >> myOdometer >> myNumberReroutes;
7902 3356 : myWaitingTimeCollector.setState(attrs.getString(SUMO_ATTR_WAITINGTIME));
7903 3356 : if (stopped) {
7904 222 : double realPos = getPositionOnLane();
7905 222 : double entryPos = attrs.getOpt<double>(SUMO_ATTR_ENTRYPOS, getID().c_str(), ok, realPos);
7906 222 : myStops.front().startedFromState = true;
7907 222 : if (entryPos != realPos) {
7908 1 : myStops.front().entryPos = entryPos;
7909 : }
7910 222 : myLane = const_cast<MSLane*>(myStops.front().lane);
7911 222 : myStopDist = 0;
7912 222 : myState.myPos = entryPos; // fake position for replication stop entry which happened before the position was updated
7913 222 : processNextStop(getSpeed());
7914 222 : myState.myPos = realPos; // reset fake position
7915 222 : if (myStops.front().pars.parking != ParkingType::ONROAD) {
7916 : // processNextStop is called again during MSVehicleTransfer::loadState
7917 204 : stopDuration += getActionStepLength();
7918 : }
7919 222 : myStops.front().duration = stopDuration;
7920 222 : if (!MSGlobals::gUseStopStarted) {
7921 : SUMOVehicleParameter::Stop& pars = const_cast<SUMOVehicleParameter::Stop&>(myStops.front().pars);
7922 222 : pars.parametersSet &= ~STOP_STARTED_SET;
7923 : }
7924 : }
7925 3356 : myLaneChangeModel->loadState(attrs);
7926 : // no need to reset myCachedPosition here since state loading happens directly after creation
7927 3356 : }
7928 :
7929 : void
7930 32 : MSVehicle::loadPreviousApproaching(MSLink* link, bool setRequest,
7931 : SUMOTime arrivalTime, double arrivalSpeed,
7932 : double arrivalSpeedBraking,
7933 : double dist, double leaveSpeed) {
7934 : // ensure that approach information is reset on the next call to setApproachingForAllLinks
7935 32 : myLFLinkLanes.push_back(DriveProcessItem(link, 0, 0, setRequest,
7936 : arrivalTime, arrivalSpeed, arrivalSpeedBraking, dist, leaveSpeed));
7937 :
7938 32 : }
7939 :
7940 :
7941 : std::shared_ptr<MSSimpleDriverState>
7942 2565618 : MSVehicle::getDriverState() const {
7943 2565618 : return myDriverState->getDriverState();
7944 : }
7945 :
7946 :
7947 : double
7948 623295108 : MSVehicle::getFriction() const {
7949 623295108 : return myFrictionDevice == nullptr ? 1. : myFrictionDevice->getMeasuredFriction();
7950 : }
7951 :
7952 :
7953 : void
7954 196 : MSVehicle::setPreviousSpeed(double prevSpeed, double prevAcceleration) {
7955 196 : myState.mySpeed = MAX2(0., prevSpeed);
7956 : // also retcon acceleration
7957 196 : if (prevAcceleration != std::numeric_limits<double>::min()) {
7958 8 : myAcceleration = prevAcceleration;
7959 : } else {
7960 188 : myAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
7961 : }
7962 196 : }
7963 :
7964 :
7965 : double
7966 1867278052 : MSVehicle::getCurrentApparentDecel() const {
7967 : //return MAX2(-myAcceleration, getCarFollowModel().getApparentDecel());
7968 1867278052 : return getCarFollowModel().getApparentDecel();
7969 : }
7970 :
7971 : /****************************************************************************/
7972 : bool
7973 32 : MSVehicle::setExitManoeuvre() {
7974 32 : return (myManoeuvre.configureExitManoeuvre(this));
7975 : }
7976 :
7977 : /* -------------------------------------------------------------------------
7978 : * methods of MSVehicle::manoeuvre
7979 : * ----------------------------------------------------------------------- */
7980 :
7981 4500146 : MSVehicle::Manoeuvre::Manoeuvre() : myManoeuvreStop(""), myManoeuvreStartTime(0), myManoeuvreCompleteTime(0), myManoeuvreType(MSVehicle::MANOEUVRE_NONE), myGUIIncrement(0) {}
7982 :
7983 :
7984 0 : MSVehicle::Manoeuvre::Manoeuvre(const Manoeuvre& manoeuvre) {
7985 0 : myManoeuvreStop = manoeuvre.myManoeuvreStop;
7986 0 : myManoeuvreStartTime = manoeuvre.myManoeuvreStartTime;
7987 0 : myManoeuvreCompleteTime = manoeuvre.myManoeuvreCompleteTime;
7988 0 : myManoeuvreType = manoeuvre.myManoeuvreType;
7989 0 : myGUIIncrement = manoeuvre.myGUIIncrement;
7990 0 : }
7991 :
7992 :
7993 : MSVehicle::Manoeuvre&
7994 0 : MSVehicle::Manoeuvre::operator=(const Manoeuvre& manoeuvre) {
7995 0 : myManoeuvreStop = manoeuvre.myManoeuvreStop;
7996 0 : myManoeuvreStartTime = manoeuvre.myManoeuvreStartTime;
7997 0 : myManoeuvreCompleteTime = manoeuvre.myManoeuvreCompleteTime;
7998 0 : myManoeuvreType = manoeuvre.myManoeuvreType;
7999 0 : myGUIIncrement = manoeuvre.myGUIIncrement;
8000 0 : return *this;
8001 : }
8002 :
8003 :
8004 : bool
8005 0 : MSVehicle::Manoeuvre::operator!=(const Manoeuvre& manoeuvre) {
8006 0 : return (myManoeuvreStop != manoeuvre.myManoeuvreStop ||
8007 0 : myManoeuvreStartTime != manoeuvre.myManoeuvreStartTime ||
8008 0 : myManoeuvreCompleteTime != manoeuvre.myManoeuvreCompleteTime ||
8009 0 : myManoeuvreType != manoeuvre.myManoeuvreType ||
8010 0 : myGUIIncrement != manoeuvre.myGUIIncrement
8011 0 : );
8012 : }
8013 :
8014 :
8015 : double
8016 450 : MSVehicle::Manoeuvre::getGUIIncrement() const {
8017 450 : return (myGUIIncrement);
8018 : }
8019 :
8020 :
8021 : MSVehicle::ManoeuvreType
8022 2971 : MSVehicle::Manoeuvre::getManoeuvreType() const {
8023 2971 : return (myManoeuvreType);
8024 : }
8025 :
8026 :
8027 : MSVehicle::ManoeuvreType
8028 2971 : MSVehicle::getManoeuvreType() const {
8029 2971 : return (myManoeuvre.getManoeuvreType());
8030 : }
8031 :
8032 :
8033 : void
8034 30 : MSVehicle::setManoeuvreType(const MSVehicle::ManoeuvreType mType) {
8035 30 : myManoeuvre.setManoeuvreType(mType);
8036 30 : }
8037 :
8038 :
8039 : void
8040 30 : MSVehicle::Manoeuvre::setManoeuvreType(const MSVehicle::ManoeuvreType mType) {
8041 30 : myManoeuvreType = mType;
8042 30 : }
8043 :
8044 :
8045 : bool
8046 30 : MSVehicle::Manoeuvre::configureEntryManoeuvre(MSVehicle* veh) {
8047 30 : if (!veh->hasStops()) {
8048 : return false; // should never happen - checked before call
8049 : }
8050 :
8051 30 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
8052 30 : const MSStop& stop = veh->getNextStop();
8053 :
8054 30 : int manoeuverAngle = stop.parkingarea->getLastFreeLotAngle();
8055 30 : double GUIAngle = stop.parkingarea->getLastFreeLotGUIAngle();
8056 30 : if (abs(GUIAngle) < 0.1) {
8057 : GUIAngle = -0.1; // Wiggle vehicle on parallel entry
8058 : }
8059 30 : myManoeuvreVehicleID = veh->getID();
8060 30 : myManoeuvreStop = stop.parkingarea->getID();
8061 30 : myManoeuvreType = MSVehicle::MANOEUVRE_ENTRY;
8062 30 : myManoeuvreStartTime = currentTime;
8063 30 : myManoeuvreCompleteTime = currentTime + veh->myType->getEntryManoeuvreTime(manoeuverAngle);
8064 30 : myGUIIncrement = GUIAngle / (STEPS2TIME(myManoeuvreCompleteTime - myManoeuvreStartTime) / TS);
8065 :
8066 : #ifdef DEBUG_STOPS
8067 : if (veh->isSelected()) {
8068 : 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 <<
8069 : " endTime=" << myManoeuvreCompleteTime << " manoeuvre time=" << myManoeuvreCompleteTime - currentTime << " parkArea=" << myManoeuvreStop << std::endl;
8070 : }
8071 : #endif
8072 :
8073 30 : return (true);
8074 : }
8075 :
8076 :
8077 : bool
8078 32 : MSVehicle::Manoeuvre::configureExitManoeuvre(MSVehicle* veh) {
8079 : // At the moment we only want to set for parking areas
8080 32 : if (!veh->hasStops()) {
8081 : return true;
8082 : }
8083 32 : if (veh->getNextStop().parkingarea == nullptr) {
8084 : return true;
8085 : }
8086 :
8087 30 : if (myManoeuvreType != MSVehicle::MANOEUVRE_NONE) {
8088 : return (false);
8089 : }
8090 :
8091 30 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
8092 :
8093 30 : int manoeuverAngle = veh->getCurrentParkingArea()->getManoeuverAngle(*veh);
8094 30 : double GUIAngle = veh->getCurrentParkingArea()->getGUIAngle(*veh);
8095 30 : if (abs(GUIAngle) < 0.1) {
8096 : GUIAngle = 0.1; // Wiggle vehicle on parallel exit
8097 : }
8098 :
8099 30 : myManoeuvreVehicleID = veh->getID();
8100 30 : myManoeuvreStop = veh->getCurrentParkingArea()->getID();
8101 30 : myManoeuvreType = MSVehicle::MANOEUVRE_EXIT;
8102 30 : myManoeuvreStartTime = currentTime;
8103 30 : myManoeuvreCompleteTime = currentTime + veh->myType->getExitManoeuvreTime(manoeuverAngle);
8104 30 : myGUIIncrement = -GUIAngle / (STEPS2TIME(myManoeuvreCompleteTime - myManoeuvreStartTime) / TS);
8105 30 : if (veh->remainingStopDuration() > 0) {
8106 20 : myManoeuvreCompleteTime += veh->remainingStopDuration();
8107 : }
8108 :
8109 : #ifdef DEBUG_STOPS
8110 : if (veh->isSelected()) {
8111 : std::cout << "EXIT manoeuvre start: vehicle=" << veh->getID() << " Manoeuvre Angle=" << manoeuverAngle << " increment=" << RAD2DEG(myGUIIncrement) << " currentTime=" << currentTime
8112 : << " endTime=" << myManoeuvreCompleteTime << " manoeuvre time=" << myManoeuvreCompleteTime - currentTime << " parkArea=" << myManoeuvreStop << std::endl;
8113 : }
8114 : #endif
8115 :
8116 : return (true);
8117 : }
8118 :
8119 :
8120 : bool
8121 222 : MSVehicle::Manoeuvre::entryManoeuvreIsComplete(MSVehicle* veh) {
8122 : // At the moment we only want to consider parking areas - need to check because we could be setting up a manoeuvre
8123 222 : if (!veh->hasStops()) {
8124 : return (true);
8125 : }
8126 : MSStop* currentStop = &veh->myStops.front();
8127 222 : if (currentStop->parkingarea == nullptr) {
8128 : return true;
8129 220 : } else if (currentStop->parkingarea->getID() != myManoeuvreStop || MSVehicle::MANOEUVRE_ENTRY != myManoeuvreType) {
8130 30 : if (configureEntryManoeuvre(veh)) {
8131 30 : MSNet::getInstance()->informVehicleStateListener(veh, MSNet::VehicleState::MANEUVERING);
8132 30 : return (false);
8133 : } else { // cannot configure entry so stop trying
8134 : return true;
8135 : }
8136 190 : } else if (MSNet::getInstance()->getCurrentTimeStep() < myManoeuvreCompleteTime) {
8137 : return false;
8138 : } else { // manoeuvre complete
8139 30 : myManoeuvreType = MSVehicle::MANOEUVRE_NONE;
8140 30 : return true;
8141 : }
8142 : }
8143 :
8144 :
8145 : bool
8146 0 : MSVehicle::Manoeuvre::manoeuvreIsComplete(const ManoeuvreType checkType) const {
8147 0 : if (checkType != myManoeuvreType) {
8148 : return true; // we're not maneuvering / wrong manoeuvre
8149 : }
8150 :
8151 0 : if (MSNet::getInstance()->getCurrentTimeStep() < myManoeuvreCompleteTime) {
8152 : return false;
8153 : } else {
8154 : return true;
8155 : }
8156 : }
8157 :
8158 :
8159 : bool
8160 6266 : MSVehicle::Manoeuvre::manoeuvreIsComplete() const {
8161 6266 : return (MSNet::getInstance()->getCurrentTimeStep() >= myManoeuvreCompleteTime);
8162 : }
8163 :
8164 :
8165 : bool
8166 6266 : MSVehicle::manoeuvreIsComplete() const {
8167 6266 : return (myManoeuvre.manoeuvreIsComplete());
8168 : }
8169 :
8170 :
8171 : std::pair<double, double>
8172 7371 : MSVehicle::estimateTimeToNextStop() const {
8173 7371 : if (hasStops()) {
8174 7371 : MSLane* lane = myLane;
8175 7371 : if (lane == nullptr) {
8176 : // not in network
8177 84 : lane = getEdge()->getLanes()[0];
8178 : }
8179 : const MSStop& stop = myStops.front();
8180 : auto it = myCurrEdge + 1;
8181 : // drive to end of current edge
8182 7371 : double dist = (lane->getLength() - getPositionOnLane());
8183 7371 : double travelTime = lane->getEdge().getMinimumTravelTime(this) * dist / lane->getLength();
8184 : // drive until stop edge
8185 8582 : while (it != myRoute->end() && it < stop.edge) {
8186 1211 : travelTime += (*it)->getMinimumTravelTime(this);
8187 1211 : dist += (*it)->getLength();
8188 : it++;
8189 : }
8190 : // drive up to the stop position
8191 7371 : const double stopEdgeDist = stop.pars.endPos - (lane == stop.lane ? lane->getLength() : 0);
8192 7371 : dist += stopEdgeDist;
8193 7371 : travelTime += stop.lane->getEdge().getMinimumTravelTime(this) * (stopEdgeDist / stop.lane->getLength());
8194 : // estimate time loss due to acceleration and deceleration
8195 : // maximum speed is limited by available distance:
8196 : const double a = getCarFollowModel().getMaxAccel();
8197 : const double b = getCarFollowModel().getMaxDecel();
8198 7371 : const double c = getSpeed();
8199 : const double d = dist;
8200 7371 : const double len = getVehicleType().getLength();
8201 7371 : const double vs = MIN2(MAX2(stop.getSpeed(), 0.0), stop.lane->getVehicleMaxSpeed(this));
8202 : // distAccel = (v - c)^2 / (2a)
8203 : // distDecel = (v + vs)*(v - vs) / 2b = (v^2 - vs^2) / (2b)
8204 : // distAccel + distDecel < d
8205 7371 : 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))))
8206 14463 : + pow((a * vs), 2))))) * 0.5) + (c * b)) / (b + a));
8207 7371 : it = myCurrEdge;
8208 : double v0 = c;
8209 7371 : bool v0Stable = getAcceleration() == 0 && v0 > 0;
8210 : double timeLossAccel = 0;
8211 : double timeLossDecel = 0;
8212 : double timeLossLength = 0;
8213 17397 : while (it != myRoute->end() && it <= stop.edge) {
8214 10026 : double v = MIN2(maxVD, (*it)->getVehicleMaxSpeed(this));
8215 10026 : double edgeLength = (it == stop.edge ? stop.pars.endPos : (*it)->getLength()) - (it == myCurrEdge ? getPositionOnLane() : 0);
8216 10026 : if (edgeLength <= len && v0Stable && v0 < v) {
8217 : const double lengthDist = MIN2(len, edgeLength);
8218 20 : const double dTL = lengthDist / v0 - lengthDist / v;
8219 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " el=" << edgeLength << " lDist=" << lengthDist << " newTLL=" << dTL<< "\n";
8220 20 : timeLossLength += dTL;
8221 : }
8222 10026 : if (edgeLength > len) {
8223 8914 : const double dv = v - v0;
8224 8914 : if (dv > 0) {
8225 : // timeLossAccel = timeAccel - timeMaxspeed = dv / a - distAccel / v
8226 6399 : const double dTA = dv / a - dv * (v + v0) / (2 * a * v);
8227 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLA=" << dTA << "\n";
8228 6399 : timeLossAccel += dTA;
8229 : // time loss from vehicle length
8230 2515 : } else if (dv < 0) {
8231 : // timeLossDecel = timeDecel - timeMaxspeed = dv / b - distDecel / v
8232 405 : const double dTD = -dv / b + dv * (v + v0) / (2 * b * v0);
8233 : //std::cout << " e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLD=" << dTD << "\n";
8234 405 : timeLossDecel += dTD;
8235 : }
8236 : v0 = v;
8237 : v0Stable = true;
8238 : }
8239 : it++;
8240 : }
8241 : // final deceleration to stop (may also be acceleration or deceleration to waypoint speed)
8242 : double v = vs;
8243 7371 : const double dv = v - v0;
8244 7371 : if (dv > 0) {
8245 : // timeLossAccel = timeAccel - timeMaxspeed = dv / a - distAccel / v
8246 144 : const double dTA = dv / a - dv * (v + v0) / (2 * a * v);
8247 : //std::cout << " final e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLA=" << dTA << "\n";
8248 144 : timeLossAccel += dTA;
8249 : // time loss from vehicle length
8250 7227 : } else if (dv < 0) {
8251 : // timeLossDecel = timeDecel - timeMaxspeed = dv / b - distDecel / v
8252 7202 : const double dTD = -dv / b + dv * (v + v0) / (2 * b * v0);
8253 : //std::cout << " final e=" << (*it)->getID() << " v0=" << v0 << " v=" << v << " newTLD=" << dTD << "\n";
8254 7202 : timeLossDecel += dTD;
8255 : }
8256 7371 : const double result = travelTime + timeLossAccel + timeLossDecel + timeLossLength;
8257 : //std::cout << SIMTIME << " v=" << c << " a=" << a << " b=" << b << " maxVD=" << maxVD << " tt=" << travelTime
8258 : // << " ta=" << timeLossAccel << " td=" << timeLossDecel << " tl=" << timeLossLength << " res=" << result << "\n";
8259 7371 : return {MAX2(0.0, result), dist};
8260 : } else {
8261 0 : return {INVALID_DOUBLE, INVALID_DOUBLE};
8262 : }
8263 : }
8264 :
8265 :
8266 : double
8267 2392 : MSVehicle::getStopDelay() const {
8268 2392 : if (hasStops() && myStops.front().pars.until >= 0) {
8269 : const MSStop& stop = myStops.front();
8270 1552 : SUMOTime estimatedDepart = MSNet::getInstance()->getCurrentTimeStep() - DELTA_T;
8271 1552 : if (stop.reached) {
8272 765 : return STEPS2TIME(estimatedDepart + stop.duration - stop.pars.until);
8273 : }
8274 787 : if (stop.pars.duration > 0) {
8275 585 : estimatedDepart += stop.pars.duration;
8276 : }
8277 787 : estimatedDepart += TIME2STEPS(estimateTimeToNextStop().first);
8278 787 : const double result = MAX2(0.0, STEPS2TIME(estimatedDepart - stop.pars.until));
8279 787 : return result;
8280 : } else {
8281 : // vehicles cannot drive before 'until' so stop delay can never be
8282 : // negative and we can use -1 to signal "undefined"
8283 : return -1;
8284 : }
8285 : }
8286 :
8287 :
8288 : double
8289 5445 : MSVehicle::getStopArrivalDelay() const {
8290 5445 : if (hasStops() && myStops.front().pars.arrival >= 0) {
8291 : const MSStop& stop = myStops.front();
8292 4274 : if (stop.reached) {
8293 1267 : return STEPS2TIME(stop.pars.started - stop.pars.arrival);
8294 : } else {
8295 3007 : return STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) + estimateTimeToNextStop().first - STEPS2TIME(stop.pars.arrival);
8296 : }
8297 : } else {
8298 : // vehicles can arrive earlier than planned so arrival delay can be negative
8299 : return INVALID_DOUBLE;
8300 : }
8301 : }
8302 :
8303 :
8304 : const MSEdge*
8305 3094342201 : MSVehicle::getCurrentEdge() const {
8306 3094342201 : return myLane != nullptr ? &myLane->getEdge() : getEdge();
8307 : }
8308 :
8309 :
8310 : const MSEdge*
8311 3932 : MSVehicle::getNextEdgePtr() const {
8312 3932 : if (myLane == nullptr || (myCurrEdge + 1) == myRoute->end()) {
8313 8 : return nullptr;
8314 : }
8315 3924 : if (myLane->isInternal()) {
8316 568 : return &myLane->getCanonicalSuccessorLane()->getEdge();
8317 : } else {
8318 3356 : const MSEdge* nextNormal = succEdge(1);
8319 3356 : const MSEdge* nextInternal = myLane->getEdge().getInternalFollowingEdge(nextNormal, getVClass());
8320 3356 : return nextInternal ? nextInternal : nextNormal;
8321 : }
8322 : }
8323 :
8324 :
8325 : const MSLane*
8326 1602 : MSVehicle::getPreviousLane(const MSLane* current, int& furtherIndex) const {
8327 1602 : if (furtherIndex < (int)myFurtherLanes.size()) {
8328 1225 : return myFurtherLanes[furtherIndex++];
8329 : } else {
8330 : // try to use route information
8331 377 : int routeIndex = getRoutePosition();
8332 : bool resultInternal;
8333 377 : if (MSGlobals::gUsingInternalLanes && MSNet::getInstance()->hasInternalLinks()) {
8334 0 : if (myLane->isInternal()) {
8335 0 : if (furtherIndex % 2 == 0) {
8336 0 : routeIndex -= (furtherIndex + 0) / 2;
8337 : resultInternal = false;
8338 : } else {
8339 0 : routeIndex -= (furtherIndex + 1) / 2;
8340 : resultInternal = false;
8341 : }
8342 : } else {
8343 0 : if (furtherIndex % 2 != 0) {
8344 0 : routeIndex -= (furtherIndex + 1) / 2;
8345 : resultInternal = false;
8346 : } else {
8347 0 : routeIndex -= (furtherIndex + 2) / 2;
8348 : resultInternal = true;
8349 : }
8350 : }
8351 : } else {
8352 377 : routeIndex -= furtherIndex;
8353 : resultInternal = false;
8354 : }
8355 377 : furtherIndex++;
8356 377 : if (routeIndex >= 0) {
8357 163 : if (resultInternal) {
8358 0 : const MSEdge* prevNormal = myRoute->getEdges()[routeIndex];
8359 0 : for (MSLane* cand : prevNormal->getLanes()) {
8360 0 : for (MSLink* link : cand->getLinkCont()) {
8361 0 : if (link->getLane() == current) {
8362 0 : if (link->getViaLane() != nullptr) {
8363 : return link->getViaLane();
8364 : } else {
8365 0 : return const_cast<MSLane*>(link->getLaneBefore());
8366 : }
8367 : }
8368 : }
8369 : }
8370 : } else {
8371 163 : return myRoute->getEdges()[routeIndex]->getLanes()[0];
8372 : }
8373 : }
8374 : }
8375 : return current;
8376 : }
8377 :
8378 : SUMOTime
8379 1517288086 : MSVehicle::getWaitingTimeFor(const MSLink* link) const {
8380 : // this vehicle currently has the highest priority on the allway_stop
8381 1517288086 : return link == myHaveStoppedFor ? SUMOTime_MAX : getWaitingTime();
8382 : }
8383 :
8384 :
8385 : void
8386 694 : MSVehicle::resetApproachOnReroute() {
8387 : bool diverged = false;
8388 : const ConstMSEdgeVector& route = myRoute->getEdges();
8389 694 : int ri = getRoutePosition();
8390 2928 : for (const DriveProcessItem& dpi : myLFLinkLanes) {
8391 2234 : if (dpi.myLink != nullptr) {
8392 2231 : if (!diverged) {
8393 1998 : const MSEdge* next = route[ri + 1];
8394 1998 : if (&dpi.myLink->getLane()->getEdge() != next) {
8395 : diverged = true;
8396 : } else {
8397 1932 : if (dpi.myLink->getViaLane() == nullptr) {
8398 : ri++;
8399 : }
8400 : }
8401 : }
8402 : if (diverged) {
8403 299 : dpi.myLink->removeApproaching(this);
8404 : }
8405 : }
8406 : }
8407 694 : }
8408 :
8409 :
8410 : bool
8411 13604356 : MSVehicle::instantStopping() const {
8412 13604356 : return myInfluencer && !myInfluencer->considerMaxDeceleration();
8413 : }
8414 :
8415 : /****************************************************************************/
|
