44 #define DEBUG_COND (veh->isSelected())
47 #define DEBUG_COND2 (gDebugFlag1)
62 myHeadwayTime(vtype->getParameter().getCFParam(
SUMO_ATTR_TAU, 1.0)) {
81 return speed * (headwayTime + 0.5 * speed / decel);
92 const int steps = int(speed / speedReduction);
93 return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
98 MSCFModel::freeSpeed(
const double currentSpeed,
const double decel,
const double dist,
const double targetSpeed,
const bool onInsertion,
const double actionStepLength) {
114 const double y =
MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
115 const double yFull = floor(y);
116 const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
117 const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) *
ACCEL2SPEED(decel);
118 return DIST2SPEED(
MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
133 assert(currentSpeed >= 0);
134 assert(targetSpeed >= 0);
136 const double dt = onInsertion ? 0 : actionStepLength;
137 const double v0 = currentSpeed;
138 const double vT = targetSpeed;
139 const double b = decel;
140 const double d = dist - NUMERICAL_EPS;
150 if (0.5 * (v0 + vT)*dt >= d) {
152 return v0 +
TS * (vT - v0) / actionStepLength;
154 const double q = ((dt * v0 - 2 * d) * b - vT * vT);
155 const double p = 0.5 * b * dt;
156 const double vN = -p + sqrt(p * p - q);
157 return v0 +
TS * (vN - v0) / actionStepLength;
165 const double oldV = veh->
getSpeed();
185 #ifdef DEBUG_FINALIZE_SPEED
187 std::cout <<
"\n" <<
SIMTIME <<
" FINALIZE_SPEED\n";
191 vMax =
MAX2(vMin, vMax);
194 #ifdef DEBUG_FINALIZE_SPEED
195 double vDawdle = vNext;
197 assert(vNext >= vMin);
198 assert(vNext <= vMax);
201 assert(vNext >= vMin);
202 assert(vNext <= vMax);
204 #ifdef DEBUG_FINALIZE_SPEED
207 <<
"veh '" << veh->
getID() <<
"' oldV=" << oldV
211 <<
" vStop=" << vStop
212 <<
" vDawdle=" << vDawdle
213 <<
" vNext=" << vNext
227 const double gap = (vNext - vL) *
300 double leaderMinDist = gap2pred +
distAfterTime(duration, predSpeed, -predMaxDecel);
305 const int a = (int)ceil(duration /
TS -
TS);
311 if (
gDebugFlag2) std::cout <<
" followSpeedTransient"
312 <<
" duration=" << duration
313 <<
" gap=" << gap2pred
314 <<
" leaderMinDist=" << leaderMinDist
319 <<
" x=" << (b + leaderMinDist) / duration
321 return (b + leaderMinDist) / duration;
326 while (bg < leaderMinDist) {
335 const double fullBrakingSeconds = sqrt(leaderMinDist * 2 /
myDecel);
336 if (fullBrakingSeconds >= duration) {
340 return leaderMinDist / duration + duration *
getMaxDecel() / 2;
342 return fullBrakingSeconds *
myDecel;
350 return (speed + 0.5 * accel * t) * t;
352 const double decel = -accel;
353 if (speed <= decel * t) {
367 const double speed2 = speed - t * decel;
368 return 0.5 * (speed + speed2) * t;
377 const double accelTime = (arrivalSpeed - currentSpeed) / accel;
378 const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
379 if (dist >= accelWay) {
380 const double nonAccelWay = dist - accelWay;
382 return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
386 return TIME2STEPS((currentSpeed + -1 * sqrt(currentSpeed * currentSpeed + 2 * accel * dist)) * (-1 / accel));
396 if (dist < NUMERICAL_EPS) {
400 if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
405 if (fabs(accel) < NUMERICAL_EPS) {
409 double p = speed / accel;
413 return (-p - sqrt(p * p + 2 * dist / accel));
418 double t1 = (maxSpeed - speed) / accel;
420 double d1 = speed * t1 + 0.5 * accel * t1 * t1;
423 return (-p + sqrt(p * p + 2 * dist / accel));
425 return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
439 assert(accel == decel);
441 assert(initialSpeed == 0);
442 assert(arrivalSpeed == 0);
443 assert(maxSpeed > 0);
446 double accelTime = (maxSpeed - initialSpeed) / accel;
448 double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
450 if (accelDist >= dist * 0.5) {
452 arrivalTime = 4 * sqrt(dist / accel);
455 const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
456 arrivalTime = accelTime + constSpeedTime;
464 assert(time > 0 || dist == 0);
467 }
else if (time * speed > 2 * dist) {
470 return - 0.5 * speed * speed / dist;
474 return 2 * (dist / time - speed) / time;
488 double arrivalSpeedBraking;
491 if (dist < currentSpeed) {
499 return arrivalSpeedBraking;
506 MSCFModel::gapExtrapolation(
const double duration,
const double currentGap,
double v1,
double v2,
double a1,
double a2,
const double maxV1,
const double maxV2) {
508 double newGap = currentGap;
511 for (
unsigned int steps = 1; steps *
TS <= duration; ++steps) {
512 v1 =
MIN2(
MAX2(v1 + a1, 0.), maxV1);
513 v2 =
MIN2(
MAX2(v2 + a2, 0.), maxV2);
514 newGap +=
TS * (v1 - v2);
519 double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
522 if (a1 < 0 && v1 > 0) {
523 const double leaderStopTime = - v1 / a1;
524 t1 =
MIN2(leaderStopTime, duration);
525 }
else if (a1 >= 0) {
529 if (a2 < 0 && v2 > 0) {
530 const double followerStopTime = -v2 / a2;
531 t2 =
MIN2(followerStopTime, duration);
532 }
else if (a2 >= 0) {
536 if (a1 > 0 && v1 < maxV1) {
537 const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
538 t3 =
MIN2(leaderMaxSpeedTime, duration);
539 }
else if (a1 <= 0) {
543 if (a2 > 0 && v2 < maxV2) {
544 const double followerMaxSpeedTime = (maxV2 - v2) / a2;
545 t4 =
MIN2(followerMaxSpeedTime, duration);
546 }
else if (a2 <= 0) {
558 std::list<double>::const_iterator i;
560 for (i = l.begin(); i != l.end(); ++i) {
562 double dt =
MIN2(*i, duration) - tLast;
565 newGap += dv * dt + da * dt * dt / 2.;
569 if (*i == t1 || *i == t3) {
574 if (*i == t2 || *i == t4) {
579 tLast =
MIN2(*i, duration);
580 if (tLast == duration) {
585 if (duration != tLast) {
587 assert(a1 == 0. && a2 == 0.);
588 double dt = duration - tLast;
600 MSCFModel::passingTime(
const double lastPos,
const double passedPos,
const double currentPos,
const double lastSpeed,
const double currentSpeed) {
602 assert(passedPos <= currentPos);
603 assert(passedPos >= lastPos);
604 assert(currentPos > lastPos);
605 assert(currentSpeed >= 0);
607 if (passedPos > currentPos || passedPos < lastPos) {
608 std::stringstream ss;
612 ss <<
"passingTime(): given argument passedPos = " << passedPos <<
" doesn't lie within [lastPos, currentPos] = [" << lastPos <<
", " << currentPos <<
"]\nExtrapolating...";
613 std::cout << ss.str() <<
"\n";
616 const double lastCoveredDist = currentPos - lastPos;
617 const double extrapolated = passedPos > currentPos ?
TS * (passedPos - lastPos) / lastCoveredDist :
TS * (currentPos - passedPos) / lastCoveredDist;
619 }
else if (currentSpeed < 0) {
620 WRITE_ERROR(
"passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
624 const double distanceOldToPassed = passedPos - lastPos;
628 if (currentSpeed == 0) {
631 const double t = distanceOldToPassed / currentSpeed;
639 if (currentSpeed > 0) {
644 assert(currentSpeed == 0 && lastSpeed != 0);
648 a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
655 if (fabs(a) < NUMERICAL_EPS) {
657 const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
661 const double va = lastSpeed / a;
662 const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
663 assert(t < 1 && t >= 0);
667 const double va = lastSpeed / a;
668 const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
680 assert(t >= 0 && t <=
TS);
688 if (dist <
TS * v0 / 2) {
691 const double accel = - v0 * v0 / (2 * dist);
693 return v0 + accel * t;
696 const double accel = 2 * (dist /
TS - v0) /
TS;
698 return v0 + accel * t;
710 (
double)sqrt(
MAX2(0., 2 * dist * accel + v * v)));
768 const double g = gap - NUMERICAL_EPS;
780 const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
781 const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
782 assert(h <= g + NUMERICAL_EPS);
785 const double r = (g - h) / (n * s + t);
786 const double x = n * b + r;
796 const double g =
MAX2(0., gap - NUMERICAL_EPS);
812 const double btau = decel * headway;
813 const double v0 = -btau + sqrt(btau * btau + 2 * decel * g);
822 const double tau = headway == 0 ?
TS : headway;
823 const double v0 =
MAX2(0., currentSpeed);
825 if (v0 * tau >= 2 * g) {
837 const double a = -v0 * v0 / (2 * g);
852 const double btau2 = decel * tau / 2;
853 const double v1 = -btau2 + sqrt(btau2 * btau2 + decel * (2 * g - tau * v0));
854 const double a = (v1 - v0) / tau;
893 if (origSafeDecel >
myDecel + NUMERICAL_EPS) {
899 #ifdef DEBUG_EMERGENCYDECEL
901 std::cout <<
SIMTIME <<
" initial vsafe=" << x
902 <<
" egoSpeed=" << egoSpeed <<
" (origSafeDecel=" << origSafeDecel <<
")"
903 <<
" predSpeed=" << predSpeed <<
" (predDecel=" << predMaxDecel <<
")"
912 safeDecel =
MIN2(safeDecel, origSafeDecel);
918 #ifdef DEBUG_EMERGENCYDECEL
920 std::cout <<
" -> corrected emergency deceleration: " << safeDecel <<
" newVSafe=" << x << std::endl;
939 const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
941 const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
943 #ifdef DEBUG_EMERGENCYDECEL
945 std::cout <<
SIMTIME <<
" calculateEmergencyDeceleration()"
946 <<
" gap=" << gap <<
" egoSpeed=" << egoSpeed <<
" predSpeed=" << predSpeed
947 <<
" predBrakeDist=" << predBrakeDist
953 if (b1 <= predMaxDecel) {
955 #ifdef DEBUG_EMERGENCYDECEL
957 std::cout <<
" case 1 ..." << std::endl;
962 #ifdef DEBUG_EMERGENCYDECEL
964 std::cout <<
" case 2 ...";
969 assert(gap < 0 || predSpeed < egoSpeed);
974 const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
976 #ifdef DEBUG_EMERGENCYDECEL
978 std::cout <<
" b2=" << b2 << std::endl;
995 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap, pred);
996 const double perceivedSpeedDifference = veh->
getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
998 #ifdef DEBUG_DRIVER_ERRORS
1002 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
1003 <<
" speed=" << speed <<
" gap=" << gap <<
" leaderSpeed=" << predSpeed
1004 <<
"\n perceivedGap=" << perceivedGap <<
" perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
1005 <<
" perceivedSpeedDifference=" << perceivedSpeedDifference
1007 const double exactFollowSpeed =
followSpeed(veh, speed, gap, predSpeed, predMaxDecel);
1008 const double errorFollowSpeed =
followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel);
1009 const double accelError =
SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
1010 std::cout <<
" gapError=" << perceivedGap - gap <<
" dvError=" << perceivedSpeedDifference - (predSpeed - speed)
1011 <<
"\n resulting accelError: " << accelError << std::endl;
1018 predSpeed = speed + perceivedSpeedDifference;
1033 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap);
1035 #ifdef DEBUG_DRIVER_ERRORS
1039 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
1040 <<
" speed=" << speed <<
" gap=" << gap <<
"\n perceivedGap=" << perceivedGap << std::endl;
1041 const double exactStopSpeed =
stopSpeed(veh, speed, gap);
1042 const double errorStopSpeed =
stopSpeed(veh, speed, perceivedGap);
1043 const double accelError =
SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1044 std::cout <<
" gapError=" << perceivedGap - gap <<
"\n resulting accelError: " << accelError << std::endl;
#define EMERGENCY_DECEL_AMPLIFIER
@ 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.
double stopSpeed(const MSVehicle *const veh, const double speed, double gap) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
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.
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 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 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...
virtual double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
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 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.
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.
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const =0
Computes the vehicle's follow speed (no dawdling)
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].
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....
double myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
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.
bool hasDriverState() const
Whether this vehicle is equipped with a MSDriverState.
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.
double getSpeed() const
Returns the vehicle's current speed.
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
const MSLane * getLane() const
Returns the lane the vehicle is on.
The car-following model and parameter.
double getMaxSpeed() const
Get vehicle's maximum speed [m/s].
const std::string & getID() const
Returns the id.
Structure representing possible vehicle parameter.