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;