47 #define EST_REAC_THRESHOLD 3.
54 myTwoSqrtAccelDecel(double(2 * sqrt(myAccel * myDecel))),
112 double x = (-b + sqrt(b * b - 4.*a * c)) / (2.*a);
116 if (origSafeDecel >
myDecel + NUMERICAL_EPS) {
122 #ifdef DEBUG_EMERGENCYDECEL
124 std::cout <<
SIMTIME <<
" initial vsafe=" << x
125 <<
" egoSpeed=" << egoSpeed <<
" (origSafeDecel=" << origSafeDecel <<
")"
126 <<
" predSpeed=" << predSpeed <<
" (predDecel=" << predMaxDecel <<
")"
135 safeDecel =
MIN2(safeDecel, origSafeDecel);
141 #ifdef DEBUG_EMERGENCYDECEL
143 std::cout <<
" -> corrected emergency deceleration: " << safeDecel << std::endl;
173 vsafe = (-b + sqrt(b * b - 4.*a * c)) / (2.*a);
193 double drivingerror = 0.0;
196 if (s / vars->
mys_est >= 0.5) {
198 }
else if (s / vars->
mys_est < 0.1) {
200 }
else if (s / vars->
mys_est < 0.15) {
202 }
else if (s / vars->
mys_est < 0.2) {
204 }
else if (s / vars->
mys_est < 0.25) {
206 }
else if (s / vars->
mys_est < 0.3) {
208 }
else if (s / vars->
mys_est < 0.5) {
224 const double oldV = veh->
getSpeed();
230 for (
auto it = vars->
stop.cbegin(); it != vars->
stop.cend(); ++it) {
231 if (vPos > oldV +
ACCEL2SPEED(it->first) - NUMERICAL_EPS && vPos < oldV +
ACCEL2SPEED(it->first) + NUMERICAL_EPS) {
232 vPos =
_v(veh, it->second, oldV, 0, vars->
v0_int,
false, 1);
245 vMax =
MAX2(vMin, vMax);
265 }
else if (vNext <= oldV && vNext < vMax - NUMERICAL_EPS && oldV >
EST_REAC_THRESHOLD) {
290 vNext =
MAX2(vNext, vMin);
356 double result =
_v(veh, gap2pred, speed, predSpeed, vars->
v0_int,
true,
update);
369 double result =
_v(veh, gap, speed, 0, vars->
v0_int,
false, 0);
379 MSCFModel_EIDM::freeSpeed(
const double currentSpeed,
const double decel,
const double dist,
const double targetSpeed,
const bool onInsertion) {
395 const double y =
MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
396 const double yFull = floor(y);
397 const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
398 const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) *
ACCEL2SPEED(decel);
399 return DIST2SPEED(
MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
414 assert(currentSpeed >= 0);
415 assert(targetSpeed >= 0);
417 const double dt = onInsertion ? 0 :
TS;
418 const double v0 = currentSpeed;
419 const double vT = targetSpeed;
420 const double b = decel;
421 const double d = dist - NUMERICAL_EPS;
430 if (0.5 * (v0 + vT)*dt >= d) {
434 const double q = ((dt * v0 - 2 * d) * b - vT * vT);
435 const double p = 0.5 * b * dt;
436 return -p + sqrt(p * p - q);
462 double vSafe, remaining_time, targetDecel;
475 targetDecel = (speed - maxSpeed) / remaining_time;
508 vSafe =
_v(veh, 500., speed, maxSpeed, vars->
v0_int,
true,
update);
522 const double vNext = veh->
getSpeed() + acc;
523 const double gap = (vNext - vL) * (veh->
getSpeed() + vL) / (2 *
myDecel) + vL;
535 const double delta_v = speed - leaderSpeed;
551 const double delta_v = speed - leaderSpeed;
557 if (speed <= vars->v0_int) {
558 erg = sqrt((s * s) / (
MAX2(targetDecel /
myAccel + 1.0, 1.0)));
572 double v_limcurr, v_limprev;
579 std::vector<MSLink*>::const_iterator link =
MSLane::succLinkSec(*veh, view, *lane, bestLaneConts);
591 double speedlim = 200;
631 if ((*link)->getViaLane() ==
nullptr) {
634 lane = (*link)->getViaLaneOrLane();
645 if (!(v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) ||
646 (v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS && !(v_limprev < v_limcurr + NUMERICAL_EPS && v_limprev > v_limcurr - NUMERICAL_EPS))) {
648 if ((v_limcurr < v_limprev && v_limcurr < v0 && v_limprev > v0) ||
649 (v_limcurr > v_limprev && v_limcurr > v0 && v_limprev < v0)) {
652 if (v_limcurr >= v0 - NUMERICAL_EPS) {
660 if (v_limcurr < v0 + NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS) {
668 }
else if (!(v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) ||
669 (v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS && !(v_limprev < v_limcurr + NUMERICAL_EPS && v_limprev > v_limcurr - NUMERICAL_EPS))) {
671 if ((v_limcurr < v_limprev && v_limcurr < v0 && v_limprev > v0) ||
672 (v_limcurr > v_limprev && v_limcurr > v0 && v_limprev < v0)) {
675 if (v_limcurr >= v0 - NUMERICAL_EPS) {
683 if (v_limcurr < v0 + NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS) {
694 const double predSpeed,
const double desSpeed,
const bool respectMinGap,
const int update)
const {
696 double v0 = desSpeed;
708 double wantedacc = 0., a_free;
709 double wouldacc = 0., woulds, woulda_free;
711 double estSpeed, estleaderSpeed, estGap;
712 double current_estSpeed, current_estGap, current_estleaderSpeed;
715 double a_leader = 0.01;
716 double newSpeed = egoSpeed;
729 if (current_gap < myType->getMinGap() - 0.04) {
734 if (current_gap <= NUMERICAL_EPS) {
738 double newGap = current_gap;
743 current_estSpeed = newSpeed;
746 current_estGap = newGap;
747 }
else if (!respectMinGap && newGap < myType->getMinGap()) {
754 estSpeed = current_estSpeed;
755 estleaderSpeed = current_estleaderSpeed;
756 estGap = current_estGap;
770 estSpeed = current_estSpeed;
771 estleaderSpeed = current_estleaderSpeed;
772 estGap = current_estGap;
777 double korrektor = 0.5;
805 if (estGap < myType->getMinGap() && s < estGap) {
828 if (estSpeed <= v0) {
831 wantedacc = a_corr *
myAccel * (1. - (s * s) / (estGap * estGap));
833 wantedacc = a_free * (1. - pow(s / estGap, 2 * a_corr *
myAccel / a_free));
838 wantedacc = a_free + a_corr *
myAccel * (1. - (s * s) / (estGap * estGap));
847 woulds =
MAX2(0., current_estSpeed * headwayTime + current_estSpeed * (current_estSpeed - current_estleaderSpeed) /
myTwoSqrtAccelDecel);
850 if (current_estSpeed <= v0) {
851 woulda_free = a_corr *
myAccel * (1. - pow(current_estSpeed / v0,
myDelta));
852 if (woulds >= current_estGap) {
853 wouldacc = a_corr *
myAccel * (1. - (woulds * woulds) / (current_estGap * current_estGap));
855 wouldacc = woulda_free * (1. - pow(woulds / current_estGap, 2 * a_corr *
myAccel / woulda_free));
859 if (woulds >= current_estGap) {
860 wouldacc = woulda_free + a_corr *
myAccel * (1. - (woulds * woulds) / (current_estGap * current_estGap));
862 wouldacc = woulda_free;
878 if (vars->
minaccel > wantedacc - NUMERICAL_EPS && estSpeed < 2 &&
896 double calc_gap = estGap;
909 if (estSpeed <= v0) {
912 acc = a_corr *
myAccel * (1. - (s * s) / (calc_gap * calc_gap));
914 acc = a_free * (1. - pow(s / calc_gap, 2 * a_corr *
myAccel / a_free));
919 acc = a_free + a_corr *
myAccel * (1. - (s * s) / (calc_gap * calc_gap));
931 if (leader !=
nullptr && respectMinGap && estleaderSpeed >= 0.01) {
934 if (estGap > s * 3 / 2) {
935 a_leader = a_leader * (s * 3 / 2) / estGap;
940 if (estleaderSpeed < 0.01) {
941 estleaderSpeed = 0.01;
947 if (estleaderSpeed * (estSpeed - estleaderSpeed) <= -2 * estGap * a_leader) {
948 a_cah = (estSpeed * estSpeed * a_leader) / (estleaderSpeed * estleaderSpeed - 2 * estGap * a_leader);
950 if (estSpeed - estleaderSpeed >= 0) {
951 a_cah = a_leader - ((estSpeed - estleaderSpeed) * (estSpeed - estleaderSpeed)) / (2 * estGap);
1009 vars->
stop.push_back(std::make_pair(acc, gap2pred));
1012 return MAX2(0., newSpeed);
#define EMERGENCY_DECEL_AMPLIFIER
#define EST_REAC_THRESHOLD
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)....
@ PARTLEFT
The link is a partial left direction.
@ RIGHT
The link is a (hard) right direction.
@ TURN
The link is a 180 degree turn.
@ LEFT
The link is a (hard) left direction.
@ STRAIGHT
The link is a straight direction.
@ TURN_LEFTHAND
The link is a 180 degree turn (left-hand network)
@ PARTRIGHT
The link is a partial right direction.
@ NODIR
The link has no direction (is a dead end link)
@ SUMO_ATTR_CF_EIDM_T_ACC_MAX
@ SUMO_ATTR_CF_EIDM_EPSILON_ACC
@ SUMO_ATTR_CF_EIDM_T_LOOK_AHEAD
@ SUMO_ATTR_CF_EIDM_USEVEHDYNAMICS
@ SUMO_ATTR_CF_EIDM_C_COOLNESS
@ SUMO_ATTR_CF_EIDM_SIG_ERROR
@ SUMO_ATTR_CF_EIDM_T_REACTION
@ SUMO_ATTR_CF_EIDM_T_PERSISTENCE_ESTIMATE
@ SUMO_ATTR_CF_EIDM_SIG_GAP
@ SUMO_ATTR_CF_EIDM_JERK_MAX
@ SUMO_ATTR_CF_IDM_STEPPING
@ SUMO_ATTR_COLLISION_MINGAP_FACTOR
@ SUMO_ATTR_CF_EIDM_M_FLATNESS
@ SUMO_ATTR_CF_EIDM_M_BEGIN
@ SUMO_ATTR_CF_EIDM_T_PERSISTENCE_DRIVE
@ SUMO_ATTR_CF_EIDM_SIG_LEADER
bool gDebugFlag1
global utility flags for debugging
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 bool isSelected() const
whether this vehicle is selected in the GUI
double getChosenSpeedFactor() const
Returns the precomputed factor by which the driver wants to be faster than the speed limit.
std::vector< std::pair< double, double > > stop
double _v(const MSVehicle *const veh, const double gap2pred, const double mySpeed, const double predSpeed, const double desSpeed, const bool respectMinGap, const int update) const
const double myTPersEstimate
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
const double myTPersDrive
~MSCFModel_EIDM()
Destructor.
double stopSpeed(const MSVehicle *const veh, const double speed, double gap, double decel) const
Computes the vehicle's safe speed for approaching a non-moving obstacle.
const double mySigmaleader
const double myEpsilonacc
double maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
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....
double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences.
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.
const double myTwoSqrtAccelDecel
MSCFModel_EIDM(const MSVehicleType *vtype)
Constructor.
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...
double internalsecuregap(const MSVehicle *const veh, const double speed, const double leaderSpeed, const double targetDecel) const
double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle's safe speed.
void internalspeedlimit(MSVehicle *const veh, const double oldV) const
double getSecureGap(const MSVehicle *const veh, const MSVehicle *const, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
MSCFModel * duplicate(const MSVehicleType *vtype) const
Duplicates the car-following model.
double interactionGap(const MSVehicle *const, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
const double mySigmaerror
double patchSpeedBeforeLCEIDM(const MSVehicle *veh, double vMin, double vMax, VehicleVariables *vars) const
Applies dawdling / driving error.
The car-following model abstraction.
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
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 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...
double myEmergencyDecel
The vehicle's maximum emergency deceleration [m/s^2].
double myCollisionMinGapFactor
The factor of minGap that must be maintained to avoid a collision event.
double calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const
Returns the minimal deceleration for following the given leader safely.
double myDecel
The vehicle's maximum deceleration [m/s^2].
double myAccel
The vehicle's maximum acceleration [m/s^2].
const MSVehicleType * myType
The type to which this model definition belongs to.
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 myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
static bool gSemiImplicitEulerUpdate
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
Representation of a lane in the micro simulation.
static std::vector< MSLink * >::const_iterator succLinkSec(const SUMOVehicle &veh, int nRouteSuccs, const MSLane &succLinkSource, const std::vector< MSLane * > &conts)
double getLength() const
Returns the lane's length.
bool isLinkEnd(std::vector< MSLink * >::const_iterator &i) const
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.
MSAbstractLaneChangeModel & getLaneChangeModel()
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
std::pair< const MSVehicle *const, double > getLeader(double dist=0) const
Returns the leader of the vehicle looking for a fixed distance.
MSCFModel::VehicleVariables * getCarFollowVariables() const
Returns the vehicle's car following model variables.
double getSpeed() const
Returns the vehicle's current speed.
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
double getPositionOnLane() const
Get the vehicle's position along the lane.
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].
double getMinGap() const
Get the free space in front of vehicles of this class.
const SUMOVTypeParameter & getParameter() const
static double randNorm(double mean, double variance, SumoRNG *rng=nullptr)
Access to a random number from a normal distribution.
double getCFParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.