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