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