classes for X3D tools added

src/edu/nps/moves/dis7/utilities/stream/X3dCoordinates.java

+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;
+    }
+
+}

src/edu/nps/moves/dis7/utilities/stream/X3dCreateInterpolators.java

+/*
+ * 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);
+    }
+
+}

src/edu/nps/moves/dis7/utilities/stream/X3dSlidingWindowCompression.java

+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;
+
+    }
+;
+
+}