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 int a = (int)ceil(duration /
TS -
TS);
308 if (bg <= leaderMinDist) {
312 if (
gDebugFlag2) std::cout <<
" followSpeedTransient"
313 <<
" duration=" << duration
314 <<
" gap=" << gap2pred
315 <<
" leaderMinDist=" << leaderMinDist
320 <<
" x=" << (b + leaderMinDist) / duration
322 return (b + leaderMinDist) / duration;
327 while (bg < leaderMinDist) {
336 const double fullBrakingSeconds = sqrt(leaderMinDist * 2 /
myDecel);
337 if (fullBrakingSeconds >= duration) {
341 return leaderMinDist / duration + duration *
getMaxDecel() / 2;
343 return fullBrakingSeconds *
myDecel;
351 return (speed + 0.5 * accel * t) * t;
353 const double decel = -accel;
354 if (speed <= decel * t) {
368 const double speed2 = speed - t * decel;
369 return 0.5 * (speed + speed2) * t;
376 const double accelTime = (arrivalSpeed - currentSpeed) / accel;
377 const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
378 const double nonAccelWay =
MAX2(0., dist - accelWay);
382 return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
391 if (dist < NUMERICAL_EPS) {
395 if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
400 if (fabs(accel) < NUMERICAL_EPS) {
404 double p = speed / accel;
408 return (-p - sqrt(p * p + 2 * dist / accel));
413 double t1 = (maxSpeed - speed) / accel;
415 double d1 = speed * t1 + 0.5 * accel * t1 * t1;
418 return (-p + sqrt(p * p + 2 * dist / accel));
420 return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
434 assert(accel == decel);
436 assert(initialSpeed == 0);
437 assert(arrivalSpeed == 0);
438 assert(maxSpeed > 0);
441 double accelTime = (maxSpeed - initialSpeed) / accel;
443 double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
445 if (accelDist >= dist * 0.5) {
447 arrivalTime = 4 * sqrt(dist / accel);
450 const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
451 arrivalTime = accelTime + constSpeedTime;
459 assert(time > 0 || dist == 0);
462 }
else if (time * speed > 2 * dist) {
465 return - 0.5 * speed * speed / dist;
469 return 2 * (dist / time - speed) / time;
483 double arrivalSpeedBraking;
486 if (dist < currentSpeed) {
494 return arrivalSpeedBraking;
501 MSCFModel::gapExtrapolation(
const double duration,
const double currentGap,
double v1,
double v2,
double a1,
double a2,
const double maxV1,
const double maxV2) {
503 double newGap = currentGap;
506 for (
unsigned int steps = 1; steps *
TS <= duration; ++steps) {
507 v1 =
MIN2(
MAX2(v1 + a1, 0.), maxV1);
508 v2 =
MIN2(
MAX2(v2 + a2, 0.), maxV2);
509 newGap +=
TS * (v1 - v2);
514 double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
517 if (a1 < 0 && v1 > 0) {
518 const double leaderStopTime = - v1 / a1;
519 t1 =
MIN2(leaderStopTime, duration);
520 }
else if (a1 >= 0) {
524 if (a2 < 0 && v2 > 0) {
525 const double followerStopTime = -v2 / a2;
526 t2 =
MIN2(followerStopTime, duration);
527 }
else if (a2 >= 0) {
531 if (a1 > 0 && v1 < maxV1) {
532 const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
533 t3 =
MIN2(leaderMaxSpeedTime, duration);
534 }
else if (a1 <= 0) {
538 if (a2 > 0 && v2 < maxV2) {
539 const double followerMaxSpeedTime = (maxV2 - v2) / a2;
540 t4 =
MIN2(followerMaxSpeedTime, duration);
541 }
else if (a2 <= 0) {
553 std::list<double>::const_iterator i;
555 for (i = l.begin(); i != l.end(); ++i) {
557 double dt =
MIN2(*i, duration) - tLast;
560 newGap += dv * dt + da * dt * dt / 2.;
564 if (*i == t1 || *i == t3) {
569 if (*i == t2 || *i == t4) {
574 tLast =
MIN2(*i, duration);
575 if (tLast == duration) {
580 if (duration != tLast) {
582 assert(a1 == 0. && a2 == 0.);
583 double dt = duration - tLast;
595 MSCFModel::passingTime(
const double lastPos,
const double passedPos,
const double currentPos,
const double lastSpeed,
const double currentSpeed) {
597 assert(passedPos <= currentPos);
598 assert(passedPos >= lastPos);
599 assert(currentPos > lastPos);
600 assert(currentSpeed >= 0);
602 if (passedPos > currentPos || passedPos < lastPos) {
603 std::stringstream ss;
607 ss <<
"passingTime(): given argument passedPos = " << passedPos <<
" doesn't lie within [lastPos, currentPos] = [" << lastPos <<
", " << currentPos <<
"]\nExtrapolating...";
608 std::cout << ss.str() <<
"\n";
611 const double lastCoveredDist = currentPos - lastPos;
612 const double extrapolated = passedPos > currentPos ?
TS * (passedPos - lastPos) / lastCoveredDist :
TS * (currentPos - passedPos) / lastCoveredDist;
614 }
else if (currentSpeed < 0) {
615 WRITE_ERROR(
"passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
619 const double distanceOldToPassed = passedPos - lastPos;
623 if (currentSpeed == 0) {
626 const double t = distanceOldToPassed / currentSpeed;
634 if (currentSpeed > 0) {
639 assert(currentSpeed == 0 && lastSpeed != 0);
643 a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
650 if (fabs(a) < NUMERICAL_EPS) {
652 const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
656 const double va = lastSpeed / a;
657 const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
658 assert(t < 1 && t >= 0);
662 const double va = lastSpeed / a;
663 const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
664 assert(t < 1 && t >= 0);
674 assert(t >= 0 && t <=
TS);
682 if (dist <
TS * v0 / 2) {
685 const double accel = - v0 * v0 / (2 * dist);
687 return v0 + accel * t;
690 const double accel = 2 * (dist /
TS - v0) /
TS;
692 return v0 + accel * t;
704 (
double)sqrt(
MAX2(0., 2 * dist * accel + v * v)));
761 gap -= NUMERICAL_EPS;
765 const double g = gap;
775 const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
776 const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
777 assert(h <= g + NUMERICAL_EPS);
780 const double r = (g - h) / (n * s + t);
781 const double x = n * b + r;
790 g =
MAX2(0., g - NUMERICAL_EPS);
806 const double btau =
myDecel * headway;
807 const double v0 = -btau + sqrt(btau * btau + 2 *
myDecel * g);
816 const double tau = headway == 0 ?
TS : headway;
817 const double v0 =
MAX2(0., v);
819 if (v0 * tau >= 2 * g) {
831 const double a = -v0 * v0 / (2 * g);
846 const double btau2 =
myDecel * tau / 2;
847 const double v1 = -btau2 + sqrt(btau2 * btau2 +
myDecel * (2 * g - tau * v0));
848 const double a = (v1 - v0) / tau;
887 if (origSafeDecel >
myDecel + NUMERICAL_EPS) {
893 #ifdef DEBUG_EMERGENCYDECEL
895 std::cout <<
SIMTIME <<
" initial vsafe=" << x
896 <<
" egoSpeed=" << egoSpeed <<
" (origSafeDecel=" << origSafeDecel <<
")"
897 <<
" predSpeed=" << predSpeed <<
" (predDecel=" << predMaxDecel <<
")"
906 safeDecel =
MIN2(safeDecel, origSafeDecel);
912 #ifdef DEBUG_EMERGENCYDECEL
914 std::cout <<
" -> corrected emergency deceleration: " << safeDecel <<
" newVSafe=" << x << std::endl;
933 const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
935 const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
937 #ifdef DEBUG_EMERGENCYDECEL
939 std::cout <<
SIMTIME <<
" calculateEmergencyDeceleration()"
940 <<
" gap=" << gap <<
" egoSpeed=" << egoSpeed <<
" predSpeed=" << predSpeed
941 <<
" predBrakeDist=" << predBrakeDist
947 if (b1 <= predMaxDecel) {
949 #ifdef DEBUG_EMERGENCYDECEL
951 std::cout <<
" case 1 ..." << std::endl;
956 #ifdef DEBUG_EMERGENCYDECEL
958 std::cout <<
" case 2 ...";
963 assert(gap < 0 || predSpeed < egoSpeed);
968 const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
970 #ifdef DEBUG_EMERGENCYDECEL
972 std::cout <<
" b2=" << b2 << std::endl;
989 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap, pred);
990 const double perceivedSpeedDifference = veh->
getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
992 #ifdef DEBUG_DRIVER_ERRORS
996 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
997 <<
" speed=" << speed <<
" gap=" << gap <<
" leaderSpeed=" << predSpeed
998 <<
"\n perceivedGap=" << perceivedGap <<
" perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
999 <<
" perceivedSpeedDifference=" << perceivedSpeedDifference
1001 const double exactFollowSpeed =
followSpeed(veh, speed, gap, predSpeed, predMaxDecel);
1002 const double errorFollowSpeed =
followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel);
1003 const double accelError =
SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
1004 std::cout <<
" gapError=" << perceivedGap - gap <<
" dvError=" << perceivedSpeedDifference - (predSpeed - speed)
1005 <<
"\n resulting accelError: " << accelError << std::endl;
1012 predSpeed = speed + perceivedSpeedDifference;
1027 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap);
1029 #ifdef DEBUG_DRIVER_ERRORS
1033 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
1034 <<
" speed=" << speed <<
" gap=" << gap <<
"\n perceivedGap=" << perceivedGap << std::endl;
1035 const double exactStopSpeed =
stopSpeed(veh, speed, gap);
1036 const double errorStopSpeed =
stopSpeed(veh, speed, perceivedGap);
1037 const double accelError =
SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1038 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.
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 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.
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...
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 myEmergencyDecel
The vehicle's maximum emergency deceleration [m/s^2].
double maximumSafeStopSpeedEuler(double gap, double headway=-1) const
Returns the maximum next velocity for stopping within gap when using the semi-implicit Euler update.
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.
double maximumSafeStopSpeed(double gap, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
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.
static double distAfterTime(double t, double speed, double accel)
calculates the distance travelled after accelerating for time t
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.
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
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 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)
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.