diff --git a/src/main/java/io/eiren/gui/TrackersList.java b/src/main/java/io/eiren/gui/TrackersList.java index 474e44cfe0..6458d9171a 100644 --- a/src/main/java/io/eiren/gui/TrackersList.java +++ b/src/main/java/io/eiren/gui/TrackersList.java @@ -102,6 +102,8 @@ private class TrackerRow extends EJBag { JLabel bat; JLabel ping; JLabel raw; + JLabel adj; + JLabel adjYaw; @AWTThread public TrackerRow(Tracker t) { @@ -178,6 +180,13 @@ public void actionPerformed(ActionEvent e) { add(bat = new JLabel("0"), c(3, 4, 0, GridBagConstraints.FIRST_LINE_START)); add(new JLabel("Raw:"), c(0, 5, 0, GridBagConstraints.FIRST_LINE_START)); add(raw = new JLabel("0 0 0 0"), s(c(1, 5, 0, GridBagConstraints.FIRST_LINE_START), 3, 1)); + + if(t instanceof ReferenceAdjustedTracker) { + add(new JLabel("Adj:"), c(0, 6, 0, GridBagConstraints.FIRST_LINE_START)); + add(adj = new JLabel("0 0 0 0"), c(1, 6, 0, GridBagConstraints.FIRST_LINE_START)); + add(new JLabel("AdjY:"), c(2, 6, 0, GridBagConstraints.FIRST_LINE_START)); + add(adjYaw = new JLabel("0 0 0 0"), c(3, 6, 0, GridBagConstraints.FIRST_LINE_START)); + } setBorder(BorderFactory.createLineBorder(new Color(0x663399), 4, true)); TrackersList.this.add(this); @@ -207,8 +216,17 @@ public void update() { if(t instanceof TrackerWithBattery) bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) t).getBatteryVoltage(), 1)); Tracker t2 = t; - if(t instanceof ReferenceAdjustedTracker) + if(t instanceof ReferenceAdjustedTracker) { t2 = ((ReferenceAdjustedTracker) t).getTracker(); + ((ReferenceAdjustedTracker) t).adjustmentAttachment.toAngles(angles); + adj.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0) + + " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0) + + " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); + ((ReferenceAdjustedTracker) t).adjustmentYaw.toAngles(angles); + adjYaw.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0) + + " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0) + + " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); + } if(t2 instanceof IMUTracker) ping.setText(String.valueOf(((IMUTracker) t2).ping)); t2.getRotation(q); diff --git a/src/main/java/io/eiren/gui/VRServerGUI.java b/src/main/java/io/eiren/gui/VRServerGUI.java index 6d9bb9d3b3..7f516c1d83 100644 --- a/src/main/java/io/eiren/gui/VRServerGUI.java +++ b/src/main/java/io/eiren/gui/VRServerGUI.java @@ -6,6 +6,7 @@ import io.eiren.util.StringUtils; import io.eiren.util.ann.AWTThread; +import io.eiren.vr.Main; import io.eiren.vr.VRServer; import io.eiren.vr.bridge.NamedPipeVRBridge; @@ -31,7 +32,7 @@ public class VRServerGUI extends JFrame { @AWTThread public VRServerGUI(VRServer server) { - super("SlimeVR Server"); + super("SlimeVR Server (" + Main.VERSION + ")"); //increaseFontSize(); this.server = server; diff --git a/src/main/java/io/eiren/vr/Main.java b/src/main/java/io/eiren/vr/Main.java index f956bacdac..f0ecee24d0 100644 --- a/src/main/java/io/eiren/vr/Main.java +++ b/src/main/java/io/eiren/vr/Main.java @@ -8,7 +8,10 @@ public class Main { - @SuppressWarnings("unused") + public static String VERSION = "0.0.7 DEV 1"; + + public static VRServer vrServer; + public static void main(String[] args) { System.setProperty("awt.useSystemAAFontSettings", "on"); System.setProperty("swing.aatext", "true"); @@ -21,7 +24,7 @@ public static void main(String[] args) { } try { - VRServer vrServer = new VRServer(); + vrServer = new VRServer(); vrServer.start(); new VRServerGUI(vrServer); } catch(Throwable e) { diff --git a/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java b/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java index 2d677b3465..c8b8d32a15 100644 --- a/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java +++ b/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java @@ -46,9 +46,9 @@ public void saveConfig(TrackerConfig config) { public void resetFull(Quaternion reference) { Quaternion sensorRotation = new Quaternion(); tracker.getRotation(sensorRotation); - float[] angles = new float[3]; - sensorRotation.toAngles(angles); - sensorRotation.fromAngles(angles[0], 0, angles[2]); + //float[] angles = new float[3]; + //sensorRotation.toAngles(angles); + //sensorRotation.fromAngles(angles[0], 0, angles[2]); adjustmentAttachment.set(sensorRotation).inverseLocal(); resetYaw(reference); @@ -71,7 +71,8 @@ public void resetYaw(Quaternion reference) { Quaternion sensorRotation = new Quaternion(); tracker.getRotation(sensorRotation); - sensorRotation.multLocal(adjustmentAttachment); + adjustmentAttachment.mult(sensorRotation, sensorRotation); + //sensorRotation.multLocal(adjustmentAttachment); sensorRotation.toAngles(angles); sensorRotation.fromAngles(0, angles[1], 0); @@ -82,7 +83,8 @@ public void resetYaw(Quaternion reference) { } protected void adjustInternal(Quaternion store) { - store.multLocal(adjustmentAttachment); + //store.multLocal(adjustmentAttachment); + adjustmentAttachment.mult(store, store); adjustmentYaw.mult(store, store); } diff --git a/src/test/java/io/eiren/unit/ReferenceAdjustmentsFullTests.java b/src/test/java/io/eiren/unit/ReferenceAdjustmentsFullTests.java new file mode 100644 index 0000000000..d06a8c167a --- /dev/null +++ b/src/test/java/io/eiren/unit/ReferenceAdjustmentsFullTests.java @@ -0,0 +1,257 @@ +package io.eiren.unit; + +import com.jme3.math.FastMath; +import com.jme3.math.Quaternion; + +import io.eiren.math.FloatMath; +import io.eiren.util.StringUtils; +import io.eiren.vr.processor.TransformNode; +import io.eiren.vr.trackers.ComputedTracker; +import io.eiren.vr.trackers.ReferenceAdjustedTracker; + +import static org.junit.Assert.*; + +import java.util.HashSet; +import java.util.Set; + +import org.junit.Test; + +/** + * Tests {@link ReferenceAdjustedTracker#resetFull(Quaternion)} + */ +public class ReferenceAdjustmentsFullTests { + + private Set testedTrackerNames = new HashSet<>(); + + @Test + public void check0to0() { + yawTest(0, 0); + } + + @Test + public void check45to0() { + yawTest(0, 45); + } + + @Test + public void check90to0() { + yawTest(0, 90); + } + + @Test + public void check180to0() { + yawTest(0, 180); + } + + @Test + public void check270to0() { + yawTest(0, 270); + } + + @Test + public void check0to45() { + yawTest(45, 0); + } + + @Test + public void check45to45() { + yawTest(45, 45); + } + + @Test + public void check90to45() { + yawTest(45, 90); + } + + @Test + public void check180to45() { + yawTest(45, 180); + } + + @Test + public void check270to45() { + yawTest(45, 270); + } + + @Test + public void check0to90() { + yawTest(90, 0); + } + + @Test + public void check45to90() { + yawTest(90, 45); + } + + @Test + public void check90to90() { + yawTest(90, 90); + } + + @Test + public void check180to90() { + yawTest(90, 180); + } + + @Test + public void check270to90() { + yawTest(90, 270); + } + + @Test + public void check0to180() { + yawTest(180, 0); + } + + @Test + public void check45to180() { + yawTest(180, 45); + } + + @Test + public void check90to180() { + yawTest(180, 90); + } + + @Test + public void check180to180() { + yawTest(180, 180); + } + + @Test + public void check270to180() { + yawTest(180, 270); + } + + private void yawTest(int refYaw, int trackerYaw) { + checkReferenceAdjustmentFull(q(0, refYaw, 0), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")"); + checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")"); + checkReferenceAdjustmentFull(q(15, refYaw, 0), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")"); + checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")"); + checkReferenceAdjustmentFull(q(0, refYaw, 0), q(15, trackerYaw, 0), refYaw, "Tracker(15," + trackerYaw + ",0/" + refYaw + ")"); + checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerYaw, 15), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")"); + checkReferenceAdjustmentFull(q(15, refYaw, 0), q(15, trackerYaw, 15), refYaw, "Tracker(15," + trackerYaw + ",0/" + refYaw + ")"); + checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerYaw, 15), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")"); + } + + public void checkReferenceAdjustmentFull(Quaternion referenceQuat, Quaternion trackerQuat, int refYaw, String name) { + ComputedTracker tracker = new ComputedTracker("test"); + tracker.rotation.set(trackerQuat); + ReferenceAdjustedTracker adj = new ReferenceAdjustedTracker<>(tracker); + adj.resetFull(referenceQuat); + Quaternion read = new Quaternion(); + assertTrue("Adjusted tracker didn't return rotation", adj.getRotation(read)); + + // Use only yaw HMD rotation + Quaternion targetTrackerRotation = new Quaternion(referenceQuat); + float[] angles = new float[3]; + targetTrackerRotation.toAngles(angles); + targetTrackerRotation.fromAngles(0, angles[1], 0); + + assertEquals("Adjusted quat is not equal to reference quat (" + toDegs(targetTrackerRotation) + " vs " + toDegs(read) + ")", new QuatEqualFullWithEpsilon(targetTrackerRotation), new QuatEqualFullWithEpsilon(read)); + if(refYaw == 0) + testAdjustedTracker(tracker, adj, name, refYaw); + } + + //private static int errors = 0; + //private static int successes = 0; + + private void testAdjustedTracker(ComputedTracker tracker, ReferenceAdjustedTracker adj, String name, int refYaw) { + if(!testedTrackerNames.add(name)) + return; + + final Quaternion trackerBase = new Quaternion(); + trackerBase.set(tracker.rotation); + + Quaternion rotation = new Quaternion(); + Quaternion read = new Quaternion(); + Quaternion diff = new Quaternion(); + float[] angles = new float[3]; + float[] anglesAdj = new float[3]; + float[] anglesDiff = new float[3]; + + TransformNode trackerNode = new TransformNode(name, true); + TransformNode rotationNode = new TransformNode("Rot", true); + trackerNode.attachChild(rotationNode); + + trackerNode.localTransform.setRotation(trackerBase); + + for(int yaw = 0; yaw <= 360; yaw += 90) { + for(int pitch = -90; pitch <= 90; pitch += 30) { + for(int roll = -90; roll <= 90; roll += 30) { + rotation.fromAngles(pitch, yaw, roll); + rotationNode.localTransform.setRotation(rotation); + trackerNode.update(); + rotationNode.update(); + tracker.rotation.set(rotationNode.worldTransform.getRotation()); + tracker.rotation.toAngles(angles); + + adj.getRotation(read); + read.toAngles(anglesAdj); + + diff.set(read).inverseLocal().multLocal(rotation); + + diff.toAngles(anglesDiff); + assertTrue(name + ". Rot: " + yaw + "/" + pitch + ". " + + "Angles: " + StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[0] * FastMath.RAD_TO_DEG, 1) + ", " + + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[1] * FastMath.RAD_TO_DEG, 1) + ", " + + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[2] * FastMath.RAD_TO_DEG, 1) + ". Diff: " + + StringUtils.prettyNumber(anglesDiff[0] * FastMath.RAD_TO_DEG, 1) + ", " + + StringUtils.prettyNumber(anglesDiff[1] * FastMath.RAD_TO_DEG, 1) + ", " + + StringUtils.prettyNumber(anglesDiff[2] * FastMath.RAD_TO_DEG, 1), + FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2])); + } + } + } + //System.out.println("Errors: " + errors + ", successes: " + successes); + } + + public static String toDegs(Quaternion q) { + float[] degs = new float[3]; + q.toAngles(degs); + return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0); + } + + public static Quaternion q(float pitch, float yaw, float roll) { + return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD); + } + + public static class QuatEqualFullWithEpsilon { + + private final Quaternion q; + + public QuatEqualFullWithEpsilon(Quaternion q) { + this.q = q; + } + + @Override + public String toString() { + return String.valueOf(q); + } + + @Override + public int hashCode() { + return q.hashCode(); + } + + @Override + public boolean equals(Object obj) { + if(obj instanceof Quaternion) + obj = new QuatEqualFullWithEpsilon((Quaternion) obj); + if(!(obj instanceof QuatEqualFullWithEpsilon)) + return false; + Quaternion q2 = ((QuatEqualFullWithEpsilon) obj).q; + float[] degs1 = new float[3]; + q.toAngles(degs1); + float[] degs2 = new float[3]; + q2.toAngles(degs2); + if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD) + degs1[1] += FastMath.TWO_PI; + if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD) + degs2[1] += FastMath.TWO_PI; + return FloatMath.equalsWithEpsilon(degs1[0], degs2[0]) + && FloatMath.equalsWithEpsilon(degs1[1], degs2[1]) + && FloatMath.equalsWithEpsilon(degs1[2], degs2[2]); + } + } +} diff --git a/src/test/java/io/eiren/unity/ReferenceAdjustmentsYawTests.java b/src/test/java/io/eiren/unit/ReferenceAdjustmentsYawTests.java similarity index 99% rename from src/test/java/io/eiren/unity/ReferenceAdjustmentsYawTests.java rename to src/test/java/io/eiren/unit/ReferenceAdjustmentsYawTests.java index 51c6f6d959..30c266e16e 100644 --- a/src/test/java/io/eiren/unity/ReferenceAdjustmentsYawTests.java +++ b/src/test/java/io/eiren/unit/ReferenceAdjustmentsYawTests.java @@ -1,4 +1,4 @@ -package io.eiren.unity; +package io.eiren.unit; import com.jme3.math.FastMath; import com.jme3.math.Quaternion; diff --git a/src/test/java/io/eiren/unity/ReferenceAdjustmentsFullTests.java b/src/test/java/io/eiren/unity/ReferenceAdjustmentsFullTests.java deleted file mode 100644 index abb0fe5304..0000000000 --- a/src/test/java/io/eiren/unity/ReferenceAdjustmentsFullTests.java +++ /dev/null @@ -1,196 +0,0 @@ -package io.eiren.unity; - -import com.jme3.math.FastMath; -import com.jme3.math.Quaternion; - -import io.eiren.math.FloatMath; -import io.eiren.util.StringUtils; -import io.eiren.vr.trackers.ComputedTracker; -import io.eiren.vr.trackers.ReferenceAdjustedTracker; - -import static org.junit.Assert.*; - -import org.junit.Test; - -/** - * Tests {@link ReferenceAdjustedTracker#resetFull(Quaternion)} - */ -public class ReferenceAdjustmentsFullTests { - - @Test - public void check0to0() { - yawTest(0, 0); - } - - @Test - public void check45to0() { - yawTest(0, 45); - } - - @Test - public void check90to0() { - yawTest(0, 90); - } - - @Test - public void check180to0() { - yawTest(0, 180); - } - - @Test - public void check270to0() { - yawTest(0, 270); - } - - @Test - public void check0to45() { - yawTest(45, 0); - } - - @Test - public void check45to45() { - yawTest(45, 45); - } - - @Test - public void check90to45() { - yawTest(45, 90); - } - - @Test - public void check180to45() { - yawTest(45, 180); - } - - @Test - public void check270to45() { - yawTest(45, 270); - } - - @Test - public void check0to90() { - yawTest(90, 0); - } - - @Test - public void check45to90() { - yawTest(90, 45); - } - - @Test - public void check90to90() { - yawTest(90, 90); - } - - @Test - public void check180to90() { - yawTest(90, 180); - } - - @Test - public void check270to90() { - yawTest(90, 270); - } - - @Test - public void check0to180() { - yawTest(180, 0); - } - - @Test - public void check45to180() { - yawTest(180, 45); - } - - @Test - public void check90to180() { - yawTest(180, 90); - } - - @Test - public void check180to180() { - yawTest(180, 180); - } - - @Test - public void check270to180() { - yawTest(180, 270); - } - - private void yawTest(float refYaw, float trackerTaw) { - checkReferenceAdjustmentFull(q(0, refYaw, 0), q(0, trackerTaw, 0)); - checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerTaw, 0)); - checkReferenceAdjustmentFull(q(15, refYaw, 0), q(0, trackerTaw, 0)); - checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerTaw, 0)); - checkReferenceAdjustmentFull(q(0, refYaw, 0), q(15, trackerTaw, 0)); - checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerTaw, 15)); - checkReferenceAdjustmentFull(q(15, refYaw, 0), q(15, trackerTaw, 15)); - checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerTaw, 15)); - } - - public static void checkReferenceAdjustmentFull(Quaternion referenceQuat, Quaternion trackerQuat) { - ComputedTracker tracker = new ComputedTracker("test"); - tracker.rotation.set(trackerQuat); - ReferenceAdjustedTracker adj = new ReferenceAdjustedTracker(tracker); - adj.resetFull(referenceQuat); - Quaternion read = new Quaternion(); - assertTrue("Adjusted tracker didn't return rotation", adj.getRotation(read)); - - // Use only yaw HMD rotation - Quaternion targetTrackerRotation = new Quaternion(referenceQuat); - float[] angles = new float[3]; - targetTrackerRotation.toAngles(angles); - targetTrackerRotation.fromAngles(0, angles[1], 0); - - assertEquals("Adjusted quat is not equal to reference quat (" + toDegs(targetTrackerRotation) + " vs " + toDegs(read) + ")", new QuatEqualFullWithEpsilon(targetTrackerRotation), new QuatEqualFullWithEpsilon(read)); - } - - public static String toDegs(Quaternion q) { - float[] degs = new float[3]; - q.toAngles(degs); - return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0); - } - - public static Quaternion q(float pitch, float yaw, float roll) { - return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD); - } - - public static class QuatEqualFullWithEpsilon { - - private final Quaternion q; - - public QuatEqualFullWithEpsilon(Quaternion q) { - this.q = q; - } - - @Override - public String toString() { - return String.valueOf(q); - } - - @Override - public int hashCode() { - return q.hashCode(); - } - - @Override - public boolean equals(Object obj) { - if(obj instanceof Quaternion) - obj = new QuatEqualFullWithEpsilon((Quaternion) obj); - if(!(obj instanceof QuatEqualFullWithEpsilon)) - return false; - Quaternion q2 = ((QuatEqualFullWithEpsilon) obj).q; - float[] degs1 = new float[3]; - q.toAngles(degs1); - float[] degs2 = new float[3]; - q2.toAngles(degs2); - if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD) - degs1[1] += FastMath.TWO_PI; - if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD) - degs2[1] += FastMath.TWO_PI; - return FloatMath.equalsWithEpsilon(degs1[0], degs2[0]) - && FloatMath.equalsWithEpsilon(degs1[1], degs2[1]) - && FloatMath.equalsWithEpsilon(degs1[2], degs2[2]); - } - } -}