Eclipse SUMO - Simulation of Urban MObility
MSCFModel.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3// Copyright (C) 2001-2022 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/****************************************************************************/
23// The car-following model abstraction
24/****************************************************************************/
25#include <config.h>
26
27#include <cmath>
28#include <microsim/MSGlobals.h>
30#include <microsim/MSVehicle.h>
31#include <microsim/MSNet.h>
32#include <microsim/MSLane.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// ===========================================================================
56 myType(vtype),
57 myAccel(vtype->getParameter().getCFParam(SUMO_ATTR_ACCEL, SUMOVTypeParameter::getDefaultAccel(vtype->getParameter().vehicleClass))),
58 myDecel(vtype->getParameter().getCFParam(SUMO_ATTR_DECEL, SUMOVTypeParameter::getDefaultDecel(vtype->getParameter().vehicleClass))),
59 myEmergencyDecel(vtype->getParameter().getCFParam(SUMO_ATTR_EMERGENCYDECEL,
60 SUMOVTypeParameter::getDefaultEmergencyDecel(vtype->getParameter().vehicleClass, myDecel, MSGlobals::gDefaultEmergencyDecel))),
61 myApparentDecel(vtype->getParameter().getCFParam(SUMO_ATTR_APPARENTDECEL, myDecel)),
62 myCollisionMinGapFactor(vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 1)),
63 myHeadwayTime(vtype->getParameter().getCFParam(SUMO_ATTR_TAU, 1.0)),
64 myStartupDelay(TIME2STEPS(vtype->getParameter().getCFParam(SUMO_ATTR_STARTUP_DELAY, 0.0)))
65{ }
66
67
69
70
72
73
74double
75MSCFModel::brakeGap(const double speed, const double decel, const double headwayTime) const {
77 return brakeGapEuler(speed, decel, headwayTime);
78 } else {
79 // ballistic
80 if (speed <= 0) {
81 return 0.;
82 } else {
83 return speed * (headwayTime + 0.5 * speed / decel);
84 }
85 }
86}
87
88
89double
90MSCFModel::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 const double speedReduction = ACCEL2SPEED(decel);
94 const int steps = int(speed / speedReduction);
95 return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
96}
97
98
99double
100MSCFModel::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
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 const double v = SPEED2DIST(targetSpeed);
112 if (dist < v) {
113 return targetSpeed;
114 }
115 const double b = ACCEL2DIST(decel);
116 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 const double yFull = floor(y);
118 const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
119 const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
120 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 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 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 if (0.5 * (v0 + vT)*dt >= d) {
153 // Attain vT after time asl
154 return v0 + TS * (vT - v0) / actionStepLength;
155 } else {
156 const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
157 const double p = 0.5 * b * dt;
158 const double vN = -p + sqrt(p * p - q); // target speed at time t0+asl
159 return v0 + TS * (vN - v0) / actionStepLength;
160 }
161 }
162}
163
164double
165MSCFModel::finalizeSpeed(MSVehicle* const veh, double vPos) const {
166 // save old v for optional acceleration computation
167 const double oldV = veh->getSpeed();
168 // process stops (includes update of stopping state)
169 const double vStop = MIN2(vPos, veh->processNextStop(vPos));
170 // apply deceleration bounds
171 const double vMinEmergency = minNextSpeedEmergency(oldV, veh);
172 // vPos contains the uppper bound on safe speed. allow emergency braking here
173 const double vMin = MIN2(minNextSpeed(oldV, veh), MAX2(vPos, vMinEmergency));
174 const double fric = veh->getFriction();
175 // adapt speed limit of road to "perceived" friction
176 const double factor = fric == 1. ? 1. : -0.3491 * fric * fric + 0.8922 * fric + 0.4493; //2nd degree polyfit
177
178 // aMax: Maximal admissible acceleration until the next action step, such that the vehicle's maximal
179 // desired speed on the current lane will not be exceeded when the
180 // acceleration is maintained until the next action step.
181 double aMax = (veh->getLane()->getVehicleMaxSpeed(veh) * factor - oldV) / veh->getActionStepLengthSecs();
182 // apply planned speed constraints and acceleration constraints
183 double vMax = MIN3(oldV + ACCEL2SPEED(aMax), maxNextSpeed(oldV, veh), vStop);
184 // do not exceed max decel even if it is unsafe
185#ifdef _DEBUG
186 //if (vMin > vMax) {
187 // WRITE_WARNING("Maximum speed of vehicle '" + veh->getID() + "' is lower than the minimum speed (min: " + toString(vMin) + ", max: " + toString(vMax) + ").");
188 //}
189#endif
190
191#ifdef DEBUG_FINALIZE_SPEED
192 if (DEBUG_COND) {
193 std::cout << "\n" << SIMTIME << " FINALIZE_SPEED\n";
194 }
195#endif
196
197 vMax = MAX2(vMin, vMax);
198 double vNext = patchSpeedBeforeLC(veh, vMin, vMax);
199#ifdef DEBUG_FINALIZE_SPEED
200 double vDawdle = vNext;
201#endif
202 assert(vNext >= vMin);
203 assert(vNext <= vMax);
204 // apply lane-changing related speed adaptations
205 vNext = veh->getLaneChangeModel().patchSpeed(vMin, vNext, vMax, *this);
206#ifdef DEBUG_FINALIZE_SPEED
207 double vPatchLC = vNext;
208#endif
209 // apply further speed adaptations
210 vNext = applyStartupDelay(veh, vMin, vNext);
211
212 assert(vNext >= vMinEmergency); // stronger braking is permitted in lane-changing related emergencies
213 assert(vNext <= vMax);
214
215#ifdef DEBUG_FINALIZE_SPEED
216 if (DEBUG_COND) {
217 std::cout << std::setprecision(gPrecision)
218 << "veh '" << veh->getID() << "' oldV=" << oldV
219 << " vPos" << vPos
220 << " vMin=" << vMin
221 << " aMax=" << aMax
222 << " vMax=" << vMax
223 << " vStop=" << vStop
224 << " vDawdle=" << vDawdle
225 << " vPatchLC=" << vPatchLC
226 << " vNext=" << vNext
227 << "\n";
228 }
229#endif
230 return vNext;
231}
232
233
234double
235MSCFModel::applyStartupDelay(const MSVehicle* veh, const double vMin, const double vMax, const SUMOTime addTime) const {
236 UNUSED_PARAMETER(vMin);
237 // timeSinceStartup was already incremented by DELTA_T
238 if (veh->getTimeSinceStartup() > 0 && veh->getTimeSinceStartup() - DELTA_T < myStartupDelay + addTime) {
239 assert(veh->getSpeed() <= SUMO_const_haltingSpeed);
240 const SUMOTime remainingDelay = myStartupDelay + addTime - (veh->getTimeSinceStartup() - DELTA_T);
241 //std::cout << SIMTIME << " applyStartupDelay veh=" << veh->getID() << " remainingDelay=" << remainingDelay << "\n";
242 if (remainingDelay >= DELTA_T) {
243 // delay startup by at least a whole step
244 return 0;
245 } else {
246 // reduce acceleration for fractional startup delay
247 return (double)(DELTA_T - remainingDelay) / (double)DELTA_T * vMax;
248 }
249 }
250 return vMax;
251}
252
253
254double
255MSCFModel::interactionGap(const MSVehicle* const veh, double vL) const {
256 // Resolve the vsafe equation to gap. Assume predecessor has
257 // speed != 0 and that vsafe will be the current speed plus acceleration,
258 // i.e that with this gap there will be no interaction.
259 const double vNext = MIN2(maxNextSpeed(veh->getSpeed(), veh), veh->getLane()->getVehicleMaxSpeed(veh));
260 const double gap = (vNext - vL) *
261 ((veh->getSpeed() + vL) / (2.*myDecel) + myHeadwayTime) +
262 vL * myHeadwayTime;
263
264 // Don't allow timeHeadWay < deltaT situations.
265 return MAX2(gap, SPEED2DIST(vNext));
266}
267
268
269double
270MSCFModel::maxNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
271 return MIN2(speed + (double) ACCEL2SPEED(getMaxAccel()), myType->getMaxSpeed());
272}
273
274
275double
276MSCFModel::minNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
278 return MAX2(speed - ACCEL2SPEED(myDecel), 0.);
279 } else {
280 // NOTE: ballistic update allows for negative speeds to indicate a stop within the next timestep
281 return speed - ACCEL2SPEED(myDecel);
282 }
283}
284
285
286double
287MSCFModel::minNextSpeedEmergency(double speed, const MSVehicle* const /*veh*/) const {
289 return MAX2(speed - ACCEL2SPEED(myEmergencyDecel), 0.);
290 } else {
291 // NOTE: ballistic update allows for negative speeds to indicate a stop within the next timestep
292 return speed - ACCEL2SPEED(myEmergencyDecel);
293 }
294}
295
296
297
298double
299MSCFModel::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion, const CalcReason /*usage*/) const {
300 if (maxSpeed < 0.) {
301 // can occur for ballistic update (in context of driving at red light)
302 return maxSpeed;
303 }
304 double vSafe = freeSpeed(speed, myDecel, seen, maxSpeed, onInsertion, veh->getActionStepLengthSecs());
305 return vSafe;
306}
307
308
309double
310MSCFModel::insertionFollowSpeed(const MSVehicle* const /* v */, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
312 return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
313 } else {
314 // NOTE: Even for ballistic update, the current speed is irrelevant at insertion, therefore passing 0. (Leo)
315 return maximumSafeFollowSpeed(gap2pred, 0., predSpeed, predMaxDecel, true);
316 }
317}
318
319
320double
321MSCFModel::insertionStopSpeed(const MSVehicle* const veh, double speed, double gap) const {
323 return stopSpeed(veh, speed, gap, CalcReason::FUTURE);
324 } else {
325 return MIN2(maximumSafeStopSpeed(gap, myDecel, 0., true, 0.), myType->getMaxSpeed());
326 }
327}
328
329
330double
331MSCFModel::followSpeedTransient(double duration, const MSVehicle* const /*veh*/, double /*speed*/, double gap2pred, double predSpeed, double predMaxDecel) const {
332 // minimium distance covered by the leader if braking
333 double leaderMinDist = gap2pred + distAfterTime(duration, predSpeed, -predMaxDecel);
334 // if ego would not brake it could drive with speed leaderMinDist / duration
335 // due to potentential ego braking it can safely drive faster
337 // number of potential braking steps
338 const int a = (int)ceil(duration / TS - TS);
339 // can we brake for the whole time?
340 if (brakeGap(a * myDecel, myDecel, 0) <= leaderMinDist) {
341 // braking continuously for duration
342 // distance reduction due to braking
343 const double b = TS * getMaxDecel() * 0.5 * (a * a - a);
344 if (gDebugFlag2) std::cout << " followSpeedTransient"
345 << " duration=" << duration
346 << " gap=" << gap2pred
347 << " leaderMinDist=" << leaderMinDist
348 << " decel=" << getMaxDecel()
349 << " a=" << a
350 << " bg=" << brakeGap(a * myDecel, myDecel, 0)
351 << " b=" << b
352 << " x=" << (b + leaderMinDist) / duration
353 << "\n";
354 return (b + leaderMinDist) / duration;
355 } else {
356 // @todo improve efficiency
357 double bg = 0;
358 double speed = 0;
359 while (bg < leaderMinDist) {
360 speed += ACCEL2SPEED(myDecel);
361 bg += SPEED2DIST(speed);
362 }
363 speed -= DIST2SPEED(bg - leaderMinDist);
364 return speed;
365 }
366 } else {
367 // can we brake for the whole time?
368 const double fullBrakingSeconds = sqrt(leaderMinDist * 2 / myDecel);
369 if (fullBrakingSeconds >= duration) {
370 // braking continuously for duration
371 // average speed after braking for duration is x2 = x - 0.5 * duration * myDecel
372 // x2 * duration <= leaderMinDist must hold
373 return leaderMinDist / duration + duration * getMaxDecel() / 2;
374 } else {
375 return fullBrakingSeconds * myDecel;
376 }
377 }
378}
379
380double
381MSCFModel::distAfterTime(double t, double speed, const double accel) const {
382 if (accel >= 0.) {
383 return (speed + 0.5 * accel * t) * t;
384 }
385 const double decel = -accel;
386 if (speed <= decel * t) {
387 // braking to a full stop
388 return brakeGap(speed, decel, 0);
389 }
391 // @todo improve efficiency
392 double result = 0;
393 while (t > 0) {
394 speed -= ACCEL2SPEED(decel);
395 result += MAX2(0.0, SPEED2DIST(speed));
396 t -= TS;
397 }
398 return result;
399 } else {
400 const double speed2 = speed - t * decel;
401 return 0.5 * (speed + speed2) * t;
402 }
403}
404
405
407MSCFModel::getMinimalArrivalTime(double dist, double currentSpeed, double arrivalSpeed) const {
408 if (dist <= 0.) {
409 return 0;
410 }
411 // will either drive as fast as possible and decelerate as late as possible
412 // or accelerate as fast as possible and then hold that speed
413 const double accel = (arrivalSpeed >= currentSpeed) ? getMaxAccel() : -getMaxDecel();
414 const double accelTime = accel == 0. ? 0. : (arrivalSpeed - currentSpeed) / accel;
415 const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
416 if (dist >= accelWay) {
417 const double nonAccelWay = dist - accelWay;
418 const double nonAccelSpeed = MAX3(currentSpeed, arrivalSpeed, SUMO_const_haltingSpeed);
419 return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
420 }
421 // find time x so that
422 // x * (currentSpeed + currentSpeed + x * accel) * 0.5 = dist
423 return TIME2STEPS(-(currentSpeed - sqrt(currentSpeed * currentSpeed + 2 * accel * dist)) / accel);
424}
425
426
427double
428MSCFModel::estimateArrivalTime(double dist, double speed, double maxSpeed, double accel) {
429 assert(speed >= 0.);
430 assert(dist >= 0.);
431
432 if (dist < NUMERICAL_EPS) {
433 return 0.;
434 }
435
436 if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
437 // distance will never be covered with these values
438 return INVALID_DOUBLE;
439 }
440
441 if (fabs(accel) < NUMERICAL_EPS) {
442 return dist / speed;
443 }
444
445 double p = speed / accel;
446
447 if (accel < 0.) {
448 // we already know, that the distance will be covered despite breaking
449 return (-p - sqrt(p * p + 2 * dist / accel));
450 }
451
452 // Here, accel > 0
453 // t1 is the time to use the given acceleration
454 double t1 = (maxSpeed - speed) / accel;
455 // distance covered until t1
456 double d1 = speed * t1 + 0.5 * accel * t1 * t1;
457 if (d1 >= dist) {
458 // dist is covered before changing the speed
459 return (-p + sqrt(p * p + 2 * dist / accel));
460 } else {
461 return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
462 }
463
464}
465
466double
467MSCFModel::estimateArrivalTime(double dist, double initialSpeed, double arrivalSpeed, double maxSpeed, double accel, double decel) {
468 UNUSED_PARAMETER(arrivalSpeed); // only in assertion
469 UNUSED_PARAMETER(decel); // only in assertion
470 if (dist <= 0) {
471 return 0.;
472 }
473
474 // stub-assumptions
475 assert(accel == decel);
476 assert(accel > 0);
477 assert(initialSpeed == 0);
478 assert(arrivalSpeed == 0);
479 assert(maxSpeed > 0);
480
481
482 double accelTime = (maxSpeed - initialSpeed) / accel;
483 // "ballistic" estimate for the distance covered during acceleration phase
484 double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
485 double arrivalTime;
486 if (accelDist >= dist * 0.5) {
487 // maximal speed will not be attained during maneuver
488 arrivalTime = 4 * sqrt(dist / accel);
489 } else {
490 // Calculate time to move with constant, maximal lateral speed
491 const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
492 arrivalTime = accelTime + constSpeedTime;
493 }
494 return arrivalTime;
495}
496
497
498double
499MSCFModel::avoidArrivalAccel(double dist, double time, double speed, double maxDecel) {
500 assert(time > 0 || dist == 0);
501 if (dist <= 0) {
502 return -maxDecel;
503 } else if (time * speed > 2 * dist) {
504 // stop before dist is necessary. We need
505 // d = v*v/(2*a)
506 return - 0.5 * speed * speed / dist;
507 } else {
508 // we seek the solution a of
509 // d = v*t + a*t*t/2
510 return 2 * (dist / time - speed) / time;
511 }
512}
513
514
515double
516MSCFModel::getMinimalArrivalSpeed(double dist, double currentSpeed) const {
517 // ballistic update
518 return estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
519}
520
521
522double
523MSCFModel::getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const {
524 double arrivalSpeedBraking;
525 // Because we use a continuous formula for computing the possible slow-down
526 // we need to handle the mismatch with the discrete dynamics
527 if (dist < currentSpeed) {
528 arrivalSpeedBraking = INVALID_SPEED; // no time left for braking after this step
529 // (inserted max() to get rid of arrivalSpeed dependency within method) (Leo)
530 } else if (2 * (dist - currentSpeed * getHeadwayTime()) * -getMaxDecel() + currentSpeed * currentSpeed >= 0) {
531 arrivalSpeedBraking = estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
532 } else {
533 arrivalSpeedBraking = getMaxDecel();
534 }
535 return arrivalSpeedBraking;
536}
537
538
539
540
541double
542MSCFModel::gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1, double a2, const double maxV1, const double maxV2) {
543
544 double newGap = currentGap;
545
547 for (unsigned int steps = 1; steps * TS <= duration; ++steps) {
548 v1 = MIN2(MAX2(v1 + a1, 0.), maxV1);
549 v2 = MIN2(MAX2(v2 + a2, 0.), maxV2);
550 newGap += TS * (v1 - v2);
551 }
552 } else {
553 // determine times t1, t2 for which vehicles can break until stop (within duration)
554 // and t3, t4 for which they reach their maximal speed on their current lanes.
555 double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
556
557 // t1: ego veh stops
558 if (a1 < 0 && v1 > 0) {
559 const double leaderStopTime = - v1 / a1;
560 t1 = MIN2(leaderStopTime, duration);
561 } else if (a1 >= 0) {
562 t1 = duration;
563 }
564 // t2: veh2 stops
565 if (a2 < 0 && v2 > 0) {
566 const double followerStopTime = -v2 / a2;
567 t2 = MIN2(followerStopTime, duration);
568 } else if (a2 >= 0) {
569 t2 = duration;
570 }
571 // t3: ego veh reaches vMax
572 if (a1 > 0 && v1 < maxV1) {
573 const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
574 t3 = MIN2(leaderMaxSpeedTime, duration);
575 } else if (a1 <= 0) {
576 t3 = duration;
577 }
578 // t4: veh2 reaches vMax
579 if (a2 > 0 && v2 < maxV2) {
580 const double followerMaxSpeedTime = (maxV2 - v2) / a2;
581 t4 = MIN2(followerMaxSpeedTime, duration);
582 } else if (a2 <= 0) {
583 t4 = duration;
584 }
585
586 // NOTE: this assumes that the accelerations a1 and a2 are constant over the next
587 // followerBreakTime seconds (if no vehicle stops before or reaches vMax)
588 std::list<double> l;
589 l.push_back(t1);
590 l.push_back(t2);
591 l.push_back(t3);
592 l.push_back(t4);
593 l.sort();
594 std::list<double>::const_iterator i;
595 double tLast = 0.;
596 for (i = l.begin(); i != l.end(); ++i) {
597 if (*i != tLast) {
598 double dt = MIN2(*i, duration) - tLast; // time between *i and tLast
599 double dv = v1 - v2; // current velocity difference
600 double da = a1 - a2; // current acceleration difference
601 newGap += dv * dt + da * dt * dt / 2.; // update gap
602 v1 += dt * a1;
603 v2 += dt * a2;
604 }
605 if (*i == t1 || *i == t3) {
606 // ego veh reached velocity bound
607 a1 = 0.;
608 }
609
610 if (*i == t2 || *i == t4) {
611 // veh2 reached velocity bound
612 a2 = 0.;
613 }
614
615 tLast = MIN2(*i, duration);
616 if (tLast == duration) {
617 break;
618 }
619 }
620
621 if (duration != tLast) {
622 // (both vehicles have zero acceleration)
623 assert(a1 == 0. && a2 == 0.);
624 double dt = duration - tLast; // remaining time until duration
625 double dv = v1 - v2; // current velocity difference
626 newGap += dv * dt; // update gap
627 }
628 }
629
630 return newGap;
631}
632
633
634
635double
636MSCFModel::passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed) {
637
638 assert(passedPos <= currentPos);
639 assert(passedPos >= lastPos);
640 assert(currentPos > lastPos);
641 assert(currentSpeed >= 0);
642
643 if (passedPos > currentPos || passedPos < lastPos) {
644 std::stringstream ss;
645 // Debug (Leo)
647 // NOTE: error is guarded to maintain original test output for euler update (Leo).
648 ss << "passingTime(): given argument passedPos = " << passedPos << " doesn't lie within [lastPos, currentPos] = [" << lastPos << ", " << currentPos << "]\nExtrapolating...";
649 std::cout << ss.str() << "\n";
650 WRITE_ERROR(ss.str());
651 }
652 const double lastCoveredDist = currentPos - lastPos;
653 const double extrapolated = passedPos > currentPos ? TS * (passedPos - lastPos) / lastCoveredDist : TS * (currentPos - passedPos) / lastCoveredDist;
654 return extrapolated;
655 } else if (currentSpeed < 0) {
656 WRITE_ERROR(TL("passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet."));
657 return -1;
658 }
659
660 const double distanceOldToPassed = passedPos - lastPos; // assert: >=0
661
663 // euler update (constantly moving with currentSpeed during [0,TS])
664 if (currentSpeed == 0) {
665 return TS;
666 }
667 const double t = distanceOldToPassed / currentSpeed;
668 return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
669
670 } else {
671 // ballistic update (constant acceleration a during [0,TS], except in case of a stop)
672
673 // determine acceleration
674 double a;
675 if (currentSpeed > 0) {
676 // the acceleration was constant within the last time step
677 a = SPEED2ACCEL(currentSpeed - lastSpeed);
678 } else {
679 // the currentSpeed is zero (the last was not because lastPos<currentPos).
680 assert(currentSpeed == 0 && lastSpeed != 0);
681 // In general the stop has taken place within the last time step.
682 // The acceleration (a<0) is obtained from
683 // deltaPos = - lastSpeed^2/(2*a)
684 a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
685
686 assert(a < 0);
687 }
688
689 // determine passing time t
690 // we solve distanceOldToPassed = lastSpeed*t + a*t^2/2
691 if (fabs(a) < NUMERICAL_EPS) {
692 // treat as constant speed within [0, TS]
693 const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
694 return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
695 } else if (a > 0) {
696 // positive acceleration => only one positive solution
697 const double va = lastSpeed / a;
698 const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
699 assert(t < 1 && t >= 0);
700 return t;
701 } else {
702 // negative acceleration => two positive solutions (pick the smaller one.)
703 const double va = lastSpeed / a;
704 const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
705 // emergency braking at red light could give results out of the admissible result range
706 // because the dynamics are euler-like (full forward speed with instant deceleration)
707 return MIN2(TS, MAX2(0., t));
708 }
709 }
710}
711
712
713double
714MSCFModel::speedAfterTime(const double t, const double v0, const double dist) {
715 assert(dist >= 0);
716 assert(t >= 0 && t <= TS);
718 // euler: constant speed within [0,TS]
719 return DIST2SPEED(dist);
720 } else {
721 // ballistic: piecewise constant acceleration in [0,TS] (may become 0 for a stop within TS)
722 // We reconstruct acceleration at time t=0. Note that the covered distance in case
723 // of a stop exactly at t=TS is TS*v0/2.
724 if (dist < TS * v0 / 2) {
725 // stop must have occurred within [0,TS], use dist = -v0^2/(2a) (stopping dist),
726 // i.e., a = -v0^2/(2*dist)
727 const double accel = - v0 * v0 / (2 * dist);
728 // The speed at time t is then
729 return v0 + accel * t;
730 } else {
731 // no stop occurred within [0,TS], thus (from dist = v0*TS + accel*TS^2/2)
732 const double accel = 2 * (dist / TS - v0) / TS;
733 // The speed at time t is then
734 return v0 + accel * t;
735 }
736 }
737}
738
739
740
741
742double
743MSCFModel::estimateSpeedAfterDistance(const double dist, const double v, const double accel) const {
744 // dist=v*t + 0.5*accel*t^2, solve for t and use v1 = v + accel*t
745 return MIN2(myType->getMaxSpeed(),
746 (double)sqrt(MAX2(0., 2 * dist * accel + v * v)));
747}
748
749
750
751double
752MSCFModel::maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion, double headway) const {
753 double vsafe;
755 vsafe = maximumSafeStopSpeedEuler(gap, decel, onInsertion, headway);
756 } else {
757 vsafe = maximumSafeStopSpeedBallistic(gap, decel, currentSpeed, onInsertion, headway);
758 }
759
760// if (myDecel != myEmergencyDecel) {
761//#ifdef DEBUG_EMERGENCYDECEL
762// if (true) {
763// std::cout << SIMTIME << " maximumSafeStopSpeed()"
764// << " g=" << g
765// << " v=" << v
766// << " initial vsafe=" << vsafe << "(decel=" << SPEED2ACCEL(v-vsafe) << ")" << std::endl;
767// }
768//#endif
769//
770// if (vsafe < v - ACCEL2SPEED(myDecel + NUMERICAL_EPS)) {
771// // emergency deceleration required
772//
773//#ifdef DEBUG_EMERGENCYDECEL
774// if (true) {
775// std::cout << SIMTIME << " maximumSafeStopSpeed() results in emergency deceleration "
776// << "initial vsafe=" << vsafe << " egoSpeed=" << v << "(decel=" << SPEED2ACCEL(v-vsafe) << ")" << std::endl;
777// }
778//#endif
779//
780// const double safeDecel = calculateEmergencyDeceleration(g, v, 0., 1);
781// assert(myDecel <= safeDecel);
782// vsafe = v - ACCEL2SPEED(myDecel + EMERGENCY_DECEL_AMPLIFIER * (safeDecel - myDecel));
783//
784// if(MSGlobals::gSemiImplicitEulerUpdate) {
785// vsafe = MAX2(vsafe,0.);
786// }
787//
788//#ifdef DEBUG_EMERGENCYDECEL
789// if (true) {
790// std::cout << " -> corrected emergency deceleration: " << SPEED2ACCEL(v-vsafe) << std::endl;
791// }
792//#endif
793//
794// }
795// }
796
797 return vsafe;
798}
799
800
801double
802MSCFModel::maximumSafeStopSpeedEuler(double gap, double decel, bool /* onInsertion */, double headway) const {
803 // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
804 const double g = gap - NUMERICAL_EPS;
805 if (g < 0.) {
806 return 0.;
807 }
808 const double b = ACCEL2SPEED(decel);
809 const double t = headway >= 0 ? headway : myHeadwayTime;
810 const double s = TS;
811
812 // h = the distance that would be covered if it were possible to stop
813 // exactly after gap and decelerate with b every simulation step
814 // h = 0.5 * n * (n-1) * b * s + n * b * t (solve for n)
815 //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));
816 const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
817 const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
818 assert(h <= g + NUMERICAL_EPS);
819 // compute the additional speed that must be used during deceleration to fix
820 // the discrepancy between g and h
821 const double r = (g - h) / (n * s + t);
822 const double x = n * b + r;
823 assert(x >= 0);
824 return x;
825// return onInsertion ? x + b: x; // see #2574
826}
827
828
829double
830MSCFModel::maximumSafeStopSpeedBallistic(double gap, double decel, double currentSpeed, bool onInsertion, double headway) const {
831 // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
832 const double g = MAX2(0., gap - NUMERICAL_EPS);
833 headway = headway >= 0 ? headway : myHeadwayTime;
834
835 // (Leo) Note that in contrast to the Euler update, for the ballistic update
836 // the distance covered in the coming step depends on the current velocity, in general.
837 // one exception is the situation when the vehicle is just being inserted.
838 // In that case, it will not cover any distance until the next timestep by convention.
839
840 // We treat the latter case first:
841 if (onInsertion) {
842 // The distance covered with constant insertion speed v0 until time tau is given as
843 // G1 = tau*v0
844 // The distance covered between time tau and the stopping moment at time tau+v0/b is
845 // G2 = v0^2/(2b),
846 // where b is an assumed constant deceleration (= decel)
847 // We solve g = G1 + G2 for v0:
848 const double btau = decel * headway;
849 const double v0 = -btau + sqrt(btau * btau + 2 * decel * g);
850 return v0;
851 }
852
853 // In the usual case during the driving task, the vehicle goes by
854 // a current speed v0=v, and we seek to determine a safe acceleration a (possibly <0)
855 // such that starting to break after accelerating with a for the time tau=headway
856 // still allows us to stop in time.
857
858 const double tau = headway == 0 ? TS : headway;
859 const double v0 = MAX2(0., currentSpeed);
860 // We first consider the case that a stop has to take place within time tau
861 if (v0 * tau >= 2 * g) {
862 if (g == 0.) {
863 if (v0 > 0.) {
864 // indicate to brake as hard as possible
866 } else {
867 // stay stopped
868 return 0.;
869 }
870 }
871 // In general we solve g = v0^2/(-2a), where the the rhs is the distance
872 // covered until stop when breaking with a<0
873 const double a = -v0 * v0 / (2 * g);
874 return v0 + a * TS;
875 }
876
877 // The last case corresponds to a situation, where the vehicle may go with a positive
878 // speed v1 = v0 + tau*a after time tau.
879 // The distance covered until time tau is given as
880 // G1 = tau*(v0+v1)/2
881 // The distance covered between time tau and the stopping moment at time tau+v1/b is
882 // G2 = v1^2/(2b),
883 // where b is an assumed constant deceleration (= decel)
884 // We solve g = G1 + G2 for v1>0:
885 // <=> 0 = v1^2 + b*tau*v1 + b*tau*v0 - 2bg
886 // => v1 = -b*tau/2 + sqrt( (b*tau)^2/4 + b(2g - tau*v0) )
887
888 const double btau2 = decel * tau / 2;
889 const double v1 = -btau2 + sqrt(btau2 * btau2 + decel * (2 * g - tau * v0));
890 const double a = (v1 - v0) / tau;
891 return v0 + a * TS;
892}
893
894
896double
897MSCFModel::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
898 // the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
899 // the leaders starts braking hard until stopped
900 // unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
901 // (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
902 // 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.
903 // @todo: this is a conservative estimate for safe speed which could be increased
904
905// // For negative gaps, we return the lowest meaningful value by convention
906// // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)
907
908// // It must be done. Otherwise, negative gaps at high speeds can create nonsense results from the call to maximumSafeStopSpeed() below
909
910// if(gap<0){
911// if(MSGlobals::gSemiImplicitEulerUpdate){
912// return 0.;
913// } else {
914// return -INVALID_SPEED;
915// }
916// }
917
918 // The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
919 // if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
920 // NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
921 // the follower accelerates for a short period of time. Refs #2310 (Leo)
922 // const double headway = predSpeed > 0. ? myHeadwayTime : 0.;
923
924 const double headway = myHeadwayTime;
925 double x;
926 if (gap >= 0 || MSGlobals::gComputeLC) {
927 x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), myDecel, egoSpeed, onInsertion, headway);
928 } else {
929 x = egoSpeed - ACCEL2SPEED(myEmergencyDecel);
931 x = MAX2(x, 0.);
932 }
933 }
934
935 if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
936 double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
937 if (origSafeDecel > myDecel + NUMERICAL_EPS) {
938 // Braking harder than myDecel was requested -> calculate required emergency deceleration.
939 // Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
940 // 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,
941 // such that braking harder than myDecel is required.
942
943 double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
944#ifdef DEBUG_EMERGENCYDECEL
945 if (DEBUG_COND2) {
946 std::cout << SIMTIME << " initial vsafe=" << x
947 << " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
948 << " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
949 << " safeDecel=" << safeDecel
950 << std::endl;
951 }
952#endif
953 // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
954 safeDecel = MAX2(safeDecel, myDecel);
955 // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
956 safeDecel = MIN2(safeDecel, origSafeDecel);
957 x = egoSpeed - ACCEL2SPEED(safeDecel);
959 x = MAX2(x, 0.);
960 }
961
962#ifdef DEBUG_EMERGENCYDECEL
963 if (DEBUG_COND2) {
964 std::cout << " -> corrected emergency deceleration: " << safeDecel << " newVSafe=" << x << std::endl;
965 }
966#endif
967
968 }
969 }
970 assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
971 assert(!ISNAN(x));
972 return x;
973}
974
975
976double
977MSCFModel::calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const {
978 // There are two cases:
979 // 1) Either, stopping in time is possible with a deceleration b <= predMaxDecel, then this value is returned
980 // 2) Or, b > predMaxDecel is required in this case the minimal value b allowing to stop safely under the assumption maxPredDecel=b is returned
981 if (gap <= 0.) {
982 return myEmergencyDecel;
983 }
984
985 // Apparent braking distance for the leader
986 const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
987 // Required deceleration according to case 1)
988 const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
989
990#ifdef DEBUG_EMERGENCYDECEL
991 if (DEBUG_COND2) {
992 std::cout << SIMTIME << " calculateEmergencyDeceleration()"
993 << " gap=" << gap << " egoSpeed=" << egoSpeed << " predSpeed=" << predSpeed
994 << " predBrakeDist=" << predBrakeDist
995 << " b1=" << b1
996 << std::endl;
997 }
998#endif
999
1000 if (b1 <= predMaxDecel) {
1001 // Case 1) applies
1002#ifdef DEBUG_EMERGENCYDECEL
1003 if (DEBUG_COND2) {
1004 std::cout << " case 1 ..." << std::endl;
1005 }
1006#endif
1007 return b1;
1008 }
1009#ifdef DEBUG_EMERGENCYDECEL
1010 if (DEBUG_COND2) {
1011 std::cout << " case 2 ...";
1012 }
1013#endif
1014
1015 // Case 2) applies
1016 // Required deceleration according to case 2)
1017 const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
1018
1019#ifdef DEBUG_EMERGENCYDECEL
1020 if (DEBUG_COND2) {
1021 std::cout << " b2=" << b2 << std::endl;
1022 }
1023#endif
1024 return b2;
1025}
1026
1027
1028
1029void
1030MSCFModel::applyHeadwayAndSpeedDifferencePerceptionErrors(const MSVehicle* const veh, double speed, double& gap, double& predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
1031 UNUSED_PARAMETER(speed);
1032 UNUSED_PARAMETER(predMaxDecel);
1033 if (!veh->hasDriverState()) {
1034 return;
1035 }
1036
1037 // Obtain perceived gap and headway from the driver state
1038 const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap, pred);
1039 const double perceivedSpeedDifference = veh->getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
1040
1041#ifdef DEBUG_DRIVER_ERRORS
1042 if (DEBUG_COND) {
1043 if (!veh->getDriverState()->debugLocked()) {
1044 veh->getDriverState()->lockDebug();
1045 std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
1046 << " speed=" << speed << " gap=" << gap << " leaderSpeed=" << predSpeed
1047 << "\n perceivedGap=" << perceivedGap << " perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
1048 << " perceivedSpeedDifference=" << perceivedSpeedDifference
1049 << std::endl;
1050 const double exactFollowSpeed = followSpeed(veh, speed, gap, predSpeed, predMaxDecel, pred, CalcReason::FUTURE);
1051 const double errorFollowSpeed = followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel, pred, CalcReason::FUTURE);
1052 const double accelError = SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
1053 std::cout << " gapError=" << perceivedGap - gap << " dvError=" << perceivedSpeedDifference - (predSpeed - speed)
1054 << "\n resulting accelError: " << accelError << std::endl;
1055 veh->getDriverState()->unlockDebug();
1056 }
1057 }
1058#endif
1059
1060 gap = perceivedGap;
1061 predSpeed = speed + perceivedSpeedDifference;
1062}
1063
1064
1065void
1066MSCFModel::applyHeadwayPerceptionError(const MSVehicle* const veh, double speed, double& gap) const {
1067 UNUSED_PARAMETER(speed);
1068 if (!veh->hasDriverState()) {
1069 return;
1070 }
1071 // @todo: Provide objectID (e.g. pointer address for the relevant object at the given distance(gap))
1072 // This is for item related management of known object and perception updates when the distance
1073 // changes significantly. (Should not be too important for stationary objects though.)
1074
1075 // Obtain perceived gap from driver state
1076 const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap);
1077
1078#ifdef DEBUG_DRIVER_ERRORS
1079 if (DEBUG_COND) {
1080 if (!veh->getDriverState()->debugLocked()) {
1081 veh->getDriverState()->lockDebug();
1082 std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
1083 << " speed=" << speed << " gap=" << gap << "\n perceivedGap=" << perceivedGap << std::endl;
1084 const double exactStopSpeed = stopSpeed(veh, speed, gap, CalcReason::FUTURE);
1085 const double errorStopSpeed = stopSpeed(veh, speed, perceivedGap, CalcReason::FUTURE);
1086 const double accelError = SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1087 std::cout << " gapError=" << perceivedGap - gap << "\n resulting accelError: " << accelError << std::endl;
1088 veh->getDriverState()->unlockDebug();
1089 }
1090 }
1091#endif
1092
1093 gap = perceivedGap;
1094}
1095
1096
1097/****************************************************************************/
long long int SUMOTime
Definition: GUI.h:36
#define DEBUG_COND2
Definition: MSCFModel.cpp:48
#define DEBUG_COND
Definition: MSCFModel.cpp:44
#define EMERGENCY_DECEL_AMPLIFIER
Definition: MSCFModel.h:33
#define INVALID_SPEED
Definition: MSCFModel.h:31
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:274
#define TL(string)
Definition: MsgHandler.h:282
SUMOTime DELTA_T
Definition: SUMOTime.cpp:37
#define SPEED2DIST(x)
Definition: SUMOTime.h:44
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:50
#define TS
Definition: SUMOTime.h:41
#define SIMTIME
Definition: SUMOTime.h:61
#define TIME2STEPS(x)
Definition: SUMOTime.h:56
#define DIST2SPEED(x)
Definition: SUMOTime.h:46
#define ACCEL2DIST(x)
Definition: SUMOTime.h:48
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:52
@ SUMO_ATTR_STARTUP_DELAY
@ SUMO_ATTR_APPARENTDECEL
@ SUMO_ATTR_DECEL
@ SUMO_ATTR_EMERGENCYDECEL
@ SUMO_ATTR_COLLISION_MINGAP_FACTOR
@ SUMO_ATTR_ACCEL
@ SUMO_ATTR_TAU
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:25
bool gDebugFlag2
Definition: StdDefs.cpp:34
const double INVALID_DOUBLE
Definition: StdDefs.h:60
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:30
T MIN3(T a, T b, T c)
Definition: StdDefs.h:84
T MIN2(T a, T b)
Definition: StdDefs.h:71
T ISNAN(T a)
Definition: StdDefs.h:112
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:58
T MAX2(T a, T b)
Definition: StdDefs.h:77
T MAX3(T a, T b, T c)
Definition: StdDefs.h:91
virtual double patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel)=0
Called to adapt the speed in order to allow a lane change. It uses information on LC-related desired ...
double estimateSpeedAfterDistance(const double dist, const double v, const double accel) const
Definition: MSCFModel.cpp:743
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:270
static double gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1=0, double a2=0, const double maxV1=std::numeric_limits< double >::max(), const double maxV2=std::numeric_limits< double >::max())
return the resulting gap if, starting with gap currentGap, two vehicles continue with constant accele...
Definition: MSCFModel.cpp:542
virtual double minNextSpeedEmergency(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed after emergency braking, given the current speed (depends on the numerical ...
Definition: MSCFModel.cpp:287
virtual double followSpeedTransient(double duration, const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const
Computes the vehicle's follow speed that avoids a collision for the given amount of time.
Definition: MSCFModel.cpp:331
virtual double applyStartupDelay(const MSVehicle *veh, const double vMin, const double vMax, const SUMOTime addTime=0) const
apply speed adaptation on startup
Definition: MSCFModel.cpp:235
double maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
Definition: MSCFModel.cpp:752
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
Definition: MSCFModel.cpp:90
virtual double interactionGap(const MSVehicle *const veh, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
Definition: MSCFModel.cpp:255
static double avoidArrivalAccel(double dist, double time, double speed, double maxDecel)
Computes the acceleration needed to arrive not before the given time.
Definition: MSCFModel.cpp:499
double getMinimalArrivalSpeed(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance.
Definition: MSCFModel.cpp:516
virtual double patchSpeedBeforeLC(const MSVehicle *veh, double vMin, double vMax) const
apply custom speed adaptations within the given speed bounds
Definition: MSCFModel.h:102
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed without a leader.
Definition: MSCFModel.cpp:299
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
Definition: MSCFModel.cpp:276
virtual double insertionFollowSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle's safe speed (no dawdling) This method is used during the insertion stage....
Definition: MSCFModel.cpp:310
SUMOTime myStartupDelay
The startup delay after halting [s].
Definition: MSCFModel.h:698
SUMOTime getMinimalArrivalTime(double dist, double currentSpeed, double arrivalSpeed) const
Computes the minimal time needed to cover a distance given the desired speed at arrival.
Definition: MSCFModel.cpp:407
void applyHeadwayPerceptionError(const MSVehicle *const veh, double speed, double &gap) const
Overwrites gap by the perceived value obtained from the vehicle's driver state.
Definition: MSCFModel.cpp:1066
static double speedAfterTime(const double t, const double oldSpeed, const double dist)
Calculates the speed after a time t \in [0,TS] given the initial speed and the distance traveled in a...
Definition: MSCFModel.cpp:714
static double passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed)
Calculates the time at which the position passedPosition has been passed In case of a ballistic updat...
Definition: MSCFModel.cpp:636
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences. Called at most once per simulation...
Definition: MSCFModel.cpp:165
virtual ~MSCFModel()
Destructor.
Definition: MSCFModel.cpp:68
double maximumSafeStopSpeedEuler(double gap, double decel, bool onInsertion, double headway) const
Returns the maximum next velocity for stopping within gap when using the semi-implicit Euler update.
Definition: MSCFModel.cpp:802
double myEmergencyDecel
The vehicle's maximum emergency deceleration [m/s^2].
Definition: MSCFModel.h:688
void applyHeadwayAndSpeedDifferencePerceptionErrors(const MSVehicle *const veh, double speed, double &gap, double &predSpeed, double predMaxDecel, const MSVehicle *const pred) const
Overwrites gap2pred and predSpeed by the perceived values obtained from the vehicle's driver state,...
Definition: MSCFModel.cpp:1030
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
Definition: MSCFModel.cpp:897
CalcReason
What the return value of stop/follow/free-Speed is used for.
Definition: MSCFModel.h:77
double calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const
Returns the minimal deceleration for following the given leader safely.
Definition: MSCFModel.cpp:977
MSCFModel(const MSVehicleType *vtype)
Constructor.
Definition: MSCFModel.cpp:55
double myDecel
The vehicle's maximum deceleration [m/s^2].
Definition: MSCFModel.h:686
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:254
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:373
const MSVehicleType * myType
The type to which this model definition belongs to.
Definition: MSCFModel.h:680
virtual double distAfterTime(double t, double speed, double accel) const
calculates the distance travelled after accelerating for time t
Definition: MSCFModel.cpp:381
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:262
double maximumSafeStopSpeedBallistic(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap when using the ballistic positional update.
Definition: MSCFModel.cpp:830
double getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance for Euler update.
Definition: MSCFModel.cpp:523
static double estimateArrivalTime(double dist, double speed, double maxSpeed, double accel)
Computes the time needed to travel a distance dist given an initial speed and constant acceleration....
Definition: MSCFModel.cpp:428
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
double myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
Definition: MSCFModel.h:695
double stopSpeed(const MSVehicle *const veh, const double speed, double gap, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
Definition: MSCFModel.h:168
virtual double insertionStopSpeed(const MSVehicle *const veh, double speed, double gap) const
Computes the vehicle's safe speed for approaching an obstacle at insertion without constraints due to...
Definition: MSCFModel.cpp:321
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
Definition: MSCFModel.h:309
static bool gSemiImplicitEulerUpdate
Definition: MSGlobals.h:53
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
Definition: MSGlobals.h:136
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition: MSLane.h:547
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
SUMOTime getTimeSinceStartup() const
Returns the SUMOTime spent driving since startup (speed was larger than 0.1m/s)
Definition: MSVehicle.h:673
bool hasDriverState() const
Whether this vehicle is equipped with a MSDriverState.
Definition: MSVehicle.h:1009
double getFriction() const
Returns the current friction on the road as perceived by the friction device.
Definition: MSVehicle.cpp:7161
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5367
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:529
std::shared_ptr< MSSimpleDriverState > getDriverState() const
Returns the vehicle driver's state.
Definition: MSVehicle.cpp:7155
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:577
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:486
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
Definition: MSVehicle.cpp:1593
The car-following model and parameter.
Definition: MSVehicleType.h:63
double getMaxSpeed() const
Get vehicle's (technical) maximum speed [m/s].
const std::string & getID() const
Returns the id.
Definition: Named.h:74
Structure representing possible vehicle parameter.