Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSCFModel_EIDM.cpp
15 : /// @author Dominik Salles
16 : /// @date Fri, 06 Jul 2018
17 :
18 : /// Originalfile MSCFModel_IDM.cpp from
19 : /// @author Tobias Mayer
20 : /// @author Daniel Krajzewicz
21 : /// @author Michael Behrisch
22 : /// @date Thu, 03 Sep 2009
23 : ///
24 : // The Extended Intelligent Driver Model (EIDM) car-following model
25 : //
26 : // Publication: Salles, Dominik, S. Kaufmann and H. Reuss. “Extending the Intelligent Driver
27 : // Model in SUMO and Verifying the Drive Off Trajectories with Aerial
28 : // Measurements.” (2020).
29 : /****************************************************************************/
30 :
31 :
32 : // ===========================================================================
33 : // included modules
34 : // ===========================================================================
35 : #include <config.h>
36 :
37 : #include "MSCFModel_EIDM.h"
38 : #include <microsim/MSVehicle.h>
39 : #include <microsim/MSLane.h>
40 : #include <microsim/MSEdge.h>
41 : #include <microsim/MSLink.h>
42 : #include <utils/common/RandHelper.h>
43 : #include <utils/common/SUMOTime.h>
44 :
45 : //#define DEBUG_V
46 :
47 : #define EST_REAC_THRESHOLD 3. // under this threshold estimation, error and reaction time variables don't get taken into account
48 : #define ClutchEngageSpeed 0.5 // When a vehicle is below this speed, we assume a "slow to start", that is because of clutch operation / powertrain inertia
49 : #define EIDM_POS_ACC_EPS 0.05 // some slack number to ensure smoother position, speed and acceleration update
50 :
51 : // ===========================================================================
52 : // method definitions
53 : // ===========================================================================
54 253 : MSCFModel_EIDM::MSCFModel_EIDM(const MSVehicleType* vtype) :
55 253 : MSCFModel(vtype), myDelta(vtype->getParameter().getCFParam(SUMO_ATTR_CF_IDM_DELTA, 4.)),
56 253 : myTwoSqrtAccelDecel(double(2 * sqrt(myAccel * myDecel))),
57 253 : myIterations(MAX2(1, int(TS / vtype->getParameter().getCFParam(SUMO_ATTR_CF_IDM_STEPPING, .25) + .5))),
58 253 : myTPersDrive(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_PERSISTENCE_DRIVE, 3)),
59 253 : myTreaction(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_REACTION, 0.5)),
60 253 : myTpreview(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_LOOK_AHEAD, 4)),
61 253 : myTPersEstimate(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_PERSISTENCE_ESTIMATE, 10)),
62 253 : myCcoolness(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_C_COOLNESS, 0.99)),
63 253 : mySigmaleader(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_LEADER, 0.02)),
64 253 : mySigmagap(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_GAP, 0.1)),
65 253 : mySigmaerror(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_ERROR, 0.04)),
66 253 : myJerkmax(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_JERK_MAX, 3.)),
67 253 : myEpsilonacc(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_EPSILON_ACC, 1.)),
68 253 : myTaccmax(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_ACC_MAX, 1.2)),
69 253 : myMflatness(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_M_FLATNESS, 2.)),
70 253 : myMbegin(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_M_BEGIN, 0.7)),
71 506 : myUseVehDynamics(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_USEVEHDYNAMICS, 0) == 1)
72 : //, myMaxVehPreview(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_MAX_VEH_PREVIEW, 0))
73 : {
74 : // IDM does not drive very precise and may violate minGap on occasion
75 253 : myCollisionMinGapFactor = vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 0.1);
76 253 : }
77 :
78 506 : MSCFModel_EIDM::~MSCFModel_EIDM() {}
79 :
80 : double
81 32139 : MSCFModel_EIDM::insertionFollowSpeed(const MSVehicle* const /*veh*/, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
82 32139 : if (MSGlobals::gSemiImplicitEulerUpdate) {
83 32139 : return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
84 : } else {
85 : // Not Done/checked yet for the ballistic update model!!!!
86 : // NOTE: Even for ballistic update, the current speed is irrelevant at insertion, therefore passing 0. (Leo)
87 : // return maximumSafeFollowSpeed(gap2pred, 0., predSpeed, predMaxDecel, true);
88 0 : return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
89 : }
90 : }
91 :
92 :
93 : double
94 4 : MSCFModel_EIDM::insertionStopSpeed(const MSVehicle* const /*veh*/, double speed, double gap) const {
95 4 : if (MSGlobals::gSemiImplicitEulerUpdate) {
96 4 : return maximumSafeStopSpeed(gap, myDecel, speed, true, myHeadwayTime);
97 : } else {
98 : // Not Done/checked yet for the ballistic update model!!!!
99 : // return MIN2(maximumSafeStopSpeed(gap, myDecel, 0., true, 0.), myType->getMaxSpeed());
100 0 : return MIN2(maximumSafeStopSpeed(gap, myDecel, speed, true, myHeadwayTime), myType->getMaxSpeed());
101 : }
102 : }
103 :
104 : double
105 32139 : MSCFModel_EIDM::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion, const CalcReason /* usage */) const {
106 : double x;
107 32139 : if (gap >= 0 || MSGlobals::gComputeLC) {
108 : double a = 1.;
109 32139 : double b = myHeadwayTime * myTwoSqrtAccelDecel - predSpeed;
110 32139 : double c = -sqrt(1 + myDecel / (2 * myAccel)) * gap * myTwoSqrtAccelDecel;
111 : // with myDecel/myAccel, the intended deceleration is myDecel
112 : // with myDecel/(2*myAccel), the intended deceleration is myDecel/2
113 : // with the IIDM, if gap=s, then the acceleration is zero and if gap<s, then the term v/vMax is not present
114 :
115 : // double c = -sqrt(1 - pow(egoSpeed / MAX2(NUMERICAL_EPS, desSpeed), myDelta) + myDecel / (2 * myAccel)) * gap * myTwoSqrtAccelDecel; // c calculation when using the IDM!
116 :
117 : // myDecel is positive, but is intended as negative value here, therefore + instead of -
118 : // quadratic formula
119 32139 : x = (-b + sqrt(b * b - 4.*a * c)) / (2.*a);
120 32139 : } else {
121 0 : x = egoSpeed - ACCEL2SPEED(myEmergencyDecel);
122 0 : if (MSGlobals::gSemiImplicitEulerUpdate) {
123 : x = MAX2(x, 0.);
124 : }
125 : }
126 :
127 32139 : if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
128 0 : double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
129 0 : if (origSafeDecel > myDecel + NUMERICAL_EPS) {
130 : // Braking harder than myDecel was requested -> calculate required emergency deceleration.
131 : // Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
132 : // can result in corrupted values (leading to intersecting trajectories) if, e.g. leader and follower are fast (leader still faster) and the gap is very small,
133 : // such that braking harder than myDecel is required.
134 :
135 0 : double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
136 : #ifdef DEBUG_EMERGENCYDECEL
137 : if (DEBUG_COND2) {
138 : std::cout << SIMTIME << " initial vsafe=" << x
139 : << " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
140 : << " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
141 : << " safeDecel=" << safeDecel
142 : << std::endl;
143 : }
144 : #endif
145 : // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
146 0 : safeDecel = MAX2(safeDecel, myDecel);
147 : // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
148 : safeDecel = MIN2(safeDecel, origSafeDecel);
149 0 : x = egoSpeed - ACCEL2SPEED(safeDecel);
150 0 : if (MSGlobals::gSemiImplicitEulerUpdate) {
151 : x = MAX2(x, 0.);
152 : }
153 :
154 : #ifdef DEBUG_EMERGENCYDECEL
155 : if (DEBUG_COND2) {
156 : std::cout << " -> corrected emergency deceleration: " << safeDecel << std::endl;
157 : }
158 : #endif
159 :
160 : }
161 : }
162 : assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
163 : assert(!std::isnan(x));
164 32139 : return x;
165 : }
166 :
167 : double
168 4 : MSCFModel_EIDM::maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion, double headway) const {
169 : double vsafe;
170 4 : if (MSGlobals::gSemiImplicitEulerUpdate) {
171 4 : const double g = gap - NUMERICAL_EPS;
172 4 : if (g < 0) {
173 : return 0;
174 : }
175 : double a = 1.;
176 4 : double b = headway * myTwoSqrtAccelDecel - 0.;
177 4 : double c = -sqrt(1 + decel / (2 * myAccel)) * g * myTwoSqrtAccelDecel;
178 : // with decel/myAccel, the intended deceleration is decel
179 : // with decel/(2*myAccel), the intended deceleration is decel/2
180 : // with the IIDM, if gap=s, then the acceleration is zero and if gap<s, then the term currentSpeed/vMax is not present
181 :
182 : // double c = -sqrt(1 - pow(currentSpeed / MAX2(NUMERICAL_EPS, desSpeed), myDelta) + decel / (2 * myAccel)) * g * myTwoSqrtAccelDecel; // c calculation when using the IDM!
183 :
184 : // decel is positive, but is intended as negative value here, therefore + instead of -
185 : // quadratic formula
186 4 : vsafe = (-b + sqrt(b * b - 4.*a * c)) / (2.*a);
187 : } else {
188 : // Not Done/checked yet for the ballistic update model!!!!
189 0 : vsafe = maximumSafeStopSpeedBallistic(gap, decel, currentSpeed, onInsertion, headway);
190 : }
191 :
192 : return vsafe;
193 : }
194 :
195 : double
196 5752359 : MSCFModel_EIDM::patchSpeedBeforeLCEIDM(const MSVehicle* /*veh*/, double vMin, double vMax, const VehicleVariables* vars) const {
197 : // The default value of SUMO_ATTR_JM_SIGMA_MINOR is sigma, not sigmaerror (used in EIDM),
198 : // so we do not use SUMO_ATTR_JM_SIGMA_MINOR as parameter here, because this could confuse users...
199 : //const double sigma = (veh->passingMinor()
200 : // ? veh->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_SIGMA_MINOR, myDawdle)
201 : // : myDawdle);
202 :
203 : // dawdling/drivingerror is now calculated here (in finalizeSpeed, not in stop-/follow-/freeSpeed anymore):
204 : // Instead of just multiplying mySigmaerror with vars->myw_error, we add a factor depending on the criticality of the situation,
205 : // measured with s*/gap. Because when the driver drives "freely" (nothing in front) he may dawdle more than in e.g. congested traffic!
206 5752359 : double s = MAX2(0., vars->myv_est * myHeadwayTime + vars->myv_est * (vars->myv_est - vars->myv_est_l) / myTwoSqrtAccelDecel);
207 5752359 : if (vars->myrespectMinGap) {
208 5600414 : s += myType->getMinGap() + EIDM_POS_ACC_EPS;
209 : } else {
210 151945 : const double minGapStop_EPS = 0.05 + 0.20 * MAX2(0.25, myAccel);
211 151945 : s += minGapStop_EPS + EIDM_POS_ACC_EPS;
212 : }
213 5752359 : const double intensity = MIN2(myAccel, MAX2(vMax - 0.5 * myAccel, 0.0));
214 5752359 : const double criticality = MIN2(MAX2(s / vars->mys_est - 0.5, -0.4), 0.0);
215 :
216 5752359 : const double drivingerror = mySigmaerror * vars->myw_error * intensity * (2.75 * 2.75 * criticality * criticality + 1.0);
217 :
218 : // else: the vehicle is very slow and we do not add driving error (= 0), because
219 : // we should not prevent vehicles from driving just due to dawdling
220 : // if someone is starting, he should definitely start
221 :
222 : //const double vDawdle = MAX2(vMin, dawdle2(vMax, sigma, veh->getRNG()));
223 5752359 : const double vDawdle = MAX2(vMin, vMax + ACCEL2SPEED(drivingerror));
224 5752359 : return vDawdle;
225 : }
226 :
227 : double
228 5752359 : MSCFModel_EIDM::slowToStartTerm(MSVehicle* const veh, const double newSpeed, const double currentSpeed, const double vMax, VehicleVariables* vars) const {
229 : // Drive Off Activation and Term
230 :
231 5752359 : if (newSpeed == 0 || newSpeed <= currentSpeed) {
232 : return newSpeed;
233 : }
234 :
235 : double remainingDelay = 0.0;
236 2535253 : if (newSpeed != vMax) {
237 0 : remainingDelay = STEPS2TIME(DELTA_T - (myStartupDelay - (veh->getTimeSinceStartup() - DELTA_T)));
238 : }
239 :
240 : double v_corr = currentSpeed;
241 10872320 : for (int i = 0; i < myIterations; i++) {
242 : // @ToDo: Check if all clauses are still needed or if we need to add more for all possible drive off cases?!
243 : // When we reach this point, "newSpeed > currentSpeed" already holds
244 : // Activation of the Drive Off term, when
245 68315 : if (currentSpeed < ClutchEngageSpeed && // The start speed is lower than ClutchEngageSpeed m/s
246 68315 : vars->t_off + 4. - NUMERICAL_EPS < (SIMTIME - remainingDelay - TS * (myIterations - i - 1.) / myIterations) && // the last activation is at least 4 seconds ago
247 8359688 : vars->myap_update == 0 && // the last activation is at least 4 seconds ago AND an Action Point was reached
248 10330 : veh->getAcceleration() < MIN2(myAccel / 4, 0.2)) { // && respectMinGap) { // the driver hasn't started accelerating yet (<0.2)
249 10077 : vars->t_off = (SIMTIME - remainingDelay - TS * (myIterations - i - 1.) / myIterations); // activate the drive off term
250 : }
251 : // Calculation of the Drive Off term
252 8337067 : if (vars->t_off + myTaccmax + NUMERICAL_EPS > (SIMTIME - remainingDelay - TS * (myIterations - i - 1.) / myIterations)) {
253 55934 : v_corr = v_corr + (newSpeed - currentSpeed) / myIterations * (tanh((((SIMTIME - remainingDelay - TS * (myIterations - i - 1.) / myIterations) - vars->t_off) * 2. / myTaccmax - myMbegin) * myMflatness) + 1.) / 2.;
254 : } else {
255 8281133 : v_corr = v_corr + (newSpeed - currentSpeed) / myIterations;
256 : }
257 : }
258 : return v_corr;
259 : }
260 :
261 : double
262 5752363 : MSCFModel_EIDM::finalizeSpeed(MSVehicle* const veh, double vPos) const {
263 : // finalizeSpeed is only called once every timestep!
264 :
265 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
266 : // save old v for optional acceleration computation
267 5752363 : const double oldV = veh->getSpeed();
268 :
269 : // @ToDo: Maybe change whole calculation to calculate real freeSpeed/stopSpeed/followSpeed in every call and here calculate resulting speed with reaction Time and update?!
270 : // @ToDo: Could check which call resulted in speed update with stop-vector containing all calculated accelerations!
271 : // Check whether the speed update results from a stop calculation, if so, run _v-function again with the saved variables from stopSpeed
272 : double _vPos = vPos;
273 5752363 : if ((vPos <= SUMO_const_haltingSpeed && vPos <= oldV) || !(vPos > oldV + ACCEL2SPEED(vars->realacc) - NUMERICAL_EPS && vPos < oldV + ACCEL2SPEED(vars->realacc) + NUMERICAL_EPS)) {
274 1374097 : for (auto it = vars->stop.cbegin(); it != vars->stop.cend(); ++it) {
275 75048 : if (vPos > oldV + ACCEL2SPEED(it->first) - NUMERICAL_EPS && vPos < oldV + ACCEL2SPEED(it->first) + NUMERICAL_EPS) {
276 43833 : _vPos = _v(veh, it->second, oldV, 0, vars->v0_int, false, 1, CalcReason::CURRENT);
277 : }
278 : }
279 : }
280 :
281 : // process stops (includes update of stopping state)
282 5752359 : const double vStop = MIN2(_vPos, veh->processNextStop(_vPos));
283 : // apply deceleration bounds
284 5752359 : const double vMinEmergency = minNextSpeedEmergency(oldV, veh);
285 : // _vPos contains the uppper bound on safe speed. allow emergency braking here
286 5752359 : const double vMin = MIN2(minNextSpeed(oldV, veh), MAX2(_vPos, vMinEmergency));
287 : // apply planned speed constraints and acceleration constraints
288 5752359 : double vMax = MIN2(maxNextSpeed(oldV, veh), vStop);
289 : vMax = MAX2(vMin, vMax);
290 :
291 : #ifdef DEBUG_V
292 : if (veh->isSelected()) {
293 : std::cout << SIMTIME
294 : << " EIDM::finalizeSpeed "
295 : << " veh=" << veh->getID()
296 : << " oldV=" << oldV
297 : << " vPos=" << vPos
298 : << " _vPos=" << _vPos
299 : << " vStop=" << vStop
300 : << " vMinEmergency=" << vMinEmergency
301 : << " vMin=" << vMin
302 : << " vMax=" << vMax
303 : << "\n";
304 : }
305 : #endif
306 :
307 : // apply further speed adaptations
308 5752359 : double vNext = patchSpeedBeforeLCEIDM(veh, vMin, vMax, vars);
309 :
310 5752359 : if (MSGlobals::gSemiImplicitEulerUpdate) {
311 :
312 : // The model does not directly use vNext from patchSpeed (like the original MSCFModel::finalizeSpeed function),
313 : // but rather slowly adapts to vNext.
314 5752323 : vNext = veh->getLaneChangeModel().patchSpeed(vMin, vNext, vMax, *this);
315 :
316 : // Bound the positive change of the acceleration with myJerkmax
317 5752323 : if (vNext > oldV && oldV > ClutchEngageSpeed * 2 && vars->t_off + myTaccmax + NUMERICAL_EPS < SIMTIME) {
318 : // At junctions with minor priority acceleration will still jump because after finalizeSpeed "MAX2(vNext, vSafeMin)" is called, vSafeMin is higher and vNext from finalizeSpeed is then ignored!!!
319 : // If we put this jmax-Part into _v-function (via old calc_gap implementation), then vehicle can't drive over junction because it thinks it won't make it in time before a foe may appear!
320 2497026 : if (myJerkmax * TS + veh->getAcceleration() < 0.) { // If driver wants to accelerate, but is still decelerating, then we use a factor of 2!
321 2040 : vNext = MAX2(oldV + MIN2(vNext - oldV, (myJerkmax * 2 * TS + veh->getAcceleration()) * TS), 0.); // change in acceleration (jerk) is bounded by myJerkmax*2
322 : } else {
323 2500653 : vNext = MAX2(oldV + MIN2(vNext - oldV, (myJerkmax * TS + veh->getAcceleration()) * TS), 0.); // change in acceleration (jerk) is bounded by myJerkmax
324 : }
325 3255297 : } else if (vNext <= oldV && vNext < vMax - NUMERICAL_EPS && oldV > ClutchEngageSpeed * 2) {
326 : // Slowing down the deceleration like this may be critical!!! Vehicle can also not come back from Emergency braking fast enough!
327 : /*if (vNext - oldV < -myJerkmax * TS + veh->getAcceleration()) { // driver wants to brake harder than before, change in acceleration is then bounded by -myJerkmax
328 : vNext = MAX2(oldV + (-myJerkmax * TS + veh->getAcceleration()) * TS, 0.);
329 : } else if (vNext - oldV > myJerkmax * TS + veh->getAcceleration()) { // driver wants to brake less harder than before, change in acceleration is then bounded by +myJerkmax
330 : vNext = MAX2(oldV + (myJerkmax * TS + veh->getAcceleration()) * TS, 0.);
331 : } else {
332 : vNext = vNext; // Do nothing, as the new vNext is in the range of +/- jerk!
333 : }*/
334 :
335 : // Workaround letting the vehicle not brake hard for Lane Change reasons (vNext), but only for safety Car following / stopping reasons (vMax)!
336 509954 : vNext = MAX2(oldV + MIN2(vMax - oldV, MAX2(vNext - oldV, (-myJerkmax * TS + veh->getAcceleration()) * TS)), 0.);
337 : }
338 :
339 : } else {
340 : // Not Done/checked yet for the ballistic update model!!!!
341 :
342 : // for ballistic update, negative vnext must be allowed to
343 : // indicate a stop within the coming timestep (i.e., to attain negative values)
344 36 : vNext = veh->getLaneChangeModel().patchSpeed(vMin, vMax, vMax, *this);
345 : // (Leo) finalizeSpeed() is responsible for assuring that the next
346 : // velocity is chosen in accordance with maximal decelerations.
347 : // At this point vNext may also be negative indicating a stop within next step.
348 : // Moreover, because maximumSafeStopSpeed() does not consider deceleration bounds
349 : // vNext can be a large negative value at this point. We cap vNext here.
350 : vNext = MAX2(vNext, vMin);
351 : }
352 :
353 : // Driving off correction term: First we check for StartupDelay, then calculate Speed with SlowToStartTerm
354 : // Apply Startup delay (time) before driving off
355 5752359 : SUMOTime addTime = vars->myap_update * DELTA_T;
356 5752359 : if (myStartupDelay + addTime - (veh->getTimeSinceStartup() - DELTA_T) < DELTA_T) {
357 : addTime = (SUMOTime)0;
358 : }
359 5752359 : double vDelay = applyStartupDelay(veh, vMin, vNext, addTime);
360 : // Apply the slow to start term to the acceleration/speed when driving off
361 5752359 : vNext = slowToStartTerm(veh, vDelay, oldV, vNext, vars);
362 : #ifdef DEBUG_V
363 : if (veh->isSelected()) {
364 : std::cout << SIMTIME
365 : << " EIDM::finalizeSpeed (2) "
366 : << " veh=" << veh->getID()
367 : << " timeSinceStartup=" << veh->getTimeSinceStartup()
368 : << " myap_update=" << vars->myap_update
369 : << " addTime=" << addTime
370 : << " vDelay=" << vDelay
371 : << " oldV=" << oldV
372 : << " vNext=" << vNext
373 : << "\n";
374 : }
375 : #endif
376 :
377 : // Update the desired speed
378 5752359 : internalspeedlimit(veh, oldV);
379 :
380 5752359 : if (vNext > EST_REAC_THRESHOLD) { // update the Wiener-Prozess variables
381 4288869 : vars->myw_gap = exp(-TS / myTPersEstimate) * vars->myw_gap + sqrt(2 * TS / myTPersEstimate) * RandHelper::randNorm(0, 0.5); // variance of 1 can create very high values and may be too high!
382 4288869 : vars->myw_speed = exp(-TS / myTPersEstimate) * vars->myw_speed + sqrt(2 * TS / myTPersEstimate) * RandHelper::randNorm(0, 0.5); // variance of 1 can create very high values and may be too high!
383 4288869 : vars->myw_error = exp(-TS / myTPersDrive) * vars->myw_error + sqrt(2 * TS / myTPersDrive) * RandHelper::randNorm(0, 1);
384 : } // else all those w_... are zero by default
385 :
386 : // Update the Action-point reaction time
387 5752359 : if (vars->myap_update == 0) { // Update all saved acceleration variables for further calculcation between two action points
388 3798743 : vars->lastacc = vars->minaccel;
389 3798743 : vars->wouldacc = vars->minaccel;
390 3798743 : vars->lastrealacc = vars->realacc;
391 3798743 : vars->lastleaderacc = vars->realleaderacc;
392 : }
393 :
394 : #ifdef DEBUG_V
395 : if (veh->isSelected()) {
396 : std::cout << SIMTIME
397 : << " EIDM::finalizeSpeed (3) "
398 : << " veh=" << veh->getID()
399 : << " vars->myw_gap=" << vars->myw_gap
400 : << " vars->myw_speed=" << vars->myw_speed
401 : << " vars->myw_error=" << vars->myw_error
402 : << " vars->lastacc=" << vars->lastacc
403 : << " vars->lastrealacc=" << vars->lastrealacc
404 : << " vars->lastleaderacc=" << vars->lastleaderacc
405 : << "\n";
406 : }
407 : #endif
408 :
409 : // Set myap_update back to 0 when maximal reaction time is reached,
410 : // else add 1 for the next time step
411 5752359 : if (double(vars->myap_update) >= double(myTreaction / TS - 1 - NUMERICAL_EPS)) {
412 3760772 : vars->myap_update = 0;
413 : } else {
414 1991587 : vars->myap_update = vars->myap_update + 1;
415 : }
416 :
417 : // Here the acceleration the vehicle would adapt to without a reaction time is compared to the last acceleration update at the last action point.
418 : // If between two action points the vehicle would like to brake harder than -myEpsilonacc, then an action point is called for the next time step (myap_update = 0).
419 : // This update is only used when wouldacc becomes myEpsilonacc lower than lastacc! When accelerating (wouldacc > lastacc), always the maximal reaction time is used!
420 : // @ToDo: Check how to use a stable reaction time below EST_REAC_THRESHOLD m/s when braking without oscillating acceleration, then this boundary could be eliminated.
421 : // @ToDo: Use this asynchron action point update also for accelerating (like introduced by Wagner/Hoogendorn/Treiber), not only for keeping the CF-model stable!
422 5752359 : if ((vars->wouldacc - vars->lastacc) < -myEpsilonacc || vars->wouldacc < -getEmergencyDecel() || (oldV < EST_REAC_THRESHOLD && vNext < oldV)) {
423 : // When this if-clause holds, then the driver should react immediately!
424 : // 1. When the change in deceleration is lower than -myEpsilonacc
425 : // 2. When the intended deceleration is lower than emergencyDecel
426 : // 3. When the vehicle is slow and decelerating
427 83111 : vars->myap_update = 0;
428 : }
429 :
430 : // Set the "inner" variables of the car-following model back to the default values for the next time step
431 5752359 : vars->minaccel = 100;
432 5752359 : vars->realacc = 100;
433 5752359 : vars->realleaderacc = 100;
434 :
435 : vars->stop.clear();
436 :
437 5752359 : return vNext;
438 : }
439 :
440 :
441 : double
442 33002660 : MSCFModel_EIDM::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double /*predMaxDecel*/, const MSVehicle* const /*pred*/, const CalcReason usage) const {
443 : // applyHeadwayAndSpeedDifferencePerceptionErrors(veh, speed, gap2pred, predSpeed, predMaxDecel, pred);
444 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
445 :
446 : // This update-variable is used to check what called followSpeed (LC- or CF-model), see enum CalcReason for more information
447 : // Here we don't directly use gComputeLC, which is also 0 and 1, because in freeSpeed we have another call (update = 2),
448 : // which is needed to differentiate between the different cases/calculations/needed output/saved variables
449 : int update = 1;
450 : CalcReason _vUsage = usage;
451 33002660 : if (MSGlobals::gComputeLC) {
452 : _vUsage = CalcReason::LANE_CHANGE;
453 : }
454 33002660 : if (_vUsage == CalcReason::LANE_CHANGE || _vUsage == CalcReason::FUTURE) {
455 : update = 0;
456 : }
457 :
458 : #ifdef DEBUG_V
459 : if (veh->isSelected()) {
460 : std::cout << SIMTIME
461 : << " EIDM::followSpeed "
462 : << " veh=" << veh->getID()
463 : << " speed=" << speed
464 : << " gap2pred=" << gap2pred
465 : << " predSpeed=" << predSpeed
466 : << " vars->v0_int=" << vars->v0_int
467 : << " update=" << update
468 : << "\n";
469 : }
470 : #endif
471 :
472 33002660 : double result = _v(veh, gap2pred, speed, predSpeed, vars->v0_int, true, update, _vUsage);
473 33002660 : return result;
474 : }
475 :
476 :
477 : double
478 14755171 : MSCFModel_EIDM::stopSpeed(const MSVehicle* const veh, const double speed, double gap, double /*decel*/, const CalcReason usage) const {
479 : // applyHeadwayPerceptionError(veh, speed, gap);
480 : // if (gap < 0.01) {
481 : // return 0;
482 : // }
483 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
484 :
485 : // This update-variable is used to check what called stopSpeed (LC- or CF-model), see enum CalcReason for more information
486 : // Here we don't directly use gComputeLC, which is also 0 and 1, because in freeSpeed we have another call (update = 2),
487 : // which is needed to differentiate between the different cases/calculations/needed output/saved variables
488 : int update = 1;
489 : CalcReason _vUsage = usage;
490 14755171 : if (MSGlobals::gComputeLC) {
491 : _vUsage = CalcReason::LANE_CHANGE;
492 : }
493 14755171 : if (_vUsage == CalcReason::LANE_CHANGE || _vUsage == CalcReason::FUTURE || usage == CalcReason::CURRENT_WAIT) {
494 : update = 0;
495 : }
496 :
497 : #ifdef DEBUG_V
498 : if (veh->isSelected()) {
499 : std::cout << SIMTIME
500 : << " EIDM::stopSpeed "
501 : << " veh=" << veh->getID()
502 : << " speed=" << speed
503 : << " gap=" << gap
504 : << " vars->v0_int=" << vars->v0_int
505 : << " update=" << update
506 : << "\n";
507 : }
508 : #endif
509 :
510 14755171 : double result = _v(veh, gap, speed, 0, vars->v0_int, false, update, _vUsage);
511 : // From Sumo_IDM-implementation:
512 : // if (gap > 0 && speed < NUMERICAL_EPS && result < NUMERICAL_EPS) {
513 : // // ensure that stops can be reached:
514 : // result = maximumSafeStopSpeed(gap, decel, speed, false, veh->getActionStepLengthSecs());
515 : // }
516 14755171 : return result;
517 : }
518 :
519 : double
520 16 : MSCFModel_EIDM::freeSpeed(const double currentSpeed, const double decel, const double dist, const double targetSpeed, const bool onInsertion) {
521 : // XXX: (Leo) This seems to be exclusively called with decel = myDecel (max deceleration) and is not overridden
522 : // by any specific CFModel. That may cause undesirable hard braking (at junctions where the vehicle
523 : // changes to a road with a lower speed limit).
524 :
525 16 : if (MSGlobals::gSemiImplicitEulerUpdate) {
526 : // adapt speed to succeeding lane, no reaction time is involved
527 : // when breaking for y steps the following distance g is covered
528 : // (drive with v in the final step)
529 : // g = (y^2 + y) * 0.5 * b + y * v
530 : // y = ((((sqrt((b + 2.0*v)*(b + 2.0*v) + 8.0*b*g)) - b)*0.5 - v)/b)
531 16 : const double v = SPEED2DIST(targetSpeed);
532 16 : if (dist < v) {
533 : return targetSpeed;
534 : }
535 0 : const double b = ACCEL2DIST(decel);
536 0 : const double y = MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
537 0 : const double yFull = floor(y);
538 0 : const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
539 0 : const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
540 0 : return DIST2SPEED(MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
541 : } else {
542 : // ballistic update (Leo)
543 : // calculate maximum next speed vN that is adjustable to vT=targetSpeed after a distance d=dist
544 : // and given a maximal deceleration b=decel, denote the current speed by v0.
545 : // the distance covered by a trajectory that attains vN in the next timestep and decelerates afterwards
546 : // with b is given as
547 : // d = 0.5*dt*(v0+vN) + (t-dt)*vN - 0.5*b*(t-dt)^2, (1)
548 : // where time t of arrival at d with speed vT is
549 : // t = dt + (vN-vT)/b. (2)
550 : // We insert (2) into (1) to obtain
551 : // d = 0.5*dt*(v0+vN) + vN*(vN-vT)/b - 0.5*b*((vN-vT)/b)^2
552 : // 0 = (dt*b*v0 - vT*vT - 2*b*d) + dt*b*vN + vN*vN
553 : // and solve for vN
554 :
555 : assert(currentSpeed >= 0);
556 : assert(targetSpeed >= 0);
557 :
558 0 : const double dt = onInsertion ? 0 : TS; // handles case that vehicle is inserted just now (at the end of move)
559 : const double v0 = currentSpeed;
560 : const double vT = targetSpeed;
561 : const double b = decel;
562 0 : const double d = dist - NUMERICAL_EPS; // prevent returning a value > targetSpeed due to rounding errors
563 :
564 : // Solvability for positive vN (if d is small relative to v0):
565 : // 1) If 0.5*(v0+vT)*dt > d, we set vN=vT.
566 : // (In case vT<v0, this implies that on the interpolated trajectory there are points beyond d where
567 : // the interpolated velocity is larger than vT, but at least on the temporal discretization grid, vT is not exceeded)
568 : // 2) We ignore the (possible) constraint vN >= v0 - b*dt, which could lead to a problem if v0 - t*b > vT.
569 : // (finalizeSpeed() is responsible for assuring that the next velocity is chosen in accordance with maximal decelerations)
570 :
571 0 : if (0.5 * (v0 + vT)*dt >= d) {
572 : return vT; // (#)
573 : }
574 :
575 0 : const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
576 0 : const double p = 0.5 * b * dt;
577 0 : return -p + sqrt(p * p - q);
578 : }
579 : }
580 :
581 : double
582 9828868 : MSCFModel_EIDM::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion, const CalcReason usage) const {
583 :
584 : // @ToDo: Set new internal speed limit/desired speed <maxSpeed> here and change it over time in internalspeedlimit()!
585 :
586 9828868 : if (maxSpeed < 0.) {
587 : // can occur for ballistic update (in context of driving at red light)
588 : return maxSpeed;
589 : }
590 :
591 : // This update-variable is used to check what called freeSpeed (LC- or CF-model), see enum CalcReason for more information
592 : // Here we don't directly use gComputeLC, which is also 0 and 1, because we have another call (update = 2),
593 : // which is needed to differentiate between the different cases/calculations/needed output/saved variables
594 : int update = 1;
595 : CalcReason _vUsage = usage;
596 9828868 : if (MSGlobals::gComputeLC) {
597 : _vUsage = CalcReason::LANE_CHANGE;
598 : }
599 9828868 : if (_vUsage == CalcReason::LANE_CHANGE || _vUsage == CalcReason::FUTURE) {
600 : update = 0;
601 : }
602 :
603 : #ifdef DEBUG_V
604 : if (veh->isSelected()) {
605 : std::cout << SIMTIME
606 : << " EIDM::freeSpeed "
607 : << " veh=" << veh->getID()
608 : << " speed=" << speed
609 : << " seen=" << seen
610 : << " maxSpeed=" << maxSpeed
611 : << " update=" << update
612 : << " onInsertion=" << onInsertion
613 : << "\n";
614 : }
615 : #endif
616 :
617 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
618 :
619 : double vSafe, remaining_time, targetDecel;
620 : double secGap;
621 9828868 : if (onInsertion) {
622 : // Needed for the Insertion-Calculation to check, if insertion at this "speed" is possible to reach "maxSpeed" in the given distance "seen" (vehicle can max decelerate with myDecel!)!
623 : // @ToDo: Could maybe be changed to maximumSafeFollowSpeed instead of freeSpeed-Krauss calculation!
624 16 : vSafe = freeSpeed(speed, myDecel, seen, maxSpeed, onInsertion);
625 : } else {
626 : // driver needs to brake, because he is faster than the desired speed limit <maxSpeed> on the next lane or the next upcoming event (e.g. red light violation)
627 : // The adaption/braking starts when the <seen> time-distance is lower than myTpreview+myTreaction
628 9828852 : if (maxSpeed < speed && seen < speed * (myTpreview + myTreaction)) {
629 :
630 23258 : update = update == 0 ? 0 : 2;
631 :
632 23258 : remaining_time = MAX3((seen - speed * myTreaction) / speed, myTreaction / 2, TS); // The remaining time is at least a time step or the reaction time of the driver
633 23258 : targetDecel = (speed - maxSpeed) / remaining_time; // The needed constant deceleration to reach maxSpeed before reaching the next lane/event
634 :
635 : // targetDecel is not set immediatly, if the vehicle is far enough away from the event (bounded by myJerkmax)
636 23258 : if (remaining_time > myTpreview - targetDecel / myJerkmax) {
637 55 : targetDecel = (myTpreview - remaining_time) * myJerkmax;
638 : }
639 :
640 : // calculate distance which would result in the accel-value targetDecel at this <speed> and leaderspeed <0>
641 23258 : if (vars->myap_update == 0 || update == 0) { // at action point update
642 21548 : secGap = internalsecuregap(veh, speed, 0., targetDecel);
643 : } else { // between two action points
644 1710 : secGap = internalsecuregap(veh, vars->myv_est + vars->lastrealacc * vars->myap_update * TS, 0., targetDecel);
645 : }
646 :
647 43316 : vSafe = _v(veh, MAX2(seen, secGap), speed, 0., vars->v0_int, true, update, _vUsage);
648 :
649 : // Add this for "old implementation" when vehicle doesn't HAVE to reach maxspeed at seen-distance!
650 : // @ToDo: See #7644: <double v = MIN2(maxV, laneMaxV);> in MSVehicle::planMoveInternal! -> DONE!
651 : // But still: Is it better, if the vehicle brakes early enough to reach the next lane with its speed limit?
652 : // Instead of driving too fast for a while on the new lane, which can be more "human", but leads to other problems (junction model, traffic light braking...)
653 : /* if (seen < speed*myTpreview || seen < veh->getLane()->getVehicleMaxSpeed(veh)*myTpreview / 2) {
654 : remaining_time = speed < veh->getLane()->getVehicleMaxSpeed(veh) / 2 ? seen / (veh->getLane()->getVehicleMaxSpeed(veh) / 2) : seen / MAX2(speed, 0.01);
655 : if (vars->v0_int > maxSpeed + NUMERICAL_EPS && vars->v0_old > vars->v0_int + NUMERICAL_EPS) {
656 : maxSpeed = MAX2(maxSpeed, MIN2(vars->v0_int, vars->v0_old - (vars->v0_old - maxSpeed) / myTpreview * (myTpreview - remaining_time)));
657 : vSafe = _v(veh, 500., speed, maxSpeed, maxSpeed, true, update, _vUsage);
658 : } else if (vars->v0_int > maxSpeed + NUMERICAL_EPS) {
659 : maxSpeed = MAX2(maxSpeed, vars->v0_int - (vars->v0_int - maxSpeed) / myTpreview * (myTpreview - remaining_time));
660 : vSafe = _v(veh, 500., speed, maxSpeed, maxSpeed, true, update, _vUsage);
661 : } else {
662 : vSafe = _v(veh, 500., speed, maxSpeed, vars->v0_int, true, update, _vUsage);
663 : }
664 : */
665 : } else { // when the <speed> is lower than <maxSpeed> or the next lane/event is not seen with myTpreview+myTreaction yet
666 9805594 : vSafe = _v(veh, 500., speed, maxSpeed, vars->v0_int, true, update, _vUsage);
667 : }
668 : }
669 :
670 : return vSafe;
671 : }
672 :
673 : /// @todo update interactionGap logic to IDM
674 : double
675 0 : MSCFModel_EIDM::interactionGap(const MSVehicle* const veh, double vL) const {
676 : // Resolve the IDM equation to gap. Assume predecessor has
677 : // speed != 0 and that vsafe will be the current speed plus acceleration,
678 : // i.e that with this gap there will be no interaction.
679 0 : const double acc = myAccel * (1. - pow(veh->getSpeed() / veh->getLane()->getVehicleMaxSpeed(veh), myDelta));
680 0 : const double vNext = veh->getSpeed() + acc;
681 0 : const double gap = (vNext - vL) * (veh->getSpeed() + vL) / (2 * myDecel) + vL;
682 :
683 : // Don't allow timeHeadWay < deltaT situations.
684 0 : return MAX2(gap, SPEED2DIST(vNext));
685 :
686 : // Only needed for old Lane Change Model????
687 : }
688 :
689 : double
690 33498626 : MSCFModel_EIDM::getSecureGap(const MSVehicle* const /*veh*/, const MSVehicle* const /*pred*/, const double speed, const double leaderSpeed, const double /*leaderMaxDecel*/) const {
691 : // SecureGap depends on v0 and may be higher than just looking at s* (In case of the IDM)
692 : //VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
693 33498626 : const double delta_v = speed - leaderSpeed;
694 33498626 : double s = MAX2(0.0, speed * myHeadwayTime + speed * delta_v / myTwoSqrtAccelDecel); // is calculated without MinGap because it is compared to a gap without MinGap!
695 : // For the IDM: - pow(speed / veh->getLane()->getVehicleMaxSpeed(veh), myDelta)) must be added to (myDecel / myAccel + 1)!
696 : // For the IIDM: Left out the case check for estSpeed > v0, assuming this is not needed here. The vehicle therefore may brake harder when newSpeed > v0 occurs!
697 : // The secure gap is calculated using -myDecel as secure maximal acceleration (using myDecel/myAccel)!
698 :
699 33498626 : double erg = sqrt((s * s) / (myDecel / myAccel + 1.0));
700 33498626 : return MIN2(s, erg);
701 : }
702 :
703 : // Only needed when vehicle has to reach laneMaxV before entering the new lane, see #7644
704 : double
705 23258 : MSCFModel_EIDM::internalsecuregap(const MSVehicle* const veh, const double speed, const double leaderSpeed, const double targetDecel) const {
706 : // SecureGap depends on v0 and may be higher than just looking at s* (In case of the IDM)
707 : // internalsecuregap uses a targetDecel instead of myDecel!
708 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
709 23258 : const double delta_v = speed - leaderSpeed;
710 23258 : double s = MAX2(0.0, speed * myHeadwayTime + speed * delta_v / myTwoSqrtAccelDecel); // is calculated without MinGap because it is compared to a gap without MinGap!
711 : // For the IDM: - pow(speed / veh->getLane()->getVehicleMaxSpeed(veh), myDelta)) must be added to (myDecel / myAccel + 1)!
712 : // the secure gap is calculated using -myDecel as secure maximal acceleration (using myDecel/myAccel)!
713 :
714 : double erg;
715 23258 : if (speed <= vars->v0_int) {
716 309 : erg = sqrt((s * s) / (MAX2(targetDecel / myAccel + 1.0, 1.0)));
717 : } else { // speed > v0
718 22949 : double a_free = - myDecel * (1.0 - pow(vars->v0_int / speed, myAccel * myDelta / myDecel));
719 33398 : erg = sqrt((s * s) / (MAX2(targetDecel / myAccel + 1.0 + a_free / myAccel, 1.0)));
720 : }
721 :
722 23258 : return erg;
723 : }
724 :
725 : void
726 5752359 : MSCFModel_EIDM::internalspeedlimit(MSVehicle* const veh, const double oldV) const {
727 :
728 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
729 :
730 : double v_limcurr, v_limprev;
731 5752359 : v_limcurr = vars->v0_int; // model internal desired speed limit
732 5752359 : v_limprev = vars->v0_old; // previous desired speed limit for calculation reasons
733 :
734 5752359 : const MSLane* lane = veh->getLane();
735 5752359 : const std::vector<MSLane*>& bestLaneConts = veh->getBestLanesContinuation();
736 : int view = 1;
737 5752359 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*veh, view, *lane, bestLaneConts);
738 5752359 : double seen = lane->getLength() - veh->getPositionOnLane();
739 5752359 : double v0 = lane->getVehicleMaxSpeed(veh); // current desired lane speed
740 :
741 : // @ToDo: nextTurn is only defined in sublane-model calculation?!
742 : // @ToDo: So cannot use it yet, but the next turn or speed recommendation for the street curvature (e.g. vmax=sqrt(a_transverse*Radius), a_transverse=3-4m/s^2)
743 : // @ToDo: should not come from here, but from MSVehicle/the street network
744 : // const std::pair<double, LinkDirection> nextTurn = veh->getNextTurn();
745 :
746 : // When driving on the last lane/link, the vehicle shouldn't adapt to the lane after anymore.
747 : // Otherwise we check the <seen> time-distance and whether is lower than myTpreview
748 5752359 : if (lane->isLinkEnd(link) != 1 && (seen < oldV * myTpreview || seen < v0 * myTpreview / 2)) {
749 : double speedlim = 200;
750 : while (true) { // loop over all upcoming edges/lanes/links until the <seen> time-distance is higher than myTpreview
751 2019433 : if (lane->isLinkEnd(link) != 1 && (seen < oldV * myTpreview || seen < v0 * myTpreview / 2)) { // @ToDo: add && (*link)->havePriority()???
752 : // @ToDo: When vehicles already brake because of a minor link, it may not be necessary to adapt the internal desired speed when turning...
753 : // @ToDo: The vehicle brakes anyway and it may happen, that is brakes too hard because of the low internal desired speed and takes too long
754 : // @ToDo: to accelerate again, because the internal desired speed must rise again?!
755 : // @ToDo: It can't be done via (*link)->havePriority() (vehicle will not brake for other vehicles, so it needs to brake for curve radius),
756 : // @ToDo: because then turning at traffic lights or somewhere else might be missing (traffic light links don't have priority definition?!)
757 1321906 : LinkDirection dir = (*link)->getDirection();
758 1321906 : switch (dir) {
759 : case LinkDirection::NODIR:
760 : break;
761 : case LinkDirection::STRAIGHT:
762 : break;
763 73587 : case LinkDirection::TURN:
764 : speedlim = 4;
765 73587 : break;
766 0 : case LinkDirection::TURN_LEFTHAND:
767 : speedlim = 4;
768 0 : break;
769 58559 : case LinkDirection::LEFT:
770 : speedlim = 8;
771 58559 : break;
772 43597 : case LinkDirection::RIGHT:
773 : speedlim = 6;
774 43597 : break;
775 0 : case LinkDirection::PARTLEFT:
776 : speedlim = 12;
777 0 : break;
778 0 : case LinkDirection::PARTRIGHT:
779 : speedlim = 12;
780 0 : break;
781 : }
782 :
783 1321906 : if (v0 > speedlim * veh->getChosenSpeedFactor() + NUMERICAL_EPS) {
784 117929 : v0 = speedlim * veh->getChosenSpeedFactor();
785 : }
786 : } else {
787 : break;
788 : }
789 1321906 : if ((*link)->getViaLane() == nullptr) {
790 691347 : ++view;
791 : }
792 : lane = (*link)->getViaLaneOrLane();
793 : // @ToDo: Previously: (seen < oldV*myTpreview / 2 || seen < v0*myTpreview / 4)! Changed freeSpeed, so also changed v0-calculation here.
794 : // @ToDo: Vehicle now decelerates to new Speedlimit before reaching new edge (not /2 anymore)!
795 : // @ToDo: v0 for changing speed limits when seen < oldV*myTpreview, not seen < oldV*myTpreview/2 anymore!!!
796 1321906 : if (v0 > lane->getVehicleMaxSpeed(veh)) {
797 0 : v0 = lane->getVehicleMaxSpeed(veh);
798 : }
799 1321906 : seen += lane->getLength();
800 1321906 : link = MSLane::succLinkSec(*veh, view, *lane, bestLaneConts);
801 : }
802 :
803 697527 : if (!(v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) || // if v_limprev!=v0, then the upcoming v0 is different, than the old desired v_limprev and therefore v0_int must change slowly to the new v0
804 637891 : (v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS && !(v_limprev < v_limcurr + NUMERICAL_EPS && v_limprev > v_limcurr - NUMERICAL_EPS))) { // When v_limprev==v0, but v_limprev!=v_limcurr, then we may have a special case and need to slowly change v_limcurr to v0
805 :
806 59640 : if ((v_limcurr < v_limprev + NUMERICAL_EPS && v_limcurr < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) || // important when v_limcurr < v0 < v_limprev --> v_limcurr was decreasing, but needs to suddenly increase again
807 59640 : (v_limcurr > v_limprev - NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS && v_limprev < v0 + NUMERICAL_EPS)) { // important when v_limcurr > v0 > v_limprev --> v_limcurr was increasing, but needs to suddenly decrease again
808 4 : vars->v0_old = v_limcurr;
809 : } else {
810 59636 : if (v_limcurr >= v0 - NUMERICAL_EPS) { // v_limcurr is too high and needs to decrease
811 59636 : v_limcurr = MAX2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
812 : } else { // v_limcurr is too low and needs to increase
813 0 : v_limcurr = MIN2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
814 : }
815 : }
816 :
817 : // when v_limcurr reaches v0, then update v_limprev=v0
818 59640 : if (v_limcurr < v0 + NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS) {
819 1165 : vars->v0_old = v0;
820 1165 : vars->v0_int = v0;
821 : } else { // else just update the internal desired speed with v_limcurr
822 58475 : vars->v0_int = v_limcurr;
823 : }
824 : }
825 :
826 5054832 : } else if (!(v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) || // if v_limprev!=v0, then the upcoming v0 is different, than the old desired v_limprev and therefore v0_int must change slowly to the new v0
827 4994216 : (v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS && !(v_limprev < v_limcurr + NUMERICAL_EPS && v_limprev > v_limcurr - NUMERICAL_EPS))) { // When v_limprev==v0, but v_limprev!=v_limcurr, then we may have a special case and need to slowly change v_limcurr to v0
828 :
829 60959 : if ((v_limcurr < v_limprev + NUMERICAL_EPS && v_limcurr < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) || // important when v_limcurr < v0 < v_limprev --> v_limcurr was decreasing, but needs to suddenly increase again
830 60616 : (v_limcurr > v_limprev - NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS && v_limprev < v0 + NUMERICAL_EPS)) { // important when v_limcurr > v0 > v_limprev --> v_limcurr was increasing, but needs to suddenly decrease again
831 343 : vars->v0_old = v_limcurr;
832 : } else {
833 60616 : if (v_limcurr >= v0 - NUMERICAL_EPS) { // v_limcurr is too high and needs to decrease
834 0 : v_limcurr = MAX2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
835 : } else { // v_limcurr is too low and needs to increase
836 60616 : v_limcurr = MIN2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
837 : }
838 : }
839 :
840 : // when v_limcurr reaches v0, then update v_limprev=v0
841 60959 : if (v_limcurr < v0 + NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS) {
842 1508 : vars->v0_old = v0;
843 1508 : vars->v0_int = v0;
844 : } else { // else just update the internal desired speed with v_limcurr
845 59451 : vars->v0_int = v_limcurr;
846 : }
847 : }
848 5752359 : }
849 :
850 : double
851 57630516 : MSCFModel_EIDM::_v(const MSVehicle* const veh, const double gap2pred, const double egoSpeed,
852 : const double predSpeed, const double desSpeed, const bool respectMinGap, const int update, const CalcReason usage) const {
853 :
854 : double v0 = MAX2(NUMERICAL_EPS, desSpeed);
855 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
856 :
857 : // @ToDo: Where to put such an insertion function/update, which only needs to be calculated once at the first step?????!
858 : // For the first iteration
859 57630516 : if (vars->v0_old == 0) {
860 : vars = (VehicleVariables*)veh->getCarFollowVariables();
861 1737 : vars->v0_old = MAX2(NUMERICAL_EPS, veh->getLane()->getVehicleMaxSpeed(veh));
862 1737 : vars->v0_int = MAX2(NUMERICAL_EPS, veh->getLane()->getVehicleMaxSpeed(veh));
863 1737 : v0 = MAX2(NUMERICAL_EPS, veh->getLane()->getVehicleMaxSpeed(veh));
864 : }
865 :
866 : double wantedacc = 0., a_free;
867 : double wouldacc = 0., woulds, woulda_free;
868 :
869 : double estSpeed, estleaderSpeed, estGap;
870 : double current_estSpeed, current_estGap, current_estleaderSpeed;
871 : double current_gap;
872 : double acc = 0.;
873 : double a_leader = NUMERICAL_EPS; // Default without a leader, should not be 0!
874 : double newSpeed = egoSpeed;
875 : bool lastrespectMinGap = respectMinGap;
876 57630516 : const double minGapStop_EPS = 0.05 + 0.20 * MAX2(0.25, myAccel);
877 :
878 : // When doing the Follow-Calculation in adapttoLeader (MSVehicle.cpp) the mingap gets subtracted from the current gap (maybe this is needed for the Krauss-Model!).
879 : // For the IDM this Mingap is needed or else the vehicle will stop at two times mingap behind the leader!
880 57630516 : if (respectMinGap) {
881 85663024 : current_gap = MAX2(NUMERICAL_EPS, gap2pred + MAX2(NUMERICAL_EPS, myType->getMinGap() - 0.25)); // 0.25m tolerance because of reaction time and estimated variables
882 : } else {
883 : // gap2pred may go to 0 when offset is reached (e.g. 1m Offset -> gap2pred=0, when vehicle stands at 0.95m, gap2pred is still 0 and does not become -0.05m (negative)!)
884 14799004 : current_gap = MAX2(NUMERICAL_EPS, gap2pred + minGapStop_EPS);
885 : }
886 :
887 : double newGap = current_gap;
888 :
889 231552152 : for (int i = 0; i < myIterations; i++) {
890 :
891 : // Using Action-Point reaction time: update the variables, when myap_update is zero and update is 1
892 : current_estSpeed = newSpeed;
893 173921640 : if (respectMinGap) {
894 130767953 : current_estleaderSpeed = MAX2(predSpeed - newGap * mySigmaleader * vars->myw_speed, 0.0); // estimated variable with Wiener Prozess
895 : } else {
896 : // @ToDo: Use this??? driver would always know, that junctions, traffic lights, etc. have v=0!
897 : // @ToDo: With estimated variables predSpeed > 0 is possible! > 0 may result in oscillating
898 : current_estleaderSpeed = predSpeed;
899 : }
900 173921640 : if (update == 2) { // For freeSpeed
901 : current_estGap = newGap; // not-estimated variable
902 : } else {
903 173835115 : if (respectMinGap) {
904 130681428 : current_estGap = newGap * exp(mySigmagap * vars->myw_gap); // estimated variable with Wiener Prozess
905 : } else {
906 81421490 : current_estGap = newGap * exp(mySigmagap * vars->myw_gap * MIN2(current_estSpeed / EST_REAC_THRESHOLD, 1.0)); // estimated variable with Wiener Prozess
907 : }
908 : }
909 :
910 173921640 : if (vars->myap_update == 0 && usage == CalcReason::CURRENT) { // update variables with current observation
911 : estSpeed = current_estSpeed;
912 : estleaderSpeed = current_estleaderSpeed; // estimated variable with Wiener Prozess
913 : estGap = current_estGap; // estimated variable with Wiener Prozess
914 94942388 : } else if (usage == CalcReason::CURRENT) { // use stored variables (reaction time)
915 7114128 : estSpeed = MAX2(vars->myv_est + vars->lastrealacc * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations), 0.0);
916 : // estSpeed = vars->myv_est;
917 7114128 : if (update == 2) { // For freeSpeed
918 : estGap = newGap; // not-estimated variable
919 1710 : estleaderSpeed = MAX2(vars->myv_est_l + vars->lastleaderacc * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations) - vars->mys_est * mySigmaleader * vars->myw_speed, 0.0);
920 : // estleaderSpeed = MAX2(vars->myv_est_l - vars->mys_est * mySigmaleader*vars->myw_speed, 0.0);
921 : } else {
922 7112418 : lastrespectMinGap = vars->myrespectMinGap;
923 7112418 : if (lastrespectMinGap) {
924 6773850 : estleaderSpeed = MAX2(vars->myv_est_l + vars->lastleaderacc * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations) - vars->mys_est * mySigmaleader * vars->myw_speed, 0.0);
925 : // estleaderSpeed = MAX2(vars->myv_est_l - vars->mys_est * mySigmaleader*vars->myw_speed, 0.0);
926 6773850 : estGap = vars->mys_est * exp(mySigmagap * vars->myw_gap) - ((vars->myv_est + estSpeed) / 2. - (vars->myv_est_l + estleaderSpeed) / 2.) * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations); // estimated variable with Wiener Prozess
927 : } else {
928 : // @ToDo: Use this??? driver would always know, that junctions, traffic lights, etc. have v=0!
929 : // @ToDo: With estimated variables predSpeed > 0 is possible! > 0 may result in oscillating
930 338568 : estleaderSpeed = vars->myv_est_l;
931 484091 : estGap = vars->mys_est * exp(mySigmagap * vars->myw_gap * MIN2(current_estSpeed / EST_REAC_THRESHOLD, 1.0)) - ((vars->myv_est + estSpeed) / 2. - (vars->myv_est_l + estleaderSpeed) / 2.) * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations); // estimated variable with Wiener Prozess
932 : }
933 : }
934 : } else { // use actual variables without reaction time
935 : estSpeed = current_estSpeed;
936 : estleaderSpeed = current_estleaderSpeed; // estimated variable with Wiener Prozess
937 : estGap = current_estGap; // estimated variable with Wiener Prozess
938 : }
939 :
940 : // ToDo: The headway can change for IDMM based on the scenario, should something like that also be integrated here???
941 173921640 : double headwayTime = myHeadwayTime;
942 : MSVehicle* leader;
943 :
944 173921640 : double s = MAX2(0., estSpeed * headwayTime + estSpeed * (estSpeed - estleaderSpeed) / myTwoSqrtAccelDecel);
945 173921640 : if (lastrespectMinGap) {
946 130569501 : s += myType->getMinGap() + EIDM_POS_ACC_EPS;
947 : } else {
948 43352139 : s += minGapStop_EPS + EIDM_POS_ACC_EPS;
949 : }
950 :
951 : // Because of the reaction time and estimated variables, s* can become lower than gap when the vehicle needs to brake/is braking, that results in the vehicle accelerating again...
952 : // Here: When the gap is very small, s* is influenced to then always be bigger than the gap. With this there are no oscillations in accel at small gaps!
953 173921640 : if (lastrespectMinGap) {
954 : // The allowed position error when coming to a stop behind a leader is higher with higher timesteps (approx. 0.5m at 1.0s timstep, 0.1m at 0.1s)
955 130569501 : if (estGap < myType->getMinGap() + (TS * 10 + 1) * EIDM_POS_ACC_EPS && estSpeed < EST_REAC_THRESHOLD && s < estGap * sqrt(1 + 2 * EIDM_POS_ACC_EPS / myAccel)) {
956 3926102 : s = estGap * sqrt(1 + 2 * EIDM_POS_ACC_EPS / myAccel);
957 : }
958 : } else {
959 43352139 : if (estGap < minGapStop_EPS + 2 * EIDM_POS_ACC_EPS && s < estGap * sqrt(1 + EIDM_POS_ACC_EPS / myAccel)) {
960 : // when the vehicle wants to stop (stopSpeed), it may take long to come to a full stop
961 : // To lower this stop time, we restrict the deceleration to always be higher than 0.05m/s^2 when stopping
962 120397 : s = estGap * sqrt(1 + EIDM_POS_ACC_EPS / myAccel);
963 : }
964 : }
965 :
966 : // IDM calculation:
967 : // wantedacc = myAccel * (1. - pow(estSpeed / v0, myDelta) - (s * s) / (estGap * estGap));
968 :
969 : // IIDM calculation -NOT- from the original Treiber/Kesting publication:
970 : // With the saved variables from the last Action Point
971 : /*double wantedacc;
972 : double a_free = myAccel * (1. - pow(estSpeed / v0, myDelta));
973 : if (s >= estGap) { // This is the IIDM
974 : wantedacc = myAccel * (1. - (s * s) / (estGap * estGap));
975 : } else {
976 : wantedacc = a_free * (1. - pow(s / estGap, 2*myAccel / fabs(a_free)));
977 : }*/ // Old calculation form without the distinction between v > v0 and v <= v0!!! Published it in the EIDM with this form, but may be worse than original IIDM!
978 :
979 : // IIDM calculation from the original Treiber/Kesting publication:
980 : // With the saved variables from the last Action Point
981 173921640 : if (estSpeed <= v0) {
982 169238310 : a_free = myAccel * (1. - pow(estSpeed / v0, myDelta));
983 169238310 : if (s >= estGap) {
984 11490766 : wantedacc = myAccel * (1. - (s * s) / (estGap * estGap));
985 : } else {
986 157747544 : wantedacc = a_free * (1. - pow(s / estGap, 2 * myAccel / a_free));
987 : }
988 : } else { // estSpeed > v0
989 4683330 : a_free = - myDecel * (1. - pow(v0 / estSpeed, myAccel * myDelta / myDecel));
990 4683330 : if (s >= estGap) {
991 282342 : wantedacc = a_free + myAccel * (1. - (s * s) / (estGap * estGap));
992 : } else {
993 : wantedacc = a_free;
994 : }
995 : }
996 173921640 : wantedacc = SPEED2ACCEL(MAX2(0.0, estSpeed + ACCEL2SPEED(wantedacc)) - estSpeed);
997 :
998 : // IIDM calculation from the original Treiber/Kesting publication:
999 : // With the current variables (what would the acceleration be without reaction time)
1000 173921640 : if (usage == CalcReason::CURRENT) {
1001 86093380 : woulds = MAX2(0., current_estSpeed * headwayTime + current_estSpeed * (current_estSpeed - current_estleaderSpeed) / myTwoSqrtAccelDecel); // s_soll
1002 86093380 : if (respectMinGap) {
1003 85904319 : woulds += myType->getMinGap() + EIDM_POS_ACC_EPS; // when behind a vehicle use MinGap and when at a junction then not????
1004 : } else {
1005 189061 : woulds += minGapStop_EPS + EIDM_POS_ACC_EPS;
1006 : }
1007 :
1008 86093380 : if (current_estSpeed <= v0) {
1009 83121216 : woulda_free = myAccel * (1. - pow(current_estSpeed / v0, myDelta));
1010 83121216 : if (woulds >= current_estGap) {
1011 1800234 : wouldacc = myAccel * (1. - (woulds * woulds) / (current_estGap * current_estGap));
1012 : } else {
1013 81320982 : wouldacc = woulda_free * (1. - pow(woulds / current_estGap, 2 * myAccel / woulda_free));
1014 : }
1015 : } else { // current_estSpeed > v0
1016 2972164 : woulda_free = - myDecel * (1. - pow(v0 / current_estSpeed, myAccel * myDelta / myDecel));
1017 2972164 : if (woulds >= current_estGap) {
1018 68342 : wouldacc = woulda_free + myAccel * (1. - (woulds * woulds) / (current_estGap * current_estGap));
1019 : } else {
1020 : wouldacc = woulda_free;
1021 : }
1022 : }
1023 171749869 : wouldacc = SPEED2ACCEL(MAX2(0.0, current_estSpeed + ACCEL2SPEED(wouldacc)) - current_estSpeed);
1024 : }
1025 :
1026 : // @ToDo: calc_gap is just estGap here, used to have an extra calc_gap calculation (like jmax), but doesn't work well here with the junction calculation:
1027 : // @ToDo: The driver would slowly start accelerating when he thinks the junction is clear, but may still decelerate for a bit and not jump to acceleration.
1028 : // @ToDo: This causes the driver not to drive over the junction because he thinks he won't make it in time before a foe may appear!
1029 :
1030 : // IIDM calculation -NOT- from the original Treiber/Kesting publication:
1031 : // Resulting acceleration also with the correct drive off term.
1032 : double calc_gap = estGap;
1033 : /*a_free = myAccel * (1. - pow(estSpeed / v0, myDelta));
1034 : if (s >= calc_gap) { // This is the IIDM
1035 : acc = myAccel * (1. - (s * s) / (calc_gap * calc_gap));
1036 : } else {
1037 : acc = a_free * (1. - pow(s / calc_gap, 2*myAccel / fabs(a_free)));
1038 : } */ // Old calculation form without the distinction between v > v0 and v <= v0!!! Published it in the EIDM with this form, but may be worse than original IIDM!
1039 :
1040 : // IDM calculation:
1041 : // acc = myAccel * (1. - pow(estSpeed / v0, myDelta) - (s * s) / (calc_gap * calc_gap));
1042 :
1043 : // IIDM calculation from the original Treiber/Kesting publication:
1044 : // Resulting acceleration also with the correct drive off term.
1045 173921640 : if (estSpeed <= v0) {
1046 169238310 : a_free = myAccel * (1. - pow(estSpeed / v0, myDelta));
1047 169238310 : if (s >= calc_gap) {
1048 11490766 : acc = myAccel * (1. - (s * s) / (calc_gap * calc_gap));
1049 : } else {
1050 157747544 : acc = a_free * (1. - pow(s / calc_gap, 2 * myAccel / a_free));
1051 : }
1052 : } else { // estSpeed > v0
1053 4683330 : a_free = - myDecel * (1. - pow(v0 / estSpeed, myAccel * myDelta / myDecel));
1054 4683330 : if (s >= calc_gap) {
1055 282342 : acc = a_free + myAccel * (1. - (s * s) / (calc_gap * calc_gap));
1056 : } else {
1057 : acc = a_free;
1058 : }
1059 : }
1060 :
1061 : double a_cah;
1062 : // Coolness from Enhanced Intelligent Driver Model, when gap "jump" to a smaller gap accurs
1063 : // @ToDo: Maybe without usage == CalcReason::CURRENT??? To let all calculations profit from Coolness??? (e.g. also lane change calculation)
1064 173921640 : if (vars->minaccel > wantedacc - NUMERICAL_EPS && usage == CalcReason::CURRENT) {
1065 :
1066 76226065 : leader = (MSVehicle*)veh->getLeader(estGap + 25).first;
1067 76226061 : if (leader != nullptr && lastrespectMinGap && estleaderSpeed >= SPEED_EPS) {
1068 72596916 : a_leader = MIN2(leader->getAcceleration(), myAccel);
1069 : // Change a_leader to lower values when far away from leader or else far away leaders influence the ego-vehicle!
1070 72596916 : if (estGap > s * 3 / 2) { // maybe estGap > 2*s???
1071 56073527 : a_leader = a_leader * (s * 3 / 2) / estGap;
1072 : }
1073 : }
1074 :
1075 : // speed of the leader shouldnt become zero, because then problems with the calculation occur
1076 76226061 : if (estleaderSpeed < SPEED_EPS) {
1077 : estleaderSpeed = SPEED_EPS;
1078 : }
1079 :
1080 76226061 : if (vars->t_off + myTaccmax + NUMERICAL_EPS < (SIMTIME - TS * (myIterations - i - 1.) / myIterations) && egoSpeed > SUMO_const_haltingSpeed) {
1081 :
1082 : // Enhanced Intelligent Driver Model
1083 72006099 : if (estleaderSpeed * (estSpeed - estleaderSpeed) <= -2 * estGap * a_leader) {
1084 45630457 : a_cah = (estSpeed * estSpeed * a_leader) / (estleaderSpeed * estleaderSpeed - 2 * estGap * a_leader);
1085 : } else {
1086 26375642 : if (estSpeed - estleaderSpeed >= 0) {
1087 21575995 : a_cah = a_leader - ((estSpeed - estleaderSpeed) * (estSpeed - estleaderSpeed)) / (2 * estGap);
1088 : } else {
1089 : a_cah = a_leader;
1090 : }
1091 : }
1092 :
1093 72006099 : if (acc >= a_cah) {
1094 : // do nothing, meaning acc = acc_IDM;
1095 : } else {
1096 4932824 : acc = (1 - myCcoolness) * acc + myCcoolness * (a_cah + myDecel * tanh((acc - a_cah) / myDecel));
1097 : }
1098 : }
1099 : }
1100 :
1101 173921636 : newSpeed = MAX2(0.0, current_estSpeed + ACCEL2SPEED(acc) / myIterations);
1102 :
1103 : // Euler Update as future gap prediction, this will be the "real" gap with this timestep and speed calculation
1104 : // Although this is the correct gap prediction, the calculation is unstable with this method
1105 : //newGap = MAX2(NUMERICAL_EPS, current_gap - SPEED2DIST(newSpeed - predSpeed) * ((i + 1.) / myIterations));
1106 :
1107 : // Ballistic Update as future gap prediction, this will be the "real" gap with this timestep and speed calculation
1108 : // Although this is the correct gap prediction, the calculation is unstable with this method
1109 : //newGap = MAX2(NUMERICAL_EPS, current_gap - SPEED2DIST(MAX2(0.0, current_estSpeed + 0.5 * ACCEL2SPEED(acc) ((i + 1.) / myIterations)) - predSpeed) * ((i + 1.) / myIterations));
1110 :
1111 : // We cannot rely on sub-timesteps, because the here calculated "sub"-gap will not match the "full"-gap calculation of the Euler/Ballistic Update.
1112 : // The "full"-gap is only calculated with the last measured newSpeed, while the "sub"-gap uses all "sub"-newSpeeds
1113 : // Example: In the last iteration-step newSpeed becomes 0. Therefore in the Euler Update, the vehicle does not move for the whole timestep!
1114 : // Example: But in the "sub"-gaps the vehicle may have moved. Therefore, stops can sometimes not be reached
1115 292076556 : newGap = MAX2(NUMERICAL_EPS, newGap - MAX2(0., SPEED2DIST(newSpeed - predSpeed) / myIterations));
1116 : // Ballistic:
1117 : //newGap = MAX2(NUMERICAL_EPS, newGap - MAX2(0., SPEED2DIST(MAX2(0.0, current_estSpeed + 0.5 * ACCEL2SPEED(acc) / myIterations) - predSpeed) / myIterations));
1118 :
1119 : // To always reach stops in high-timestep simulations, we adapt the speed to the actual distance that is covered:
1120 : // This may only be needed for Euler Update...
1121 173921636 : if (myIterations > 1 && newSpeed < EST_REAC_THRESHOLD * TS && !lastrespectMinGap) {
1122 2314325 : newSpeed = MAX2(0.0, predSpeed + DIST2SPEED(current_gap - newGap) * myIterations / (i + 1.));
1123 : }
1124 : }
1125 :
1126 : // The "real" acceleration after iterations
1127 115261024 : acc = SPEED2ACCEL(MIN2(newSpeed, maxNextSpeed(egoSpeed, veh)) - veh->getSpeed());
1128 :
1129 : #ifdef DEBUG_V
1130 : if (veh->isSelected()) {
1131 : std::cout << SIMTIME
1132 : << " EIDM::_v "
1133 : << " veh=" << veh->getID()
1134 : << " vars->minaccel=" << vars->minaccel
1135 : << " vars->myap_update=" << vars->myap_update
1136 : << " vars->myv_est_l=" << vars->myv_est_l
1137 : << " vars->myv_est=" << vars->myv_est
1138 : << " vars->mys_est=" << vars->mys_est
1139 : << " vars->wouldacc=" << vars->wouldacc
1140 : << " vars->realacc=" << vars->realacc
1141 : << "\n";
1142 : std::cout << SIMTIME
1143 : << " EIDM::_v (2) "
1144 : << " veh=" << veh->getID()
1145 : << " newSpeed=" << newSpeed
1146 : << " newGap=" << newGap
1147 : << " predSpeed=" << predSpeed
1148 : << " estSpeed=" << estSpeed
1149 : << " estleaderSpeed=" << estleaderSpeed
1150 : << " estGap=" << estGap
1151 : << " wantedacc=" << wantedacc
1152 : << " wouldacc=" << wouldacc
1153 : << " acc=" << acc
1154 : << "\n";
1155 : }
1156 : #endif
1157 :
1158 : // wantedacc is already calculated at this point. acc may still change (because of coolness and drive off), but the ratio should stay the same!
1159 : // this means when vars->minaccel > wantedacc stands, so should vars->minaccel > acc!
1160 : // When updating at an Action Point, store the observed variables for the next time steps until the next Action Point.
1161 57630512 : if (vars->minaccel > wantedacc - NUMERICAL_EPS && vars->myap_update == 0 && usage == CalcReason::CURRENT) {
1162 18902126 : vars->myv_est_l = predSpeed;
1163 18902126 : vars->myv_est = egoSpeed;
1164 18902126 : if (update == 2) { // For freeSpeed
1165 18627 : vars->mys_est = current_gap + myTreaction * egoSpeed;
1166 : } else {
1167 18883499 : vars->mys_est = current_gap;
1168 : }
1169 18902126 : vars->myrespectMinGap = respectMinGap;
1170 : }
1171 :
1172 57630512 : if (usage == CalcReason::CURRENT && vars->wouldacc > wouldacc) {
1173 1361891 : vars->wouldacc = wouldacc;
1174 : }
1175 :
1176 : // It can happen that wantedacc ist higher than previous calculated wantedacc, BUT acc is lower than prev calculated values!
1177 : // This often occurs because of "coolness"+Iteration and in this case "acc" is set to the previous (higher) calculated value!
1178 57630512 : if (vars->realacc > acc && vars->minaccel <= wantedacc - NUMERICAL_EPS && usage == CalcReason::CURRENT) {
1179 : acc = vars->realacc;
1180 4614 : newSpeed = MAX2(0.0, egoSpeed + ACCEL2SPEED(acc));
1181 : }
1182 :
1183 : // Capture the relevant variables, because it was determined, that this call will result in the acceleration update (vars->minaccel > wantedacc)
1184 57630512 : if (vars->minaccel > wantedacc - NUMERICAL_EPS && usage == CalcReason::CURRENT) {
1185 26016053 : vars->minaccel = wantedacc;
1186 26016053 : if (vars->realacc > acc) {
1187 5938894 : vars->realacc = acc;
1188 : }
1189 26016053 : vars->realleaderacc = a_leader;
1190 : }
1191 :
1192 : // Save the parameters for a potential update in finalizeSpeed, when _v was called in a stopSpeed-function!!!
1193 57630512 : if (vars->minaccel > wantedacc - NUMERICAL_EPS && usage == CalcReason::CURRENT_WAIT && !respectMinGap) {
1194 9739580 : vars->stop.push_back(std::make_pair(acc, gap2pred));
1195 : }
1196 :
1197 57630512 : return MAX2(0., newSpeed);
1198 : }
1199 :
1200 :
1201 : MSCFModel*
1202 0 : MSCFModel_EIDM::duplicate(const MSVehicleType* vtype) const {
1203 0 : return new MSCFModel_EIDM(vtype);
1204 : }
|
