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.Length;
20 import org.djunits.value.vdouble.scalar.Speed;
21 import org.djunits.value.vdouble.scalar.Time;
22 import org.djutils.event.TimedEventType;
23 import org.djutils.exceptions.Throw;
24 import org.djutils.immutablecollections.Immutable;
25 import org.djutils.immutablecollections.ImmutableArrayList;
26 import org.djutils.immutablecollections.ImmutableLinkedHashMap;
27 import org.djutils.immutablecollections.ImmutableList;
28 import org.djutils.immutablecollections.ImmutableMap;
29 import org.djutils.multikeymap.MultiKeyMap;
30 import org.opentrafficsim.core.geometry.OTSGeometryException;
31 import org.opentrafficsim.core.gtu.GTUDirectionality;
32 import org.opentrafficsim.core.gtu.GTUException;
33 import org.opentrafficsim.core.gtu.GTUType;
34 import org.opentrafficsim.core.gtu.RelativePosition;
35 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
36 import org.opentrafficsim.core.network.LateralDirectionality;
37 import org.opentrafficsim.core.network.Link;
38 import org.opentrafficsim.core.network.NetworkException;
39 import org.opentrafficsim.core.network.Node;
40 import org.opentrafficsim.core.perception.HistoryManager;
41 import org.opentrafficsim.core.perception.collections.HistoricalArrayList;
42 import org.opentrafficsim.core.perception.collections.HistoricalList;
43 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
44 import org.opentrafficsim.road.network.RoadNetwork;
45 import org.opentrafficsim.road.network.lane.object.AbstractLaneBasedObject;
46 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
47 import org.opentrafficsim.road.network.lane.object.sensor.AbstractSensor;
48 import org.opentrafficsim.road.network.lane.object.sensor.DestinationSensor;
49 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
50 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
51
52 import nl.tudelft.simulation.dsol.SimRuntimeException;
53 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEvent;
54 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
55 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
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(Laneinal 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 Set<Lane> lanes = new LinkedHashSet<>(1);
471 for (CrossSectionElement cse : this.parentLink.getCrossSectionElementList())
472 {
473 if (cse instanceof Lane && !cse.equals(this))
474 {
475 Laneef="../../../../../org/opentrafficsim/road/network/lane/Lane.html#Lane">Lane lane = (Lane) cse;
476 if (laterallyAdjacentAndAccessible(lane, direction, gtuType, drivingDirection, legal))
477 {
478 lanes.add(lane);
479 }
480 }
481 }
482 return lanes;
483 }, gtuType, drivingDirection, legal);
484 }
485
486 /** Lateral alignment margin for longitudinally connected Lanes. */
487 static final Length ADJACENT_MARGIN = new Length(0.2, LengthUnit.METER);
488
489 /**
490 * Determine whether another lane is adjacent to this lane (dependent on distance) and accessible (dependent on stripes) for
491 * a certain GTU type (dependent on usability of the adjacent lane for that GTU type). This method assumes that when there
492 * is NO stripe between two adjacent lanes that are accessible for the GTU type, the GTU can enter that lane. <br>
493 * @param lane Lane; the other lane to evaluate
494 * @param direction LateralDirectionality; the direction to look at, relative to the DESIGN LINE of the link. This is a very
495 * important aspect to note: all information is stored relative to the direction of the design line, and not in a
496 * driving direction, which can vary for lanes that can be driven in two directions (e.g. at overtaking).
497 * @param gtuType GTUType; the GTU type to check the accessibility for
498 * @param drivingDirection GTUDirectionality; driving direction of the GTU
499 * @param legal boolean; whether to check legal possibility
500 * @return boolean; true if the other lane is adjacent to this lane and accessible for the given GTU type; false otherwise
501 */
502 private boolean laterallyAdjacentAndAccessible(final Lane lane, final LateralDirectionality direction,
503 final GTUType gtuType, final GTUDirectionality drivingDirection, final boolean legal)
504 {
505 if (!lane.getLaneType().isCompatible(gtuType, drivingDirection))
506 {
507 // not accessible for the given GTU type
508 return false;
509 }
510
511 if (direction.equals(LateralDirectionality.LEFT))
512 {
513 // TODO take the cross section slices into account...
514 if (lane.getDesignLineOffsetAtBegin().si + ADJACENT_MARGIN.si > getDesignLineOffsetAtBegin().si
515 && lane.getDesignLineOffsetAtEnd().si + ADJACENT_MARGIN.si > getDesignLineOffsetAtEnd().si
516 && (lane.getDesignLineOffsetAtBegin().si - lane.getBeginWidth().si / 2.0)
517 - (getDesignLineOffsetAtBegin().si + getBeginWidth().si / 2.0) < ADJACENT_MARGIN.si
518 && (lane.getDesignLineOffsetAtEnd().si - lane.getEndWidth().si / 2.0)
519 - (getDesignLineOffsetAtEnd().si + getEndWidth().si / 2.0) < ADJACENT_MARGIN.si)
520 {
521 // look at stripes between the two lanes
522 if (legal)
523 {
524 for (CrossSectionElement cse : this.parentLink.getCrossSectionElementList())
525 {
526 if (cse instanceof Stripe)
527 {
528 Stripe../../../../../org/opentrafficsim/road/network/lane/Stripe.html#Stripe">Stripe stripe = (Stripe) cse;
529 // TODO take the cross section slices into account...
530 if ((getDesignLineOffsetAtBegin().si < stripe.getDesignLineOffsetAtBegin().si
531 && stripe.getDesignLineOffsetAtBegin().si < lane.getDesignLineOffsetAtBegin().si)
532 || (getDesignLineOffsetAtEnd().si < stripe.getDesignLineOffsetAtEnd().si
533 && stripe.getDesignLineOffsetAtEnd().si < lane.getDesignLineOffsetAtEnd().si))
534 {
535 if (!stripe.isPermeable(gtuType, LateralDirectionality.LEFT))
536 {
537 // there is a stripe forbidding to cross to the adjacent lane
538 return false;
539 }
540 }
541 }
542 }
543 }
544 // the lanes are adjacent, and there is no stripe forbidding us to enter that lane
545 // or there is no stripe at all
546 return true;
547 }
548 }
549
550 else
551 // direction.equals(LateralDirectionality.RIGHT)
552 {
553 // TODO take the cross section slices into account...
554 if (lane.getDesignLineOffsetAtBegin().si < getDesignLineOffsetAtBegin().si + ADJACENT_MARGIN.si
555 && lane.getDesignLineOffsetAtEnd().si < getDesignLineOffsetAtEnd().si + ADJACENT_MARGIN.si
556 && (getDesignLineOffsetAtBegin().si - getBeginWidth().si / 2.0)
557 - (lane.getDesignLineOffsetAtBegin().si + lane.getBeginWidth().si / 2.0) < ADJACENT_MARGIN.si
558 && (getDesignLineOffsetAtEnd().si - getEndWidth().si / 2.0)
559 - (lane.getDesignLineOffsetAtEnd().si + lane.getEndWidth().si / 2.0) < ADJACENT_MARGIN.si)
560 {
561 // look at stripes between the two lanes
562 if (legal)
563 {
564 for (CrossSectionElement cse : this.parentLink.getCrossSectionElementList())
565 {
566 if (cse instanceof Stripe)
567 {
568 Stripe../../../../../org/opentrafficsim/road/network/lane/Stripe.html#Stripe">Stripe stripe = (Stripe) cse;
569 // TODO take the cross section slices into account...
570 if ((getDesignLineOffsetAtBegin().si > stripe.getDesignLineOffsetAtBegin().si
571 && stripe.getDesignLineOffsetAtBegin().si > lane.getDesignLineOffsetAtBegin().si)
572 || (getDesignLineOffsetAtEnd().si > stripe.getDesignLineOffsetAtEnd().si
573 && stripe.getDesignLineOffsetAtEnd().si > lane.getDesignLineOffsetAtEnd().si))
574 {
575 if (!stripe.isPermeable(gtuType, LateralDirectionality.RIGHT))
576 {
577 // there is a stripe forbidding to cross to the adjacent lane
578 return false;
579 }
580 }
581 }
582 }
583 }
584 // the lanes are adjacent, and there is no stripe forbidding us to enter that lane
585 // or there is no stripe at all
586 return true;
587 }
588 }
589
590 // no lanes were found that are close enough laterally.
591 return false;
592 }
593
594 /**
595 * Insert a sensor at the right place in the sensor list of this Lane.
596 * @param sensor SingleSensor; the sensor to add
597 * @throws NetworkException when the position of the sensor is beyond (or before) the range of this Lane
598 */
599 public final void addSensor(final SingleSensor sensor) throws NetworkException
600 {
601 double position = sensor.getLongitudinalPosition().si;
602 if (position < 0 || position > getLength().getSI())
603 {
604 throw new NetworkException("Illegal position for sensor " + position + " valid range is 0.." + getLength().getSI());
605 }
606 if (this.parentLink.getNetwork().containsObject(sensor.getFullId()))
607 {
608 throw new NetworkException("Network already contains an object with the name " + sensor.getFullId());
609 }
610 List<SingleSensor> sensorList = this.sensors.get(position);
611 if (null == sensorList)
612 {
613 sensorList = new ArrayList<>(1);
614 this.sensors.put(position, sensorList);
615 }
616 sensorList.add(sensor);
617 this.parentLink.getNetwork().addObject(sensor);
618 fireTimedEvent(Lane.SENSOR_ADD_EVENT, new Object[] { sensor.getId(), sensor },
619 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<SimTimeDoubleUnit> event = new SimEvent<>(new SimTimeDoubleUnit(triggerTime), this, sensor,
819 "trigger", new Object[] { gtu });
820 gtu.getSimulator().scheduleEvent(event);
821 gtu.addTrigger(this, event);
822 }
823 else if (sensor.getLongitudinalPosition().si < minPos
824 && (sensor instanceof SinkSensor || sensor instanceof DestinationSensor))
825 {
826 // TODO this is a hack for when sink sensors aren't perfectly adjacent or the GTU overshoots with nose
827 // due to curvature
828 SimEvent<SimTimeDoubleUnit> event =
829 new SimEvent<>(new SimTimeDoubleUnit(gtu.getSimulator().getSimulatorTime()), this, sensor,
830 "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((final int index) -> {
1166 LaneBasedGTU gtu = list.get(index);
1167 return gtu.position(this, gtu.getRelativePositions().get(relativePosition), when).si;
1168 }, list.size(), position.si);
1169 if (direction.equals(GTUDirectionality.DIR_PLUS))
1170 {
1171 if (search[1] < list.size())
1172 {
1173 return list.get(search[1]);
1174 }
1175 }
1176 else
1177 {
1178 if (search[0] >= 0)
1179 {
1180 return list.get(search[0]);
1181 }
1182 }
1183 return null;
1184 }
1185
1186 /**
1187 * Searches for objects just before and after a given position.
1188 * @param positions Positions; functional interface returning positions at indices
1189 * @param listSize int; number of objects in the underlying list
1190 * @param position double; position
1191 * @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
1192 * exactly at the position int[1] - int[0] = 2. If all objects have a higher position int[0] = -1, if all objects
1193 * have a lower position int[1] = listSize.
1194 * @throws GTUException ...
1195 */
1196 private int[] lineSearch(final Positions positions, final int listSize, final double position) throws GTUException
1197 {
1198 int[] out = new int[2];
1199 // line search only works if the position is within the original domain, first catch 4 outside situations
1200 double pos0 = positions.get(0);
1201 double posEnd;
1202 if (position < pos0)
1203 {
1204 out[0] = -1;
1205 out[1] = 0;
1206 }
1207 else if (position == pos0)
1208 {
1209 out[0] = -1;
1210 out[1] = 1;
1211 }
1212 else if (position > (posEnd = positions.get(listSize - 1)))
1213 {
1214 out[0] = listSize - 1;
1215 out[1] = listSize;
1216 }
1217 else if (position == posEnd)
1218 {
1219 out[0] = listSize - 2;
1220 out[1] = listSize;
1221 }
1222 else
1223 {
1224 int low = 0;
1225 int mid = (int) ((listSize - 1) * position / this.length.si);
1226 mid = mid < 0 ? 0 : mid >= listSize ? listSize - 1 : mid;
1227 int high = listSize - 1;
1228 while (high - low > 1)
1229 {
1230 double midPos = positions.get(mid);
1231 if (midPos < position)
1232 {
1233 low = mid;
1234 }
1235 else if (midPos > position)
1236 {
1237 high = mid;
1238 }
1239 else
1240 {
1241 low = mid - 1;
1242 high = mid + 1;
1243 break;
1244 }
1245 mid = (low + high) / 2;
1246 }
1247 out[0] = low;
1248 out[1] = high;
1249 }
1250 return out;
1251 }
1252
1253 /**
1254 * Get the first object where the relativePosition is in front of a certain position on the lane, in a driving direction on
1255 * this lane, compared to the DESIGN LINE. Perception should iterate over results from this method to see what is most
1256 * limiting.
1257 * @param position Length; the position after which the relative position of an object will be searched.
1258 * @param direction GTUDirectionality; whether we are looking in the the center line direction or against the center line
1259 * direction.
1260 * @return List<LaneBasedObject>; the first object(s) before a position on this lane in the given direction, or null
1261 * if no object could be found.
1262 */
1263 public final List<LaneBasedObject> getObjectAhead(final Length position, final GTUDirectionality direction)
1264 {
1265 if (direction.equals(GTUDirectionality.DIR_PLUS))
1266 {
1267 for (double distance : this.laneBasedObjects.keySet())
1268 {
1269 if (distance > position.si)
1270 {
1271 return new ArrayList<>(this.laneBasedObjects.get(distance));
1272 }
1273 }
1274 }
1275 else
1276 {
1277 NavigableMap<Double, List<LaneBasedObject>> reverseLBO =
1278 (NavigableMap<Double, List<LaneBasedObject>>) this.laneBasedObjects;
1279 for (double distance : reverseLBO.descendingKeySet())
1280 {
1281 if (distance < position.si)
1282 {
1283 return new ArrayList<>(this.laneBasedObjects.get(distance));
1284 }
1285 }
1286 }
1287 return null;
1288 }
1289
1290 /**
1291 * Get the first object where the relativePosition is behind of a certain position on the lane, in a driving direction on
1292 * this lane, compared to the DESIGN LINE. Perception should iterate over results from this method to see what is most
1293 * limiting.
1294 * @param position Length; the position after which the relative position of an object will be searched.
1295 * @param direction GTUDirectionality; whether we are looking in the the center line direction or against the center line
1296 * direction.
1297 * @return List<LaneBasedObject>; the first object(s) after a position on this lane in the given direction, or null if
1298 * no object could be found.
1299 */
1300 public final List<LaneBasedObject> getObjectBehind(final Length position, final GTUDirectionality direction)
1301 {
1302 if (direction.equals(GTUDirectionality.DIR_PLUS))
1303 {
1304 return getObjectAhead(position, GTUDirectionality.DIR_MINUS);
1305 }
1306 return getObjectAhead(position, GTUDirectionality.DIR_PLUS);
1307 }
1308
1309 /**
1310 * Get the first GTU where the relativePosition is behind a certain position on the lane, in a driving direction on this
1311 * lane, compared to the DESIGN LINE.
1312 * @param position Length; the position before which the relative position of a GTU will be searched.
1313 * @param direction GTUDirectionality; whether we are looking in the the center line direction or against the center line
1314 * direction.
1315 * @param relativePosition RelativePosition.TYPE; the relative position of the GTU we are looking for.
1316 * @param when Time; the time for which to evaluate the positions.
1317 * @return LaneBasedGTU; the first GTU after a position on this lane in the given direction, or null if no GTU could be
1318 * found.
1319 * @throws GTUException when there is a problem with the position of the GTUs on the lane.
1320 */
1321 public final LaneBasedGTU getGtuBehind(final Length position, final GTUDirectionality direction,
1322 final RelativePosition.TYPE relativePosition, final Time when) throws GTUException
1323 {
1324 if (direction.equals(GTUDirectionality.DIR_PLUS))
1325 {
1326 return getGtuAhead(position, GTUDirectionality.DIR_MINUS, relativePosition, when);
1327 }
1328 return getGtuAhead(position, GTUDirectionality.DIR_PLUS, relativePosition, when);
1329 }
1330
1331 /*
1332 * TODO only center position? Or also width? What is a good cutoff? Base on average width of the GTU type that can drive on
1333 * 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
1334 * acceptable.
1335 */
1336 /** Lateral alignment margin for longitudinally connected Lanes. */
1337 public static final Length MARGIN = new Length(0.5, LengthUnit.METER);
1338
1339 /**
1340 * NextLanes returns the successor lane(s) in the design line direction, if any exist.<br>
1341 * The next lane(s) are cached, as it is too expensive to make the calculation every time. There are several possibilities:
1342 * (1) Returning an empty set when there is no successor lane in the design direction or there is no longitudinal transfer
1343 * possible to a successor lane in the design direction. (2) Returning a set with just one lane if the lateral position of
1344 * the successor lane matches the lateral position of this lane (based on an overlap of the lateral positions of the two
1345 * joining lanes of more than a certain percentage). (3) Multiple lanes in case the Node where the underlying Link for this
1346 * Lane has multiple "outgoing" Links, and there are multiple lanes that match the lateral position of this lane.<br>
1347 * The next lanes can differ per GTU type. For instance, a lane where cars and buses are allowed can have a next lane where
1348 * only buses are allowed, forcing the cars to leave that lane.
1349 * @param gtuType the GTU type for which we return the next lanes, use {@code null} to return all next lanes and their
1350 * design direction
1351 * @return set of Lanes following this lane for the given GTU type.
1352 */
1353 // TODO this should return something immutable
1354 public final Map<Lane, GTUDirectionality> nextLanes(final GTUType gtuType)
1355 {
1356 if (this.nextLanes == null)
1357 {
1358 this.nextLanes = new LinkedHashMap<>(1);
1359 }
1360 if (!this.nextLanes.containsKey(gtuType))
1361 {
1362 // TODO determine if this should synchronize on this.nextLanes
1363 Map<Lane, GTUDirectionality> laneMap = new LinkedHashMap<>(1);
1364 this.nextLanes.put(gtuType, laneMap);
1365 // Construct (and cache) the result.
1366 for (Link link : getParentLink().getEndNode().getLinks())
1367 {
1368 if (!(link.equals(this.getParentLink())) && link instanceof CrossSectionLink)
1369 {
1370 for (CrossSectionElement cse : ((CrossSectionLink) link).getCrossSectionElementList())
1371 {
1372 if (cse instanceof Lane)
1373 {
1374 Laneef="../../../../../org/opentrafficsim/road/network/lane/Lane.html#Lane">Lane lane = (Lane) cse;
1375 Length jumpToStart = this.getCenterLine().getLast().distance(lane.getCenterLine().getFirst());
1376 Length jumpToEnd = this.getCenterLine().getLast().distance(lane.getCenterLine().getLast());
1377 // this, parentLink ---> O ---> lane, link
1378 if (jumpToStart.lt(MARGIN) && jumpToStart.lt(jumpToEnd)
1379 && link.getStartNode().equals(getParentLink().getEndNode()))
1380 {
1381 // Would the GTU move in the design line direction or against it?
1382 // TODO And is it aligned with its next lane?
1383 if (gtuType == null || lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_PLUS))
1384 {
1385 laneMap.put(lane, GTUDirectionality.DIR_PLUS);
1386 }
1387 else if (lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_MINUS))// getDirectionality(gtuType).isBackwardOrBoth())
1388 {
1389 laneMap.put(lane, GTUDirectionality.DIR_MINUS);
1390 }
1391 }
1392 // this, parentLink ---> O <--- lane, link
1393 else if (jumpToEnd.lt(MARGIN) && jumpToEnd.lt(jumpToStart)
1394 && link.getEndNode().equals(getParentLink().getEndNode()))
1395 {
1396 // Would the GTU move in the design line direction or against it?
1397 // TODO And is it aligned with its next lane?
1398 if (lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_PLUS))// getDirectionality(gtuType).isForwardOrBoth())
1399 {
1400 laneMap.put(lane, GTUDirectionality.DIR_PLUS);
1401 }
1402 else if (gtuType == null
1403 || lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_MINUS))// getDirectionality(gtuType).isBackwardOrBoth())
1404 {
1405 laneMap.put(lane, GTUDirectionality.DIR_MINUS);
1406 }
1407 }
1408 // else: not a "connected" lane
1409 }
1410 }
1411 }
1412 }
1413 }
1414 return this.nextLanes.get(gtuType);
1415 }
1416
1417 /**
1418 * PrevLanes returns the predecessor lane(s) relative to the design line direction, if any exist.<br>
1419 * The previous lane(s) are cached, as it is too expensive to make the calculation every time. There are several
1420 * possibilities: (1) Returning an empty set when there is no predecessor lane relative to the design direction or there is
1421 * no longitudinal transfer possible to a predecessor lane relative to the design direction. (2) Returning a set with just
1422 * one lane if the lateral position of the predecessor lane matches the lateral position of this lane (based on an overlap
1423 * of the lateral positions of the two joining lanes of more than a certain percentage). (3) Multiple lanes in case the Node
1424 * where the underlying Link for this Lane has multiple "incoming" Links, and there are multiple lanes that match the
1425 * lateral position of this lane.<br>
1426 * The previous lanes can differ per GTU type. For instance, a lane where cars and buses are allowed can be preceded by a
1427 * lane where only buses are allowed.
1428 * @param gtuType the GTU type for which we return the next lanes, use {@code null} to return all prev lanes and their
1429 * design direction
1430 * @return set of Lanes following this lane for the given GTU type.
1431 */
1432 // TODO this should return something immutable
1433 public final Map<Lane, GTUDirectionality> prevLanes(final GTUType gtuType)
1434 {
1435 if (this.prevLanes == null)
1436 {
1437 this.prevLanes = new LinkedHashMap<>(1);
1438 }
1439 if (!this.prevLanes.containsKey(gtuType))
1440 {
1441 Map<Lane, GTUDirectionality> laneMap = new LinkedHashMap<>(1);
1442 this.prevLanes.put(gtuType, laneMap);
1443 // Construct (and cache) the result.
1444 for (Link link : getParentLink().getStartNode().getLinks())
1445 {
1446 if (!(link.equals(this.getParentLink())) && link instanceof CrossSectionLink)
1447 {
1448 for (CrossSectionElement cse : ((CrossSectionLink) link).getCrossSectionElementList())
1449 {
1450 if (cse instanceof Lane)
1451 {
1452 Laneef="../../../../../org/opentrafficsim/road/network/lane/Lane.html#Lane">Lane lane = (Lane) cse;
1453 Length jumpToStart = this.getCenterLine().getFirst().distance(lane.getCenterLine().getFirst());
1454 Length jumpToEnd = this.getCenterLine().getFirst().distance(lane.getCenterLine().getLast());
1455 // lane, link <---- O ----> this, parentLink
1456 if (jumpToStart.lt(MARGIN) && jumpToStart.lt(jumpToEnd)
1457 && link.getStartNode().equals(getParentLink().getStartNode()))
1458 {
1459 // does the GTU move in the design line direction or against it?
1460 // TODO And is it aligned with its next lane?
1461 if (lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_PLUS))// getDirectionality(gtuType).isForwardOrBoth())
1462 {
1463 laneMap.put(lane, GTUDirectionality.DIR_PLUS);
1464 }
1465 else if (gtuType == null
1466 || lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_MINUS))// getDirectionality(gtuType).isBackwardOrBoth())
1467 {
1468 laneMap.put(lane, GTUDirectionality.DIR_MINUS);
1469 }
1470 }
1471 // lane, link ----> O ----> this, parentLink
1472 else if (jumpToEnd.lt(MARGIN) && jumpToEnd.lt(jumpToStart)
1473 && link.getEndNode().equals(getParentLink().getStartNode()))
1474 {
1475 // does the GTU move in the design line direction or against it?
1476 // TODO And is it aligned with its next lane?
1477 if (gtuType == null || lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_PLUS))// getDirectionality(gtuType).isForwardOrBoth())
1478 {
1479 laneMap.put(lane, GTUDirectionality.DIR_PLUS);
1480 }
1481 else if (lane.getLaneType().isCompatible(gtuType, GTUDirectionality.DIR_MINUS))// getDirectionality(gtuType).isBackwardOrBoth())
1482 {
1483 laneMap.put(lane, GTUDirectionality.DIR_MINUS);
1484 }
1485 }
1486 // else: not a "connected" lane
1487 }
1488 }
1489 }
1490 }
1491 }
1492 return this.prevLanes.get(gtuType);
1493 }
1494
1495 /**
1496 * Returns the lanes that could be followed in a given direction and for the given GTU type.
1497 * @param direction GTUDirectionality; gtu direction
1498 * @param gtuType GTUType; gtu type
1499 * @return lanes that can be followed in a given direction and for the given GTU type
1500 */
1501 public final synchronized ImmutableMap<Lane, GTUDirectionality> downstreamLanes(final GTUDirectionality direction,
1502 final GTUType gtuType)
1503 {
1504 return this.downLanes.get(() -> {
1505 Map<Lane, GTUDirectionality> downMap =
1506 new LinkedHashMap<>(direction.isPlus() ? nextLanes(gtuType) : prevLanes(gtuType)); // safe copy
1507 Node downNode = direction.isPlus() ? getParentLink().getEndNode() : getParentLink().getStartNode();
1508 Iterator<Entry<Lane, GTUDirectionality>> iterator = downMap.entrySet().iterator();
1509 while (iterator.hasNext())
1510 {
1511 Entry<Lane, GTUDirectionality> entry = iterator.next();
1512 if ((entry.getValue().isPlus() && !entry.getKey().getParentLink().getStartNode().equals(downNode))
1513 || (entry.getValue().isMinus() && !entry.getKey().getParentLink().getEndNode().equals(downNode)))
1514 {
1515 // cannot move onto this lane
1516 iterator.remove();
1517 }
1518 }
1519 return new ImmutableLinkedHashMap<>(downMap, Immutable.WRAP);
1520 }, gtuType, direction);
1521 }
1522
1523 /**
1524 * Returns the lanes that could precede in a given direction and for the given GTU type.
1525 * @param direction GTUDirectionality; gtu direction
1526 * @param gtuType GTUType; gtu type
1527 * @return lanes that can be followed in a given direction and for the given GTU type
1528 */
1529 public final synchronized ImmutableMap<Lane, GTUDirectionality> upstreamLanes(final GTUDirectionality direction,
1530 final GTUType gtuType)
1531 {
1532 return this.upLanes.get(() -> {
1533 Map<Lane, GTUDirectionality> upMap =
1534 new LinkedHashMap<>(direction.isPlus() ? prevLanes(gtuType) : nextLanes(gtuType)); // safe copy
1535 Node upNode = direction.isPlus() ? getParentLink().getStartNode() : getParentLink().getEndNode();
1536 Iterator<Entry<Lane, GTUDirectionality>> iterator = upMap.entrySet().iterator();
1537 while (iterator.hasNext())
1538 {
1539 Entry<Lane, GTUDirectionality> entry = iterator.next();
1540 if ((entry.getValue().isPlus() && !entry.getKey().getParentLink().getEndNode().equals(upNode))
1541 || (entry.getValue().isMinus() && !entry.getKey().getParentLink().getStartNode().equals(upNode)))
1542 {
1543 // cannot have come from this lane
1544 iterator.remove();
1545 }
1546 }
1547 return new ImmutableLinkedHashMap<>(upMap, Immutable.WRAP);
1548 }, gtuType, direction);
1549 }
1550
1551 /**
1552 * 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
1553 * if no lane could be found. The method ignores all legal restrictions such as allowable directions and stripes.<br>
1554 * A lane is called adjacent to another lane if the lateral edges are not more than a delta distance apart. This means that
1555 * a lane that <i>overlaps</i> with another lane is <b>not</b> returned as an adjacent lane. <br>
1556 * <b>Note:</b> LEFT and RIGHT are seen from the direction of the GTU, in its forward driving direction. <br>
1557 * @param lateralDirection LateralDirectionality; LEFT or RIGHT.
1558 * @param gtuType GTUType; the type of GTU for which to return the adjacent lanes.
1559 * @param drivingDirection GTUDirectionality; the driving direction of the GTU on <code>this</code> Lane
1560 * @return the set of lanes that are accessible, or null if there is no lane that is accessible with a matching driving
1561 * direction.
1562 */
1563 public final Set<Lane> accessibleAdjacentLanesPhysical(final LateralDirectionality lateralDirection, final GTUType gtuType,
1564 final GTUDirectionality drivingDirection)
1565 {
1566 LateralDirectionality dir =
1567 drivingDirection.equals(GTUDirectionality.DIR_PLUS) ? lateralDirection : lateralDirection.flip();
1568 return neighbors(dir, gtuType, drivingDirection, false);
1569 }
1570
1571 /**
1572 * 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
1573 * if no lane could be found. The method takes the LongitidinalDirectionality of the lane into account. In other words, if
1574 * 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
1575 * lane is not DIR_PLUS or DIR_BOTH, it will not be included.<br>
1576 * A lane is called adjacent to another lane if the lateral edges are not more than a delta distance apart. This means that
1577 * a lane that <i>overlaps</i> with another lane is <b>not</b> returned as an adjacent lane. <br>
1578 * <b>Note:</b> LEFT and RIGHT are seen from the direction of the GTU, in its forward driving direction. <br>
1579 * @param lateralDirection LateralDirectionality; LEFT or RIGHT.
1580 * @param gtuType GTUType; the type of GTU for which to return the adjacent lanes.
1581 * @param drivingDirection GTUDirectionality; the driving direction of the GTU on <code>this</code> Lane
1582 * @return the set of lanes that are accessible, or null if there is no lane that is accessible with a matching driving
1583 * direction.
1584 */
1585 public final Set<Lane> accessibleAdjacentLanesLegal(final LateralDirectionality lateralDirection, final GTUType gtuType,
1586 final GTUDirectionality drivingDirection)
1587 {
1588 Set<Lane> candidates = new LinkedHashSet<>(1);
1589 LateralDirectionality dir =
1590 drivingDirection.equals(GTUDirectionality.DIR_PLUS) ? lateralDirection : lateralDirection.flip();
1591 for (Lane lane : neighbors(dir, gtuType, drivingDirection, true))
1592 {
1593 if (lane.getLaneType().isCompatible(gtuType, drivingDirection))
1594 {
1595 candidates.add(lane);
1596 }
1597 }
1598 return candidates;
1599 }
1600
1601 /**
1602 * 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
1603 * 90 km/h.
1604 * @param gtuType GTUType; the GTU type to provide the speed limit for
1605 * @return the speedLimit.
1606 * @throws NetworkException on network inconsistency
1607 */
1608 public final Speed getSpeedLimit(final GTUType gtuType) throws NetworkException
1609 {
1610 Speed speedLimit = this.cachedSpeedLimits.get(gtuType);
1611 if (speedLimit == null)
1612 {
1613 if (this.speedLimitMap.containsKey(gtuType))
1614 {
1615 speedLimit = this.speedLimitMap.get(gtuType);
1616 }
1617 else if (gtuType.getParent() != null)
1618 {
1619 speedLimit = getSpeedLimit(gtuType.getParent());
1620 }
1621 else
1622 {
1623 throw new NetworkException("No speed limit set for GTUType " + gtuType + " on lane " + toString());
1624 }
1625 this.cachedSpeedLimits.put(gtuType, speedLimit);
1626 }
1627 return speedLimit;
1628 }
1629
1630 /**
1631 * Get the lowest speed limit of this lane.
1632 * @return the lowest speedLimit.
1633 * @throws NetworkException on network inconsistency
1634 */
1635 public final Speed getLowestSpeedLimit() throws NetworkException
1636 {
1637 Throw.when(this.speedLimitMap.isEmpty(), NetworkException.class, "Lane %s has no speed limits set.", toString());
1638 Speed out = Speed.POSITIVE_INFINITY;
1639 for (GTUType gtuType : this.speedLimitMap.keySet())
1640 {
1641 out = Speed.min(out, this.speedLimitMap.get(gtuType));
1642 }
1643 return out;
1644 }
1645
1646 /**
1647 * Get the highest speed limit of this lane.
1648 * @return the highest speedLimit.
1649 * @throws NetworkException on network inconsistency
1650 */
1651 public final Speed getHighestSpeedLimit() throws NetworkException
1652 {
1653 Throw.when(this.speedLimitMap.isEmpty(), NetworkException.class, "Lane %s has no speed limits set.", toString());
1654 Speed out = Speed.ZERO;
1655 for (GTUType gtuType : this.speedLimitMap.keySet())
1656 {
1657 out = Speed.max(out, this.speedLimitMap.get(gtuType));
1658 }
1659 return out;
1660 }
1661
1662 /**
1663 * 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
1664 * 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
1665 * used additive, or subtractive. <br>
1666 * In <b>additive use</b>, do not set the speed limit for GTUType.ALL. Now, one by one, the allowed maximum speeds for each
1667 * 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.
1668 * <br>
1669 * In <b>subtractive use</b>, set the speed limit for GTUType.ALL to the most common one. Override the speed limit for
1670 * certain GTUTypes to a different value. An example is a lane on a highway where all vehicles, except truck (CAR, BUS,
1671 * MOTORCYCLE, etc.), can drive 120 km/h, but trucks are allowed only 90 km/h. In that case, set the speed limit for
1672 * GTUType.ALL to 120 km/h, and for TRUCK to 90 km/h.
1673 * @param gtuType GTUType; the GTU type to provide the speed limit for
1674 * @param speedLimit Speed; the speed limit for this gtu type
1675 */
1676 public final void setSpeedLimit(final GTUType gtuType, final Speed speedLimit)
1677 {
1678 this.speedLimitMap.put(gtuType, speedLimit);
1679 this.cachedSpeedLimits.clear();
1680 }
1681
1682 /**
1683 * Remove the set speed limit for a GTUType. If the speed limit for GTUType.ALL will be removed, there will not be a
1684 * 'default' speed limit anymore. If the speed limit for a certain GTUType is removed, its speed limit will default to the
1685 * speed limit of GTUType.ALL. <br>
1686 * <b>Note</b>: if no speed limit is known for a GTUType, getSpeedLimit will throw a NetworkException when the speed limit
1687 * is retrieved for that GTUType.
1688 * @param gtuType GTUType; the GTU type to provide the speed limit for
1689 */
1690 public final void removeSpeedLimit(final GTUType gtuType)
1691 {
1692 this.speedLimitMap.remove(gtuType);
1693 this.cachedSpeedLimits.clear();
1694 }
1695
1696 /**
1697 * @return laneType.
1698 */
1699 public final LaneType getLaneType()
1700 {
1701 return this.laneType;
1702 }
1703
1704 /**
1705 * This method sets the directionality of the lane for a GTU type. It might be that the driving direction in the lane is
1706 * 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
1707 * (i.e., bicycles can also go in the other direction; we see this on some city streets). If the directionality for a
1708 * GTUType is set to NONE, this means that the given GTUType cannot use the Lane. If a Directionality is set for
1709 * GTUType.ALL, the getDirectionality will default to these settings when there is no specific entry for a given
1710 * directionality. This means that the settings can be used additive, or restrictive. <br>
1711 * In <b>additive use</b>, set the directionality for GTUType.ALL to NONE, or do not set the directionality for GTUType.ALL.
1712 * Now, one by one, the allowed directionalities can be added. An example is a lane on a highway, which we only open for
1713 * CAR, TRUCK and BUS. <br>
1714 * In <b>restrictive use</b>, set the directionality for GTUType.ALL to BOTH, FORWARD, or BACKWARD. Override the
1715 * directionality for certain GTUTypes to a more restrictive access, e.g. to NONE. An example is a lane that is open for all
1716 * road users, except TRUCK.
1717 * @param gtuType the GTU type to set the directionality for.
1718 * @param directionality the longitudinal directionality of the link (FORWARD, BACKWARD, BOTH or NONE) for the given GTU
1719 * type.
1720 * @throws NetworkException when the lane directionality for the given GTUType is inconsistent with the Link directionality
1721 * to which the lane belongs.
1722 */
1723 // public final void addDirectionality(final GTUType gtuType, final LongitudinalDirectionality directionality)
1724 // throws NetworkException
1725 // {
1726 // this.directionalityMap.put(gtuType, directionality);
1727 // checkDirectionality();
1728 // }
1729
1730 /**
1731 * This method removes an earlier provided directionality of the lane for a given GTU type, e.g. for maintenance of the
1732 * lane. After removing, the directionality for the GTU will fall back to the provided directionality for GTUType.ALL (if
1733 * present). Thereby removing a directionality is different from setting the directionality to NONE.
1734 * @param gtuType the GTU type to remove the directionality for on this lane.
1735 */
1736 // public final void removeDirectionality(final GTUType gtuType)
1737 // {
1738 // this.directionalityMap.remove(gtuType);
1739 // }
1740
1741 /**
1742 * Check whether the directionalities for the GTU types for this lane are consistent with the directionalities of the
1743 * overarching Link.
1744 * @throws NetworkException when the lane directionality for a given GTUType is inconsistent with the Link directionality to
1745 * which the lane belongs.
1746 */
1747 private void checkDirectionality() throws NetworkException
1748 {
1749 // TODO check that the directionality of this Lane does not conflict with that of the parent the OTSLink
1750 // for (GTUType gtuType : this.directionalityMap.keySet())
1751 // {
1752 // LongitudinalDirectionality directionality = this.directionalityMap.get(gtuType);
1753 // if (!getParentLink().getDirectionality(gtuType).contains(directionality))
1754 // {
1755 // throw new NetworkException("Lane " + toString() + " allows " + gtuType + " a directionality of "
1756 // + directionality + " which is not present in the overarching link " + getParentLink().toString());
1757 // }
1758 // }
1759 }
1760
1761 /**
1762 * @return gtuList.
1763 */
1764 public final ImmutableList<LaneBasedGTU> getGtuList()
1765 {
1766 // TODO let HistoricalArrayList return an Immutable (WRAP) of itself
1767 return this.gtuList == null ? new ImmutableArrayList<>(new ArrayList<>())
1768 : new ImmutableArrayList<>(this.gtuList, Immutable.COPY);
1769 }
1770
1771 /**
1772 * Returns the list of GTU's at the specified time.
1773 * @param time Time; time
1774 * @return list of GTU's at the specified times
1775 */
1776 public final List<LaneBasedGTU> getGtuList(final Time time)
1777 {
1778 if (time.equals(this.gtuListTime))
1779 {
1780 return this.gtuListAtTime;
1781 }
1782 this.gtuListTime = time;
1783 this.gtuListAtTime = this.gtuList == null ? new ArrayList<>() : this.gtuList.get(time);
1784 return this.gtuListAtTime;
1785 }
1786
1787 /**
1788 * Returns the number of GTU's.
1789 * @return int; number of GTU's.
1790 */
1791 public final int numberOfGtus()
1792 {
1793 return this.gtuList.size();
1794 }
1795
1796 /**
1797 * Returns the number of GTU's at specified time.
1798 * @param time Time; time
1799 * @return int; number of GTU's.
1800 */
1801 public final int numberOfGtus(final Time time)
1802 {
1803 return getGtuList(time).size();
1804 }
1805
1806 /**
1807 * Returns the index of the given GTU, or -1 if not present.
1808 * @param gtu LaneBasedGTU; gtu to get the index of
1809 * @return int; index of the given GTU, or -1 if not present
1810 */
1811 public final int indexOfGtu(final LaneBasedGTU gtu)
1812 {
1813 return Collections.binarySearch(this.gtuList, gtu, (gtu1, gtu2) -> {
1814 try
1815 {
1816 return gtu1.position(this, gtu1.getReference()).compareTo(gtu2.position(this, gtu2.getReference()));
1817 }
1818 catch (GTUException exception)
1819 {
1820 throw new RuntimeException(exception);
1821 }
1822 });
1823 }
1824
1825 /**
1826 * Returns the index of the given GTU, or -1 if not present, at specified time.
1827 * @param gtu LaneBasedGTU; gtu to get the index of
1828 * @param time Time; time
1829 * @return int; index of the given GTU, or -1 if not present
1830 */
1831 public final int indexOfGtu(final LaneBasedGTU gtu, final Time time)
1832 {
1833 return Collections.binarySearch(getGtuList(time), gtu, (gtu1, gtu2) -> {
1834 try
1835 {
1836 return Double.compare(gtu1.fractionalPosition(this, gtu1.getReference(), time),
1837 gtu2.fractionalPosition(this, gtu2.getReference(), time));
1838 }
1839 catch (GTUException exception)
1840 {
1841 throw new RuntimeException(exception);
1842 }
1843 });
1844 }
1845
1846 /**
1847 * Returns the index'th GTU.
1848 * @param index int; index of the GTU
1849 * @return LaneBasedGTU; the index'th GTU
1850 */
1851 public final LaneBasedGTU getGtu(final int index)
1852 {
1853 return this.gtuList.get(index);
1854 }
1855
1856 /**
1857 * Returns the index'th GTU at specified time.
1858 * @param index int; index of the GTU
1859 * @param time Time; time
1860 * @return LaneBasedGTU; the index'th GTU
1861 */
1862 public final LaneBasedGTU getGtu(final int index, final Time time)
1863 {
1864 return getGtuList(time).get(index);
1865 }
1866
1867 /** {@inheritDoc} */
1868 @Override
1869 @SuppressWarnings("checkstyle:designforextension")
1870 public double getZ()
1871 {
1872 return 0.0;
1873 }
1874
1875 /** {@inheritDoc} */
1876 @Override
1877 public final String toString()
1878 {
1879 CrossSectionLink link = getParentLink();
1880 return String.format("Lane %s of %s", getId(), link.getId());
1881 }
1882
1883 /** Cache of the hashCode. */
1884 private Integer cachedHashCode = null;
1885
1886 /** {@inheritDoc} */
1887 @SuppressWarnings("checkstyle:designforextension")
1888 @Override
1889 public int hashCode()
1890 {
1891 if (this.cachedHashCode == null)
1892 {
1893 final int prime = 31;
1894 int result = super.hashCode();
1895 result = prime * result + ((this.laneType == null) ? 0 : this.laneType.hashCode());
1896 this.cachedHashCode = result;
1897 }
1898 return this.cachedHashCode;
1899 }
1900
1901 /** {@inheritDoc} */
1902 @SuppressWarnings({ "checkstyle:designforextension", "checkstyle:needbraces" })
1903 @Override
1904 public boolean equals(final Object obj)
1905 {
1906 if (this == obj)
1907 return true;
1908 if (!super.equals(obj))
1909 return false;
1910 if (getClass() != obj.getClass())
1911 return false;
1912 Lanef="../../../../../org/opentrafficsim/road/network/lane/Lane.html#Lane">Lane other = (Lane) obj;
1913 if (this.laneType == null)
1914 {
1915 if (other.laneType != null)
1916 return false;
1917 }
1918 else if (!this.laneType.equals(other.laneType))
1919 return false;
1920 return true;
1921 }
1922
1923 /** {@inheritDoc} */
1924 @Override
1925 @SuppressWarnings("checkstyle:designforextension")
1926 public Lane clone(final CrossSectionLink newParentLink, final SimulatorInterface.TimeDoubleUnit newSimulator)
1927 throws NetworkException
1928 {
1929 Lanenetwork/lane/Lane.html#Lane">Lane newLane = new Lane(newParentLink, this);
1930 // nextLanes, prevLanes, nextNeighbors, rightNeighbors are filled at first request
1931
1932 SortedMap<Double, List<SingleSensor>> newSensorMap = new TreeMap<>();
1933 for (double distance : this.sensors.keySet())
1934 {
1935 List<SingleSensor> newSensorList = new ArrayList<>();
1936 for (SingleSensor sensor : this.sensors.get(distance))
1937 {
1938 SingleSensor newSensor = ((AbstractSensor) sensor).clone(newLane, newSimulator);
1939 newSensorList.add(newSensor);
1940 }
1941 newSensorMap.put(distance, newSensorList);
1942 }
1943 newLane.sensors.clear();
1944 newLane.sensors.putAll(newSensorMap);
1945
1946 SortedMap<Double, List<LaneBasedObject>> newLaneBasedObjectMap = new TreeMap<>();
1947 for (double distance : this.laneBasedObjects.keySet())
1948 {
1949 List<LaneBasedObject> newLaneBasedObjectList = new ArrayList<>();
1950 for (LaneBasedObject lbo : this.laneBasedObjects.get(distance))
1951 {
1952 AbstractLaneBasedObjectfficsim/road/network/lane/object/AbstractLaneBasedObject.html#AbstractLaneBasedObject">AbstractLaneBasedObject laneBasedObject = (AbstractLaneBasedObject) lbo;
1953 LaneBasedObject newLbo = laneBasedObject.clone(newLane, newSimulator);
1954 newLaneBasedObjectList.add(newLbo);
1955 }
1956 newLaneBasedObjectMap.put(distance, newLaneBasedObjectList);
1957 }
1958 newLane.laneBasedObjects.clear();
1959 newLane.laneBasedObjects.putAll(newLaneBasedObjectMap);
1960
1961 return newLane;
1962 }
1963
1964 /**
1965 * Functional interface that can be used for line searches of objects on the lane.
1966 * <p>
1967 * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
1968 * <br>
1969 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
1970 * <p>
1971 * @version $Revision$, $LastChangedDate$, by $Author$, initial version 28 jan. 2018 <br>
1972 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
1973 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
1974 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
1975 */
1976 private interface Positions
1977 {
1978 /**
1979 * Returns the position of the index'th element.
1980 * @param index int; index
1981 * @return double; position of the index'th element
1982 * @throws GTUException on exception
1983 */
1984 double get(int index) throws GTUException;
1985 }
1986
1987 }