From 8727b4969857f8bffa6de95641345351a360061a Mon Sep 17 00:00:00 2001
From: brennenstuhl <tobias.brennenstuhl.gy@nps.edu>
Date: Mon, 22 Jun 2020 18:14:06 -0700
Subject: [PATCH] DIS reference logs updated
---
.../src/PduStreamTools/PlayerPlainText.java | 284 ++----------------
.../src/PduStreamTools/README.md | 5 +
.../src/PduStreamTools/RecorderPlainText.java | 12 +-
.../{Coordinates.java => X3DCoordinates.java} | 13 +-
...ators.java => X3DCreateInterpolators.java} | 198 +++++++-----
...eX3dLineSet.java => X3DCreateLineSet.java} | 14 +-
....java => X3DSlidingWindowCompression.java} | 83 +----
7 files changed, 186 insertions(+), 423 deletions(-)
create mode 100644 BrennenstuhlTobias/src/PduStreamTools/README.md
rename BrennenstuhlTobias/src/PduStreamTools/{Coordinates.java => X3DCoordinates.java} (74%)
rename BrennenstuhlTobias/src/PduStreamTools/{CreateX3dInterpolators.java => X3DCreateInterpolators.java} (57%)
rename BrennenstuhlTobias/src/PduStreamTools/{CreateX3dLineSet.java => X3DCreateLineSet.java} (89%)
rename BrennenstuhlTobias/src/PduStreamTools/{SlidingWindowCompression.java => X3DSlidingWindowCompression.java} (68%)
diff --git a/BrennenstuhlTobias/src/PduStreamTools/PlayerPlainText.java b/BrennenstuhlTobias/src/PduStreamTools/PlayerPlainText.java
index d831a3f..6ba5eed 100644
--- a/BrennenstuhlTobias/src/PduStreamTools/PlayerPlainText.java
+++ b/BrennenstuhlTobias/src/PduStreamTools/PlayerPlainText.java
@@ -1,24 +1,15 @@
-/*
- * 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 PduStreamTools;
/**
* Copyright (c) 2008-2019, MOVES Institute, Naval Postgraduate School. All
* rights reserved. This work is licensed under the BSD open source license,
* available at https://www.movesinstitute.org/licenses/bsd.html
+ *
*/
-
import static PduStreamTools.RecorderBase64.COMMENT_MARKER;
import static PduStreamTools.RecorderBase64.START_COMMENT_MARKER;
import static PduStreamTools.RecorderBase64.STOP_COMMENT_MARKER;
import com.google.common.primitives.Longs;
-import edu.nps.moves.dis7.EntityStatePdu;
-import edu.nps.moves.dis7.Pdu;
-import edu.nps.moves.dis7.enumerations.DISPDUType;
-import edu.nps.moves.dis7.util.PduFactory;
import java.io.*;
import java.net.DatagramPacket;
@@ -27,14 +18,17 @@ import java.net.InetAddress;
import java.nio.ByteBuffer;
import java.nio.IntBuffer;
import java.nio.file.Path;
-import java.text.NumberFormat;
import java.util.Arrays;
-import java.util.LinkedHashMap;
-import java.util.Locale;
-import java.util.Set;
-import java.util.TreeMap;
import java.util.regex.Pattern;
+/**
+ * PlayerPlainText.java created on Mar 2, 2020 MOVES Institute Naval Postgraduate
+ * School, Monterey, CA, USA www.nps.edu
+ *
+ * @author Mike Bailey, jmbailey@nps.edu
+ * @author Tobias Brennenstuhl, tobias.brennenstuhl.gy@nps.edu
+ */
+
public class PlayerPlainText {
private Path disLogDirectory;
@@ -78,26 +72,13 @@ public class PlayerPlainText {
Arrays.sort(fs, (f1, f2) -> {
return f1.getName().compareTo(f2.getName());
});
-
+
dsock = new DatagramSocket();
-
+
// -------------------- Begin Variables for Position Interpolator
- //Test external SourceCode
- CreateX3dInterpolators x3dInterpolators = new CreateX3dInterpolators();
- CreateX3dLineSet x3dLineSet = new CreateX3dLineSet();
+ X3DCreateInterpolators x3dInterpolators = new X3DCreateInterpolators();
+ X3DCreateLineSet x3dLineSet = new X3DCreateLineSet();
byte[] globalByteBufferForX3dInterPolators = null;
-// Boolean firstTimeStamp = true;
-// int firstLocalTimeStamp = 0;
-//
-// double firstLocalX = 0;
-// double firstLocalY = 0;
-// double firstLocalZ = 0;
-//
-// LinkedHashMap<Double, Coordinates> testMap = new LinkedHashMap<>();
-//
-// //Setting up a NumberFormatter for limitting the decimal count to 3
-// NumberFormat coordinateNumberFormat = NumberFormat.getInstance(new Locale("en", "US"));
-// coordinateNumberFormat.setMaximumFractionDigits(3);
// -------------------- End Variables for Position Interpolator
for (File f : fs) {
@@ -160,7 +141,7 @@ public class PlayerPlainText {
int tempInt = Integer.parseInt(tempString);
arr[x] = tempInt;
-
+
}
// Credit: https://stackoverflow.com/questions/1086054/how-to-convert-int-to-byte
@@ -218,9 +199,6 @@ public class PlayerPlainText {
//Therefore a shortBuffer is created where only every forth value is stored.
//it must be done with modulo instead of testing for "0" because a "0" could be there as value and not as padding
byte[] bufferShort = new byte[byteBuffer2.array().length / 4];
-
-
-
int bufferShortCounter = 0;
for (int i = 1; i < byteBuffer2.array().length; i++) {
@@ -235,76 +213,12 @@ public class PlayerPlainText {
}
DatagramPacket packet = new DatagramPacket(bufferShort, bufferShort.length, addr, port);
-
+
globalByteBufferForX3dInterPolators = new byte[byteBuffer2.array().length / 4];
globalByteBufferForX3dInterPolators = bufferShort.clone();
-
+
dsock.send(packet);
- //PDU Factory
-// PduFactory pduFactory = new PduFactory();
-// Pdu localPdu = null;
-//
-// if (arr[2] == 1) {
-//
-// 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);
-// localEspdu.unmarshal(espduBuffer);
-//
-// 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().getY();
-// firstLocalZ = localEspdu.getEntityLocation().getZ();
-//
-// firstTimeStamp = false;
-// }
-//
-// localTimeStamp = localPdu.getTimestamp();
-// localX = localEspdu.getEntityLocation().getX();
-// localY = localEspdu.getEntityLocation().getY();
-// localZ = localEspdu.getEntityLocation().getZ();
-// 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,300,000 was derived from a stream that is 61 seconds long. The number was adjusted to get a timesensor with 61 seconds
-// //ToDo find the real conversion between TimeStampDelta and seconds
-// localTimeStamp = localTimeStamp / 1300000;
-//
-// //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 Coordinates(localX, localY, localZ, localPhi, localPsi, localTheta));
-//
-// }
-//
-// }
x3dInterpolators.addPointsToMap(globalByteBufferForX3dInterPolators);
x3dLineSet.addPointsToMap(globalByteBufferForX3dInterPolators);
pduCount++;
@@ -320,168 +234,10 @@ public class PlayerPlainText {
line = brdr.readLine();
}
brdr.close();
-//
-// //Compression of the testMap.
-// //Remove all collinear points.
-// Compressor regression = new Compressor(testMap);
-//
-// TreeMap<Double, Coordinates> returnMap = new TreeMap<>();
-//
-// //To turn of the compression just comment the next line out and the very next in.
-// returnMap = regression.doCompression();
-// //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
-// var keyKeyValueSetPositionInterpolator = new LinkedHashMap<Double, String>();
-//
-// var keyKeyValueSetOrientationInterpolatorX = new LinkedHashMap<Double, String>();
-// var keyKeyValueSetOrientationInterpolatorY = new LinkedHashMap<Double, String>();
-// var 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(tempPsi);
-// localOrientationStringZ = " 0 0 1 " + coordinateNumberFormat.format(tempTheta);
-//
-// 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
-// orientationKeyX += coordinateNumberFormat.format(k) + " ";
-// orientationKeyValueX += keyKeyValueSetOrientationInterpolatorX.get(k) + " ";
-//
-// //OrientationInterpolator for Y
-// orientationKeyY += coordinateNumberFormat.format(k) + " ";
-// orientationKeyValueY += keyKeyValueSetOrientationInterpolatorY.get(k) + " ";
-//
-// //OrientationInterpolator for Z
-// 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);
-//
-// //Printing OrientationInterpolator for X to the console
-// System.out.println(orientationInterpolatorToCopyX);
-//
-// //Printing OrientationInterpolator for Y to the console
-// System.out.println(orientationInterpolatorToCopyY);
-//
-// //Printing OrientationInterpolator for Z to the console
-// System.out.println(orientationInterpolatorToCopyZ);
-
-
-
- x3dInterpolators.makeX3dInterpolator();
- x3dLineSet.makeX3dLineSet();
-
-
+
+ x3dInterpolators.makeX3dInterpolator();
+ x3dLineSet.makeX3dLineSet();
+
}
} catch (Exception ex) {
System.err.println("Exception reading/writing pdus: " + ex.getClass().getSimpleName() + ": " + ex.getLocalizedMessage());
diff --git a/BrennenstuhlTobias/src/PduStreamTools/README.md b/BrennenstuhlTobias/src/PduStreamTools/README.md
new file mode 100644
index 0000000..4b4f639
--- /dev/null
+++ b/BrennenstuhlTobias/src/PduStreamTools/README.md
@@ -0,0 +1,5 @@
+Future Work:
+
+- add support for multiple entities
+- connect to Framework that autogenerates nodes (instead of string output)
+- add ANT build script for Unit-Testing
\ No newline at end of file
diff --git a/BrennenstuhlTobias/src/PduStreamTools/RecorderPlainText.java b/BrennenstuhlTobias/src/PduStreamTools/RecorderPlainText.java
index 52853fa..4981f42 100644
--- a/BrennenstuhlTobias/src/PduStreamTools/RecorderPlainText.java
+++ b/BrennenstuhlTobias/src/PduStreamTools/RecorderPlainText.java
@@ -1,8 +1,3 @@
-/*
- * 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 PduStreamTools;
import com.google.common.primitives.Longs;
@@ -22,6 +17,13 @@ import java.io.IOException;
import java.nio.file.Path;
import java.util.Arrays;
+/**
+ * RecorderPlainText.java created on Mar 2, 2020 MOVES Institute Naval Postgraduate
+ * School, Monterey, CA, USA www.nps.edu
+ *
+ * @author Mike Bailey, jmbailey@nps.edu
+ * @author Tobias Brennenstuhl, tobias.brennenstuhl.gy@nps.edu
+ */
public class RecorderPlainText implements PduReceiver
{
diff --git a/BrennenstuhlTobias/src/PduStreamTools/Coordinates.java b/BrennenstuhlTobias/src/PduStreamTools/X3DCoordinates.java
similarity index 74%
rename from BrennenstuhlTobias/src/PduStreamTools/Coordinates.java
rename to BrennenstuhlTobias/src/PduStreamTools/X3DCoordinates.java
index 71f7c95..e84e23d 100644
--- a/BrennenstuhlTobias/src/PduStreamTools/Coordinates.java
+++ b/BrennenstuhlTobias/src/PduStreamTools/X3DCoordinates.java
@@ -1,17 +1,12 @@
-/*
- * 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 PduStreamTools;
/**
* This class is a holder for coordinates and angles of ESPDUs to store them in
* HashMaps
*
- * @author Tobias Brennenstuhl
+ * @author Tobias Brennenstuhl @ NPS
*/
-public class Coordinates {
+public class X3DCoordinates {
private double x;
private double y;
@@ -20,7 +15,7 @@ public class Coordinates {
private double psi;
private double theta;
- public Coordinates(double x, double y, double z, double phi, double psi, 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);
@@ -30,7 +25,7 @@ public class Coordinates {
}
- public Coordinates() {
+ public X3DCoordinates() {
this.setX(0.0);
this.setY(0.0);
this.setZ(0.0);
diff --git a/BrennenstuhlTobias/src/PduStreamTools/CreateX3dInterpolators.java b/BrennenstuhlTobias/src/PduStreamTools/X3DCreateInterpolators.java
similarity index 57%
rename from BrennenstuhlTobias/src/PduStreamTools/CreateX3dInterpolators.java
rename to BrennenstuhlTobias/src/PduStreamTools/X3DCreateInterpolators.java
index 7d5b5d5..56b546f 100644
--- a/BrennenstuhlTobias/src/PduStreamTools/CreateX3dInterpolators.java
+++ b/BrennenstuhlTobias/src/PduStreamTools/X3DCreateInterpolators.java
@@ -22,7 +22,7 @@ import java.util.logging.Logger;
*
* @author Tobias Brennenstuhl @ NPS
*/
-public class CreateX3dInterpolators {
+public class X3DCreateInterpolators {
private byte[] bufferShort;
@@ -33,14 +33,17 @@ public class CreateX3dInterpolators {
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, Coordinates> testMap = new LinkedHashMap<>();
+ 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 CreateX3dInterpolators() {
+ public X3DCreateInterpolators() {
//3 significant digits equals milimeter position accuracy and 0.001 radians = 0.0572963266634555‬ degrees
coordinateNumberFormat.setMaximumFractionDigits(3);
@@ -52,71 +55,71 @@ public class CreateX3dInterpolators {
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(CreateX3dInterpolators.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;
+ //PDU Factory
+ PduFactory pduFactory = new PduFactory();
+ Pdu localPdu = null;
- //Store the first timestamp to subtract it from all others
- //Same with X,Y,Z to create a local coordiante system
- if (firstTimeStamp) {
+ localPdu = pduFactory.createPdu(bufferShort);
- firstLocalTimeStamp = localPdu.getTimestamp();
- localTimeStamp = localPdu.getTimestamp();
- firstLocalX = localEspdu.getEntityLocation().getX();
- firstLocalY = localEspdu.getEntityLocation().getZ();
- firstLocalZ = -1*localEspdu.getEntityLocation().getY();
+ // 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);
+ }
- firstTimeStamp = false;
- }
+ double localTimeStamp = 0;
+ double localX = 0;
+ double localY = 0;
+ double localZ = 0;
- 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();
+ double localPhi = 0;
+ double localPsi = 0;
+ double localTheta = 0;
- localTimeStamp = localTimeStamp - firstLocalTimeStamp;
- localX = localX - firstLocalX;
- localY = localY - firstLocalY;
- localZ = localZ - firstLocalZ;
+ //Store the first timestamp to subtract it from all others
+ //Same with X,Y,Z to create a local coordiante system
+ if (firstTimeStamp) {
- //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,300,000 was derived from a stream that is 61 seconds long. The number was adjusted to get a timesensor with 61 seconds
- //ToDo find the real conversion between TimeStampDelta and seconds
- localTimeStamp = localTimeStamp / 1300000;
+ firstLocalTimeStamp = localPdu.getTimestamp();
+ localTimeStamp = localPdu.getTimestamp();
+ firstLocalX = localEspdu.getEntityLocation().getX();
+ firstLocalY = localEspdu.getEntityLocation().getZ();
+ firstLocalZ = -1 * localEspdu.getEntityLocation().getY();
- //Only add to stream if it is an ESPDU
- //ToDo: Add support for multiple Entities
- if ((localPdu.getPduType() != null) && (localPdu.getPduType() == DISPDUType.ENTITY_STATE)) {
+ firstTimeStamp = false;
+ }
- testMap.put((double) localTimeStamp, new Coordinates(localX, localY, localZ, localPhi, localPsi, localTheta));
+ 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));
- }
+ }
}
}
@@ -125,16 +128,16 @@ public class CreateX3dInterpolators {
//Compression of the testMap.
//Remove all collinear points.
- SlidingWindowCompression slidingWindowCompression = new SlidingWindowCompression(testMap);
+ X3DSlidingWindowCompression slidingWindowCompression = new X3DSlidingWindowCompression(testMap);
- TreeMap<Double, Coordinates> returnMap = new TreeMap<>();
+ 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");
+ System.out.println("Writing Position and Orientation Interpolator:");
Set<Double> keys = returnMap.keySet();
//Set<Double> keys = tempKeyKeyValueSetPositionInterPolator.keySet();
String positionKey = "key = '";
@@ -152,7 +155,12 @@ public class CreateX3dInterpolators {
String orientationKeyZ = "key = '";
String orientationKeyValueZ = "keyValue = '";
String orientationInterpolatorToCopyZ = "<OrientationInterpolator DEF='EntityOrientationZ' ";
-
+
+ String waypointInterpolatorWayPoints = "value = '";
+ String waypointInterpolatorLegDurations = "value = '";
+ String waypointInterpolatorWayPointsToCopy = "<fieldValue name='waypoints' ";
+ String waypointInterpolatorLegDurationsToCopy = "<fieldValue name='legDurations' ";
+
//Find highest time to do the normalization
double lastTimeStamp = 0;
@@ -171,13 +179,18 @@ public class CreateX3dInterpolators {
var keyKeyValueSetOrientationInterpolatorX = new LinkedHashMap<Double, String>();
var keyKeyValueSetOrientationInterpolatorY = new LinkedHashMap<Double, String>();
var keyKeyValueSetOrientationInterpolatorZ = new LinkedHashMap<Double, String>();
-
+
+ var waypointInterpolatorValueSet = new LinkedHashMap<Double, String>();
+
+ double kLegduration = 0;
+
for (Double k : keys) {
String localCoordinateString;
String localOrientationStringX;
String localOrientationStringY;
String localOrientationStringZ;
+ String localWaypointInterpolator;
double tempX = returnMap.get(k).getX();
double tempY = returnMap.get(k).getY();
@@ -189,13 +202,16 @@ public class CreateX3dInterpolators {
localCoordinateString = " " + coordinateNumberFormat.format(tempX) + " " + coordinateNumberFormat.format(tempY) + " " + coordinateNumberFormat.format(tempZ);
localOrientationStringX = " 1 0 0 " + coordinateNumberFormat.format(tempPhi);
- localOrientationStringY = " 0 1 0 " + coordinateNumberFormat.format(tempPsi);
- localOrientationStringZ = " 0 0 1 " + coordinateNumberFormat.format(tempTheta);
+ 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);
+
+ waypointInterpolatorValueSet.put(60*(k-kLegduration)/lastTimeStamp,localCoordinateString);
+ kLegduration = k;
}
@@ -210,7 +226,6 @@ public class CreateX3dInterpolators {
timeSensor += "' loop = 'true'/>";
//Printing the timeSensor to the console
-
System.out.println(timeSensor);
//Setting up PositionInterpolator and OrientationInterpolator
@@ -221,19 +236,42 @@ public class CreateX3dInterpolators {
positionKey += coordinateNumberFormat.format(k) + " ";
positionKeyValue += keyKeyValueSetPositionInterpolator.get(k) + " ";
- //OrientationInterpolator for X
+ //OrientationInterpolator for X (phi)
orientationKeyX += coordinateNumberFormat.format(k) + " ";
orientationKeyValueX += keyKeyValueSetOrientationInterpolatorX.get(k) + " ";
- //OrientationInterpolator for Y
+ //OrientationInterpolator for Y (theta)
orientationKeyY += coordinateNumberFormat.format(k) + " ";
orientationKeyValueY += keyKeyValueSetOrientationInterpolatorY.get(k) + " ";
- //OrientationInterpolator for Z
+ //OrientationInterpolator for Z (psi)
orientationKeyZ += coordinateNumberFormat.format(k) + " ";
orientationKeyValueZ += keyKeyValueSetOrientationInterpolatorZ.get(k) + " ";
+
+
+
}
+
+ //WayPointInterpolator
+ keys = waypointInterpolatorValueSet.keySet();
+
+ for (Double k : keys) {
+
+ waypointInterpolatorWayPoints += waypointInterpolatorValueSet.get(k) + " ";
+
+ if (k!=0) {
+
+ waypointInterpolatorLegDurations += coordinateNumberFormat.format(k) + " ";
+ }
+ }
+
+
+
+
+
+
+
positionKey += "' ";
positionKeyValue += "' ";
@@ -245,6 +283,9 @@ public class CreateX3dInterpolators {
orientationKeyZ += "' ";
orientationKeyValueZ += "' ";
+
+ waypointInterpolatorWayPoints += "' ";
+ waypointInterpolatorLegDurations += "' ";
//PositionInterpolator
positionInterpolatorToCopy += positionKey + "\n";
@@ -265,18 +306,33 @@ public class CreateX3dInterpolators {
orientationInterpolatorToCopyZ += orientationKeyY + "\n";
orientationInterpolatorToCopyZ += orientationKeyValueZ;
orientationInterpolatorToCopyZ += "/>";
+
+ //WaypointInterpolator
+ waypointInterpolatorWayPointsToCopy += waypointInterpolatorWayPoints;
+ waypointInterpolatorWayPointsToCopy += "/>";
+
+ waypointInterpolatorLegDurationsToCopy += waypointInterpolatorLegDurations;
+ waypointInterpolatorLegDurationsToCopy += "/>";
//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(orientationInterpolatorToCopyX);
+ 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(orientationInterpolatorToCopyZ);
+ System.out.println(orientationInterpolatorToCopyX);
+
+ //WaypointInterpolator:
+ System.out.println(waypointInterpolatorWayPointsToCopy);
+ System.out.println(waypointInterpolatorLegDurationsToCopy);
+
}
}
diff --git a/BrennenstuhlTobias/src/PduStreamTools/CreateX3dLineSet.java b/BrennenstuhlTobias/src/PduStreamTools/X3DCreateLineSet.java
similarity index 89%
rename from BrennenstuhlTobias/src/PduStreamTools/CreateX3dLineSet.java
rename to BrennenstuhlTobias/src/PduStreamTools/X3DCreateLineSet.java
index 5a9337f..cb95656 100644
--- a/BrennenstuhlTobias/src/PduStreamTools/CreateX3dLineSet.java
+++ b/BrennenstuhlTobias/src/PduStreamTools/X3DCreateLineSet.java
@@ -22,7 +22,7 @@ import java.util.logging.Logger;
*
* @author Tobias Brennenstuhl @ NPS
*/
-public class CreateX3dLineSet {
+public class X3DCreateLineSet {
private byte[] bufferShort;
@@ -34,13 +34,13 @@ public class CreateX3dLineSet {
private double firstLocalY = 0;
private double firstLocalZ = 0;
- private LinkedHashMap<Double, Coordinates> testMap = new LinkedHashMap<>();
+ 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 CreateX3dLineSet() {
+ public X3DCreateLineSet() {
//3 significant digits equals milimeter position accuracy and 0.001 radians = 0.0572963266634555‬ degrees
coordinateNumberFormat.setMaximumFractionDigits(3);
@@ -66,7 +66,7 @@ public class CreateX3dLineSet {
try {
localEspdu.unmarshal(espduBuffer);
} catch (Exception ex) {
- Logger.getLogger(CreateX3dLineSet.class.getName()).log(Level.SEVERE, null, ex);
+ Logger.getLogger(X3DCreateLineSet.class.getName()).log(Level.SEVERE, null, ex);
}
double localTimeStamp = 0;
@@ -112,7 +112,7 @@ public class CreateX3dLineSet {
//ToDo: Add support for multiple Entities
if ((localPdu.getPduType() != null) && (localPdu.getPduType() == DISPDUType.ENTITY_STATE)) {
- testMap.put((double) localTimeStamp, new Coordinates(localX, localY, localZ, 0.0, 0.0, 0.0));
+ testMap.put((double) localTimeStamp, new X3DCoordinates(localX, localY, localZ, 0.0, 0.0, 0.0));
}
}
@@ -123,9 +123,9 @@ public class CreateX3dLineSet {
//Compression of the testMap.
//Remove all collinear points.
- SlidingWindowCompression regression = new SlidingWindowCompression(testMap);
+ X3DSlidingWindowCompression regression = new X3DSlidingWindowCompression(testMap);
- TreeMap<Double, Coordinates> returnMap = new TreeMap<>();
+ TreeMap<Double, X3DCoordinates> returnMap = new TreeMap<>();
//To turn of the compression just comment the next line out and the very next in.
returnMap = regression.doSlidingWindow();
diff --git a/BrennenstuhlTobias/src/PduStreamTools/SlidingWindowCompression.java b/BrennenstuhlTobias/src/PduStreamTools/X3DSlidingWindowCompression.java
similarity index 68%
rename from BrennenstuhlTobias/src/PduStreamTools/SlidingWindowCompression.java
rename to BrennenstuhlTobias/src/PduStreamTools/X3DSlidingWindowCompression.java
index ac8a50d..7fd860f 100644
--- a/BrennenstuhlTobias/src/PduStreamTools/SlidingWindowCompression.java
+++ b/BrennenstuhlTobias/src/PduStreamTools/X3DSlidingWindowCompression.java
@@ -1,8 +1,3 @@
-/*
- * 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 PduStreamTools;
import static java.lang.Math.pow;
@@ -15,48 +10,33 @@ import java.util.TreeMap;
/**
*
- * @author tobia
+ * @author Tobias Brennenstuhl @ NPS
*/
-public class SlidingWindowCompression {
+public class X3DSlidingWindowCompression {
- private LinkedHashMap<Double, Coordinates> localMap;
+ private LinkedHashMap<Double, X3DCoordinates> localMap;
- public SlidingWindowCompression(LinkedHashMap<Double, Coordinates> localHashMap) {
+ public X3DSlidingWindowCompression(LinkedHashMap<Double, X3DCoordinates> localHashMap) {
- //copy the given HashMap to a localMap
this.localMap = new LinkedHashMap<>();
-
Set<Double> keys = localHashMap.keySet();
-
for (Double k : keys) {
-
localMap.put(k, localHashMap.get(k));
-
}
-
- //System.out.println("DISTools.Regression.<init>()");
-
}
- public TreeMap<Double, Coordinates> doSlidingWindow() {
+ 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, Coordinates> streamMap = new TreeMap<>();
-
- Set<Double> keys = localMap.keySet();
-
+ TreeMap<Double, X3DCoordinates> streamMap = new TreeMap<>();
//Copy LinkedHashMap into TreeMap to be able to pull the first element.
streamMap.putAll(localMap);
-
- TreeMap<Double, Coordinates> returnMap = new TreeMap<>();
+ 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, Coordinates> slidingWindow = new TreeMap<>();
-
- boolean addToWindow = true;
+ TreeMap<Double, X3DCoordinates> slidingWindow = new TreeMap<>();
while (streamMap.size() > 0) {
slidingWindow.put(streamMap.firstEntry().getKey(), streamMap.get(streamMap.firstEntry().getKey()));
@@ -74,52 +54,36 @@ public class SlidingWindowCompression {
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++) {
- //Fix Loop to fill Arrays
tList.add(i, k[i]);
-
- phiList.add(i, slidingWindow.get(k[i]).getPhi());
+ 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 triangle according to the homepage
+ //Calculate Area of Triangle
+ //Credit: http://www.ambrsoft.com/TrigoCalc/Line3D/LineColinear.htm
for (int i = 0; i < slidingWindow.size(); i++) {
- //Calculate Area of Triangle
- //http://www.ambrsoft.com/TrigoCalc/Line3D/LineColinear.htm
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)) {
-
- //@Debug
- //System.out.println("Threshold met. Break");
-
//grab the first and the last point from the sliding window and push it to the returnMap
- Coordinates firstPoint = new Coordinates();
+ X3DCoordinates firstPoint = new X3DCoordinates();
firstPoint.setX(xList.get(0));
firstPoint.setY(yList.get(0));
firstPoint.setZ(zList.get(0));
@@ -127,8 +91,7 @@ public class SlidingWindowCompression {
firstPoint.setPsi(psiList.get(0));
firstPoint.setTheta(thetaList.get(0));
- //ToDo add the angles to the Coordinates
- Coordinates lastPoint = new Coordinates(xList.get(i), yList.get(i), zList.get(i), phiList.get(i), psiList.get(i), thetaList.get(i));
+ 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);
@@ -142,7 +105,6 @@ public class SlidingWindowCompression {
phiList.clear();
psiList.clear();
thetaList.clear();
-
break;
}
@@ -150,22 +112,15 @@ public class SlidingWindowCompression {
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 regression test and push it to the returnMap
+ //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++) {
-
- //ToDo: Add the angles to the coordinates
- Coordinates leftPoints = new Coordinates(xList.get(j), yList.get(j), zList.get(j), phiList.get(j), psiList.get(j), thetaList.get(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);
-
}
}
@@ -175,12 +130,6 @@ public class SlidingWindowCompression {
return returnMap;
}
-
- ;
-
- private void calcValues() {
-
- }
;
}
--
GitLab