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