45 #define DEBUG_COND (veh->isSelected())
48 #define DEBUG_COND2 (gDebugFlag1)
63 myHeadwayTime(vtype->getParameter().getCFParam(
SUMO_ATTR_TAU, 1.0)) {
82 return speed * (headwayTime + 0.5 * speed / decel);
93 const int steps = int(speed / speedReduction);
94 return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
99 MSCFModel::freeSpeed(
const double currentSpeed,
const double decel,
const double dist,
const double targetSpeed,
const bool onInsertion,
const double actionStepLength) {
115 const double y =
MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
116 const double yFull = floor(y);
117 const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
118 const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) *
ACCEL2SPEED(decel);
119 return DIST2SPEED(
MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
134 assert(currentSpeed >= 0);
135 assert(targetSpeed >= 0);
137 const double dt = onInsertion ? 0 : actionStepLength;
138 const double v0 = currentSpeed;
139 const double vT = targetSpeed;
140 const double b = decel;
151 if (0.5 * (v0 + vT)*dt >= d) {
153 return v0 +
TS * (vT - v0) / actionStepLength;
155 const double q = ((dt * v0 - 2 * d) * b - vT * vT);
156 const double p = 0.5 * b * dt;
157 const double vN = -p + sqrt(p * p - q);
158 return v0 +
TS * (vN - v0) / actionStepLength;
166 const double oldV = veh->
getSpeed();
186 #ifdef DEBUG_FINALIZE_SPEED
188 std::cout <<
"\n" <<
SIMTIME <<
" FINALIZE_SPEED\n";
192 vMax =
MAX2(vMin, vMax);
195 #ifdef DEBUG_FINALIZE_SPEED
196 double vDawdle = vNext;
198 assert(vNext >= vMin);
199 assert(vNext <= vMax);
202 assert(vNext >= vMin);
203 assert(vNext <= vMax);
205 #ifdef DEBUG_FINALIZE_SPEED
208 <<
"veh '" << veh->
getID() <<
"' oldV=" << oldV
212 <<
" vStop=" << vStop
213 <<
" vDawdle=" << vDawdle
214 <<
" vNext=" << vNext
228 const double gap = (vNext - vL) *
301 double leaderMinDist = gap2pred +
distAfterTime(duration, predSpeed, -predMaxDecel);
306 int a = (int)ceil(duration /
TS -
TS);
309 if (bg <= leaderMinDist) {
313 if (
gDebugFlag2) std::cout <<
" followSpeedTransient"
314 <<
" duration=" << duration
315 <<
" gap=" << gap2pred
316 <<
" leaderMinDist=" << leaderMinDist
321 <<
" x=" << (b + leaderMinDist) / duration
323 return (b + leaderMinDist) / duration;
328 while (bg < leaderMinDist) {
337 const double fullBrakingSeconds = sqrt(leaderMinDist * 2 /
myDecel);
338 if (fullBrakingSeconds >= duration) {
342 return leaderMinDist / duration + duration *
getMaxDecel() / 2;
344 return fullBrakingSeconds *
myDecel;
352 return (speed + 0.5 * accel * t) * t;
354 const double decel = -accel;
355 if (speed <= decel * t) {
369 const double speed2 = speed - t * decel;
370 return 0.5 * (speed + speed2) * t;
377 const double accelTime = (arrivalSpeed - currentSpeed) / accel;
378 const double accelWay = accelTime * (arrivalSpeed + currentSpeed) * 0.5;
379 const double nonAccelWay =
MAX2(0., dist - accelWay);
383 return TIME2STEPS(accelTime + nonAccelWay / nonAccelSpeed);
396 if ((accel < 0. && -0.5 * speed * speed / accel < dist) || (accel <= 0. && speed == 0.)) {
405 double p = speed / accel;
409 return (-p - sqrt(p * p + 2 * dist / accel));
414 double t1 = (maxSpeed - speed) / accel;
416 double d1 = speed * t1 + 0.5 * accel * t1 * t1;
419 return (-p + sqrt(p * p + 2 * dist / accel));
421 return (-p + sqrt(p * p + 2 * d1 / accel)) + (dist - d1) / maxSpeed;
435 assert(accel == decel);
437 assert(initialSpeed == 0);
438 assert(arrivalSpeed == 0);
439 assert(maxSpeed > 0);
442 double accelTime = (maxSpeed - initialSpeed) / accel;
444 double accelDist = accelTime * (initialSpeed + 0.5 * (maxSpeed - initialSpeed));
446 if (accelDist >= dist * 0.5) {
448 arrivalTime = 4 * sqrt(dist / accel);
451 const double constSpeedTime = (dist - accelDist * 2) / maxSpeed;
452 arrivalTime = accelTime + constSpeedTime;
460 assert(time > 0 || dist == 0);
463 }
else if (time * speed > 2 * dist) {
466 return - 0.5 * speed * speed / dist;
470 return 2 * (dist / time - speed) / time;
484 double arrivalSpeedBraking;
487 if (dist < currentSpeed) {
495 return arrivalSpeedBraking;
502 MSCFModel::gapExtrapolation(
const double duration,
const double currentGap,
double v1,
double v2,
double a1,
double a2,
const double maxV1,
const double maxV2) {
504 double newGap = currentGap;
507 for (
unsigned int steps = 1; steps *
TS <= duration; ++steps) {
508 v1 =
MIN2(
MAX2(v1 + a1, 0.), maxV1);
509 v2 =
MIN2(
MAX2(v2 + a2, 0.), maxV2);
510 newGap +=
TS * (v1 - v2);
515 double t1 = 0, t2 = 0, t3 = 0, t4 = 0;
518 if (a1 < 0 && v1 > 0) {
519 const double leaderStopTime = - v1 / a1;
520 t1 =
MIN2(leaderStopTime, duration);
521 }
else if (a1 >= 0) {
525 if (a2 < 0 && v2 > 0) {
526 const double followerStopTime = -v2 / a2;
527 t2 =
MIN2(followerStopTime, duration);
528 }
else if (a2 >= 0) {
532 if (a1 > 0 && v1 < maxV1) {
533 const double leaderMaxSpeedTime = (maxV1 - v1) / a1;
534 t3 =
MIN2(leaderMaxSpeedTime, duration);
535 }
else if (a1 <= 0) {
539 if (a2 > 0 && v2 < maxV2) {
540 const double followerMaxSpeedTime = (maxV2 - v2) / a2;
541 t4 =
MIN2(followerMaxSpeedTime, duration);
542 }
else if (a2 <= 0) {
554 std::list<double>::const_iterator i;
556 for (i = l.begin(); i != l.end(); ++i) {
558 double dt =
MIN2(*i, duration) - tLast;
561 newGap += dv * dt + da * dt * dt / 2.;
565 if (*i == t1 || *i == t3) {
570 if (*i == t2 || *i == t4) {
575 tLast =
MIN2(*i, duration);
576 if (tLast == duration) {
581 if (duration != tLast) {
583 assert(a1 == 0. && a2 == 0.);
584 double dt = duration - tLast;
596 MSCFModel::passingTime(
const double lastPos,
const double passedPos,
const double currentPos,
const double lastSpeed,
const double currentSpeed) {
598 assert(passedPos <= currentPos);
599 assert(passedPos >= lastPos);
600 assert(currentPos > lastPos);
601 assert(currentSpeed >= 0);
603 if (passedPos > currentPos || passedPos < lastPos) {
604 std::stringstream ss;
608 ss <<
"passingTime(): given argument passedPos = " << passedPos <<
" doesn't lie within [lastPos, currentPos] = [" << lastPos <<
", " << currentPos <<
"]\nExtrapolating...";
609 std::cout << ss.str() <<
"\n";
612 const double lastCoveredDist = currentPos - lastPos;
613 const double extrapolated = passedPos > currentPos ?
TS * (passedPos - lastPos) / lastCoveredDist :
TS * (currentPos - passedPos) / lastCoveredDist;
615 }
else if (currentSpeed < 0) {
616 WRITE_ERROR(
"passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
620 const double distanceOldToPassed = passedPos - lastPos;
624 if (currentSpeed == 0) {
627 const double t = distanceOldToPassed / currentSpeed;
635 if (currentSpeed > 0) {
640 assert(currentSpeed == 0 && lastSpeed != 0);
644 a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));
653 const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
657 const double va = lastSpeed / a;
658 const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
659 assert(t < 1 && t >= 0);
663 const double va = lastSpeed / a;
664 const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
665 assert(t < 1 && t >= 0);
675 assert(t >= 0 && t <=
TS);
683 if (dist <
TS * v0 / 2) {
686 const double accel = - v0 * v0 / (2 * dist);
688 return v0 + accel * t;
691 const double accel = 2 * (dist /
TS - v0) /
TS;
693 return v0 + accel * t;
705 (double)sqrt(
MAX2(0., 2 * dist * accel + v * v)));
766 const double g = gap;
776 const double n = floor(.5 - ((t + (sqrt(((s * s) + (4.0 * ((s * (2.0 * g / b - t)) + (t * t))))) * -0.5)) / s));
777 const double h = 0.5 * n * (n - 1) * b * s + n * b * t;
781 const double r = (g - h) / (n * s + t);
782 const double x = n * b + r;
807 const double btau =
myDecel * headway;
808 const double v0 = -btau + sqrt(btau * btau + 2 *
myDecel * g);
817 const double tau = headway == 0 ?
TS : headway;
818 const double v0 =
MAX2(0., v);
820 if (v0 * tau >= 2 * g) {
832 const double a = -v0 * v0 / (2 * g);
847 const double btau2 =
myDecel * tau / 2;
848 const double v1 = -btau2 + sqrt(btau2 * btau2 +
myDecel * (2 * g - tau * v0));
849 const double a = (v1 - v0) / tau;
894 #ifdef DEBUG_EMERGENCYDECEL
896 std::cout <<
SIMTIME <<
" initial vsafe=" << x
897 <<
" egoSpeed=" << egoSpeed <<
" (origSafeDecel=" << origSafeDecel <<
")"
898 <<
" predSpeed=" << predSpeed <<
" (predDecel=" << predMaxDecel <<
")"
907 safeDecel =
MIN2(safeDecel, origSafeDecel);
913 #ifdef DEBUG_EMERGENCYDECEL
915 std::cout <<
" -> corrected emergency deceleration: " << safeDecel <<
" newVSafe=" << x << std::endl;
934 const double predBrakeDist = 0.5 * predSpeed * predSpeed / predMaxDecel;
936 const double b1 = 0.5 * egoSpeed * egoSpeed / (gap + predBrakeDist);
938 #ifdef DEBUG_EMERGENCYDECEL
940 std::cout <<
SIMTIME <<
" calculateEmergencyDeceleration()"
941 <<
" gap=" << gap <<
" egoSpeed=" << egoSpeed <<
" predSpeed=" << predSpeed
942 <<
" predBrakeDist=" << predBrakeDist
948 if (b1 <= predMaxDecel) {
950 #ifdef DEBUG_EMERGENCYDECEL
952 std::cout <<
" case 1 ..." << std::endl;
957 #ifdef DEBUG_EMERGENCYDECEL
959 std::cout <<
" case 2 ...";
964 assert(gap < 0 || predSpeed < egoSpeed);
969 const double b2 = 0.5 * (egoSpeed * egoSpeed - predSpeed * predSpeed) / gap;
971 #ifdef DEBUG_EMERGENCYDECEL
973 std::cout <<
" b2=" << b2 << std::endl;
990 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap, pred);
991 const double perceivedSpeedDifference = veh->
getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);
993 #ifdef DEBUG_DRIVER_ERRORS
997 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
998 <<
" speed=" << speed <<
" gap=" << gap <<
" leaderSpeed=" << predSpeed
999 <<
"\n perceivedGap=" << perceivedGap <<
" perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
1000 <<
" perceivedSpeedDifference=" << perceivedSpeedDifference
1002 const double exactFollowSpeed =
followSpeed(veh, speed, gap, predSpeed, predMaxDecel);
1003 const double errorFollowSpeed =
followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel);
1004 const double accelError =
SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
1005 std::cout <<
" gapError=" << perceivedGap - gap <<
" dvError=" << perceivedSpeedDifference - (predSpeed - speed)
1006 <<
"\n resulting accelError: " << accelError << std::endl;
1013 predSpeed = speed + perceivedSpeedDifference;
1028 const double perceivedGap = veh->
getDriverState()->getPerceivedHeadway(gap);
1030 #ifdef DEBUG_DRIVER_ERRORS
1034 std::cout <<
SIMTIME <<
" veh '" << veh->
getID() <<
"' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
1035 <<
" speed=" << speed <<
" gap=" << gap <<
"\n perceivedGap=" << perceivedGap << std::endl;
1036 const double exactStopSpeed =
stopSpeed(veh, speed, gap);
1037 const double errorStopSpeed =
stopSpeed(veh, speed, perceivedGap);
1038 const double accelError =
SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
1039 std::cout <<
" gapError=" << perceivedGap - gap <<
"\n resulting accelError: " << accelError << std::endl;