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-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
22 // The car-following model abstraction
23 /****************************************************************************/
24 
25 
26 // ===========================================================================
27 // included modules
28 // ===========================================================================
29 #ifdef _MSC_VER
30 #include <windows_config.h>
31 #else
32 #include <config.h>
33 #endif
34 
35 #include <cmath>
36 #include <microsim/MSGlobals.h>
37 #include <microsim/MSVehicleType.h>
38 #include <microsim/MSVehicle.h>
39 #include <microsim/MSLane.h>
41 #include "MSCFModel.h"
42 
43 
44 // ===========================================================================
45 // method definitions
46 // ===========================================================================
47 MSCFModel::MSCFModel(const MSVehicleType* vtype, double accel,
48  double decel, double emergencyDecel, double apparentDecel, double headwayTime) :
49  myType(vtype),
50  myAccel(accel),
51  myDecel(decel),
52  myEmergencyDecel(emergencyDecel),
53  myApparentDecel(apparentDecel),
54  myHeadwayTime(headwayTime) {
55 }
56 
57 
59 
60 
62 
63 
64 double
65 MSCFModel::brakeGap(const double speed, const double decel, const double headwayTime) {
67  return brakeGapEuler(speed, decel, 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 
79 double
80 MSCFModel::brakeGapEuler(const double speed, const double decel, const double headwayTime) {
81  /* one possibility to speed this up is to calculate speedReduction * steps * (steps+1) / 2
82  for small values of steps (up to 10 maybe) and store them in an array */
83  const double speedReduction = ACCEL2SPEED(decel);
84  const int steps = int(speed / speedReduction);
85  return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
86 }
87 
88 
89 double
90 MSCFModel::freeSpeed(const double currentSpeed, const double decel, const double dist, const double targetSpeed, const bool onInsertion, const double actionStepLength) {
91  // XXX: (Leo) This seems to be exclusively called with decel = myDecel (max deceleration) and is not overridden
92  // by any specific CFModel. That may cause undesirable hard braking (at junctions where the vehicle
93  // changes to a road with a lower speed limit).
94 
96  // adapt speed to succeeding lane, no reaction time is involved
97  // when breaking for y steps the following distance g is covered
98  // (drive with v in the final step)
99  // g = (y^2 + y) * 0.5 * b + y * v
100  // y = ((((sqrt((b + 2.0*v)*(b + 2.0*v) + 8.0*b*g)) - b)*0.5 - v)/b)
101  const double v = SPEED2DIST(targetSpeed);
102  if (dist < v) {
103  return targetSpeed;
104  }
105  const double b = ACCEL2DIST(decel);
106  const double y = MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
107  const double yFull = floor(y);
108  const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
109  const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
110  return DIST2SPEED(MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
111  } else {
112  // ballistic update (Leo)
113  // calculate maximum next speed vN that is adjustable to vT=targetSpeed after a distance d=dist
114  // and given a maximal deceleration b=decel, denote the current speed by v0.
115  // the distance covered by a trajectory that attains vN in the next action step (length=dt) and decelerates afterwards
116  // with b is given as
117  // d = 0.5*dt*(v0+vN) + (t-dt)*vN - 0.5*b*(t-dt)^2, (1)
118  // where time t of arrival at d with speed vT is
119  // t = dt + (vN-vT)/b. (2)
120  // We insert (2) into (1) to obtain
121  // d = 0.5*dt*(v0+vN) + vN*(vN-vT)/b - 0.5*b*((vN-vT)/b)^2
122  // 0 = (dt*b*v0 - vT*vT - 2*b*d) + dt*b*vN + vN*vN
123  // and solve for vN
124 
125  assert(currentSpeed >= 0);
126  assert(targetSpeed >= 0);
127 
128  const double dt = onInsertion ? 0 : actionStepLength; // handles case that vehicle is inserted just now (at the end of move)
129  const double v0 = currentSpeed;
130  const double vT = targetSpeed;
131  const double b = decel;
132  const double d = dist - NUMERICAL_EPS; // prevent returning a value > targetSpeed due to rounding errors
133 
134  // Solvability for positive vN (if d is small relative to v0):
135  // 1) If 0.5*(v0+vT)*dt > d, we set vN=vT.
136  // (In case vT<v0, this implies that on the interpolated trajectory there are points beyond d where
137  // the interpolated velocity is larger than vT, but at least on the temporal discretization grid, vT is not exceeded)
138  // 2) We ignore the (possible) constraint vN >= v0 - b*dt, which could lead to a problem if v0 - t*b > vT.
139  // (moveHelper() is responsible for assuring that the next velocity is chosen in accordance with maximal decelerations)
140 
141  // If implied accel a leads to v0 + a*asl < vT, choose acceleration s.th. v0 + a*asl = vT
142  if (0.5 * (v0 + vT)*dt >= d) {
143  // Attain vT after time asl
144  return v0 + TS * (vT - v0) / actionStepLength;
145  } else {
146  const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
147  const double p = 0.5 * b * dt;
148  const double vN = -p + sqrt(p * p - q); // target speed at time t0+asl
149  return v0 + TS * (vN - v0) / actionStepLength;
150  }
151  }
152 }
153 
154 double
155 MSCFModel::moveHelper(MSVehicle* const veh, double vPos) const {
156  const double oldV = veh->getSpeed(); // save old v for optional acceleration computation
157  const double vSafe = MIN2(vPos, veh->processNextStop(vPos)); // process stops
158  // we need the acceleration for emission computation;
159  // in this case, we neglect dawdling, nonetheless, using
160  // vSafe does not incorporate speed reduction due to interaction
161  // on lane changing
162  double vMin, vNext;
163  // aMax: Maximal admissible acceleration until the next action step, such that the vehicle's maximal
164  // desired speed on the current lane will not be exceeded.
165  double aMax = (veh->getMaxSpeedOnLane() - oldV) / veh->getActionStepLengthSecs();
166  const double vMax = MIN3(oldV + ACCEL2SPEED(aMax), maxNextSpeed(oldV, veh), vSafe);
168  // we cannot rely on never braking harder than maxDecel because TraCI or strange cf models may decide to do so
169  // vMin = MIN2(getSpeedAfterMaxDecel(oldV), vMax);
170  vMin = MIN2(minNextSpeed(oldV, veh), vMax);
171  vNext = veh->getLaneChangeModel().patchSpeed(vMin, vMax, vMax, *this);
172  } else {
173  // for ballistic update, negative vnext must be allowed to
174  // indicate a stop within the coming timestep (i.e., to attain negative values)
175  vMin = MIN2(minNextSpeed(oldV, veh), vMax);
176  vNext = veh->getLaneChangeModel().patchSpeed(vMin, vMax, vMax, *this);
177  // (Leo) moveHelper() is responsible for assuring that the next
178  // velocity is chosen in accordance with maximal decelerations.
179  // At this point vNext may also be negative indicating a stop within next step.
180  // Moreover, because maximumSafeStopSpeed() does not consider deceleration bounds
181  // vNext can be a large negative value at this point. We cap vNext here.
182  vNext = MAX2(vNext, vMin);
183  }
184  return vNext;
185 }
186 
187 
188 double
189 MSCFModel::interactionGap(const MSVehicle* const veh, double vL) const {
190  // Resolve the vsafe equation to gap. Assume predecessor has
191  // speed != 0 and that vsafe will be the current speed plus acceleration,
192  // i.e that with this gap there will be no interaction.
193  const double vNext = MIN2(maxNextSpeed(veh->getSpeed(), veh), veh->getLane()->getVehicleMaxSpeed(veh));
194  const double gap = (vNext - vL) *
195  ((veh->getSpeed() + vL) / (2.*myDecel) + myHeadwayTime) +
196  vL * myHeadwayTime;
197 
198  // Don't allow timeHeadWay < deltaT situations.
199  return MAX2(gap, SPEED2DIST(vNext));
200 }
201 
202 
203 double
204 MSCFModel::maxNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
205  return MIN2(speed + (double) ACCEL2SPEED(getMaxAccel()), myType->getMaxSpeed());
206 }
207 
208 
209 double
210 MSCFModel::minNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
212 // return MAX2(speed - ACCEL2SPEED(getMaxDecel()), 0.);
213  return MAX2(speed - ACCEL2SPEED(myEmergencyDecel), 0.);
214  } else {
215  // NOTE: ballistic update allows for negative speeds to indicate a stop within the next timestep
216 // return speed - ACCEL2SPEED(getMaxDecel());
217  return speed - ACCEL2SPEED(myEmergencyDecel);
218  }
219 }
220 
221 
222 double
223 MSCFModel::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion) const {
224  if (maxSpeed < 0.) {
225  // can occur for ballistic update (in context of driving at red light)
226  return maxSpeed;
227  }
228  double vSafe = freeSpeed(speed, myDecel, seen, maxSpeed, onInsertion, veh->getActionStepLengthSecs());
229  return vSafe;
230 }
231 
232 
233 double
234 MSCFModel::insertionFollowSpeed(const MSVehicle* const /* v */, double speed, double gap2pred, double predSpeed, double predMaxDecel) const {
236  return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel);
237  } else {
238  // NOTE: Even for ballistic update, the current speed is irrelevant at insertion, therefore passing 0. (Leo)
239  return maximumSafeFollowSpeed(gap2pred, 0., predSpeed, predMaxDecel, true);
240  }
241 }
242 
243 
244 double
245 MSCFModel::insertionStopSpeed(const MSVehicle* const veh, double speed, double gap) const {
247  return stopSpeed(veh, speed, gap);
248  } else {
249  return MIN2(maximumSafeStopSpeed(gap, 0., true, 0.), myType->getMaxSpeed());
250  }
251 }
252 
253 
254 double
255 MSCFModel::followSpeedTransient(double duration, const MSVehicle* const /*veh*/, double /*speed*/, double gap2pred, double predSpeed, double predMaxDecel) const {
256  // minimium distance covered by the leader if braking
257  double leaderMinDist = gap2pred + distAfterTime(duration, predSpeed, -predMaxDecel);
258  // if ego would not brake it could drive with speed leaderMinDist / duration
259  // due to potentential ego braking it can safely drive faster
261  // number of potential braking steps
262  int a = (int)ceil(duration / TS - TS);
263  // can we brake for the whole time?
264  const double bg = brakeGap(a * myDecel, myDecel, 0);
265  if (bg <= leaderMinDist) {
266  // braking continuously for duration
267  // distance reduction due to braking
268  double b = TS * getMaxDecel() * 0.5 * (a * a - a);
269  if (gDebugFlag2) std::cout << " followSpeedTransient"
270  << " duration=" << duration
271  << " gap=" << gap2pred
272  << " leaderMinDist=" << leaderMinDist
273  << " decel=" << getMaxDecel()
274  << " a=" << a
275  << " bg=" << bg
276  << " b=" << b
277  << " x=" << (b + leaderMinDist) / duration
278  << "\n";
279  return (b + leaderMinDist) / duration;
280  } else {
281  // @todo improve efficiency
282  double bg = 0;
283  double speed = 0;
284  while (bg < leaderMinDist) {
285  speed += ACCEL2SPEED(myDecel);
286  bg += SPEED2DIST(speed);
287  }
288  speed -= DIST2SPEED(bg - leaderMinDist);
289  return speed;
290  }
291  } else {
292  // can we brake for the whole time?
293  const double fullBrakingSeconds = sqrt(leaderMinDist * 2 / myDecel);
294  if (fullBrakingSeconds >= duration) {
295  // braking continuously for duration
296  // average speed after braking for duration is x2 = x - 0.5 * duration * myDecel
297  // x2 * duration <= leaderMinDist must hold
298  return leaderMinDist / duration + duration * getMaxDecel() / 2;
299  } else {
300  return fullBrakingSeconds * myDecel;
301  }
302  }
303 }
304 
305 double
306 MSCFModel::distAfterTime(double t, double speed, const double accel) {
307  if (accel >= 0.) {
308  return (speed + 0.5 * accel * t) * t;
309  }
310  const double decel = -accel;
311  if (speed <= decel * t) {
312  // braking to a full stop
313  return brakeGap(speed, decel, 0);
314  }
316  // @todo improve efficiency
317  double result = 0;
318  while (t > 0) {
319  speed -= ACCEL2SPEED(decel);
320  result += MAX2(0.0, SPEED2DIST(speed));
321  t -= TS;
322  }
323  return result;
324  } else {
325  const double speed2 = speed - t * decel;
326  return 0.5 * (speed + speed2) * t;
327  }
328 }
329 
330 SUMOTime
331 MSCFModel::getMinimalArrivalTime(double dist, double currentSpeed, double arrivalSpeed) const {
332  const double accel = (arrivalSpeed >= currentSpeed) ? getMaxAccel() : -getMaxDecel();
333  const double accelTime = (arrivalSpeed - currentSpeed) / accel;
334  const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
335  const double nonAccelWay = MAX2(0., dist - accelWay);
336  // will either drive as fast as possible and decelerate as late as possible
337  // or accelerate as fast as possible and then hold that speed
338  const double nonAccelSpeed = MAX3(currentSpeed, arrivalSpeed, SUMO_const_haltingSpeed);
339  return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
340 }
341 
342 
343 double
344 MSCFModel::estimateArrivalTime(double dist, double speed, double maxSpeed, double accel) {
345  assert(speed >= 0.);
346  assert(dist >= 0.);
347 
348  if (dist == 0.) {
349  return 0.;
350  }
351 
352  if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
353  // distance will never be covered with these values
354  return INVALID_DOUBLE;
355  }
356 
357  if (accel == 0.) {
358  return dist / speed;
359  }
360 
361  double p = speed / accel;
362 
363  if (accel < 0.) {
364  // we already know, that the distance will be covered despite breaking
365  return (-p - sqrt(p * p + 2 * dist / accel));
366  }
367 
368  // Here, accel > 0
369  // t1 is the time to use the given acceleration
370  double t1 = (maxSpeed - speed) / accel;
371  // distance covered until t1
372  double d1 = speed * t1 + 0.5 * accel * t1 * t1;
373  if (d1 >= dist) {
374  // dist is covered before changing the speed
375  return (-p + sqrt(p * p + 2 * dist / accel));
376  } else {
377  return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
378  }
379 
380 }
381 
382 double
383 MSCFModel::estimateArrivalTime(double dist, double initialSpeed, double arrivalSpeed, double maxSpeed, double accel, double decel) {
384  UNUSED_PARAMETER(arrivalSpeed); // only in assertion
385  UNUSED_PARAMETER(decel); // only in assertion
386  if (dist <= 0) {
387  return 0.;
388  }
389 
390  // stub-assumptions
391  assert(accel == decel);
392  assert(accel > 0);
393  assert(initialSpeed == 0);
394  assert(arrivalSpeed == 0);
395  assert(maxSpeed > 0);
396 
397 
398  double accelTime = (maxSpeed - initialSpeed) / accel;
399  // "ballistic" estimate for the distance covered during acceleration phase
400  double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
401  double arrivalTime;
402  if (accelDist >= dist * 0.5) {
403  // maximal speed will not be attained during maneuver
404  arrivalTime = 4 * sqrt(accelDist) / accel;
405  } else {
406  // Calculate time to move with constant, maximal lateral speed
407  const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
408  arrivalTime = accelTime + constSpeedTime;
409  }
410  return arrivalTime;
411 }
412 
413 
414 double
415 MSCFModel::avoidArrivalAccel(double dist, double time, double speed) {
416  assert(time > 0 || dist == 0);
417  if (dist <= 0) {
418  return -std::numeric_limits<double>::max();
419  } else if (time * speed > 2 * dist) {
420  // stop before dist is necessary. We need
421  // d = v*v/(2*a)
422  return - 0.5 * speed * speed / dist;
423  } else {
424  // we seek the solution a of
425  // d = v*t + a*t*t/2
426  return 2 * (dist / time - speed) / time;
427  }
428 }
429 
430 
431 double
432 MSCFModel::getMinimalArrivalSpeed(double dist, double currentSpeed) const {
433  // ballistic update
434  return estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
435 }
436 
437 
438 double
439 MSCFModel::getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const {
440  double arrivalSpeedBraking;
441  // Because we use a continuous formula for computing the possible slow-down
442  // we need to handle the mismatch with the discrete dynamics
443  if (dist < currentSpeed) {
444  arrivalSpeedBraking = INVALID_SPEED; // no time left for braking after this step
445  // (inserted max() to get rid of arrivalSpeed dependency within method) (Leo)
446  } else if (2 * (dist - currentSpeed * getHeadwayTime()) * -getMaxDecel() + currentSpeed * currentSpeed >= 0) {
447  arrivalSpeedBraking = estimateSpeedAfterDistance(dist - currentSpeed * getHeadwayTime(), currentSpeed, -getMaxDecel());
448  } else {
449  arrivalSpeedBraking = getMaxDecel();
450  }
451  return arrivalSpeedBraking;
452 }
453 
454 
455 
456 
457 double
458 MSCFModel::gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1, double a2, const double maxV1, const double maxV2) {
459 
460  double newGap = currentGap;
461 
463  for (unsigned int steps = 1; steps * TS <= duration; ++steps) {
464  v1 = MIN2(MAX2(v1 + a1, 0.), maxV1);
465  v2 = MIN2(MAX2(v2 + a2, 0.), maxV2);
466  newGap += TS * (v1 - v2);
467  }
468  } else {
469  // determine times t1, t2 for which vehicles can break until stop (within duration)
470  // and t3, t4 for which they reach their maximal speed on their current lanes.
471  double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
472 
473  // t1: ego veh stops
474  if (a1 < 0 && v1 > 0) {
475  const double leaderStopTime = - v1 / a1;
476  t1 = MIN2(leaderStopTime, duration);
477  } else if (a1 >= 0) {
478  t1 = duration;
479  }
480  // t2: veh2 stops
481  if (a2 < 0 && v2 > 0) {
482  const double followerStopTime = -v2 / a2;
483  t2 = MIN2(followerStopTime, duration);
484  } else if (a2 >= 0) {
485  t2 = duration;
486  }
487  // t3: ego veh reaches vMax
488  if (a1 > 0 && v1 < maxV1) {
489  const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
490  t3 = MIN2(leaderMaxSpeedTime, duration);
491  } else if (a1 <= 0) {
492  t3 = duration;
493  }
494  // t4: veh2 reaches vMax
495  if (a2 > 0 && v2 < maxV2) {
496  const double followerMaxSpeedTime = (maxV2 - v2) / a2;
497  t4 = MIN2(followerMaxSpeedTime, duration);
498  } else if (a2 <= 0) {
499  t4 = duration;
500  }
501 
502  // NOTE: this assumes that the accelerations a1 and a2 are constant over the next
503  // followerBreakTime seconds (if no vehicle stops before or reaches vMax)
504  std::list<double> l;
505  l.push_back(t1);
506  l.push_back(t2);
507  l.push_back(t3);
508  l.push_back(t4);
509  l.sort();
510  std::list<double>::const_iterator i;
511  double tLast = 0.;
512  for (i = l.begin(); i != l.end(); ++i) {
513  if (*i != tLast) {
514  double dt = MIN2(*i, duration) - tLast; // time between *i and tLast
515  double dv = v1 - v2; // current velocity difference
516  double da = a1 - a2; // current acceleration difference
517  newGap += dv * dt + da * dt * dt / 2.; // update gap
518  v1 += dt * a1;
519  v2 += dt * a2;
520  }
521  if (*i == t1 || *i == t3) {
522  // ego veh reached velocity bound
523  a1 = 0.;
524  }
525 
526  if (*i == t2 || *i == t4) {
527  // veh2 reached velocity bound
528  a2 = 0.;
529  }
530 
531  tLast = MIN2(*i, duration);
532  if (tLast == duration) {
533  break;
534  }
535  }
536 
537  if (duration != tLast) {
538  // (both vehicles have zero acceleration)
539  assert(a1 == 0. && a2 == 0.);
540  double dt = duration - tLast; // remaining time until duration
541  double dv = v1 - v2; // current velocity difference
542  newGap += dv * dt; // update gap
543  }
544  }
545 
546  return newGap;
547 }
548 
549 
550 
551 double
552 MSCFModel::passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed) {
553 
554  assert(passedPos <= currentPos && passedPos >= lastPos && currentPos > lastPos);
555  assert(currentSpeed >= 0);
556 
557  if (passedPos > currentPos || passedPos < lastPos) {
558  std::stringstream ss;
559  // Debug (Leo)
561  // NOTE: error is guarded to maintain original test output for euler update (Leo).
562  ss << "passingTime(): given argument passedPos = " << passedPos << " doesn't lie within [lastPos, currentPos] = [" << lastPos << ", " << currentPos << "]\nExtrapolating...";
563  std::cout << ss.str() << "\n";
564  WRITE_ERROR(ss.str());
565  }
566  const double lastCoveredDist = currentPos - lastPos;
567  const double extrapolated = passedPos > currentPos ? TS * (passedPos - lastPos) / lastCoveredDist : TS * (currentPos - passedPos) / lastCoveredDist;
568  return extrapolated;
569  } else if (currentSpeed < 0) {
570  WRITE_ERROR("passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
571  return -1;
572  }
573 
574  const double distanceOldToPassed = passedPos - lastPos; // assert: >=0
575 
577  // euler update (constantly moving with currentSpeed during [0,TS])
578  const double t = distanceOldToPassed / currentSpeed;
579  return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
580 
581  } else {
582  // ballistic update (constant acceleration a during [0,TS], except in case of a stop)
583 
584  // determine acceleration
585  double a;
586  if (currentSpeed > 0) {
587  // the acceleration was constant within the last time step
588  a = SPEED2ACCEL(currentSpeed - lastSpeed);
589  } else {
590  // the currentSpeed is zero (the last was not because lastPos<currentPos).
591  assert(currentSpeed == 0 && lastSpeed != 0);
592  // In general the stop has taken place within the last time step.
593  // The acceleration (a<0) is obtained from
594  // deltaPos = - lastSpeed^2/(2*a)
595  a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
596 
597  assert(a < 0);
598  }
599 
600  // determine passing time t
601  // we solve distanceOldToPassed = lastSpeed*t + a*t^2/2
602  if (fabs(a) < NUMERICAL_EPS) {
603  // treat as constant speed within [0, TS]
604  const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
605  return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
606  } else if (a > 0) {
607  // positive acceleration => only one positive solution
608  const double va = lastSpeed / a;
609  const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
610  assert(t < 1 && t >= 0);
611  return t;
612  } else {
613  // negative acceleration => two positive solutions (pick the smaller one.)
614  const double va = lastSpeed / a;
615  const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
616  assert(t < 1 && t >= 0);
617  return t;
618  }
619  }
620 }
621 
622 
623 double
624 MSCFModel::speedAfterTime(const double t, const double v0, const double dist) {
625  assert(dist >= 0);
626  assert(t >= 0 && t <= TS);
628  // euler: constant speed within [0,TS]
629  return DIST2SPEED(dist);
630  } else {
631  // ballistic: piecewise constant acceleration in [0,TS] (may become 0 for a stop within TS)
632  // We reconstruct acceleration at time t=0. Note that the covered distance in case
633  // of a stop exactly at t=TS is TS*v0/2.
634  if (dist < TS * v0 / 2) {
635  // stop must have occured within [0,TS], use dist = -v0^2/(2a) (stopping dist),
636  // i.e., a = -v0^2/(2*dist)
637  const double accel = - v0 * v0 / (2 * dist);
638  // The speed at time t is then
639  return v0 + accel * t;
640  } else {
641  // no stop occured within [0,TS], thus (from dist = v0*TS + accel*TS^2/2)
642  const double accel = 2 * (dist / TS - v0) / TS;
643  // The speed at time t is then
644  return v0 + accel * t;
645  }
646  }
647 }
648 
649 
650 
651 
652 double
653 MSCFModel::estimateSpeedAfterDistance(const double dist, const double v, const double accel) const {
654  // dist=v*t + 0.5*accel*t^2, solve for t and use v1 = v + accel*t
655  return MAX2(0., MIN2(myType->getMaxSpeed(),
656  (double)sqrt(2 * dist * accel + v * v)));
657 }
658 
659 
660 
661 double
662 MSCFModel::maximumSafeStopSpeed(double g /*gap*/, double v /*currentSpeed*/, bool onInsertion, double headway) const {
664  return maximumSafeStopSpeedEuler(g);
665  } else {
666  return maximumSafeStopSpeedBallistic(g, v, onInsertion, headway);
667  }
668 }
669 
670 
671 double
673  gap -= NUMERICAL_EPS; // lots of code relies on some slack XXX: it shouldn't...
674  if (gap <= 0) {
675  return 0;
676  } else if (gap <= ACCEL2SPEED(myDecel)) {
677  // workaround for #2310
678  return MIN2(ACCEL2SPEED(myDecel), DIST2SPEED(gap));
679  }
680  const double g = gap;
681  const double b = ACCEL2SPEED(myDecel);
682  const double t = myHeadwayTime;
683  const double s = TS;
684 
685 
686  // h = the distance that would be covered if it were possible to stop
687  // exactly after gap and decelerate with b every simulation step
688  // h = 0.5 * n * (n-1) * b * s + n * b * t (solve for n)
689  //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));
690  const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
691  const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
692  assert(h <= g + NUMERICAL_EPS);
693  // compute the additional speed that must be used during deceleration to fix
694  // the discrepancy between g and h
695  const double r = (g - h) / (n * s + t);
696  const double x = n * b + r;
697  assert(x >= 0);
698  return x;
699 }
700 
701 
702 double
703 MSCFModel::maximumSafeStopSpeedBallistic(double g /*gap*/, double v /*currentSpeed*/, bool onInsertion, double headway) const {
704  // decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
705  g = MAX2(0., g - NUMERICAL_EPS);
706  headway = headway >= 0 ? headway : myHeadwayTime;
707 
708  // (Leo) Note that in contrast to the Euler update, for the ballistic update
709  // the distance covered in the coming step depends on the current velocity, in general.
710  // one exception is the situation when the vehicle is just being inserted.
711  // In that case, it will not cover any distance until the next timestep by convention.
712 
713  // We treat the latter case first:
714  if (onInsertion) {
715  // The distance covered with constant insertion speed v0 until time tau is given as
716  // G1 = tau*v0
717  // The distance covered between time tau and the stopping moment at time tau+v0/b is
718  // G2 = v0^2/(2b),
719  // where b is an assumed constant deceleration (= myDecel)
720  // We solve g = G1 + G2 for v0:
721  const double btau = myDecel * headway;
722  const double v0 = -btau + sqrt(btau * btau + 2 * myDecel * g);
723  return v0;
724  }
725 
726  // In the usual case during the driving task, the vehicle goes by
727  // a current speed v0=v, and we seek to determine a safe acceleration a (possibly <0)
728  // such that starting to break after accelerating with a for the time tau=headway
729  // still allows us to stop in time.
730 
731  const double tau = headway;
732  const double v0 = MAX2(0., v);
733  // We first consider the case that a stop has to take place within time tau
734  if (v0 * tau >= 2 * g) {
735  if (g == 0.) {
736  if (v0 > 0.) {
737  // indicate to brake as hard as possible
738  return -INVALID_SPEED;
739  } else {
740  // stay stopped
741  return 0.;
742  }
743  }
744  // In general we solve g = v0^2/(-2a), where the the rhs is the distance
745  // covered until stop when breaking with a<0
746  const double a = -v0 * v0 / (2 * g);
747  return v0 + a * TS;
748  }
749 
750  // The last case corresponds to a situation, where the vehicle may go with a positive
751  // speed v1 = v0 + tau*a after time tau.
752  // The distance covered until time tau is given as
753  // G1 = tau*(v0+v1)/2
754  // The distance covered between time tau and the stopping moment at time tau+v1/b is
755  // G2 = v1^2/(2b),
756  // where b is an assumed constant deceleration (= myDecel)
757  // We solve g = G1 + G2 for v1>0:
758  // <=> 0 = v1^2 + b*tau*v1 + b*tau*v0 - 2bg
759  // => v1 = -b*tau/2 + sqrt( (b*tau)^2/4 + b(2g - tau*v0) )
760 
761  const double btau2 = myDecel * tau / 2;
762  const double v1 = -btau2 + sqrt(btau2 * btau2 + myDecel * (2 * g - tau * v0));
763  const double a = (v1 - v0) / tau;
764  return v0 + a * TS;
765 }
766 
767 
769 double
770 MSCFModel::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
771  // the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
772  // the leaders starts braking hard until stopped
773  // unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
774  // (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
775  // 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.
776  // @todo: this is a conservative estimate for safe speed which could be increased
777 
778 // // For negative gaps, we return the lowest meaningful value by convention
779 // // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)
780 // if(gap<0){
781 // if(MSGlobals::gSemiImplicitEulerUpdate){
782 // return 0.;
783 // } else {
784 // return -INVALID_SPEED;
785 // }
786 // }
787 
788  // The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
789  // if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
790  // NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
791  // the follower accelerates for a short period of time. Refs #2310 (Leo)
792 // const double headway = predSpeed > 0. ? myHeadwayTime : 0.;
793 
794  const double headway = myHeadwayTime;
795  const double x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), egoSpeed, onInsertion, headway);
796  assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
797  assert(!ISNAN(x));
798  return x;
799 }
800 
801 
802 /****************************************************************************/
static double speedAfterTime(const double t, const double oldSpeed, const double dist)
Calculates the speed after a time t [0,TS] given the initial speed and the distance traveled in an i...
Definition: MSCFModel.cpp:624
#define DIST2SPEED(x)
Definition: SUMOTime.h:56
double getVehicleMaxSpeed(const SUMOVehicle *const veh) const
Returns the lane&#39;s maximum speed, given a vehicle&#39;s speed limit adaptation.
Definition: MSLane.h:475
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:770
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver&#39;s reaction time tau (i...
Definition: MSCFModel.h:289
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false) const
Computes the vehicle&#39;s safe speed without a leader.
Definition: MSCFModel.cpp:223
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:83
const MSVehicleType * myType
The type to which this model definition belongs to.
Definition: MSCFModel.h:531
#define SPEED2DIST(x)
Definition: SUMOTime.h:54
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:60
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:564
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:210
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:204
T MAX2(T a, T b)
Definition: StdDefs.h:73
virtual double moveHelper(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences.
Definition: MSCFModel.cpp:155
#define TIME2STEPS(x)
Definition: SUMOTime.h:66
#define TS
Definition: SUMOTime.h:51
virtual double insertionStopSpeed(const MSVehicle *const veh, double speed, double gap) const
Computes the vehicle&#39;s safe speed for approaching an obstacle at insertion without constraints due to...
Definition: MSCFModel.cpp:245
static double avoidArrivalAccel(double dist, double time, double speed)
Computes the acceleration needed to arrive not before the given time.
Definition: MSCFModel.cpp:415
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:62
T MAX3(T a, T b, T c)
Definition: StdDefs.h:87
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:39
virtual double insertionFollowSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const
Computes the vehicle&#39;s safe speed (no dawdling) This method is used during the insertion stage...
Definition: MSCFModel.cpp:234
The car-following model and parameter.
Definition: MSVehicleType.h:72
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:3674
double getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation...
Definition: MSVehicle.h:574
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 getMaxAccel() const
Get the vehicle type&#39;s maximum acceleration [m/s^2].
Definition: MSCFModel.h:203
static double distAfterTime(double t, double speed, double accel)
calculates the distance travelled after accelerating for time t
Definition: MSCFModel.cpp:306
double getActionStepLengthSecs() const
Returns the vehicle&#39;s action step length in secs, i.e. the interval between two action points...
Definition: MSVehicle.h:516
#define ACCEL2DIST(x)
Definition: SUMOTime.h:58
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
Definition: MSCFModel.cpp:80
double getMaxSpeed() const
Get vehicle&#39;s maximum speed [m/s].
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:189
T MIN2(T a, T b)
Definition: StdDefs.h:67
double getMaxDecel() const
Get the vehicle type&#39;s maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:211
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:662
double getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance for Euler update...
Definition: MSCFModel.cpp:439
T ISNAN(T a)
Definition: StdDefs.h:108
double myDecel
The vehicle&#39;s maximum deceleration [m/s^2].
Definition: MSCFModel.h:537
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:458
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:205
virtual ~MSCFModel()
Destructor.
Definition: MSCFModel.cpp:58
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:552
double maximumSafeStopSpeedEuler(double gap) const
Returns the maximum next velocity for stopping within gap when using the semi-implicit Euler update...
Definition: MSCFModel.cpp:672
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
Definition: MSVehicle.cpp:1376
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:703
virtual double getHeadwayTime() const
Get the driver&#39;s desired headway [s].
Definition: MSCFModel.h:246
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:344
double myEmergencyDecel
The vehicle&#39;s maximum emergency deceleration [m/s^2].
Definition: MSCFModel.h:539
double getMinimalArrivalSpeed(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance.
Definition: MSCFModel.cpp:432
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:331
#define INVALID_SPEED
Definition: MSCFModel.h:39
MSCFModel(const MSVehicleType *vtype, double accel, double decel, double emergencyDecel, double apparentDecel, double headwayTime)
Constructor.
Definition: MSCFModel.cpp:47
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:57
virtual double followSpeedTransient(double duration, const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const
Computes the vehicle&#39;s follow speed that avoids a collision for the given amount of time...
Definition: MSCFModel.cpp:255
double estimateSpeedAfterDistance(const double dist, const double v, const double accel) const
Definition: MSCFModel.cpp:653
bool gDebugFlag2
Definition: StdDefs.cpp:33
static bool gSemiImplicitEulerUpdate
Definition: MSGlobals.h:62
T MIN3(T a, T b, T c)
Definition: StdDefs.h:80
long long int SUMOTime
Definition: TraCIDefs.h:51
#define NUMERICAL_EPS
Definition: config.h:151
const double INVALID_DOUBLE
Definition: StdDefs.h:59
double myHeadwayTime
The driver&#39;s desired time headway (aka reaction time tau) [s].
Definition: MSCFModel.h:544
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:482
virtual double stopSpeed(const MSVehicle *const veh, const double speed, double gap) const =0
Computes the vehicle&#39;s safe speed for approaching a non-moving obstacle (no dawdling) ...