diff --git a/src/main/java/io/eiren/gui/autobone/AutoBoneWindow.java b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java similarity index 86% rename from src/main/java/io/eiren/gui/autobone/AutoBoneWindow.java rename to src/main/java/dev/slimevr/gui/AutoBoneWindow.java index 75a1d9841..c2017a64d 100644 --- a/src/main/java/io/eiren/gui/autobone/AutoBoneWindow.java +++ b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java @@ -1,4 +1,4 @@ -package io.eiren.gui.autobone; +package dev.slimevr.gui; import javax.swing.BoxLayout; import javax.swing.JFrame; @@ -20,10 +20,16 @@ import io.eiren.util.ann.AWTThread; import io.eiren.util.collections.FastList; import io.eiren.util.logging.LogManager; import io.eiren.vr.VRServer; +import dev.slimevr.vr.autobone.AutoBone; + import javax.swing.event.MouseInputAdapter; import org.apache.commons.lang3.tuple.Pair; +import dev.slimevr.vr.poserecorder.PoseFrame; +import dev.slimevr.vr.poserecorder.PoseFrameIO; +import dev.slimevr.vr.poserecorder.PoseRecorder; + public class AutoBoneWindow extends JFrame { private static File saveDir = new File("Recordings"); @@ -31,14 +37,14 @@ public class AutoBoneWindow extends JFrame { private EJBox pane; - private final VRServer server; - private final SkeletonConfig skeletonConfig; - private final PoseRecorder poseRecorder; - private final AutoBone autoBone; + private final transient VRServer server; + private final transient SkeletonConfig skeletonConfig; + private final transient PoseRecorder poseRecorder; + private final transient AutoBone autoBone; - private Thread recordingThread = null; - private Thread saveRecordingThread = null; - private Thread autoBoneThread = null; + private transient Thread recordingThread = null; + private transient Thread saveRecordingThread = null; + private transient Thread autoBoneThread = null; private JButton saveRecordingButton; private JButton adjustButton; @@ -77,12 +83,12 @@ public class AutoBoneWindow extends JFrame { return configInfo.toString(); } - private void saveRecording(PoseFrame[] frames) { + private void saveRecording(PoseFrame frames) { if (saveDir.isDirectory() || saveDir.mkdirs()) { File saveRecording; int recordingIndex = 1; do { - saveRecording = new File(saveDir, "ABRecording" + recordingIndex++ + ".abf"); + saveRecording = new File(saveDir, "ABRecording" + recordingIndex++ + ".pfr"); } while (saveRecording.exists()); LogManager.log.info("[AutoBone] Exporting frames to \"" + saveRecording.getPath() + "\"..."); @@ -96,15 +102,15 @@ public class AutoBoneWindow extends JFrame { } } - private List> loadRecordings() { - List> recordings = new FastList>(); + private List> loadRecordings() { + List> recordings = new FastList>(); if (loadDir.isDirectory()) { File[] files = loadDir.listFiles(); if (files != null) { for (File file : files) { - if (file.isFile() && org.apache.commons.lang3.StringUtils.endsWithIgnoreCase(file.getName(), ".abf")) { + if (file.isFile() && org.apache.commons.lang3.StringUtils.endsWithIgnoreCase(file.getName(), ".pfr")) { LogManager.log.info("[AutoBone] Detected recording at \"" + file.getPath() + "\", loading frames..."); - PoseFrame[] frames = PoseFrameIO.readFromFile(file); + PoseFrame frames = PoseFrameIO.readFromFile(file); if (frames == null) { LogManager.log.severe("Reading frames from \"" + file.getPath() + "\" failed..."); @@ -119,9 +125,7 @@ public class AutoBoneWindow extends JFrame { return recordings; } - private float processFrames(PoseFrame[] frames) { - autoBone.reloadConfigValues(); - + private float processFrames(PoseFrame frames) { autoBone.minDataDistance = server.config.getInt("autobone.minimumDataDistance", autoBone.minDataDistance); autoBone.maxDataDistance = server.config.getInt("autobone.maximumDataDistance", autoBone.maxDataDistance); @@ -167,8 +171,8 @@ public class AutoBoneWindow extends JFrame { // 1000 samples at 20 ms per sample is 20 seconds int sampleCount = server.config.getInt("autobone.sampleCount", 1000); long sampleRate = server.config.getLong("autobone.sampleRateMs", 20L); - Future framesFuture = poseRecorder.startFrameRecording(sampleCount, sampleRate); - PoseFrame[] frames = framesFuture.get(); + Future framesFuture = poseRecorder.startFrameRecording(sampleCount, sampleRate); + PoseFrame frames = framesFuture.get(); LogManager.log.info("[AutoBone] Done recording!"); saveRecordingButton.setEnabled(true); @@ -219,17 +223,28 @@ public class AutoBoneWindow extends JFrame { @Override public void run() { try { - Future framesFuture = poseRecorder.getFramesAsync(); + Future framesFuture = poseRecorder.getFramesAsync(); if (framesFuture != null) { setText("Waiting for Recording..."); - PoseFrame[] frames = framesFuture.get(); + PoseFrame frames = framesFuture.get(); - if (frames.length <= 0) { + if (frames.getTrackerCount() <= 0) { + throw new IllegalStateException("Recording has no trackers"); + } + + if (frames.getMaxFrameCount() <= 0) { throw new IllegalStateException("Recording has no frames"); } setText("Saving..."); saveRecording(frames); + + setText("Recording Saved!"); + try { + Thread.sleep(3000); // Wait for 3 seconds + } catch (Exception e1) { + // Ignore + } } else { setText("No Recording..."); LogManager.log.severe("[AutoBone] Unable to save, no recording was done..."); @@ -277,17 +292,21 @@ public class AutoBoneWindow extends JFrame { public void run() { try { setText("Load..."); - List> frameRecordings = loadRecordings(); + List> frameRecordings = loadRecordings(); - if (frameRecordings.size() > 0) { + if (!frameRecordings.isEmpty()) { LogManager.log.info("[AutoBone] Done loading frames!"); } else { - Future framesFuture = poseRecorder.getFramesAsync(); + Future framesFuture = poseRecorder.getFramesAsync(); if (framesFuture != null) { setText("Waiting for Recording..."); - PoseFrame[] frames = framesFuture.get(); + PoseFrame frames = framesFuture.get(); - if (frames.length <= 0) { + if (frames.getTrackerCount() <= 0) { + throw new IllegalStateException("Recording has no trackers"); + } + + if (frames.getMaxFrameCount() <= 0) { throw new IllegalStateException("Recording has no frames"); } @@ -307,7 +326,7 @@ public class AutoBoneWindow extends JFrame { setText("Processing..."); LogManager.log.info("[AutoBone] Processing frames..."); FastList heightPercentError = new FastList(frameRecordings.size()); - for (Pair recording : frameRecordings) { + for (Pair recording : frameRecordings) { LogManager.log.info("[AutoBone] Processing frames from \"" + recording.getKey() + "\"..."); heightPercentError.add(processFrames(recording.getValue())); @@ -341,7 +360,7 @@ public class AutoBoneWindow extends JFrame { lengthsLabel.setText(lengthsString); } - if (heightPercentError.size() > 0) { + if (!heightPercentError.isEmpty()) { float mean = 0f; for (float val : heightPercentError) { mean += val; diff --git a/src/main/java/io/eiren/gui/autobone/AutoBone.java b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java similarity index 72% rename from src/main/java/io/eiren/gui/autobone/AutoBone.java rename to src/main/java/dev/slimevr/vr/autobone/AutoBone.java index 5e0eaa5d8..ccb664999 100644 --- a/src/main/java/io/eiren/gui/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java @@ -1,16 +1,20 @@ -package io.eiren.gui.autobone; +package dev.slimevr.vr.autobone; import java.util.HashMap; +import java.util.List; import java.util.Map; import java.util.Map.Entry; import java.util.function.Consumer; import com.jme3.math.Vector3f; +import dev.slimevr.vr.poserecorder.PoseFrame; +import dev.slimevr.vr.poserecorder.TrackerFrameData; import io.eiren.util.ann.ThreadSafe; import io.eiren.util.logging.LogManager; import io.eiren.util.collections.FastList; import io.eiren.vr.VRServer; +import dev.slimevr.vr.poserecorder.TrackerFrame; import io.eiren.vr.processor.HumanSkeleton; import io.eiren.vr.processor.HumanSkeletonWithLegs; import io.eiren.vr.processor.HumanSkeletonWithWaist; @@ -84,12 +88,17 @@ public class AutoBone { } public void reloadConfigValues() { + reloadConfigValues(null); + } + + public void reloadConfigValues(TrackerFrame[] frame) { // Load waist configs staticConfigs.put("Head", server.config.getFloat("body.headShift", HumanSkeletonWithWaist.HEAD_SHIFT_DEFAULT)); staticConfigs.put("Neck", server.config.getFloat("body.neckLength", HumanSkeletonWithWaist.NECK_LENGTH_DEFAULT)); configs.put("Waist", server.config.getFloat("body.waistDistance", 0.85f)); if (server.config.getBoolean("autobone.forceChestTracker", false) || + (frame != null && TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.CHEST) != null) || TrackerUtils.findTrackerForBodyPosition(server.getAllTrackers(), TrackerBodyPosition.CHEST) != null) { // If force enabled or has a chest tracker configs.put("Chest", server.config.getFloat("body.chestDistance", 0.42f)); @@ -126,9 +135,7 @@ public class AutoBone { return false; } - for (Entry entry : configs.entrySet()) { - skeleton.setSkeletonConfig(entry.getKey(), entry.getValue()); - } + configs.forEach(skeleton::setSkeletonConfig); server.saveConfig(); @@ -197,39 +204,48 @@ public class AutoBone { return length; } - public float getMaxHmdHeight(PoseFrame[] frames) { + public float getMaxHmdHeight(PoseFrame frames) { float maxHeight = 0f; - for (PoseFrame frame : frames) { - if (frame.rootPos.y > maxHeight) { - maxHeight = frame.rootPos.y; + for (TrackerFrame[] frame : frames) { + TrackerFrame hmd = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.HMD); + if (hmd != null && hmd.hasData(TrackerFrameData.POSITION) && hmd.position.y > maxHeight) { + maxHeight = hmd.position.y; } } return maxHeight; } - public void processFrames(PoseFrame[] frames) { + public void processFrames(PoseFrame frames) { processFrames(frames, -1f); } - public void processFrames(PoseFrame[] frames, Consumer epochCallback) { + public void processFrames(PoseFrame frames, Consumer epochCallback) { processFrames(frames, -1f, epochCallback); } - public void processFrames(PoseFrame[] frames, float targetHeight) { + public void processFrames(PoseFrame frames, float targetHeight) { processFrames(frames, true, targetHeight); } - public void processFrames(PoseFrame[] frames, float targetHeight, Consumer epochCallback) { + public void processFrames(PoseFrame frames, float targetHeight, Consumer epochCallback) { processFrames(frames, true, targetHeight, epochCallback); } - public float processFrames(PoseFrame[] frames, boolean calcInitError, float targetHeight) { + public float processFrames(PoseFrame frames, boolean calcInitError, float targetHeight) { return processFrames(frames, calcInitError, targetHeight, null); } - public float processFrames(PoseFrame[] frames, boolean calcInitError, float targetHeight, Consumer epochCallback) { - SimpleSkeleton skeleton1 = new SimpleSkeleton(configs, staticConfigs); - SimpleSkeleton skeleton2 = new SimpleSkeleton(configs, staticConfigs); + public float processFrames(PoseFrame frames, boolean calcInitError, float targetHeight, Consumer epochCallback) { + final int frameCount = frames.getMaxFrameCount(); + + final SimpleSkeleton skeleton1 = new SimpleSkeleton(configs, staticConfigs); + final TrackerFrame[] trackerBuffer1 = new TrackerFrame[frames.getTrackerCount()]; + + frames.getFrames(0, trackerBuffer1); + reloadConfigValues(trackerBuffer1); // Reload configs and detect chest tracker from the first frame + + final SimpleSkeleton skeleton2 = new SimpleSkeleton(configs, staticConfigs); + final TrackerFrame[] trackerBuffer2 = new TrackerFrame[frames.getTrackerCount()]; // If target height isn't specified, auto-detect if (targetHeight < 0f) { @@ -255,32 +271,27 @@ public class AutoBone { float adjustRate = epoch >= 0 ? (float)(initialAdjustRate / Math.pow(adjustRateDecay, epoch)) : 0f; - for (int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frames.length; cursorOffset++) { - for (int frameCursor = 0; frameCursor < frames.length - cursorOffset; frameCursor += cursorIncrement) { - PoseFrame frame1 = frames[frameCursor]; - PoseFrame frame2 = frames[frameCursor + cursorOffset]; - - // If there's missing data, throw an exception - if (frame1 == null || frame2 == null) { - throw new NullPointerException("Frames are missing from processing data"); - } + for (int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++) { + for (int frameCursor = 0; frameCursor < frameCount - cursorOffset; frameCursor += cursorIncrement) { + frames.getFrames(frameCursor, trackerBuffer1); + frames.getFrames(frameCursor + cursorOffset, trackerBuffer2); skeleton1.setSkeletonConfigs(configs); skeleton2.setSkeletonConfigs(configs); - skeleton1.setPoseFromFrame(frame1); - skeleton2.setPoseFromFrame(frame2); + skeleton1.setPoseFromFrame(trackerBuffer1); + skeleton2.setPoseFromFrame(trackerBuffer2); float totalLength = getLengthSum(configs); float curHeight = getHeight(configs, staticConfigs); - float errorDeriv = getErrorDeriv(frame1, frame2, skeleton1, skeleton2, targetHeight - curHeight); + float errorDeriv = getErrorDeriv(trackerBuffer1, trackerBuffer2, skeleton1, skeleton2, targetHeight - curHeight); float error = errorFunc(errorDeriv); // In case of fire if (Float.isNaN(error) || Float.isInfinite(error)) { // Extinguish LogManager.log.warning("[AutoBone] Error value is invalid, resetting variables to recover"); - reloadConfigValues(); + reloadConfigValues(trackerBuffer1); // Reset error sum values sumError = 0f; @@ -321,7 +332,7 @@ public class AutoBone { updateSkeletonBoneLength(skeleton1, skeleton2, entry.getKey(), newLength); float newHeight = isHeightVar ? curHeight + curAdjustVal : curHeight; - float newErrorDeriv = getErrorDeriv(frame1, frame2, skeleton1, skeleton2, targetHeight - newHeight); + float newErrorDeriv = getErrorDeriv(trackerBuffer1, trackerBuffer2, skeleton1, skeleton2, targetHeight - newHeight); if (newErrorDeriv < minError) { minError = newErrorDeriv; @@ -356,8 +367,8 @@ public class AutoBone { // The change in position of the ankle over time protected float getSlideErrorDeriv(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { - float slideLeft = skeleton1.getLeftFootPos().distance(skeleton2.getLeftFootPos()); - float slideRight = skeleton1.getRightFootPos().distance(skeleton2.getRightFootPos()); + float slideLeft = skeleton1.getNodePosition(TrackerBodyPosition.LEFT_ANKLE).distance(skeleton2.getNodePosition(TrackerBodyPosition.LEFT_ANKLE)); + float slideRight = skeleton1.getNodePosition(TrackerBodyPosition.RIGHT_ANKLE).distance(skeleton2.getNodePosition(TrackerBodyPosition.RIGHT_ANKLE)); // Divide by 4 to halve and average, it's halved because you want to approach a midpoint, not the other point return (slideLeft + slideRight) / 4f; @@ -365,14 +376,20 @@ public class AutoBone { // The offset between both feet at one instant and over time protected float getOffsetErrorDeriv(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { - float dist1 = Math.abs(skeleton1.getLeftFootPos().y - skeleton1.getRightFootPos().y); - float dist2 = Math.abs(skeleton2.getLeftFootPos().y - skeleton2.getRightFootPos().y); + float skeleton1Left = skeleton1.getNodePosition(TrackerBodyPosition.LEFT_ANKLE).getY(); + float skeleton1Right = skeleton1.getNodePosition(TrackerBodyPosition.RIGHT_ANKLE).getY(); - float dist3 = Math.abs(skeleton1.getLeftFootPos().y - skeleton2.getRightFootPos().y); - float dist4 = Math.abs(skeleton1.getLeftFootPos().y - skeleton2.getRightFootPos().y); + float skeleton2Left = skeleton2.getNodePosition(TrackerBodyPosition.LEFT_ANKLE).getY(); + float skeleton2Right = skeleton2.getNodePosition(TrackerBodyPosition.RIGHT_ANKLE).getY(); - float dist5 = Math.abs(skeleton1.getLeftFootPos().y - skeleton2.getLeftFootPos().y); - float dist6 = Math.abs(skeleton1.getRightFootPos().y - skeleton2.getRightFootPos().y); + float dist1 = Math.abs(skeleton1Left - skeleton1Right); + float dist2 = Math.abs(skeleton2Left - skeleton2Right); + + float dist3 = Math.abs(skeleton1Left - skeleton2Right); + float dist4 = Math.abs(skeleton2Left - skeleton1Right); + + float dist5 = Math.abs(skeleton1Left - skeleton2Left); + float dist6 = Math.abs(skeleton1Right - skeleton2Right); // Divide by 12 to halve and average, it's halved because you want to approach a midpoint, not the other point return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f; @@ -400,49 +417,18 @@ public class AutoBone { } // The distance of any points to the corresponding absolute position - protected float getPositionErrorDeriv(PoseFrame frame, SimpleSkeleton skeleton) { + protected float getPositionErrorDeriv(TrackerFrame[] frame, SimpleSkeleton skeleton) { float offset = 0f; int offsetCount = 0; - if (frame.positions != null) { - for (Entry entry : frame.positions.entrySet()) { - Vector3f nodePos = skeleton.getNodePosition(entry.getKey()); - if (nodePos != null) { - offset += Math.abs(nodePos.distance(entry.getValue())); - offsetCount++; - } + for (TrackerFrame trackerFrame : frame) { + if (trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) { + continue; } - } - return offsetCount > 0 ? offset / offsetCount : 0f; - } - - // The difference between offset of absolute position and the corresponding point over time - protected float getPositionOffsetErrorDeriv(PoseFrame frame1, PoseFrame frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { - float offset = 0f; - int offsetCount = 0; - - if (frame1.positions != null && frame2.positions != null) { - for (Entry entry : frame1.positions.entrySet()) { - Vector3f frame2Pos = frame2.positions.get(entry.getKey()); - if (frame2Pos == null) { - continue; - } - - Vector3f nodePos1 = skeleton1.getNodePosition(entry.getKey()); - if (nodePos1 == null) { - continue; - } - - Vector3f nodePos2 = skeleton2.getNodePosition(entry.getKey()); - if (nodePos2 == null) { - continue; - } - - float dist1 = Math.abs(nodePos1.distance(entry.getValue())); - float dist2 = Math.abs(nodePos2.distance(frame2Pos)); - - offset += Math.abs(dist2 - dist1); + Vector3f nodePos = skeleton.getNodePosition(trackerFrame.designation.designation); + if (nodePos != null) { + offset += Math.abs(nodePos.distance(trackerFrame.position)); offsetCount++; } } @@ -450,7 +436,42 @@ public class AutoBone { return offsetCount > 0 ? offset / offsetCount : 0f; } - protected float getErrorDeriv(PoseFrame frame1, PoseFrame frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, float heightChange) { + // The difference between offset of absolute position and the corresponding point over time + protected float getPositionOffsetErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { + float offset = 0f; + int offsetCount = 0; + + for (TrackerFrame trackerFrame1 : frame1) { + if (trackerFrame1 == null || !trackerFrame1.hasData(TrackerFrameData.POSITION)) { + continue; + } + + TrackerFrame trackerFrame2 = TrackerUtils.findTrackerForBodyPosition(frame2, trackerFrame1.designation); + if (trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) { + continue; + } + + Vector3f nodePos1 = skeleton1.getNodePosition(trackerFrame1.designation); + if (nodePos1 == null) { + continue; + } + + Vector3f nodePos2 = skeleton2.getNodePosition(trackerFrame2.designation); + if (nodePos2 == null) { + continue; + } + + float dist1 = Math.abs(nodePos1.distance(trackerFrame1.position)); + float dist2 = Math.abs(nodePos2.distance(trackerFrame2.position)); + + offset += Math.abs(dist2 - dist1); + offsetCount++; + } + + return offsetCount > 0 ? offset / offsetCount : 0f; + } + + protected float getErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, float heightChange) { float totalError = 0f; float sumWeight = 0f; diff --git a/src/main/java/io/eiren/gui/autobone/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java similarity index 63% rename from src/main/java/io/eiren/gui/autobone/SimpleSkeleton.java rename to src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java index 7cc4f3a89..2f6457a27 100644 --- a/src/main/java/io/eiren/gui/autobone/SimpleSkeleton.java +++ b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java @@ -1,15 +1,18 @@ -package io.eiren.gui.autobone; +package dev.slimevr.vr.autobone; import java.util.HashMap; import java.util.Map; -import java.util.Map.Entry; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; +import dev.slimevr.vr.poserecorder.TrackerFrameData; +import dev.slimevr.vr.poserecorder.TrackerFrame; import io.eiren.vr.processor.HumanSkeletonWithLegs; import io.eiren.vr.processor.HumanSkeletonWithWaist; +import io.eiren.vr.processor.TrackerBodyPosition; import io.eiren.vr.processor.TransformNode; +import io.eiren.vr.trackers.TrackerUtils; import io.eiren.yaml.YamlFile; public class SimpleSkeleton { @@ -58,6 +61,9 @@ public class SimpleSkeleton { protected final HashMap nodes = new HashMap(); + private Quaternion rotBuf1 = new Quaternion(); + private Quaternion rotBuf2 = new Quaternion(); + public SimpleSkeleton() { // Assemble skeleton to waist hmdNode.attachChild(headNode); @@ -97,7 +103,7 @@ public class SimpleSkeleton { }); } - public SimpleSkeleton(Iterable> configs, Iterable> altConfigs) { + public SimpleSkeleton(Map configs, Map altConfigs) { // Initialize this(); @@ -109,62 +115,92 @@ public class SimpleSkeleton { setSkeletonConfigs(configs); } - public SimpleSkeleton(Map configs, Map altConfigs) { - this(configs.entrySet(), altConfigs.entrySet()); - } - - public SimpleSkeleton(Iterable> configs) { + public SimpleSkeleton(Map configs) { this(configs, null); } - public SimpleSkeleton(Map configs) { - this(configs.entrySet()); - } + public void setPoseFromFrame(TrackerFrame[] frame) { - public void setPoseFromSkeleton(HumanSkeletonWithLegs humanSkeleton) { - TransformNode rootNode = humanSkeleton.getRootNode(); + TrackerFrame hmd = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.HMD); - // Copy headset position - hmdNode.localTransform.setTranslation(rootNode.localTransform.getTranslation()); - - // Copy all rotations - rootNode.depthFirstTraversal(visitor -> { - TransformNode targetNode = nodes.get(visitor.getName()); - - // Handle unexpected nodes gracefully - if (targetNode != null) { - targetNode.localTransform.setRotation(visitor.localTransform.getRotation()); + if (hmd != null) + { + if (hmd.hasData(TrackerFrameData.ROTATION)) + { + hmdNode.localTransform.setRotation(hmd.rotation); + headNode.localTransform.setRotation(hmd.rotation); } - }); - } - public void setPoseFromFrame(PoseFrame frame) { - // Copy headset position - hmdNode.localTransform.setTranslation(frame.rootPos); - - if (frame.rotations != null) { - // Copy all rotations - for (Entry rotation : frame.rotations.entrySet()) { - TransformNode targetNode = nodes.get(rotation.getKey()); - - // Handle unexpected nodes gracefully - if (targetNode != null) { - targetNode.localTransform.setRotation(rotation.getValue()); - } + if (hmd.hasData(TrackerFrameData.POSITION)) + { + hmdNode.localTransform.setTranslation(hmd.position); } } + TrackerFrame chest = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.CHEST, TrackerBodyPosition.WAIST); + setRotation(chest, neckNode); + + TrackerFrame waist = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST); + setRotation(waist, chestNode); + + TrackerFrame leftLeg = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.LEFT_LEG); + TrackerFrame rightLeg = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.RIGHT_LEG); + + averagePelvis(waist, leftLeg, rightLeg); + + setRotation(leftLeg, leftHipNode); + setRotation(rightLeg, rightHipNode); + + TrackerFrame leftAnkle = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.LEFT_ANKLE); + setRotation(leftAnkle, rightKneeNode); + + TrackerFrame rightAnkle = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.RIGHT_ANKLE); + setRotation(rightAnkle, leftKneeNode); + updatePose(); } - public void setSkeletonConfigs(Iterable> configs) { - for (Entry config : configs) { - setSkeletonConfig(config.getKey(), config.getValue()); + public void setRotation(TrackerFrame trackerFrame, TransformNode node) { + if (trackerFrame != null && trackerFrame.hasData(TrackerFrameData.ROTATION)) { + node.localTransform.setRotation(trackerFrame.rotation); } } + public void averagePelvis(TrackerFrame waist, TrackerFrame leftLeg, TrackerFrame rightLeg) + { + if ((leftLeg == null || rightLeg == null) || (!leftLeg.hasData(TrackerFrameData.ROTATION) || !rightLeg.hasData(TrackerFrameData.ROTATION))) + { + setRotation(waist, waistNode); + return; + } + + if (waist == null || !waist.hasData(TrackerFrameData.ROTATION)) + { + if ((leftLeg != null && rightLeg != null) && (leftLeg.hasData(TrackerFrameData.ROTATION) && rightLeg.hasData(TrackerFrameData.ROTATION))) + { + leftLeg.getRotation(rotBuf1); + rightLeg.getRotation(rotBuf2); + rotBuf1.nlerp(rotBuf2, 0.5f); + + waistNode.localTransform.setRotation(rotBuf1); + } + + return; + } + + // Average the pelvis with the waist rotation + leftLeg.getRotation(rotBuf1); + rightLeg.getRotation(rotBuf2); + rotBuf1.nlerp(rotBuf2, 0.5f); + + waist.getRotation(rotBuf2); + rotBuf1.nlerp(rotBuf2, 0.3333333f); + + waistNode.localTransform.setRotation(rotBuf1); + } + public void setSkeletonConfigs(Map configs) { - setSkeletonConfigs(configs.entrySet()); + configs.forEach(this::setSkeletonConfig); } public void setSkeletonConfig(String joint, float newLength) { @@ -259,21 +295,53 @@ public class SimpleSkeleton { hmdNode.update(); } + public TransformNode getNode(String node) { + return nodes.get(node); + } + + public TransformNode getNode(TrackerBodyPosition bodyPosition) { + return getNode(bodyPosition, false); + } + + public TransformNode getNode(TrackerBodyPosition bodyPosition, boolean rotationNode) { + if (bodyPosition == null) { + return null; + } + + switch (bodyPosition) { + case HMD: + return hmdNode; + case CHEST: + return rotationNode ? neckNode : chestNode; + case WAIST: + return rotationNode ? chestNode : waistNode; + + case LEFT_LEG: + return rotationNode ? leftHipNode : leftKneeNode; + case RIGHT_LEG: + return rotationNode ? rightHipNode : rightKneeNode; + + case LEFT_ANKLE: + return rotationNode ? leftKneeNode : leftAnkleNode; + case RIGHT_ANKLE: + return rotationNode ? rightKneeNode : rightAnkleNode; + } + + return null; + } + public Vector3f getNodePosition(String node) { - TransformNode transformNode = nodes.get(node); + TransformNode transformNode = getNode(node); return transformNode != null ? transformNode.worldTransform.getTranslation() : null; } - public Vector3f getHMDPos() { - return hmdNode.worldTransform.getTranslation(); - } + public Vector3f getNodePosition(TrackerBodyPosition bodyPosition) { + TransformNode node = getNode(bodyPosition); + if (node == null) { + return null; + } - public Vector3f getLeftFootPos() { - return leftAnkleNode.worldTransform.getTranslation(); - } - - public Vector3f getRightFootPos() { - return rightAnkleNode.worldTransform.getTranslation(); + return node.worldTransform.getTranslation(); } public void saveConfigs(YamlFile config) { diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java new file mode 100644 index 000000000..1d9105a35 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java @@ -0,0 +1,143 @@ +package dev.slimevr.vr.poserecorder; + +import java.util.Iterator; +import java.util.List; +import java.util.NoSuchElementException; + +import io.eiren.util.collections.FastList; +import io.eiren.vr.trackers.Tracker; + +public final class PoseFrame implements Iterable { + + private final FastList trackers; + + public PoseFrame(FastList trackers) { + this.trackers = trackers; + } + + public PoseFrame(int initialCapacity) { + this.trackers = new FastList(initialCapacity); + } + + public PoseFrame() { + this(5); + } + + public PoseFrameTracker addTracker(PoseFrameTracker tracker) { + trackers.add(tracker); + return tracker; + } + + public PoseFrameTracker addTracker(Tracker tracker, int initialCapacity) { + return addTracker(new PoseFrameTracker(tracker.getName(), initialCapacity)); + } + + public PoseFrameTracker addTracker(Tracker tracker) { + return addTracker(tracker, 5); + } + + public PoseFrameTracker removeTracker(int index) { + return trackers.remove(index); + } + + public PoseFrameTracker removeTracker(PoseFrameTracker tracker) { + trackers.remove(tracker); + return tracker; + } + + public void clearTrackers() { + trackers.clear(); + } + + public void fakeClearTrackers() { + trackers.fakeClear(); + } + + public int getTrackerCount() { + return trackers.size(); + } + + public List getTrackers() { + return trackers; + } + + public int getMaxFrameCount() { + int maxFrames = 0; + + for (int i = 0; i < trackers.size(); i++) { + PoseFrameTracker tracker = trackers.get(i); + if (tracker != null && tracker.getFrameCount() > maxFrames) { + maxFrames = tracker.getFrameCount(); + } + } + + return maxFrames; + } + + public int getFrames(int frameIndex, TrackerFrame[] buffer) { + for (int i = 0; i < trackers.size(); i++) { + PoseFrameTracker tracker = trackers.get(i); + buffer[i] = tracker != null ? tracker.safeGetFrame(frameIndex) : null; + } + return trackers.size(); + } + + public int getFrames(int frameIndex, List buffer) { + for (int i = 0; i < trackers.size(); i++) { + PoseFrameTracker tracker = trackers.get(i); + buffer.add(i, tracker != null ? tracker.safeGetFrame(frameIndex) : null); + } + return trackers.size(); + } + + public TrackerFrame[] getFrames(int frameIndex) { + TrackerFrame[] trackerFrames = new TrackerFrame[trackers.size()]; + getFrames(frameIndex, trackerFrames); + return trackerFrames; + } + + @Override + public Iterator iterator() { + return new PoseFrameIterator(this); + } + + public class PoseFrameIterator implements Iterator { + + private final PoseFrame poseFrame; + private final TrackerFrame[] trackerFrameBuffer; + + private int cursor = 0; + + public PoseFrameIterator(PoseFrame poseFrame) { + this.poseFrame = poseFrame; + trackerFrameBuffer = new TrackerFrame[poseFrame.getTrackerCount()]; + } + + @Override + public boolean hasNext() { + if (trackers.isEmpty()) { + return false; + } + + for (int i = 0; i < trackers.size(); i++) { + PoseFrameTracker tracker = trackers.get(i); + if (tracker != null && cursor < tracker.getFrameCount()) { + return true; + } + } + + return false; + } + + @Override + public TrackerFrame[] next() { + if (!hasNext()) { + throw new NoSuchElementException(); + } + + poseFrame.getFrames(cursor++, trackerFrameBuffer); + + return trackerFrameBuffer; + } + } +} diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java new file mode 100644 index 000000000..3c7fb460a --- /dev/null +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -0,0 +1,141 @@ +package dev.slimevr.vr.poserecorder; + +import java.io.BufferedInputStream; +import java.io.BufferedOutputStream; +import java.io.DataInputStream; +import java.io.DataOutputStream; +import java.io.File; +import java.io.FileInputStream; +import java.io.FileOutputStream; +import java.util.List; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +import io.eiren.util.collections.FastList; +import io.eiren.util.logging.LogManager; +import io.eiren.vr.processor.TrackerBodyPosition; + +public final class PoseFrameIO { + + private PoseFrameIO() { + // Do not allow instantiating + } + + public static boolean writeFrames(DataOutputStream outputStream, PoseFrame frames) { + try { + if (frames != null) { + + outputStream.writeInt(frames.getTrackerCount()); + for (PoseFrameTracker tracker : frames.getTrackers()) { + + outputStream.writeUTF(tracker.name); + outputStream.writeInt(tracker.getFrameCount()); + for (int i = 0; i < tracker.getFrameCount(); i++) { + TrackerFrame trackerFrame = tracker.safeGetFrame(i); + if (trackerFrame == null) { + outputStream.writeInt(0); + continue; + } + + outputStream.writeInt(trackerFrame.getDataFlags()); + + if (trackerFrame.hasData(TrackerFrameData.DESIGNATION)) { + outputStream.writeUTF(trackerFrame.designation.designation); + } + + if (trackerFrame.hasData(TrackerFrameData.ROTATION)) { + outputStream.writeFloat(trackerFrame.rotation.getX()); + outputStream.writeFloat(trackerFrame.rotation.getY()); + outputStream.writeFloat(trackerFrame.rotation.getZ()); + outputStream.writeFloat(trackerFrame.rotation.getW()); + } + + if (trackerFrame.hasData(TrackerFrameData.POSITION)) { + outputStream.writeFloat(trackerFrame.position.getX()); + outputStream.writeFloat(trackerFrame.position.getY()); + outputStream.writeFloat(trackerFrame.position.getZ()); + } + } + } + } else { + outputStream.writeInt(0); + } + } catch (Exception e) { + LogManager.log.severe("Error writing frame to stream", e); + return false; + } + + return true; + } + + public static boolean writeToFile(File file, PoseFrame frames) { + try (DataOutputStream outputStream = new DataOutputStream(new BufferedOutputStream(new FileOutputStream(file)))) { + writeFrames(outputStream, frames); + } catch (Exception e) { + LogManager.log.severe("Error writing frames to file", e); + return false; + } + + return true; + } + + public static PoseFrame readFrames(DataInputStream inputStream) { + try { + + int trackerCount = inputStream.readInt(); + FastList trackers = new FastList(trackerCount); + for (int i = 0; i < trackerCount; i++) { + + String name = inputStream.readUTF(); + int trackerFrameCount = inputStream.readInt(); + FastList trackerFrames = new FastList(trackerFrameCount); + for (int j = 0; j < trackerFrameCount; j++) { + int dataFlags = inputStream.readInt(); + + TrackerBodyPosition designation = null; + if (TrackerFrameData.DESIGNATION.check(dataFlags)) { + designation = TrackerBodyPosition.getByDesignation(inputStream.readUTF()); + } + + Quaternion rotation = null; + if (TrackerFrameData.ROTATION.check(dataFlags)) { + float quatX = inputStream.readFloat(); + float quatY = inputStream.readFloat(); + float quatZ = inputStream.readFloat(); + float quatW = inputStream.readFloat(); + rotation = new Quaternion(quatX, quatY, quatZ, quatW); + } + + Vector3f position = null; + if (TrackerFrameData.POSITION.check(dataFlags)) { + float posX = inputStream.readFloat(); + float posY = inputStream.readFloat(); + float posZ = inputStream.readFloat(); + position = new Vector3f(posX, posY, posZ); + } + + trackerFrames.add(new TrackerFrame(designation, rotation, position)); + } + + trackers.add(new PoseFrameTracker(name, trackerFrames)); + } + + return new PoseFrame(trackers); + } catch (Exception e) { + LogManager.log.severe("Error reading frame from stream", e); + } + + return null; + } + + public static PoseFrame readFromFile(File file) { + try { + return readFrames(new DataInputStream(new BufferedInputStream(new FileInputStream(file)))); + } catch (Exception e) { + LogManager.log.severe("Error reading frame from file", e); + } + + return null; + } +} diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameTracker.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameTracker.java new file mode 100644 index 000000000..812b70dca --- /dev/null +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameTracker.java @@ -0,0 +1,233 @@ +package dev.slimevr.vr.poserecorder; + +import java.util.Iterator; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +import io.eiren.util.collections.FastList; +import io.eiren.vr.processor.TrackerBodyPosition; +import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerConfig; +import io.eiren.vr.trackers.TrackerStatus; + +public class PoseFrameTracker implements Tracker, Iterable { + + public final String name; + + private final FastList frames; + private int frameCursor = 0; + + public PoseFrameTracker(String name, FastList frames) { + if (frames == null) { + throw new NullPointerException("frames must not be null"); + } + + this.name = name != null ? name : ""; + this.frames = frames; + } + + public PoseFrameTracker(String name, int initialCapacity) { + this(name, new FastList(initialCapacity)); + } + + public PoseFrameTracker(String name) { + this(name, 5); + } + + private int limitCursor() { + if (frameCursor < 0 || frames.isEmpty()) { + frameCursor = 0; + } else if (frameCursor >= frames.size()) { + frameCursor = frames.size() - 1; + } + + return frameCursor; + } + + public int setCursor(int index) { + frameCursor = index; + return limitCursor(); + } + + public int incrementCursor(int increment) { + frameCursor += increment; + return limitCursor(); + } + + public int incrementCursor() { + return incrementCursor(1); + } + + public int getCursor() { + return frameCursor; + } + + public int getFrameCount() { + return frames.size(); + } + + public TrackerFrame addFrame(int index, TrackerFrame trackerFrame) { + frames.add(index, trackerFrame); + return trackerFrame; + } + + public TrackerFrame addFrame(int index, Tracker tracker) { + return addFrame(index, TrackerFrame.fromTracker(tracker)); + } + + public TrackerFrame addFrame(TrackerFrame trackerFrame) { + frames.add(trackerFrame); + return trackerFrame; + } + + public TrackerFrame addFrame(Tracker tracker) { + return addFrame(TrackerFrame.fromTracker(tracker)); + } + + public TrackerFrame removeFrame(int index) { + TrackerFrame trackerFrame = frames.remove(index); + limitCursor(); + return trackerFrame; + } + + public TrackerFrame removeFrame(TrackerFrame trackerFrame) { + frames.remove(trackerFrame); + limitCursor(); + return trackerFrame; + } + + public void clearFrames() { + frames.clear(); + limitCursor(); + } + + public void fakeClearFrames() { + frames.fakeClear(); + limitCursor(); + } + + public TrackerFrame getFrame(int index) { + return frames.get(index); + } + + public TrackerFrame getFrame() { + return getFrame(frameCursor); + } + + public TrackerFrame safeGetFrame(int index) { + try { + return getFrame(index); + } catch (Exception e) { + return null; + } + } + + public TrackerFrame safeGetFrame() { + return safeGetFrame(frameCursor); + } + + //#region Tracker Interface Implementation + @Override + public boolean getRotation(Quaternion store) { + TrackerFrame frame = safeGetFrame(); + if (frame != null && frame.hasData(TrackerFrameData.ROTATION)) { + store.set(frame.rotation); + return true; + } + + store.set(0, 0, 0, 1); + return false; + } + + @Override + public boolean getPosition(Vector3f store) { + TrackerFrame frame = safeGetFrame(); + if (frame != null && frame.hasData(TrackerFrameData.POSITION)) { + store.set(frame.position); + return true; + } + + store.set(0, 0, 0); + return false; + } + + @Override + public String getName() { + return name; + } + + @Override + public TrackerStatus getStatus() { + return TrackerStatus.OK; + } + + @Override + public void loadConfig(TrackerConfig config) { + throw new UnsupportedOperationException("PoseFrameTracker does not implement configuration"); + } + + @Override + public void saveConfig(TrackerConfig config) { + throw new UnsupportedOperationException("PoseFrameTracker does not implement configuration"); + } + + @Override + public float getConfidenceLevel() { + return 0; + } + + @Override + public void resetFull(Quaternion reference) { + throw new UnsupportedOperationException("PoseFrameTracker does not implement calibration"); + } + + @Override + public void resetYaw(Quaternion reference) { + throw new UnsupportedOperationException("PoseFrameTracker does not implement calibration"); + } + + @Override + public void tick() { + throw new UnsupportedOperationException("PoseFrameTracker does not implement this method"); + } + + @Override + public TrackerBodyPosition getBodyPosition() { + TrackerFrame frame = safeGetFrame(); + return frame == null ? null : frame.designation; + } + + @Override + public void setBodyPosition(TrackerBodyPosition position) { + throw new UnsupportedOperationException("PoseFrameTracker does not allow setting the body position"); + } + + @Override + public boolean userEditable() { + return false; + } + + @Override + public boolean hasRotation() { + TrackerFrame frame = safeGetFrame(); + return frame != null && frame.hasData(TrackerFrameData.ROTATION); + } + + @Override + public boolean hasPosition() { + TrackerFrame frame = safeGetFrame(); + return frame != null && frame.hasData(TrackerFrameData.POSITION); + } + + @Override + public boolean isComputed() { + return true; + } + //#endregion + + @Override + public Iterator iterator() { + return frames.iterator(); + } +} diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseRecorder.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseRecorder.java new file mode 100644 index 000000000..14150b09c --- /dev/null +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseRecorder.java @@ -0,0 +1,163 @@ +package dev.slimevr.vr.poserecorder; + +import java.util.List; +import java.util.concurrent.CompletableFuture; +import java.util.concurrent.ExecutionException; +import java.util.concurrent.Future; + +import org.apache.commons.lang3.tuple.Pair; + +import io.eiren.util.ann.VRServerThread; +import io.eiren.util.collections.FastList; +import io.eiren.util.logging.LogManager; +import io.eiren.vr.VRServer; +import io.eiren.vr.trackers.Tracker; + +public class PoseRecorder { + + protected PoseFrame poseFrame = null; + + protected int numFrames = -1; + protected int frameCursor = 0; + protected long frameRecordingInterval = 60L; + protected long nextFrameTimeMs = -1L; + + protected CompletableFuture currentRecording; + + protected final VRServer server; + FastList> trackers = new FastList>(); + + public PoseRecorder(VRServer server) { + this.server = server; + server.addOnTick(this::onTick); + } + + @VRServerThread + public void onTick() { + if (numFrames > 0) { + PoseFrame poseFrame = this.poseFrame; + List> trackers = this.trackers; + if (poseFrame != null && trackers != null) { + if (frameCursor < numFrames) { + if (System.currentTimeMillis() >= nextFrameTimeMs) { + nextFrameTimeMs = System.currentTimeMillis() + frameRecordingInterval; + + int cursor = frameCursor++; + for (Pair tracker : trackers) { + // Add a frame for each tracker + tracker.getRight().addFrame(cursor, tracker.getLeft()); + } + + // If done, send finished recording + if (frameCursor >= numFrames) { + internalStopRecording(); + } + } + } else { + // If done and hasn't yet, send finished recording + internalStopRecording(); + } + } + } + } + + public synchronized Future startFrameRecording(int numFrames, long interval) { + return startFrameRecording(numFrames, interval, server.getAllTrackers()); + } + + public synchronized Future startFrameRecording(int numFrames, long interval, List trackers) { + if (numFrames < 1) { + throw new IllegalArgumentException("numFrames must at least have a value of 1"); + } + if (interval < 1) { + throw new IllegalArgumentException("interval must at least have a value of 1"); + } + if (trackers == null) { + throw new IllegalArgumentException("trackers must not be null"); + } + if (trackers.isEmpty()) { + throw new IllegalArgumentException("trackers must have at least one entry"); + } + if (!isReadyToRecord()) { + throw new IllegalStateException("PoseRecorder isn't ready to record!"); + } + + cancelFrameRecording(); + + poseFrame = new PoseFrame(trackers.size()); + + // Update tracker list + this.trackers.ensureCapacity(trackers.size()); + for (Tracker tracker : trackers) { + // Ignore null and computed trackers + if (tracker == null || tracker.isComputed()) { + continue; + } + + // Pair tracker with recording + this.trackers.add(Pair.of(tracker, poseFrame.addTracker(tracker, numFrames))); + } + + this.frameCursor = 0; + this.numFrames = numFrames; + + frameRecordingInterval = interval; + nextFrameTimeMs = -1L; + + LogManager.log.info("[PoseRecorder] Recording " + numFrames + " samples at a " + interval + " ms frame interval"); + + currentRecording = new CompletableFuture(); + return currentRecording; + } + + private void internalStopRecording() { + CompletableFuture currentRecording = this.currentRecording; + if (currentRecording != null && !currentRecording.isDone()) { + // Stop the recording, returning the frames recorded + currentRecording.complete(poseFrame); + } + + numFrames = -1; + frameCursor = 0; + trackers.clear(); + poseFrame = null; + } + + public synchronized void stopFrameRecording() { + internalStopRecording(); + } + + public synchronized void cancelFrameRecording() { + CompletableFuture currentRecording = this.currentRecording; + if (currentRecording != null && !currentRecording.isDone()) { + // Cancel the current recording and return nothing + currentRecording.cancel(true); + } + + numFrames = -1; + frameCursor = 0; + trackers.clear(); + poseFrame = null; + } + + public boolean isReadyToRecord() { + return server.getTrackersCount() > 0; + } + + public boolean isRecording() { + return numFrames > frameCursor; + } + + public boolean hasRecording() { + return currentRecording != null; + } + + public Future getFramesAsync() { + return currentRecording; + } + + public PoseFrame getFrames() throws ExecutionException, InterruptedException { + CompletableFuture currentRecording = this.currentRecording; + return currentRecording != null ? currentRecording.get() : null; + } +} diff --git a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java new file mode 100644 index 000000000..fc9a606af --- /dev/null +++ b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java @@ -0,0 +1,173 @@ +package dev.slimevr.vr.poserecorder; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +import io.eiren.vr.processor.TrackerBodyPosition; +import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerConfig; +import io.eiren.vr.trackers.TrackerStatus; + +public final class TrackerFrame implements Tracker { + + private int dataFlags = 0; + + public final TrackerBodyPosition designation; + public final Quaternion rotation; + public final Vector3f position; + + public TrackerFrame(TrackerBodyPosition designation, Quaternion rotation, Vector3f position) { + this.designation = designation; + if (designation != null) { + dataFlags |= TrackerFrameData.DESIGNATION.flag; + } + + this.rotation = rotation; + if (rotation != null) { + dataFlags |= TrackerFrameData.ROTATION.flag; + } + + this.position = position; + if (position != null) { + dataFlags |= TrackerFrameData.POSITION.flag; + } + } + + public static TrackerFrame fromTracker(Tracker tracker) { + if (tracker == null) { + return null; + } + + // If the tracker is not ready + if (tracker.getStatus() != TrackerStatus.OK && tracker.getStatus() != TrackerStatus.BUSY && tracker.getStatus() != TrackerStatus.OCCLUDED) { + return null; + } + + // If tracker has no data + if (tracker.getBodyPosition() == null && !tracker.hasRotation() && !tracker.hasPosition()) { + return null; + } + + Quaternion rotation = null; + if (tracker.hasRotation()) { + rotation = new Quaternion(); + if (!tracker.getRotation(rotation)) { + // If getting the rotation failed, set it back to null + rotation = null; + } + } + + Vector3f position = null; + if (tracker.hasPosition()) { + position = new Vector3f(); + if (!tracker.getPosition(position)) { + // If getting the position failed, set it back to null + position = null; + } + } + + return new TrackerFrame(tracker.getBodyPosition(), rotation, position); + } + + public int getDataFlags() { + return dataFlags; + } + + public boolean hasData(TrackerFrameData flag) { + return flag.check(dataFlags); + } + + //#region Tracker Interface Implementation + @Override + public boolean getRotation(Quaternion store) { + if (hasData(TrackerFrameData.ROTATION)) { + store.set(rotation); + return true; + } + + store.set(0, 0, 0, 1); + return false; + } + + @Override + public boolean getPosition(Vector3f store) { + if (hasData(TrackerFrameData.POSITION)) { + store.set(position); + return true; + } + + store.set(0, 0, 0); + return false; + } + + @Override + public String getName() { + return "TrackerFrame:/" + (designation != null ? designation.designation : "null"); + } + + @Override + public TrackerStatus getStatus() { + return TrackerStatus.OK; + } + + @Override + public void loadConfig(TrackerConfig config) { + throw new UnsupportedOperationException("TrackerFrame does not implement configuration"); + } + + @Override + public void saveConfig(TrackerConfig config) { + throw new UnsupportedOperationException("TrackerFrame does not implement configuration"); + } + + @Override + public float getConfidenceLevel() { + return 0; + } + + @Override + public void resetFull(Quaternion reference) { + throw new UnsupportedOperationException("TrackerFrame does not implement calibration"); + } + + @Override + public void resetYaw(Quaternion reference) { + throw new UnsupportedOperationException("TrackerFrame does not implement calibration"); + } + + @Override + public void tick() { + throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + } + + @Override + public TrackerBodyPosition getBodyPosition() { + return designation; + } + + @Override + public void setBodyPosition(TrackerBodyPosition position) { + throw new UnsupportedOperationException("TrackerFrame does not allow setting the body position"); + } + + @Override + public boolean userEditable() { + return false; + } + + @Override + public boolean hasRotation() { + return hasData(TrackerFrameData.ROTATION); + } + + @Override + public boolean hasPosition() { + return hasData(TrackerFrameData.POSITION); + } + + @Override + public boolean isComputed() { + return true; + } + //#endregion +} diff --git a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrameData.java b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrameData.java new file mode 100644 index 000000000..7282b973e --- /dev/null +++ b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrameData.java @@ -0,0 +1,17 @@ +package dev.slimevr.vr.poserecorder; + +public enum TrackerFrameData { + DESIGNATION(0), + ROTATION(1), + POSITION(2); + + public final int flag; + + TrackerFrameData(int id) { + this.flag = 1 << id; + } + + public boolean check(int dataFlags) { + return (dataFlags & this.flag) != 0; + } +} diff --git a/src/main/java/io/eiren/gui/SkeletonConfig.java b/src/main/java/io/eiren/gui/SkeletonConfig.java index 03858271a..17b61d1e9 100644 --- a/src/main/java/io/eiren/gui/SkeletonConfig.java +++ b/src/main/java/io/eiren/gui/SkeletonConfig.java @@ -11,7 +11,7 @@ import javax.swing.JCheckBox; import javax.swing.JLabel; import javax.swing.event.MouseInputAdapter; -import io.eiren.gui.autobone.AutoBoneWindow; +import dev.slimevr.gui.AutoBoneWindow; import io.eiren.util.StringUtils; import io.eiren.util.ann.ThreadSafe; import io.eiren.vr.VRServer; diff --git a/src/main/java/io/eiren/gui/autobone/PoseFrame.java b/src/main/java/io/eiren/gui/autobone/PoseFrame.java deleted file mode 100644 index 81b1d75d5..000000000 --- a/src/main/java/io/eiren/gui/autobone/PoseFrame.java +++ /dev/null @@ -1,37 +0,0 @@ -package io.eiren.gui.autobone; - -import java.util.HashMap; - -import com.jme3.math.Quaternion; -import com.jme3.math.Vector3f; - -import io.eiren.vr.processor.HumanSkeletonWithLegs; -import io.eiren.vr.processor.TransformNode; - -public final class PoseFrame { - - public final Vector3f rootPos; - public final HashMap rotations; - public final HashMap positions; - - public PoseFrame(Vector3f rootPos, HashMap rotations, HashMap positions) { - this.rootPos = rootPos; - this.rotations = rotations; - this.positions = positions; - } - - public PoseFrame(HumanSkeletonWithLegs skeleton) { - // Copy headset position - TransformNode rootNode = skeleton.getRootNode(); - this.rootPos = new Vector3f(rootNode.localTransform.getTranslation()); - - // Copy all rotations - this.rotations = new HashMap(); - rootNode.depthFirstTraversal(visitor -> { - // Insert a copied quaternion so it isn't changed by reference - rotations.put(visitor.getName(), new Quaternion(visitor.localTransform.getRotation())); - }); - - this.positions = null; - } -} diff --git a/src/main/java/io/eiren/gui/autobone/PoseFrameIO.java b/src/main/java/io/eiren/gui/autobone/PoseFrameIO.java deleted file mode 100644 index 56cdf9875..000000000 --- a/src/main/java/io/eiren/gui/autobone/PoseFrameIO.java +++ /dev/null @@ -1,174 +0,0 @@ -package io.eiren.gui.autobone; - -import java.io.BufferedInputStream; -import java.io.BufferedOutputStream; -import java.io.DataInputStream; -import java.io.DataOutputStream; -import java.io.File; -import java.io.FileInputStream; -import java.io.FileOutputStream; -import java.util.HashMap; -import java.util.Map.Entry; - -import com.jme3.math.Quaternion; -import com.jme3.math.Vector3f; - -import io.eiren.util.logging.LogManager; - -public final class PoseFrameIO { - - private PoseFrameIO() { - // Do not allow instantiating - } - - public static boolean writeToFile(File file, PoseFrame[] frames) { - try (DataOutputStream outputStream = new DataOutputStream(new BufferedOutputStream(new FileOutputStream(file)))) { - // Write every frame - outputStream.writeInt(frames.length); - for (PoseFrame frame : frames) { - writeFrame(outputStream, frame); - } - } catch (Exception e) { - LogManager.log.severe("Error writing frames to file", e); - return false; - } - - return true; - } - - public static boolean writeFrame(DataOutputStream outputStream, PoseFrame frame) { - try { - // Write root position vector - outputStream.writeFloat(frame.rootPos.x); - outputStream.writeFloat(frame.rootPos.y); - outputStream.writeFloat(frame.rootPos.z); - - if (frame.rotations != null) { - // Write rotations - outputStream.writeInt(frame.rotations.size()); - for (Entry entry : frame.rotations.entrySet()) { - // Write the label string - outputStream.writeUTF(entry.getKey()); - - // Write the rotation quaternion - Quaternion quat = entry.getValue(); - outputStream.writeFloat(quat.getX()); - outputStream.writeFloat(quat.getY()); - outputStream.writeFloat(quat.getZ()); - outputStream.writeFloat(quat.getW()); - } - } else { - outputStream.writeInt(0); - } - - if (frame.positions != null) { - // Write positions - outputStream.writeInt(frame.positions.size()); - for (Entry entry : frame.positions.entrySet()) { - // Write the label string - outputStream.writeUTF(entry.getKey()); - - // Write the rotation quaternion - Vector3f vec = entry.getValue(); - outputStream.writeFloat(vec.getX()); - outputStream.writeFloat(vec.getY()); - outputStream.writeFloat(vec.getZ()); - } - } else { - outputStream.writeInt(0); - } - } catch (Exception e) { - LogManager.log.severe("Error writing frame to stream", e); - return false; - } - - return true; - } - - public static boolean writeFrame(File file, PoseFrame frame) { - try (DataOutputStream outputStream = new DataOutputStream(new BufferedOutputStream(new FileOutputStream(file)))) { - writeFrame(outputStream, frame); - } catch (Exception e) { - LogManager.log.severe("Error writing frame to file", e); - return false; - } - - return true; - } - - public static PoseFrame[] readFromFile(File file) { - try (DataInputStream inputStream = new DataInputStream(new BufferedInputStream(new FileInputStream(file)))) { - int frameCount = inputStream.readInt(); - - PoseFrame[] frames = new PoseFrame[frameCount]; - for (int i = 0; i < frameCount; i++) { - frames[i] = readFrame(inputStream); - } - - return frames; - } catch (Exception e) { - LogManager.log.severe("Error reading frames from file", e); - } - - return null; - } - - public static PoseFrame readFrame(DataInputStream inputStream) { - try { - float vecX = inputStream.readFloat(); - float vecY = inputStream.readFloat(); - float vecZ = inputStream.readFloat(); - - Vector3f vector = new Vector3f(vecX, vecY, vecZ); - - int rotationCount = inputStream.readInt(); - HashMap rotations = null; - if (rotationCount > 0) { - rotations = new HashMap(rotationCount); - for (int j = 0; j < rotationCount; j++) { - String label = inputStream.readUTF(); - - float quatX = inputStream.readFloat(); - float quatY = inputStream.readFloat(); - float quatZ = inputStream.readFloat(); - float quatW = inputStream.readFloat(); - Quaternion quaternion = new Quaternion(quatX, quatY, quatZ, quatW); - - rotations.put(label, quaternion); - } - } - - int positionCount = inputStream.readInt(); - HashMap positions = null; - if (positionCount > 0) { - positions = new HashMap(positionCount); - for (int j = 0; j < positionCount; j++) { - String label = inputStream.readUTF(); - - float posX = inputStream.readFloat(); - float posY = inputStream.readFloat(); - float posZ = inputStream.readFloat(); - Vector3f position = new Vector3f(posX, posY, posZ); - - positions.put(label, position); - } - } - - return new PoseFrame(vector, rotations, positions); - } catch (Exception e) { - LogManager.log.severe("Error reading frame from stream", e); - } - - return null; - } - - public static PoseFrame readFrame(File file) { - try { - return readFrame(new DataInputStream(new BufferedInputStream(new FileInputStream(file)))); - } catch (Exception e) { - LogManager.log.severe("Error reading frame from file", e); - } - - return null; - } -} diff --git a/src/main/java/io/eiren/gui/autobone/PoseRecorder.java b/src/main/java/io/eiren/gui/autobone/PoseRecorder.java deleted file mode 100644 index 5af0ba186..000000000 --- a/src/main/java/io/eiren/gui/autobone/PoseRecorder.java +++ /dev/null @@ -1,136 +0,0 @@ -package io.eiren.gui.autobone; - -import java.util.concurrent.CompletableFuture; -import java.util.concurrent.ExecutionException; -import java.util.concurrent.Future; - -import io.eiren.util.ann.ThreadSafe; -import io.eiren.util.ann.VRServerThread; -import io.eiren.util.collections.FastList; -import io.eiren.util.logging.LogManager; -import io.eiren.vr.VRServer; -import io.eiren.vr.processor.HumanSkeleton; -import io.eiren.vr.processor.HumanSkeletonWithLegs; - -public class PoseRecorder { - - protected final FastList frames = new FastList(); - - protected int numFrames = -1; - protected long frameRecordingInterval = 60L; - protected long nextFrameTimeMs = -1L; - - protected CompletableFuture currentRecording; - - protected final VRServer server; - HumanSkeletonWithLegs skeleton = null; - - public PoseRecorder(VRServer server) { - this.server = server; - server.addOnTick(this::onTick); - server.addSkeletonUpdatedCallback(this::skeletonUpdated); - } - - @VRServerThread - public void onTick() { - if (numFrames > 0) { - HumanSkeletonWithLegs skeleton = this.skeleton; - if (skeleton != null) { - if (frames.size() < numFrames) { - if (System.currentTimeMillis() >= nextFrameTimeMs) { - nextFrameTimeMs = System.currentTimeMillis() + frameRecordingInterval; - frames.add(new PoseFrame(skeleton)); - - // If done, send finished recording - if (frames.size() >= numFrames) { - internalStopRecording(); - } - } - } else { - // If done and hasn't yet, send finished recording - internalStopRecording(); - } - } - } - } - - @ThreadSafe - public void skeletonUpdated(HumanSkeleton newSkeleton) { - if (newSkeleton instanceof HumanSkeletonWithLegs) { - skeleton = (HumanSkeletonWithLegs) newSkeleton; - } - } - - public synchronized Future startFrameRecording(int numFrames, long interval) { - if (numFrames < 1) { - throw new IllegalArgumentException("numFrames must at least have a value of 1"); - } - if (interval < 1) { - throw new IllegalArgumentException("interval must at least have a value of 1"); - } - if (!isReadyToRecord()) { - throw new IllegalStateException("PoseRecorder isn't ready to record!"); - } - - cancelFrameRecording(); - - // Clear old frames and ensure new size can be held - frames.clear(); - frames.ensureCapacity(numFrames); - - this.numFrames = numFrames; - - frameRecordingInterval = interval; - nextFrameTimeMs = -1L; - - LogManager.log.info("[PoseRecorder] Recording " + numFrames + " samples at a " + interval + " ms frame interval"); - - currentRecording = new CompletableFuture(); - return currentRecording; - } - - private void internalStopRecording() { - CompletableFuture currentRecording = this.currentRecording; - if (currentRecording != null && !currentRecording.isDone()) { - // Stop the recording, returning the frames recorded - currentRecording.complete(frames.toArray(new PoseFrame[0])); - } - - numFrames = -1; - } - - public synchronized void stopFrameRecording() { - internalStopRecording(); - } - - public synchronized void cancelFrameRecording() { - CompletableFuture currentRecording = this.currentRecording; - if (currentRecording != null && !currentRecording.isDone()) { - // Cancel the current recording and return nothing - currentRecording.cancel(true); - } - - numFrames = -1; - } - - public boolean isReadyToRecord() { - return skeleton != null; - } - - public boolean isRecording() { - return numFrames > frames.size(); - } - - public boolean hasRecording() { - return currentRecording != null; - } - - public Future getFramesAsync() { - return currentRecording; - } - - public PoseFrame[] getFrames() throws ExecutionException, InterruptedException { - CompletableFuture currentRecording = this.currentRecording; - return currentRecording != null ? currentRecording.get() : null; - } -} diff --git a/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java b/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java index 9ca588cc9..3780953ae 100644 --- a/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java +++ b/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java @@ -34,6 +34,6 @@ public enum TrackerBodyPosition { static { for(TrackerBodyPosition tbp : values()) - byDesignation.put(tbp.designation, tbp); + byDesignation.put(tbp.designation.toLowerCase(), tbp); } } \ No newline at end of file diff --git a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java index eddf2ff12..19da58688 100644 --- a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java +++ b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java @@ -28,7 +28,10 @@ public class ComputedTracker implements Tracker { @Override public void loadConfig(TrackerConfig config) { - bodyPosition = TrackerBodyPosition.getByDesignation(config.designation); + // Loading a config is an act of user editing, therefore it shouldn't not be allowed if editing is not allowed + if (userEditable()) { + bodyPosition = TrackerBodyPosition.getByDesignation(config.designation); + } } @Override @@ -98,4 +101,9 @@ public class ComputedTracker implements Tracker { public boolean hasPosition() { return hasPosition; } + + @Override + public boolean isComputed() { + return true; + } } diff --git a/src/main/java/io/eiren/vr/trackers/HMDTracker.java b/src/main/java/io/eiren/vr/trackers/HMDTracker.java index 69de748fd..cb26402e6 100644 --- a/src/main/java/io/eiren/vr/trackers/HMDTracker.java +++ b/src/main/java/io/eiren/vr/trackers/HMDTracker.java @@ -21,4 +21,9 @@ public class HMDTracker extends ComputedTracker implements TrackerWithTPS { public void dataTick() { timer.update(); } + + @Override + public boolean isComputed() { + return false; + } } diff --git a/src/main/java/io/eiren/vr/trackers/IMUTracker.java b/src/main/java/io/eiren/vr/trackers/IMUTracker.java index ff032204b..e1e5d3adc 100644 --- a/src/main/java/io/eiren/vr/trackers/IMUTracker.java +++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java @@ -52,17 +52,20 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { @Override public void loadConfig(TrackerConfig config) { - if(config.mountingRotation != null) { - mounting = TrackerMountingRotation.valueOf(config.mountingRotation); - if(mounting != null) { - rotAdjust.set(mounting.quaternion); + // Loading a config is an act of user editing, therefore it shouldn't not be allowed if editing is not allowed + if (userEditable()) { + if(config.mountingRotation != null) { + mounting = TrackerMountingRotation.valueOf(config.mountingRotation); + if(mounting != null) { + rotAdjust.set(mounting.quaternion); + } else { + rotAdjust.loadIdentity(); + } } else { rotAdjust.loadIdentity(); } - } else { - rotAdjust.loadIdentity(); + bodyPosition = TrackerBodyPosition.getByDesignation(config.designation); } - bodyPosition = TrackerBodyPosition.getByDesignation(config.designation); } public TrackerMountingRotation getMountingRotation() { @@ -235,4 +238,9 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { public boolean hasPosition() { return false; } + + @Override + public boolean isComputed() { + return false; + } } diff --git a/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java b/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java index b3e6f85a6..1f124b672 100644 --- a/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java +++ b/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java @@ -156,4 +156,9 @@ public class ReferenceAdjustedTracker implements Tracker { public boolean hasPosition() { return tracker.hasPosition(); } -} \ No newline at end of file + + @Override + public boolean isComputed() { + return tracker.isComputed(); + } +} diff --git a/src/main/java/io/eiren/vr/trackers/SteamVRTracker.java b/src/main/java/io/eiren/vr/trackers/SteamVRTracker.java index 88029b652..9fa4a1050 100644 --- a/src/main/java/io/eiren/vr/trackers/SteamVRTracker.java +++ b/src/main/java/io/eiren/vr/trackers/SteamVRTracker.java @@ -26,4 +26,9 @@ public class SteamVRTracker extends ComputedTracker implements TrackerWithTPS { public boolean userEditable() { return true; } + + @Override + public boolean isComputed() { + return false; + } } diff --git a/src/main/java/io/eiren/vr/trackers/Tracker.java b/src/main/java/io/eiren/vr/trackers/Tracker.java index 569ec6da6..7e87308c8 100644 --- a/src/main/java/io/eiren/vr/trackers/Tracker.java +++ b/src/main/java/io/eiren/vr/trackers/Tracker.java @@ -36,4 +36,6 @@ public interface Tracker { public boolean hasRotation(); public boolean hasPosition(); + + public boolean isComputed(); } diff --git a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java index 237297fd8..58cf21143 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java +++ b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java @@ -8,24 +8,50 @@ public class TrackerUtils { private TrackerUtils() { } - - public static Tracker findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position) { - for(int i = 0; i < allTrackers.size(); ++i) { - Tracker t = allTrackers.get(i); - if(t.getBodyPosition() == position) + + public static T findTrackerForBodyPosition(T[] allTrackers, TrackerBodyPosition position) { + for(int i = 0; i < allTrackers.length; ++i) { + T t = allTrackers[i]; + if(t != null && t.getBodyPosition() == position) return t; } return null; } - - public static Tracker findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { - Tracker t = findTrackerForBodyPosition(allTrackers, position); + + public static T findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position) { + for(int i = 0; i < allTrackers.size(); ++i) { + T t = allTrackers.get(i); + if(t != null && t.getBodyPosition() == position) + return t; + } + return null; + } + + public static T findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { + T t = findTrackerForBodyPosition(allTrackers, position); if(t != null) return t; return findTrackerForBodyPosition(allTrackers, altPosition); } - - public static Tracker findTrackerForBodyPositionOrEmpty(List allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { + + public static T findTrackerForBodyPosition(T[] allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { + T t = findTrackerForBodyPosition(allTrackers, position); + if(t != null) + return t; + return findTrackerForBodyPosition(allTrackers, altPosition); + } + + public static Tracker findTrackerForBodyPositionOrEmpty(List allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { + Tracker t = findTrackerForBodyPosition(allTrackers, position); + if(t != null) + return t; + t = findTrackerForBodyPosition(allTrackers, altPosition); + if(t != null) + return t; + return new ComputedTracker("Empty tracker", false, false); + } + + public static Tracker findTrackerForBodyPositionOrEmpty(Tracker[] allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { Tracker t = findTrackerForBodyPosition(allTrackers, position); if(t != null) return t;