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-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
26   * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
27   * <p>
28   * @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <br>
29   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
30   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
31   * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
32   * @param <H> headway type
33   * @param <U> underlying object type
34   * @param <C> counter type
35   */
36  public abstract class AbstractPerceptionIterable<H extends Headway, U, C> extends AbstractPerceptionReiterable<H, U>
37  {
38  
39      /** Root record. */
40      private final LaneRecord<?> root;
41  
42      /** Initial position. */
43      private final Length initialPosition;
44  
45      /** Search downstream (or upstream). */
46      private final boolean downstream;
47  
48      /** Max distance. */
49      private final double maxDistance;
50  
51      /** Position to which distance are calculated by subclasses. */
52      private final RelativePosition relativePosition;
53  
54      /** Route of the GTU. */
55      private final Route route;
56  
57      /**
58       * Constructor.
59       * @param perceivingGtu LaneBasedGTU; perceiving GTU
60       * @param root LaneRecord&lt;?&gt;; root record
61       * @param initialPosition Length; initial position
62       * @param downstream boolean; search downstream (or upstream)
63       * @param maxDistance Length; max distance to search
64       * @param relativePosition RelativePosition; position to which distance are calculated by subclasses
65       * @param route Route; route of the GTU, may be {@code null}
66       */
67      public AbstractPerceptionIterable(final LaneBasedGTU perceivingGtu, final LaneRecord<?> root, final Length initialPosition,
68              final boolean downstream, final Length maxDistance, 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(LaneRecord<?> 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, LaneRecord<?> 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.getDx();
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-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
133      * <br>
134      * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
135      * <p>
136      * @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <br>
137      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
138      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
139      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
140      */
141     private class PrimaryIterator implements Iterator<PrimaryIteratorEntry>
142     {
143 
144         /** Map containing the objects found per branch. */
145         private SortedMap<PrimaryIteratorEntry, LaneRecord<?>> map;
146 
147         /** Position per record where the search was halted. */
148         private Map<LaneRecord<?>, Length> positions = new LinkedHashMap<>();
149 
150         /** Items returned to prevent duplicates. */
151         private Set<U> returnedItems = new LinkedHashSet<>();
152 
153         /** Sets of remaining objects at the same location. */
154         private Map<LaneRecord<?>, Queue<PrimaryIteratorEntry>> queues = new LinkedHashMap<>();
155 
156         /** Counter objects per lane. */
157         private Map<LaneRecord<?>, C> counters = new LinkedHashMap<>();
158 
159         /** Record regarding a postponed call to {@code getNext()}. */
160         private LaneRecord<?> postponedRecord = null;
161 
162         /** Position regarding a postponed call to {@code getNext()}. */
163         private Length postponedPosition = null;
164 
165         /** Constructor. */
166         PrimaryIterator()
167         {
168             //
169         }
170 
171         /** {@inheritDoc} */
172         @Override
173         public boolean hasNext()
174         {
175             // (re)start the process
176             startProcess();
177 
178             // check next value
179             return !this.map.isEmpty();
180         }
181 
182         /** {@inheritDoc} */
183         @Override
184         public PrimaryIteratorEntry next()
185         {
186             // (re)start the process
187             startProcess();
188 
189             // get and remove next
190             PrimaryIteratorEntry nextEntry = this.map.firstKey();
191             U next = nextEntry.getObject();
192             LaneRecord<?> record = this.map.get(nextEntry);
193             this.map.remove(nextEntry);
194 
195             // see if we can obtain the next from a queue
196             Queue<PrimaryIteratorEntry> queue = this.queues.get(next);
197             if (queue != null)
198             {
199                 PrimaryIteratorEntry nextNext = queue.poll();
200                 this.map.put(nextNext, record); // next object is made available in the map
201                 if (queue.isEmpty())
202                 {
203                     this.queues.remove(record);
204                 }
205                 preventDuplicateEntries(nextEntry.getObject());
206                 return nextNext;
207             }
208 
209             // prepare for next
210             this.postponedRecord = record;
211             this.postponedPosition = this.positions.get(record); // position;
212             preventDuplicateEntries(nextEntry.getObject());
213             return nextEntry;
214         }
215 
216         /**
217          * Prevents that duplicate (and further) records are returned for the given object as splits later on merge.
218          * @param object U; object for which a {@code PrimaryIteratorEntry} will be returned
219          */
220         private void preventDuplicateEntries(final U object)
221         {
222             this.returnedItems.add(object); // prevents new items to be added over alive branches (that should die out)
223             Iterator<PrimaryIteratorEntry> it = this.map.keySet().iterator();
224             while (it.hasNext())
225             {
226                 PrimaryIteratorEntry entry = it.next();
227                 if (entry.getObject().equals(object))
228                 {
229                     it.remove();
230                 }
231             }
232         }
233 
234         /**
235          * Starts or restarts the process.
236          */
237         @SuppressWarnings("synthetic-access")
238         private void startProcess()
239         {
240             if (this.postponedRecord != null)
241             {
242                 // restart the process; perform prepareNext() that was postponed
243                 prepareNext(this.postponedRecord, this.postponedPosition);
244                 this.postponedRecord = null;
245                 this.postponedPosition = null;
246             }
247             else if (this.map == null)
248             {
249                 // start the process
250                 this.map = new TreeMap<>();
251                 prepareNext(AbstractPerceptionIterable.this.root, AbstractPerceptionIterable.this.initialPosition);
252             }
253         }
254 
255         /**
256          * Iterative method that continues a search on the next lanes if no object is found.
257          * @param record LaneRecord&lt;?&gt;; record
258          * @param position Length; position
259          */
260         @SuppressWarnings("synthetic-access")
261         private void prepareNext(final LaneRecord<?> record, final Length position)
262         {
263             Entry next = Try.assign(() -> AbstractPerceptionIterable.this.getNext(record, position, this.counters.get(record)),
264                     "Exception while deriving next object.");
265             if (next == null)
266             {
267                 this.counters.remove(record);
268                 double distance = AbstractPerceptionIterable.this.downstream
269                         ? record.getStartDistance().si + record.getLength().si : -record.getStartDistance().si;
270                 // TODO this let's us ignore an object that is registered on the next lane, but who's tail may be on this lane
271                 if (distance < AbstractPerceptionIterable.this.maxDistance)
272                 {
273                     if (AbstractPerceptionIterable.this.downstream)
274                     {
275                         for (LaneRecord<?> nextRecord : record.getNext())
276                         {
277                             if (isOnRoute(nextRecord))
278                             {
279                                 prepareNext(nextRecord, Length.instantiateSI(-1e-9));
280                             }
281                         }
282                     }
283                     else
284                     {
285                         for (LaneRecord<?> nextRecord : record.getPrev())
286                         {
287                             if (isOnRoute(nextRecord))
288                             {
289                                 prepareNext(nextRecord, nextRecord.getLength());
290                             }
291                         }
292                     }
293                 }
294             }
295             else
296             {
297                 this.counters.put(record, next.counter);
298                 if (next.isSet())
299                 {
300                     Iterator<U> it = next.set.iterator();
301                     U nextNext = it.next();
302                     if (!this.returnedItems.contains(nextNext))
303                     {
304                         Length distance = getDistance(nextNext, record, next.position);
305                         if (distance == null // null means the object overlaps and is close
306                                 || distance.si <= AbstractPerceptionIterable.this.maxDistance)
307                         {
308                             // next object is made available in the map
309                             this.map.put(new PrimaryIteratorEntry(nextNext, distance), record);
310                             this.positions.put(record, next.position);
311                             if (next.set.size() > 1)
312                             {
313                                 // remaining at this location are made available in a queue
314                                 Queue<PrimaryIteratorEntry> queue = new LinkedList<>();
315                                 while (it.hasNext())
316                                 {
317                                     nextNext = it.next();
318                                     queue.add(new PrimaryIteratorEntry(nextNext, getDistance(nextNext, record, next.position)));
319                                 }
320                                 this.queues.put(record, queue);
321                             }
322                         }
323                     }
324                 }
325                 else if (!this.returnedItems.contains(next.object))
326                 {
327                     Length distance = getDistance(next.object, record, next.position);
328                     if (distance == null // null means the object overlaps and is close
329                             || distance.si <= AbstractPerceptionIterable.this.maxDistance)
330                     {
331                         // next object is made available in the map
332                         this.map.put(new PrimaryIteratorEntry(next.object, distance), record);
333                         this.positions.put(record, next.position);
334                     }
335                 }
336             }
337         }
338 
339     }
340 
341     /**
342      * Returns whether the record is on the route.
343      * @param record LaneRecord&lt;?&gt;; record
344      * @return boolean; whether the record is on the route
345      */
346     final boolean isOnRoute(final LaneRecord<?> record)
347     {
348         if (this.route == null)
349         {
350             return true;
351         }
352         Link link = record.getLane().getParentLink();
353         int from;
354         int to;
355         if (record.getDirection().isPlus())
356         {
357             from = this.route.indexOf(link.getStartNode());
358             to = this.route.indexOf(link.getEndNode());
359         }
360         else
361         {
362             from = this.route.indexOf(link.getEndNode());
363             to = this.route.indexOf(link.getStartNode());
364         }
365         return from != -1 && to != -1 && to - from == 1;
366     }
367 
368     /**
369      * Class of objects for subclasses to return. This can contain either a single object, or a set if there are multiple
370      * objects at a single location.
371      * <p>
372      * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
373      * <br>
374      * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
375      * <p>
376      * @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <br>
377      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
378      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
379      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
380      */
381     protected class Entry
382     {
383 
384         /** Set. */
385         private final Set<U> set;
386 
387         /** Object. */
388         private final U object;
389 
390         /** Counter. */
391         private final C counter;
392 
393         /** Position on the lane. */
394         private final Length position;
395 
396         /**
397          * Constructor.
398          * @param object U; object
399          * @param counter C; counter, may be {@code null}
400          * @param position Length; position
401          */
402         public Entry(final U object, final C counter, final Length position)
403         {
404             this.set = null;
405             this.object = object;
406             this.counter = counter;
407             this.position = position;
408         }
409 
410         /**
411          * Constructor.
412          * @param set Set&lt;U&gt;; set
413          * @param counter C; counter, may be {@code null}
414          * @param position Length; position
415          */
416         public Entry(final Set<U> set, final C counter, final Length position)
417         {
418             this.set = set;
419             this.object = null;
420             this.counter = counter;
421             this.position = position;
422         }
423 
424         /**
425          * Returns whether this entry contains a set.
426          * @return whether this entry contains a set
427          */
428         final boolean isSet()
429         {
430             return this.set != null;
431         }
432 
433         /**
434          * Returns the underlying object. Use {@code !isSet()} to check whether there is an object.
435          * @return U; underlying set
436          */
437         public U getObject()
438         {
439             return this.object;
440         }
441 
442         /**
443          * Returns the underlying set. Use {@code isSet()} to check whether there is a set.
444          * @return Set&lt;U&gt;; underlying set
445          */
446         public Set<U> getSet()
447         {
448             return this.set;
449         }
450 
451     }
452 
453 }