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-2020 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) {
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, 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  int a = (int)ceil(duration / TS - TS);
306  // can we brake for the whole time?
307  const double bg = brakeGap(a * myDecel, myDecel, 0);
308  if (bg <= leaderMinDist) {
309  // braking continuously for duration
310  // distance reduction due to braking
311  double b = TS * getMaxDecel() * 0.5 * (a * a - a);
312  if (gDebugFlag2) std::cout << " followSpeedTransient"
313  << " duration=" << duration
314  << " gap=" << gap2pred
315  << " leaderMinDist=" << leaderMinDist
316  << " decel=" << getMaxDecel()
317  << " a=" << a
318  << " bg=" << bg
319  << " b=" << b
320  << " x=" << (b + leaderMinDist) / duration
321  << "\n";
322  return (b + leaderMinDist) / duration;
323  } else {
324  // @todo improve efficiency
325  double bg = 0;
326  double speed = 0;
327  while (bg < leaderMinDist) {
328  speed += ACCEL2SPEED(myDecel);
329  bg += SPEED2DIST(speed);
330  }
331  speed -= DIST2SPEED(bg - leaderMinDist);
332  return speed;
333  }
334  } else {
335  // can we brake for the whole time?
336  const double fullBrakingSeconds = sqrt(leaderMinDist * 2 / myDecel);
337  if (fullBrakingSeconds >= duration) {
338  // braking continuously for duration
339  // average speed after braking for duration is x2 = x - 0.5 * duration * myDecel
340  // x2 * duration <= leaderMinDist must hold
341  return leaderMinDist / duration + duration * getMaxDecel() / 2;
342  } else {
343  return fullBrakingSeconds * myDecel;
344  }
345  }
346 }
347 
348 double
349 MSCFModel::distAfterTime(double t, double speed, const double accel) {
350  if (accel >= 0.) {
351  return (speed + 0.5 * accel * t) * t;
352  }
353  const double decel = -accel;
354  if (speed <= decel * t) {
355  // braking to a full stop
356  return brakeGap(speed, decel, 0);
357  }
359  // @todo improve efficiency
360  double result = 0;
361  while (t > 0) {
362  speed -= ACCEL2SPEED(decel);
363  result += MAX2(0.0, SPEED2DIST(speed));
364  t -= TS;
365  }
366  return result;
367  } else {
368  const double speed2 = speed - t * decel;
369  return 0.5 * (speed + speed2) * t;
370  }
371 }
372 
373 SUMOTime
374 MSCFModel::getMinimalArrivalTime(double dist, double currentSpeed, double arrivalSpeed) const {
375  const double accel = (arrivalSpeed >= currentSpeed) ? getMaxAccel() : -getMaxDecel();
376  const double accelTime = (arrivalSpeed - currentSpeed) / accel;
377  const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
378  const double nonAccelWay = MAX2(0., dist - accelWay);
379  // will either drive as fast as possible and decelerate as late as possible
380  // or accelerate as fast as possible and then hold that speed
381  const double nonAccelSpeed = MAX3(currentSpeed, arrivalSpeed, SUMO_const_haltingSpeed);
382  return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
383 }
384 
385 
386 double
387 MSCFModel::estimateArrivalTime(double dist, double speed, double maxSpeed, double accel) {
388  assert(speed >= 0.);
389  assert(dist >= 0.);
390 
391  if (dist < NUMERICAL_EPS) {
392  return 0.;
393  }
394 
395  if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
396  // distance will never be covered with these values
397  return INVALID_DOUBLE;
398  }
399 
400  if (fabs(accel) < NUMERICAL_EPS) {
401  return dist / speed;
402  }
403 
404  double p = speed / accel;
405 
406  if (accel < 0.) {
407  // we already know, that the distance will be covered despite breaking
408  return (-p - sqrt(p * p + 2 * dist / accel));
409  }
410 
411  // Here, accel > 0
412  // t1 is the time to use the given acceleration
413  double t1 = (maxSpeed - speed) / accel;
414  // distance covered until t1
415  double d1 = speed * t1 + 0.5 * accel * t1 * t1;
416  if (d1 >= dist) {
417  // dist is covered before changing the speed
418  return (-p + sqrt(p * p + 2 * dist / accel));
419  } else {
420  return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
421  }
422 
423 }
424 
425 double
426 MSCFModel::estimateArrivalTime(double dist, double initialSpeed, double arrivalSpeed, double maxSpeed, double accel, double decel) {
427  UNUSED_PARAMETER(arrivalSpeed); // only in assertion
428  UNUSED_PARAMETER(decel); // only in assertion
429  if (dist <= 0) {
430  return 0.;
431  }
432 
433  // stub-assumptions
434  assert(accel == decel);
435  assert(accel > 0);
436  assert(initialSpeed == 0);
437  assert(arrivalSpeed == 0);
438  assert(maxSpeed > 0);
439 
440 
441  double accelTime = (maxSpeed - initialSpeed) / accel;
442  // "ballistic" estimate for the distance covered during acceleration phase
443  double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
444  double arrivalTime;
445  if (accelDist >= dist * 0.5) {
446  // maximal speed will not be attained during maneuver
447  arrivalTime = 4 * sqrt(dist / accel);
448  } else {
449  // Calculate time to move with constant, maximal lateral speed
450  const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
451  arrivalTime = accelTime + constSpeedTime;
452  }
453  return arrivalTime;
454 }
455 
456 
457 double
458 MSCFModel::avoidArrivalAccel(double dist, double time, double speed, double maxDecel) {
459  assert(time > 0 || dist == 0);
460  if (dist <= 0) {
461  return -maxDecel;
462  } else if (time * speed > 2 * dist) {
463  // stop before dist is necessary. We need
464  // d = v*v/(2*a)
465  return - 0.5 * speed * speed / dist;
466  } else {
467  // we seek the solution a of
468  // d = v*t + a*t*t/2
469  return 2 * (dist / time - speed) / time;
470  }
471 }
472 
473 
474 double
475 MSCFModel::getMinimalArrivalSpeed(double dist, double currentSpeed) const {
476  // ballistic update
477  return estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
478 }
479 
480 
481 double
482 MSCFModel::getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const {
483  double arrivalSpeedBraking;
484  // Because we use a continuous formula for computing the possible slow-down
485  // we need to handle the mismatch with the discrete dynamics
486  if (dist < currentSpeed) {
487  arrivalSpeedBraking = INVALID_SPEED; // no time left for braking after this step
488  // (inserted max() to get rid of arrivalSpeed dependency within method) (Leo)
489  } else if (2 * (dist - currentSpeed * getHeadwayTime()) * -getMaxDecel() + currentSpeed * currentSpeed >= 0) {
490  arrivalSpeedBraking = estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
491  } else {
492  arrivalSpeedBraking = getMaxDecel();
493  }
494  return arrivalSpeedBraking;
495 }
496 
497 
498 
499 
500 double
501 MSCFModel::gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1, double a2, const double maxV1, const double maxV2) {
502 
503  double newGap = currentGap;
504 
506  for (unsigned int steps = 1; steps * TS <= duration; ++steps) {
507  v1 = MIN2(MAX2(v1 + a1, 0.), maxV1);
508  v2 = MIN2(MAX2(v2 + a2, 0.), maxV2);
509  newGap += TS * (v1 - v2);
510  }
511  } else {
512  // determine times t1, t2 for which vehicles can break until stop (within duration)
513  // and t3, t4 for which they reach their maximal speed on their current lanes.
514  double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
515 
516  // t1: ego veh stops
517  if (a1 < 0 && v1 > 0) {
518  const double leaderStopTime = - v1 / a1;
519  t1 = MIN2(leaderStopTime, duration);
520  } else if (a1 >= 0) {
521  t1 = duration;
522  }
523  // t2: veh2 stops
524  if (a2 < 0 && v2 > 0) {
525  const double followerStopTime = -v2 / a2;
526  t2 = MIN2(followerStopTime, duration);
527  } else if (a2 >= 0) {
528  t2 = duration;
529  }
530  // t3: ego veh reaches vMax
531  if (a1 > 0 && v1 < maxV1) {
532  const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
533  t3 = MIN2(leaderMaxSpeedTime, duration);
534  } else if (a1 <= 0) {
535  t3 = duration;
536  }
537  // t4: veh2 reaches vMax
538  if (a2 > 0 && v2 < maxV2) {
539  const double followerMaxSpeedTime = (maxV2 - v2) / a2;
540  t4 = MIN2(followerMaxSpeedTime, duration);
541  } else if (a2 <= 0) {
542  t4 = duration;
543  }
544 
545  // NOTE: this assumes that the accelerations a1 and a2 are constant over the next
546  // followerBreakTime seconds (if no vehicle stops before or reaches vMax)
547  std::list<double> l;
548  l.push_back(t1);
549  l.push_back(t2);
550  l.push_back(t3);
551  l.push_back(t4);
552  l.sort();
553  std::list<double>::const_iterator i;
554  double tLast = 0.;
555  for (i = l.begin(); i != l.end(); ++i) {
556  if (*i != tLast) {
557  double dt = MIN2(*i, duration) - tLast; // time between *i and tLast
558  double dv = v1 - v2; // current velocity difference
559  double da = a1 - a2; // current acceleration difference
560  newGap += dv * dt + da * dt * dt / 2.; // update gap
561  v1 += dt * a1;
562  v2 += dt * a2;
563  }
564  if (*i == t1 || *i == t3) {
565  // ego veh reached velocity bound
566  a1 = 0.;
567  }
568 
569  if (*i == t2 || *i == t4) {
570  // veh2 reached velocity bound
571  a2 = 0.;
572  }
573 
574  tLast = MIN2(*i, duration);
575  if (tLast == duration) {
576  break;
577  }
578  }
579 
580  if (duration != tLast) {
581  // (both vehicles have zero acceleration)
582  assert(a1 == 0. && a2 == 0.);
583  double dt = duration - tLast; // remaining time until duration
584  double dv = v1 - v2; // current velocity difference
585  newGap += dv * dt; // update gap
586  }
587  }
588 
589  return newGap;
590 }
591 
592 
593 
594 double
595 MSCFModel::passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed) {
596 
597  assert(passedPos <= currentPos);
598  assert(passedPos >= lastPos);
599  assert(currentPos > lastPos);
600  assert(currentSpeed >= 0);
601 
602  if (passedPos > currentPos || passedPos < lastPos) {
603  std::stringstream ss;
604  // Debug (Leo)
606  // NOTE: error is guarded to maintain original test output for euler update (Leo).
607  ss << "passingTime(): given argument passedPos = " << passedPos << " doesn't lie within [lastPos, currentPos] = [" << lastPos << ", " << currentPos << "]\nExtrapolating...";
608  std::cout << ss.str() << "\n";
609  WRITE_ERROR(ss.str());
610  }
611  const double lastCoveredDist = currentPos - lastPos;
612  const double extrapolated = passedPos > currentPos ? TS * (passedPos - lastPos) / lastCoveredDist : TS * (currentPos - passedPos) / lastCoveredDist;
613  return extrapolated;
614  } else if (currentSpeed < 0) {
615  WRITE_ERROR("passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
616  return -1;
617  }
618 
619  const double distanceOldToPassed = passedPos - lastPos; // assert: >=0
620 
622  // euler update (constantly moving with currentSpeed during [0,TS])
623  if (currentSpeed == 0) {
624  return TS;
625  }
626  const double t = distanceOldToPassed / currentSpeed;
627  return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
628 
629  } else {
630  // ballistic update (constant acceleration a during [0,TS], except in case of a stop)
631 
632  // determine acceleration
633  double a;
634  if (currentSpeed > 0) {
635  // the acceleration was constant within the last time step
636  a = SPEED2ACCEL(currentSpeed - lastSpeed);
637  } else {
638  // the currentSpeed is zero (the last was not because lastPos<currentPos).
639  assert(currentSpeed == 0 && lastSpeed != 0);
640  // In general the stop has taken place within the last time step.
641  // The acceleration (a<0) is obtained from
642  // deltaPos = - lastSpeed^2/(2*a)
643  a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
644 
645  assert(a < 0);
646  }
647 
648  // determine passing time t
649  // we solve distanceOldToPassed = lastSpeed*t + a*t^2/2
650  if (fabs(a) < NUMERICAL_EPS) {
651  // treat as constant speed within [0, TS]
652  const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
653  return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
654  } else if (a > 0) {
655  // positive acceleration => only one positive solution
656  const double va = lastSpeed / a;
657  const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
658  assert(t < 1 && t >= 0);
659  return t;
660  } else {
661  // negative acceleration => two positive solutions (pick the smaller one.)
662  const double va = lastSpeed / a;
663  const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
664  assert(t < 1 && t >= 0);
665  return t;
666  }
667  }
668 }
669 
670 
671 double
672 MSCFModel::speedAfterTime(const double t, const double v0, const double dist) {
673  assert(dist >= 0);
674  assert(t >= 0 && t <= TS);
676  // euler: constant speed within [0,TS]
677  return DIST2SPEED(dist);
678  } else {
679  // ballistic: piecewise constant acceleration in [0,TS] (may become 0 for a stop within TS)
680  // We reconstruct acceleration at time t=0. Note that the covered distance in case
681  // of a stop exactly at t=TS is TS*v0/2.
682  if (dist < TS * v0 / 2) {
683  // stop must have occurred within [0,TS], use dist = -v0^2/(2a) (stopping dist),
684  // i.e., a = -v0^2/(2*dist)
685  const double accel = - v0 * v0 / (2 * dist);
686  // The speed at time t is then
687  return v0 + accel * t;
688  } else {
689  // no stop occurred within [0,TS], thus (from dist = v0*TS + accel*TS^2/2)
690  const double accel = 2 * (dist / TS - v0) / TS;
691  // The speed at time t is then
692  return v0 + accel * t;
693  }
694  }
695 }
696 
697 
698 
699 
700 double
701 MSCFModel::estimateSpeedAfterDistance(const double dist, const double v, const double accel) const {
702  // dist=v*t + 0.5*accel*t^2, solve for t and use v1 = v + accel*t
703  return MIN2(myType->getMaxSpeed(),
704  (double)sqrt(MAX2(0., 2 * dist * accel + v * v)));
705 }
706 
707 
708 
709 double
710 MSCFModel::maximumSafeStopSpeed(double g /*gap*/, double v /*currentSpeed*/, bool onInsertion, double headway) const {
711  double vsafe;
713  vsafe = maximumSafeStopSpeedEuler(g, headway);
714  } else {
715  vsafe = maximumSafeStopSpeedBallistic(g, v, onInsertion, headway);
716  }
717 
718 // if (myDecel != myEmergencyDecel) {
719 //#ifdef DEBUG_EMERGENCYDECEL
720 // if (true) {
721 // std::cout << SIMTIME << " maximumSafeStopSpeed()"
722 // << " g=" << g
723 // << " v=" << v
724 // << " initial vsafe=" << vsafe << "(decel=" << SPEED2ACCEL(v-vsafe) << ")" << std::endl;
725 // }
726 //#endif
727 //
728 // if (vsafe < v - ACCEL2SPEED(myDecel + NUMERICAL_EPS)) {
729 // // emergency deceleration required
730 //
731 //#ifdef DEBUG_EMERGENCYDECEL
732 // if (true) {
733 // std::cout << SIMTIME << " maximumSafeStopSpeed() results in emergency deceleration "
734 // << "initial vsafe=" << vsafe << " egoSpeed=" << v << "(decel=" << SPEED2ACCEL(v-vsafe) << ")" << std::endl;
735 // }
736 //#endif
737 //
738 // const double safeDecel = calculateEmergencyDeceleration(g, v, 0., 1);
739 // assert(myDecel <= safeDecel);
740 // vsafe = v - ACCEL2SPEED(myDecel + EMERGENCY_DECEL_AMPLIFIER * (safeDecel - myDecel));
741 //
742 // if(MSGlobals::gSemiImplicitEulerUpdate) {
743 // vsafe = MAX2(vsafe,0.);
744 // }
745 //
746 //#ifdef DEBUG_EMERGENCYDECEL
747 // if (true) {
748 // std::cout << " -> corrected emergency deceleration: " << SPEED2ACCEL(v-vsafe) << std::endl;
749 // }
750 //#endif
751 //
752 // }
753 // }
754 
755  return vsafe;
756 }
757 
758 
759 double
760 MSCFModel::maximumSafeStopSpeedEuler(double gap, double headway) const {
761  gap -= NUMERICAL_EPS; // lots of code relies on some slack XXX: it shouldn't...
762  if (gap <= 0) {
763  return 0;
764  }
765  const double g = gap;
766  const double b = ACCEL2SPEED(myDecel);
767  const double t = headway >= 0 ? headway : myHeadwayTime;
768  const double s = TS;
769 
770 
771  // h = the distance that would be covered if it were possible to stop
772  // exactly after gap and decelerate with b every simulation step
773  // h = 0.5 * n * (n-1) * b * s + n * b * t (solve for n)
774  //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));
775  const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
776  const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
777  assert(h <= g + NUMERICAL_EPS);
778  // compute the additional speed that must be used during deceleration to fix
779  // the discrepancy between g and h
780  const double r = (g - h) / (n * s + t);
781  const double x = n * b + r;
782  assert(x >= 0);
783  return x;
784 }
785 
786 
787 double
788 MSCFModel::maximumSafeStopSpeedBallistic(double g /*gap*/, double v /*currentSpeed*/, bool onInsertion, double headway) const {
789  // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
790  g = MAX2(0., g - NUMERICAL_EPS);
791  headway = headway >= 0 ? headway : myHeadwayTime;
792 
793  // (Leo) Note that in contrast to the Euler update, for the ballistic update
794  // the distance covered in the coming step depends on the current velocity, in general.
795  // one exception is the situation when the vehicle is just being inserted.
796  // In that case, it will not cover any distance until the next timestep by convention.
797 
798  // We treat the latter case first:
799  if (onInsertion) {
800  // The distance covered with constant insertion speed v0 until time tau is given as
801  // G1 = tau*v0
802  // The distance covered between time tau and the stopping moment at time tau+v0/b is
803  // G2 = v0^2/(2b),
804  // where b is an assumed constant deceleration (= myDecel)
805  // We solve g = G1 + G2 for v0:
806  const double btau = myDecel * headway;
807  const double v0 = -btau + sqrt(btau * btau + 2 * myDecel * g);
808  return v0;
809  }
810 
811  // In the usual case during the driving task, the vehicle goes by
812  // a current speed v0=v, and we seek to determine a safe acceleration a (possibly <0)
813  // such that starting to break after accelerating with a for the time tau=headway
814  // still allows us to stop in time.
815 
816  const double tau = headway == 0 ? TS : headway;
817  const double v0 = MAX2(0., v);
818  // We first consider the case that a stop has to take place within time tau
819  if (v0 * tau >= 2 * g) {
820  if (g == 0.) {
821  if (v0 > 0.) {
822  // indicate to brake as hard as possible
823  return -ACCEL2SPEED(myEmergencyDecel);
824  } else {
825  // stay stopped
826  return 0.;
827  }
828  }
829  // In general we solve g = v0^2/(-2a), where the the rhs is the distance
830  // covered until stop when breaking with a<0
831  const double a = -v0 * v0 / (2 * g);
832  return v0 + a * TS;
833  }
834 
835  // The last case corresponds to a situation, where the vehicle may go with a positive
836  // speed v1 = v0 + tau*a after time tau.
837  // The distance covered until time tau is given as
838  // G1 = tau*(v0+v1)/2
839  // The distance covered between time tau and the stopping moment at time tau+v1/b is
840  // G2 = v1^2/(2b),
841  // where b is an assumed constant deceleration (= myDecel)
842  // We solve g = G1 + G2 for v1>0:
843  // <=> 0 = v1^2 + b*tau*v1 + b*tau*v0 - 2bg
844  // => v1 = -b*tau/2 + sqrt( (b*tau)^2/4 + b(2g - tau*v0) )
845 
846  const double btau2 = myDecel * tau / 2;
847  const double v1 = -btau2 + sqrt(btau2 * btau2 + myDecel * (2 * g - tau * v0));
848  const double a = (v1 - v0) / tau;
849  return v0 + a * TS;
850 }
851 
852 
854 double
855 MSCFModel::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
856  // the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
857  // the leaders starts braking hard until stopped
858  // unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
859  // (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
860  // 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.
861  // @todo: this is a conservative estimate for safe speed which could be increased
862 
863 // // For negative gaps, we return the lowest meaningful value by convention
864 // // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)
865 
866 // // It must be done. Otherwise, negative gaps at high speeds can create nonsense results from the call to maximumSafeStopSpeed() below
867 
868 // if(gap<0){
869 // if(MSGlobals::gSemiImplicitEulerUpdate){
870 // return 0.;
871 // } else {
872 // return -INVALID_SPEED;
873 // }
874 // }
875 
876  // The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
877  // if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
878  // NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
879  // the follower accelerates for a short period of time. Refs #2310 (Leo)
880  // const double headway = predSpeed > 0. ? myHeadwayTime : 0.;
881 
882  const double headway = myHeadwayTime;
883  double x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), egoSpeed, onInsertion, headway);
884 
885  if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
886  double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
887  if (origSafeDecel > myDecel + NUMERICAL_EPS) {
888  // Braking harder than myDecel was requested -> calculate required emergency deceleration.
889  // Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
890  // 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,
891  // such that braking harder than myDecel is required.
892 
893 #ifdef DEBUG_EMERGENCYDECEL
894  if (DEBUG_COND2) {
895  std::cout << SIMTIME << " initial vsafe=" << x
896  << " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
897  << " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
898  << std::endl;
899  }
900 #endif
901 
902  double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
903  // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
904  safeDecel = MAX2(safeDecel, myDecel);
905  // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
906  safeDecel = MIN2(safeDecel, origSafeDecel);
907  x = egoSpeed - ACCEL2SPEED(safeDecel);
909  x = MAX2(x, 0.);
910  }
911 
912 #ifdef DEBUG_EMERGENCYDECEL
913  if (DEBUG_COND2) {
914  std::cout << " -> corrected emergency deceleration: " << safeDecel << " newVSafe=" << x << std::endl;
915  }
916 #endif
917 
918  }
919  }
920  assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
921  assert(!ISNAN(x));
922  return x;
923 }
924 
925 
926 double
927 MSCFModel::calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const {
928  // There are two cases:
929  // 1) Either, stopping in time is possible with a deceleration b <= predMaxDecel, then this value is returned
930  // 2) Or, b > predMaxDecel is required in this case the minimal value b allowing to stop safely under the assumption maxPredDecel=b is returned
931 
932  // Apparent braking distance for the leader
933  const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
934  // Required deceleration according to case 1)
935  const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
936 
937 #ifdef DEBUG_EMERGENCYDECEL
938  if (DEBUG_COND2) {
939  std::cout << SIMTIME << " calculateEmergencyDeceleration()"
940  << " gap=" << gap << " egoSpeed=" << egoSpeed << " predSpeed=" << predSpeed
941  << " predBrakeDist=" << predBrakeDist
942  << " b1=" << b1
943  << std::endl;
944  }
945 #endif
946 
947  if (b1 <= predMaxDecel) {
948  // Case 1) applies
949 #ifdef DEBUG_EMERGENCYDECEL
950  if (DEBUG_COND2) {
951  std::cout << " case 1 ..." << std::endl;
952  }
953 #endif
954  return b1;
955  }
956 #ifdef DEBUG_EMERGENCYDECEL
957  if (DEBUG_COND2) {
958  std::cout << " case 2 ...";
959  }
960 #endif
961 
962  // Case 2) applies
963  assert(gap < 0 || predSpeed < egoSpeed);
964  if (gap <= 0.) {
965  return -ACCEL2SPEED(myEmergencyDecel);
966  }
967  // Required deceleration according to case 2)
968  const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
969 
970 #ifdef DEBUG_EMERGENCYDECEL
971  if (DEBUG_COND2) {
972  std::cout << " b2=" << b2 << std::endl;
973  }
974 #endif
975  return b2;
976 }
977 
978 
979 
980 void
981 MSCFModel::applyHeadwayAndSpeedDifferencePerceptionErrors(const MSVehicle* const veh, double speed, double& gap, double& predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
982  UNUSED_PARAMETER(speed);
983  UNUSED_PARAMETER(predMaxDecel);
984  if (!veh->hasDriverState()) {
985  return;
986  }
987 
988  // Obtain perceived gap and headway from the driver state
989  const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap, pred);
990  const double perceivedSpeedDifference = veh->getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
991 
992 #ifdef DEBUG_DRIVER_ERRORS
993  if (DEBUG_COND) {
994  if (!veh->getDriverState()->debugLocked()) {
995  veh->getDriverState()->lockDebug();
996  std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
997  << " speed=" << speed << " gap=" << gap << " leaderSpeed=" << predSpeed
998  << "\n perceivedGap=" << perceivedGap << " perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
999  << " perceivedSpeedDifference=" << perceivedSpeedDifference
1000  << std::endl;
1001  const double exactFollowSpeed = followSpeed(veh, speed, gap, predSpeed, predMaxDecel);
1002  const double errorFollowSpeed = followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel);
1003  const double accelError = SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
1004  std::cout << " gapError=" << perceivedGap - gap << " dvError=" << perceivedSpeedDifference - (predSpeed - speed)
1005  << "\n resulting accelError: " << accelError << std::endl;
1006  veh->getDriverState()->unlockDebug();
1007  }
1008  }
1009 #endif
1010 
1011  gap = perceivedGap;
1012  predSpeed = speed + perceivedSpeedDifference;
1013 }
1014 
1015 
1016 void
1017 MSCFModel::applyHeadwayPerceptionError(const MSVehicle* const veh, double speed, double& gap) const {
1018  UNUSED_PARAMETER(speed);
1019  if (!veh->hasDriverState()) {
1020  return;
1021  }
1022  // @todo: Provide objectID (e.g. pointer address for the relevant object at the given distance(gap))
1023  // This is for item related management of known object and perception updates when the distance
1024  // changes significantly. (Should not be too important for stationary objects though.)
1025 
1026  // Obtain perceived gap from driver state
1027  const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap);
1028 
1029 #ifdef DEBUG_DRIVER_ERRORS
1030  if (DEBUG_COND) {
1031  if (!veh->getDriverState()->debugLocked()) {
1032  veh->getDriverState()->lockDebug();
1033  std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
1034  << " speed=" << speed << " gap=" << gap << "\n perceivedGap=" << perceivedGap << std::endl;
1035  const double exactStopSpeed = stopSpeed(veh, speed, gap);
1036  const double errorStopSpeed = stopSpeed(veh, speed, perceivedGap);
1037  const double accelError = SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1038  std::cout << " gapError=" << perceivedGap - gap << "\n resulting accelError: " << accelError << std::endl;
1039  veh->getDriverState()->unlockDebug();
1040  }
1041  }
1042 #endif
1043 
1044  gap = perceivedGap;
1045 }
1046 
1047 
1048 /****************************************************************************/
#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:284
#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:31
#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:32
const double INVALID_DOUBLE
Definition: StdDefs.h:62
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:29
T MIN3(T a, T b, T c)
Definition: StdDefs.h:86
T MIN2(T a, T b)
Definition: StdDefs.h:73
T ISNAN(T a)
Definition: StdDefs.h:114
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:60
T MAX2(T a, T b)
Definition: StdDefs.h:79
T MAX3(T a, T b, T c)
Definition: StdDefs.h:93
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:701
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:237
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:501
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 maximumSafeStopSpeedBallistic(double gap, 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:788
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:458
double getMinimalArrivalSpeed(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance.
Definition: MSCFModel.cpp:475
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:374
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:1017
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:672
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:595
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 myEmergencyDecel
The vehicle's maximum emergency deceleration [m/s^2].
Definition: MSCFModel.h:621
double maximumSafeStopSpeedEuler(double gap, double headway=-1) const
Returns the maximum next velocity for stopping within gap when using the semi-implicit Euler update.
Definition: MSCFModel.cpp:760
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:981
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:855
double maximumSafeStopSpeed(double gap, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
Definition: MSCFModel.cpp:710
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:927
static double distAfterTime(double t, double speed, double accel)
calculates the distance travelled after accelerating for time t
Definition: MSCFModel.cpp:349
MSCFModel(const MSVehicleType *vtype)
Constructor.
Definition: MSCFModel.cpp:54
double myDecel
The vehicle's maximum deceleration [m/s^2].
Definition: MSCFModel.h:619
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:208
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:311
const MSVehicleType * myType
The type to which this model definition belongs to.
Definition: MSCFModel.h:613
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:216
double getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance for Euler update.
Definition: MSCFModel.cpp:482
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:387
double myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
Definition: MSCFModel.h:628
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 stopSpeed(const MSVehicle *const veh, const double speed, double gap) const =0
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
Definition: MSCFModel.h:257
static bool gSemiImplicitEulerUpdate
Definition: MSGlobals.h:53
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
Definition: MSGlobals.h:112
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition: MSLane.h:517
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
bool hasDriverState() const
Whether this vehicle is equipped with a MSDriverState.
Definition: MSVehicle.h:969
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4699
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:502
std::shared_ptr< MSSimpleDriverState > getDriverState() const
Returns the vehicle driver's state.
Definition: MSVehicle.cpp:6404
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:458
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
Definition: MSVehicle.cpp:1480
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:550
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:73
Structure representing possible vehicle parameter.