1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2002-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file MSLeaderInfo.cpp
11 /// @author Jakob Erdmann
12 /// @date Oct 2015
13 /// @version $Id$
14 ///
15 // Information about vehicles ahead (may be multiple vehicles if
16 // lateral-resolution is active)
17 /****************************************************************************/
18
19
20 // ===========================================================================
21 // included modules
22 // ===========================================================================
23 #include <config.h>
24
25 #include <cassert>
26 #include <cmath>
27 #include <utils/common/ToString.h>
28 #include <microsim/MSGlobals.h>
29 #include <microsim/MSVehicle.h>
30 #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
31 #include <microsim/MSNet.h>
32 #include "MSLeaderInfo.h"
33
34
35 // ===========================================================================
36 // static member variables
37 // ===========================================================================
38
39
40 // ===========================================================================
41 // MSLeaderInfo member method definitions
42 // ===========================================================================
MSLeaderInfo(const MSLane * lane,const MSVehicle * ego,double latOffset)43 MSLeaderInfo::MSLeaderInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
44 myWidth(lane->getWidth()),
45 myVehicles(MAX2(1, int(ceil(myWidth / MSGlobals::gLateralResolution))), (MSVehicle*)nullptr),
46 myFreeSublanes((int)myVehicles.size()),
47 egoRightMost(-1),
48 egoLeftMost(-1),
49 myHasVehicles(false) {
50 if (ego != nullptr) {
51 getSubLanes(ego, latOffset, egoRightMost, egoLeftMost);
52 // filter out sublanes not of interest to ego
53 myFreeSublanes -= egoRightMost;
54 myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
55 }
56 }
57
58
~MSLeaderInfo()59 MSLeaderInfo::~MSLeaderInfo() { }
60
61
62 int
addLeader(const MSVehicle * veh,bool beyond,double latOffset)63 MSLeaderInfo::addLeader(const MSVehicle* veh, bool beyond, double latOffset) {
64 if (veh == nullptr) {
65 return myFreeSublanes;
66 }
67 if (myVehicles.size() == 1) {
68 // speedup for the simple case
69 if (!beyond || myVehicles[0] == 0) {
70 myVehicles[0] = veh;
71 myFreeSublanes = 0;
72 myHasVehicles = true;
73 }
74 return myFreeSublanes;
75 }
76 // map center-line based coordinates into [0, myWidth] coordinates
77 int rightmost, leftmost;
78 getSubLanes(veh, latOffset, rightmost, leftmost);
79 //if (gDebugFlag1) std::cout << " addLeader veh=" << veh->getID() << " beyond=" << beyond << " latOffset=" << latOffset << " rightmost=" << rightmost << " leftmost=" << leftmost << " myFreeSublanes=" << myFreeSublanes << "\n";
80 for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
81 if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
82 && (!beyond || myVehicles[sublane] == 0)) {
83 if (myVehicles[sublane] == 0) {
84 myFreeSublanes--;
85 }
86 myVehicles[sublane] = veh;
87 myHasVehicles = true;
88 }
89 }
90 return myFreeSublanes;
91 }
92
93
94 void
clear()95 MSLeaderInfo::clear() {
96 myVehicles.assign(myVehicles.size(), (MSVehicle*)nullptr);
97 myFreeSublanes = (int)myVehicles.size();
98 if (egoRightMost >= 0) {
99 myFreeSublanes -= egoRightMost;
100 myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
101 }
102 }
103
104
105 void
getSubLanes(const MSVehicle * veh,double latOffset,int & rightmost,int & leftmost) const106 MSLeaderInfo::getSubLanes(const MSVehicle* veh, double latOffset, int& rightmost, int& leftmost) const {
107 if (myVehicles.size() == 1) {
108 // speedup for the simple case
109 rightmost = 0;
110 leftmost = 0;
111 return;
112 }
113 // map center-line based coordinates into [0, myWidth] coordinates
114 const double vehCenter = veh->getLateralPositionOnLane() + 0.5 * myWidth + latOffset;
115 const double vehHalfWidth = 0.5 * veh->getVehicleType().getWidth();
116 double rightVehSide = MAX2(0., vehCenter - vehHalfWidth);
117 double leftVehSide = MIN2(myWidth, vehCenter + vehHalfWidth);
118 // Reserve some additional space if the vehicle is performing a maneuver continuation.
119 if (veh->getActionStepLength() != DELTA_T) {
120 if (veh->getLaneChangeModel().getManeuverDist() < 0. || veh->getLaneChangeModel().getSpeedLat() < 0.) {
121 const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), -MIN2(0., veh->getLaneChangeModel().getManeuverDist()));
122 rightVehSide -= maneuverDist;
123 }
124 if (veh->getLaneChangeModel().getManeuverDist() > 0. || veh->getLaneChangeModel().getSpeedLat() > 0.) {
125 const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), MAX2(0., veh->getLaneChangeModel().getManeuverDist()));
126 leftVehSide += maneuverDist;
127 }
128 }
129
130 rightmost = MAX2(0, (int)floor((rightVehSide + NUMERICAL_EPS) / MSGlobals::gLateralResolution));
131 leftmost = MIN2((int)myVehicles.size() - 1, (int)floor((leftVehSide - NUMERICAL_EPS) / MSGlobals::gLateralResolution));
132 //if (veh->getID() == "Pepoli_11_41") std::cout << SIMTIME << " veh=" << veh->getID()
133 // << std::setprecision(10)
134 // << " posLat=" << veh->getLateralPositionOnLane()
135 // << " latOffset=" << latOffset
136 // << " rightVehSide=" << rightVehSide
137 // << " leftVehSide=" << leftVehSide
138 // << " rightmost=" << rightmost
139 // << " leftmost=" << leftmost
140 // << std::setprecision(2)
141 // << "\n";
142 }
143
144
145 void
getSublaneBorders(int sublane,double latOffset,double & rightSide,double & leftSide) const146 MSLeaderInfo::getSublaneBorders(int sublane, double latOffset, double& rightSide, double& leftSide) const {
147 assert(sublane >= 0);
148 assert(sublane < (int)myVehicles.size());
149 const double res = MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : myWidth;
150 rightSide = sublane * res + latOffset;
151 leftSide = MIN2((sublane + 1) * res, myWidth) + latOffset;
152 }
153
154
155 const MSVehicle*
operator [](int sublane) const156 MSLeaderInfo::operator[](int sublane) const {
157 assert(sublane >= 0);
158 assert(sublane < (int)myVehicles.size());
159 return myVehicles[sublane];
160 }
161
162
163 std::string
toString() const164 MSLeaderInfo::toString() const {
165 std::ostringstream oss;
166 oss.setf(std::ios::fixed, std::ios::floatfield);
167 oss << std::setprecision(2);
168 for (int i = 0; i < (int)myVehicles.size(); ++i) {
169 oss << Named::getIDSecure(myVehicles[i]);
170 if (i < (int)myVehicles.size() - 1) {
171 oss << ", ";
172 }
173 }
174 oss << " free=" << myFreeSublanes;
175 return oss.str();
176 }
177
178
179 bool
hasStoppedVehicle() const180 MSLeaderInfo::hasStoppedVehicle() const {
181 if (!myHasVehicles) {
182 return false;
183 }
184 for (int i = 0; i < (int)myVehicles.size(); ++i) {
185 if (myVehicles[0] != 0 && myVehicles[0]->isStopped()) {
186 return true;
187 }
188 }
189 return false;
190 }
191
192 // ===========================================================================
193 // MSLeaderDistanceInfo member method definitions
194 // ===========================================================================
195
196
MSLeaderDistanceInfo(const MSLane * lane,const MSVehicle * ego,double latOffset)197 MSLeaderDistanceInfo::MSLeaderDistanceInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
198 MSLeaderInfo(lane, ego, latOffset),
199 myDistances(myVehicles.size(), std::numeric_limits<double>::max()) {
200 }
201
202
MSLeaderDistanceInfo(const CLeaderDist & cLeaderDist,const MSLane * dummy)203 MSLeaderDistanceInfo::MSLeaderDistanceInfo(const CLeaderDist& cLeaderDist, const MSLane* dummy) :
204 MSLeaderInfo(dummy, nullptr, 0),
205 myDistances(1, cLeaderDist.second) {
206 assert(myVehicles.size() == 1);
207 myVehicles[0] = cLeaderDist.first;
208 myHasVehicles = cLeaderDist.first != nullptr;
209 }
210
~MSLeaderDistanceInfo()211 MSLeaderDistanceInfo::~MSLeaderDistanceInfo() { }
212
213
214 int
addLeader(const MSVehicle * veh,double gap,double latOffset,int sublane)215 MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, double gap, double latOffset, int sublane) {
216 //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
217 // std::cout << " BREAKPOINT\n";
218 //}
219 if (veh == nullptr) {
220 return myFreeSublanes;
221 }
222 if (myVehicles.size() == 1) {
223 // speedup for the simple case
224 sublane = 0;
225 }
226 if (sublane >= 0 && sublane < (int)myVehicles.size()) {
227 // sublane is already given
228 if (gap < myDistances[sublane]) {
229 if (myVehicles[sublane] == 0) {
230 myFreeSublanes--;
231 }
232 myVehicles[sublane] = veh;
233 myDistances[sublane] = gap;
234 myHasVehicles = true;
235 }
236 return myFreeSublanes;
237 }
238 int rightmost, leftmost;
239 getSubLanes(veh, latOffset, rightmost, leftmost);
240 for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
241 if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
242 && gap < myDistances[sublane]) {
243 if (myVehicles[sublane] == 0) {
244 myFreeSublanes--;
245 }
246 myVehicles[sublane] = veh;
247 myDistances[sublane] = gap;
248 myHasVehicles = true;
249 }
250 }
251 return myFreeSublanes;
252 }
253
254
255 void
clear()256 MSLeaderDistanceInfo::clear() {
257 MSLeaderInfo::clear();
258 myDistances.assign(myVehicles.size(), std::numeric_limits<double>::max());
259 }
260
261
262 CLeaderDist
operator [](int sublane) const263 MSLeaderDistanceInfo::operator[](int sublane) const {
264 assert(sublane >= 0);
265 assert(sublane < (int)myVehicles.size());
266 return std::make_pair(myVehicles[sublane], myDistances[sublane]);
267 }
268
269
270 std::string
toString() const271 MSLeaderDistanceInfo::toString() const {
272 std::ostringstream oss;
273 oss.setf(std::ios::fixed, std::ios::floatfield);
274 oss << std::setprecision(2);
275 for (int i = 0; i < (int)myVehicles.size(); ++i) {
276 oss << Named::getIDSecure(myVehicles[i]) << ":";
277 if (myVehicles[i] == 0) {
278 oss << "inf";
279 } else {
280 oss << myDistances[i];
281 }
282 if (i < (int)myVehicles.size() - 1) {
283 oss << ", ";
284 }
285 }
286 oss << " free=" << myFreeSublanes;
287 return oss.str();
288 }
289
290
291 // ===========================================================================
292 // MSCriticalFollowerDistanceInfo member method definitions
293 // ===========================================================================
294
295
MSCriticalFollowerDistanceInfo(const MSLane * lane,const MSVehicle * ego,double latOffset)296 MSCriticalFollowerDistanceInfo::MSCriticalFollowerDistanceInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
297 MSLeaderDistanceInfo(lane, ego, latOffset),
298 myMissingGaps(myVehicles.size(), -std::numeric_limits<double>::max()) {
299 }
300
301
~MSCriticalFollowerDistanceInfo()302 MSCriticalFollowerDistanceInfo::~MSCriticalFollowerDistanceInfo() { }
303
304
305 int
addFollower(const MSVehicle * veh,const MSVehicle * ego,double gap,double latOffset,int sublane)306 MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, double gap, double latOffset, int sublane) {
307 if (veh == nullptr) {
308 return myFreeSublanes;
309 }
310 const double requiredGap = veh->getCarFollowModel().getSecureGap(veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
311 const double missingGap = requiredGap - gap;
312 /*
313 if (ego->getID() == "disabled" || gDebugFlag1) {
314 std::cout << " addFollower veh=" << veh->getID()
315 << " ego=" << ego->getID()
316 << " gap=" << gap
317 << " reqGap=" << requiredGap
318 << " missingGap=" << missingGap
319 << " latOffset=" << latOffset
320 << " sublane=" << sublane
321 << "\n";
322 if (sublane > 0) {
323 std::cout
324 << " dists[s]=" << myDistances[sublane]
325 << " gaps[s]=" << myMissingGaps[sublane]
326 << "\n";
327 } else {
328 std::cout << toString() << "\n";
329 }
330 }
331 */
332 if (myVehicles.size() == 1) {
333 // speedup for the simple case
334 sublane = 0;
335 }
336 if (sublane >= 0 && sublane < (int)myVehicles.size()) {
337 // sublane is already given
338 // overlapping vehicles are stored preferably
339 // among those vehicles with missing gap, closer ones are preferred
340 if ((missingGap > myMissingGaps[sublane]
341 || (missingGap > 0 && gap < myDistances[sublane])
342 || (gap < 0 && myDistances[sublane] > 0))
343 && !(gap > 0 && myDistances[sublane] < 0)
344 && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
345 ) {
346 if (myVehicles[sublane] == 0) {
347 myFreeSublanes--;
348 }
349 myVehicles[sublane] = veh;
350 myDistances[sublane] = gap;
351 myMissingGaps[sublane] = missingGap;
352 myHasVehicles = true;
353 }
354 return myFreeSublanes;
355 }
356 int rightmost, leftmost;
357 getSubLanes(veh, latOffset, rightmost, leftmost);
358 for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
359 if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
360 // overlapping vehicles are stored preferably
361 // among those vehicles with missing gap, closer ones are preferred
362 && (missingGap > myMissingGaps[sublane]
363 || (missingGap > 0 && gap < myDistances[sublane])
364 || (gap < 0 && myDistances[sublane] > 0))
365 && !(gap > 0 && myDistances[sublane] < 0)
366 && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
367 ) {
368 if (myVehicles[sublane] == 0) {
369 myFreeSublanes--;
370 }
371 myVehicles[sublane] = veh;
372 myDistances[sublane] = gap;
373 myMissingGaps[sublane] = missingGap;
374 myHasVehicles = true;
375 }
376 }
377 return myFreeSublanes;
378 }
379
380
381 void
clear()382 MSCriticalFollowerDistanceInfo::clear() {
383 MSLeaderDistanceInfo::clear();
384 myMissingGaps.assign(myVehicles.size(), -std::numeric_limits<double>::max());
385 }
386
387
388 std::string
toString() const389 MSCriticalFollowerDistanceInfo::toString() const {
390 std::ostringstream oss;
391 oss.setf(std::ios::fixed, std::ios::floatfield);
392 oss << std::setprecision(2);
393 for (int i = 0; i < (int)myVehicles.size(); ++i) {
394 oss << Named::getIDSecure(myVehicles[i]) << ":";
395 if (myVehicles[i] == 0) {
396 oss << "inf:-inf";
397 } else {
398 oss << myDistances[i] << ":" << myMissingGaps[i];
399 }
400 if (i < (int)myVehicles.size() - 1) {
401 oss << ", ";
402 }
403 }
404 oss << " free=" << myFreeSublanes;
405 return oss.str();
406 }
407 /****************************************************************************/
408
409