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