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.ValueRuntimeException;
14 import org.djunits.value.storage.StorageType;
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.djunits.value.vfloat.vector.base.FloatVector;
25 import org.djutils.exceptions.Throw;
26 import org.opentrafficsim.kpi.interfaces.GtuDataInterface;
27 import org.opentrafficsim.kpi.sampling.data.ExtendedDataType;
28 import org.opentrafficsim.kpi.sampling.meta.MetaData;
29 import org.opentrafficsim.kpi.sampling.meta.FilterDataType;
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 if (!this.extendedData.isEmpty())
148 {
149 Throw.whenNull(gtu, "GTU may not be null.");
150 }
151 if (this.size == this.x.length)
152 {
153 int cap = this.size + (this.size >> 1);
154 this.x = Arrays.copyOf(this.x, cap);
155 this.v = Arrays.copyOf(this.v, cap);
156 this.a = Arrays.copyOf(this.a, cap);
157 this.t = Arrays.copyOf(this.t, cap);
158 }
159 this.x[this.size] = (float) this.kpiLaneDirection.getPositionInDirection(position).si;
160 this.v[this.size] = (float) speed.si;
161 this.a[this.size] = (float) acceleration.si;
162 this.t[this.size] = (float) time.si;
163 for (ExtendedDataType<?, ?, ?, G> extendedDataType : this.extendedData.keySet())
164 {
165 appendValue(extendedDataType, gtu);
166 }
167 this.size++;
168 }
169
170
171
172
173
174 @SuppressWarnings("unchecked")
175 private <T, S> void appendValue(final ExtendedDataType<T, ?, S, G> extendedDataType, final G gtu)
176 {
177 S in = (S) this.extendedData.get(extendedDataType);
178 S out = extendedDataType.setValue(in, this.size, extendedDataType.getValue(gtu));
179 if (in != out)
180 {
181 this.extendedData.put(extendedDataType, out);
182 }
183 }
184
185
186
187
188 public int size()
189 {
190 return this.size;
191 }
192
193
194
195
196 public String getGtuId()
197 {
198 return this.gtuId;
199 }
200
201
202
203
204
205 public float[] getX()
206 {
207 return Arrays.copyOf(this.x, this.size);
208 }
209
210
211
212
213 public float[] getV()
214 {
215 return Arrays.copyOf(this.v, this.size);
216 }
217
218
219
220
221 public float[] getA()
222 {
223 return Arrays.copyOf(this.a, this.size);
224 }
225
226
227
228
229 public float[] getT()
230 {
231 return Arrays.copyOf(this.t, this.size);
232 }
233
234
235
236
237
238
239 public int binarySearchX(final float position)
240 {
241 if (this.x[0] >= position)
242 {
243 return 0;
244 }
245 int index = Arrays.binarySearch(this.x, 0, this.size, position);
246 return index < 0 ? -index - 2 : index;
247 }
248
249
250
251
252
253
254 public int binarySearchT(final float time)
255 {
256 if (this.t[0] >= time)
257 {
258 return 0;
259 }
260 int index = Arrays.binarySearch(this.t, 0, this.size, time);
261 return index < 0 ? -index - 2 : index;
262 }
263
264
265
266
267
268
269
270 public float getX(final int index) throws SamplingException
271 {
272 checkSample(index);
273 return this.x[index];
274 }
275
276
277
278
279
280
281
282 public float getV(final int index) throws SamplingException
283 {
284 checkSample(index);
285 return this.v[index];
286 }
287
288
289
290
291
292
293
294 public float getA(final int index) throws SamplingException
295 {
296 checkSample(index);
297 return this.a[index];
298 }
299
300
301
302
303
304
305
306 public float getT(final int index) throws SamplingException
307 {
308 checkSample(index);
309 return this.t[index];
310 }
311
312
313
314
315
316
317
318
319
320
321 @SuppressWarnings("unchecked")
322 public <T, S> T getExtendedData(final ExtendedDataType<T, ?, S, ?> extendedDataType, final int index)
323 throws SamplingException
324 {
325 checkSample(index);
326 return extendedDataType.getStorageValue((S) this.extendedData.get(extendedDataType), index);
327 }
328
329
330
331
332
333
334 private void checkSample(final int index) throws SamplingException
335 {
336 Throw.when(index < 0 || index >= this.size, SamplingException.class, "Index is out of bounds.");
337 }
338
339
340
341
342
343 public FloatLengthVector getPosition()
344 {
345 try
346 {
347 return FloatVector.instantiate(getX(), LengthUnit.SI, StorageType.DENSE);
348 }
349 catch (ValueRuntimeException exception)
350 {
351
352 throw new RuntimeException("Could not return trajectory data.", exception);
353 }
354 }
355
356
357
358
359 public FloatSpeedVector getSpeed()
360 {
361 try
362 {
363 return FloatVector.instantiate(getV(), SpeedUnit.SI, StorageType.DENSE);
364 }
365 catch (ValueRuntimeException exception)
366 {
367
368 throw new RuntimeException("Could not return trajectory data.", exception);
369 }
370 }
371
372
373
374
375 public FloatAccelerationVector getAcceleration()
376 {
377 try
378 {
379 return FloatVector.instantiate(getA(), AccelerationUnit.SI, StorageType.DENSE);
380 }
381 catch (ValueRuntimeException exception)
382 {
383
384 throw new RuntimeException("Could not return trajectory data.", exception);
385 }
386 }
387
388
389
390
391 public FloatTimeVector getTime()
392 {
393 try
394 {
395 return FloatVector.instantiate(getT(), TimeUnit.BASE_SECOND, StorageType.DENSE);
396 }
397 catch (ValueRuntimeException exception)
398 {
399
400 throw new RuntimeException("Could not return trajectory data.", exception);
401 }
402 }
403
404
405
406
407
408 public Length getTotalLength()
409 {
410
411
412 if (this.size == 0)
413 {
414 return Length.ZERO;
415 }
416 return new Length(this.x[this.size - 1] - this.x[0], LengthUnit.SI);
417 }
418
419
420
421
422
423 public Duration getTotalDuration()
424 {
425
426
427 if (this.size == 0)
428 {
429 return Duration.ZERO;
430 }
431 return new Duration(this.t[this.size - 1] - this.t[0], DurationUnit.SI);
432 }
433
434
435
436
437
438 public boolean contains(final FilterDataType<?> metaDataType)
439 {
440 return this.metaData.contains(metaDataType);
441 }
442
443
444
445
446
447
448 public <T> T getMetaData(final FilterDataType<T> metaDataType)
449 {
450 return this.metaData.get(metaDataType);
451 }
452
453
454
455
456
457 public Set<FilterDataType<?>> getFilterDataTypes()
458 {
459 return this.metaData.getMetaDataTypes();
460 }
461
462
463
464
465
466 public boolean contains(final ExtendedDataType<?, ?, ?, ?> extendedDataType)
467 {
468 return this.extendedData.containsKey(extendedDataType);
469 }
470
471
472
473
474
475
476
477
478 @SuppressWarnings("unchecked")
479 public <O, S> O getExtendedData(final ExtendedDataType<?, O, S, ?> extendedDataType) throws SamplingException
480 {
481 Throw.when(!this.extendedData.containsKey(extendedDataType), SamplingException.class,
482 "Extended data type %s is not in the trajectory.", extendedDataType);
483 return extendedDataType.convert((S) this.extendedData.get(extendedDataType), this.size);
484 }
485
486
487
488
489
490 public Set<ExtendedDataType<?, ?, ?, G>> getExtendedDataTypes()
491 {
492 return this.extendedData.keySet();
493 }
494
495
496
497
498
499
500
501
502
503
504 @SuppressWarnings("synthetic-access")
505 public SpaceTimeView getSpaceTimeView(final Length startPosition, final Length endPosition, final Time startTime,
506 final Time endTime)
507 {
508 if (size() == 0)
509 {
510 return new SpaceTimeView(Length.ZERO, Duration.ZERO);
511 }
512 Length length0 = this.kpiLaneDirection.getPositionInDirection(startPosition);
513 Length length1 = this.kpiLaneDirection.getPositionInDirection(endPosition);
514 Boundaries bounds = spaceBoundaries(length0, length1).intersect(timeBoundaries(startTime, endTime));
515 double xFrom;
516 double tFrom;
517 if (bounds.fFrom > 0.0)
518 {
519 xFrom = this.x[bounds.from] * (1 - bounds.fFrom) + this.x[bounds.from + 1] * bounds.fFrom;
520 tFrom = this.t[bounds.from] * (1 - bounds.fFrom) + this.t[bounds.from + 1] * bounds.fFrom;
521 }
522 else
523 {
524 xFrom = this.x[bounds.from];
525 tFrom = this.t[bounds.from];
526 }
527 double xTo;
528 double tTo;
529 if (bounds.fTo > 0.0)
530 {
531 xTo = this.x[bounds.to] * (1 - bounds.fTo) + this.x[bounds.to + 1] * bounds.fTo;
532 tTo = this.t[bounds.to] * (1 - bounds.fTo) + this.t[bounds.to + 1] * bounds.fTo;
533 }
534 else
535 {
536 xTo = this.x[bounds.to];
537 tTo = this.t[bounds.to];
538 }
539 return new SpaceTimeView(Length.instantiateSI(xTo - xFrom), Duration.instantiateSI(tTo - tFrom));
540 }
541
542
543
544
545
546
547
548
549
550
551 public Trajectory<G> subSet(final Length startPosition, final Length endPosition)
552 {
553 Throw.whenNull(startPosition, "Start position may not be null");
554 Throw.whenNull(endPosition, "End position may not be null");
555 Length length0 = this.kpiLaneDirection.getPositionInDirection(startPosition);
556 Length length1 = this.kpiLaneDirection.getPositionInDirection(endPosition);
557 Throw.when(length0.gt(length1), IllegalArgumentException.class,
558 "Start position should be smaller than end position in the direction of travel");
559 if (this.size == 0)
560 {
561 return new Trajectory<>(this.gtuId, this.metaData, this.extendedData.keySet(), this.kpiLaneDirection);
562 }
563 return subSet(spaceBoundaries(length0, length1));
564 }
565
566
567
568
569
570
571
572
573
574 public Trajectory<G> subSet(final Time startTime, final Time endTime)
575 {
576 Throw.whenNull(startTime, "Start time may not be null");
577 Throw.whenNull(endTime, "End time may not be null");
578 Throw.when(startTime.gt(endTime), IllegalArgumentException.class, "Start time should be smaller than end time.");
579 if (this.size == 0)
580 {
581 return new Trajectory<>(this.gtuId, this.metaData, this.extendedData.keySet(), this.kpiLaneDirection);
582 }
583 return subSet(timeBoundaries(startTime, endTime));
584 }
585
586
587
588
589
590
591
592
593
594
595
596 public Trajectory<G> subSet(final Length startPosition, final Length endPosition, final Time startTime, final Time endTime)
597 {
598
599 Throw.whenNull(startPosition, "Start position may not be null");
600 Throw.whenNull(endPosition, "End position may not be null");
601 Length length0 = this.kpiLaneDirection.getPositionInDirection(startPosition);
602 Length length1 = this.kpiLaneDirection.getPositionInDirection(endPosition);
603 Throw.when(length0.gt(length1), IllegalArgumentException.class,
604 "Start position should be smaller than end position in the direction of travel");
605 Throw.whenNull(startTime, "Start time may not be null");
606 Throw.whenNull(endTime, "End time may not be null");
607 Throw.when(startTime.gt(endTime), IllegalArgumentException.class, "Start time should be smaller than end time.");
608 if (this.size == 0)
609 {
610 return new Trajectory<>(this.gtuId, this.metaData, this.extendedData.keySet(), this.kpiLaneDirection);
611 }
612 return subSet(spaceBoundaries(length0, length1).intersect(timeBoundaries(startTime, endTime)));
613 }
614
615
616
617
618
619
620
621 private Boundaries spaceBoundaries(final Length startPosition, final Length endPosition)
622 {
623 if (startPosition.si > this.x[this.size - 1] || endPosition.si < this.x[0])
624 {
625 return new Boundaries(0, 0.0, 0, 0.0);
626 }
627
628 float startPos = (float) startPosition.si;
629 float endPos = (float) endPosition.si;
630 Boundary from = getBoundaryAtPosition(startPos, false);
631 Boundary to = getBoundaryAtPosition(endPos, true);
632 return new Boundaries(from.index, from.fraction, to.index, to.fraction);
633
634
635
636
637
638
639
640
641
642
643
644
645
646 }
647
648
649
650
651
652
653
654 private Boundaries timeBoundaries(final Time startTime, final Time endTime)
655 {
656 if (startTime.si > this.t[this.size - 1] || endTime.si < this.t[0])
657 {
658 return new Boundaries(0, 0.0, 0, 0.0);
659 }
660
661 float startTim = (float) startTime.si;
662 float endTim = (float) endTime.si;
663 Boundary from = getBoundaryAtTime(startTim, false);
664 Boundary to = getBoundaryAtTime(endTim, true);
665 return new Boundaries(from.index, from.fraction, to.index, to.fraction);
666
667
668
669
670
671
672
673
674
675
676
677
678
679 }
680
681
682
683
684
685
686
687 private Boundary getBoundaryAtPosition(final float position, final boolean end)
688 {
689 int index = binarySearchX(position);
690 double fraction = 0;
691 if (end ? index < this.size - 1 : this.x[index] < position)
692 {
693 fraction = (position - this.x[index]) / (this.x[index + 1] - this.x[index]);
694 }
695 return new Boundary(index, fraction);
696 }
697
698
699
700
701
702
703
704 private Boundary getBoundaryAtTime(final float time, final boolean end)
705 {
706 int index = binarySearchT(time);
707 double fraction = 0;
708 if (end ? index < this.size - 1 : this.t[index] < time)
709 {
710 fraction = (time - this.t[index]) / (this.t[index + 1] - this.t[index]);
711 }
712 return new Boundary(index, fraction);
713 }
714
715
716
717
718
719
720 public Time getTimeAtPosition(final Length position)
721 {
722 return Time.instantiateSI(getBoundaryAtPosition((float) position.si, false).getValue(this.t));
723 }
724
725
726
727
728
729
730 public Speed getSpeedAtPosition(final Length position)
731 {
732 return Speed.instantiateSI(getBoundaryAtPosition((float) position.si, false).getValue(this.v));
733 }
734
735
736
737
738
739
740 public Acceleration getAccelerationAtPosition(final Length position)
741 {
742 return Acceleration.instantiateSI(getBoundaryAtPosition((float) position.si, false).getValue(this.a));
743 }
744
745
746
747
748
749
750 public Length getPositionAtTime(final Time time)
751 {
752 return Length.instantiateSI(getBoundaryAtTime((float) time.si, false).getValue(this.x));
753 }
754
755
756
757
758
759
760 public Speed getSpeedAtTime(final Time time)
761 {
762 return Speed.instantiateSI(getBoundaryAtTime((float) time.si, false).getValue(this.v));
763 }
764
765
766
767
768
769
770 public Acceleration getAccelerationAtTime(final Time time)
771 {
772 return Acceleration.instantiateSI(getBoundaryAtTime((float) time.si, false).getValue(this.a));
773 }
774
775
776
777
778
779
780
781
782 @SuppressWarnings("unchecked")
783 private <T, S> Trajectory<G> subSet(final Boundaries bounds)
784 {
785 Trajectory<G> out = new Trajectory<>(this.gtuId, this.metaData, this.extendedData.keySet(), this.kpiLaneDirection);
786 if (bounds.from < bounds.to)
787 {
788 int nBefore = bounds.fFrom < 1.0 ? 1 : 0;
789 int nAfter = bounds.fTo > 0.0 ? 1 : 0;
790 int n = bounds.to - bounds.from + nBefore + nAfter;
791 out.x = new float[n];
792 out.v = new float[n];
793 out.a = new float[n];
794 out.t = new float[n];
795 System.arraycopy(this.x, bounds.from + 1, out.x, nBefore, bounds.to - bounds.from);
796 System.arraycopy(this.v, bounds.from + 1, out.v, nBefore, bounds.to - bounds.from);
797 System.arraycopy(this.a, bounds.from + 1, out.a, nBefore, bounds.to - bounds.from);
798 System.arraycopy(this.t, bounds.from + 1, out.t, nBefore, bounds.to - bounds.from);
799 if (nBefore == 1)
800 {
801 out.x[0] = (float) (this.x[bounds.from] * (1 - bounds.fFrom) + this.x[bounds.from + 1] * bounds.fFrom);
802 out.v[0] = (float) (this.v[bounds.from] * (1 - bounds.fFrom) + this.v[bounds.from + 1] * bounds.fFrom);
803 out.a[0] = (float) (this.a[bounds.from] * (1 - bounds.fFrom) + this.a[bounds.from + 1] * bounds.fFrom);
804 out.t[0] = (float) (this.t[bounds.from] * (1 - bounds.fFrom) + this.t[bounds.from + 1] * bounds.fFrom);
805 }
806 if (nAfter == 1)
807 {
808 out.x[n - 1] = (float) (this.x[bounds.to] * (1 - bounds.fTo) + this.x[bounds.to + 1] * bounds.fTo);
809 out.v[n - 1] = (float) (this.v[bounds.to] * (1 - bounds.fTo) + this.v[bounds.to + 1] * bounds.fTo);
810 out.a[n - 1] = (float) (this.a[bounds.to] * (1 - bounds.fTo) + this.a[bounds.to + 1] * bounds.fTo);
811 out.t[n - 1] = (float) (this.t[bounds.to] * (1 - bounds.fTo) + this.t[bounds.to + 1] * bounds.fTo);
812 }
813 out.size = n;
814 for (ExtendedDataType<?, ?, ?, G> extendedDataType : this.extendedData.keySet())
815 {
816 int j = 0;
817 ExtendedDataType<T, ?, S, G> edt = (ExtendedDataType<T, ?, S, G>) extendedDataType;
818 S fromList = (S) this.extendedData.get(extendedDataType);
819 S toList = edt.initializeStorage();
820 try
821 {
822 if (nBefore == 1)
823 {
824 toList = edt.setValue(toList, j, ((ExtendedDataType<T, ?, ?, G>) extendedDataType).interpolate(edt
825 .getStorageValue(fromList, bounds.from), edt.getStorageValue(fromList, bounds.from + 1),
826 bounds.fFrom));
827 j++;
828 }
829 for (int i = bounds.from + 1; i <= bounds.to; i++)
830 {
831 toList = edt.setValue(toList, j, edt.getStorageValue(fromList, i));
832 j++;
833 }
834 if (nAfter == 1)
835 {
836 toList = edt.setValue(toList, j, ((ExtendedDataType<T, ?, ?, G>) extendedDataType).interpolate(edt
837 .getStorageValue(fromList, bounds.to), edt.getStorageValue(fromList, bounds.to + 1), bounds.fTo));
838 }
839 }
840 catch (SamplingException se)
841 {
842
843 throw new RuntimeException("Error while obtaining subset of trajectory.", se);
844 }
845 out.extendedData.put(extendedDataType, toList);
846 }
847 }
848 return out;
849 }
850
851
852 @Override
853 public int hashCode()
854 {
855 final int prime = 31;
856 int result = 1;
857 result = prime * result + ((this.gtuId == null) ? 0 : this.gtuId.hashCode());
858 result = prime * result + this.size;
859 if (this.size > 0)
860 {
861 result = prime * result + Float.floatToIntBits(this.t[0]);
862 }
863 return result;
864 }
865
866
867 @Override
868 public boolean equals(final Object obj)
869 {
870 if (this == obj)
871 {
872 return true;
873 }
874 if (obj == null)
875 {
876 return false;
877 }
878 if (getClass() != obj.getClass())
879 {
880 return false;
881 }
882 Trajectory<?> other = (Trajectory<?>) obj;
883 if (this.size != other.size)
884 {
885 return false;
886 }
887 if (this.gtuId == null)
888 {
889 if (other.gtuId != null)
890 {
891 return false;
892 }
893 }
894 else if (!this.gtuId.equals(other.gtuId))
895 {
896 return false;
897 }
898 if (this.size > 0)
899 {
900 if (this.t[0] != other.t[0])
901 {
902 return false;
903 }
904 }
905 return true;
906 }
907
908
909 @Override
910 public String toString()
911 {
912 if (this.size > 0)
913 {
914 return "Trajectory [size=" + this.size + ", x={" + this.x[0] + "..." + this.x[this.size - 1] + "}, t={" + this.t[0]
915 + "..." + this.t[this.size - 1] + "}, metaData=" + this.metaData + ", gtuId=" + this.gtuId + "]";
916 }
917 return "Trajectory [size=" + this.size + ", x={}, t={}, metaData=" + this.metaData + ", gtuId=" + this.gtuId + "]";
918 }
919
920
921
922
923
924
925
926
927
928
929
930
931 public class Boundary
932 {
933
934 @SuppressWarnings("checkstyle:visibilitymodifier")
935 public final int index;
936
937
938 @SuppressWarnings("checkstyle:visibilitymodifier")
939 public final double fraction;
940
941
942
943
944
945 Boundary(final int index, final double fraction)
946 {
947 this.index = index;
948 this.fraction = fraction;
949 }
950
951
952 @Override
953 public final String toString()
954 {
955 return "Boundary [index=" + this.index + ", fraction=" + this.fraction + "]";
956 }
957
958
959
960
961
962
963 public double getValue(final float[] array)
964 {
965 if (this.fraction == 0.0)
966 {
967 return array[this.index];
968 }
969 if (this.fraction == 1.0)
970 {
971 return array[this.index + 1];
972 }
973 return (1 - this.fraction) * array[this.index] + this.fraction * array[this.index + 1];
974 }
975 }
976
977
978
979
980
981
982
983
984
985
986
987
988 private class Boundaries
989 {
990
991 @SuppressWarnings("checkstyle:visibilitymodifier")
992 public final int from;
993
994
995 @SuppressWarnings("checkstyle:visibilitymodifier")
996 public final double fFrom;
997
998
999 @SuppressWarnings("checkstyle:visibilitymodifier")
1000 public final int to;
1001
1002
1003 @SuppressWarnings("checkstyle:visibilitymodifier")
1004 public final double fTo;
1005
1006
1007
1008
1009
1010
1011
1012 Boundaries(final int from, final double fFrom, final int to, final double fTo)
1013 {
1014 Throw.when(from < 0 || from > Trajectory.this.size() - 1, IllegalArgumentException.class,
1015 "Argument from (%d) is out of bounds.", from);
1016 Throw.when(fFrom < 0 || fFrom > 1, IllegalArgumentException.class, "Argument fFrom (%f) is out of bounds.", fFrom);
1017 Throw.when(from == Trajectory.this.size() && fFrom > 0, IllegalArgumentException.class,
1018 "Arguments from (%d) and fFrom (%f) are out of bounds.", from, fFrom);
1019 Throw.when(to < 0 || to >= Trajectory.this.size(), IllegalArgumentException.class,
1020 "Argument to (%d) is out of bounds.", to);
1021 Throw.when(fTo < 0 || fTo > 1, IllegalArgumentException.class, "Argument fTo (%f) is out of bounds.", fTo);
1022 Throw.when(to == Trajectory.this.size() && fTo > 0, IllegalArgumentException.class,
1023 "Arguments to (%d) and fTo (%f) are out of bounds.", to, fTo);
1024 this.from = from;
1025 this.fFrom = fFrom;
1026 this.to = to;
1027 this.fTo = fTo;
1028 }
1029
1030
1031
1032
1033
1034 public Boundaries intersect(final Boundaries boundaries)
1035 {
1036 if (this.to < boundaries.from || boundaries.to < this.from || this.to == boundaries.from
1037 && this.fTo < boundaries.fFrom || boundaries.to == this.from && boundaries.fTo < this.fFrom)
1038 {
1039 return new Boundaries(0, 0.0, 0, 0.0);
1040 }
1041 int newFrom;
1042 double newFFrom;
1043 if (this.from > boundaries.from || this.from == boundaries.from && this.fFrom > boundaries.fFrom)
1044 {
1045 newFrom = this.from;
1046 newFFrom = this.fFrom;
1047 }
1048 else
1049 {
1050 newFrom = boundaries.from;
1051 newFFrom = boundaries.fFrom;
1052 }
1053 int newTo;
1054 double newFTo;
1055 if (this.to < boundaries.to || this.to == boundaries.to && this.fTo < boundaries.fTo)
1056 {
1057 newTo = this.to;
1058 newFTo = this.fTo;
1059 }
1060 else
1061 {
1062 newTo = boundaries.to;
1063 newFTo = boundaries.fTo;
1064 }
1065 return new Boundaries(newFrom, newFFrom, newTo, newFTo);
1066 }
1067
1068
1069 @Override
1070 public final String toString()
1071 {
1072 return "Boundaries [from=" + this.from + ", fFrom=" + this.fFrom + ", to=" + this.to + ", fTo=" + this.fTo + "]";
1073 }
1074
1075 }
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089 public static class SpaceTimeView
1090 {
1091
1092
1093 final Length distance;
1094
1095
1096 final Duration time;
1097
1098
1099
1100
1101
1102
1103 private SpaceTimeView(final Length distance, final Duration time)
1104 {
1105 this.distance = distance;
1106 this.time = time;
1107 }
1108
1109
1110
1111
1112
1113 public final Length getDistance()
1114 {
1115 return this.distance;
1116 }
1117
1118
1119
1120
1121
1122 public final Duration getTime()
1123 {
1124 return this.time;
1125 }
1126
1127
1128 @Override
1129 public String toString()
1130 {
1131 return "SpaceTimeView [distance=" + this.distance + ", time=" + this.time + "]";
1132 }
1133 }
1134
1135 }