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