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