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