46 #define DEBUG_COND (veh->isSelected()) 49 #define DEBUG_COND2 (gDebugFlag1) 64 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;
100 MSCFModel::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;
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();
187 #ifdef DEBUG_FINALIZE_SPEED 189 std::cout <<
"\n" <<
SIMTIME <<
" FINALIZE_SPEED\n";
193 vMax =
MAX2(vMin, vMax);
196 #ifdef DEBUG_FINALIZE_SPEED 197 double vDawdle = vNext;
199 assert(vNext >= vMin);
200 assert(vNext <= vMax);
203 assert(vNext >= vMin);
204 assert(vNext <= vMax);
206 #ifdef DEBUG_FINALIZE_SPEED 209 <<
"veh '" << veh->
getID() <<
"' oldV=" << oldV
213 <<
" vStop=" << vStop
214 <<
" vDawdle=" << vDawdle
215 <<
" vNext=" << vNext
229 const double gap = (vNext - vL) *
302 double leaderMinDist = gap2pred +
distAfterTime(duration, predSpeed, -predMaxDecel);
307 int a = (int)ceil(duration /
TS -
TS);
310 if (bg <= leaderMinDist) {
314 if (
gDebugFlag2) std::cout <<
" followSpeedTransient" 315 <<
" duration=" << duration
316 <<
" gap=" << gap2pred
317 <<
" leaderMinDist=" << leaderMinDist
322 <<
" x=" << (b + leaderMinDist) / duration
324 return (b + leaderMinDist) / duration;
329 while (bg < leaderMinDist) {
338 const double fullBrakingSeconds = sqrt(leaderMinDist * 2 /
myDecel);
339 if (fullBrakingSeconds >= duration) {
343 return leaderMinDist / duration + duration *
getMaxDecel() / 2;
345 return fullBrakingSeconds *
myDecel;
353 return (speed + 0.5 * accel * t) * t;
355 const double decel = -accel;
356 if (speed <= decel * t) {
370 const double speed2 = speed - t * decel;
371 return 0.5 * (speed + speed2) * t;
378 const double accelTime = (arrivalSpeed - currentSpeed) / accel;
379 const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
380 const double nonAccelWay =
MAX2(0., dist - accelWay);
384 return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
397 if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
406 double p = speed / accel;
410 return (-p - sqrt(p * p + 2 * dist / accel));
415 double t1 = (maxSpeed - speed) / accel;
417 double d1 = speed * t1 + 0.5 * accel * t1 * t1;
420 return (-p + sqrt(p * p + 2 * dist / accel));
422 return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
436 assert(accel == decel);
438 assert(initialSpeed == 0);
439 assert(arrivalSpeed == 0);
440 assert(maxSpeed > 0);
443 double accelTime = (maxSpeed - initialSpeed) / accel;
445 double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
447 if (accelDist >= dist * 0.5) {
449 arrivalTime = 4 * sqrt(accelDist) / accel;
452 const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
453 arrivalTime = accelTime + constSpeedTime;
461 assert(time > 0 || dist == 0);
464 }
else if (time * speed > 2 * dist) {
467 return - 0.5 * speed * speed / dist;
471 return 2 * (dist / time - speed) / time;
485 double arrivalSpeedBraking;
488 if (dist < currentSpeed) {
496 return arrivalSpeedBraking;
503 MSCFModel::gapExtrapolation(
const double duration,
const double currentGap,
double v1,
double v2,
double a1,
double a2,
const double maxV1,
const double maxV2) {
505 double newGap = currentGap;
508 for (
unsigned int steps = 1; steps *
TS <= duration; ++steps) {
509 v1 =
MIN2(
MAX2(v1 + a1, 0.), maxV1);
510 v2 =
MIN2(
MAX2(v2 + a2, 0.), maxV2);
511 newGap += TS * (v1 - v2);
516 double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
519 if (a1 < 0 && v1 > 0) {
520 const double leaderStopTime = - v1 / a1;
521 t1 =
MIN2(leaderStopTime, duration);
522 }
else if (a1 >= 0) {
526 if (a2 < 0 && v2 > 0) {
527 const double followerStopTime = -v2 / a2;
528 t2 =
MIN2(followerStopTime, duration);
529 }
else if (a2 >= 0) {
533 if (a1 > 0 && v1 < maxV1) {
534 const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
535 t3 =
MIN2(leaderMaxSpeedTime, duration);
536 }
else if (a1 <= 0) {
540 if (a2 > 0 && v2 < maxV2) {
541 const double followerMaxSpeedTime = (maxV2 - v2) / a2;
542 t4 =
MIN2(followerMaxSpeedTime, duration);
543 }
else if (a2 <= 0) {
555 std::list<double>::const_iterator i;
557 for (i = l.begin(); i != l.end(); ++i) {
559 double dt =
MIN2(*i, duration) - tLast;
562 newGap += dv * dt + da * dt * dt / 2.;
566 if (*i == t1 || *i == t3) {
571 if (*i == t2 || *i == t4) {
576 tLast =
MIN2(*i, duration);
577 if (tLast == duration) {
582 if (duration != tLast) {
584 assert(a1 == 0. && a2 == 0.);
585 double dt = duration - tLast;
597 MSCFModel::passingTime(
const double lastPos,
const double passedPos,
const double currentPos,
const double lastSpeed,
const double currentSpeed) {
599 assert(passedPos <= currentPos);
600 assert(passedPos >= lastPos);
601 assert(currentPos > lastPos);
602 assert(currentSpeed >= 0);
604 if (passedPos > currentPos || passedPos < lastPos) {
605 std::stringstream ss;
609 ss <<
"passingTime(): given argument passedPos = " << passedPos <<
" doesn't lie within [lastPos, currentPos] = [" << lastPos <<
", " << currentPos <<
"]\nExtrapolating...";
610 std::cout << ss.str() <<
"\n";
613 const double lastCoveredDist = currentPos - lastPos;
614 const double extrapolated = passedPos > currentPos ?
TS * (passedPos - lastPos) / lastCoveredDist :
TS * (currentPos - passedPos) / lastCoveredDist;
616 }
else if (currentSpeed < 0) {
617 WRITE_ERROR(
"passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
621 const double distanceOldToPassed = passedPos - lastPos;
625 if (currentSpeed == 0) {
628 const double t = distanceOldToPassed / currentSpeed;
636 if (currentSpeed > 0) {
641 assert(currentSpeed == 0 && lastSpeed != 0);
645 a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
654 const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
658 const double va = lastSpeed / a;
659 const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
660 assert(t < 1 && t >= 0);
664 const double va = lastSpeed / a;
665 const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
666 assert(t < 1 && t >= 0);
676 assert(t >= 0 && t <=
TS);
684 if (dist <
TS * v0 / 2) {
687 const double accel = - v0 * v0 / (2 * dist);
689 return v0 + accel * t;
692 const double accel = 2 * (dist /
TS - v0) /
TS;
694 return v0 + accel * t;
706 (double)sqrt(
MAX2(0., 2 * dist * accel + v * v)));
767 const double g = gap;
777 const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
778 const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
782 const double r = (g - h) / (n * s + t);
783 const double x = n * b + r;
808 const double btau =
myDecel * headway;
809 const double v0 = -btau + sqrt(btau * btau + 2 *
myDecel * g);
818 const double tau = headway == 0 ?
TS : headway;
819 const double v0 =
MAX2(0., v);
821 if (v0 * tau >= 2 * g) {
833 const double a = -v0 * v0 / (2 * g);
848 const double btau2 =
myDecel * tau / 2;
849 const double v1 = -btau2 + sqrt(btau2 * btau2 +
myDecel * (2 * g - tau * v0));
850 const double a = (v1 - v0) / tau;
895 #ifdef DEBUG_EMERGENCYDECEL 897 std::cout <<
SIMTIME <<
" initial vsafe=" << x
898 <<
" egoSpeed=" << egoSpeed <<
" (origSafeDecel=" << origSafeDecel <<
")" 899 <<
" predSpeed=" << predSpeed <<
" (predDecel=" << predMaxDecel <<
")" 908 safeDecel =
MIN2(safeDecel, origSafeDecel);
914 #ifdef DEBUG_EMERGENCYDECEL 916 std::cout <<
" -> corrected emergency deceleration: " << safeDecel <<
" newVSafe=" << x << std::endl;
935 const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
937 const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
939 #ifdef DEBUG_EMERGENCYDECEL 941 std::cout <<
SIMTIME <<
" calculateEmergencyDeceleration()" 942 <<
" gap=" << gap <<
" egoSpeed=" << egoSpeed <<
" predSpeed=" << predSpeed
943 <<
" predBrakeDist=" << predBrakeDist
949 if (b1 <= predMaxDecel) {
951 #ifdef DEBUG_EMERGENCYDECEL 953 std::cout <<
" case 1 ..." << std::endl;
958 #ifdef DEBUG_EMERGENCYDECEL 960 std::cout <<
" case 2 ...";
965 assert(gap < 0 || predSpeed < egoSpeed);
970 const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
972 #ifdef DEBUG_EMERGENCYDECEL 974 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;
1022 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap);
1024 #ifdef DEBUG_DRIVER_ERRORS 1028 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n" 1029 <<
" speed=" << speed <<
" gap=" << gap <<
"\n perceivedGap=" << perceivedGap << std::endl;
1030 const double exactStopSpeed =
stopSpeed(veh, speed, gap);
1031 const double errorStopSpeed =
stopSpeed(veh, speed, perceivedGap);
1032 const double accelError =
SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1033 std::cout <<
" gapError=" << perceivedGap - gap <<
"\n resulting accelError: " << accelError << std::endl;
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 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()
#define EMERGENCY_DECEL_AMPLIFIER
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...
Structure representing possible vehicle parameter.
int gPrecision
the precision for floating point outputs
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
MSCFModel(const MSVehicleType *vtype)
Constructor.
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
static double avoidArrivalAccel(double dist, double time, double speed, double maxDecel)
Computes the acceleration needed to arrive not before the given time.
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...
#define UNUSED_PARAMETER(x)
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences. Called at most once per simulation...
The car-following model and parameter.
MSAbstractLaneChangeModel & getLaneChangeModel()
std::shared_ptr< MSSimpleDriverState > getDriverState() const
Returns the vehicle driver's state.
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].
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...
bool hasDriverState() const
Whether this vehicle is equipped with a MSDriverState.
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...
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 ...
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 calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const
Returns the minimal deceleration for following the given leader safely.
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].
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...
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 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.
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
virtual double patchSpeedBeforeLC(const MSVehicle *veh, double vMin, double vMax) const
apply custom speed adaptations within the given speed bounds
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.
double maximumSafeStopSpeedEuler(double gap, double headway=-1) const
Returns the maximum next velocity for stopping within gap when using the semi-implicit Euler update...
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)
const std::string & getID() const
Returns the name of the vehicle.
void applyHeadwayPerceptionError(const MSVehicle *const veh, double speed, double &gap) const
Overwrites gap by the perceived value obtained from the vehicle's driver state.
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
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) ...