double
MSCFModel::minNextSpeedEmergency(double speed, const MSVehicle* const /*veh*/) const {
if (MSGlobals::gSemiImplicitEulerUpdate) {
return MAX2(speed - ACCEL2SPEED(myEmergencyDecel), 0.);
} else {
// NOTE: ballistic update allows for negative speeds to indicate a stop within the next timestep
return speed - ACCEL2SPEED(myEmergencyDecel);
}
}
double
MSCFModel_Kerner::_v(const MSVehicle* const veh, double speed, double vfree, double gap, double predSpeed) const {
if (predSpeed == 0 && gap < 0.01) {
return 0;
}
// !!! in the following, the prior step is not considered!!!
double G = MAX2((double) 0, (double)(SPEED2DIST(myK * speed) + myPhi / myAccel * speed * (speed - predSpeed)));
double vcond = gap > G ? speed + ACCEL2SPEED(myAccel) : speed + MAX2(ACCEL2SPEED(-myDecel), MIN2(ACCEL2SPEED(myAccel), predSpeed - speed));
double vsafe = (double)(-1. * myTauDecel + sqrt(myTauDecel * myTauDecel + (predSpeed * predSpeed) + (2. * myDecel * gap)));
VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
double va = MAX2((double) 0, MIN3(vfree, vsafe, vcond)) + vars->rand;
double v = MAX2((double) 0, MIN4(vfree, va, speed + ACCEL2SPEED(myAccel), vsafe));
return v;
}
SUMOReal
MSCFModel_Krauss::dawdle(SUMOReal speed) const {
// generate random number out of [0,1]
SUMOReal random = RandHelper::rand();
// Dawdle.
if (speed < myAccel) {
// we should not prevent vehicles from driving just due to dawdling
// if someone is starting, he should definitely start
// (but what about slow-to-start?)!!!
speed -= ACCEL2SPEED(myDawdle * speed * random);
} else {
speed -= ACCEL2SPEED(myDawdle * myAccel * random);
}
return MAX2(SUMOReal(0), speed);
}
double
MSCFModel::maximumSafeStopSpeedEuler(double gap, double headway) const {
gap -= NUMERICAL_EPS; // lots of code relies on some slack XXX: it shouldn't...
if (gap <= 0) {
return 0;
}
const double g = gap;
const double b = ACCEL2SPEED(myDecel);
const double t = headway >= 0 ? headway : myHeadwayTime;
const double s = TS;
// h = the distance that would be covered if it were possible to stop
// exactly after gap and decelerate with b every simulation step
// h = 0.5 * n * (n-1) * b * s + n * b * t (solve for n)
//n = ((1.0/2.0) - ((t + (pow(((s*s) + (4.0*((s*((2.0*h/b) - t)) + (t*t)))), (1.0/2.0))*sign/2.0))/s));
const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
assert(h <= g + NUMERICAL_EPS);
// compute the additional speed that must be used during deceleration to fix
// the discrepancy between g and h
const double r = (g - h) / (n * s + t);
const double x = n * b + r;
assert(x >= 0);
return x;
}
double
MSCFModel::brakeGapEuler(const double speed, const double decel, const double headwayTime) {
/* one possibility to speed this up is to calculate speedReduction * steps * (steps+1) / 2
for small values of steps (up to 10 maybe) and store them in an array */
const double speedReduction = ACCEL2SPEED(decel);
const int steps = int(speed / speedReduction);
return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
}
SUMOReal
MSCFModel_KraussPS::maxNextSpeed(SUMOReal speed, const MSVehicle* const veh) const {
const SUMOReal gravity = 9.80665;
const SUMOReal aMax = MAX2(0., getMaxAccel() - gravity * sin(DEG2RAD(veh->getSlope())));
// assuming drag force is proportional to the square of speed
const SUMOReal vMax = sqrt(aMax / getMaxAccel()) * myType->getMaxSpeed();
return MIN2(speed + (SUMOReal) ACCEL2SPEED(aMax), vMax);
}
double
MSCFModel::finalizeSpeed(MSVehicle* const veh, double vPos) const {
// save old v for optional acceleration computation
const double oldV = veh->getSpeed();
// process stops (includes update of stopping state)
const double vStop = MIN2(vPos, veh->processNextStop(vPos));
// apply deceleration bounds
const double vMinEmergency = minNextSpeedEmergency(oldV, veh);
// vPos contains the uppper bound on safe speed. allow emergency braking here
const double vMin = MIN2(minNextSpeed(oldV, veh), MAX2(vPos, vMinEmergency));
// aMax: Maximal admissible acceleration until the next action step, such that the vehicle's maximal
// desired speed on the current lane will not be exceeded when the
// acceleration is maintained until the next action step.
double aMax = (veh->getLane()->getVehicleMaxSpeed(veh) - oldV) / veh->getActionStepLengthSecs();
// apply planned speed constraints and acceleration constraints
double vMax = MIN3(oldV + ACCEL2SPEED(aMax), maxNextSpeed(oldV, veh), vStop);
// do not exceed max decel even if it is unsafe
#ifdef _DEBUG
//if (vMin > vMax) {
// WRITE_WARNING("Maximum speed of vehicle '" + veh->getID() + "' is lower than the minimum speed (min: " + toString(vMin) + ", max: " + toString(vMax) + ").");
//}
#endif
#ifdef DEBUG_FINALIZE_SPEED
if DEBUG_COND {
std::cout << "\n" << SIMTIME << " FINALIZE_SPEED\n";
}
#endif
vMax = MAX2(vMin, vMax);
// apply further speed adaptations
double vNext = patchSpeedBeforeLC(veh, vMin, vMax);
#ifdef DEBUG_FINALIZE_SPEED
double vDawdle = vNext;
#endif
assert(vNext >= vMin);
assert(vNext <= vMax);
// apply lane-changing related speed adaptations
vNext = veh->getLaneChangeModel().patchSpeed(vMin, vNext, vMax, *this);
assert(vNext >= vMin);
assert(vNext <= vMax);
#ifdef DEBUG_FINALIZE_SPEED
if DEBUG_COND {
std::cout << std::setprecision(gPrecision)
<< "veh '" << veh->getID() << "' oldV=" << oldV
<< " vPos" << vPos
<< " vMin=" << vMin
<< " vMax=" << vMax
<< " vStop=" << vStop
<< " vDawdle=" << vDawdle
<< " vNext=" << vNext
<< "\n";
}
#endif
return vNext;
}
SUMOReal
MSCFModel_Wiedemann::krauss_vsafe(SUMOReal gap, SUMOReal predSpeed) const {
if (predSpeed == 0 && gap < 0.01) {
return 0;
}
const SUMOReal tauDecel = myDecel * myHeadwayTime;
const SUMOReal speedReduction = ACCEL2SPEED(myDecel);
const int predSteps = int(predSpeed / speedReduction);
const SUMOReal leaderContrib = 2. * myDecel * (gap + SPEED2DIST(predSteps * predSpeed - speedReduction * predSteps * (predSteps + 1) / 2));
return (SUMOReal)(-tauDecel + sqrt(tauDecel * tauDecel + leaderContrib));
}
double
MSCFModel::calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const {
// There are two cases:
// 1) Either, stopping in time is possible with a deceleration b <= predMaxDecel, then this value is returned
// 2) Or, b > predMaxDecel is required in this case the minimal value b allowing to stop safely under the assumption maxPredDecel=b is returned
// Apparent braking distance for the leader
const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
// Required deceleration according to case 1)
const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
#ifdef DEBUG_EMERGENCYDECEL
if (DEBUG_COND2) {
std::cout << SIMTIME << " calculateEmergencyDeceleration()"
<< " gap=" << gap << " egoSpeed=" << egoSpeed << " predSpeed=" << predSpeed
<< " predBrakeDist=" << predBrakeDist
<< " b1=" << b1
<< std::endl;
}
#endif
if (b1 <= predMaxDecel) {
// Case 1) applies
#ifdef DEBUG_EMERGENCYDECEL
if (DEBUG_COND2) {
std::cout << " case 1 ..." << std::endl;
}
#endif
return b1;
}
#ifdef DEBUG_EMERGENCYDECEL
if (DEBUG_COND2) {
std::cout << " case 2 ...";
}
#endif
// Case 2) applies
assert(gap < 0 || predSpeed < egoSpeed);
if (gap <= 0.) {
return -ACCEL2SPEED(myEmergencyDecel);
}
// Required deceleration according to case 2)
const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
#ifdef DEBUG_EMERGENCYDECEL
if (DEBUG_COND2) {
std::cout << " b2=" << b2 << std::endl;
}
#endif
return b2;
}
double
MSCFModel::followSpeedTransient(double duration, const MSVehicle* const /*veh*/, double /*speed*/, double gap2pred, double predSpeed, double predMaxDecel) const {
// minimium distance covered by the leader if braking
double leaderMinDist = gap2pred + distAfterTime(duration, predSpeed, -predMaxDecel);
// if ego would not brake it could drive with speed leaderMinDist / duration
// due to potentential ego braking it can safely drive faster
if (MSGlobals::gSemiImplicitEulerUpdate) {
// number of potential braking steps
int a = (int)ceil(duration / TS - TS);
// can we brake for the whole time?
const double bg = brakeGap(a * myDecel, myDecel, 0);
if (bg <= leaderMinDist) {
// braking continuously for duration
// distance reduction due to braking
double b = TS * getMaxDecel() * 0.5 * (a * a - a);
if (gDebugFlag2) std::cout << " followSpeedTransient"
<< " duration=" << duration
<< " gap=" << gap2pred
<< " leaderMinDist=" << leaderMinDist
<< " decel=" << getMaxDecel()
<< " a=" << a
<< " bg=" << bg
<< " b=" << b
<< " x=" << (b + leaderMinDist) / duration
<< "\n";
return (b + leaderMinDist) / duration;
} else {
// @todo improve efficiency
double bg = 0;
double speed = 0;
while (bg < leaderMinDist) {
speed += ACCEL2SPEED(myDecel);
bg += SPEED2DIST(speed);
}
speed -= DIST2SPEED(bg - leaderMinDist);
return speed;
}
} else {
// can we brake for the whole time?
const double fullBrakingSeconds = sqrt(leaderMinDist * 2 / myDecel);
if (fullBrakingSeconds >= duration) {
// braking continuously for duration
// average speed after braking for duration is x2 = x - 0.5 * duration * myDecel
// x2 * duration <= leaderMinDist must hold
return leaderMinDist / duration + duration * getMaxDecel() / 2;
} else {
return fullBrakingSeconds * myDecel;
}
}
}
// uses the safe speed and preferred acceleration with the same NORMAL tau to compute stopSpeed
SUMOReal
MSCFModel_PWag2009::stopSpeed(const MSVehicle* const /* veh */, const SUMOReal speed, SUMOReal gap) const {
if (gap < 0.01) {
return 0;
}
const SUMOReal vsafe = -myTauDecel + sqrt(myTauDecel * myTauDecel + 2.0 * myDecel * gap);
const SUMOReal asafe = SPEED2ACCEL(vsafe - speed);
// VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
SUMOReal apref = myDecelDivTau * (gap - 2 * speed * myHeadwayTime) / (speed + myTauDecel);
if (apref <= asafe) {
apref = MIN2(apref, myAccel);
apref = MAX2(apref, -myDecel);
} else {
apref = asafe;
}
return MAX2((SUMOReal)0, vsafe + ACCEL2SPEED(apref));
}
SUMOReal
MSCFModel_KraussOrig1::moveHelper(MSVehicle* const veh, SUMOReal vPos) const {
const SUMOReal oldV = veh->getSpeed(); // save old v for optional acceleration computation
const SUMOReal vSafe = MIN2(vPos, veh->processNextStop(vPos)); // process stops
// we need the acceleration for emission computation;
// in this case, we neglect dawdling, nonetheless, using
// vSafe does not incorporate speed reduction due to interaction
// on lane changing
veh->setPreDawdleAcceleration(SPEED2ACCEL(vSafe - oldV));
const SUMOReal vMin = MAX2((SUMOReal) 0, oldV - ACCEL2SPEED(myDecel));
const SUMOReal vMax = MIN3(veh->getLane()->getMaxSpeed(), maxNextSpeed(oldV), vSafe);
#ifdef _DEBUG
if (vMin > vMax) {
WRITE_WARNING("Vehicle's '" + veh->getID() + "' maximum speed is lower than the minimum speed (min: " + toString(vMin) + ", max: " + toString(vMax) + ").");
}
#endif
return veh->getLaneChangeModel().patchSpeed(vMin, MAX2(vMin, dawdle(vMax)), vMax, *this);
}
SUMOReal
MSCFModel_KraussOrig1::moveHelper(MSVehicle * const veh, const MSLane * const lane, SUMOReal vPos) const throw() {
SUMOReal oldV = veh->getSpeed(); // save old v for optional acceleration computation
SUMOReal vSafe = MIN2(vPos, veh->processNextStop(vPos)); // process stops
// we need the acceleration for emission computation;
// in this case, we neglect dawdling, nonetheless, using
// vSafe does not incorporate speed reduction due to interaction
// on lane changing
veh->setPreDawdleAcceleration(SPEED2ACCEL(vSafe-oldV));
//
SUMOReal vNext = dawdle(MIN3(lane->getMaxSpeed(), maxNextSpeed(oldV), vSafe));
vNext =
veh->getLaneChangeModel().patchSpeed(
MAX2((SUMOReal) 0, oldV-(SUMOReal)ACCEL2SPEED(myDecel)), //!!! reverify
vNext,
MIN3(vSafe, veh->getLane().getMaxSpeed(), maxNextSpeed(oldV)),//vaccel(myState.mySpeed, myLane->maxSpeed())),
vSafe);
return MIN4(vNext, vSafe, veh->getLane().getMaxSpeed(), maxNextSpeed(oldV));
}
SUMOReal
MSCFModel_PWag2009::followSpeed(const MSVehicle* const veh, SUMOReal speed, SUMOReal gap, SUMOReal predSpeed, SUMOReal /*predMaxDecel*/) const {
if (predSpeed == 0 && gap < 0.01) {
return 0;
}
const SUMOReal vsafe = -myTauLastDecel + sqrt(myTauLastDecel * myTauLastDecel + predSpeed * predSpeed + 2.0 * myDecel * gap);
const SUMOReal asafe = SPEED2ACCEL(vsafe - speed);
VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
SUMOReal apref = vars->aOld;
if (apref <= asafe && RandHelper::rand() <= myActionPointProbability * TS) {
apref = myDecelDivTau * (gap + (predSpeed - speed) * myHeadwayTime - speed * myHeadwayTime) / (speed + myTauDecel);
apref = MIN2(apref, myAccel);
apref = MAX2(apref, -myDecel);
apref += myDawdle * RandHelper::rand((SUMOReal) - 1., (SUMOReal)1.);
}
if (apref > asafe) {
apref = asafe;
}
return MAX2((SUMOReal)0, speed + ACCEL2SPEED(apref));
}
int
MSLCM_LC2013::slowDownForBlocked(MSVehicle** blocked, int state) {
// if this vehicle is blocking someone in front, we maybe decelerate to let him in
if ((*blocked) != 0) {
SUMOReal gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
if (gap > POSITION_EPS) {
if (myVehicle.getSpeed() < ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
state |= LCA_AMBACKBLOCKER_STANDING;
} else {
state |= LCA_AMBACKBLOCKER;
}
myVSafes.push_back(myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(),
(SUMOReal)(gap - POSITION_EPS), (*blocked)->getSpeed(),
(*blocked)->getCarFollowModel().getMaxDecel()));
}
}
}
return state;
}
SUMOReal
MSCFModel_Wiedemann::_v(const MSVehicle* veh, SUMOReal predSpeed, SUMOReal gap) const {
const VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
const SUMOReal dx = gap + myType->getLength(); // wiedemann uses brutto gap
const SUMOReal v = veh->getSpeed();
const SUMOReal vpref = veh->getMaxSpeed();
const SUMOReal dv = v - predSpeed;
const SUMOReal bx = myAX + (1 + 7 * mySecurity) * sqrt(v); // Harding propose a factor of *.8 here
const SUMOReal ex = 2 - myEstimation; // + RandHelper::randNorm(0.5, 0.15)
const SUMOReal sdx = myAX + ex * (bx - myAX); /// the distance at which we drift out of following
const SUMOReal sdv_root = (dx - myAX) / myCX;
const SUMOReal sdv = sdv_root * sdv_root;
const SUMOReal cldv = sdv * ex * ex;
const SUMOReal opdv = cldv * (-1 - 2 * RandHelper::randNorm(0.5, 0.15));
// select the regime, get new acceleration, compute new speed based
SUMOReal accel;
if (dx <= bx) {
accel = emergency(dv, dx);
} else if (dx < sdx) {
if (dv > cldv) {
accel = approaching(dv, dx, bx);
} else if (dv > opdv) {
accel = following(vars->accelSign);
} else {
accel = fullspeed(v, vpref, dx, bx);
}
} else {
if (dv > sdv && dx < D_MAX) { //@note other versions have an disjunction instead of conjunction
accel = approaching(dv, dx, bx);
} else {
accel = fullspeed(v, vpref, dx, bx);
}
}
// since we have hard constrainst on accel we may as well use them here
accel = MAX2(MIN2(accel, myAccel), -myDecel);
const SUMOReal vNew = MAX2(SUMOReal(0), v + ACCEL2SPEED(accel)); // don't allow negative speeds
return vNew;
}
double
MSCFModel::distAfterTime(double t, double speed, const double accel) {
if (accel >= 0.) {
return (speed + 0.5 * accel * t) * t;
}
const double decel = -accel;
if (speed <= decel * t) {
// braking to a full stop
return brakeGap(speed, decel, 0);
}
if (MSGlobals::gSemiImplicitEulerUpdate) {
// @todo improve efficiency
double result = 0;
while (t > 0) {
speed -= ACCEL2SPEED(decel);
result += MAX2(0.0, SPEED2DIST(speed));
t -= TS;
}
return result;
} else {
const double speed2 = speed - t * decel;
return 0.5 * (speed + speed2) * t;
}
}
SUMOReal
MSCFModel_Daniel1::dawdle(SUMOReal speed) const {
return MAX2(SUMOReal(0), speed - ACCEL2SPEED(myDawdle * myAccel * RandHelper::rand()));
}
double
HelpersEnergy::compute(const SUMOEmissionClass /* c */, const PollutantsInterface::EmissionType e, const double v, const double a, const double slope, const std::map<int, double>* param) const {
if (e != PollutantsInterface::ELEC) {
return 0.;
}
if (param == 0) {
param = &myDefaultParameter;
}
//@ToDo: All formulas below work with the logic of the euler update (refs #860).
// Approximation order could be improved. Refs. #2592.
const double lastV = v - ACCEL2SPEED(a);
const double mass = param->find(SUMO_ATTR_VEHICLEMASS)->second;
// calculate potential energy difference
double energyDiff = mass * 9.81 * sin(DEG2RAD(slope)) * SPEED2DIST(v);
// kinetic energy difference of vehicle
energyDiff += 0.5 * mass * (v * v - lastV * lastV);
// add rotational energy diff of internal rotating elements
energyDiff += param->find(SUMO_ATTR_INTERNALMOMENTOFINERTIA)->second * (v * v - lastV * lastV);
// Energy loss through Air resistance [Ws]
// Calculate energy losses:
// EnergyLoss,Air = 1/2 * rho_air [kg/m^3] * myFrontSurfaceArea [m^2] * myAirDragCoefficient [-] * v_Veh^2 [m/s] * s [m]
// ... with rho_air [kg/m^3] = 1,2041 kg/m^3 (at T = 20C)
// ... with s [m] = v_Veh [m/s] * TS [s]
energyDiff += 0.5 * 1.2041 * param->find(SUMO_ATTR_FRONTSURFACEAREA)->second * param->find(SUMO_ATTR_AIRDRAGCOEFFICIENT)->second * v * v * SPEED2DIST(v);
// Energy loss through Roll resistance [Ws]
// ... (fabs(veh.getSpeed())>=0.01) = 0, if vehicle isn't moving
// EnergyLoss,Tire = c_R [-] * F_N [N] * s [m]
// ... with c_R = ~0.012 (car tire on asphalt)
// ... with F_N [N] = myMass [kg] * g [m/s^2]
energyDiff += param->find(SUMO_ATTR_ROLLDRAGCOEFFICIENT)->second * 9.81 * mass * SPEED2DIST(v);
// Energy loss through friction by radial force [Ws]
// If angle of vehicle was changed
const double angleDiff = param->find(SUMO_ATTR_ANGLE)->second;
if (angleDiff != 0.) {
// Compute new radio
double radius = SPEED2DIST(v) / fabs(angleDiff);
// Check if radius is in the interval [0.0001 - 10000] (To avoid overflow and division by zero)
if (radius < 0.0001) {
radius = 0.0001;
} else if (radius > 10000) {
radius = 10000;
}
// EnergyLoss,internalFrictionRadialForce = c [m] * F_rad [N];
// Energy loss through friction by radial force [Ws]
energyDiff += param->find(SUMO_ATTR_RADIALDRAGCOEFFICIENT)->second * mass * v * v / radius;
}
// EnergyLoss,constantConsumers
// Energy loss through constant loads (e.g. A/C) [Ws]
energyDiff += param->find(SUMO_ATTR_CONSTANTPOWERINTAKE)->second;
//E_Bat = E_kin_pot + EnergyLoss;
if (energyDiff > 0) {
// Assumption: Efficiency of myPropulsionEfficiency when accelerating
energyDiff /= param->find(SUMO_ATTR_PROPULSIONEFFICIENCY)->second;
} else {
// Assumption: Efficiency of myRecuperationEfficiency when recuperating
energyDiff *= param->find(SUMO_ATTR_RECUPERATIONEFFICIENCY)->second;
}
// convert from [Ws] to [Wh] (3600s / 1h):
return energyDiff / 3600.;
}
void
MSLCM_LC2013::informFollower(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
int dir,
const std::pair<MSVehicle*, SUMOReal>& neighFollow,
SUMOReal remainingSeconds,
SUMOReal plannedSpeed) {
if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0) {
assert(neighFollow.first != 0);
MSVehicle* nv = neighFollow.first;
// are we fast enough to cut in without any help?
if (plannedSpeed - nv->getSpeed() >= HELP_OVERTAKE) {
const SUMOReal neededGap = nv->getCarFollowModel().getSecureGap(nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->getSpeed())) {
// follower might even accelerate but not to much
msgPass.informNeighFollower(new Info(plannedSpeed - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
return;
}
}
// decide whether we will request help to cut in before the follower or allow to be overtaken
// PARAMETERS
// assume other vehicle will assume the equivalent of 1 second of
// maximum deceleration to help us (will probably be spread over
// multiple seconds)
// -----------
const SUMOReal helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR ;
// change in the gap between ego and blocker over 1 second (not STEP!)
const SUMOReal neighNewSpeed = MAX2((SUMOReal)0, nv->getSpeed() - ACCEL2SPEED(helpDecel));
const SUMOReal neighNewSpeed1s = MAX2((SUMOReal)0, nv->getSpeed() - helpDecel);
const SUMOReal dv = plannedSpeed - neighNewSpeed1s;
// new gap between follower and self in case the follower does brake for 1s
const SUMOReal decelGap = neighFollow.second + dv;
const SUMOReal secureGap = nv->getCarFollowModel().getSecureGap(neighNewSpeed1s, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
if (decelGap > 0 && decelGap >= secureGap) {
// if the blocking neighbor brakes it could actually help
// how hard does it actually need to be?
const SUMOReal vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
nv, nv->getSpeed(), neighFollow.second, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
} else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
// decelerating once is sufficient to open up a large enough gap in time
msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
} else {
SUMOReal vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
//if (dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE &&
// nv->getSpeed() > myVehicle.getSpeed()) {
if (nv->getSpeed() > myVehicle.getSpeed() &&
((dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE)
|| (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
)) {
// let the follower slow down to increase the likelyhood that later vehicles will be slow enough to help
// follower should still be fast enough to open a gap
vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
return;
}
}
msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
// this follower is supposed to overtake us. slow down smoothly to allow this
const SUMOReal overtakeDist = (neighFollow.second // follower reaches ego back
+ myVehicle.getVehicleType().getLengthWithGap() // follower reaches ego front
+ nv->getVehicleType().getLength() // follower back at ego front
+ myVehicle.getCarFollowModel().getSecureGap( // follower has safe dist to ego
plannedSpeed, vhelp, nv->getCarFollowModel().getMaxDecel()));
// speed difference to create a sufficiently large gap
const SUMOReal needDV = overtakeDist / remainingSeconds;
// make sure the deceleration is not to strong
myVSafes.push_back(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())));
}
}
}
SUMOReal
MSLCM_LC2013::informLeader(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
int dir,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
SUMOReal remainingSeconds) {
SUMOReal plannedSpeed = MIN2(myVehicle.getSpeed(),
myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), myLeftSpace - myLeadingBlockerLength));
for (std::vector<SUMOReal>::const_iterator i = myVSafes.begin(); i != myVSafes.end(); ++i) {
SUMOReal v = (*i);
if (v >= myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
plannedSpeed = MIN2(plannedSpeed, v);
}
}
if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
assert(neighLead.first != 0);
MSVehicle* nv = neighLead.first;
// decide whether we want to overtake the leader or follow it
const SUMOReal dv = plannedSpeed - nv->getSpeed();
const SUMOReal overtakeDist = (neighLead.second // drive to back of follower
+ nv->getVehicleType().getLengthWithGap() // drive to front of follower
+ myVehicle.getVehicleType().getLength() // ego back reaches follower front
+ nv->getCarFollowModel().getSecureGap( // save gap to follower
nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()));
if (dv < 0
// overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
|| (dir == LCA_MLEFT && !myVehicle.congested())
// not enough space to overtake?
|| myLeftSpace < overtakeDist
// not enough time to overtake?
|| dv * remainingSeconds < overtakeDist) {
// cannot overtake
msgPass.informNeighLeader(new Info(-1, dir | LCA_AMBLOCKINGLEADER), &myVehicle);
// slow down smoothly to follow leader
const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
if (targetSpeed < myVehicle.getSpeed()) {
// slow down smoothly to follow leader
const SUMOReal decel = ACCEL2SPEED(MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds)));
const SUMOReal nextSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() - decel);
myVSafes.push_back(nextSpeed);
return nextSpeed;
} else {
// leader is fast enough anyway
myVSafes.push_back(targetSpeed);
return plannedSpeed;
}
} else {
// overtaking, leader should not accelerate
msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
return -1;
}
} else if (neighLead.first != 0) { // (remainUnblocked)
// we are not blocked now. make sure we stay far enough from the leader
MSVehicle* nv = neighLead.first;
const SUMOReal nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
const SUMOReal dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
myVSafes.push_back(targetSpeed);
return MIN2(targetSpeed, plannedSpeed);
} else {
// not overtaking
return plannedSpeed;
}
}
int
MSLaneChanger::checkChange(
int laneOffset,
const MSLane* targetLane,
const std::pair<MSVehicle* const, SUMOReal>& leader,
const std::pair<MSVehicle* const, SUMOReal>& neighLead,
const std::pair<MSVehicle* const, SUMOReal>& neighFollow,
const std::vector<MSVehicle::LaneQ>& preb) const {
MSVehicle* vehicle = veh(myCandi);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout
<< "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'"
<< std::endl;
}
#endif
int blocked = 0;
int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER);
int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER);
// overlap
if (neighFollow.first != 0 && neighFollow.second < 0) {
blocked |= (blockedByFollower | LCA_OVERLAPPING);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " overlapping with follower..."
<< std::endl;
}
#endif
}
if (neighLead.first != 0 && neighLead.second < 0) {
blocked |= (blockedByLeader | LCA_OVERLAPPING);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " overlapping with leader..."
<< std::endl;
}
#endif
}
// safe back gap
if ((blocked & blockedByFollower) == 0 && neighFollow.first != 0) {
// !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
if (neighFollow.second < neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first->getSpeed(), vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel())) {
blocked |= blockedByFollower;
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " back gap unsafe: "
<< "gap = " << neighFollow.second
<< ", secureGap = "
<< neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first->getSpeed(),
vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel())
<< std::endl;
}
#endif
}
}
// safe front gap
if ((blocked & blockedByLeader) == 0 && neighLead.first != 0) {
// !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
if (neighLead.second < vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())) {
blocked |= blockedByLeader;
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " front gap unsafe: "
<< "gap = " << neighLead.second
<< ", secureGap = "
<< vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(),
neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())
<< std::endl;
}
#endif
}
}
MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first);
int state = blocked | vehicle->getLaneChangeModel().wantsChange(
laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked));
if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != 0) {
// do are more carefull (but expensive) check to ensure that a
// safety-critical leader is not being overloocked
const SUMOReal seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
const SUMOReal speed = vehicle->getSpeed();
const SUMOReal dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
if (seen < dist) {
std::pair<MSVehicle* const, SUMOReal> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
if (neighLead2.first != 0 && neighLead2.first != neighLead.first
&& (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
state |= blockedByLeader;
}
}
}
if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
// ensure that merging is safe for any upcoming zipper links after changing
if (vehicle->unsafeLinkAhead(targetLane)) {
state |= blockedByLeader;
}
}
if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) {
// ensure that a continuous lane change manoeuvre can be completed
// before the next turning movement
SUMOReal seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
const SUMOReal decel = vehicle->getCarFollowModel().getMaxDecel() * STEPS2TIME(MSGlobals::gLaneChangeDuration);
const SUMOReal avgSpeed = 0.5 * (
MAX2((SUMOReal)0, vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
MAX2((SUMOReal)0, vehicle->getSpeed() - decel));
const SUMOReal space2change = avgSpeed * STEPS2TIME(MSGlobals::gLaneChangeDuration);
// for finding turns it doesn't matter whether we look along the current lane or the target lane
const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
int view = 1;
MSLane* nextLane = vehicle->getLane();
MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
while (!nextLane->isLinkEnd(link) && seen <= space2change) {
if ((*link)->getDirection() == LINKDIR_LEFT || (*link)->getDirection() == LINKDIR_RIGHT
// the lanes after an internal junction are on different
// edges and do not allow lane-changing
|| (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
) {
state |= LCA_INSUFFICIENT_SPACE;
break;
}
#ifdef HAVE_INTERNAL_LANES
if ((*link)->getViaLane() == 0) {
view++;
}
#else
view++;
#endif
nextLane = (*link)->getViaLaneOrLane();
seen += nextLane->getLength();
// get the next link used
link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
}
if (nextLane->isLinkEnd(link) && seen < space2change) {
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n";
}
#endif
state |= LCA_INSUFFICIENT_SPACE;
}
if ((state & LCA_BLOCKED) == 0) {
// check for dangerous leaders in case the target lane changes laterally between
// now and the lane-changing midpoint
const SUMOReal speed = vehicle->getSpeed();
seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
nextLane = vehicle->getLane();
view = 1;
const SUMOReal dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
while (!nextLane->isLinkEnd(link) && seen <= space2change && seen <= dist) {
nextLane = (*link)->getViaLaneOrLane();
MSLane* targetLane = nextLane->getParallelLane(laneOffset);
if (targetLane == 0) {
state |= LCA_INSUFFICIENT_SPACE;
break;
} else {
std::pair<MSVehicle* const, SUMOReal> neighLead2 = targetLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
if (neighLead2.first != 0 && neighLead2.first != neighLead.first
&& (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
state |= blockedByLeader;
break;
}
}
#ifdef HAVE_INTERNAL_LANES
if ((*link)->getViaLane() == 0) {
view++;
}
#else
view++;
#endif
seen += nextLane->getLength();
// get the next link used
link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
}
}
}
#ifndef NO_TRACI
#ifdef DEBUG_CHECK_CHANGE
const int oldstate = state;
#endif
// let TraCI influence the wish to change lanes and the security to take
state = vehicle->influenceChangeDecision(state);
#endif
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << vehicle->getID()
<< " oldState=" << toString((LaneChangeAction)oldstate)
<< " newState=" << toString((LaneChangeAction)state)
<< ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
<< ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
<< "\n";
}
#endif
return state;
}
int
MSLCM_DK2004::wantsChangeToRight(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
const std::pair<MSVehicle*, SUMOReal>& leader,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
const std::pair<MSVehicle*, SUMOReal>& neighFollow,
const MSLane& neighLane,
const std::vector<MSVehicle::LaneQ>& preb,
MSVehicle** lastBlocked) {
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(&myVehicle)->getGlID())) {
int bla = 0;
}
#endif
MSVehicle::LaneQ curr, best;
int bestLaneOffset = 0;
SUMOReal currentDist = 0;
SUMOReal neighDist = 0;
SUMOReal neighExtDist = 0;
SUMOReal currExtDist = 0;
int currIdx = 0;
for (int p = 0; p < (int) preb.size(); ++p) {
if (preb[p].lane == myVehicle.getLane()) {
curr = preb[p];
bestLaneOffset = curr.bestLaneOffset;
currentDist = curr.length;
currExtDist = curr.lane->getLength();
neighDist = preb[p - 1].length;
neighExtDist = preb[p - 1].lane->getLength();
best = preb[p + bestLaneOffset];
currIdx = p;
}
}
// keep information about being a leader/follower
int ret = (myOwnState & 0x00ffff00);
if (leader.first != 0
&&
(myOwnState & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0
&&
(leader.first->getLaneChangeModel().getOwnState()&LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
myOwnState &= (0xffffffff - LCA_AMBLOCKINGFOLLOWER_DONTBRAKE);
if (myVehicle.getSpeed() > 0.1) {
myOwnState |= LCA_AMBACKBLOCKER;
} else {
ret |= LCA_AMBACKBLOCKER;
myDontBrake = true;
}
}
// process information about the last blocked vehicle
// if this vehicle is blocking someone in front, we maybe decelerate to let him in
if ((*lastBlocked) != 0) {
SUMOReal gap = (*lastBlocked)->getPositionOnLane() - (*lastBlocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
if (gap > 0.1) {
if (myVehicle.getSpeed() < ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
if ((*lastBlocked)->getSpeed() < 0.1) {
ret |= LCA_AMBACKBLOCKER_STANDING;
} else {
ret |= LCA_AMBACKBLOCKER;
}
myVSafes.push_back(myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), (SUMOReal)(gap - 0.1), (*lastBlocked)->getSpeed(), (*lastBlocked)->getCarFollowModel().getMaxDecel()));
(*lastBlocked) = 0;
}
return ret;
}
}
// we try to estimate the distance which is necessary to get on a lane
// we have to get on in order to keep our route
// we assume we need something that depends on our velocity
// and compare this with the free space on our wished lane
//
// if the free space is somehow less than the space we need, we should
// definitely try to get to the desired lane
//
// this rule forces our vehicle to change the lane if a lane changing is necessary soon
SUMOReal rv = myVehicle.getSpeed() > LOOK_FORWARD_SPEED_DIVIDER
? myVehicle.getSpeed() * (SUMOReal) LOOK_FORWARD_FAR
: myVehicle.getSpeed() * (SUMOReal) LOOK_FORWARD_NEAR;
rv += myVehicle.getVehicleType().getLengthWithGap() * (SUMOReal) 2.;
SUMOReal tdist = currentDist - myVehicle.getPositionOnLane() - best.occupation * (SUMOReal) JAM_FACTOR2;
if (fabs(best.length - curr.length) > MIN2((SUMOReal) .1, best.lane->getLength()) && bestLaneOffset < 0 && currentDistDisallows(tdist/*currentDist*/, bestLaneOffset, rv)) {
informBlocker(msgPass, blocked, LCA_MRIGHT, neighLead, neighFollow);
if (neighLead.second > 0 && neighLead.second > leader.second) {
myVSafes.push_back(myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()) - (SUMOReal) 0.5);
}
// letting vehicles merge in at the end of the lane in case of counter-lane change, step#1, right
// if there is a leader and he wants to change to left (we want to change to right)
if (neighLead.first != 0 && (neighLead.first->getLaneChangeModel().getOwnState()&LCA_LEFT) != 0) {
// save at least his length in myLeadingBlockerLength
myLeadingBlockerLength = MAX2(neighLead.first->getVehicleType().getLengthWithGap(), myLeadingBlockerLength);
// save the left space
myLeftSpace = currentDist - myVehicle.getPositionOnLane();
}
//
return ret | LCA_RIGHT | LCA_URGENT;
}
// the opposite lane-changing direction should be done than the one examined herein
// we'll check whether we assume we could change anyhow and get back in time...
//
// this rule prevents the vehicle from moving in opposite direction of the best lane
// unless the way till the end where the vehicle has to be on the best lane
// is long enough
SUMOReal maxJam = MAX2(preb[currIdx - 1].occupation, preb[currIdx].occupation);
SUMOReal neighLeftPlace = MAX2((SUMOReal) 0, neighDist - myVehicle.getPositionOnLane() - maxJam);
if (bestLaneOffset >= 0 && (currentDistDisallows(neighLeftPlace, bestLaneOffset + 2, rv))) {
// ...we will not change the lane if not
return ret;
}
// if the current lane is the best and a lane-changing would cause a situation
// of which we assume we will not be able to return to the lane we have to be on...
//
// this rule prevents the vehicle from leaving the current, best lane when it is
// close to this lane's end
if (currExtDist > neighExtDist && (neighLeftPlace * 2. < rv/*||currE[currIdx+1].length<currentDist*/)) {
return ret;
}
// let's also regard the case where the vehicle is driving on a highway...
// in this case, we do not want to get to the dead-end of an on-ramp
//
// THIS RULE APPLIES ONLY TO CHANGING TO THE RIGHT LANE
if (bestLaneOffset == 0 && preb[currIdx - 1].bestLaneOffset != 0 && myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle) > 80. / 3.6) {
return ret;
}
// --------
// -------- make place on current lane if blocking follower
if (amBlockingFollowerPlusNB()
&&
(currentDistAllows(neighDist, bestLaneOffset, rv) || neighDist >= currentDist)) {
return ret | LCA_RIGHT | LCA_URGENT;
}
// --------
// -------- security checks for krauss
// (vsafe fails when gap<0)
if ((blocked & LCA_BLOCKED) != 0) {
return ret;
}
// --------
// -------- higher speed
if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
return ret;
}
SUMOReal thisLaneVSafe = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
SUMOReal neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
if (neighLead.first == 0) {
neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighDist, 0, 0));
} else {
// @todo: what if leader is below safe gap?!!!
neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
}
if (leader.first == 0) {
thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), currentDist, 0, 0));
} else {
// @todo: what if leader is below safe gap?!!!
thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
}
thisLaneVSafe = MIN2(thisLaneVSafe, myVehicle.getVehicleType().getMaxSpeed());
neighLaneVSafe = MIN2(neighLaneVSafe, myVehicle.getVehicleType().getMaxSpeed());
if (thisLaneVSafe - neighLaneVSafe > 5. / 3.6) {
// ok, the current lane is faster than the right one...
if (myChangeProbability < 0) {
myChangeProbability /= 2.0;
}
} else {
// ok, the right lane is faster than the current
myChangeProbability -= (SUMOReal)((neighLaneVSafe - thisLaneVSafe) / (myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle)));
}
// let's recheck the "Rechtsfahrgebot"
SUMOReal vmax = MIN2(myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle), myVehicle.getVehicleType().getMaxSpeed());
vmax -= (SUMOReal)(5. / 2.6);
if (neighLaneVSafe >= vmax) {
#ifndef NO_TRACI
/* if there was a request by TraCI for changing to this lane
and holding it, this rule is ignored */
if (myChangeRequest != MSVehicle::REQUEST_HOLD) {
#endif
myChangeProbability -= (SUMOReal)((neighLaneVSafe - vmax) / (vmax));
#ifndef NO_TRACI
}
#endif
}
if (myChangeProbability < -2 && neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { //./MAX2((SUMOReal) .1, myVehicle.getSpeed())) { // -.1
return ret | LCA_RIGHT | LCA_SPEEDGAIN;
}
// --------
#ifndef NO_TRACI
// If there is a request by TraCI, try to change the lane
if (myChangeRequest == MSVehicle::REQUEST_RIGHT) {
return ret | LCA_RIGHT;
}
#endif
return ret;
}
/** Returns the SK-vsafe. */
double
MSCFModel::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
// the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
// the leaders starts braking hard until stopped
// unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
// (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
// To make things safe, we ensure that the leaders brake distance is computed with an deceleration that is at least as high as the follower's.
// @todo: this is a conservative estimate for safe speed which could be increased
// // For negative gaps, we return the lowest meaningful value by convention
// // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)
// // It must be done. Otherwise, negative gaps at high speeds can create nonsense results from the call to maximumSafeStopSpeed() below
// if(gap<0){
// if(MSGlobals::gSemiImplicitEulerUpdate){
// return 0.;
// } else {
// return -INVALID_SPEED;
// }
// }
// The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
// if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
// NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
// the follower accelerates for a short period of time. Refs #2310 (Leo)
// const double headway = predSpeed > 0. ? myHeadwayTime : 0.;
const double headway = myHeadwayTime;
double x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), egoSpeed, onInsertion, headway);
if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
if (origSafeDecel > myDecel + NUMERICAL_EPS) {
// Braking harder than myDecel was requested -> calculate required emergency deceleration.
// Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
// can result in corrupted values (leading to intersecting trajectories) if, e.g. leader and follower are fast (leader still faster) and the gap is very small,
// such that braking harder than myDecel is required.
#ifdef DEBUG_EMERGENCYDECEL
if (DEBUG_COND2) {
std::cout << SIMTIME << " initial vsafe=" << x
<< " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
<< " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
<< std::endl;
}
#endif
double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
// Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
safeDecel = MAX2(safeDecel, myDecel);
// don't brake harder than originally planned (possible due to euler/ballistic mismatch)
safeDecel = MIN2(safeDecel, origSafeDecel);
x = egoSpeed - ACCEL2SPEED(safeDecel);
if (MSGlobals::gSemiImplicitEulerUpdate) {
x = MAX2(x, 0.);
}
#ifdef DEBUG_EMERGENCYDECEL
if (DEBUG_COND2) {
std::cout << " -> corrected emergency deceleration: " << safeDecel << " newVSafe=" << x << std::endl;
}
#endif
}
}
assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
assert(!ISNAN(x));
return x;
}
double
MSCFModel::maxNextSpeed(double speed, const MSVehicle* const /*veh*/) const {
return MIN2(speed + (double) ACCEL2SPEED(getMaxAccel()), myType->getMaxSpeed());
}
double
MSCFModel::freeSpeed(const double currentSpeed, const double decel, const double dist, const double targetSpeed, const bool onInsertion, const double actionStepLength) {
// XXX: (Leo) This seems to be exclusively called with decel = myDecel (max deceleration) and is not overridden
// by any specific CFModel. That may cause undesirable hard braking (at junctions where the vehicle
// changes to a road with a lower speed limit).
if (MSGlobals::gSemiImplicitEulerUpdate) {
// adapt speed to succeeding lane, no reaction time is involved
// when breaking for y steps the following distance g is covered
// (drive with v in the final step)
// g = (y^2 + y) * 0.5 * b + y * v
// y = ((((sqrt((b + 2.0*v)*(b + 2.0*v) + 8.0*b*g)) - b)*0.5 - v)/b)
const double v = SPEED2DIST(targetSpeed);
if (dist < v) {
return targetSpeed;
}
const double b = ACCEL2DIST(decel);
const double y = MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
const double yFull = floor(y);
const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
return DIST2SPEED(MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
} else {
// ballistic update (Leo)
// calculate maximum next speed vN that is adjustable to vT=targetSpeed after a distance d=dist
// and given a maximal deceleration b=decel, denote the current speed by v0.
// the distance covered by a trajectory that attains vN in the next action step (length=dt) and decelerates afterwards
// with b is given as
// d = 0.5*dt*(v0+vN) + (t-dt)*vN - 0.5*b*(t-dt)^2, (1)
// where time t of arrival at d with speed vT is
// t = dt + (vN-vT)/b. (2)
// We insert (2) into (1) to obtain
// d = 0.5*dt*(v0+vN) + vN*(vN-vT)/b - 0.5*b*((vN-vT)/b)^2
// 0 = (dt*b*v0 - vT*vT - 2*b*d) + dt*b*vN + vN*vN
// and solve for vN
assert(currentSpeed >= 0);
assert(targetSpeed >= 0);
const double dt = onInsertion ? 0 : actionStepLength; // handles case that vehicle is inserted just now (at the end of move)
const double v0 = currentSpeed;
const double vT = targetSpeed;
const double b = decel;
const double d = dist - NUMERICAL_EPS; // prevent returning a value > targetSpeed due to rounding errors
// Solvability for positive vN (if d is small relative to v0):
// 1) If 0.5*(v0+vT)*dt > d, we set vN=vT.
// (In case vT<v0, this implies that on the interpolated trajectory there are points beyond d where
// the interpolated velocity is larger than vT, but at least on the temporal discretization grid, vT is not exceeded)
// 2) We ignore the (possible) constraint vN >= v0 - b*dt, which could lead to a problem if v0 - t*b > vT.
// (finalizeSpeed() is responsible for assuring that the next velocity is chosen in accordance with maximal decelerations)
// If implied accel a leads to v0 + a*asl < vT, choose acceleration s.th. v0 + a*asl = vT
if (0.5 * (v0 + vT)*dt >= d) {
// Attain vT after time asl
return v0 + TS * (vT - v0) / actionStepLength;
} else {
const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
const double p = 0.5 * b * dt;
const double vN = -p + sqrt(p * p - q); // target speed at time t0+asl
return v0 + TS * (vN - v0) / actionStepLength;
}
}
}
double
MSCFModel::maximumSafeStopSpeedBallistic(double g /*gap*/, double v /*currentSpeed*/, bool onInsertion, double headway) const {
// decrease gap slightly (to avoid passing end of lane by values of magnitude ~1e-12, when exact stop is required)
g = MAX2(0., g - NUMERICAL_EPS);
headway = headway >= 0 ? headway : myHeadwayTime;
// (Leo) Note that in contrast to the Euler update, for the ballistic update
// the distance covered in the coming step depends on the current velocity, in general.
// one exception is the situation when the vehicle is just being inserted.
// In that case, it will not cover any distance until the next timestep by convention.
// We treat the latter case first:
if (onInsertion) {
// The distance covered with constant insertion speed v0 until time tau is given as
// G1 = tau*v0
// The distance covered between time tau and the stopping moment at time tau+v0/b is
// G2 = v0^2/(2b),
// where b is an assumed constant deceleration (= myDecel)
// We solve g = G1 + G2 for v0:
const double btau = myDecel * headway;
const double v0 = -btau + sqrt(btau * btau + 2 * myDecel * g);
return v0;
}
// In the usual case during the driving task, the vehicle goes by
// a current speed v0=v, and we seek to determine a safe acceleration a (possibly <0)
// such that starting to break after accelerating with a for the time tau=headway
// still allows us to stop in time.
const double tau = headway == 0 ? TS : headway;
const double v0 = MAX2(0., v);
// We first consider the case that a stop has to take place within time tau
if (v0 * tau >= 2 * g) {
if (g == 0.) {
if (v0 > 0.) {
// indicate to brake as hard as possible
return -ACCEL2SPEED(myEmergencyDecel);
} else {
// stay stopped
return 0.;
}
}
// In general we solve g = v0^2/(-2a), where the the rhs is the distance
// covered until stop when breaking with a<0
const double a = -v0 * v0 / (2 * g);
return v0 + a * TS;
}
// The last case corresponds to a situation, where the vehicle may go with a positive
// speed v1 = v0 + tau*a after time tau.
// The distance covered until time tau is given as
// G1 = tau*(v0+v1)/2
// The distance covered between time tau and the stopping moment at time tau+v1/b is
// G2 = v1^2/(2b),
// where b is an assumed constant deceleration (= myDecel)
// We solve g = G1 + G2 for v1>0:
// <=> 0 = v1^2 + b*tau*v1 + b*tau*v0 - 2bg
// => v1 = -b*tau/2 + sqrt( (b*tau)^2/4 + b(2g - tau*v0) )
const double btau2 = myDecel * tau / 2;
const double v1 = -btau2 + sqrt(btau2 * btau2 + myDecel * (2 * g - tau * v0));
const double a = (v1 - v0) / tau;
return v0 + a * TS;
}
SUMOReal
MSCFModel_KraussOrig1::dawdle(SUMOReal speed) const throw() {
return MAX2(SUMOReal(0), speed - ACCEL2SPEED(myDawdle * myAccel * RandHelper::rand()));
}