1 package org.opentrafficsim.road.gtu.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.Optional;
13 import java.util.Set;
14 import java.util.SortedSet;
15 import java.util.TreeSet;
16 import java.util.function.Function;
17
18 import org.djunits.value.vdouble.scalar.Duration;
19 import org.djunits.value.vdouble.scalar.Length;
20 import org.opentrafficsim.core.gtu.RelativePosition;
21 import org.opentrafficsim.core.network.LateralDirectionality;
22 import org.opentrafficsim.core.network.Link;
23 import org.opentrafficsim.core.network.route.Route;
24 import org.opentrafficsim.road.gtu.LaneBasedGtu;
25 import org.opentrafficsim.road.gtu.perception.RelativeLane;
26 import org.opentrafficsim.road.gtu.perception.structure.NavigatingIterable.Entry;
27 import org.opentrafficsim.road.network.Lane;
28 import org.opentrafficsim.road.network.LanePosition;
29 import org.opentrafficsim.road.network.object.LaneBasedObject;
30
31 /**
32 * The lane structure provides a way to see the world for a lane based model.
33 * <p>
34 * Copyright (c) 2024-2026 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
35 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
36 * </p>
37 * @author Wouter Schakel
38 */
39 public class LaneStructure
40 {
41
42 /** GTU. */
43 private final LaneBasedGtu egoGtu;
44
45 /** Length to build lane structure upstream of GTU, or upstream relative to a downstream merge. */
46 private Length upstream;
47
48 /** Length to build lane structure downstream of GTU. */
49 private Length downstream;
50
51 /** Time at which the structure was updated. */
52 private Duration updated = null;
53
54 /** Cross section of lane records at different relative lanes. */
55 private final Map<RelativeLane, Set<LaneRecord>> crossSection = new LinkedHashMap<>();
56
57 /** Cross section of lane records directly found laterally from the root. */
58 private final Map<RelativeLane, LaneRecord> rootCrossSection = new LinkedHashMap<>();
59
60 /**
61 * Constructor.
62 * @param gtu the GTU.
63 * @param upstream guaranteed distance within which objects are found upstream of the GTU, or upstream of downstream merge.
64 * @param downstream guaranteed distance within which objects are found downstream of the GTU.
65 */
66 public LaneStructure(final LaneBasedGtu gtu, final Length upstream, final Length downstream)
67 {
68 this.egoGtu = gtu;
69 this.upstream = upstream;
70 this.downstream = downstream;
71 }
72
73 /**
74 * Returns an iterator over objects perceived on a relative lane, ordered close to far. This can be objects on different
75 * roads, e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two
76 * on-ramps that are very close by, or even the shoulder. Objects that are partially downstream are also included.
77 * <p>
78 * This method does not support finding objects that start at upstream lanes, but who's length reach the GTU.
79 * @param <T> type of {@code LaneBasedObject}.
80 * @param relativeLane lane.
81 * @param clazz class of lane-based object type.
82 * @param position relative position relative to which objects are found and distances are given.
83 * @param onRoute whether the objects have to be on-route.
84 * @return 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.egoGtu.getRelativePositions().get(position).dx();
91 return new NavigatingIterable<>(clazz, this.downstream,
92 start((record) -> startDownstream(record, position), relativeLane), (record) ->
93 {
94 // this navigator only includes records of lanes on the route
95 Set<LaneRecord> set = record.getNext();
96 set.removeIf((r) -> onRoute
97 && !r.isOnRoute(LaneStructure.this.egoGtu.getStrategicalPlanner().getRoute().orElse(null)));
98 return set;
99 }, (record) ->
100 {
101 // this navigator selects all objects fully or partially downstream
102 List<LaneBasedObject> list = record.getLane().getLaneBasedObjects();
103 if (list.isEmpty())
104 {
105 return list;
106 }
107 Length pos = record.getStartDistance().neg().plus(dx);
108 int from = 0;
109 while (from < list.size()
110 && list.get(from).getLongitudinalPosition().plus(list.get(from).getLength()).lt(pos))
111 {
112 from++;
113 }
114 if (from > list.size() - 1)
115 {
116 return Collections.emptyList();
117 }
118 return list.subList(from, list.size());
119 }, (t, r) -> r.getStartDistance().plus(t.getLongitudinalPosition()).minus(dx));
120 }
121
122 /**
123 * Returns an iterator over objects perceived on a relative lane, ordered close to far. This can be objects on different
124 * roads, e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two
125 * on-ramps that are very close by, or even the shoulder. Objects that are partially upstream are also included.
126 * @param <T> type of {@code LaneBasedObject}.
127 * @param relativeLane lane.
128 * @param clazz class of lane-based object type.
129 * @param position relative position relative to which objects are found and distances are given.
130 * @return iterator over objects.
131 */
132 public <T extends LaneBasedObject> Iterable<Entry<T>> getUpstreamObjects(final RelativeLane relativeLane,
133 final Class<T> clazz, final RelativePosition.Type position)
134 {
135 update();
136 Length dx = LaneStructure.this.egoGtu.getRelativePositions().get(position).dx();
137 return new NavigatingIterable<>(clazz, this.upstream, start((record) -> startUpstream(record, position), relativeLane),
138 (record) ->
139 {
140 // this navigator combines the upstream and lateral records
141 Set<LaneRecord> set = new LinkedHashSet<>(record.getPrev());
142 set.addAll(record.lateral());
143 return set;
144 }, (record) ->
145 {
146 // this lister reverses the list
147 List<LaneBasedObject> list = record.getLane().getLaneBasedObjects();
148 if (list.isEmpty())
149 {
150 return list;
151 }
152 Length pos = record.getStartDistance().neg().plus(dx);
153 int to = list.size();
154 while (to >= 0 && list.get(to).getLongitudinalPosition().gt(pos))
155 {
156 to--;
157 }
158 if (to < 0)
159 {
160 return Collections.emptyList();
161 }
162 list = list.subList(0, to);
163 Collections.reverse(list);
164 return list;
165 }, (t, r) -> r.getStartDistance().plus(t.getLongitudinalPosition()).plus(dx).neg());
166 }
167
168 /**
169 * Returns an iterator over GTUs perceived on a relative lane, ordered close to far. This can be GTUs on different roads,
170 * e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two on-ramps
171 * that are very close by, or even the shoulder. When from a lane on the route, a downstream lane is not on the route, GTUs
172 * on the downstream lane are not included. A split conflict should deal with possible GTUs there.
173 * @param relativeLane lane.
174 * @param egoPosition position of ego GTU relative to which objects are found.
175 * @param otherPosition position of other GTU that must be downstream of egoPosition.
176 * @param egoDistancePosition position of ego GTU from which the distance is determined.
177 * @param otherDistancePosition position of other GTU to which the distance is determined.
178 * @return iterator over GTUs.
179 */
180 public Iterable<Entry<LaneBasedGtu>> getDownstreamGtus(final RelativeLane relativeLane,
181 final RelativePosition.Type egoPosition, final RelativePosition.Type otherPosition,
182 final RelativePosition.Type egoDistancePosition, final RelativePosition.Type otherDistancePosition)
183 {
184 update();
185 Length dx = LaneStructure.this.egoGtu.getRelativePositions().get(egoPosition).dx();
186 Length dxDistance = LaneStructure.this.egoGtu.getRelativePositions().get(egoDistancePosition).dx();
187 Optional<Route> route = LaneStructure.this.egoGtu.getStrategicalPlanner().getRoute();
188 return new NavigatingIterable<>(LaneBasedGtu.class, this.downstream,
189 start((record) -> startDownstream(record, egoPosition), relativeLane), (record) ->
190 {
191 // this navigator ignores downstream lanes that are not on the route, if the current record is on the route
192 Set<LaneRecord> next = new LinkedHashSet<>(record.getNext());
193 if (route.isPresent() && record.getLane().getLink().getEndNode().getLinks().size() > 2
194 && route.get().containsLink(record.getLane().getLink()))
195 {
196 Iterator<LaneRecord> it = next.iterator();
197 while (it.hasNext())
198 {
199 LaneRecord down = it.next();
200 if (!route.get().containsLink(down.getLane().getLink()))
201 {
202 it.remove();
203 }
204 }
205 }
206 return next;
207 }, (record) ->
208 {
209 // this lister finds the relevant sublist of GTUs
210 List<LaneBasedGtu> gtus = record.getLane().getGtuList().toList();
211 if (gtus.isEmpty())
212 {
213 return gtus;
214 }
215 int from = 0;
216 Length pos = Length.max(record.getStartDistance().neg().plus(dx), Length.ZERO);
217 while (from < gtus.size() && (position(gtus.get(from), record, otherPosition).lt(pos)
218 || gtus.get(from).getId().equals(this.egoGtu.getId())))
219 {
220 from++;
221 }
222 int to = gtus.size() - 1;
223 while (to >= 0 && gtus.get(to).getId().equals(this.egoGtu.getId()))
224 {
225 to--;
226 }
227 if (from > to)
228 {
229 return Collections.emptyList();
230 }
231 if (from > 0 || to < gtus.size() - 1)
232 {
233 gtus = gtus.subList(from, to + 1);
234 }
235 return gtus;
236 }, (t, r) -> r.getStartDistance().plus(position(t, r, otherDistancePosition)).minus(dxDistance));
237 }
238
239 /**
240 * Returns an iterator over GTUs perceived on a relative lane, ordered close to far. This can be GTUs on different roads,
241 * e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two on-ramps
242 * that are very close by, or even the shoulder.
243 * @param relativeLane lane.
244 * @param egoPosition position of ego GTU relative to which objects are found.
245 * @param otherPosition position of other GTU that must be upstream of egoPosition.
246 * @param egoDistancePosition position of ego GTU from which the distance is determined.
247 * @param otherDistancePosition position of other GTU to which the distance is determined.
248 * @return iterator over GTUs.
249 */
250 public Iterable<Entry<LaneBasedGtu>> getUpstreamGtus(final RelativeLane relativeLane,
251 final RelativePosition.Type egoPosition, final RelativePosition.Type otherPosition,
252 final RelativePosition.Type egoDistancePosition, final RelativePosition.Type otherDistancePosition)
253 {
254 update();
255 Length dx = LaneStructure.this.egoGtu.getRelativePositions().get(egoPosition).dx();
256 Length dxDistance = LaneStructure.this.egoGtu.getRelativePositions().get(egoDistancePosition).dx();
257 return new NavigatingIterable<>(LaneBasedGtu.class, this.upstream,
258 start((record) -> startUpstream(record, egoPosition), relativeLane), (record) ->
259 {
260 // this navigator combines the upstream and lateral records
261 Set<LaneRecord> set = new LinkedHashSet<>(record.getPrev());
262 set.addAll(record.lateral());
263 return set;
264 }, (record) ->
265 {
266 // this lister finds the relevant sublist of GTUs and reverses it
267 List<LaneBasedGtu> gtus = record.getLane().getGtuList().toList();
268 if (gtus.isEmpty())
269 {
270 return gtus;
271 }
272 int from = 0;
273 while (from < gtus.size() && gtus.get(from).getId().equals(this.egoGtu.getId()))
274 {
275 from++;
276 }
277 int to = gtus.size() - 1;
278 Length pos = Length.min(record.getStartDistance().neg().plus(dx), record.getLength());
279 while (to >= 0 && (position(gtus.get(to), record, otherPosition).gt(pos)
280 || gtus.get(to).getId().equals(this.egoGtu.getId())))
281 {
282 to--;
283 }
284 if (from > to)
285 {
286 return Collections.emptyList();
287 }
288 if (from > 0 || to < gtus.size() - 1)
289 {
290 gtus = gtus.subList(from, to + 1);
291 }
292 Collections.reverse(gtus);
293 return gtus;
294 }, (t, r) -> dxDistance.minus(r.getStartDistance().plus(position(t, r, otherDistancePosition))));
295 }
296
297 /**
298 * Returns an iterator over GTUs perceived on a relative lane, ordered close to far. This can be GTUs on different roads,
299 * e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two on-ramps
300 * that are very close by, or even the shoulder. This function differs from {@code getDownstreamGtus()} in that it will halt
301 * further searching on on branch it finds a GTU on.
302 * @param relativeLane lane.
303 * @param egoPosition position of ego GTU relative to which objects are found.
304 * @param otherPosition position of other GTU that must be downstream of egoPosition.
305 * @param egoDistancePosition position of ego GTU from which the distance is determined.
306 * @param otherDistancePosition position of other GTU to which the distance is determined.
307 * @return iterator over GTUs.
308 */
309 public Iterable<Entry<LaneBasedGtu>> getFirstDownstreamGtus(final RelativeLane relativeLane,
310 final RelativePosition.Type egoPosition, final RelativePosition.Type otherPosition,
311 final RelativePosition.Type egoDistancePosition, final RelativePosition.Type otherDistancePosition)
312 {
313 update();
314 Length dx = LaneStructure.this.egoGtu.getRelativePositions().get(egoPosition).dx();
315 Length dxDistance = LaneStructure.this.egoGtu.getRelativePositions().get(egoDistancePosition).dx();
316 return new NavigatingIterable<>(LaneBasedGtu.class, this.downstream,
317 start((record) -> startDownstream(record, egoPosition), relativeLane), (record) ->
318 {
319 // this navigator only returns records when there are no GTUs on the lane
320 return record.getLane()
321 .getGtuAhead(record.getStartDistance().neg().plus(dx), otherPosition,
322 record.getLane().getNetwork().getSimulator().getSimulatorTime())
323 .isEmpty() ? record.getNext() : new LinkedHashSet<>();
324 }, (record) ->
325 {
326 // this lister finds the first GTU and returns it as the only GTU in the list
327 Optional<LaneBasedGtu> down = record.getLane().getGtuAhead(record.getStartDistance().neg().plus(dx),
328 otherPosition, record.getLane().getNetwork().getSimulator().getSimulatorTime());
329 return down.isEmpty() ? Collections.emptyList() : List.of(down.get());
330 }, (t, r) -> r.getStartDistance().plus(position(t, r, otherDistancePosition)).minus(dxDistance));
331 }
332
333 /**
334 * Returns an iterator over GTUs perceived on a relative lane, ordered close to far. This can be GTUs on different roads,
335 * e.g. from the main line on the right-most lane, the right-hand relative lane can give objects upstream of two on-ramps
336 * that are very close by, or even the shoulder. This function differs from {@code getDownstreamGtus()} in that it will halt
337 * further searching on on branch it finds a GTU on.
338 * @param relativeLane lane.
339 * @param egoPosition position of ego GTU relative to which objects are found.
340 * @param otherPosition position of other GTU that must be upstream of egoPosition.
341 * @param egoDistancePosition position of ego GTU from which the distance is determined.
342 * @param otherDistancePosition position of other GTU to which the distance is determined.
343 * @return iterator over GTUs.
344 */
345 public Iterable<Entry<LaneBasedGtu>> getFirstUpstreamGtus(final RelativeLane relativeLane,
346 final RelativePosition.Type egoPosition, final RelativePosition.Type otherPosition,
347 final RelativePosition.Type egoDistancePosition, final RelativePosition.Type otherDistancePosition)
348 {
349 update();
350 Length dx = LaneStructure.this.egoGtu.getRelativePositions().get(egoPosition).dx();
351 Length dxDistance = LaneStructure.this.egoGtu.getRelativePositions().get(egoDistancePosition).dx();
352 return new NavigatingIterable<>(LaneBasedGtu.class, this.upstream,
353 start((record) -> startUpstream(record, egoPosition), relativeLane), (record) ->
354 {
355 // this navigator only returns records when there are no GTUs on the lane (it may thus ignore a GTU on a
356 // lateral lane that is closer) and combines the upstream and lateral records
357 Optional<LaneBasedGtu> gtu = record.getLane().getGtuBehind(record.getStartDistance().neg().plus(dx),
358 otherPosition, record.getLane().getNetwork().getSimulator().getSimulatorTime());
359 Set<LaneRecord> set = new LinkedHashSet<>();
360 if (gtu.isEmpty())
361 {
362 set.addAll(record.getPrev());
363 set.addAll(record.lateral());
364 }
365 return set;
366 }, (record) ->
367 {
368 // this lister finds the first GTU and returns it as the only GTU in the list
369 Optional<LaneBasedGtu> up = record.getLane().getGtuBehind(record.getStartDistance().neg().plus(dx),
370 otherPosition, record.getLane().getNetwork().getSimulator().getSimulatorTime());
371 return up.isEmpty() ? Collections.emptyList() : List.of(up.get());
372 }, (t, r) -> dxDistance.minus(r.getStartDistance().plus(position(t, r, otherDistancePosition))));
373 }
374
375 /**
376 * Gathers the records using a starter logic on all records in the cross section on the relative lane.
377 * @param starter starter logic
378 * @param relativeLane relative lane
379 * @return the records using a starter logic on all records in the cross section on the relative lane
380 */
381 private Collection<LaneRecord> start(final Function<LaneRecord, Collection<LaneRecord>> starter,
382 final RelativeLane relativeLane)
383 {
384 Collection<LaneRecord> collection = new LinkedHashSet<>();
385 if (this.crossSection.containsKey(relativeLane))
386 {
387 for (LaneRecord record : this.crossSection.get(relativeLane))
388 {
389 for (LaneRecord start : starter.apply(record))
390 {
391 collection.add(start);
392 }
393 }
394 }
395 return collection;
396 }
397
398 /**
399 * Recursively move to upstream records if the relative position is upstream of the record, to start a downstream search
400 * from these upstream records.
401 * @param record current record in search.
402 * @param position relative position type.
403 * @return records to start from.
404 */
405 private Collection<LaneRecord> startDownstream(final LaneRecord record, final RelativePosition.Type position)
406 {
407 if (position(LaneStructure.this.egoGtu, record, position).ge0())
408 {
409 return Set.of(record); // position is on the lane
410 }
411 Set<LaneRecord> set = new LinkedHashSet<>();
412 for (LaneRecord up : record.getPrev())
413 {
414 set.addAll(startDownstream(up, position));
415 }
416 return set;
417 }
418
419 /**
420 * Recursively move to downstream records if the relative position is downstream of the record, to start an upstream search
421 * from these downstream records.
422 * @param record current record in search.
423 * @param position relative position type.
424 * @return records to start from.
425 */
426 private Collection<LaneRecord> startUpstream(final LaneRecord record, final RelativePosition.Type position)
427 {
428 if (position(LaneStructure.this.egoGtu, record, position).lt(record.getLane().getLength()))
429 {
430 return Set.of(record); // position is on the lane
431 }
432 Set<LaneRecord> set = new LinkedHashSet<>();
433 for (LaneRecord down : record.getNext())
434 {
435 set.addAll(startUpstream(down, position));
436 }
437 return set;
438 }
439
440 /**
441 * Returns the position of the GTU on the lane of the given record.
442 * @param gtu gtu.
443 * @param record lane record.
444 * @param positionType relative position type.
445 * @return position of the GTU on the lane of the given record.
446 */
447 private Length position(final LaneBasedGtu gtu, final LaneRecordInterface<?> record,
448 final RelativePosition.Type positionType)
449 {
450 if (gtu.equals(LaneStructure.this.egoGtu))
451 {
452 return Length.ofSI(-record.getStartDistance().si + gtu.getRelativePositions().get(positionType).dx().si);
453 }
454 return gtu.getPosition(record.getLane(), gtu.getRelativePositions().get(positionType));
455 }
456
457 /**
458 * Updates the structure when required.
459 */
460 private synchronized void update()
461 {
462 if (this.updated != null && this.updated.equals(this.egoGtu.getSimulator().getSimulatorTime()))
463 {
464 return;
465 }
466
467 this.crossSection.clear();
468 this.rootCrossSection.clear();
469 Set<Lane> visited = new LinkedHashSet<>();
470 Deque<LaneRecord> downQueue = new LinkedList<>();
471 Deque<LaneRecord> upQueue = new LinkedList<>();
472 Deque<LaneRecord> latDownQueue = new LinkedList<>();
473 Deque<LaneRecord> latUpQueue = new LinkedList<>();
474 LanePosition position = this.egoGtu.getPosition();
475 LaneRecord root = new LaneRecord(position.lane(), RelativeLane.CURRENT, position.position().neg(), Length.ZERO);
476 visited.add(position.lane());
477 addToCrossSection(root);
478 downQueue.add(root);
479 upQueue.add(root);
480 latDownQueue.add(root); // does not matter which lat queue this is, it is the root cross section
481 this.rootCrossSection.put(root.getRelativeLane(), root);
482 while (!downQueue.isEmpty() || !upQueue.isEmpty() || !latDownQueue.isEmpty() || !latUpQueue.isEmpty())
483 {
484 if (!downQueue.isEmpty())
485 {
486 nextDown(visited, downQueue, latDownQueue);
487 }
488 else if (!upQueue.isEmpty())
489 {
490 nextUp(visited, upQueue, latUpQueue);
491 }
492 else
493 {
494 nextLateral(visited, downQueue, upQueue, latDownQueue, latUpQueue);
495 }
496 }
497 this.updated = this.egoGtu.getSimulator().getSimulatorTime();
498 }
499
500 /**
501 * Progress to downstream lanes of first record in the downstream queue.
502 * @param visited visited records in the entire structure so far
503 * @param downQueue queue of records to be processed in downstream search
504 * @param latDownQueue queue of records to be processed in lateral direction, as part of a downstream search
505 */
506 private void nextDown(final Set<Lane> visited, final Deque<LaneRecord> downQueue, final Deque<LaneRecord> latDownQueue)
507 {
508 LaneRecord record = downQueue.poll();
509 Set<Lane> downstreamLanes = record.getLane().nextLanes(null);
510 if (!record.getLane().getType().isCompatible(this.egoGtu.getType()))
511 {
512 /*
513 * Progress downstream from an incompatible lane only to other incompatible lanes. Compatible lanes downstream of an
514 * incompatible lane will have to be found through a lateral move. Only in this way can a merge be detected.
515 */
516 downstreamLanes = new LinkedHashSet<>(downstreamLanes); // safe copy
517 downstreamLanes.removeAll(record.getLane().nextLanes(this.egoGtu.getType()));
518 }
519 for (Lane lane : downstreamLanes)
520 {
521 LaneRecord down = new LaneRecord(lane, record.getRelativeLane(), record.getEndDistance(), Length.ZERO);
522 record.addNext(down);
523 down.addPrev(record);
524 visited.add(lane);
525 addToCrossSection(down);
526 if (down.getEndDistance().lt(this.downstream))
527 {
528 downQueue.add(down);
529 }
530 latDownQueue.add(down);
531 }
532 }
533
534 /**
535 * Progress to upstream lanes of first record in the upstream queue.
536 * @param visited visited records in the entire structure so far
537 * @param upQueue queue of records to be processed in upstream search
538 * @param latUpQueue queue of records to be processed in lateral direction, as part of an upstream search
539 */
540 private void nextUp(final Set<Lane> visited, final Deque<LaneRecord> upQueue, final Deque<LaneRecord> latUpQueue)
541 {
542 LaneRecord record = upQueue.poll();
543 for (Lane lane : record.getLane().prevLanes(null))
544 {
545 /*
546 * Upstream of a merge we ignore visited lanes. Upstream not of a merge, we just continue. I.e. on a roundabout one
547 * lane can be both upstream and downstream.
548 */
549 if (!visited.contains(lane) || record.getMergeDistance().eq0())
550 {
551 LaneRecord up = new LaneRecord(lane, record.getRelativeLane(),
552 record.getStartDistance().minus(lane.getLength()), record.getMergeDistance());
553 record.addPrev(up);
554 up.addNext(record);
555 visited.add(lane);
556 addToCrossSection(up);
557 if (up.getStartDistance().neg().plus(up.getMergeDistance()).lt(this.upstream))
558 {
559 upQueue.add(up);
560 }
561 latUpQueue.add(up);
562 }
563 }
564 }
565
566 /**
567 * Progress to lateral lanes for downstream search if any, or for upstream search otherwise.
568 * @param visited visited records in the entire structure so far
569 * @param downQueue queue of records to be processed in downstream search
570 * @param upQueue queue of records to be processed in upstream search
571 * @param latDownQueue queue of records to be processed in lateral direction, as part of a downstream search
572 * @param latUpQueue queue of records to be processed in lateral direction, as part of an upstream search
573 */
574 private void nextLateral(final Set<Lane> visited, final Deque<LaneRecord> downQueue, final Deque<LaneRecord> upQueue,
575 final Deque<LaneRecord> latDownQueue, final Deque<LaneRecord> latUpQueue)
576 {
577 boolean down;
578 Deque<LaneRecord> latQueue;
579 if (!latDownQueue.isEmpty())
580 {
581 down = true;
582 latQueue = latDownQueue;
583 }
584 else
585 {
586 down = false;
587 latQueue = latUpQueue;
588 }
589 LaneRecord record = latQueue.poll();
590 for (LateralDirectionality latDirection : LateralDirectionality.LEFT_AND_RIGHT)
591 {
592 for (Lane lane : record.getLane().accessibleAdjacentLanesPhysical(latDirection, null))
593 {
594 if (!visited.contains(lane))
595 {
596 /*
597 * The relative lane stays the same if we are searching upstream. This is because traffic on this adjacent
598 * lane will have to change lane to this existing relative lane, before it can be in relevant interaction
599 * with the perceiving GTU. One can think of two lanes merging in to one just before an on-ramp. Traffic on
600 * both lanes is then considered to be on the same relative lane as the acceleration lane. Otherwise the
601 * lateral lane is shifted once.
602 */
603 RelativeLane relativeLane = !down ? record.getRelativeLane() : (latDirection.isLeft()
604 ? record.getRelativeLane().getLeft() : record.getRelativeLane().getRight());
605
606 /*
607 * If the zero-position is on the record, the fractional position is used. Otherwise the start distance is
608 * such that the start is equal in a downstream search, and the end is equal in an upstream search.
609 */
610 Length startDistance;
611 if (record.getStartDistance().lt0() && record.getEndDistance().gt0())
612 {
613 startDistance = lane.getLength()
614 .times(record.getStartDistance().neg().si / record.getLane().getLength().si).neg();
615 }
616 else if (down)
617 {
618 startDistance = record.getStartDistance();
619 }
620 else
621 {
622 startDistance = record.getEndDistance().minus(lane.getLength());
623 }
624
625 /*
626 * If the adjacent lane is found in a downstream search and its upstream links are different, we are dealing
627 * with a merge at a distance of the start of these two lanes.
628 */
629 Length mergeDistance;
630 if (down && record.getStartDistance().gt0())
631 {
632 if (!getUpstreamLinks(lane).equals(getUpstreamLinks(record.getLane())))
633 {
634 mergeDistance = record.getStartDistance();
635 }
636 else
637 {
638 mergeDistance = record.getMergeDistance(); // zero, or continue same value in downstream branch
639 }
640 }
641 else
642 {
643 mergeDistance = record.getMergeDistance(); // zero, or continue same value in upstream branch
644 }
645 LaneRecord lat = new LaneRecord(lane, relativeLane, startDistance, mergeDistance);
646 if (!down)
647 {
648 record.addLateral(lat);
649 }
650 visited.add(lane);
651 addToCrossSection(lat);
652 // from the cross-section directly from the root, we initiate both an upstream and downstream search
653 if (this.rootCrossSection.containsValue(record))
654 {
655 this.rootCrossSection.put(lat.getRelativeLane(), lat);
656 latDownQueue.add(lat); // does not matter which lat queue this is, it is the root cross section
657 if (lat.getEndDistance().lt(this.downstream))
658 {
659 downQueue.add(lat);
660 }
661 if (lat.getStartDistance().neg().lt(this.upstream))
662 {
663 upQueue.add(lat);
664 }
665 }
666 else if (down)
667 {
668 latDownQueue.add(lat);
669 if (lat.getEndDistance().lt(this.downstream))
670 {
671 downQueue.add(lat);
672 if (mergeDistance.gt0())
673 {
674 upQueue.add(lat);
675 }
676 }
677 }
678 else
679 {
680 latUpQueue.add(lat);
681 if (lat.getStartDistance().neg().plus(lat.getMergeDistance()).lt(this.upstream))
682 {
683 upQueue.add(lat);
684 }
685 }
686 }
687 }
688 }
689 }
690
691 /**
692 * Adds the lane to the cross-section, if the zero position is somewhere on the lane (negative start distance, positive end
693 * distance).
694 * @param record record.
695 */
696 private void addToCrossSection(final LaneRecord record)
697 {
698 if (record.getStartDistance().le0() && record.getEndDistance().gt0())
699 {
700 this.crossSection.computeIfAbsent(record.getRelativeLane(), (r) -> new LinkedHashSet<>()).add(record);
701 }
702 }
703
704 /**
705 * Returns the links upstream of the lane.
706 * @param lane lane.
707 * @return upstream lanes.
708 */
709 private Set<Link> getUpstreamLinks(final Lane lane)
710 {
711 Set<Link> set = new LinkedHashSet<>();
712 for (Lane prev : lane.prevLanes(null))
713 {
714 set.add(prev.getLink());
715 }
716 return set;
717 }
718
719 /**
720 * Returns all the lanes that are in the root cross-section, i.e. to our direct left and right.
721 * @return set of lanes in the root cross-section.
722 */
723 public SortedSet<RelativeLane> getRootCrossSection()
724 {
725 update();
726 return new TreeSet<>(this.rootCrossSection.keySet());
727 }
728
729 /**
730 * Returns whether the lane exists within the structure.
731 * @param lane lane.
732 * @return whether the lane exists within the structure.
733 */
734 public boolean exists(final RelativeLane lane)
735 {
736 update();
737 return this.crossSection.containsKey(lane);
738 }
739
740 /**
741 * Returns the root record on the given lane.
742 * @param lane lane.
743 * @return root record on the lane.
744 */
745 public LaneRecord getRootRecord(final RelativeLane lane)
746 {
747 update();
748 return this.rootCrossSection.get(lane);
749 }
750
751 /**
752 * Returns the set of records in the cross-section on the given lane.
753 * @param lane lane.
754 * @return set of records in the cross-section on the given lane.
755 */
756 public Set<LaneRecord> getCrossSectionRecords(final RelativeLane lane)
757 {
758 return new LinkedHashSet<>(this.crossSection.get(lane));
759 }
760
761 }