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