View Javadoc
1   package org.opentrafficsim.road.network.speed;
2   
3   import java.io.Serializable;
4   import java.util.ArrayList;
5   import java.util.HashMap;
6   import java.util.List;
7   import java.util.Map;
8   import java.util.SortedSet;
9   import java.util.TreeSet;
10  
11  import org.djunits.unit.LengthUnit;
12  import org.djunits.value.vdouble.scalar.Acceleration;
13  import org.djunits.value.vdouble.scalar.Duration;
14  import org.djunits.value.vdouble.scalar.Length;
15  import org.djunits.value.vdouble.scalar.Speed;
16  import org.djutils.exceptions.Throw;
17  
18  /**
19   * Prospect of speed limits ahead, both legal and otherwise (e.g. curve, speed bump).
20   * <p>
21   * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
22   * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
23   * <p>
24   * @version $Revision$, $LastChangedDate$, by $Author$, initial version May 1, 2016 <br>
25   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
26   * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
27   */
28  public class SpeedLimitProspect implements Serializable
29  {
30  
31      /** */
32      private static final long serialVersionUID = 20160501L;
33  
34      /** Spatial prospect of speed info. */
35      private final SortedSet<SpeedLimitEntry<?>> prospect = new TreeSet<>();
36  
37      /** Source objects for the speed info additions. */
38      private final Map<Object, SpeedLimitEntry<?>> addSources = new HashMap<>();
39  
40      /** Source objects for the speed info removals. */
41      private final Map<Object, SpeedLimitEntry<?>> removeSources = new HashMap<>();
42  
43      /** Last odometer value. */
44      private Length odometer;
45  
46      /**
47       * Constructor.
48       * @param odometer Length; odometer value
49       */
50      public SpeedLimitProspect(final Length odometer)
51      {
52          this.odometer = odometer;
53      }
54  
55      /**
56       * Updates the distance values.
57       * @param newOdometer Length; odometer value
58       */
59      public void update(final Length newOdometer)
60      {
61          Length dx = newOdometer.minus(this.odometer);
62          for (SpeedLimitEntry<?> entry : this.prospect)
63          {
64              entry.move(dx);
65          }
66      }
67  
68      /**
69       * Returns whether the given source is already added in the prospect.
70       * @param source Object; source
71       * @return whether the given source is already added in the prospect
72       */
73      public final boolean containsAddSource(final Object source)
74      {
75          return this.addSources.containsKey(source);
76      }
77  
78      /**
79       * Returns whether the given source is already removed in the prospect.
80       * @param source Object; source
81       * @return whether the given source is already removed in the prospect
82       */
83      public final boolean containsRemoveSource(final Object source)
84      {
85          return this.removeSources.containsKey(source);
86      }
87  
88      /**
89       * Returns the odometer value at which the last update was performed.
90       * @return Length; odometer value at which the last update was performed
91       */
92      public final Length getOdometer()
93      {
94          return this.odometer;
95      }
96  
97      /**
98       * Sets the speed info of a speed limit type.
99       * @param distance Length; location to set info for a speed limit type
100      * @param speedLimitType SpeedLimitType&lt;T&gt;; speed limit type to set the info for
101      * @param speedInfo T; speed info to set
102      * @param source Object; source object
103      * @param <T> class of speed info
104      * @throws IllegalStateException if speed info for a specific speed limit type is set or removed twice at the same distance
105      * @throws IllegalStateException if speed info for a specific speed limit type is set twice with negative distance
106      * @throws NullPointerException if any input is null
107      */
108     public final <T> void addSpeedInfo(final Length distance, final SpeedLimitType<T> speedLimitType, final T speedInfo,
109             final Object source)
110     {
111         Throw.whenNull(distance, "Distance may not be null.");
112         Throw.whenNull(speedLimitType, "Speed limit type may not be null.");
113         Throw.whenNull(speedInfo, "Speed info may not be null.");
114         checkAndAdd(new SpeedLimitEntry<>(distance, speedLimitType, speedInfo), source, false);
115     }
116 
117     /**
118      * Removes the speed info of a speed limit type.
119      * @param distance Length; distance to remove speed info of a speed limit type
120      * @param speedLimitType SpeedLimitType&lt;?&gt;; speed limit type to remove speed info of
121      * @param source Object; source object
122      * @throws IllegalStateException if speed info for a specific speed limit type is set or removed twice at the same distance
123      * @throws IllegalArgumentException if the speed limit type is {@code MAX_VEHICLE_SPEED}
124      * @throws IllegalArgumentException if the distance is negative
125      * @throws NullPointerException if any input is null
126      */
127     @SuppressWarnings({ "unchecked", "rawtypes" })
128     public final void removeSpeedInfo(final Length distance, final SpeedLimitType<?> speedLimitType, final Object source)
129     {
130         Throw.whenNull(distance, "Distance may not be null.");
131         Throw.when(distance.si < 0, IllegalArgumentException.class,
132                 "Removing speed info in the past is not allowed. " + "Only add still active speed info.");
133         Throw.whenNull(speedLimitType, "Speed limit type may not be null.");
134         Throw.when(speedLimitType.equals(SpeedLimitTypes.MAX_VEHICLE_SPEED), IllegalArgumentException.class,
135                 "May not remove the maximum vehicle speed.");
136         // null value does not comply to being a T for SpeedLimitType<T> but is separately treated
137         checkAndAdd(new SpeedLimitEntry(distance, speedLimitType, null), source, true);
138     }
139 
140     /**
141      * Checks the speed limit entry before adding to the prospect.
142      * @param speedLimitEntry SpeedLimitEntry&lt;?&gt;; speed limit entry to add
143      * @param source Object; source object
144      * @param remove boolean; whether the source causes a removal of info
145      * @throws IllegalStateException if the speed entry forms an undefined set with any existing entry
146      */
147     private void checkAndAdd(final SpeedLimitEntry<?> speedLimitEntry, final Object source, final boolean remove)
148     {
149         for (SpeedLimitEntry<?> s : this.prospect)
150         {
151             if (s.getSpeedLimitType().equals(speedLimitEntry.getSpeedLimitType()))
152             {
153                 /*
154                  * For entries at the same distance, the speed limit type may not be the same, this leaves us with an undefined
155                  * state as it cannot be derived which remains valid further on.
156                  */
157                 Throw.when(s.getDistance().equals(speedLimitEntry.getDistance()), IllegalStateException.class,
158                         "Info " + "of speed limit type '%s' is set twice at the same location (%s). This is undefined. "
159                                 + "Either remove speed info, or overwrite with new speed info.",
160                         s.getSpeedLimitType(), s.getDistance());
161             }
162         }
163         if (remove)
164         {
165             SpeedLimitEntry<?> prev = this.removeSources.get(source);
166             if (prev != null)
167             {
168                 this.prospect.remove(prev);
169             }
170             this.removeSources.put(source, speedLimitEntry);
171         }
172         else
173         {
174             SpeedLimitEntry<?> prev = this.addSources.get(source);
175             if (prev != null)
176             {
177                 this.prospect.remove(prev);
178             }
179             this.addSources.put(source, speedLimitEntry);
180         }
181         this.prospect.add(speedLimitEntry);
182     }
183 
184     /**
185      * Returns the distances at which a change in the prospect is present in order (upstream first). If multiple changes are
186      * present at the same distance, only one distance is returned in the list.
187      * @return distances at which a change in the prospect is present in order (upstream first)
188      */
189     public final List<Length> getDistances()
190     {
191         List<Length> list = new ArrayList<>();
192         for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
193         {
194             list.add(speedLimitEntry.getDistance());
195         }
196         return list;
197     }
198 
199     /**
200      * Returns the distances at which a change of the given speed limit type in the prospect is present in order (most upstream
201      * first). If multiple changes are present at the same distance, only one distance is returned in the list.
202      * @param speedLimitType SpeedLimitType&lt;?&gt;; speed limit type to get the distances of
203      * @return distances at which a change of the given speed limit type in the prospect is present in order
204      */
205     public final List<Length> getDistances(final SpeedLimitType<?> speedLimitType)
206     {
207         return getDistancesInRange(speedLimitType, null, null);
208     }
209 
210     /**
211      * Returns the upstream distances at which a change of the given speed limit type in the prospect is present in order (most
212      * upstream first). If multiple changes are present at the same distance, only one distance is returned in the list.
213      * @param speedLimitType SpeedLimitType&lt;?&gt;; speed limit type to get the distances of
214      * @return distances at which a change of the given speed limit type in the prospect is present in order
215      */
216     public final List<Length> getUpstreamDistances(final SpeedLimitType<?> speedLimitType)
217     {
218         return getDistancesInRange(speedLimitType, null, Length.ZERO);
219     }
220 
221     /**
222      * Returns the downstream distances at which a change of the given speed limit type in the prospect is present in order
223      * (most upstream first). If multiple changes are present at the same distance, only one distance is returned in the list.
224      * @param speedLimitType SpeedLimitType&lt;?&gt;; speed limit type to get the distances of
225      * @return distances at which a change of the given speed limit type in the prospect is present in order
226      */
227     public final List<Length> getDownstreamDistances(final SpeedLimitType<?> speedLimitType)
228     {
229         return getDistancesInRange(speedLimitType, Length.ZERO, null);
230     }
231 
232     /**
233      * Returns the distances between limits at which a change of the given speed limit type in the prospect is present in order
234      * (most upstream first). If multiple changes are present at the same distance, only one distance is returned in the list.
235      * @param speedLimitType SpeedLimitType&lt;?&gt;; speed limit type to get the distances of
236      * @param min Length; minimum distance, may be {@code null} for no minimum limit
237      * @param max Length; maximum distance, may be {@code null} for no maximum limit
238      * @return distances at which a change of the given speed limit type in the prospect is present in order
239      */
240     private List<Length> getDistancesInRange(final SpeedLimitType<?> speedLimitType, final Length min, final Length max)
241     {
242         List<Length> list = new ArrayList<>();
243         for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
244         {
245             if (speedLimitEntry.getSpeedLimitType().equals(speedLimitType)
246                     && (min == null || speedLimitEntry.getDistance().gt(min))
247                     && (max == null || speedLimitEntry.getDistance().le(max)))
248             {
249                 list.add(speedLimitEntry.getDistance());
250             }
251         }
252         return list;
253     }
254 
255     /**
256      * Returns whether the given speed limit type is changed at the given distance.
257      * @param distance Length; distance to check
258      * @param speedLimitType SpeedLimitType&lt;?&gt;; speed limit type to check
259      * @return whether the given speed limit type is changed at the given distance
260      * @throws NullPointerException if distance is null
261      */
262     public final boolean speedInfoChanged(final Length distance, final SpeedLimitType<?> speedLimitType)
263     {
264         Throw.whenNull(distance, "Distance may not be null.");
265         for (SpeedLimitEntry<?> sle : this.prospect)
266         {
267             if (sle.getDistance().eq(distance) && sle.getSpeedLimitType().equals(speedLimitType))
268             {
269                 return true;
270             }
271         }
272         return false;
273     }
274 
275     /**
276      * Returns the speed info of given speed limit type where it changed. If the change was removing the speed limit type (e.g.
277      * end of corner), then {@code null} is returned.
278      * @param distance Length; distance where the info changed
279      * @param speedLimitType SpeedLimitType&lt;T&gt;; speed limit type
280      * @return speed info of given speed limit type where it changed, {@code null} if speed limit type is removed
281      * @throws IllegalArgumentException if the speed info did not change at the given distance for the speed limit type
282      * @param <T> class of the speed limit type info
283      */
284     public final <T> T getSpeedInfoChange(final Length distance, final SpeedLimitType<T> speedLimitType)
285     {
286         for (SpeedLimitEntry<?> sle : this.prospect)
287         {
288             if (sle.getDistance().eq(distance) && sle.getSpeedLimitType().equals(speedLimitType))
289             {
290                 @SuppressWarnings("unchecked")
291                 T info = (T) sle.getSpeedInfo();
292                 return info;
293             }
294         }
295         throw new IllegalArgumentException("Speed info of speed limit type '" + speedLimitType.getId()
296                 + "' is requested at a distance '" + distance + "' where the info is not changed.");
297     }
298 
299     /**
300      * Returns the speed info at a given location.
301      * @param distance Length; where to get the speed info
302      * @return speed info at a given distance
303      * @throws NullPointerException if distance is null
304      */
305     public final SpeedLimitInfo getSpeedLimitInfo(final Length distance)
306     {
307         Throw.whenNull(distance, "Distance may not be null.");
308         SpeedLimitInfo speedLimitInfo = new SpeedLimitInfo();
309         for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
310         {
311             // use compareTo as this also determines order in this.prospect
312             if (speedLimitEntry.getDistance().compareTo(distance) > 0)
313             {
314                 // remaining entries are further ahead
315                 return speedLimitInfo;
316             }
317             // make appropriate change to speedLimitInfo
318             if (speedLimitEntry.getSpeedInfo() == null)
319             {
320                 speedLimitInfo.removeSpeedInfo(speedLimitEntry.getSpeedLimitType());
321             }
322             else
323             {
324                 // method addSpeedInfo guarantees that speedInfo in speedLimitEntry is T
325                 // for speedLimitType in speedLimitEntry is SpeedLimitType<T>, null is checked above
326                 setAsType(speedLimitInfo, speedLimitEntry);
327             }
328         }
329         return speedLimitInfo;
330     }
331 
332     /**
333      * Returns the speed info at a location following an acceleration over some duration.
334      * @param speed Speed; current speed
335      * @param acceleration Acceleration; acceleration to apply
336      * @param time Duration; duration of acceleration
337      * @return speed info at a given distance
338      * @throws NullPointerException if any input is null
339      */
340     public final SpeedLimitInfo getSpeedLimitInfo(final Speed speed, final Acceleration acceleration, final Duration time)
341     {
342         Throw.whenNull(speed, "Speed may not be null.");
343         Throw.whenNull(acceleration, "Acceleration may not be null.");
344         Throw.whenNull(time, "Time may not be null.");
345         return getSpeedLimitInfo(new Length(speed.si * time.si + .5 * acceleration.si * time.si * time.si, LengthUnit.SI));
346     }
347 
348     /**
349      * Sets speed info for a speed limit type in speed limit info by explicitly casting the types. From the context it should be
350      * certain that the speed info inside the speed limit entry matches the declared info type of the speed limit type inside
351      * the entry, i.e. {@code speedLimitEntry.getSpeedLimitType() = SpeedLimitType<T>} and
352      * {@code speedLimitEntry.getSpeedInfo() = T}.
353      * @param speedLimitInfo SpeedLimitInfo; speed limit info to put speed info in
354      * @param speedLimitEntry SpeedLimitEntry&lt;?&gt;; entry with speed limit type and speed info to set
355      * @param <T> underlying speed info class depending on speed limit type
356      */
357     @SuppressWarnings("unchecked")
358     private <T> void setAsType(final SpeedLimitInfo speedLimitInfo, final SpeedLimitEntry<?> speedLimitEntry)
359     {
360         SpeedLimitType<T> speedLimitType = (SpeedLimitType<T>) speedLimitEntry.getSpeedLimitType();
361         T speedInfoOfType = (T) speedLimitEntry.getSpeedInfo();
362         speedLimitInfo.addSpeedInfo(speedLimitType, speedInfoOfType);
363     }
364 
365     /**
366      * Builds speed limit info with only MAX_VEHICLE_SPEED and the given speed limit type, where the speed info is obtained at
367      * the given distance.
368      * @param distance Length; distance to get the speed info at
369      * @param speedLimitType SpeedLimitType&lt;T&gt;; speed limit type of which to include the info
370      * @param <T> class of speed info of given speed limit type
371      * @return speed limit info with only MAX_VEHICLE_SPEED and the given speed limit type
372      */
373     public final <T> SpeedLimitInfo buildSpeedLimitInfo(final Length distance, final SpeedLimitType<T> speedLimitType)
374     {
375         SpeedLimitInfo out = new SpeedLimitInfo();
376         out.addSpeedInfo(speedLimitType, getSpeedInfoChange(distance, speedLimitType));
377         for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
378         {
379             if (speedLimitEntry.getDistance().gt(distance))
380             {
381                 break;
382             }
383             if (speedLimitEntry.getSpeedLimitType().equals(SpeedLimitTypes.MAX_VEHICLE_SPEED))
384             {
385                 out.addSpeedInfo(SpeedLimitTypes.MAX_VEHICLE_SPEED,
386                         SpeedLimitTypes.MAX_VEHICLE_SPEED.getInfoClass().cast(speedLimitEntry.getSpeedInfo()));
387             }
388         }
389         return out;
390     }
391 
392     /** {@inheritDoc} */
393     @Override
394     public final String toString()
395     {
396         StringBuilder stringBuilder = new StringBuilder("SpeedLimitProspect [");
397         String sep = "";
398         for (SpeedLimitEntry<?> sle : this.prospect)
399         {
400             stringBuilder.append(sep).append(sle.getDistance()).append(": ");
401             if (sle.getSpeedInfo() == null)
402             {
403                 stringBuilder.append(sle.getSpeedLimitType().getId()).append("=END");
404             }
405             else
406             {
407                 stringBuilder.append(sle.getSpeedLimitType().getId()).append("=");
408                 stringBuilder.append(sle.getSpeedInfo());
409             }
410             sep = ", ";
411         }
412         stringBuilder.append("]");
413         return stringBuilder.toString();
414     }
415 
416     /**
417      * Stores speed limit type and it's speed info with a location.
418      * <p>
419      * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
420      * <br>
421      * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
422      * <p>
423      * @version $Revision$, $LastChangedDate$, by $Author$, initial version May 1, 2016 <br>
424      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
425      * @param <T> class of speed info
426      */
427     private static class SpeedLimitEntry<T> implements Comparable<SpeedLimitEntry<?>>, Serializable
428     {
429 
430         /** */
431         private static final long serialVersionUID = 20160501L;
432 
433         /** Location of the speed info. */
434         private Length distance;
435 
436         /** Speed limit type. */
437         private final SpeedLimitType<T> speedLimitType;
438 
439         /** Speed info. */
440         private final T speedInfo;
441 
442         /**
443          * Constructor.
444          * @param distance Length; location of the speed info
445          * @param speedLimitType SpeedLimitType&lt;T&gt;; speed limit type
446          * @param speedInfo T; speed info
447          */
448         SpeedLimitEntry(final Length distance, final SpeedLimitType<T> speedLimitType, final T speedInfo)
449         {
450             this.distance = distance;
451             this.speedLimitType = speedLimitType;
452             this.speedInfo = speedInfo;
453         }
454 
455         /**
456          * Returns the location of the speed info.
457          * @return location of the speed info
458          */
459         public final Length getDistance()
460         {
461             return this.distance;
462         }
463 
464         /**
465          * Returns the speed limit type.
466          * @return speed limit type
467          */
468         public final SpeedLimitType<T> getSpeedLimitType()
469         {
470             return this.speedLimitType;
471         }
472 
473         /**
474          * Returns the speed info.
475          * @return the speed info
476          */
477         public final T getSpeedInfo()
478         {
479             return this.speedInfo;
480         }
481 
482         /**
483          * Move the record by a given distance.
484          * @param dist Length; distance to move
485          */
486         public final void move(final Length dist)
487         {
488             this.distance = this.distance.minus(dist);
489         }
490 
491         /** {@inheritDoc} */
492         @Override
493         public final int hashCode()
494         {
495             final int prime = 31;
496             int result = 1;
497             result = prime * result + this.distance.hashCode();
498             result = prime * result + this.speedInfo.hashCode();
499             result = prime * result + this.speedLimitType.hashCode();
500             return result;
501         }
502 
503         /** {@inheritDoc} */
504         @Override
505         public final boolean equals(final Object obj)
506         {
507             if (this == obj)
508             {
509                 return true;
510             }
511             if (obj == null)
512             {
513                 return false;
514             }
515             if (getClass() != obj.getClass())
516             {
517                 return false;
518             }
519             SpeedLimitEntry<?> other = (SpeedLimitEntry<?>) obj;
520             if (!this.distance.equals(other.distance))
521             {
522                 return false;
523             }
524             if (!this.speedLimitType.equals(other.speedLimitType))
525             {
526                 return false;
527             }
528             if (this.speedInfo == null)
529             {
530                 if (other.speedInfo != null)
531                 {
532                     return false;
533                 }
534             }
535             else if (!this.speedInfo.equals(other.speedInfo))
536             {
537                 return false;
538             }
539             return true;
540         }
541 
542         /** {@inheritDoc} */
543         @Override
544         public final int compareTo(final SpeedLimitEntry<?> speedLimitEntry)
545         {
546             if (this.equals(speedLimitEntry))
547             {
548                 return 0;
549             }
550             // order by distance
551             int comp = this.distance.compareTo(speedLimitEntry.distance);
552             if (comp != 0)
553             {
554                 return comp;
555             }
556             // order by speed limit type
557             comp = this.speedLimitType.getId().compareTo(speedLimitEntry.speedLimitType.getId());
558             if (comp != 0)
559             {
560                 return comp;
561             }
562             // equal distance and speed limit type is not allowed, so below code is not used
563             // if this requirement changes, compareTo should still work
564             if (this.speedInfo == null)
565             {
566                 if (speedLimitEntry.speedInfo == null)
567                 {
568                     return 0; // both null
569                 }
570                 return -1; // null under non-null
571             }
572             else if (speedLimitEntry.speedInfo == null)
573             {
574                 return 1; // non-null over null
575             }
576             return this.speedInfo.hashCode() < speedLimitEntry.speedInfo.hashCode() ? -1 : 1;
577         }
578 
579         /** {@inheritDoc} */
580         @Override
581         public final String toString()
582         {
583             return "SpeedLimitEntry [distance=" + this.distance + ", speedLimitType=" + this.speedLimitType + ", speedInfo="
584                     + this.speedInfo + "]";
585         }
586 
587     }
588 
589 }