diff --git a/resources/firewall_uninstall.bat b/resources/firewall_uninstall.bat index 115eb1d6a..678450e86 100644 --- a/resources/firewall_uninstall.bat +++ b/resources/firewall_uninstall.bat @@ -1,5 +1,5 @@ @echo off -echo Installing firewall rules... +echo Uninstalling firewall rules... rem Discovery defauly port netsh advfirewall firewall delete rule name="SlimeVR UDP 35903 incoming" diff --git a/src/main/java/dev/slimevr/autobone/AutoBone.java b/src/main/java/dev/slimevr/autobone/AutoBone.java index d10809eb9..18a43aeb2 100644 --- a/src/main/java/dev/slimevr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/autobone/AutoBone.java @@ -15,14 +15,13 @@ import dev.slimevr.poserecorder.PoseFrameTracker; import dev.slimevr.poserecorder.PoseFrames; import dev.slimevr.poserecorder.TrackerFrame; import dev.slimevr.poserecorder.TrackerFrameData; +import dev.slimevr.vr.processor.HumanPoseProcessor; import dev.slimevr.vr.processor.skeleton.HumanSkeleton; -import dev.slimevr.vr.processor.skeleton.SimpleSkeleton; import dev.slimevr.vr.processor.skeleton.SkeletonConfig; import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; import dev.slimevr.vr.trackers.TrackerPosition; import dev.slimevr.vr.trackers.TrackerRole; import dev.slimevr.vr.trackers.TrackerUtils; -import io.eiren.util.ann.ThreadSafe; import io.eiren.util.logging.LogManager; import io.eiren.util.collections.FastList; @@ -77,8 +76,6 @@ public class AutoBone { protected final VRServer server; - protected SimpleSkeleton skeleton = null; - // This is filled by reloadConfigValues() public final EnumMap configs = new EnumMap(SkeletonConfigValue.class); public final EnumMap staticConfigs = new EnumMap(SkeletonConfigValue.class); @@ -90,10 +87,7 @@ public class AutoBone { public AutoBone(VRServer server) { this.server = server; - reloadConfigValues(); - - server.addSkeletonUpdatedCallback(this::skeletonUpdated); } public void reloadConfigValues() { @@ -139,17 +133,18 @@ public class AutoBone { staticConfigs.put(SkeletonConfigValue.SKELETON_OFFSET, 0f); } - @ThreadSafe - public void skeletonUpdated(HumanSkeleton newSkeleton) { - if(newSkeleton instanceof SimpleSkeleton) { - skeleton = (SimpleSkeleton) newSkeleton; - applyConfigToSkeleton(newSkeleton); - LogManager.log.info("[AutoBone] Received updated skeleton"); - } + /** + * A simple utility method to get the {@link HumanSkeleton} from the {@link VRServer} + * @return The {@link HumanSkeleton} associated with the {@link VRServer}, or null if there is none available + * @see {@link VRServer}, {@link HumanSkeleton} + */ + private HumanSkeleton getSkeleton() { + HumanPoseProcessor humanPoseProcessor = server != null ? server.humanPoseProcessor : null; + return humanPoseProcessor != null ? humanPoseProcessor.getSkeleton() : null; } - + public void applyConfig() { - if(!applyConfigToSkeleton(skeleton)) { + if(!applyConfigToSkeleton(getSkeleton())) { // Unable to apply to skeleton, save directly saveConfigs(); } @@ -287,10 +282,14 @@ public class AutoBone { // If target height isn't specified, auto-detect if(targetHeight < 0f) { + // Get the current skeleton from the server + HumanSkeleton skeleton = getSkeleton(); if(skeleton != null) { + // If there is a skeleton available, calculate the target height from its configs targetHeight = sumSelectConfigs(heightConfigs, skeleton.getSkeletonConfig()); LogManager.log.warning("[AutoBone] Target height loaded from skeleton (Make sure you reset before running!): " + targetHeight); } else { + // Otherwise if there is no skeleton available, attempt to get the max HMD height from the recording float hmdHeight = getMaxHmdHeight(frames); if(hmdHeight <= 0.50f) { LogManager.log.warning("[AutoBone] Max headset height detected (Value seems too low, did you not stand up straight while measuring?): " + hmdHeight); @@ -303,12 +302,14 @@ public class AutoBone { } } + // Epoch loop, each epoch is one full iteration over the full dataset for(int epoch = calcInitError ? -1 : 0; epoch < numEpochs; epoch++) { float sumError = 0f; int errorCount = 0; float adjustRate = epoch >= 0 ? (initialAdjustRate / FastMath.pow(adjustRateDecay, epoch)) : 0f; + // Iterate over the frames using a cursor and an offset for comparing frames a certain number of frames apart for(int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++) { for(int frameCursor = 0; frameCursor < frameCount - cursorOffset; frameCursor += cursorIncrement) { int frameCursor2 = frameCursor + cursorOffset; @@ -559,42 +560,48 @@ public class AutoBone { float sumWeight = 0f; if(slideErrorFactor > 0f) { + // This is the main error function, this calculates the distance between the foot positions on both frames totalError += getSlideErrorDeriv(skeleton1, skeleton2) * distScale * slideErrorFactor; sumWeight += slideErrorFactor; } if(offsetSlideErrorFactor > 0f) { + // This error function compares the distance between the feet on each frame and returns the offset between them totalError += getOffsetSlideErrorDeriv(skeleton1, skeleton2) * distScale * offsetSlideErrorFactor; sumWeight += offsetSlideErrorFactor; } if(offsetErrorFactor > 0f) { + // This error function compares the height of each foot in each frame totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * distScale * offsetErrorFactor; sumWeight += offsetErrorFactor; } if(proportionErrorFactor > 0f) { + // This error function compares the current values to general expected proportions to keep measurements in line // Either skeleton will work fine, skeleton1 is used as a default totalError += getProportionErrorDeriv(skeleton1.skeletonConfig) * proportionErrorFactor; sumWeight += proportionErrorFactor; } if(heightErrorFactor > 0f) { + // This error function compares the height change to the actual measured height of the headset totalError += FastMath.abs(heightChange) * heightErrorFactor; sumWeight += heightErrorFactor; } if(positionErrorFactor > 0f) { + // This error function compares the position of an assigned tracker with the position on the skeleton totalError += (getPositionErrorDeriv(frames, cursor1, skeleton1) + getPositionErrorDeriv(frames, cursor2, skeleton2) / 2f) * distScale * positionErrorFactor; sumWeight += positionErrorFactor; } if(positionOffsetErrorFactor > 0f) { + // This error function compares the offset of the position of an assigned tracker with the position on the skeleton totalError += getPositionOffsetErrorDeriv(frames, cursor1, cursor2, skeleton1, skeleton2) * distScale * positionOffsetErrorFactor; sumWeight += positionOffsetErrorFactor; } - // Minimize sliding, minimize foot height offset, minimize change in total height return sumWeight > 0f ? totalError / sumWeight : 0f; } diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index 3ceffcdf4..9487ad57a 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -143,6 +143,9 @@ public class TrackersList extends EJBoxNoStretch { JLabel adj; JLabel adjYaw; JLabel correction; + JLabel signalStrength; + JLabel rotQuat; + JLabel rotAdj; @AWTThread public TrackerPanel(Tracker t) { @@ -216,6 +219,7 @@ public class TrackersList extends EJBoxNoStretch { add(new JLabel("TPS"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); if(realTracker 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)); } row++; if(t.hasRotation()) @@ -224,6 +228,7 @@ public class TrackersList extends EJBoxNoStretch { add(position = new JLabel("0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); if(realTracker 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) { add(tps = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); @@ -240,11 +245,19 @@ public class TrackersList extends EJBoxNoStretch { row++; add(new JLabel("Raw:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add(raw = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1)); + /* + if(realTracker 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(realTracker instanceof IMUTracker) { add(new JLabel("Raw mag:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add(rawMag = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1)); + add(new JLabel("Hash:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(new JLabel(String.format("0x%8x", realTracker.hashCode())), s(c(3, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1)); row++; add(new JLabel("Cal:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add(calibration = new JLabel("0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); @@ -253,6 +266,8 @@ public class TrackersList extends EJBoxNoStretch { row++; add(new JLabel("Correction:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); add(correction = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1)); + add(new JLabel("RotAdj:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + add(rotAdj = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); row++; } //*/ @@ -297,7 +312,7 @@ public class TrackersList extends EJBoxNoStretch { tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1)); } if(realTracker instanceof TrackerWithBattery) - bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 1)); + bat.setText(String.format("%d%% (%sV)", Math.round(((TrackerWithBattery) realTracker).getBatteryLevel()), StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 2))); if(t instanceof ReferenceAdjustedTracker) { ((ReferenceAdjustedTracker) t).attachmentFix.toAngles(angles); if(adj != null) @@ -313,6 +328,17 @@ public class TrackersList extends EJBoxNoStretch { if(realTracker instanceof IMUTracker) { if(ping != null) ping.setText(String.valueOf(((IMUTracker) realTracker).ping)); + if (signalStrength != null) { + int signal = ((IMUTracker) realTracker).signalStrength; + if (signal == -1) { + signalStrength.setText("N/A"); + } else { + // -40 dBm is excellent, -95 dBm is very poor + int percentage = (signal - -95) * (100 - 0) / (-40 - -95) + 0; + percentage = Math.max(Math.min(percentage, 100), 0); + signalStrength.setText(String.valueOf(percentage) + "% " + "(" + String.valueOf(signal) + " dBm" + ")"); + } + } } realTracker.getRotation(q); q.toAngles(angles); @@ -320,21 +346,34 @@ public class TrackersList extends EJBoxNoStretch { + " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0) + " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)); if(realTracker instanceof IMUTracker) { - ((IMUTracker) realTracker).rotMagQuaternion.toAngles(angles); + IMUTracker imu = (IMUTracker) realTracker; + imu.rotMagQuaternion.toAngles(angles); if(rawMag != null) rawMag.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(calibration != null) - calibration.setText(((IMUTracker) realTracker).calibrationStatus + " / " + ((IMUTracker) realTracker).magCalibrationStatus); + calibration.setText(imu.calibrationStatus + " / " + imu.magCalibrationStatus); if(magAccuracy != null) - magAccuracy.setText(StringUtils.prettyNumber(((IMUTracker) realTracker).magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°"); - ((IMUTracker) realTracker).getCorrection(q); + magAccuracy.setText(StringUtils.prettyNumber(imu.magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°"); + imu.getCorrection(q); q.toAngles(angles); if(correction != null) correction.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(rotQuat != null) { + imu.rotQuaternion.toAngles(angles); + rotQuat.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(rotAdj != null) { + imu.rotAdjust.toAngles(angles); + rotAdj.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)); + } } } } diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java index 120fbf7e3..95fa5bf80 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java @@ -7,13 +7,13 @@ public enum SkeletonConfigValue { HEAD("Head", "headShift", "Head shift", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HEAD}), NECK("Neck", "neckLength", "Neck length", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.NECK}), - TORSO("Torso", "torsoLength", "Torso length", 0.64f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}), - CHEST("Chest", "chestDistance", "Chest distance", 0.32f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}), + TORSO("Torso", "torsoLength", "Torso length", 0.6f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}), + CHEST("Chest", "chestDistance", "Chest distance", 0.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}), WAIST("Waist", "waistDistance", "Waist distance", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST, SkeletonNodeOffset.HIP}), HIP_OFFSET("Hip offset", "hipOffset", "Hip offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HIP_TRACKER}), - HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}), - LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.86f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}), - KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.43f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}), + HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.28f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}), + LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.88f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}), + KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.44f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}), FOOT_LENGTH("Foot length", "footLength", "Foot length", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.FOOT}), FOOT_OFFSET("Foot offset", "footOffset", "Foot offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.ANKLE}), SKELETON_OFFSET("Skeleton offset", "skeletonOffset", "Skeleton offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST_TRACKER, SkeletonNodeOffset.HIP_TRACKER, SkeletonNodeOffset.KNEE_TRACKER, SkeletonNodeOffset.FOOT_TRACKER}), diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index 538c8156b..81bf5bfd3 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -4,19 +4,18 @@ import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; -import io.eiren.math.FloatMath; import io.eiren.util.BufferedTimer; public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { public static final float MAX_MAG_CORRECTION_ACCURACY = 5 * FastMath.RAD_TO_DEG; - public final Vector3f gyroVector = new Vector3f(); - public final Vector3f accelVector = new Vector3f(); + //public final Vector3f gyroVector = new Vector3f(); + //public final Vector3f accelVector = new Vector3f(); public final Vector3f magVector = new Vector3f(); public final Quaternion rotQuaternion = new Quaternion(); public final Quaternion rotMagQuaternion = new Quaternion(); - protected final Quaternion rotAdjust = new Quaternion(); + public final Quaternion rotAdjust = new Quaternion(); protected final Quaternion correction = new Quaternion(); protected TrackerMountingRotation mounting = null; protected TrackerStatus status = TrackerStatus.OK; @@ -27,6 +26,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { protected final TrackersUDPServer server; protected float confidence = 0; protected float batteryVoltage = 0; + protected float batteryLevel = 0; public int calibrationStatus = 0; public int magCalibrationStatus = 0; public float magnetometerAccuracy = 0; @@ -35,6 +35,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { protected BufferedTimer timer = new BufferedTimer(1f); public int ping = -1; + public int signalStrength = -1; public TrackerPosition bodyPosition = null; @@ -147,7 +148,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { @Override public float getBatteryLevel() { - return FloatMath.mapValue(getBatteryVoltage(), 3.6f, 4.2f, 0f, 1f); + return batteryLevel; } @Override @@ -155,6 +156,10 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { return batteryVoltage; } + public void setBatteryLevel(float level) { + this.batteryLevel = level; + } + public void setBatteryVoltage(float voltage) { this.batteryVoltage = voltage; } diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java index a1de034f6..23d0003fa 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java @@ -109,6 +109,8 @@ public class TrackersUDPServer extends Thread { } } if(firmware.length() == 0) { + // TODO Owo track can report firmware + // Will be not important after refactoring, but need not forget firmware.append("owoTrack"); tracker.isOwoTrack = true; } @@ -332,11 +334,16 @@ public class TrackersUDPServer extends Thread { if(connection == null) break; bb.getLong(); + float voltage = bb.getFloat(); + float level = bb.getFloat() * 100; // Use default level if recieved 0 if(connection.sensors.size() > 0) { Collection trackers = connection.sensors.values(); Iterator iterator = trackers.iterator(); - while(iterator.hasNext()) - iterator.next().setBatteryVoltage(bb.getFloat()); + while(iterator.hasNext()) { + IMUTracker tr = iterator.next(); + tr.setBatteryVoltage(voltage); + tr.setBatteryLevel(bb.getFloat() * 100); + } } break; case 13: // PACKET_TAP @@ -381,6 +388,17 @@ public class TrackersUDPServer extends Thread { socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address)); System.out.println("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus); break; + case 19: + if(connection == null) + break; + bb.getLong(); + sensorId = bb.get() & 0xFF; + tracker = connection.sensors.get(sensorId); + if(tracker == null) + break; + int signalStrength = bb.get(); + tracker.signalStrength = signalStrength; + break; default: System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); break;