1 package org.opentrafficsim.kpi.sampling;
2
3 import java.util.Arrays;
4 import java.util.LinkedHashMap;
5 import java.util.Map;
6 import java.util.Set;
7
8 import org.djunits.unit.AccelerationUnit;
9 import org.djunits.unit.DurationUnit;
10 import org.djunits.unit.LengthUnit;
11 import org.djunits.unit.SpeedUnit;
12 import org.djunits.unit.TimeUnit;
13 import org.djunits.value.StorageType;
14 import org.djunits.value.ValueException;
15 import org.djunits.value.vdouble.scalar.Acceleration;
16 import org.djunits.value.vdouble.scalar.Duration;
17 import org.djunits.value.vdouble.scalar.Length;
18 import org.djunits.value.vdouble.scalar.Speed;
19 import org.djunits.value.vdouble.scalar.Time;
20 import org.djunits.value.vfloat.vector.FloatAccelerationVector;
21 import org.djunits.value.vfloat.vector.FloatLengthVector;
22 import org.djunits.value.vfloat.vector.FloatSpeedVector;
23 import org.djunits.value.vfloat.vector.FloatTimeVector;
24 import org.opentrafficsim.kpi.interfaces.GtuDataInterface;
25 import org.opentrafficsim.kpi.sampling.data.ExtendedDataType;
26 import org.opentrafficsim.kpi.sampling.meta.MetaData;
27 import org.opentrafficsim.kpi.sampling.meta.MetaDataType;
28
29 import nl.tudelft.simulation.language.Throw;
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48 public final class Trajectory<G extends GtuDataInterface>
49 {
50
51
52 private static final int DEFAULT_CAPACITY = 10;
53
54
55 private int size = 0;
56
57
58
59
60
61 private float[] x = new float[DEFAULT_CAPACITY];
62
63
64 private float[] v = new float[DEFAULT_CAPACITY];
65
66
67 private float[] a = new float[DEFAULT_CAPACITY];
68
69
70 private float[] t = new float[DEFAULT_CAPACITY];
71
72
73 private final String gtuId;
74
75
76 private final MetaData metaData;
77
78
79 private final Map<ExtendedDataType<?, ?, ?, G>, Object> extendedData = new LinkedHashMap<>();
80
81
82 private final KpiLaneDirection kpiLaneDirection;
83
84
85
86
87
88
89
90 public Trajectory(final GtuDataInterface gtu, final MetaData metaData, final Set<ExtendedDataType<?, ?, ?, G>> extendedData,
91 final KpiLaneDirection kpiLaneDirection)
92 {
93 this(gtu == null ? null : gtu.getId(), metaData, extendedData, kpiLaneDirection);
94 }
95
96
97
98
99
100
101
102
103 private Trajectory(final String gtuId, final MetaData metaData, final Set<ExtendedDataType<?, ?, ?, G>> extendedData,
104 final KpiLaneDirection kpiLaneDirection)
105 {
106 Throw.whenNull(gtuId, "GTU may not be null.");
107 Throw.whenNull(metaData, "Meta data may not be null.");
108 Throw.whenNull(extendedData, "Extended data may not be null.");
109 Throw.whenNull(kpiLaneDirection, "Lane direction may not be null.");
110 this.gtuId = gtuId;
111 this.metaData = new MetaData(metaData);
112 for (ExtendedDataType<?, ?, ?, G> dataType : extendedData)
113 {
114 this.extendedData.put(dataType, dataType.initializeStorage());
115 }
116 this.kpiLaneDirection = kpiLaneDirection;
117 }
118
119
120
121
122
123
124
125
126
127 public void add(final Length position, final Speed speed, final Acceleration acceleration, final Time time)
128 {
129 add(position, speed, acceleration, time, null);
130 }
131
132
133
134
135
136
137
138
139
140
141 public void add(final Length position, final Speed speed, final Acceleration acceleration, final Time time, final G gtu)
142 {
143 Throw.whenNull(position, "Position may not be null.");
144 Throw.whenNull(speed, "Speed may not be null.");
145 Throw.whenNull(acceleration, "Acceleration may not be null.");
146 Throw.whenNull(time, "Time may not be null.");
147 Throw.whenNull(gtu, "GTU may not be null.");
148 if (this.size == this.x.length)
149 {
150 int cap = this.size + (this.size >> 1);
151 this.x = Arrays.copyOf(this.x, cap);
152 this.v = Arrays.copyOf(this.v, cap);
153 this.a = Arrays.copyOf(this.a, cap);
154 this.t = Arrays.copyOf(this.t, cap);
155 }
156 this.x[this.size] = (float) this.kpiLaneDirection.getPositionInDirection(position).si;
157 this.v[this.size] = (float) speed.si;
158 this.a[this.size] = (float) acceleration.si;
159 this.t[this.size] = (float) time.si;
160 for (ExtendedDataType<?, ?, ?, G> extendedDataType : this.extendedData.keySet())
161 {
162 appendValue(extendedDataType, gtu);
163 }
164 this.size++;
165 }
166
167
168
169
170
171 @SuppressWarnings("unchecked")
172 private <T, S> void appendValue(final ExtendedDataType<T, ?, S, G> extendedDataType, final G gtu)
173 {
174 S in = (S) this.extendedData.get(extendedDataType);
175 S out = extendedDataType.setValue(in, this.size, extendedDataType.getValue(gtu));
176 if (in != out)
177 {
178 this.extendedData.put(extendedDataType, out);
179 }
180 }
181
182
183
184
185 public int size()
186 {
187 return this.size;
188 }
189
190
191
192
193 public String getGtuId()
194 {
195 return this.gtuId;
196 }
197
198
199
200
201
202 public float[] getX()
203 {
204 return Arrays.copyOf(this.x, this.size);
205 }
206
207
208
209
210 public float[] getV()
211 {
212 return Arrays.copyOf(this.v, this.size);
213 }
214
215
216
217
218 public float[] getA()
219 {
220 return Arrays.copyOf(this.a, this.size);
221 }
222
223
224
225
226 public float[] getT()
227 {
228 return Arrays.copyOf(this.t, this.size);
229 }
230
231
232
233
234
235
236
237 public float getX(final int index) throws SamplingException
238 {
239 checkSample(index);
240 return this.x[index];
241 }
242
243
244
245
246
247
248
249 public float getV(final int index) throws SamplingException
250 {
251 checkSample(index);
252 return this.v[index];
253 }
254
255
256
257
258
259
260
261 public float getA(final int index) throws SamplingException
262 {
263 checkSample(index);
264 return this.a[index];
265 }
266
267
268
269
270
271
272
273 public float getT(final int index) throws SamplingException
274 {
275 checkSample(index);
276 return this.t[index];
277 }
278
279
280
281
282
283
284
285
286
287
288 @SuppressWarnings("unchecked")
289 public <T, S> T getExtendedData(final ExtendedDataType<T, ?, S, ?> extendedDataType, final int index)
290 throws SamplingException
291 {
292 checkSample(index);
293 return extendedDataType.getStorageValue((S) this.extendedData.get(extendedDataType), index);
294 }
295
296
297
298
299
300
301 private void checkSample(final int index) throws SamplingException
302 {
303 Throw.when(index < 0 || index >= this.size, SamplingException.class, "Index is out of bounds.");
304 }
305
306
307
308
309
310 public FloatLengthVector getPosition()
311 {
312 try
313 {
314 return new FloatLengthVector(getX(), LengthUnit.SI, StorageType.DENSE);
315 }
316 catch (ValueException exception)
317 {
318
319 throw new RuntimeException("Could not return trajectory data.", exception);
320 }
321 }
322
323
324
325
326 public FloatSpeedVector getSpeed()
327 {
328 try
329 {
330 return new FloatSpeedVector(getV(), SpeedUnit.SI, StorageType.DENSE);
331 }
332 catch (ValueException exception)
333 {
334
335 throw new RuntimeException("Could not return trajectory data.", exception);
336 }
337 }
338
339
340
341
342 public FloatAccelerationVector getAcceleration()
343 {
344 try
345 {
346 return new FloatAccelerationVector(getA(), AccelerationUnit.SI, StorageType.DENSE);
347 }
348 catch (ValueException exception)
349 {
350
351 throw new RuntimeException("Could not return trajectory data.", exception);
352 }
353 }
354
355
356
357
358 public FloatTimeVector getTime()
359 {
360 try
361 {
362 return new FloatTimeVector(getT(), TimeUnit.BASE_SECOND, StorageType.DENSE);
363 }
364 catch (ValueException exception)
365 {
366
367 throw new RuntimeException("Could not return trajectory data.", exception);
368 }
369 }
370
371
372
373
374
375 public Length getTotalLength()
376 {
377
378
379 if (this.size == 0)
380 {
381 return Length.ZERO;
382 }
383 return new Length(this.x[this.size - 1] - this.x[0], LengthUnit.SI);
384 }
385
386
387
388
389
390 public Duration getTotalDuration()
391 {
392
393
394 if (this.size == 0)
395 {
396 return Duration.ZERO;
397 }
398 return new Duration(this.t[this.size - 1] - this.t[0], DurationUnit.SI);
399 }
400
401
402
403
404
405 public boolean contains(final MetaDataType<?> metaDataType)
406 {
407 return this.metaData.contains(metaDataType);
408 }
409
410
411
412
413
414
415 public <T> T getMetaData(final MetaDataType<T> metaDataType)
416 {
417 return this.metaData.get(metaDataType);
418 }
419
420
421
422
423
424 public Set<MetaDataType<?>> getMetaDataTypes()
425 {
426 return this.metaData.getMetaDataTypes();
427 }
428
429
430
431
432
433 public boolean contains(final ExtendedDataType<?, ?, ?, ?> extendedDataType)
434 {
435 return this.extendedData.containsKey(extendedDataType);
436 }
437
438
439
440
441
442
443
444
445 @SuppressWarnings("unchecked")
446 public <O, S> O getExtendedData(final ExtendedDataType<?, O, S, ?> extendedDataType) throws SamplingException
447 {
448 Throw.when(!this.extendedData.containsKey(extendedDataType), SamplingException.class,
449 "Extended data type %s is not in the trajectory.", extendedDataType);
450 return extendedDataType.convert((S) this.extendedData.get(extendedDataType), this.size);
451 }
452
453
454
455
456
457 public Set<ExtendedDataType<?, ?, ?, G>> getExtendedDataTypes()
458 {
459 return this.extendedData.keySet();
460 }
461
462
463
464
465
466
467
468
469
470
471 public Trajectory<G> subSet(final Length startPosition, final Length endPosition)
472 {
473 Throw.whenNull(startPosition, "Start position may not be null");
474 Throw.whenNull(endPosition, "End position may not be null");
475 Length length0 = this.kpiLaneDirection.getPositionInDirection(startPosition);
476 Length length1 = this.kpiLaneDirection.getPositionInDirection(endPosition);
477 Throw.when(length0.gt(length1), IllegalArgumentException.class,
478 "Start position should be smaller than end position in the direction of travel");
479 return subSet(spaceBoundaries(length0, length1));
480 }
481
482
483
484
485
486
487
488
489
490
491 public Trajectory<G> subSet(final Time startTime, final Time endTime)
492 {
493 Throw.whenNull(startTime, "Start time may not be null");
494 Throw.whenNull(endTime, "End time may not be null");
495 Throw.when(startTime.gt(endTime), IllegalArgumentException.class, "Start time should be smaller than end time.");
496 return subSet(timeBoundaries(startTime, endTime));
497 }
498
499
500
501
502
503
504
505
506
507
508
509
510 public Trajectory<G> subSet(final Length startPosition, final Length endPosition, final Time startTime, final Time endTime)
511 {
512
513 Throw.whenNull(startPosition, "Start position may not be null");
514 Throw.whenNull(endPosition, "End position may not be null");
515 Length length0 = this.kpiLaneDirection.getPositionInDirection(startPosition);
516 Length length1 = this.kpiLaneDirection.getPositionInDirection(endPosition);
517 Throw.when(length0.gt(length1), IllegalArgumentException.class,
518 "Start position should be smaller than end position in the direction of travel");
519 Throw.whenNull(startTime, "Start time may not be null");
520 Throw.whenNull(endTime, "End time may not be null");
521 Throw.when(startTime.gt(endTime), IllegalArgumentException.class, "Start time should be smaller than end time.");
522 return subSet(spaceBoundaries(length0, length1).intersect(timeBoundaries(startTime, endTime)));
523 }
524
525
526
527
528
529
530
531 private Boundaries spaceBoundaries(final Length startPosition, final Length endPosition)
532 {
533 if (startPosition.si > this.x[this.size - 1] || endPosition.si < this.x[0])
534 {
535 return new Boundaries(0, 0.0, 0, 0.0);
536 }
537 int from = 0;
538 double fFrom = 0;
539
540 float startPos = (float) startPosition.si;
541 float endPos = (float) endPosition.si;
542 while (startPos > this.x[from + 1] && from < this.size - 1)
543 {
544 from++;
545 }
546 if (this.x[from] < startPos)
547 {
548 fFrom = (startPos - this.x[from]) / (this.x[from + 1] - this.x[from]);
549 }
550 int to = this.size - 1;
551 double fTo = 0;
552 while (endPos < this.x[to] && to > 0)
553 {
554 to--;
555 }
556 if (to < this.size - 1)
557 {
558 fTo = (endPos - this.x[to]) / (this.x[to + 1] - this.x[to]);
559 }
560 return new Boundaries(from, fFrom, to, fTo);
561 }
562
563
564
565
566
567
568
569 private Boundaries timeBoundaries(final Time startTime, final Time endTime)
570 {
571 if (startTime.si > this.t[this.size - 1] || endTime.si < this.t[0])
572 {
573 return new Boundaries(0, 0.0, 0, 0.0);
574 }
575 int from = 0;
576 double fFrom = 0;
577
578 float startTim = (float) startTime.si;
579 float endTim = (float) endTime.si;
580 while (startTim > this.t[from + 1] && from < this.size - 1)
581 {
582 from++;
583 }
584 if (this.t[from] < startTim)
585 {
586 fFrom = (startTim - this.t[from]) / (this.t[from + 1] - this.t[from]);
587 }
588 int to = this.size - 1;
589 double fTo = 0;
590 while (endTim < this.t[to] && to > 0)
591 {
592 to--;
593 }
594 if (to < this.size - 1)
595 {
596 fTo = (endTim - this.t[to]) / (this.t[to + 1] - this.t[to]);
597 }
598 return new Boundaries(from, fFrom, to, fTo);
599 }
600
601
602
603
604
605
606
607
608 @SuppressWarnings("unchecked")
609 private <T, S> Trajectory<G> subSet(final Boundaries bounds)
610 {
611 Trajectory<G> out = new Trajectory<>(this.gtuId, this.metaData, this.extendedData.keySet(), this.kpiLaneDirection);
612 if (bounds.from < bounds.to)
613 {
614 int nBefore = bounds.fFrom < 1.0 ? 1 : 0;
615 int nAfter = bounds.fTo > 0.0 ? 1 : 0;
616 int n = bounds.to - bounds.from + nBefore + nAfter;
617 out.x = new float[n];
618 out.v = new float[n];
619 out.a = new float[n];
620 out.t = new float[n];
621 System.arraycopy(this.x, bounds.from + 1, out.x, nBefore, bounds.to - bounds.from);
622 System.arraycopy(this.v, bounds.from + 1, out.v, nBefore, bounds.to - bounds.from);
623 System.arraycopy(this.a, bounds.from + 1, out.a, nBefore, bounds.to - bounds.from);
624 System.arraycopy(this.t, bounds.from + 1, out.t, nBefore, bounds.to - bounds.from);
625 if (nBefore == 1)
626 {
627 out.x[0] = (float) (this.x[bounds.from] * (1 - bounds.fFrom) + this.x[bounds.from + 1] * bounds.fFrom);
628 out.v[0] = (float) (this.v[bounds.from] * (1 - bounds.fFrom) + this.v[bounds.from + 1] * bounds.fFrom);
629 out.a[0] = (float) (this.a[bounds.from] * (1 - bounds.fFrom) + this.a[bounds.from + 1] * bounds.fFrom);
630 out.t[0] = (float) (this.t[bounds.from] * (1 - bounds.fFrom) + this.t[bounds.from + 1] * bounds.fFrom);
631 }
632 if (nAfter == 1)
633 {
634 out.x[n - 1] = (float) (this.x[bounds.to] * (1 - bounds.fTo) + this.x[bounds.to + 1] * bounds.fTo);
635 out.v[n - 1] = (float) (this.v[bounds.to] * (1 - bounds.fTo) + this.v[bounds.to + 1] * bounds.fTo);
636 out.a[n - 1] = (float) (this.a[bounds.to] * (1 - bounds.fTo) + this.a[bounds.to + 1] * bounds.fTo);
637 out.t[n - 1] = (float) (this.t[bounds.to] * (1 - bounds.fTo) + this.t[bounds.to + 1] * bounds.fTo);
638 }
639 out.size = n;
640 for (ExtendedDataType<?, ?, ?, G> extendedDataType : this.extendedData.keySet())
641 {
642 int j = 0;
643 ExtendedDataType<T, ?, S, G> edt = (ExtendedDataType<T, ?, S, G>) extendedDataType;
644 S fromList = (S) this.extendedData.get(extendedDataType);
645 S toList = edt.initializeStorage();
646 try
647 {
648 if (nBefore == 1)
649 {
650 edt.setValue(toList, j,
651 ((ExtendedDataType<T, ?, ?, G>) extendedDataType).interpolate(
652 edt.getStorageValue(fromList, bounds.from),
653 edt.getStorageValue(fromList, bounds.from + 1), bounds.fFrom));
654 j++;
655 }
656 for (int i = bounds.from + 1; i < bounds.to; i++)
657 {
658 edt.setValue(toList, j, edt.getStorageValue(fromList, i));
659 j++;
660 }
661 if (nAfter == 1)
662 {
663 edt.setValue(toList, j,
664 ((ExtendedDataType<T, ?, ?, G>) extendedDataType).interpolate(
665 edt.getStorageValue(fromList, bounds.to), edt.getStorageValue(fromList, bounds.to + 1),
666 bounds.fTo));
667 }
668 }
669 catch (SamplingException se)
670 {
671
672 throw new RuntimeException("Error while obtaining subset of trajectory.", se);
673 }
674 out.extendedData.put(extendedDataType, toList);
675 }
676 }
677 return out;
678 }
679
680
681 @Override
682 public int hashCode()
683 {
684 final int prime = 31;
685 int result = 1;
686 result = prime * result + ((this.gtuId == null) ? 0 : this.gtuId.hashCode());
687 result = prime * result + this.size;
688 if (this.size > 0)
689 {
690 result = prime * result + Float.floatToIntBits(this.t[0]);
691 }
692 return result;
693 }
694
695
696 @Override
697 public boolean equals(final Object obj)
698 {
699 if (this == obj)
700 {
701 return true;
702 }
703 if (obj == null)
704 {
705 return false;
706 }
707 if (getClass() != obj.getClass())
708 {
709 return false;
710 }
711 Trajectory<?> other = (Trajectory<?>) obj;
712 if (this.size != other.size)
713 {
714 return false;
715 }
716 if (this.gtuId == null)
717 {
718 if (other.gtuId != null)
719 {
720 return false;
721 }
722 }
723 else if (!this.gtuId.equals(other.gtuId))
724 {
725 return false;
726 }
727 if (this.size > 0)
728 {
729 if (this.t[0] != other.t[0])
730 {
731 return false;
732 }
733 }
734 return true;
735 }
736
737
738 @Override
739 public String toString()
740 {
741 if (this.size > 0)
742 {
743 return "Trajectory [size=" + this.size + ", x={" + this.x[0] + "..." + this.x[this.size - 1] + "}, t={" + this.t[0]
744 + "..." + this.t[this.size - 1] + "}, metaData=" + this.metaData + ", gtuId=" + this.gtuId + "]";
745 }
746 return "Trajectory [size=" + this.size + ", x={}, t={}, metaData=" + this.metaData + ", gtuId=" + this.gtuId + "]";
747 }
748
749
750
751
752
753
754
755
756
757
758
759
760 private class Boundaries
761 {
762
763 @SuppressWarnings("checkstyle:visibilitymodifier")
764 public final int from;
765
766
767 @SuppressWarnings("checkstyle:visibilitymodifier")
768 public final double fFrom;
769
770
771 @SuppressWarnings("checkstyle:visibilitymodifier")
772 public final int to;
773
774
775 @SuppressWarnings("checkstyle:visibilitymodifier")
776 public final double fTo;
777
778
779
780
781
782
783
784 Boundaries(final int from, final double fFrom, final int to, final double fTo)
785 {
786 Throw.when(from < 0 || from > Trajectory.this.size() - 1, IllegalArgumentException.class,
787 "Argument from (%d) is out of bounds.", from);
788 Throw.when(fFrom < 0 || fFrom > 1, IllegalArgumentException.class, "Argument fFrom (%f) is out of bounds.", fFrom);
789 Throw.when(from == Trajectory.this.size() && fFrom > 0, IllegalArgumentException.class,
790 "Arguments from (%d) and fFrom (%f) are out of bounds.", from, fFrom);
791 Throw.when(to < 0 || to >= Trajectory.this.size(), IllegalArgumentException.class,
792 "Argument to (%d) is out of bounds.", to);
793 Throw.when(fTo < 0 || fTo > 1, IllegalArgumentException.class, "Argument fTo (%f) is out of bounds.", fTo);
794 Throw.when(to == Trajectory.this.size() && fTo > 0, IllegalArgumentException.class,
795 "Arguments to (%d) and fTo (%f) are out of bounds.", to, fTo);
796 this.from = from;
797 this.fFrom = fFrom;
798 this.to = to;
799 this.fTo = fTo;
800 }
801
802
803
804
805
806 public Boundaries intersect(final Boundaries boundaries)
807 {
808 int newFrom;
809 double newFFrom;
810 if (this.from > boundaries.from || this.from == boundaries.from && this.fFrom > boundaries.fFrom)
811 {
812 newFrom = this.from;
813 newFFrom = this.fFrom;
814 }
815 else
816 {
817 newFrom = boundaries.from;
818 newFFrom = boundaries.fFrom;
819 }
820 int newTo;
821 double newFTo;
822 if (this.to < boundaries.to || this.to == boundaries.to && this.fTo < boundaries.fTo)
823 {
824 newTo = this.to;
825 newFTo = this.fTo;
826 }
827 else
828 {
829 newTo = boundaries.to;
830 newFTo = boundaries.fTo;
831 }
832 return new Boundaries(newFrom, newFFrom, newTo, newFTo);
833 }
834
835
836 @Override
837 public final String toString()
838 {
839 return "Boundaries [from=" + this.from + ", fFrom=" + this.fFrom + ", to=" + this.to + ", fTo=" + this.fTo + "]";
840 }
841
842 }
843
844 }