SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
MSCFModel.cpp
Go to the documentation of this file.
1 /****************************************************************************/
12 // The car-following model abstraction
13 /****************************************************************************/
14 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
15 // Copyright (C) 2001-2016 DLR (http://www.dlr.de/) and contributors
16 /****************************************************************************/
17 //
18 // This file is part of SUMO.
19 // SUMO is free software: you can redistribute it and/or modify
20 // it under the terms of the GNU General Public License as published by
21 // the Free Software Foundation, either version 3 of the License, or
22 // (at your option) any later version.
23 //
24 /****************************************************************************/
25 
26 
27 // ===========================================================================
28 // included modules
29 // ===========================================================================
30 #ifdef _MSC_VER
31 #include <windows_config.h>
32 #else
33 #include <config.h>
34 #endif
35 
36 #include <math.h>
37 #include <microsim/MSGlobals.h>
38 #include <microsim/MSVehicleType.h>
39 #include <microsim/MSVehicle.h>
40 #include <microsim/MSLane.h>
42 #include "MSCFModel.h"
43 
44 
45 // ===========================================================================
46 // method definitions
47 // ===========================================================================
48 MSCFModel::MSCFModel(const MSVehicleType* vtype, const SUMOReal accel,
49  const SUMOReal decel, const SUMOReal headwayTime)
50  : myType(vtype), myAccel(accel), myDecel(decel), myHeadwayTime(headwayTime) {
51 }
52 
53 
55 
56 
58 
59 
61 MSCFModel::brakeGap(const SUMOReal speed, const SUMOReal decel, const SUMOReal headwayTime) {
63  /* one possibility to speed this up is to calculate speedReduction * steps * (steps+1) / 2
64  for small values of steps (up to 10 maybe) and store them in an array */
65  const SUMOReal speedReduction = ACCEL2SPEED(decel);
66  const int steps = int(speed / speedReduction);
67  return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
68  } else {
69  // ballistic
70  if (speed <= 0) {
71  return 0.;
72  } else {
73  return speed * (headwayTime + 0.5 * speed / decel);
74  }
75  }
76 }
77 
78 
80 MSCFModel::freeSpeed(const SUMOReal currentSpeed, const SUMOReal decel, const SUMOReal dist, const SUMOReal targetSpeed, const bool onInsertion) {
81  // XXX: (Leo) This seems to be exclusively called with decel = myDecel (max deceleration) and is not overridden
82  // by any specific CFModel. That may cause undesirable hard braking (at junctions where the vehicle
83  // changes to a road with a lower speed limit). It relies on the same logic as the maximalSafeSpeed calculations.
84  // XXX: Don't duplicate the code from there, if possible!!! Can maximumSafeStopSpeed() call freeSpeed? (Leo)
85  // -> not is the reaction time / desired headway is to be regarded...
86 
88  // adapt speed to succeeding lane, no reaction time is involved
89  // when breaking for y steps the following distance g is covered
90  // (drive with v in the final step)
91  // g = (y^2 + y) * 0.5 * b + y * v
92  // y = ((((sqrt((b + 2.0*v)*(b + 2.0*v) + 8.0*b*g)) - b)*0.5 - v)/b)
93  const SUMOReal v = SPEED2DIST(targetSpeed);
94  if (dist < v) {
95  return targetSpeed;
96  }
97  const SUMOReal b = ACCEL2DIST(decel);
98  const SUMOReal y = MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
99  const SUMOReal yFull = floor(y);
100  const SUMOReal exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
101  const SUMOReal fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
102  return DIST2SPEED(MAX2((SUMOReal)0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
103  } else {
104  // ballistic update (Leo)
105  // calculate maximum next speed vN that is adjustable to vT=targetSpeed after a distance d=dist
106  // and given a maximal deceleration b=decel, denote the current speed by v0.
107  // the distance covered by a trajectory that attains vN in the next timestep and decelerates afterwards
108  // with b is given as
109  // d = 0.5*dt*(v0+vN) + (t-dt)*vN - 0.5*b*(t-dt)^2, (1)
110  // where time t of arrival at d with speed vT is
111  // t = dt + (vN-vT)/b. (2)
112  // We insert (2) into (1) to obtain
113  // d = 0.5*dt*(v0+vN) + vN*(vN-vT)/b - 0.5*b*((vN-vT)/b)^2
114  // 0 = (dt*b*v0 - vT*vT - 2*b*d) + dt*b*vN + vN*vN
115  // and solve for vN
116 
117  assert(currentSpeed >= 0);
118  assert(targetSpeed >= 0);
119 
120  const SUMOReal dt = onInsertion ? 0 : TS; // handles case that vehicle is inserted just now (at the end of move)
121  const SUMOReal v0 = currentSpeed;
122  const SUMOReal vT = targetSpeed;
123  const SUMOReal b = decel;
124  const SUMOReal d = dist - NUMERICAL_EPS; // prevent returning a value > targetSpeed due to rounding errors
125 
126  // Solvability for positive vN (if d is small relative to v0):
127  // 1) If 0.5*(v0+vT)*dt > d, we set vN=vT.
128  // (In case vT<v0, this implies that on the interpolated trajectory there are points beyond d where
129  // the interpolated velocity is larger than vT, but at least on the temporal discretization grid, vT is not exceeded)
130  // 2) We ignore the (possible) constraint vN >= v0 - b*dt, which could lead to a problem if v0 - t*b > vT.
131  // (moveHelper() is responsible for assuring that the next velocity is chosen in accordance with maximal decelerations)
132 
133  if (0.5 * (v0 + vT)*dt >= d) {
134  return vT; // (#)
135  }
136 
137  const SUMOReal q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
138  const SUMOReal p = 0.5 * b * dt;
139  return -p + sqrt(p * p - q);
140  }
141 }
142 
143 SUMOReal
144 MSCFModel::moveHelper(MSVehicle* const veh, SUMOReal vPos) const {
145  const SUMOReal oldV = veh->getSpeed(); // save old v for optional acceleration computation
146  const SUMOReal vSafe = MIN2(vPos, veh->processNextStop(vPos)); // process stops
147  // we need the acceleration for emission computation;
148  // in this case, we neglect dawdling, nonetheless, using
149  // vSafe does not incorporate speed reduction due to interaction
150  // on lane changing
151  SUMOReal vMin, vNext;
152  const SUMOReal vMax = MIN3(veh->getMaxSpeedOnLane(), maxNextSpeed(oldV, veh), vSafe);
154  // we cannot rely on never braking harder than maxDecel because TraCI or strange cf models may decide to do so
155  vMin = MIN2(getSpeedAfterMaxDecel(oldV), vMax);
156  vNext = veh->getLaneChangeModel().patchSpeed(vMin, vMax, vMax, *this);
157  } else {
158  // for ballistic update, negative vnext must be allowed to
159  // indicate a stop within the coming timestep (i.e., to attain negative values)
160  vMin = MIN2(minNextSpeed(oldV, veh), vMax);
161  vNext = veh->getLaneChangeModel().patchSpeed(vMin, vMax, vMax, *this);
162  // (Leo) moveHelper() is responsible for assuring that the next
163  // velocity is chosen in accordance with maximal decelerations.
164  // At this point vNext may also be negative indicating a stop within next step.
165  // Moreover, because maximumSafeStopSpeed() does not consider deceleration bounds
166  // vNext can be a large negative value at this point. We cap vNext here.
167  vNext = MAX2(vNext, vMin);
168  }
169 
170  return vNext;
171 }
172 
173 
174 SUMOReal
175 MSCFModel::interactionGap(const MSVehicle* const veh, SUMOReal vL) const {
176  // Resolve the vsafe equation to gap. Assume predecessor has
177  // speed != 0 and that vsafe will be the current speed plus acceleration,
178  // i.e that with this gap there will be no interaction.
179  const SUMOReal vNext = MIN2(maxNextSpeed(veh->getSpeed(), veh), veh->getLane()->getVehicleMaxSpeed(veh));
180  const SUMOReal gap = (vNext - vL) *
181  ((veh->getSpeed() + vL) / (2.*myDecel) + myHeadwayTime) +
182  vL * myHeadwayTime;
183 
184  // Don't allow timeHeadWay < deltaT situations.
185  return MAX2(gap, SPEED2DIST(vNext));
186 }
187 
188 
189 SUMOReal
190 MSCFModel::maxNextSpeed(SUMOReal speed, const MSVehicle* const /*veh*/) const {
191  return MIN2(speed + (SUMOReal) ACCEL2SPEED(getMaxAccel()), myType->getMaxSpeed());
192 }
193 
194 SUMOReal
195 MSCFModel::minNextSpeed(SUMOReal speed, const MSVehicle* const /*veh*/) const {
197  return MAX2(speed - ACCEL2SPEED(getMaxDecel()), (SUMOReal)0.);
198  } else {
199  // NOTE: ballistic update allows for negative speeds to indicate a stop within the next timestep
200  return speed - ACCEL2SPEED(getMaxDecel());
201  }
202 }
203 
204 
205 SUMOReal
206 MSCFModel::freeSpeed(const MSVehicle* const /* veh */, SUMOReal speed, SUMOReal seen, SUMOReal maxSpeed, const bool onInsertion) const {
207  SUMOReal vSafe = freeSpeed(speed, myDecel, seen, maxSpeed, onInsertion);
208  return vSafe;
209 }
210 
211 
212 SUMOReal
213 MSCFModel::insertionFollowSpeed(const MSVehicle* const /* v */, SUMOReal speed, SUMOReal gap2pred, SUMOReal predSpeed, SUMOReal predMaxDecel) const {
215  return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel);
216  } else {
217  // NOTE: Even for ballistic update, the current speed is irrelevant at insertion, therefore passing 0. (Leo)
218  return maximumSafeFollowSpeed(gap2pred, 0., predSpeed, predMaxDecel, true);
219  }
220 }
221 
222 
223 SUMOReal
224 MSCFModel::insertionStopSpeed(const MSVehicle* const veh, SUMOReal speed, SUMOReal gap) const {
226  return stopSpeed(veh, speed, gap);
227  } else {
228  return MIN2(maximumSafeStopSpeed(gap, 0., true, 0.), myType->getMaxSpeed());
229  }
230 }
231 
232 
233 SUMOTime
234 MSCFModel::getMinimalArrivalTime(SUMOReal dist, SUMOReal currentSpeed, SUMOReal arrivalSpeed) const {
235  const SUMOReal accel = (arrivalSpeed >= currentSpeed) ? getMaxAccel() : -getMaxDecel();
236  const SUMOReal accelTime = (arrivalSpeed - currentSpeed) / accel;
237  const SUMOReal accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
238  const SUMOReal nonAccelWay = MAX2(SUMOReal(0), dist - accelWay);
239  // will either drive as fast as possible and decelerate as late as possible
240  // or accelerate as fast as possible and then hold that speed
241  const SUMOReal nonAccelSpeed = MAX3(currentSpeed, arrivalSpeed, SUMO_const_haltingSpeed);
242  return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
243 }
244 
245 
246 SUMOReal
248  // ballistic update
249  return estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
250 }
251 
252 
253 SUMOReal
255  SUMOReal arrivalSpeedBraking;
256  // Because we use a continuous formula for computing the possible slow-down
257  // we need to handle the mismatch with the discrete dynamics
258  if (dist < currentSpeed) {
259  arrivalSpeedBraking = INVALID_SPEED; // no time left for braking after this step
260  // (inserted max() to get rid of arrivalSpeed dependency within method) (Leo)
261  } else if (2 * (dist - currentSpeed * getHeadwayTime()) * -getMaxDecel() + currentSpeed * currentSpeed >= 0) {
262  arrivalSpeedBraking = estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
263  } else {
264  arrivalSpeedBraking = getMaxDecel();
265  }
266  return arrivalSpeedBraking;
267 }
268 
269 
270 
271 
272 SUMOReal
273 MSCFModel::gapExtrapolation(const SUMOReal duration, const SUMOReal currentGap, SUMOReal v1, SUMOReal v2, SUMOReal a1, SUMOReal a2, const SUMOReal maxV1, const SUMOReal maxV2) {
274 
275  SUMOReal newGap = currentGap;
276 
278  for (unsigned int steps = 1; steps * TS <= duration; ++steps) {
279  v1 = MIN2(MAX2(v1 + a1, (SUMOReal)0.), maxV1);
280  v2 = MIN2(MAX2(v2 + a2, (SUMOReal)0.), maxV2);
281  newGap += TS * (v1 - v2);
282  }
283  } else {
284  // determine times t1, t2 for which vehicles can break until stop (within duration)
285  // and t3, t4 for which they reach their maximal speed on their current lanes.
286  SUMOReal t1 = 0, t2 = 0, t3 = 0, t4 = 0;
287 
288  // t1: ego veh stops
289  if (a1 < 0 && v1 > 0) {
290  const SUMOReal leaderStopTime = - v1 / a1;
291  t1 = MIN2(leaderStopTime, duration);
292  } else if (a1 >= 0) {
293  t1 = duration;
294  }
295  // t2: veh2 stops
296  if (a2 < 0 && v2 > 0) {
297  const SUMOReal followerStopTime = -v2 / a2;
298  t2 = MIN2(followerStopTime, duration);
299  } else if (a2 >= 0) {
300  t2 = duration;
301  }
302  // t3: ego veh reaches vMax
303  if (a1 > 0 && v1 < maxV1) {
304  const SUMOReal leaderMaxSpeedTime = (maxV1 - v1) / a1;
305  t3 = MIN2(leaderMaxSpeedTime, duration);
306  } else if (a1 <= 0) {
307  t3 = duration;
308  }
309  // t4: veh2 reaches vMax
310  if (a2 > 0 && v2 < maxV2) {
311  const SUMOReal followerMaxSpeedTime = (maxV2 - v2) / a2;
312  t4 = MIN2(followerMaxSpeedTime, duration);
313  } else if (a2 <= 0) {
314  t4 = duration;
315  }
316 
317  // NOTE: this assumes that the accelerations a1 and a2 are constant over the next
318  // followerBreakTime seconds (if no vehicle stops before or reaches vMax)
319  std::list<SUMOReal> l;
320  l.push_back(t1);
321  l.push_back(t2);
322  l.push_back(t3);
323  l.push_back(t4);
324  l.sort();
325  std::list<SUMOReal>::const_iterator i;
326  SUMOReal tLast = 0.;
327  for (i = l.begin(); i != l.end(); ++i) {
328  if (*i != tLast) {
329  SUMOReal dt = MIN2(*i, duration) - tLast; // time between *i and tLast
330  SUMOReal dv = v1 - v2; // current velocity difference
331  SUMOReal da = a1 - a2; // current acceleration difference
332  newGap += dv * dt + da * dt * dt / 2.; // update gap
333  v1 += dt * a1;
334  v2 += dt * a2;
335  }
336  if (*i == t1 || *i == t3) {
337  // ego veh reached velocity bound
338  a1 = 0.;
339  }
340 
341  if (*i == t2 || *i == t4) {
342  // veh2 reached velocity bound
343  a2 = 0.;
344  }
345 
346  tLast = MIN2(*i, duration);
347  if (tLast == duration) {
348  break;
349  }
350  }
351 
352  if (duration != tLast) {
353  // (both vehicles have zero acceleration)
354  assert(a1 == 0. && a2 == 0.);
355  SUMOReal dt = duration - tLast; // remaining time until duration
356  SUMOReal dv = v1 - v2; // current velocity difference
357  newGap += dv * dt; // update gap
358  }
359  }
360 
361  return newGap;
362 }
363 
364 
365 
366 SUMOReal
367 MSCFModel::passingTime(const SUMOReal lastPos, const SUMOReal passedPos, const SUMOReal currentPos, const SUMOReal lastSpeed, const SUMOReal currentSpeed) {
368 
369  assert(passedPos <= currentPos && passedPos >= lastPos && currentPos > lastPos);
370  assert(currentSpeed >= 0);
371 
372  if (passedPos > currentPos || passedPos < lastPos) {
373  std::stringstream ss;
374  // Debug (Leo)
376  // NOTE: error is guarded to maintain original test output for euler update (Leo).
377  ss << "passingTime(): given argument passedPos = " << passedPos << " doesn't lie within [lastPos, currentPos] = [" << lastPos << ", " << currentPos << "]\nExtrapolating...";
378  std::cout << ss.str() << "\n";
379  WRITE_ERROR(ss.str());
380  }
381  const SUMOReal lastCoveredDist = currentPos - lastPos;
382  const SUMOReal extrapolated = passedPos > currentPos ? TS * (passedPos - lastPos) / lastCoveredDist : TS * (currentPos - passedPos) / lastCoveredDist;
383  return extrapolated;
384  } else if (currentSpeed < 0) {
385  WRITE_ERROR("passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
386  return -1;
387  }
388 
389  const SUMOReal distanceOldToPassed = passedPos - lastPos; // assert: >=0
390 
392  // euler update (constantly moving with currentSpeed during [0,TS])
393  const SUMOReal t = distanceOldToPassed / currentSpeed;
394  return t;
395 
396  } else {
397  // ballistic update (constant acceleration a during [0,TS], except in case of a stop)
398 
399  // determine acceleration
400  SUMOReal a;
401  if (currentSpeed > 0) {
402  // the acceleration was constant within the last time step
403  a = SPEED2ACCEL(currentSpeed - lastSpeed);
404  } else {
405  // the currentSpeed is zero (the last was not because lastPos<currentPos).
406  assert(currentSpeed == 0 && lastSpeed != 0);
407  // In general the stop has taken place within the last time step.
408  // The acceleration (a<0) is obtained from
409  // deltaPos = - lastSpeed^2/(2*a)
410  a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
411 
412  assert(a < 0);
413  }
414 
415  // determine passing time t
416  // we solve distanceOldToPassed = lastSpeed*t + a*t^2/2
417  if (fabs(a) < NUMERICAL_EPS) {
418  // treat as constant speed within [0, TS]
419  const SUMOReal t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
420  return t;
421  } else if (a > 0) {
422  // positive acceleration => only one positive solution
423  const SUMOReal va = lastSpeed / a;
424  const SUMOReal t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
425  assert(t < 1 && t >= 0);
426  return t;
427  } else {
428  // negative acceleration => two positive solutions (pick the smaller one.)
429  const SUMOReal va = lastSpeed / a;
430  const SUMOReal t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
431  assert(t < 1 && t >= 0);
432  return t;
433  }
434  }
435 }
436 
437 
438 SUMOReal
439 MSCFModel::speedAfterTime(const SUMOReal t, const SUMOReal v0, const SUMOReal dist) {
440  assert(dist >= 0);
441  assert(t >= 0 && t <= TS);
443  // euler: constant speed within [0,TS]
444  return DIST2SPEED(dist);
445  } else {
446  // ballistic: piecewise constant acceleration in [0,TS] (may become 0 for a stop within TS)
447  // We reconstruct acceleration at time t=0. Note that the covered distance in case
448  // of a stop exactly at t=TS is TS*v0/2.
449  if (dist < TS * v0 / 2) {
450  // stop must have occured within [0,TS], use dist = -v0^2/(2a) (stopping dist),
451  // i.e., a = -v0^2/(2*dist)
452  const SUMOReal accel = - v0 * v0 / (2 * dist);
453  // The speed at time t is then
454  return v0 + accel * t;
455  } else {
456  // no stop occured within [0,TS], thus (from dist = v0*TS + accel*TS^2/2)
457  const SUMOReal accel = 2 * (dist / TS - v0) / TS;
458  // The speed at time t is then
459  return v0 + accel * t;
460  }
461  }
462 }
463 
464 
465 
466 
467 SUMOReal
468 MSCFModel::estimateSpeedAfterDistance(const SUMOReal dist, const SUMOReal v, const SUMOReal accel) const {
469  // dist=v*t + 0.5*accel*t^2, solve for t and use v1 = v + accel*t
470  return MAX2((SUMOReal)0., MIN2(myType->getMaxSpeed(),
471  (SUMOReal)sqrt(2 * dist * accel + v * v)));
472 }
473 
474 
475 
476 SUMOReal
477 MSCFModel::maximumSafeStopSpeed(SUMOReal g /*gap*/, SUMOReal v /*currentSpeed*/, bool onInsertion, SUMOReal headway) const {
479  return maximumSafeStopSpeedEuler(g);
480  } else {
481  return maximumSafeStopSpeedBallistic(g, v, onInsertion, headway);
482  }
483 }
484 
485 
486 SUMOReal
488  gap -= NUMERICAL_EPS; // lots of code relies on some slack XXX: it shouldn't...
489  if (gap <= 0) {
490  return 0;
491  } else if (gap <= ACCEL2SPEED(myDecel)) {
492  // workaround for #2310
493  return MIN2(ACCEL2SPEED(myDecel), DIST2SPEED(gap));
494  }
495  const SUMOReal g = gap;
496  const SUMOReal b = ACCEL2SPEED(myDecel);
497  const SUMOReal t = myHeadwayTime;
498  const SUMOReal s = TS;
499 
500 
501  // h = the distance that would be covered if it were possible to stop
502  // exactly after gap and decelerate with b every simulation step
503  // h = 0.5 * n * (n-1) * b * s + n * b * t (solve for n)
504  //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));
505  const SUMOReal n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
506  const SUMOReal h = 0.5 * n * (n - 1) * b * s + n * b * t;
507  assert(h <= g + NUMERICAL_EPS);
508  // compute the additional speed that must be used during deceleration to fix
509  // the discrepancy between g and h
510  const SUMOReal r = (g - h) / (n * s + t);
511  const SUMOReal x = n * b + r;
512  assert(x >= 0);
513  return x;
514 }
515 
516 
517 SUMOReal
518 MSCFModel::maximumSafeStopSpeedBallistic(SUMOReal g /*gap*/, SUMOReal v /*currentSpeed*/, bool onInsertion, SUMOReal headway) const {
519  // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
520  g = MAX2((SUMOReal)0., g - NUMERICAL_EPS);
521  headway = headway >= 0 ? headway : myHeadwayTime;
522 
523  // (Leo) Note that in contrast to the Euler update, for the ballistic update
524  // the distance covered in the coming step depends on the current velocity, in general.
525  // one exception is the situation when the vehicle is just being inserted.
526  // In that case, it will not cover any distance until the next timestep by convention.
527 
528  // We treat the latter case first:
529  if (onInsertion) {
530  // The distance covered with constant insertion speed v0 until time tau is given as
531  // G1 = tau*v0
532  // The distance covered between time tau and the stopping moment at time tau+v0/b is
533  // G2 = v0^2/(2b),
534  // where b is an assumed constant deceleration (= myDecel)
535  // We solve g = G1 + G2 for v0:
536  const SUMOReal btau = myDecel * headway;
537  const SUMOReal v0 = -btau + sqrt(btau * btau + 2 * myDecel * g);
538  return v0;
539  }
540 
541  // In the usual case during the driving task, the vehicle goes by
542  // a current speed v0=v, and we seek to determine a safe acceleration a (possibly <0)
543  // such that starting to break after accelerating with a for the time tau
544  // still allows us to stop in time.
545 
546  const SUMOReal tau = headway;
547  const SUMOReal v0 = MAX2((SUMOReal)0., v);
548  // We first consider the case that a stop has to take place within time tau
549  if (v0 * tau >= 2 * g) {
550  if (g == 0.) {
551  if (v0 > 0.) {
552  // indicate to brake as hard as possible
553  return -INVALID_SPEED;
554  } else {
555  // stay stopped
556  return 0.;
557  }
558  }
559  // In general we solve g = v0^2/(-2a), where the the rhs is the distance
560  // covered until stop when breaking with a<0
561  const SUMOReal a = -v0 * v0 / (2 * g);
562  return v0 + a * TS;
563  }
564 
565  // The last case corresponds to a situation, where the vehicle may go with a positive
566  // speed v1 = v0 + tau*a after time tau.
567  // The distance covered until time tau is given as
568  // G1 = tau*(v0+v1)/2
569  // The distance covered between time tau and the stopping moment at time tau+v1/b is
570  // G2 = v1^2/(2b),
571  // where b is an assumed constant deceleration (= myDecel)
572  // We solve g = G1 + G2 for v1>0:
573  // <=> 0 = v1^2 + b*tau*v1 + b*tau*v0 - 2bg
574  // => v1 = -b*tau/2 + sqrt( (b*tau)^2/4 + b(2g - tau*v0) )
575 
576  const SUMOReal btau2 = myDecel * tau / 2;
577  const SUMOReal v1 = -btau2 + sqrt(btau2 * btau2 + myDecel * (2 * g - tau * v0));
578  const SUMOReal a = (v1 - v0) / tau;
579  return v0 + a * TS;
580 }
581 
582 
584 SUMOReal
585 MSCFModel::maximumSafeFollowSpeed(SUMOReal gap, SUMOReal egoSpeed, SUMOReal predSpeed, SUMOReal predMaxDecel, bool onInsertion) const {
586  // the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
587  // the leaders starts braking hard until stopped
588  // unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
589  // (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
590  // 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.
591  // @todo: this is a conservative estimate for safe speed which could be increased
592 
593 // // For negative gaps, we return the lowest meaningful value by convention
594 // // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)
595 // if(gap<0){
596 // if(MSGlobals::gSemiImplicitEulerUpdate){
597 // return 0.;
598 // } else {
599 // return -INVALID_SPEED;
600 // }
601 // }
602 
603  // The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
604  // if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
605  // NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
606  // the follower accelerates for a short period of time. Refs #2310 (Leo)
607 // const SUMOReal headway = predSpeed > 0. ? myHeadwayTime : 0.;
608 
609  const SUMOReal headway = myHeadwayTime;
610  const SUMOReal x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), egoSpeed, onInsertion, headway);
611  assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
612  assert(!ISNAN(x));
613  return x;
614 }
615 
616 
617 /****************************************************************************/
#define DIST2SPEED(x)
Definition: SUMOTime.h:57
SUMOReal getSpeedAfterMaxDecel(SUMOReal v) const
Returns the velocity after maximum deceleration.
Definition: MSCFModel.h:302
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:82
SUMOReal getMaxSpeed() const
Get vehicle's maximum speed [m/s].
long long int SUMOTime
Definition: SUMOTime.h:43
const MSVehicleType * myType
The type to which this model definition belongs to.
Definition: MSCFModel.h:463
#define SPEED2DIST(x)
Definition: SUMOTime.h:55
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:61
SUMOReal maximumSafeStopSpeed(SUMOReal gap, SUMOReal currentSpeed, bool onInsertion=false, SUMOReal headway=-1) const
Returns the maximum next velocity for stopping within gap.
Definition: MSCFModel.cpp:477
virtual SUMOReal insertionFollowSpeed(const MSVehicle *const veh, SUMOReal speed, SUMOReal gap2pred, SUMOReal predSpeed, SUMOReal predMaxDecel) const
Computes the vehicle's safe speed (no dawdling) This method is used during the insertion stage...
Definition: MSCFModel.cpp:213
virtual SUMOReal maxNextSpeed(SUMOReal speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:190
T MAX2(T a, T b)
Definition: StdDefs.h:75
MSCFModel(const MSVehicleType *vtype, SUMOReal accel, SUMOReal decel, SUMOReal headwayTime)
Constructor.
Definition: MSCFModel.cpp:48
SUMOReal myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
Definition: MSCFModel.h:472
#define TIME2STEPS(x)
Definition: SUMOTime.h:66
#define TS
Definition: SUMOTime.h:52
SUMOReal processNextStop(SUMOReal currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
Definition: MSVehicle.cpp:1027
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:63
T MAX3(T a, T b, T c)
Definition: StdDefs.h:89
SUMOReal minNextSpeed(SUMOReal 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:195
The car-following model and parameter.
Definition: MSVehicleType.h:74
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:2702
SUMOTime getMinimalArrivalTime(SUMOReal dist, SUMOReal currentSpeed, SUMOReal arrivalSpeed) const
Computes the minimal time needed to cover a distance given the desired speed at arrival.
Definition: MSCFModel.cpp:234
static SUMOReal speedAfterTime(const SUMOReal t, const SUMOReal oldSpeed, const SUMOReal dist)
Calculates the speed after a time t [0,TS] given the initial speed and the distance traveled in an i...
Definition: MSCFModel.cpp:439
SUMOReal brakeGap(const SUMOReal speed) const
Returns the distance the vehicle needs to halt including driver's reaction time, assuming that during...
Definition: MSCFModel.h:263
#define ACCEL2DIST(x)
Definition: SUMOTime.h:59
SUMOReal getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation...
Definition: MSVehicle.h:497
virtual SUMOReal interactionGap(const MSVehicle *const veh, SUMOReal vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
Definition: MSCFModel.cpp:175
SUMOReal getMinimalArrivalSpeed(SUMOReal dist, SUMOReal currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance.
Definition: MSCFModel.cpp:247
T MIN2(T a, T b)
Definition: StdDefs.h:69
static SUMOReal passingTime(const SUMOReal lastPos, const SUMOReal passedPos, const SUMOReal currentPos, const SUMOReal lastSpeed, const SUMOReal currentSpeed)
Calculates the time at which the position passedPosition has been passed In case of a ballistic updat...
Definition: MSCFModel.cpp:367
SUMOReal getMaxDecel() const
Get the vehicle type's maximum deceleration [m/s^2].
Definition: MSCFModel.h:201
virtual SUMOReal freeSpeed(const MSVehicle *const veh, SUMOReal speed, SUMOReal seen, SUMOReal maxSpeed, const bool onInsertion=false) const
Computes the vehicle's safe speed without a leader.
Definition: MSCFModel.cpp:206
T ISNAN(T a)
Definition: StdDefs.h:110
virtual SUMOReal getHeadwayTime() const
Get the driver's reaction time [s].
Definition: MSCFModel.h:220
virtual SUMOReal moveHelper(MSVehicle *const veh, SUMOReal vPos) const
Applies interaction with stops and lane changing model influences.
Definition: MSCFModel.cpp:144
SUMOReal maximumSafeFollowSpeed(SUMOReal gap, SUMOReal egoSpeed, SUMOReal predSpeed, SUMOReal predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
Definition: MSCFModel.cpp:585
SUMOReal getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:193
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:206
virtual ~MSCFModel()
Destructor.
Definition: MSCFModel.cpp:54
SUMOReal maximumSafeStopSpeedBallistic(SUMOReal gap, SUMOReal currentSpeed, bool onInsertion=false, SUMOReal headway=-1) const
Returns the maximum next velocity for stopping within gap when using the ballistic positional update...
Definition: MSCFModel.cpp:518
SUMOReal maximumSafeStopSpeedEuler(SUMOReal gap) const
Returns the maximum next velocity for stopping within gap when using the semi-implicit Euler update...
Definition: MSCFModel.cpp:487
virtual SUMOReal patchSpeed(const SUMOReal min, const SUMOReal wanted, const SUMOReal max, const MSCFModel &cfModel)=0
Called to adapt the speed in order to allow a lane change.
virtual SUMOReal stopSpeed(const MSVehicle *const veh, const SUMOReal speed, SUMOReal gap) const =0
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling) ...
const SUMOReal SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:57
#define INVALID_SPEED
Definition: MSCFModel.h:40
SUMOReal getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:441
SUMOReal getMinimalArrivalSpeedEuler(SUMOReal dist, SUMOReal currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance for Euler update...
Definition: MSCFModel.cpp:254
#define SUMOReal
Definition: config.h:214
static bool gSemiImplicitEulerUpdate
Definition: MSGlobals.h:63
SUMOReal estimateSpeedAfterDistance(const SUMOReal dist, const SUMOReal v, const SUMOReal accel) const
Definition: MSCFModel.cpp:468
T MIN3(T a, T b, T c)
Definition: StdDefs.h:82
#define NUMERICAL_EPS
Definition: config.h:161
static SUMOReal gapExtrapolation(const SUMOReal duration, const SUMOReal currentGap, SUMOReal v1, SUMOReal v2, SUMOReal a1=0, SUMOReal a2=0, const SUMOReal maxV1=std::numeric_limits< SUMOReal >::max(), const SUMOReal maxV2=std::numeric_limits< SUMOReal >::max())
return the resulting gap if, starting with gap currentGap, two vehicles continue with constant accele...
Definition: MSCFModel.cpp:273
virtual SUMOReal insertionStopSpeed(const MSVehicle *const veh, SUMOReal speed, SUMOReal gap) const
Computes the vehicle's safe speed for approaching an obstacle at insertion without constraints due to...
Definition: MSCFModel.cpp:224
SUMOReal getVehicleMaxSpeed(const SUMOVehicle *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition: MSLane.h:458
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:487
SUMOReal myDecel
The vehicle's maximum deceleration [m/s^2].
Definition: MSCFModel.h:469