48 double decel,
double emergencyDecel,
double apparentDecel,
double headwayTime) :
52 myEmergencyDecel(emergencyDecel),
53 myApparentDecel(apparentDecel),
54 myHeadwayTime(headwayTime) {
73 return speed * (headwayTime + 0.5 * speed / decel);
84 const int steps = int(speed / speedReduction);
85 return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
90 MSCFModel::freeSpeed(
const double currentSpeed,
const double decel,
const double dist,
const double targetSpeed,
const bool onInsertion,
const double actionStepLength) {
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;
125 assert(currentSpeed >= 0);
126 assert(targetSpeed >= 0);
128 const double dt = onInsertion ? 0 : actionStepLength;
129 const double v0 = currentSpeed;
130 const double vT = targetSpeed;
131 const double b = decel;
142 if (0.5 * (v0 + vT)*dt >= d) {
144 return v0 +
TS * (vT - v0) / actionStepLength;
146 const double q = ((dt * v0 - 2 * d) * b - vT * vT);
147 const double p = 0.5 * b * dt;
148 const double vN = -p + sqrt(p * p - q);
149 return v0 +
TS * (vN - v0) / actionStepLength;
156 const double oldV = veh->
getSpeed();
182 vNext =
MAX2(vNext, vMin);
194 const double gap = (vNext - vL) *
257 double leaderMinDist = gap2pred +
distAfterTime(duration, predSpeed, -predMaxDecel);
262 int a = (int)ceil(duration /
TS -
TS);
265 if (bg <= leaderMinDist) {
269 if (
gDebugFlag2) std::cout <<
" followSpeedTransient" 270 <<
" duration=" << duration
271 <<
" gap=" << gap2pred
272 <<
" leaderMinDist=" << leaderMinDist
277 <<
" x=" << (b + leaderMinDist) / duration
279 return (b + leaderMinDist) / duration;
284 while (bg < leaderMinDist) {
293 const double fullBrakingSeconds = sqrt(leaderMinDist * 2 /
myDecel);
294 if (fullBrakingSeconds >= duration) {
298 return leaderMinDist / duration + duration *
getMaxDecel() / 2;
300 return fullBrakingSeconds *
myDecel;
308 return (speed + 0.5 * accel * t) * t;
310 const double decel = -accel;
311 if (speed <= decel * t) {
325 const double speed2 = speed - t * decel;
326 return 0.5 * (speed + speed2) * t;
333 const double accelTime = (arrivalSpeed - currentSpeed) / accel;
334 const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
335 const double nonAccelWay =
MAX2(0., dist - accelWay);
339 return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
352 if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
361 double p = speed / accel;
365 return (-p - sqrt(p * p + 2 * dist / accel));
370 double t1 = (maxSpeed - speed) / accel;
372 double d1 = speed * t1 + 0.5 * accel * t1 * t1;
375 return (-p + sqrt(p * p + 2 * dist / accel));
377 return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
391 assert(accel == decel);
393 assert(initialSpeed == 0);
394 assert(arrivalSpeed == 0);
395 assert(maxSpeed > 0);
398 double accelTime = (maxSpeed - initialSpeed) / accel;
400 double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
402 if (accelDist >= dist * 0.5) {
404 arrivalTime = 4 * sqrt(accelDist) / accel;
407 const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
408 arrivalTime = accelTime + constSpeedTime;
416 assert(time > 0 || dist == 0);
418 return -std::numeric_limits<double>::max();
419 }
else if (time * speed > 2 * dist) {
422 return - 0.5 * speed * speed / dist;
426 return 2 * (dist / time - speed) / time;
440 double arrivalSpeedBraking;
443 if (dist < currentSpeed) {
451 return arrivalSpeedBraking;
458 MSCFModel::gapExtrapolation(
const double duration,
const double currentGap,
double v1,
double v2,
double a1,
double a2,
const double maxV1,
const double maxV2) {
460 double newGap = currentGap;
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);
471 double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
474 if (a1 < 0 && v1 > 0) {
475 const double leaderStopTime = - v1 / a1;
476 t1 =
MIN2(leaderStopTime, duration);
477 }
else if (a1 >= 0) {
481 if (a2 < 0 && v2 > 0) {
482 const double followerStopTime = -v2 / a2;
483 t2 =
MIN2(followerStopTime, duration);
484 }
else if (a2 >= 0) {
488 if (a1 > 0 && v1 < maxV1) {
489 const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
490 t3 =
MIN2(leaderMaxSpeedTime, duration);
491 }
else if (a1 <= 0) {
495 if (a2 > 0 && v2 < maxV2) {
496 const double followerMaxSpeedTime = (maxV2 - v2) / a2;
497 t4 =
MIN2(followerMaxSpeedTime, duration);
498 }
else if (a2 <= 0) {
510 std::list<double>::const_iterator i;
512 for (i = l.begin(); i != l.end(); ++i) {
514 double dt =
MIN2(*i, duration) - tLast;
517 newGap += dv * dt + da * dt * dt / 2.;
521 if (*i == t1 || *i == t3) {
526 if (*i == t2 || *i == t4) {
531 tLast =
MIN2(*i, duration);
532 if (tLast == duration) {
537 if (duration != tLast) {
539 assert(a1 == 0. && a2 == 0.);
540 double dt = duration - tLast;
552 MSCFModel::passingTime(
const double lastPos,
const double passedPos,
const double currentPos,
const double lastSpeed,
const double currentSpeed) {
554 assert(passedPos <= currentPos && passedPos >= lastPos && currentPos > lastPos);
555 assert(currentSpeed >= 0);
557 if (passedPos > currentPos || passedPos < lastPos) {
558 std::stringstream ss;
562 ss <<
"passingTime(): given argument passedPos = " << passedPos <<
" doesn't lie within [lastPos, currentPos] = [" << lastPos <<
", " << currentPos <<
"]\nExtrapolating...";
563 std::cout << ss.str() <<
"\n";
566 const double lastCoveredDist = currentPos - lastPos;
567 const double extrapolated = passedPos > currentPos ?
TS * (passedPos - lastPos) / lastCoveredDist :
TS * (currentPos - passedPos) / lastCoveredDist;
569 }
else if (currentSpeed < 0) {
570 WRITE_ERROR(
"passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
574 const double distanceOldToPassed = passedPos - lastPos;
578 const double t = distanceOldToPassed / currentSpeed;
586 if (currentSpeed > 0) {
591 assert(currentSpeed == 0 && lastSpeed != 0);
595 a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
604 const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
608 const double va = lastSpeed / a;
609 const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
610 assert(t < 1 && t >= 0);
614 const double va = lastSpeed / a;
615 const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
616 assert(t < 1 && t >= 0);
626 assert(t >= 0 && t <=
TS);
634 if (dist <
TS * v0 / 2) {
637 const double accel = - v0 * v0 / (2 * dist);
639 return v0 + accel * t;
642 const double accel = 2 * (dist /
TS - v0) /
TS;
644 return v0 + accel * t;
656 (double)sqrt(2 * dist * accel + v * v)));
680 const double g = gap;
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;
695 const double r = (g - h) / (n * s + t);
696 const double x = n * b + r;
721 const double btau =
myDecel * headway;
722 const double v0 = -btau + sqrt(btau * btau + 2 *
myDecel * g);
731 const double tau = headway;
732 const double v0 =
MAX2(0., v);
734 if (v0 * tau >= 2 * g) {
746 const double a = -v0 * v0 / (2 * g);
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;
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...
double getVehicleMaxSpeed(const SUMOVehicle *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i...
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.
Representation of a vehicle in the micro simulation.
const MSVehicleType * myType
The type to which this model definition belongs to.
virtual ~VehicleVariables()
MSLane * getLane() const
Returns the lane the vehicle is on.
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...
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
virtual double moveHelper(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences.
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...
static double avoidArrivalAccel(double dist, double time, double speed)
Computes the acceleration needed to arrive not before the given time.
#define UNUSED_PARAMETER(x)
virtual double insertionFollowSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const
Computes the vehicle's safe speed (no dawdling) This method is used during the insertion stage...
The car-following model and parameter.
MSAbstractLaneChangeModel & getLaneChangeModel()
double getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation...
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's maximum acceleration [m/s^2].
static double distAfterTime(double t, double speed, double accel)
calculates the distance travelled after accelerating for time t
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points...
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
double getMaxSpeed() const
Get vehicle'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.
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
double maximumSafeStopSpeed(double gap, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
double getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance for Euler update...
double myDecel
The vehicle's maximum deceleration [m/s^2].
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...
virtual ~MSCFModel()
Destructor.
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...
double maximumSafeStopSpeedEuler(double gap) const
Returns the maximum next velocity for stopping within gap when using the semi-implicit Euler update...
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
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...
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
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...
double myEmergencyDecel
The vehicle's maximum emergency deceleration [m/s^2].
double getMinimalArrivalSpeed(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance.
SUMOTime getMinimalArrivalTime(double dist, double currentSpeed, double arrivalSpeed) const
Computes the minimal time needed to cover a distance given the desired speed at arrival.
MSCFModel(const MSVehicleType *vtype, double accel, double decel, double emergencyDecel, double apparentDecel, double headwayTime)
Constructor.
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
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...
double estimateSpeedAfterDistance(const double dist, const double v, const double accel) const
static bool gSemiImplicitEulerUpdate
const double INVALID_DOUBLE
double myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
double getSpeed() const
Returns the vehicle's current speed.
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) ...