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); + } }