1 package org.opentrafficsim.road.gtu.lane.perception.structure;
2
3 import java.util.Collection;
4 import java.util.Collections;
5 import java.util.Deque;
6 import java.util.Iterator;
7 import java.util.LinkedHashMap;
8 import java.util.LinkedHashSet;
9 import java.util.LinkedList;
10 import java.util.List;
11 import java.util.Map;
12 import java.util.NoSuchElementException;
13 import java.util.Set;
14 import java.util.SortedSet;
15 import java.util.TreeSet;
16 import java.util.function.BiFunction;
17 import java.util.function.Function;
18
19 import org.djunits.value.vdouble.scalar.Length;
20 import org.djunits.value.vdouble.scalar.Time;
21 import org.djutils.exceptions.Throw;
22 import org.djutils.exceptions.Try;
23 import org.opentrafficsim.core.gtu.RelativePosition;
24 import org.opentrafficsim.core.network.LateralDirectionality;
25 import org.opentrafficsim.core.network.Link;
26 import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
27 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
28 import org.opentrafficsim.road.network.lane.Lane;
29 import org.opentrafficsim.road.network.lane.LanePosition;
30 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
31
32 /**
33 * The lane structure provides a way to see the world for a lane based model.
34 * <p>
35 * Copyright (c) 2024-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
36 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
37 * </p>
38 * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
39 */
40 public class LaneStructure
41 {
42
43 /** GTU. */
44 private final LaneBasedGtu gtu;
45
46 /** Length to build lane structure upstream of GTU, or upstream relative to a downstream merge. */
47 private Length upstream;
48
49 /** Length to build lane structure downstream of GTU. */
50 private Length downstream;
51
52 /** Time at which the structure was updated. */
53 private Time updated = null;
54
55 /** Cross section of lane records at different relative lanes. */
56 private final Map<RelativeLane, Set<LaneRecord>> crossSection = new LinkedHashMap<>();
57
58 /** Cross section of lane records directly found laterally from the root. */
59 private final Map<RelativeLane, LaneRecord> rootCrossSection = new LinkedHashMap<>();
60
61 /**
62 * Constructor.
63 * @param gtu LaneBasedGtu; the GTU.
64 * @param upstream Length; guaranteed distance within which objects are found upstream of the GTU, or upstream of downstream
65 * merge.
66 * @param downstream Length; guaranteed distance within which objects are found downstream of the GTU.
67 */
68 public LaneStructure(final LaneBasedGtu gtu, final Length upstream, final Length downstream)
69 {
70 this.gtu = gtu;
71 this.upstream = upstream;
72 this.downstream = downstream;
73 }
74
75 /**
76 * Returns an iterator over objects perceived on a relative lane, ordered close to far. This can be objects on different
77 * roads, e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two
78 * on-ramps that are very close by, or even the shoulder. Objects that are partially downstream are also included.
79 * @param <T> type of {@code LaneBasedObject}.
80 * @param relativeLane RelativeLane; lane.
81 * @param clazz Class<T>; class of lane-based object type.
82 * @param position RelativePosition.Type; position relative to which objects are found and distances are given.
83 * @param onRoute boolean; whether the objects have to be on-route.
84 * @return Iterator<Entry<T>>; iterator over objects.
85 */
86 public <T extends LaneBasedObject> Iterable<Entry<T>> getDownstreamObjects(final RelativeLane relativeLane,
87 final Class<T> clazz, final RelativePosition.Type position, final boolean onRoute)
88 {
89 update();
90 Length dx = LaneStructure.this.gtu.getRelativePositions().get(position).dx();
91 return new NavigatingIterable<>(relativeLane, clazz, this.downstream, (record) -> startDownstream(record, position),
92 (record) ->
93 {
94 // this navigator only includes records of lanes on the route
95 Set<LaneRecord> set = record.getNext();
96 set.removeIf((r) -> onRoute && !r.isOnRoute(LaneStructure.this.gtu.getStrategicalPlanner().getRoute()));
97 return set;
98 }, (record) ->
99 {
100 // this navigator selects all objects fully or partially downstream
101 List<LaneBasedObject> list = record.getLane().getLaneBasedObjects();
102 if (list.isEmpty())
103 {
104 return list;
105 }
106 Length pos = record.getStartDistance().neg().plus(dx);
107 int from = 0;
108 while (from < list.size()
109 && list.get(from).getLongitudinalPosition().plus(list.get(from).getLength()).lt(pos))
110 {
111 from++;
112 }
113 if (from > list.size() - 1)
114 {
115 return Collections.EMPTY_LIST;
116 }
117 return list.subList(from, list.size());
118 }, (t, r) -> r.getStartDistance().plus(t.getLongitudinalPosition()).minus(dx));
119 }
120
121 /**
122 * Returns an iterator over objects perceived on a relative lane, ordered close to far. This can be objects on different
123 * roads, e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two
124 * on-ramps that are very close by, or even the shoulder. Objects that are partially upstream are also included.
125 * @param <T> type of {@code LaneBasedObject}.
126 * @param relativeLane RelativeLane; lane.
127 * @param clazz Class<T>; class of lane-based object type.
128 * @param position RelativePosition.Type; position relative to which objects are found and distances are given.
129 * @return Iterator<Entry<T>>; iterator over objects.
130 */
131 public <T extends LaneBasedObject> Iterable<Entry<T>> getUpstreamObjects(final RelativeLane relativeLane,
132 final Class<T> clazz, final RelativePosition.Type position)
133 {
134 update();
135 Length dx = LaneStructure.this.gtu.getRelativePositions().get(position).dx();
136 return new NavigatingIterable<T>(relativeLane, clazz, this.upstream, (record) -> startUpstream(record, position),
137 (record) ->
138 {
139 // this navigator combines the upstream and lateral records
140 Set<LaneRecord> set = new LinkedHashSet<>(record.getPrev());
141 set.addAll(record.lateral());
142 return set;
143 }, (record) ->
144 {
145 // this lister reverses the list
146 List<LaneBasedObject> list = record.getLane().getLaneBasedObjects();
147 if (list.isEmpty())
148 {
149 return list;
150 }
151 Length pos = record.getStartDistance().neg().plus(dx);
152 int to = list.size();
153 while (to >= 0 && list.get(to).getLongitudinalPosition().gt(pos))
154 {
155 to--;
156 }
157 if (to < 0)
158 {
159 return Collections.EMPTY_LIST;
160 }
161 list = list.subList(0, to);
162 Collections.reverse(list);
163 return list;
164 }, (t, r) -> r.getStartDistance().plus(t.getLongitudinalPosition()).plus(dx).neg());
165 }
166
167 /**
168 * Returns an iterator over GTUs perceived on a relative lane, ordered close to far. This can be GTUs on different roads,
169 * e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two on-ramps
170 * that are very close by, or even the shoulder.
171 * @param relativeLane RelativeLane; lane.
172 * @param egoPosition RelativePosition; position of ego GTU relative to which objects are found.
173 * @param otherPosition RelativePosition; position of other GTU that must be downstream of egoPosition.
174 * @param egoDistancePosition RelativePosition; position of ego GTU from which the distance is determined.
175 * @param otherDistancePosition RelativePosition; position of other GTU to which the distance is determined.
176 * @return Iterator<Entry<LaneBasedGtu>>; iterator over GTUs.
177 */
178 public Iterable<Entry<LaneBasedGtu>> getDownstreamGtus(final RelativeLane relativeLane,
179 final RelativePosition.Type egoPosition, final RelativePosition.Type otherPosition,
180 final RelativePosition.Type egoDistancePosition, final RelativePosition.Type otherDistancePosition)
181 {
182 update();
183 Length dx = LaneStructure.this.gtu.getRelativePositions().get(egoPosition).dx();
184 Length dxDistance = LaneStructure.this.gtu.getRelativePositions().get(egoDistancePosition).dx();
185 return new NavigatingIterable<>(relativeLane, LaneBasedGtu.class, this.downstream,
186 (record) -> startDownstream(record, egoPosition), (record) -> record.getNext(), (record) ->
187 {
188 // this lister finds the relevant sublist of GTUs
189 List<LaneBasedGtu> gtus = record.getLane().getGtuList().toList();
190 if (gtus.isEmpty())
191 {
192 return gtus;
193 }
194 int from = 0;
195 Length pos = Length.max(record.getStartDistance().neg().plus(dx), Length.ZERO);
196 while (from < gtus.size() && (position(gtus.get(from), record, otherPosition).lt(pos)
197 || gtus.get(from).getId().equals(this.gtu.getId())))
198 {
199 from++;
200 }
201 int to = gtus.size() - 1;
202 while (to >= 0 && (position(gtus.get(to), record, otherPosition).gt(record.getLane().getLength())
203 || gtus.get(to).getId().equals(this.gtu.getId())))
204 {
205 to--;
206 }
207 if (from > to)
208 {
209 return Collections.EMPTY_LIST;
210 }
211 if (from > 0 || to < gtus.size() - 1)
212 {
213 gtus = gtus.subList(from, to + 1);
214 }
215 return gtus;
216 }, (t, r) -> r.getStartDistance().plus(position(t, r, otherDistancePosition)).minus(dxDistance));
217 }
218
219 /**
220 * Returns an iterator over GTUs perceived on a relative lane, ordered close to far. This can be GTUs on different roads,
221 * e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two on-ramps
222 * that are very close by, or even the shoulder.
223 * @param relativeLane RelativeLane; lane.
224 * @param egoPosition RelativePosition; position of ego GTU relative to which objects are found.
225 * @param otherPosition RelativePosition; position of other GTU that must be upstream of egoPosition.
226 * @param egoDistancePosition RelativePosition; position of ego GTU from which the distance is determined.
227 * @param otherDistancePosition RelativePosition; position of other GTU to which the distance is determined.
228 * @return Iterator<Entry<LaneBasedGtu>>; iterator over GTUs.
229 */
230 public Iterable<Entry<LaneBasedGtu>> getUpstreamGtus(final RelativeLane relativeLane,
231 final RelativePosition.Type egoPosition, final RelativePosition.Type otherPosition,
232 final RelativePosition.Type egoDistancePosition, final RelativePosition.Type otherDistancePosition)
233 {
234 update();
235 Length dx = LaneStructure.this.gtu.getRelativePositions().get(egoPosition).dx();
236 Length dxDistance = LaneStructure.this.gtu.getRelativePositions().get(egoDistancePosition).dx();
237 return new NavigatingIterable<>(relativeLane, LaneBasedGtu.class, this.upstream,
238 (record) -> startUpstream(record, egoPosition), (record) ->
239 {
240 // this navigator combines the upstream and lateral records
241 Set<LaneRecord> set = new LinkedHashSet<>(record.getPrev());
242 set.addAll(record.lateral());
243 return set;
244 }, (record) ->
245 {
246 // this lister finds the relevant sublist of GTUs and reverses it
247 List<LaneBasedGtu> gtus = record.getLane().getGtuList().toList();
248 if (gtus.isEmpty())
249 {
250 return gtus;
251 }
252 int from = 0;
253 while (from < gtus.size() && (position(gtus.get(from), record, otherPosition).lt0()
254 || gtus.get(from).getId().equals(this.gtu.getId())))
255 {
256 from++;
257 }
258 int to = gtus.size() - 1;
259 Length pos = Length.min(record.getStartDistance().neg().plus(dx), record.getLane().getLength());
260 while (to >= 0 && (position(gtus.get(to), record, otherPosition).gt(pos)
261 || gtus.get(to).getId().equals(this.gtu.getId())))
262 {
263 to--;
264 }
265 if (from > to)
266 {
267 return Collections.EMPTY_LIST;
268 }
269 if (from > 0 || to < gtus.size() - 1)
270 {
271 gtus = gtus.subList(from, to + 1);
272 }
273 Collections.reverse(gtus);
274 return gtus;
275 }, (t, r) -> dxDistance.minus(r.getStartDistance().plus(position(t, r, otherDistancePosition))));
276 }
277
278 /**
279 * Returns an iterator over GTUs perceived on a relative lane, ordered close to far. This can be GTUs on different roads,
280 * e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two on-ramps
281 * that are very close by, or even the shoulder. This function differs from {@code getDownstreamGtus()} in that it will halt
282 * further searching on on branch it finds a GTU on.
283 * @param relativeLane RelativeLane; lane.
284 * @param egoPosition RelativePosition; position of ego GTU relative to which objects are found.
285 * @param otherPosition RelativePosition; position of other GTU that must be downstream of egoPosition.
286 * @param egoDistancePosition RelativePosition; position of ego GTU from which the distance is determined.
287 * @param otherDistancePosition RelativePosition; position of other GTU to which the distance is determined.
288 * @return Iterator<Entry<LaneBasedGtu>>; iterator over GTUs.
289 */
290 public Iterable<Entry<LaneBasedGtu>> getFirstDownstreamGtus(final RelativeLane relativeLane,
291 final RelativePosition.Type egoPosition, final RelativePosition.Type otherPosition,
292 final RelativePosition.Type egoDistancePosition, final RelativePosition.Type otherDistancePosition)
293 {
294 update();
295 Length dx = LaneStructure.this.gtu.getRelativePositions().get(egoPosition).dx();
296 Length dxDistance = LaneStructure.this.gtu.getRelativePositions().get(egoDistancePosition).dx();
297 return new NavigatingIterable<>(relativeLane, LaneBasedGtu.class, this.downstream,
298 (record) -> startDownstream(record, egoPosition), (record) ->
299 {
300 // this navigator only returns records when there are no GTUs on the lane
301 return Try.assign(
302 () -> record.getLane().getGtuAhead(record.getStartDistance().neg().plus(dx), otherPosition,
303 record.getLane().getNetwork().getSimulator().getSimulatorAbsTime()),
304 "Problem with GTU") == null ? record.getNext() : new LinkedHashSet<>();
305 }, (record) ->
306 {
307 // this lister finds the first GTU and returns it as the only GTU in the list
308 LaneBasedGtu down =
309 Try.assign(
310 () -> record.getLane().getGtuAhead(record.getStartDistance().neg().plus(dx), otherPosition,
311 record.getLane().getNetwork().getSimulator().getSimulatorAbsTime()),
312 "Problem with GTU");
313 return down == null ? Collections.EMPTY_LIST : List.of(down);
314 }, (t, r) -> r.getStartDistance().plus(position(t, r, otherDistancePosition)).minus(dxDistance));
315 }
316
317 /**
318 * Returns an iterator over GTUs perceived on a relative lane, ordered close to far. This can be GTUs on different roads,
319 * e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two on-ramps
320 * that are very close by, or even the shoulder. This function differs from {@code getDownstreamGtus()} in that it will halt
321 * further searching on on branch it finds a GTU on.
322 * @param relativeLane RelativeLane; lane.
323 * @param egoPosition RelativePosition; position of ego GTU relative to which objects are found.
324 * @param otherPosition RelativePosition; position of other GTU that must be upstream of egoPosition.
325 * @param egoDistancePosition RelativePosition; position of ego GTU from which the distance is determined.
326 * @param otherDistancePosition RelativePosition; position of other GTU to which the distance is determined.
327 * @return Iterator<Entry<LaneBasedGtu>>; iterator over GTUs.
328 */
329 public Iterable<Entry<LaneBasedGtu>> getFirstUpstreamGtus(final RelativeLane relativeLane,
330 final RelativePosition.Type egoPosition, final RelativePosition.Type otherPosition,
331 final RelativePosition.Type egoDistancePosition, final RelativePosition.Type otherDistancePosition)
332 {
333 update();
334 Length dx = LaneStructure.this.gtu.getRelativePositions().get(egoPosition).dx();
335 Length dxDistance = LaneStructure.this.gtu.getRelativePositions().get(egoDistancePosition).dx();
336 return new NavigatingIterable<>(relativeLane, LaneBasedGtu.class, this.upstream,
337 (record) -> startUpstream(record, egoPosition), (record) ->
338 {
339 // this navigator only returns records when there are no GTUs on the lane (it may thus ignore a GTU on a
340 // lateral
341 // lane that is closer) and combines the upstream and lateral records
342 LaneBasedGtu gtu =
343 Try.assign(
344 () -> record.getLane().getGtuBehind(record.getStartDistance().neg().plus(dx), otherPosition,
345 record.getLane().getNetwork().getSimulator().getSimulatorAbsTime()),
346 "Problem with GTU");
347 Set<LaneRecord> set = new LinkedHashSet<>();
348 if (gtu == null)
349 {
350 set.addAll(record.getPrev());
351 set.addAll(record.lateral());
352 }
353 return set;
354 }, (record) ->
355 {
356 // this lister finds the first GTU and returns it as the only GTU in the list
357 LaneBasedGtu up =
358 Try.assign(
359 () -> record.getLane().getGtuBehind(record.getStartDistance().neg().plus(dx), otherPosition,
360 record.getLane().getNetwork().getSimulator().getSimulatorAbsTime()),
361 "Problem with GTU");
362 return up == null ? Collections.EMPTY_LIST : List.of(up);
363 }, (t, r) -> dxDistance.minus(r.getStartDistance().plus(position(t, r, otherDistancePosition))));
364 }
365
366 /**
367 * Recursively move to upstream records if the relative position is upstream of the record, to start a downstream search
368 * from these upstream records.
369 * @param record LaneRecord; current record in search.
370 * @param position RelativePosition.Type; relative position type.
371 * @return Collection<LaneRecord>; records to start from.
372 */
373 private Collection<LaneRecord> startDownstream(final LaneRecord record, final RelativePosition.Type position)
374 {
375 if (position(LaneStructure.this.gtu, record, position).ge0())
376 {
377 return Set.of(record); // position is on the lane
378 }
379 Set<LaneRecord> set = new LinkedHashSet<>();
380 for (LaneRecord up : record.getPrev())
381 {
382 set.addAll(startDownstream(up, position));
383 }
384 return set;
385 }
386
387 /**
388 * Recursively move to downstream records if the relative position is downstream of the record, to start an upstream search
389 * from these downstream records.
390 * @param record LaneRecord; current record in search.
391 * @param position RelativePosition.Type; relative position type.
392 * @return Collection<LaneRecord>; records to start from.
393 */
394 private Collection<LaneRecord> startUpstream(final LaneRecord record, final RelativePosition.Type position)
395 {
396 if (position(LaneStructure.this.gtu, record, position).lt(record.getLane().getLength()))
397 {
398 return Set.of(record); // position is on the lane
399 }
400 Set<LaneRecord> set = new LinkedHashSet<>();
401 for (LaneRecord down : record.getNext())
402 {
403 set.addAll(startUpstream(down, position));
404 }
405 return set;
406 }
407
408 /**
409 * Returns the position of the GTU on the lane of the given record.
410 * @param gtu LaneBasedGtu; gtu.
411 * @param record LaneRecord; lane record.
412 * @param positionType RelativePosition.Type; relative position type.
413 * @return Length; position of the GTU on the lane of the given record.
414 */
415 private final Length position(final LaneBasedGtu gtu, final LaneRecord record, final RelativePosition.Type positionType)
416 {
417 if (gtu.equals(LaneStructure.this.gtu))
418 {
419 return record.getStartDistance().neg().plus(gtu.getRelativePositions().get(positionType).dx());
420 }
421 return Try.assign(() -> gtu.position(record.getLane(), gtu.getRelativePositions().get(positionType)),
422 "Unable to obtain position %s of GTU.", positionType);
423 }
424
425 /**
426 * Updates the structure when required.
427 */
428 private synchronized void update()
429 {
430 if (this.updated != null && this.updated.equals(this.gtu.getSimulator().getSimulatorAbsTime()))
431 {
432 return;
433 }
434
435 this.crossSection.clear();
436 Set<Lane> visited = new LinkedHashSet<>();
437 Deque<LaneRecord> downQueue = new LinkedList<>();
438 Deque<LaneRecord> upQueue = new LinkedList<>();
439 Deque<LaneRecord> latDownQueue = new LinkedList<>();
440 Deque<LaneRecord> latUpQueue = new LinkedList<>();
441 LanePosition position = Try.assign(() -> this.gtu.getReferencePosition(), "GTU does not have a reference position.");
442 LaneRecord root = new LaneRecord(position.lane(), RelativeLane.CURRENT, position.position().neg(), Length.ZERO);
443 visited.add(position.lane());
444 addToCrossSection(root);
445 downQueue.add(root);
446 upQueue.add(root);
447 latDownQueue.add(root);
448 this.rootCrossSection.put(root.getRelativeLane(), root);
449 while (!downQueue.isEmpty() || !upQueue.isEmpty() || !latDownQueue.isEmpty() || !latUpQueue.isEmpty())
450 {
451 if (!downQueue.isEmpty())
452 {
453 LaneRecord record = downQueue.poll();
454 Set<Lane> downstreamLanes = record.getLane().nextLanes(null);
455 if (!record.getLane().getType().isCompatible(this.gtu.getType()))
456 {
457 /*
458 * Progress downstream from an incompatible lane only to other incompatible lanes. Compatible lanes
459 * downstream of an incompatible lane will have to be found through a lateral move. Only in this way can a
460 * merge be detected.
461 */
462 downstreamLanes.removeAll(record.getLane().nextLanes(this.gtu.getType()));
463 }
464 for (Lane lane : downstreamLanes)
465 {
466 LaneRecord down = new LaneRecord(lane, record.getRelativeLane(), record.getEndDistance(), Length.ZERO);
467 record.addNext(down);
468 down.addPrev(record);
469 visited.add(lane);
470 addToCrossSection(down);
471 if (down.getEndDistance().lt(this.downstream))
472 {
473 downQueue.add(down);
474 }
475 latDownQueue.add(down);
476 }
477 }
478 else if (!upQueue.isEmpty())
479 {
480 LaneRecord record = upQueue.poll();
481 for (Lane lane : record.getLane().prevLanes(null))
482 {
483 /*
484 * Upstream of a merge we ignore visited lanes. Upstream not of a merge, we just continue. I.e. on a
485 * roundabout one lane can be both upstream and downstream.
486 */
487 if (!visited.contains(lane) || record.getMergeDistance().eq0())
488 {
489 LaneRecord up = new LaneRecord(lane, record.getRelativeLane(),
490 record.getStartDistance().minus(lane.getLength()), record.getMergeDistance());
491 record.addPrev(up);
492 up.addNext(record);
493 visited.add(lane);
494 addToCrossSection(up);
495 if (up.getStartDistance().neg().plus(up.getMergeDistance()).lt(this.upstream))
496 {
497 upQueue.add(up);
498 }
499 latUpQueue.add(up);
500 }
501 }
502 }
503 else
504 {
505 boolean down;
506 Deque<LaneRecord> latQueue;
507 if (!latDownQueue.isEmpty())
508 {
509 down = true;
510 latQueue = latDownQueue;
511 }
512 else
513 {
514 down = false;
515 latQueue = latUpQueue;
516 }
517 LaneRecord record = latQueue.poll();
518 for (LateralDirectionality latDirection : new LateralDirectionality[] {LateralDirectionality.LEFT,
519 LateralDirectionality.RIGHT})
520 {
521 for (Lane lane : record.getLane().accessibleAdjacentLanesPhysical(latDirection, null))
522 {
523 if (!visited.contains(lane))
524 {
525 /*
526 * The relative lane stays the same if we are searching upstream. This is because traffic on this
527 * adjacent lane will have to change lane to this existing relative lane, before it can be in
528 * relevant interaction with the perceiving GTU. One can think of two lanes merging in to one just
529 * before an on-ramp. Traffic on both lanes is then considered to be on the same relative lane as
530 * the acceleration lane. Otherwise the lateral lane is shifted once.
531 */
532 RelativeLane relativeLane = !down ? record.getRelativeLane() : (latDirection.isLeft()
533 ? record.getRelativeLane().getLeft() : record.getRelativeLane().getRight());
534
535 /*
536 * If the zero-position is on the record, the fractional position is used. Otherwise the start
537 * distance is such that the start is equal in a downstream search, and the end is equal in an
538 * upstream search.
539 */
540 Length startDistance;
541 if (record.getStartDistance().lt0() && record.getEndDistance().gt0())
542 {
543 startDistance = lane.getLength()
544 .times(record.getStartDistance().neg().si / record.getLane().getLength().si).neg();
545 }
546 else if (down)
547 {
548 startDistance = record.getStartDistance();
549 }
550 else
551 {
552 startDistance = record.getEndDistance().minus(lane.getLength());
553 }
554
555 /*
556 * If the adjacent lane is found in a downstream search and its upstream links are different, we are
557 * dealing with a merge at a distance of the start of these two lanes.
558 */
559 Length mergeDistance;
560 if (down && record.getStartDistance().gt0())
561 {
562 if (!getUpstreamLinks(lane).equals(getUpstreamLinks(record.getLane())))
563 {
564 mergeDistance = record.getStartDistance();
565 }
566 else
567 {
568 mergeDistance = Length.ZERO;
569 }
570 }
571 else
572 {
573 mergeDistance = record.getMergeDistance();
574 }
575 LaneRecord lat = new LaneRecord(lane, relativeLane, startDistance, mergeDistance);
576 if (!down)
577 {
578 record.addLateral(lat);
579 }
580 visited.add(lane);
581 addToCrossSection(lat);
582 // from the cross-section directly from the root, we initiate both an upstream and downstream search
583 // if (this.rootCrossSection.contains(record))
584 if (this.rootCrossSection.containsValue(record))
585 {
586 // this.rootCrossSection.add(lat);
587 this.rootCrossSection.put(lat.getRelativeLane(), lat);
588 latDownQueue.add(lat); // does not matter which lat queue this is, it is root the cross section
589 if (lat.getEndDistance().lt(this.downstream))
590 {
591 downQueue.add(lat);
592 }
593 if (lat.getStartDistance().neg().lt(this.upstream))
594 {
595 upQueue.add(lat);
596 }
597 }
598 else if (down)
599 {
600 latDownQueue.add(lat);
601 if (lat.getEndDistance().lt(this.downstream))
602 {
603 downQueue.add(lat);
604 if (mergeDistance.gt0())
605 {
606 upQueue.add(lat);
607 }
608 }
609 }
610 else
611 {
612 latUpQueue.add(lat);
613 if (lat.getStartDistance().neg().plus(lat.getMergeDistance()).lt(this.upstream))
614 {
615 upQueue.add(lat);
616 }
617 }
618
619 }
620 }
621 }
622 }
623 }
624 this.updated = this.gtu.getSimulator().getSimulatorAbsTime();
625 }
626
627 /**
628 * Adds the lane to the cross-section, if the zero position is somewhere on the lane (negative start distance, positive end
629 * distance).
630 * @param record LaneRecord; record.
631 */
632 private void addToCrossSection(final LaneRecord record)
633 {
634 if (record.getStartDistance().le0() && record.getEndDistance().gt0())
635 {
636 this.crossSection.computeIfAbsent(record.getRelativeLane(), (r) -> new LinkedHashSet<>()).add(record);
637 }
638 }
639
640 /**
641 * Returns the links upstream of the lane.
642 * @param lane Lane; lane.
643 * @return Set<Link>; upstream lanes.
644 */
645 private Set<Link> getUpstreamLinks(final Lane lane)
646 {
647 Set<Link> set = new LinkedHashSet<>();
648 for (Lane prev : lane.prevLanes(null))
649 {
650 set.add(prev.getLink());
651 }
652 return set;
653 }
654
655 /**
656 * Returns all the lanes that are in the root cross-section, i.e. to our direct left and right.
657 * @return SortedSet<RelativeLane>; set of lanes in the root cross-section.
658 */
659 public SortedSet<RelativeLane> getRootCrossSection()
660 {
661 update();
662 return new TreeSet<>(this.rootCrossSection.keySet());
663 }
664
665 /**
666 * Returns whether the lane exists within the structure.
667 * @param lane RelativeLane; lane.
668 * @return boolean; whether the lane exists within the structure.
669 */
670 public boolean exists(final RelativeLane lane)
671 {
672 update();
673 return this.crossSection.containsKey(lane);
674 }
675
676 /**
677 * Returns the root record on the given lane.
678 * @param lane RelativeLane; lane.
679 * @return LaneRecord; root record on the lane.
680 */
681 public LaneRecord getRootRecord(final RelativeLane lane)
682 {
683 update();
684 return this.rootCrossSection.get(lane);
685 }
686
687 /**
688 * Returns the set of records in the cross-section on the given lane.
689 * @param lane RelativeLane; lane.
690 * @return Set<LaneRecord>; set of records in the cross-section on the given lane.
691 */
692 public Set<LaneRecord> getCrossSectionRecords(final RelativeLane lane)
693 {
694 return new LinkedHashSet<>(this.crossSection.get(lane));
695 }
696
697 /**
698 * Iterable over entries (with distance and merge distance stored) of objects. The iterable uses a navigator, lister and
699 * distancer.
700 * <ul>
701 * <li><i>navigator</i>; returns a collection of lane records to continue a search from a covered lane record.</li>
702 * <li><i>lister</i>; returns a list of objects from a lane record. The list must be ordered in the search direction (close
703 * to far). Objects of any type may be returned as the navigating iterator will check whether objects are of type {@code T}.
704 * In order to only include objects in the correct range, the lister must account for the start distance of the record, and
705 * any possible relative position of the GTU.</li>
706 * <li><i>distancer</i>; returns distance of an object. The distancer must account for any possible relative position of the
707 * GTUs.</li>
708 * </ul>
709 * <p>
710 * Copyright (c) 2024-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
711 * <br>
712 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
713 * </p>
714 * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
715 * @param <T> type of object.
716 */
717 // TODO: Generalize this class so it can be used by conflicts as well. This will probably require the relative lane to be
718 // removed, and the starter to include obtaining the cross section.
719 private class NavigatingIterable<T> implements Iterable<Entry<T>>
720 {
721 /** Relative lane. */
722 private final RelativeLane relativeLane;
723
724 /** Class of lane-based object type. */
725 private final Class<T> clazz;
726
727 /** Range within which objects are included. */
728 private final Length range;
729
730 /** Moves up- or downstream as the relevant relative position may be on those lanes. */
731 private final Function<LaneRecord, Collection<LaneRecord>> starter;
732
733 /** Navigator which gives the next records. */
734 private final Function<LaneRecord, Collection<LaneRecord>> navigator;
735
736 /** Obtains ordered list of objects from lane. */
737 private final Function<LaneRecord, List<?>> lister;
738
739 /** Returns distance of object. */
740 private final BiFunction<T, LaneRecord, Length> distancer;
741
742 /**
743 * Constructor.
744 * @param relativeLane RelativeLane; relative lane.
745 * @param clazz Class<T>; class of lane-based object type.
746 * @param range Length; range within which objects are included.
747 * @param starter Function<LaneRecord2, Collection<LaneRecord2>>; starter.
748 * @param navigator Function<LaneRecord, Collection<LaneRecord2>>; navigator.
749 * @param lister Function<LaneRecord, List<?>>; obtains ordered list of objects from lane.
750 * @param distancer BiFunction<T, LaneRecord, Length>; returns distance of object.
751 */
752 public NavigatingIterable(final RelativeLane relativeLane, final Class<T> clazz, final Length range,
753 final Function<LaneRecord, Collection<LaneRecord>> starter,
754 final Function<LaneRecord, Collection<LaneRecord>> navigator, final Function<LaneRecord, List<?>> lister,
755 final BiFunction<T, LaneRecord, Length> distancer)
756 {
757 this.relativeLane = relativeLane;
758 this.clazz = clazz;
759 this.range = range;
760 this.starter = starter;
761 this.navigator = navigator;
762 this.lister = lister;
763 this.distancer = distancer;
764 }
765
766 /** {@inheritDoc} */
767 @Override
768 public Iterator<Entry<T>> iterator()
769 {
770 // map of currently iterated records and their object iterators, create map from initial cross-section
771 Map<LaneRecord, ObjectIterator<T>> map = new LinkedHashMap<>();
772 // TODO: the next two for-loops should be external in something that just gives a set of LaneRecords
773 for (LaneRecord record : LaneStructure.this.crossSection.computeIfAbsent(NavigatingIterable.this.relativeLane,
774 (l) -> new LinkedHashSet<>()))
775 {
776 for (LaneRecord start : this.starter.apply(record))
777 {
778 map.put(start, new ObjectIterator<>(start, NavigatingIterable.this.clazz, NavigatingIterable.this.lister,
779 NavigatingIterable.this.distancer));
780 }
781 }
782 return new Iterator<Entry<T>>()
783 {
784 /** Next entry as found by {@code hasNext()}. */
785 private Entry<T> next;
786
787 /** {@inheritDoc} */
788 @Override
789 public boolean hasNext()
790 {
791 if (this.next != null)
792 {
793 return true;
794 }
795 // update the map with records that have something to produce
796 Map<LaneRecord, ObjectIterator<T>> mapCopy = new LinkedHashMap<>(map);
797 for (Map.Entry<LaneRecord, ObjectIterator<T>> mapEntry : mapCopy.entrySet())
798 {
799 if (!mapEntry.getValue().hasNext())
800 {
801 updateMapRecursive(mapEntry.getKey());
802 }
803 }
804 if (map.isEmpty())
805 {
806 this.next = null;
807 }
808 else if (map.size() == 1)
809 {
810 this.next = map.values().iterator().next().next();
811 }
812 else
813 {
814 // loop map and find closest object
815 Length minDistance = Length.POSITIVE_INFINITY;
816 ObjectIterator<T> closestObjectIterator = null;
817 for (ObjectIterator<T> objectIterator : map.values())
818 {
819 Entry<T> entry = objectIterator.poll();
820 if (entry.distance().lt(minDistance))
821 {
822 minDistance = entry.distance();
823 closestObjectIterator = objectIterator;
824 }
825 }
826 this.next = closestObjectIterator.next(); // advance the object iterator; it was only polled above
827 }
828 if (this.next != null && this.next.distance().gt(NavigatingIterable.this.range))
829 {
830 this.next = null; // next object out of range
831 }
832 return this.next != null;
833 }
834
835 /** {@inheritDoc} */
836 @Override
837 public Entry<T> next()
838 {
839 Throw.when(!hasNext(), NoSuchElementException.class, "No more object of type %s.",
840 NavigatingIterable.this.clazz);
841 Entry<T> n = this.next;
842 this.next = null;
843 return n;
844 }
845
846 /**
847 * Updates the map so it contains a record only if it has an object to return. If not, further records are added
848 * to the map through the navigator and consecutively checked.
849 * @param record LaneRecord; lane record.
850 */
851 private void updateMapRecursive(final LaneRecord record)
852 {
853 if (!map.containsKey(record) || map.get(record).hasNext())
854 {
855 return;
856 }
857 map.remove(record);
858 for (LaneRecord next : NavigatingIterable.this.navigator.apply(record))
859 {
860 map.put(next, new ObjectIterator<>(next, NavigatingIterable.this.clazz, NavigatingIterable.this.lister,
861 NavigatingIterable.this.distancer));
862 updateMapRecursive(next);
863 }
864 }
865 };
866 }
867 }
868
869 /**
870 * Iterator over objects on a {@code LaneRecord}. This is used by {@code NavigatingIterable} to find object.
871 * <p>
872 * Copyright (c) 2024-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
873 * <br>
874 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
875 * </p>
876 * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
877 * @param <T> type of object.
878 */
879 private class ObjectIterator<T> implements Iterator<Entry<T>>
880 {
881 /** Lane record. */
882 private final LaneRecord record;
883
884 /** Class of lane-based object type. */
885 private final Class<T> clazz;
886
887 /** List of objects from lane, within correct range for downstream. */
888 private final List<?> list;
889
890 /** Index of current entry. */
891 private int index;
892
893 /** Returns distance of object. */
894 private final BiFunction<T, LaneRecord, Length> distancer;
895
896 /** Poll entry, that next will return. */
897 private Entry<T> poll;
898
899 /**
900 * Constructor. The lister may return objects of any type. This class will check whether objects are of type T.
901 * @param record LaneRecord; lane record.
902 * @param clazz Class<T>; class of lane-based object type.
903 * @param lister Function<LaneRecord, List<?>>; obtains ordered list of objects from lane.
904 * @param distancer BiFunction<T, LaneRecord, Length>; returns distance of object.
905 */
906 public ObjectIterator(final LaneRecord record, final Class<T> clazz, final Function<LaneRecord, List<?>> lister,
907 final BiFunction<T, LaneRecord, Length> distancer)
908 {
909 this.record = record;
910 this.clazz = clazz;
911 this.list = lister.apply(record);
912 this.distancer = distancer;
913 }
914
915 /** {@inheritDoc} */
916 @Override
917 public boolean hasNext()
918 {
919 while (this.index < this.list.size() && !this.list.get(this.index).getClass().isAssignableFrom(this.clazz))
920 {
921 this.index++;
922 }
923 return this.index < this.list.size();
924 }
925
926 /** {@inheritDoc} */
927 @Override
928 public Entry<T> next()
929 {
930 Throw.when(!hasNext(), NoSuchElementException.class, "No more object of type %s.", this.clazz);
931 Entry<T> entry = poll();
932 this.index++;
933 this.poll = null;
934 return entry;
935 }
936
937 /**
938 * Returns the entry that {@code next()} will return, without advancing the iterator.
939 * @return Entry<T>; poll entry.
940 */
941 public Entry<T> poll()
942 {
943 if (this.poll == null)
944 {
945 @SuppressWarnings("unchecked") // isAssignableFrom in hasNext() checks this
946 T t = (T) this.list.get(this.index);
947 this.poll = new Entry<>(this.distancer.apply(t, this.record), this.record.getMergeDistance(), t);
948 }
949 return this.poll;
950 }
951
952 }
953
954 /**
955 * Container for a perceived object with the distance towards it and the distance until the road of the object and the road
956 * of the perceiving GTU merge.
957 * <p>
958 * Copyright (c) 2024-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
959 * <br>
960 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
961 * </p>
962 * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
963 * @param <T> type of object.
964 * @param distance Length; distance to object.
965 * @param merge Length; distance until the road of the object and the road of the perceiving GTU merge.
966 * @param object T; the perceived object.
967 */
968 public record Entry<T>(Length distance, Length merge, T object)
969 {
970 }
971
972 }