Bump version to 0.1.3

Skeleton refactor

.github/workflows/gradle.yml

@@ -10,12 +10,12 @@ jobs:
     runs-on: ubuntu-latest
 
     steps:
-      - uses: actions/checkout@v2.3.4
+      - uses: actions/checkout@v2.4.0
         with:
             submodules: recursive
 
       - name: Set up JDK 11
-        uses: actions/setup-java@v2.1.0
+        uses: actions/setup-java@v2.4.0
         with:
           java-version: '11'
           distribution: 'adopt'
@@ -31,12 +31,12 @@ jobs:
     runs-on: ubuntu-latest
 
     steps:
-      - uses: actions/checkout@v2.3.4
+      - uses: actions/checkout@v2.4.0
         with:
             submodules: recursive
 
       - name: Set up JDK 11
-        uses: actions/setup-java@v2.1.0
+        uses: actions/setup-java@v2.4.0
         with:
           java-version: '11'
           distribution: 'adopt'

resources/run.bat

@@ -1 +1,19 @@
-@java -Xmx512M -jar slimevr.jar
+@echo off
+setlocal enableextensions
+cd /d "%~dp0"
+
+where java >nul 2>&1
+if %errorlevel% EQU 0 (
+    java -Xmx512M -jar slimevr.jar
+) else (
+    echo Java was not found in your system.
+    echo.
+    echo Either use SlimeVR Installer to install the server by following this link:
+    echo https://github.com/SlimeVR/SlimeVR-Installer/releases/latest/download/slimevr_web_installer.exe
+    echo.
+    echo Or download Java 11 by following this link:
+    echo https://adoptium.net/releases.html?variant=openjdk11^&jvmVariant=hotspot
+)
+if %errorlevel% NEQ 0 (
+    pause
+)

src/main/java/dev/slimevr/Main.java

@@ -1,18 +1,21 @@
-package io.eiren.vr;
+package dev.slimevr;
 
 import java.io.File;
+import java.io.IOException;
+import java.net.ServerSocket;
 
 import javax.swing.JOptionPane;
 
 import org.apache.commons.lang3.JavaVersion;
 import org.apache.commons.lang3.SystemUtils;
 
+import dev.slimevr.gui.Keybinding;
 import dev.slimevr.gui.VRServerGUI;
 import io.eiren.util.logging.LogManager;
 
 public class Main {
 	
-	public static String VERSION = "0.1.1";
+	public static String VERSION = "0.1.3";
 	
 	public static VRServer vrServer;
 	
@@ -34,6 +37,16 @@ public class Main {
 			return;
 		}
 		
+		try {
+			new ServerSocket(6969).close();
+			new ServerSocket(35903).close();
+			new ServerSocket(21110).close();
+		} catch (IOException e) {
+			LogManager.log.severe("SlimeVR start-up error! Required ports are busy. Make sure there is no other instance of SlimeVR Server running.");
+			JOptionPane.showMessageDialog(null, "SlimeVR start-up error! Required ports are busy. Make sure there is no other instance of SlimeVR Server running.", "SlimeVR: Ports are busy", JOptionPane.ERROR_MESSAGE);
+			return;
+		}
+
 		try {
 			vrServer = new VRServer();
 			vrServer.start();

src/main/java/dev/slimevr/VRServer.java

@@ -1,4 +1,4 @@
-package io.eiren.vr;
+package dev.slimevr;
 
 import java.io.File;
 import java.io.FileInputStream;
@@ -20,21 +20,21 @@ import dev.slimevr.bridge.NamedPipeBridge;
 import dev.slimevr.bridge.SteamVRPipeInputBridge;
 import dev.slimevr.bridge.VMCBridge;
 import dev.slimevr.bridge.WebSocketVRBridge;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.HumanPoseProcessor;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerConfig;
+import dev.slimevr.vr.trackers.TrackersUDPServer;
 import io.eiren.util.OperatingSystem;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.util.ann.ThreadSecure;
-import io.eiren.util.ann.VRServerThread;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.processor.HumanPoseProcessor;
-import io.eiren.vr.processor.HumanSkeleton;
-import io.eiren.vr.trackers.HMDTracker;
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.TrackersUDPServer;
 import io.eiren.yaml.YamlException;
 import io.eiren.yaml.YamlFile;
 import io.eiren.yaml.YamlNode;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerConfig;
 
 public class VRServer extends Thread {
 	

src/main/java/dev/slimevr/autobone/AutoBone.java

@@ -1,24 +1,30 @@
 package dev.slimevr.autobone;
 
-import java.util.HashMap;
+import java.util.EnumMap;
+import java.util.List;
 import java.util.Map;
 import java.util.Map.Entry;
 import java.util.function.Consumer;
 
+import com.jme3.math.FastMath;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.VRServer;
+import dev.slimevr.poserecorder.PoseFrameSkeleton;
+import dev.slimevr.poserecorder.PoseFrameTracker;
 import dev.slimevr.poserecorder.PoseFrames;
 import dev.slimevr.poserecorder.TrackerFrame;
 import dev.slimevr.poserecorder.TrackerFrameData;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+import dev.slimevr.vr.processor.skeleton.SimpleSkeleton;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.TrackerUtils;
 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.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerUtils;
 
 public class AutoBone {
 	
@@ -49,12 +55,17 @@ public class AutoBone {
 	public float adjustRateDecay = 1.01f;
 	
 	public float slideErrorFactor = 1.0f;
+	public float offsetSlideErrorFactor = 0.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;
 	
+	// TODO Needs much more work, probably going to rethink how the errors work to avoid this barely functional workaround @ButterscotchVanilla
+	// For scaling distances, since smaller sizes will cause smaller distances
+	//private float totalLengthBase = 2f;
+	
 	// Human average is probably 1.1235 (SD 0.07)
 	public float legBodyRatio = 1.1235f;
 	// SD of 0.07, capture 68% within range
@@ -62,18 +73,20 @@ public class AutoBone {
 	
 	// Assume these to be approximately half
 	public float kneeLegRatio = 0.5f;
-	public float chestWaistRatio = 0.5f;
+	public float chestTorsoRatio = 0.5f;
 	
 	protected final VRServer server;
 	
-	protected HumanSkeletonWithLegs skeleton = null;
+	protected SimpleSkeleton 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 EnumMap<SkeletonConfigValue, Float> configs = new EnumMap<SkeletonConfigValue, Float>(SkeletonConfigValue.class);
+	public final EnumMap<SkeletonConfigValue, Float> staticConfigs = new EnumMap<SkeletonConfigValue, Float>(SkeletonConfigValue.class);
 	
-	public final FastList<String> heightConfigs = new FastList<String>(new String[]{"Neck", "Waist", "Legs length"
-	});
+	public final FastList<SkeletonConfigValue> heightConfigs = new FastList<SkeletonConfigValue>(new SkeletonConfigValue[]{
+			SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, SkeletonConfigValue.LEGS_LENGTH});
+	public final FastList<SkeletonConfigValue> lengthConfigs = new FastList<SkeletonConfigValue>(new SkeletonConfigValue[]{
+			SkeletonConfigValue.HEAD, SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, SkeletonConfigValue.HIPS_WIDTH, SkeletonConfigValue.LEGS_LENGTH});
 	
 	public AutoBone(VRServer server) {
 		this.server = server;
@@ -87,31 +100,48 @@ public class AutoBone {
 		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, TrackerPosition.CHEST) != null) || TrackerUtils.findTrackerForBodyPosition(server.getAllTrackers(), TrackerPosition.CHEST) != null) {
+	private float readFromConfig(SkeletonConfigValue configValue) {
+		return server.config.getFloat(configValue.configKey, configValue.defaultValue);
+	}
+	
+	public void reloadConfigValues(List<PoseFrameTracker> trackers) {
+		// Load torso configs
+		staticConfigs.put(SkeletonConfigValue.HEAD, readFromConfig(SkeletonConfigValue.HEAD));
+		staticConfigs.put(SkeletonConfigValue.NECK, readFromConfig(SkeletonConfigValue.NECK));
+		configs.put(SkeletonConfigValue.TORSO, readFromConfig(SkeletonConfigValue.TORSO));
+		if(server.config.getBoolean("autobone.forceChestTracker", false) || (trackers != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.CHEST) != null)) {
 			// If force enabled or has a chest tracker
-			configs.put("Chest", server.config.getFloat("body.chestDistance", 0.42f));
+			staticConfigs.remove(SkeletonConfigValue.CHEST);
+			configs.put(SkeletonConfigValue.CHEST, readFromConfig(SkeletonConfigValue.CHEST));
 		} else {
 			// Otherwise, make sure it's not used
-			configs.remove("Chest");
-			staticConfigs.put("Chest", server.config.getFloat("body.chestDistance", 0.42f));
+			configs.remove(SkeletonConfigValue.CHEST);
+			staticConfigs.put(SkeletonConfigValue.CHEST, readFromConfig(SkeletonConfigValue.CHEST));
+		}
+		if(server.config.getBoolean("autobone.forceHipTracker", false) || (trackers != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.HIP) != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.WAIST) != null)) {
+			// If force enabled or has a hip tracker and waist tracker
+			staticConfigs.remove(SkeletonConfigValue.WAIST);
+			configs.put(SkeletonConfigValue.WAIST, readFromConfig(SkeletonConfigValue.WAIST));
+		} else {
+			// Otherwise, make sure it's not used
+			configs.remove(SkeletonConfigValue.WAIST);
+			staticConfigs.put(SkeletonConfigValue.WAIST, readFromConfig(SkeletonConfigValue.WAIST));
 		}
 		
 		// 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));
+		staticConfigs.put(SkeletonConfigValue.HIPS_WIDTH, readFromConfig(SkeletonConfigValue.HIPS_WIDTH));
+		configs.put(SkeletonConfigValue.LEGS_LENGTH, readFromConfig(SkeletonConfigValue.LEGS_LENGTH));
+		configs.put(SkeletonConfigValue.KNEE_HEIGHT, readFromConfig(SkeletonConfigValue.KNEE_HEIGHT));
+		
+		// Keep "feet" at ankles
+		staticConfigs.put(SkeletonConfigValue.FOOT_LENGTH, 0f);
+		staticConfigs.put(SkeletonConfigValue.FOOT_OFFSET, 0f);
 	}
 	
 	@ThreadSafe
 	public void skeletonUpdated(HumanSkeleton newSkeleton) {
-		if(newSkeleton instanceof HumanSkeletonWithLegs) {
-			skeleton = (HumanSkeletonWithLegs) newSkeleton;
+		if(newSkeleton instanceof SimpleSkeleton) {
+			skeleton = (SimpleSkeleton) newSkeleton;
 			applyConfigToSkeleton(newSkeleton);
 			LogManager.log.info("[AutoBone] Received updated skeleton");
 		}
@@ -129,40 +159,37 @@ public class AutoBone {
 			return false;
 		}
 		
-		configs.forEach(skeleton::setSkeletonConfig);
-		
+		SkeletonConfig skeletonConfig = skeleton.getSkeletonConfig();
+		skeletonConfig.setConfigs(configs, null);
+		skeletonConfig.saveToConfig(server.config);
 		server.saveConfig();
 		
 		LogManager.log.info("[AutoBone] Configured skeleton bone lengths");
 		return true;
 	}
 	
-	private void setConfig(String name, String path) {
-		Float value = configs.get(name);
+	private void setConfig(SkeletonConfigValue config) {
+		Float value = configs.get(config);
 		if(value != null) {
-			server.config.setProperty(path, value);
+			server.config.setProperty(config.configKey, 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");
+		for(SkeletonConfigValue config : SkeletonConfigValue.values) {
+			setConfig(config);
+		}
 		
 		server.saveConfig();
 	}
 	
-	public Float getConfig(String config) {
+	public Float getConfig(SkeletonConfigValue 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) {
+	public Float getConfig(SkeletonConfigValue config, Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigValue, Float> configsAlt) {
 		if(configs == null) {
 			throw new NullPointerException("Argument \"configs\" must not be null");
 		}
@@ -171,27 +198,46 @@ public class AutoBone {
 		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;
+	public float sumSelectConfigs(List<SkeletonConfigValue> selection, Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigValue, Float> configsAlt) {
+		float sum = 0f;
 		
-		for(String heightConfig : heightConfigs) {
-			Float length = getConfig(heightConfig, configs, configsAlt);
+		for(SkeletonConfigValue config : selection) {
+			Float length = getConfig(config, configs, configsAlt);
 			if(length != null) {
-				height += length;
+				sum += length;
 			}
 		}
 		
-		return height;
+		return sum;
 	}
 	
-	public float getLengthSum(Map<String, Float> configs) {
+	public float sumSelectConfigs(List<SkeletonConfigValue> selection, SkeletonConfig skeletonConfig) {
+		float sum = 0f;
+		
+		for(SkeletonConfigValue config : selection) {
+			sum += skeletonConfig.getConfig(config);
+		}
+		
+		return sum;
+	}
+	
+	public float getLengthSum(Map<SkeletonConfigValue, Float> configs) {
+		return getLengthSum(configs, null);
+	}
+	
+	public float getLengthSum(Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigValue, Float> configsAlt) {
 		float length = 0f;
 		
-		for(float boneLength : configs.values()) {
+		if(configsAlt != null) {
+			for(Entry<SkeletonConfigValue, Float> config : configsAlt.entrySet()) {
+				// If there isn't a duplicate config
+				if(!configs.containsKey(config.getKey())) {
+					length += config.getValue();
+				}
+			}
+		}
+		
+		for(Float boneLength : configs.values()) {
 			length += boneLength;
 		}
 		
@@ -232,19 +278,16 @@ public class AutoBone {
 	public float processFrames(PoseFrames 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()];
+		List<PoseFrameTracker> trackers = frames.getTrackers();
+		reloadConfigValues(trackers); // Reload configs and detect chest tracker from the first frame
 		
-		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()];
+		final PoseFrameSkeleton skeleton1 = new PoseFrameSkeleton(trackers, null, configs, staticConfigs);
+		final PoseFrameSkeleton skeleton2 = new PoseFrameSkeleton(trackers, null, configs, staticConfigs);
 		
 		// If target height isn't specified, auto-detect
 		if(targetHeight < 0f) {
 			if(skeleton != null) {
-				targetHeight = getHeight(skeleton.getSkeletonConfig());
+				targetHeight = sumSelectConfigs(heightConfigs, skeleton.getSkeletonConfig());
 				LogManager.log.warning("[AutoBone] Target height loaded from skeleton (Make sure you reset before running!): " + targetHeight);
 			} else {
 				float hmdHeight = getMaxHmdHeight(frames);
@@ -263,29 +306,32 @@ public class AutoBone {
 			float sumError = 0f;
 			int errorCount = 0;
 			
-			float adjustRate = epoch >= 0 ? (float) (initialAdjustRate / Math.pow(adjustRateDecay, epoch)) : 0f;
+			float adjustRate = epoch >= 0 ? (initialAdjustRate / FastMath.pow(adjustRateDecay, epoch)) : 0f;
 			
 			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);
+					int frameCursor2 = frameCursor + cursorOffset;
 					
-					skeleton1.setSkeletonConfigs(configs);
-					skeleton2.setSkeletonConfigs(configs);
+					skeleton1.skeletonConfig.setConfigs(configs, null);
+					skeleton2.skeletonConfig.setConfigs(configs, null);
 					
-					skeleton1.setPoseFromFrame(trackerBuffer1);
-					skeleton2.setPoseFromFrame(trackerBuffer2);
+					skeleton1.setCursor(frameCursor);
+					skeleton1.updatePose();
+					
+					skeleton2.setCursor(frameCursor2);
+					skeleton2.updatePose();
 					
 					float totalLength = getLengthSum(configs);
-					float curHeight = getHeight(configs, staticConfigs);
-					float errorDeriv = getErrorDeriv(trackerBuffer1, trackerBuffer2, skeleton1, skeleton2, targetHeight - curHeight);
+					float curHeight = sumSelectConfigs(heightConfigs, configs, staticConfigs);
+					//float scaleLength = sumSelectConfigs(lengthConfigs, configs, staticConfigs);
+					float errorDeriv = getErrorDeriv(frames, frameCursor, frameCursor2, skeleton1, skeleton2, targetHeight - curHeight, 1f);
 					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(trackerBuffer1);
+						reloadConfigValues(trackers);
 						
 						// Reset error sum values
 						sumError = 0f;
@@ -301,7 +347,12 @@ public class AutoBone {
 					
 					float adjustVal = error * adjustRate;
 					
-					for(Entry<String, Float> entry : configs.entrySet()) {
+					// If there is no adjustment whatsoever, skip this
+					if(adjustVal == 0f) {
+						continue;
+					}
+					
+					for(Entry<SkeletonConfigValue, Float> entry : configs.entrySet()) {
 						// Skip adjustment if the epoch is before starting (for logging only)
 						if(epoch < 0) {
 							break;
@@ -311,6 +362,7 @@ public class AutoBone {
 						
 						// Try positive and negative adjustments
 						boolean isHeightVar = heightConfigs.contains(entry.getKey());
+						//boolean isLengthVar = lengthConfigs.contains(entry.getKey());
 						float minError = errorDeriv;
 						float finalNewLength = -1f;
 						for(int i = 0; i < 2; i++) {
@@ -326,7 +378,8 @@ public class AutoBone {
 							updateSkeletonBoneLength(skeleton1, skeleton2, entry.getKey(), newLength);
 							
 							float newHeight = isHeightVar ? curHeight + curAdjustVal : curHeight;
-							float newErrorDeriv = getErrorDeriv(trackerBuffer1, trackerBuffer2, skeleton1, skeleton2, targetHeight - newHeight);
+							//float newScaleLength = isLengthVar ? scaleLength + curAdjustVal : scaleLength;
+							float newErrorDeriv = getErrorDeriv(frames, frameCursor, frameCursor2, skeleton1, skeleton2, targetHeight - newHeight, 1f);
 							
 							if(newErrorDeriv < minError) {
 								minError = newErrorDeriv;
@@ -353,53 +406,80 @@ public class AutoBone {
 			}
 		}
 		
-		float finalHeight = getHeight(configs, staticConfigs);
+		float finalHeight = sumSelectConfigs(heightConfigs, configs, staticConfigs);
 		LogManager.log.info("[AutoBone] Target height: " + targetHeight + " New height: " + finalHeight);
 		
-		return Math.abs(finalHeight - targetHeight);
+		return FastMath.abs(finalHeight - targetHeight);
 	}
 	
 	// The change in position of the ankle over time
-	protected float getSlideErrorDeriv(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) {
-		float slideLeft = skeleton1.getNodePosition(TrackerPosition.LEFT_ANKLE).distance(skeleton2.getNodePosition(TrackerPosition.LEFT_ANKLE));
-		float slideRight = skeleton1.getNodePosition(TrackerPosition.RIGHT_ANKLE).distance(skeleton2.getNodePosition(TrackerPosition.RIGHT_ANKLE));
+	protected float getSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) {
+		float slideLeft = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position.distance(skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position);
+		float slideRight = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position.distance(skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position);
 		
 		// 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 change in distance between both of the ankles over time
+	protected float getOffsetSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) {
+		Vector3f leftFoot1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position;
+		Vector3f rightFoot1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position;
+		
+		Vector3f leftFoot2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position;
+		Vector3f rightFoot2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position;
+		
+		float slideDist1 = leftFoot1.distance(rightFoot1);
+		float slideDist2 = leftFoot2.distance(rightFoot2);
+		
+		float slideDist3 = leftFoot1.distance(rightFoot2);
+		float slideDist4 = leftFoot2.distance(rightFoot1);
+		
+		float dist1 = FastMath.abs(slideDist1 - slideDist2);
+		float dist2 = FastMath.abs(slideDist3 - slideDist4);
+		
+		float dist3 = FastMath.abs(slideDist1 - slideDist3);
+		float dist4 = FastMath.abs(slideDist1 - slideDist4);
+		
+		float dist5 = FastMath.abs(slideDist2 - slideDist3);
+		float dist6 = FastMath.abs(slideDist2 - slideDist4);
+		
+		// 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 offset between both feet at one instant and over time
-	protected float getOffsetErrorDeriv(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) {
-		float skeleton1Left = skeleton1.getNodePosition(TrackerPosition.LEFT_ANKLE).getY();
-		float skeleton1Right = skeleton1.getNodePosition(TrackerPosition.RIGHT_ANKLE).getY();
+	protected float getOffsetErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) {
+		float leftFoot1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY();
+		float rightFoot1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY();
 		
-		float skeleton2Left = skeleton2.getNodePosition(TrackerPosition.LEFT_ANKLE).getY();
-		float skeleton2Right = skeleton2.getNodePosition(TrackerPosition.RIGHT_ANKLE).getY();
+		float leftFoot2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY();
+		float rightFoot2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY();
 		
-		float dist1 = Math.abs(skeleton1Left - skeleton1Right);
-		float dist2 = Math.abs(skeleton2Left - skeleton2Right);
+		float dist1 = FastMath.abs(leftFoot1 - rightFoot1);
+		float dist2 = FastMath.abs(leftFoot2 - rightFoot2);
 		
-		float dist3 = Math.abs(skeleton1Left - skeleton2Right);
-		float dist4 = Math.abs(skeleton2Left - skeleton1Right);
+		float dist3 = FastMath.abs(leftFoot1 - rightFoot2);
+		float dist4 = FastMath.abs(leftFoot2 - rightFoot1);
 		
-		float dist5 = Math.abs(skeleton1Left - skeleton2Left);
-		float dist6 = Math.abs(skeleton1Right - skeleton2Right);
+		float dist5 = FastMath.abs(leftFoot1 - leftFoot2);
+		float dist6 = FastMath.abs(rightFoot1 - rightFoot2);
 		
 		// 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");
+	protected float getProportionErrorDeriv(SkeletonConfig skeleton) {
+		float neckLength = skeleton.getConfig(SkeletonConfigValue.NECK);
+		float chestLength = skeleton.getConfig(SkeletonConfigValue.CHEST);
+		float torsoLength = skeleton.getConfig(SkeletonConfigValue.TORSO);
+		float legsLength = skeleton.getConfig(SkeletonConfigValue.LEGS_LENGTH);
+		float kneeHeight = skeleton.getConfig(SkeletonConfigValue.KNEE_HEIGHT);
 		
-		float chestWaist = chestLength != null && waistLength != null ? Math.abs((chestLength / waistLength) - chestWaistRatio) : 0f;
-		float legBody = legsLength != null && waistLength != null && neckLength != null ? Math.abs((legsLength / (waistLength + neckLength)) - legBodyRatio) : 0f;
-		float kneeLeg = kneeHeight != null && legsLength != null ? Math.abs((kneeHeight / legsLength) - kneeLegRatio) : 0f;
+		float chestTorso = FastMath.abs((chestLength / torsoLength) - chestTorsoRatio);
+		float legBody = FastMath.abs((legsLength / (torsoLength + neckLength)) - legBodyRatio);
+		float kneeLeg = FastMath.abs((kneeHeight / legsLength) - kneeLegRatio);
 		
 		if(legBody <= legBodyRatioRange) {
 			legBody = 0f;
@@ -407,22 +487,26 @@ public class AutoBone {
 			legBody -= legBodyRatioRange;
 		}
 		
-		return (chestWaist + legBody + kneeLeg) / 3f;
+		return (chestTorso + legBody + kneeLeg) / 3f;
 	}
 	
 	// The distance of any points to the corresponding absolute position
-	protected float getPositionErrorDeriv(TrackerFrame[] frame, SimpleSkeleton skeleton) {
+	protected float getPositionErrorDeriv(PoseFrames frames, int cursor, PoseFrameSkeleton skeleton) {
 		float offset = 0f;
 		int offsetCount = 0;
 		
-		for(TrackerFrame trackerFrame : frame) {
+		List<PoseFrameTracker> trackers = frames.getTrackers();
+		for(int i = 0; i < trackers.size(); i++) {
+			PoseFrameTracker tracker = trackers.get(i);
+			
+			TrackerFrame trackerFrame = tracker.safeGetFrame(cursor);
 			if(trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) {
 				continue;
 			}
 			
-			Vector3f nodePos = skeleton.getNodePosition(trackerFrame.designation.designation);
+			Vector3f nodePos = skeleton.getComputedTracker(trackerFrame.designation.trackerRole).position;
 			if(nodePos != null) {
-				offset += Math.abs(nodePos.distance(trackerFrame.position));
+				offset += FastMath.abs(nodePos.distance(trackerFrame.position));
 				offsetCount++;
 			}
 		}
@@ -431,72 +515,81 @@ public class AutoBone {
 	}
 	
 	// The difference between offset of absolute position and the corresponding point over time
-	protected float getPositionOffsetErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) {
+	protected float getPositionOffsetErrorDeriv(PoseFrames frames, int cursor1, int cursor2, PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) {
 		float offset = 0f;
 		int offsetCount = 0;
 		
-		for(TrackerFrame trackerFrame1 : frame1) {
+		List<PoseFrameTracker> trackers = frames.getTrackers();
+		for(int i = 0; i < trackers.size(); i++) {
+			PoseFrameTracker tracker = trackers.get(i);
+			
+			TrackerFrame trackerFrame1 = tracker.safeGetFrame(cursor1);
 			if(trackerFrame1 == null || !trackerFrame1.hasData(TrackerFrameData.POSITION)) {
 				continue;
 			}
 			
-			TrackerFrame trackerFrame2 = TrackerUtils.findTrackerForBodyPosition(frame2, trackerFrame1.designation);
+			TrackerFrame trackerFrame2 = tracker.safeGetFrame(cursor2);
 			if(trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) {
 				continue;
 			}
 			
-			Vector3f nodePos1 = skeleton1.getNodePosition(trackerFrame1.designation);
+			Vector3f nodePos1 = skeleton1.getComputedTracker(trackerFrame1.designation.trackerRole).position;
 			if(nodePos1 == null) {
 				continue;
 			}
 			
-			Vector3f nodePos2 = skeleton2.getNodePosition(trackerFrame2.designation);
+			Vector3f nodePos2 = skeleton2.getComputedTracker(trackerFrame2.designation.trackerRole).position;
 			if(nodePos2 == null) {
 				continue;
 			}
 			
-			float dist1 = Math.abs(nodePos1.distance(trackerFrame1.position));
-			float dist2 = Math.abs(nodePos2.distance(trackerFrame2.position));
+			float dist1 = FastMath.abs(nodePos1.distance(trackerFrame1.position));
+			float dist2 = FastMath.abs(nodePos2.distance(trackerFrame2.position));
 			
-			offset += Math.abs(dist2 - dist1);
+			offset += FastMath.abs(dist2 - dist1);
 			offsetCount++;
 		}
 		
 		return offsetCount > 0 ? offset / offsetCount : 0f;
 	}
 	
-	protected float getErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, float heightChange) {
+	protected float getErrorDeriv(PoseFrames frames, int cursor1, int cursor2, PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2, float heightChange, float distScale) {
 		float totalError = 0f;
 		float sumWeight = 0f;
 		
 		if(slideErrorFactor > 0f) {
-			totalError += getSlideErrorDeriv(skeleton1, skeleton2) * slideErrorFactor;
+			totalError += getSlideErrorDeriv(skeleton1, skeleton2) * distScale * slideErrorFactor;
 			sumWeight += slideErrorFactor;
 		}
 		
+		if(offsetSlideErrorFactor > 0f) {
+			totalError += getOffsetSlideErrorDeriv(skeleton1, skeleton2) * distScale * offsetSlideErrorFactor;
+			sumWeight += offsetSlideErrorFactor;
+		}
+		
 		if(offsetErrorFactor > 0f) {
-			totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * offsetErrorFactor;
+			totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * distScale * offsetErrorFactor;
 			sumWeight += offsetErrorFactor;
 		}
 		
 		if(proportionErrorFactor > 0f) {
 			// Either skeleton will work fine, skeleton1 is used as a default
-			totalError += getProportionErrorDeriv(skeleton1) * proportionErrorFactor;
+			totalError += getProportionErrorDeriv(skeleton1.skeletonConfig) * proportionErrorFactor;
 			sumWeight += proportionErrorFactor;
 		}
 		
 		if(heightErrorFactor > 0f) {
-			totalError += Math.abs(heightChange) * heightErrorFactor;
+			totalError += FastMath.abs(heightChange) * heightErrorFactor;
 			sumWeight += heightErrorFactor;
 		}
 		
 		if(positionErrorFactor > 0f) {
-			totalError += (getPositionErrorDeriv(frame1, skeleton1) + getPositionErrorDeriv(frame2, skeleton2) / 2f) * positionErrorFactor;
+			totalError += (getPositionErrorDeriv(frames, cursor1, skeleton1) + getPositionErrorDeriv(frames, cursor2, skeleton2) / 2f) * distScale * positionErrorFactor;
 			sumWeight += positionErrorFactor;
 		}
 		
 		if(positionOffsetErrorFactor > 0f) {
-			totalError += getPositionOffsetErrorDeriv(frame1, frame2, skeleton1, skeleton2) * positionOffsetErrorFactor;
+			totalError += getPositionOffsetErrorDeriv(frames, cursor1, cursor2, skeleton1, skeleton2) * distScale * positionOffsetErrorFactor;
 			sumWeight += positionOffsetErrorFactor;
 		}
 		
@@ -509,8 +602,11 @@ public class AutoBone {
 		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);
+	protected void updateSkeletonBoneLength(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2, SkeletonConfigValue config, float newLength) {
+		skeleton1.skeletonConfig.setConfig(config, newLength);
+		skeleton1.updatePoseAffectedByConfig(config);
+		
+		skeleton2.skeletonConfig.setConfig(config, newLength);
+		skeleton2.updatePoseAffectedByConfig(config);
 	}
 }

src/main/java/dev/slimevr/autobone/SimpleSkeleton.java

@@ -1,352 +0,0 @@
-package dev.slimevr.autobone;
-
-import java.util.HashMap;
-import java.util.Map;
-
-import com.jme3.math.Quaternion;
-import com.jme3.math.Vector3f;
-
-import dev.slimevr.poserecorder.TrackerFrame;
-import dev.slimevr.poserecorder.TrackerFrameData;
-import io.eiren.vr.processor.HumanSkeletonWithLegs;
-import io.eiren.vr.processor.HumanSkeletonWithWaist;
-import io.eiren.vr.processor.TransformNode;
-import io.eiren.vr.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerUtils;
-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>();
-	
-	private Quaternion rotBuf1 = new Quaternion();
-	private Quaternion rotBuf2 = new Quaternion();
-	
-	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(Map<String, Float> configs, Map<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) {
-		this(configs, null);
-	}
-	
-	public void setPoseFromFrame(TrackerFrame[] frame) {
-		
-		TrackerFrame hmd = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.HMD);
-		
-		if(hmd != null) {
-			if(hmd.hasData(TrackerFrameData.ROTATION)) {
-				hmdNode.localTransform.setRotation(hmd.rotation);
-				headNode.localTransform.setRotation(hmd.rotation);
-			}
-			
-			if(hmd.hasData(TrackerFrameData.POSITION)) {
-				hmdNode.localTransform.setTranslation(hmd.position);
-			}
-		}
-		
-		TrackerFrame chest = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.CHEST, TrackerPosition.WAIST);
-		setRotation(chest, neckNode);
-		
-		TrackerFrame waist = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.WAIST, TrackerPosition.CHEST);
-		setRotation(waist, chestNode);
-		
-		TrackerFrame leftLeg = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.LEFT_LEG);
-		TrackerFrame rightLeg = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.RIGHT_LEG);
-		
-		averagePelvis(waist, leftLeg, rightLeg);
-		
-		setRotation(leftLeg, leftHipNode);
-		setRotation(rightLeg, rightHipNode);
-		
-		TrackerFrame leftAnkle = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.LEFT_ANKLE);
-		setRotation(leftAnkle, rightKneeNode);
-		
-		TrackerFrame rightAnkle = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.RIGHT_ANKLE);
-		setRotation(rightAnkle, leftKneeNode);
-		
-		updatePose();
-	}
-	
-	public void setRotation(TrackerFrame trackerFrame, TransformNode node) {
-		if(trackerFrame != null && trackerFrame.hasData(TrackerFrameData.ROTATION)) {
-			node.localTransform.setRotation(trackerFrame.rotation);
-		}
-	}
-	
-	public void averagePelvis(TrackerFrame waist, TrackerFrame leftLeg, TrackerFrame rightLeg) {
-		if((leftLeg == null || rightLeg == null) || (!leftLeg.hasData(TrackerFrameData.ROTATION) || !rightLeg.hasData(TrackerFrameData.ROTATION))) {
-			setRotation(waist, waistNode);
-			return;
-		}
-		
-		if(waist == null || !waist.hasData(TrackerFrameData.ROTATION)) {
-			if(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) {
-		configs.forEach(this::setSkeletonConfig);
-	}
-	
-	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 TransformNode getNode(String node) {
-		return nodes.get(node);
-	}
-	
-	public TransformNode getNode(TrackerPosition bodyPosition) {
-		return getNode(bodyPosition, false);
-	}
-	
-	public TransformNode getNode(TrackerPosition bodyPosition, boolean rotationNode) {
-		if(bodyPosition == null) {
-			return null;
-		}
-		
-		switch(bodyPosition) {
-		case HMD:
-			return hmdNode;
-		case CHEST:
-			return rotationNode ? neckNode : chestNode;
-		case WAIST:
-			return rotationNode ? chestNode : waistNode;
-		
-		case LEFT_LEG:
-			return rotationNode ? leftHipNode : leftKneeNode;
-		case RIGHT_LEG:
-			return rotationNode ? rightHipNode : rightKneeNode;
-		
-		case LEFT_ANKLE:
-			return rotationNode ? leftKneeNode : leftAnkleNode;
-		case RIGHT_ANKLE:
-			return rotationNode ? rightKneeNode : rightAnkleNode;
-		}
-		
-		return null;
-	}
-	
-	public Vector3f getNodePosition(String node) {
-		TransformNode transformNode = getNode(node);
-		return transformNode != null ? transformNode.worldTransform.getTranslation() : null;
-	}
-	
-	public Vector3f getNodePosition(TrackerPosition bodyPosition) {
-		TransformNode node = getNode(bodyPosition);
-		if(node == null) {
-			return null;
-		}
-		
-		return node.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/dev/slimevr/bridge/Bridge.java

@@ -1,7 +1,7 @@
 package dev.slimevr.bridge;
 
-import io.eiren.util.ann.VRServerThread;
-import io.eiren.vr.trackers.ShareableTracker;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.trackers.ShareableTracker;
 
 /**
  * Bridge handles sending and recieving tracker data

src/main/java/dev/slimevr/bridge/NamedPipeBridge.java

@@ -9,17 +9,17 @@ import com.sun.jna.platform.win32.WinBase;
 import com.sun.jna.platform.win32.WinError;
 import com.sun.jna.ptr.IntByReference;
 
+import dev.slimevr.Main;
 import dev.slimevr.bridge.Pipe.PipeState;
 import dev.slimevr.bridge.ProtobufMessages.ProtobufMessage;
 import dev.slimevr.bridge.ProtobufMessages.TrackerAdded;
-import io.eiren.util.ann.VRServerThread;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.VRTracker;
 import io.eiren.util.logging.LogManager;
-import io.eiren.vr.Main;
-import io.eiren.vr.trackers.HMDTracker;
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerRole;
-import io.eiren.vr.trackers.VRTracker;
 
 public class NamedPipeBridge extends ProtobufBridge<VRTracker> implements Runnable {
 	

src/main/java/dev/slimevr/bridge/NamedPipeVRBridge.java

@@ -12,15 +12,15 @@ import com.sun.jna.platform.win32.WinBase;
 import com.sun.jna.platform.win32.WinNT.HANDLE;
 import com.sun.jna.ptr.IntByReference;
 
+import dev.slimevr.VRServer;
 import dev.slimevr.bridge.Pipe.PipeState;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerStatus;
 import io.eiren.util.collections.FastList;
 import io.eiren.util.logging.LogManager;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.HMDTracker;
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerStatus;
 
 public class NamedPipeVRBridge extends Thread implements Bridge {
 

src/main/java/dev/slimevr/bridge/OpenVRNativeBridge.java

@@ -1,7 +1,7 @@
 package dev.slimevr.bridge;
 
-import io.eiren.util.ann.VRServerThread;
-import io.eiren.vr.trackers.ShareableTracker;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.trackers.ShareableTracker;
 
 public class OpenVRNativeBridge implements Bridge {
 	

src/main/java/dev/slimevr/bridge/ProtobufBridge.java

@@ -11,21 +11,21 @@ import java.util.concurrent.LinkedBlockingQueue;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.Main;
 import dev.slimevr.bridge.ProtobufMessages.Position;
 import dev.slimevr.bridge.ProtobufMessages.ProtobufMessage;
 import dev.slimevr.bridge.ProtobufMessages.TrackerAdded;
 import dev.slimevr.bridge.ProtobufMessages.TrackerStatus;
 import dev.slimevr.bridge.ProtobufMessages.UserAction;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.VRTracker;
 import io.eiren.util.ann.Synchronize;
 import io.eiren.util.ann.ThreadSafe;
-import io.eiren.util.ann.VRServerThread;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.Main;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.HMDTracker;
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.TrackerRole;
-import io.eiren.vr.trackers.VRTracker;
 
 public abstract class ProtobufBridge<T extends VRTracker> implements Bridge {
 
@@ -189,7 +189,7 @@ public abstract class ProtobufBridge<T extends VRTracker> implements Bridge {
 	protected void trackerStatusRecieved(TrackerStatus trackerStatus) {
 		T tracker = getInternalRemoteTrackerById(trackerStatus.getTrackerId());
 		if(tracker != null) {
-			tracker.setStatus(io.eiren.vr.trackers.TrackerStatus.getById(trackerStatus.getStatusValue()));
+			tracker.setStatus(dev.slimevr.vr.trackers.TrackerStatus.getById(trackerStatus.getStatusValue()));
 		}
 	}
     
@@ -214,11 +214,11 @@ public abstract class ProtobufBridge<T extends VRTracker> implements Bridge {
 		synchronized(remoteTrackersByTrackerId) {
 			Iterator<Entry<Integer, T>> iterator = remoteTrackersByTrackerId.entrySet().iterator();
 			while(iterator.hasNext()) {
-				iterator.next().getValue().setStatus(io.eiren.vr.trackers.TrackerStatus.DISCONNECTED);
+				iterator.next().getValue().setStatus(dev.slimevr.vr.trackers.TrackerStatus.DISCONNECTED);
 			}
 		}
 		if(hmdTracker != null) {
-			hmd.setStatus(io.eiren.vr.trackers.TrackerStatus.DISCONNECTED);
+			hmd.setStatus(dev.slimevr.vr.trackers.TrackerStatus.DISCONNECTED);
 		}
 	}
 

src/main/java/dev/slimevr/bridge/ProtobufMessages.java

@@ -2506,7 +2506,7 @@ public final class ProtobufMessages {
       }
       private com.google.protobuf.MapField<java.lang.String, java.lang.String>
       internalGetMutableActionArguments() {
-        onChanged();;
+        onChanged();
         if (actionArguments_ == null) {
           actionArguments_ = com.google.protobuf.MapField.newMapField(
               ActionArgumentsDefaultEntryHolder.defaultEntry);
@@ -4632,7 +4632,7 @@ public final class ProtobufMessages {
       }
       private com.google.protobuf.MapField<java.lang.String, java.lang.String>
       internalGetMutableExtra() {
-        onChanged();;
+        onChanged();
         if (extra_ == null) {
           extra_ = com.google.protobuf.MapField.newMapField(
               ExtraDefaultEntryHolder.defaultEntry);
@@ -5099,7 +5099,7 @@ public final class ProtobufMessages {
 	public int getNumber() {
         return this.value;
       }
-    };
+    }
 
     @Override
 	public MessageCase
@@ -5791,7 +5791,7 @@ public final class ProtobufMessages {
           message_ = null;
         }
         messageCase_ = 1;
-        onChanged();;
+        onChanged();
         return positionBuilder_;
       }
 
@@ -5932,7 +5932,7 @@ public final class ProtobufMessages {
           message_ = null;
         }
         messageCase_ = 2;
-        onChanged();;
+        onChanged();
         return userActionBuilder_;
       }
 
@@ -6073,7 +6073,7 @@ public final class ProtobufMessages {
           message_ = null;
         }
         messageCase_ = 3;
-        onChanged();;
+        onChanged();
         return trackerAddedBuilder_;
       }
 
@@ -6214,7 +6214,7 @@ public final class ProtobufMessages {
           message_ = null;
         }
         messageCase_ = 4;
-        onChanged();;
+        onChanged();
         return trackerStatusBuilder_;
       }
       @java.lang.Override

src/main/java/dev/slimevr/bridge/SteamVRPipeInputBridge.java

@@ -16,14 +16,14 @@ import com.sun.jna.platform.win32.WinBase;
 import com.sun.jna.platform.win32.WinError;
 import com.sun.jna.ptr.IntByReference;
 
+import dev.slimevr.VRServer;
 import dev.slimevr.bridge.Pipe.PipeState;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerStatus;
+import dev.slimevr.vr.trackers.VRTracker;
 import io.eiren.util.collections.FastList;
 import io.eiren.util.logging.LogManager;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.trackers.VRTracker;
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerStatus;
 
 public class SteamVRPipeInputBridge extends Thread implements Bridge {
 

src/main/java/dev/slimevr/bridge/VMCBridge.java

@@ -2,8 +2,8 @@ package dev.slimevr.bridge;
 
 import java.net.InetAddress;
 
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
 
 public class VMCBridge extends Thread implements Bridge {
 	

src/main/java/dev/slimevr/bridge/WebSocketVRBridge.java

@@ -17,15 +17,15 @@ import org.json.JSONObject;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.Main;
+import dev.slimevr.VRServer;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerStatus;
 import io.eiren.util.collections.FastList;
 import io.eiren.util.logging.LogManager;
-import io.eiren.vr.Main;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.HMDTracker;
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerStatus;
 
 public class WebSocketVRBridge extends WebSocketServer implements Bridge {
 	

src/main/java/dev/slimevr/gui/AbstractComponentListener.java

@@ -20,5 +20,4 @@ public abstract class AbstractComponentListener implements ComponentListener {
 	@Override
 	public void componentHidden(ComponentEvent e) {
 	}
-	
-}
+}

src/main/java/dev/slimevr/gui/AbstractWindowListener.java

@@ -32,4 +32,4 @@ public abstract class AbstractWindowListener implements WindowListener {
 	@Override
 	public void windowDeactivated(WindowEvent e) {
 	}
-}
+}

src/main/java/dev/slimevr/gui/AutoBoneWindow.java

@@ -10,24 +10,24 @@ 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.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;
 
+import dev.slimevr.VRServer;
 import dev.slimevr.autobone.AutoBone;
 import dev.slimevr.gui.swing.EJBox;
 import dev.slimevr.poserecorder.PoseFrames;
 import dev.slimevr.poserecorder.PoseFrameIO;
 import dev.slimevr.poserecorder.PoseRecorder;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
 
 public class AutoBoneWindow extends JFrame {
 	
@@ -37,7 +37,7 @@ public class AutoBoneWindow extends JFrame {
 	private EJBox pane;
 	
 	private final transient VRServer server;
-	private final transient SkeletonConfig skeletonConfig;
+	private final transient SkeletonConfigGUI skeletonConfig;
 	private final transient PoseRecorder poseRecorder;
 	private final transient AutoBone autoBone;
 	
@@ -52,7 +52,7 @@ public class AutoBoneWindow extends JFrame {
 	private JLabel processLabel;
 	private JLabel lengthsLabel;
 	
-	public AutoBoneWindow(VRServer server, SkeletonConfig skeletonConfig) {
+	public AutoBoneWindow(VRServer server, SkeletonConfigGUI skeletonConfig) {
 		super("Skeleton Auto-Configuration");
 		
 		this.server = server;
@@ -67,17 +67,14 @@ public class AutoBoneWindow extends JFrame {
 	}
 	
 	private String getLengthsString() {
-		boolean first = true;
-		StringBuilder configInfo = new StringBuilder("");
-		for(Entry<String, Float> entry : autoBone.configs.entrySet()) {
-			if(!first) {
+		final StringBuilder configInfo = new StringBuilder();
+		autoBone.configs.forEach((key, value) -> {
+			if(configInfo.length() > 0) {
 				configInfo.append(", ");
-			} else {
-				first = false;
 			}
 			
-			configInfo.append(entry.getKey() + ": " + StringUtils.prettyNumber(entry.getValue() * 100f, 2));
-		}
+			configInfo.append(key.stringVal + ": " + StringUtils.prettyNumber(value * 100f, 2));
+		});
 		
 		return configInfo.toString();
 	}
@@ -134,6 +131,7 @@ public class AutoBoneWindow extends JFrame {
 		autoBone.adjustRateDecay = server.config.getFloat("autobone.adjustRateDecay", autoBone.adjustRateDecay);
 		
 		autoBone.slideErrorFactor = server.config.getFloat("autobone.slideErrorFactor", autoBone.slideErrorFactor);
+		autoBone.offsetSlideErrorFactor = server.config.getFloat("autobone.offsetSlideErrorFactor", autoBone.offsetSlideErrorFactor);
 		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);
@@ -339,21 +337,21 @@ public class AutoBoneWindow extends JFrame {
 												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 neckLength = autoBone.getConfig(SkeletonConfigValue.NECK);
+												Float chestDistance = autoBone.getConfig(SkeletonConfigValue.CHEST);
+												Float torsoLength = autoBone.getConfig(SkeletonConfigValue.TORSO);
+												Float hipWidth = autoBone.getConfig(SkeletonConfigValue.HIPS_WIDTH);
+												Float legsLength = autoBone.getConfig(SkeletonConfigValue.LEGS_LENGTH);
+												Float kneeHeight = autoBone.getConfig(SkeletonConfigValue.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 neckTorso = neckLength != null && torsoLength != null ? neckLength / torsoLength : 0f;
+												float chestTorso = chestDistance != null && torsoLength != null ? chestDistance / torsoLength : 0f;
+												float torsoWaist = hipWidth != null && torsoLength != null ? hipWidth / torsoLength : 0f;
+												float legTorso = legsLength != null && torsoLength != null ? legsLength / torsoLength : 0f;
+												float legBody = legsLength != null && torsoLength != null && neckLength != null ? legsLength / (torsoLength + 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) + "}]");
+												LogManager.log.info("[AutoBone] Ratios: [{Neck-Torso: " + StringUtils.prettyNumber(neckTorso) + "}, {Chest-Torso: " + StringUtils.prettyNumber(chestTorso) + "}, {Torso-Waist: " + StringUtils.prettyNumber(torsoWaist) + "}, {Leg-Torso: " + StringUtils.prettyNumber(legTorso) + "}, {Leg-Body: " + StringUtils.prettyNumber(legBody) + "}, {Knee-Leg: " + StringUtils.prettyNumber(kneeLeg) + "}]");
 												
 												String lengthsString = getLengthsString();
 												LogManager.log.info("[AutoBone] Length values: " + lengthsString);

src/main/java/dev/slimevr/gui/CalibrationWindow.java

@@ -13,9 +13,9 @@ import javax.swing.border.EmptyBorder;
 import javax.swing.event.MouseInputAdapter;
 
 import dev.slimevr.gui.swing.EJBox;
+import dev.slimevr.vr.trackers.CalibratingTracker;
+import dev.slimevr.vr.trackers.Tracker;
 import io.eiren.util.ann.AWTThread;
-import io.eiren.vr.trackers.CalibratingTracker;
-import io.eiren.vr.trackers.Tracker;
 
 public class CalibrationWindow extends JFrame {
 	

src/main/java/dev/slimevr/gui/Keybinding.java

@@ -0,0 +1,62 @@
+package dev.slimevr.gui;
+
+import com.melloware.jintellitype.JIntellitype;
+import com.melloware.jintellitype.JIntellitypeException;
+
+import dev.slimevr.VRServer;
+
+import com.melloware.jintellitype.HotkeyListener;
+import io.eiren.util.ann.AWTThread;
+import io.eiren.util.logging.LogManager;
+
+public class Keybinding implements HotkeyListener {
+	public final VRServer server;
+	private static final int RESET = 1;
+	private static final int QUICK_RESET = 2;
+	
+	@AWTThread
+	public Keybinding(VRServer server) {
+		this.server = server;
+		
+		try {
+			if(JIntellitype.getInstance() instanceof JIntellitype) {
+				JIntellitype.getInstance().addHotKeyListener(this);
+				
+				String resetBinding = this.server.config.getString("keybindings.reset");
+				if(resetBinding == null) {
+					resetBinding = "CTRL+ALT+SHIFT+Y";
+					this.server.config.setProperty("keybindings.reset", resetBinding);
+				}
+				JIntellitype.getInstance().registerHotKey(RESET, resetBinding);
+				LogManager.log.info("[Keybinding] Bound reset to " + resetBinding);
+				
+				String quickResetBinding = this.server.config.getString("keybindings.quickReset");
+				if(quickResetBinding == null) {
+					quickResetBinding = "CTRL+ALT+SHIFT+U";
+					this.server.config.setProperty("keybindings.quickReset", quickResetBinding);
+				}
+				JIntellitype.getInstance().registerHotKey(QUICK_RESET, quickResetBinding);
+				LogManager.log.info("[Keybinding] Bound quick reset to " + quickResetBinding);
+			}
+		} catch(JIntellitypeException je) {
+			LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer.");
+		} catch(ExceptionInInitializerError e) {
+			LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer.");
+		}
+	}
+	
+	@AWTThread
+	@Override
+	public void onHotKey(int identifier) {
+		switch(identifier) {
+		case RESET:
+			LogManager.log.info("[Keybinding] Reset pressed");
+			server.resetTrackers();
+			break;
+		case QUICK_RESET:
+			LogManager.log.info("[Keybinding] Quick reset pressed");
+			server.resetTrackersYaw();
+			break;
+		}
+	}
+}

src/main/java/dev/slimevr/gui/ScalableFont.java

@@ -22,7 +22,7 @@ public class ScalableFont extends Font {
 		super(font);
 		
 		if(font instanceof ScalableFont) {
-			ScalableFont sourceFont = (ScalableFont)font;
+			ScalableFont sourceFont = (ScalableFont) font;
 			
 			this.initSize = sourceFont.getInitSize();
 			this.initPointSize = sourceFont.getInitSize2D();
@@ -39,7 +39,7 @@ public class ScalableFont extends Font {
 		super(font);
 		
 		if(font instanceof ScalableFont) {
-			ScalableFont sourceFont = (ScalableFont)font;
+			ScalableFont sourceFont = (ScalableFont) font;
 			
 			this.initSize = sourceFont.getInitSize();
 			this.initPointSize = sourceFont.getInitSize2D();
@@ -84,8 +84,7 @@ public class ScalableFont extends Font {
 		
 		float newPointSize = initPointSize * scale;
 		
-		this.size = (int)(newPointSize + 0.5);
+		this.size = (int) (newPointSize + 0.5);
 		this.pointSize = newPointSize;
 	}
-	
 }

src/main/java/dev/slimevr/gui/SkeletonConfigGUI.java

@@ -8,21 +8,22 @@ import javax.swing.JButton;
 import javax.swing.JLabel;
 import javax.swing.event.MouseInputAdapter;
 
+import dev.slimevr.VRServer;
 import dev.slimevr.gui.swing.ButtonTimer;
 import dev.slimevr.gui.swing.EJBagNoStretch;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.ThreadSafe;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.processor.HumanSkeleton;
 
-public class SkeletonConfig extends EJBagNoStretch {
+public class SkeletonConfigGUI extends EJBagNoStretch {
 
 	private final VRServer server;
 	private final VRServerGUI gui;
 	private final AutoBoneWindow autoBone;
-	private Map<String, SkeletonLabel> labels = new HashMap<>();
+	private Map<SkeletonConfigValue, SkeletonLabel> labels = new HashMap<>();
 
-	public SkeletonConfig(VRServer server, VRServerGUI gui) {
+	public SkeletonConfigGUI(VRServer server, VRServerGUI gui) {
 		super(false, true);
 		this.server = server;
 		this.gui = gui;
@@ -91,7 +92,7 @@ public class SkeletonConfig extends EJBagNoStretch {
 			row++;
 			//*/
 
-			add(new TimedResetButton("Reset All", "All"), s(c(1, row, 2), 3, 1));
+			add(new TimedResetButton("Reset All"), s(c(1, row, 2), 3, 1));
 			add(new JButton("Auto") {{
 				addMouseListener(new MouseInputAdapter() {
 					@Override
@@ -103,75 +104,25 @@ public class SkeletonConfig extends EJBagNoStretch {
 			}}, s(c(4, row, 2), 3, 1));
 			row++;
 
-			add(new JLabel("Chest"), c(0, row, 2));
-			add(new AdjButton("+", "Chest", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Chest"), c(2, row, 2));
-			add(new AdjButton("-", "Chest", -0.01f), c(3, row, 2));
-			add(new ResetButton("Reset", "Chest"), c(4, row, 2));
-			row++;
+			for (SkeletonConfigValue config : SkeletonConfigValue.values) {
+				add(new JLabel(config.label), c(0, row, 2));
+				add(new AdjButton("+", config, 0.01f), c(1, row, 2));
+				add(new SkeletonLabel(config), c(2, row, 2));
+				add(new AdjButton("-", config, -0.01f), c(3, row, 2));
 
-			add(new JLabel("Waist"), c(0, row, 2));
-			add(new AdjButton("+", "Waist", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Waist"), c(2, row, 2));
-			add(new AdjButton("-", "Waist", -0.01f), c(3, row, 2));
-			add(new TimedResetButton("Reset", "Waist"), c(4, row, 2));
-			row++;
+				// Only use a timer on configs that need time to get into position for
+				switch (config) {
+				case TORSO:
+				case LEGS_LENGTH:
+					add(new TimedResetButton("Reset", config), c(4, row, 2));
+					break;
+				default:
+					add(new ResetButton("Reset", config), c(4, row, 2));
+					break;
+				}
 
-			add(new JLabel("Hips width"), c(0, row, 2));
-			add(new AdjButton("+", "Hips width", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Hips width"), c(2, row, 2));
-			add(new AdjButton("-", "Hips width", -0.01f), c(3, row, 2));
-			add(new ResetButton("Reset", "Hips width"), c(4, row, 2));
-			row++;
-
-			add(new JLabel("Legs length"), c(0, row, 2));
-			add(new AdjButton("+", "Legs length", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Legs length"), c(2, row, 2));
-			add(new AdjButton("-", "Legs length", -0.01f), c(3, row, 2));
-			add(new TimedResetButton("Reset", "Legs length"), c(4, row, 2));
-			row++;
-
-			add(new JLabel("Knee height"), c(0, row, 2));
-			add(new AdjButton("+", "Knee height", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Knee height"), c(2, row, 2));
-			add(new AdjButton("-", "Knee height", -0.01f), c(3, row, 2));
-			add(new TimedResetButton("Reset", "Knee height"), c(4, row, 2));
-			row++;
-
-			add(new JLabel("Foot length"), c(0, row, 2));
-			add(new AdjButton("+", "Foot length", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Foot length"), c(2, row, 2));
-			add(new AdjButton("-", "Foot length", -0.01f), c(3, row, 2));
-			add(new ResetButton("Reset", "Foot length"), c(4, row, 2));
-			row++;
-
-			add(new JLabel("Foot offset"), c(0, row, 2));
-			add(new AdjButton("+", "Foot offset", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Foot offset"), c(2, row, 2));
-			add(new AdjButton("-", "Foot offset", -0.01f), c(3, row, 2));
-			add(new ResetButton("Reset", "Foot offset"), c(4, row, 2));
-			row++;
-
-			add(new JLabel("Head offset"), c(0, row, 2));
-			add(new AdjButton("+", "Head", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Head"), c(2, row, 2));
-			add(new AdjButton("-", "Head", -0.01f), c(3, row, 2));
-			add(new ResetButton("Reset", "Head"), c(4, row, 2));
-			row++;
-
-			add(new JLabel("Neck length"), c(0, row, 2));
-			add(new AdjButton("+", "Neck", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Neck"), c(2, row, 2));
-			add(new AdjButton("-", "Neck", -0.01f), c(3, row, 2));
-			add(new ResetButton("Reset", "Neck"), c(4, row, 2));
-			row++;
-
-			add(new JLabel("Virtual waist"), c(0, row, 2));
-			add(new AdjButton("+", "Virtual waist", 0.01f), c(1, row, 2));
-			add(new SkeletonLabel("Virtual waist"), c(2, row, 2));
-			add(new AdjButton("-", "Virtual waist", -0.01f), c(3, row, 2));
-			add(new ResetButton("Reset", "Virtual waist"), c(4, row, 2));
-			row++;
+				row++;
+			}
 
 			gui.refresh();
 		});
@@ -186,30 +137,41 @@ public class SkeletonConfig extends EJBagNoStretch {
 		});
 	}
 
-	private void change(String joint, float diff) {
+	private void change(SkeletonConfigValue joint, float diff) {
+		// Update config value
 		float current = server.humanPoseProcessor.getSkeletonConfig(joint);
 		server.humanPoseProcessor.setSkeletonConfig(joint, current + diff);
+		server.humanPoseProcessor.getSkeletonConfig().saveToConfig(server.config);
 		server.saveConfig();
+
+		// Update GUI
 		labels.get(joint).setText(StringUtils.prettyNumber((current + diff) * 100, 0));
 	}
 
-	private void reset(String joint) {
+	private void reset(SkeletonConfigValue joint) {
+		// Update config value
 		server.humanPoseProcessor.resetSkeletonConfig(joint);
+		server.humanPoseProcessor.getSkeletonConfig().saveToConfig(server.config);
 		server.saveConfig();
-		if(!"All".equals(joint)) {
-			float current = server.humanPoseProcessor.getSkeletonConfig(joint);
-			labels.get(joint).setText(StringUtils.prettyNumber((current) * 100, 0));
-		} else {
-			labels.forEach((jnt, label) -> {
-				float current = server.humanPoseProcessor.getSkeletonConfig(jnt);
-				label.setText(StringUtils.prettyNumber((current) * 100, 0));
-			});
-		}
+
+		// Update GUI
+		float current = server.humanPoseProcessor.getSkeletonConfig(joint);
+		labels.get(joint).setText(StringUtils.prettyNumber((current) * 100, 0));
+	}
+
+	private void resetAll() {
+		// Update config value
+		server.humanPoseProcessor.resetAllSkeletonConfigs();
+		server.humanPoseProcessor.getSkeletonConfig().saveToConfig(server.config);
+		server.saveConfig();
+
+		// Update GUI
+		refreshAll();
 	}
 
 	private class SkeletonLabel extends JLabel {
 
-		public SkeletonLabel(String joint) {
+		public SkeletonLabel(SkeletonConfigValue joint) {
 			super(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint) * 100, 0));
 			labels.put(joint, this);
 		}
@@ -217,7 +179,7 @@ public class SkeletonConfig extends EJBagNoStretch {
 
 	private class AdjButton extends JButton {
 
-		public AdjButton(String text, String joint, float diff) {
+		public AdjButton(String text, SkeletonConfigValue joint, float diff) {
 			super(text);
 			addMouseListener(new MouseInputAdapter() {
 				@Override
@@ -230,7 +192,7 @@ public class SkeletonConfig extends EJBagNoStretch {
 
 	private class ResetButton extends JButton {
 
-		public ResetButton(String text, String joint) {
+		public ResetButton(String text, SkeletonConfigValue joint) {
 			super(text);
 			addMouseListener(new MouseInputAdapter() {
 				@Override
@@ -243,7 +205,7 @@ public class SkeletonConfig extends EJBagNoStretch {
 
 	private class TimedResetButton extends JButton {
 
-		public TimedResetButton(String text, String joint) {
+		public TimedResetButton(String text, SkeletonConfigValue joint) {
 			super(text);
 			addMouseListener(new MouseInputAdapter() {
 				@Override
@@ -252,5 +214,15 @@ public class SkeletonConfig extends EJBagNoStretch {
 				}
 			});
 		}
+
+		public TimedResetButton(String text) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					ButtonTimer.runTimer(TimedResetButton.this, 3, text, () -> resetAll());
+				}
+			});
+		}
 	}
 }

src/main/java/dev/slimevr/gui/SkeletonList.java

@@ -9,14 +9,14 @@ import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.VRServer;
 import dev.slimevr.gui.swing.EJBagNoStretch;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.TransformNode;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.ThreadSafe;
-import io.eiren.util.ann.VRServerThread;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.processor.HumanSkeleton;
-import io.eiren.vr.processor.TransformNode;
 
 public class SkeletonList extends EJBagNoStretch {
 	

src/main/java/dev/slimevr/gui/TrackersList.java

@@ -17,23 +17,23 @@ import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.VRServer;
 import dev.slimevr.gui.swing.EJBagNoStretch;
 import dev.slimevr.gui.swing.EJBoxNoStretch;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.IMUTracker;
+import dev.slimevr.vr.trackers.ReferenceAdjustedTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerConfig;
+import dev.slimevr.vr.trackers.TrackerMountingRotation;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerWithBattery;
+import dev.slimevr.vr.trackers.TrackerWithTPS;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.AWTThread;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.trackers.ReferenceAdjustedTracker;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.HMDTracker;
-import io.eiren.vr.trackers.IMUTracker;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerConfig;
-import io.eiren.vr.trackers.TrackerMountingRotation;
-import io.eiren.vr.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerWithBattery;
-import io.eiren.vr.trackers.TrackerWithTPS;
 
 public class TrackersList extends EJBoxNoStretch {
 	

src/main/java/dev/slimevr/gui/VRServerGUI.java

@@ -5,18 +5,18 @@ import javax.swing.*;
 import javax.swing.border.EmptyBorder;
 import javax.swing.event.MouseInputAdapter;
 
+import dev.slimevr.Main;
+import dev.slimevr.VRServer;
 import dev.slimevr.bridge.NamedPipeBridge;
 import dev.slimevr.gui.swing.ButtonTimer;
 import dev.slimevr.gui.swing.EJBagNoStretch;
 import dev.slimevr.gui.swing.EJBox;
 import dev.slimevr.gui.swing.EJBoxNoStretch;
+import dev.slimevr.vr.trackers.TrackerRole;
 import io.eiren.util.MacOSX;
 import io.eiren.util.OperatingSystem;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.AWTThread;
-import io.eiren.vr.Main;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.trackers.TrackerRole;
 
 import java.awt.Component;
 import java.awt.Container;
@@ -202,7 +202,7 @@ public class VRServerGUI extends JFrame {
 				add(l = new JLabel("Body proportions"));
 				l.setFont(l.getFont().deriveFont(Font.BOLD));
 				l.setAlignmentX(0.5f);
-				add(new SkeletonConfig(server, VRServerGUI.this));
+				add(new SkeletonConfigGUI(server, VRServerGUI.this));
 				add(Box.createVerticalStrut(10));
 				if(server.hasBridge(NamedPipeBridge.class)) {
 					NamedPipeBridge br = server.getVRBridge(NamedPipeBridge.class);

src/main/java/dev/slimevr/hardware/magentometer/Magneto.java

@@ -1,4 +1,4 @@
-package io.eiren.hardware.magentometer;
+package dev.slimevr.hardware.magentometer;
 
 import com.sun.jna.Library;
 import com.sun.jna.Native;

src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java

@@ -11,9 +11,9 @@ import java.io.FileOutputStream;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.vr.trackers.TrackerPosition;
 import io.eiren.util.collections.FastList;
 import io.eiren.util.logging.LogManager;
-import io.eiren.vr.trackers.TrackerPosition;
 
 public final class PoseFrameIO {
 	
@@ -26,7 +26,6 @@ public final class PoseFrameIO {
 			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++) {

src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java

@@ -0,0 +1,72 @@
+package dev.slimevr.poserecorder;
+
+import java.util.List;
+import java.util.Map;
+
+import dev.slimevr.VRServer;
+import dev.slimevr.vr.processor.ComputedHumanPoseTracker;
+import dev.slimevr.vr.processor.skeleton.SimpleSkeleton;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
+import dev.slimevr.vr.trackers.Tracker;
+
+public class PoseFrameSkeleton extends SimpleSkeleton {
+	
+	private int frameCursor = 0;
+	
+	protected PoseFrameSkeleton(List<? extends ComputedHumanPoseTracker> computedTrackers) {
+		super(computedTrackers);
+	}
+	
+	public PoseFrameSkeleton(VRServer server, List<? extends ComputedHumanPoseTracker> computedTrackers) {
+		super(server, computedTrackers);
+	}
+	
+	public PoseFrameSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers) {
+		super(trackers, computedTrackers);
+	}
+	
+	public PoseFrameSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers, Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigValue, Float> altConfigs) {
+		super(trackers, computedTrackers, configs, altConfigs);
+	}
+	
+	public PoseFrameSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers, Map<SkeletonConfigValue, Float> configs) {
+		super(trackers, computedTrackers, configs);
+	}
+	
+	private int limitCursor() {
+		if(frameCursor < 0) {
+			frameCursor = 0;
+		}
+		
+		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;
+	}
+	
+	// Get tracker for specific frame
+	@Override
+	protected Tracker trackerPreUpdate(Tracker tracker) {
+		if(tracker instanceof PoseFrameTracker) {
+			// Return frame if available, otherwise return the original tracker
+			TrackerFrame frame = ((PoseFrameTracker) tracker).safeGetFrame(frameCursor);
+			return frame == null ? tracker : frame;
+		}
+		return tracker;
+	}
+}

src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java

@@ -5,11 +5,11 @@ import java.util.Iterator;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerConfig;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerStatus;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerConfig;
-import io.eiren.vr.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerStatus;
 
 public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
 	
@@ -137,7 +137,7 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
 			return true;
 		}
 		
-		store.set(0, 0, 0, 1);
+		store.set(Quaternion.IDENTITY);
 		return false;
 	}
 	
@@ -149,7 +149,7 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
 			return true;
 		}
 		
-		store.set(0, 0, 0);
+		store.set(Vector3f.ZERO);
 		return false;
 	}
 	
@@ -175,7 +175,7 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
 	
 	@Override
 	public float getConfidenceLevel() {
-		return 0;
+		return 1f;
 	}
 	
 	@Override
@@ -231,7 +231,7 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
 	public Iterator<TrackerFrame> iterator() {
 		return frames.iterator();
 	}
-
+	
 	@Override
 	public int getTrackerId() {
 		return this.trackerId;

src/main/java/dev/slimevr/poserecorder/PoseFrames.java

@@ -4,8 +4,8 @@ import java.util.Iterator;
 import java.util.List;
 import java.util.NoSuchElementException;
 
+import dev.slimevr.vr.trackers.Tracker;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.trackers.Tracker;
 
 public final class PoseFrames implements Iterable<TrackerFrame[]> {
 	

src/main/java/dev/slimevr/poserecorder/PoseRecorder.java

@@ -7,82 +7,82 @@ import java.util.concurrent.Future;
 
 import org.apache.commons.lang3.tuple.Pair;
 
-import io.eiren.util.ann.VRServerThread;
+import dev.slimevr.VRServer;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.trackers.Tracker;
 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 PoseFrames poseFrame = null;
-
+	
 	protected int numFrames = -1;
 	protected int frameCursor = 0;
 	protected long frameRecordingInterval = 60L;
 	protected long nextFrameTimeMs = -1L;
-
+	
 	protected CompletableFuture<PoseFrames> 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) {
+		if(numFrames <= 0) {
 			return;
 		}
-
+		
 		PoseFrames poseFrame = this.poseFrame;
 		List<Pair<Tracker, PoseFrameTracker>> trackers = this.trackers;
-		if (poseFrame == null || trackers == null) {
+		if(poseFrame == null || trackers == null) {
 			return;
 		}
-
-		if (frameCursor >= numFrames) {
+		
+		if(frameCursor >= numFrames) {
 			// If done and hasn't yet, send finished recording
 			stopFrameRecording();
 			return;
 		}
 		
 		long curTime = System.currentTimeMillis();
-		if (curTime < nextFrameTimeMs) {
+		if(curTime < nextFrameTimeMs) {
 			return;
 		}
-
+		
 		nextFrameTimeMs += frameRecordingInterval;
-
+		
 		// To prevent duplicate frames, make sure the frame time is always in the future
-		if (nextFrameTimeMs <= curTime) {
+		if(nextFrameTimeMs <= curTime) {
 			nextFrameTimeMs = curTime + frameRecordingInterval;
 		}
-
+		
 		// Make sure it's synchronized since this is the server thread interacting with
 		// an unknown outside thread controlling this class
-		synchronized (this) {
+		synchronized(this) {
 			// A stopped recording will be accounted for by an empty "trackers" list
 			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) {
 				stopFrameRecording();
 			}
 		}
 	}
-
+	
 	public synchronized Future<PoseFrames> startFrameRecording(int numFrames, long intervalMs) {
 		return startFrameRecording(numFrames, intervalMs, server.getAllTrackers());
 	}
-
+	
 	public synchronized Future<PoseFrames> startFrameRecording(int numFrames, long intervalMs, List<Tracker> trackers) {
 		if(numFrames < 1) {
 			throw new IllegalArgumentException("numFrames must at least have a value of 1");
@@ -99,11 +99,11 @@ public class PoseRecorder {
 		if(!isReadyToRecord()) {
 			throw new IllegalStateException("PoseRecorder isn't ready to record!");
 		}
-
+		
 		cancelFrameRecording();
-
+		
 		poseFrame = new PoseFrames(trackers.size());
-
+		
 		// Update tracker list
 		this.trackers.ensureCapacity(trackers.size());
 		for(Tracker tracker : trackers) {
@@ -111,65 +111,65 @@ public class PoseRecorder {
 			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 = intervalMs;
 		nextFrameTimeMs = -1L;
-
+		
 		LogManager.log.info("[PoseRecorder] Recording " + numFrames + " samples at a " + intervalMs + " ms frame interval");
-
+		
 		currentRecording = new CompletableFuture<PoseFrames>();
 		return currentRecording;
 	}
-
+	
 	public synchronized void stopFrameRecording() {
 		CompletableFuture<PoseFrames> 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 cancelFrameRecording() {
 		CompletableFuture<PoseFrames> 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 synchronized boolean isReadyToRecord() {
 		return server.getTrackersCount() > 0;
 	}
-
+	
 	public synchronized boolean isRecording() {
 		return numFrames > frameCursor;
 	}
-
+	
 	public synchronized boolean hasRecording() {
 		return currentRecording != null;
 	}
-
+	
 	public synchronized Future<PoseFrames> getFramesAsync() {
 		return currentRecording;
 	}
-
+	
 	public synchronized PoseFrames getFrames() throws ExecutionException, InterruptedException {
 		CompletableFuture<PoseFrames> currentRecording = this.currentRecording;
 		return currentRecording != null ? currentRecording.get() : null;

src/main/java/dev/slimevr/poserecorder/TrackerFrame.java

@@ -3,10 +3,10 @@ package dev.slimevr.poserecorder;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerConfig;
-import io.eiren.vr.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerStatus;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerConfig;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerStatus;
 
 public final class TrackerFrame implements Tracker {
 	
@@ -86,7 +86,7 @@ public final class TrackerFrame implements Tracker {
 			return true;
 		}
 		
-		store.set(0, 0, 0, 1);
+		store.set(Quaternion.IDENTITY);
 		return false;
 	}
 	
@@ -97,7 +97,7 @@ public final class TrackerFrame implements Tracker {
 			return true;
 		}
 		
-		store.set(0, 0, 0);
+		store.set(Vector3f.ZERO);
 		return false;
 	}
 	
@@ -123,7 +123,7 @@ public final class TrackerFrame implements Tracker {
 	
 	@Override
 	public float getConfidenceLevel() {
-		return 0;
+		return 1f;
 	}
 	
 	@Override
@@ -171,7 +171,7 @@ public final class TrackerFrame implements Tracker {
 		return true;
 	}
 	//#endregion
-
+	
 	@Override
 	public int getTrackerId() {
 		return this.trackerId;

src/main/java/dev/slimevr/posestreamer/BVHFileStream.java

@@ -8,166 +8,257 @@ import java.io.IOException;
 import java.io.OutputStream;
 import java.io.OutputStreamWriter;
 
+import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
+import com.jme3.math.Transform;
 import com.jme3.math.Vector3f;
 
 import org.apache.commons.lang3.StringUtils;
 
-import io.eiren.vr.processor.HumanSkeleton;
-import io.eiren.vr.processor.TransformNode;
+import dev.slimevr.vr.processor.TransformNode;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
 
 public class BVHFileStream extends PoseDataStream {
-
+	
 	private static final int LONG_MAX_VALUE_DIGITS = Long.toString(Long.MAX_VALUE).length();
-	private static final float POS_SCALE = 10f;
-
+	private static final float OFFSET_SCALE = 100f;
+	private static final float POSITION_SCALE = 100f;
+	
 	private long frameCount = 0;
 	private final BufferedWriter writer;
-
+	
 	private long frameCountOffset;
-
+	
 	private float[] angleBuf = new float[3];
 	private Quaternion rotBuf = new Quaternion();
-
+	
+	private HumanSkeleton wrappedSkeleton;
+	private TransformNodeWrapper rootNode;
+	
 	public BVHFileStream(OutputStream outputStream) {
 		super(outputStream);
 		writer = new BufferedWriter(new OutputStreamWriter(outputStream), 4096);
 	}
-
+	
 	public BVHFileStream(File file) throws FileNotFoundException {
 		super(file);
 		writer = new BufferedWriter(new OutputStreamWriter(outputStream), 4096);
 	}
-
+	
 	public BVHFileStream(String file) throws FileNotFoundException {
 		super(file);
 		writer = new BufferedWriter(new OutputStreamWriter(outputStream), 4096);
 	}
-
+	
 	private String getBufferedFrameCount(long frameCount) {
 		String frameString = Long.toString(frameCount);
 		int bufferCount = LONG_MAX_VALUE_DIGITS - frameString.length();
-
+		
 		return bufferCount > 0 ? frameString + StringUtils.repeat(' ', bufferCount) : frameString;
 	}
-
-	private void writeTransformNodeHierarchy(TransformNode node) throws IOException {
-		writeTransformNodeHierarchy(node, 0);
+	
+	private TransformNodeWrapper wrapSkeletonIfNew(HumanSkeleton skeleton) {
+		TransformNodeWrapper wrapper = rootNode;
+		
+		// If the wrapped skeleton is missing or the skeleton is updated
+		if(wrapper == null || skeleton != wrappedSkeleton) {
+			wrapper = wrapSkeleton(skeleton);
+		}
+		
+		return wrapper;
 	}
-
-	private void writeTransformNodeHierarchy(TransformNode node, int level) throws IOException {
+	
+	private TransformNodeWrapper wrapSkeleton(HumanSkeleton skeleton) {
+		TransformNodeWrapper wrapper = wrapSkeletonNodes(skeleton.getRootNode());
+		
+		wrappedSkeleton = skeleton;
+		rootNode = wrapper;
+		
+		return wrapper;
+	}
+	
+	protected TransformNodeWrapper wrapSkeletonNodes(TransformNode rootNode) {
+		return TransformNodeWrapper.wrapFullHierarchy(rootNode);
+	}
+	
+	private void writeNodeHierarchy(TransformNodeWrapper node) throws IOException {
+		writeNodeHierarchy(node, 0);
+	}
+	
+	private void writeNodeHierarchy(TransformNodeWrapper node, int level) throws IOException {
+		// Don't write end sites at populated nodes
+		if(node.children.isEmpty() && node.getParent().children.size() > 1) {
+			return;
+		}
+		
 		String indentLevel = StringUtils.repeat("\t", level);
 		String nextIndentLevel = indentLevel + "\t";
-
+		
 		// Handle ends
-		if (node.children.isEmpty()) {
+		if(node.children.isEmpty()) {
 			writer.write(indentLevel + "End Site\n");
 		} else {
 			writer.write((level > 0 ? indentLevel + "JOINT " : "ROOT ") + node.getName() + "\n");
 		}
 		writer.write(indentLevel + "{\n");
-
-		if (level > 0) {
+		
+		// Ignore the root offset and original root offset
+		if(level > 0 && node.wrappedNode.getParent() != null) {
 			Vector3f offset = node.localTransform.getTranslation();
-			writer.write(nextIndentLevel + "OFFSET " + Float.toString(offset.getX() * POS_SCALE) + " " + Float.toString(offset.getY() * POS_SCALE) + " " + Float.toString(offset.getZ() * POS_SCALE) + "\n");
+			float reverseMultiplier = node.hasReversedHierarchy() ? -1 : 1;
+			writer.write(nextIndentLevel + "OFFSET " + Float.toString(offset.getX() * OFFSET_SCALE * reverseMultiplier) + " " + Float.toString(offset.getY() * OFFSET_SCALE * reverseMultiplier) + " " + Float.toString(offset.getZ() * OFFSET_SCALE * reverseMultiplier) + "\n");
 		} else {
 			writer.write(nextIndentLevel + "OFFSET 0.0 0.0 0.0\n");
 		}
-
+		
 		// Handle ends
-		if (!node.children.isEmpty()) {
+		if(!node.children.isEmpty()) {
 			// Only give position for root
-			if (level > 0) {
+			if(level > 0) {
 				writer.write(nextIndentLevel + "CHANNELS 3 Zrotation Xrotation Yrotation\n");
 			} else {
 				writer.write(nextIndentLevel + "CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation\n");
 			}
-
-			for (TransformNode childNode : node.children) {
-				writeTransformNodeHierarchy(childNode, level + 1);
+			
+			for(TransformNodeWrapper childNode : node.children) {
+				writeNodeHierarchy(childNode, level + 1);
 			}
 		}
-
+		
 		writer.write(indentLevel + "}\n");
 	}
-
+	
 	@Override
 	public void writeHeader(HumanSkeleton skeleton, PoseStreamer streamer) throws IOException {
-		if (skeleton == null) {
+		if(skeleton == null) {
 			throw new NullPointerException("skeleton must not be null");
 		}
-		if (streamer == null) {
+		if(streamer == null) {
 			throw new NullPointerException("streamer must not be null");
 		}
-
+		
 		writer.write("HIERARCHY\n");
-		writeTransformNodeHierarchy(skeleton.getRootNode());
-
+		writeNodeHierarchy(wrapSkeletonIfNew(skeleton));
+		
 		writer.write("MOTION\n");
 		writer.write("Frames: ");
-
+		
 		// Get frame offset for finishing writing the file
-		if (outputStream instanceof FileOutputStream) {
-			FileOutputStream fileOutputStream = (FileOutputStream)outputStream;
+		if(outputStream instanceof FileOutputStream) {
+			FileOutputStream fileOutputStream = (FileOutputStream) outputStream;
 			// Flush buffer to get proper offset
 			writer.flush();
 			frameCountOffset = fileOutputStream.getChannel().position();
 		}
-
+		
 		writer.write(getBufferedFrameCount(frameCount) + "\n");
-
+		
 		// Frame time in seconds
 		writer.write("Frame Time: " + (streamer.frameRecordingInterval / 1000d) + "\n");
 	}
-
-	private void writeTransformHierarchyRotation(TransformNode node, Quaternion inverseRootRot) throws IOException {
-		rotBuf = node.localTransform.getRotation(rotBuf);
-
-		// Adjust to local rotation
-		if (inverseRootRot != null) {
-			rotBuf = inverseRootRot.mult(rotBuf, rotBuf);
+	
+	// Roughly based off code from https://github.com/TrackLab/ViRe/blob/50a987eff4db31036b2ebaeb5a28983cd473f267/Assets/Scripts/BVH/BVHRecorder.cs
+	private float[] quatToXyzAngles(Quaternion q, float[] angles) {
+		if(angles == null) {
+			angles = new float[3];
+		} else if(angles.length != 3) {
+			throw new IllegalArgumentException("Angles array must have three elements");
 		}
-
-		angleBuf = rotBuf.toAngles(angleBuf);
-		writer.write(Float.toString((float)Math.toDegrees(angleBuf[2])) + " " + Float.toString((float)Math.toDegrees(angleBuf[0])) + " " + Float.toString((float)Math.toDegrees(angleBuf[1])));
-
-		// Get inverse rotation for child local rotations
-		Quaternion inverseRot = node.localTransform.getRotation().inverse();
-		for (TransformNode childNode : node.children) {
-			if (childNode.children.isEmpty()) {
-				// If it's an end node, skip
-				continue;
+		
+		float x = q.getX();
+		float y = q.getY();
+		float z = q.getZ();
+		float w = q.getW();
+		
+		// Roll (X)
+		float sinrCosp = -2f * (x * y - w * z);
+		float cosrCosp = w * w - x * x + y * y - z * z;
+		angles[0] = FastMath.atan2(sinrCosp, cosrCosp);
+		
+		// Pitch (Y)
+		float sinp = 2f * (y * z + w * x);
+		// Use 90 degrees if out of range
+		angles[1] = FastMath.abs(sinp) >= 1f ? FastMath.copysign(FastMath.PI / 2f, sinp) : FastMath.asin(sinp);
+		
+		// Yaw (Z)
+		float sinyCosp = -2f * (x * z - w * y);
+		float cosyCosp = w * w - x * x - y * y + z * z;
+		angles[2] = FastMath.atan2(sinyCosp, cosyCosp);
+		
+		return angles;
+	}
+	
+	private void writeNodeHierarchyRotation(TransformNodeWrapper node, Quaternion inverseRootRot) throws IOException {
+		Transform transform = node.worldTransform;
+		
+		/*
+		if (node.hasReversedHierarchy()) {
+			for (TransformNodeWrapper childNode : node.children) {
+				// If the hierarchy is fully reversed, set the rotation for the upper bone
+				if (childNode.hasReversedHierarchy()) {
+					transform = childNode.worldTransform;
+					break;
+				}
+			}
+		}
+		*/
+		
+		rotBuf = transform.getRotation(rotBuf);
+		
+		// Adjust to local rotation
+		if(inverseRootRot != null) {
+			rotBuf = rotBuf.multLocal(inverseRootRot);
+		}
+		
+		// Yaw (Z), roll (X), pitch (Y) (intrinsic)
+		// angleBuf = rotBuf.toAngles(angleBuf);
+		
+		// Roll (X), pitch (Y), yaw (Z) (intrinsic)
+		angleBuf = quatToXyzAngles(rotBuf.normalizeLocal(), angleBuf);
+		
+		// Output in order of roll (Z), pitch (X), yaw (Y) (extrinsic)
+		writer.write(Float.toString(angleBuf[0] * FastMath.RAD_TO_DEG) + " " + Float.toString(angleBuf[1] * FastMath.RAD_TO_DEG) + " " + Float.toString(angleBuf[2] * FastMath.RAD_TO_DEG));
+		
+		// Get inverse rotation for child local rotations
+		if(!node.children.isEmpty()) {
+			Quaternion inverseRot = transform.getRotation().inverse();
+			for(TransformNodeWrapper childNode : node.children) {
+				if(childNode.children.isEmpty()) {
+					// If it's an end node, skip
+					continue;
+				}
+				
+				// Add spacing
+				writer.write(" ");
+				writeNodeHierarchyRotation(childNode, inverseRot);
 			}
-
-			// Add spacing
-			writer.write(" ");
-			writeTransformHierarchyRotation(childNode, inverseRot);
 		}
 	}
-
+	
 	@Override
 	public void writeFrame(HumanSkeleton skeleton) throws IOException {
-		if (skeleton == null) {
+		if(skeleton == null) {
 			throw new NullPointerException("skeleton must not be null");
 		}
-
-		TransformNode root = skeleton.getRootNode();
-		Vector3f rootPos = root.localTransform.getTranslation();
-
+		
+		TransformNodeWrapper rootNode = wrapSkeletonIfNew(skeleton);
+		
+		Vector3f rootPos = rootNode.worldTransform.getTranslation();
+		
 		// Write root position
-		writer.write(Float.toString(rootPos.getX() * POS_SCALE) + " " + Float.toString(rootPos.getY() * POS_SCALE) + " " + Float.toString(rootPos.getZ() * POS_SCALE) + " ");
-		writeTransformHierarchyRotation(root, null);
-
+		writer.write(Float.toString(rootPos.getX() * POSITION_SCALE) + " " + Float.toString(rootPos.getY() * POSITION_SCALE) + " " + Float.toString(rootPos.getZ() * POSITION_SCALE) + " ");
+		writeNodeHierarchyRotation(rootNode, null);
+		
 		writer.newLine();
-
+		
 		frameCount++;
 	}
-
+	
 	@Override
 	public void writeFooter(HumanSkeleton skeleton) throws IOException {
 		// Write the final frame count for files
-		if (outputStream instanceof FileOutputStream) {
-			FileOutputStream fileOutputStream = (FileOutputStream)outputStream;
+		if(outputStream instanceof FileOutputStream) {
+			FileOutputStream fileOutputStream = (FileOutputStream) outputStream;
 			// Flush before anything else
 			writer.flush();
 			// Seek to the count offset
@@ -176,7 +267,7 @@ public class BVHFileStream extends PoseDataStream {
 			writer.write(Long.toString(frameCount));
 		}
 	}
-
+	
 	@Override
 	public void close() throws IOException {
 		writer.close();

src/main/java/dev/slimevr/posestreamer/PoseDataStream.java

@@ -6,37 +6,37 @@ import java.io.FileOutputStream;
 import java.io.IOException;
 import java.io.OutputStream;
 
-import io.eiren.vr.processor.HumanSkeleton;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
 
 public abstract class PoseDataStream implements AutoCloseable {
-
+	
 	protected boolean closed = false;
 	protected final OutputStream outputStream;
-
+	
 	protected PoseDataStream(OutputStream outputStream) {
 		this.outputStream = outputStream;
 	}
-
+	
 	protected PoseDataStream(File file) throws FileNotFoundException {
 		this(new FileOutputStream(file));
 	}
-
+	
 	protected PoseDataStream(String file) throws FileNotFoundException {
 		this(new FileOutputStream(file));
 	}
-
+	
 	public void writeHeader(HumanSkeleton skeleton, PoseStreamer streamer) throws IOException {
 	}
-
+	
 	abstract void writeFrame(HumanSkeleton skeleton) throws IOException;
-
+	
 	public void writeFooter(HumanSkeleton skeleton) throws IOException {
 	}
-
+	
 	public boolean isClosed() {
 		return closed;
 	}
-
+	
 	@Override
 	public void close() throws IOException {
 		outputStream.close();

src/main/java/dev/slimevr/posestreamer/PoseStreamer.java

@@ -2,109 +2,115 @@ package dev.slimevr.posestreamer;
 
 import java.io.IOException;
 
-import io.eiren.util.ann.VRServerThread;
+import dev.slimevr.VRServer;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
 import io.eiren.util.logging.LogManager;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.processor.HumanSkeleton;
 
 public class PoseStreamer {
-
+	
 	protected long frameRecordingInterval = 60L;
 	protected long nextFrameTimeMs = -1L;
-
+	
 	private HumanSkeleton skeleton;
 	private PoseDataStream poseFileStream;
-
+	
 	protected final VRServer server;
-
+	
 	public PoseStreamer(VRServer server) {
 		this.server = server;
-
+		
 		// Register callbacks/events
 		server.addSkeletonUpdatedCallback(this::onSkeletonUpdated);
 		server.addOnTick(this::onTick);
 	}
-
+	
 	@VRServerThread
 	public void onSkeletonUpdated(HumanSkeleton skeleton) {
 		this.skeleton = skeleton;
 	}
-
+	
 	@VRServerThread
 	public void onTick() {
 		PoseDataStream poseFileStream = this.poseFileStream;
-		if (poseFileStream == null) {
+		if(poseFileStream == null) {
 			return;
 		}
-
+		
 		HumanSkeleton skeleton = this.skeleton;
-		if (skeleton == null) {
+		if(skeleton == null) {
 			return;
 		}
-			
+		
 		long curTime = System.currentTimeMillis();
-		if (curTime < nextFrameTimeMs) {
+		if(curTime < nextFrameTimeMs) {
 			return;
 		}
-			
+		
 		nextFrameTimeMs += frameRecordingInterval;
-
+		
 		// To prevent duplicate frames, make sure the frame time is always in the future
-		if (nextFrameTimeMs <= curTime) {
+		if(nextFrameTimeMs <= curTime) {
 			nextFrameTimeMs = curTime + frameRecordingInterval;
 		}
 		
 		// Make sure it's synchronized since this is the server thread interacting with
 		// an unknown outside thread controlling this class
-		synchronized (this) {
+		synchronized(this) {
 			// Make sure the stream is open before trying to write
-			if (poseFileStream.isClosed()) {
+			if(poseFileStream.isClosed()) {
 				return;
 			}
 			
 			try {
 				poseFileStream.writeFrame(skeleton);
-			} catch (Exception e) {
+			} catch(Exception e) {
 				// Handle any exceptions without crashing the program
 				LogManager.log.severe("[PoseStreamer] Exception while saving frame", e);
 			}
 		}
 	}
-
+	
 	public synchronized void setFrameInterval(long intervalMs) {
 		if(intervalMs < 1) {
 			throw new IllegalArgumentException("intervalMs must at least have a value of 1");
 		}
-
+		
 		this.frameRecordingInterval = intervalMs;
 	}
-
+	
 	public synchronized long getFrameInterval() {
 		return frameRecordingInterval;
 	}
-
+	
 	public synchronized void setOutput(PoseDataStream poseFileStream) throws IOException {
 		poseFileStream.writeHeader(skeleton, this);
 		this.poseFileStream = poseFileStream;
 		nextFrameTimeMs = -1L; // Reset the frame timing
 	}
-
+	
 	public synchronized void setOutput(PoseDataStream poseFileStream, long intervalMs) throws IOException {
 		setFrameInterval(intervalMs);
 		setOutput(poseFileStream);
 	}
-
+	
 	public synchronized PoseDataStream getOutput() {
 		return poseFileStream;
 	}
-
+	
 	public synchronized void closeOutput() throws IOException {
 		PoseDataStream poseFileStream = this.poseFileStream;
-
-		if (poseFileStream != null) {
-			poseFileStream.writeFooter(skeleton);
-			poseFileStream.close();
+		
+		if(poseFileStream != null) {
+			closeOutput(poseFileStream);
 			this.poseFileStream = null;
 		}
 	}
+	
+	public synchronized void closeOutput(PoseDataStream poseFileStream) throws IOException {
+		if(poseFileStream != null) {
+			poseFileStream.writeFooter(skeleton);
+			poseFileStream.close();
+		}
+	}
 }

src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java

@@ -0,0 +1,63 @@
+package dev.slimevr.posestreamer;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.OutputStream;
+
+import dev.slimevr.vr.processor.TransformNode;
+
+public class StdBVHFileStream extends BVHFileStream {
+	
+	public StdBVHFileStream(OutputStream outputStream) {
+		super(outputStream);
+	}
+	
+	public StdBVHFileStream(File file) throws FileNotFoundException {
+		super(file);
+	}
+	
+	public StdBVHFileStream(String file) throws FileNotFoundException {
+		super(file);
+	}
+	
+	@Override
+	protected TransformNodeWrapper wrapSkeletonNodes(TransformNode rootNode) {
+		TransformNode newRoot = getNodeFromHierarchy(rootNode, "Hip");
+		if(newRoot == null) {
+			return null;
+		}
+		
+		TransformNodeWrapper wrappedRoot = TransformNodeWrapper.wrapHierarchyDown(newRoot);
+		
+		/*
+		// If should wrap up hierarchy
+		if (newRoot.getParent() != null) {
+			// Create an extra node for full proper rotation
+			TransformNodeWrapper spineWrapper = new TransformNodeWrapper(new TransformNode("Spine", false), true, 1);
+			wrappedRoot.attachChild(spineWrapper);
+		
+			// Wrap up on top of the spine node
+			TransformNodeWrapper.wrapNodeHierarchyUp(newRoot, spineWrapper);
+		}
+		*/
+		
+		TransformNodeWrapper.wrapNodeHierarchyUp(wrappedRoot);
+		
+		return wrappedRoot;
+	}
+	
+	private TransformNode getNodeFromHierarchy(TransformNode node, String name) {
+		if(node.getName().equalsIgnoreCase(name)) {
+			return node;
+		}
+		
+		for(TransformNode child : node.children) {
+			TransformNode result = getNodeFromHierarchy(child, name);
+			if(result != null) {
+				return result;
+			}
+		}
+		
+		return null;
+	}
+}

src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java

@@ -0,0 +1,158 @@
+package dev.slimevr.posestreamer;
+
+import java.util.List;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Transform;
+
+import dev.slimevr.vr.processor.TransformNode;
+import io.eiren.util.collections.FastList;
+
+public class TransformNodeWrapper {
+	
+	public final TransformNode wrappedNode;
+	
+	protected String name;
+	
+	public final Transform localTransform;
+	public final Transform worldTransform;
+	
+	private boolean reversedHierarchy = false;
+	
+	protected TransformNodeWrapper parent;
+	public final List<TransformNodeWrapper> children;
+	
+	public TransformNodeWrapper(TransformNode nodeToWrap, String name, boolean reversedHierarchy, int initialChildCapacity) {
+		this.wrappedNode = nodeToWrap;
+		
+		this.name = name;
+		
+		this.localTransform = nodeToWrap.localTransform;
+		this.worldTransform = nodeToWrap.worldTransform;
+		
+		this.reversedHierarchy = reversedHierarchy;
+		
+		this.children = new FastList<>(initialChildCapacity);
+	}
+	
+	public TransformNodeWrapper(TransformNode nodeToWrap, String name, int initialChildCapacity) {
+		this(nodeToWrap, name, false, initialChildCapacity);
+	}
+	
+	public TransformNodeWrapper(TransformNode nodeToWrap, String name) {
+		this(nodeToWrap, name, false, 5);
+	}
+	
+	public TransformNodeWrapper(TransformNode nodeToWrap, boolean reversedHierarchy, int initialChildCapacity) {
+		this(nodeToWrap, nodeToWrap.getName(), reversedHierarchy, initialChildCapacity);
+	}
+	
+	public TransformNodeWrapper(TransformNode nodeToWrap, boolean reversedHierarchy) {
+		this(nodeToWrap, nodeToWrap.getName(), reversedHierarchy, 5);
+	}
+	
+	public TransformNodeWrapper(TransformNode nodeToWrap, int initialChildCapacity) {
+		this(nodeToWrap, nodeToWrap.getName(), initialChildCapacity);
+	}
+	
+	public TransformNodeWrapper(TransformNode nodeToWrap) {
+		this(nodeToWrap, nodeToWrap.getName());
+	}
+	
+	public static TransformNodeWrapper wrapFullHierarchy(TransformNode root) {
+		return wrapNodeHierarchyUp(wrapHierarchyDown(root));
+	}
+	
+	public static TransformNodeWrapper wrapHierarchyDown(TransformNode root) {
+		return wrapNodeHierarchyDown(new TransformNodeWrapper(root, root.children.size()));
+	}
+	
+	public static TransformNodeWrapper wrapNodeHierarchyDown(TransformNodeWrapper root) {
+		for(TransformNode child : root.wrappedNode.children) {
+			root.attachChild(wrapHierarchyDown(child));
+		}
+		
+		return root;
+	}
+	
+	public static TransformNodeWrapper wrapHierarchyUp(TransformNode root) {
+		return wrapNodeHierarchyUp(new TransformNodeWrapper(root, root.getParent() != null ? 1 : 0));
+	}
+	
+	public static TransformNodeWrapper wrapNodeHierarchyUp(TransformNodeWrapper root) {
+		return wrapNodeHierarchyUp(root.wrappedNode, root);
+	}
+	
+	public static TransformNodeWrapper wrapNodeHierarchyUp(TransformNode root, TransformNodeWrapper target) {
+		TransformNode parent = root.getParent();
+		if(parent == null) {
+			return target;
+		}
+		
+		// Flip the offset for these reversed nodes
+		TransformNodeWrapper wrapper = new TransformNodeWrapper(parent, true, (parent.getParent() != null ? 1 : 0) + Math.max(0, parent.children.size() - 1));
+		target.attachChild(wrapper);
+		
+		// Re-attach other children
+		if(parent.children.size() > 1) {
+			for(TransformNode child : parent.children) {
+				// Skip the original node
+				if(child == target.wrappedNode) {
+					continue;
+				}
+				
+				wrapper.attachChild(wrapHierarchyDown(child));
+			}
+		}
+		
+		// Continue up the hierarchy
+		wrapNodeHierarchyUp(wrapper);
+		// Return original node
+		return target;
+	}
+	
+	public boolean hasReversedHierarchy() {
+		return reversedHierarchy;
+	}
+	
+	public void setReversedHierarchy(boolean reversedHierarchy) {
+		this.reversedHierarchy = reversedHierarchy;
+	}
+	
+	public boolean hasLocalRotation() {
+		return wrappedNode.localRotation;
+	}
+	
+	public Quaternion calculateLocalRotation(Quaternion relativeTo, Quaternion result) {
+		return calculateLocalRotationInverse(relativeTo.inverse(), result);
+	}
+	
+	public Quaternion calculateLocalRotationInverse(Quaternion inverseRelativeTo, Quaternion result) {
+		if(result == null) {
+			result = new Quaternion();
+		}
+		
+		return worldTransform.getRotation().mult(inverseRelativeTo, result);
+	}
+	
+	public void attachChild(TransformNodeWrapper node) {
+		if(node.parent != null) {
+			throw new IllegalArgumentException("The child node must not already have a parent");
+		}
+		
+		this.children.add(node);
+		node.parent = this;
+	}
+	
+	public TransformNodeWrapper getParent() {
+		return parent;
+	}
+	
+	public String getName() {
+		return name;
+	}
+	
+	public void setName(String name) {
+		this.name = name;
+	}
+}

src/main/java/dev/slimevr/util/ann/VRServerThread.java

@@ -1,4 +1,4 @@
-package io.eiren.util.ann;
+package dev.slimevr.util.ann;
 
 import java.lang.annotation.Retention;
 import java.lang.annotation.RetentionPolicy;

src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java

@@ -1,17 +1,17 @@
-package io.eiren.vr.processor;
+package dev.slimevr.vr.processor;
 
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.TrackerWithTPS;
 import io.eiren.util.BufferedTimer;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.TrackerRole;
-import io.eiren.vr.trackers.TrackerWithTPS;
 
 public class ComputedHumanPoseTracker extends ComputedTracker implements TrackerWithTPS, ShareableTracker {
 	
 	public final ComputedHumanPoseTrackerPosition skeletonPosition;
 	protected final TrackerRole trackerRole;
 	protected BufferedTimer timer = new BufferedTimer(1f);
-
+	
 	public ComputedHumanPoseTracker(int trackerId, ComputedHumanPoseTrackerPosition skeletonPosition, TrackerRole role) {
 		super(trackerId, "human://" + skeletonPosition.name(), true, true);
 		this.skeletonPosition = skeletonPosition;
@@ -27,7 +27,7 @@ public class ComputedHumanPoseTracker extends ComputedTracker implements Tracker
 	public void dataTick() {
 		timer.update();
 	}
-
+	
 	@Override
 	public TrackerRole getTrackerRole() {
 		return trackerRole;

src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTrackerPosition.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.processor;
+package dev.slimevr.vr.processor;
 
 public enum ComputedHumanPoseTrackerPosition {
 	

src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java

@@ -1,17 +1,21 @@
-package io.eiren.vr.processor;
+package dev.slimevr.vr.processor;
 
 import java.util.List;
 import java.util.function.Consumer;
 
+import dev.slimevr.VRServer;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+import dev.slimevr.vr.processor.skeleton.SimpleSkeleton;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.TrackerStatus;
 import io.eiren.util.ann.ThreadSafe;
-import io.eiren.util.ann.VRServerThread;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.trackers.HMDTracker;
-import io.eiren.vr.trackers.ShareableTracker;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerRole;
-import io.eiren.vr.trackers.TrackerStatus;
 
 public class HumanPoseProcessor {
 	
@@ -19,7 +23,7 @@ public class HumanPoseProcessor {
 	private final List<ComputedHumanPoseTracker> computedTrackers = new FastList<>();
 	private final List<Consumer<HumanSkeleton>> onSkeletonUpdated = new FastList<>();
 	private HumanSkeleton skeleton;
-
+	
 	public HumanPoseProcessor(VRServer server, HMDTracker hmd) {
 		this.server = server;
 		computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.WAIST, TrackerRole.WAIST));
@@ -33,7 +37,7 @@ public class HumanPoseProcessor {
 	public HumanSkeleton getSkeleton() {
 		return skeleton;
 	}
-
+	
 	@VRServerThread
 	public void addSkeletonUpdatedCallback(Consumer<HumanSkeleton> consumer) {
 		onSkeletonUpdated.add(consumer);
@@ -42,23 +46,32 @@ public class HumanPoseProcessor {
 	}
 	
 	@ThreadSafe
-	public void setSkeletonConfig(String key, float newLength) {
+	public void setSkeletonConfig(SkeletonConfigValue key, float newLength) {
 		if(skeleton != null)
-			skeleton.setSkeletonConfig(key, newLength);
+			skeleton.getSkeletonConfig().setConfig(key, newLength);
 	}
-
+	
 	@ThreadSafe
-	public void resetSkeletonConfig(String key) {
+	public void resetSkeletonConfig(SkeletonConfigValue key) {
 		if(skeleton != null)
 			skeleton.resetSkeletonConfig(key);
 	}
 	
 	@ThreadSafe
-	public float getSkeletonConfig(String key) {
+	public void resetAllSkeletonConfigs() {
+		if(skeleton != null)
+			skeleton.resetAllSkeletonConfigs();
+	}
+	
+	@ThreadSafe
+	public SkeletonConfig getSkeletonConfig() {
+		return skeleton.getSkeletonConfig();
+	}
+	
+	@ThreadSafe
+	public float getSkeletonConfig(SkeletonConfigValue key) {
 		if(skeleton != null) {
-			Number f = skeleton.getSkeletonConfig().get(key);
-			if(f != null)
-				return f.floatValue();
+			return skeleton.getSkeletonConfig().getConfig(key);
 		}
 		return 0.0f;
 	}
@@ -67,44 +80,44 @@ public class HumanPoseProcessor {
 	public List<? extends ShareableTracker> getComputedTrackers() {
 		return computedTrackers;
 	}
-
+	
 	@VRServerThread
 	public void trackerAdded(Tracker tracker) {
 		updateSekeltonModel();
 	}
-
+	
 	@VRServerThread
 	public void trackerUpdated(Tracker tracker) {
 		updateSekeltonModel();
 	}
-
+	
 	@VRServerThread
 	private void updateSekeltonModel() {
 		disconnectAllTrackers();
-		skeleton = new HumanSkeletonWithLegs(server, computedTrackers);
+		skeleton = new SimpleSkeleton(server, computedTrackers);
 		for(int i = 0; i < onSkeletonUpdated.size(); ++i)
 			onSkeletonUpdated.get(i).accept(skeleton);
 	}
-
+	
 	@VRServerThread
 	private void disconnectAllTrackers() {
 		for(int i = 0; i < computedTrackers.size(); ++i) {
 			computedTrackers.get(i).setStatus(TrackerStatus.DISCONNECTED);
 		}
 	}
-
+	
 	@VRServerThread
 	public void update() {
 		if(skeleton != null)
 			skeleton.updatePose();
 	}
-
+	
 	@VRServerThread
 	public void resetTrackers() {
 		if(skeleton != null)
 			skeleton.resetTrackersFull();
 	}
-
+	
 	@VRServerThread
 	public void resetTrackersYaw() {
 		if(skeleton != null)

src/main/java/dev/slimevr/vr/processor/TransformNode.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.processor;
+package dev.slimevr.vr.processor;
 
 import java.util.List;
 import java.util.function.Consumer;
@@ -26,10 +26,18 @@ public class TransformNode {
 	}
 	
 	public void attachChild(TransformNode node) {
+		if(node.parent != null) {
+			throw new IllegalArgumentException("The child node must not already have a parent");
+		}
+		
 		this.children.add(node);
 		node.parent = this;
 	}
 	
+	public TransformNode getParent() {
+		return parent;
+	}
+	
 	public void update() {
 		updateWorldTransforms(); // Call update on each frame because we have relatively few nodes
 		for(int i = 0; i < children.size(); ++i)
@@ -58,14 +66,11 @@ public class TransformNode {
 	public String getName() {
 		return name;
 	}
-
-    public void combineWithParentGlobalRotation(Transform parent) {
-    	worldTransform.getScale().multLocal(parent.getScale());
-    	worldTransform.getTranslation().multLocal(parent.getScale());
-        
-        parent
-            .getRotation()
-            .multLocal(worldTransform.getTranslation())
-            .addLocal(parent.getTranslation());
-    }
+	
+	public void combineWithParentGlobalRotation(Transform parent) {
+		worldTransform.getScale().multLocal(parent.getScale());
+		worldTransform.getTranslation().multLocal(parent.getScale());
+		
+		parent.getRotation().multLocal(worldTransform.getTranslation()).addLocal(parent.getTranslation());
+	}
 }

src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java

@@ -0,0 +1,33 @@
+package dev.slimevr.vr.processor.skeleton;
+
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.TransformNode;
+import io.eiren.util.ann.ThreadSafe;
+
+public abstract class HumanSkeleton {
+	
+	@VRServerThread
+	public abstract void updatePose();
+	
+	@ThreadSafe
+	public abstract TransformNode getRootNode();
+	
+	@ThreadSafe
+	public abstract SkeletonConfig getSkeletonConfig();
+	
+	@ThreadSafe
+	public abstract void resetSkeletonConfig(SkeletonConfigValue config);
+	
+	@ThreadSafe
+	public void resetAllSkeletonConfigs() {
+		for(SkeletonConfigValue config : SkeletonConfigValue.values) {
+			resetSkeletonConfig(config);
+		}
+	}
+	
+	@VRServerThread
+	public abstract void resetTrackersFull();
+	
+	@VRServerThread
+	public abstract void resetTrackersYaw();
+}

src/main/java/dev/slimevr/vr/processor/skeleton/SimpleSkeleton.java

@@ -0,0 +1,779 @@
+package dev.slimevr.vr.processor.skeleton;
+
+import java.util.List;
+import java.util.Map;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import dev.slimevr.VRServer;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.ComputedHumanPoseTracker;
+import dev.slimevr.vr.processor.ComputedHumanPoseTrackerPosition;
+import dev.slimevr.vr.processor.TransformNode;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.TrackerStatus;
+import dev.slimevr.vr.trackers.TrackerUtils;
+import io.eiren.util.collections.FastList;
+
+public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallback {
+	
+	public static final float DEFAULT_FLOOR_OFFSET = 0.05f;
+	
+	//#region Upper body nodes (torso)
+	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 chestNode = new TransformNode("Chest", false);
+	protected final TransformNode waistNode = new TransformNode("Waist", false);
+	protected final TransformNode hipNode = new TransformNode("Hip", false);
+	protected final TransformNode trackerWaistNode = new TransformNode("Waist-Tracker", false);
+	//#endregion
+	
+	//#region Lower body nodes (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 leftFootNode = new TransformNode("Left-Foot", 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);
+	protected final TransformNode rightFootNode = new TransformNode("Right-Foot", false);
+	
+	protected float minKneePitch = 0f * FastMath.DEG_TO_RAD;
+	protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD;
+	
+	protected float kneeLerpFactor = 0.5f;
+	//#endregion
+	
+	//#region Tracker Input
+	protected Tracker hmdTracker;
+	protected Tracker chestTracker;
+	protected Tracker waistTracker;
+	protected Tracker hipTracker;
+	
+	protected Tracker leftLegTracker;
+	protected Tracker leftAnkleTracker;
+	protected Tracker leftFootTracker;
+	
+	protected Tracker rightLegTracker;
+	protected Tracker rightAnkleTracker;
+	protected Tracker rightFootTracker;
+	//#endregion
+	
+	//#region Tracker Output
+	protected ComputedHumanPoseTracker computedChestTracker;
+	protected ComputedHumanPoseTracker computedWaistTracker;
+	
+	protected ComputedHumanPoseTracker computedLeftKneeTracker;
+	protected ComputedHumanPoseTracker computedLeftFootTracker;
+	
+	protected ComputedHumanPoseTracker computedRightKneeTracker;
+	protected ComputedHumanPoseTracker computedRightFootTracker;
+	//#endregion
+	
+	protected boolean extendedPelvisModel = true;
+	protected boolean extendedKneeModel = false;
+	
+	public final SkeletonConfig skeletonConfig;
+	
+	//#region Buffers
+	private Vector3f posBuf = new Vector3f();
+	
+	private Quaternion rotBuf1 = new Quaternion();
+	private Quaternion rotBuf2 = new Quaternion();
+	
+	protected final Vector3f hipVector = new Vector3f();
+	protected final Vector3f ankleVector = new Vector3f();
+	
+	protected final Quaternion kneeRotation = new Quaternion();
+	//#endregion
+	
+	//#region Constructors
+	protected SimpleSkeleton(List<? extends ComputedHumanPoseTracker> computedTrackers) {
+		//#region Assemble skeleton to hip
+		hmdNode.attachChild(headNode);
+		headNode.attachChild(neckNode);
+		neckNode.attachChild(chestNode);
+		chestNode.attachChild(waistNode);
+		waistNode.attachChild(hipNode);
+		hipNode.attachChild(trackerWaistNode);
+		//#endregion
+		
+		//#region Assemble skeleton to feet
+		hipNode.attachChild(leftHipNode);
+		hipNode.attachChild(rightHipNode);
+		
+		leftHipNode.attachChild(leftKneeNode);
+		rightHipNode.attachChild(rightKneeNode);
+		
+		leftKneeNode.attachChild(leftAnkleNode);
+		rightKneeNode.attachChild(rightAnkleNode);
+		
+		leftAnkleNode.attachChild(leftFootNode);
+		rightAnkleNode.attachChild(rightFootNode);
+		//#endregion
+		
+		// Set default skeleton configuration (callback automatically sets initial offsets)
+		skeletonConfig = new SkeletonConfig(true, this);
+		
+		if(computedTrackers != null) {
+			setComputedTrackers(computedTrackers);
+		}
+		fillNullComputedTrackers(true);
+	}
+	
+	public SimpleSkeleton(VRServer server, List<? extends ComputedHumanPoseTracker> computedTrackers) {
+		this(computedTrackers);
+		setTrackersFromServer(server);
+		skeletonConfig.loadFromConfig(server.config);
+	}
+	
+	public SimpleSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers) {
+		this(computedTrackers);
+		
+		if(trackers != null) {
+			setTrackersFromList(trackers);
+		} else {
+			setTrackersFromList(new FastList<Tracker>(0));
+		}
+	}
+	
+	public SimpleSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers, Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigValue, Float> altConfigs) {
+		// Initialize
+		this(trackers, computedTrackers);
+		
+		// Set configs
+		if(altConfigs != null) {
+			// Set alts first, so if there's any overlap it doesn't affect the values
+			skeletonConfig.setConfigs(altConfigs, null);
+		}
+		skeletonConfig.setConfigs(configs, null);
+	}
+	
+	public SimpleSkeleton(List<? extends Tracker> trackers, List<? extends ComputedHumanPoseTracker> computedTrackers, Map<SkeletonConfigValue, Float> configs) {
+		this(trackers, computedTrackers, configs, null);
+	}
+	//#endregion
+	
+	//#region Set Trackers
+	public void setTrackersFromList(List<? extends Tracker> trackers, boolean setHmd) {
+		if(setHmd) {
+			this.hmdTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.HMD);
+		}
+		
+		this.chestTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.CHEST, TrackerPosition.WAIST, TrackerPosition.HIP);
+		this.waistTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.WAIST, TrackerPosition.CHEST, TrackerPosition.HIP);
+		this.hipTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.HIP, TrackerPosition.WAIST, TrackerPosition.CHEST);
+		
+		this.leftLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.LEFT_LEG, TrackerPosition.LEFT_ANKLE, null);
+		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.LEFT_ANKLE, TrackerPosition.LEFT_LEG, null);
+		this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_FOOT);
+		
+		this.rightLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.RIGHT_LEG, TrackerPosition.RIGHT_ANKLE, null);
+		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.RIGHT_ANKLE, TrackerPosition.RIGHT_LEG, null);
+		this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_FOOT);
+	}
+	
+	public void setTrackersFromList(List<? extends Tracker> trackers) {
+		setTrackersFromList(trackers, true);
+	}
+	
+	public void setTrackersFromServer(VRServer server) {
+		this.hmdTracker = server.hmdTracker;
+		setTrackersFromList(server.getAllTrackers(), false);
+	}
+	
+	public void setComputedTracker(ComputedHumanPoseTracker tracker) {
+		switch(tracker.getTrackerRole()) {
+		case CHEST:
+			computedChestTracker = tracker;
+			break;
+		case WAIST:
+			computedWaistTracker = tracker;
+			break;
+		
+		case LEFT_KNEE:
+			computedLeftKneeTracker = tracker;
+			break;
+		case LEFT_FOOT:
+			computedLeftFootTracker = tracker;
+			break;
+		
+		case RIGHT_KNEE:
+			computedRightKneeTracker = tracker;
+			break;
+		case RIGHT_FOOT:
+			computedRightFootTracker = tracker;
+			break;
+		}
+	}
+	
+	public void setComputedTrackers(List<? extends ComputedHumanPoseTracker> trackers) {
+		for(int i = 0; i < trackers.size(); ++i) {
+			setComputedTracker(trackers.get(i));
+		}
+	}
+	
+	public void setComputedTrackersAndFillNull(List<? extends ComputedHumanPoseTracker> trackers, boolean onlyFillWaistAndFeet) {
+		setComputedTrackers(trackers);
+		fillNullComputedTrackers(onlyFillWaistAndFeet);
+	}
+	
+	public void fillNullComputedTrackers(boolean onlyFillWaistAndFeet) {
+		if(computedWaistTracker == null) {
+			computedWaistTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.WAIST, TrackerRole.WAIST);
+			computedWaistTracker.setStatus(TrackerStatus.OK);
+		}
+		
+		if(computedLeftFootTracker == null) {
+			computedLeftFootTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_FOOT, TrackerRole.LEFT_FOOT);
+			computedLeftFootTracker.setStatus(TrackerStatus.OK);
+		}
+		
+		if(computedRightFootTracker == null) {
+			computedRightFootTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_FOOT, TrackerRole.RIGHT_FOOT);
+			computedRightFootTracker.setStatus(TrackerStatus.OK);
+		}
+		
+		if(!onlyFillWaistAndFeet) {
+			if(computedChestTracker == null) {
+				computedChestTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.CHEST, TrackerRole.CHEST);
+				computedChestTracker.setStatus(TrackerStatus.OK);
+			}
+			
+			if(computedLeftKneeTracker == null) {
+				computedLeftKneeTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_KNEE, TrackerRole.LEFT_KNEE);
+				computedLeftKneeTracker.setStatus(TrackerStatus.OK);
+			}
+			
+			if(computedRightKneeTracker == null) {
+				computedRightKneeTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_KNEE, TrackerRole.RIGHT_KNEE);
+				computedRightKneeTracker.setStatus(TrackerStatus.OK);
+			}
+		}
+	}
+	//#endregion
+	
+	//#region Get Trackers
+	public ComputedHumanPoseTracker getComputedTracker(TrackerRole trackerRole) {
+		switch(trackerRole) {
+		case CHEST:
+			return computedChestTracker;
+		case WAIST:
+			return computedWaistTracker;
+		
+		case LEFT_KNEE:
+			return computedLeftKneeTracker;
+		case LEFT_FOOT:
+			return computedLeftFootTracker;
+		
+		case RIGHT_KNEE:
+			return computedRightKneeTracker;
+		case RIGHT_FOOT:
+			return computedRightFootTracker;
+		}
+		
+		return null;
+	}
+	//#endregion
+	
+	//#region Processing
+	// Useful for sub-classes that need to return a sub-tracker (like PoseFrameTracker -> TrackerFrame)
+	protected Tracker trackerPreUpdate(Tracker tracker) {
+		return tracker;
+	}
+	
+	// Updates the pose from tracker positions
+	@VRServerThread
+	@Override
+	public void updatePose() {
+		updateLocalTransforms();
+		hmdNode.update();
+		updateComputedTrackers();
+	}
+	
+	//#region Update the node transforms from the trackers
+	protected void updateLocalTransforms() {
+		//#region Pass all trackers through trackerPreUpdate
+		Tracker hmdTracker = trackerPreUpdate(this.hmdTracker);
+		
+		Tracker chestTracker = trackerPreUpdate(this.chestTracker);
+		Tracker waistTracker = trackerPreUpdate(this.waistTracker);
+		Tracker hipTracker = trackerPreUpdate(this.hipTracker);
+		
+		Tracker leftLegTracker = trackerPreUpdate(this.leftLegTracker);
+		Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker);
+		Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker);
+		
+		Tracker rightLegTracker = trackerPreUpdate(this.rightLegTracker);
+		Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker);
+		Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker);
+		//#endregion
+		
+		if(hmdTracker != null) {
+			if(hmdTracker.getPosition(posBuf)) {
+				hmdNode.localTransform.setTranslation(posBuf);
+			}
+			if(hmdTracker.getRotation(rotBuf1)) {
+				hmdNode.localTransform.setRotation(rotBuf1);
+				headNode.localTransform.setRotation(rotBuf1);
+			}
+		} else {
+			// Set to zero
+			hmdNode.localTransform.setTranslation(Vector3f.ZERO);
+			hmdNode.localTransform.setRotation(Quaternion.IDENTITY);
+			headNode.localTransform.setRotation(Quaternion.IDENTITY);
+		}
+		
+		if(chestTracker.getRotation(rotBuf1)) {
+			neckNode.localTransform.setRotation(rotBuf1);
+		}
+		if(waistTracker.getRotation(rotBuf1)) {
+			chestNode.localTransform.setRotation(rotBuf1);
+		}
+		if(hipTracker.getRotation(rotBuf1)) {
+			waistNode.localTransform.setRotation(rotBuf1);
+			trackerWaistNode.localTransform.setRotation(rotBuf1);
+			hipNode.localTransform.setRotation(rotBuf1);
+		}
+		
+		// Left Leg
+		leftLegTracker.getRotation(rotBuf1);
+		leftAnkleTracker.getRotation(rotBuf2);
+		
+		if(extendedKneeModel)
+			calculateKneeLimits(rotBuf1, rotBuf2, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
+		
+		leftHipNode.localTransform.setRotation(rotBuf1);
+		leftKneeNode.localTransform.setRotation(rotBuf2);
+		leftAnkleNode.localTransform.setRotation(rotBuf2);
+		leftFootNode.localTransform.setRotation(rotBuf2);
+		
+		if(leftFootTracker != null) {
+			leftFootTracker.getRotation(rotBuf2);
+			leftAnkleNode.localTransform.setRotation(rotBuf2);
+			leftFootNode.localTransform.setRotation(rotBuf2);
+		}
+		
+		// Right Leg
+		rightLegTracker.getRotation(rotBuf1);
+		rightAnkleTracker.getRotation(rotBuf2);
+		
+		if(extendedKneeModel)
+			calculateKneeLimits(rotBuf1, rotBuf2, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
+		
+		rightHipNode.localTransform.setRotation(rotBuf1);
+		rightKneeNode.localTransform.setRotation(rotBuf2);
+		rightAnkleNode.localTransform.setRotation(rotBuf2);
+		rightFootNode.localTransform.setRotation(rotBuf2);
+		
+		if(rightFootTracker != null) {
+			rightFootTracker.getRotation(rotBuf2);
+			rightAnkleNode.localTransform.setRotation(rotBuf2);
+			rightFootNode.localTransform.setRotation(rotBuf2);
+		}
+		
+		if(extendedPelvisModel) {
+			// Average pelvis between two legs
+			leftHipNode.localTransform.getRotation(rotBuf1);
+			rightHipNode.localTransform.getRotation(rotBuf2);
+			rotBuf2.nlerp(rotBuf1, 0.5f);
+			chestNode.localTransform.getRotation(rotBuf1);
+			rotBuf2.nlerp(rotBuf1, 0.3333333f);
+			hipNode.localTransform.setRotation(rotBuf2);
+			//trackerWaistNode.localTransform.setRotation(rotBuf2); // <== Provides cursed results from my test in VRChat when sitting or laying down -Erimel
+			// TODO : Correct the trackerWaistNode without getting cursed results (only correct yaw?)
+			// TODO : Use vectors to add like 50% of waist tracker yaw to waist node to reduce drift and let user take weird poses
+		}
+	}
+	//#endregion
+	
+	//#region Knee Model
+	// Knee basically has only 1 DoF (pitch), average yaw and roll between knee and hip
+	protected void calculateKneeLimits(Quaternion hipBuf, Quaternion kneeBuf, float hipConfidence, float kneeConfidence) {
+		ankleVector.set(0, -1, 0);
+		hipVector.set(0, -1, 0);
+		hipBuf.multLocal(hipVector);
+		kneeBuf.multLocal(ankleVector);
+		kneeRotation.angleBetweenVectors(hipVector, ankleVector); // Find knee angle
+		
+		// Substract knee angle from knee rotation. With perfect leg and perfect
+		// sensors result should match hip rotation perfectly
+		kneeBuf.multLocal(kneeRotation.inverse());
+		
+		// Average knee and hip with a slerp
+		hipBuf.slerp(kneeBuf, 0.5f); // TODO : Use confidence to calculate changeAmt
+		kneeBuf.set(hipBuf);
+		
+		// Return knee angle into knee rotation
+		kneeBuf.multLocal(kneeRotation);
+	}
+	
+	public static float normalizeRad(float angle) {
+		return FastMath.normalize(angle, -FastMath.PI, FastMath.PI);
+	}
+	
+	public static float interpolateRadians(float factor, float start, float end) {
+		float angle = FastMath.abs(end - start);
+		if(angle > FastMath.PI) {
+			if(end > start) {
+				start += FastMath.TWO_PI;
+			} else {
+				end += FastMath.TWO_PI;
+			}
+		}
+		float val = start + (end - start) * factor;
+		return normalizeRad(val);
+	}
+	//#endregion
+	
+	//#region Update the output trackers
+	protected void updateComputedTrackers() {
+		if(computedChestTracker != null) {
+			computedChestTracker.position.set(chestNode.worldTransform.getTranslation());
+			computedChestTracker.rotation.set(neckNode.worldTransform.getRotation());
+			computedChestTracker.dataTick();
+		}
+		
+		if(computedWaistTracker != null) {
+			computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation());
+			computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation());
+			computedWaistTracker.dataTick();
+		}
+		
+		if(computedLeftKneeTracker != null) {
+			computedLeftKneeTracker.position.set(leftKneeNode.worldTransform.getTranslation());
+			computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation());
+			computedLeftKneeTracker.dataTick();
+		}
+		
+		if(computedLeftFootTracker != null) {
+			computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation());
+			computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation());
+			computedLeftFootTracker.dataTick();
+		}
+		
+		if(computedRightKneeTracker != null) {
+			computedRightKneeTracker.position.set(rightKneeNode.worldTransform.getTranslation());
+			computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation());
+			computedRightKneeTracker.dataTick();
+		}
+		
+		if(computedRightFootTracker != null) {
+			computedRightFootTracker.position.set(rightFootNode.worldTransform.getTranslation());
+			computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation());
+			computedRightFootTracker.dataTick();
+		}
+	}
+	//#endregion
+	//#endregion
+	
+	//#region Skeleton Config
+	@Override
+	public void updateConfigState(SkeletonConfigValue config, float newValue) {
+		// Do nothing, the node offset callback handles all that's needed
+	}
+	
+	@Override
+	public void updateToggleState(SkeletonConfigToggle configToggle, boolean newValue) {
+		if(configToggle == null) {
+			return;
+		}
+		
+		// Cache the values of these configs
+		switch(configToggle) {
+		case EXTENDED_PELVIS_MODEL:
+			extendedPelvisModel = newValue;
+			break;
+		case EXTENDED_KNEE_MODEL:
+			extendedKneeModel = newValue;
+			break;
+		}
+	}
+	
+	@Override
+	public void updateNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset) {
+		if(nodeOffset == null) {
+			return;
+		}
+		
+		switch(nodeOffset) {
+		case HEAD:
+			headNode.localTransform.setTranslation(offset);
+			break;
+		case NECK:
+			neckNode.localTransform.setTranslation(offset);
+			break;
+		case CHEST:
+			chestNode.localTransform.setTranslation(offset);
+			break;
+		case WAIST:
+			waistNode.localTransform.setTranslation(offset);
+			break;
+		case HIP:
+			hipNode.localTransform.setTranslation(offset);
+			break;
+		case HIP_TRACKER:
+			trackerWaistNode.localTransform.setTranslation(offset);
+			break;
+		
+		case LEFT_HIP:
+			leftHipNode.localTransform.setTranslation(offset);
+			break;
+		case RIGHT_HIP:
+			rightHipNode.localTransform.setTranslation(offset);
+			break;
+		
+		case KNEE:
+			leftKneeNode.localTransform.setTranslation(offset);
+			rightKneeNode.localTransform.setTranslation(offset);
+			break;
+		case ANKLE:
+			leftAnkleNode.localTransform.setTranslation(offset);
+			rightAnkleNode.localTransform.setTranslation(offset);
+			break;
+		case FOOT:
+			leftFootNode.localTransform.setTranslation(offset);
+			rightFootNode.localTransform.setTranslation(offset);
+			break;
+		}
+	}
+	
+	public void updatePoseAffectedByConfig(SkeletonConfigValue config) {
+		switch(config) {
+		case HEAD:
+			headNode.update();
+			updateComputedTrackers();
+			break;
+		case NECK:
+			neckNode.update();
+			updateComputedTrackers();
+			break;
+		case TORSO:
+			hipNode.update();
+			updateComputedTrackers();
+			break;
+		case CHEST:
+			chestNode.update();
+			updateComputedTrackers();
+			break;
+		case WAIST:
+			waistNode.update();
+			updateComputedTrackers();
+			break;
+		case HIP_OFFSET:
+			trackerWaistNode.update();
+			updateComputedTrackers();
+			break;
+		case HIPS_WIDTH:
+			leftHipNode.update();
+			rightHipNode.update();
+			updateComputedTrackers();
+			break;
+		case KNEE_HEIGHT:
+			leftKneeNode.update();
+			rightKneeNode.update();
+			break;
+		case LEGS_LENGTH:
+			leftKneeNode.update();
+			rightKneeNode.update();
+			updateComputedTrackers();
+			break;
+		case FOOT_LENGTH:
+			leftFootNode.update();
+			rightFootNode.update();
+			updateComputedTrackers();
+			break;
+		case FOOT_OFFSET:
+			leftAnkleNode.update();
+			rightAnkleNode.update();
+			updateComputedTrackers();
+			break;
+		}
+	}
+	//#endregion
+	
+	@Override
+	public TransformNode getRootNode() {
+		return hmdNode;
+	}
+	
+	@Override
+	public SkeletonConfig getSkeletonConfig() {
+		return skeletonConfig;
+	}
+	
+	@Override
+	public void resetSkeletonConfig(SkeletonConfigValue config) {
+		if(config == null) {
+			return;
+		}
+		
+		Vector3f vec;
+		float height;
+		switch(config) {
+		case HEAD:
+			skeletonConfig.setConfig(SkeletonConfigValue.HEAD, null);
+			break;
+		case NECK:
+			skeletonConfig.setConfig(SkeletonConfigValue.NECK, null);
+			break;
+		case TORSO: // Distance from shoulders to hip (full torso length)
+			vec = new Vector3f();
+			hmdTracker.getPosition(vec);
+			height = vec.y;
+			if(height > 0.5f) { // Reset only if floor level is right, TODO: read floor level from SteamVR if it's not 0
+				skeletonConfig.setConfig(SkeletonConfigValue.TORSO, ((height) / 2.0f) - skeletonConfig.getConfig(SkeletonConfigValue.NECK));
+			} else// if floor level is incorrect
+			{
+				skeletonConfig.setConfig(SkeletonConfigValue.TORSO, null);
+			}
+			break;
+		case CHEST: //Chest is roughly half of the upper body (shoulders to chest)
+			skeletonConfig.setConfig(SkeletonConfigValue.CHEST, skeletonConfig.getConfig(SkeletonConfigValue.TORSO) / 2.0f);
+			break;
+		case WAIST: // waist length is from hips to waist
+			skeletonConfig.setConfig(SkeletonConfigValue.WAIST, null);
+			break;
+		case HIP_OFFSET:
+			skeletonConfig.setConfig(SkeletonConfigValue.HIP_OFFSET, null);
+			break;
+		case HIPS_WIDTH:
+			skeletonConfig.setConfig(SkeletonConfigValue.HIPS_WIDTH, null);
+			break;
+		case FOOT_LENGTH:
+			skeletonConfig.setConfig(SkeletonConfigValue.FOOT_LENGTH, null);
+			break;
+		case FOOT_OFFSET:
+			skeletonConfig.setConfig(SkeletonConfigValue.FOOT_OFFSET, null);
+			break;
+		case LEGS_LENGTH: // Set legs length to be 5cm above floor level
+			vec = new Vector3f();
+			hmdTracker.getPosition(vec);
+			height = vec.y;
+			if(height > 0.5f) { // Reset only if floor level is right, todo: read floor level from SteamVR if it's not 0
+				skeletonConfig.setConfig(SkeletonConfigValue.LEGS_LENGTH, height - skeletonConfig.getConfig(SkeletonConfigValue.NECK) - skeletonConfig.getConfig(SkeletonConfigValue.TORSO) - DEFAULT_FLOOR_OFFSET);
+			} else //if floor level is incorrect
+			{
+				skeletonConfig.setConfig(SkeletonConfigValue.LEGS_LENGTH, null);
+			}
+			resetSkeletonConfig(SkeletonConfigValue.KNEE_HEIGHT);
+			break;
+		case KNEE_HEIGHT: // Knees are at 50% of the legs by default
+			skeletonConfig.setConfig(SkeletonConfigValue.KNEE_HEIGHT, skeletonConfig.getConfig(SkeletonConfigValue.LEGS_LENGTH) / 2.0f);
+			break;
+		}
+	}
+	
+	@Override
+	public void resetTrackersFull() {
+		//#region Pass all trackers through trackerPreUpdate
+		Tracker hmdTracker = trackerPreUpdate(this.hmdTracker);
+		
+		Tracker chestTracker = trackerPreUpdate(this.chestTracker);
+		Tracker waistTracker = trackerPreUpdate(this.waistTracker);
+		Tracker hipTracker = trackerPreUpdate(this.hipTracker);
+		
+		Tracker leftLegTracker = trackerPreUpdate(this.leftLegTracker);
+		Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker);
+		Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker);
+		
+		Tracker rightLegTracker = trackerPreUpdate(this.rightLegTracker);
+		Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker);
+		Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker);
+		//#endregion
+		
+		// Each tracker uses the tracker before it to adjust itself,
+		// so trackers that don't need adjustments could be used too
+		Quaternion referenceRotation = new Quaternion();
+		hmdTracker.getRotation(referenceRotation);
+		
+		chestTracker.resetFull(referenceRotation);
+		chestTracker.getRotation(referenceRotation);
+		
+		waistTracker.resetFull(referenceRotation);
+		waistTracker.getRotation(referenceRotation);
+		
+		hipTracker.resetFull(referenceRotation);
+		hipTracker.getRotation(referenceRotation);
+		
+		leftLegTracker.resetFull(referenceRotation);
+		rightLegTracker.resetFull(referenceRotation);
+		leftLegTracker.getRotation(referenceRotation);
+		
+		leftAnkleTracker.resetFull(referenceRotation);
+		leftAnkleTracker.getRotation(referenceRotation);
+		
+		if(leftFootTracker != null) {
+			leftFootTracker.resetFull(referenceRotation);
+		}
+		
+		rightLegTracker.getRotation(referenceRotation);
+		
+		rightAnkleTracker.resetFull(referenceRotation);
+		rightAnkleTracker.getRotation(referenceRotation);
+		
+		if(rightFootTracker != null) {
+			rightFootTracker.resetFull(referenceRotation);
+		}
+	}
+	
+	@Override
+	@VRServerThread
+	public void resetTrackersYaw() {
+		//#region Pass all trackers through trackerPreUpdate
+		Tracker hmdTracker = trackerPreUpdate(this.hmdTracker);
+		
+		Tracker chestTracker = trackerPreUpdate(this.chestTracker);
+		Tracker waistTracker = trackerPreUpdate(this.waistTracker);
+		Tracker hipTracker = trackerPreUpdate(this.hipTracker);
+		
+		Tracker leftLegTracker = trackerPreUpdate(this.leftLegTracker);
+		Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker);
+		Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker);
+		
+		Tracker rightLegTracker = trackerPreUpdate(this.rightLegTracker);
+		Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker);
+		Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker);
+		//#endregion
+		
+		// Each tracker uses the tracker before it to adjust itself,
+		// so trackers that don't need adjustments could be used too
+		Quaternion referenceRotation = new Quaternion();
+		hmdTracker.getRotation(referenceRotation);
+		
+		chestTracker.resetYaw(referenceRotation);
+		chestTracker.getRotation(referenceRotation);
+		
+		waistTracker.resetYaw(referenceRotation);
+		waistTracker.getRotation(referenceRotation);
+		
+		hipTracker.resetYaw(referenceRotation);
+		hipTracker.getRotation(referenceRotation);
+		
+		leftLegTracker.resetYaw(referenceRotation);
+		rightLegTracker.resetYaw(referenceRotation);
+		leftLegTracker.getRotation(referenceRotation);
+		
+		leftAnkleTracker.resetYaw(referenceRotation);
+		leftAnkleTracker.getRotation(referenceRotation);
+		
+		if(leftFootTracker != null) {
+			leftFootTracker.resetYaw(referenceRotation);
+		}
+		
+		rightLegTracker.getRotation(referenceRotation);
+		
+		rightAnkleTracker.resetYaw(referenceRotation);
+		rightAnkleTracker.getRotation(referenceRotation);
+		
+		if(rightFootTracker != null) {
+			rightFootTracker.resetYaw(referenceRotation);
+		}
+	}
+}

src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java

@@ -0,0 +1,366 @@
+package dev.slimevr.vr.processor.skeleton;
+
+import java.util.EnumMap;
+import java.util.Map;
+
+import com.jme3.math.Vector3f;
+
+import io.eiren.util.logging.LogManager;
+import io.eiren.yaml.YamlFile;
+
+public class SkeletonConfig {
+	
+	protected final EnumMap<SkeletonConfigValue, Float> configs = new EnumMap<SkeletonConfigValue, Float>(SkeletonConfigValue.class);
+	protected final EnumMap<SkeletonConfigToggle, Boolean> toggles = new EnumMap<SkeletonConfigToggle, Boolean>(SkeletonConfigToggle.class);
+	protected final EnumMap<SkeletonNodeOffset, Vector3f> nodeOffsets = new EnumMap<SkeletonNodeOffset, Vector3f>(SkeletonNodeOffset.class);
+	
+	protected final boolean autoUpdateOffsets;
+	protected final SkeletonConfigCallback callback;
+	
+	public SkeletonConfig(boolean autoUpdateOffsets) {
+		this.autoUpdateOffsets = autoUpdateOffsets;
+		this.callback = null;
+		
+		callCallbackOnAll(true);
+		
+		if(autoUpdateOffsets) {
+			computeAllNodeOffsets();
+		}
+	}
+	
+	public SkeletonConfig(boolean autoUpdateOffsets, SkeletonConfigCallback callback) {
+		this.autoUpdateOffsets = autoUpdateOffsets;
+		this.callback = callback;
+		
+		callCallbackOnAll(true);
+		
+		if(autoUpdateOffsets) {
+			computeAllNodeOffsets();
+		}
+	}
+	
+	public SkeletonConfig(Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigToggle, Boolean> toggles, boolean autoUpdateOffsets, SkeletonConfigCallback callback) {
+		this.autoUpdateOffsets = autoUpdateOffsets;
+		this.callback = callback;
+		setConfigs(configs, toggles);
+		
+		callCallbackOnAll(true);
+	}
+	
+	public SkeletonConfig(Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigToggle, Boolean> toggles, boolean autoUpdateOffsets) {
+		this(configs, toggles, autoUpdateOffsets, null);
+	}
+	
+	public SkeletonConfig(SkeletonConfig skeletonConfig, boolean autoUpdateOffsets, SkeletonConfigCallback callback) {
+		this.autoUpdateOffsets = autoUpdateOffsets;
+		this.callback = callback;
+		setConfigs(skeletonConfig);
+		
+		callCallbackOnAll(true);
+	}
+	
+	public SkeletonConfig(SkeletonConfig skeletonConfig, boolean autoUpdateOffsets) {
+		this(skeletonConfig, autoUpdateOffsets, null);
+	}
+	
+	private void callCallbackOnAll(boolean defaultOnly) {
+		if(callback == null) {
+			return;
+		}
+		
+		for(SkeletonConfigValue config : SkeletonConfigValue.values) {
+			try {
+				Float val = configs.get(config);
+				if(!defaultOnly || val == null) {
+					callback.updateConfigState(config, val == null ? config.defaultValue : val);
+				}
+			} catch(Exception e) {
+				LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e);
+			}
+		}
+		
+		for(SkeletonConfigToggle config : SkeletonConfigToggle.values) {
+			try {
+				Boolean val = toggles.get(config);
+				if(!defaultOnly || val == null) {
+					callback.updateToggleState(config, val == null ? config.defaultValue : val);
+				}
+			} catch(Exception e) {
+				LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e);
+			}
+		}
+	}
+	
+	public Float setConfig(SkeletonConfigValue config, Float newValue, boolean computeOffsets) {
+		Float origVal = newValue != null ? configs.put(config, newValue) : configs.remove(config);
+		
+		// Re-compute the affected offsets
+		if(computeOffsets && autoUpdateOffsets && config.affectedOffsets != null) {
+			for(SkeletonNodeOffset offset : config.affectedOffsets) {
+				computeNodeOffset(offset);
+			}
+		}
+		
+		if(callback != null) {
+			try {
+				callback.updateConfigState(config, newValue != null ? newValue : config.defaultValue);
+			} catch(Exception e) {
+				LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e);
+			}
+		}
+		
+		return origVal;
+	}
+	
+	public Float setConfig(SkeletonConfigValue config, Float newValue) {
+		return setConfig(config, newValue, true);
+	}
+	
+	public Float setConfig(String config, Float newValue) {
+		return setConfig(SkeletonConfigValue.getByStringValue(config), newValue);
+	}
+	
+	public float getConfig(SkeletonConfigValue config) {
+		if(config == null) {
+			return 0f;
+		}
+		
+		// IMPORTANT!! This null check is necessary, getOrDefault seems to randomly decide to return null at times, so this is a secondary check
+		Float val = configs.getOrDefault(config, config.defaultValue);
+		return val != null ? val : config.defaultValue;
+	}
+	
+	public float getConfig(String config) {
+		if(config == null) {
+			return 0f;
+		}
+		return getConfig(SkeletonConfigValue.getByStringValue(config));
+	}
+	
+	public Boolean setToggle(SkeletonConfigToggle config, Boolean newValue) {
+		Boolean origVal = newValue != null ? toggles.put(config, newValue) : toggles.remove(config);
+		
+		if(callback != null) {
+			try {
+				callback.updateToggleState(config, newValue != null ? newValue : config.defaultValue);
+			} catch(Exception e) {
+				LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e);
+			}
+		}
+		
+		return origVal;
+	}
+	
+	public Boolean setToggle(String config, Boolean newValue) {
+		return setToggle(SkeletonConfigToggle.getByStringValue(config), newValue);
+	}
+	
+	public boolean getToggle(SkeletonConfigToggle config) {
+		if(config == null) {
+			return false;
+		}
+		
+		// IMPORTANT!! This null check is necessary, getOrDefault seems to randomly decide to return null at times, so this is a secondary check
+		Boolean val = toggles.getOrDefault(config, config.defaultValue);
+		return val != null ? val : config.defaultValue;
+	}
+	
+	public boolean getToggle(String config) {
+		if(config == null) {
+			return false;
+		}
+		
+		return getToggle(SkeletonConfigToggle.getByStringValue(config));
+	}
+	
+	protected void setNodeOffset(SkeletonNodeOffset nodeOffset, float x, float y, float z) {
+		Vector3f offset = nodeOffsets.get(nodeOffset);
+		
+		if(offset == null) {
+			offset = new Vector3f(x, y, z);
+			nodeOffsets.put(nodeOffset, offset);
+		} else {
+			offset.set(x, y, z);
+		}
+		
+		if(callback != null) {
+			try {
+				callback.updateNodeOffset(nodeOffset, offset);
+			} catch(Exception e) {
+				LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e);
+			}
+		}
+	}
+	
+	protected void setNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset) {
+		if(offset == null) {
+			setNodeOffset(nodeOffset, 0f, 0f, 0f);
+			return;
+		}
+		
+		setNodeOffset(nodeOffset, offset.x, offset.y, offset.z);
+	}
+	
+	public Vector3f getNodeOffset(SkeletonNodeOffset nodeOffset) {
+		return nodeOffsets.getOrDefault(nodeOffset, Vector3f.ZERO);
+	}
+	
+	public void computeNodeOffset(SkeletonNodeOffset nodeOffset) {
+		switch(nodeOffset) {
+		case HEAD:
+			setNodeOffset(nodeOffset, 0, 0, getConfig(SkeletonConfigValue.HEAD));
+			break;
+		case NECK:
+			setNodeOffset(nodeOffset, 0, -getConfig(SkeletonConfigValue.NECK), 0);
+			break;
+		case CHEST:
+			setNodeOffset(nodeOffset, 0, -getConfig(SkeletonConfigValue.CHEST), 0);
+			break;
+		case WAIST:
+			setNodeOffset(nodeOffset, 0, (getConfig(SkeletonConfigValue.CHEST) - getConfig(SkeletonConfigValue.TORSO) + getConfig(SkeletonConfigValue.WAIST)), 0);
+			break;
+		case HIP:
+			setNodeOffset(nodeOffset, 0, -getConfig(SkeletonConfigValue.WAIST), 0);
+			break;
+		case HIP_TRACKER:
+			setNodeOffset(nodeOffset, 0, getConfig(SkeletonConfigValue.HIP_OFFSET), 0);
+			break;
+		
+		case LEFT_HIP:
+			setNodeOffset(nodeOffset, -getConfig(SkeletonConfigValue.HIPS_WIDTH) / 2f, 0, 0);
+			break;
+		case RIGHT_HIP:
+			setNodeOffset(nodeOffset, getConfig(SkeletonConfigValue.HIPS_WIDTH) / 2f, 0, 0);
+			break;
+		
+		case KNEE:
+			setNodeOffset(nodeOffset, 0, -(getConfig(SkeletonConfigValue.LEGS_LENGTH) - getConfig(SkeletonConfigValue.KNEE_HEIGHT)), 0);
+			break;
+		case ANKLE:
+			setNodeOffset(nodeOffset, 0, -getConfig(SkeletonConfigValue.KNEE_HEIGHT), -getConfig(SkeletonConfigValue.FOOT_OFFSET));
+			break;
+		case FOOT:
+			setNodeOffset(nodeOffset, 0, 0, -getConfig(SkeletonConfigValue.FOOT_LENGTH));
+			break;
+		}
+	}
+	
+	public void computeAllNodeOffsets() {
+		for(SkeletonNodeOffset offset : SkeletonNodeOffset.values) {
+			computeNodeOffset(offset);
+		}
+	}
+	
+	public void setConfigs(Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigToggle, Boolean> toggles) {
+		if(configs != null) {
+			configs.forEach((key, value) -> {
+				// Do not recalculate the offsets, these are done in bulk at the end
+				setConfig(key, value, false);
+			});
+		}
+		
+		if(toggles != null) {
+			toggles.forEach(this::setToggle);
+		}
+		
+		if(autoUpdateOffsets) {
+			computeAllNodeOffsets();
+		}
+	}
+	
+	public void setStringConfigs(Map<String, Float> configs, Map<String, Boolean> toggles) {
+		if(configs != null) {
+			configs.forEach((key, value) -> {
+				// Do not recalculate the offsets, these are done in bulk at the end
+				setConfig(SkeletonConfigValue.getByStringValue(key), value, false);
+			});
+		}
+		
+		if(toggles != null) {
+			toggles.forEach((key, value) -> {
+				setToggle(SkeletonConfigToggle.getByStringValue(key), value);
+			});
+		}
+		
+		if(autoUpdateOffsets) {
+			computeAllNodeOffsets();
+		}
+	}
+	
+	public void setConfigs(SkeletonConfig skeletonConfig) {
+		setConfigs(skeletonConfig.configs, skeletonConfig.toggles);
+	}
+	
+	//#region Cast utilities for config reading
+	private static Float castFloat(Object o) {
+		if(o == null) {
+			return null;
+		} else if(o instanceof Float) {
+			return (Float) o;
+		} else if(o instanceof Double) {
+			return ((Double) o).floatValue();
+		} else if(o instanceof Byte) {
+			return (float) (Byte) o;
+		} else if(o instanceof Integer) {
+			return (float) (Integer) o;
+		} else if(o instanceof Long) {
+			return (float) (Long) o;
+		} else {
+			return null;
+		}
+	}
+	
+	private static Boolean castBoolean(Object o) {
+		if(o == null) {
+			return null;
+		} else if(o instanceof Boolean) {
+			return (Boolean) o;
+		} else {
+			return null;
+		}
+	}
+	//#endregion
+	
+	public void loadFromConfig(YamlFile config) {
+		for(SkeletonConfigValue configValue : SkeletonConfigValue.values) {
+			Float val = castFloat(config.getProperty(configValue.configKey));
+			if(val != null) {
+				// Do not recalculate the offsets, these are done in bulk at the end
+				setConfig(configValue, val, false);
+			}
+		}
+		
+		for(SkeletonConfigToggle configValue : SkeletonConfigToggle.values) {
+			Boolean val = castBoolean(config.getProperty(configValue.configKey));
+			if(val != null) {
+				setToggle(configValue, val);
+			}
+		}
+		
+		if(autoUpdateOffsets) {
+			computeAllNodeOffsets();
+		}
+	}
+	
+	public void saveToConfig(YamlFile config) {
+		// Write all possible values, this keeps configs consistent even if defaults were changed
+		for(SkeletonConfigValue value : SkeletonConfigValue.values) {
+			config.setProperty(value.configKey, getConfig(value));
+		}
+		
+		for(SkeletonConfigToggle value : SkeletonConfigToggle.values) {
+			config.setProperty(value.configKey, getToggle(value));
+		}
+	}
+	
+	public void resetConfigs() {
+		configs.clear();
+		toggles.clear();
+		
+		callCallbackOnAll(false);
+		
+		if(autoUpdateOffsets) {
+			computeAllNodeOffsets();
+		}
+	}
+}

src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigCallback.java

@@ -0,0 +1,12 @@
+package dev.slimevr.vr.processor.skeleton;
+
+import com.jme3.math.Vector3f;
+
+public interface SkeletonConfigCallback {
+	
+	public void updateConfigState(SkeletonConfigValue config, float newValue);
+	
+	public void updateToggleState(SkeletonConfigToggle configToggle, boolean newValue);
+	
+	public void updateNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset);
+}

src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java

@@ -0,0 +1,38 @@
+package dev.slimevr.vr.processor.skeleton;
+
+import java.util.HashMap;
+import java.util.Map;
+
+public enum SkeletonConfigToggle {
+	
+	EXTENDED_PELVIS_MODEL("Extended pelvis model", "extendedPelvis", true),
+	EXTENDED_KNEE_MODEL("Extended knee model", "extendedKnee", false),
+	;
+	
+	private static final String CONFIG_PREFIX = "body.model.";
+	
+	public final String stringVal;
+	public final String configKey;
+	
+	public final boolean defaultValue;
+	
+	public static final SkeletonConfigToggle[] values = values();
+	private static final Map<String, SkeletonConfigToggle> byStringVal = new HashMap<>();
+	
+	private SkeletonConfigToggle(String stringVal, String configKey, boolean defaultValue) {
+		this.stringVal = stringVal;
+		this.configKey = CONFIG_PREFIX + configKey;
+		
+		this.defaultValue = defaultValue;
+	}
+	
+	public static SkeletonConfigToggle getByStringValue(String stringVal) {
+		return stringVal == null ? null : byStringVal.get(stringVal.toLowerCase());
+	}
+	
+	static {
+		for(SkeletonConfigToggle configVal : values()) {
+			byStringVal.put(configVal.stringVal.toLowerCase(), configVal);
+		}
+	}
+}

src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java

@@ -0,0 +1,53 @@
+package dev.slimevr.vr.processor.skeleton;
+
+import java.util.HashMap;
+import java.util.Map;
+
+public enum SkeletonConfigValue {
+	
+	HEAD("Head", "headShift", "Head shift", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HEAD}),
+	NECK("Neck", "neckLength", "Neck length", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.NECK}),
+	TORSO("Torso", "torsoLength", "Torso length", 0.64f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}),
+	CHEST("Chest", "chestDistance", "Chest distance", 0.32f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}),
+	WAIST("Waist", "waistDistance", "Waist distance", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST, SkeletonNodeOffset.HIP}),
+	HIP_OFFSET("Hip offset", "hipOffset", "Hip offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HIP_TRACKER}),
+	HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}),
+	LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.86f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}),
+	KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.43f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}),
+	FOOT_LENGTH("Foot length", "footLength", "Foot length", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.FOOT}),
+	FOOT_OFFSET("Foot offset", "footOffset", "Foot offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.ANKLE}),
+	;
+	
+	private static final String CONFIG_PREFIX = "body.";
+	
+	public final String stringVal;
+	public final String configKey;
+	public final String label;
+	
+	public final float defaultValue;
+	
+	public final SkeletonNodeOffset[] affectedOffsets;
+	
+	public static final SkeletonConfigValue[] values = values();
+	private static final Map<String, SkeletonConfigValue> byStringVal = new HashMap<>();
+	
+	private SkeletonConfigValue(String stringVal, String configKey, String label, float defaultValue, SkeletonNodeOffset[] affectedOffsets) {
+		this.stringVal = stringVal;
+		this.configKey = CONFIG_PREFIX + configKey;
+		this.label = label;
+		
+		this.defaultValue = defaultValue;
+		
+		this.affectedOffsets = affectedOffsets == null ? new SkeletonNodeOffset[0] : affectedOffsets;
+	}
+	
+	public static SkeletonConfigValue getByStringValue(String stringVal) {
+		return stringVal == null ? null : byStringVal.get(stringVal.toLowerCase());
+	}
+	
+	static {
+		for(SkeletonConfigValue configVal : values()) {
+			byStringVal.put(configVal.stringVal.toLowerCase(), configVal);
+		}
+	}
+}

src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonNodeOffset.java

@@ -0,0 +1,19 @@
+package dev.slimevr.vr.processor.skeleton;
+
+public enum SkeletonNodeOffset {
+	
+	HEAD,
+	NECK,
+	CHEST,
+	WAIST,
+	HIP,
+	HIP_TRACKER,
+	LEFT_HIP,
+	RIGHT_HIP,
+	KNEE,
+	ANKLE,
+	FOOT,
+	;
+	
+	public static final SkeletonNodeOffset[] values = values();
+}

src/main/java/dev/slimevr/vr/trackers/BnoTap.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 public class BnoTap {
 	

src/main/java/dev/slimevr/vr/trackers/CalibratingTracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import java.util.function.Consumer;
 

src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;

src/main/java/dev/slimevr/vr/trackers/DeviceType.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 public enum DeviceType {
 	HMD,

src/main/java/dev/slimevr/vr/trackers/HMDTracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import io.eiren.util.BufferedTimer;
 

src/main/java/dev/slimevr/vr/trackers/IMUTracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;

src/main/java/dev/slimevr/vr/trackers/MPUTracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import java.nio.ByteBuffer;
 

src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;

src/main/java/dev/slimevr/vr/trackers/ShareableTracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 public interface ShareableTracker extends Tracker {
 	

src/main/java/dev/slimevr/vr/trackers/Tracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import java.util.concurrent.atomic.AtomicInteger;
 

src/main/java/dev/slimevr/vr/trackers/TrackerConfig.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import com.jme3.math.Quaternion;
 

src/main/java/dev/slimevr/vr/trackers/TrackerMountingRotation.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;

src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import java.util.EnumMap;
 import java.util.HashMap;
@@ -10,6 +10,7 @@ public enum TrackerPosition {
 	HMD("HMD", TrackerRole.HMD),
 	CHEST("body:chest", TrackerRole.CHEST),
 	WAIST("body:waist", TrackerRole.WAIST),
+	HIP("body:hip", null),
 	LEFT_LEG("body:left_leg", TrackerRole.LEFT_KNEE),
 	RIGHT_LEG("body:right_leg", TrackerRole.RIGHT_KNEE),
 	LEFT_ANKLE("body:left_ankle", null),

src/main/java/dev/slimevr/vr/trackers/TrackerRole.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 public enum TrackerRole {
 	

src/main/java/dev/slimevr/vr/trackers/TrackerStatus.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 public enum TrackerStatus {
 	

src/main/java/dev/slimevr/vr/trackers/TrackerUtils.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import java.util.List;
 
@@ -8,6 +8,8 @@ public class TrackerUtils {
 	}
 
 	public static <T extends Tracker> T findTrackerForBodyPosition(T[] allTrackers, TrackerPosition position) {
+		if(position == null)
+			return null;
 		for(int i = 0; i < allTrackers.length; ++i) {
 			T t = allTrackers[i];
 			if(t != null && t.getBodyPosition() == position)
@@ -17,6 +19,8 @@ public class TrackerUtils {
 	}
 
 	public static <T extends Tracker> T findTrackerForBodyPosition(List<T> allTrackers, TrackerPosition position) {
+		if(position == null)
+			return null;
 		for(int i = 0; i < allTrackers.size(); ++i) {
 			T t = allTrackers.get(i);
 			if(t != null && t.getBodyPosition() == position)
@@ -32,18 +36,24 @@ public class TrackerUtils {
 		return findTrackerForBodyPosition(allTrackers, altPosition);
 	}
 
-	public static <T extends Tracker> T findTrackerForBodyPosition(T[] allTrackers, TrackerPosition position, TrackerPosition altPosition) {
+	public static <T extends Tracker> T findTrackerForBodyPosition(T[] allTrackers, TrackerPosition position, TrackerPosition altPosition, TrackerPosition secondAltPosition) {
 		T t = findTrackerForBodyPosition(allTrackers, position);
 		if(t != null)
 			return t;
-		return findTrackerForBodyPosition(allTrackers, altPosition);
+		t = findTrackerForBodyPosition(allTrackers, altPosition);
+		if(t != null)
+			return t;
+		return findTrackerForBodyPosition(allTrackers, secondAltPosition);
 	}
 
-	public static Tracker findTrackerForBodyPositionOrEmpty(List<? extends Tracker> allTrackers, TrackerPosition position, TrackerPosition altPosition) {
+	public static Tracker findTrackerForBodyPositionOrEmpty(List<? extends Tracker> allTrackers, TrackerPosition position, TrackerPosition altPosition, TrackerPosition secondAltPosition) {
 		Tracker t = findTrackerForBodyPosition(allTrackers, position);
 		if(t != null)
 			return t;
 		t = findTrackerForBodyPosition(allTrackers, altPosition);
+		if(t != null)
+			return t;
+		t = findTrackerForBodyPosition(allTrackers, secondAltPosition);
 		if(t != null)
 			return t;
 		return new ComputedTracker(Tracker.getNextLocalTrackerId(), "Empty tracker", false, false);

src/main/java/dev/slimevr/vr/trackers/TrackerWithBattery.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 public interface TrackerWithBattery {
 	

src/main/java/dev/slimevr/vr/trackers/TrackerWithTPS.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 public interface TrackerWithTPS {
 	

src/main/java/dev/slimevr/vr/trackers/TrackersUDPServer.java

@@ -1,14 +1,19 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import java.io.IOException;
 import java.io.UnsupportedEncodingException;
 import java.net.DatagramPacket;
 import java.net.DatagramSocket;
+import java.net.Inet6Address;
 import java.net.InetAddress;
+import java.net.InetSocketAddress;
+import java.net.NetworkInterface;
 import java.net.SocketAddress;
 import java.net.SocketTimeoutException;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
+import java.util.ArrayList;
+import java.util.Enumeration;
 import java.util.HashMap;
 import java.util.Iterator;
 import java.util.List;
@@ -146,9 +151,51 @@ public class TrackersUDPServer extends Thread {
 		StringBuilder serialBuffer2 = new StringBuilder();
 		try {
 			socket = new DatagramSocket(port);
+
+			// Why not just 255.255.255.255? Because Windows.
+			// https://social.technet.microsoft.com/Forums/windows/en-US/72e7387a-9f2c-4bf4-a004-c89ddde1c8aa/how-to-fix-the-global-broadcast-address-255255255255-behavior-on-windows
+			ArrayList<SocketAddress> addresses = new ArrayList<SocketAddress>();
+			Enumeration<NetworkInterface> ifaces = NetworkInterface.getNetworkInterfaces();
+			while (ifaces.hasMoreElements()) {
+				NetworkInterface iface = ifaces.nextElement();
+				// Ignore loopback, PPP, virtual and disabled devices
+				if (iface.isLoopback() || !iface.isUp() || iface.isPointToPoint() || iface.isVirtual()) {
+					continue;
+				}
+				Enumeration<InetAddress> iaddrs = iface.getInetAddresses();
+				while (iaddrs.hasMoreElements()) {
+					InetAddress iaddr = iaddrs.nextElement();
+					// Ignore IPv6 addresses
+					if (iaddr instanceof Inet6Address) {
+						continue;
+					}
+					String[] iaddrParts = iaddr.getHostAddress().split("\\.");
+					addresses.add(new InetSocketAddress(String.format("%s.%s.%s.255", iaddrParts[0], iaddrParts[1], iaddrParts[2]), port));
+				}
+			}
+			byte[] dummyPacket = new byte[] {0x0};
+
+			long prevPacketTime = System.currentTimeMillis();
 			socket.setSoTimeout(250);
 			while(true) {
 				try {
+					boolean hasActiveTrackers = false;
+					for (TrackerConnection tracker: trackers) {
+						if (tracker.sensors.get(0).getStatus() == TrackerStatus.OK) {
+							hasActiveTrackers = true;
+							break;
+						}
+					}
+					if (!hasActiveTrackers) {
+						long discoveryPacketTime = System.currentTimeMillis();
+						if ((discoveryPacketTime - prevPacketTime) >= 2000) {
+							for (SocketAddress addr: addresses) {
+								socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, addr));
+							}
+							prevPacketTime = discoveryPacketTime;
+						}
+					}
+
 					DatagramPacket recieve = new DatagramPacket(rcvBuffer, rcvBuffer.length);
 					socket.receive(recieve);
 					bb.rewind();

src/main/java/dev/slimevr/vr/trackers/VRTracker.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import io.eiren.util.BufferedTimer;
 

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

@@ -1,52 +0,0 @@
-package io.eiren.vr;
-
-import com.melloware.jintellitype.JIntellitype;
-import com.melloware.jintellitype.HotkeyListener;
-import io.eiren.util.ann.AWTThread;
-import io.eiren.util.logging.LogManager;
-
-public class Keybinding implements HotkeyListener {
-	public final VRServer server;
-	private static final int RESET = 1;
-	private static final int QUICK_RESET = 2;
-
-	@AWTThread
-	public Keybinding(VRServer server) {
-		this.server = server;
-
-		if(JIntellitype.isJIntellitypeSupported()) {
-			JIntellitype.getInstance().addHotKeyListener(this);
-
-			String resetBinding = this.server.config.getString("keybindings.reset");
-			if(resetBinding == null) {
-				resetBinding = "CTRL+ALT+SHIFT+Y";
-				this.server.config.setProperty("keybindings.reset", resetBinding);
-			}
-			JIntellitype.getInstance().registerHotKey(RESET, resetBinding);
-			LogManager.log.info("[Keybinding] Bound reset to " + resetBinding);
-
-			String quickResetBinding = this.server.config.getString("keybindings.quickReset");
-			if(quickResetBinding == null) {
-				quickResetBinding = "CTRL+ALT+SHIFT+U";
-				this.server.config.setProperty("keybindings.quickReset", quickResetBinding);
-			}
-			JIntellitype.getInstance().registerHotKey(QUICK_RESET, quickResetBinding);
-			LogManager.log.info("[Keybinding] Bound quick reset to " + quickResetBinding);
-		}
-	}
-
-	@AWTThread
-	@Override
-	public void onHotKey(int identifier) {
-		switch(identifier) {
-		case RESET:
-			LogManager.log.info("[Keybinding] Reset pressed");
-			server.resetTrackers();
-			break;
-		case QUICK_RESET:
-			LogManager.log.info("[Keybinding] Quick reset pressed");
-			server.resetTrackersYaw();
-			break;
-		}
-	}
-}

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

@@ -1,30 +0,0 @@
-package io.eiren.vr.processor;
-
-import java.util.Map;
-
-import io.eiren.util.ann.ThreadSafe;
-import io.eiren.util.ann.VRServerThread;
-
-public abstract class HumanSkeleton {
-
-	@VRServerThread
-	public abstract void updatePose();
-	
-	@ThreadSafe
-	public abstract TransformNode getRootNode();
-
-	@ThreadSafe
-	public abstract Map<String, Float> getSkeletonConfig();
-
-	@ThreadSafe
-	public abstract void setSkeletonConfig(String key, float newLength);
-	
-	@ThreadSafe
-	public abstract void resetSkeletonConfig(String joint);
-
-	@VRServerThread
-	public abstract void resetTrackersFull();
-	
-	@VRServerThread
-	public abstract void resetTrackersYaw();
-}

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

@@ -1,426 +0,0 @@
-package io.eiren.vr.processor;
-
-import java.util.List;
-
-import com.jme3.math.FastMath;
-import com.jme3.math.Quaternion;
-import com.jme3.math.Vector3f;
-
-import io.eiren.util.ann.VRServerThread;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerRole;
-import io.eiren.vr.trackers.TrackerStatus;
-import io.eiren.vr.trackers.TrackerUtils;
-
-public class HumanSkeletonWithLegs extends HumanSkeletonWithWaist {
-	
-	public static final float HIPS_WIDTH_DEFAULT = 0.3f;
-	public static final float FOOT_LENGTH_DEFAULT = 0.05f;
-	public static final float DEFAULT_FLOOR_OFFSET = 0.05f;
-	
-	protected final Quaternion hipBuf = new Quaternion();
-	protected final Quaternion kneeBuf = new Quaternion();
-	protected final Vector3f hipVector = new Vector3f();
-	protected final Vector3f ankleVector = new Vector3f();
-	protected final Quaternion kneeRotation = new Quaternion();
-	
-	protected final Tracker leftLegTracker;
-	protected final Tracker leftAnkleTracker;
-	protected final Tracker leftFootTracker;
-	protected final ComputedHumanPoseTracker computedLeftFootTracker;
-	protected final ComputedHumanPoseTracker computedLeftKneeTracker;
-	protected final Tracker rightLegTracker;
-	protected final Tracker rightAnkleTracker;
-	protected final Tracker rightFootTracker;
-	protected final ComputedHumanPoseTracker computedRightFootTracker;
-	protected final ComputedHumanPoseTracker computedRightKneeTracker;
-	
-	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 leftFootNode = new TransformNode("Left-Foot", 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);
-	protected final TransformNode rightFootNode = new TransformNode("Right-Foot", false);
-	
-	/**
-	 * Distance between centers of both hips
-	 */
-	protected float hipsWidth = HIPS_WIDTH_DEFAULT;
-	/**
-	 * Length from waist to knees
-	 */
-	protected float kneeHeight = 0.42f;
-	/**
-	 * Distance from waist to ankle
-	 */
-	protected float legsLength = 0.84f;
-	protected float footLength = FOOT_LENGTH_DEFAULT;
-	protected float footOffset = 0f; //horizontal forward/backwards translation feet offset for avatars with bent knees
-	
-	protected float minKneePitch = 0f * FastMath.DEG_TO_RAD;
-	protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD;
-	
-	protected float kneeLerpFactor = 0.5f;
-	
-	protected boolean extendedPelvisModel = true;
-	protected boolean extendedKneeModel = false;
-
-	public HumanSkeletonWithLegs(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
-		super(server, computedTrackers);
-		List<Tracker> allTracekrs = server.getAllTrackers();
-		this.leftLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTracekrs, TrackerPosition.LEFT_LEG, TrackerPosition.LEFT_ANKLE);
-		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTracekrs, TrackerPosition.LEFT_ANKLE, TrackerPosition.LEFT_LEG);
-		this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerPosition.LEFT_FOOT);
-		this.rightLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTracekrs, TrackerPosition.RIGHT_LEG, TrackerPosition.RIGHT_ANKLE);
-		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTracekrs, TrackerPosition.RIGHT_ANKLE, TrackerPosition.RIGHT_LEG);
-		this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerPosition.RIGHT_FOOT);
-		ComputedHumanPoseTracker lat = null;
-		ComputedHumanPoseTracker rat = null;
-		ComputedHumanPoseTracker rkt = null;
-		ComputedHumanPoseTracker lkt = null;
-		for(int i = 0; i < computedTrackers.size(); ++i) {
-			ComputedHumanPoseTracker t = computedTrackers.get(i);
-			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.LEFT_FOOT)
-				lat = t;
-			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.RIGHT_FOOT)
-				rat = t;
-			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.LEFT_KNEE)
-				lkt = t;
-			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.RIGHT_KNEE)
-				rkt = t;
-		}
-		if(lat == null)
-			lat = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_FOOT, TrackerRole.LEFT_FOOT);
-		if(rat == null)
-			rat = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_FOOT, TrackerRole.RIGHT_FOOT);
-		computedLeftFootTracker = lat;
-		computedRightFootTracker = rat;
-		computedLeftKneeTracker = lkt;
-		computedRightKneeTracker = rkt;
-		lat.setStatus(TrackerStatus.OK);
-		rat.setStatus(TrackerStatus.OK);
-		hipsWidth = server.config.getFloat("body.hipsWidth", hipsWidth);
-		kneeHeight = server.config.getFloat("body.kneeHeight", kneeHeight);
-		legsLength = server.config.getFloat("body.legsLength", legsLength);
-		footLength = server.config.getFloat("body.footLength", footLength);
-		footOffset = server.config.getFloat("body.footOffset", footOffset);
-		//extendedPelvisModel = server.config.getBoolean("body.model.extendedPelvis", extendedPelvisModel);
-		extendedKneeModel = server.config.getBoolean("body.model.extendedKnee", extendedKneeModel);
-		
-		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, -footOffset);
-		
-		rightKneeNode.attachChild(rightAnkleNode);
-		rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset);
-
-		leftAnkleNode.attachChild(leftFootNode);
-		leftFootNode.localTransform.setTranslation(0, 0, -footLength);
-		
-		rightAnkleNode.attachChild(rightFootNode);
-		rightFootNode.localTransform.setTranslation(0, 0, -footLength);
-		
-		configMap.put("Hips width", hipsWidth);
-		configMap.put("Legs length", legsLength);
-		configMap.put("Knee height", kneeHeight);
-		configMap.put("Foot length", footLength);
-		configMap.put("Foot offset", footOffset);
-	}
-	
-	@Override
-	public void resetSkeletonConfig(String joint) {
-		super.resetSkeletonConfig(joint);
-		switch(joint) {
-		case "All":
-			// Resets from the parent already performed
-			resetSkeletonConfig("Hips width");
-			resetSkeletonConfig("Foot length");
-			resetSkeletonConfig("Foot offset");
-			resetSkeletonConfig("Legs length");
-			break;
-		case "Hips width":
-			setSkeletonConfig(joint, HIPS_WIDTH_DEFAULT);
-			break;
-		case "Foot length":
-			setSkeletonConfig(joint, FOOT_LENGTH_DEFAULT);
-			break;
-		case "Foot offset":
-			setSkeletonConfig(joint, 0f);
-			break;
-		case "Legs length": // Set legs length to be 5cm above floor level
-			Vector3f vec = new Vector3f();
-			hmdTracker.getPosition(vec);
-			float height = vec.y;
-			if(height > 0.5f) { // Reset only if floor level is right, todo: read floor level from SteamVR if it's not 0
-				setSkeletonConfig(joint, height - neckLength - waistDistance - DEFAULT_FLOOR_OFFSET);
-			}
-			resetSkeletonConfig("Knee height");
-			break;
-		case "Knee height": // Knees are at 50% of the legs by default
-			setSkeletonConfig(joint, legsLength / 2.0f);
-			break;
-		}
-	}
-	
-	@Override
-	public void setSkeletonConfig(String joint, float newLength) {
-		super.setSkeletonConfig(joint, newLength);
-		switch(joint) {
-		case "Hips width":
-			hipsWidth = newLength;
-			server.config.setProperty("body.hipsWidth", hipsWidth);
-			leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
-			rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
-			break;
-		case "Knee height":
-			kneeHeight = newLength;
-			server.config.setProperty("body.kneeHeight", kneeHeight);
-			leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset);
-			rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset);
-			leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
-			rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
-			break;
-		case "Legs length":
-			legsLength = newLength;
-			server.config.setProperty("body.legsLength", legsLength);
-			leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
-			rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
-			break;
-		case "Foot length":
-			footLength = newLength;
-			server.config.setProperty("body.footLength", footLength);
-			leftFootNode.localTransform.setTranslation(0, 0, -footLength);
-			rightFootNode.localTransform.setTranslation(0, 0, -footLength);
-			break;
-		case "Foot offset":
-			footOffset = newLength;
-			server.config.setProperty("body.footOffset", footOffset);
-			leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset);
-			rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset);
-			break;
-		}
-	}
-	
-	@Override
-	public boolean getSkeletonConfigBoolean(String config) {
-		switch(config) {
-		case "Extended pelvis model":
-			return extendedPelvisModel;
-		case "Extended knee model":
-			return extendedKneeModel;
-		}
-		return super.getSkeletonConfigBoolean(config);
-	}
-	
-	@Override
-	public void setSkeletonConfigBoolean(String config, boolean newState) {
-		switch(config) {
-		case "Extended pelvis model":
-			extendedPelvisModel = newState;
-			server.config.setProperty("body.model.extendedPelvis", newState);
-			break;
-		case "Extended knee model":
-			extendedKneeModel = newState;
-			server.config.setProperty("body.model.extendedKnee", newState);
-			break;
-		default:
-			super.setSkeletonConfigBoolean(config, newState);
-			break;
-		}
-	}
-	
-	@Override
-	public void updateLocalTransforms() {
-		super.updateLocalTransforms();
-		// Left Leg
-		leftLegTracker.getRotation(hipBuf);
-		leftAnkleTracker.getRotation(kneeBuf);
-
-		if(extendedKneeModel)
-			calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
-		
-		leftHipNode.localTransform.setRotation(hipBuf);
-		leftKneeNode.localTransform.setRotation(kneeBuf);
-		leftAnkleNode.localTransform.setRotation(kneeBuf);
-		leftFootNode.localTransform.setRotation(kneeBuf);
-
-		if(leftFootTracker != null) {
-			leftFootTracker.getRotation(kneeBuf);
-			leftAnkleNode.localTransform.setRotation(kneeBuf);
-			leftFootNode.localTransform.setRotation(kneeBuf);
-		}
-		
-		// Right Leg
-		rightLegTracker.getRotation(hipBuf);
-		rightAnkleTracker.getRotation(kneeBuf);
-		
-		if(extendedKneeModel)
-			calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
-		
-		rightHipNode.localTransform.setRotation(hipBuf);
-		rightKneeNode.localTransform.setRotation(kneeBuf);
-		rightAnkleNode.localTransform.setRotation(kneeBuf);
-		rightFootNode.localTransform.setRotation(kneeBuf);
-		
-		if(rightFootTracker != null) {
-			rightFootTracker.getRotation(kneeBuf);
-			rightAnkleNode.localTransform.setRotation(kneeBuf);
-			rightFootNode.localTransform.setRotation(kneeBuf);
-		}
-
-		if(extendedPelvisModel) {
-			// Average pelvis between two legs
-			leftHipNode.localTransform.getRotation(hipBuf);
-			rightHipNode.localTransform.getRotation(kneeBuf);
-			kneeBuf.nlerp(hipBuf, 0.5f);
-			chestNode.localTransform.getRotation(hipBuf);
-			kneeBuf.nlerp(hipBuf, 0.3333333f);
-			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
-		}
-	}
-	
-	// Knee basically has only 1 DoF (pitch), average yaw and roll between knee and hip
-	protected void calculateKneeLimits(Quaternion hipBuf, Quaternion kneeBuf, float hipConfidense, float kneeConfidense) {
-		ankleVector.set(0, -1, 0);
-		hipVector.set(0, -1, 0);
-		hipBuf.multLocal(hipVector);
-		kneeBuf.multLocal(ankleVector);
-		kneeRotation.angleBetweenVectors(hipVector, ankleVector); // Find knee angle
-		
-		// Substract knee angle from knee rotation. With perfect leg and perfect
-		// sensors result should match hip rotation perfectly
-		kneeBuf.multLocal(kneeRotation.inverse());
-		
-		// Average knee and hip with a slerp
-		hipBuf.slerp(kneeBuf, 0.5f); // TODO : Use confidence to calculate changeAmt
-		kneeBuf.set(hipBuf);
-
-		// Return knee angle into knee rotation
-		kneeBuf.multLocal(kneeRotation);
-	}
-	
-	public static float normalizeRad(float angle) {
-		return FastMath.normalize(angle, -FastMath.PI, FastMath.PI);
-	}
-	
-	public static float interpolateRadians(float factor, float start, float end) {
-		float angle = Math.abs(end - start);
-		if(angle > FastMath.PI) {
-			if(end > start) {
-				start += FastMath.TWO_PI;
-			} else {
-				end += FastMath.TWO_PI;
-			}
-		}
-		float val = start + (end - start) * factor;
-		return normalizeRad(val);
-	}
-	
-	@Override
-	protected void updateComputedTrackers() {
-		super.updateComputedTrackers();
-		
-		if(computedLeftFootTracker != null) {
-			computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation());
-			computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation());
-			computedLeftFootTracker.dataTick();
-		}
-		
-		if(computedLeftKneeTracker != null) {
-			computedLeftKneeTracker.position.set(leftKneeNode.worldTransform.getTranslation());
-			computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation());
-			computedLeftKneeTracker.dataTick();
-		}
-		
-		if(computedRightFootTracker != null) {
-			computedRightFootTracker.position.set(rightFootNode.worldTransform.getTranslation());
-			computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation());
-			computedRightFootTracker.dataTick();
-		}
-		
-		if(computedRightKneeTracker != null) {
-			computedRightKneeTracker.position.set(rightKneeNode.worldTransform.getTranslation());
-			computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation());
-			computedRightKneeTracker.dataTick();
-		}
-	}
-	
-	@Override
-	@VRServerThread
-	public void resetTrackersFull() {
-		// Each tracker uses the tracker before it to adjust iteself,
-		// so trackers that don't need adjustments could be used too
-		super.resetTrackersFull();
-		// Start with waist, it was reset in the parent
-		Quaternion referenceRotation = new Quaternion();
-		this.waistTracker.getRotation(referenceRotation);
-		
-		this.leftLegTracker.resetFull(referenceRotation);
-		this.rightLegTracker.resetFull(referenceRotation);
-		this.leftLegTracker.getRotation(referenceRotation);
-		
-		this.leftAnkleTracker.resetFull(referenceRotation);
-		this.leftAnkleTracker.getRotation(referenceRotation);
-		
-		if(this.leftFootTracker != null) {
-			this.leftFootTracker.resetFull(referenceRotation);
-		}
-
-		this.rightLegTracker.getRotation(referenceRotation);
-		
-		this.rightAnkleTracker.resetFull(referenceRotation);
-		this.rightAnkleTracker.getRotation(referenceRotation);
-		
-		if(this.rightFootTracker != null) {
-			this.rightFootTracker.resetFull(referenceRotation);
-		}
-	}
-	
-	@Override
-	@VRServerThread
-	public void resetTrackersYaw() {
-		// Each tracker uses the tracker before it to adjust iteself,
-		// so trackers that don't need adjustments could be used too
-		super.resetTrackersYaw();
-		// Start with waist, it was reset in the parent
-		Quaternion referenceRotation = new Quaternion();
-		this.waistTracker.getRotation(referenceRotation);
-		
-		this.leftLegTracker.resetYaw(referenceRotation);
-		this.rightLegTracker.resetYaw(referenceRotation);
-		this.leftLegTracker.getRotation(referenceRotation);
-		
-		this.leftAnkleTracker.resetYaw(referenceRotation);
-		this.leftAnkleTracker.getRotation(referenceRotation);
-		
-		if(this.leftFootTracker != null) {
-			this.leftFootTracker.resetYaw(referenceRotation);
-		}
-
-		this.rightLegTracker.getRotation(referenceRotation);
-		
-		this.rightAnkleTracker.resetYaw(referenceRotation);
-		this.rightAnkleTracker.getRotation(referenceRotation);
-		
-		if(this.rightFootTracker != null) {
-			this.rightFootTracker.resetYaw(referenceRotation);
-		}
-	}
-}

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

@@ -1,261 +0,0 @@
-package io.eiren.vr.processor;
-
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
-
-import com.jme3.math.Quaternion;
-import com.jme3.math.Vector3f;
-
-import io.eiren.util.ann.VRServerThread;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.trackers.HMDTracker;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerPosition;
-import io.eiren.vr.trackers.TrackerStatus;
-import io.eiren.vr.trackers.TrackerUtils;
-
-public class HumanSkeletonWithWaist extends HumanSkeleton {
-	
-	public static final float HEAD_SHIFT_DEFAULT = 0.1f;
-	public static final float NECK_LENGTH_DEFAULT = 0.1f;
-	
-	protected final Map<String, Float> configMap = new HashMap<>();
-	protected final VRServer server;
-	
-	protected final float[] waistAngles = new float[3];
-	protected final Quaternion qBuf = new Quaternion();
-	protected final Vector3f vBuf = new Vector3f();
-	
-	protected final Tracker waistTracker;
-	protected final Tracker chestTracker;
-	protected final HMDTracker hmdTracker;
-	protected final ComputedHumanPoseTracker computedWaistTracker;
-	protected final ComputedHumanPoseTracker computedChestTracker;
-	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 final TransformNode trackerWaistNode = new TransformNode("Waist-Tracker", false);
-	
-	protected float chestDistance = 0.42f;
-	/**
-	 * Distance from eyes to waist
-	 */
-	protected float waistDistance = 0.85f;
-	/**
-	 * Distance from eyes to waist, defines reported
-	 * tracker position, if you want to move resulting
-	 * tracker up or down from actual waist
-	 */
-	protected float trackerWaistDistance = 0.0f;
-	/**
-	 * Distance from eyes to the base of the neck
-	 */
-	protected float neckLength = NECK_LENGTH_DEFAULT;
-	/**
-	 * Distance from eyes to ear
-	 */
-	protected float headShift = HEAD_SHIFT_DEFAULT;
-	
-	public HumanSkeletonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
-		List<Tracker> allTracekrs = server.getAllTrackers();
-		this.waistTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTracekrs, TrackerPosition.WAIST, TrackerPosition.CHEST);
-		this.chestTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTracekrs, TrackerPosition.CHEST, TrackerPosition.WAIST);
-		this.hmdTracker = server.hmdTracker;
-		this.server = server;
-		ComputedHumanPoseTracker cwt = null;
-		ComputedHumanPoseTracker cct = null;
-		for(int i = 0; i < computedTrackers.size(); ++i) {
-			ComputedHumanPoseTracker t = computedTrackers.get(i);
-			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.WAIST)
-				cwt = t;
-			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.CHEST)
-				cct = t;
-		}
-		computedWaistTracker = cwt;
-		computedChestTracker = cct;
-		cwt.setStatus(TrackerStatus.OK);
-		headShift = server.config.getFloat("body.headShift", headShift);
-		neckLength = server.config.getFloat("body.neckLength", neckLength);
-		chestDistance = server.config.getFloat("body.chestDistance", chestDistance);
-		waistDistance = server.config.getFloat("body.waistDistance", waistDistance);
-		trackerWaistDistance = server.config.getFloat("body.trackerWaistDistance", trackerWaistDistance);
-		// Build skeleton
-		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);
-		
-		chestNode.attachChild(trackerWaistNode);
-		trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
-		
-		configMap.put("Head", headShift);
-		configMap.put("Neck", neckLength);
-		configMap.put("Chest", chestDistance);
-		configMap.put("Waist", waistDistance);
-		configMap.put("Virtual waist", trackerWaistDistance);
-	}
-	
-	@Override
-	public void resetSkeletonConfig(String joint) {
-		switch(joint) {
-		case "All": // Reset all joints according to height
-			resetSkeletonConfig("Head");
-			resetSkeletonConfig("Neck");
-			resetSkeletonConfig("Virtual waist");
-			resetSkeletonConfig("Waist");
-			resetSkeletonConfig("Chest");
-			break;
-		case "Head":
-			setSkeletonConfig(joint, HEAD_SHIFT_DEFAULT);
-			break;
-		case "Neck":
-			setSkeletonConfig(joint, NECK_LENGTH_DEFAULT);
-			break;
-		case "Virtual waist":
-			setSkeletonConfig(joint, 0.0f);
-			break;
-		case "Chest":
-			setSkeletonConfig(joint, waistDistance / 2.0f);
-			break;
-		case "Waist": // Puts Waist in the middle of the height
-			Vector3f vec = new Vector3f();
-			hmdTracker.getPosition(vec);
-			float height = vec.y;
-			if(height > 0.5f) { // Reset only if floor level is right, todo: read floor level from SteamVR if it's not 0
-				setSkeletonConfig(joint, (height) / 2.0f);
-			}
-			break;
-		}
-	}
-	
-	@Override
-	public Map<String, Float> getSkeletonConfig() {
-		return configMap;
-	}
-	
-	@Override
-	public void setSkeletonConfig(String joint, float newLength) {
-		configMap.put(joint, newLength);
-		switch(joint) {
-		case "Head":
-			headShift = newLength;
-			server.config.setProperty("body.headShift", headShift);
-			headNode.localTransform.setTranslation(0, 0, headShift);
-			break;
-		case "Neck":
-			neckLength = newLength;
-			server.config.setProperty("body.neckLength", neckLength);
-			neckNode.localTransform.setTranslation(0, -neckLength, 0);
-			break;
-		case "Waist":
-			waistDistance = newLength;
-			server.config.setProperty("body.waistDistance", waistDistance);
-			waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0);
-			trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
-			break;
-		case "Chest":
-			chestDistance = newLength;
-			server.config.setProperty("body.chestDistance", chestDistance);
-			chestNode.localTransform.setTranslation(0, -chestDistance, 0);
-			waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0);
-			trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
-			break;
-		case "Virtual waist":
-			trackerWaistDistance = newLength;
-			server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance);
-			trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
-			break;
-		}
-	}
-	
-	public boolean getSkeletonConfigBoolean(String config) {
-		return false;
-	}
-	
-	public void setSkeletonConfigBoolean(String config, boolean newState) {
-	}
-	
-	@Override
-	public TransformNode getRootNode() {
-		return hmdNode;
-	}
-	
-	@Override
-	@VRServerThread
-	public void updatePose() {
-		updateLocalTransforms();
-		hmdNode.update();
-		updateComputedTrackers();
-	}
-	
-	protected void updateLocalTransforms() {
-		if(hmdTracker.getPosition(vBuf)) {
-			hmdNode.localTransform.setTranslation(vBuf);
-		}
-		if(hmdTracker.getRotation(qBuf)) {
-			hmdNode.localTransform.setRotation(qBuf);
-			headNode.localTransform.setRotation(qBuf);
-		}
-		
-		if(chestTracker.getRotation(qBuf))
-			neckNode.localTransform.setRotation(qBuf);
-		
-		if(waistTracker.getRotation(qBuf)) {
-			trackerWaistNode.localTransform.setRotation(qBuf);
-			chestNode.localTransform.setRotation(qBuf);
-			waistNode.localTransform.setRotation(qBuf);
-		}
-	}
-	
-	protected void updateComputedTrackers() {
-		if(computedWaistTracker != null) {
-			computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation());
-			computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation());
-			computedWaistTracker.dataTick();
-		}
-		
-		if(computedChestTracker != null) {
-			computedChestTracker.position.set(chestNode.worldTransform.getTranslation());
-			computedChestTracker.rotation.set(neckNode.worldTransform.getRotation());
-			computedChestTracker.dataTick();
-		}
-	}
-	
-	@Override
-	@VRServerThread
-	public void resetTrackersFull() {
-		// Each tracker uses the tracker before it to adjust iteself,
-		// so trackers that don't need adjustments could be used too
-		Quaternion referenceRotation = new Quaternion();
-		server.hmdTracker.getRotation(referenceRotation);
-		
-		this.chestTracker.resetFull(referenceRotation);
-		this.chestTracker.getRotation(referenceRotation);
-		
-		this.waistTracker.resetFull(referenceRotation);
-	}
-	
-	@Override
-	@VRServerThread
-	public void resetTrackersYaw() {
-		// Each tracker uses the tracker before it to adjust iteself,
-		// so trackers that don't need adjustments could be used too
-		Quaternion referenceRotation = new Quaternion();
-		server.hmdTracker.getRotation(referenceRotation);
-		
-		this.chestTracker.resetYaw(referenceRotation);
-		this.chestTracker.getRotation(referenceRotation);
-		
-		this.waistTracker.resetYaw(referenceRotation);
-	}
-}

src/test/java/dev/slimevr/unit/ReferenceAdjustmentsTests.java

@@ -1,14 +1,14 @@
-package io.eiren.unit;
+package dev.slimevr.unit;
 
 import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 
+import dev.slimevr.vr.processor.TransformNode;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.ReferenceAdjustedTracker;
+import dev.slimevr.vr.trackers.Tracker;
 import io.eiren.math.FloatMath;
 import io.eiren.util.StringUtils;
-import io.eiren.vr.processor.TransformNode;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.ReferenceAdjustedTracker;
-import io.eiren.vr.trackers.Tracker;
 
 import static org.junit.jupiter.api.Assertions.*;
 import static org.junit.jupiter.api.DynamicTest.dynamicTest;
@@ -57,7 +57,9 @@ public class ReferenceAdjustmentsTests {
 				));
 	}
 	
-	@TestFactory
+	// TODO : Test is not passing because the test is wrong
+	// See issue https://github.com/SlimeVR/SlimeVR-Server/issues/55
+	//@TestFactory
 	Stream<DynamicTest> getTestsForRotation() {
 		return getAnglesSet().map((p) -> 
 			IntStream.of(yaws).mapToObj((refYaw) ->