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