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 Length; 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 Length; 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 Object; 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 Object; 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 Length; 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 Length; location to set info for a speed limit type
99 * @param speedLimitType SpeedLimitType<T>; speed limit type to set the info for
100 * @param speedInfo T; speed info to set
101 * @param source Object; 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 Length; distance to remove speed info of a speed limit type
119 * @param speedLimitType SpeedLimitType<?>; speed limit type to remove speed info of
120 * @param source Object; 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 SpeedLimitEntry<?>; speed limit entry to add
142 * @param source Object; source object
143 * @param remove boolean; 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 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 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 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 SpeedLimitType<?>; speed limit type to get the distances of
235 * @param min Length; minimum distance, may be {@code null} for no minimum limit
236 * @param max Length; 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 Length; distance to check
257 * @param speedLimitType 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 Length; distance where the info changed
278 * @param speedLimitType SpeedLimitType<T>; 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 Length; 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 Speed; current speed
334 * @param acceleration Acceleration; acceleration to apply
335 * @param time Duration; 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 SpeedLimitInfo; speed limit info to put speed info in
353 * @param speedLimitEntry 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 Length; distance to get the speed info at
368 * @param speedLimitType SpeedLimitType<T>; 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 /** {@inheritDoc} */
392 @Override
393 public final String toString()
394 {
395 StringBuilder stringBuilder = new StringBuilder("SpeedLimitProspect [");
396 String sep = "";
397 for (SpeedLimitEntry<?> sle : this.prospect)
398 {
399 stringBuilder.append(sep).append(sle.getDistance()).append(": ");
400 if (sle.getSpeedInfo() == null)
401 {
402 stringBuilder.append(sle.getSpeedLimitType().getId()).append("=END");
403 }
404 else
405 {
406 stringBuilder.append(sle.getSpeedLimitType().getId()).append("=");
407 stringBuilder.append(sle.getSpeedInfo());
408 }
409 sep = ", ";
410 }
411 stringBuilder.append("]");
412 return stringBuilder.toString();
413 }
414
415 /**
416 * Stores speed limit type and it's speed info with a location.
417 * <p>
418 * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
419 * <br>
420 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
421 * </p>
422 * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
423 * @param <T> class of speed info
424 */
425 private static class SpeedLimitEntry<T> implements Comparable<SpeedLimitEntry<?>>, Serializable
426 {
427
428 /** */
429 private static final long serialVersionUID = 20160501L;
430
431 /** Location of the speed info. */
432 private Length distance;
433
434 /** Speed limit type. */
435 private final SpeedLimitType<T> speedLimitType;
436
437 /** Speed info. */
438 private final T speedInfo;
439
440 /**
441 * Constructor.
442 * @param distance Length; location of the speed info
443 * @param speedLimitType SpeedLimitType<T>; speed limit type
444 * @param speedInfo T; speed info
445 */
446 SpeedLimitEntry(final Length distance, final SpeedLimitType<T> speedLimitType, final T speedInfo)
447 {
448 this.distance = distance;
449 this.speedLimitType = speedLimitType;
450 this.speedInfo = speedInfo;
451 }
452
453 /**
454 * Returns the location of the speed info.
455 * @return location of the speed info
456 */
457 public final Length getDistance()
458 {
459 return this.distance;
460 }
461
462 /**
463 * Returns the speed limit type.
464 * @return speed limit type
465 */
466 public final SpeedLimitType<T> getSpeedLimitType()
467 {
468 return this.speedLimitType;
469 }
470
471 /**
472 * Returns the speed info.
473 * @return the speed info
474 */
475 public final T getSpeedInfo()
476 {
477 return this.speedInfo;
478 }
479
480 /**
481 * Move the record by a given distance.
482 * @param dist Length; distance to move
483 */
484 public final void move(final Length dist)
485 {
486 this.distance = this.distance.minus(dist);
487 }
488
489 /** {@inheritDoc} */
490 @Override
491 public final int hashCode()
492 {
493 final int prime = 31;
494 int result = 1;
495 result = prime * result + this.distance.hashCode();
496 result = prime * result + this.speedInfo.hashCode();
497 result = prime * result + this.speedLimitType.hashCode();
498 return result;
499 }
500
501 /** {@inheritDoc} */
502 @Override
503 public final boolean equals(final Object obj)
504 {
505 if (this == obj)
506 {
507 return true;
508 }
509 if (obj == null)
510 {
511 return false;
512 }
513 if (getClass() != obj.getClass())
514 {
515 return false;
516 }
517 SpeedLimitEntry<?> other = (SpeedLimitEntry<?>) obj;
518 if (!this.distance.equals(other.distance))
519 {
520 return false;
521 }
522 if (!this.speedLimitType.equals(other.speedLimitType))
523 {
524 return false;
525 }
526 if (this.speedInfo == null)
527 {
528 if (other.speedInfo != null)
529 {
530 return false;
531 }
532 }
533 else if (!this.speedInfo.equals(other.speedInfo))
534 {
535 return false;
536 }
537 return true;
538 }
539
540 /** {@inheritDoc} */
541 @Override
542 public final int compareTo(final SpeedLimitEntry<?> speedLimitEntry)
543 {
544 if (this.equals(speedLimitEntry))
545 {
546 return 0;
547 }
548 // order by distance
549 int comp = this.distance.compareTo(speedLimitEntry.distance);
550 if (comp != 0)
551 {
552 return comp;
553 }
554 // order by speed limit type
555 comp = this.speedLimitType.getId().compareTo(speedLimitEntry.speedLimitType.getId());
556 if (comp != 0)
557 {
558 return comp;
559 }
560 // equal distance and speed limit type is not allowed, so below code is not used
561 // if this requirement changes, compareTo should still work
562 if (this.speedInfo == null)
563 {
564 if (speedLimitEntry.speedInfo == null)
565 {
566 return 0; // both null
567 }
568 return -1; // null under non-null
569 }
570 else if (speedLimitEntry.speedInfo == null)
571 {
572 return 1; // non-null over null
573 }
574 return this.speedInfo.hashCode() < speedLimitEntry.speedInfo.hashCode() ? -1 : 1;
575 }
576
577 /** {@inheritDoc} */
578 @Override
579 public final String toString()
580 {
581 return "SpeedLimitEntry [distance=" + this.distance + ", speedLimitType=" + this.speedLimitType + ", speedInfo="
582 + this.speedInfo + "]";
583 }
584
585 }
586
587 }