View Javadoc
1   package org.opentrafficsim.road.gtu.lane.perception;
2   
3   import java.util.Iterator;
4   import java.util.LinkedHashMap;
5   import java.util.LinkedHashSet;
6   import java.util.LinkedList;
7   import java.util.Map;
8   import java.util.Queue;
9   import java.util.Set;
10  import java.util.SortedMap;
11  import java.util.TreeMap;
12  
13  import org.djunits.value.vdouble.scalar.Length;
14  import org.djutils.exceptions.Try;
15  import org.opentrafficsim.core.gtu.GtuException;
16  import org.opentrafficsim.core.gtu.RelativePosition;
17  import org.opentrafficsim.core.network.Link;
18  import org.opentrafficsim.core.network.route.Route;
19  import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
20  import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
21  
22  /**
23   * Abstract iterable that figures out how to find the next nearest object, including splits.
24   * <p>
25   * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
26   * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
27   * </p>
28   * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
29   * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
30   * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
31   * @param <H> headway type
32   * @param <U> underlying object type
33   * @param <C> counter type
34   */
35  public abstract class AbstractPerceptionIterable<H extends Headway, U, C> extends AbstractPerceptionReiterable<H, U>
36  {
37  
38      /** Root record. */
39      private final LaneRecordInterface<?> root;
40  
41      /** Initial position. */
42      private final Length initialPosition;
43  
44      /** Search downstream (or upstream). */
45      private final boolean downstream;
46  
47      /** Max distance. */
48      private final double maxDistance;
49  
50      /** Position to which distance are calculated by subclasses. */
51      private final RelativePosition relativePosition;
52  
53      /** Route of the GTU. */
54      private final Route route;
55  
56      /**
57       * Constructor.
58       * @param perceivingGtu LaneBasedGtu; perceiving GTU
59       * @param root LaneRecord&lt;?&gt;; root record
60       * @param initialPosition Length; initial position
61       * @param downstream boolean; search downstream (or upstream)
62       * @param maxDistance Length; max distance to search
63       * @param relativePosition RelativePosition; position to which distance are calculated by subclasses
64       * @param route Route; route of the GTU, may be {@code null}
65       */
66      public AbstractPerceptionIterable(final LaneBasedGtu perceivingGtu, final LaneRecordInterface<?> root,
67              final Length initialPosition, final boolean downstream, final Length maxDistance,
68              final RelativePosition relativePosition, final Route route)
69      {
70          super(perceivingGtu);
71          this.root = root;
72          this.initialPosition = initialPosition;
73          this.downstream = downstream;
74          this.maxDistance = maxDistance.si;
75          this.relativePosition = relativePosition;
76          this.route = route;
77      }
78  
79      /**
80       * Whether the iterable searches downstream.
81       * @return boolean; whether the iterable searches downstream
82       */
83      public boolean isDownstream()
84      {
85          return this.downstream;
86      }
87  
88      /** {@inheritDoc} */
89      @Override
90      public Iterator<PrimaryIteratorEntry> primaryIterator()
91      {
92          return new PrimaryIterator();
93      }
94  
95      /**
96       * Returns the next object(s) on the lane represented by the record. This should only consider objects on the given lane.
97       * This method should not check the distance towards objects with the maximum distance. The counter will be {@code null} for
98       * the first object(s). For following object(s) it is whatever value is given with the previous output {@code Entry}. Hence,
99       * this method maintains its own counting system.
100      * @param record LaneRecord&lt;?&gt;; record representing the lane and direction
101      * @param position Length; position to look beyond
102      * @param counter C; counter
103      * @return next object(s) on the lane or {@code null} if none
104      * @throws GtuException on any exception in the process
105      */
106     protected abstract Entry getNext(LaneRecordInterface<?> record, Length position, C counter) throws GtuException;
107 
108     /**
109      * Returns the distance to the object. The position fed in to this method is directly taken from an {@code Entry} returned
110      * by {@code getNext}. The two methods need to be consistent with each other.
111      * @param object U; underlying object
112      * @param record LaneRecord&lt;?&gt;; record representing the lane and direction
113      * @param position Length; position of the object on the lane
114      * @return Length; distance to the object
115      */
116     protected abstract Length getDistance(U object, LaneRecordInterface<?> record, Length position);
117 
118     /**
119      * Returns the longitudinal length of the relevant relative position such that distances to this points can be calculated.
120      * @return Length; the longitudinal length of the relevant relative position such that distances to this points can be
121      *         calculated
122      */
123     protected Length getDx()
124     {
125         return this.relativePosition.dx();
126     }
127 
128     /**
129      * The primary iterator is used by all returned iterators to find the next object. This contains the core algorithm to deal
130      * with splits and multiple objects at a single location.
131      * <p>
132      * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
133      * <br>
134      * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
135      * </p>
136      * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
137      * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
138      * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
139      */
140     private class PrimaryIterator implements Iterator<PrimaryIteratorEntry>
141     {
142 
143         /** Map containing the objects found per branch. */
144         private SortedMap<PrimaryIteratorEntry, LaneRecordInterface<?>> map;
145 
146         /** Position per record where the search was halted. */
147         private Map<LaneRecordInterface<?>, Length> positions = new LinkedHashMap<>();
148 
149         /** Items returned to prevent duplicates. */
150         private Set<U> returnedItems = new LinkedHashSet<>();
151 
152         /** Sets of remaining objects at the same location. */
153         private Map<LaneRecordInterface<?>, Queue<PrimaryIteratorEntry>> queues = new LinkedHashMap<>();
154 
155         /** Counter objects per lane. */
156         private Map<LaneRecordInterface<?>, C> counters = new LinkedHashMap<>();
157 
158         /** Record regarding a postponed call to {@code getNext()}. */
159         private LaneRecordInterface<?> postponedRecord = null;
160 
161         /** Position regarding a postponed call to {@code getNext()}. */
162         private Length postponedPosition = null;
163 
164         /** Constructor. */
165         PrimaryIterator()
166         {
167             //
168         }
169 
170         /** {@inheritDoc} */
171         @Override
172         public boolean hasNext()
173         {
174             // (re)start the process
175             startProcess();
176 
177             // check next value
178             return !this.map.isEmpty();
179         }
180 
181         /** {@inheritDoc} */
182         @Override
183         public PrimaryIteratorEntry next()
184         {
185             // (re)start the process
186             startProcess();
187 
188             // get and remove next
189             PrimaryIteratorEntry nextEntry = this.map.firstKey();
190             U next = nextEntry.getObject();
191             LaneRecordInterface<?> record = this.map.get(nextEntry);
192             this.map.remove(nextEntry);
193 
194             // see if we can obtain the next from a queue
195             Queue<PrimaryIteratorEntry> queue = this.queues.get(next);
196             if (queue != null)
197             {
198                 PrimaryIteratorEntry nextNext = queue.poll();
199                 this.map.put(nextNext, record); // next object is made available in the map
200                 if (queue.isEmpty())
201                 {
202                     this.queues.remove(record);
203                 }
204                 preventDuplicateEntries(nextEntry.getObject());
205                 return nextNext;
206             }
207 
208             // prepare for next
209             this.postponedRecord = record;
210             this.postponedPosition = this.positions.get(record); // position;
211             preventDuplicateEntries(nextEntry.getObject());
212             return nextEntry;
213         }
214 
215         /**
216          * Prevents that duplicate (and further) records are returned for the given object as splits later on merge.
217          * @param object U; object for which a {@code PrimaryIteratorEntry} will be returned
218          */
219         private void preventDuplicateEntries(final U object)
220         {
221             this.returnedItems.add(object); // prevents new items to be added over alive branches (that should die out)
222             Iterator<PrimaryIteratorEntry> it = this.map.keySet().iterator();
223             while (it.hasNext())
224             {
225                 PrimaryIteratorEntry entry = it.next();
226                 if (entry.getObject().equals(object))
227                 {
228                     it.remove();
229                 }
230             }
231         }
232 
233         /**
234          * Starts or restarts the process.
235          */
236         @SuppressWarnings("synthetic-access")
237         private void startProcess()
238         {
239             if (this.postponedRecord != null)
240             {
241                 // restart the process; perform prepareNext() that was postponed
242                 prepareNext(this.postponedRecord, this.postponedPosition);
243                 this.postponedRecord = null;
244                 this.postponedPosition = null;
245             }
246             else if (this.map == null)
247             {
248                 // start the process
249                 this.map = new TreeMap<>();
250                 prepareNext(AbstractPerceptionIterable.this.root, AbstractPerceptionIterable.this.initialPosition);
251             }
252         }
253 
254         /**
255          * Iterative method that continues a search on the next lanes if no object is found.
256          * @param record LaneRecord&lt;?&gt;; record
257          * @param position Length; position
258          */
259         @SuppressWarnings("synthetic-access")
260         private void prepareNext(final LaneRecordInterface<?> record, final Length position)
261         {
262             Entry next = Try.assign(() -> AbstractPerceptionIterable.this.getNext(record, position, this.counters.get(record)),
263                     "Exception while deriving next object.");
264             if (next == null)
265             {
266                 this.counters.remove(record);
267                 double distance = AbstractPerceptionIterable.this.downstream
268                         ? record.getStartDistance().si + record.getLength().si : -record.getStartDistance().si;
269                 // TODO this let's us ignore an object that is registered on the next lane, but who's tail may be on this lane
270                 if (distance < AbstractPerceptionIterable.this.maxDistance)
271                 {
272                     if (AbstractPerceptionIterable.this.downstream)
273                     {
274                         for (LaneRecordInterface<?> nextRecord : record.getNext())
275                         {
276                             if (isOnRoute(nextRecord))
277                             {
278                                 prepareNext(nextRecord, Length.instantiateSI(-1e-9));
279                             }
280                         }
281                     }
282                     else
283                     {
284                         for (LaneRecordInterface<?> nextRecord : record.getPrev())
285                         {
286                             if (isOnRoute(nextRecord))
287                             {
288                                 prepareNext(nextRecord, nextRecord.getLength());
289                             }
290                         }
291                     }
292                 }
293             }
294             else
295             {
296                 this.counters.put(record, next.counter);
297                 if (next.isSet())
298                 {
299                     Iterator<U> it = next.set.iterator();
300                     U nextNext = it.next();
301                     if (!this.returnedItems.contains(nextNext))
302                     {
303                         Length distance = getDistance(nextNext, record, next.position);
304                         if (distance == null // null means the object overlaps and is close
305                                 || distance.si <= AbstractPerceptionIterable.this.maxDistance)
306                         {
307                             // next object is made available in the map
308                             this.map.put(new PrimaryIteratorEntry(nextNext, distance), record);
309                             this.positions.put(record, next.position);
310                             if (next.set.size() > 1)
311                             {
312                                 // remaining at this location are made available in a queue
313                                 Queue<PrimaryIteratorEntry> queue = new LinkedList<>();
314                                 while (it.hasNext())
315                                 {
316                                     nextNext = it.next();
317                                     queue.add(new PrimaryIteratorEntry(nextNext, getDistance(nextNext, record, next.position)));
318                                 }
319                                 this.queues.put(record, queue);
320                             }
321                         }
322                     }
323                 }
324                 else if (!this.returnedItems.contains(next.object))
325                 {
326                     Length distance = getDistance(next.object, record, next.position);
327                     if (distance == null // null means the object overlaps and is close
328                             || distance.si <= AbstractPerceptionIterable.this.maxDistance)
329                     {
330                         // next object is made available in the map
331                         this.map.put(new PrimaryIteratorEntry(next.object, distance), record);
332                         this.positions.put(record, next.position);
333                     }
334                 }
335             }
336         }
337 
338     }
339 
340     /**
341      * Returns whether the record is on the route.
342      * @param record LaneRecord&lt;?&gt;; record
343      * @return boolean; whether the record is on the route
344      */
345     final boolean isOnRoute(final LaneRecordInterface<?> record)
346     {
347         if (this.route == null)
348         {
349             return true;
350         }
351         Link link = record.getLane().getLink();
352         int from;
353         int to;
354         from = this.route.indexOf(link.getStartNode());
355         to = this.route.indexOf(link.getEndNode());
356         return from != -1 && to != -1 && to - from == 1;
357     }
358 
359     /**
360      * Class of objects for subclasses to return. This can contain either a single object, or a set if there are multiple
361      * objects at a single location.
362      * <p>
363      * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
364      * <br>
365      * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
366      * </p>
367      * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
368      * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
369      * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
370      */
371     protected class Entry
372     {
373 
374         /** Set. */
375         private final Set<U> set;
376 
377         /** Object. */
378         private final U object;
379 
380         /** Counter. */
381         private final C counter;
382 
383         /** Position on the lane. */
384         private final Length position;
385 
386         /**
387          * Constructor.
388          * @param object U; object
389          * @param counter C; counter, may be {@code null}
390          * @param position Length; position
391          */
392         public Entry(final U object, final C counter, final Length position)
393         {
394             this.set = null;
395             this.object = object;
396             this.counter = counter;
397             this.position = position;
398         }
399 
400         /**
401          * Constructor.
402          * @param set Set&lt;U&gt;; set
403          * @param counter C; counter, may be {@code null}
404          * @param position Length; position
405          */
406         public Entry(final Set<U> set, final C counter, final Length position)
407         {
408             this.set = set;
409             this.object = null;
410             this.counter = counter;
411             this.position = position;
412         }
413 
414         /**
415          * Returns whether this entry contains a set.
416          * @return whether this entry contains a set
417          */
418         final boolean isSet()
419         {
420             return this.set != null;
421         }
422 
423         /**
424          * Returns the underlying object. Use {@code !isSet()} to check whether there is an object.
425          * @return U; underlying set
426          */
427         public U getObject()
428         {
429             return this.object;
430         }
431 
432         /**
433          * Returns the underlying set. Use {@code isSet()} to check whether there is a set.
434          * @return Set&lt;U&gt;; underlying set
435          */
436         public Set<U> getSet()
437         {
438             return this.set;
439         }
440 
441     }
442 
443 }