View Javadoc
1   package org.opentrafficsim.road.gtu.lane.perception;
2   
3   import java.util.HashMap;
4   import java.util.Iterator;
5   import java.util.LinkedList;
6   import java.util.Map;
7   import java.util.Queue;
8   import java.util.Set;
9   import java.util.SortedMap;
10  import java.util.TreeMap;
11  
12  import org.djunits.value.vdouble.scalar.Length;
13  import org.djutils.exceptions.Try;
14  import org.opentrafficsim.core.gtu.GTUException;
15  import org.opentrafficsim.core.gtu.RelativePosition;
16  import org.opentrafficsim.core.network.Link;
17  import org.opentrafficsim.core.network.route.Route;
18  import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
19  import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
20  
21  /**
22   * Abstract iterable that figures out how to find the next nearest object, including splits.
23   * <p>
24   * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
25   * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
26   * <p>
27   * @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <br>
28   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
29   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
30   * @author <a href="http://www.transport.citg.tudelft.nl">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 LaneRecord<?> 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 LaneRecord<?> root, final Length initialPosition,
67              final boolean downstream, final Length maxDistance, final RelativePosition relativePosition, final Route route)
68      {
69          super(perceivingGtu);
70          this.root = root;
71          this.initialPosition = initialPosition;
72          this.downstream = downstream;
73          this.maxDistance = maxDistance.si;
74          this.relativePosition = relativePosition;
75          this.route = route;
76      }
77  
78      /** {@inheritDoc} */
79      @Override
80      public Iterator<PrimaryIteratorEntry> primaryIterator()
81      {
82          return new PrimaryIterator();
83      }
84  
85      /**
86       * Returns the next object(s) on the lane represented by the record. This should only consider objects on the given lane.
87       * This method should not check the distance towards objects with the maximum distance.
88       * @param record LaneRecord&lt;?&gt;; record representing the lane and direction
89       * @param position Length; position to look beyond
90       * @param counter C; counter
91       * @return next object(s) on the lane or {@code null} if none
92       * @throws GTUException on any exception in the process
93       */
94      protected abstract Entry getNext(LaneRecord<?> record, Length position, C counter) throws GTUException;
95  
96      /**
97       * Returns the distance to the object. The position fed in to this method is directly taken from an {@code Entry} returned
98       * by {@code getNext}. The two methods need to be consistent with each other.
99       * @param object U; underlying object
100      * @param record LaneRecord&lt;?&gt;; record representing the lane and direction
101      * @param position Length; position of the object on the lane
102      * @return Length; distance to the object
103      */
104     protected abstract Length getDistance(U object, LaneRecord<?> record, Length position);
105 
106     /**
107      * Returns the longitudinal length of the relevant relative position such that distances to this points can be calculated.
108      * @return Length; the longitudinal length of the relevant relative position such that distances to this points can be
109      *         calculated
110      */
111     protected Length getDx()
112     {
113         return this.relativePosition.getDx();
114     }
115 
116     /**
117      * The primary iterator is used by all returned iterators to find the next object. This contains the core algorithm to deal
118      * with splits and multiple objects at a single location.
119      * <p>
120      * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
121      * <br>
122      * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
123      * <p>
124      * @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <br>
125      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
126      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
127      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
128      */
129     private class PrimaryIterator implements Iterator<PrimaryIteratorEntry>
130     {
131 
132         /** Map containing the objects found per branch. */
133         private SortedMap<PrimaryIteratorEntry, LaneRecord<?>> map;
134 
135         /** Position on the lane of each object. */
136         private Map<U, Length> positions = new HashMap<>();
137 
138         /** Sets of remaining objects at the same location. */
139         private Map<LaneRecord<?>, Queue<PrimaryIteratorEntry>> queues = new HashMap<>();
140 
141         /** Counter objects per lane. */
142         private Map<LaneRecord<?>, C> counters = new HashMap<>();
143 
144         /** Record regarding a postponed call to {@code getNext()}. */
145         private LaneRecord<?> postponedRecord = null;
146 
147         /** Position regarding a postponed call to {@code getNext()}. */
148         private Length postponedPosition = null;
149 
150         /** Constructor. */
151         PrimaryIterator()
152         {
153             //
154         }
155 
156         /** {@inheritDoc} */
157         @Override
158         public boolean hasNext()
159         {
160             // (re)start the process
161             startProcess();
162 
163             // check next value
164             return !this.map.isEmpty();
165         }
166 
167         /** {@inheritDoc} */
168         @Override
169         public PrimaryIteratorEntry next()
170         {
171             // (re)start the process
172             startProcess();
173 
174             // get and remove next
175             PrimaryIteratorEntry nextEntry = this.map.firstKey();
176             U next = nextEntry.object;
177             LaneRecord<?> record = this.map.get(nextEntry);
178             Length position = this.positions.get(next);
179             this.map.remove(nextEntry);
180 
181             // see if we can obtain the next from a queue
182             Queue<PrimaryIteratorEntry> queue = this.queues.get(next);
183             if (queue != null)
184             {
185                 PrimaryIteratorEntry nextNext = queue.poll();
186                 this.map.put(nextNext, record); // next object is made available in the map
187                 this.positions.put(nextNext.object, position);
188                 if (queue.isEmpty())
189                 {
190                     this.queues.remove(record);
191                 }
192                 return new PrimaryIteratorEntry(nextNext.object, getDistance(nextNext.object, record, position));
193             }
194 
195             // prepare for next
196             this.postponedRecord = record;
197             this.postponedPosition = position;
198             return new PrimaryIteratorEntry(next, getDistance(next, record, position));
199         }
200 
201         /**
202          * Starts or restarts the process.
203          */
204         @SuppressWarnings("synthetic-access")
205         private void startProcess()
206         {
207             if (this.postponedRecord != null)
208             {
209                 // restart the process; perform prepareNext() that was postponed
210                 prepareNext(this.postponedRecord, this.postponedPosition);
211                 this.postponedRecord = null;
212                 this.postponedPosition = null;
213             }
214             else if (this.map == null)
215             {
216                 // start the process
217                 this.map = new TreeMap<>();
218                 prepareNext(AbstractPerceptionIterable.this.root, AbstractPerceptionIterable.this.initialPosition);
219             }
220         }
221 
222         /**
223          * Iterative method that continues a search on the next lanes if no object is found.
224          * @param record LaneRecord&lt;?&gt;; record
225          * @param position Length; position
226          */
227         @SuppressWarnings("synthetic-access")
228         private void prepareNext(final LaneRecord<?> record, final Length position)
229         {
230             Entry next = Try.assign(() -> AbstractPerceptionIterable.this.getNext(record, position, this.counters.get(record)),
231                     "Exception while deriving next object.");
232             if (next == null)
233             {
234                 this.counters.remove(record);
235                 double distance = AbstractPerceptionIterable.this.downstream
236                         ? record.getStartDistance().si + record.getLength().si : -record.getStartDistance().si;
237                 // TODO this let's us ignore an object that is registered on the next lane, but who's tail may be on this lane
238                 if (distance < AbstractPerceptionIterable.this.maxDistance)
239                 {
240                     if (AbstractPerceptionIterable.this.downstream)
241                     {
242                         for (LaneRecord<?> nextRecord : record.getNext())
243                         {
244                             if (isOnRoute(nextRecord))
245                             {
246                                 prepareNext(nextRecord, Length.createSI(-1e-9));
247                             }
248                         }
249                     }
250                     else
251                     {
252                         for (LaneRecord<?> nextRecord : record.getPrev())
253                         {
254                             if (isOnRoute(nextRecord))
255                             {
256                                 prepareNext(nextRecord, nextRecord.getLength());
257                             }
258                         }
259                     }
260                 }
261             }
262             else
263             {
264                 this.counters.put(record, next.counter);
265                 if (next.isSet())
266                 {
267                     Iterator<U> it = next.set.iterator();
268                     U nextNext = it.next();
269                     Length distance = getDistance(nextNext, record, next.position);
270                     if (distance == null // null means the object overlaps and is close
271                             || distance.si <= AbstractPerceptionIterable.this.maxDistance)
272                     {
273                         // next object is made available in the map
274                         this.map.put(new PrimaryIteratorEntry(nextNext, distance), record);
275                         this.positions.put(nextNext, next.position);
276                         if (next.set.size() > 1)
277                         {
278                             // remaining at this location are made available in a queue
279                             Queue<PrimaryIteratorEntry> queue = new LinkedList<>();
280                             while (it.hasNext())
281                             {
282                                 nextNext = it.next();
283                                 queue.add(new PrimaryIteratorEntry(nextNext, getDistance(nextNext, record, next.position)));
284                             }
285                             this.queues.put(record, queue);
286                         }
287                     }
288                 }
289                 else
290                 {
291                     Length distance = getDistance(next.object, record, next.position);
292                     if (distance == null // null means the object overlaps and is close
293                             || distance.si <= AbstractPerceptionIterable.this.maxDistance)
294                     {
295                         // next object is made available in the map
296                         this.map.put(new PrimaryIteratorEntry(next.object, distance), record);
297                         this.positions.put(next.object, next.position);
298                     }
299                 }
300             }
301         }
302 
303     }
304 
305     /**
306      * Returns whether the record is on the route.
307      * @param record LaneRecord&lt;?&gt;; record
308      * @return boolean; whether the record is on the route
309      */
310     final boolean isOnRoute(final LaneRecord<?> record)
311     {
312         if (this.route == null)
313         {
314             return true;
315         }
316         Link link = record.getLane().getParentLink();
317         int from;
318         int to;
319         if (record.getDirection().isPlus())
320         {
321             from = this.route.indexOf(link.getStartNode());
322             to = this.route.indexOf(link.getEndNode());
323         }
324         else
325         {
326             from = this.route.indexOf(link.getEndNode());
327             to = this.route.indexOf(link.getStartNode());
328         }
329         return from != -1 && to != -1 && to - from == 1;
330     }
331 
332     /**
333      * Class of objects for subclasses to return. This can contain either a single object, or a set if there are multiple
334      * objects at a single location.
335      * <p>
336      * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
337      * <br>
338      * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
339      * <p>
340      * @version $Revision$, $LastChangedDate$, by $Author$, initial version 16 feb. 2018 <br>
341      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
342      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
343      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
344      */
345     protected class Entry
346     {
347 
348         /** Set. */
349         private final Set<U> set;
350 
351         /** Object. */
352         private final U object;
353 
354         /** Counter. */
355         private final C counter;
356 
357         /** Position on the lane. */
358         private final Length position;
359 
360         /**
361          * Constructor.
362          * @param object U; object
363          * @param counter C; counter, may be {@code null}
364          * @param position Length; position
365          */
366         public Entry(final U object, final C counter, final Length position)
367         {
368             this.set = null;
369             this.object = object;
370             this.counter = counter;
371             this.position = position;
372         }
373 
374         /**
375          * Constructor.
376          * @param set Set&lt;U&gt;; set
377          * @param counter C; counter, may be {@code null}
378          * @param position Length; position
379          */
380         public Entry(final Set<U> set, final C counter, final Length position)
381         {
382             this.set = set;
383             this.object = null;
384             this.counter = counter;
385             this.position = position;
386         }
387 
388         /**
389          * Returns whether this entry contains a set.
390          * @return whether this entry contains a set
391          */
392         final boolean isSet()
393         {
394             return this.set != null;
395         }
396 
397     }
398 
399 }