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