Merge pull request #49 from ButterscotchVanilla/autobone-positions

AutoBone PoseFrame file format rework and other related fixes
This commit is contained in:
Eiren Rain authored and GitHub committed 2021-09-18 03:04:29 +03:00
commit 18cea30f72
21 files changed
+1216 -526

No files matched your search

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