1 package org.opentrafficsim.road.network.lane;
2
3 import java.io.Serializable;
4 import java.util.ArrayList;
5 import java.util.Collections;
6 import java.util.Iterator;
7 import java.util.LinkedHashMap;
8 import java.util.LinkedHashSet;
9 import java.util.List;
10 import java.util.Map;
11 import java.util.Map.Entry;
12 import java.util.NavigableMap;
13 import java.util.Set;
14 import java.util.SortedMap;
15 import java.util.TreeMap;
16
17 import org.djunits.unit.LengthUnit;
18 import org.djunits.unit.TimeUnit;
19 import org.djunits.value.vdouble.scalar.Duration;
20 import org.djunits.value.vdouble.scalar.Length;
21 import org.djunits.value.vdouble.scalar.Speed;
22 import org.djunits.value.vdouble.scalar.Time;
23 import org.djutils.event.TimedEventType;
24 import org.djutils.exceptions.Throw;
25 import org.djutils.immutablecollections.Immutable;
26 import org.djutils.immutablecollections.ImmutableArrayList;
27 import org.djutils.immutablecollections.ImmutableLinkedHashMap;
28 import org.djutils.immutablecollections.ImmutableList;
29 import org.djutils.immutablecollections.ImmutableMap;
30 import org.djutils.multikeymap.MultiKeyMap;
31 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
32 import org.opentrafficsim.core.geometry.OTSGeometryException;
33 import org.opentrafficsim.core.gtu.GTUDirectionality;
34 import org.opentrafficsim.core.gtu.GTUException;
35 import org.opentrafficsim.core.gtu.GTUType;
36 import org.opentrafficsim.core.gtu.RelativePosition;
37 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
38 import org.opentrafficsim.core.network.LateralDirectionality;
39 import org.opentrafficsim.core.network.Link;
40 import org.opentrafficsim.core.network.NetworkException;
41 import org.opentrafficsim.core.network.Node;
42 import org.opentrafficsim.core.perception.HistoryManager;
43 import org.opentrafficsim.core.perception.collections.HistoricalArrayList;
44 import org.opentrafficsim.core.perception.collections.HistoricalList;
45 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
46 import org.opentrafficsim.road.network.RoadNetwork;
47 import org.opentrafficsim.road.network.lane.object.AbstractLaneBasedObject;
48 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
49 import org.opentrafficsim.road.network.lane.object.sensor.AbstractSensor;
50 import org.opentrafficsim.road.network.lane.object.sensor.DestinationSensor;
51 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
52 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
53
54 import nl.tudelft.simulation.dsol.SimRuntimeException;
55 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEvent;
56
57 /**
58 * The Lane is the CrossSectionElement of a CrossSectionLink on which GTUs can drive. The Lane stores several important
59 * properties, such as the successor lane(s), predecessor lane(s), and adjacent lane(s), all separated per GTU type. It can, for
60 * instance, be that a truck is not allowed to move into an adjacent lane, while a car is allowed to do so. Furthermore, the
61 * lane contains sensors that can be triggered by passing GTUs. The Lane class also contains methods to determine to trigger the
62 * sensors at exactly calculated and scheduled times, given the movement of the GTUs. <br>
63 * Finally, the Lane stores the GTUs on the lane, and contains several access methods to determine successor and predecessor
64 * GTUs, as well as methods to add a GTU to a lane (either at the start or in the middle when changing lanes), and remove a GTU
65 * from the lane (either at the end, or in the middle when changing onto another lane).
66 * <p>
67 * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
68 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
69 * <p>
70 * $LastChangedDate: 2015-09-24 14:17:07 +0200 (Thu, 24 Sep 2015) $, @version $Revision: 1407 $, by $Author: averbraeck $,
71 * initial version Aug 19, 2014 <br>
72 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
73 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
74 */
75 public class Lane extends CrossSectionElement implements Serializable
76 {
77 /** */
78 private static final long serialVersionUID = 20150826L;
79
80 /** Type of lane to deduce compatibility with GTU types. */
81 private final LaneType laneType;
82
83 /**
84 * SHOULD NOT BE IN Lane (but in LaneType). The directions in which vehicles can drive, i.e., in direction of geometry,
85 * reverse, or both. This can differ per GTU type. In an overtake lane, cars might overtake and trucks not. It might be that
86 * the lane (e.g., a street in a city) is FORWARD (from start node of the link to end node of the link) for the GTU type
87 * CAR, but BOTH for the GTU type BICYCLE (i.e., bicycles can also go in the other direction, opposite to the drawing
88 * direction of the Link). If the directionality for a GTUType is set to NONE, this means that the given GTUType cannot use
89 * the Lane. If a Directionality is set for GTUType.ALL, the getDirectionality will default to these settings when there is
90 * no specific entry for a given directionality. This means that the settings can be used additive, or restrictive. <br>
91 * In <b>additive use</b>, set the directionality for GTUType.ALL to NONE, or do not set the directionality for GTUType.ALL.
92 * Now, one by one, the allowed directionalities can be added. An example is a lane on a highway, which we only open for
93 * CAR, TRUCK and BUS. <br>
94 * In <b>restrictive use</b>, set the directionality for GTUType.ALL to BOTH, FORWARD, or BACKWARD. Override the
95 * directionality for certain GTUTypes to a more restrictive access, e.g. to NONE. An example is a lane that is open for all
96 * road users, except TRUCK.
97 */
98 // private final Map<GTUType, LongitudinalDirectionality> directionalityMap;
99
100 /**
101 * The speed limit of this lane, which can differ per GTU type. Cars might be allowed to drive 120 km/h and trucks 90 km/h.
102 * If the speed limit is the same for all GTU types, GTUType.ALL will be used. This means that the settings can be used
103 * additive, or subtractive. <br>
104 * In <b>additive use</b>, do not set the speed limit for GTUType.ALL. Now, one by one, the allowed maximum speeds for each
105 * of the GTU Types have be added. Do this when there are few GTU types or the speed limits per TU type are very different.
106 * <br>
107 * In <b>subtractive use</b>, set the speed limit for GTUType.ALL to the most common one. Override the speed limit for
108 * certain GTUTypes to a different value. An example is a lane on a highway where all vehicles, except truck (CAR, BUS,
109 * MOTORCYCLE, etc.), can drive 120 km/h, but trucks are allowed only 90 km/h. In that case, set the speed limit for
110 * GTUType.ALL to 120 km/h, and for TRUCK to 90 km/h.
111 */
112 // TODO allow for direction-dependent speed limit
113 private Map<GTUType, Speed> speedLimitMap;
114
115 /** Cached speed limits; these are cleared when a speed limit is changed. */
116 private final Map<GTUType, Speed> cachedSpeedLimits = new LinkedHashMap<>();
117
118 /**
119 * Sensors on the lane to trigger behavior of the GTU, sorted by longitudinal position. The triggering of sensors is done
120 * per GTU type, so different GTUs can trigger different sensors.
121 */
122 // TODO allow for direction-dependent sensors
123 private final SortedMap<Double, List<SingleSensor>> sensors = new TreeMap<>();
124
125 /**
126 * Objects on the lane can be observed by the GTU. Examples are signs, speed signs, blocks, and traffic lights. They are
127 * sorted by longitudinal position.
128 */
129 // TODO allow for direction-dependent lane objects
130 private final SortedMap<Double, List<LaneBasedObject>> laneBasedObjects = new TreeMap<>();
131
132 /** GTUs ordered by increasing longitudinal position; increasing in the direction of the center line. */
133 private final HistoricalList<LaneBasedGTU> gtuList;
134
135 /** Last returned past GTU list. */
136 private List<LaneBasedGTU> gtuListAtTime = null;
137
138 /** Time of last returned GTU list. */
139 private Time gtuListTime = null;
140
141 /**
142 * Adjacent left lanes that some GTU types can change onto. Left is defined relative to the direction of the design line of
143 * the link (and the direction of the center line of the lane). In terms of offsets, 'left' lanes always have a more
144 * positive offset than the current lane. Initially empty so we can calculate and cache the first time the method is called.
145 */
146 private final MultiKeyMap<Set<Lane>> leftNeighbours =
147 new MultiKeyMap<>(GTUType.class, GTUDirectionality.class, Boolean.class);
148
149 /**
150 * Adjacent right lanes that some GTU types can change onto. Right is defined relative to the direction of the design line
151 * of the link (and the direction of the center line of the lane). In terms of offsets, 'right' lanes always have a more
152 * negative offset than the current lane. Initially empty so we can calculate and cache the first time the method is called.
153 */
154 private final MultiKeyMap<Set<Lane>> rightNeighbours =
155 new MultiKeyMap<>(GTUType.class, GTUDirectionality.class, Boolean.class);
156
157 /**
158 * Next lane(s) following this lane that some GTU types can drive from or onto. Next is defined in the direction of the
159 * design line. Initially null so we can calculate and cache the first time the method is called.
160 */
161 private Map<GTUType, Map<Lane, GTUDirectionality>> nextLanes = null;
162
163 /**
164 * Previous lane(s) preceding this lane that some GTU types can drive from or onto. Previous is defined relative to the
165 * direction of the design line. Initially null so we can calculate and cache the first time the method is called.
166 */
167 private Map<GTUType, Map<Lane, GTUDirectionality>> prevLanes = null;
168
169 /**
170 * Downstream lane(s) following this lane that some GTU types can drive onto given the direction. Initially empty so we can
171 * calculate and cache the first time the method is called.
172 */
173 private MultiKeyMap<ImmutableMap<Lane, GTUDirectionality>> downLanes =
174 new MultiKeyMap<>(GTUType.class, GTUDirectionality.class);
175
176 /**
177 * Previous lane(s) preceding this lane that some GTU types can drive from given the direction. Initially empty so we can
178 * calculate and cache the first time the method is called.
179 */
180 private MultiKeyMap<ImmutableMap<Lane, GTUDirectionality>> upLanes =
181 new MultiKeyMap<>(GTUType.class, GTUDirectionality.class);
182
183 /**
184 * The <b>timed</b> event type for pub/sub indicating the addition of a GTU to the lane. <br>
185 * Payload: Object[] {String gtuId, LaneBasedGTU gtu, int count_after_addition}
186 */
187 public static final TimedEventType GTU_ADD_EVENT = new TimedEventType("LANE.GTU.ADD");
188 // public static final TimedEventType GTU_ADD_EVENT = new TimedEventType("LANE.GTU.ADD",
189 // new MetaData("GTU added to lane", "GTU added",
190 // new ObjectDescriptor[] { new ObjectDescriptor("Id of newly added GTU", "GTU id", String.class),
191 // new ObjectDescriptor("New number of GTUs in lane", "GTU count", Integer.class) }));
192
193 /**
194 * The <b>timed</b> event type for pub/sub indicating the removal of a GTU from the lane. <br>
195 * Payload: Object[] {String gtuId, LaneBasedGTU gtu, int count_after_removal, Length position}
196 */
197 public static final TimedEventType GTU_REMOVE_EVENT = new TimedEventType("LANE.GTU.REMOVE");
198
199 /**
200 * The <b>timed</b> event type for pub/sub indicating the addition of a Sensor to the lane. <br>
201 * Payload: Object[] {String sensorId, Sensor sensor}
202 */
203 public static final TimedEventType SENSOR_ADD_EVENT = new TimedEventType("LANE.SENSOR.ADD");
204
205 /**
206 * The <b>timed</b> event type for pub/sub indicating the removal of a Sensor from the lane. <br>
207 * Payload: Object[] {String sensorId, Sensor sensor}
208 */
209 public static final TimedEventType SENSOR_REMOVE_EVENT = new TimedEventType("LANE.SENSOR.REMOVE");
210
211 /**
212 * The event type for pub/sub indicating the addition of a LaneBasedObject to the lane. <br>
213 * Payload: Object[] {LaneBasedObject laneBasedObject}
214 */
215 public static final TimedEventType OBJECT_ADD_EVENT = new TimedEventType("LANE.OBJECT.ADD");
216
217 /**
218 * The event type for pub/sub indicating the removal of a LaneBasedObject from the lane. <br>
219 * Payload: Object[] {LaneBasedObject laneBasedObject}
220 */
221 public static final TimedEventType OBJECT_REMOVE_EVENT = new TimedEventType("LANE.OBJECT.REMOVE");
222
223 /**
224 * Construct a new Lane.
225 * @param parentLink CrossSectionLink; the link to which the new Lane will belong (must be constructed first)
226 * @param id String; the id of this lane within the link; should be unique within the link.
227 * @param lateralOffsetAtStart Length; the lateral offset of the design line of the new CrossSectionLink with respect to the
228 * design line of the parent Link at the start of the parent Link
229 * @param lateralOffsetAtEnd Length; the lateral offset of the design line of the new CrossSectionLink with respect to the
230 * design line of the parent Link at the end of the parent Link
231 * @param beginWidth Length; start width, positioned <i>symmetrically around</i> the design line
232 * @param endWidth Length; end width, positioned <i>symmetrically around</i> the design line
233 * @param laneType LaneType; the type of lane to deduce compatibility with GTU types
234 * @param speedLimitMap Map<GTUType, Speed>; speed limit on this lane, specified per GTU Type
235 * @param fixGradualLateralOffset boolean; true if gradualLateralOffset needs to be fixed
236 * @throws OTSGeometryException when creation of the center line or contour geometry fails
237 * @throws NetworkException when id equal to null or not unique
238 */
239 @SuppressWarnings("checkstyle:parameternumber")
240 public Lane(final CrossSectionLink parentLink, final String id, final Length lateralOffsetAtStart,
241 final Length lateralOffsetAtEnd, final Length beginWidth, final Length endWidth, final LaneType laneType,
242 final Map<GTUType, Speed> speedLimitMap, final boolean fixGradualLateralOffset)
243 throws OTSGeometryException, NetworkException
244 {
245 super(parentLink, id, lateralOffsetAtStart, lateralOffsetAtEnd, beginWidth, endWidth, fixGradualLateralOffset);
246 this.laneType = laneType;
247 checkDirectionality();
248 this.speedLimitMap = speedLimitMap;
249 this.gtuList = new HistoricalArrayList<>(getManager(parentLink));
250 }
251
252 /**
253 * Construct a new Lane.
254 * @param parentLink CrossSectionLink; the link to which the new Lane will belong (must be constructed first)
255 * @param id String; the id of this lane within the link; should be unique within the link.
256 * @param lateralOffsetAtStart Length; the lateral offset of the design line of the new CrossSectionLink with respect to the
257 * design line of the parent Link at the start of the parent Link
258 * @param lateralOffsetAtEnd Length; the lateral offset of the design line of the new CrossSectionLink with respect to the
259 * design line of the parent Link at the end of the parent Link
260 * @param beginWidth Length; start width, positioned <i>symmetrically around</i> the design line
261 * @param endWidth Length; end width, positioned <i>symmetrically around</i> the design line
262 * @param laneType LaneType; the type of lane to deduce compatibility with GTU types
263 * @param speedLimitMap Map<GTUType, Speed>; speed limit on this lane, specified per GTU Type
264 * @throws OTSGeometryException when creation of the center line or contour geometry fails
265 * @throws NetworkException when id equal to null or not unique
266 */
267 @SuppressWarnings("checkstyle:parameternumber")
268 public Lane(final CrossSectionLink parentLink, final String id, final Length lateralOffsetAtStart,
269 final Length lateralOffsetAtEnd, final Length beginWidth, final Length endWidth, final LaneType laneType,
270 final Map<GTUType, Speed> speedLimitMap) throws OTSGeometryException, NetworkException
271 {
272 this(parentLink, id, lateralOffsetAtStart, lateralOffsetAtEnd, beginWidth, endWidth, laneType, speedLimitMap, false);
273 }
274
275 /**
276 * Construct a new Lane.
277 * @param parentLink CrossSectionLink; the link to which the element will belong (must be constructed first)
278 * @param id String; the id of this lane within the link; should be unique within the link.
279 * @param lateralOffsetAtStart Length; the lateral offset of the design line of the new CrossSectionLink with respect to the
280 * design line of the parent Link at the start of the parent Link
281 * @param lateralOffsetAtEnd Length; the lateral offset of the design line of the new CrossSectionLink with respect to the
282 * design line of the parent Link at the end of the parent Link
283 * @param beginWidth Length; start width, positioned <i>symmetrically around</i> the design line
284 * @param endWidth Length; end width, positioned <i>symmetrically around</i> the design line
285 * @param laneType LaneType; the type of lane to deduce compatibility with GTU types
286 * @param speedLimit Speed; speed limit on this lane
287 * @param fixGradualLateralOffset boolean; true if gradualLateralOffset needs to be fixed
288 * @throws OTSGeometryException when creation of the center line or contour geometry fails
289 * @throws NetworkException when id equal to null or not unique
290 */
291 @SuppressWarnings("checkstyle:parameternumber")
292 public Lane(final CrossSectionLink parentLink, final String id, final Length lateralOffsetAtStart,
293 final Length lateralOffsetAtEnd, final Length beginWidth, final Length endWidth, final LaneType laneType,
294 final Speed speedLimit, final boolean fixGradualLateralOffset) throws OTSGeometryException, NetworkException
295 {
296 super(parentLink, id, lateralOffsetAtStart, lateralOffsetAtEnd, beginWidth, endWidth, fixGradualLateralOffset);
297 this.laneType = laneType;
298 checkDirectionality();
299 this.speedLimitMap = new LinkedHashMap<>();
300 this.speedLimitMap.put(parentLink.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), speedLimit);
301 this.gtuList = new HistoricalArrayList<>(getManager(parentLink));
302 }
303
304 /**
305 * Construct a new Lane.
306 * @param parentLink CrossSectionLink; the link to which the element will belong (must be constructed first)
307 * @param id String; the id of this lane within the link; should be unique within the link.
308 * @param lateralOffsetAtStart Length; the lateral offset of the design line of the new CrossSectionLink with respect to the
309 * design line of the parent Link at the start of the parent Link
310 * @param lateralOffsetAtEnd Length; the lateral offset of the design line of the new CrossSectionLink with respect to the
311 * design line of the parent Link at the end of the parent Link
312 * @param beginWidth Length; start width, positioned <i>symmetrically around</i> the design line
313 * @param endWidth Length; end width, positioned <i>symmetrically around</i> the design line
314 * @param laneType LaneType; the type of lane to deduce compatibility with GTU types
315 * @param speedLimit Speed; speed limit on this lane
316 * @throws OTSGeometryException when creation of the center line or contour geometry fails
317 * @throws NetworkException when id equal to null or not unique
318 */
319 @SuppressWarnings("checkstyle:parameternumber")
320 public Lane(final CrossSectionLink parentLink, final String id, final Length lateralOffsetAtStart,
321 final Length lateralOffsetAtEnd, final Length beginWidth, final Length endWidth, final LaneType laneType,
322 final Speed speedLimit) throws OTSGeometryException, NetworkException
323 {
324 this(parentLink, id, lateralOffsetAtStart, lateralOffsetAtEnd, beginWidth, endWidth, laneType, speedLimit, false);
325 }
326
327 /**
328 * Construct a new Lane.
329 * @param parentLink CrossSectionLink; the link to which the element will belong (must be constructed first)
330 * @param id String; the id of this lane within the link; should be unique within the link.
331 * @param lateralOffset Length; the lateral offset of the design line of the new CrossSectionLink with respect to the design
332 * line of the parent Link
333 * @param width Length; width, positioned <i>symmetrically around</i> the design line
334 * @param laneType LaneType; type of lane to deduce compatibility with GTU types
335 * @param speedLimitMap Map<GTUType, Speed>; the speed limit on this lane, specified per GTU Type
336 * @throws OTSGeometryException when creation of the center line or contour geometry fails
337 * @throws NetworkException when id equal to null or not unique
338 */
339 @SuppressWarnings("checkstyle:parameternumber")
340 public Lane(final CrossSectionLink parentLink, final String id, final Length lateralOffset, final Length width,
341 final LaneType laneType, final Map<GTUType, Speed> speedLimitMap) throws OTSGeometryException, NetworkException
342 {
343 super(parentLink, id, lateralOffset, width);
344 this.laneType = laneType;
345 checkDirectionality();
346 this.speedLimitMap = speedLimitMap;
347 this.gtuList = new HistoricalArrayList<>(getManager(parentLink));
348 }
349
350 /**
351 * Construct a speed limit map that contains the provided speed limit for VEHICLE.
352 * @param speedLimit Speed; the speed limit
353 * @param network RoadNetwork; the road network (needed to obtain the VEHICLE GTU type)
354 * @return Map<GTUType, Speed>; the speed limit map
355 */
356 private static Map<GTUType, Speed> constructDefaultSpeedLimitMap(final Speed speedLimit, final RoadNetwork network)
357 {
358 Map<GTUType, Speed> result = new LinkedHashMap<>();
359 result.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), speedLimit);
360 return result;
361 }
362
363 /**
364 * Construct a new Lane.
365 * @param parentLink CrossSectionLink; the link to which the element belongs (must be constructed first)
366 * @param id String; the id of this lane within the link; should be unique within the link
367 * @param lateralOffset Length; the lateral offset of the design line of the new CrossSectionLink with respect to the design
368 * line of the parent Link
369 * @param width Length; width, positioned <i>symmetrically around</i> the design line
370 * @param laneType LaneType; the type of lane to deduce compatibility with GTU types
371 * @param speedLimit Speed; the speed limit on this lane
372 * @throws OTSGeometryException when creation of the center line or contour geometry fails
373 * @throws NetworkException when id equal to null or not unique
374 */
375 @SuppressWarnings("checkstyle:parameternumber")
376 public Lane(final CrossSectionLink parentLink, final String id, final Length lateralOffset, final Length width,
377 final LaneType laneType, final Speed speedLimit) throws OTSGeometryException, NetworkException
378 {
379 this(parentLink, id, lateralOffset, width, laneType,
380 constructDefaultSpeedLimitMap(speedLimit, parentLink.getNetwork()));
381 }
382
383 /**
384 * Construct a new Lane.
385 * @param parentLink CrossSectionLink; the link to which the element belongs (must be constructed first)
386 * @param id String; the id of this lane within the link; should be unique within the link.
387 * @param crossSectionSlices List<CrossSectionSlice>; the offsets and widths at positions along the line, relative to
388 * the design line of the parent link. If there is just one with and offset, there should just be one element in
389 * the list with Length = 0. If there are more slices, the last one should be at the length of the design line.
390 * If not, a NetworkException is thrown.
391 * @param laneType LaneType; the type of lane to deduce compatibility with GTU types
392 * @param speedLimitMap Map<GTUType, Speed>; the speed limit on this lane, specified per GTU Type
393 * @throws OTSGeometryException when creation of the center line or contour geometry fails
394 * @throws NetworkException when id equal to null or not unique
395 */
396 @SuppressWarnings("checkstyle:parameternumber")
397 public Lane(final CrossSectionLink parentLink, final String id, final List<CrossSectionSlice> crossSectionSlices,
398 final LaneType laneType, final Map<GTUType, Speed> speedLimitMap) throws OTSGeometryException, NetworkException
399 {
400 super(parentLink, id, crossSectionSlices);
401 this.laneType = laneType;
402 checkDirectionality();
403 this.speedLimitMap = speedLimitMap;
404 this.gtuList = new HistoricalArrayList<>(getManager(parentLink));
405 }
406
407 /**
408 * Construct a new Lane.
409 * @param parentLink CrossSectionLink; the link to which the element belongs (must be constructed first)
410 * @param id String; the id of this lane within the link; should be unique within the link.
411 * @param crossSectionSlices List<CrossSectionSlice>; the offsets and widths at positions along the line, relative to
412 * the design line of the parent link. If there is just one with and offset, there should just be one element in
413 * the list with Length = 0. If there are more slices, the last one should be at the length of the design line.
414 * If not, a NetworkException is thrown.
415 * @param laneType LaneType; the type of lane to deduce compatibility with GTU types
416 * @param speedLimit Speed; the speed limit on this lane
417 * @throws OTSGeometryException when creation of the center line or contour geometry fails
418 * @throws NetworkException when id equal to null or not unique
419 */
420 @SuppressWarnings("checkstyle:parameternumber")
421 public Lane(final CrossSectionLink parentLink, final String id, final List<CrossSectionSlice> crossSectionSlices,
422 final LaneType laneType, final Speed speedLimit) throws OTSGeometryException, NetworkException
423 {
424 this(parentLink, id, crossSectionSlices, laneType, constructDefaultSpeedLimitMap(speedLimit, parentLink.getNetwork()));
425 }
426
427 /**
428 * Clone a Lane for a new network.
429 * @param newParentLink CrossSectionLink; the new link to which the clone belongs
430 * @param cse Lane; the element to clone from
431 * @throws NetworkException if link already exists in the network, if name of the link is not unique, or if the start node
432 * or the end node of the link are not registered in the network.
433 */
434 protected Lane(final CrossSectionLink newParentLink, final Lane cse) throws NetworkException
435 {
436 super(newParentLink, newParentLink.getNetwork().getSimulator(), cse);
437 this.laneType = cse.laneType;
438 this.speedLimitMap = new LinkedHashMap<>(cse.speedLimitMap);
439 this.gtuList = new HistoricalArrayList<>(getManager(newParentLink));
440 }
441
442 /**
443 * Obtains the history manager from the parent link.
444 * @param parentLink CrossSectionLink; parent link
445 * @return HistoryManager; history manager
446 */
447 private HistoryManager getManager(final CrossSectionLink parentLink)
448 {
449 return parentLink.getSimulator().getReplication().getHistoryManager(parentLink.getSimulator());
450 }
451
452 // TODO constructor calls with this(...)
453
454 /**
455 * Retrieve one of the sets of neighboring Lanes that is accessible for the given type of GTU. A defensive copy of the
456 * internal data structure is returned.
457 * @param direction LateralDirectionality; either LEFT or RIGHT, relative to the DESIGN LINE of the link (and the direction
458 * of the center line of the lane). In terms of offsets, 'left' lanes always have a more positive offset than the
459 * current lane, and 'right' lanes a more negative offset.
460 * @param gtuType GTUType; the GTU type to check the accessibility for
461 * @param drivingDirection GTUDirectionality; driving direction of the GTU
462 * @param legal boolean; whether to check legal possibility
463 * @return Set<Lane>; the indicated set of neighboring Lanes
464 */
465 private Set<Lane> neighbors(final LateralDirectionality direction, final GTUType gtuType,
466 final GTUDirectionality drivingDirection, final boolean legal)
467 {
468 MultiKeyMap<Set<Lane>> cache = direction.isLeft() ? this.leftNeighbours : this.rightNeighbours;
469 return cache.get(() ->
470 {
471 Set<Lane> lanes = new LinkedHashSet<>(1);
472 for (CrossSectionElement cse : this.parentLink.getCrossSectionElementList())
473 {
474 if (cse instanceof Lane && !cse.equals(this))
475 {
476 Lane lane = (Lane) cse;
477 if (laterallyAdjacentAndAccessible(lane, direction, gtuType, drivingDirection, legal))
478 {
479 lanes.add(lane);
480 }
481 }
482 }
483 return lanes;
484 }, gtuType, drivingDirection, legal);
485 }
486
487 /** Lateral alignment margin for longitudinally connected Lanes. */
488 static final Length ADJACENT_MARGIN = new Length(0.2, LengthUnit.METER);
489
490 /**
491 * Determine whether another lane is adjacent to this lane (dependent on distance) and accessible (dependent on stripes) for
492 * a certain GTU type (dependent on usability of the adjacent lane for that GTU type). This method assumes that when there
493 * is NO stripe between two adjacent lanes that are accessible for the GTU type, the GTU can enter that lane. <br>
494 * @param lane Lane; the other lane to evaluate
495 * @param direction LateralDirectionality; the direction to look at, relative to the DESIGN LINE of the link. This is a very
496 * important aspect to note: all information is stored relative to the direction of the design line, and not in a
497 * driving direction, which can vary for lanes that can be driven in two directions (e.g. at overtaking).
498 * @param gtuType GTUType; the GTU type to check the accessibility for
499 * @param drivingDirection GTUDirectionality; driving direction of the GTU
500 * @param legal boolean; whether to check legal possibility
501 * @return boolean; true if the other lane is adjacent to this lane and accessible for the given GTU type; false otherwise
502 */
503 private boolean laterallyAdjacentAndAccessible(final Lane lane, final LateralDirectionality direction,
504 final GTUType gtuType, final GTUDirectionality drivingDirection, final boolean legal)
505 {
506 if (!lane.getLaneType().isCompatible(gtuType, drivingDirection))
507 {
508 // not accessible for the given GTU type
509 return false;
510 }
511
512 if (direction.equals(LateralDirectionality.LEFT))
513 {
514 // TODO take the cross section slices into account...
515 if (lane.getDesignLineOffsetAtBegin().si + ADJACENT_MARGIN.si > getDesignLineOffsetAtBegin().si
516 && lane.getDesignLineOffsetAtEnd().si + ADJACENT_MARGIN.si > getDesignLineOffsetAtEnd().si
517 && (lane.getDesignLineOffsetAtBegin().si - lane.getBeginWidth().si / 2.0)
518 - (getDesignLineOffsetAtBegin().si + getBeginWidth().si / 2.0) < ADJACENT_MARGIN.si
519 && (lane.getDesignLineOffsetAtEnd().si - lane.getEndWidth().si / 2.0)
520 - (getDesignLineOffsetAtEnd().si + getEndWidth().si / 2.0) < ADJACENT_MARGIN.si)
521 {
522 // look at stripes between the two lanes
523 if (legal)
524 {
525 for (CrossSectionElement cse : this.parentLink.getCrossSectionElementList())
526 {
527 if (cse instanceof Stripe)
528 {
529 Stripe stripe = (Stripe) cse;
530 // TODO take the cross section slices into account...
531 if ((getDesignLineOffsetAtBegin().si < stripe.getDesignLineOffsetAtBegin().si
532 && stripe.getDesignLineOffsetAtBegin().si < lane.getDesignLineOffsetAtBegin().si)
533 || (getDesignLineOffsetAtEnd().si < stripe.getDesignLineOffsetAtEnd().si
534 && stripe.getDesignLineOffsetAtEnd().si < lane.getDesignLineOffsetAtEnd().si))
535 {
536 if (!stripe.isPermeable(gtuType, LateralDirectionality.LEFT))
537 {
538 // there is a stripe forbidding to cross to the adjacent lane
539 return false;
540 }
541 }
542 }
543 }
544 }
545 // the lanes are adjacent, and there is no stripe forbidding us to enter that lane
546 // or there is no stripe at all
547 return true;
548 }
549 }
550
551 else
552 // direction.equals(LateralDirectionality.RIGHT)
553 {
554 // TODO take the cross section slices into account...
555 if (lane.getDesignLineOffsetAtBegin().si < getDesignLineOffsetAtBegin().si + ADJACENT_MARGIN.si
556 && lane.getDesignLineOffsetAtEnd().si < getDesignLineOffsetAtEnd().si + ADJACENT_MARGIN.si
557 && (getDesignLineOffsetAtBegin().si - getBeginWidth().si / 2.0)
558 - (lane.getDesignLineOffsetAtBegin().si + lane.getBeginWidth().si / 2.0) < ADJACENT_MARGIN.si
559 && (getDesignLineOffsetAtEnd().si - getEndWidth().si / 2.0)
560 - (lane.getDesignLineOffsetAtEnd().si + lane.getEndWidth().si / 2.0) < ADJACENT_MARGIN.si)
561 {
562 // look at stripes between the two lanes
563 if (legal)
564 {
565 for (CrossSectionElement cse : this.parentLink.getCrossSectionElementList())
566 {
567 if (cse instanceof Stripe)
568 {
569 Stripe stripe = (Stripe) cse;
570 // TODO take the cross section slices into account...
571 if ((getDesignLineOffsetAtBegin().si > stripe.getDesignLineOffsetAtBegin().si
572 && stripe.getDesignLineOffsetAtBegin().si > lane.getDesignLineOffsetAtBegin().si)
573 || (getDesignLineOffsetAtEnd().si > stripe.getDesignLineOffsetAtEnd().si
574 && stripe.getDesignLineOffsetAtEnd().si > lane.getDesignLineOffsetAtEnd().si))
575 {
576 if (!stripe.isPermeable(gtuType, LateralDirectionality.RIGHT))
577 {
578 // there is a stripe forbidding to cross to the adjacent lane
579 return false;
580 }
581 }
582 }
583 }
584 }
585 // the lanes are adjacent, and there is no stripe forbidding us to enter that lane
586 // or there is no stripe at all
587 return true;
588 }
589 }
590
591 // no lanes were found that are close enough laterally.
592 return false;
593 }
594
595 /**
596 * Insert a sensor at the right place in the sensor list of this Lane.
597 * @param sensor SingleSensor; the sensor to add
598 * @throws NetworkException when the position of the sensor is beyond (or before) the range of this Lane
599 */
600 public final void addSensor(final SingleSensor sensor) throws NetworkException
601 {
602 double position = sensor.getLongitudinalPosition().si;
603 if (position < 0 || position > getLength().getSI())
604 {
605 throw new NetworkException("Illegal position for sensor " + position + " valid range is 0.." + getLength().getSI());
606 }
607 if (this.parentLink.getNetwork().containsObject(sensor.getFullId()))
608 {
609 throw new NetworkException("Network already contains an object with the name " + sensor.getFullId());
610 }
611 List<SingleSensor> sensorList = this.sensors.get(position);
612 if (null == sensorList)
613 {
614 sensorList = new ArrayList<>(1);
615 this.sensors.put(position, sensorList);
616 }
617 sensorList.add(sensor);
618 this.parentLink.getNetwork().addObject(sensor);
619 fireTimedEvent(Lane.SENSOR_ADD_EVENT, new Object[] {sensor.getId(), sensor}, sensor.getSimulator().getSimulatorTime());
620 }
621
622 /**
623 * Remove a sensor from the sensor list of this Lane.
624 * @param sensor SingleSensor; the sensor to remove.
625 * @throws NetworkException when the sensor was not found on this Lane
626 */
627 public final void removeSensor(final SingleSensor sensor) throws NetworkException
628 {
629 fireTimedEvent(Lane.SENSOR_REMOVE_EVENT, new Object[] {sensor.getId(), sensor},
630 sensor.getSimulator().getSimulatorTime());
631 List<SingleSensor> sensorList = this.sensors.get(sensor.getLongitudinalPosition().si);
632 if (null == sensorList)
633 {
634 throw new NetworkException("No sensor at " + sensor.getLongitudinalPosition().si);
635 }
636 sensorList.remove(sensor);
637 if (sensorList.size() == 0)
638 {
639 this.sensors.remove(sensor.getLongitudinalPosition().si);
640 }
641 this.parentLink.getNetwork().removeObject(sensor);
642 }
643
644 /**
645 * Retrieve the list of Sensors of this Lane in the specified distance range for the given GTUType. The resulting list is a
646 * defensive copy.
647 * @param minimumPosition Length; the minimum distance on the Lane (inclusive)
648 * @param maximumPosition Length; the maximum distance on the Lane (inclusive)
649 * @param gtuType GTUType; the GTU type to provide the sensors for
650 * @param direction GTUDirectionality; direction of movement of the GTU
651 * @return List<Sensor>; list of the sensor in the specified range. This is a defensive copy.
652 */
653 public final List<SingleSensor> getSensors(final Length minimumPosition, final Length maximumPosition,
654 final GTUType gtuType, final GTUDirectionality direction)
655 {
656 List<SingleSensor> sensorList = new ArrayList<>(1);
657 for (List<SingleSensor> sl : this.sensors.values())
658 {
659 for (SingleSensor sensor : sl)
660 {
661 if (sensor.isCompatible(gtuType, direction) && sensor.getLongitudinalPosition().ge(minimumPosition)
662 && sensor.getLongitudinalPosition().le(maximumPosition))
663 {
664 sensorList.add(sensor);
665 }
666 }
667 }
668 return sensorList;
669 }
670
671 /**
672 * Retrieve the list of Sensors of this Lane that are triggered by the given GTUType. The resulting list is a defensive
673 * copy.
674 * @param gtuType GTUType; the GTU type to provide the sensors for
675 * @param direction GTUDirectionality; direction of movement of the GTU
676 * @return List<Sensor>; list of the sensors, in ascending order for the location on the Lane
677 */
678 public final List<SingleSensor> getSensors(final GTUType gtuType, final GTUDirectionality direction)
679 {
680 List<SingleSensor> sensorList = new ArrayList<>(1);
681 for (List<SingleSensor> sl : this.sensors.values())
682 {
683 for (SingleSensor sensor : sl)
684 {
685 if (sensor.isCompatible(gtuType, direction))
686 {
687 sensorList.add(sensor);
688 }
689 }
690 }
691 return sensorList;
692 }
693
694 /**
695 * Retrieve the list of all Sensors of this Lane. The resulting list is a defensive copy.
696 * @return List<Sensor>; list of the sensors, in ascending order for the location on the Lane
697 */
698 public final List<SingleSensor> getSensors()
699 {
700 if (this.sensors == null)
701 {
702 return new ArrayList<>();
703 }
704 List<SingleSensor> sensorList = new ArrayList<>(1);
705 for (List<SingleSensor> sl : this.sensors.values())
706 {
707 for (SingleSensor sensor : sl)
708 {
709 sensorList.add(sensor);
710 }
711 }
712 return sensorList;
713 }
714
715 /**
716 * Retrieve the list of Sensors of this Lane for the given GTUType. The resulting Map is a defensive copy.
717 * @param gtuType GTUType; the GTU type to provide the sensors for
718 * @param direction GTUDirectionality; direction of movement of the GTU
719 * @return SortedMap<Double, List<Sensor>>; all sensors on this lane for the given GTUType as a map per distance
720 */
721 public final SortedMap<Double, List<SingleSensor>> getSensorMap(final GTUType gtuType, final GTUDirectionality direction)
722 {
723 SortedMap<Double, List<SingleSensor>> sensorMap = new TreeMap<>();
724 for (double d : this.sensors.keySet())
725 {
726 List<SingleSensor> sensorList = new ArrayList<>(1);
727 for (List<SingleSensor> sl : this.sensors.values())
728 {
729 for (SingleSensor sensor : sl)
730 {
731 if (sensor.getLongitudinalPosition().si == d && sensor.isCompatible(gtuType, direction))
732 {
733 sensorList.add(sensor);
734 }
735 }
736 }
737 if (sensorList.size() > 0)
738 {
739 sensorMap.put(d, sensorList);
740 }
741 }
742 // System.out.println("getSensorMap returns");
743 // for (Double key : sensorMap.keySet())
744 // {
745 // System.out.println("\t" + key + " -> " + (sensorMap.get(key).size()) + " sensors");
746 // for (Sensor s : sensorMap.get(key))
747 // {
748 // System.out.println("\t\t" + s);
749 // }
750 // }
751 return sensorMap;
752 }
753
754 /**
755 * Schedule triggering of the sensors for a certain time step; from now until the nextEvaluationTime of the GTU.
756 * @param gtu LaneBasedGTU; the lane based GTU for which to schedule triggering of the sensors.
757 * @param referenceStartSI double; the SI distance of the GTU reference point on the lane at the current time
758 * @param referenceMoveSI double; the SI distance traveled in the next time step.
759 * @throws NetworkException when GTU not on this lane.
760 * @throws SimRuntimeException when method cannot be scheduled.
761 */
762 public final void scheduleSensorTriggers(final LaneBasedGTU gtu, final double referenceStartSI,
763 final double referenceMoveSI) throws NetworkException, SimRuntimeException
764 {
765 GTUDirectionality drivingDirection;
766 double minPos;
767 double maxPos;
768 if (referenceMoveSI >= 0)
769 {
770 drivingDirection = GTUDirectionality.DIR_PLUS;
771 minPos = referenceStartSI + gtu.getRear().getDx().si;
772 maxPos = referenceStartSI + gtu.getFront().getDx().si + referenceMoveSI;
773 }
774 else
775 {
776 drivingDirection = GTUDirectionality.DIR_MINUS;
777 minPos = referenceStartSI - gtu.getFront().getDx().si + referenceMoveSI;
778 maxPos = referenceStartSI - gtu.getRear().getDx().si;
779 }
780 Map<Double, List<SingleSensor>> map = this.sensors.subMap(minPos, maxPos);
781 for (double pos : map.keySet())
782 {
783 for (SingleSensor sensor : map.get(pos))
784 {
785 if (sensor.isCompatible(gtu.getGTUType(), drivingDirection))
786 {
787 double dx = gtu.getRelativePositions().get(sensor.getPositionType()).getDx().si;
788 if (drivingDirection.isPlus())
789 {
790 minPos = referenceStartSI + dx;
791 maxPos = minPos + referenceMoveSI;
792 }
793 else
794 {
795 maxPos = referenceStartSI - dx;
796 minPos = maxPos + referenceMoveSI;
797 }
798 if (minPos <= sensor.getLongitudinalPosition().si && maxPos > sensor.getLongitudinalPosition().si)
799 {
800 double d = drivingDirection.isPlus() ? sensor.getLongitudinalPosition().si - minPos
801 : maxPos - sensor.getLongitudinalPosition().si;
802 if (d < 0)
803 {
804 throw new NetworkException("scheduleTriggers for gtu: " + gtu + ", d<0 d=" + d);
805 }
806 OperationalPlan oPlan = gtu.getOperationalPlan();
807 Time triggerTime = oPlan.timeAtDistance(Length.instantiateSI(d));
808 if (triggerTime.gt(oPlan.getEndTime()))
809 {
810 System.err.println("Time=" + gtu.getSimulator().getSimulatorTime().getSI()
811 + " - Scheduling trigger at " + triggerTime.getSI() + "s. > " + oPlan.getEndTime().getSI()
812 + "s. (nextEvalTime) for sensor " + sensor + " , gtu " + gtu);
813 System.err.println(" v=" + gtu.getSpeed() + ", a=" + gtu.getAcceleration() + ", lane=" + toString()
814 + ", refStartSI=" + referenceStartSI + ", moveSI=" + referenceMoveSI);
815 triggerTime = new Time(oPlan.getEndTime().getSI() - Math.ulp(oPlan.getEndTime().getSI()),
816 TimeUnit.DEFAULT);
817 }
818 SimEvent<Duration> event =
819 new SimEvent<>(new Duration(triggerTime.minus(gtu.getSimulator().getStartTimeAbs())), this,
820 sensor, "trigger", new Object[] {gtu});
821 gtu.getSimulator().scheduleEvent(event);
822 gtu.addTrigger(this, event);
823 }
824 else if (sensor.getLongitudinalPosition().si < minPos
825 && (sensor instanceof SinkSensor || sensor instanceof DestinationSensor))
826 {
827 // TODO this is a hack for when sink sensors aren't perfectly adjacent or the GTU overshoots with nose
828 // due to curvature
829 SimEvent<Duration> event = new SimEvent<>(new Duration(gtu.getSimulator().getSimulatorTime()), this,
830 sensor, "trigger", new Object[] {gtu});
831 gtu.getSimulator().scheduleEvent(event);
832 gtu.addTrigger(this, event);
833 }
834 }
835 }
836 }
837 }
838
839 /**
840 * Insert a laneBasedObject at the right place in the laneBasedObject list of this Lane. Register it in the network WITH the
841 * Lane id.
842 * @param laneBasedObject LaneBasedObject; the laneBasedObject to add
843 * @throws NetworkException when the position of the laneBasedObject is beyond (or before) the range of this Lane
844 */
845 public final synchronized void addLaneBasedObject(final LaneBasedObject laneBasedObject) throws NetworkException
846 {
847 double position = laneBasedObject.getLongitudinalPosition().si;
848 if (position < 0 || position > getLength().getSI())
849 {
850 throw new NetworkException(
851 "Illegal position for laneBasedObject " + position + " valid range is 0.." + getLength().getSI());
852 }
853 if (this.parentLink.getNetwork().containsObject(laneBasedObject.getFullId()))
854 {
855 throw new NetworkException("Network already contains an object with the name " + laneBasedObject.getFullId());
856 }
857 List<LaneBasedObject> laneBasedObjectList = this.laneBasedObjects.get(position);
858 if (null == laneBasedObjectList)
859 {
860 laneBasedObjectList = new ArrayList<>(1);
861 this.laneBasedObjects.put(position, laneBasedObjectList);
862 }
863 laneBasedObjectList.add(laneBasedObject);
864 this.parentLink.getNetwork().addObject(laneBasedObject);
865 fireTimedEvent(Lane.OBJECT_ADD_EVENT, new Object[] {laneBasedObject},
866 getParentLink().getSimulator().getSimulatorTime());
867 }
868
869 /**
870 * Remove a laneBasedObject from the laneBasedObject list of this Lane.
871 * @param laneBasedObject LaneBasedObject; the laneBasedObject to remove.
872 * @throws NetworkException when the laneBasedObject was not found on this Lane
873 */
874 public final synchronized void removeLaneBasedObject(final LaneBasedObject laneBasedObject) throws NetworkException
875 {
876 fireTimedEvent(Lane.OBJECT_REMOVE_EVENT, new Object[] {laneBasedObject},
877 getParentLink().getSimulator().getSimulatorTime());
878 List<LaneBasedObject> laneBasedObjectList =
879 this.laneBasedObjects.get(laneBasedObject.getLongitudinalPosition().getSI());
880 if (null == laneBasedObjectList)
881 {
882 throw new NetworkException("No laneBasedObject at " + laneBasedObject.getLongitudinalPosition().si);
883 }
884 laneBasedObjectList.remove(laneBasedObject);
885 if (laneBasedObjectList.isEmpty())
886 {
887 this.laneBasedObjects.remove(laneBasedObject.getLongitudinalPosition().doubleValue());
888 }
889 this.parentLink.getNetwork().removeObject(laneBasedObject);
890 }
891
892 /**
893 * Retrieve the list of LaneBasedObjects of this Lane in the specified distance range. The resulting list is a defensive
894 * copy.
895 * @param minimumPosition Length; the minimum distance on the Lane (inclusive)
896 * @param maximumPosition Length; the maximum distance on the Lane (inclusive)
897 * @return List<LaneBasedObject>; list of the laneBasedObject in the specified range. This is a defensive copy.
898 */
899 public final List<LaneBasedObject> getLaneBasedObjects(final Length minimumPosition, final Length maximumPosition)
900 {
901 List<LaneBasedObject> laneBasedObjectList = new ArrayList<>(1);
902 for (List<LaneBasedObject> lbol : this.laneBasedObjects.values())
903 {
904 for (LaneBasedObject lbo : lbol)
905 {
906 if (lbo.getLongitudinalPosition().ge(minimumPosition) && lbo.getLongitudinalPosition().le(maximumPosition))
907 {
908 laneBasedObjectList.add(lbo);
909 }
910 }
911 }
912 return laneBasedObjectList;
913 }
914
915 /**
916 * Retrieve the list of all LaneBasedObjects of this Lane. The resulting list is a defensive copy.
917 * @return List<LaneBasedObject>; list of the laneBasedObjects, in ascending order for the location on the Lane
918 */
919 public final List<LaneBasedObject> getLaneBasedObjects()
920 {
921 if (this.laneBasedObjects == null)
922 {
923 return new ArrayList<>();
924 }
925 List<LaneBasedObject> laneBasedObjectList = new ArrayList<>(1);
926 for (List<LaneBasedObject> lbol : this.laneBasedObjects.values())
927 {
928 for (LaneBasedObject lbo : lbol)
929 {
930 laneBasedObjectList.add(lbo);
931 }
932 }
933 return laneBasedObjectList;
934 }
935
936 /**
937 * Retrieve the list of LaneBasedObjects of this Lane. The resulting Map is a defensive copy.
938 * @return SortedMap<Double, List<LaneBasedObject>>; all laneBasedObjects on this lane
939 */
940 public final SortedMap<Double, List<LaneBasedObject>> getLaneBasedObjectMap()
941 {
942 SortedMap<Double, List<LaneBasedObject>> laneBasedObjectMap = new TreeMap<>();
943 for (double d : this.laneBasedObjects.keySet())
944 {
945 List<LaneBasedObject> laneBasedObjectList = new ArrayList<>(1);
946 for (LaneBasedObject lbo : this.laneBasedObjects.get(d))
947 {
948 laneBasedObjectList.add(lbo);
949 }
950 laneBasedObjectMap.put(d, laneBasedObjectList);
951 }
952 return laneBasedObjectMap;
953 }
954
955 /**
956 * Transform a fraction on the lane to a relative length (can be less than zero or larger than the lane length).
957 * @param fraction double; fraction relative to the lane length.
958 * @return Length; the longitudinal length corresponding to the fraction.
959 */
960 public final Length position(final double fraction)
961 {
962 if (this.length.getDisplayUnit().isBaseSIUnit())
963 {
964 return new Length(this.length.si * fraction, LengthUnit.SI);
965 }
966 return new Length(this.length.getInUnit() * fraction, this.length.getDisplayUnit());
967 }
968
969 /**
970 * Transform a fraction on the lane to a relative length in SI units (can be less than zero or larger than the lane length).
971 * @param fraction double; fraction relative to the lane length.
972 * @return double; length corresponding to the fraction, in SI units.
973 */
974 public final double positionSI(final double fraction)
975 {
976 return this.length.si * fraction;
977 }
978
979 /**
980 * Transform a position on the lane (can be less than zero or larger than the lane length) to a fraction.
981 * @param position Length; relative length on the lane (may be less than zero or larger than the lane length).
982 * @return fraction double; fraction relative to the lane length.
983 */
984 public final double fraction(final Length position)
985 {
986 return position.si / this.length.si;
987 }
988
989 /**
990 * Transform a position on the lane in SI units (can be less than zero or larger than the lane length) to a fraction.
991 * @param positionSI double; relative length on the lane in SI units (may be less than zero or larger than the lane length).
992 * @return double; fraction relative to the lane length.
993 */
994 public final double fractionSI(final double positionSI)
995 {
996 return positionSI / this.length.si;
997 }
998
999 /**
1000 * Add a LaneBasedGTU to the list of this Lane.
1001 * @param gtu LaneBasedGTU; the GTU to add
1002 * @param fractionalPosition double; the fractional position that the newly added GTU will have on this Lane
1003 * @return int; the rank that the newly added GTU has on this Lane (should be 0, except when the GTU enters this Lane due to
1004 * a lane change operation)
1005 * @throws GTUException when the fractionalPosition is outside the range 0..1, or the GTU is already registered on this Lane
1006 */
1007 public final int addGTU(final LaneBasedGTU gtu, final double fractionalPosition) throws GTUException
1008 {
1009 // TODO: should this change when we drive in the opposite direction?
1010 int index;
1011 // check if we are the first
1012 if (this.gtuList.size() == 0)
1013 {
1014 this.gtuList.add(gtu);
1015 index = 0;
1016 }
1017 else
1018 {
1019 /*-
1020 // check if we can add at the front
1021 LaneBasedGTU lastGTU = this.gtuList.get(this.gtuList.size() - 1);
1022 if (fractionalPosition < lastGTU.fractionalPosition(this, lastGTU.getFront()))
1023 {
1024 // this.gtuList.add(gtu); // XXX: AV 20190113
1025 // index = this.gtuList.size() - 1; // XXX: AV 20190113
1026 this.gtuList.add(0, gtu);
1027 index = 0;
1028 }
1029 else
1030 */
1031 {
1032 // figure out the rank for the new GTU
1033 for (index = 0; index < this.gtuList.size(); index++)
1034 {
1035 LaneBasedGTU otherGTU = this.gtuList.get(index);
1036 if (gtu == otherGTU)
1037 {
1038 throw new GTUException(gtu + " already registered on Lane " + this + " [registered lanes: "
1039 + gtu.positions(gtu.getFront()).keySet() + "] locations: "
1040 + gtu.positions(gtu.getFront()).values() + " time: " + gtu.getSimulator().getSimulatorTime());
1041 }
1042 if (otherGTU.fractionalPosition(this, otherGTU.getFront()) >= fractionalPosition)
1043 {
1044 break;
1045 }
1046 }
1047 this.gtuList.add(index, gtu);
1048 /*-
1049 for (int i = 0; i < this.gtuList.size(); i++)
1050 {
1051 LaneBasedGTU gtui = this.gtuList.get(i);
1052 System.out.println(i + ": GTU." + gtui.getId() + " at pos: " + gtui.position(this, gtui.getFront()));
1053 }
1054 System.out.println();
1055 */
1056 }
1057 }
1058 // fireTimedEvent(Lane.GTU_ADD_EVENT, new Object[] { gtu.getId(), gtu, this.gtuList.size() },
1059 // gtu.getSimulator().getSimulatorTime());
1060 fireTimedEvent(Lane.GTU_ADD_EVENT, new Object[] {gtu.getId(), this.gtuList.size()},
1061 gtu.getSimulator().getSimulatorTime());
1062 getParentLink().addGTU(gtu);
1063 return index;
1064 }
1065
1066 /**
1067 * Add a LaneBasedGTU to the list of this Lane.
1068 * @param gtu LaneBasedGTU; the GTU to add
1069 * @param longitudinalPosition Length; the longitudinal position that the newly added GTU will have on this Lane
1070 * @return int; the rank that the newly added GTU has on this Lane (should be 0, except when the GTU enters this Lane due to
1071 * a lane change operation)
1072 * @throws GTUException when longitudinalPosition is negative or exceeds the length of this Lane
1073 */
1074 public final int addGTU(final LaneBasedGTU gtu, final Length longitudinalPosition) throws GTUException
1075 {
1076 return addGTU(gtu, longitudinalPosition.getSI() / getLength().getSI());
1077 }
1078
1079 /**
1080 * Remove a GTU from the GTU list of this lane.
1081 * @param gtu LaneBasedGTU; the GTU to remove.
1082 * @param removeFromParentLink boolean; when the GTU leaves the last lane of the parentLink of this Lane
1083 * @param position Length; last position of the GTU
1084 */
1085 public final void removeGTU(final LaneBasedGTU gtu, final boolean removeFromParentLink, final Length position)
1086 {
1087 boolean contained = this.gtuList.remove(gtu);
1088 if (contained)
1089 {
1090 fireTimedEvent(Lane.GTU_REMOVE_EVENT, new Object[] {gtu.getId(), gtu, this.gtuList.size(), position},
1091 gtu.getSimulator().getSimulatorTime());
1092 }
1093 if (removeFromParentLink)
1094 {
1095 this.parentLink.removeGTU(gtu);
1096 }
1097 }
1098
1099 /**
1100 * Get the last GTU on the lane, relative to a driving direction on this lane.
1101 * @param direction GTUDirectionality; whether we are looking in the the design line direction or against the center line
1102 * direction.
1103 * @return LaneBasedGTU; the last GTU on this lane in the given direction, or null if no GTU could be found.
1104 * @throws GTUException when there is a problem with the position of the GTUs on the lane.
1105 */
1106 public final LaneBasedGTU getLastGtu(final GTUDirectionality direction) throws GTUException
1107 {
1108 if (this.gtuList.size() == 0)
1109 {
1110 return null;
1111 }
1112 if (direction.equals(GTUDirectionality.DIR_PLUS))
1113 {
1114 return this.gtuList.get(this.gtuList.size() - 1);
1115 }
1116 else
1117 {
1118 return this.gtuList.get(0);
1119 }
1120 }
1121
1122 /**
1123 * Get the first GTU on the lane, relative to a driving direction on this lane.
1124 * @param direction GTUDirectionality; whether we are looking in the the design line direction or against the center line
1125 * direction.
1126 * @return LaneBasedGTU; the first GTU on this lane in the given direction, or null if no GTU could be found.
1127 * @throws GTUException when there is a problem with the position of the GTUs on the lane.
1128 */
1129 public final LaneBasedGTU getFirstGtu(final GTUDirectionality direction) throws GTUException
1130 {
1131 if (this.gtuList.size() == 0)
1132 {
1133 return null;
1134 }
1135 if (direction.equals(GTUDirectionality.DIR_PLUS))
1136 {
1137 return this.gtuList.get(0);
1138 }
1139 else
1140 {
1141 return this.gtuList.get(this.gtuList.size() - 1);
1142 }
1143 }
1144
1145 /**
1146 * Get the first GTU where the relativePosition is in front of another GTU on the lane, in a driving direction on this lane,
1147 * compared to the DESIGN LINE.
1148 * @param position Length; the position before which the relative position of a GTU will be searched.
1149 * @param direction GTUDirectionality; whether we are looking in the the center line direction or against the center line
1150 * direction.
1151 * @param relativePosition RelativePosition.TYPE; the relative position we want to compare against
1152 * @param when Time; the time for which to evaluate the positions.
1153 * @return LaneBasedGTU; the first GTU before a position on this lane in the given direction, or null if no GTU could be
1154 * found.
1155 * @throws GTUException when there is a problem with the position of the GTUs on the lane.
1156 */
1157 public final LaneBasedGTU getGtuAhead(final Length position, final GTUDirectionality direction,
1158 final RelativePosition.TYPE relativePosition, final Time when) throws GTUException
1159 {
1160 List<LaneBasedGTU> list = this.gtuList.get(when);
1161 if (list.isEmpty())
1162 {
1163 return null;
1164 }
1165 int[] search = lineSearch((
1166 final int index
1167 ) ->
1168 {
1169 LaneBasedGTU gtu = list.get(index);
1170 return gtu.position(this, gtu.getRelativePositions().get(relativePosition), when).si;
1171 }, list.size(), position.si);
1172 if (direction.equals(GTUDirectionality.DIR_PLUS))
1173 {
1174 if (search[1] < list.size())
1175 {
1176 return list.get(search[1]);
1177 }
1178 }
1179 else
1180 {
1181 if (search[0] >= 0)
1182 {
1183 return list.get(search[0]);
1184 }
1185 }
1186 return null;
1187 }
1188
1189 /**
1190 * Searches for objects just before and after a given position.
1191 * @param positions Positions; functional interface returning positions at indices
1192 * @param listSize int; number of objects in the underlying list
1193 * @param position double; position
1194 * @return int[2]; Where int[0] is the index of the object with lower position, and int[1] with higher. In case an object is
1195 * exactly at the position int[1] - int[0] = 2. If all objects have a higher position int[0] = -1, if all objects
1196 * have a lower position int[1] = listSize.
1197 * @throws GTUException ...
1198 */
1199 private int[] lineSearch(final Positions positions, final int listSize, final double position) throws GTUException
1200 {
1201 int[] out = new int[2];
1202 // line search only works if the position is within the original domain, first catch 4 outside situations
1203 double pos0 = positions.get(0);
1204 double posEnd;
1205 if (position < pos0)
1206 {
1207 out[0] = -1;
1208 out[1] = 0;
1209 }
1210 else if (position == pos0)
1211 {
1212 out[0] = -1;
1213 out[1] = 1;
1214 }
1215 else if (position > (posEnd = positions.get(listSize - 1)))
1216 {
1217 out[0] = listSize - 1;
1218 out[1] = listSize;
1219 }
1220 else if (position == posEnd)
1221 {
1222 out[0] = listSize - 2;
1223 out[1] = listSize;
1224 }
1225 else
1226 {
1227 int low = 0;
1228 int mid = (int) ((listSize - 1) * position / this.length.si);
1229 mid = mid < 0 ? 0 : mid >= listSize ? listSize - 1 : mid;
1230 int high = listSize - 1;
1231 while (high - low > 1)
1232 {
1233 double midPos = positions.get(mid);
1234 if (midPos < position)
1235 {
1236 low = mid;
1237 }
1238 else if (midPos > position)
1239 {
1240 high = mid;
1241 }
1242 else
1243 {
1244 low = mid - 1;
1245 high = mid + 1;
1246 break;
1247 }
1248 mid = (low + high) / 2;
1249 }
1250 out[0] = low;
1251 out[1] = high;
1252 }
1253 return out;
1254 }
1255
1256 /**
1257 * Get the first object where the relativePosition is in front of a certain position on the lane, in a driving direction on
1258 * this lane, compared to the DESIGN LINE. Perception should iterate over results from this method to see what is most
1259 * limiting.
1260 * @param position Length; the position after which the relative position of an object will be searched.
1261 * @param direction GTUDirectionality; whether we are looking in the the center line direction or against the center line
1262 * direction.
1263 * @return List<LaneBasedObject>; the first object(s) before a position on this lane in the given direction, or null
1264 * if no object could be found.
1265 */
1266 public final List<LaneBasedObject> getObjectAhead(final Length position, final GTUDirectionality direction)
1267 {
1268 if (direction.equals(GTUDirectionality.DIR_PLUS))
1269 {
1270 for (double distance : this.laneBasedObjects.keySet())
1271 {
1272 if (distance > position.si)
1273 {
1274 return new ArrayList<>(this.laneBasedObjects.get(distance));
1275 }
1276 }
1277 }
1278 else
1279 {
1280 NavigableMap<Double, List<LaneBasedObject>> reverseLBO =
1281 (NavigableMap<Double, List<LaneBasedObject>>) this.laneBasedObjects;
1282 for (double distance : reverseLBO.descendingKeySet())
1283 {
1284 if (distance < position.si)
1285 {
1286 return new ArrayList<>(this.laneBasedObjects.get(distance));
1287 }
1288 }
1289 }
1290 return null;
1291 }
1292
1293 /**
1294 * Get the first object where the relativePosition is behind of a certain position on the lane, in a driving direction on
1295 * this lane, compared to the DESIGN LINE. Perception should iterate over results from this method to see what is most
1296 * limiting.
1297 * @param position Length; the position after which the relative position of an object will be searched.
1298 * @param direction GTUDirectionality; whether we are looking in the the center line direction or against the center line
1299 * direction.
1300 * @return List<LaneBasedObject>; the first object(s) after a position on this lane in the given direction, or null if
1301 * no object could be found.
1302 */
1303 public final List<LaneBasedObject> getObjectBehind(final Length position, final GTUDirectionality direction)
1304 {
1305 if (direction.equals(GTUDirectionality.DIR_PLUS))
1306 {
1307 return getObjectAhead(position, GTUDirectionality.DIR_MINUS);
1308 }
1309 return getObjectAhead(position, GTUDirectionality.DIR_PLUS);
1310 }
1311
1312 /**
1313 * Get the first GTU where the relativePosition is behind a certain position on the lane, in a driving direction on this
1314 * lane, compared to the DESIGN LINE.
1315 * @param position Length; the position before which the relative position of a GTU will be searched.
1316 * @param direction GTUDirectionality; whether we are looking in the the center line direction or against the center line
1317 * direction.
1318 * @param relativePosition RelativePosition.TYPE; the relative position of the GTU we are looking for.
1319 * @param when Time; the time for which to evaluate the positions.
1320 * @return LaneBasedGTU; the first GTU after a position on this lane in the given direction, or null if no GTU could be
1321 * found.
1322 * @throws GTUException when there is a problem with the position of the GTUs on the lane.
1323 */
1324 public final LaneBasedGTU getGtuBehind(final Length position, final GTUDirectionality direction,
1325 final RelativePosition.TYPE relativePosition, final Time when) throws GTUException
1326 {
1327 if (direction.equals(GTUDirectionality.DIR_PLUS))
1328 {
1329 return getGtuAhead(position, GTUDirectionality.DIR_MINUS, relativePosition, when);
1330 }
1331 return getGtuAhead(position, GTUDirectionality.DIR_PLUS, relativePosition, when);
1332 }
1333
1334 /*
1335 * TODO only center position? Or also width? What is a good cutoff? Base on average width of the GTU type that can drive on
1336 * this Lane? E.g., for a Tram or Train, a 5 cm deviation is a problem; for a Car or a Bicycle, more deviation is
1337 * acceptable.
1338 */
1339 /** Lateral alignment margin for longitudinally connected Lanes. */
1340 public static final Length MARGIN = new Length(0.5, LengthUnit.METER);
1341
1342 /**
1343 * NextLanes returns the successor lane(s) in the design line direction, if any exist.<br>
1344 * The next lane(s) are cached, as it is too expensive to make the calculation every time. There are several possibilities:
1345 * (1) Returning an empty set when there is no successor lane in the design direction or there is no longitudinal transfer
1346 * possible to a successor lane in the design direction. (2) Returning a set with just one lane if the lateral position of
1347 * the successor lane matches the lateral position of this lane (based on an overlap of the lateral positions of the two
1348 * joining lanes of more than a certain percentage). (3) Multiple lanes in case the Node where the underlying Link for this
1349 * Lane has multiple "outgoing" Links, and there are multiple lanes that match the lateral position of this lane.<br>
1350 * The next lanes can differ per GTU type. For instance, a lane where cars and buses are allowed can have a next lane where
1351 * only buses are allowed, forcing the cars to leave that lane.
1352 * @param gtuType the GTU type for which we return the next lanes, use {@code null} to return all next lanes and their
1353 * design direction
1354 * @return set of Lanes following this lane for the given GTU type.
1355 */
1356 // TODO this should return something immutable
1357 public final Map<Lane, GTUDirectionality> nextLanes(final GTUType gtuType)
1358 {
1359 if (this.nextLanes == null)
1360 {
1361 this.nextLanes = new LinkedHashMap<>(1);
1362 }
1363 if (!this.nextLanes.containsKey(gtuType))
1364 {
1365 // TODO determine if this should synchronize on this.nextLanes
1366 Map<Lane, GTUDirectionality> laneMap = new LinkedHashMap<>(1);
1367 this.nextLanes.put(gtuType, laneMap);
1368 // Construct (and cache) the result.
1369 for (Link link : getParentLink().getEndNode().getLinks())
1370 {
1371 if (!(link.equals(this.getParentLink())) && link instanceof CrossSectionLink)
1372 {
1373 for (CrossSectionElement cse : ((CrossSectionLink) link).getCrossSectionElementList())
1374 {
1375 if (cse instanceof Lane)
1376 {
1377 Lane lane = (Lane) cse;
1378 Length jumpToStart = this.getCenterLine().getLast().distance(lane.getCenterLine().getFirst());
1379 Length jumpToEnd = this.getCenterLine().getLast().distance(lane.getCenterLine().getLast());
1380 // this, parentLink ---> O ---> lane, link
1381 if (jumpToStart.lt(MARGIN) && jumpToStart.lt(jumpToEnd)
1382 && link.getStartNode().equals(getParentLink().getEndNode()))
1383 {
1384 // Would the GTU move in the design line direction or against it?
1385 // TODO And is it aligned with its next lane?
1386 if (gtuType == null || lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_PLUS))
1387 {
1388 laneMap.put(lane, GTUDirectionality.DIR_PLUS);
1389 }
1390 else if (lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_MINUS))// getDirectionality(gtuType).isBackwardOrBoth())
1391 {
1392 laneMap.put(lane, GTUDirectionality.DIR_MINUS);
1393 }
1394 }
1395 // this, parentLink ---> O <--- lane, link
1396 else if (jumpToEnd.lt(MARGIN) && jumpToEnd.lt(jumpToStart)
1397 && link.getEndNode().equals(getParentLink().getEndNode()))
1398 {
1399 // Would the GTU move in the design line direction or against it?
1400 // TODO And is it aligned with its next lane?
1401 if (lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_PLUS))// getDirectionality(gtuType).isForwardOrBoth())
1402 {
1403 laneMap.put(lane, GTUDirectionality.DIR_PLUS);
1404 }
1405 else if (gtuType == null
1406 || lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_MINUS))// getDirectionality(gtuType).isBackwardOrBoth())
1407 {
1408 laneMap.put(lane, GTUDirectionality.DIR_MINUS);
1409 }
1410 }
1411 // else: not a "connected" lane
1412 }
1413 }
1414 }
1415 }
1416 }
1417 return this.nextLanes.get(gtuType);
1418 }
1419
1420 /**
1421 * PrevLanes returns the predecessor lane(s) relative to the design line direction, if any exist.<br>
1422 * The previous lane(s) are cached, as it is too expensive to make the calculation every time. There are several
1423 * possibilities: (1) Returning an empty set when there is no predecessor lane relative to the design direction or there is
1424 * no longitudinal transfer possible to a predecessor lane relative to the design direction. (2) Returning a set with just
1425 * one lane if the lateral position of the predecessor lane matches the lateral position of this lane (based on an overlap
1426 * of the lateral positions of the two joining lanes of more than a certain percentage). (3) Multiple lanes in case the Node
1427 * where the underlying Link for this Lane has multiple "incoming" Links, and there are multiple lanes that match the
1428 * lateral position of this lane.<br>
1429 * The previous lanes can differ per GTU type. For instance, a lane where cars and buses are allowed can be preceded by a
1430 * lane where only buses are allowed.
1431 * @param gtuType the GTU type for which we return the next lanes, use {@code null} to return all prev lanes and their
1432 * design direction
1433 * @return set of Lanes following this lane for the given GTU type.
1434 */
1435 // TODO this should return something immutable
1436 public final Map<Lane, GTUDirectionality> prevLanes(final GTUType gtuType)
1437 {
1438 if (this.prevLanes == null)
1439 {
1440 this.prevLanes = new LinkedHashMap<>(1);
1441 }
1442 if (!this.prevLanes.containsKey(gtuType))
1443 {
1444 Map<Lane, GTUDirectionality> laneMap = new LinkedHashMap<>(1);
1445 this.prevLanes.put(gtuType, laneMap);
1446 // Construct (and cache) the result.
1447 for (Link link : getParentLink().getStartNode().getLinks())
1448 {
1449 if (!(link.equals(this.getParentLink())) && link instanceof CrossSectionLink)
1450 {
1451 for (CrossSectionElement cse : ((CrossSectionLink) link).getCrossSectionElementList())
1452 {
1453 if (cse instanceof Lane)
1454 {
1455 Lane lane = (Lane) cse;
1456 Length jumpToStart = this.getCenterLine().getFirst().distance(lane.getCenterLine().getFirst());
1457 Length jumpToEnd = this.getCenterLine().getFirst().distance(lane.getCenterLine().getLast());
1458 // lane, link <---- O ----> this, parentLink
1459 if (jumpToStart.lt(MARGIN) && jumpToStart.lt(jumpToEnd)
1460 && link.getStartNode().equals(getParentLink().getStartNode()))
1461 {
1462 // does the GTU move in the design line direction or against it?
1463 // TODO And is it aligned with its next lane?
1464 if (lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_PLUS))// getDirectionality(gtuType).isForwardOrBoth())
1465 {
1466 laneMap.put(lane, GTUDirectionality.DIR_PLUS);
1467 }
1468 else if (gtuType == null
1469 || lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_MINUS))// getDirectionality(gtuType).isBackwardOrBoth())
1470 {
1471 laneMap.put(lane, GTUDirectionality.DIR_MINUS);
1472 }
1473 }
1474 // lane, link ----> O ----> this, parentLink
1475 else if (jumpToEnd.lt(MARGIN) && jumpToEnd.lt(jumpToStart)
1476 && link.getEndNode().equals(getParentLink().getStartNode()))
1477 {
1478 // does the GTU move in the design line direction or against it?
1479 // TODO And is it aligned with its next lane?
1480 if (gtuType == null || lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_PLUS))// getDirectionality(gtuType).isForwardOrBoth())
1481 {
1482 laneMap.put(lane, GTUDirectionality.DIR_PLUS);
1483 }
1484 else if (lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_MINUS))// getDirectionality(gtuType).isBackwardOrBoth())
1485 {
1486 laneMap.put(lane, GTUDirectionality.DIR_MINUS);
1487 }
1488 }
1489 // else: not a "connected" lane
1490 }
1491 }
1492 }
1493 }
1494 }
1495 return this.prevLanes.get(gtuType);
1496 }
1497
1498 /**
1499 * Returns the lanes that could be followed in a given direction and for the given GTU type.
1500 * @param direction GTUDirectionality; gtu direction
1501 * @param gtuType GTUType; gtu type
1502 * @return lanes that can be followed in a given direction and for the given GTU type
1503 */
1504 public final synchronized ImmutableMap<Lane, GTUDirectionality> downstreamLanes(final GTUDirectionality direction,
1505 final GTUType gtuType)
1506 {
1507 return this.downLanes.get(() ->
1508 {
1509 Map<Lane, GTUDirectionality> downMap =
1510 new LinkedHashMap<>(direction.isPlus() ? nextLanes(gtuType) : prevLanes(gtuType)); // safe copy
1511 Node downNode = direction.isPlus() ? getParentLink().getEndNode() : getParentLink().getStartNode();
1512 Iterator<Entry<Lane, GTUDirectionality>> iterator = downMap.entrySet().iterator();
1513 while (iterator.hasNext())
1514 {
1515 Entry<Lane, GTUDirectionality> entry = iterator.next();
1516 if ((entry.getValue().isPlus() && !entry.getKey().getParentLink().getStartNode().equals(downNode))
1517 || (entry.getValue().isMinus() && !entry.getKey().getParentLink().getEndNode().equals(downNode)))
1518 {
1519 // cannot move onto this lane
1520 iterator.remove();
1521 }
1522 }
1523 return new ImmutableLinkedHashMap<>(downMap, Immutable.WRAP);
1524 }, gtuType, direction);
1525 }
1526
1527 /**
1528 * Returns the lanes that could precede in a given direction and for the given GTU type.
1529 * @param direction GTUDirectionality; gtu direction
1530 * @param gtuType GTUType; gtu type
1531 * @return lanes that can be followed in a given direction and for the given GTU type
1532 */
1533 public final synchronized ImmutableMap<Lane, GTUDirectionality> upstreamLanes(final GTUDirectionality direction,
1534 final GTUType gtuType)
1535 {
1536 return this.upLanes.get(() ->
1537 {
1538 Map<Lane, GTUDirectionality> upMap =
1539 new LinkedHashMap<>(direction.isPlus() ? prevLanes(gtuType) : nextLanes(gtuType)); // safe copy
1540 Node upNode = direction.isPlus() ? getParentLink().getStartNode() : getParentLink().getEndNode();
1541 Iterator<Entry<Lane, GTUDirectionality>> iterator = upMap.entrySet().iterator();
1542 while (iterator.hasNext())
1543 {
1544 Entry<Lane, GTUDirectionality> entry = iterator.next();
1545 if ((entry.getValue().isPlus() && !entry.getKey().getParentLink().getEndNode().equals(upNode))
1546 || (entry.getValue().isMinus() && !entry.getKey().getParentLink().getStartNode().equals(upNode)))
1547 {
1548 // cannot have come from this lane
1549 iterator.remove();
1550 }
1551 }
1552 return new ImmutableLinkedHashMap<>(upMap, Immutable.WRAP);
1553 }, gtuType, direction);
1554 }
1555
1556 /**
1557 * Determine the set of lanes to the left or to the right of this lane, which are accessible from this lane, or an empty set
1558 * if no lane could be found. The method ignores all legal restrictions such as allowable directions and stripes.<br>
1559 * A lane is called adjacent to another lane if the lateral edges are not more than a delta distance apart. This means that
1560 * a lane that <i>overlaps</i> with another lane is <b>not</b> returned as an adjacent lane. <br>
1561 * <b>Note:</b> LEFT and RIGHT are seen from the direction of the GTU, in its forward driving direction. <br>
1562 * @param lateralDirection LateralDirectionality; LEFT or RIGHT.
1563 * @param gtuType GTUType; the type of GTU for which to return the adjacent lanes.
1564 * @param drivingDirection GTUDirectionality; the driving direction of the GTU on <code>this</code> Lane
1565 * @return the set of lanes that are accessible, or null if there is no lane that is accessible with a matching driving
1566 * direction.
1567 */
1568 public final Set<Lane> accessibleAdjacentLanesPhysical(final LateralDirectionality lateralDirection, final GTUType gtuType,
1569 final GTUDirectionality drivingDirection)
1570 {
1571 LateralDirectionality dir =
1572 drivingDirection.equals(GTUDirectionality.DIR_PLUS) ? lateralDirection : lateralDirection.flip();
1573 return neighbors(dir, gtuType, drivingDirection, false);
1574 }
1575
1576 /**
1577 * Determine the set of lanes to the left or to the right of this lane, which are accessible from this lane, or an empty set
1578 * if no lane could be found. The method takes the LongitidinalDirectionality of the lane into account. In other words, if
1579 * we drive in the DIR_PLUS direction and look for a lane on the LEFT, and there is a lane but the Directionality of that
1580 * lane is not DIR_PLUS or DIR_BOTH, it will not be included.<br>
1581 * A lane is called adjacent to another lane if the lateral edges are not more than a delta distance apart. This means that
1582 * a lane that <i>overlaps</i> with another lane is <b>not</b> returned as an adjacent lane. <br>
1583 * <b>Note:</b> LEFT and RIGHT are seen from the direction of the GTU, in its forward driving direction. <br>
1584 * @param lateralDirection LateralDirectionality; LEFT or RIGHT.
1585 * @param gtuType GTUType; the type of GTU for which to return the adjacent lanes.
1586 * @param drivingDirection GTUDirectionality; the driving direction of the GTU on <code>this</code> Lane
1587 * @return the set of lanes that are accessible, or null if there is no lane that is accessible with a matching driving
1588 * direction.
1589 */
1590 public final Set<Lane> accessibleAdjacentLanesLegal(final LateralDirectionality lateralDirection, final GTUType gtuType,
1591 final GTUDirectionality drivingDirection)
1592 {
1593 Set<Lane> candidates = new LinkedHashSet<>(1);
1594 LateralDirectionality dir =
1595 drivingDirection.equals(GTUDirectionality.DIR_PLUS) ? lateralDirection : lateralDirection.flip();
1596 for (Lane lane : neighbors(dir, gtuType, drivingDirection, true))
1597 {
1598 if (lane.getLaneType().isCompatible(gtuType, drivingDirection))
1599 {
1600 candidates.add(lane);
1601 }
1602 }
1603 return candidates;
1604 }
1605
1606 /**
1607 * Get the speed limit of this lane, which can differ per GTU type. E.g., cars might be allowed to drive 120 km/h and trucks
1608 * 90 km/h.
1609 * @param gtuType GTUType; the GTU type to provide the speed limit for
1610 * @return the speedLimit.
1611 * @throws NetworkException on network inconsistency
1612 */
1613 public final Speed getSpeedLimit(final GTUType gtuType) throws NetworkException
1614 {
1615 Speed speedLimit = this.cachedSpeedLimits.get(gtuType);
1616 if (speedLimit == null)
1617 {
1618 if (this.speedLimitMap.containsKey(gtuType))
1619 {
1620 speedLimit = this.speedLimitMap.get(gtuType);
1621 }
1622 else if (gtuType.getParent() != null)
1623 {
1624 speedLimit = getSpeedLimit(gtuType.getParent());
1625 }
1626 else
1627 {
1628 throw new NetworkException("No speed limit set for GTUType " + gtuType + " on lane " + toString());
1629 }
1630 this.cachedSpeedLimits.put(gtuType, speedLimit);
1631 }
1632 return speedLimit;
1633 }
1634
1635 /**
1636 * Get the lowest speed limit of this lane.
1637 * @return the lowest speedLimit.
1638 * @throws NetworkException on network inconsistency
1639 */
1640 public final Speed getLowestSpeedLimit() throws NetworkException
1641 {
1642 Throw.when(this.speedLimitMap.isEmpty(), NetworkException.class, "Lane %s has no speed limits set.", toString());
1643 Speed out = Speed.POSITIVE_INFINITY;
1644 for (GTUType gtuType : this.speedLimitMap.keySet())
1645 {
1646 out = Speed.min(out, this.speedLimitMap.get(gtuType));
1647 }
1648 return out;
1649 }
1650
1651 /**
1652 * Get the highest speed limit of this lane.
1653 * @return the highest speedLimit.
1654 * @throws NetworkException on network inconsistency
1655 */
1656 public final Speed getHighestSpeedLimit() throws NetworkException
1657 {
1658 Throw.when(this.speedLimitMap.isEmpty(), NetworkException.class, "Lane %s has no speed limits set.", toString());
1659 Speed out = Speed.ZERO;
1660 for (GTUType gtuType : this.speedLimitMap.keySet())
1661 {
1662 out = Speed.max(out, this.speedLimitMap.get(gtuType));
1663 }
1664 return out;
1665 }
1666
1667 /**
1668 * Set the speed limit of this lane, which can differ per GTU type. Cars might be allowed to drive 120 km/h and trucks 90
1669 * km/h. If the speed limit is the same for all GTU types, GTUType.ALL will be used. This means that the settings can be
1670 * used additive, or subtractive. <br>
1671 * In <b>additive use</b>, do not set the speed limit for GTUType.ALL. Now, one by one, the allowed maximum speeds for each
1672 * of the GTU Types have be added. Do this when there are few GTU types or the speed limits per TU type are very different.
1673 * <br>
1674 * In <b>subtractive use</b>, set the speed limit for GTUType.ALL to the most common one. Override the speed limit for
1675 * certain GTUTypes to a different value. An example is a lane on a highway where all vehicles, except truck (CAR, BUS,
1676 * MOTORCYCLE, etc.), can drive 120 km/h, but trucks are allowed only 90 km/h. In that case, set the speed limit for
1677 * GTUType.ALL to 120 km/h, and for TRUCK to 90 km/h.
1678 * @param gtuType GTUType; the GTU type to provide the speed limit for
1679 * @param speedLimit Speed; the speed limit for this gtu type
1680 */
1681 public final void setSpeedLimit(final GTUType gtuType, final Speed speedLimit)
1682 {
1683 this.speedLimitMap.put(gtuType, speedLimit);
1684 this.cachedSpeedLimits.clear();
1685 }
1686
1687 /**
1688 * Remove the set speed limit for a GTUType. If the speed limit for GTUType.ALL will be removed, there will not be a
1689 * 'default' speed limit anymore. If the speed limit for a certain GTUType is removed, its speed limit will default to the
1690 * speed limit of GTUType.ALL. <br>
1691 * <b>Note</b>: if no speed limit is known for a GTUType, getSpeedLimit will throw a NetworkException when the speed limit
1692 * is retrieved for that GTUType.
1693 * @param gtuType GTUType; the GTU type to provide the speed limit for
1694 */
1695 public final void removeSpeedLimit(final GTUType gtuType)
1696 {
1697 this.speedLimitMap.remove(gtuType);
1698 this.cachedSpeedLimits.clear();
1699 }
1700
1701 /**
1702 * @return laneType.
1703 */
1704 public final LaneType getLaneType()
1705 {
1706 return this.laneType;
1707 }
1708
1709 /**
1710 * This method sets the directionality of the lane for a GTU type. It might be that the driving direction in the lane is
1711 * FORWARD (from start node of the link to end node of the link) for the GTU type CAR, but BOTH for the GTU type BICYCLE
1712 * (i.e., bicycles can also go in the other direction; we see this on some city streets). If the directionality for a
1713 * GTUType is set to NONE, this means that the given GTUType cannot use the Lane. If a Directionality is set for
1714 * GTUType.ALL, the getDirectionality will default to these settings when there is no specific entry for a given
1715 * directionality. This means that the settings can be used additive, or restrictive. <br>
1716 * In <b>additive use</b>, set the directionality for GTUType.ALL to NONE, or do not set the directionality for GTUType.ALL.
1717 * Now, one by one, the allowed directionalities can be added. An example is a lane on a highway, which we only open for
1718 * CAR, TRUCK and BUS. <br>
1719 * In <b>restrictive use</b>, set the directionality for GTUType.ALL to BOTH, FORWARD, or BACKWARD. Override the
1720 * directionality for certain GTUTypes to a more restrictive access, e.g. to NONE. An example is a lane that is open for all
1721 * road users, except TRUCK.
1722 * @param gtuType the GTU type to set the directionality for.
1723 * @param directionality the longitudinal directionality of the link (FORWARD, BACKWARD, BOTH or NONE) for the given GTU
1724 * type.
1725 * @throws NetworkException when the lane directionality for the given GTUType is inconsistent with the Link directionality
1726 * to which the lane belongs.
1727 */
1728 // public final void addDirectionality(final GTUType gtuType, final LongitudinalDirectionality directionality)
1729 // throws NetworkException
1730 // {
1731 // this.directionalityMap.put(gtuType, directionality);
1732 // checkDirectionality();
1733 // }
1734
1735 /**
1736 * This method removes an earlier provided directionality of the lane for a given GTU type, e.g. for maintenance of the
1737 * lane. After removing, the directionality for the GTU will fall back to the provided directionality for GTUType.ALL (if
1738 * present). Thereby removing a directionality is different from setting the directionality to NONE.
1739 * @param gtuType the GTU type to remove the directionality for on this lane.
1740 */
1741 // public final void removeDirectionality(final GTUType gtuType)
1742 // {
1743 // this.directionalityMap.remove(gtuType);
1744 // }
1745
1746 /**
1747 * Check whether the directionalities for the GTU types for this lane are consistent with the directionalities of the
1748 * overarching Link.
1749 * @throws NetworkException when the lane directionality for a given GTUType is inconsistent with the Link directionality to
1750 * which the lane belongs.
1751 */
1752 private void checkDirectionality() throws NetworkException
1753 {
1754 // TODO check that the directionality of this Lane does not conflict with that of the parent the OTSLink
1755 // for (GTUType gtuType : this.directionalityMap.keySet())
1756 // {
1757 // LongitudinalDirectionality directionality = this.directionalityMap.get(gtuType);
1758 // if (!getParentLink().getDirectionality(gtuType).contains(directionality))
1759 // {
1760 // throw new NetworkException("Lane " + toString() + " allows " + gtuType + " a directionality of "
1761 // + directionality + " which is not present in the overarching link " + getParentLink().toString());
1762 // }
1763 // }
1764 }
1765
1766 /**
1767 * @return gtuList.
1768 */
1769 public final ImmutableList<LaneBasedGTU> getGtuList()
1770 {
1771 // TODO let HistoricalArrayList return an Immutable (WRAP) of itself
1772 return this.gtuList == null ? new ImmutableArrayList<>(new ArrayList<>())
1773 : new ImmutableArrayList<>(this.gtuList, Immutable.COPY);
1774 }
1775
1776 /**
1777 * Returns the list of GTU's at the specified time.
1778 * @param time Time; time
1779 * @return list of GTU's at the specified times
1780 */
1781 public final List<LaneBasedGTU> getGtuList(final Time time)
1782 {
1783 if (time.equals(this.gtuListTime))
1784 {
1785 return this.gtuListAtTime;
1786 }
1787 this.gtuListTime = time;
1788 this.gtuListAtTime = this.gtuList == null ? new ArrayList<>() : this.gtuList.get(time);
1789 return this.gtuListAtTime;
1790 }
1791
1792 /**
1793 * Returns the number of GTU's.
1794 * @return int; number of GTU's.
1795 */
1796 public final int numberOfGtus()
1797 {
1798 return this.gtuList.size();
1799 }
1800
1801 /**
1802 * Returns the number of GTU's at specified time.
1803 * @param time Time; time
1804 * @return int; number of GTU's.
1805 */
1806 public final int numberOfGtus(final Time time)
1807 {
1808 return getGtuList(time).size();
1809 }
1810
1811 /**
1812 * Returns the index of the given GTU, or -1 if not present.
1813 * @param gtu LaneBasedGTU; gtu to get the index of
1814 * @return int; index of the given GTU, or -1 if not present
1815 */
1816 public final int indexOfGtu(final LaneBasedGTU gtu)
1817 {
1818 return Collections.binarySearch(this.gtuList, gtu, (
1819 gtu1, gtu2
1820 ) ->
1821 {
1822 try
1823 {
1824 return gtu1.position(this, gtu1.getReference()).compareTo(gtu2.position(this, gtu2.getReference()));
1825 }
1826 catch (GTUException exception)
1827 {
1828 throw new RuntimeException(exception);
1829 }
1830 });
1831 }
1832
1833 /**
1834 * Returns the index of the given GTU, or -1 if not present, at specified time.
1835 * @param gtu LaneBasedGTU; gtu to get the index of
1836 * @param time Time; time
1837 * @return int; index of the given GTU, or -1 if not present
1838 */
1839 public final int indexOfGtu(final LaneBasedGTU gtu, final Time time)
1840 {
1841 return Collections.binarySearch(getGtuList(time), gtu, (
1842 gtu1, gtu2
1843 ) ->
1844 {
1845 try
1846 {
1847 return Double.compare(gtu1.fractionalPosition(this, gtu1.getReference(), time),
1848 gtu2.fractionalPosition(this, gtu2.getReference(), time));
1849 }
1850 catch (GTUException exception)
1851 {
1852 throw new RuntimeException(exception);
1853 }
1854 });
1855 }
1856
1857 /**
1858 * Returns the index'th GTU.
1859 * @param index int; index of the GTU
1860 * @return LaneBasedGTU; the index'th GTU
1861 */
1862 public final LaneBasedGTU getGtu(final int index)
1863 {
1864 return this.gtuList.get(index);
1865 }
1866
1867 /**
1868 * Returns the index'th GTU at specified time.
1869 * @param index int; index of the GTU
1870 * @param time Time; time
1871 * @return LaneBasedGTU; the index'th GTU
1872 */
1873 public final LaneBasedGTU getGtu(final int index, final Time time)
1874 {
1875 return getGtuList(time).get(index);
1876 }
1877
1878 /** {@inheritDoc} */
1879 @Override
1880 @SuppressWarnings("checkstyle:designforextension")
1881 public double getZ()
1882 {
1883 return 0.0;
1884 }
1885
1886 /** {@inheritDoc} */
1887 @Override
1888 public final String toString()
1889 {
1890 CrossSectionLink link = getParentLink();
1891 return String.format("Lane %s of %s", getId(), link.getId());
1892 }
1893
1894 /** Cache of the hashCode. */
1895 private Integer cachedHashCode = null;
1896
1897 /** {@inheritDoc} */
1898 @SuppressWarnings("checkstyle:designforextension")
1899 @Override
1900 public int hashCode()
1901 {
1902 if (this.cachedHashCode == null)
1903 {
1904 final int prime = 31;
1905 int result = super.hashCode();
1906 result = prime * result + ((this.laneType == null) ? 0 : this.laneType.hashCode());
1907 this.cachedHashCode = result;
1908 }
1909 return this.cachedHashCode;
1910 }
1911
1912 /** {@inheritDoc} */
1913 @SuppressWarnings({"checkstyle:designforextension", "checkstyle:needbraces"})
1914 @Override
1915 public boolean equals(final Object obj)
1916 {
1917 if (this == obj)
1918 return true;
1919 if (!super.equals(obj))
1920 return false;
1921 if (getClass() != obj.getClass())
1922 return false;
1923 Lane other = (Lane) obj;
1924 if (this.laneType == null)
1925 {
1926 if (other.laneType != null)
1927 return false;
1928 }
1929 else if (!this.laneType.equals(other.laneType))
1930 return false;
1931 return true;
1932 }
1933
1934 /** {@inheritDoc} */
1935 @Override
1936 @SuppressWarnings("checkstyle:designforextension")
1937 public Lane clone(final CrossSectionLink newParentLink, final OTSSimulatorInterface newSimulator) throws NetworkException
1938 {
1939 Lane newLane = new Lane(newParentLink, this);
1940 // nextLanes, prevLanes, nextNeighbors, rightNeighbors are filled at first request
1941
1942 SortedMap<Double, List<SingleSensor>> newSensorMap = new TreeMap<>();
1943 for (double distance : this.sensors.keySet())
1944 {
1945 List<SingleSensor> newSensorList = new ArrayList<>();
1946 for (SingleSensor sensor : this.sensors.get(distance))
1947 {
1948 SingleSensor newSensor = ((AbstractSensor) sensor).clone(newLane, newSimulator);
1949 newSensorList.add(newSensor);
1950 }
1951 newSensorMap.put(distance, newSensorList);
1952 }
1953 newLane.sensors.clear();
1954 newLane.sensors.putAll(newSensorMap);
1955
1956 SortedMap<Double, List<LaneBasedObject>> newLaneBasedObjectMap = new TreeMap<>();
1957 for (double distance : this.laneBasedObjects.keySet())
1958 {
1959 List<LaneBasedObject> newLaneBasedObjectList = new ArrayList<>();
1960 for (LaneBasedObject lbo : this.laneBasedObjects.get(distance))
1961 {
1962 AbstractLaneBasedObject laneBasedObject = (AbstractLaneBasedObject) lbo;
1963 LaneBasedObject newLbo = laneBasedObject.clone(newLane, newSimulator);
1964 newLaneBasedObjectList.add(newLbo);
1965 }
1966 newLaneBasedObjectMap.put(distance, newLaneBasedObjectList);
1967 }
1968 newLane.laneBasedObjects.clear();
1969 newLane.laneBasedObjects.putAll(newLaneBasedObjectMap);
1970
1971 return newLane;
1972 }
1973
1974 /**
1975 * Functional interface that can be used for line searches of objects on the lane.
1976 * <p>
1977 * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
1978 * <br>
1979 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
1980 * <p>
1981 * @version $Revision$, $LastChangedDate$, by $Author$, initial version 28 jan. 2018 <br>
1982 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
1983 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
1984 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
1985 */
1986 private interface Positions
1987 {
1988 /**
1989 * Returns the position of the index'th element.
1990 * @param index int; index
1991 * @return double; position of the index'th element
1992 * @throws GTUException on exception
1993 */
1994 double get(int index) throws GTUException;
1995 }
1996
1997 }