Please review any and all PUBLIC repositories, groups and associate files. These allow anyone on the Internet to access without authentication. Repository and group owners are responsible for their content and permission settings. Go to your project(s), click on Settings > General and expand the "Visibility, project features, permissions" to change this setting.

Angles and Orientation Interpolator added

parent e5f45a90
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
!End!End of DIS capture file, Pdusave2.dislog.
!Begin!Beginning of DIS capture file, Pdusave3.dislog.
AAAAAAAAAAA=,BwABAQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAbVXeQ=,BwACAgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAA2Rr6w=,BwADAgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAABRrkWw=,BwAEAQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAABsPCRg=,BwAFAwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAACGZ4zQ=,BwAGAwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAACg3VEw=,BwAHAwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAC7oFew=,BwAIAwAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAADWCwgg=,BwAJAwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAADxGO3Q=,BwAKAwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAEMCNPQ=,BwALBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAEmVlRg=,BwAMBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAFA2GUw=,BwANBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAFbiNdw=,BwAOBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAF15UXg=,BwAPBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAEAAAAAAAA=
AAAAAGQnM4w=,BwAQBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAGrCg1Q=,BwARBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAHFfeDQ=,BwASBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAHfvWvw=,BwATBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAH6Hmyw=,BwAUBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAIUR0gA=,BwAVBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAIuqxkQ=,BwAWBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAJJa6Og=,BwAXBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAJj6rCQ=,BwAYBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAJ/VGug=,BwAZBAAAAAAAAAAAAAAAAAAAAAAAAAcAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAKaUocg=,BwAaBAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAIAAAAAAAA
AAAAAK1bslg=,BwAbBAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAALRIqaQ=,BwAcBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAALrrUYA=,BwAdBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAMGPM9A=,BwAeBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAMhFiSw=,BwAfBAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAIAAAAAAAA
AAAAAM70eGQ=,BwAgBAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAANXJxXw=,BwAhBwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAANxncfQ=,BwAiBwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAOMOnUg=,BwAjBwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAOm9fOA=,BwAkBwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAPBpCng=,BwAlCAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAPcC1AQ=,BwAmCAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAP2ZFag=,BwAnCAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAQQouMQ=,BwAoCAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAQrM6fw=,BwApCQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAARFsZVg=,BwAqCQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAARgwKJg=,BwArCQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAR7FhuQ=,BwAsCQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAASVUh4A=,BwAtCQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAASyiu2A=,BwAuCwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAATM1mQw=,BwAvCwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAATnSBTQ=,BwAwCwAAAAAAAAAAAAAAAAAAAA==
AAAAAUB6M+g=,BwAxCwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAUcYixQ=,BwAyCwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAU2sqdg=,BwAzCgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAVRbbOw=,BwA0CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAVrp4Uw=,BwA1CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAWGDxvg=,BwA2CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAWgV6lA=,BwA3CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAEAAAAAAAA=
AAAAAW7AXkQ=,BwA4CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAXVuePw=,BwA5CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAXwTzyw=,BwA6CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAYKY6Wg=,BwA7CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAYkvgJg=,BwA8CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAY/wRdQ=,BwA9CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAZaFqyg=,BwA+CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAZ0sT7g=,BwA/CgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAaPEBsw=,BwBACgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAapaaaQ=,BwBBCgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAbD2duA=,BwBCAQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAbeYyMw=,BwBDAQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAb5KhNA=,BwBEAgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAcTqV0g=,BwBFAgAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAcuS3Xw=,BwBGDQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=
AAAAAdIkMcw=,BwBHDQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==
AAAAAdmFxXA=,BwBIAQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAD6QAAAAA=
!End!End of DIS capture file, Pdusave3.dislog.
!End!End of DIS capture file, Pdusave5.dislog.
......@@ -117,7 +117,8 @@ public class Compressor {
firstPoint.setY(yList.get(0));
firstPoint.setZ(zList.get(0));
Coordinates lastPoint = new Coordinates(xList.get(i), yList.get(i), zList.get(i));
//ToDo add the angles to the Coordinates
Coordinates lastPoint = new Coordinates(xList.get(i), yList.get(i), zList.get(i), 0.0, 0.0, 0.0);
returnMap.put(tList.get(0), firstPoint);
returnMap.put(tList.get(i), lastPoint);
......@@ -139,7 +140,8 @@ public class Compressor {
//grab the first and the last point from the regression test and push it to the returnMap
for (int j = 0; j < slidingWindow.size(); j++) {
Coordinates leftPoints = new Coordinates(xList.get(j), yList.get(j), zList.get(j));
//ToDo: Add the angles to the coordinates
Coordinates leftPoints = new Coordinates(xList.get(j), yList.get(j), zList.get(j), 0.0, 0.0, 0.0);
returnMap.put(tList.get(j), leftPoints);
......
......@@ -6,28 +6,62 @@
package PduStreamTools;
/**
* This class is a holder for coordinates and angles of ESPDUs to store them in
* HashMaps
*
* @author tobia
* @author Tobias Brennenstuhl
*/
public class Coordinates {
private double x;
private double y;
private double z;
private double phi;
private double psi;
private double theta;
public Coordinates(double x, double y, double z) {
public Coordinates(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 Coordinates() {
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;
......@@ -52,7 +86,5 @@ public class Coordinates {
public void setZ(double z) {
this.z = z;
}
}
......@@ -50,9 +50,9 @@ public class PduListenerSaver {
System.out.println("Beginning pdu save to directory " + outDir);
//RecorderPlainText stores an unencoded PduLog
RecorderPlainText recorder = new RecorderPlainText(outDir, mcast, port);
//RecorderPlainText recorder = new RecorderPlainText(outDir, mcast, port);
//RecorderBase64 stores a BASE64 encoded PduLog
//RecorderBase64 recorder = new RecorderBase64(outDir, mcast, port);
RecorderBase64 recorder = new RecorderBase64(outDir, mcast, port);
//RecorderBinary stores a Binary PduLog
//RecorderBinary recorder = new RecorderBinary(outDir, mcast, port);
......
......@@ -53,9 +53,9 @@ public class PduReaderPlayer {
try {
//Use Tobis Code without decryption
//Does only work with unecrypted PduLogs
PlayerPlainText player = new PlayerPlainText(mcast, port, new File(outDir).toPath());
//PlayerPlainText player = new PlayerPlainText(mcast, port, new File(outDir).toPath());
//Use Mike Baileys Code to store a file in BASE64 format
//PlayerBase64 player = new PlayerBase64(mcast, port, new File(outDir).toPath());
PlayerBase64 player = new PlayerBase64(mcast, port, new File(outDir).toPath());
player.startResume();
......
......@@ -79,11 +79,9 @@ public class PlayerPlainText {
Arrays.sort(fs, (f1, f2) -> {
return f1.getName().compareTo(f2.getName());
});
//Arrays.sort(fs, Comparator.comparing(File::getName));
dsock = new DatagramSocket();
Base64.Decoder decdr = Base64.getDecoder();
// -------------------- Begin Variables for Position Interpolator
Boolean firstTimeStamp = true;
int firstLocalTimeStamp = 0;
......@@ -91,6 +89,10 @@ public class PlayerPlainText {
double firstLocalX = 0;
double firstLocalY = 0;
double firstLocalZ = 0;
double firstLocalPhi = 0;
double firstLocalPsi = 0;
double firstLocalTheta = 0;
LinkedHashMap<Double, Coordinates> testMap = new LinkedHashMap<>();
......@@ -108,8 +110,7 @@ public class PlayerPlainText {
while (paused) {
sleep(500l); // half sec
}
if (line.length() <= 0)
; // blank lines ok
if (line.length() <= 0); // blank lines ok
else if (line.startsWith(COMMENT_MARKER)) {
if (handleComment(line, f)) {
// here if we got an end comment
......@@ -136,8 +137,6 @@ public class PlayerPlainText {
startNanoTime = System.nanoTime();
}
//byte[] pduTimeBytes = decdr.decode(sa[0]);
//byte[] pduTimeBytes = sa[0].getBytes();
byte[] pduTimeBytes = null;
String[] splitString;
......@@ -162,8 +161,7 @@ public class PlayerPlainText {
int tempInt = Integer.parseInt(tempString);
arr[x] = tempInt;
//System.out.println(tempInt);
}
// Credit: https://stackoverflow.com/questions/1086054/how-to-convert-int-to-byte
......@@ -216,9 +214,6 @@ public class PlayerPlainText {
intBuffer = byteBuffer2.asIntBuffer();
intBuffer.put(arr);
//Decoding is no longer needed
//byte[] buffer = decdr.decode(sa[1]);
//byte[] buffer = sa[1].getBytes();
byte[] buffer = byteBuffer2.array();
//When the byteBuffer stores the arry of Integers into the byte array it store a 7 as 0 0 0 7.
......@@ -260,6 +255,10 @@ public class PlayerPlainText {
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
......@@ -270,7 +269,11 @@ public class PlayerPlainText {
firstLocalX = localEspdu.getEntityLocation().getX();
firstLocalY = localEspdu.getEntityLocation().getY();
firstLocalZ = localEspdu.getEntityLocation().getZ();
// firstLocalPhi = localEspdu.getEntityOrientation().getPhi();
// firstLocalPsi = localEspdu.getEntityOrientation().getPsi();
// firstLocalTheta = localEspdu.getEntityOrientation().getTheta();
firstTimeStamp = false;
}
......@@ -278,24 +281,28 @@ public class PlayerPlainText {
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;
// localPhi = localPhi - firstLocalPhi;
// localPsi = localPsi - firstLocalPsi;
// localTheta = localTheta - firstLocalTheta;
//Divide TimeStamp by 1,000,000 to get something close to a second per Unit.
localTimeStamp = localTimeStamp / 1000000;
//Only add to stream if it is an ESPDU
//ToDo: Add support for multiple Entities
if ((localPdu.getPduType() != null) && (localPdu.getPduType() == DISPDUType.ENTITY_STATE)) {
//System.out.println("Timestamp: " + localTimeStamp);
//String outPutString = new String("Location: x: " + localX + " y: " + localY + " z: " + localZ);
//Put timestamps and coordinates in a Map to have them stored for a regression
testMap.put((double) localTimeStamp, new Coordinates(localX, localY, localZ));
//System.out.println(outPutString);
//String positionInterpolatorString = new String(" " + coordinateNumberFormat.format(localZ) + " " + coordinateNumberFormat.format(localY) + " " + coordinateNumberFormat.format(localX));
//tempKeyKeyValueSetPositionInterPolator.put(localTimeStamp, positionInterpolatorString);
testMap.put((double) localTimeStamp, new Coordinates(localX, localY, localZ, localPhi, localPsi, localTheta));
}
......@@ -314,22 +321,36 @@ public class PlayerPlainText {
}
brdr.close();
//Regression of the testMap
//Compression of the testMap.
//Remove all collinear points.
Compressor regression = new Compressor(testMap);
TreeMap<Double, Coordinates> returnMap = new TreeMap<>();
returnMap = regression.doCompression();
//returnMap.putAll(testMap);
//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 Interpolator");
System.out.println("Writing Position and Rotation Interpolator");
Set<Double> keys = returnMap.keySet();
//Set<Double> keys = tempKeyKeyValueSetPositionInterPolator.keySet();
String key = "key = '";
String keyValue = "keyValue = '";
String interpolatorToCopy = "<PositionInterpolator DEF='Entity' ";
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;
......@@ -344,23 +365,43 @@ public class PlayerPlainText {
}
//Normalize all times in the set
var keyKeyValueSetPositionInterPolator = new LinkedHashMap<Double, String>();
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);
keyKeyValueSetPositionInterpolator.put(k / lastTimeStamp, localCoordinateString);
keyKeyValueSetOrientationInterpolatorX.put(k / lastTimeStamp, localOrientationStringX);
keyKeyValueSetOrientationInterpolatorY.put(k / lastTimeStamp, localOrientationStringY);
keyKeyValueSetOrientationInterpolatorZ.put(k / lastTimeStamp, localOrientationStringZ);
}
keys = keyKeyValueSetPositionInterPolator.keySet();
keys = keyKeyValueSetPositionInterpolator.keySet();
//Setting up the timeSensor
//Only one timeSensor for both interpolators is needed
String timeSensor = "<TimeSensor DEF='PduStreamClock' cycleInterval='";
timeSensor += lastTimeStamp;
......@@ -370,21 +411,70 @@ public class PlayerPlainText {
//Printing the timeSensor to the console
System.out.println(timeSensor);
//Seting up the PositionInterpolator
//Setting up PositionInterpolator and OrientationInterpolator
for (Double k : keys) {
//System.out.println(k+" -- " + keyKeyValuePositionInterPolator.get(k));
key += coordinateNumberFormat.format(k) + " ";
keyValue += keyKeyValueSetPositionInterPolator.get(k) + " ";
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) + " ";
}
key += "' ";
keyValue += "' ";
interpolatorToCopy += key + "\n";
interpolatorToCopy += keyValue;
interpolatorToCopy += "/>";
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(interpolatorToCopy);
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);
}
} catch (Exception ex) {
System.err.println("Exception reading/writing pdus: " + ex.getClass().getSimpleName() + ": " + ex.getLocalizedMessage());
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment