From 5b0f8afa4e77a6ec3e558c4ade7aad4a91798b46 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Sun, 12 Sep 2021 09:07:09 -0400 Subject: [PATCH 01/16] Change namespaces, change PoseRecorder format, use TrackerBodyPosition for designation --- .../slimevr/gui}/AutoBoneWindow.java | 19 +++- .../slimevr/vr}/autobone/AutoBone.java | 94 ++++++++++-------- .../slimevr/vr}/autobone/SimpleSkeleton.java | 99 +++++++++++-------- .../slimevr/vr/poserecorder/PoseFrame.java | 44 +++++++++ .../slimevr/vr/poserecorder}/PoseFrameIO.java | 95 +++++++++++------- .../vr/poserecorder}/PoseRecorder.java | 40 ++++---- .../slimevr/vr/poserecorder/TrackerFrame.java | 70 +++++++++++++ .../vr/poserecorder/TrackerFrameData.java | 17 ++++ .../java/io/eiren/gui/SkeletonConfig.java | 2 +- .../java/io/eiren/gui/autobone/PoseFrame.java | 37 ------- 10 files changed, 343 insertions(+), 174 deletions(-) rename src/main/java/{io/eiren/gui/autobone => dev/slimevr/gui}/AutoBoneWindow.java (97%) rename src/main/java/{io/eiren/gui => dev/slimevr/vr}/autobone/AutoBone.java (85%) rename src/main/java/{io/eiren/gui => dev/slimevr/vr}/autobone/SimpleSkeleton.java (79%) create mode 100644 src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java rename src/main/java/{io/eiren/gui/autobone => dev/slimevr/vr/poserecorder}/PoseFrameIO.java (62%) rename src/main/java/{io/eiren/gui/autobone => dev/slimevr/vr/poserecorder}/PoseRecorder.java (81%) create mode 100644 src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java create mode 100644 src/main/java/dev/slimevr/vr/poserecorder/TrackerFrameData.java delete mode 100644 src/main/java/io/eiren/gui/autobone/PoseFrame.java diff --git a/src/main/java/io/eiren/gui/autobone/AutoBoneWindow.java b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java similarity index 97% rename from src/main/java/io/eiren/gui/autobone/AutoBoneWindow.java rename to src/main/java/dev/slimevr/gui/AutoBoneWindow.java index 75a1d9841..8b4f65ff4 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"); @@ -82,7 +88,7 @@ public class AutoBoneWindow extends JFrame { 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() + "\"..."); @@ -102,7 +108,7 @@ public class AutoBoneWindow extends JFrame { 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); @@ -230,6 +236,13 @@ public class AutoBoneWindow extends JFrame { 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..."); diff --git a/src/main/java/io/eiren/gui/autobone/AutoBone.java b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java similarity index 85% rename from src/main/java/io/eiren/gui/autobone/AutoBone.java rename to src/main/java/dev/slimevr/vr/autobone/AutoBone.java index 5e0eaa5d8..9027fbce0 100644 --- a/src/main/java/io/eiren/gui/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java @@ -1,4 +1,4 @@ -package io.eiren.gui.autobone; +package dev.slimevr.vr.autobone; import java.util.HashMap; import java.util.Map; @@ -7,10 +7,13 @@ 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; @@ -200,8 +203,9 @@ public class AutoBone { public float getMaxHmdHeight(PoseFrame[] frames) { float maxHeight = 0f; for (PoseFrame frame : frames) { - if (frame.rootPos.y > maxHeight) { - maxHeight = frame.rootPos.y; + TrackerFrame hmd = frame.trackerFrames.get(TrackerBodyPosition.HMD); + if (hmd != null && hmd.hasData(TrackerFrameData.POSITION) && hmd.position.y > maxHeight) { + maxHeight = hmd.position.y; } } return maxHeight; @@ -356,8 +360,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 +369,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; @@ -404,13 +414,15 @@ public class AutoBone { 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.trackerFrames.values()) { + if (!trackerFrame.hasData(TrackerFrameData.POSITION)) { + continue; + } + + Vector3f nodePos = skeleton.getNodePosition(trackerFrame.designation.designation); + if (nodePos != null) { + offset += Math.abs(nodePos.distance(trackerFrame.position)); + offsetCount++; } } @@ -422,29 +434,31 @@ public class AutoBone { 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); - offsetCount++; + for (TrackerFrame trackerFrame1 : frame1.trackerFrames.values()) { + if (!trackerFrame1.hasData(TrackerFrameData.POSITION)) { + continue; } + + TrackerFrame trackerFrame2 = frame2.trackerFrames.get(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; diff --git a/src/main/java/io/eiren/gui/autobone/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java similarity index 79% rename from src/main/java/io/eiren/gui/autobone/SimpleSkeleton.java rename to src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java index 7cc4f3a89..4b0b362e3 100644 --- a/src/main/java/io/eiren/gui/autobone/SimpleSkeleton.java +++ b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java @@ -1,14 +1,17 @@ -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.PoseFrame; +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.yaml.YamlFile; @@ -121,35 +124,18 @@ public class SimpleSkeleton { this(configs.entrySet()); } - public void setPoseFromSkeleton(HumanSkeletonWithLegs humanSkeleton) { - TransformNode rootNode = humanSkeleton.getRootNode(); - - // 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()); - } - }); - } - public void setPoseFromFrame(PoseFrame frame) { - // Copy headset position - hmdNode.localTransform.setTranslation(frame.rootPos); + for (TrackerFrame trackerFrame : frame.trackerFrames.values()) { + // Set HMD position + if (trackerFrame.designation == TrackerBodyPosition.HMD && trackerFrame.hasData(TrackerFrameData.POSITION)) { + hmdNode.localTransform.setTranslation(trackerFrame.position); + } - 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()); + // Set joint rotations + if (trackerFrame.hasData(TrackerFrameData.ROTATION)) { + TransformNode node = getNode(trackerFrame.designation, true); + if (node != null) { + node.localTransform.setRotation(trackerFrame.rotation); } } } @@ -256,24 +242,59 @@ public class SimpleSkeleton { } public void updatePose() { + // Rotate hips with waist + waistNode.localTransform.setRotation(chestNode.localTransform.getRotation()); hmdNode.update(); } - public Vector3f getNodePosition(String node) { + public TransformNode getNode(String node) { TransformNode transformNode = nodes.get(node); + return transformNode; + } + + 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 = 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..3bebc2c3e --- /dev/null +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java @@ -0,0 +1,44 @@ +package dev.slimevr.vr.poserecorder; + +import java.util.HashMap; +import java.util.List; + +import io.eiren.vr.processor.TrackerBodyPosition; +import io.eiren.vr.trackers.Tracker; + +public final class PoseFrame { + + public final HashMap trackerFrames; + + public PoseFrame(HashMap trackerFrames) { + this.trackerFrames = trackerFrames; + } + + public PoseFrame(List trackerFrames) { + HashMap trackerFramesMap = new HashMap(trackerFrames.size()); + + for (TrackerFrame trackerFrame : trackerFrames) { + trackerFramesMap.put(trackerFrame.designation, trackerFrame); + } + + this.trackerFrames = trackerFramesMap; + } + + public static PoseFrame fromTrackers(List trackers) { + if (trackers == null || trackers.isEmpty()) { + return null; + } + + HashMap trackerFrames = new HashMap(trackers.size()); + + for (Tracker tracker : trackers) { + TrackerFrame trackerFrame = TrackerFrame.fromTracker(tracker); + + if (trackerFrame != null) { + trackerFrames.put(trackerFrame.designation, trackerFrame); + } + } + + return new PoseFrame(trackerFrames); + } +} diff --git a/src/main/java/io/eiren/gui/autobone/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java similarity index 62% rename from src/main/java/io/eiren/gui/autobone/PoseFrameIO.java rename to src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java index 56cdf9875..fb3c3036a 100644 --- a/src/main/java/io/eiren/gui/autobone/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -1,4 +1,4 @@ -package io.eiren.gui.autobone; +package dev.slimevr.vr.poserecorder; import java.io.BufferedInputStream; import java.io.BufferedOutputStream; @@ -7,13 +7,14 @@ 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 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 { @@ -38,41 +39,28 @@ public final class PoseFrameIO { 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 != null && frame.trackerFrames != null) { + outputStream.writeInt(frame.trackerFrames.size()); - 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()); + for (TrackerFrame trackerFrame : frame.trackerFrames.values()) { + outputStream.writeInt(trackerFrame.getDataFlags()); - // 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 (trackerFrame.hasData(TrackerFrameData.DESIGNATION)) { + outputStream.writeUTF(trackerFrame.designation.designation); + } - 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()); + 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()); + } - // Write the rotation quaternion - Vector3f vec = entry.getValue(); - outputStream.writeFloat(vec.getX()); - outputStream.writeFloat(vec.getY()); - outputStream.writeFloat(vec.getZ()); + if (trackerFrame.hasData(TrackerFrameData.POSITION)) { + outputStream.writeFloat(trackerFrame.position.getX()); + outputStream.writeFloat(trackerFrame.position.getY()); + outputStream.writeFloat(trackerFrame.position.getZ()); + } } } else { outputStream.writeInt(0); @@ -115,6 +103,7 @@ public final class PoseFrameIO { public static PoseFrame readFrame(DataInputStream inputStream) { try { + /* float vecX = inputStream.readFloat(); float vecY = inputStream.readFloat(); float vecZ = inputStream.readFloat(); @@ -155,6 +144,44 @@ public final class PoseFrameIO { } return new PoseFrame(vector, rotations, positions); + */ + + int trackerFrameCount = inputStream.readInt(); + if (trackerFrameCount > 0) { + List trackerFrames = new FastList(trackerFrameCount); + + for (int i = 0; i < trackerFrameCount; i++) { + 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)); + } + + return new PoseFrame(trackerFrames); + } else { + return null; + } } catch (Exception e) { LogManager.log.severe("Error reading frame from stream", e); } diff --git a/src/main/java/io/eiren/gui/autobone/PoseRecorder.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseRecorder.java similarity index 81% rename from src/main/java/io/eiren/gui/autobone/PoseRecorder.java rename to src/main/java/dev/slimevr/vr/poserecorder/PoseRecorder.java index 5af0ba186..6f427774b 100644 --- a/src/main/java/io/eiren/gui/autobone/PoseRecorder.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseRecorder.java @@ -1,16 +1,15 @@ -package io.eiren.gui.autobone; +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 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; +import io.eiren.vr.trackers.Tracker; public class PoseRecorder { @@ -23,44 +22,42 @@ public class PoseRecorder { protected CompletableFuture currentRecording; protected final VRServer server; - HumanSkeletonWithLegs skeleton = null; + List trackers = 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) { + List trackers = this.trackers; + if (trackers != null) { if (frames.size() < numFrames) { if (System.currentTimeMillis() >= nextFrameTimeMs) { nextFrameTimeMs = System.currentTimeMillis() + frameRecordingInterval; - frames.add(new PoseFrame(skeleton)); + PoseFrame frame = PoseFrame.fromTrackers(trackers); - // If done, send finished recording - if (frames.size() >= numFrames) { - internalStopRecording(); + if (frame != null) { + frames.add(frame); + + // If done, send finished recording + if (frames.size() >= numFrames) { + internalStopRecording(); + } } } } else { // If done and hasn't yet, send finished recording internalStopRecording(); } + } else { + this.trackers = server.getAllTrackers(); } } } - @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"); @@ -74,6 +71,9 @@ public class PoseRecorder { cancelFrameRecording(); + // Update tracker list + this.trackers = server.getAllTrackers(); + // Clear old frames and ensure new size can be held frames.clear(); frames.ensureCapacity(numFrames); @@ -114,7 +114,7 @@ public class PoseRecorder { } public boolean isReadyToRecord() { - return skeleton != null; + return server.getTrackersCount() > 0; } public boolean isRecording() { 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..e038a7ac6 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java @@ -0,0 +1,70 @@ +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; + +public final class TrackerFrame { + + 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 TrackerFrame(TrackerBodyPosition designation, Quaternion rotation) { + this(designation, rotation, null); + } + + public TrackerFrame(TrackerBodyPosition designation, Vector3f position) { + this(designation, null, position); + } + + public static TrackerFrame fromTracker(Tracker tracker) { + // If null or has no data + if (tracker == null || (!tracker.hasRotation() && !tracker.hasPosition())) { + return null; + } + + Quaternion rotation = null; + if (tracker.hasRotation()) { + rotation = new Quaternion(); + tracker.getRotation(rotation); + } + + Vector3f position = null; + if (tracker.hasPosition()) { + position = new Vector3f(); + tracker.getPosition(position); + } + + return new TrackerFrame(tracker.getBodyPosition(), rotation, position); + } + + public int getDataFlags() { + return dataFlags; + } + + public boolean hasData(TrackerFrameData flag) { + return flag.check(dataFlags); + } +} 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; - } -} From f6a29260334c052b861a248ad47bd78d4514b383 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Sun, 12 Sep 2021 10:03:41 -0400 Subject: [PATCH 02/16] Remove comment and useless if statement --- .../slimevr/vr/poserecorder/PoseFrameIO.java | 81 ++++--------------- 1 file changed, 17 insertions(+), 64 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java index fb3c3036a..e2ec2aad5 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -103,85 +103,38 @@ public final class PoseFrameIO { public static PoseFrame readFrame(DataInputStream inputStream) { try { - /* - float vecX = inputStream.readFloat(); - float vecY = inputStream.readFloat(); - float vecZ = inputStream.readFloat(); + int trackerFrameCount = inputStream.readInt(); - Vector3f vector = new Vector3f(vecX, vecY, vecZ); + List trackerFrames = new FastList(trackerFrameCount); + for (int i = 0; i < trackerFrameCount; i++) { + int dataFlags = inputStream.readInt(); - 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(); + 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(); - Quaternion quaternion = new Quaternion(quatX, quatY, quatZ, quatW); - - rotations.put(label, quaternion); + rotation = new Quaternion(quatX, quatY, quatZ, quatW); } - } - - 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(); + Vector3f position = null; + if (TrackerFrameData.POSITION.check(dataFlags)) { 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); - */ - - int trackerFrameCount = inputStream.readInt(); - if (trackerFrameCount > 0) { - List trackerFrames = new FastList(trackerFrameCount); - - for (int i = 0; i < trackerFrameCount; i++) { - 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)); + position = new Vector3f(posX, posY, posZ); } - return new PoseFrame(trackerFrames); - } else { - return null; + trackerFrames.add(new TrackerFrame(designation, rotation, position)); } + + return new PoseFrame(trackerFrames); } catch (Exception e) { LogManager.log.severe("Error reading frame from stream", e); } From 00e63db029fc0dcf29205b9f32a25006224877cb Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Sun, 12 Sep 2021 10:06:16 -0400 Subject: [PATCH 03/16] Use HashMap directly --- src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java index e2ec2aad5..66fd347b7 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -7,12 +7,11 @@ import java.io.DataOutputStream; import java.io.File; import java.io.FileInputStream; import java.io.FileOutputStream; -import java.util.List; +import java.util.HashMap; 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; @@ -105,7 +104,7 @@ public final class PoseFrameIO { try { int trackerFrameCount = inputStream.readInt(); - List trackerFrames = new FastList(trackerFrameCount); + HashMap trackerFrames = new HashMap(trackerFrameCount); for (int i = 0; i < trackerFrameCount; i++) { int dataFlags = inputStream.readInt(); @@ -131,7 +130,7 @@ public final class PoseFrameIO { position = new Vector3f(posX, posY, posZ); } - trackerFrames.add(new TrackerFrame(designation, rotation, position)); + trackerFrames.put(designation, new TrackerFrame(designation, rotation, position)); } return new PoseFrame(trackerFrames); From efb065f558a77208dcdaa45363b6ff90f44f988e Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Sun, 12 Sep 2021 10:24:46 -0400 Subject: [PATCH 04/16] Fix TrackerBodyPosition.getByDesignation capitalization --- src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From bc542a7bb19d581c2c6f784a1a2cb2220492e51c Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Sun, 12 Sep 2021 12:53:03 -0400 Subject: [PATCH 05/16] Don't put null designations in the trackers --- src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java index 66fd347b7..940c75ac7 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -130,7 +130,9 @@ public final class PoseFrameIO { position = new Vector3f(posX, posY, posZ); } - trackerFrames.put(designation, new TrackerFrame(designation, rotation, position)); + if (designation != null) { + trackerFrames.put(designation, new TrackerFrame(designation, rotation, position)); + } } return new PoseFrame(trackerFrames); From 45ad0698b19db166a873b9436bfebabaedea477d Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 01:06:36 -0400 Subject: [PATCH 06/16] Allow multiple TrackerFrames with the same designation in PoseFrame, make TrackerFrame extend Tracker --- .../dev/slimevr/vr/autobone/AutoBone.java | 8 +- .../slimevr/vr/autobone/SimpleSkeleton.java | 2 +- .../slimevr/vr/poserecorder/PoseFrame.java | 31 ++--- .../slimevr/vr/poserecorder/PoseFrameIO.java | 14 ++- .../slimevr/vr/poserecorder/TrackerFrame.java | 106 +++++++++++++++++- .../io/eiren/vr/trackers/TrackerUtils.java | 12 +- 6 files changed, 139 insertions(+), 34 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/autobone/AutoBone.java b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java index 9027fbce0..a6337baf7 100644 --- a/src/main/java/dev/slimevr/vr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java @@ -203,7 +203,7 @@ public class AutoBone { public float getMaxHmdHeight(PoseFrame[] frames) { float maxHeight = 0f; for (PoseFrame frame : frames) { - TrackerFrame hmd = frame.trackerFrames.get(TrackerBodyPosition.HMD); + TrackerFrame hmd = frame.findTracker(TrackerBodyPosition.HMD); if (hmd != null && hmd.hasData(TrackerFrameData.POSITION) && hmd.position.y > maxHeight) { maxHeight = hmd.position.y; } @@ -414,7 +414,7 @@ public class AutoBone { float offset = 0f; int offsetCount = 0; - for (TrackerFrame trackerFrame : frame.trackerFrames.values()) { + for (TrackerFrame trackerFrame : frame.trackerFrames) { if (!trackerFrame.hasData(TrackerFrameData.POSITION)) { continue; } @@ -434,12 +434,12 @@ public class AutoBone { float offset = 0f; int offsetCount = 0; - for (TrackerFrame trackerFrame1 : frame1.trackerFrames.values()) { + for (TrackerFrame trackerFrame1 : frame1.trackerFrames) { if (!trackerFrame1.hasData(TrackerFrameData.POSITION)) { continue; } - TrackerFrame trackerFrame2 = frame2.trackerFrames.get(trackerFrame1.designation); + TrackerFrame trackerFrame2 = frame2.findTracker(trackerFrame1.designation); if (trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) { continue; } diff --git a/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java index 4b0b362e3..a4a34b06e 100644 --- a/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java +++ b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java @@ -125,7 +125,7 @@ public class SimpleSkeleton { } public void setPoseFromFrame(PoseFrame frame) { - for (TrackerFrame trackerFrame : frame.trackerFrames.values()) { + for (TrackerFrame trackerFrame : frame.trackerFrames) { // Set HMD position if (trackerFrame.designation == TrackerBodyPosition.HMD && trackerFrame.hasData(TrackerFrameData.POSITION)) { hmdNode.localTransform.setTranslation(trackerFrame.position); diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java index 3bebc2c3e..3756603e2 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java @@ -1,27 +1,18 @@ package dev.slimevr.vr.poserecorder; -import java.util.HashMap; import java.util.List; +import io.eiren.util.collections.FastList; import io.eiren.vr.processor.TrackerBodyPosition; import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerUtils; public final class PoseFrame { - public final HashMap trackerFrames; - - public PoseFrame(HashMap trackerFrames) { - this.trackerFrames = trackerFrames; - } + public final List trackerFrames; public PoseFrame(List trackerFrames) { - HashMap trackerFramesMap = new HashMap(trackerFrames.size()); - - for (TrackerFrame trackerFrame : trackerFrames) { - trackerFramesMap.put(trackerFrame.designation, trackerFrame); - } - - this.trackerFrames = trackerFramesMap; + this.trackerFrames = trackerFrames; } public static PoseFrame fromTrackers(List trackers) { @@ -29,16 +20,26 @@ public final class PoseFrame { return null; } - HashMap trackerFrames = new HashMap(trackers.size()); + List trackerFrames = new FastList(trackers.size()); for (Tracker tracker : trackers) { TrackerFrame trackerFrame = TrackerFrame.fromTracker(tracker); if (trackerFrame != null) { - trackerFrames.put(trackerFrame.designation, trackerFrame); + trackerFrames.add(trackerFrame); } } return new PoseFrame(trackerFrames); } + + //#region Easy Utility Access + public TrackerFrame findTracker(TrackerBodyPosition designation) { + return (TrackerFrame)TrackerUtils.findTrackerForBodyPosition(trackerFrames, designation); + } + + public TrackerFrame findTracker(TrackerBodyPosition designation, TrackerBodyPosition altDesignation) { + return (TrackerFrame)TrackerUtils.findTrackerForBodyPosition(trackerFrames, designation, altDesignation); + } + //#endregion } diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java index 940c75ac7..41ea2fc89 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -8,10 +8,12 @@ import java.io.File; import java.io.FileInputStream; import java.io.FileOutputStream; import java.util.HashMap; +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; @@ -41,7 +43,7 @@ public final class PoseFrameIO { if (frame != null && frame.trackerFrames != null) { outputStream.writeInt(frame.trackerFrames.size()); - for (TrackerFrame trackerFrame : frame.trackerFrames.values()) { + for (TrackerFrame trackerFrame : frame.trackerFrames) { outputStream.writeInt(trackerFrame.getDataFlags()); if (trackerFrame.hasData(TrackerFrameData.DESIGNATION)) { @@ -104,13 +106,15 @@ public final class PoseFrameIO { try { int trackerFrameCount = inputStream.readInt(); - HashMap trackerFrames = new HashMap(trackerFrameCount); + List trackerFrames = new FastList(trackerFrameCount); for (int i = 0; i < trackerFrameCount; i++) { int dataFlags = inputStream.readInt(); + String designationString = null; TrackerBodyPosition designation = null; if (TrackerFrameData.DESIGNATION.check(dataFlags)) { - designation = TrackerBodyPosition.getByDesignation(inputStream.readUTF()); + designationString = inputStream.readUTF(); + designation = TrackerBodyPosition.getByDesignation(designationString); } Quaternion rotation = null; @@ -130,9 +134,7 @@ public final class PoseFrameIO { position = new Vector3f(posX, posY, posZ); } - if (designation != null) { - trackerFrames.put(designation, new TrackerFrame(designation, rotation, position)); - } + trackerFrames.add(new TrackerFrame(designationString, designation, rotation, position)); } return new PoseFrame(trackerFrames); diff --git a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java index e038a7ac6..015432db3 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java @@ -5,16 +5,25 @@ 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 { +public final class TrackerFrame implements Tracker { private int dataFlags = 0; + public final String name; public final TrackerBodyPosition designation; public final Quaternion rotation; public final Vector3f position; - public TrackerFrame(TrackerBodyPosition designation, Quaternion rotation, Vector3f position) { + public TrackerFrame(String designationString, TrackerBodyPosition designation, Quaternion rotation, Vector3f position) { + if (designationString != null) { + this.name = "TrackerFrame:/" + designationString; + } else { + this.name = "TrackerFrame"; + } + this.designation = designation; if (designation != null) { dataFlags |= TrackerFrameData.DESIGNATION.flag; @@ -31,6 +40,10 @@ public final class TrackerFrame { } } + public TrackerFrame(TrackerBodyPosition designation, Quaternion rotation, Vector3f position) { + this(designation == null ? null : designation.designation, designation, rotation, position); + } + public TrackerFrame(TrackerBodyPosition designation, Quaternion rotation) { this(designation, rotation, null); } @@ -67,4 +80,93 @@ public final class TrackerFrame { 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 name; + } + + @Override + public TrackerStatus getStatus() { + return TrackerStatus.OK; + } + + @Override + public void loadConfig(TrackerConfig config) { + throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + } + + @Override + public void saveConfig(TrackerConfig config) { + throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + } + + @Override + public float getConfidenceLevel() { + return 0; + } + + @Override + public void resetFull(Quaternion reference) { + throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + } + + @Override + public void resetYaw(Quaternion reference) { + throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + } + + @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 implement this method"); + } + + @Override + public boolean userEditable() { + return false; + } + + @Override + public boolean hasRotation() { + return hasData(TrackerFrameData.ROTATION); + } + + @Override + public boolean hasPosition() { + return hasData(TrackerFrameData.POSITION); + } + //#endregion } diff --git a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java index 237297fd8..2052f9aa5 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java +++ b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java @@ -8,8 +8,8 @@ public class TrackerUtils { private TrackerUtils() { } - - public static Tracker findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position) { + + 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) @@ -17,15 +17,15 @@ public class TrackerUtils { } return null; } - - public static Tracker findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { + + public static Tracker findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { Tracker 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 Tracker findTrackerForBodyPositionOrEmpty(List allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { Tracker t = findTrackerForBodyPosition(allTrackers, position); if(t != null) return t; From 81bbb4008b851295da129aa9006c053c96a1ad01 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 01:45:35 -0400 Subject: [PATCH 07/16] Only allow loading tracker configs if the tracker is user editable --- .../io/eiren/vr/trackers/ComputedTracker.java | 5 ++++- .../java/io/eiren/vr/trackers/IMUTracker.java | 17 ++++++++++------- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java index eddf2ff12..6287dfa0d 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 diff --git a/src/main/java/io/eiren/vr/trackers/IMUTracker.java b/src/main/java/io/eiren/vr/trackers/IMUTracker.java index ff032204b..ddcea6699 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() { From d92ea0a39eb011c356b0f802d0d03d730e6973cb Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 02:52:06 -0400 Subject: [PATCH 08/16] Small clean-up and ignore computed trackers in PoseFrame by default --- .../java/dev/slimevr/gui/AutoBoneWindow.java | 18 +++++++++--------- .../slimevr/vr/autobone/SimpleSkeleton.java | 3 +-- .../dev/slimevr/vr/poserecorder/PoseFrame.java | 11 +++++++++++ .../slimevr/vr/poserecorder/PoseFrameIO.java | 1 - 4 files changed, 21 insertions(+), 12 deletions(-) diff --git a/src/main/java/dev/slimevr/gui/AutoBoneWindow.java b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java index 8b4f65ff4..1e9a93681 100644 --- a/src/main/java/dev/slimevr/gui/AutoBoneWindow.java +++ b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java @@ -37,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; @@ -292,7 +292,7 @@ public class AutoBoneWindow extends JFrame { setText("Load..."); List> frameRecordings = loadRecordings(); - if (frameRecordings.size() > 0) { + if (!frameRecordings.isEmpty()) { LogManager.log.info("[AutoBone] Done loading frames!"); } else { Future framesFuture = poseRecorder.getFramesAsync(); @@ -354,7 +354,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/dev/slimevr/vr/autobone/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java index a4a34b06e..280b963c8 100644 --- a/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java +++ b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java @@ -248,8 +248,7 @@ public class SimpleSkeleton { } public TransformNode getNode(String node) { - TransformNode transformNode = nodes.get(node); - return transformNode; + return nodes.get(node); } public TransformNode getNode(TrackerBodyPosition bodyPosition) { diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java index 3756603e2..adb67993a 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java @@ -4,6 +4,7 @@ import java.util.List; import io.eiren.util.collections.FastList; import io.eiren.vr.processor.TrackerBodyPosition; +import io.eiren.vr.trackers.ComputedTracker; import io.eiren.vr.trackers.Tracker; import io.eiren.vr.trackers.TrackerUtils; @@ -15,7 +16,12 @@ public final class PoseFrame { this.trackerFrames = trackerFrames; } + // Ignore computed trackers by default public static PoseFrame fromTrackers(List trackers) { + return fromTrackers(trackers, false); + } + + public static PoseFrame fromTrackers(List trackers, boolean includeComputed) { if (trackers == null || trackers.isEmpty()) { return null; } @@ -23,6 +29,11 @@ public final class PoseFrame { List trackerFrames = new FastList(trackers.size()); for (Tracker tracker : trackers) { + // Ignore computed trackers if they aren't requested + if (!includeComputed && tracker.getClass().equals(ComputedTracker.class)) { + continue; + } + TrackerFrame trackerFrame = TrackerFrame.fromTracker(tracker); if (trackerFrame != null) { diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java index 41ea2fc89..d1aa5adc2 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -7,7 +7,6 @@ import java.io.DataOutputStream; import java.io.File; import java.io.FileInputStream; import java.io.FileOutputStream; -import java.util.HashMap; import java.util.List; import com.jme3.math.Quaternion; From 63e259689f4998ed0141b54b22ce07e4a92970f4 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 04:01:10 -0400 Subject: [PATCH 09/16] Handle computed trackers better and handle tracker status in data collection --- .../java/dev/slimevr/vr/poserecorder/PoseFrame.java | 4 +++- .../dev/slimevr/vr/poserecorder/TrackerFrame.java | 13 ++++++++++++- .../java/io/eiren/vr/trackers/ComputedTracker.java | 5 +++++ src/main/java/io/eiren/vr/trackers/HMDTracker.java | 5 +++++ src/main/java/io/eiren/vr/trackers/IMUTracker.java | 5 +++++ .../eiren/vr/trackers/ReferenceAdjustedTracker.java | 7 ++++++- .../java/io/eiren/vr/trackers/SteamVRTracker.java | 5 +++++ src/main/java/io/eiren/vr/trackers/Tracker.java | 2 ++ 8 files changed, 43 insertions(+), 3 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java index adb67993a..f1c716ad8 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java @@ -3,9 +3,11 @@ package dev.slimevr.vr.poserecorder; import java.util.List; import io.eiren.util.collections.FastList; +import io.eiren.vr.processor.ComputedHumanPoseTracker; import io.eiren.vr.processor.TrackerBodyPosition; import io.eiren.vr.trackers.ComputedTracker; import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerStatus; import io.eiren.vr.trackers.TrackerUtils; public final class PoseFrame { @@ -30,7 +32,7 @@ public final class PoseFrame { for (Tracker tracker : trackers) { // Ignore computed trackers if they aren't requested - if (!includeComputed && tracker.getClass().equals(ComputedTracker.class)) { + if (!includeComputed && tracker.isComputed()) { continue; } diff --git a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java index 015432db3..3935c6724 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java @@ -58,14 +58,20 @@ public final class TrackerFrame implements Tracker { return null; } + // If the tracker is not ready + if (tracker.getStatus() != TrackerStatus.OK && tracker.getStatus() != TrackerStatus.OCCLUDED) { + return null; + } + Quaternion rotation = null; if (tracker.hasRotation()) { rotation = new Quaternion(); tracker.getRotation(rotation); } + // If the tracker is occluded, don't get the position Vector3f position = null; - if (tracker.hasPosition()) { + if (tracker.hasPosition() && tracker.getStatus() != TrackerStatus.OCCLUDED) { position = new Vector3f(); tracker.getPosition(position); } @@ -168,5 +174,10 @@ public final class TrackerFrame implements Tracker { public boolean hasPosition() { return hasData(TrackerFrameData.POSITION); } + + @Override + public boolean isComputed() { + return true; + } //#endregion } diff --git a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java index 6287dfa0d..19da58688 100644 --- a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java +++ b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java @@ -101,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 ddcea6699..e1e5d3adc 100644 --- a/src/main/java/io/eiren/vr/trackers/IMUTracker.java +++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java @@ -238,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(); } From b3073e6938c4d3c030ad09fe1fc58533729ca971 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 04:20:43 -0400 Subject: [PATCH 10/16] Handle busy status and add better exception messages --- .../slimevr/vr/poserecorder/TrackerFrame.java | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java index 3935c6724..9e4d8591e 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java @@ -53,13 +53,18 @@ public final class TrackerFrame implements Tracker { } public static TrackerFrame fromTracker(Tracker tracker) { - // If null or has no data - if (tracker == null || (!tracker.hasRotation() && !tracker.hasPosition())) { + if (tracker == null) { return null; } // If the tracker is not ready - if (tracker.getStatus() != TrackerStatus.OK && tracker.getStatus() != TrackerStatus.OCCLUDED) { + if (tracker.getStatus() != TrackerStatus.OK && tracker.getStatus() != TrackerStatus.BUSY && tracker.getStatus() != TrackerStatus.OCCLUDED) { + return null; + } + + boolean hasPosition = tracker.hasPosition() && tracker.getStatus() != TrackerStatus.OCCLUDED; + // If tracker has no data + if (tracker.getBodyPosition() == null && !tracker.hasRotation() && !hasPosition) { return null; } @@ -71,7 +76,7 @@ public final class TrackerFrame implements Tracker { // If the tracker is occluded, don't get the position Vector3f position = null; - if (tracker.hasPosition() && tracker.getStatus() != TrackerStatus.OCCLUDED) { + if (hasPosition) { position = new Vector3f(); tracker.getPosition(position); } @@ -122,12 +127,12 @@ public final class TrackerFrame implements Tracker { @Override public void loadConfig(TrackerConfig config) { - throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + throw new UnsupportedOperationException("TrackerFrame does not implement configuration"); } @Override public void saveConfig(TrackerConfig config) { - throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + throw new UnsupportedOperationException("TrackerFrame does not implement configuration"); } @Override @@ -137,12 +142,12 @@ public final class TrackerFrame implements Tracker { @Override public void resetFull(Quaternion reference) { - throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + throw new UnsupportedOperationException("TrackerFrame does not implement calibration"); } @Override public void resetYaw(Quaternion reference) { - throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + throw new UnsupportedOperationException("TrackerFrame does not implement calibration"); } @Override @@ -157,7 +162,7 @@ public final class TrackerFrame implements Tracker { @Override public void setBodyPosition(TrackerBodyPosition position) { - throw new UnsupportedOperationException("TrackerFrame does not implement this method"); + throw new UnsupportedOperationException("TrackerFrame does not allow setting the body position"); } @Override From 5e1c45bc09ddcaaaafc44ee40e75ec7bd9bb463b Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 08:50:08 -0400 Subject: [PATCH 11/16] Record individual trackers with PoseFrame and optimize iterations --- .../java/dev/slimevr/gui/AutoBoneWindow.java | 38 +-- .../dev/slimevr/vr/autobone/AutoBone.java | 65 +++-- .../slimevr/vr/autobone/SimpleSkeleton.java | 58 ++--- .../slimevr/vr/poserecorder/PoseFrame.java | 149 ++++++++--- .../slimevr/vr/poserecorder/PoseFrameIO.java | 156 ++++++------ .../vr/poserecorder/PoseFrameTracker.java | 233 ++++++++++++++++++ .../slimevr/vr/poserecorder/PoseRecorder.java | 83 ++++--- .../slimevr/vr/poserecorder/TrackerFrame.java | 23 +- .../io/eiren/vr/trackers/TrackerUtils.java | 36 ++- 9 files changed, 595 insertions(+), 246 deletions(-) create mode 100644 src/main/java/dev/slimevr/vr/poserecorder/PoseFrameTracker.java diff --git a/src/main/java/dev/slimevr/gui/AutoBoneWindow.java b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java index 1e9a93681..520c6a5b2 100644 --- a/src/main/java/dev/slimevr/gui/AutoBoneWindow.java +++ b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java @@ -83,7 +83,7 @@ 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; @@ -102,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(), ".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..."); @@ -125,7 +125,7 @@ public class AutoBoneWindow extends JFrame { return recordings; } - private float processFrames(PoseFrame[] frames) { + private float processFrames(PoseFrame frames) { autoBone.reloadConfigValues(); autoBone.minDataDistance = server.config.getInt("autobone.minimumDataDistance", autoBone.minDataDistance); @@ -173,8 +173,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); @@ -225,12 +225,16 @@ 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"); } @@ -290,17 +294,21 @@ public class AutoBoneWindow extends JFrame { public void run() { try { setText("Load..."); - List> frameRecordings = loadRecordings(); + List> frameRecordings = loadRecordings(); 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"); } @@ -320,7 +328,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())); diff --git a/src/main/java/dev/slimevr/vr/autobone/AutoBone.java b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java index a6337baf7..7adf5648c 100644 --- a/src/main/java/dev/slimevr/vr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java @@ -1,6 +1,7 @@ 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; @@ -129,9 +130,7 @@ public class AutoBone { return false; } - for (Entry entry : configs.entrySet()) { - skeleton.setSkeletonConfig(entry.getKey(), entry.getValue()); - } + configs.forEach(skeleton::setSkeletonConfig); server.saveConfig(); @@ -200,10 +199,10 @@ public class AutoBone { return length; } - public float getMaxHmdHeight(PoseFrame[] frames) { + public float getMaxHmdHeight(PoseFrame frames) { float maxHeight = 0f; - for (PoseFrame frame : frames) { - TrackerFrame hmd = frame.findTracker(TrackerBodyPosition.HMD); + 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; } @@ -211,29 +210,34 @@ public class AutoBone { 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()]; + + 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) { @@ -259,25 +263,20 @@ 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 @@ -325,7 +324,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; @@ -410,11 +409,11 @@ 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; - for (TrackerFrame trackerFrame : frame.trackerFrames) { + for (TrackerFrame trackerFrame : frame) { if (!trackerFrame.hasData(TrackerFrameData.POSITION)) { continue; } @@ -430,16 +429,16 @@ public class AutoBone { } // The difference between offset of absolute position and the corresponding point over time - protected float getPositionOffsetErrorDeriv(PoseFrame frame1, PoseFrame frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { + protected float getPositionOffsetErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) { float offset = 0f; int offsetCount = 0; - for (TrackerFrame trackerFrame1 : frame1.trackerFrames) { + for (TrackerFrame trackerFrame1 : frame1) { if (!trackerFrame1.hasData(TrackerFrameData.POSITION)) { continue; } - TrackerFrame trackerFrame2 = frame2.findTracker(trackerFrame1.designation); + TrackerFrame trackerFrame2 = TrackerUtils.findTrackerForBodyPosition(frame2, trackerFrame1.designation); if (trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) { continue; } @@ -464,7 +463,7 @@ public class AutoBone { return offsetCount > 0 ? offset / offsetCount : 0f; } - protected float getErrorDeriv(PoseFrame frame1, PoseFrame frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, float heightChange) { + 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/dev/slimevr/vr/autobone/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java index 280b963c8..e0a34f288 100644 --- a/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java +++ b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java @@ -1,12 +1,11 @@ package dev.slimevr.vr.autobone; import java.util.HashMap; +import java.util.List; import java.util.Map; -import java.util.Map.Entry; import com.jme3.math.Vector3f; -import dev.slimevr.vr.poserecorder.PoseFrame; import dev.slimevr.vr.poserecorder.TrackerFrameData; import dev.slimevr.vr.poserecorder.TrackerFrame; import io.eiren.vr.processor.HumanSkeletonWithLegs; @@ -100,7 +99,7 @@ public class SimpleSkeleton { }); } - public SimpleSkeleton(Iterable> configs, Iterable> altConfigs) { + public SimpleSkeleton(Map configs, Map altConfigs) { // Initialize this(); @@ -112,45 +111,48 @@ 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 trackerFrame) { + if (trackerFrame == null) { + return; + } + + // Set HMD position + if (trackerFrame.designation == TrackerBodyPosition.HMD && trackerFrame.hasData(TrackerFrameData.POSITION)) { + hmdNode.localTransform.setTranslation(trackerFrame.position); + } + + // Set joint rotations + if (trackerFrame.hasData(TrackerFrameData.ROTATION)) { + TransformNode node = getNode(trackerFrame.designation, true); + if (node != null) { + node.localTransform.setRotation(trackerFrame.rotation); + } + } } - public void setPoseFromFrame(PoseFrame frame) { - for (TrackerFrame trackerFrame : frame.trackerFrames) { - // Set HMD position - if (trackerFrame.designation == TrackerBodyPosition.HMD && trackerFrame.hasData(TrackerFrameData.POSITION)) { - hmdNode.localTransform.setTranslation(trackerFrame.position); - } - - // Set joint rotations - if (trackerFrame.hasData(TrackerFrameData.ROTATION)) { - TransformNode node = getNode(trackerFrame.designation, true); - if (node != null) { - node.localTransform.setRotation(trackerFrame.rotation); - } - } + public void setPoseFromFrame(List frame) { + for (int i = 0; i < frame.size(); i++) { + TrackerFrame trackerFrame = frame.get(i); + setPoseFromFrame(trackerFrame); } updatePose(); } - public void setSkeletonConfigs(Iterable> configs) { - for (Entry config : configs) { - setSkeletonConfig(config.getKey(), config.getValue()); + public void setPoseFromFrame(TrackerFrame[] frame) { + for (TrackerFrame trackerFrame : frame) { + setPoseFromFrame(trackerFrame); } + + updatePose(); } public void setSkeletonConfigs(Map configs) { - setSkeletonConfigs(configs.entrySet()); + configs.forEach(this::setSkeletonConfig); } public void setSkeletonConfig(String joint, float newLength) { diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java index f1c716ad8..1d9105a35 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrame.java @@ -1,58 +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.processor.ComputedHumanPoseTracker; -import io.eiren.vr.processor.TrackerBodyPosition; -import io.eiren.vr.trackers.ComputedTracker; import io.eiren.vr.trackers.Tracker; -import io.eiren.vr.trackers.TrackerStatus; -import io.eiren.vr.trackers.TrackerUtils; -public final class PoseFrame { +public final class PoseFrame implements Iterable { - public final List trackerFrames; + private final FastList trackers; - public PoseFrame(List trackerFrames) { - this.trackerFrames = trackerFrames; + public PoseFrame(FastList trackers) { + this.trackers = trackers; } - // Ignore computed trackers by default - public static PoseFrame fromTrackers(List trackers) { - return fromTrackers(trackers, false); + public PoseFrame(int initialCapacity) { + this.trackers = new FastList(initialCapacity); } - public static PoseFrame fromTrackers(List trackers, boolean includeComputed) { - if (trackers == null || trackers.isEmpty()) { - return null; - } + public PoseFrame() { + this(5); + } - List trackerFrames = new FastList(trackers.size()); + public PoseFrameTracker addTracker(PoseFrameTracker tracker) { + trackers.add(tracker); + return tracker; + } - for (Tracker tracker : trackers) { - // Ignore computed trackers if they aren't requested - if (!includeComputed && tracker.isComputed()) { - continue; - } + public PoseFrameTracker addTracker(Tracker tracker, int initialCapacity) { + return addTracker(new PoseFrameTracker(tracker.getName(), initialCapacity)); + } - TrackerFrame trackerFrame = TrackerFrame.fromTracker(tracker); + public PoseFrameTracker addTracker(Tracker tracker) { + return addTracker(tracker, 5); + } - if (trackerFrame != null) { - trackerFrames.add(trackerFrame); + 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 new PoseFrame(trackerFrames); + return maxFrames; } - //#region Easy Utility Access - public TrackerFrame findTracker(TrackerBodyPosition designation) { - return (TrackerFrame)TrackerUtils.findTrackerForBodyPosition(trackerFrames, designation); + 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 TrackerFrame findTracker(TrackerBodyPosition designation, TrackerBodyPosition altDesignation) { - return (TrackerFrame)TrackerUtils.findTrackerForBodyPosition(trackerFrames, designation, altDesignation); + 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; + } } - //#endregion } diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java index d1aa5adc2..986b7004d 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -22,44 +22,40 @@ public final class 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) { + public static boolean writeFrame(DataOutputStream outputStream, PoseFrame frames) { try { - if (frame != null && frame.trackerFrames != null) { - outputStream.writeInt(frame.trackerFrames.size()); + if (frames != null) { - for (TrackerFrame trackerFrame : frame.trackerFrames) { - outputStream.writeInt(trackerFrame.getDataFlags()); + outputStream.writeInt(frames.getTrackerCount()); + for (PoseFrameTracker tracker : frames.getTrackers()) { - if (trackerFrame.hasData(TrackerFrameData.DESIGNATION)) { - outputStream.writeUTF(trackerFrame.designation.designation); - } + 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; + } - 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()); - } + outputStream.writeInt(trackerFrame.getDataFlags()); - if (trackerFrame.hasData(TrackerFrameData.POSITION)) { - outputStream.writeFloat(trackerFrame.position.getX()); - outputStream.writeFloat(trackerFrame.position.getY()); - outputStream.writeFloat(trackerFrame.position.getZ()); + 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 { @@ -73,70 +69,62 @@ public final class PoseFrameIO { return true; } - public static boolean writeFrame(File file, PoseFrame frame) { + public static boolean writeToFile(File file, PoseFrame frames) { try (DataOutputStream outputStream = new DataOutputStream(new BufferedOutputStream(new FileOutputStream(file)))) { - writeFrame(outputStream, frame); + writeFrame(outputStream, frames); } catch (Exception e) { - LogManager.log.severe("Error writing frame to file", e); + LogManager.log.severe("Error writing frames 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) { + public static PoseFrame readFrames(DataInputStream inputStream) { try { - int trackerFrameCount = inputStream.readInt(); - List trackerFrames = new FastList(trackerFrameCount); - for (int i = 0; i < trackerFrameCount; i++) { - int dataFlags = inputStream.readInt(); + int trackerCount = inputStream.readInt(); + FastList trackers = new FastList(trackerCount); + for (int i = 0; i < trackerCount; i++) { - String designationString = null; - TrackerBodyPosition designation = null; - if (TrackerFrameData.DESIGNATION.check(dataFlags)) { - designationString = inputStream.readUTF(); - designation = TrackerBodyPosition.getByDesignation(designationString); + String name = inputStream.readUTF(); + + int trackerFrameCount = inputStream.readInt(); + FastList trackerFrames = new FastList(trackerFrameCount); + for (int j = 0; j < trackerFrameCount; j++) { + int dataFlags = inputStream.readInt(); + + String designationString = null; + TrackerBodyPosition designation = null; + if (TrackerFrameData.DESIGNATION.check(dataFlags)) { + designationString = inputStream.readUTF(); + designation = TrackerBodyPosition.getByDesignation(designationString); + } + + 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)); } - 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(designationString, designation, rotation, position)); + trackers.add(new PoseFrameTracker(name, trackerFrames)); } - return new PoseFrame(trackerFrames); + return new PoseFrame(trackers); } catch (Exception e) { LogManager.log.severe("Error reading frame from stream", e); } @@ -144,9 +132,9 @@ public final class PoseFrameIO { return null; } - public static PoseFrame readFrame(File file) { + public static PoseFrame readFromFile(File file) { try { - return readFrame(new DataInputStream(new BufferedInputStream(new FileInputStream(file)))); + return readFrames(new DataInputStream(new BufferedInputStream(new FileInputStream(file)))); } catch (Exception e) { LogManager.log.severe("Error reading frame from file", e); } 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 index 6f427774b..14150b09c 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseRecorder.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseRecorder.java @@ -5,6 +5,8 @@ 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; @@ -13,16 +15,17 @@ import io.eiren.vr.trackers.Tracker; public class PoseRecorder { - protected final FastList frames = new FastList(); + protected PoseFrame poseFrame = null; protected int numFrames = -1; + protected int frameCursor = 0; protected long frameRecordingInterval = 60L; protected long nextFrameTimeMs = -1L; - protected CompletableFuture currentRecording; + protected CompletableFuture currentRecording; protected final VRServer server; - List trackers = null; + FastList> trackers = new FastList>(); public PoseRecorder(VRServer server) { this.server = server; @@ -32,52 +35,70 @@ public class PoseRecorder { @VRServerThread public void onTick() { if (numFrames > 0) { - List trackers = this.trackers; - if (trackers != null) { - if (frames.size() < numFrames) { + PoseFrame poseFrame = this.poseFrame; + List> trackers = this.trackers; + if (poseFrame != null && trackers != null) { + if (frameCursor < numFrames) { if (System.currentTimeMillis() >= nextFrameTimeMs) { nextFrameTimeMs = System.currentTimeMillis() + frameRecordingInterval; - PoseFrame frame = PoseFrame.fromTrackers(trackers); - if (frame != null) { - frames.add(frame); + 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 (frames.size() >= numFrames) { - internalStopRecording(); - } + // If done, send finished recording + if (frameCursor >= numFrames) { + internalStopRecording(); } } } else { // If done and hasn't yet, send finished recording internalStopRecording(); } - } else { - this.trackers = server.getAllTrackers(); } } } - public synchronized Future startFrameRecording(int numFrames, long interval) { + 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 = server.getAllTrackers(); + this.trackers.ensureCapacity(trackers.size()); + for (Tracker tracker : trackers) { + // Ignore null and computed trackers + if (tracker == null || tracker.isComputed()) { + continue; + } - // Clear old frames and ensure new size can be held - frames.clear(); - frames.ensureCapacity(numFrames); + // Pair tracker with recording + this.trackers.add(Pair.of(tracker, poseFrame.addTracker(tracker, numFrames))); + } + this.frameCursor = 0; this.numFrames = numFrames; frameRecordingInterval = interval; @@ -85,18 +106,21 @@ public class PoseRecorder { LogManager.log.info("[PoseRecorder] Recording " + numFrames + " samples at a " + interval + " ms frame interval"); - currentRecording = new CompletableFuture(); + currentRecording = new CompletableFuture(); return currentRecording; } private void internalStopRecording() { - CompletableFuture currentRecording = this.currentRecording; + CompletableFuture currentRecording = this.currentRecording; if (currentRecording != null && !currentRecording.isDone()) { // Stop the recording, returning the frames recorded - currentRecording.complete(frames.toArray(new PoseFrame[0])); + currentRecording.complete(poseFrame); } numFrames = -1; + frameCursor = 0; + trackers.clear(); + poseFrame = null; } public synchronized void stopFrameRecording() { @@ -104,13 +128,16 @@ public class PoseRecorder { } public synchronized void cancelFrameRecording() { - CompletableFuture currentRecording = this.currentRecording; + 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() { @@ -118,19 +145,19 @@ public class PoseRecorder { } public boolean isRecording() { - return numFrames > frames.size(); + return numFrames > frameCursor; } public boolean hasRecording() { return currentRecording != null; } - public Future getFramesAsync() { + public Future getFramesAsync() { return currentRecording; } - public PoseFrame[] getFrames() throws ExecutionException, InterruptedException { - CompletableFuture currentRecording = this.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 index 9e4d8591e..f37c4ccc9 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java @@ -12,18 +12,11 @@ public final class TrackerFrame implements Tracker { private int dataFlags = 0; - public final String name; public final TrackerBodyPosition designation; public final Quaternion rotation; public final Vector3f position; - public TrackerFrame(String designationString, TrackerBodyPosition designation, Quaternion rotation, Vector3f position) { - if (designationString != null) { - this.name = "TrackerFrame:/" + designationString; - } else { - this.name = "TrackerFrame"; - } - + public TrackerFrame(TrackerBodyPosition designation, Quaternion rotation, Vector3f position) { this.designation = designation; if (designation != null) { dataFlags |= TrackerFrameData.DESIGNATION.flag; @@ -40,18 +33,6 @@ public final class TrackerFrame implements Tracker { } } - public TrackerFrame(TrackerBodyPosition designation, Quaternion rotation, Vector3f position) { - this(designation == null ? null : designation.designation, designation, rotation, position); - } - - public TrackerFrame(TrackerBodyPosition designation, Quaternion rotation) { - this(designation, rotation, null); - } - - public TrackerFrame(TrackerBodyPosition designation, Vector3f position) { - this(designation, null, position); - } - public static TrackerFrame fromTracker(Tracker tracker) { if (tracker == null) { return null; @@ -117,7 +98,7 @@ public final class TrackerFrame implements Tracker { @Override public String getName() { - return name; + return "TrackerFrame:/" + (designation != null ? designation.designation : "null"); } @Override diff --git a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java index 2052f9aa5..3d9776643 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java +++ b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java @@ -9,17 +9,33 @@ 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); + public static T findTrackerForBodyPosition(T[] allTrackers, TrackerBodyPosition position) { + for(int i = 0; i < allTrackers.length; ++i) { + T t = allTrackers[i]; if(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.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 T findTrackerForBodyPosition(T[] allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) { + T t = findTrackerForBodyPosition(allTrackers, position); if(t != null) return t; return findTrackerForBodyPosition(allTrackers, altPosition); @@ -34,4 +50,14 @@ public class TrackerUtils { 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; + t = findTrackerForBodyPosition(allTrackers, altPosition); + if(t != null) + return t; + return new ComputedTracker("Empty tracker", false, false); + } } From 582bac8050315658e26a29f7e9065074b4330201 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 09:08:22 -0400 Subject: [PATCH 12/16] Check recording for chest tracker when loading AutoBone configs --- src/main/java/dev/slimevr/gui/AutoBoneWindow.java | 2 -- .../java/dev/slimevr/vr/autobone/AutoBone.java | 14 +++++++++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/dev/slimevr/gui/AutoBoneWindow.java b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java index 520c6a5b2..c2017a64d 100644 --- a/src/main/java/dev/slimevr/gui/AutoBoneWindow.java +++ b/src/main/java/dev/slimevr/gui/AutoBoneWindow.java @@ -126,8 +126,6 @@ public class AutoBoneWindow extends JFrame { } private float processFrames(PoseFrame frames) { - autoBone.reloadConfigValues(); - autoBone.minDataDistance = server.config.getInt("autobone.minimumDataDistance", autoBone.minDataDistance); autoBone.maxDataDistance = server.config.getInt("autobone.maximumDataDistance", autoBone.maxDataDistance); diff --git a/src/main/java/dev/slimevr/vr/autobone/AutoBone.java b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java index 7adf5648c..ccb664999 100644 --- a/src/main/java/dev/slimevr/vr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/vr/autobone/AutoBone.java @@ -88,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)); @@ -236,6 +241,9 @@ public class AutoBone { 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()]; @@ -283,7 +291,7 @@ public class AutoBone { 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; @@ -414,7 +422,7 @@ public class AutoBone { int offsetCount = 0; for (TrackerFrame trackerFrame : frame) { - if (!trackerFrame.hasData(TrackerFrameData.POSITION)) { + if (trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) { continue; } @@ -434,7 +442,7 @@ public class AutoBone { int offsetCount = 0; for (TrackerFrame trackerFrame1 : frame1) { - if (!trackerFrame1.hasData(TrackerFrameData.POSITION)) { + if (trackerFrame1 == null || !trackerFrame1.hasData(TrackerFrameData.POSITION)) { continue; } From cb33dac3b9bbe49239629bc482743fd561bafd52 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 09:14:16 -0400 Subject: [PATCH 13/16] Handle getRotation and getPosition responses properly --- .../slimevr/vr/poserecorder/TrackerFrame.java | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java index f37c4ccc9..fc9a606af 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/TrackerFrame.java @@ -43,23 +43,27 @@ public final class TrackerFrame implements Tracker { return null; } - boolean hasPosition = tracker.hasPosition() && tracker.getStatus() != TrackerStatus.OCCLUDED; // If tracker has no data - if (tracker.getBodyPosition() == null && !tracker.hasRotation() && !hasPosition) { + if (tracker.getBodyPosition() == null && !tracker.hasRotation() && !tracker.hasPosition()) { return null; } Quaternion rotation = null; if (tracker.hasRotation()) { rotation = new Quaternion(); - tracker.getRotation(rotation); + if (!tracker.getRotation(rotation)) { + // If getting the rotation failed, set it back to null + rotation = null; + } } - // If the tracker is occluded, don't get the position Vector3f position = null; - if (hasPosition) { + if (tracker.hasPosition()) { position = new Vector3f(); - tracker.getPosition(position); + if (!tracker.getPosition(position)) { + // If getting the position failed, set it back to null + position = null; + } } return new TrackerFrame(tracker.getBodyPosition(), rotation, position); From a300663a9e1702465f0c32502de8e065da4bbd90 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Tue, 14 Sep 2021 10:36:35 -0400 Subject: [PATCH 14/16] Spelling fix and check for null in TrackerUtils --- .../java/dev/slimevr/vr/poserecorder/PoseFrameIO.java | 9 +++------ src/main/java/io/eiren/vr/trackers/TrackerUtils.java | 4 ++-- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java index 986b7004d..3c7fb460a 100644 --- a/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/vr/poserecorder/PoseFrameIO.java @@ -22,7 +22,7 @@ public final class PoseFrameIO { // Do not allow instantiating } - public static boolean writeFrame(DataOutputStream outputStream, PoseFrame frames) { + public static boolean writeFrames(DataOutputStream outputStream, PoseFrame frames) { try { if (frames != null) { @@ -71,7 +71,7 @@ public final class PoseFrameIO { public static boolean writeToFile(File file, PoseFrame frames) { try (DataOutputStream outputStream = new DataOutputStream(new BufferedOutputStream(new FileOutputStream(file)))) { - writeFrame(outputStream, frames); + writeFrames(outputStream, frames); } catch (Exception e) { LogManager.log.severe("Error writing frames to file", e); return false; @@ -88,17 +88,14 @@ public final class PoseFrameIO { 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(); - String designationString = null; TrackerBodyPosition designation = null; if (TrackerFrameData.DESIGNATION.check(dataFlags)) { - designationString = inputStream.readUTF(); - designation = TrackerBodyPosition.getByDesignation(designationString); + designation = TrackerBodyPosition.getByDesignation(inputStream.readUTF()); } Quaternion rotation = null; diff --git a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java index 3d9776643..58cf21143 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackerUtils.java +++ b/src/main/java/io/eiren/vr/trackers/TrackerUtils.java @@ -12,7 +12,7 @@ public class TrackerUtils { public static T findTrackerForBodyPosition(T[] allTrackers, TrackerBodyPosition position) { for(int i = 0; i < allTrackers.length; ++i) { T t = allTrackers[i]; - if(t.getBodyPosition() == position) + if(t != null && t.getBodyPosition() == position) return t; } return null; @@ -21,7 +21,7 @@ public class TrackerUtils { public static T findTrackerForBodyPosition(List allTrackers, TrackerBodyPosition position) { for(int i = 0; i < allTrackers.size(); ++i) { T t = allTrackers.get(i); - if(t.getBodyPosition() == position) + if(t != null && t.getBodyPosition() == position) return t; } return null; From 4109d1c82586e5703e198a07095d2e2456e311a6 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Thu, 16 Sep 2021 00:30:22 -0400 Subject: [PATCH 15/16] Add pelvis averaging for SimpleSkeleton and fix neck rotation --- .../slimevr/vr/autobone/SimpleSkeleton.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java index e0a34f288..846f7a6ce 100644 --- a/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java +++ b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java @@ -4,6 +4,7 @@ import java.util.HashMap; import java.util.List; import java.util.Map; +import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; import dev.slimevr.vr.poserecorder.TrackerFrameData; @@ -60,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); @@ -244,8 +248,20 @@ public class SimpleSkeleton { } public void updatePose() { + // Rotate neck with the HMD + headNode.localTransform.setRotation(hmdNode.localTransform.getRotation()); + // Rotate hips with waist - waistNode.localTransform.setRotation(chestNode.localTransform.getRotation()); + // waistNode.localTransform.setRotation(chestNode.localTransform.getRotation()); + + // Average pelvis between two legs + leftHipNode.localTransform.getRotation(rotBuf1); + rightHipNode.localTransform.getRotation(rotBuf2); + rotBuf1.nlerp(rotBuf2, 0.5f); + chestNode.localTransform.getRotation(rotBuf2); + rotBuf1.nlerp(rotBuf2, 0.3333333f); + waistNode.localTransform.setRotation(rotBuf1); + hmdNode.update(); } From 6d103d4ff98267985993ca07f65b170d7ef5a102 Mon Sep 17 00:00:00 2001 From: ButterscotchVanilla Date: Fri, 17 Sep 2021 19:18:24 -0400 Subject: [PATCH 16/16] Get all trackers directly in setPoseFromFrame --- .../slimevr/vr/autobone/SimpleSkeleton.java | 108 +++++++++++------- 1 file changed, 69 insertions(+), 39 deletions(-) diff --git a/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java index 846f7a6ce..2f6457a27 100644 --- a/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java +++ b/src/main/java/dev/slimevr/vr/autobone/SimpleSkeleton.java @@ -1,7 +1,6 @@ package dev.slimevr.vr.autobone; import java.util.HashMap; -import java.util.List; import java.util.Map; import com.jme3.math.Quaternion; @@ -13,6 +12,7 @@ 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 { @@ -119,40 +119,84 @@ public class SimpleSkeleton { this(configs, null); } - public void setPoseFromFrame(TrackerFrame trackerFrame) { - if (trackerFrame == null) { + public void setPoseFromFrame(TrackerFrame[] frame) { + + TrackerFrame hmd = TrackerUtils.findTrackerForBodyPosition(frame, TrackerBodyPosition.HMD); + + if (hmd != null) + { + if (hmd.hasData(TrackerFrameData.ROTATION)) + { + hmdNode.localTransform.setRotation(hmd.rotation); + headNode.localTransform.setRotation(hmd.rotation); + } + + 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 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; } - // Set HMD position - if (trackerFrame.designation == TrackerBodyPosition.HMD && trackerFrame.hasData(TrackerFrameData.POSITION)) { - hmdNode.localTransform.setTranslation(trackerFrame.position); - } + 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); - // Set joint rotations - if (trackerFrame.hasData(TrackerFrameData.ROTATION)) { - TransformNode node = getNode(trackerFrame.designation, true); - if (node != null) { - node.localTransform.setRotation(trackerFrame.rotation); + waistNode.localTransform.setRotation(rotBuf1); } - } - } - public void setPoseFromFrame(List frame) { - for (int i = 0; i < frame.size(); i++) { - TrackerFrame trackerFrame = frame.get(i); - setPoseFromFrame(trackerFrame); + return; } - updatePose(); - } + // Average the pelvis with the waist rotation + leftLeg.getRotation(rotBuf1); + rightLeg.getRotation(rotBuf2); + rotBuf1.nlerp(rotBuf2, 0.5f); - public void setPoseFromFrame(TrackerFrame[] frame) { - for (TrackerFrame trackerFrame : frame) { - setPoseFromFrame(trackerFrame); - } + waist.getRotation(rotBuf2); + rotBuf1.nlerp(rotBuf2, 0.3333333f); - updatePose(); + waistNode.localTransform.setRotation(rotBuf1); } public void setSkeletonConfigs(Map configs) { @@ -248,20 +292,6 @@ public class SimpleSkeleton { } public void updatePose() { - // Rotate neck with the HMD - headNode.localTransform.setRotation(hmdNode.localTransform.getRotation()); - - // Rotate hips with waist - // waistNode.localTransform.setRotation(chestNode.localTransform.getRotation()); - - // Average pelvis between two legs - leftHipNode.localTransform.getRotation(rotBuf1); - rightHipNode.localTransform.getRotation(rotBuf2); - rotBuf1.nlerp(rotBuf2, 0.5f); - chestNode.localTransform.getRotation(rotBuf2); - rotBuf1.nlerp(rotBuf2, 0.3333333f); - waistNode.localTransform.setRotation(rotBuf1); - hmdNode.update(); }