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<T>; 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<?>; 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<?>; 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<?>; 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<?>; 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<?>; 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<?>; 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<?>; 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<T>; 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<?>; 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<T>; 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<T>; 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 }