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