44#define DEBUG_COND (veh->isSelected())
48#define DEBUG_COND2 (gDebugFlag1)
63 myHeadwayTime(vtype->getParameter().getCFParam(
SUMO_ATTR_TAU, 1.0)),
83 return speed * (headwayTime + 0.5 * speed / decel);
94 const int steps = int(speed / speedReduction);
95 return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
100MSCFModel::freeSpeed(
const double currentSpeed,
const double decel,
const double dist,
const double targetSpeed,
const bool onInsertion,
const double actionStepLength) {
116 const double y =
MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
117 const double yFull = floor(y);
118 const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
119 const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) *
ACCEL2SPEED(decel);
120 return DIST2SPEED(
MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
135 assert(currentSpeed >= 0);
136 assert(targetSpeed >= 0);
138 const double dt = onInsertion ? 0 : actionStepLength;
139 const double v0 = currentSpeed;
140 const double vT = targetSpeed;
141 const double b = decel;
142 const double d = dist - NUMERICAL_EPS;
152 if (0.5 * (v0 + vT)*dt >= d) {
154 return v0 +
TS * (vT - v0) / actionStepLength;
156 const double q = ((dt * v0 - 2 * d) * b - vT * vT);
157 const double p = 0.5 * b * dt;
158 const double vN = -p + sqrt(p * p - q);
159 return v0 +
TS * (vN - v0) / actionStepLength;
167 const double oldV = veh->
getSpeed();
176 const double factor = fric == 1. ? 1. : -0.3491 * fric * fric + 0.8922 * fric + 0.4493;
191#ifdef DEBUG_FINALIZE_SPEED
193 std::cout <<
"\n" <<
SIMTIME <<
" FINALIZE_SPEED\n";
197 vMax =
MAX2(vMin, vMax);
199#ifdef DEBUG_FINALIZE_SPEED
200 double vDawdle = vNext;
202 assert(vNext >= vMin);
203 assert(vNext <= vMax);
206#ifdef DEBUG_FINALIZE_SPEED
207 double vPatchLC = vNext;
212 assert(vNext >= vMinEmergency);
213 assert(vNext <= vMax);
215#ifdef DEBUG_FINALIZE_SPEED
218 <<
"veh '" << veh->
getID() <<
"' oldV=" << oldV
223 <<
" vStop=" << vStop
224 <<
" vDawdle=" << vDawdle
225 <<
" vPatchLC=" << vPatchLC
226 <<
" vNext=" << vNext
242 if (remainingDelay >=
DELTA_T) {
247 return (
double)(
DELTA_T - remainingDelay) / (
double)
DELTA_T * vMax;
260 const double gap = (vNext - vL) *
323 return stopSpeed(veh, speed, gap, CalcReason::FUTURE);
333 double leaderMinDist = gap2pred +
distAfterTime(duration, predSpeed, -predMaxDecel);
338 const int a = (int)ceil(duration /
TS -
TS);
344 if (
gDebugFlag2) std::cout <<
" followSpeedTransient"
345 <<
" duration=" << duration
346 <<
" gap=" << gap2pred
347 <<
" leaderMinDist=" << leaderMinDist
352 <<
" x=" << (b + leaderMinDist) / duration
354 return (b + leaderMinDist) / duration;
359 while (bg < leaderMinDist) {
368 const double fullBrakingSeconds = sqrt(leaderMinDist * 2 /
myDecel);
369 if (fullBrakingSeconds >= duration) {
373 return leaderMinDist / duration + duration *
getMaxDecel() / 2;
375 return fullBrakingSeconds *
myDecel;
383 return (speed + 0.5 * accel * t) * t;
385 const double decel = -accel;
386 if (speed <= decel * t) {
400 const double speed2 = speed - t * decel;
401 return 0.5 * (speed + speed2) * t;
414 const double accelTime = accel == 0. ? 0. : (arrivalSpeed - currentSpeed) / accel;
415 const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
416 if (dist >= accelWay) {
417 const double nonAccelWay = dist - accelWay;
419 return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
423 return TIME2STEPS(-(currentSpeed - sqrt(currentSpeed * currentSpeed + 2 * accel * dist)) / accel);
432 if (dist < NUMERICAL_EPS) {
436 if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
441 if (fabs(accel) < NUMERICAL_EPS) {
445 double p = speed / accel;
449 return (-p - sqrt(p * p + 2 * dist / accel));
454 double t1 = (maxSpeed - speed) / accel;
456 double d1 = speed * t1 + 0.5 * accel * t1 * t1;
459 return (-p + sqrt(p * p + 2 * dist / accel));
461 return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
475 assert(accel == decel);
477 assert(initialSpeed == 0);
478 assert(arrivalSpeed == 0);
479 assert(maxSpeed > 0);
482 double accelTime = (maxSpeed - initialSpeed) / accel;
484 double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
486 if (accelDist >= dist * 0.5) {
488 arrivalTime = 4 * sqrt(dist / accel);
491 const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
492 arrivalTime = accelTime + constSpeedTime;
500 assert(time > 0 || dist == 0);
503 }
else if (time * speed > 2 * dist) {
506 return - 0.5 * speed * speed / dist;
510 return 2 * (dist / time - speed) / time;
524 double arrivalSpeedBraking;
527 if (dist < currentSpeed) {
535 return arrivalSpeedBraking;
542MSCFModel::gapExtrapolation(
const double duration,
const double currentGap,
double v1,
double v2,
double a1,
double a2,
const double maxV1,
const double maxV2) {
544 double newGap = currentGap;
547 for (
unsigned int steps = 1; steps *
TS <= duration; ++steps) {
548 v1 =
MIN2(
MAX2(v1 + a1, 0.), maxV1);
549 v2 =
MIN2(
MAX2(v2 + a2, 0.), maxV2);
550 newGap +=
TS * (v1 - v2);
555 double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
558 if (a1 < 0 && v1 > 0) {
559 const double leaderStopTime = - v1 / a1;
560 t1 =
MIN2(leaderStopTime, duration);
561 }
else if (a1 >= 0) {
565 if (a2 < 0 && v2 > 0) {
566 const double followerStopTime = -v2 / a2;
567 t2 =
MIN2(followerStopTime, duration);
568 }
else if (a2 >= 0) {
572 if (a1 > 0 && v1 < maxV1) {
573 const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
574 t3 =
MIN2(leaderMaxSpeedTime, duration);
575 }
else if (a1 <= 0) {
579 if (a2 > 0 && v2 < maxV2) {
580 const double followerMaxSpeedTime = (maxV2 - v2) / a2;
581 t4 =
MIN2(followerMaxSpeedTime, duration);
582 }
else if (a2 <= 0) {
594 std::list<double>::const_iterator i;
596 for (i = l.begin(); i != l.end(); ++i) {
598 double dt =
MIN2(*i, duration) - tLast;
601 newGap += dv * dt + da * dt * dt / 2.;
605 if (*i == t1 || *i == t3) {
610 if (*i == t2 || *i == t4) {
615 tLast =
MIN2(*i, duration);
616 if (tLast == duration) {
621 if (duration != tLast) {
623 assert(a1 == 0. && a2 == 0.);
624 double dt = duration - tLast;
636MSCFModel::passingTime(
const double lastPos,
const double passedPos,
const double currentPos,
const double lastSpeed,
const double currentSpeed) {
638 assert(passedPos <= currentPos);
639 assert(passedPos >= lastPos);
640 assert(currentPos > lastPos);
641 assert(currentSpeed >= 0);
643 if (passedPos > currentPos || passedPos < lastPos) {
644 std::stringstream ss;
648 ss <<
"passingTime(): given argument passedPos = " << passedPos <<
" doesn't lie within [lastPos, currentPos] = [" << lastPos <<
", " << currentPos <<
"]\nExtrapolating...";
649 std::cout << ss.str() <<
"\n";
652 const double lastCoveredDist = currentPos - lastPos;
653 const double extrapolated = passedPos > currentPos ?
TS * (passedPos - lastPos) / lastCoveredDist :
TS * (currentPos - passedPos) / lastCoveredDist;
655 }
else if (currentSpeed < 0) {
656 WRITE_ERROR(
TL(
"passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet."));
660 const double distanceOldToPassed = passedPos - lastPos;
664 if (currentSpeed == 0) {
667 const double t = distanceOldToPassed / currentSpeed;
675 if (currentSpeed > 0) {
680 assert(currentSpeed == 0 && lastSpeed != 0);
684 a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
691 if (fabs(a) < NUMERICAL_EPS) {
693 const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
697 const double va = lastSpeed / a;
698 const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
699 assert(t < 1 && t >= 0);
703 const double va = lastSpeed / a;
704 const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
716 assert(t >= 0 && t <=
TS);
724 if (dist <
TS * v0 / 2) {
727 const double accel = - v0 * v0 / (2 * dist);
729 return v0 + accel * t;
732 const double accel = 2 * (dist /
TS - v0) /
TS;
734 return v0 + accel * t;
746 (
double)sqrt(
MAX2(0., 2 * dist * accel + v * v)));
804 const double g = gap - NUMERICAL_EPS;
816 const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
817 const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
818 assert(h <= g + NUMERICAL_EPS);
821 const double r = (g - h) / (n * s + t);
822 const double x = n * b + r;
832 const double g =
MAX2(0., gap - NUMERICAL_EPS);
848 const double btau = decel * headway;
849 const double v0 = -btau + sqrt(btau * btau + 2 * decel * g);
858 const double tau = headway == 0 ?
TS : headway;
859 const double v0 =
MAX2(0., currentSpeed);
861 if (v0 * tau >= 2 * g) {
873 const double a = -v0 * v0 / (2 * g);
888 const double btau2 = decel * tau / 2;
889 const double v1 = -btau2 + sqrt(btau2 * btau2 + decel * (2 * g - tau * v0));
890 const double a = (v1 - v0) / tau;
937 if (origSafeDecel >
myDecel + NUMERICAL_EPS) {
944#ifdef DEBUG_EMERGENCYDECEL
946 std::cout <<
SIMTIME <<
" initial vsafe=" << x
947 <<
" egoSpeed=" << egoSpeed <<
" (origSafeDecel=" << origSafeDecel <<
")"
948 <<
" predSpeed=" << predSpeed <<
" (predDecel=" << predMaxDecel <<
")"
949 <<
" safeDecel=" << safeDecel
956 safeDecel =
MIN2(safeDecel, origSafeDecel);
962#ifdef DEBUG_EMERGENCYDECEL
964 std::cout <<
" -> corrected emergency deceleration: " << safeDecel <<
" newVSafe=" << x << std::endl;
986 const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
988 const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
990#ifdef DEBUG_EMERGENCYDECEL
992 std::cout <<
SIMTIME <<
" calculateEmergencyDeceleration()"
993 <<
" gap=" << gap <<
" egoSpeed=" << egoSpeed <<
" predSpeed=" << predSpeed
994 <<
" predBrakeDist=" << predBrakeDist
1000 if (b1 <= predMaxDecel) {
1002#ifdef DEBUG_EMERGENCYDECEL
1004 std::cout <<
" case 1 ..." << std::endl;
1009#ifdef DEBUG_EMERGENCYDECEL
1011 std::cout <<
" case 2 ...";
1017 const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
1019#ifdef DEBUG_EMERGENCYDECEL
1021 std::cout <<
" b2=" << b2 << std::endl;
1038 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap, pred);
1039 const double perceivedSpeedDifference = veh->
getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
1041#ifdef DEBUG_DRIVER_ERRORS
1045 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
1046 <<
" speed=" << speed <<
" gap=" << gap <<
" leaderSpeed=" << predSpeed
1047 <<
"\n perceivedGap=" << perceivedGap <<
" perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
1048 <<
" perceivedSpeedDifference=" << perceivedSpeedDifference
1050 const double exactFollowSpeed =
followSpeed(veh, speed, gap, predSpeed, predMaxDecel, pred, CalcReason::FUTURE);
1051 const double errorFollowSpeed =
followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel, pred, CalcReason::FUTURE);
1052 const double accelError =
SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
1053 std::cout <<
" gapError=" << perceivedGap - gap <<
" dvError=" << perceivedSpeedDifference - (predSpeed - speed)
1054 <<
"\n resulting accelError: " << accelError << std::endl;
1061 predSpeed = speed + perceivedSpeedDifference;
1076 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap);
1078#ifdef DEBUG_DRIVER_ERRORS
1082 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
1083 <<
" speed=" << speed <<
" gap=" << gap <<
"\n perceivedGap=" << perceivedGap << std::endl;
1084 const double exactStopSpeed =
stopSpeed(veh, speed, gap, CalcReason::FUTURE);
1085 const double errorStopSpeed =
stopSpeed(veh, speed, perceivedGap, CalcReason::FUTURE);
1086 const double accelError =
SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1087 std::cout <<
" gapError=" << perceivedGap - gap <<
"\n resulting accelError: " << accelError << std::endl;
#define EMERGENCY_DECEL_AMPLIFIER
@ SUMO_ATTR_STARTUP_DELAY
@ SUMO_ATTR_APPARENTDECEL
@ SUMO_ATTR_EMERGENCYDECEL
@ SUMO_ATTR_COLLISION_MINGAP_FACTOR
int gPrecision
the precision for floating point outputs
const double INVALID_DOUBLE
#define UNUSED_PARAMETER(x)
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
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 ...
virtual ~VehicleVariables()
double estimateSpeedAfterDistance(const double dist, const double v, const double accel) const
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
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 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 ...
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.
virtual double applyStartupDelay(const MSVehicle *veh, const double vMin, const double vMax, const SUMOTime addTime=0) const
apply speed adaptation on startup
double maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
virtual double interactionGap(const MSVehicle *const veh, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
static double avoidArrivalAccel(double dist, double time, double speed, double maxDecel)
Computes the acceleration needed to arrive not before the given time.
double getMinimalArrivalSpeed(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance.
virtual double patchSpeedBeforeLC(const MSVehicle *veh, double vMin, double vMax) const
apply custom speed adaptations within the given speed bounds
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed without a leader.
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 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....
SUMOTime myStartupDelay
The startup delay after halting [s].
SUMOTime getMinimalArrivalTime(double dist, double currentSpeed, double arrivalSpeed) const
Computes the minimal time needed to cover a distance given the desired speed at arrival.
void applyHeadwayPerceptionError(const MSVehicle *const veh, double speed, double &gap) const
Overwrites gap by the perceived value obtained from the vehicle's driver state.
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...
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...
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences. Called at most once per simulation...
virtual ~MSCFModel()
Destructor.
double maximumSafeStopSpeedEuler(double gap, double decel, bool onInsertion, double headway) const
Returns the maximum next velocity for stopping within gap when using the semi-implicit Euler update.
double myEmergencyDecel
The vehicle's maximum emergency deceleration [m/s^2].
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,...
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
CalcReason
What the return value of stop/follow/free-Speed is used for.
double calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const
Returns the minimal deceleration for following the given leader safely.
MSCFModel(const MSVehicleType *vtype)
Constructor.
double myDecel
The vehicle's maximum deceleration [m/s^2].
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
const MSVehicleType * myType
The type to which this model definition belongs to.
virtual double distAfterTime(double t, double speed, double accel) const
calculates the distance travelled after accelerating for time t
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
double maximumSafeStopSpeedBallistic(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap when using the ballistic positional update.
double getMinimalArrivalSpeedEuler(double dist, double currentSpeed) const
Computes the minimal possible arrival speed after covering a given distance for Euler update.
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....
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
double myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
double stopSpeed(const MSVehicle *const veh, const double speed, double gap, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
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...
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
static bool gSemiImplicitEulerUpdate
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Representation of a vehicle in the micro simulation.
SUMOTime getTimeSinceStartup() const
Returns the SUMOTime spent driving since startup (speed was larger than 0.1m/s)
bool hasDriverState() const
Whether this vehicle is equipped with a MSDriverState.
double getFriction() const
Returns the current friction on the road as perceived by the friction device.
MSAbstractLaneChangeModel & getLaneChangeModel()
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
std::shared_ptr< MSSimpleDriverState > getDriverState() const
Returns the vehicle driver's state.
const MSLane * getLane() const
Returns the lane the vehicle is on.
double getSpeed() const
Returns the vehicle's current speed.
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
The car-following model and parameter.
double getMaxSpeed() const
Get vehicle's (technical) maximum speed [m/s].
const std::string & getID() const
Returns the id.
Structure representing possible vehicle parameter.