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