Change name of steamvr trackers to legs instead of feet to make it less confusing

Add AutoBone, a skeleton auto-configuration algorithm

.editorconfig

@@ -0,0 +1,11 @@
+# This file is for unifying the coding style for different editors and IDEs
+# See editorconfig.org
+
+root = true
+
+[*]
+indent_style = tab
+end_of_line = lf
+charset = utf-8
+trim_trailing_whitespace = true
+insert_final_newline = true

.github/workflows/gradle.yml

@@ -1,17 +1,12 @@
 # This workflow will build a Java project with Gradle
 # For more information see: https://help.github.com/actions/language-and-framework-guides/building-and-testing-java-with-gradle
 
-name: Build SlimeVR Server with Gradle
+name: SlimeVR Server
 
-on:
-  push:
-    branches: [ main ]
-  pull_request:
-    branches: [ main ]
+on: [push, pull_request]
 
 jobs:
-  build:
-
+  test:
     runs-on: ubuntu-latest
 
     steps:
@@ -24,10 +19,10 @@ jobs:
         # Relative path under $GITHUB_WORKSPACE to place the repository
         path: Slime Java Commons
     
-    - name: Set up JDK 11
+    - name: Set up JDK 8
       uses: actions/setup-java@v2.1.0
       with:
-        java-version: '11'
+        java-version: '8'
         distribution: 'adopt'
         
     - name: Grant execute permission for gradlew
@@ -35,6 +30,28 @@ jobs:
       
     - name: Test with Gradle
       run: ./gradlew clean test
+
+  build:
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v2.3.4
+    
+    - name: Clone Slime Java Commons
+      uses: actions/checkout@v2.3.4
+      with:
+        repository: Eirenliel/slime-java-commons
+        # Relative path under $GITHUB_WORKSPACE to place the repository
+        path: Slime Java Commons
+    
+    - name: Set up JDK 8
+      uses: actions/setup-java@v2.1.0
+      with:
+        java-version: '8'
+        distribution: 'adopt'
+        
+    - name: Grant execute permission for gradlew
+      run: chmod +x gradlew
       
     - name: Build with Gradle
       run: ./gradlew clean serverJar

build.gradle

@@ -11,6 +11,24 @@ plugins {
     id 'java-library'
 }
 
+sourceCompatibility = 1.8
+targetCompatibility = 1.8
+
+// Set compiler to use UTF-8
+compileJava.options.encoding = 'UTF-8'
+compileTestJava.options.encoding = 'UTF-8'
+javadoc.options.encoding = 'UTF-8'
+
+tasks.withType(JavaCompile) {
+	options.encoding = 'UTF-8'
+}
+tasks.withType(Test) {
+	systemProperty('file.encoding', 'UTF-8')
+}
+tasks.withType(Javadoc){
+	options.encoding = 'UTF-8'
+}
+
 repositories {
     // Use jcenter for resolving dependencies.
     // You can declare any Maven/Ivy/file repository here.
@@ -31,7 +49,7 @@ dependencies {
 
     // This dependency is used internally, and not exposed to consumers on their own compile classpath.
     implementation 'com.google.guava:guava:28.2-jre'
-    
+
 
     // Use JUnit test framework
 	testImplementation platform('org.junit:junit-bom:5.7.2')
@@ -48,7 +66,7 @@ task serverJar (type: Jar, dependsOn: subprojects.tasks['build']) {
     manifest {
         attributes 'Main-Class': 'io.eiren.vr.Main'
     }
-    
+
     // Pack all dependencies within the JAR
     from {
         configurations.compile.collect { it.isDirectory() ? it : zipTree(it) }

src/main/java/io/eiren/gui/SkeletonConfig.java

@@ -11,6 +11,7 @@ import javax.swing.JCheckBox;
 import javax.swing.JLabel;
 import javax.swing.event.MouseInputAdapter;
 
+import io.eiren.gui.autobone.AutoBoneWindow;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.vr.VRServer;
@@ -18,26 +19,28 @@ import io.eiren.vr.processor.HumanSkeletonWithLegs;
 import io.eiren.vr.processor.HumanSkeleton;
 
 public class SkeletonConfig extends EJBag {
-	
+
 	private final VRServer server;
 	private final VRServerGUI gui;
+	private final AutoBoneWindow autoBone;
 	private Map<String, SkeletonLabel> labels = new HashMap<>();
-	
+
 	public SkeletonConfig(VRServer server, VRServerGUI gui) {
 		super();
 		this.server = server;
 		this.gui = gui;
+		this.autoBone = new AutoBoneWindow(server, this);
 
 		setAlignmentY(TOP_ALIGNMENT);
 		server.humanPoseProcessor.addSkeletonUpdatedCallback(this::skeletonUpdated);
 		skeletonUpdated(null);
 	}
-	
+
 	@ThreadSafe
 	public void skeletonUpdated(HumanSkeleton newSkeleton) {
 		java.awt.EventQueue.invokeLater(() -> {
 			removeAll();
-			
+
 			int row = 0;
 
 			add(new JCheckBox("Extended pelvis model") {{
@@ -63,7 +66,7 @@ public class SkeletonConfig extends EJBag {
 				}
 			}}, s(c(0, row, 1), 3, 1));
 			row++;
-			
+
 			/*
 			add(new JCheckBox("Extended knee model") {{
 				addItemListener(new ItemListener() {
@@ -89,17 +92,26 @@ public class SkeletonConfig extends EJBag {
 			}}, s(c(0, row, 1), 3, 1));
 			row++;
 			//*/
-			
+
 			add(new TimedResetButton("Reset All", "All"), s(c(1, row, 1), 3, 1));
+			add(new JButton("Auto") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						autoBone.setVisible(true);
+						autoBone.toFront();
+					}
+				});
+			}}, s(c(4, row, 1), 3, 1));
 			row++;
-			
+
 			add(new JLabel("Chest"), c(0, row, 1));
 			add(new AdjButton("+", "Chest", 0.01f), c(1, row, 1));
 			add(new SkeletonLabel("Chest"), c(2, row, 1));
 			add(new AdjButton("-", "Chest", -0.01f), c(3, row, 1));
 			add(new ResetButton("Reset", "Chest"), c(4, row, 1));
 			row++;
-			
+
 			add(new JLabel("Waist"), c(0, row, 1));
 			add(new AdjButton("+", "Waist", 0.01f), c(1, row, 1));
 			add(new SkeletonLabel("Waist"), c(2, row, 1));
@@ -148,25 +160,34 @@ public class SkeletonConfig extends EJBag {
 			add(new AdjButton("-", "Neck", -0.01f), c(3, row, 1));
 			add(new ResetButton("Reset", "Neck"), c(4, row, 1));
 			row++;
-			
+
 			add(new JLabel("Virtual waist"), c(0, row, 1));
 			add(new AdjButton("+", "Virtual waist", 0.01f), c(1, row, 1));
 			add(new SkeletonLabel("Virtual waist"), c(2, row, 1));
 			add(new AdjButton("-", "Virtual waist", -0.01f), c(3, row, 1));
 			add(new ResetButton("Reset", "Virtual waist"), c(4, row, 1));
 			row++;
-			
+
 			gui.refresh();
 		});
 	}
-	
+
+	@ThreadSafe
+	public void refreshAll() {
+		java.awt.EventQueue.invokeLater(() -> {
+			labels.forEach((joint, label) -> {
+				label.setText(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint) * 100, 0));
+			});
+		});
+	}
+
 	private void change(String joint, float diff) {
 		float current = server.humanPoseProcessor.getSkeletonConfig(joint);
 		server.humanPoseProcessor.setSkeletonConfig(joint, current + diff);
 		server.saveConfig();
 		labels.get(joint).setText(StringUtils.prettyNumber((current + diff) * 100, 0));
 	}
-	
+
 	private void reset(String joint) {
 		server.humanPoseProcessor.resetSkeletonConfig(joint);
 		server.saveConfig();
@@ -180,17 +201,17 @@ public class SkeletonConfig extends EJBag {
 			});
 		}
 	}
-	
+
 	private class SkeletonLabel extends JLabel {
-		
+
 		public SkeletonLabel(String joint) {
 			super(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint) * 100, 0));
 			labels.put(joint, this);
 		}
 	}
-	
+
 	private class AdjButton extends JButton {
-		
+
 		public AdjButton(String text, String joint, float diff) {
 			super(text);
 			addMouseListener(new MouseInputAdapter() {
@@ -201,9 +222,9 @@ public class SkeletonConfig extends EJBag {
 			});
 		}
 	}
-	
+
 	private class ResetButton extends JButton {
-		
+
 		public ResetButton(String text, String joint) {
 			super(text);
 			addMouseListener(new MouseInputAdapter() {
@@ -214,9 +235,9 @@ public class SkeletonConfig extends EJBag {
 			});
 		}
 	}
-	
+
 	private class TimedResetButton extends JButton {
-		
+
 		public TimedResetButton(String text, String joint) {
 			super(text);
 			addMouseListener(new MouseInputAdapter() {

src/main/java/io/eiren/gui/SkeletonList.java

@@ -18,6 +18,8 @@ import io.eiren.vr.processor.HumanSkeleton;
 import io.eiren.vr.processor.TransformNode;
 
 public class SkeletonList extends EJBag {
+	
+	private static final long UPDATE_DELAY = 50;
 
 	Quaternion q = new Quaternion();
 	Vector3f v = new Vector3f();
@@ -26,6 +28,7 @@ public class SkeletonList extends EJBag {
 	private final VRServer server;
 	private final VRServerGUI gui;
 	private final List<NodeStatus> nodes = new FastList<>();
+	private long lastUpdate = 0;
 	
 	public SkeletonList(VRServer server, VRServerGUI gui) {
 		super();
@@ -62,6 +65,9 @@ public class SkeletonList extends EJBag {
 	
 	@VRServerThread
 	public void updateBones() {
+		if(lastUpdate + UPDATE_DELAY > System.currentTimeMillis())
+			return;
+		lastUpdate = System.currentTimeMillis();
 		java.awt.EventQueue.invokeLater(() -> {
 			for(int i = 0; i < nodes.size(); ++i)
 				nodes.get(i).update();

src/main/java/io/eiren/gui/TrackersList.java

@@ -34,6 +34,8 @@ import io.eiren.vr.trackers.TrackerWithTPS;
 
 public class TrackersList extends EJBox {
 	
+	private static final long UPDATE_DELAY = 50;
+	
 	Quaternion q = new Quaternion();
 	Vector3f v = new Vector3f();
 	float[] angles = new float[3];
@@ -42,6 +44,7 @@ public class TrackersList extends EJBox {
 	
 	private final VRServer server;
 	private final VRServerGUI gui;
+	private long lastUpdate = 0;
 
 	public TrackersList(VRServer server, VRServerGUI gui) {
 		super(BoxLayout.PAGE_AXIS);
@@ -78,6 +81,9 @@ public class TrackersList extends EJBox {
 	
 	@ThreadSafe
 	public void updateTrackers() {
+		if(lastUpdate + UPDATE_DELAY > System.currentTimeMillis())
+			return;
+		lastUpdate = System.currentTimeMillis();
 		java.awt.EventQueue.invokeLater(() -> {
 			for(int i = 0; i < trackers.size(); ++i)
 				trackers.get(i).update();

src/main/java/io/eiren/gui/VRServerGUI.java

@@ -131,10 +131,10 @@ public class VRServerGUI extends JFrame {
 				JComboBox<String> trackersSelect;
 				add(trackersSelect = new JComboBox<>());
 				trackersSelect.addItem("Waist");
-				trackersSelect.addItem("Waist + Feet");
-				trackersSelect.addItem("Waist + Feet + Chest");
-				trackersSelect.addItem("Waist + Feet + Knees");
-				trackersSelect.addItem("Waist + Feet + Chest + Knees");
+				trackersSelect.addItem("Waist + Legs");
+				trackersSelect.addItem("Waist + Legs + Chest");
+				trackersSelect.addItem("Waist + Legs + Knees");
+				trackersSelect.addItem("Waist + Legs + Chest + Knees");
 				switch(server.config.getInt("virtualtrackers", 3)) {
 				case 1:
 					trackersSelect.setSelectedIndex(0);

src/main/java/io/eiren/gui/autobone/AutoBone.java

@@ -0,0 +1,512 @@
+package io.eiren.gui.autobone;
+
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Map.Entry;
+import java.util.function.Consumer;
+
+import com.jme3.math.Vector3f;
+
+import io.eiren.util.ann.ThreadSafe;
+import io.eiren.util.logging.LogManager;
+import io.eiren.util.collections.FastList;
+import io.eiren.vr.VRServer;
+import io.eiren.vr.processor.HumanSkeleton;
+import io.eiren.vr.processor.HumanSkeletonWithLegs;
+import io.eiren.vr.processor.HumanSkeletonWithWaist;
+import io.eiren.vr.processor.TrackerBodyPosition;
+import io.eiren.vr.trackers.TrackerUtils;
+
+public class AutoBone {
+
+	public class Epoch {
+
+		public final int epoch;
+		public final float epochError;
+
+		public Epoch(int epoch, float epochError) {
+			this.epoch = epoch;
+			this.epochError = epochError;
+		}
+
+		@Override
+		public String toString() {
+			return "Epoch: " + epoch + ", Epoch Error: " + epochError;
+		}
+	}
+
+	public int cursorIncrement = 1;
+
+	public int minDataDistance = 2;
+	public int maxDataDistance = 32;
+
+	public int numEpochs = 5;
+
+	public float initialAdjustRate = 2.5f;
+	public float adjustRateDecay = 1.01f;
+
+	public float slideErrorFactor = 1.0f;
+	public float offsetErrorFactor = 0.0f;
+	public float proportionErrorFactor = 0.2f;
+	public float heightErrorFactor = 0.1f;
+	public float positionErrorFactor = 0.0f;
+	public float positionOffsetErrorFactor = 0.0f;
+
+	/*
+	public float NECK_WAIST_RATIO_MIN = 0.2f;
+	public float NECK_WAIST_RATIO_MAX = 0.3f;
+
+	public float CHEST_WAIST_RATIO_MIN = 0.35f;
+	public float CHEST_WAIST_RATIO_MAX = 0.6f;
+
+	public float HIP_MIN = 0.08f;
+	public float HIP_WAIST_RATIO_MAX = 0.4f;
+
+	// Human average is 1.1235 (SD 0.07)
+	public float LEG_WAIST_RATIO_MIN = 1.1235f - ((0.07f * 3f) + 0.05f);
+	public float LEG_WAIST_RATIO_MAX = 1.1235f + ((0.07f * 3f) + 0.05f);
+
+	public float KNEE_LEG_RATIO_MIN = 0.42f;
+	public float KNEE_LEG_RATIO_MAX = 0.58f;
+	*/
+
+	protected final VRServer server;
+
+	protected HumanSkeletonWithLegs skeleton = null;
+
+	// This is filled by reloadConfigValues()
+	public final HashMap<String, Float> configs = new HashMap<String, Float>();
+	public final HashMap<String, Float> staticConfigs = new HashMap<String, Float>();
+
+	public final FastList<String> heightConfigs = new FastList<String>(new String[] {
+		"Neck",
+		"Waist",
+		"Legs length"
+	});
+
+	public AutoBone(VRServer server) {
+		this.server = server;
+
+		reloadConfigValues();
+
+		server.addSkeletonUpdatedCallback(this::skeletonUpdated);
+	}
+
+	public void reloadConfigValues() {
+		// 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) ||
+		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));
+		} else {
+			// Otherwise, make sure it's not used
+			configs.remove("Chest");
+			staticConfigs.put("Chest", server.config.getFloat("body.chestDistance", 0.42f));
+		}
+
+		// Load leg configs
+		staticConfigs.put("Hips width", server.config.getFloat("body.hipsWidth", HumanSkeletonWithLegs.HIPS_WIDTH_DEFAULT));
+		configs.put("Legs length", server.config.getFloat("body.legsLength", 0.84f));
+		configs.put("Knee height", server.config.getFloat("body.kneeHeight", 0.42f));
+	}
+
+	@ThreadSafe
+	public void skeletonUpdated(HumanSkeleton newSkeleton) {
+		if (newSkeleton instanceof HumanSkeletonWithLegs) {
+			skeleton = (HumanSkeletonWithLegs)newSkeleton;
+			applyConfigToSkeleton(newSkeleton);
+			LogManager.log.info("[AutoBone] Received updated skeleton");
+		}
+	}
+
+	public void applyConfig() {
+		if (!applyConfigToSkeleton(skeleton)) {
+			// Unable to apply to skeleton, save directly
+			saveConfigs();
+		}
+	}
+
+	public boolean applyConfigToSkeleton(HumanSkeleton skeleton) {
+		if (skeleton == null) {
+			return false;
+		}
+
+		for (Entry<String, Float> entry : configs.entrySet()) {
+			skeleton.setSkeletonConfig(entry.getKey(), entry.getValue());
+		}
+
+		server.saveConfig();
+
+		LogManager.log.info("[AutoBone] Configured skeleton bone lengths");
+		return true;
+	}
+
+	private void setConfig(String name, String path) {
+		Float value = configs.get(name);
+		if (value != null) {
+			server.config.setProperty(path, value);
+		}
+	}
+
+	// This doesn't require a skeleton, therefore can be used if skeleton is null
+	public void saveConfigs() {
+		setConfig("Head", "body.headShift");
+		setConfig("Neck", "body.neckLength");
+		setConfig("Waist", "body.waistDistance");
+		setConfig("Chest", "body.chestDistance");
+		setConfig("Hips width", "body.hipsWidth");
+		setConfig("Legs length", "body.legsLength");
+		setConfig("Knee height", "body.kneeHeight");
+
+		server.saveConfig();
+	}
+
+	public Float getConfig(String config) {
+		Float configVal = configs.get(config);
+		return configVal != null ? configVal : staticConfigs.get(config);
+	}
+
+	public Float getConfig(String config, Map<String, Float> configs, Map<String, Float> configsAlt) {
+		if (configs == null) {
+			throw new NullPointerException("Argument \"configs\" must not be null");
+		}
+
+		Float configVal = configs.get(config);
+		return configVal != null || configsAlt == null ? configVal : configsAlt.get(config);
+	}
+
+	public float getHeight(Map<String, Float> configs) {
+		return getHeight(configs, null);
+	}
+
+	public float getHeight(Map<String, Float> configs, Map<String, Float> configsAlt) {
+		float height = 0f;
+
+		for (String heightConfig : heightConfigs) {
+			Float length = getConfig(heightConfig, configs, configsAlt);
+			if (length != null) {
+				height += length;
+			}
+		}
+
+		return height;
+	}
+
+	public float getLengthSum(Map<String, Float> configs) {
+		float length = 0f;
+
+		for (float boneLength : configs.values()) {
+			length += boneLength;
+		}
+
+		return length;
+	}
+
+	public float getMaxHmdHeight(PoseFrame[] frames) {
+		float maxHeight = 0f;
+		for (PoseFrame frame : frames) {
+			if (frame.rootPos.y > maxHeight) {
+				maxHeight = frame.rootPos.y;
+			}
+		}
+		return maxHeight;
+	}
+
+	public void processFrames(PoseFrame[] frames) {
+		processFrames(frames, -1f);
+	}
+
+	public void processFrames(PoseFrame[] frames, Consumer<Epoch> epochCallback) {
+		processFrames(frames, -1f, epochCallback);
+	}
+
+	public void processFrames(PoseFrame[] frames, float targetHeight) {
+		processFrames(frames, true, targetHeight);
+	}
+
+	public void processFrames(PoseFrame[] frames, float targetHeight, Consumer<Epoch> epochCallback) {
+		processFrames(frames, true, targetHeight, epochCallback);
+	}
+
+	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);
+
+		// If target height isn't specified, auto-detect
+		if (targetHeight < 0f) {
+			if (skeleton != null) {
+				targetHeight = getHeight(skeleton.getSkeletonConfig());
+				LogManager.log.warning("[AutoBone] Target height loaded from skeleton (Make sure you reset before running!): " + targetHeight);
+			} else {
+				float hmdHeight = getMaxHmdHeight(frames);
+				if (hmdHeight <= 0.50f) {
+					LogManager.log.warning("[AutoBone] Max headset height detected (Value seems too low, did you not stand up straight while measuring?): " + hmdHeight);
+				} else {
+					LogManager.log.info("[AutoBone] Max headset height detected: " + hmdHeight);
+				}
+
+				// Estimate target height from HMD height
+				targetHeight = hmdHeight;
+			}
+		}
+
+		for (int epoch = calcInitError ? -1 : 0; epoch < numEpochs; epoch++) {
+			float sumError = 0f;
+			int errorCount = 0;
+
+			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");
+					}
+
+					skeleton1.setSkeletonConfigs(configs);
+					skeleton2.setSkeletonConfigs(configs);
+
+					skeleton1.setPoseFromFrame(frame1);
+					skeleton2.setPoseFromFrame(frame2);
+
+					float totalLength = getLengthSum(configs);
+					float curHeight = getHeight(configs, staticConfigs);
+					float errorDeriv = getErrorDeriv(frame1, frame2, 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();
+
+						// Reset error sum values
+						sumError = 0f;
+						errorCount = 0;
+
+						// Continue on new data
+						continue;
+					}
+
+					// Store the error count for logging purposes
+					sumError += errorDeriv;
+					errorCount++;
+
+					float adjustVal = error * adjustRate;
+
+					for (Entry<String, Float> entry : configs.entrySet()) {
+						// Skip adjustment if the epoch is before starting (for logging only)
+						if (epoch < 0) {
+							break;
+						}
+
+						float originalLength = entry.getValue();
+
+						// Try positive and negative adjustments
+						boolean isHeightVar = heightConfigs.contains(entry.getKey());
+						float minError = errorDeriv;
+						float finalNewLength = -1f;
+						for (int i = 0; i < 2; i++) {
+							// Scale by the ratio for smooth adjustment and more stable results
+							float curAdjustVal = ((i == 0 ? adjustVal : -adjustVal) * originalLength) / totalLength;
+							float newLength = originalLength + curAdjustVal;
+
+							// No small or negative numbers!!! Bad algorithm!
+							if (newLength < 0.01f) {
+								continue;
+							}
+
+							updateSkeletonBoneLength(skeleton1, skeleton2, entry.getKey(), newLength);
+
+							float newHeight = isHeightVar ? curHeight + curAdjustVal : curHeight;
+							float newErrorDeriv = getErrorDeriv(frame1, frame2, skeleton1, skeleton2, targetHeight - newHeight);
+
+							if (newErrorDeriv < minError) {
+								minError = newErrorDeriv;
+								finalNewLength = newLength;
+							}
+						}
+
+						if (finalNewLength > 0f) {
+							entry.setValue(finalNewLength);
+						}
+
+						// Reset the length to minimize bias in other variables, it's applied later
+						updateSkeletonBoneLength(skeleton1, skeleton2, entry.getKey(), originalLength);
+					}
+				}
+			}
+
+			// Calculate average error over the epoch
+			float avgError = errorCount > 0 ? sumError / errorCount : -1f;
+			LogManager.log.info("[AutoBone] Epoch " + (epoch + 1) + " average error: " + avgError);
+
+			if (epochCallback != null) {
+				epochCallback.accept(new Epoch(epoch + 1, avgError));
+			}
+		}
+
+		float finalHeight = getHeight(configs, staticConfigs);
+		LogManager.log.info("[AutoBone] Target height: " + targetHeight + " New height: " + finalHeight);
+
+		return Math.abs(finalHeight - targetHeight);
+	}
+
+	// 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());
+
+		// 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;
+	}
+
+	// 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 dist3 = Math.abs(skeleton1.getLeftFootPos().y - skeleton2.getRightFootPos().y);
+		float dist4 = Math.abs(skeleton1.getLeftFootPos().y - skeleton2.getRightFootPos().y);
+
+		float dist5 = Math.abs(skeleton1.getLeftFootPos().y - skeleton2.getLeftFootPos().y);
+		float dist6 = Math.abs(skeleton1.getRightFootPos().y - skeleton2.getRightFootPos().y);
+
+		// 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;
+	}
+
+	// The distance from average human proportions
+	protected float getProportionErrorDeriv(SimpleSkeleton skeleton) {
+		Float neckLength = skeleton.getSkeletonConfig("Neck");
+		Float chestLength = skeleton.getSkeletonConfig("Chest");
+		Float waistLength = skeleton.getSkeletonConfig("Waist");
+		Float legsLength = skeleton.getSkeletonConfig("Legs length");
+		Float kneeHeight = skeleton.getSkeletonConfig("Knee height");
+
+		float chestWaist = chestLength != null && waistLength != null ? Math.abs((chestLength / waistLength) - 0.5f) : 0f;
+		float legBody = legsLength != null && waistLength != null && neckLength != null ? Math.abs((legsLength / (waistLength + neckLength)) - 1.1235f) : 0f;
+		float kneeLeg = kneeHeight != null && legsLength != null ? Math.abs((kneeHeight / legsLength) - 0.5f) : 0f;
+
+		// SD of 0.07, capture 68% within range
+		float sdValue = 0.07f;
+		if (legBody <= sdValue) {
+			legBody = 0f;
+		} else {
+			legBody -= sdValue;
+		}
+
+		return (chestWaist + legBody + kneeLeg) / 3f;
+	}
+
+	// The distance of any points to the corresponding absolute position
+	protected float getPositionErrorDeriv(PoseFrame 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++;
+				}
+			}
+		}
+
+		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);
+				offsetCount++;
+			}
+		}
+
+		return offsetCount > 0 ? offset / offsetCount : 0f;
+	}
+
+	protected float getErrorDeriv(PoseFrame frame1, PoseFrame frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, float heightChange) {
+		float totalError = 0f;
+		float sumWeight = 0f;
+
+		if (slideErrorFactor > 0f) {
+			totalError += getSlideErrorDeriv(skeleton1, skeleton2) * slideErrorFactor;
+			sumWeight += slideErrorFactor;
+		}
+
+		if (offsetErrorFactor > 0f) {
+			totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * offsetErrorFactor;
+			sumWeight += offsetErrorFactor;
+		}
+
+		if (proportionErrorFactor > 0f) {
+			// Either skeleton will work fine, skeleton1 is used as a default
+			totalError += getProportionErrorDeriv(skeleton1) * proportionErrorFactor;
+			sumWeight += proportionErrorFactor;
+		}
+
+		if (heightErrorFactor > 0f) {
+			totalError += Math.abs(heightChange) * heightErrorFactor;
+			sumWeight += heightErrorFactor;
+		}
+
+		if (positionErrorFactor > 0f) {
+			totalError += (getPositionErrorDeriv(frame1, skeleton1) + getPositionErrorDeriv(frame2, skeleton2) / 2f) * positionErrorFactor;
+			sumWeight += positionErrorFactor;
+		}
+
+		if (positionOffsetErrorFactor > 0f) {
+			totalError += getPositionOffsetErrorDeriv(frame1, frame2, skeleton1, skeleton2) * positionOffsetErrorFactor;
+			sumWeight += positionOffsetErrorFactor;
+		}
+
+		// Minimize sliding, minimize foot height offset, minimize change in total height
+		return sumWeight > 0f ? totalError / sumWeight : 0f;
+	}
+
+	// Mean square error function
+	protected static float errorFunc(float errorDeriv) {
+		return 0.5f * (errorDeriv * errorDeriv);
+	}
+
+	protected void updateSkeletonBoneLength(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, String joint, float newLength) {
+		skeleton1.setSkeletonConfig(joint, newLength, true);
+		skeleton2.setSkeletonConfig(joint, newLength, true);
+	}
+}

src/main/java/io/eiren/gui/autobone/AutoBoneWindow.java

@@ -0,0 +1,414 @@
+package io.eiren.gui.autobone;
+
+import javax.swing.BoxLayout;
+import javax.swing.JFrame;
+import javax.swing.JLabel;
+import javax.swing.JScrollPane;
+import javax.swing.ScrollPaneConstants;
+import javax.swing.JButton;
+import javax.swing.border.EmptyBorder;
+import java.awt.event.MouseEvent;
+import java.io.File;
+import java.util.List;
+import java.util.Map.Entry;
+import java.util.concurrent.Future;
+
+import io.eiren.gui.EJBox;
+import io.eiren.gui.SkeletonConfig;
+import io.eiren.util.StringUtils;
+import io.eiren.util.ann.AWTThread;
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+import io.eiren.vr.VRServer;
+import javax.swing.event.MouseInputAdapter;
+
+import org.apache.commons.lang3.tuple.Pair;
+
+public class AutoBoneWindow extends JFrame {
+
+	private static File saveDir = new File("Recordings");
+	private static File loadDir = new File("LoadRecordings");
+
+	private EJBox pane;
+
+	private final VRServer server;
+	private final SkeletonConfig skeletonConfig;
+	private final PoseRecorder poseRecorder;
+	private final AutoBone autoBone;
+
+	private Thread recordingThread = null;
+	private Thread saveRecordingThread = null;
+	private Thread autoBoneThread = null;
+
+	private JButton saveRecordingButton;
+	private JButton adjustButton;
+	private JButton applyButton;
+
+	private JLabel processLabel;
+	private JLabel lengthsLabel;
+
+	public AutoBoneWindow(VRServer server, SkeletonConfig skeletonConfig) {
+		super("Skeleton Auto-Configuration");
+
+		this.server = server;
+		this.skeletonConfig = skeletonConfig;
+		this.poseRecorder = new PoseRecorder(server);
+		this.autoBone = new AutoBone(server);
+
+		getContentPane().setLayout(new BoxLayout(getContentPane(), BoxLayout.PAGE_AXIS));
+		add(new JScrollPane(pane = new EJBox(BoxLayout.PAGE_AXIS), ScrollPaneConstants.VERTICAL_SCROLLBAR_AS_NEEDED, ScrollPaneConstants.HORIZONTAL_SCROLLBAR_AS_NEEDED));
+
+		build();
+	}
+
+	private String getLengthsString() {
+		boolean first = true;
+		StringBuilder configInfo = new StringBuilder("");
+		for (Entry<String, Float> entry : autoBone.configs.entrySet()) {
+			if (!first) {
+				configInfo.append(", ");
+			} else {
+				first = false;
+			}
+
+			configInfo.append(entry.getKey() + ": " + StringUtils.prettyNumber(entry.getValue() * 100f, 2));
+		}
+
+		return configInfo.toString();
+	}
+
+	private void saveRecording(PoseFrame[] frames) {
+		if (saveDir.isDirectory() || saveDir.mkdirs()) {
+			File saveRecording;
+			int recordingIndex = 1;
+			do {
+				saveRecording = new File(saveDir, "ABRecording" + recordingIndex++ + ".abf");
+			} while (saveRecording.exists());
+
+			LogManager.log.info("[AutoBone] Exporting frames to \"" + saveRecording.getPath() + "\"...");
+			if (PoseFrameIO.writeToFile(saveRecording, frames)) {
+				LogManager.log.info("[AutoBone] Done exporting! Recording can be found at \"" + saveRecording.getPath() + "\".");
+			} else {
+				LogManager.log.severe("[AutoBone] Failed to export the recording to \"" + saveRecording.getPath() + "\".");
+			}
+		} else {
+			LogManager.log.severe("[AutoBone] Failed to create the recording directory \"" + saveDir.getPath() + "\".");
+		}
+	}
+
+	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")) {
+						LogManager.log.info("[AutoBone] Detected recording at \"" + file.getPath() + "\", loading frames...");
+						PoseFrame[] frames = PoseFrameIO.readFromFile(file);
+
+						if (frames == null) {
+							LogManager.log.severe("Reading frames from \"" + file.getPath() + "\" failed...");
+						} else {
+							recordings.add(Pair.of(file.getName(), frames));
+						}
+					}
+				}
+			}
+		}
+
+		return recordings;
+	}
+
+	private float processFrames(PoseFrame[] frames) {
+		autoBone.reloadConfigValues();
+
+		autoBone.minDataDistance = server.config.getInt("autobone.minimumDataDistance", autoBone.minDataDistance);
+		autoBone.maxDataDistance = server.config.getInt("autobone.maximumDataDistance", autoBone.maxDataDistance);
+
+		autoBone.numEpochs = server.config.getInt("autobone.epochCount", autoBone.numEpochs);
+
+		autoBone.initialAdjustRate = server.config.getFloat("autobone.adjustRate", autoBone.initialAdjustRate);
+		autoBone.adjustRateDecay = server.config.getFloat("autobone.adjustRateDecay", autoBone.adjustRateDecay);
+
+		autoBone.slideErrorFactor = server.config.getFloat("autobone.slideErrorFactor", autoBone.slideErrorFactor);
+		autoBone.offsetErrorFactor = server.config.getFloat("autobone.offsetErrorFactor", autoBone.offsetErrorFactor);
+		autoBone.proportionErrorFactor = server.config.getFloat("autobone.proportionErrorFactor", autoBone.proportionErrorFactor);
+		autoBone.heightErrorFactor = server.config.getFloat("autobone.heightErrorFactor", autoBone.heightErrorFactor);
+		autoBone.positionErrorFactor = server.config.getFloat("autobone.positionErrorFactor", autoBone.positionErrorFactor);
+		autoBone.positionOffsetErrorFactor = server.config.getFloat("autobone.positionOffsetErrorFactor", autoBone.positionOffsetErrorFactor);
+
+		boolean calcInitError = server.config.getBoolean("autobone.calculateInitialError", true);
+		float targetHeight = server.config.getFloat("autobone.manualTargetHeight", -1f);
+		return autoBone.processFrames(frames, calcInitError, targetHeight, (epoch) -> {
+			processLabel.setText(epoch.toString());
+			lengthsLabel.setText(getLengthsString());
+		});
+	}
+
+	@AWTThread
+	private void build() {
+		pane.add(new EJBox(BoxLayout.LINE_AXIS) {{
+			setBorder(new EmptyBorder(i(5)));
+			add(new JButton("Start Recording") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						// Prevent running multiple times
+						if (!isEnabled() || recordingThread != null) {
+							return;
+						}
+
+						Thread thread = new Thread() {
+							@Override
+							public void run() {
+								try {
+									if (poseRecorder.isReadyToRecord()) {
+										setText("Recording...");
+										// 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();
+										LogManager.log.info("[AutoBone] Done recording!");
+
+										saveRecordingButton.setEnabled(true);
+										adjustButton.setEnabled(true);
+
+										if (server.config.getBoolean("autobone.saveRecordings", false)) {
+											setText("Saving...");
+											saveRecording(frames);
+										}
+									} else {
+										setText("Not Ready...");
+										LogManager.log.severe("[AutoBone] Unable to record...");
+										Thread.sleep(3000); // Wait for 3 seconds
+										return;
+									}
+								} catch (Exception e) {
+									setText("Recording Failed...");
+									LogManager.log.severe("[AutoBone] Failed recording!", e);
+									try {
+										Thread.sleep(3000); // Wait for 3 seconds
+									} catch (Exception e1) {
+										// Ignore
+									}
+								} finally {
+									setText("Start Recording");
+									recordingThread = null;
+								}
+							}
+						};
+
+						recordingThread = thread;
+						thread.start();
+					}
+				});
+			}});
+
+			add(saveRecordingButton = new JButton("Save Recording") {{
+				setEnabled(poseRecorder.hasRecording());
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						// Prevent running multiple times
+						if (!isEnabled() || saveRecordingThread != null) {
+							return;
+						}
+
+						Thread thread = new Thread() {
+							@Override
+							public void run() {
+								try {
+									Future<PoseFrame[]> framesFuture = poseRecorder.getFramesAsync();
+									if (framesFuture != null) {
+										setText("Waiting for Recording...");
+										PoseFrame[] frames = framesFuture.get();
+
+										if (frames.length <= 0) {
+											throw new IllegalStateException("Recording has no frames");
+										}
+
+										setText("Saving...");
+										saveRecording(frames);
+									} else {
+										setText("No Recording...");
+										LogManager.log.severe("[AutoBone] Unable to save, no recording was done...");
+										try {
+											Thread.sleep(3000); // Wait for 3 seconds
+										} catch (Exception e1) {
+											// Ignore
+										}
+										return;
+									}
+								} catch (Exception e) {
+									setText("Saving Failed...");
+									LogManager.log.severe("[AutoBone] Failed to save recording!", e);
+									try {
+										Thread.sleep(3000); // Wait for 3 seconds
+									} catch (Exception e1) {
+										// Ignore
+									}
+								} finally {
+									setText("Save Recording");
+									saveRecordingThread = null;
+								}
+							}
+						};
+
+						saveRecordingThread = thread;
+						thread.start();
+					}
+				});
+			}});
+
+			add(adjustButton = new JButton("Auto-Adjust") {{
+				// If there are files to load, enable the button
+				setEnabled(poseRecorder.hasRecording() || (loadDir.isDirectory() && loadDir.list().length > 0));
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						// Prevent running multiple times
+						if (!isEnabled() || autoBoneThread != null) {
+							return;
+						}
+
+						Thread thread = new Thread() {
+							@Override
+							public void run() {
+								try {
+									setText("Load...");
+									List<Pair<String, PoseFrame[]>> frameRecordings = loadRecordings();
+
+									if (frameRecordings.size() > 0) {
+										LogManager.log.info("[AutoBone] Done loading frames!");
+									} else {
+										Future<PoseFrame[]> framesFuture = poseRecorder.getFramesAsync();
+										if (framesFuture != null) {
+											setText("Waiting for Recording...");
+											PoseFrame[] frames = framesFuture.get();
+
+											if (frames.length <= 0) {
+												throw new IllegalStateException("Recording has no frames");
+											}
+
+											frameRecordings.add(Pair.of("<Recording>", frames));
+										} else {
+											setText("No Recordings...");
+											LogManager.log.severe("[AutoBone] No recordings found in \"" + loadDir.getPath() + "\" and no recording was done...");
+											try {
+												Thread.sleep(3000); // Wait for 3 seconds
+											} catch (Exception e1) {
+												// Ignore
+											}
+											return;
+										}
+									}
+
+									setText("Processing...");
+									LogManager.log.info("[AutoBone] Processing frames...");
+									FastList<Float> heightPercentError = new FastList<Float>(frameRecordings.size());
+									for (Pair<String, PoseFrame[]> recording : frameRecordings) {
+										LogManager.log.info("[AutoBone] Processing frames from \"" + recording.getKey() + "\"...");
+
+										heightPercentError.add(processFrames(recording.getValue()));
+										LogManager.log.info("[AutoBone] Done processing!");
+										applyButton.setEnabled(true);
+
+										//#region Stats/Values
+										Float neckLength = autoBone.getConfig("Neck");
+										Float chestLength = autoBone.getConfig("Chest");
+										Float waistLength = autoBone.getConfig("Waist");
+										Float hipWidth = autoBone.getConfig("Hips width");
+										Float legsLength = autoBone.getConfig("Legs length");
+										Float kneeHeight = autoBone.getConfig("Knee height");
+
+										float neckWaist = neckLength != null && waistLength != null ? neckLength / waistLength : 0f;
+										float chestWaist = chestLength != null && waistLength != null ? chestLength / waistLength : 0f;
+										float hipWaist = hipWidth != null && waistLength != null ? hipWidth / waistLength : 0f;
+										float legWaist = legsLength != null && waistLength != null ? legsLength / waistLength : 0f;
+										float legBody = legsLength != null && waistLength != null && neckLength != null ? legsLength / (waistLength + neckLength) : 0f;
+										float kneeLeg = kneeHeight != null && legsLength != null ? kneeHeight / legsLength : 0f;
+
+										LogManager.log.info("[AutoBone] Ratios: [{Neck-Waist: " + StringUtils.prettyNumber(neckWaist) +
+										"}, {Chest-Waist: " + StringUtils.prettyNumber(chestWaist) +
+										"}, {Hip-Waist: " + StringUtils.prettyNumber(hipWaist) +
+										"}, {Leg-Waist: " + StringUtils.prettyNumber(legWaist) +
+										"}, {Leg-Body: " + StringUtils.prettyNumber(legBody) +
+										"}, {Knee-Leg: " + StringUtils.prettyNumber(kneeLeg) + "}]");
+
+										String lengthsString = getLengthsString();
+										LogManager.log.info("[AutoBone] Length values: " + lengthsString);
+										lengthsLabel.setText(lengthsString);
+									}
+
+									if (heightPercentError.size() > 0) {
+										float mean = 0f;
+										for (float val : heightPercentError) {
+											mean += val;
+										}
+										mean /= heightPercentError.size();
+
+										float std = 0f;
+										for (float val : heightPercentError) {
+											float stdVal = val - mean;
+											std += stdVal * stdVal;
+										}
+										std = (float)Math.sqrt(std / heightPercentError.size());
+
+										LogManager.log.info("[AutoBone] Average height error: " + StringUtils.prettyNumber(mean, 6) + " (SD " + StringUtils.prettyNumber(std, 6) + ")");
+									}
+									//#endregion
+								} catch (Exception e) {
+									setText("Failed...");
+									LogManager.log.severe("[AutoBone] Failed adjustment!", e);
+									try {
+										Thread.sleep(3000); // Wait for 3 seconds
+									} catch (Exception e1) {
+										// Ignore
+									}
+								} finally {
+									setText("Auto-Adjust");
+									autoBoneThread = null;
+								}
+							}
+						};
+
+						autoBoneThread = thread;
+						thread.start();
+					}
+				});
+			}});
+
+			add(applyButton = new JButton("Apply Values") {{
+				setEnabled(false);
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						if (!isEnabled()) {
+							return;
+						}
+
+						autoBone.applyConfig();
+						// Update GUI values after applying
+						skeletonConfig.refreshAll();
+					}
+				});
+			}});
+		}});
+
+		pane.add(new EJBox(BoxLayout.LINE_AXIS) {{
+			setBorder(new EmptyBorder(i(5)));
+			add(processLabel = new JLabel("Processing has not been started..."));
+		}});
+
+		pane.add(new EJBox(BoxLayout.LINE_AXIS) {{
+			setBorder(new EmptyBorder(i(5)));
+			add(lengthsLabel = new JLabel(getLengthsString()));
+		}});
+
+		// Pack and display
+		pack();
+		setLocationRelativeTo(null);
+		setVisible(false);
+	}
+}

src/main/java/io/eiren/gui/autobone/PoseFrame.java

@@ -0,0 +1,37 @@
+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;
+	}
+}

src/main/java/io/eiren/gui/autobone/PoseFrameIO.java

@@ -0,0 +1,174 @@
+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;
+	}
+}

src/main/java/io/eiren/gui/autobone/PoseRecorder.java

@@ -0,0 +1,136 @@
+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;
+	}
+}

src/main/java/io/eiren/gui/autobone/SimpleSkeleton.java

@@ -0,0 +1,291 @@
+package io.eiren.gui.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 io.eiren.vr.processor.HumanSkeletonWithLegs;
+import io.eiren.vr.processor.HumanSkeletonWithWaist;
+import io.eiren.vr.processor.TransformNode;
+import io.eiren.yaml.YamlFile;
+
+public class SimpleSkeleton {
+
+	// Waist
+	protected final TransformNode hmdNode = new TransformNode("HMD", false);
+	protected final TransformNode headNode = new TransformNode("Head", false);
+	protected final TransformNode neckNode = new TransformNode("Neck", false);
+	protected final TransformNode waistNode = new TransformNode("Waist", false);
+	protected final TransformNode chestNode = new TransformNode("Chest", false);
+
+	protected float chestDistance = 0.42f;
+	/**
+	 * Distance from eyes to waist
+	 */
+	protected float waistDistance = 0.85f;
+	/**
+	 * Distance from eyes to the base of the neck
+	 */
+	protected float neckLength = HumanSkeletonWithWaist.NECK_LENGTH_DEFAULT;
+	/**
+	 * Distance from eyes to ear
+	 */
+	protected float headShift = HumanSkeletonWithWaist.HEAD_SHIFT_DEFAULT;
+
+	// Legs
+	protected final TransformNode leftHipNode = new TransformNode("Left-Hip", false);
+	protected final TransformNode leftKneeNode = new TransformNode("Left-Knee", false);
+	protected final TransformNode leftAnkleNode = new TransformNode("Left-Ankle", false);
+	protected final TransformNode rightHipNode = new TransformNode("Right-Hip", false);
+	protected final TransformNode rightKneeNode = new TransformNode("Right-Knee", false);
+	protected final TransformNode rightAnkleNode = new TransformNode("Right-Ankle", false);
+
+	/**
+	 * Distance between centers of both hips
+	 */
+	protected float hipsWidth = HumanSkeletonWithLegs.HIPS_WIDTH_DEFAULT;
+	/**
+	 * Length from waist to knees
+	 */
+	protected float kneeHeight = 0.42f;
+	/**
+	 * Distance from waist to ankle
+	 */
+	protected float legsLength = 0.84f;
+
+	protected final HashMap<String, TransformNode> nodes = new HashMap<String, TransformNode>();
+
+	public SimpleSkeleton() {
+		// Assemble skeleton to waist
+		hmdNode.attachChild(headNode);
+		headNode.localTransform.setTranslation(0, 0, headShift);
+
+		headNode.attachChild(neckNode);
+		neckNode.localTransform.setTranslation(0, -neckLength, 0);
+
+		neckNode.attachChild(chestNode);
+		chestNode.localTransform.setTranslation(0, -chestDistance, 0);
+
+		chestNode.attachChild(waistNode);
+		waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0);
+
+		// Assemble skeleton to feet
+		waistNode.attachChild(leftHipNode);
+		leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
+
+		waistNode.attachChild(rightHipNode);
+		rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
+
+		leftHipNode.attachChild(leftKneeNode);
+		leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+
+		rightHipNode.attachChild(rightKneeNode);
+		rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+
+		leftKneeNode.attachChild(leftAnkleNode);
+		leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+
+		rightKneeNode.attachChild(rightAnkleNode);
+		rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+
+		// Set up a HashMap to get nodes by name easily
+		hmdNode.depthFirstTraversal(visitor -> {
+			nodes.put(visitor.getName(), visitor);
+		});
+	}
+
+	public SimpleSkeleton(Iterable<Entry<String, Float>> configs, Iterable<Entry<String, Float>> altConfigs) {
+		// Initialize
+		this();
+
+		// Set configs
+		if (altConfigs != null) {
+			// Set alts first, so if there's any overlap it doesn't affect the values
+			setSkeletonConfigs(altConfigs);
+		}
+		setSkeletonConfigs(configs);
+	}
+
+	public SimpleSkeleton(Map<String, Float> configs, Map<String, Float> altConfigs) {
+		this(configs.entrySet(), altConfigs.entrySet());
+	}
+
+	public SimpleSkeleton(Iterable<Entry<String, Float>> configs) {
+		this(configs, null);
+	}
+
+	public SimpleSkeleton(Map<String, Float> configs) {
+		this(configs.entrySet());
+	}
+
+	public void setPoseFromSkeleton(HumanSkeletonWithLegs humanSkeleton) {
+		TransformNode rootNode = humanSkeleton.getRootNode();
+
+		// Copy headset position
+		hmdNode.localTransform.setTranslation(rootNode.localTransform.getTranslation());
+
+		// Copy all rotations
+		rootNode.depthFirstTraversal(visitor -> {
+			TransformNode targetNode = nodes.get(visitor.getName());
+
+			// Handle unexpected nodes gracefully
+			if (targetNode != null) {
+				targetNode.localTransform.setRotation(visitor.localTransform.getRotation());
+			}
+		});
+	}
+
+	public void setPoseFromFrame(PoseFrame frame) {
+		// Copy headset position
+		hmdNode.localTransform.setTranslation(frame.rootPos);
+
+		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());
+				}
+			}
+		}
+
+		updatePose();
+	}
+
+	public void setSkeletonConfigs(Iterable<Entry<String, Float>> configs) {
+		for (Entry<String, Float> config : configs) {
+			setSkeletonConfig(config.getKey(), config.getValue());
+		}
+	}
+
+	public void setSkeletonConfigs(Map<String, Float> configs) {
+		setSkeletonConfigs(configs.entrySet());
+	}
+
+	public void setSkeletonConfig(String joint, float newLength) {
+		setSkeletonConfig(joint, newLength, false);
+	}
+
+	public void setSkeletonConfig(String joint, float newLength, boolean updatePose) {
+		switch(joint) {
+		case "Head":
+			headShift = newLength;
+			headNode.localTransform.setTranslation(0, 0, headShift);
+			if (updatePose) {
+				headNode.update();
+			}
+			break;
+		case "Neck":
+			neckLength = newLength;
+			neckNode.localTransform.setTranslation(0, -neckLength, 0);
+			if (updatePose) {
+				neckNode.update();
+			}
+			break;
+		case "Waist":
+			waistDistance = newLength;
+			waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0);
+			if (updatePose) {
+				waistNode.update();
+			}
+			break;
+		case "Chest":
+			chestDistance = newLength;
+			chestNode.localTransform.setTranslation(0, -chestDistance, 0);
+			waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0);
+			if (updatePose) {
+				chestNode.update();
+			}
+			break;
+		case "Hips width":
+			hipsWidth = newLength;
+			leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
+			rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
+			if (updatePose) {
+				leftHipNode.update();
+				rightHipNode.update();
+			}
+			break;
+		case "Knee height":
+			kneeHeight = newLength;
+			leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+			rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+			leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+			rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+			if (updatePose) {
+				leftKneeNode.update();
+				rightKneeNode.update();
+			}
+			break;
+		case "Legs length":
+			legsLength = newLength;
+			leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+			rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+			if (updatePose) {
+				leftKneeNode.update();
+				rightKneeNode.update();
+			}
+			break;
+		}
+	}
+
+	public Float getSkeletonConfig(String joint) {
+		switch(joint) {
+		case "Head":
+			return headShift;
+		case "Neck":
+			return neckLength;
+		case "Waist":
+			return waistDistance;
+		case "Chest":
+			return chestDistance;
+		case "Hips width":
+			return hipsWidth;
+		case "Knee height":
+			return kneeHeight;
+		case "Legs length":
+			return legsLength;
+		}
+
+		return null;
+	}
+
+	public void updatePose() {
+		hmdNode.update();
+	}
+
+	public Vector3f getNodePosition(String node) {
+		TransformNode transformNode = nodes.get(node);
+		return transformNode != null ? transformNode.worldTransform.getTranslation() : null;
+	}
+
+	public Vector3f getHMDPos() {
+		return hmdNode.worldTransform.getTranslation();
+	}
+
+	public Vector3f getLeftFootPos() {
+		return leftAnkleNode.worldTransform.getTranslation();
+	}
+
+	public Vector3f getRightFootPos() {
+		return rightAnkleNode.worldTransform.getTranslation();
+	}
+
+	public void saveConfigs(YamlFile config) {
+		// Save waist configs
+		config.setProperty("body.headShift", headShift);
+		config.setProperty("body.neckLength", neckLength);
+		config.setProperty("body.waistDistance", waistDistance);
+		config.setProperty("body.chestDistance", chestDistance);
+
+		// Save leg configs
+		config.setProperty("body.hipsWidth", hipsWidth);
+		config.setProperty("body.kneeHeight", kneeHeight);
+		config.setProperty("body.legsLength", legsLength);
+	}
+}

src/main/java/io/eiren/vr/Main.java

@@ -7,7 +7,7 @@ import io.eiren.util.logging.LogManager;
 
 public class Main {
 	
-	public static String VERSION = "0.0.14";
+	public static String VERSION = "0.0.15";
 	
 	public static VRServer vrServer;
 	

src/main/java/io/eiren/vr/VRServer.java

@@ -19,6 +19,7 @@ import io.eiren.util.ann.ThreadSecure;
 import io.eiren.util.ann.VRServerThread;
 import io.eiren.util.collections.FastList;
 import io.eiren.vr.bridge.NamedPipeVRBridge;
+import io.eiren.vr.bridge.SteamVRPipeInputBridge;
 import io.eiren.vr.bridge.VMCBridge;
 import io.eiren.vr.bridge.VRBridge;
 import io.eiren.vr.processor.HumanPoseProcessor;
@@ -57,6 +58,10 @@ public class VRServer extends Thread {
 		NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers, this);
 		tasks.add(() -> driverBridge.start());
 		bridges.add(driverBridge);
+		// Create named pipe bridge for SteamVR input
+		SteamVRPipeInputBridge steamVRInput = new SteamVRPipeInputBridge(this);
+		tasks.add(() -> steamVRInput.start());
+		bridges.add(steamVRInput);
 		
 		// Create VMCBridge
 		try {
@@ -189,14 +194,13 @@ public class VRServer extends Thread {
 					break;
 				task.run();
 			} while(true);
-			for(int i = 0; i < trackers.size(); ++i)
-				trackers.get(i).tick();
-			
 			for(int i = 0; i < onTick.size(); ++i) {
 				this.onTick.get(i).run();
 			}
 			for(int i = 0; i < bridges.size(); ++i)
 				bridges.get(i).dataRead();
+			for(int i = 0; i < trackers.size(); ++i)
+				trackers.get(i).tick();
 			humanPoseProcessor.update();
 			for(int i = 0; i < bridges.size(); ++i)
 				bridges.get(i).dataWrite();

src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java

@@ -183,7 +183,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	
 	private boolean tryOpeningPipe(Pipe pipe) {
 		if(Kernel32.INSTANCE.ConnectNamedPipe(pipe.pipeHandle, null)) {
-			pipe.state = NamedPipeVRBridge.PipeState.OPEN;
+			pipe.state = PipeState.OPEN;
 			LogManager.log.info("[VRBridge] Pipe " + pipe.name + " is open");
 			return true;
 		}
@@ -246,21 +246,4 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 		} catch(Exception e) {
 		}
 	}
-	
-	private static class Pipe {
-		final String name;
-		final HANDLE pipeHandle;
-		PipeState state = PipeState.CREATED;
-		
-		public Pipe(HANDLE pipeHandle, String name) {
-			this.pipeHandle = pipeHandle;
-			this.name = name;
-		}
-	}
-	
-	private static enum PipeState {
-		CREATED,
-		OPEN,
-		ERROR;
-	}
 }

src/main/java/io/eiren/vr/bridge/Pipe.java

@@ -0,0 +1,14 @@
+package io.eiren.vr.bridge;
+
+import com.sun.jna.platform.win32.WinNT.HANDLE;
+
+class Pipe {
+	final String name;
+	final HANDLE pipeHandle;
+	PipeState state = PipeState.CREATED;
+	
+	public Pipe(HANDLE pipeHandle, String name) {
+		this.pipeHandle = pipeHandle;
+		this.name = name;
+	}
+}

src/main/java/io/eiren/vr/bridge/PipeState.java

@@ -0,0 +1,7 @@
+package io.eiren.vr.bridge;
+
+enum PipeState {
+	CREATED,
+	OPEN,
+	ERROR;
+}

src/main/java/io/eiren/vr/bridge/SteamVRPipeInputBridge.java

@@ -0,0 +1,263 @@
+package io.eiren.vr.bridge;
+
+import java.io.IOException;
+import java.util.HashMap;
+import java.util.Iterator;
+import java.util.List;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+import org.apache.commons.lang3.StringUtils;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+import com.sun.jna.platform.win32.Kernel32;
+import com.sun.jna.platform.win32.WinBase;
+import com.sun.jna.ptr.IntByReference;
+
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+import io.eiren.vr.VRServer;
+import io.eiren.vr.processor.TrackerBodyPosition;
+import io.eiren.vr.trackers.SteamVRTracker;
+import io.eiren.vr.trackers.TrackerStatus;
+
+public class SteamVRPipeInputBridge extends Thread implements VRBridge {
+
+	private static final int MAX_COMMAND_LENGTH = 2048;
+	public static final String PipeName = "\\\\.\\pipe\\SlimeVRInput";
+
+	private final byte[] buffArray = new byte[1024];
+	private final VRServer server;
+	private final StringBuilder commandBuilder = new StringBuilder(1024);
+	private final List<SteamVRTracker> trackers = new FastList<>();
+	private final Map<Integer, SteamVRTracker> trackersInternal = new HashMap<>();
+	private AtomicBoolean newData = new AtomicBoolean(false);
+	private final Vector3f vBuffer = new Vector3f();
+	private final Quaternion qBuffer = new Quaternion();
+	private Pipe pipe;
+	
+	public SteamVRPipeInputBridge(VRServer server) {
+		this.server = server;
+	}
+	
+	@Override
+	public void run() {
+		try {
+			createPipes();
+			while(true) {
+				waitForPipesToOpen();
+				if(areAllPipesOpen()) {
+					boolean pipesUpdated = updatePipes(); // Update at HMDs frequency
+					if(!pipesUpdated) {
+						Thread.sleep(5); // Up to 200Hz
+					}
+				} else {
+					Thread.sleep(10);
+				}
+			}
+		} catch(Exception e) {
+			e.printStackTrace();
+		}
+	}
+	
+	private void waitForPipesToOpen() {
+		if(pipe.state == PipeState.CREATED) {
+			tryOpeningPipe(pipe);
+		}
+	}
+	
+	public boolean updatePipes() throws IOException {
+		if(pipe.state == PipeState.OPEN) {
+			IntByReference bytesAvailable = new IntByReference(0);
+			if(Kernel32.INSTANCE.PeekNamedPipe(pipe.pipeHandle, null, 0, null, bytesAvailable, null)) {
+				if(bytesAvailable.getValue() > 0) {
+					while(Kernel32.INSTANCE.ReadFile(pipe.pipeHandle, buffArray, buffArray.length, bytesAvailable, null)) {
+						int bytesRead = bytesAvailable.getValue();
+						for(int i = 0; i < bytesRead; ++i) {
+							char c = (char) buffArray[i];
+							if(c == '\n') {
+								executeInputCommand();
+								commandBuilder.setLength(0);
+							} else {
+								commandBuilder.append(c);
+								if(commandBuilder.length() >= MAX_COMMAND_LENGTH)
+									throw new IOException("Command from the pipe is too long");
+							}
+						}
+						if(bytesRead < buffArray.length)
+							break; // Don't repeat, we read all available bytes
+					}
+					return true;
+				}
+			}
+		}
+		return false;
+	}
+	
+	private void executeInputCommand() throws IOException {
+		String[] command = commandBuilder.toString().split(" ");
+		switch(command[0]) {
+		case "ADD": // Add new tracker
+			if(command.length < 4)
+				throw new IOException("Error in ADD command. Command requires at least 4 arguments. Supplied: " + commandBuilder.toString());
+			SteamVRTracker internalTracker = new SteamVRTracker(Integer.parseInt(command[1]), StringUtils.join(command, " ", 3, command.length));
+			int roleId = Integer.parseInt(command[2]);
+			if(roleId >= 0 && roleId < SteamVRInputRoles.values.length) {
+				SteamVRInputRoles svrRole = SteamVRInputRoles.values[roleId];
+				internalTracker.bodyPosition = svrRole.bodyPosition;
+			}
+			SteamVRTracker oldTracker;
+			synchronized(trackersInternal) {
+				oldTracker = trackersInternal.put(internalTracker.id, internalTracker);
+			}
+			if(oldTracker != null)
+				throw new IOException("New tracker added with the same id. Supplied: " + commandBuilder.toString());
+			newData.set(true);
+			break;
+		case "UPD": // Update tracker data
+			if(command.length < 9)
+				throw new IOException("Error in UPD command. Command requires at least 9 arguments. Supplied: " + commandBuilder.toString());
+			int id = Integer.parseInt(command[1]);
+			double x = Double.parseDouble(command[2]);
+			double y = Double.parseDouble(command[3]);
+			double z = Double.parseDouble(command[4]);
+			double qw = Double.parseDouble(command[5]);
+			double qx = Double.parseDouble(command[6]);
+			double qy = Double.parseDouble(command[7]);
+			double qz = Double.parseDouble(command[8]);
+			internalTracker = trackersInternal.get(id);
+			if(internalTracker != null) {
+				internalTracker.position.set((float) x, (float) y, (float) z);
+				internalTracker.rotation.set((float) qx, (float) qy, (float) qz, (float) qw);
+				internalTracker.dataTick();
+				newData.set(true);
+			}
+			break;
+		case "STA": // Update tracker status
+			if(command.length < 3)
+				throw new IOException("Error in STA command. Command requires at least 3 arguments. Supplied: " + commandBuilder.toString());
+			id = Integer.parseInt(command[1]);
+			int status = Integer.parseInt(command[2]);
+			TrackerStatus st = TrackerStatus.getById(status);
+			if(st == null)
+				throw new IOException("Unrecognized status id. Supplied: " + commandBuilder.toString());
+			internalTracker = trackersInternal.get(id);
+			if(internalTracker != null) {
+				internalTracker.setStatus(st);
+				newData.set(true);
+			}
+			break;
+		}
+	}
+	
+	@Override
+	public void dataRead() {
+		// Not used, only input
+	}
+	
+	@Override
+	public void dataWrite() {
+		if(newData.getAndSet(false)) {
+			if(trackers.size() < trackersInternal.size()) {
+				// Add new trackers
+				synchronized(trackersInternal) {
+					Iterator<SteamVRTracker> iterator = trackersInternal.values().iterator();
+					internal: while(iterator.hasNext()) {
+						SteamVRTracker internalTracker = iterator.next();
+						for(int i = 0; i < trackers.size(); ++i) {
+							SteamVRTracker t = trackers.get(i);
+							if(t.id == internalTracker.id)
+								continue internal;
+						}
+						// Tracker is not found in current trackers
+						SteamVRTracker tracker = new SteamVRTracker(internalTracker.id, internalTracker.getName());
+						tracker.bodyPosition = internalTracker.bodyPosition;
+						trackers.add(tracker);
+						server.registerTracker(tracker);
+					}
+				}
+			}
+			for(int i = 0; i < trackers.size(); ++i) {
+				SteamVRTracker tracker = trackers.get(i);
+				SteamVRTracker internal = trackersInternal.get(tracker.id);
+				if(internal == null)
+					throw new NullPointerException("Lost internal tracker somehow: " + tracker.id); // Shouln't really happen even, but better to catch it like this
+				if(internal.getPosition(vBuffer))
+					tracker.position.set(vBuffer);
+				if(internal.getRotation(qBuffer))
+					tracker.rotation.set(qBuffer);
+				tracker.dataTick();
+			}
+		}
+	}
+	
+	private boolean tryOpeningPipe(Pipe pipe) {
+		if(Kernel32.INSTANCE.ConnectNamedPipe(pipe.pipeHandle, null)) {
+			pipe.state = PipeState.OPEN;
+			LogManager.log.info("[SteamVRPipeInputBridge] Pipe " + pipe.name + " is open");
+			return true;
+		}
+		
+		LogManager.log.info("[SteamVRPipeInputBridge] Error connecting to pipe " + pipe.name + ": " + Kernel32.INSTANCE.GetLastError());
+		return false;
+	}
+	
+	private boolean areAllPipesOpen() {
+		if(pipe == null || pipe.state == PipeState.CREATED) {
+			return false;
+		}
+		return true;
+	}
+	
+	private void createPipes() throws IOException {
+		try {
+			pipe = new Pipe(Kernel32.INSTANCE.CreateNamedPipe(PipeName, WinBase.PIPE_ACCESS_DUPLEX, // dwOpenMode
+					WinBase.PIPE_TYPE_BYTE | WinBase.PIPE_READMODE_BYTE | WinBase.PIPE_WAIT, // dwPipeMode
+					1, // nMaxInstances,
+					1024 * 16, // nOutBufferSize,
+					1024 * 16, // nInBufferSize,
+					0, // nDefaultTimeOut,
+					null), PipeName); // lpSecurityAttributes
+			LogManager.log.info("[SteamVRPipeInputBridge] Pipe " + pipe.name + " created");
+			if(WinBase.INVALID_HANDLE_VALUE.equals(pipe.pipeHandle))
+				throw new IOException("Can't open " + PipeName + " pipe: " + Kernel32.INSTANCE.GetLastError());
+			LogManager.log.info("[SteamVRPipeInputBridge] Pipes are open");
+		} catch(IOException e) {
+			safeDisconnect(pipe);
+			throw e;
+		}
+	}
+	
+	public static void safeDisconnect(Pipe pipe) {
+		try {
+			if(pipe != null && pipe.pipeHandle != null)
+				Kernel32.INSTANCE.DisconnectNamedPipe(pipe.pipeHandle);
+		} catch(Exception e) {
+		}
+	}
+	
+	public enum SteamVRInputRoles {
+		HEAD(TrackerBodyPosition.HMD),
+		LEFT_HAND(TrackerBodyPosition.LEFT_CONTROLLER),
+		RIGHT_HAND(TrackerBodyPosition.RIGHT_CONTROLLER),
+		LEFT_FOOT(TrackerBodyPosition.LEFT_FOOT),
+		RIGHT_FOOT(TrackerBodyPosition.RIGHT_FOOT),
+		LEFT_SHOULDER(TrackerBodyPosition.NONE),
+		RIGHT_SHOULDER(TrackerBodyPosition.NONE),
+		LEFT_ELBOW(TrackerBodyPosition.NONE),
+		RIGHT_ELBOW(TrackerBodyPosition.NONE),
+		LEFT_KNEE(TrackerBodyPosition.LEFT_LEG),
+		RIGHT_KNEE(TrackerBodyPosition.RIGHT_LEG),
+		WAIST(TrackerBodyPosition.WAIST),
+		CHEST(TrackerBodyPosition.CHEST),
+		;
+		
+		private static final SteamVRInputRoles[] values = values();
+		public final TrackerBodyPosition bodyPosition;
+		
+		private SteamVRInputRoles(TrackerBodyPosition slimeVrPosition) {
+			this.bodyPosition = slimeVrPosition;
+		}
+	}
+}

src/main/java/io/eiren/vr/processor/HumanSkeletonWithLegs.java

@@ -265,6 +265,7 @@ public class HumanSkeletonWithLegs extends HumanSkeletonWithWaist {
 			kneeBuf.slerp(hipBuf, 0.5f);
 			waistNode.localTransform.setRotation(kneeBuf);
 			// TODO : Use vectors to add like 50% of wasit tracker yaw to waist node to reduce drift and let user take weird poses
+			// TODO Set virtual waist node yaw to that of waist node
 		}
 	}
 	

src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java

@@ -15,6 +15,8 @@ public enum TrackerBodyPosition {
 	RIGHT_ANKLE("body:right_ankle"),
 	LEFT_FOOT("body:left_foot"),
 	RIGHT_FOOT("body:right_foot"),
+	LEFT_CONTROLLER("body:left_controller"),
+	RIGHT_CONTROLLER("body:right_conroller"),
 	;
 	
 	public final String designation;

src/main/java/io/eiren/vr/trackers/SteamVRTracker.java

@@ -0,0 +1,24 @@
+package io.eiren.vr.trackers;
+
+import io.eiren.util.BufferedTimer;
+
+public class SteamVRTracker extends ComputedTracker implements TrackerWithTPS {
+
+	public final int id;
+	protected BufferedTimer timer = new BufferedTimer(1f);
+	
+	public SteamVRTracker(int id, String name) {
+		super(name);
+		this.id = id;
+	}
+	
+	@Override
+	public float getTPS() {
+		return timer.getAverageFPS();
+	}
+	
+	@Override
+	public void dataTick() {
+		timer.update();
+	}
+}

src/main/java/io/eiren/vr/trackers/TrackerStatus.java

@@ -2,15 +2,31 @@ package io.eiren.vr.trackers;
 
 public enum TrackerStatus {
 	
-	DISCONNECTED(false),
-	OK(true),
-	BUSY(true),
-	ERROR(false),
+	DISCONNECTED(0, false),
+	OK(1, true),
+	BUSY(2, true),
+	ERROR(3, false),
+	OCCLUDED(4, false),
 	;
 	
+	private static final TrackerStatus byId[] = new TrackerStatus[5];
+	
+	public final int id;
 	public final boolean sendData;
 	
-	private TrackerStatus(boolean sendData) {
+	private TrackerStatus(int id, boolean sendData) {
 		this.sendData = sendData;
+		this.id = id;
+	}
+	
+	public static TrackerStatus getById(int id) {
+		if(id < 0 || id >= byId.length)
+			return null;
+		return byId[id];
+	}
+	
+	static {
+		for(TrackerStatus st : values())
+			byId[st.id] = st;
 	}
 }