diff --git a/src/main/java/dev/slimevr/VRServer.java b/src/main/java/dev/slimevr/VRServer.java index 21f7a58e49..e0855863fd 100644 --- a/src/main/java/dev/slimevr/VRServer.java +++ b/src/main/java/dev/slimevr/VRServer.java @@ -1,5 +1,6 @@ package dev.slimevr; +import dev.slimevr.autobone.AutoBoneHandler; import dev.slimevr.bridge.Bridge; import dev.slimevr.bridge.VMCBridge; import dev.slimevr.platform.windows.WindowsNamedPipeBridge; @@ -27,7 +28,6 @@ import java.util.*; import java.util.concurrent.LinkedBlockingQueue; import java.util.function.Consumer; -import dev.slimevr.autobone.AutoBoneHandler; public class VRServer extends Thread { @@ -297,11 +297,9 @@ public void updateTrackersFilters(TrackerFilters filter, float amount, int ticks IMUTracker imu; for (Tracker t : this.getAllTrackers()) { - Tracker realTracker = t; - if (t instanceof ReferenceAdjustedTracker) - realTracker = ((ReferenceAdjustedTracker) t).getTracker(); - if (realTracker instanceof IMUTracker) { - imu = (IMUTracker) realTracker; + Tracker tracker = t.get(); + if (tracker instanceof IMUTracker) { + imu = (IMUTracker) tracker; imu.setFilter(filter.name(), amount, ticks); } } diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index a76220bc9c..08e1c8e290 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -43,13 +43,12 @@ public TrackersList(VRServer server, VRServerGUI gui) { } private static int getTrackerSort(Tracker t) { - if (t instanceof ReferenceAdjustedTracker) - t = ((ReferenceAdjustedTracker) t).getTracker(); - if (t instanceof IMUTracker) + Tracker tracker = t.get(); + if (tracker instanceof IMUTracker) return 0; - if (t instanceof HMDTracker) + if (tracker instanceof HMDTracker) return 100; - if (t instanceof ComputedTracker) + if (tracker instanceof ComputedTracker) return 200; return 1000; } @@ -72,9 +71,7 @@ private void build() { boolean first = true; for (TrackerPanel tr : trackers) { - Tracker t = tr.t; - if (t instanceof ReferenceAdjustedTracker) - t = ((ReferenceAdjustedTracker) t).getTracker(); + Tracker t = tr.t.get(); if (currentClass != t.getClass()) { currentClass = t.getClass(); if (line != null) @@ -162,9 +159,7 @@ public TrackerPanel(Tracker t) { public TrackerPanel build() { int row = 0; - Tracker realTracker = t; - if (t instanceof ReferenceAdjustedTracker) - realTracker = ((ReferenceAdjustedTracker) t).getTracker(); + Tracker tracker = t.get(); removeAll(); JLabel nameLabel; add( @@ -206,8 +201,8 @@ public void actionPerformed(ActionEvent e) { server.trackerUpdated(t); } }); - if (realTracker instanceof IMUTracker) { - IMUTracker imu = (IMUTracker) realTracker; + if (tracker instanceof IMUTracker) { + IMUTracker imu = (IMUTracker) tracker; JComboBox mountSelect; add( mountSelect = new JComboBox<>(), @@ -242,7 +237,7 @@ public void actionPerformed(ActionEvent e) { if (t.hasPosition()) add(new JLabel("Position"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); add(new JLabel("TPS"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); - if (realTracker instanceof IMUTracker) { + if (tracker instanceof IMUTracker) { add(new JLabel("Ping"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add(new JLabel("Signal"), c(4, row, 2, GridBagConstraints.FIRST_LINE_START)); } @@ -257,14 +252,14 @@ public void actionPerformed(ActionEvent e) { position = new JLabel("0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START) ); - if (realTracker instanceof IMUTracker) { + if (tracker instanceof IMUTracker) { add(ping = new JLabel(""), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add( signalStrength = new JLabel(""), c(4, row, 2, GridBagConstraints.FIRST_LINE_START) ); } - if (realTracker instanceof TrackerWithTPS) { + if (tracker instanceof TrackerWithTPS) { add(tps = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); } else { add(new JLabel(""), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); @@ -275,7 +270,7 @@ public void actionPerformed(ActionEvent e) { status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, row, 2, GridBagConstraints.FIRST_LINE_START) ); - if (realTracker instanceof TrackerWithBattery) { + if (tracker instanceof TrackerWithBattery) { add(new JLabel("Battery:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add(bat = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); } @@ -286,13 +281,13 @@ public void actionPerformed(ActionEvent e) { s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1) ); - if (debug && realTracker instanceof IMUTracker) { + if (debug && tracker instanceof IMUTracker) { add(new JLabel("Quat:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add(rotQuat = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); } row++; - if (debug && realTracker instanceof IMUTracker) { + if (debug && tracker instanceof IMUTracker) { add(new JLabel("Raw mag:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add( rawMag = new JLabel("0 0 0"), @@ -300,7 +295,7 @@ public void actionPerformed(ActionEvent e) { ); add(new JLabel("Gyro fix:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add( - new JLabel(String.format("0x%8x", realTracker.hashCode())), + new JLabel(String.format("0x%8x", tracker.hashCode())), s(c(3, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1) ); row++; @@ -356,9 +351,7 @@ public void actionPerformed(ActionEvent e) { public void update() { if (position == null && rotation == null) return; - Tracker realTracker = t; - if (t instanceof ReferenceAdjustedTracker) - realTracker = ((ReferenceAdjustedTracker) t).getTracker(); + Tracker tracker = t.get(); t.getRotation(q); t.getPosition(v); q.toAngles(angles); @@ -383,11 +376,11 @@ public void update() { ); status.setText(t.getStatus().toString().toLowerCase()); - if (realTracker instanceof TrackerWithTPS) { - tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1)); + if (tracker instanceof TrackerWithTPS) { + tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) tracker).getTPS(), 1)); } - if (realTracker instanceof TrackerWithBattery) { - TrackerWithBattery twb = (TrackerWithBattery) realTracker; + if (tracker instanceof TrackerWithBattery) { + TrackerWithBattery twb = (TrackerWithBattery) tracker; float level = twb.getBatteryLevel(); float voltage = twb.getBatteryVoltage(); if (level == 0.0f) { @@ -442,11 +435,11 @@ public void update() { ); } } - if (realTracker instanceof IMUTracker) { + if (tracker instanceof IMUTracker) { if (ping != null) - ping.setText(String.valueOf(((IMUTracker) realTracker).ping)); + ping.setText(String.valueOf(((IMUTracker) tracker).ping)); if (signalStrength != null) { - int signal = ((IMUTracker) realTracker).signalStrength; + int signal = ((IMUTracker) tracker).signalStrength; if (signal == -1) { signalStrength.setText("N/A"); } else { @@ -457,7 +450,7 @@ public void update() { } } } - realTracker.getRotation(q); + tracker.getRotation(q); q.toAngles(angles); raw .setText( @@ -467,8 +460,8 @@ public void update() { + " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0) ); - if (realTracker instanceof IMUTracker) { - IMUTracker imu = (IMUTracker) realTracker; + if (tracker instanceof IMUTracker) { + IMUTracker imu = (IMUTracker) tracker; if (rawMag != null) { imu.rotMagQuaternion.toAngles(angles); rawMag diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java index eb84b03c8c..10b79265be 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java @@ -261,4 +261,9 @@ public int getTrackerNum() { public UDPDevice getDevice() { return null; } + + @Override + public Tracker get() { + return this; + } } diff --git a/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java b/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java index afcaca9e6f..49eb3ed683 100644 --- a/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java +++ b/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java @@ -192,4 +192,9 @@ public int getTrackerNum() { public UDPDevice getDevice() { return null; } + + @Override + public Tracker get() { + return null; + } } diff --git a/src/main/java/dev/slimevr/protocol/RPCHandler.java b/src/main/java/dev/slimevr/protocol/RPCHandler.java index 73f9b6e585..4edb072c19 100644 --- a/src/main/java/dev/slimevr/protocol/RPCHandler.java +++ b/src/main/java/dev/slimevr/protocol/RPCHandler.java @@ -195,23 +195,22 @@ public void onAssignTrackerRequest(GenericConnection conn, RpcMessageHeader mess Tracker tracker = this.api.server.getTrackerById(req.trackerId().unpack()); if (tracker == null) return; - - TrackerPosition.getByBodyPart(req.bodyPosition()).ifPresent(tracker::setBodyPosition); - - if (tracker instanceof ReferenceAdjustedTracker) { - ReferenceAdjustedTracker refTracker = (ReferenceAdjustedTracker) tracker; - if (refTracker.getTracker() instanceof IMUTracker) { - IMUTracker imu = (IMUTracker) refTracker.getTracker(); - imu - .setMountingRotation( - new Quaternion( - req.mountingRotation().x(), - req.mountingRotation().y(), - req.mountingRotation().z(), - req.mountingRotation().w() - ) - ); - } + tracker = tracker.get(); + + TrackerPosition pos = TrackerPosition.getByBodyPart(req.bodyPosition()).orElse(null); + tracker.setBodyPosition(pos); + + if (tracker instanceof IMUTracker) { + IMUTracker imu = (IMUTracker) tracker; + imu + .setMountingRotation( + new Quaternion( + req.mountingRotation().x(), + req.mountingRotation().y(), + req.mountingRotation().z(), + req.mountingRotation().w() + ) + ); } this.api.server.trackerUpdated(tracker); } diff --git a/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java b/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java index 579d2a2a53..82b8df91f9 100644 --- a/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java +++ b/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java @@ -43,4 +43,9 @@ public TrackerRole getTrackerRole() { public UDPDevice getDevice() { return null; } + + @Override + public Tracker get() { + return this; + } } diff --git a/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java b/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java index 9a6ab345c7..4027d84478 100644 --- a/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java @@ -153,4 +153,9 @@ public int getTrackerNum() { public UDPDevice getDevice() { return null; } + + @Override + public Tracker get() { + return this; + } } diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index 9aa114b4f2..6c8e7146ef 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -310,6 +310,11 @@ public String getDescriptiveName() { return this.descriptiveName; } + @Override + public Tracker get() { + return this; + } + public enum CalibrationAccuracy { UNRELIABLE(0), LOW(1), MEDIUM(2), HIGH(3),; diff --git a/src/main/java/dev/slimevr/vr/trackers/MPUTracker.java b/src/main/java/dev/slimevr/vr/trackers/MPUTracker.java deleted file mode 100644 index 8b1b0dedb2..0000000000 --- a/src/main/java/dev/slimevr/vr/trackers/MPUTracker.java +++ /dev/null @@ -1,165 +0,0 @@ -package dev.slimevr.vr.trackers; - -import dev.slimevr.VRServer; -import dev.slimevr.vr.trackers.udp.UDPDevice; -import dev.slimevr.vr.trackers.udp.TrackersUDPServer; - -import java.nio.ByteBuffer; - - -public class MPUTracker extends IMUTracker { - - public ConfigurationData newCalibrationData; - - public MPUTracker( - UDPDevice device, - int trackerId, - int trackerNum, - String name, - String descriptiveName, - TrackersUDPServer server, - VRServer vrserver - ) { - super(device, trackerId, trackerNum, name, descriptiveName, server, vrserver); - } - - public static class ConfigurationData { - - // accel offsets and correction matrix - float[] A_B = new float[3]; - float[][] A_Ainv = new float[3][3]; - // mag offsets and correction matrix - float[] M_B = new float[3]; - float[][] M_Ainv = new float[3][3]; - // raw offsets, determined for gyro at rest - float[] G_off = new float[3]; - int deviceId = -1; - int deviceMode = -1; - - public ConfigurationData( - double[] accelBasis, - double[] accelAInv, - double[] magBasis, - double[] magAInv, - double[] gyroOffset - ) { - A_B[0] = (float) accelBasis[0]; - A_B[1] = (float) accelBasis[1]; - A_B[2] = (float) accelBasis[2]; - - A_Ainv[0][0] = (float) accelAInv[0]; - A_Ainv[0][1] = (float) accelAInv[1]; - A_Ainv[0][2] = (float) accelAInv[2]; - A_Ainv[1][0] = (float) accelAInv[3]; - A_Ainv[1][1] = (float) accelAInv[4]; - A_Ainv[1][2] = (float) accelAInv[5]; - A_Ainv[2][0] = (float) accelAInv[6]; - A_Ainv[2][1] = (float) accelAInv[7]; - A_Ainv[2][2] = (float) accelAInv[8]; - - M_B[0] = (float) magBasis[0]; - M_B[1] = (float) magBasis[1]; - M_B[2] = (float) magBasis[2]; - - M_Ainv[0][0] = (float) magAInv[0]; - M_Ainv[0][1] = (float) magAInv[1]; - M_Ainv[0][2] = (float) magAInv[2]; - M_Ainv[1][0] = (float) magAInv[3]; - M_Ainv[1][1] = (float) magAInv[4]; - M_Ainv[1][2] = (float) magAInv[5]; - M_Ainv[2][0] = (float) magAInv[6]; - M_Ainv[2][1] = (float) magAInv[7]; - M_Ainv[2][2] = (float) magAInv[8]; - - G_off[0] = (float) gyroOffset[0]; - G_off[1] = (float) gyroOffset[1]; - G_off[2] = (float) gyroOffset[2]; - } - - public ConfigurationData(ByteBuffer buffer) { - deviceMode = buffer.getInt(); - deviceId = buffer.getInt(); - // Data is read in reverse, because it was reversed when sending - G_off[2] = buffer.getFloat(); - G_off[1] = buffer.getFloat(); - G_off[0] = buffer.getFloat(); - - M_Ainv[2][2] = buffer.getFloat(); - M_Ainv[2][1] = buffer.getFloat(); - M_Ainv[2][0] = buffer.getFloat(); - M_Ainv[1][2] = buffer.getFloat(); - M_Ainv[1][1] = buffer.getFloat(); - M_Ainv[1][0] = buffer.getFloat(); - M_Ainv[0][2] = buffer.getFloat(); - M_Ainv[0][1] = buffer.getFloat(); - M_Ainv[0][0] = buffer.getFloat(); - - M_B[2] = buffer.getFloat(); - M_B[1] = buffer.getFloat(); - M_B[0] = buffer.getFloat(); - - A_Ainv[2][2] = buffer.getFloat(); - A_Ainv[2][1] = buffer.getFloat(); - A_Ainv[2][0] = buffer.getFloat(); - A_Ainv[1][2] = buffer.getFloat(); - A_Ainv[1][1] = buffer.getFloat(); - A_Ainv[1][0] = buffer.getFloat(); - A_Ainv[0][2] = buffer.getFloat(); - A_Ainv[0][1] = buffer.getFloat(); - A_Ainv[0][0] = buffer.getFloat(); - - A_B[2] = buffer.getFloat(); - A_B[1] = buffer.getFloat(); - A_B[0] = buffer.getFloat(); - } - - public String toTextMatrix() { - StringBuilder sb = new StringBuilder(); - sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", A_B[0], A_B[1], A_B[2])); - sb - .append( - String - .format("{{%9.5f,%9.5f,%9.5f},\n", A_Ainv[0][0], A_Ainv[0][1], A_Ainv[0][2]) - ); - sb - .append( - String - .format(" {%9.5f,%9.5f,%9.5f},\n", A_Ainv[1][0], A_Ainv[1][1], A_Ainv[1][2]) - ); - sb - .append( - String - .format( - " {%9.5f,%9.5f,%9.5f}},\n", - A_Ainv[2][0], - A_Ainv[2][1], - A_Ainv[2][2] - ) - ); - sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", M_B[0], M_B[1], M_B[2])); - sb - .append( - String - .format("{{%9.5f,%9.5f,%9.5f},\n", M_Ainv[0][0], M_Ainv[0][1], M_Ainv[0][2]) - ); - sb - .append( - String - .format(" {%9.5f,%9.5f,%9.5f},\n", M_Ainv[1][0], M_Ainv[1][1], M_Ainv[1][2]) - ); - sb - .append( - String - .format( - " {%9.5f,%9.5f,%9.5f}},\n", - M_Ainv[2][0], - M_Ainv[2][1], - M_Ainv[2][2] - ) - ); - sb.append(String.format("{%8.2f, %8.2f, %8.2f}};\n", G_off[0], G_off[1], G_off[2])); - - return sb.toString(); - } - } -} diff --git a/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java b/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java index 042303eca8..7576b1f59d 100644 --- a/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java @@ -16,10 +16,6 @@ public ReferenceAdjustedTracker(E tracker) { this.tracker = tracker; } - public E getTracker() { - return this.tracker; - } - @Override public boolean userEditable() { return this.tracker.userEditable(); @@ -179,4 +175,9 @@ public String getDescriptiveName() { public Device getDevice() { return tracker.getDevice(); } + + @Override + public Tracker get() { + return this.tracker; + } } diff --git a/src/main/java/dev/slimevr/vr/trackers/Tracker.java b/src/main/java/dev/slimevr/vr/trackers/Tracker.java index 24a2424ac0..7921ea348c 100644 --- a/src/main/java/dev/slimevr/vr/trackers/Tracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/Tracker.java @@ -52,6 +52,8 @@ static int getNextLocalTrackerId() { Device getDevice(); + Tracker get(); + default String getDescriptiveName() { return getName(); }