void
MSCFModel::applyHeadwayAndSpeedDifferencePerceptionErrors(const MSVehicle* const veh, double speed, double& gap, double& predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
UNUSED_PARAMETER(speed);
UNUSED_PARAMETER(predMaxDecel);
assert(veh->hasDriverState());
// Obtain perceived gap and headway from the driver state
const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap, pred);
const double perceivedSpeedDifference = veh->getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
#ifdef DEBUG_DRIVER_ERRORS
if DEBUG_COND {
if (!veh->getDriverState()->debugLocked()) {
veh->getDriverState()->lockDebug();
std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
<< " speed=" << speed << " gap=" << gap << " leaderSpeed=" << predSpeed
<< "\n perceivedGap=" << perceivedGap << " perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
<< " perceivedSpeedDifference=" << perceivedSpeedDifference
<< std::endl;
const double exactFollowSpeed = followSpeed(veh, speed, gap, predSpeed, predMaxDecel);
const double errorFollowSpeed = followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel);
const double accelError = SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
std::cout << " gapError=" << perceivedGap - gap << " dvError=" << perceivedSpeedDifference - (predSpeed - speed)
<< "\n resulting accelError: " << accelError << std::endl;
veh->getDriverState()->unlockDebug();
}
}
#endif
gap = perceivedGap;
predSpeed = speed + perceivedSpeedDifference;
}
void
MSCFModel::applyHeadwayPerceptionError(const MSVehicle* const veh, double speed, double& gap) const {
UNUSED_PARAMETER(speed);
assert(veh->hasDriverState());
// Obtain perceived gap from driver state
const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap);
#ifdef DEBUG_DRIVER_ERRORS
if DEBUG_COND {
if (!veh->getDriverState()->debugLocked()) {
veh->getDriverState()->lockDebug();
std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
<< " speed=" << speed << " gap=" << gap << "\n perceivedGap=" << perceivedGap << std::endl;
const double exactStopSpeed = stopSpeed(veh, speed, gap);
const double errorStopSpeed = stopSpeed(veh, speed, perceivedGap);
const double accelError = SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
std::cout << " gapError=" << perceivedGap - gap << "\n resulting accelError: " << accelError << std::endl;
veh->getDriverState()->unlockDebug();
}
}
#endif
gap = perceivedGap;
}
SUMOReal
MSCFModel_PWag2009::moveHelper(MSVehicle* const veh, SUMOReal vPos) const {
const SUMOReal vNext = MSCFModel::moveHelper(veh, vPos);
VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
SUMOReal apref = SPEED2ACCEL(vNext - veh->getSpeed());
vars->aOld = apref;
return vNext;
}
// 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));
}
/** 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::passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed) {
assert(passedPos <= currentPos);
assert(passedPos >= lastPos);
assert(currentPos > lastPos);
assert(currentSpeed >= 0);
if (passedPos > currentPos || passedPos < lastPos) {
std::stringstream ss;
// Debug (Leo)
if (!MSGlobals::gSemiImplicitEulerUpdate) {
// NOTE: error is guarded to maintain original test output for euler update (Leo).
ss << "passingTime(): given argument passedPos = " << passedPos << " doesn't lie within [lastPos, currentPos] = [" << lastPos << ", " << currentPos << "]\nExtrapolating...";
std::cout << ss.str() << "\n";
WRITE_ERROR(ss.str());
}
const double lastCoveredDist = currentPos - lastPos;
const double extrapolated = passedPos > currentPos ? TS * (passedPos - lastPos) / lastCoveredDist : TS * (currentPos - passedPos) / lastCoveredDist;
return extrapolated;
} else if (currentSpeed < 0) {
WRITE_ERROR("passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
return -1;
}
const double distanceOldToPassed = passedPos - lastPos; // assert: >=0
if (MSGlobals::gSemiImplicitEulerUpdate) {
// euler update (constantly moving with currentSpeed during [0,TS])
const double t = distanceOldToPassed / currentSpeed;
return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
} else {
// ballistic update (constant acceleration a during [0,TS], except in case of a stop)
// determine acceleration
double a;
if (currentSpeed > 0) {
// the acceleration was constant within the last time step
a = SPEED2ACCEL(currentSpeed - lastSpeed);
} else {
// the currentSpeed is zero (the last was not because lastPos<currentPos).
assert(currentSpeed == 0 && lastSpeed != 0);
// In general the stop has taken place within the last time step.
// The acceleration (a<0) is obtained from
// deltaPos = - lastSpeed^2/(2*a)
a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
assert(a < 0);
}
// determine passing time t
// we solve distanceOldToPassed = lastSpeed*t + a*t^2/2
if (fabs(a) < NUMERICAL_EPS) {
// treat as constant speed within [0, TS]
const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
} else if (a > 0) {
// positive acceleration => only one positive solution
const double va = lastSpeed / a;
const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
assert(t < 1 && t >= 0);
return t;
} else {
// negative acceleration => two positive solutions (pick the smaller one.)
const double va = lastSpeed / a;
const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
assert(t < 1 && t >= 0);
return t;
}
}
}