Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 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 298446 : MSCFModel::MSCFModel(const MSVehicleType* vtype) :
56 298446 : myType(vtype),
57 298446 : myAccel(vtype->getParameter().getCFParam(SUMO_ATTR_ACCEL, SUMOVTypeParameter::getDefaultAccel(vtype->getParameter().vehicleClass))),
58 298446 : myDecel(vtype->getParameter().getCFParam(SUMO_ATTR_DECEL, SUMOVTypeParameter::getDefaultDecel(vtype->getParameter().vehicleClass))),
59 298446 : myEmergencyDecel(vtype->getParameter().getCFParam(SUMO_ATTR_EMERGENCYDECEL,
60 298446 : SUMOVTypeParameter::getDefaultEmergencyDecel(vtype->getParameter().vehicleClass, myDecel, MSGlobals::gDefaultEmergencyDecel))),
61 298446 : myApparentDecel(vtype->getParameter().getCFParam(SUMO_ATTR_APPARENTDECEL, myDecel)),
62 298446 : myCollisionMinGapFactor(vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 1)),
63 298446 : myHeadwayTime(vtype->getParameter().getCFParam(SUMO_ATTR_TAU, 1.0)),
64 298446 : myStartupDelay(TIME2STEPS(vtype->getParameter().getCFParam(SUMO_ATTR_STARTUP_DELAY, 0.0))),
65 298446 : myMaxAccelProfile(vtype->getParameter().getCFProfile(SUMO_ATTR_MAXACCEL_PROFILE, SUMOVTypeParameter::getDefaultMaxAccelProfile(vtype->getParameter().vehicleClass, myAccel))),
66 596892 : myDesAccelProfile(vtype->getParameter().getCFProfile(SUMO_ATTR_DESACCEL_PROFILE, SUMOVTypeParameter::getDefaultDesAccelProfile(vtype->getParameter().vehicleClass, myAccel)))
67 298446 : { }
68 :
69 :
70 294516 : MSCFModel::~MSCFModel() {}
71 :
72 :
73 46272 : MSCFModel::VehicleVariables::~VehicleVariables() {}
74 :
75 :
76 : double
77 12881493158 : MSCFModel::brakeGap(const double speed, const double decel, const double headwayTime) const {
78 12881493158 : if (MSGlobals::gSemiImplicitEulerUpdate) {
79 11865593150 : return brakeGapEuler(speed, decel, headwayTime);
80 : } else {
81 : // ballistic
82 1015900008 : if (speed <= 0) {
83 : return 0.;
84 : } else {
85 851105573 : return speed * (headwayTime + 0.5 * speed / decel);
86 : }
87 : }
88 : }
89 :
90 :
91 : double
92 11869440113 : 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 11869440113 : const double speedReduction = ACCEL2SPEED(decel);
96 11869440113 : const int steps = int(speed / speedReduction);
97 11869440113 : return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
98 : }
99 :
100 :
101 : double
102 793399872 : 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 793399872 : 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 745252040 : const double v = SPEED2DIST(targetSpeed);
114 745252040 : if (dist < v) {
115 : return targetSpeed;
116 : }
117 705982618 : const double b = ACCEL2DIST(decel);
118 705982618 : 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 705982618 : const double yFull = floor(y);
120 705982618 : const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
121 705982618 : const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
122 1175239307 : 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 48147832 : 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 48147832 : 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 48147832 : if (0.5 * (v0 + vT)*dt >= d) {
155 : // Attain vT after time asl
156 1505223 : return v0 + TS * (vT - v0) / actionStepLength;
157 : } else {
158 46642609 : const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
159 46642609 : const double p = 0.5 * b * dt;
160 46642609 : const double vN = -p + sqrt(p * p - q); // target speed at time t0+asl
161 46642609 : return v0 + TS * (vN - v0) / actionStepLength;
162 : }
163 : }
164 : }
165 :
166 :
167 : double
168 3286266152 : 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 3286266152 : const double maxDecel = MAX2(myDecel, leaderMaxDecel);
173 3286266152 : const double bgLeader = brakeGap(leaderSpeed, maxDecel, 0);
174 3286266152 : double secureGap = MAX2(0.0, brakeGap(speed, myDecel, myHeadwayTime) - bgLeader);
175 3286266152 : 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 983985239 : 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 971255861 : secureGap = MIN2(secureGap, secureGapDecel / veh->getLaneChangeModel().getSafetyFactor());
183 : }
184 3286266152 : return secureGap;
185 : }
186 :
187 :
188 : double
189 602826088 : MSCFModel::finalizeSpeed(MSVehicle* const veh, double vPos) const {
190 : // save old v for optional acceleration computation
191 602826088 : const double oldV = veh->getSpeed();
192 : // process stops (includes update of stopping state)
193 602826088 : const double vStop = MIN2(vPos, veh->processNextStop(vPos));
194 : // apply deceleration bounds
195 598551164 : const double vMinEmergency = minNextSpeedEmergency(oldV, veh);
196 : // vPos contains the uppper bound on safe speed. allow emergency braking here
197 598551164 : const double vMin = MIN2(minNextSpeed(oldV, veh), MAX2(vPos, vMinEmergency));
198 598551164 : const double fric = veh->getFriction();
199 : // adapt speed limit of road to "perceived" friction
200 598551164 : 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 1197102328 : double aMax = (MAX2(veh->getLane()->getVehicleMaxSpeed(veh), vPos) * factor - oldV) / veh->getActionStepLengthSecs();
206 : // apply planned speed constraints and acceleration constraints
207 598551164 : 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 598551164 : 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 598551164 : 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 598551164 : 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 598551164 : return vNext;
255 : }
256 :
257 :
258 : double
259 604313085 : 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 604313085 : if (veh->getTimeSinceStartup() > 0 && veh->getTimeSinceStartup() - DELTA_T < myStartupDelay + addTime) {
263 : assert(veh->getSpeed() <= SUMO_const_haltingSpeed);
264 46882 : const SUMOTime remainingDelay = myStartupDelay + addTime - (veh->getTimeSinceStartup() - DELTA_T);
265 : //std::cout << SIMTIME << " applyStartupDelay veh=" << veh->getID() << " remainingDelay=" << remainingDelay << "\n";
266 46882 : 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 4258550714 : MSCFModel::maxNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
315 4258550714 : return MIN2(speed + (double) ACCEL2SPEED(getCurrentAccel(speed)), myType->getMaxSpeed());
316 : }
317 :
318 :
319 : double
320 1149336964 : MSCFModel::minNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
321 1149336964 : if (MSGlobals::gSemiImplicitEulerUpdate) {
322 1094531817 : 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 54805147 : return speed - ACCEL2SPEED(myDecel);
326 : }
327 : }
328 :
329 :
330 : double
331 782643175 : MSCFModel::minNextSpeedEmergency(double speed, const MSVehicle* const /*veh*/) const {
332 782643175 : if (MSGlobals::gSemiImplicitEulerUpdate) {
333 576406096 : 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 206237079 : return speed - ACCEL2SPEED(myEmergencyDecel);
337 : }
338 : }
339 :
340 :
341 :
342 : double
343 793399862 : MSCFModel::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion, const CalcReason /*usage*/) const {
344 793399862 : if (maxSpeed < 0.) {
345 : // can occur for ballistic update (in context of driving at red light)
346 : return maxSpeed;
347 : }
348 793399857 : double vSafe = freeSpeed(speed, myDecel, seen, maxSpeed, onInsertion, veh->getActionStepLengthSecs());
349 793399857 : return vSafe;
350 : }
351 :
352 :
353 : double
354 6786566 : MSCFModel::insertionFollowSpeed(const MSVehicle* const /* v */, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
355 6786566 : if (MSGlobals::gSemiImplicitEulerUpdate) {
356 5805446 : 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 981120 : return maximumSafeFollowSpeed(gap2pred, 0., predSpeed, predMaxDecel, true);
360 : }
361 : }
362 :
363 :
364 : double
365 42382 : MSCFModel::insertionStopSpeed(const MSVehicle* const veh, double speed, double gap) const {
366 42382 : if (MSGlobals::gSemiImplicitEulerUpdate) {
367 38614 : return stopSpeed(veh, speed, gap, CalcReason::FUTURE);
368 : } else {
369 3768 : return MIN2(maximumSafeStopSpeed(gap, myDecel, 0., true, 0., false), myType->getMaxSpeed());
370 : }
371 : }
372 :
373 :
374 : double
375 446108 : 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 446108 : 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 446108 : if (MSGlobals::gSemiImplicitEulerUpdate) {
381 : // number of potential braking steps
382 376782 : const int a = (int)ceil(duration / TS - TS);
383 : // can we brake for the whole time?
384 376782 : if (brakeGap(a * myDecel, myDecel, 0) <= leaderMinDist) {
385 : // braking continuously for duration
386 : // distance reduction due to braking
387 322721 : const double b = TS * getMaxDecel() * 0.5 * (a * a - a);
388 322721 : 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 322721 : return (b + leaderMinDist) / duration;
399 : } else {
400 : // @todo improve efficiency
401 : double bg = 0;
402 : double speed = 0;
403 1360634 : while (bg < leaderMinDist) {
404 1306573 : speed += ACCEL2SPEED(myDecel);
405 1306573 : bg += SPEED2DIST(speed);
406 : }
407 54061 : speed -= DIST2SPEED(bg - leaderMinDist);
408 54061 : return speed;
409 : }
410 : } else {
411 : // can we brake for the whole time?
412 69326 : const double fullBrakingSeconds = sqrt(leaderMinDist * 2 / myDecel);
413 69326 : 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 56274 : return leaderMinDist / duration + duration * getMaxDecel() / 2;
418 : } else {
419 13052 : return fullBrakingSeconds * myDecel;
420 : }
421 : }
422 : }
423 :
424 : double
425 446108 : MSCFModel::distAfterTime(double t, double speed, const double accel) const {
426 446108 : if (accel >= 0.) {
427 0 : return (speed + 0.5 * accel * t) * t;
428 : }
429 446108 : const double decel = -accel;
430 446108 : if (speed <= decel * t) {
431 : // braking to a full stop
432 281849 : return brakeGap(speed, decel, 0);
433 : }
434 164259 : if (MSGlobals::gSemiImplicitEulerUpdate) {
435 : // @todo improve efficiency
436 : double result = 0;
437 688530 : while (t > 0) {
438 549498 : speed -= ACCEL2SPEED(decel);
439 549498 : result += MAX2(0.0, SPEED2DIST(speed));
440 549498 : t -= TS;
441 : }
442 139032 : return result;
443 : } else {
444 25227 : const double speed2 = speed - t * decel;
445 25227 : return 0.5 * (speed + speed2) * t;
446 : }
447 : }
448 :
449 :
450 : SUMOTime
451 809514415 : MSCFModel::getMinimalArrivalTime(double dist, double currentSpeed, double arrivalSpeed) const {
452 809514415 : 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 809408424 : const double accel = (arrivalSpeed >= currentSpeed) ? getMaxAccel() : -getMaxDecel();
458 809408424 : const double accelTime = accel == 0. ? 0. : (arrivalSpeed - currentSpeed) / accel;
459 809408424 : const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
460 809408424 : if (dist >= accelWay) {
461 800030738 : const double nonAccelWay = dist - accelWay;
462 : const double nonAccelSpeed = MAX3(currentSpeed, arrivalSpeed, SUMO_const_haltingSpeed);
463 800030738 : return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
464 : }
465 : // find time x so that
466 : // x * (currentSpeed + currentSpeed + x * accel) * 0.5 = dist
467 9377686 : return TIME2STEPS(-(currentSpeed - sqrt(currentSpeed * currentSpeed + 2 * accel * dist)) / accel);
468 : }
469 :
470 :
471 : double
472 295946 : MSCFModel::estimateArrivalTime(double dist, double speed, double maxSpeed, double accel) {
473 : assert(speed >= 0.);
474 : assert(dist >= 0.);
475 :
476 295946 : if (dist < NUMERICAL_EPS) {
477 : return 0.;
478 : }
479 :
480 295946 : if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
481 : // distance will never be covered with these values
482 : return INVALID_DOUBLE;
483 : }
484 :
485 117912 : if (fabs(accel) < NUMERICAL_EPS) {
486 99973 : return dist / speed;
487 : }
488 :
489 17939 : double p = speed / accel;
490 :
491 17939 : if (accel < 0.) {
492 : // we already know, that the distance will be covered despite breaking
493 17939 : return (-p - sqrt(p * p + 2 * dist / accel));
494 : }
495 :
496 : // Here, accel > 0
497 : // t1 is the time to use the given acceleration
498 0 : double t1 = (maxSpeed - speed) / accel;
499 : // distance covered until t1
500 0 : double d1 = speed * t1 + 0.5 * accel * t1 * t1;
501 0 : if (d1 >= dist) {
502 : // dist is covered before changing the speed
503 0 : return (-p + sqrt(p * p + 2 * dist / accel));
504 : } else {
505 0 : return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
506 : }
507 :
508 : }
509 :
510 : double
511 68799 : MSCFModel::estimateArrivalTime(double dist, double initialSpeed, double arrivalSpeed, double maxSpeed, double accel, double decel) {
512 : UNUSED_PARAMETER(arrivalSpeed); // only in assertion
513 : UNUSED_PARAMETER(decel); // only in assertion
514 68799 : if (dist <= 0) {
515 : return 0.;
516 : }
517 :
518 : // stub-assumptions
519 : assert(accel == decel);
520 : assert(accel > 0);
521 : assert(initialSpeed == 0);
522 : assert(arrivalSpeed == 0);
523 : assert(maxSpeed > 0);
524 :
525 :
526 68799 : double accelTime = (maxSpeed - initialSpeed) / accel;
527 : // "ballistic" estimate for the distance covered during acceleration phase
528 68799 : double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
529 : double arrivalTime;
530 68799 : if (accelDist >= dist * 0.5) {
531 : // maximal speed will not be attained during maneuver
532 2958 : arrivalTime = 4 * sqrt(dist / accel);
533 : } else {
534 : // Calculate time to move with constant, maximal lateral speed
535 65841 : const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
536 65841 : arrivalTime = accelTime + constSpeedTime;
537 : }
538 : return arrivalTime;
539 : }
540 :
541 :
542 : double
543 693584 : MSCFModel::avoidArrivalAccel(double dist, double time, double speed, double maxDecel) {
544 : assert(time > 0 || dist == 0);
545 693584 : if (dist <= 0) {
546 612 : return -maxDecel;
547 692972 : } else if (time * speed > 2 * dist) {
548 : // stop before dist is necessary. We need
549 : // d = v*v/(2*a)
550 4883 : return - 0.5 * speed * speed / dist;
551 : } else {
552 : // we seek the solution a of
553 : // d = v*t + a*t*t/2
554 688089 : return 2 * (dist / time - speed) / time;
555 : }
556 : }
557 :
558 :
559 : double
560 2659136 : MSCFModel::getMinimalArrivalSpeed(double dist, double currentSpeed) const {
561 : // ballistic update
562 2659136 : return estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
563 : }
564 :
565 :
566 : double
567 50910773 : MSCFModel::getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const {
568 : double arrivalSpeedBraking;
569 : // Because we use a continuous formula for computing the possible slow-down
570 : // we need to handle the mismatch with the discrete dynamics
571 50910773 : if (dist < currentSpeed) {
572 : arrivalSpeedBraking = INVALID_SPEED; // no time left for braking after this step
573 : // (inserted max() to get rid of arrivalSpeed dependency within method) (Leo)
574 26199078 : } else if (2 * (dist - currentSpeed * getHeadwayTime()) * -getMaxDecel() + currentSpeed * currentSpeed >= 0) {
575 26197770 : arrivalSpeedBraking = estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
576 : } else {
577 : arrivalSpeedBraking = getMaxDecel();
578 : }
579 50910773 : return arrivalSpeedBraking;
580 : }
581 :
582 :
583 :
584 :
585 : double
586 24573920 : MSCFModel::gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1, double a2, const double maxV1, const double maxV2) {
587 :
588 : double newGap = currentGap;
589 :
590 24573920 : if (MSGlobals::gSemiImplicitEulerUpdate) {
591 20252254 : for (unsigned int steps = 1; steps * TS <= duration; ++steps) {
592 0 : v1 = MIN2(MAX2(v1 + a1, 0.), maxV1);
593 0 : v2 = MIN2(MAX2(v2 + a2, 0.), maxV2);
594 0 : newGap += TS * (v1 - v2);
595 : }
596 : } else {
597 : // determine times t1, t2 for which vehicles can break until stop (within duration)
598 : // and t3, t4 for which they reach their maximal speed on their current lanes.
599 4321666 : double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
600 :
601 : // t1: ego veh stops
602 4321666 : if (a1 < 0 && v1 > 0) {
603 1431538 : const double leaderStopTime = - v1 / a1;
604 1431538 : t1 = MIN2(leaderStopTime, duration);
605 4321666 : } else if (a1 >= 0) {
606 2822419 : t1 = duration;
607 : }
608 : // t2: veh2 stops
609 4321666 : if (a2 < 0 && v2 > 0) {
610 156097 : const double followerStopTime = -v2 / a2;
611 156097 : t2 = MIN2(followerStopTime, duration);
612 4321666 : } else if (a2 >= 0) {
613 4131690 : t2 = duration;
614 : }
615 : // t3: ego veh reaches vMax
616 4321666 : if (a1 > 0 && v1 < maxV1) {
617 1761874 : const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
618 1761874 : t3 = MIN2(leaderMaxSpeedTime, duration);
619 4321666 : } else if (a1 <= 0) {
620 2559792 : t3 = duration;
621 : }
622 : // t4: veh2 reaches vMax
623 4321666 : if (a2 > 0 && v2 < maxV2) {
624 1688903 : const double followerMaxSpeedTime = (maxV2 - v2) / a2;
625 1688903 : t4 = MIN2(followerMaxSpeedTime, duration);
626 4321666 : } else if (a2 <= 0) {
627 2632763 : t4 = duration;
628 : }
629 :
630 : // NOTE: this assumes that the accelerations a1 and a2 are constant over the next
631 : // followerBreakTime seconds (if no vehicle stops before or reaches vMax)
632 : std::list<double> l;
633 : l.push_back(t1);
634 : l.push_back(t2);
635 : l.push_back(t3);
636 : l.push_back(t4);
637 4321666 : l.sort();
638 : std::list<double>::const_iterator i;
639 : double tLast = 0.;
640 4533944 : for (i = l.begin(); i != l.end(); ++i) {
641 4533944 : if (*i != tLast) {
642 2502470 : double dt = MIN2(*i, duration) - tLast; // time between *i and tLast
643 2502470 : double dv = v1 - v2; // current velocity difference
644 2502470 : double da = a1 - a2; // current acceleration difference
645 2502470 : newGap += dv * dt + da * dt * dt / 2.; // update gap
646 2502470 : v1 += dt * a1;
647 2502470 : v2 += dt * a2;
648 : }
649 4533944 : if (*i == t1 || *i == t3) {
650 : // ego veh reached velocity bound
651 : a1 = 0.;
652 : }
653 :
654 4533944 : if (*i == t2 || *i == t4) {
655 : // veh2 reached velocity bound
656 : a2 = 0.;
657 : }
658 :
659 : tLast = MIN2(*i, duration);
660 4533944 : if (tLast == duration) {
661 : break;
662 : }
663 : }
664 :
665 4321666 : if (duration != tLast) {
666 : // (both vehicles have zero acceleration)
667 : assert(a1 == 0. && a2 == 0.);
668 0 : double dt = duration - tLast; // remaining time until duration
669 0 : double dv = v1 - v2; // current velocity difference
670 0 : newGap += dv * dt; // update gap
671 : }
672 : }
673 :
674 24573920 : return newGap;
675 : }
676 :
677 :
678 :
679 : double
680 47659400 : MSCFModel::passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed) {
681 :
682 : assert(passedPos <= currentPos);
683 : assert(passedPos >= lastPos);
684 : assert(currentPos > lastPos);
685 : assert(currentSpeed >= 0);
686 :
687 47659400 : if (passedPos > currentPos || passedPos < lastPos) {
688 0 : std::stringstream ss;
689 : // Debug (Leo)
690 0 : if (!MSGlobals::gSemiImplicitEulerUpdate) {
691 : // NOTE: error is guarded to maintain original test output for euler update (Leo).
692 0 : ss << "passingTime(): given argument passedPos = " << passedPos << " doesn't lie within [lastPos, currentPos] = [" << lastPos << ", " << currentPos << "]\nExtrapolating...";
693 0 : std::cout << ss.str() << "\n";
694 0 : WRITE_ERROR(ss.str());
695 : }
696 0 : const double lastCoveredDist = currentPos - lastPos;
697 0 : const double extrapolated = passedPos > currentPos ? TS * (passedPos - lastPos) / lastCoveredDist : TS * (currentPos - passedPos) / lastCoveredDist;
698 : return extrapolated;
699 47659400 : } else if (currentSpeed < 0) {
700 0 : WRITE_ERROR("passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
701 0 : return -1;
702 : }
703 :
704 47659400 : const double distanceOldToPassed = passedPos - lastPos; // assert: >=0
705 :
706 47659400 : if (MSGlobals::gSemiImplicitEulerUpdate) {
707 : // euler update (constantly moving with currentSpeed during [0,TS])
708 46234856 : if (currentSpeed == 0) {
709 0 : return TS;
710 : }
711 46234856 : const double t = distanceOldToPassed / currentSpeed;
712 46234856 : return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
713 :
714 : } else {
715 : // ballistic update (constant acceleration a during [0,TS], except in case of a stop)
716 :
717 : // determine acceleration
718 : double a;
719 1424544 : if (currentSpeed > 0) {
720 : // the acceleration was constant within the last time step
721 1424396 : a = SPEED2ACCEL(currentSpeed - lastSpeed);
722 : } else {
723 : // the currentSpeed is zero (the last was not because lastPos<currentPos).
724 : assert(currentSpeed == 0 && lastSpeed != 0);
725 : // In general the stop has taken place within the last time step.
726 : // The acceleration (a<0) is obtained from
727 : // deltaPos = - lastSpeed^2/(2*a)
728 148 : a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
729 :
730 : assert(a < 0);
731 : }
732 :
733 : // determine passing time t
734 : // we solve distanceOldToPassed = lastSpeed*t + a*t^2/2
735 1424544 : if (fabs(a) < NUMERICAL_EPS) {
736 : // treat as constant speed within [0, TS]
737 359974 : const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
738 359974 : return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
739 1064570 : } else if (a > 0) {
740 : // positive acceleration => only one positive solution
741 572979 : const double va = lastSpeed / a;
742 572979 : const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
743 : assert(t < 1 && t >= 0);
744 572979 : return t;
745 : } else {
746 : // negative acceleration => two positive solutions (pick the smaller one.)
747 491591 : const double va = lastSpeed / a;
748 491591 : const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
749 : // emergency braking at red light could give results out of the admissible result range
750 : // because the dynamics are euler-like (full forward speed with instant deceleration)
751 491591 : return MIN2(TS, MAX2(0., t));
752 : }
753 : }
754 : }
755 :
756 :
757 : double
758 61679186 : MSCFModel::speedAfterTime(const double t, const double v0, const double dist) {
759 : assert(dist >= 0);
760 : assert(t >= 0 && t <= TS);
761 61679186 : if (MSGlobals::gSemiImplicitEulerUpdate) {
762 : // euler: constant speed within [0,TS]
763 60617769 : return DIST2SPEED(dist);
764 : } else {
765 : // ballistic: piecewise constant acceleration in [0,TS] (may become 0 for a stop within TS)
766 : // We reconstruct acceleration at time t=0. Note that the covered distance in case
767 : // of a stop exactly at t=TS is TS*v0/2.
768 1061417 : if (dist < TS * v0 / 2) {
769 : // stop must have occurred within [0,TS], use dist = -v0^2/(2a) (stopping dist),
770 : // i.e., a = -v0^2/(2*dist)
771 82 : const double accel = - v0 * v0 / (2 * dist);
772 : // The speed at time t is then
773 82 : return v0 + accel * t;
774 : } else {
775 : // no stop occurred within [0,TS], thus (from dist = v0*TS + accel*TS^2/2)
776 1061335 : const double accel = 2 * (dist / TS - v0) / TS;
777 : // The speed at time t is then
778 1061335 : return v0 + accel * t;
779 : }
780 : }
781 : }
782 :
783 :
784 :
785 :
786 : double
787 1973399304 : MSCFModel::estimateSpeedAfterDistance(const double dist, const double v, const double accel) const {
788 : // dist=v*t + 0.5*accel*t^2, solve for t and use v1 = v + accel*t
789 1973399304 : return MIN2(myType->getMaxSpeed(),
790 1973399304 : (double)sqrt(MAX2(0., 2 * dist * accel + v * v)));
791 : }
792 :
793 :
794 :
795 : double
796 3055574976 : MSCFModel::maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion, double headway, bool relaxEmergency) const {
797 : double vsafe;
798 3055574976 : if (MSGlobals::gSemiImplicitEulerUpdate) {
799 2812528046 : vsafe = maximumSafeStopSpeedEuler(gap, decel, onInsertion, headway);
800 : } else {
801 243046930 : vsafe = maximumSafeStopSpeedBallistic(gap, decel, currentSpeed, onInsertion, headway);
802 : }
803 :
804 3055574976 : if (relaxEmergency && myDecel != myEmergencyDecel) {
805 : #ifdef DEBUG_EMERGENCYDECEL
806 : if (true) {
807 : std::cout << SIMTIME << " maximumSafeStopSpeed()"
808 : << " g=" << gap
809 : << " v=" << currentSpeed
810 : << " initial vsafe=" << vsafe << "(decel=" << SPEED2ACCEL(v - vsafe) << ")" << std::endl;
811 : }
812 : #endif
813 :
814 748849735 : double origSafeDecel = SPEED2ACCEL(currentSpeed - vsafe);
815 748849735 : if (origSafeDecel > myDecel + NUMERICAL_EPS) {
816 : // emergency deceleration required
817 :
818 : #ifdef DEBUG_EMERGENCYDECEL
819 : if (true) {
820 : std::cout << SIMTIME << " maximumSafeStopSpeed() results in emergency deceleration "
821 : << "initial vsafe=" << vsafe << " egoSpeed=" << v << "(decel=" << SPEED2ACCEL(v - vsafe) << ")" << std::endl;
822 : }
823 : #endif
824 :
825 22476775 : double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, currentSpeed, 0., 1);
826 : // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
827 22476775 : safeDecel = MAX2(safeDecel, myDecel);
828 : // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
829 : safeDecel = MIN2(safeDecel, origSafeDecel);
830 22476775 : vsafe = currentSpeed - ACCEL2SPEED(safeDecel);
831 22476775 : if (MSGlobals::gSemiImplicitEulerUpdate) {
832 : vsafe = MAX2(vsafe, 0.);
833 : }
834 :
835 : #ifdef DEBUG_EMERGENCYDECEL
836 : if (true) {
837 : std::cout << " -> corrected emergency deceleration: " << SPEED2ACCEL(v - vsafe) << std::endl;
838 : }
839 : #endif
840 :
841 : }
842 : }
843 :
844 3055574976 : return vsafe;
845 : }
846 :
847 :
848 : double
849 2812528046 : MSCFModel::maximumSafeStopSpeedEuler(double gap, double decel, bool /* onInsertion */, double headway) const {
850 : // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
851 2812528046 : const double g = gap - NUMERICAL_EPS;
852 2812528046 : if (g < 0.) {
853 : return 0.;
854 : }
855 2728576255 : const double b = ACCEL2SPEED(decel);
856 2728576255 : const double t = headway >= 0 ? headway : myHeadwayTime;
857 : const double s = TS;
858 :
859 : // h = the distance that would be covered if it were possible to stop
860 : // exactly after gap and decelerate with b every simulation step
861 : // h = 0.5 * n * (n-1) * b * s + n * b * t (solve for n)
862 : //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));
863 2728576255 : const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
864 2728576255 : const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
865 : assert(h <= g + NUMERICAL_EPS);
866 : // compute the additional speed that must be used during deceleration to fix
867 : // the discrepancy between g and h
868 2728576255 : const double r = (g - h) / (n * s + t);
869 2728576255 : const double x = n * b + r;
870 : assert(x >= 0);
871 2728576255 : return x;
872 : // return onInsertion ? x + b: x; // see #2574
873 : }
874 :
875 :
876 : double
877 243810243 : MSCFModel::maximumSafeStopSpeedBallistic(double gap, double decel, double currentSpeed, bool onInsertion, double headway) const {
878 : // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
879 243810243 : const double g = MAX2(0., gap - NUMERICAL_EPS);
880 243810243 : headway = headway >= 0 ? headway : myHeadwayTime;
881 :
882 : // (Leo) Note that in contrast to the Euler update, for the ballistic update
883 : // the distance covered in the coming step depends on the current velocity, in general.
884 : // one exception is the situation when the vehicle is just being inserted.
885 : // In that case, it will not cover any distance until the next timestep by convention.
886 :
887 : // We treat the latter case first:
888 243810243 : if (onInsertion) {
889 : // The distance covered with constant insertion speed v0 until time tau is given as
890 : // G1 = tau*v0
891 : // The distance covered between time tau and the stopping moment at time tau+v0/b is
892 : // G2 = v0^2/(2b),
893 : // where b is an assumed constant deceleration (= decel)
894 : // We solve g = G1 + G2 for v0:
895 5495558 : const double btau = decel * headway;
896 5495558 : const double v0 = -btau + sqrt(btau * btau + 2 * decel * g);
897 5495558 : return v0;
898 : }
899 :
900 : // In the usual case during the driving task, the vehicle goes by
901 : // a current speed v0=v, and we seek to determine a safe acceleration a (possibly <0)
902 : // such that starting to break after accelerating with a for the time tau=headway
903 : // still allows us to stop in time.
904 :
905 238314685 : const double tau = headway == 0 ? TS : headway;
906 : const double v0 = MAX2(0., currentSpeed);
907 : // We first consider the case that a stop has to take place within time tau
908 : // (the distance driven when decelerating from v0 to 0 in tau is v0 * tau / 2)
909 238314685 : if (g <= v0 * tau * 0.5) {
910 15976283 : if (g == 0.) {
911 8449679 : if (v0 > 0.) {
912 : // indicate to brake as hard as possible
913 6542102 : return -ACCEL2SPEED(myEmergencyDecel);
914 : } else {
915 : // stay stopped
916 : return 0.;
917 : }
918 : }
919 : // In general we solve g = v0^2/(-2a), where the rhs is the distance
920 : // covered until stop when braking with a<0
921 7526604 : const double a = -v0 * v0 / (2 * g);
922 7526604 : return v0 + a * TS;
923 : }
924 :
925 : // The last case corresponds to a situation, where the vehicle may go with a positive
926 : // speed v1 = v0 + tau*a after time tau. (v1 is the maximum possible speed
927 : // for this and unconstrained by current speed or acceleration limits)
928 : //
929 : // The distance covered until time tau is given as
930 : // G1 = tau*(v0+v1)/2
931 : // The distance covered between time tau and the stopping moment at time tau+v1/b is
932 : // G2 = v1^2/(2b),
933 : // where b is an assumed constant deceleration (= decel)
934 : // We solve g = G1 + G2 for v1>0:
935 : // <=> 0 = v1^2 + b*tau*v1 + b*tau*v0 - 2bg
936 : // => v1 = -b*tau/2 + sqrt( (b*tau)^2/4 + b(2g - tau*v0) )
937 :
938 222338402 : const double btau2 = decel * tau / 2;
939 222338402 : const double v1 = -btau2 + sqrt(btau2 * btau2 + decel * (2 * g - tau * v0));
940 222338402 : const double a = (v1 - v0) / tau;
941 222338402 : return v0 + a * TS;
942 : }
943 :
944 :
945 : /** Returns the SK-vsafe. */
946 : double
947 2257726032 : MSCFModel::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
948 : // the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
949 : // the leaders starts braking hard until stopped
950 : // unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
951 : // (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
952 : // 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.
953 : // @todo: this is a conservative estimate for safe speed which could be increased
954 :
955 : // // For negative gaps, we return the lowest meaningful value by convention
956 : // // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)
957 :
958 : // // It must be done. Otherwise, negative gaps at high speeds can create nonsense results from the call to maximumSafeStopSpeed() below
959 :
960 : // if(gap<0){
961 : // if(MSGlobals::gSemiImplicitEulerUpdate){
962 : // return 0.;
963 : // } else {
964 : // return -INVALID_SPEED;
965 : // }
966 : // }
967 :
968 : // The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
969 : // if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
970 : // NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
971 : // the follower accelerates for a short period of time. Refs #2310 (Leo)
972 : // const double headway = predSpeed > 0. ? myHeadwayTime : 0.;
973 :
974 2257726032 : const double headway = myHeadwayTime;
975 : double x;
976 2257726032 : if (gap >= 0 || MSGlobals::gComputeLC) {
977 4401351790 : x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), myDecel, egoSpeed, onInsertion, headway, false);
978 : } else {
979 736818 : x = egoSpeed - ACCEL2SPEED(myEmergencyDecel);
980 736818 : if (MSGlobals::gSemiImplicitEulerUpdate) {
981 : x = MAX2(x, 0.);
982 : }
983 : }
984 :
985 2257726032 : if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
986 1433332184 : double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
987 1433332184 : if (origSafeDecel > myDecel + NUMERICAL_EPS) {
988 : // Braking harder than myDecel was requested -> calculate required emergency deceleration.
989 : // Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
990 : // 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,
991 : // such that braking harder than myDecel is required.
992 :
993 3420264 : double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
994 : #ifdef DEBUG_EMERGENCYDECEL
995 : if (DEBUG_COND2) {
996 : std::cout << SIMTIME << " initial vsafe=" << x
997 : << " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
998 : << " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
999 : << " safeDecel=" << safeDecel
1000 : << std::endl;
1001 : }
1002 : #endif
1003 : // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
1004 3420264 : safeDecel = MAX2(safeDecel, myDecel);
1005 : // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
1006 : safeDecel = MIN2(safeDecel, origSafeDecel);
1007 3420264 : x = egoSpeed - ACCEL2SPEED(safeDecel);
1008 3420264 : if (MSGlobals::gSemiImplicitEulerUpdate) {
1009 : x = MAX2(x, 0.);
1010 : }
1011 :
1012 : #ifdef DEBUG_EMERGENCYDECEL
1013 : if (DEBUG_COND2) {
1014 : std::cout << " -> corrected emergency deceleration: " << safeDecel << " newVSafe=" << x << std::endl;
1015 : }
1016 : #endif
1017 :
1018 : }
1019 : }
1020 : assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
1021 : assert(!std::isnan(x));
1022 2257726032 : return x;
1023 : }
1024 :
1025 :
1026 : double
1027 25897039 : MSCFModel::calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const {
1028 : // There are two cases:
1029 : // 1) Either, stopping in time is possible with a deceleration b <= predMaxDecel, then this value is returned
1030 : // 2) Or, b > predMaxDecel is required in this case the minimal value b allowing to stop safely under the assumption maxPredDecel=b is returned
1031 25897039 : if (gap <= 0.) {
1032 2181315 : return myEmergencyDecel;
1033 : }
1034 :
1035 : // Apparent braking distance for the leader
1036 23715724 : const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
1037 : // Required deceleration according to case 1)
1038 23715724 : const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
1039 :
1040 : #ifdef DEBUG_EMERGENCYDECEL
1041 : if (DEBUG_COND2) {
1042 : std::cout << SIMTIME << " calculateEmergencyDeceleration()"
1043 : << " gap=" << gap << " egoSpeed=" << egoSpeed << " predSpeed=" << predSpeed
1044 : << " predBrakeDist=" << predBrakeDist
1045 : << " b1=" << b1
1046 : << std::endl;
1047 : }
1048 : #endif
1049 :
1050 23715724 : if (b1 <= predMaxDecel) {
1051 : // Case 1) applies
1052 : #ifdef DEBUG_EMERGENCYDECEL
1053 : if (DEBUG_COND2) {
1054 : std::cout << " case 1 ..." << std::endl;
1055 : }
1056 : #endif
1057 : return b1;
1058 : }
1059 : #ifdef DEBUG_EMERGENCYDECEL
1060 : if (DEBUG_COND2) {
1061 : std::cout << " case 2 ...";
1062 : }
1063 : #endif
1064 :
1065 : // Case 2) applies
1066 : // Required deceleration according to case 2)
1067 22956481 : const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
1068 :
1069 : #ifdef DEBUG_EMERGENCYDECEL
1070 : if (DEBUG_COND2) {
1071 : std::cout << " b2=" << b2 << std::endl;
1072 : }
1073 : #endif
1074 22956481 : return b2;
1075 : }
1076 :
1077 :
1078 : void
1079 598798753 : MSCFModel::applyOwnSpeedPerceptionError(const MSVehicle* const veh, double& speed) const {
1080 598798753 : if (!veh->hasDriverState()) {
1081 : return;
1082 : }
1083 768240 : speed = veh->getDriverState()->getPerceivedOwnSpeed(speed);
1084 : }
1085 :
1086 :
1087 : void
1088 2309241666 : MSCFModel::applyHeadwayAndSpeedDifferencePerceptionErrors(const MSVehicle* const veh, double speed, double& gap, double& predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
1089 : UNUSED_PARAMETER(speed);
1090 : UNUSED_PARAMETER(predMaxDecel);
1091 2309241666 : if (!veh->hasDriverState()) {
1092 : return;
1093 : }
1094 :
1095 : // Obtain perceived gap and headway from the driver state
1096 908818 : const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap, pred);
1097 908818 : const double perceivedSpeedDifference = veh->getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
1098 :
1099 : #ifdef DEBUG_DRIVER_ERRORS
1100 : if (DEBUG_COND) {
1101 : if (!veh->getDriverState()->debugLocked()) {
1102 : veh->getDriverState()->lockDebug();
1103 : std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
1104 : << " speed=" << speed << " gap=" << gap << " leaderSpeed=" << predSpeed
1105 : << "\n perceivedGap=" << perceivedGap << " perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
1106 : << " perceivedSpeedDifference=" << perceivedSpeedDifference
1107 : << std::endl;
1108 : const double exactFollowSpeed = followSpeed(veh, speed, gap, predSpeed, predMaxDecel, pred, CalcReason::FUTURE);
1109 : const double errorFollowSpeed = followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel, pred, CalcReason::FUTURE);
1110 : const double accelError = SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
1111 : std::cout << " gapError=" << perceivedGap - gap << " dvError=" << perceivedSpeedDifference - (predSpeed - speed)
1112 : << "\n resulting accelError: " << accelError << std::endl;
1113 : veh->getDriverState()->unlockDebug();
1114 : }
1115 : }
1116 : #endif
1117 :
1118 908818 : gap = perceivedGap;
1119 908818 : predSpeed = speed + perceivedSpeedDifference;
1120 : }
1121 :
1122 :
1123 : void
1124 750895111 : MSCFModel::applyHeadwayPerceptionError(const MSVehicle* const veh, double speed, double& gap) const {
1125 : UNUSED_PARAMETER(speed);
1126 750895111 : if (!veh->hasDriverState()) {
1127 : return;
1128 : }
1129 : // @todo: Provide objectID (e.g. pointer address for the relevant object at the given distance(gap))
1130 : // This is for item related management of known object and perception updates when the distance
1131 : // changes significantly. (Should not be too important for stationary objects though.)
1132 :
1133 : // Obtain perceived gap from driver state
1134 234889 : const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap);
1135 :
1136 : #ifdef DEBUG_DRIVER_ERRORS
1137 : if (DEBUG_COND) {
1138 : if (!veh->getDriverState()->debugLocked()) {
1139 : veh->getDriverState()->lockDebug();
1140 : std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
1141 : << " speed=" << speed << " gap=" << gap << "\n perceivedGap=" << perceivedGap << std::endl;
1142 : const double exactStopSpeed = stopSpeed(veh, speed, gap, CalcReason::FUTURE);
1143 : const double errorStopSpeed = stopSpeed(veh, speed, perceivedGap, CalcReason::FUTURE);
1144 : const double accelError = SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1145 : std::cout << " gapError=" << perceivedGap - gap << "\n resulting accelError: " << accelError << std::endl;
1146 : veh->getDriverState()->unlockDebug();
1147 : }
1148 : }
1149 : #endif
1150 :
1151 234889 : gap = perceivedGap;
1152 : }
1153 :
1154 :
1155 : double
1156 4782901123 : MSCFModel::getCurrentAccel(const double speed) const {
1157 4782901123 : double result = myAccel;
1158 4782901123 : if (!myDesAccelProfile.empty()) {
1159 9636 : result = MIN2(result, LinearApproxHelpers::getInterpolatedValue(myDesAccelProfile, speed));
1160 : }
1161 4782901123 : if (!myMaxAccelProfile.empty()) {
1162 : // @todo maxAccel should interact with slope
1163 0 : result = MIN2(result, LinearApproxHelpers::getInterpolatedValue(myMaxAccelProfile, speed));
1164 : }
1165 4782901123 : return result;
1166 : }
1167 :
1168 : /****************************************************************************/
|
