Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSCFModel.cpp
15 : /// @author Tobias Mayer
16 : /// @author Daniel Krajzewicz
17 : /// @author Jakob Erdmann
18 : /// @author Michael Behrisch
19 : /// @author Laura Bieker
20 : /// @author Leonhard Lücken
21 : /// @date Mon, 27 Jul 2009
22 : ///
23 : // The car-following model abstraction
24 : /****************************************************************************/
25 : #include <config.h>
26 :
27 : #include <cmath>
28 : #include <microsim/MSGlobals.h>
29 : #include <microsim/MSVehicleType.h>
30 : #include <microsim/MSVehicle.h>
31 : #include <microsim/MSNet.h>
32 : #include <microsim/MSLane.h>
33 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
34 : #include <microsim/MSDriverState.h>
35 : #include "MSCFModel.h"
36 :
37 : // ===========================================================================
38 : // DEBUG constants
39 : // ===========================================================================
40 : //#define DEBUG_FINALIZE_SPEED
41 : //#define DEBUG_DRIVER_ERRORS
42 : //#define DEBUG_EMERGENCYDECEL
43 : //#define DEBUG_COND (true)
44 : #define DEBUG_COND (veh->isSelected())
45 : //#define DEBUG_COND (veh->getID() == "follower")
46 : //#define DEBUG_COND2 (SIMTIME == 176)
47 : //#define DEBUG_COND2 (true)
48 : #define DEBUG_COND2 (gDebugFlag1)
49 :
50 :
51 :
52 : // ===========================================================================
53 : // method definitions
54 : // ===========================================================================
55 315026 : MSCFModel::MSCFModel(const MSVehicleType* vtype) :
56 315026 : myType(vtype),
57 315026 : myAccel(vtype->getParameter().getCFParam(SUMO_ATTR_ACCEL, SUMOVTypeParameter::getDefaultAccel(vtype->getParameter().vehicleClass))),
58 315026 : myDecel(vtype->getParameter().getCFParam(SUMO_ATTR_DECEL, SUMOVTypeParameter::getDefaultDecel(vtype->getParameter().vehicleClass))),
59 315026 : myEmergencyDecel(vtype->getParameter().getCFParam(SUMO_ATTR_EMERGENCYDECEL,
60 315026 : SUMOVTypeParameter::getDefaultEmergencyDecel(vtype->getParameter().vehicleClass, myDecel, MSGlobals::gDefaultEmergencyDecel))),
61 315026 : myApparentDecel(vtype->getParameter().getCFParam(SUMO_ATTR_APPARENTDECEL, myDecel)),
62 315026 : myCollisionMinGapFactor(vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 1)),
63 315026 : myHeadwayTime(vtype->getParameter().getCFParam(SUMO_ATTR_TAU, 1.0)),
64 315026 : myStartupDelay(TIME2STEPS(vtype->getParameter().getCFParam(SUMO_ATTR_STARTUP_DELAY, 0.0))),
65 315026 : myMaxAccelProfile(vtype->getParameter().getCFProfile(SUMO_ATTR_MAXACCEL_PROFILE, SUMOVTypeParameter::getDefaultMaxAccelProfile(vtype->getParameter().vehicleClass, myAccel))),
66 630052 : myDesAccelProfile(vtype->getParameter().getCFProfile(SUMO_ATTR_DESACCEL_PROFILE, SUMOVTypeParameter::getDefaultDesAccelProfile(vtype->getParameter().vehicleClass, myAccel)))
67 315026 : { }
68 :
69 :
70 311173 : MSCFModel::~MSCFModel() {}
71 :
72 :
73 46319 : MSCFModel::VehicleVariables::~VehicleVariables() {}
74 :
75 :
76 : double
77 12877442572 : MSCFModel::brakeGap(const double speed, const double decel, const double headwayTime) const {
78 12877442572 : if (MSGlobals::gSemiImplicitEulerUpdate) {
79 11860998804 : return brakeGapEuler(speed, decel, headwayTime);
80 : } else {
81 : // ballistic
82 1016443768 : if (speed <= 0) {
83 : return 0.;
84 : } else {
85 853672290 : return speed * (headwayTime + 0.5 * speed / decel);
86 : }
87 : }
88 : }
89 :
90 :
91 : double
92 11865206576 : MSCFModel::brakeGapEuler(const double speed, const double decel, const double headwayTime) {
93 : /* one possibility to speed this up is to calculate speedReduction * steps * (steps+1) / 2
94 : for small values of steps (up to 10 maybe) and store them in an array */
95 11865206576 : const double speedReduction = ACCEL2SPEED(decel);
96 11865206576 : const int steps = int(speed / speedReduction);
97 11865206576 : return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
98 : }
99 :
100 :
101 : double
102 830493988 : MSCFModel::freeSpeed(const double currentSpeed, const double decel, const double dist, const double targetSpeed, const bool onInsertion, const double actionStepLength) {
103 : // XXX: (Leo) This seems to be exclusively called with decel = myDecel (max deceleration) and is not overridden
104 : // by any specific CFModel. That may cause undesirable hard braking (at junctions where the vehicle
105 : // changes to a road with a lower speed limit).
106 :
107 830493988 : if (MSGlobals::gSemiImplicitEulerUpdate) {
108 : // adapt speed to succeeding lane, no reaction time is involved
109 : // when breaking for y steps the following distance g is covered
110 : // (drive with v in the final step)
111 : // g = (y^2 + y) * 0.5 * b + y * v
112 : // y = ((((sqrt((b + 2.0*v)*(b + 2.0*v) + 8.0*b*g)) - b)*0.5 - v)/b)
113 782092069 : const double v = SPEED2DIST(targetSpeed);
114 782092069 : if (dist < v) {
115 : return targetSpeed;
116 : }
117 738079854 : const double b = ACCEL2DIST(decel);
118 738079854 : const double y = MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
119 738079854 : const double yFull = floor(y);
120 738079854 : const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
121 738079854 : const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
122 1225421345 : return DIST2SPEED(MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
123 : } else {
124 : // ballistic update (Leo)
125 : // calculate maximum next speed vN that is adjustable to vT=targetSpeed after a distance d=dist
126 : // and given a maximal deceleration b=decel, denote the current speed by v0.
127 : // the distance covered by a trajectory that attains vN in the next action step (length=dt) and decelerates afterwards
128 : // with b is given as
129 : // d = 0.5*dt*(v0+vN) + (t-dt)*vN - 0.5*b*(t-dt)^2, (1)
130 : // where time t of arrival at d with speed vT is
131 : // t = dt + (vN-vT)/b. (2)
132 : // We insert (2) into (1) to obtain
133 : // d = 0.5*dt*(v0+vN) + vN*(vN-vT)/b - 0.5*b*((vN-vT)/b)^2
134 : // 0 = (dt*b*v0 - vT*vT - 2*b*d) + dt*b*vN + vN*vN
135 : // and solve for vN
136 :
137 : assert(currentSpeed >= 0);
138 : assert(targetSpeed >= 0);
139 :
140 48401919 : const double dt = onInsertion ? 0 : actionStepLength; // handles case that vehicle is inserted just now (at the end of move)
141 : const double v0 = currentSpeed;
142 : const double vT = targetSpeed;
143 : const double b = decel;
144 48401919 : const double d = dist - NUMERICAL_EPS; // prevent returning a value > targetSpeed due to rounding errors
145 :
146 : // Solvability for positive vN (if d is small relative to v0):
147 : // 1) If 0.5*(v0+vT)*dt > d, we set vN=vT.
148 : // (In case vT<v0, this implies that on the interpolated trajectory there are points beyond d where
149 : // the interpolated velocity is larger than vT, but at least on the temporal discretization grid, vT is not exceeded)
150 : // 2) We ignore the (possible) constraint vN >= v0 - b*dt, which could lead to a problem if v0 - t*b > vT.
151 : // (finalizeSpeed() is responsible for assuring that the next velocity is chosen in accordance with maximal decelerations)
152 :
153 : // If implied accel a leads to v0 + a*asl < vT, choose acceleration s.th. v0 + a*asl = vT
154 48401919 : if (0.5 * (v0 + vT)*dt >= d) {
155 : // Attain vT after time asl
156 1511792 : return v0 + TS * (vT - v0) / actionStepLength;
157 : } else {
158 46890127 : const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
159 46890127 : const double p = 0.5 * b * dt;
160 46890127 : const double vN = -p + sqrt(p * p - q); // target speed at time t0+asl
161 46890127 : return v0 + TS * (vN - v0) / actionStepLength;
162 : }
163 : }
164 : }
165 :
166 :
167 : double
168 3197166642 : MSCFModel::getSecureGap(const MSVehicle* const veh, const MSVehicle* const /*pred*/, const double speed, const double leaderSpeed, const double leaderMaxDecel) const {
169 : // The solution approach leaderBrakeGap >= followerBrakeGap is not
170 : // secure when the follower can brake harder than the leader because the paths may still cross.
171 : // As a workaround we use a value of leaderDecel which errs on the side of caution
172 3197166642 : const double maxDecel = MAX2(myDecel, leaderMaxDecel);
173 3197166642 : const double bgLeader = brakeGap(leaderSpeed, maxDecel, 0);
174 3197166642 : double secureGap = MAX2(0.0, brakeGap(speed, myDecel, myHeadwayTime) - bgLeader);
175 3197166642 : if (MSGlobals::gComputeLC && veh->getAcceleration() < -NUMERICAL_EPS) {
176 : // vehicle can react instantly in the next step
177 : // we only apply 'myHeadWayTime' to avoid sudden braking after lane change
178 : // thus we can reduce the required brakeGap if the vehicle is braking anyway
179 : // (but we shouldn't assume continued emergency deceleration)
180 967982320 : const double secureGapDecel = MAX2(0.0, brakeGap(speed, MIN2(-veh->getAcceleration(), myDecel), 0) - bgLeader);
181 : // the secureGapDecel doesn't leave room for lcAssertive behavior
182 956364458 : secureGap = MIN2(secureGap, secureGapDecel / veh->getLaneChangeModel().getSafetyFactor());
183 : }
184 3197166642 : return secureGap;
185 : }
186 :
187 :
188 : double
189 623690900 : MSCFModel::finalizeSpeed(MSVehicle* const veh, double vPos) const {
190 : // save old v for optional acceleration computation
191 623690900 : const double oldV = veh->getSpeed();
192 : // process stops (includes update of stopping state)
193 623690900 : const double vStop = MIN2(vPos, veh->processNextStop(vPos));
194 : // apply deceleration bounds
195 621549751 : const double vMinEmergency = minNextSpeedEmergency(oldV, veh);
196 : // vPos contains the uppper bound on safe speed. allow emergency braking here
197 621549751 : const double vMin = MIN2(minNextSpeed(oldV, veh), MAX2(vPos, vMinEmergency));
198 621549751 : const double fric = veh->getFriction();
199 : // adapt speed limit of road to "perceived" friction
200 621549751 : const double factor = fric == 1. ? 1. : -0.3491 * fric * fric + 0.8922 * fric + 0.4493; //2nd degree polyfit
201 :
202 : // aMax: Maximal admissible acceleration until the next action step, such that the vehicle's maximal
203 : // desired speed on the current lane will not be exceeded when the
204 : // acceleration is maintained until the next action step.
205 1243099502 : double aMax = (MAX2(veh->getLane()->getVehicleMaxSpeed(veh), vPos) * factor - oldV) / veh->getActionStepLengthSecs();
206 : // apply planned speed constraints and acceleration constraints
207 621549751 : double vMax = MIN3(oldV + ACCEL2SPEED(aMax), maxNextSpeed(oldV, veh), vStop);
208 : // do not exceed max decel even if it is unsafe
209 : #ifdef _DEBUG
210 : //if (vMin > vMax) {
211 : // WRITE_WARNINGF(TL("Maximum speed of vehicle '%' is lower than the minimum speed (min: %, max: %)."), veh->getID(), toString(vMin), toString(vMax));
212 : //}
213 : #endif
214 :
215 : #ifdef DEBUG_FINALIZE_SPEED
216 : if (DEBUG_COND) {
217 : std::cout << "\n" << SIMTIME << " FINALIZE_SPEED\n";
218 : }
219 : #endif
220 :
221 : vMax = MAX2(vMin, vMax);
222 621549751 : double vNext = patchSpeedBeforeLC(veh, vMin, vMax);
223 : #ifdef DEBUG_FINALIZE_SPEED
224 : double vDawdle = vNext;
225 : #endif
226 : assert(vNext >= vMin);
227 : assert(vNext <= vMax);
228 : // apply lane-changing related speed adaptations
229 621549751 : vNext = veh->getLaneChangeModel().patchSpeed(vMin, vNext, vMax, *this);
230 : #ifdef DEBUG_FINALIZE_SPEED
231 : double vPatchLC = vNext;
232 : #endif
233 : // apply further speed adaptations
234 621549751 : vNext = applyStartupDelay(veh, vMin, vNext);
235 :
236 : assert(vNext >= vMinEmergency); // stronger braking is permitted in lane-changing related emergencies
237 : assert(vNext <= vMax);
238 :
239 : #ifdef DEBUG_FINALIZE_SPEED
240 : if (DEBUG_COND) {
241 : std::cout << std::setprecision(gPrecision)
242 : << "veh '" << veh->getID() << "' oldV=" << oldV
243 : << " vPos" << vPos
244 : << " vMin=" << vMin
245 : << " aMax=" << aMax
246 : << " vMax=" << vMax
247 : << " vStop=" << vStop
248 : << " vDawdle=" << vDawdle
249 : << " vPatchLC=" << vPatchLC
250 : << " vNext=" << vNext
251 : << "\n";
252 : }
253 : #endif
254 621549751 : return vNext;
255 : }
256 :
257 :
258 : double
259 627311379 : MSCFModel::applyStartupDelay(const MSVehicle* veh, const double vMin, const double vMax, const SUMOTime addTime) const {
260 : UNUSED_PARAMETER(vMin);
261 : // timeSinceStartup was already incremented by DELTA_T
262 627311379 : if (veh->getTimeSinceStartup() > 0 && veh->getTimeSinceStartup() - DELTA_T < myStartupDelay + addTime) {
263 : assert(veh->getSpeed() <= SUMO_const_haltingSpeed);
264 46878 : const SUMOTime remainingDelay = myStartupDelay + addTime - (veh->getTimeSinceStartup() - DELTA_T);
265 : //std::cout << SIMTIME << " applyStartupDelay veh=" << veh->getID() << " remainingDelay=" << remainingDelay << "\n";
266 46878 : if (remainingDelay >= DELTA_T) {
267 : // delay startup by at least a whole step
268 : return 0;
269 : } else {
270 : // reduce acceleration for fractional startup delay
271 2899 : return (double)(DELTA_T - remainingDelay) / (double)DELTA_T * vMax;
272 : }
273 : }
274 : return vMax;
275 : }
276 :
277 :
278 : double
279 0 : MSCFModel::interpolateProfile(const double speed, const std::vector<std::pair<double, double> > profile) const {
280 : double val;
281 : // extrapolate, means using the first/last value of the array
282 0 : if (speed < profile[0].first) {
283 0 : val = profile[0].second;
284 0 : } else if (speed > profile.back().first) {
285 0 : val = profile.back().second;
286 : } else { // interpolate
287 : int x = 0;
288 0 : while (speed > profile[x + 1].first) {
289 : x++;
290 : }
291 0 : double diff = (profile[x + 1].second - profile[x].second) / (profile[x + 1].first - profile[x].first);
292 0 : val = profile[x].second + diff * (speed - profile[x].first);
293 : }
294 0 : return val;
295 : }
296 :
297 :
298 : double
299 0 : MSCFModel::interactionGap(const MSVehicle* const veh, double vL) const {
300 : // Resolve the vsafe equation to gap. Assume predecessor has
301 : // speed != 0 and that vsafe will be the current speed plus acceleration,
302 : // i.e that with this gap there will be no interaction.
303 0 : const double vNext = MIN2(maxNextSpeed(veh->getSpeed(), veh), veh->getLane()->getVehicleMaxSpeed(veh));
304 0 : const double gap = (vNext - vL) *
305 0 : ((veh->getSpeed() + vL) / (2.*myDecel) + myHeadwayTime) +
306 0 : vL * myHeadwayTime;
307 :
308 : // Don't allow timeHeadWay < deltaT situations.
309 0 : return MAX2(gap, SPEED2DIST(vNext));
310 : }
311 :
312 :
313 : double
314 5092256601 : MSCFModel::maxNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
315 5092256601 : return MIN2(speed + (double) ACCEL2SPEED(getCurrentAccel(speed)), myType->getMaxSpeed());
316 : }
317 :
318 :
319 : double
320 1193177255 : MSCFModel::minNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
321 1193177255 : if (MSGlobals::gSemiImplicitEulerUpdate) {
322 1139631056 : return MAX2(speed - ACCEL2SPEED(myDecel), 0.);
323 : } else {
324 : // NOTE: ballistic update allows for negative speeds to indicate a stop within the next timestep
325 53546199 : return speed - ACCEL2SPEED(myDecel);
326 : }
327 : }
328 :
329 :
330 : double
331 796401582 : MSCFModel::minNextSpeedEmergency(double speed, const MSVehicle* const /*veh*/) const {
332 796401582 : if (MSGlobals::gSemiImplicitEulerUpdate) {
333 599874429 : return MAX2(speed - ACCEL2SPEED(myEmergencyDecel), 0.);
334 : } else {
335 : // NOTE: ballistic update allows for negative speeds to indicate a stop within the next timestep
336 196527153 : return speed - ACCEL2SPEED(myEmergencyDecel);
337 : }
338 : }
339 :
340 :
341 :
342 : double
343 830493978 : MSCFModel::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion, const CalcReason /*usage*/) const {
344 830493978 : if (maxSpeed < 0.) {
345 : // can occur for ballistic update (in context of driving at red light)
346 : return maxSpeed;
347 : }
348 830493973 : double vSafe = freeSpeed(speed, myDecel, seen, maxSpeed, onInsertion, veh->getActionStepLengthSecs());
349 830493973 : return vSafe;
350 : }
351 :
352 :
353 : double
354 7991117 : MSCFModel::insertionFollowSpeed(const MSVehicle* const /* v */, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
355 7991117 : if (MSGlobals::gSemiImplicitEulerUpdate) {
356 7014801 : return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
357 : } else {
358 : // NOTE: Even for ballistic update, the current speed is irrelevant at insertion, therefore passing 0. (Leo)
359 976316 : return maximumSafeFollowSpeed(gap2pred, 0., predSpeed, predMaxDecel, true);
360 : }
361 : }
362 :
363 :
364 : double
365 57427 : MSCFModel::insertionStopSpeed(const MSVehicle* const veh, double speed, double gap) const {
366 57427 : if (MSGlobals::gSemiImplicitEulerUpdate) {
367 53779 : return stopSpeed(veh, speed, gap, CalcReason::FUTURE);
368 : } else {
369 3648 : return MIN2(maximumSafeStopSpeed(gap, myDecel, 0., true, 0., false), myType->getMaxSpeed());
370 : }
371 : }
372 :
373 :
374 : double
375 478347 : MSCFModel::followSpeedTransient(double duration, const MSVehicle* const /*veh*/, double /*speed*/, double gap2pred, double predSpeed, double predMaxDecel) const {
376 : // minimium distance covered by the leader if braking
377 478347 : double leaderMinDist = gap2pred + distAfterTime(duration, predSpeed, -predMaxDecel);
378 : // if ego would not brake it could drive with speed leaderMinDist / duration
379 : // due to potentential ego braking it can safely drive faster
380 478347 : if (MSGlobals::gSemiImplicitEulerUpdate) {
381 : // number of potential braking steps
382 407786 : const int a = (int)ceil(duration / TS - TS);
383 : // can we brake for the whole time?
384 407786 : if (brakeGap(a * myDecel, myDecel, 0) <= leaderMinDist) {
385 : // braking continuously for duration
386 : // distance reduction due to braking
387 345423 : const double b = TS * getMaxDecel() * 0.5 * (a * a - a);
388 345423 : if (gDebugFlag2) std::cout << " followSpeedTransient"
389 : << " duration=" << duration
390 : << " gap=" << gap2pred
391 : << " leaderMinDist=" << leaderMinDist
392 : << " decel=" << getMaxDecel()
393 : << " a=" << a
394 0 : << " bg=" << brakeGap(a * myDecel, myDecel, 0)
395 : << " b=" << b
396 0 : << " x=" << (b + leaderMinDist) / duration
397 0 : << "\n";
398 345423 : return (b + leaderMinDist) / duration;
399 : } else {
400 : // @todo improve efficiency
401 : double bg = 0;
402 : double speed = 0;
403 1426971 : while (bg < leaderMinDist) {
404 1364608 : speed += ACCEL2SPEED(myDecel);
405 1364608 : bg += SPEED2DIST(speed);
406 : }
407 62363 : speed -= DIST2SPEED(bg - leaderMinDist);
408 62363 : return speed;
409 : }
410 : } else {
411 : // can we brake for the whole time?
412 70561 : const double fullBrakingSeconds = sqrt(leaderMinDist * 2 / myDecel);
413 70561 : if (fullBrakingSeconds >= duration) {
414 : // braking continuously for duration
415 : // average speed after braking for duration is x2 = x - 0.5 * duration * myDecel
416 : // x2 * duration <= leaderMinDist must hold
417 56936 : return leaderMinDist / duration + duration * getMaxDecel() / 2;
418 : } else {
419 13625 : return fullBrakingSeconds * myDecel;
420 : }
421 : }
422 : }
423 :
424 : double
425 478347 : MSCFModel::distAfterTime(double t, double speed, const double accel) const {
426 478347 : if (accel >= 0.) {
427 0 : return (speed + 0.5 * accel * t) * t;
428 : }
429 478347 : const double decel = -accel;
430 478347 : if (speed <= decel * t) {
431 : // braking to a full stop
432 310585 : return brakeGap(speed, decel, 0);
433 : }
434 167762 : if (MSGlobals::gSemiImplicitEulerUpdate) {
435 : // @todo improve efficiency
436 : double result = 0;
437 706728 : while (t > 0) {
438 564009 : speed -= ACCEL2SPEED(decel);
439 564009 : result += MAX2(0.0, SPEED2DIST(speed));
440 564009 : t -= TS;
441 : }
442 142719 : return result;
443 : } else {
444 25043 : const double speed2 = speed - t * decel;
445 25043 : return 0.5 * (speed + speed2) * t;
446 : }
447 : }
448 :
449 :
450 : SUMOTime
451 848728361 : MSCFModel::getMinimalArrivalTime(double dist, double currentSpeed, double arrivalSpeed) const {
452 848728361 : if (dist <= 0.) {
453 : return 0;
454 : }
455 : // will either drive as fast as possible and decelerate as late as possible
456 : // or accelerate as fast as possible and then hold that speed
457 : arrivalSpeed = MAX2(0.0, arrivalSpeed); // ballistic model may call this with -std::numeric_limits<double>::max()
458 848614808 : const double accel = (arrivalSpeed >= currentSpeed) ? getMaxAccel() : -getMaxDecel();
459 848614808 : const double accelTime = accel == 0. ? 0. : (arrivalSpeed - currentSpeed) / accel;
460 848614808 : const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
461 848614808 : if (dist >= accelWay) {
462 838200676 : const double nonAccelWay = dist - accelWay;
463 : const double nonAccelSpeed = MAX3(currentSpeed, arrivalSpeed, SUMO_const_haltingSpeed);
464 838200676 : return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
465 : }
466 : // find time x so that
467 : // x * (currentSpeed + currentSpeed + x * accel) * 0.5 = dist
468 10414132 : return TIME2STEPS(-(currentSpeed - sqrt(currentSpeed * currentSpeed + 2 * accel * dist)) / accel);
469 : }
470 :
471 :
472 : double
473 294647 : MSCFModel::estimateArrivalTime(double dist, double speed, double maxSpeed, double accel) {
474 : assert(speed >= 0.);
475 : assert(dist >= 0.);
476 :
477 294647 : if (dist < NUMERICAL_EPS) {
478 : return 0.;
479 : }
480 :
481 294647 : if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
482 : // distance will never be covered with these values
483 : return INVALID_DOUBLE;
484 : }
485 :
486 118203 : if (fabs(accel) < NUMERICAL_EPS) {
487 100289 : return dist / speed;
488 : }
489 :
490 17914 : double p = speed / accel;
491 :
492 17914 : if (accel < 0.) {
493 : // we already know, that the distance will be covered despite breaking
494 17914 : return (-p - sqrt(p * p + 2 * dist / accel));
495 : }
496 :
497 : // Here, accel > 0
498 : // t1 is the time to use the given acceleration
499 0 : double t1 = (maxSpeed - speed) / accel;
500 : // distance covered until t1
501 0 : double d1 = speed * t1 + 0.5 * accel * t1 * t1;
502 0 : if (d1 >= dist) {
503 : // dist is covered before changing the speed
504 0 : return (-p + sqrt(p * p + 2 * dist / accel));
505 : } else {
506 0 : return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
507 : }
508 :
509 : }
510 :
511 : double
512 69526 : MSCFModel::estimateArrivalTime(double dist, double initialSpeed, double arrivalSpeed, double maxSpeed, double accel, double decel) {
513 : UNUSED_PARAMETER(arrivalSpeed); // only in assertion
514 : UNUSED_PARAMETER(decel); // only in assertion
515 69526 : if (dist <= 0) {
516 : return 0.;
517 : }
518 :
519 : // stub-assumptions
520 : assert(accel == decel);
521 : assert(accel > 0);
522 : assert(initialSpeed == 0);
523 : assert(arrivalSpeed == 0);
524 : assert(maxSpeed > 0);
525 :
526 :
527 69526 : double accelTime = (maxSpeed - initialSpeed) / accel;
528 : // "ballistic" estimate for the distance covered during acceleration phase
529 69526 : double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
530 : double arrivalTime;
531 69526 : if (accelDist >= dist * 0.5) {
532 : // maximal speed will not be attained during maneuver
533 2940 : arrivalTime = 4 * sqrt(dist / accel);
534 : } else {
535 : // Calculate time to move with constant, maximal lateral speed
536 66586 : const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
537 66586 : arrivalTime = accelTime + constSpeedTime;
538 : }
539 : return arrivalTime;
540 : }
541 :
542 :
543 : double
544 744873 : MSCFModel::avoidArrivalAccel(double dist, double time, double speed, double maxDecel) {
545 : assert(time > 0 || dist == 0);
546 744873 : if (dist <= 0) {
547 602 : return -maxDecel;
548 744271 : } else if (time * speed > 2 * dist) {
549 : // stop before dist is necessary. We need
550 : // d = v*v/(2*a)
551 5975 : return - 0.5 * speed * speed / dist;
552 : } else {
553 : // we seek the solution a of
554 : // d = v*t + a*t*t/2
555 738296 : return 2 * (dist / time - speed) / time;
556 : }
557 : }
558 :
559 :
560 : double
561 2674233 : MSCFModel::getMinimalArrivalSpeed(double dist, double currentSpeed) const {
562 : // ballistic update
563 2674233 : return estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
564 : }
565 :
566 :
567 : double
568 53663054 : MSCFModel::getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const {
569 : double arrivalSpeedBraking;
570 : // Because we use a continuous formula for computing the possible slow-down
571 : // we need to handle the mismatch with the discrete dynamics
572 53663054 : if (dist < currentSpeed) {
573 : arrivalSpeedBraking = INVALID_SPEED; // no time left for braking after this step
574 : // (inserted max() to get rid of arrivalSpeed dependency within method) (Leo)
575 26935321 : } else if (2 * (dist - currentSpeed * getHeadwayTime()) * -getMaxDecel() + currentSpeed * currentSpeed >= 0) {
576 26933896 : arrivalSpeedBraking = estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
577 : } else {
578 : arrivalSpeedBraking = getMaxDecel();
579 : }
580 53663054 : return arrivalSpeedBraking;
581 : }
582 :
583 :
584 :
585 :
586 : double
587 24769780 : MSCFModel::gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1, double a2, const double maxV1, const double maxV2) {
588 :
589 : double newGap = currentGap;
590 :
591 24769780 : if (MSGlobals::gSemiImplicitEulerUpdate) {
592 20324010 : for (unsigned int steps = 1; steps * TS <= duration; ++steps) {
593 0 : v1 = MIN2(MAX2(v1 + a1, 0.), maxV1);
594 0 : v2 = MIN2(MAX2(v2 + a2, 0.), maxV2);
595 0 : newGap += TS * (v1 - v2);
596 : }
597 : } else {
598 : // determine times t1, t2 for which vehicles can break until stop (within duration)
599 : // and t3, t4 for which they reach their maximal speed on their current lanes.
600 4445770 : double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
601 :
602 : // t1: ego veh stops
603 4445770 : if (a1 < 0 && v1 > 0) {
604 1457257 : const double leaderStopTime = - v1 / a1;
605 1457257 : t1 = MIN2(leaderStopTime, duration);
606 4445770 : } else if (a1 >= 0) {
607 2914504 : t1 = duration;
608 : }
609 : // t2: veh2 stops
610 4445770 : if (a2 < 0 && v2 > 0) {
611 156108 : const double followerStopTime = -v2 / a2;
612 156108 : t2 = MIN2(followerStopTime, duration);
613 4445770 : } else if (a2 >= 0) {
614 4249913 : t2 = duration;
615 : }
616 : // t3: ego veh reaches vMax
617 4445770 : if (a1 > 0 && v1 < maxV1) {
618 1784555 : const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
619 1784555 : t3 = MIN2(leaderMaxSpeedTime, duration);
620 4445770 : } else if (a1 <= 0) {
621 2661215 : t3 = duration;
622 : }
623 : // t4: veh2 reaches vMax
624 4445770 : if (a2 > 0 && v2 < maxV2) {
625 1713131 : const double followerMaxSpeedTime = (maxV2 - v2) / a2;
626 1713131 : t4 = MIN2(followerMaxSpeedTime, duration);
627 4445770 : } else if (a2 <= 0) {
628 2732639 : t4 = duration;
629 : }
630 :
631 : // NOTE: this assumes that the accelerations a1 and a2 are constant over the next
632 : // followerBreakTime seconds (if no vehicle stops before or reaches vMax)
633 : std::list<double> l;
634 : l.push_back(t1);
635 : l.push_back(t2);
636 : l.push_back(t3);
637 : l.push_back(t4);
638 4445770 : l.sort();
639 : std::list<double>::const_iterator i;
640 : double tLast = 0.;
641 4664462 : for (i = l.begin(); i != l.end(); ++i) {
642 4664462 : if (*i != tLast) {
643 2528615 : double dt = MIN2(*i, duration) - tLast; // time between *i and tLast
644 2528615 : double dv = v1 - v2; // current velocity difference
645 2528615 : double da = a1 - a2; // current acceleration difference
646 2528615 : newGap += dv * dt + da * dt * dt / 2.; // update gap
647 2528615 : v1 += dt * a1;
648 2528615 : v2 += dt * a2;
649 : }
650 4664462 : if (*i == t1 || *i == t3) {
651 : // ego veh reached velocity bound
652 : a1 = 0.;
653 : }
654 :
655 4664462 : if (*i == t2 || *i == t4) {
656 : // veh2 reached velocity bound
657 : a2 = 0.;
658 : }
659 :
660 : tLast = MIN2(*i, duration);
661 4664462 : if (tLast == duration) {
662 : break;
663 : }
664 : }
665 :
666 4445770 : if (duration != tLast) {
667 : // (both vehicles have zero acceleration)
668 : assert(a1 == 0. && a2 == 0.);
669 0 : double dt = duration - tLast; // remaining time until duration
670 0 : double dv = v1 - v2; // current velocity difference
671 0 : newGap += dv * dt; // update gap
672 : }
673 : }
674 :
675 24769780 : return newGap;
676 : }
677 :
678 :
679 :
680 : double
681 53236888 : MSCFModel::passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed) {
682 :
683 : assert(passedPos <= currentPos);
684 : assert(passedPos >= lastPos);
685 : assert(currentPos > lastPos);
686 : assert(currentSpeed >= 0);
687 :
688 53236888 : if (passedPos > currentPos || passedPos < lastPos) {
689 0 : std::stringstream ss;
690 : // Debug (Leo)
691 0 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
692 : // NOTE: error is guarded to maintain original test output for euler update (Leo).
693 0 : ss << "passingTime(): given argument passedPos = " << passedPos << " doesn't lie within [lastPos, currentPos] = [" << lastPos << ", " << currentPos << "]\nExtrapolating...";
694 0 : std::cout << ss.str() << "\n";
695 0 : WRITE_ERROR(ss.str());
696 : }
697 0 : const double lastCoveredDist = currentPos - lastPos;
698 0 : const double extrapolated = passedPos > currentPos ? TS * (passedPos - lastPos) / lastCoveredDist : TS * (currentPos - passedPos) / lastCoveredDist;
699 : return extrapolated;
700 53236888 : } else if (currentSpeed < 0) {
701 0 : WRITE_ERROR("passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
702 0 : return -1;
703 : }
704 :
705 53236888 : const double distanceOldToPassed = passedPos - lastPos; // assert: >=0
706 :
707 53236888 : if (MSGlobals::gSemiImplicitEulerUpdate) {
708 : // euler update (constantly moving with currentSpeed during [0,TS])
709 51814038 : if (currentSpeed == 0) {
710 0 : return TS;
711 : }
712 51814038 : const double t = distanceOldToPassed / currentSpeed;
713 51814038 : return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
714 :
715 : } else {
716 : // ballistic update (constant acceleration a during [0,TS], except in case of a stop)
717 :
718 : // determine acceleration
719 : double a;
720 1422850 : if (currentSpeed > 0) {
721 : // the acceleration was constant within the last time step
722 1422702 : a = SPEED2ACCEL(currentSpeed - lastSpeed);
723 : } else {
724 : // the currentSpeed is zero (the last was not because lastPos<currentPos).
725 : assert(currentSpeed == 0 && lastSpeed != 0);
726 : // In general the stop has taken place within the last time step.
727 : // The acceleration (a<0) is obtained from
728 : // deltaPos = - lastSpeed^2/(2*a)
729 148 : a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
730 :
731 : assert(a < 0);
732 : }
733 :
734 : // determine passing time t
735 : // we solve distanceOldToPassed = lastSpeed*t + a*t^2/2
736 1422850 : if (fabs(a) < NUMERICAL_EPS) {
737 : // treat as constant speed within [0, TS]
738 360020 : const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
739 360020 : return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
740 1062830 : } else if (a > 0) {
741 : // positive acceleration => only one positive solution
742 571350 : const double va = lastSpeed / a;
743 571350 : const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
744 : assert(t < 1 && t >= 0);
745 571350 : return t;
746 : } else {
747 : // negative acceleration => two positive solutions (pick the smaller one.)
748 491480 : const double va = lastSpeed / a;
749 491480 : const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
750 : // emergency braking at red light could give results out of the admissible result range
751 : // because the dynamics are euler-like (full forward speed with instant deceleration)
752 491480 : return MIN2(TS, MAX2(0., t));
753 : }
754 : }
755 : }
756 :
757 :
758 : double
759 67385305 : MSCFModel::speedAfterTime(const double t, const double v0, const double dist) {
760 : assert(dist >= 0);
761 : assert(t >= 0 && t <= TS);
762 67385305 : if (MSGlobals::gSemiImplicitEulerUpdate) {
763 : // euler: constant speed within [0,TS]
764 66325166 : return DIST2SPEED(dist);
765 : } else {
766 : // ballistic: piecewise constant acceleration in [0,TS] (may become 0 for a stop within TS)
767 : // We reconstruct acceleration at time t=0. Note that the covered distance in case
768 : // of a stop exactly at t=TS is TS*v0/2.
769 1060139 : if (dist < TS * v0 / 2) {
770 : // stop must have occurred within [0,TS], use dist = -v0^2/(2a) (stopping dist),
771 : // i.e., a = -v0^2/(2*dist)
772 82 : const double accel = - v0 * v0 / (2 * dist);
773 : // The speed at time t is then
774 82 : return v0 + accel * t;
775 : } else {
776 : // no stop occurred within [0,TS], thus (from dist = v0*TS + accel*TS^2/2)
777 1060057 : const double accel = 2 * (dist / TS - v0) / TS;
778 : // The speed at time t is then
779 1060057 : return v0 + accel * t;
780 : }
781 : }
782 : }
783 :
784 :
785 :
786 :
787 : double
788 2058216488 : MSCFModel::estimateSpeedAfterDistance(const double dist, const double v, const double accel) const {
789 : // dist=v*t + 0.5*accel*t^2, solve for t and use v1 = v + accel*t
790 2058216488 : return MIN2(myType->getMaxSpeed(),
791 2058216488 : (double)sqrt(MAX2(0., 2 * dist * accel + v * v)));
792 : }
793 :
794 :
795 :
796 : double
797 3159106763 : MSCFModel::maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion, double headway, bool relaxEmergency) const {
798 : double vsafe;
799 3159106763 : if (MSGlobals::gSemiImplicitEulerUpdate) {
800 2925973011 : vsafe = maximumSafeStopSpeedEuler(gap, decel, onInsertion, headway);
801 : } else {
802 233133752 : vsafe = maximumSafeStopSpeedBallistic(gap, decel, currentSpeed, onInsertion, headway);
803 : }
804 :
805 3159106763 : if (relaxEmergency && myDecel != myEmergencyDecel) {
806 : #ifdef DEBUG_EMERGENCYDECEL
807 : if (true) {
808 : std::cout << SIMTIME << " maximumSafeStopSpeed()"
809 : << " g=" << gap
810 : << " v=" << currentSpeed
811 : << " initial vsafe=" << vsafe << "(decel=" << SPEED2ACCEL(v - vsafe) << ")" << std::endl;
812 : }
813 : #endif
814 :
815 787050731 : double origSafeDecel = SPEED2ACCEL(currentSpeed - vsafe);
816 787050731 : if (origSafeDecel > myDecel + NUMERICAL_EPS) {
817 : // emergency deceleration required
818 :
819 : #ifdef DEBUG_EMERGENCYDECEL
820 : if (true) {
821 : std::cout << SIMTIME << " maximumSafeStopSpeed() results in emergency deceleration "
822 : << "initial vsafe=" << vsafe << " egoSpeed=" << v << "(decel=" << SPEED2ACCEL(v - vsafe) << ")" << std::endl;
823 : }
824 : #endif
825 :
826 22901196 : double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, currentSpeed, 0., 1);
827 : // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
828 22901196 : safeDecel = MAX2(safeDecel, myDecel);
829 : // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
830 : safeDecel = MIN2(safeDecel, origSafeDecel);
831 22901196 : vsafe = currentSpeed - ACCEL2SPEED(safeDecel);
832 22901196 : if (MSGlobals::gSemiImplicitEulerUpdate) {
833 : vsafe = MAX2(vsafe, 0.);
834 : }
835 :
836 : #ifdef DEBUG_EMERGENCYDECEL
837 : if (true) {
838 : std::cout << " -> corrected emergency deceleration: " << SPEED2ACCEL(v - vsafe) << std::endl;
839 : }
840 : #endif
841 :
842 : }
843 : }
844 :
845 3159106763 : return vsafe;
846 : }
847 :
848 :
849 : double
850 2925973011 : MSCFModel::maximumSafeStopSpeedEuler(double gap, double decel, bool /* onInsertion */, double headway) const {
851 : // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
852 2925973011 : const double g = gap - NUMERICAL_EPS;
853 2925973011 : if (g < 0.) {
854 : return 0.;
855 : }
856 2842969435 : const double b = ACCEL2SPEED(decel);
857 2842969435 : const double t = headway >= 0 ? headway : myHeadwayTime;
858 : const double s = TS;
859 :
860 : // h = the distance that would be covered if it were possible to stop
861 : // exactly after gap and decelerate with b every simulation step
862 : // h = 0.5 * n * (n-1) * b * s + n * b * t (solve for n)
863 : //n = ((1.0/2.0) - ((t + (pow(((s*s) + (4.0*((s*((2.0*h/b) - t)) + (t*t)))), (1.0/2.0))*sign/2.0))/s));
864 2842969435 : const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
865 2842969435 : const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
866 : assert(h <= g + NUMERICAL_EPS);
867 : // compute the additional speed that must be used during deceleration to fix
868 : // the discrepancy between g and h
869 2842969435 : const double r = (g - h) / (n * s + t);
870 2842969435 : const double x = n * b + r;
871 : assert(x >= 0);
872 2842969435 : return x;
873 : // return onInsertion ? x + b: x; // see #2574
874 : }
875 :
876 :
877 : double
878 233897065 : MSCFModel::maximumSafeStopSpeedBallistic(double gap, double decel, double currentSpeed, bool onInsertion, double headway) const {
879 : // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
880 233897065 : const double g = MAX2(0., gap - NUMERICAL_EPS);
881 233897065 : headway = headway >= 0 ? headway : myHeadwayTime;
882 :
883 : // (Leo) Note that in contrast to the Euler update, for the ballistic update
884 : // the distance covered in the coming step depends on the current velocity, in general.
885 : // one exception is the situation when the vehicle is just being inserted.
886 : // In that case, it will not cover any distance until the next timestep by convention.
887 :
888 : // We treat the latter case first:
889 233897065 : if (onInsertion) {
890 : // The distance covered with constant insertion speed v0 until time tau is given as
891 : // G1 = tau*v0
892 : // The distance covered between time tau and the stopping moment at time tau+v0/b is
893 : // G2 = v0^2/(2b),
894 : // where b is an assumed constant deceleration (= decel)
895 : // We solve g = G1 + G2 for v0:
896 5487508 : const double btau = decel * headway;
897 5487508 : const double v0 = -btau + sqrt(btau * btau + 2 * decel * g);
898 5487508 : return v0;
899 : }
900 :
901 : // In the usual case during the driving task, the vehicle goes by
902 : // a current speed v0=v, and we seek to determine a safe acceleration a (possibly <0)
903 : // such that starting to break after accelerating with a for the time tau=headway
904 : // still allows us to stop in time.
905 :
906 228409557 : const double tau = headway == 0 ? TS : headway;
907 : const double v0 = MAX2(0., currentSpeed);
908 : // We first consider the case that a stop has to take place within time tau
909 : // (the distance driven when decelerating from v0 to 0 in tau is v0 * tau / 2)
910 228409557 : if (g <= v0 * tau * 0.5) {
911 16405568 : if (g == 0.) {
912 8856746 : if (v0 > 0.) {
913 : // indicate to brake as hard as possible
914 6880413 : return -ACCEL2SPEED(myEmergencyDecel);
915 : } else {
916 : // stay stopped
917 : return 0.;
918 : }
919 : }
920 : // In general we solve g = v0^2/(-2a), where the rhs is the distance
921 : // covered until stop when braking with a<0
922 7548822 : const double a = -v0 * v0 / (2 * g);
923 7548822 : return v0 + a * TS;
924 : }
925 :
926 : // The last case corresponds to a situation, where the vehicle may go with a positive
927 : // speed v1 = v0 + tau*a after time tau. (v1 is the maximum possible speed
928 : // for this and unconstrained by current speed or acceleration limits)
929 : //
930 : // The distance covered until time tau is given as
931 : // G1 = tau*(v0+v1)/2
932 : // The distance covered between time tau and the stopping moment at time tau+v1/b is
933 : // G2 = v1^2/(2b),
934 : // where b is an assumed constant deceleration (= decel)
935 : // We solve g = G1 + G2 for v1>0:
936 : // <=> 0 = v1^2 + b*tau*v1 + b*tau*v0 - 2bg
937 : // => v1 = -b*tau/2 + sqrt( (b*tau)^2/4 + b(2g - tau*v0) )
938 :
939 212003989 : const double btau2 = decel * tau / 2;
940 212003989 : const double v1 = -btau2 + sqrt(btau2 * btau2 + decel * (2 * g - tau * v0));
941 212003989 : const double a = (v1 - v0) / tau;
942 212003989 : return v0 + a * TS;
943 : }
944 :
945 :
946 : /** Returns the SK-vsafe. */
947 : double
948 2320302846 : MSCFModel::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
949 : // the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
950 : // the leaders starts braking hard until stopped
951 : // unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
952 : // (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
953 : // To make things safe, we ensure that the leaders brake distance is computed with an deceleration that is at least as high as the follower's.
954 : // @todo: this is a conservative estimate for safe speed which could be increased
955 :
956 : // // For negative gaps, we return the lowest meaningful value by convention
957 : // // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)
958 :
959 : // // It must be done. Otherwise, negative gaps at high speeds can create nonsense results from the call to maximumSafeStopSpeed() below
960 :
961 : // if(gap<0){
962 : // if(MSGlobals::gSemiImplicitEulerUpdate){
963 : // return 0.;
964 : // } else {
965 : // return -INVALID_SPEED;
966 : // }
967 : // }
968 :
969 : // The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
970 : // if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
971 : // NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
972 : // the follower accelerates for a short period of time. Refs #2310 (Leo)
973 : // const double headway = predSpeed > 0. ? myHeadwayTime : 0.;
974 :
975 2320302846 : const double headway = myHeadwayTime;
976 : double x;
977 2320302846 : if (gap >= 0 || MSGlobals::gComputeLC) {
978 4535672611 : x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), myDecel, egoSpeed, onInsertion, headway, false);
979 : } else {
980 895965 : x = egoSpeed - ACCEL2SPEED(myEmergencyDecel);
981 895965 : if (MSGlobals::gSemiImplicitEulerUpdate) {
982 : x = MAX2(x, 0.);
983 : }
984 : }
985 :
986 2320302846 : if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
987 1499827853 : double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
988 1499827853 : if (origSafeDecel > myDecel + NUMERICAL_EPS) {
989 : // Braking harder than myDecel was requested -> calculate required emergency deceleration.
990 : // Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
991 : // 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,
992 : // such that braking harder than myDecel is required.
993 :
994 3743059 : double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
995 : #ifdef DEBUG_EMERGENCYDECEL
996 : if (DEBUG_COND2) {
997 : std::cout << SIMTIME << " initial vsafe=" << x
998 : << " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
999 : << " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
1000 : << " safeDecel=" << safeDecel
1001 : << std::endl;
1002 : }
1003 : #endif
1004 : // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
1005 3743059 : safeDecel = MAX2(safeDecel, myDecel);
1006 : // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
1007 : safeDecel = MIN2(safeDecel, origSafeDecel);
1008 3743059 : x = egoSpeed - ACCEL2SPEED(safeDecel);
1009 3743059 : if (MSGlobals::gSemiImplicitEulerUpdate) {
1010 : x = MAX2(x, 0.);
1011 : }
1012 :
1013 : #ifdef DEBUG_EMERGENCYDECEL
1014 : if (DEBUG_COND2) {
1015 : std::cout << " -> corrected emergency deceleration: " << safeDecel << " newVSafe=" << x << std::endl;
1016 : }
1017 : #endif
1018 :
1019 : }
1020 : }
1021 : assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
1022 : assert(!std::isnan(x));
1023 2320302846 : return x;
1024 : }
1025 :
1026 :
1027 : double
1028 26644255 : MSCFModel::calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const {
1029 : // There are two cases:
1030 : // 1) Either, stopping in time is possible with a deceleration b <= predMaxDecel, then this value is returned
1031 : // 2) Or, b > predMaxDecel is required in this case the minimal value b allowing to stop safely under the assumption maxPredDecel=b is returned
1032 26644255 : if (gap <= 0.) {
1033 2260163 : return myEmergencyDecel;
1034 : }
1035 :
1036 : // Apparent braking distance for the leader
1037 24384092 : const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
1038 : // Required deceleration according to case 1)
1039 24384092 : const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
1040 :
1041 : #ifdef DEBUG_EMERGENCYDECEL
1042 : if (DEBUG_COND2) {
1043 : std::cout << SIMTIME << " calculateEmergencyDeceleration()"
1044 : << " gap=" << gap << " egoSpeed=" << egoSpeed << " predSpeed=" << predSpeed
1045 : << " predBrakeDist=" << predBrakeDist
1046 : << " b1=" << b1
1047 : << std::endl;
1048 : }
1049 : #endif
1050 :
1051 24384092 : if (b1 <= predMaxDecel) {
1052 : // Case 1) applies
1053 : #ifdef DEBUG_EMERGENCYDECEL
1054 : if (DEBUG_COND2) {
1055 : std::cout << " case 1 ..." << std::endl;
1056 : }
1057 : #endif
1058 : return b1;
1059 : }
1060 : #ifdef DEBUG_EMERGENCYDECEL
1061 : if (DEBUG_COND2) {
1062 : std::cout << " case 2 ...";
1063 : }
1064 : #endif
1065 :
1066 : // Case 2) applies
1067 : // Required deceleration according to case 2)
1068 23619743 : const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
1069 :
1070 : #ifdef DEBUG_EMERGENCYDECEL
1071 : if (DEBUG_COND2) {
1072 : std::cout << " b2=" << b2 << std::endl;
1073 : }
1074 : #endif
1075 23619743 : return b2;
1076 : }
1077 :
1078 :
1079 : void
1080 615428396 : MSCFModel::applyOwnSpeedPerceptionError(const MSVehicle* const veh, double& speed) const {
1081 615428396 : if (!veh->hasDriverState()) {
1082 : return;
1083 : }
1084 812844 : speed = veh->getDriverState()->getPerceivedOwnSpeed(speed);
1085 : }
1086 :
1087 :
1088 : void
1089 2377651727 : MSCFModel::applyHeadwayAndSpeedDifferencePerceptionErrors(const MSVehicle* const veh, double speed, double& gap, double& predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
1090 : UNUSED_PARAMETER(speed);
1091 : UNUSED_PARAMETER(predMaxDecel);
1092 2377651727 : if (!veh->hasDriverState()) {
1093 : return;
1094 : }
1095 :
1096 : // Obtain perceived gap and headway from the driver state
1097 960307 : const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap, pred);
1098 960307 : const double perceivedSpeedDifference = veh->getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
1099 :
1100 : #ifdef DEBUG_DRIVER_ERRORS
1101 : if (DEBUG_COND) {
1102 : if (!veh->getDriverState()->debugLocked()) {
1103 : veh->getDriverState()->lockDebug();
1104 : std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
1105 : << " speed=" << speed << " gap=" << gap << " leaderSpeed=" << predSpeed
1106 : << "\n perceivedGap=" << perceivedGap << " perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
1107 : << " perceivedSpeedDifference=" << perceivedSpeedDifference
1108 : << std::endl;
1109 : const double exactFollowSpeed = followSpeed(veh, speed, gap, predSpeed, predMaxDecel, pred, CalcReason::FUTURE);
1110 : const double errorFollowSpeed = followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel, pred, CalcReason::FUTURE);
1111 : const double accelError = SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
1112 : std::cout << " gapError=" << perceivedGap - gap << " dvError=" << perceivedSpeedDifference - (predSpeed - speed)
1113 : << "\n resulting accelError: " << accelError << std::endl;
1114 : veh->getDriverState()->unlockDebug();
1115 : }
1116 : }
1117 : #endif
1118 :
1119 960307 : gap = perceivedGap;
1120 960307 : predSpeed = speed + perceivedSpeedDifference;
1121 : }
1122 :
1123 :
1124 : void
1125 790982605 : MSCFModel::applyHeadwayPerceptionError(const MSVehicle* const veh, double speed, double& gap) const {
1126 : UNUSED_PARAMETER(speed);
1127 790982605 : if (!veh->hasDriverState()) {
1128 : return;
1129 : }
1130 : // @todo: Provide objectID (e.g. pointer address for the relevant object at the given distance(gap))
1131 : // This is for item related management of known object and perception updates when the distance
1132 : // changes significantly. (Should not be too important for stationary objects though.)
1133 :
1134 : // Obtain perceived gap from driver state
1135 237413 : const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap);
1136 :
1137 : #ifdef DEBUG_DRIVER_ERRORS
1138 : if (DEBUG_COND) {
1139 : if (!veh->getDriverState()->debugLocked()) {
1140 : veh->getDriverState()->lockDebug();
1141 : std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
1142 : << " speed=" << speed << " gap=" << gap << "\n perceivedGap=" << perceivedGap << std::endl;
1143 : const double exactStopSpeed = stopSpeed(veh, speed, gap, CalcReason::FUTURE);
1144 : const double errorStopSpeed = stopSpeed(veh, speed, perceivedGap, CalcReason::FUTURE);
1145 : const double accelError = SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1146 : std::cout << " gapError=" << perceivedGap - gap << "\n resulting accelError: " << accelError << std::endl;
1147 : veh->getDriverState()->unlockDebug();
1148 : }
1149 : }
1150 : #endif
1151 :
1152 237413 : gap = perceivedGap;
1153 : }
1154 :
1155 :
1156 : double
1157 5620512203 : MSCFModel::getCurrentAccel(const double speed) const {
1158 5620512203 : double result = myAccel;
1159 5620512203 : if (!myDesAccelProfile.empty()) {
1160 10676 : result = MIN2(result, LinearApproxHelpers::getInterpolatedValue(myDesAccelProfile, speed));
1161 : }
1162 5620512203 : if (!myMaxAccelProfile.empty()) {
1163 : // @todo maxAccel should interact with slope
1164 0 : result = MIN2(result, LinearApproxHelpers::getInterpolatedValue(myMaxAccelProfile, speed));
1165 : }
1166 5620512203 : return result;
1167 : }
1168 :
1169 : /****************************************************************************/
|
