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