View Javadoc
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   * Contains position, speed, acceleration and time data of a GTU, over some section. Position is relative to the start of the
33   * lane in the direction of travel, also when trajectories have been truncated at a position x > 0. Note that this regards
34   * internal data and output. Input position always refers to the design line of the lane. This class internally flips input
35   * positions and boundaries.
36   * <p>
37   * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
38   * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
39   * </p>
40   * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
41   * initial version Sep 21, 2016 <br>
42   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
43   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
44   * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
45   * @param <G> gtu data type
46   */
47  // TODO some trajectories have the first time stamp twice, with possibly different values for acceleration (and TTC)
48  public final class Trajectory<G extends GtuDataInterface>
49  {
50  
51      /** Default array capacity. */
52      private static final int DEFAULT_CAPACITY = 10;
53  
54      /** Effective length of the underlying data (arrays may be longer). */
55      private int size = 0;
56  
57      /**
58       * Position array. Position is relative to the start of the lane in the direction of travel, also when trajectories have
59       * been truncated at a position x &gt; 0.
60       */
61      private float[] x = new float[DEFAULT_CAPACITY];
62  
63      /** Speed array. */
64      private float[] v = new float[DEFAULT_CAPACITY];
65  
66      /** Acceleration array. */
67      private float[] a = new float[DEFAULT_CAPACITY];
68  
69      /** Time array. */
70      private float[] t = new float[DEFAULT_CAPACITY];
71  
72      /** GTU id. */
73      private final String gtuId;
74  
75      /** Meta data. */
76      private final MetaData metaData;
77  
78      /** Map of array data types and their values. */
79      private final Map<ExtendedDataType<?, ?, ?, G>, Object> extendedData = new LinkedHashMap<>();
80  
81      /** Direction of travel. */
82      private final KpiLaneDirection kpiLaneDirection;
83  
84      /**
85       * @param gtu GTU of this trajectory, only the id is stored.
86       * @param metaData meta data
87       * @param extendedData types of extended data
88       * @param kpiLaneDirection direction of travel
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       * Private constructor for creating subsets.
98       * @param gtuId GTU id
99       * @param metaData meta data
100      * @param extendedData types of extended data
101      * @param kpiLaneDirection direction of travel
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      * Adds values of position, speed, acceleration and time.
121      * @param position position is relative to the start of the lane in the direction of the design line, i.e. irrespective of
122      *            the travel direction, also when trajectories have been truncated at a position x &gt; 0
123      * @param speed speed
124      * @param acceleration acceleration
125      * @param time time
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      * Adds values of position, speed, acceleration and time.
134      * @param position position is relative to the start of the lane in the direction of the design line, i.e. irrespective of
135      *            the travel direction, also when trajectories have been truncated at a position x &gt; 0
136      * @param speed speed
137      * @param acceleration acceleration
138      * @param time time
139      * @param gtu gtu to add extended data for
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      * @param extendedDataType extended data type
169      * @param gtu gtu
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      * @return size of the underlying trajectory data
184      */
185     public int size()
186     {
187         return this.size;
188     }
189 
190     /**
191      * @return GTU id
192      */
193     public String getGtuId()
194     {
195         return this.gtuId;
196     }
197 
198     /**
199      * @return si position values, position is relative to the start of the lane, also when trajectories have been truncated at
200      *         a position x &gt; 0
201      */
202     public float[] getX()
203     {
204         return Arrays.copyOf(this.x, this.size);
205     }
206 
207     /**
208      * @return si speed values
209      */
210     public float[] getV()
211     {
212         return Arrays.copyOf(this.v, this.size);
213     }
214 
215     /**
216      * @return si acceleration values
217      */
218     public float[] getA()
219     {
220         return Arrays.copyOf(this.a, this.size);
221     }
222 
223     /**
224      * @return si time values
225      */
226     public float[] getT()
227     {
228         return Arrays.copyOf(this.t, this.size);
229     }
230 
231     /**
232      * Returns {@code x} value of a single sample.
233      * @param index index
234      * @return {@code x} value of a single sample
235      * @throws SamplingException if the index is out of bounds
236      */
237     public float getX(final int index) throws SamplingException
238     {
239         checkSample(index);
240         return this.x[index];
241     }
242 
243     /**
244      * Returns {@code v} value of a single sample.
245      * @param index index
246      * @return {@code v} value of a single sample
247      * @throws SamplingException if the index is out of bounds
248      */
249     public float getV(final int index) throws SamplingException
250     {
251         checkSample(index);
252         return this.v[index];
253     }
254 
255     /**
256      * Returns {@code a} value of a single sample.
257      * @param index index
258      * @return {@code a} value of a single sample
259      * @throws SamplingException if the index is out of bounds
260      */
261     public float getA(final int index) throws SamplingException
262     {
263         checkSample(index);
264         return this.a[index];
265     }
266 
267     /**
268      * Returns {@code t} value of a single sample.
269      * @param index index
270      * @return {@code t} value of a single sample
271      * @throws SamplingException if the index is out of bounds
272      */
273     public float getT(final int index) throws SamplingException
274     {
275         checkSample(index);
276         return this.t[index];
277     }
278 
279     /**
280      * Returns extended data type value of a single sample.
281      * @param extendedDataType data type from which to retrieve the data
282      * @param index index for which to retrieve the data
283      * @param <T> scalar type of extended data type
284      * @param <S> storage type of extended data type
285      * @return extended data type value of a single sample
286      * @throws SamplingException if the index is out of bounds
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      * Throws an exception if the sample index is out of bounds.
298      * @param index sample index
299      * @throws SamplingException if the sample index is out of bounds
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      * @return strongly typed copy of position, position is relative to the start of the lane, also when trajectories have been
308      *         truncated at a position x &gt; 0
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             // should not happen, inputs are not null
319             throw new RuntimeException("Could not return trajectory data.", exception);
320         }
321     }
322 
323     /**
324      * @return strongly typed copy of speed
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             // should not happen, inputs are not null
335             throw new RuntimeException("Could not return trajectory data.", exception);
336         }
337     }
338 
339     /**
340      * @return strongly typed copy of acceleration
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             // should not happen, inputs are not null
351             throw new RuntimeException("Could not return trajectory data.", exception);
352         }
353     }
354 
355     /**
356      * @return strongly typed copy of time
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             // should not happen, inputs are not null
367             throw new RuntimeException("Could not return trajectory data.", exception);
368         }
369     }
370 
371     /**
372      * @return total length of this trajectory
373      * @throws IllegalStateException if trajectory is empty
374      */
375     public Length getTotalLength()
376     {
377         // TODO do not allow empty trajectory
378         // Throw.when(this.size == 0, IllegalStateException.class, "Empty trajectory does not have a length.");
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      * @return total duration of this trajectory
388      * @throws IllegalStateException if trajectory is empty
389      */
390     public Duration getTotalDuration()
391     {
392         // TODO do not allow empty trajectory
393         // Throw.when(this.size == 0, IllegalStateException.class, "Empty trajectory does not have a duration.");
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      * @param metaDataType meta data type
403      * @return whether the trajectory contains the meta data of give type
404      */
405     public boolean contains(final MetaDataType<?> metaDataType)
406     {
407         return this.metaData.contains(metaDataType);
408     }
409 
410     /**
411      * @param metaDataType meta data type
412      * @param <T> class of meta data
413      * @return value of meta data
414      */
415     public <T> T getMetaData(final MetaDataType<T> metaDataType)
416     {
417         return this.metaData.get(metaDataType);
418     }
419 
420     /**
421      * Returns the included meta data types.
422      * @return included meta data types
423      */
424     public Set<MetaDataType<?>> getMetaDataTypes()
425     {
426         return this.metaData.getMetaDataTypes();
427     }
428 
429     /**
430      * @param extendedDataType extended data type
431      * @return whether the trajectory contains the extended data of give type
432      */
433     public boolean contains(final ExtendedDataType<?, ?, ?, ?> extendedDataType)
434     {
435         return this.extendedData.containsKey(extendedDataType);
436     }
437 
438     /**
439      * @param extendedDataType extended data type to return
440      * @param <O> output type
441      * @param <S> storage type
442      * @return values of extended data type
443      * @throws SamplingException if the extended data type is not in the trajectory
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      * Returns the included extended data types.
455      * @return included extended data types
456      */
457     public Set<ExtendedDataType<?, ?, ?, G>> getExtendedDataTypes()
458     {
459         return this.extendedData.keySet();
460     }
461 
462     /**
463      * Copies the trajectory but with a subset of the data. Longitudinal entry is only true if the original trajectory has true,
464      * and the subset is from the start.
465      * @param startPosition start position
466      * @param endPosition end position
467      * @return subset of the trajectory
468      * @throws NullPointerException if an input is null
469      * @throws IllegalArgumentException of minLength is smaller than maxLength
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      * Copies the trajectory but with a subset of the data. Longitudinal entry is only true if the original trajectory has true,
484      * and the subset is from the start.
485      * @param startTime start time
486      * @param endTime end time
487      * @return subset of the trajectory
488      * @throws NullPointerException if an input is null
489      * @throws IllegalArgumentException of minTime is smaller than maxTime
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      * Copies the trajectory but with a subset of the data. Longitudinal entry is only true if the original trajectory has true,
501      * and the subset is from the start.
502      * @param startPosition start position
503      * @param endPosition end position
504      * @param startTime start time
505      * @param endTime end time
506      * @return subset of the trajectory
507      * @throws NullPointerException if an input is null
508      * @throws IllegalArgumentException of minLength/Time is smaller than maxLength/Time
509      */
510     public Trajectory<G> subSet(final Length startPosition, final Length endPosition, final Time startTime, final Time endTime)
511     {
512         // could use this.subSet(minLength, maxLength).subSet(minTime, maxTime), but that copies twice
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      * Determine spatial boundaries.
527      * @param startPosition start position
528      * @param endPosition end position
529      * @return spatial boundaries
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         // to float needed as x is in floats and due to precision fTo > 1 may become true
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      * Determine temporal boundaries.
565      * @param startTime start time
566      * @param endTime end time
567      * @return spatial boundaries
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         // to float needed as x is in floats and due to precision fTo > 1 may become true
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      * Copies the trajectory but with a subset of the data. Data is taken from position (from + fFrom) to (to + fTo).
603      * @param bounds boundaries
604      * @param <T> type of underlying extended data value
605      * @param <S> storage type
606      * @return subset of the trajectory
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) // otherwise empty, no data in the subset
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                     // should not happen as bounds are determined internally
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     /** {@inheritDoc} */
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     /** {@inheritDoc} */
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     /** {@inheritDoc} */
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      * <p>
751      * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
752      * <br>
753      * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
754      * <p>
755      * @version $Revision$, $LastChangedDate$, by $Author$, initial version 12 okt. 2016 <br>
756      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
757      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
758      * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
759      */
760     private class Boundaries
761     {
762         /** Rounded-down from-index. */
763         @SuppressWarnings("checkstyle:visibilitymodifier")
764         public final int from;
765 
766         /** Fraction of to-index. */
767         @SuppressWarnings("checkstyle:visibilitymodifier")
768         public final double fFrom;
769 
770         /** Rounded-down to-index. */
771         @SuppressWarnings("checkstyle:visibilitymodifier")
772         public final int to;
773 
774         /** Fraction of to-index. */
775         @SuppressWarnings("checkstyle:visibilitymodifier")
776         public final double fTo;
777 
778         /**
779          * @param from from index, rounded down
780          * @param fFrom from index, fraction
781          * @param to to index, rounded down
782          * @param fTo to index, fraction
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          * @param boundaries boundaries
804          * @return intersection of both boundaries
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         /** {@inheritDoc} */
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 }