diff --git a/src/edu/nps/moves/dis7/utilities/stream/X3dCoordinates.java b/src/edu/nps/moves/dis7/utilities/stream/X3dCoordinates.java
new file mode 100644
index 0000000000000000000000000000000000000000..17d48c4595c90efa053809311ec0522a4347aa00
--- /dev/null
+++ b/src/edu/nps/moves/dis7/utilities/stream/X3dCoordinates.java
@@ -0,0 +1,85 @@
+package edu.nps.moves.dis7.utilities.stream;
+
+/**
+ * This class is a holder for coordinates and angles of ESPDUs to store them in
+ * HashMaps
+ *
+ * @author Tobias Brennenstuhl @ NPS
+ */
+public class X3dCoordinates {
+
+ private double x;
+ private double y;
+ private double z;
+ private double phi;
+ private double psi;
+ private double theta;
+
+ public X3dCoordinates(double x, double y, double z, double phi, double psi, double theta) {
+ this.setX(x);
+ this.setY(y);
+ this.setZ(z);
+ this.setPhi(phi);
+ this.setPsi(psi);
+ this.setTheta(theta);
+
+ }
+
+ public X3dCoordinates() {
+ this.setX(0.0);
+ this.setY(0.0);
+ this.setZ(0.0);
+ this.setPhi(0.0);
+ this.setPsi(0.0);
+ this.setTheta(0.0);
+ }
+
+ public double getPhi() {
+ return phi;
+ }
+
+ public void setPhi(double phi) {
+ this.phi = phi;
+ }
+
+ public double getPsi() {
+ return psi;
+ }
+
+ public void setPsi(double psi) {
+ this.psi = psi;
+ }
+
+ public double getTheta() {
+ return theta;
+ }
+
+ public void setTheta(double theta) {
+ this.theta = theta;
+ }
+
+ public double getX() {
+ return x;
+ }
+
+ public void setX(double x) {
+ this.x = x;
+ }
+
+ public double getY() {
+ return y;
+ }
+
+ public void setY(double y) {
+ this.y = y;
+ }
+
+ public double getZ() {
+ return z;
+ }
+
+ public void setZ(double z) {
+ this.z = z;
+ }
+
+}
diff --git a/src/edu/nps/moves/dis7/utilities/stream/X3dCreateInterpolators.java b/src/edu/nps/moves/dis7/utilities/stream/X3dCreateInterpolators.java
new file mode 100644
index 0000000000000000000000000000000000000000..4adb89e15b517948f6292a380429611bacacf6e1
--- /dev/null
+++ b/src/edu/nps/moves/dis7/utilities/stream/X3dCreateInterpolators.java
@@ -0,0 +1,287 @@
+/*
+ * To change this license header, choose License Headers in Project Properties.
+ * To change this template file, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package edu.nps.moves.dis7.utilities.stream;
+
+import edu.nps.moves.dis7.EntityStatePdu;
+import edu.nps.moves.dis7.Pdu;
+import edu.nps.moves.dis7.enumerations.DISPDUType;
+import edu.nps.moves.dis7.utilities.PduFactory;
+import java.nio.ByteBuffer;
+import java.text.NumberFormat;
+import java.util.LinkedHashMap;
+import java.util.Locale;
+import java.util.Set;
+import java.util.TreeMap;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+
+/**
+ *
+ * @author Tobias Brennenstuhl @ NPS
+ */
+public class X3dCreateInterpolators {
+
+ private byte[] bufferShort;
+
+ // -------------------- Begin Variables for Position Interpolator
+ private Boolean firstTimeStamp = true;
+ private int firstLocalTimeStamp = 0;
+
+ private double firstLocalX = 0;
+ private double firstLocalY = 0;
+ private double firstLocalZ = 0;
+ private double firstLocalPhi = 0;
+ private double firstLocalPsi = 0;
+ private double firstLocalTheta = 0;
+
+ private LinkedHashMap<Double, X3dCoordinates> testMap = new LinkedHashMap<>();
+
+ //Setting up a NumberFormatter for limitting the decimal count to 3
+ private NumberFormat coordinateNumberFormat = NumberFormat.getInstance(new Locale("en", "US"));
+
+ // -------------------- End Variables for Position Interpolator
+ public X3dCreateInterpolators() {
+
+ //3 significant digits equals milimeter position accuracy and 0.001 radians = 0.0572963266634555‬ degrees
+ coordinateNumberFormat.setMaximumFractionDigits(3);
+
+ }
+
+ public void addPointsToMap(byte[] localBufferShort) {
+
+ this.bufferShort = localBufferShort.clone();
+
+ if (bufferShort[2] == 1) {
+
+ //PDU Factory
+ PduFactory pduFactory = new PduFactory();
+ Pdu localPdu = null;
+
+ localPdu = pduFactory.createPdu(bufferShort);
+
+ // ToDO figure out how to do this! makeEntityStatePDU
+ EntityStatePdu localEspdu = pduFactory.makeEntityStatePdu();
+ //Put all the data we need into the localEspdu
+ ByteBuffer espduBuffer = ByteBuffer.wrap(bufferShort);
+ try {
+ localEspdu.unmarshal(espduBuffer);
+ } catch (Exception ex) {
+ Logger.getLogger(X3dCreateInterpolators.class.getName()).log(Level.SEVERE, null, ex);
+ }
+
+ double localTimeStamp = 0;
+ double localX = 0;
+ double localY = 0;
+ double localZ = 0;
+
+ double localPhi = 0;
+ double localPsi = 0;
+ double localTheta = 0;
+
+ //Store the first timestamp to subtract it from all others
+ //Same with X,Y,Z to create a local coordiante system
+ if (firstTimeStamp) {
+
+ firstLocalTimeStamp = localPdu.getTimestamp();
+ localTimeStamp = localPdu.getTimestamp();
+ firstLocalX = localEspdu.getEntityLocation().getX();
+ firstLocalY = localEspdu.getEntityLocation().getZ();
+ firstLocalZ = -1 * localEspdu.getEntityLocation().getY();
+
+ firstTimeStamp = false;
+ }
+
+ localTimeStamp = localPdu.getTimestamp();
+ localX = localEspdu.getEntityLocation().getX();
+ localY = localEspdu.getEntityLocation().getZ();
+ localZ = -1 * localEspdu.getEntityLocation().getY();
+ localPhi = localEspdu.getEntityOrientation().getPhi();
+ localPsi = localEspdu.getEntityOrientation().getPsi();
+ localTheta = localEspdu.getEntityOrientation().getTheta();
+
+ localTimeStamp = localTimeStamp - firstLocalTimeStamp;
+ localX = localX - firstLocalX;
+ localY = localY - firstLocalY;
+ localZ = localZ - firstLocalZ;
+
+ //Divide TimeStamp by 1,300,000 to get something close to a second per Unit.
+ //According to the DIS standard one tick is 3600/(2^31) seconds ~ 1.6764 µs
+ //1,100,000 was derived from a stream that is 83 seconds long. The number was adjusted to get a timesensor with 83 seconds
+ //ToDo find the real conversion between TimeStampDelta and seconds
+ localTimeStamp = localTimeStamp / 1100000;
+
+ //Only add to stream if it is an ESPDU
+ //ToDo: Add support for multiple Entities
+ if ((localPdu.getPduType() != null) && (localPdu.getPduType() == DISPDUType.ENTITY_STATE)) {
+
+ testMap.put((double) localTimeStamp, new X3dCoordinates(localX, localY, localZ, localPhi, localPsi, localTheta));
+
+ }
+ }
+
+ }
+
+ public void makeX3dInterpolator() {
+
+ //Compression of the testMap.
+ //Remove all collinear points.
+ X3dSlidingWindowCompression slidingWindowCompression = new X3dSlidingWindowCompression(testMap);
+
+ TreeMap<Double, X3dCoordinates> returnMap = new TreeMap<>();
+
+ //To turn of the compression just comment the next line out and the very next in.
+ returnMap = slidingWindowCompression.doSlidingWindow();
+ //returnMap.putAll(testMap);
+
+ //Writing all values from the KeyMap to a proper Position Interpolator String
+ System.out.println("Writing Position and Rotation Interpolator");
+ Set<Double> keys = returnMap.keySet();
+ //Set<Double> keys = tempKeyKeyValueSetPositionInterPolator.keySet();
+ String positionKey = "key = '";
+ String positionKeyValue = "keyValue = '";
+ String positionInterpolatorToCopy = "<PositionInterpolator DEF='EntityPosition' ";
+
+ String orientationKeyX = "key = '";
+ String orientationKeyValueX = "keyValue = '";
+ String orientationInterpolatorToCopyX = "<OrientationInterpolator DEF='EntityOrientationX' ";
+
+ String orientationKeyY = "key = '";
+ String orientationKeyValueY = "keyValue = '";
+ String orientationInterpolatorToCopyY = "<OrientationInterpolator DEF='EntityOrientationY' ";
+
+ String orientationKeyZ = "key = '";
+ String orientationKeyValueZ = "keyValue = '";
+ String orientationInterpolatorToCopyZ = "<OrientationInterpolator DEF='EntityOrientationZ' ";
+
+ //Find highest time to do the normalization
+ double lastTimeStamp = 0;
+
+ for (Double k : keys) {
+
+ if (k > lastTimeStamp) {
+
+ lastTimeStamp = k;
+
+ }
+ }
+
+ //Normalize all times in the set
+ LinkedHashMap keyKeyValueSetPositionInterpolator = new LinkedHashMap<Double, String>();
+
+ LinkedHashMap keyKeyValueSetOrientationInterpolatorX = new LinkedHashMap<Double, String>();
+ LinkedHashMap keyKeyValueSetOrientationInterpolatorY = new LinkedHashMap<Double, String>();
+ LinkedHashMap keyKeyValueSetOrientationInterpolatorZ = new LinkedHashMap<Double, String>();
+
+ for (Double k : keys) {
+
+ String localCoordinateString;
+ String localOrientationStringX;
+ String localOrientationStringY;
+ String localOrientationStringZ;
+
+ double tempX = returnMap.get(k).getX();
+ double tempY = returnMap.get(k).getY();
+ double tempZ = returnMap.get(k).getZ();
+
+ double tempPhi = returnMap.get(k).getPhi() / 6.28;
+ double tempPsi = returnMap.get(k).getPsi() / 6.28;
+ double tempTheta = returnMap.get(k).getTheta() / 6.28;
+
+ localCoordinateString = " " + coordinateNumberFormat.format(tempX) + " " + coordinateNumberFormat.format(tempY) + " " + coordinateNumberFormat.format(tempZ);
+ localOrientationStringX = " 1 0 0 " + coordinateNumberFormat.format(tempPhi);
+ localOrientationStringY = " 0 1 0 " + coordinateNumberFormat.format(tempTheta);
+ localOrientationStringZ = " 0 0 1 " + coordinateNumberFormat.format(tempPsi);
+
+ keyKeyValueSetPositionInterpolator.put(k / lastTimeStamp, localCoordinateString);
+ keyKeyValueSetOrientationInterpolatorX.put(k / lastTimeStamp, localOrientationStringX);
+ keyKeyValueSetOrientationInterpolatorY.put(k / lastTimeStamp, localOrientationStringY);
+ keyKeyValueSetOrientationInterpolatorZ.put(k / lastTimeStamp, localOrientationStringZ);
+
+ }
+
+ keys = keyKeyValueSetPositionInterpolator.keySet();
+
+ //Setting up the timeSensor
+ //Only one timeSensor for both interpolators is needed
+ String timeSensor = "<TimeSensor DEF='PduStreamClock' cycleInterval='";
+
+ timeSensor += lastTimeStamp;
+
+ timeSensor += "' loop = 'true'/>";
+
+ //Printing the timeSensor to the console
+ System.out.println(timeSensor);
+
+ //Setting up PositionInterpolator and OrientationInterpolator
+ for (Double k : keys) {
+ //System.out.println("Time: " + k + " Position (x,y,z) " + keyKeyValueSetPositionInterpolator.get(k));
+
+ //PositionInterpolator
+ positionKey += coordinateNumberFormat.format(k) + " ";
+ positionKeyValue += keyKeyValueSetPositionInterpolator.get(k) + " ";
+
+ //OrientationInterpolator for X (phi)
+ orientationKeyX += coordinateNumberFormat.format(k) + " ";
+ orientationKeyValueX += keyKeyValueSetOrientationInterpolatorX.get(k) + " ";
+
+ //OrientationInterpolator for Y (theta)
+ orientationKeyY += coordinateNumberFormat.format(k) + " ";
+ orientationKeyValueY += keyKeyValueSetOrientationInterpolatorY.get(k) + " ";
+
+ //OrientationInterpolator for Z (psi)
+ orientationKeyZ += coordinateNumberFormat.format(k) + " ";
+ orientationKeyValueZ += keyKeyValueSetOrientationInterpolatorZ.get(k) + " ";
+
+ }
+ positionKey += "' ";
+ positionKeyValue += "' ";
+
+ orientationKeyX += "' ";
+ orientationKeyValueX += "' ";
+
+ orientationKeyY += "' ";
+ orientationKeyValueY += "' ";
+
+ orientationKeyZ += "' ";
+ orientationKeyValueZ += "' ";
+
+ //PositionInterpolator
+ positionInterpolatorToCopy += positionKey + "\n";
+ positionInterpolatorToCopy += positionKeyValue;
+ positionInterpolatorToCopy += "/>";
+
+ //PositionInterpolator for X
+ orientationInterpolatorToCopyX += orientationKeyX + "\n";
+ orientationInterpolatorToCopyX += orientationKeyValueX;
+ orientationInterpolatorToCopyX += "/>";
+
+ //PositionInterpolator for Y
+ orientationInterpolatorToCopyY += orientationKeyY + "\n";
+ orientationInterpolatorToCopyY += orientationKeyValueY;
+ orientationInterpolatorToCopyY += "/>";
+
+ //PositionInterpolator for Z
+ orientationInterpolatorToCopyZ += orientationKeyY + "\n";
+ orientationInterpolatorToCopyZ += orientationKeyValueZ;
+ orientationInterpolatorToCopyZ += "/>";
+
+ //Printing PositionInterpolator to the console
+ System.out.println(positionInterpolatorToCopy);
+
+ //First Rotation must be around z axis by psi
+ //Printing OrientationInterpolator for X to the console
+ System.out.println(orientationInterpolatorToCopyZ);
+
+ //Second Rotation must be around resulting y (y') axis by theta
+ //Printing OrientationInterpolator for Y to the console
+ System.out.println(orientationInterpolatorToCopyY);
+
+ //last rotation must be around resulting x (x') axis by phi
+ //Printing OrientationInterpolator for Z to the console
+ System.out.println(orientationInterpolatorToCopyX);
+ }
+
+}
diff --git a/src/edu/nps/moves/dis7/utilities/stream/X3dSlidingWindowCompression.java b/src/edu/nps/moves/dis7/utilities/stream/X3dSlidingWindowCompression.java
new file mode 100644
index 0000000000000000000000000000000000000000..21f311f1e00fb4e94b22fc7375f081fe4e43e6e0
--- /dev/null
+++ b/src/edu/nps/moves/dis7/utilities/stream/X3dSlidingWindowCompression.java
@@ -0,0 +1,135 @@
+package edu.nps.moves.dis7.utilities.stream;
+
+import static java.lang.Math.pow;
+import static java.lang.Math.sqrt;
+import java.util.ArrayList;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.Set;
+import java.util.TreeMap;
+
+/**
+ *
+ * @author Tobias Brennenstuhl @ NPS
+ */
+public class X3dSlidingWindowCompression {
+
+ private LinkedHashMap<Double, X3dCoordinates> localMap;
+
+ public X3dSlidingWindowCompression(LinkedHashMap<Double, X3dCoordinates> localHashMap) {
+
+ this.localMap = new LinkedHashMap<>();
+ Set<Double> keys = localHashMap.keySet();
+ for (Double k : keys) {
+ localMap.put(k, localHashMap.get(k));
+ }
+ }
+
+ public TreeMap<Double, X3dCoordinates> doSlidingWindow() {
+
+ System.out.println("DISTools.Regression.doRegression()");
+ //Check whether points could be deleted to compress the stream
+ //https://www.crashkurs-statistik.de/einfache-lineare-regression/
+ TreeMap<Double, X3dCoordinates> streamMap = new TreeMap<>();
+ //Copy LinkedHashMap into TreeMap to be able to pull the first element.
+ streamMap.putAll(localMap);
+ TreeMap<Double, X3dCoordinates> returnMap = new TreeMap<>();
+ //TreeMap of slidingWindows will store all of the points that are currently processed
+ //use .pullFirstEntry() to get rid of the points at the beginning.
+ TreeMap<Double, X3dCoordinates> slidingWindow = new TreeMap<>();
+
+ while (streamMap.size() > 0) {
+ slidingWindow.put(streamMap.firstEntry().getKey(), streamMap.get(streamMap.firstEntry().getKey()));
+ streamMap.pollFirstEntry();
+
+ //Calculate the mean and SD
+ Set<Double> slidingWindowKeys = slidingWindow.keySet();
+
+ if (slidingWindow.size() >= 3) {
+
+ List<Double> tList = new ArrayList<>();
+ List<Double> xList = new ArrayList<>();
+ List<Double> yList = new ArrayList<>();
+ List<Double> zList = new ArrayList<>();
+ List<Double> phiList = new ArrayList<>();
+ List<Double> psiList = new ArrayList<>();
+ List<Double> thetaList = new ArrayList<>();
+
+ Double[] k = new Double[slidingWindowKeys.size()];
+ slidingWindowKeys.toArray(k);
+
+ for (int i = 0; i < slidingWindow.size(); i++) {
+
+ tList.add(i, k[i]);
+
+ phiList.add(i, slidingWindow.get(k[i]).getPhi());
+ psiList.add(i, slidingWindow.get(k[i]).getPsi());
+ thetaList.add(i, slidingWindow.get(k[i]).getTheta());
+ xList.add(i, slidingWindow.get(k[i]).getX());
+ yList.add(i, slidingWindow.get(k[i]).getY());
+ zList.add(i, slidingWindow.get(k[i]).getZ());
+
+ }
+
+ //Calculate Area of Triangle
+ //Credit: http://www.ambrsoft.com/TrigoCalc/Line3D/LineColinear.htm
+ for (int i = 0; i < slidingWindow.size(); i++) {
+
+ double a = sqrt(pow(xList.get(1) - xList.get(0), 2) + pow(yList.get(1) - yList.get(0), 2) + pow(zList.get(1) - zList.get(0), 2));
+ double b = sqrt(pow(xList.get(i) - xList.get(0), 2) + pow(yList.get(i) - yList.get(0), 2) + pow(zList.get(i) - zList.get(0), 2));
+ double c = sqrt(pow(xList.get(i) - xList.get(1), 2) + pow(yList.get(i) - yList.get(1), 2) + pow(zList.get(i) - zList.get(1), 2));
+ double s = (a + b + c) / 2;
+ double areaA = sqrt(s * (s - a) * (s - b) * (s - c));
+
+ if ((areaA >= 0.1) || (tList.get(i) - tList.get(0) >= 4.0)) {
+ //grab the first and the last point from the sliding window and push it to the returnMap
+ X3dCoordinates firstPoint = new X3dCoordinates();
+ firstPoint.setX(xList.get(0));
+ firstPoint.setY(yList.get(0));
+ firstPoint.setZ(zList.get(0));
+ firstPoint.setPhi(phiList.get(0));
+ firstPoint.setPsi(psiList.get(0));
+ firstPoint.setTheta(thetaList.get(0));
+
+ X3dCoordinates lastPoint = new X3dCoordinates(xList.get(i), yList.get(i), zList.get(i), phiList.get(i), psiList.get(i), thetaList.get(i));
+
+ returnMap.put(tList.get(0), firstPoint);
+ returnMap.put(tList.get(i), lastPoint);
+
+ slidingWindow.clear();
+
+ tList.clear();
+ xList.clear();
+ yList.clear();
+ zList.clear();
+ phiList.clear();
+ psiList.clear();
+ thetaList.clear();
+
+ break;
+ }
+
+ if ((areaA <= 0.1) && (tList.get(i) - tList.get(0) <= 4.0) && streamMap.size() == 0) {
+
+ //System.out.println("StreamMap empty. All points left will be added. Break");
+ //grab the first and the last point from the siding window and push it to the returnMap
+ for (int j = 0; j < slidingWindow.size(); j++) {
+ X3dCoordinates leftPoints = new X3dCoordinates(xList.get(j), yList.get(j), zList.get(j), phiList.get(j), psiList.get(j), thetaList.get(j));
+ returnMap.put(tList.get(j), leftPoints);
+ }
+
+ break;
+ }
+ //System.out.println("Area of Triangle: " + areaA);
+ }
+
+ }
+
+ }
+
+ return returnMap;
+
+ }
+;
+
+}