Bump version to 0.1.2

Add functional BVH recording

.classpath

@@ -26,8 +26,8 @@
 			<attribute name="test" value="true"/>
 		</attributes>
 	</classpathentry>
-	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.8/"/>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/jdk-11.0.1"/>
 	<classpathentry kind="con" path="org.eclipse.buildship.core.gradleclasspathcontainer"/>
-	<classpathentry combineaccessrules="false" kind="src" path="/Slime Java Commons"/>
+	<classpathentry combineaccessrules="false" kind="src" path="/slime-java-commons"/>
 	<classpathentry kind="output" path="bin/default"/>
 </classpath>

.editorconfig

@@ -0,0 +1,13 @@
+# This file is for unifying the coding style for different editors and IDEs
+# See editorconfig.org
+
+root = true
+
+[*]
+indent_style = tab
+end_of_line = lf
+charset = utf-8
+# This line causes problems with VSCode and potentially with other editors where all purely
+# whitespace lines are trimmed to be empty when saved, causing excessive worthless changes with Git
+#trim_trailing_whitespace = true
+insert_final_newline = true

.github/workflows/gradle.yml

@@ -1,48 +1,57 @@
 # This workflow will build a Java project with Gradle
 # For more information see: https://help.github.com/actions/language-and-framework-guides/building-and-testing-java-with-gradle
 
-name: Build SlimeVR Server with Gradle
+name: SlimeVR Server
 
-on:
-  push:
-    branches: [ main ]
-  pull_request:
-    branches: [ main ]
+on: [ push, pull_request ]
 
 jobs:
-  build:
-
+  test:
     runs-on: ubuntu-latest
 
     steps:
-    - uses: actions/checkout@v2.3.4
-    
-    - name: Clone Slime Java Commons
-      uses: actions/checkout@v2.3.4
-      with:
-        repository: Eirenliel/slime-java-commons
-        # Relative path under $GITHUB_WORKSPACE to place the repository
-        path: Slime Java Commons
-    
-    - name: Set up JDK 11
-      uses: actions/setup-java@v2.1.0
-      with:
-        java-version: '11'
-        distribution: 'adopt'
-        
-    - name: Grant execute permission for gradlew
-      run: chmod +x gradlew
-      
-    - name: Test with Gradle
-      run: ./gradlew clean test
-      
-    - name: Build with Gradle
-      run: ./gradlew clean serverJar
-      
-    - name: Upload the Server JAR as a Build Artifact
-      uses: actions/upload-artifact@v2.2.4
-      with:
-        # Artifact name
-        name: "SlimeVR-Server" # optional, default is artifact
-        # A file, directory or wildcard pattern that describes what to upload
-        path: build/libs/*
+      - uses: actions/checkout@v2.4.0
+        with:
+            submodules: recursive
+
+      - name: Set up JDK 11
+        uses: actions/setup-java@v2.4.0
+        with:
+          java-version: '11'
+          distribution: 'adopt'
+          cache: 'gradle' # will restore cache of dependencies and wrappers
+
+      - name: Grant execute permission for gradlew
+        run: chmod +x gradlew
+
+      - name: Test with Gradle
+        run: ./gradlew clean test
+
+  build:
+    runs-on: ubuntu-latest
+
+    steps:
+      - uses: actions/checkout@v2.4.0
+        with:
+            submodules: recursive
+
+      - name: Set up JDK 11
+        uses: actions/setup-java@v2.4.0
+        with:
+          java-version: '11'
+          distribution: 'adopt'
+          cache: 'gradle' # will restore cache of dependencies and wrappers
+
+      - name: Grant execute permission for gradlew
+        run: chmod +x gradlew
+
+      - name: Build with Gradle
+        run: ./gradlew clean shadowJar
+
+      - name: Upload the Server JAR as a Build Artifact
+        uses: actions/upload-artifact@v2.2.4
+        with:
+          # Artifact name
+          name: "SlimeVR-Server" # optional, default is artifact
+          # A file, directory or wildcard pattern that describes what to upload
+          path: build/libs/*

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "slime-java-commons"]
+	path = slime-java-commons
+	url = https://github.com/Eirenliel/slime-java-commons.git

LICENSE

@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2021 Eiren Rain
+Copyright (c) 2021 Eiren Rain, SlimeVR
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

README.md

@@ -13,4 +13,35 @@ Integrations:
 
 ## How to use
 
-Latest instructions are currently [here](https://gist.github.com/Eirenliel/8c0eefcdbda1076d5c2e1bf634831d20). Will be updated and republished as time goes on.
+It's recommended to download installer from here: https://github.com/SlimeVR/SlimeVR-Installer/releases/latest/download/slimevr_web_installer.exe
+
+Latest instructions are [on our site](https://docs.slimevr.dev/slimevr-setup.html).
+
+## How to build
+
+You need to execute these commands in the folder where you want this project.
+
+```bash
+# Clone repositories
+git clone --recursive https://github.com/SlimeVR/SlimeVR-Server.git
+
+# Enter the directory and build the runnable server JAR
+cd SlimeVR-Server
+gradlew shadowJar
+```
+
+Open Slime VR Server project in Eclipse or Intellij Idea
+
+run gradle command `shadowJar` to build a runnable server JAR
+
+## License Clarifications
+
+**SlimeVR software** (including server, firmware, drivers, installator, documents, and others - see licence for each case specifically) **is distributed under MIT License and is copyright of Eiren Rain and SlimeVR.** MIT Licence is a permissive license giving you rights to modify and distribute the software with little strings attached.
+
+**However, there are some limits, and if you wish to distribute software based on SlimeVR, you need to be aware of them:**
+
+* When distributing any software based on SlimeVR, you have to clarify to the end user that your software is based on SlimeVR that is distributed under MIT License and is subject to copyright of Eiren Rain
+* You must clarify either which parts of original software you're using, or what changes you did to the original software (i.e. clarify which parts of your software is covered by MIT License)
+* You must provide a copy of the original license (see LICENSE file)
+* You don't have to release your own software under MIT License or even open source at all, but you have to state that it's based on SlimeVR
+* This applies even if you distribute software without the source code

build.gradle

@@ -7,44 +7,73 @@
  */
 
 plugins {
-    // Apply the java-library plugin to add support for Java Library
-    id 'java-library'
+	id 'application'
+	id "com.github.johnrengelman.shadow" version "6.1.0"
 }
 
-repositories {
-    // Use jcenter for resolving dependencies.
-    // You can declare any Maven/Ivy/file repository here.
-    jcenter()
-    mavenCentral()
+sourceCompatibility = 1.8
+targetCompatibility = 1.8
+
+// Set compiler to use UTF-8
+compileJava.options.encoding = 'UTF-8'
+compileTestJava.options.encoding = 'UTF-8'
+javadoc.options.encoding = 'UTF-8'
+
+tasks.withType(JavaCompile) {
+	options.encoding = 'UTF-8'
+	if (JavaVersion.current().isJava9Compatible()) {
+		// TODO: Gradle 6.6
+		// options.release = 8
+		options.compilerArgs.addAll(['--release', '8'])
+	}
+}
+tasks.withType(Test) {
+	systemProperty('file.encoding', 'UTF-8')
+}
+tasks.withType(Javadoc) {
+	options.encoding = 'UTF-8'
+}
+
+allprojects {
+	repositories {
+		// Use jcenter for resolving dependencies.
+		// You can declare any Maven/Ivy/file repository here.
+		jcenter()
+		mavenCentral()
+	}
 }
 
 dependencies {
-	compile project(':Slime Java Commons')
+	compile project(':slime-java-commons')
 
-    // This dependency is exported to consumers, that is to say found on their compile classpath.
-    api 'org.apache.commons:commons-math3:3.6.1'
-    api 'org.yaml:snakeyaml:1.25'
-    api 'net.java.dev.jna:jna:5.6.0'
-    api 'net.java.dev.jna:jna-platform:5.6.0'
-    api 'com.illposed.osc:javaosc-core:0.8'
-    api 'com.fazecast:jSerialComm:[2.0.0,3.0.0)'
+	// This dependency is exported to consumers, that is to say found on their compile classpath.
+	compile 'org.apache.commons:commons-math3:3.6.1'
+	compile 'org.yaml:snakeyaml:1.25'
+	compile 'net.java.dev.jna:jna:5.6.0'
+	compile 'net.java.dev.jna:jna-platform:5.6.0'
+	compile 'com.illposed.osc:javaosc-core:0.8'
+	compile 'com.fazecast:jSerialComm:[2.0.0,3.0.0)'
+	compile 'com.google.protobuf:protobuf-java:3.17.3'
+	compile "org.java-websocket:Java-WebSocket:1.5.1"
+	compile 'com.melloware:jintellitype:1.4.0'
 
-    // This dependency is used internally, and not exposed to consumers on their own compile classpath.
-    implementation 'com.google.guava:guava:28.2-jre'
-    
+	// This dependency is used internally, and not exposed to consumers on their own compile classpath.
+	implementation 'com.google.guava:guava:28.2-jre'
 
-    // Use JUnit test framework
-    testImplementation 'junit:junit:4.12'
+	// Use JUnit test framework
+	testImplementation platform('org.junit:junit-bom:5.7.2')
+	testImplementation 'org.junit.jupiter:junit-jupiter'
+	testImplementation 'org.junit.platform:junit-platform-launcher'
+}
+test {
+	useJUnitPlatform()
 }
 
-subprojects.each { subproject -> evaluationDependsOn(subproject.path) }
-task serverJar (type: Jar, dependsOn: subprojects.tasks['build']) {
-    manifest {
-        attributes 'Main-Class': 'io.eiren.vr.Main'
-    }
-    
-    from {
-        configurations.compile.collect { it.isDirectory() ? it : zipTree(it) }
-    }
-    with jar
+shadowJar {
+	archiveBaseName.set('slimevr')
+	archiveClassifier.set('')
+	archiveVersion.set('')
+}
+application {
+	mainClassName = 'io.eiren.vr.Main'
 }

protobuf_update.bat

@@ -0,0 +1 @@
+protoc --proto_path=../SlimeVR-OpenVR-Driver/src/bridge --java_out=./src/main/java ProtobufMessages.proto

resources/LICENSE.txt

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2021 Eiren Rain, SlimeVR
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

resources/firewall.bat

@@ -0,0 +1,17 @@
+@echo off
+echo Installing firewall rules...
+
+rem Discovery defauly port
+netsh advfirewall firewall add rule name="SlimeVR UDP 35903 incoming" dir=in action=allow protocol=UDP localport=35903
+netsh advfirewall firewall add rule name="SlimeVR UDP 35903 outgoing" dir=out action=allow protocol=UDP localport=35903
+
+rem Rotational data default port
+netsh advfirewall firewall add rule name="SlimeVR UDP 6969 incoming" dir=in action=allow protocol=UDP localport=6969
+netsh advfirewall firewall add rule name="SlimeVR UDP 6969 outgoing" dir=out action=allow protocol=UDP localport=6969
+
+rem WebSocket server default port
+netsh advfirewall firewall add rule name="SlimeVR TCP 21110 incoming" dir=in action=allow protocol=TCP localport=21110
+netsh advfirewall firewall add rule name="SlimeVR TCP 21110 outgoing" dir=out action=allow protocol=TCP localport=21110
+
+echo Done!
+pause

resources/firewall_uninstall.bat

@@ -0,0 +1,17 @@
+@echo off
+echo Installing firewall rules...
+
+rem Discovery defauly port
+netsh advfirewall firewall delete rule name="SlimeVR UDP 35903 incoming"
+netsh advfirewall firewall delete rule name="SlimeVR UDP 35903 outgoing"
+
+rem Rotational data default port
+netsh advfirewall firewall delete rule name="SlimeVR UDP 6969 incoming"
+netsh advfirewall firewall delete rule name="SlimeVR UDP 6969 outgoing"
+
+rem WebSocket server default port
+netsh advfirewall firewall delete rule name="SlimeVR TCP 21110 incoming"
+netsh advfirewall firewall delete rule name="SlimeVR TCP 21110 outgoing"
+
+echo Done!
+pause

resources/run.bat

@@ -0,0 +1,19 @@
+@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
+)

settings.gradle

@@ -8,4 +8,4 @@
  */
 
 rootProject.name = 'SlimeVR Server'
-include('Slime Java Commons')
+include ':slime-java-commons'

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

@@ -0,0 +1,523 @@
+package dev.slimevr.autobone;
+
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Map.Entry;
+import java.util.function.Consumer;
+
+import com.jme3.math.Vector3f;
+
+import dev.slimevr.poserecorder.PoseFrames;
+import dev.slimevr.poserecorder.TrackerFrame;
+import dev.slimevr.poserecorder.TrackerFrameData;
+import io.eiren.util.ann.ThreadSafe;
+import io.eiren.util.logging.LogManager;
+import io.eiren.util.collections.FastList;
+import io.eiren.vr.VRServer;
+import 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 {
+	
+	public class Epoch {
+		
+		public final int epoch;
+		public final float epochError;
+		
+		public Epoch(int epoch, float epochError) {
+			this.epoch = epoch;
+			this.epochError = epochError;
+		}
+		
+		@Override
+		public String toString() {
+			return "Epoch: " + epoch + ", Epoch Error: " + epochError;
+		}
+	}
+	
+	public int cursorIncrement = 1;
+	
+	public int minDataDistance = 2;
+	public int maxDataDistance = 32;
+	
+	public int numEpochs = 5;
+	
+	public float initialAdjustRate = 2.5f;
+	public float adjustRateDecay = 1.01f;
+	
+	public float slideErrorFactor = 1.0f;
+	public float offsetErrorFactor = 0.0f;
+	public float proportionErrorFactor = 0.2f;
+	public float heightErrorFactor = 0.1f;
+	public float positionErrorFactor = 0.0f;
+	public float positionOffsetErrorFactor = 0.0f;
+	
+	// Human average is probably 1.1235 (SD 0.07)
+	public float legBodyRatio = 1.1235f;
+	// SD of 0.07, capture 68% within range
+	public float legBodyRatioRange = 0.07f;
+	
+	// Assume these to be approximately half
+	public float kneeLegRatio = 0.5f;
+	public float chestTorsoRatio = 0.5f;
+	
+	protected final VRServer server;
+	
+	protected HumanSkeletonWithLegs skeleton = null;
+	
+	// This is filled by reloadConfigValues()
+	public final HashMap<String, Float> configs = new HashMap<String, Float>();
+	public final HashMap<String, Float> staticConfigs = new HashMap<String, Float>();
+	
+	public final FastList<String> heightConfigs = new FastList<String>(new String[]{"Neck", "Torso", "Legs length"});
+	
+	public AutoBone(VRServer server) {
+		this.server = server;
+		
+		reloadConfigValues();
+		
+		server.addSkeletonUpdatedCallback(this::skeletonUpdated);
+	}
+	
+	public void reloadConfigValues() {
+		reloadConfigValues(null);
+	}
+	
+	public void reloadConfigValues(TrackerFrame[] frame) {
+		// Load torso 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("Torso", server.config.getFloat("body.torsoLength", 0.7f));
+		if(server.config.getBoolean("autobone.forceChestTracker", false) || (frame != null && TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.CHEST) != null) || TrackerUtils.findTrackerForBodyPosition(server.getAllTrackers(), TrackerPosition.CHEST) != null) {
+			// If force enabled or has a chest tracker
+			configs.put("Chest", server.config.getFloat("body.chestDistance", 0.35f));
+		} else {
+			// Otherwise, make sure it's not used
+			configs.remove("Chest");
+			staticConfigs.put("Chest", server.config.getFloat("body.chestDistance", 0.35f));
+		}
+		if(server.config.getBoolean("autobone.forceHipTracker", false) || (frame != null && TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.HIP) != null && TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.WAIST) != null) || (TrackerUtils.findTrackerForBodyPosition(server.getAllTrackers(), TrackerPosition.HIP) != null && TrackerUtils.findTrackerForBodyPosition(server.getAllTrackers(), TrackerPosition.WAIST) != null)) {
+			// If force enabled or has a hip tracker and waist tracker
+			configs.put("Waist", server.config.getFloat("body.waistDistance", 0.1f));
+		} else {
+			// Otherwise, make sure it's not used
+			configs.remove("Waist");
+			staticConfigs.put("Waist", server.config.getFloat("body.waistDistance", 0.1f));
+		}
+		
+		// Load leg configs
+		staticConfigs.put("Hips width", server.config.getFloat("body.hipsWidth", HumanSkeletonWithLegs.HIPS_WIDTH_DEFAULT));
+		configs.put("Legs length", server.config.getFloat("body.legsLength", 0.84f));
+		configs.put("Knee height", server.config.getFloat("body.kneeHeight", 0.42f));
+	}
+	
+	@ThreadSafe
+	public void skeletonUpdated(HumanSkeleton newSkeleton) {
+		if(newSkeleton instanceof HumanSkeletonWithLegs) {
+			skeleton = (HumanSkeletonWithLegs) newSkeleton;
+			applyConfigToSkeleton(newSkeleton);
+			LogManager.log.info("[AutoBone] Received updated skeleton");
+		}
+	}
+	
+	public void applyConfig() {
+		if(!applyConfigToSkeleton(skeleton)) {
+			// Unable to apply to skeleton, save directly
+			saveConfigs();
+		}
+	}
+	
+	public boolean applyConfigToSkeleton(HumanSkeleton skeleton) {
+		if(skeleton == null) {
+			return false;
+		}
+		
+		configs.forEach(skeleton::setSkeletonConfig);
+		
+		server.saveConfig();
+		
+		LogManager.log.info("[AutoBone] Configured skeleton bone lengths");
+		return true;
+	}
+	
+	private void setConfig(String name, String path) {
+		Float value = configs.get(name);
+		if(value != null) {
+			server.config.setProperty(path, value);
+		}
+	}
+	
+	// This doesn't require a skeleton, therefore can be used if skeleton is null
+	public void saveConfigs() {
+		setConfig("Head", "body.headShift");
+		setConfig("Neck", "body.neckLength");
+		setConfig("Waist", "body.waistDistance");
+		setConfig("Chest", "body.chestDistance");
+		setConfig("Torso", "body.torsoLength");
+		setConfig("Hips width", "body.hipsWidth");
+		setConfig("Legs length", "body.legsLength");
+		setConfig("Knee height", "body.kneeHeight");
+		
+		server.saveConfig();
+	}
+	
+	public Float getConfig(String config) {
+		Float configVal = configs.get(config);
+		return configVal != null ? configVal : staticConfigs.get(config);
+	}
+	
+	public Float getConfig(String config, Map<String, Float> configs, Map<String, Float> configsAlt) {
+		if(configs == null) {
+			throw new NullPointerException("Argument \"configs\" must not be null");
+		}
+		
+		Float configVal = configs.get(config);
+		return configVal != null || configsAlt == null ? configVal : configsAlt.get(config);
+	}
+	
+	public float getHeight(Map<String, Float> configs) {
+		return getHeight(configs, null);
+	}
+	
+	public float getHeight(Map<String, Float> configs, Map<String, Float> configsAlt) {
+		float height = 0f;
+		
+		for(String heightConfig : heightConfigs) {
+			Float length = getConfig(heightConfig, configs, configsAlt);
+			if(length != null) {
+				height += length;
+			}
+		}
+		
+		return height;
+	}
+	
+	public float getLengthSum(Map<String, Float> configs) {
+		float length = 0f;
+		
+		for(float boneLength : configs.values()) {
+			length += boneLength;
+		}
+		
+		return length;
+	}
+	
+	public float getMaxHmdHeight(PoseFrames frames) {
+		float maxHeight = 0f;
+		for(TrackerFrame[] frame : frames) {
+			TrackerFrame hmd = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.HMD);
+			if(hmd != null && hmd.hasData(TrackerFrameData.POSITION) && hmd.position.y > maxHeight) {
+				maxHeight = hmd.position.y;
+			}
+		}
+		return maxHeight;
+	}
+	
+	public void processFrames(PoseFrames frames) {
+		processFrames(frames, -1f);
+	}
+	
+	public void processFrames(PoseFrames frames, Consumer<Epoch> epochCallback) {
+		processFrames(frames, -1f, epochCallback);
+	}
+	
+	public void processFrames(PoseFrames frames, float targetHeight) {
+		processFrames(frames, true, targetHeight);
+	}
+	
+	public void processFrames(PoseFrames frames, float targetHeight, Consumer<Epoch> epochCallback) {
+		processFrames(frames, true, targetHeight, epochCallback);
+	}
+	
+	public float processFrames(PoseFrames frames, boolean calcInitError, float targetHeight) {
+		return processFrames(frames, calcInitError, targetHeight, null);
+	}
+	
+	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()];
+		
+		frames.getFrames(0, trackerBuffer1);
+		reloadConfigValues(trackerBuffer1); // Reload configs and detect chest tracker from the first frame
+		
+		final SimpleSkeleton skeleton2 = new SimpleSkeleton(configs, staticConfigs);
+		final TrackerFrame[] trackerBuffer2 = new TrackerFrame[frames.getTrackerCount()];
+		
+		// If target height isn't specified, auto-detect
+		if(targetHeight < 0f) {
+			if(skeleton != null) {
+				targetHeight = getHeight(skeleton.getSkeletonConfig());
+				LogManager.log.warning("[AutoBone] Target height loaded from skeleton (Make sure you reset before running!): " + targetHeight);
+			} else {
+				float hmdHeight = getMaxHmdHeight(frames);
+				if(hmdHeight <= 0.50f) {
+					LogManager.log.warning("[AutoBone] Max headset height detected (Value seems too low, did you not stand up straight while measuring?): " + hmdHeight);
+				} else {
+					LogManager.log.info("[AutoBone] Max headset height detected: " + hmdHeight);
+				}
+				
+				// Estimate target height from HMD height
+				targetHeight = hmdHeight;
+			}
+		}
+		
+		for(int epoch = calcInitError ? -1 : 0; epoch < numEpochs; epoch++) {
+			float sumError = 0f;
+			int errorCount = 0;
+			
+			float adjustRate = epoch >= 0 ? (float) (initialAdjustRate / Math.pow(adjustRateDecay, epoch)) : 0f;
+			
+			for(int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++) {
+				for(int frameCursor = 0; frameCursor < frameCount - cursorOffset; frameCursor += cursorIncrement) {
+					frames.getFrames(frameCursor, trackerBuffer1);
+					frames.getFrames(frameCursor + cursorOffset, trackerBuffer2);
+					
+					skeleton1.setSkeletonConfigs(configs);
+					skeleton2.setSkeletonConfigs(configs);
+					
+					skeleton1.setPoseFromFrame(trackerBuffer1);
+					skeleton2.setPoseFromFrame(trackerBuffer2);
+					
+					float totalLength = getLengthSum(configs);
+					float curHeight = getHeight(configs, staticConfigs);
+					float errorDeriv = getErrorDeriv(trackerBuffer1, trackerBuffer2, skeleton1, skeleton2, targetHeight - curHeight);
+					float error = errorFunc(errorDeriv);
+					
+					// In case of fire
+					if(Float.isNaN(error) || Float.isInfinite(error)) {
+						// Extinguish
+						LogManager.log.warning("[AutoBone] Error value is invalid, resetting variables to recover");
+						reloadConfigValues(trackerBuffer1);
+						
+						// Reset error sum values
+						sumError = 0f;
+						errorCount = 0;
+						
+						// Continue on new data
+						continue;
+					}
+					
+					// Store the error count for logging purposes
+					sumError += errorDeriv;
+					errorCount++;
+					
+					float adjustVal = error * adjustRate;
+					
+					for(Entry<String, Float> entry : configs.entrySet()) {
+						// Skip adjustment if the epoch is before starting (for logging only)
+						if(epoch < 0) {
+							break;
+						}
+						
+						float originalLength = entry.getValue();
+						
+						// Try positive and negative adjustments
+						boolean isHeightVar = heightConfigs.contains(entry.getKey());
+						float minError = errorDeriv;
+						float finalNewLength = -1f;
+						for(int i = 0; i < 2; i++) {
+							// Scale by the ratio for smooth adjustment and more stable results
+							float curAdjustVal = ((i == 0 ? adjustVal : -adjustVal) * originalLength) / totalLength;
+							float newLength = originalLength + curAdjustVal;
+							
+							// No small or negative numbers!!! Bad algorithm!
+							if(newLength < 0.01f) {
+								continue;
+							}
+							
+							updateSkeletonBoneLength(skeleton1, skeleton2, entry.getKey(), newLength);
+							
+							float newHeight = isHeightVar ? curHeight + curAdjustVal : curHeight;
+							float newErrorDeriv = getErrorDeriv(trackerBuffer1, trackerBuffer2, skeleton1, skeleton2, targetHeight - newHeight);
+							
+							if(newErrorDeriv < minError) {
+								minError = newErrorDeriv;
+								finalNewLength = newLength;
+							}
+						}
+						
+						if(finalNewLength > 0f) {
+							entry.setValue(finalNewLength);
+						}
+						
+						// Reset the length to minimize bias in other variables, it's applied later
+						updateSkeletonBoneLength(skeleton1, skeleton2, entry.getKey(), originalLength);
+					}
+				}
+			}
+			
+			// Calculate average error over the epoch
+			float avgError = errorCount > 0 ? sumError / errorCount : -1f;
+			LogManager.log.info("[AutoBone] Epoch " + (epoch + 1) + " average error: " + avgError);
+			
+			if(epochCallback != null) {
+				epochCallback.accept(new Epoch(epoch + 1, avgError));
+			}
+		}
+		
+		float finalHeight = getHeight(configs, staticConfigs);
+		LogManager.log.info("[AutoBone] Target height: " + targetHeight + " New height: " + finalHeight);
+		
+		return Math.abs(finalHeight - targetHeight);
+	}
+	
+	// The change in position of the ankle over time
+	protected float getSlideErrorDeriv(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) {
+		float slideLeft = skeleton1.getNodePosition(TrackerPosition.LEFT_ANKLE).distance(skeleton2.getNodePosition(TrackerPosition.LEFT_ANKLE));
+		float slideRight = skeleton1.getNodePosition(TrackerPosition.RIGHT_ANKLE).distance(skeleton2.getNodePosition(TrackerPosition.RIGHT_ANKLE));
+		
+		// Divide by 4 to halve and average, it's halved because you want to approach a midpoint, not the other point
+		return (slideLeft + slideRight) / 4f;
+	}
+	
+	// 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();
+		
+		float skeleton2Left = skeleton2.getNodePosition(TrackerPosition.LEFT_ANKLE).getY();
+		float skeleton2Right = skeleton2.getNodePosition(TrackerPosition.RIGHT_ANKLE).getY();
+		
+		float dist1 = Math.abs(skeleton1Left - skeleton1Right);
+		float dist2 = Math.abs(skeleton2Left - skeleton2Right);
+		
+		float dist3 = Math.abs(skeleton1Left - skeleton2Right);
+		float dist4 = Math.abs(skeleton2Left - skeleton1Right);
+		
+		float dist5 = Math.abs(skeleton1Left - skeleton2Left);
+		float dist6 = Math.abs(skeleton1Right - skeleton2Right);
+		
+		// Divide by 12 to halve and average, it's halved because you want to approach a midpoint, not the other point
+		return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f;
+	}
+	
+	// The distance from average human proportions
+	protected float getProportionErrorDeriv(SimpleSkeleton skeleton) {
+		Float neckLength = skeleton.getSkeletonConfig("Neck");
+		Float chestLength = skeleton.getSkeletonConfig("Chest");
+		Float torsoLength = skeleton.getSkeletonConfig("Torso");
+		Float legsLength = skeleton.getSkeletonConfig("Legs length");
+		Float kneeHeight = skeleton.getSkeletonConfig("Knee height");
+		
+		float chestTorso = chestLength != null && torsoLength != null ? Math.abs((chestLength / torsoLength) - chestTorsoRatio) : 0f;
+		float legBody = legsLength != null && torsoLength != null && neckLength != null ? Math.abs((legsLength / (torsoLength + neckLength)) - legBodyRatio) : 0f;
+		float kneeLeg = kneeHeight != null && legsLength != null ? Math.abs((kneeHeight / legsLength) - kneeLegRatio) : 0f;
+		
+		if(legBody <= legBodyRatioRange) {
+			legBody = 0f;
+		} else {
+			legBody -= legBodyRatioRange;
+		}
+		
+		return (chestTorso + legBody + kneeLeg) / 3f;
+	}
+	
+	// The distance of any points to the corresponding absolute position
+	protected float getPositionErrorDeriv(TrackerFrame[] frame, SimpleSkeleton skeleton) {
+		float offset = 0f;
+		int offsetCount = 0;
+		
+		for(TrackerFrame trackerFrame : frame) {
+			if(trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) {
+				continue;
+			}
+			
+			Vector3f nodePos = skeleton.getNodePosition(trackerFrame.designation.designation);
+			if(nodePos != null) {
+				offset += Math.abs(nodePos.distance(trackerFrame.position));
+				offsetCount++;
+			}
+		}
+		
+		return offsetCount > 0 ? offset / offsetCount : 0f;
+	}
+	
+	// The difference between offset of absolute position and the corresponding point over time
+	protected float getPositionOffsetErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2) {
+		float offset = 0f;
+		int offsetCount = 0;
+		
+		for(TrackerFrame trackerFrame1 : frame1) {
+			if(trackerFrame1 == null || !trackerFrame1.hasData(TrackerFrameData.POSITION)) {
+				continue;
+			}
+			
+			TrackerFrame trackerFrame2 = TrackerUtils.findTrackerForBodyPosition(frame2, trackerFrame1.designation);
+			if(trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) {
+				continue;
+			}
+			
+			Vector3f nodePos1 = skeleton1.getNodePosition(trackerFrame1.designation);
+			if(nodePos1 == null) {
+				continue;
+			}
+			
+			Vector3f nodePos2 = skeleton2.getNodePosition(trackerFrame2.designation);
+			if(nodePos2 == null) {
+				continue;
+			}
+			
+			float dist1 = Math.abs(nodePos1.distance(trackerFrame1.position));
+			float dist2 = Math.abs(nodePos2.distance(trackerFrame2.position));
+			
+			offset += Math.abs(dist2 - dist1);
+			offsetCount++;
+		}
+		
+		return offsetCount > 0 ? offset / offsetCount : 0f;
+	}
+	
+	protected float getErrorDeriv(TrackerFrame[] frame1, TrackerFrame[] frame2, SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, float heightChange) {
+		float totalError = 0f;
+		float sumWeight = 0f;
+		
+		if(slideErrorFactor > 0f) {
+			totalError += getSlideErrorDeriv(skeleton1, skeleton2) * slideErrorFactor;
+			sumWeight += slideErrorFactor;
+		}
+		
+		if(offsetErrorFactor > 0f) {
+			totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * offsetErrorFactor;
+			sumWeight += offsetErrorFactor;
+		}
+		
+		if(proportionErrorFactor > 0f) {
+			// Either skeleton will work fine, skeleton1 is used as a default
+			totalError += getProportionErrorDeriv(skeleton1) * proportionErrorFactor;
+			sumWeight += proportionErrorFactor;
+		}
+		
+		if(heightErrorFactor > 0f) {
+			totalError += Math.abs(heightChange) * heightErrorFactor;
+			sumWeight += heightErrorFactor;
+		}
+		
+		if(positionErrorFactor > 0f) {
+			totalError += (getPositionErrorDeriv(frame1, skeleton1) + getPositionErrorDeriv(frame2, skeleton2) / 2f) * positionErrorFactor;
+			sumWeight += positionErrorFactor;
+		}
+		
+		if(positionOffsetErrorFactor > 0f) {
+			totalError += getPositionOffsetErrorDeriv(frame1, frame2, skeleton1, skeleton2) * positionOffsetErrorFactor;
+			sumWeight += positionOffsetErrorFactor;
+		}
+		
+		// Minimize sliding, minimize foot height offset, minimize change in total height
+		return sumWeight > 0f ? totalError / sumWeight : 0f;
+	}
+	
+	// Mean square error function
+	protected static float errorFunc(float errorDeriv) {
+		return 0.5f * (errorDeriv * errorDeriv);
+	}
+	
+	protected void updateSkeletonBoneLength(SimpleSkeleton skeleton1, SimpleSkeleton skeleton2, String joint, float newLength) {
+		skeleton1.setSkeletonConfig(joint, newLength, true);
+		skeleton2.setSkeletonConfig(joint, newLength, true);
+	}
+}

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

@@ -0,0 +1,374 @@
+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 {
+	
+	// Upper body (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 waistNode = new TransformNode("Waist", false);
+	protected final TransformNode chestNode = new TransformNode("Chest", false);
+	protected final TransformNode hipNode = new TransformNode("Hip", false);
+	
+	/**
+	 * Distance from shoulders to chest
+	 */
+	protected float chestDistance = 0.35f;
+	/**
+	 * Distance from hip to waist
+	 */
+	protected float waistDistance = 0.1f;
+	/**
+	 * Distance from shoulder to hip
+	 */
+	protected float torsoLength = 0.7f;
+	/**
+	 * 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;
+	
+	// Lower body (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 hip to knees
+	 */
+	protected float kneeHeight = 0.42f;
+	/**
+	 * Distance from hip 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 hip
+		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, (chestDistance - torsoLength + waistDistance), 0);
+
+		waistNode.attachChild(hipNode);
+		hipNode.localTransform.setTranslation(0, -waistDistance, 0);
+		
+		// Assemble skeleton to feet
+		hipNode.attachChild(leftHipNode);
+		leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
+		
+		hipNode.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, TrackerPosition.HIP);
+		setRotation(chest, neckNode);
+		
+		TrackerFrame waist = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.WAIST, TrackerPosition.CHEST, TrackerPosition.HIP);
+		setRotation(waist, chestNode);
+		
+		TrackerFrame leftLeg = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.LEFT_LEG);
+		TrackerFrame rightLeg = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.RIGHT_LEG);
+		
+		TrackerFrame hip = TrackerUtils.findTrackerForBodyPosition(frame, TrackerPosition.HIP, TrackerPosition.WAIST, TrackerPosition.CHEST);
+		averagePelvis(hip, 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 hip, TrackerFrame leftLeg, TrackerFrame rightLeg) {
+		if((leftLeg == null || rightLeg == null) || (!leftLeg.hasData(TrackerFrameData.ROTATION) || !rightLeg.hasData(TrackerFrameData.ROTATION))) {
+			setRotation(hip, waistNode);
+			return;
+		}
+		if(hip == null || !hip.hasData(TrackerFrameData.ROTATION)) {
+			if(leftLeg.hasData(TrackerFrameData.ROTATION) && rightLeg.hasData(TrackerFrameData.ROTATION)) {
+				leftLeg.getRotation(rotBuf1);
+				rightLeg.getRotation(rotBuf2);
+				rotBuf1.nlerp(rotBuf2, 0.5f);
+				
+				hipNode.localTransform.setRotation(rotBuf1);
+			}
+			
+			return;
+		}
+		
+		// Average the pelvis with the waist rotation
+		leftLeg.getRotation(rotBuf1);
+		rightLeg.getRotation(rotBuf2);
+		rotBuf1.nlerp(rotBuf2, 0.5f);
+		hip.getRotation(rotBuf2);
+		rotBuf1.nlerp(rotBuf2, 0.3333333f);
+		
+		hipNode.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 "Torso":
+			torsoLength = newLength;
+			waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0);
+			if(updatePose) {
+				hipNode.update();
+			}
+			break;
+		case "Chest":
+			chestDistance = newLength;
+			chestNode.localTransform.setTranslation(0, -chestDistance, 0);
+			waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0);
+			if(updatePose) {
+				chestNode.update();
+			}
+			break;
+		case "Waist":
+			waistDistance = newLength;
+			waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0);
+			hipNode.localTransform.setTranslation(0, -waistDistance, 0);
+			if(updatePose) {
+				waistNode.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 "Torso":
+			return torsoLength;	
+		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 HIP:
+			return rotationNode ? waistNode : hipNode;
+		
+		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);
+		config.setProperty("body.torsoLength", torsoLength);
+		
+		// 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

@@ -0,0 +1,43 @@
+package dev.slimevr.bridge;
+
+import io.eiren.util.ann.VRServerThread;
+import io.eiren.vr.trackers.ShareableTracker;
+
+/**
+ * Bridge handles sending and recieving tracker data
+ * between SlimeVR and other systems like VR APIs (SteamVR, OpenXR, etc),
+ * apps and protocols (VMC, WebSocket, TIP). It can create and manage
+ * tracker recieved from the <b>remote side</b> or send shared <b>local
+ * trackers</b> to the other side.
+ */
+public interface Bridge {
+
+	@VRServerThread
+	public void dataRead();
+
+	@VRServerThread
+	public void dataWrite();
+	
+	/**
+	 * Adds shared tracker to the bridge. Bridge should notify the
+	 * other side of this tracker, if it's the type of tracker
+	 * this bridge serves, and start sending data each update
+	 * @param tracker
+	 */
+	@VRServerThread
+	public void addSharedTracker(ShareableTracker tracker);
+	
+	/**
+	 * Removes tracker from a bridge. If the other side supports
+	 * tracker removal, bridge should notify it and stop sending
+	 * new data. If it doesn't support tracker removal, the bridge
+	 * can either stop sending new data, or keep sending it if it's
+	 * available.
+	 * @param tracker
+	 */
+	@VRServerThread
+	public void removeSharedTracker(ShareableTracker tracker);
+
+	@VRServerThread
+	public void startBridge();
+}

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

@@ -0,0 +1,9 @@
+package dev.slimevr.bridge;
+
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+
+@Retention(value = RetentionPolicy.SOURCE)
+public @interface BridgeThread {
+	
+}

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

@@ -0,0 +1,224 @@
+package dev.slimevr.bridge;
+
+import java.io.IOException;
+import java.util.List;
+
+import com.google.protobuf.CodedOutputStream;
+import com.sun.jna.platform.win32.Kernel32;
+import com.sun.jna.platform.win32.WinBase;
+import com.sun.jna.platform.win32.WinError;
+import com.sun.jna.ptr.IntByReference;
+
+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 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 {
+	
+	private final TrackerRole[] defaultRoles = new TrackerRole[] {TrackerRole.WAIST, TrackerRole.LEFT_FOOT, TrackerRole.RIGHT_FOOT};
+
+	private final byte[] buffArray = new byte[2048];
+	
+	protected Pipe pipe;
+	protected final String pipeName;
+	protected final String bridgeSettingsKey;
+	protected final Thread runnerThread;
+	private final List<? extends ShareableTracker> shareableTrackers;
+	
+	public NamedPipeBridge(HMDTracker hmd, String bridgeSettingsKey, String bridgeName, String pipeName, List<? extends ShareableTracker> shareableTrackers) {
+		super(bridgeName, hmd);
+		this.pipeName = pipeName;
+		this.bridgeSettingsKey = bridgeSettingsKey;
+		this.runnerThread = new Thread(this, "Named pipe thread");
+		this.shareableTrackers = shareableTrackers;
+	}
+
+	@Override
+	@VRServerThread
+	public void startBridge() {
+		for(TrackerRole role : defaultRoles) {
+			changeShareSettings(role, Main.vrServer.config.getBoolean("bridge." + bridgeSettingsKey + ".trackers." + role.name().toLowerCase(), true));
+		}
+		for(int i = 0; i < shareableTrackers.size(); ++i) {
+			ShareableTracker tr = shareableTrackers.get(i);
+			TrackerRole role = tr.getTrackerRole();
+			changeShareSettings(role, Main.vrServer.config.getBoolean("bridge." + bridgeSettingsKey + ".trackers." + role.name().toLowerCase(), false));
+		}
+		runnerThread.start();
+	}
+
+	@VRServerThread
+	public boolean getShareSetting(TrackerRole role) {
+		for(int i = 0; i < shareableTrackers.size(); ++i) {
+			ShareableTracker tr = shareableTrackers.get(i);
+			if(tr.getTrackerRole() == role) {
+				return sharedTrackers.contains(tr);
+			}
+		}
+		return false;
+	}
+
+	@VRServerThread
+	public void changeShareSettings(TrackerRole role, boolean share) {
+		if(role == null)
+			return;
+		for(int i = 0; i < shareableTrackers.size(); ++i) {
+			ShareableTracker tr = shareableTrackers.get(i);
+			if(tr.getTrackerRole() == role) {
+				if(share) {
+					addSharedTracker(tr);
+				} else {
+					removeSharedTracker(tr);
+				}
+				Main.vrServer.config.setProperty("bridge." + bridgeSettingsKey + ".trackers." + role.name().toLowerCase(), share);
+				Main.vrServer.saveConfig();
+			}
+		}
+	}
+
+	@Override
+	@VRServerThread
+	protected VRTracker createNewTracker(TrackerAdded trackerAdded) {
+		VRTracker tracker = new VRTracker(trackerAdded.getTrackerId(), trackerAdded.getTrackerSerial(), trackerAdded.getTrackerName(), true, true);
+		TrackerRole role = TrackerRole.getById(trackerAdded.getTrackerRole());
+		if(role != null) {
+			tracker.setBodyPosition(TrackerPosition.getByRole(role));
+		}
+		return tracker;
+	}
+
+	@Override
+	@BridgeThread
+	public void run() {
+		try {
+			createPipe();
+			while(true) {
+				boolean pipesUpdated = false;
+				if(pipe.state == PipeState.CREATED) {
+					tryOpeningPipe(pipe);
+				}
+				if(pipe.state == PipeState.OPEN) {
+					pipesUpdated = updatePipe();
+					updateMessageQueue();
+				}
+				if(pipe.state == PipeState.ERROR) {
+					resetPipe();
+				}
+				if(!pipesUpdated) {
+					try {
+						Thread.sleep(5); // Up to 200Hz
+					} catch(InterruptedException e) {
+						e.printStackTrace();
+					}
+				}
+			}
+		} catch(IOException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	@BridgeThread
+	protected boolean sendMessageReal(ProtobufMessage message) {
+		if(pipe.state == PipeState.OPEN) {
+			try {
+		        int size = message.getSerializedSize();
+		        CodedOutputStream os = CodedOutputStream.newInstance(buffArray, 4, size);
+		        message.writeTo(os);
+		        size += 4;
+		        buffArray[0] = (byte) (size & 0xFF);
+		        buffArray[1] = (byte) ((size >> 8) & 0xFF);
+		        buffArray[2] = (byte) ((size >> 16) & 0xFF);
+		        buffArray[3] = (byte) ((size >> 24) & 0xFF);
+		        if(Kernel32.INSTANCE.WriteFile(pipe.pipeHandle, buffArray, size, null, null)) {
+		            return true;
+		        }
+		        pipe.state = PipeState.ERROR;
+		        LogManager.log.severe("[" + bridgeName + "] Pipe error: " + Kernel32.INSTANCE.GetLastError());
+			} catch(IOException e) {
+				e.printStackTrace();
+			}
+	    }
+		return false;
+	}
+	
+	private boolean updatePipe() throws IOException {
+		if(pipe.state == PipeState.OPEN) {
+			boolean readAnything = false;
+			IntByReference bytesAvailable = new IntByReference(0);
+			while(Kernel32.INSTANCE.PeekNamedPipe(pipe.pipeHandle, buffArray, 4, null, bytesAvailable, null)) {
+				if(bytesAvailable.getValue() >= 4) { // Got size
+					int messageLength = (buffArray[3] << 24) | (buffArray[2] << 16) | (buffArray[1] << 8) | buffArray[0];
+					if(messageLength > 1024) { // Overflow
+						LogManager.log.severe("[" + bridgeName + "] Pipe overflow. Message length: " + messageLength);
+						pipe.state = PipeState.ERROR;
+						return readAnything;
+					}
+					if(bytesAvailable.getValue() >= messageLength) {
+						if(Kernel32.INSTANCE.ReadFile(pipe.pipeHandle, buffArray, messageLength, bytesAvailable, null)) {
+							ProtobufMessage message = ProtobufMessage.parser().parseFrom(buffArray, 4, messageLength - 4);
+							messageRecieved(message);
+							readAnything = true;
+						} else {
+							pipe.state = PipeState.ERROR;
+							LogManager.log.severe("[" + bridgeName + "] Pipe error: " + Kernel32.INSTANCE.GetLastError());
+							return readAnything;
+						}
+					} else {
+						return readAnything; // Wait for more data
+					}
+				} else {
+					return readAnything; // Wait for more data
+				}
+			}
+			pipe.state = PipeState.ERROR;
+			LogManager.log.severe("[" + bridgeName + "] Pipe error: " + Kernel32.INSTANCE.GetLastError());
+		}
+		return false;
+	}
+	
+	private void resetPipe() {
+		Pipe.safeDisconnect(pipe);
+		pipe.state = PipeState.CREATED;
+		Main.vrServer.queueTask(this::disconnected);
+	}
+	
+	private void createPipe() throws IOException {
+		try {
+			pipe = new Pipe(Kernel32.INSTANCE.CreateNamedPipe(pipeName, WinBase.PIPE_ACCESS_DUPLEX, // dwOpenMode
+					WinBase.PIPE_TYPE_BYTE | WinBase.PIPE_READMODE_BYTE | WinBase.PIPE_WAIT, // dwPipeMode
+					1, // nMaxInstances,
+					1024 * 16, // nOutBufferSize,
+					1024 * 16, // nInBufferSize,
+					0, // nDefaultTimeOut,
+					null), pipeName); // lpSecurityAttributes
+			LogManager.log.info("[" + bridgeName + "] Pipe " + pipe.name + " created");
+			if(WinBase.INVALID_HANDLE_VALUE.equals(pipe.pipeHandle))
+				throw new IOException("Can't open " + pipeName + " pipe: " + Kernel32.INSTANCE.GetLastError());
+			LogManager.log.info("[" + bridgeName + "] Pipes are created");
+		} catch(IOException e) {
+			Pipe.safeDisconnect(pipe);
+			throw e;
+		}
+	}
+	
+	private boolean tryOpeningPipe(Pipe pipe) {
+		if(Kernel32.INSTANCE.ConnectNamedPipe(pipe.pipeHandle, null) || Kernel32.INSTANCE.GetLastError() == WinError.ERROR_PIPE_CONNECTED) {
+			pipe.state = PipeState.OPEN;
+			LogManager.log.info("[" + bridgeName + "] Pipe " + pipe.name + " is open");
+			Main.vrServer.queueTask(this::reconnected);
+			return true;
+		}
+		LogManager.log.info("[" + bridgeName + "] Error connecting to pipe " + pipe.name + ": " + Kernel32.INSTANCE.GetLastError());
+		return false;
+	}
+}

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

@@ -1,4 +1,4 @@
-package io.eiren.vr.bridge;
+package dev.slimevr.bridge;
 
 import java.io.IOException;
 import java.nio.charset.Charset;
@@ -12,28 +12,31 @@ import com.sun.jna.platform.win32.WinBase;
 import com.sun.jna.platform.win32.WinNT.HANDLE;
 import com.sun.jna.ptr.IntByReference;
 
+import dev.slimevr.bridge.Pipe.PipeState;
 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 VRBridge {
-	
+public class NamedPipeVRBridge extends Thread implements Bridge {
+
+	private static final int MAX_COMMAND_LENGTH = 2048;
 	public static final String HMDPipeName = "\\\\.\\pipe\\HMDPipe";
 	public static final String TrackersPipeName = "\\\\.\\pipe\\TrackPipe";
 	public static final Charset ASCII = Charset.forName("ASCII");
-	
-	private final byte[] buffer = new byte[1024];
+
+	private final byte[] buffArray = new byte[1024];
+	private final StringBuilder commandBuilder = new StringBuilder(1024);
 	private final StringBuilder sbBuffer = new StringBuilder(1024);
 	private final Vector3f vBuffer = new Vector3f();
 	private final Vector3f vBuffer2 = new Vector3f();
 	private final Quaternion qBuffer = new Quaternion();
 	private final Quaternion qBuffer2 = new Quaternion();
 	
-	private final VRServer server;
 	private Pipe hmdPipe;
 	private final HMDTracker hmd;
 	private final List<Pipe> trackerPipes;
@@ -43,41 +46,18 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	private final HMDTracker internalHMDTracker = new HMDTracker("itnernal://HMD");
 	private final AtomicBoolean newHMDData = new AtomicBoolean(false);
 	
-	private boolean spawnOneTracker = false;
-	
 	public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> shareTrackers, VRServer server) {
 		super("Named Pipe VR Bridge");
-		this.server = server;
 		this.hmd = hmd;
 		this.shareTrackers = new FastList<>(shareTrackers);
 		this.trackerPipes = new FastList<>(shareTrackers.size());
 		this.internalTrackers = new FastList<>(shareTrackers.size());
 		for(int i = 0; i < shareTrackers.size(); ++i) {
 			Tracker t = shareTrackers.get(i);
-			ComputedTracker ct = new ComputedTracker("internal://" + t.getName());
+			ComputedTracker ct = new ComputedTracker(t.getTrackerId(), "internal://" + t.getName(), true, true);
 			ct.setStatus(TrackerStatus.OK);
 			this.internalTrackers.add(ct);
 		}
-		this.spawnOneTracker = server.config.getBoolean("openvr.onetracker", spawnOneTracker);
-	}
-	
-	public boolean isOneTrackerMode() {
-		return this.spawnOneTracker;
-	}
-	
-	/**
-	 * Makes OpenVR bridge spawn only 1 tracker instead of 3, for
-	 * use with only waist/chest tracking. Requires restart.
-	 */
-	public void setSpawnOneTracker(boolean spawnOneTracker) {
-		if(spawnOneTracker == this.spawnOneTracker)
-			return;
-		this.spawnOneTracker = spawnOneTracker;
-		if(this.spawnOneTracker)
-			this.server.config.setProperty("openvr.onetracker", true);
-		else
-			this.server.config.removeProperty("openvr.onetracker");
-		this.server.saveConfig();
 	}
 	
 	@Override
@@ -133,35 +113,31 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 				if(tryOpeningPipe(trackerPipe))
 					initTrackerPipe(trackerPipe, i);
 			}
-			if(spawnOneTracker)
-				break;
 		}
 	}
 	
-	public boolean updateHMD() {
+	public boolean updateHMD() throws IOException {
 		if(hmdPipe.state == PipeState.OPEN) {
 			IntByReference bytesAvailable = new IntByReference(0);
 			if(Kernel32.INSTANCE.PeekNamedPipe(hmdPipe.pipeHandle, null, 0, null, bytesAvailable, null)) {
 				if(bytesAvailable.getValue() > 0) {
-					if(Kernel32.INSTANCE.ReadFile(hmdPipe.pipeHandle, buffer, buffer.length, bytesAvailable, null)) {
-						String str = new String(buffer, 0, bytesAvailable.getValue() - 1, ASCII);
-						String[] split = str.split("\n")[0].split(" ");
-						try {
-							double x = Double.parseDouble(split[0]);
-							double y = Double.parseDouble(split[1]);
-							double z = Double.parseDouble(split[2]);
-							double qw = Double.parseDouble(split[3]);
-							double qx = Double.parseDouble(split[4]);
-							double qy = Double.parseDouble(split[5]);
-							double qz = Double.parseDouble(split[6]);
-							
-							internalHMDTracker.position.set((float) x, (float) y, (float) z);
-							internalHMDTracker.rotation.set((float) qx, (float) qy, (float) qz, (float) qw);
-							internalHMDTracker.dataTick();
-							newHMDData.set(true);
-						} catch(NumberFormatException e) {
-							e.printStackTrace();
+					while(Kernel32.INSTANCE.ReadFile(hmdPipe.pipeHandle, buffArray, buffArray.length, bytesAvailable, null)) {
+						int bytesRead = bytesAvailable.getValue();
+						for(int i = 0; i < bytesRead; ++i) {
+							char c = (char) buffArray[i];
+							if(c == '\n') {
+								executeHMDInput();
+								commandBuilder.setLength(0);
+							} else {
+								commandBuilder.append(c);
+								if(commandBuilder.length() >= MAX_COMMAND_LENGTH) {
+									LogManager.log.severe("[VRBridge] Command from the pipe is too long, flushing buffer");
+									commandBuilder.setLength(0);
+								}
+							}
 						}
+						if(bytesRead < buffArray.length)
+							break; // Don't repeat, we read all available bytes
 					}
 					return true;
 				}
@@ -170,6 +146,30 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 		return false;
 	}
 	
+	private void executeHMDInput() throws IOException {
+		String[] split = commandBuilder.toString().split(" ");
+		if(split.length < 7) {
+			LogManager.log.severe("[VRBridge] Short HMD data recieved: " + commandBuilder.toString());
+			return;
+		}
+		try {
+			double x = Double.parseDouble(split[0]);
+			double y = Double.parseDouble(split[1]);
+			double z = Double.parseDouble(split[2]);
+			double qw = Double.parseDouble(split[3]);
+			double qx = Double.parseDouble(split[4]);
+			double qy = Double.parseDouble(split[5]);
+			double qz = Double.parseDouble(split[6]);
+			
+			internalHMDTracker.position.set((float) x, (float) y, (float) z);
+			internalHMDTracker.rotation.set((float) qx, (float) qy, (float) qz, (float) qw);
+			internalHMDTracker.dataTick();
+			newHMDData.set(true);
+		} catch(NumberFormatException e) {
+			e.printStackTrace();
+		}
+	}
+	
 	public void updateTracker(int trackerId, boolean hmdUpdated) {
 		Tracker sensor = internalTrackers.get(trackerId);
 		if(sensor.getStatus().sendData) {
@@ -181,10 +181,10 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 				sbBuffer.append(vBuffer.x).append(' ').append(vBuffer.y).append(' ').append(vBuffer.z).append(' ');
 				sbBuffer.append(qBuffer.getW()).append(' ').append(qBuffer.getX()).append(' ').append(qBuffer.getY()).append(' ').append(qBuffer.getZ()).append('\n');
 				String str = sbBuffer.toString();
-				System.arraycopy(str.getBytes(ASCII), 0, buffer, 0, str.length());
-				buffer[str.length()] = '\0';
+				System.arraycopy(str.getBytes(ASCII), 0, buffArray, 0, str.length());
+				buffArray[str.length()] = '\0';
 				IntByReference lpNumberOfBytesWritten = new IntByReference(0);
-				Kernel32.INSTANCE.WriteFile(trackerPipe.pipeHandle, buffer, str.length() + 1, lpNumberOfBytesWritten, null);
+				Kernel32.INSTANCE.WriteFile(trackerPipe.pipeHandle, buffArray, str.length() + 1, lpNumberOfBytesWritten, null);
 			}
 		}
 	}
@@ -194,12 +194,12 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	}
 	
 	private void initTrackerPipe(Pipe pipe, int trackerId) {
-		String trackerHello = (spawnOneTracker ? "1" : this.shareTrackers.size()) + " 0";
-		System.arraycopy(trackerHello.getBytes(ASCII), 0, buffer, 0, trackerHello.length());
-		buffer[trackerHello.length()] = '\0';
+		String trackerHello = this.shareTrackers.size() + " 0";
+		System.arraycopy(trackerHello.getBytes(ASCII), 0, buffArray, 0, trackerHello.length());
+		buffArray[trackerHello.length()] = '\0';
 		IntByReference lpNumberOfBytesWritten = new IntByReference(0);
 		Kernel32.INSTANCE.WriteFile(pipe.pipeHandle,
-			buffer,
+			buffArray,
 			trackerHello.length() + 1,
 			lpNumberOfBytesWritten,
 			null);
@@ -207,7 +207,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	
 	private boolean tryOpeningPipe(Pipe pipe) {
 		if(Kernel32.INSTANCE.ConnectNamedPipe(pipe.pipeHandle, null)) {
-			pipe.state = NamedPipeVRBridge.PipeState.OPEN;
+			pipe.state = PipeState.OPEN;
 			LogManager.log.info("[VRBridge] Pipe " + pipe.name + " is open");
 			return true;
 		}
@@ -252,8 +252,6 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 					throw new IOException("Can't open " + pipeName + " pipe: " + Kernel32.INSTANCE.GetLastError());
 				LogManager.log.info("[VRBridge] Pipe " + pipeName + " created");
 				trackerPipes.add(new Pipe(pipeHandle, pipeName));
-				if(spawnOneTracker)
-					break;
 			}
 			LogManager.log.info("[VRBridge] Pipes are open");
 		} catch(IOException e) {
@@ -272,21 +270,21 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 		} catch(Exception e) {
 		}
 	}
-	
-	private static class Pipe {
-		final String name;
-		final HANDLE pipeHandle;
-		PipeState state = PipeState.CREATED;
+
+	@Override
+	public void addSharedTracker(ShareableTracker tracker) {
+		// TODO Auto-generated method stub
 		
-		public Pipe(HANDLE pipeHandle, String name) {
-			this.pipeHandle = pipeHandle;
-			this.name = name;
-		}
 	}
-	
-	private static enum PipeState {
-		CREATED,
-		OPEN,
-		ERROR;
+
+	@Override
+	public void removeSharedTracker(ShareableTracker tracker) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void startBridge() {
+		start();
 	}
 }

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

@@ -0,0 +1,42 @@
+package dev.slimevr.bridge;
+
+import io.eiren.util.ann.VRServerThread;
+import io.eiren.vr.trackers.ShareableTracker;
+
+public class OpenVRNativeBridge implements Bridge {
+	
+	public OpenVRNativeBridge() {
+		// TODO Auto-generated constructor stub
+	}
+	
+	@Override
+	public void dataRead() {
+		// TODO Auto-generated method stub
+		
+	}
+	
+	@Override
+	public void dataWrite() {
+		// TODO Auto-generated method stub
+		
+	}
+	
+	@Override
+	public void addSharedTracker(ShareableTracker tracker) {
+		// TODO Auto-generated method stub
+		
+	}
+	
+	@Override
+	public void removeSharedTracker(ShareableTracker tracker) {
+		// TODO Auto-generated method stub
+		
+	}
+	
+	@Override
+	@VRServerThread
+	public void startBridge() {
+		
+	}
+	
+}

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

@@ -0,0 +1,30 @@
+package dev.slimevr.bridge;
+
+import com.sun.jna.platform.win32.Kernel32;
+import com.sun.jna.platform.win32.WinNT.HANDLE;
+
+public class Pipe {
+	
+	public final String name;
+	public final HANDLE pipeHandle;
+	public PipeState state = PipeState.CREATED;
+	
+	public Pipe(HANDLE pipeHandle, String name) {
+		this.pipeHandle = pipeHandle;
+		this.name = name;
+	}
+	
+	public static void safeDisconnect(Pipe pipe) {
+		try {
+			if(pipe != null && pipe.pipeHandle != null)
+				Kernel32.INSTANCE.DisconnectNamedPipe(pipe.pipeHandle);
+		} catch(Exception e) {
+		}
+	}
+	
+	enum PipeState {
+		CREATED,
+		OPEN,
+		ERROR;
+	}
+}

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

@@ -0,0 +1,241 @@
+package dev.slimevr.bridge;
+
+import java.util.HashMap;
+import java.util.Iterator;
+import java.util.List;
+import java.util.Map;
+import java.util.Map.Entry;
+import java.util.Queue;
+import java.util.concurrent.LinkedBlockingQueue;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+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 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 {
+
+	private final Vector3f vec1 = new Vector3f();
+	private final Quaternion quat1 = new Quaternion();
+	
+	@ThreadSafe
+	private final Queue<ProtobufMessage> inputQueue = new LinkedBlockingQueue<>();
+	@ThreadSafe
+	private final Queue<ProtobufMessage> outputQueue = new LinkedBlockingQueue<>();
+	@VRServerThread
+	protected final List<ShareableTracker> sharedTrackers = new FastList<>();
+	@Synchronize("self")
+	private final Map<String, T> remoteTrackersBySerial = new HashMap<>();
+	@Synchronize("self")
+	private final Map<Integer, T> remoteTrackersByTrackerId = new HashMap<>();
+	
+	private boolean hadNewData = false;
+	
+	private T hmdTracker;
+	private final HMDTracker hmd;
+	protected final String bridgeName;
+	
+	public ProtobufBridge(String bridgeName, HMDTracker hmd) {
+		this.bridgeName = bridgeName;
+		this.hmd = hmd;
+	}
+
+	@BridgeThread
+	protected abstract boolean sendMessageReal(ProtobufMessage message);
+
+	@BridgeThread
+	protected void messageRecieved(ProtobufMessage message) {
+		inputQueue.add(message);
+	}
+	
+	@ThreadSafe
+	protected void sendMessage(ProtobufMessage message) {
+		outputQueue.add(message);
+	}
+
+	@BridgeThread
+	protected void updateMessageQueue() {
+		ProtobufMessage message = null;
+		while((message = outputQueue.poll()) != null) {
+			if(!sendMessageReal(message))
+				return;
+		}
+	}
+	
+	@VRServerThread
+	@Override
+	public void dataRead() {
+		hadNewData = false;
+		ProtobufMessage message = null;
+		while((message = inputQueue.poll()) != null) {
+			processMessageRecieved(message);
+			hadNewData = true;
+		}
+		if(hadNewData && hmdTracker != null) {
+			trackerOverrideUpdate(hmdTracker, hmd);
+		}
+	}
+
+	@VRServerThread
+	protected void trackerOverrideUpdate(T source, ComputedTracker target) {
+		target.position.set(source.position);
+		target.rotation.set(source.rotation);
+		target.setStatus(source.getStatus());
+		target.dataTick();
+	}
+
+	@VRServerThread
+	@Override
+	public void dataWrite() {
+		if(!hadNewData) // Don't write anything if no message were recieved, we always process at the speed of the other side
+			return;
+		for(int i = 0; i < sharedTrackers.size(); ++i) {
+			writeTrackerUpdate(sharedTrackers.get(i));
+		}
+	}
+
+	@VRServerThread
+	protected void writeTrackerUpdate(ShareableTracker localTracker) {
+		Position.Builder builder = Position.newBuilder().setTrackerId(localTracker.getTrackerId());
+		if(localTracker.getPosition(vec1)) {
+			builder.setX(vec1.x);
+			builder.setY(vec1.y);
+			builder.setZ(vec1.z);
+		}
+		if(localTracker.getRotation(quat1)) {
+			builder.setQx(quat1.getX());
+			builder.setQy(quat1.getY());
+			builder.setQz(quat1.getZ());
+			builder.setQw(quat1.getW());
+		}
+		sendMessage(ProtobufMessage.newBuilder().setPosition(builder).build());
+	}
+	
+	@VRServerThread
+	protected void processMessageRecieved(ProtobufMessage message) {
+		//if(!message.hasPosition())
+		//	LogManager.log.info("[" + bridgeName + "] MSG: " + message);
+		if(message.hasPosition()) {
+			positionRecieved(message.getPosition());
+		} else if(message.hasUserAction()) {
+			userActionRecieved(message.getUserAction());
+		} else if(message.hasTrackerStatus()) {
+			trackerStatusRecieved(message.getTrackerStatus());
+		} else if(message.hasTrackerAdded()) {
+			trackerAddedRecieved(message.getTrackerAdded());
+		}
+	}
+	
+	@VRServerThread
+	protected void positionRecieved(Position positionMessage) {
+		T tracker = getInternalRemoteTrackerById(positionMessage.getTrackerId());
+		if(tracker != null) {
+			if(positionMessage.hasX())
+				tracker.position.set(positionMessage.getX(), positionMessage.getY(), positionMessage.getZ());
+			tracker.rotation.set(positionMessage.getQx(), positionMessage.getQy(), positionMessage.getQz(), positionMessage.getQw());
+			tracker.dataTick();
+		}
+	}
+	
+	@VRServerThread
+	protected abstract T createNewTracker(TrackerAdded trackerAdded);
+
+	@VRServerThread
+	protected void trackerAddedRecieved(TrackerAdded trackerAdded) {
+		T tracker = getInternalRemoteTrackerById(trackerAdded.getTrackerId());
+		if(tracker != null) {
+			// TODO reinit?
+			return;
+		}
+		tracker = createNewTracker(trackerAdded);
+		synchronized(remoteTrackersBySerial) {
+			remoteTrackersBySerial.put(tracker.getName(), tracker);
+		}
+		synchronized(remoteTrackersByTrackerId) {
+			remoteTrackersByTrackerId.put(tracker.getTrackerId(), tracker);
+		}
+		if(trackerAdded.getTrackerRole() == TrackerRole.HMD.id) {
+			hmdTracker = tracker;
+		} else {
+			Main.vrServer.registerTracker(tracker);
+		}
+	}
+
+	@VRServerThread
+	protected void userActionRecieved(UserAction userAction) {
+		switch(userAction.getName()) {
+		case "calibrate":
+			// TODO : Check pose field
+			Main.vrServer.resetTrackers();
+			break;
+		}
+	}
+
+	@VRServerThread
+	protected void trackerStatusRecieved(TrackerStatus trackerStatus) {
+		T tracker = getInternalRemoteTrackerById(trackerStatus.getTrackerId());
+		if(tracker != null) {
+			tracker.setStatus(io.eiren.vr.trackers.TrackerStatus.getById(trackerStatus.getStatusValue()));
+		}
+	}
+    
+	@ThreadSafe
+	protected T getInternalRemoteTrackerById(int trackerId) {
+		synchronized(remoteTrackersByTrackerId) {
+			return remoteTrackersByTrackerId.get(trackerId);
+		}
+	}
+
+	@VRServerThread
+	protected void reconnected() {
+		for(int i = 0; i < sharedTrackers.size(); ++i) {
+			ShareableTracker tracker = sharedTrackers.get(i);
+			TrackerAdded.Builder builder = TrackerAdded.newBuilder().setTrackerId(tracker.getTrackerId()).setTrackerName(tracker.getDescriptiveName()).setTrackerSerial(tracker.getName()).setTrackerRole(tracker.getTrackerRole().id);
+			sendMessage(ProtobufMessage.newBuilder().setTrackerAdded(builder).build());
+		}
+	}
+
+	@VRServerThread
+	protected void disconnected() {
+		synchronized(remoteTrackersByTrackerId) {
+			Iterator<Entry<Integer, T>> iterator = remoteTrackersByTrackerId.entrySet().iterator();
+			while(iterator.hasNext()) {
+				iterator.next().getValue().setStatus(io.eiren.vr.trackers.TrackerStatus.DISCONNECTED);
+			}
+		}
+		if(hmdTracker != null) {
+			hmd.setStatus(io.eiren.vr.trackers.TrackerStatus.DISCONNECTED);
+		}
+	}
+
+	@VRServerThread
+	@Override
+	public void addSharedTracker(ShareableTracker tracker) {
+		if(sharedTrackers.contains(tracker))
+			return;
+		sharedTrackers.add(tracker);
+		TrackerAdded.Builder builder = TrackerAdded.newBuilder().setTrackerId(tracker.getTrackerId()).setTrackerName(tracker.getDescriptiveName()).setTrackerSerial(tracker.getName()).setTrackerRole(tracker.getTrackerRole().id);
+		sendMessage(ProtobufMessage.newBuilder().setTrackerAdded(builder).build());
+	}
+
+	@VRServerThread
+	@Override
+	public void removeSharedTracker(ShareableTracker tracker) {
+		sharedTrackers.remove(tracker);
+		// No message can be sent to the remote side, protocol doesn't support tracker removal (yet)
+	}
+}

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

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

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

@@ -1,8 +1,11 @@
-package io.eiren.vr.bridge;
+package dev.slimevr.bridge;
 
 import java.net.InetAddress;
 
-public class VMCBridge extends Thread implements VRBridge {
+import io.eiren.vr.trackers.ShareableTracker;
+import io.eiren.vr.trackers.Tracker;
+
+public class VMCBridge extends Thread implements Bridge {
 	
 	public final int readPort;
 	public final int writePort;
@@ -28,5 +31,22 @@ public class VMCBridge extends Thread implements VRBridge {
 		// TODO Auto-generated method stub
 		
 	}
+
+	@Override
+	public void addSharedTracker(ShareableTracker tracker) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void removeSharedTracker(ShareableTracker tracker) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void startBridge() {
+		start();
+	}
 	
 }

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

@@ -0,0 +1,194 @@
+package dev.slimevr.bridge;
+
+import java.net.InetSocketAddress;
+import java.nio.ByteBuffer;
+import java.util.Collections;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+import org.java_websocket.WebSocket;
+import org.java_websocket.drafts.Draft;
+import org.java_websocket.drafts.Draft_6455;
+import org.java_websocket.handshake.ClientHandshake;
+import org.java_websocket.server.WebSocketServer;
+import org.json.JSONException;
+import org.json.JSONObject;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+import io.eiren.vr.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 {
+	
+	private final Vector3f vBuffer = new Vector3f();
+	private final Quaternion qBuffer = new Quaternion();
+	
+	private final HMDTracker hmd;
+	private final List<? extends ShareableTracker> shareTrackers;
+	private final List<ComputedTracker> internalTrackers;
+	
+	private final HMDTracker internalHMDTracker = new HMDTracker("itnernal://HMD");
+	private final AtomicBoolean newHMDData = new AtomicBoolean(false);
+	
+	public WebSocketVRBridge(HMDTracker hmd, List<? extends ShareableTracker> shareTrackers, VRServer server) {
+		super(new InetSocketAddress(21110), Collections.<Draft>singletonList(new Draft_6455()));
+		this.hmd = hmd;
+		this.shareTrackers = new FastList<>(shareTrackers);
+		this.internalTrackers = new FastList<>(shareTrackers.size());
+		for(int i = 0; i < shareTrackers.size(); ++i) {
+			Tracker t = shareTrackers.get(i);
+			ComputedTracker ct = new ComputedTracker(t.getTrackerId(), "internal://" + t.getName(), true, true);
+			ct.setStatus(TrackerStatus.OK);
+			ct.bodyPosition = t.getBodyPosition();
+			this.internalTrackers.add(ct);
+		}
+	}
+
+	@Override
+	public void dataRead() {
+		if(newHMDData.compareAndSet(true, false)) {
+			hmd.position.set(internalHMDTracker.position);
+			hmd.rotation.set(internalHMDTracker.rotation);
+			hmd.dataTick();
+		}
+	}
+
+	@Override
+	public void dataWrite() {
+		for(int i = 0; i < shareTrackers.size(); ++i) {
+			Tracker t = shareTrackers.get(i);
+			ComputedTracker it = this.internalTrackers.get(i);
+			if(t.getPosition(vBuffer))
+				it.position.set(vBuffer);
+			if(t.getRotation(qBuffer))
+				it.rotation.set(qBuffer);
+		}
+	}
+
+	@Override
+	public void onOpen(WebSocket conn, ClientHandshake handshake) {
+		LogManager.log.info("[WebSocket] New connection from: " + conn.getRemoteSocketAddress().getAddress().getHostAddress());
+		// Register trackers
+		for(int i = 0; i < internalTrackers.size(); ++i) {
+			JSONObject message = new JSONObject();
+			message.put("type", "config");
+			message.put("tracker_id", "SlimeVR Tracker " + (i + 1));
+			message.put("location", shareTrackers.get(i).getTrackerRole().name().toLowerCase());
+			message.put("tracker_type", message.optString("location"));
+			conn.send(message.toString());
+		}
+	}
+
+	@Override
+	public void onClose(WebSocket conn, int code, String reason, boolean remote) {
+		LogManager.log.info("[WebSocket] Disconnected: " + conn.getRemoteSocketAddress().getAddress().getHostAddress() + ", (" + code + ") " + reason + ". Remote: " + remote);
+	}
+
+	@Override
+	public void onMessage(WebSocket conn, ByteBuffer message) {
+		StringBuilder sb = new StringBuilder(message.limit());
+		while(message.hasRemaining()) {
+			sb.append((char) message.get());
+		}
+		onMessage(conn, sb.toString());
+	}
+	
+	@Override
+	public void onMessage(WebSocket conn, String message) {
+		//LogManager.log.info(message);
+		try {
+			JSONObject json = new JSONObject(message);
+			if(json.has("type")) {
+				switch(json.optString("type")) {
+				case "pos":
+					parsePosition(json, conn);
+					return;
+				case "action":
+					parseAction(json, conn);
+					return;
+				case "config": // TODO Ignore it for now, it should only register HMD in our test case with id 0
+					LogManager.log.info("[WebSocket] Config recieved: " + json.toString());
+					return;
+				}
+			}
+			LogManager.log.warning("[WebSocket] Unrecognized message from " + conn.getRemoteSocketAddress().getAddress().getHostAddress() + ": " + message);
+		} catch(Exception e) {
+			LogManager.log.severe("[WebSocket] Exception parsing message from " + conn.getRemoteSocketAddress().getAddress().getHostAddress() + ". Message: " + message, e);
+		}
+	}
+
+	private void parsePosition(JSONObject json, WebSocket conn) throws JSONException {
+		if(json.optInt("tracker_id") == 0) {
+			// Read HMD information
+			internalHMDTracker.position.set(json.optFloat("x"), json.optFloat("y") + 0.2f, json.optFloat("z")); // TODO Wtf is this hack? VRWorkout issue?
+			internalHMDTracker.rotation.set(json.optFloat("qx"), json.optFloat("qy"), json.optFloat("qz"), json.optFloat("qw"));
+			internalHMDTracker.dataTick();
+			newHMDData.set(true);
+			
+			// Send tracker info in reply
+			for(int i = 0; i < internalTrackers.size(); ++i) {
+				JSONObject message = new JSONObject();
+				message.put("type", "pos");
+				message.put("src", "full");
+				message.put("tracker_id", "SlimeVR Tracker " + (i + 1));
+				
+				ComputedTracker t = internalTrackers.get(i);
+				message.put("x", t.position.x);
+				message.put("y", t.position.y);
+				message.put("z", t.position.z);
+				message.put("qx", t.rotation.getX());
+				message.put("qy", t.rotation.getY());
+				message.put("qz", t.rotation.getZ());
+				message.put("qw", t.rotation.getW());
+				
+				conn.send(message.toString());
+			}
+		}
+	}
+
+	private void parseAction(JSONObject json, WebSocket conn) throws JSONException {
+		switch(json.optString("name")) {
+		case "calibrate":
+			Main.vrServer.resetTrackersYaw();
+			break;
+		}
+	}
+
+	@Override
+	public void onError(WebSocket conn, Exception ex) {
+		LogManager.log.severe("[WebSocket] Exception on connection " + (conn != null ? conn.getRemoteSocketAddress().getAddress().getHostAddress() : null), ex);
+	}
+
+	@Override
+	public void onStart() {
+		LogManager.log.info("[WebSocket] Web Socket VR Bridge started on port " + getPort());
+		setConnectionLostTimeout(0);
+	    setConnectionLostTimeout(1);
+	}
+
+	@Override
+	public void addSharedTracker(ShareableTracker tracker) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void removeSharedTracker(ShareableTracker tracker) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void startBridge() {
+		start();
+	}
+}

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

@@ -0,0 +1,24 @@
+package dev.slimevr.gui;
+
+import java.awt.event.ComponentEvent;
+import java.awt.event.ComponentListener;
+
+public abstract class AbstractComponentListener implements ComponentListener {
+	
+	@Override
+	public void componentResized(ComponentEvent e) {
+	}
+	
+	@Override
+	public void componentMoved(ComponentEvent e) {
+	}
+	
+	@Override
+	public void componentShown(ComponentEvent e) {
+	}
+	
+	@Override
+	public void componentHidden(ComponentEvent e) {
+	}
+	
+}

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

@@ -0,0 +1,35 @@
+package dev.slimevr.gui;
+
+import java.awt.event.WindowEvent;
+import java.awt.event.WindowListener;
+
+public abstract class AbstractWindowListener implements WindowListener {
+	
+	@Override
+	public void windowOpened(WindowEvent e) {
+	}
+	
+	@Override
+	public void windowClosing(WindowEvent e) {
+	}
+	
+	@Override
+	public void windowClosed(WindowEvent e) {
+	}
+	
+	@Override
+	public void windowIconified(WindowEvent e) {
+	}
+	
+	@Override
+	public void windowDeiconified(WindowEvent e) {
+	}
+	
+	@Override
+	public void windowActivated(WindowEvent e) {
+	}
+	
+	@Override
+	public void windowDeactivated(WindowEvent e) {
+	}
+}

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

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

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

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui;
 
 
 import java.awt.Container;
@@ -12,6 +12,7 @@ import javax.swing.JTextArea;
 import javax.swing.border.EmptyBorder;
 import javax.swing.event.MouseInputAdapter;
 
+import dev.slimevr.gui.swing.EJBox;
 import io.eiren.util.ann.AWTThread;
 import io.eiren.vr.trackers.CalibratingTracker;
 import io.eiren.vr.trackers.Tracker;

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

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui;
 
 import java.awt.Font;
 import java.text.AttributedCharacterIterator.Attribute;

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

@@ -0,0 +1,263 @@
+package dev.slimevr.gui;
+
+import java.awt.event.MouseEvent;
+import java.util.HashMap;
+import java.util.Map;
+
+import javax.swing.JButton;
+import javax.swing.JLabel;
+import javax.swing.event.MouseInputAdapter;
+
+import dev.slimevr.gui.swing.ButtonTimer;
+import dev.slimevr.gui.swing.EJBagNoStretch;
+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 {
+
+	private final VRServer server;
+	private final VRServerGUI gui;
+	private final AutoBoneWindow autoBone;
+	private Map<String, SkeletonLabel> labels = new HashMap<>();
+
+	public SkeletonConfig(VRServer server, VRServerGUI gui) {
+		super(false, true);
+		this.server = server;
+		this.gui = gui;
+		this.autoBone = new AutoBoneWindow(server, this);
+
+		setAlignmentY(TOP_ALIGNMENT);
+		server.humanPoseProcessor.addSkeletonUpdatedCallback(this::skeletonUpdated);
+		skeletonUpdated(null);
+	}
+
+	@ThreadSafe
+	public void skeletonUpdated(HumanSkeleton newSkeleton) {
+		java.awt.EventQueue.invokeLater(() -> {
+			removeAll();
+
+			int row = 0;
+
+			/**
+			add(new JCheckBox("Extended pelvis model") {{
+				addItemListener(new ItemListener() {
+				    @Override
+				    public void itemStateChanged(ItemEvent e) {
+				        if(e.getStateChange() == ItemEvent.SELECTED) {//checkbox has been selected
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended pelvis model", true);
+				        	}
+				        } else {
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended pelvis model", false);
+				        	}
+				        }
+				    }
+				});
+				if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+	        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+	        		setSelected(hswl.getSkeletonConfigBoolean("Extended pelvis model"));
+				}
+			}}, s(c(0, row, 2), 3, 1));
+			row++;
+			//*/
+			/*
+			add(new JCheckBox("Extended knee model") {{
+				addItemListener(new ItemListener() {
+				    @Override
+				    public void itemStateChanged(ItemEvent e) {
+				        if(e.getStateChange() == ItemEvent.SELECTED) {//checkbox has been selected
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended knee model", true);
+				        	}
+				        } else {
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended knee model", false);
+				        	}
+				        }
+				    }
+				});
+				if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+	        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+	        		setSelected(hswl.getSkeletonConfigBoolean("Extended knee model"));
+				}
+			}}, s(c(0, row, 2), 3, 1));
+			row++;
+			//*/
+
+			add(new TimedResetButton("Reset All", "All"), s(c(1, row, 2), 3, 1));
+			add(new JButton("Auto") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						autoBone.setVisible(true);
+						autoBone.toFront();
+					}
+				});
+			}}, s(c(4, row, 2), 3, 1));
+			row++;
+
+			add(new JLabel("Torso length"), c(0, row, 2));
+			add(new AdjButton("+", "Torso", 0.01f), c(1, row, 2));
+			add(new SkeletonLabel("Torso"), c(2, row, 2));
+			add(new AdjButton("-", "Torso", -0.01f), c(3, row, 2));
+			add(new TimedResetButton("Reset", "Torso"), c(4, row, 2));
+			row++;
+
+			add(new JLabel("Chest distance"), 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++;
+
+			add(new JLabel("Waist distance"), 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 ResetButton("Reset", "Waist"), c(4, row, 2));
+			row++;
+
+			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 ResetButton("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("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("Hip offset"), c(0, row, 2));
+			add(new AdjButton("+", "Hip offset", 0.01f), c(1, row, 2));
+			add(new SkeletonLabel("Hip offset"), c(2, row, 2));
+			add(new AdjButton("-", "Hip offset", -0.01f), c(3, row, 2));
+			add(new ResetButton("Reset", "Hip offset"), 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++;
+
+			gui.refresh();
+		});
+	}
+
+	@ThreadSafe
+	public void refreshAll() {
+		java.awt.EventQueue.invokeLater(() -> {
+			labels.forEach((joint, label) -> {
+				label.setText(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint) * 100, 0));
+			});
+		});
+	}
+
+	private void change(String joint, float diff) {
+		float current = server.humanPoseProcessor.getSkeletonConfig(joint);
+		server.humanPoseProcessor.setSkeletonConfig(joint, current + diff);
+		server.saveConfig();
+		labels.get(joint).setText(StringUtils.prettyNumber((current + diff) * 100, 0));
+	}
+
+	private void reset(String joint) {
+		server.humanPoseProcessor.resetSkeletonConfig(joint);
+		server.saveConfig();
+		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));
+			});
+		}
+	}
+
+	private class SkeletonLabel extends JLabel {
+
+		public SkeletonLabel(String joint) {
+			super(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint) * 100, 0));
+			labels.put(joint, this);
+		}
+	}
+
+	private class AdjButton extends JButton {
+
+		public AdjButton(String text, String joint, float diff) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					change(joint, diff);
+				}
+			});
+		}
+	}
+
+	private class ResetButton extends JButton {
+
+		public ResetButton(String text, String joint) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					reset(joint);
+				}
+			});
+		}
+	}
+
+	private class TimedResetButton extends JButton {
+
+		public TimedResetButton(String text, String joint) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					ButtonTimer.runTimer(TimedResetButton.this, 3, text, () -> reset(joint));
+				}
+			});
+		}
+	}
+}

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

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui;
 
 import java.awt.GridBagConstraints;
 import java.util.List;
@@ -9,6 +9,7 @@ import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.gui.swing.EJBagNoStretch;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.util.ann.VRServerThread;
@@ -17,19 +18,20 @@ import io.eiren.vr.VRServer;
 import io.eiren.vr.processor.HumanSkeleton;
 import io.eiren.vr.processor.TransformNode;
 
-public class SkeletonList extends EJBag {
+public class SkeletonList extends EJBagNoStretch {
+	
+	private static final long UPDATE_DELAY = 50;
 
 	Quaternion q = new Quaternion();
 	Vector3f v = new Vector3f();
 	float[] angles = new float[3];
 	
-	private final VRServer server;
 	private final VRServerGUI gui;
 	private final List<NodeStatus> nodes = new FastList<>();
+	private long lastUpdate = 0;
 	
 	public SkeletonList(VRServer server, VRServerGUI gui) {
-		super();
-		this.server = server;
+		super(false, true);
 		this.gui = gui;
 
 		setAlignmentY(TOP_ALIGNMENT);
@@ -62,6 +64,9 @@ public class SkeletonList extends EJBag {
 	
 	@VRServerThread
 	public void updateBones() {
+		if(lastUpdate + UPDATE_DELAY > System.currentTimeMillis())
+			return;
+		lastUpdate = System.currentTimeMillis();
 		java.awt.EventQueue.invokeLater(() -> {
 			for(int i = 0; i < nodes.size(); ++i)
 				nodes.get(i).update();

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

@@ -0,0 +1,353 @@
+package dev.slimevr.gui;
+
+import java.awt.Color;
+import java.awt.Font;
+import java.awt.GridBagConstraints;
+import java.awt.event.ActionEvent;
+import java.awt.event.ActionListener;
+import java.util.List;
+
+import javax.swing.BorderFactory;
+import javax.swing.Box;
+import javax.swing.BoxLayout;
+import javax.swing.JComboBox;
+import javax.swing.JLabel;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import dev.slimevr.gui.swing.EJBagNoStretch;
+import dev.slimevr.gui.swing.EJBoxNoStretch;
+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 {
+	
+	private static final long UPDATE_DELAY = 50;
+	
+	Quaternion q = new Quaternion();
+	Vector3f v = new Vector3f();
+	float[] angles = new float[3];
+	
+	private List<TrackerPanel> trackers = new FastList<>();
+	
+	private final VRServer server;
+	private final VRServerGUI gui;
+	private long lastUpdate = 0;
+
+	public TrackersList(VRServer server, VRServerGUI gui) {
+		super(BoxLayout.PAGE_AXIS, false, true);
+		this.server = server;
+		this.gui = gui;
+
+		setAlignmentY(TOP_ALIGNMENT);
+		
+		server.addNewTrackerConsumer(this::newTrackerAdded);
+	}
+
+	@AWTThread
+	private void build() {
+		removeAll();
+		
+		trackers.sort((tr1, tr2) -> getTrackerSort(tr1.t) - getTrackerSort(tr2.t));
+		
+		Class<? extends Tracker> currentClass = null;
+		
+		EJBoxNoStretch line = null;
+		boolean first = true;
+		
+		for(int i = 0; i < trackers.size(); ++i) {
+			TrackerPanel tr = trackers.get(i);
+			Tracker t = tr.t;
+			if(t instanceof ReferenceAdjustedTracker)
+				t = ((ReferenceAdjustedTracker<?>) t).getTracker();
+			if(currentClass != t.getClass()) {
+				currentClass = t.getClass();
+				if(line != null)
+					line.add(Box.createHorizontalGlue());
+				line = null;
+				line = new EJBoxNoStretch(BoxLayout.LINE_AXIS, false, true);
+				line.add(Box.createHorizontalGlue());
+				JLabel nameLabel;
+				line.add(nameLabel = new JLabel(currentClass.getSimpleName()));
+				nameLabel.setFont(nameLabel.getFont().deriveFont(Font.BOLD));
+				line.add(Box.createHorizontalGlue());
+				add(line);
+				line = null;
+			}
+			
+			if(line == null) {
+				line = new EJBoxNoStretch(BoxLayout.LINE_AXIS, false, true);
+				add(Box.createVerticalStrut(3));
+				add(line);
+				first = true;
+			} else {
+				line.add(Box.createHorizontalStrut(3));
+				first = false;
+			}
+			tr.build();
+			line.add(tr);
+			if(!first)
+				line = null;
+		}
+		validate();
+		gui.refresh();
+	}
+	
+	@ThreadSafe
+	public void updateTrackers() {
+		if(lastUpdate + UPDATE_DELAY > System.currentTimeMillis())
+			return;
+		lastUpdate = System.currentTimeMillis();
+		java.awt.EventQueue.invokeLater(() -> {
+			for(int i = 0; i < trackers.size(); ++i)
+				trackers.get(i).update();
+		});
+	}
+	
+	@ThreadSafe
+	public void newTrackerAdded(Tracker t) {
+		java.awt.EventQueue.invokeLater(() -> {
+			trackers.add(new TrackerPanel(t));
+			build();
+		});
+	}
+	
+	private class TrackerPanel extends EJBagNoStretch {
+		
+		final Tracker t;
+		JLabel position;
+		JLabel rotation;
+		JLabel status;
+		JLabel tps;
+		JLabel bat;
+		JLabel ping;
+		JLabel raw;
+		JLabel rawMag;
+		JLabel calibration;
+		JLabel magAccuracy;
+		JLabel adj;
+		JLabel adjYaw;
+		JLabel correction;
+		
+		@AWTThread
+		public TrackerPanel(Tracker t) {
+			super(false, true);
+			
+			this.t = t;
+		}
+
+		@SuppressWarnings("unchecked")
+		@AWTThread
+		public TrackerPanel build() {
+			int row = 0;
+			
+			Tracker realTracker = t;
+			if(t instanceof ReferenceAdjustedTracker)
+				realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
+			removeAll();
+			JLabel nameLabel;
+			add(nameLabel = new JLabel(t.getDescriptiveName()), s(c(0, row, 2, GridBagConstraints.FIRST_LINE_START), 4, 1));
+			nameLabel.setFont(nameLabel.getFont().deriveFont(Font.BOLD));
+			row++;
+			
+			if(t.userEditable()) {
+				TrackerConfig cfg = server.getTrackerConfig(t);
+				JComboBox<String> desSelect;
+				add(desSelect = new JComboBox<>(), s(c(0, row, 2, GridBagConstraints.FIRST_LINE_START), 2, 1));
+				for(TrackerPosition p : TrackerPosition.values) {
+					desSelect.addItem(p.name());
+				}
+				if(cfg.designation != null) {
+					TrackerPosition p = TrackerPosition.getByDesignation(cfg.designation);
+					if(p != null)
+						desSelect.setSelectedItem(p.name());
+				}
+				desSelect.addActionListener(new ActionListener() {
+					@Override
+					public void actionPerformed(ActionEvent e) {
+						TrackerPosition p = TrackerPosition.valueOf(String.valueOf(desSelect.getSelectedItem()));
+						t.setBodyPosition(p);
+						server.trackerUpdated(t);
+					}
+				});
+				if(realTracker instanceof IMUTracker) {
+					IMUTracker imu = (IMUTracker) realTracker;
+					TrackerMountingRotation tr = imu.getMountingRotation();
+					JComboBox<String> mountSelect;
+					add(mountSelect = new JComboBox<>(), s(c(2, row, 2, GridBagConstraints.FIRST_LINE_START), 2, 1));
+					for(TrackerMountingRotation p : TrackerMountingRotation.values) {
+						mountSelect.addItem(p.name());
+					}
+					if(tr != null) {
+						mountSelect.setSelectedItem(tr.name());
+					} else {
+						mountSelect.setSelectedItem(TrackerMountingRotation.BACK.name());
+					}
+					mountSelect.addActionListener(new ActionListener() {
+						@Override
+						public void actionPerformed(ActionEvent e) {
+							TrackerMountingRotation tr = TrackerMountingRotation.valueOf(String.valueOf(mountSelect.getSelectedItem()));
+							imu.setMountingRotation(tr);
+							server.trackerUpdated(t);
+						}
+					});
+				}
+				row++;
+			}
+			if(t.hasRotation())
+				add(new JLabel("Rotation"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+			if(t.hasPosition())
+				add(new JLabel("Position"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("TPS"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+			if(realTracker instanceof IMUTracker) {
+				add(new JLabel("Ping"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+			}
+			row++;
+			if(t.hasRotation())
+				add(rotation = new JLabel("0 0 0"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+			if(t.hasPosition())
+				add(position = new JLabel("0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
+			if(realTracker instanceof IMUTracker) {
+				add(ping = new JLabel(""), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+			}
+			if(realTracker instanceof TrackerWithTPS) {
+				add(tps = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+			} else {
+				add(new JLabel(""), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+			}
+			row++;
+			add(new JLabel("Status:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+			add(status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
+			if(realTracker instanceof TrackerWithBattery) {
+				add(new JLabel("Battery:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(bat = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+			}
+			row++;
+			add(new JLabel("Raw:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+			add(raw = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1));
+			row++;
+			/*
+			if(realTracker instanceof IMUTracker) {
+				add(new JLabel("Raw mag:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(rawMag = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1));
+				row++;
+				add(new JLabel("Cal:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(calibration = new JLabel("0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("Mag acc:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(magAccuracy = new JLabel("0°"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+				row++;
+				add(new JLabel("Correction:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(correction = new JLabel("0 0 0"), s(c(1, row, 2, GridBagConstraints.FIRST_LINE_START), 3, 1));
+				row++;
+			}
+			//*/
+			
+			/*
+			if(t instanceof ReferenceAdjustedTracker) {	
+				add(new JLabel("Adj:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(adj = new JLabel("0 0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("AdjY:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(adjYaw = new JLabel("0 0 0 0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+			}
+			//*/
+
+			setBorder(BorderFactory.createLineBorder(new Color(0x663399), 2, false));
+			TrackersList.this.add(this);
+			return this;
+		}
+
+		@SuppressWarnings("unchecked")
+		@AWTThread
+		public void update() {
+			if(position == null && rotation == null)
+				return;
+			Tracker realTracker = t;
+			if(t instanceof ReferenceAdjustedTracker)
+				realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
+			t.getRotation(q);
+			t.getPosition(v);
+			q.toAngles(angles);
+			
+			if(position != null)
+				position.setText(StringUtils.prettyNumber(v.x, 1)
+						+ " " + StringUtils.prettyNumber(v.y, 1)
+						+ " " + StringUtils.prettyNumber(v.z, 1));
+			if(rotation != null)
+				rotation.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			status.setText(t.getStatus().toString().toLowerCase());
+			
+			if(realTracker instanceof TrackerWithTPS) {
+				tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1));
+			}
+			if(realTracker instanceof TrackerWithBattery)
+				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 1));
+			if(t instanceof ReferenceAdjustedTracker) {
+				((ReferenceAdjustedTracker<Tracker>) t).attachmentFix.toAngles(angles);
+				if(adj != null)
+					adj.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+							+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+							+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+				((ReferenceAdjustedTracker<Tracker>) t).yawFix.toAngles(angles);
+				if(adjYaw != null)
+					adjYaw.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+							+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+							+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			}
+			if(realTracker instanceof IMUTracker) {
+				if(ping != null)
+					ping.setText(String.valueOf(((IMUTracker) realTracker).ping));
+			}
+			realTracker.getRotation(q);
+			q.toAngles(angles);
+			raw.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+					+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+					+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			if(realTracker instanceof IMUTracker) {
+				((IMUTracker) realTracker).rotMagQuaternion.toAngles(angles);
+				if(rawMag != null)
+					rawMag.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+							+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+							+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+				if(calibration != null)
+					calibration.setText(((IMUTracker) realTracker).calibrationStatus + " / " + ((IMUTracker) realTracker).magCalibrationStatus);
+				if(magAccuracy != null)
+					magAccuracy.setText(StringUtils.prettyNumber(((IMUTracker) realTracker).magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°");
+				((IMUTracker) realTracker).getCorrection(q);
+				q.toAngles(angles);
+				if(correction != null)
+					correction.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+							+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+							+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			}
+		}
+	}
+	
+	private static int getTrackerSort(Tracker t) {
+		if(t instanceof ReferenceAdjustedTracker)
+			t = ((ReferenceAdjustedTracker<?>) t).getTracker();
+		if(t instanceof IMUTracker)
+			return 0;
+		if(t instanceof HMDTracker)
+			return 100;
+		if(t instanceof ComputedTracker)
+			return 200;
+		return 1000;
+	}
+}

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

@@ -0,0 +1,338 @@
+package dev.slimevr.gui;
+
+import javax.imageio.ImageIO;
+import javax.swing.*;
+import javax.swing.border.EmptyBorder;
+import javax.swing.event.MouseInputAdapter;
+
+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 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;
+import java.awt.Dimension;
+import java.awt.Font;
+import java.awt.GraphicsConfiguration;
+import java.awt.Rectangle;
+import java.awt.event.ActionEvent;
+import java.awt.event.ActionListener;
+import java.awt.event.ComponentEvent;
+import java.awt.event.MouseEvent;
+import java.awt.image.BufferedImage;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+
+import static javax.swing.BoxLayout.PAGE_AXIS;
+import static javax.swing.BoxLayout.LINE_AXIS;
+
+public class VRServerGUI extends JFrame {
+	
+	public static final String TITLE = "SlimeVR Server (" + Main.VERSION + ")";
+	
+	public final VRServer server;
+	private final TrackersList trackersList;
+	private final SkeletonList skeletonList;
+	private JButton resetButton;
+	private EJBox pane;
+	
+	private float zoom = 1.5f;
+	private float initZoom = zoom;
+	
+	@AWTThread
+	public VRServerGUI(VRServer server) {
+		super(TITLE);
+		try {
+			UIManager.setLookAndFeel(UIManager.getSystemLookAndFeelClassName());
+		} catch(Exception e) {
+			e.printStackTrace();
+		}
+		if(OperatingSystem.getCurrentPlatform() == OperatingSystem.OSX)
+			MacOSX.setTitle(TITLE);
+		try {
+			List<BufferedImage> images = new ArrayList<BufferedImage>(6);
+			images.add(ImageIO.read(VRServerGUI.class.getResource("/icon16.png")));
+			images.add(ImageIO.read(VRServerGUI.class.getResource("/icon32.png")));
+			images.add(ImageIO.read(VRServerGUI.class.getResource("/icon48.png")));
+			images.add(ImageIO.read(VRServerGUI.class.getResource("/icon64.png")));
+			images.add(ImageIO.read(VRServerGUI.class.getResource("/icon128.png")));
+			images.add(ImageIO.read(VRServerGUI.class.getResource("/icon256.png")));
+			setIconImages(images);
+			if(OperatingSystem.getCurrentPlatform() == OperatingSystem.OSX) {
+				MacOSX.setIcons(images);
+			}
+		} catch(IOException e1) {
+			e1.printStackTrace();
+		}
+		
+		this.server = server;
+		
+		this.zoom = server.config.getFloat("zoom", zoom);
+		this.initZoom = zoom;
+		setDefaultFontSize(zoom);
+		// All components should be constructed to the current zoom level by default
+		
+		setDefaultCloseOperation(EXIT_ON_CLOSE);
+		getContentPane().setLayout(new BoxLayout(getContentPane(), PAGE_AXIS));
+		
+		this.trackersList = new TrackersList(server, this);
+		this.skeletonList = new SkeletonList(server, this);
+		
+		add(new JScrollPane(pane = new EJBox(PAGE_AXIS), ScrollPaneConstants.VERTICAL_SCROLLBAR_AS_NEEDED, ScrollPaneConstants.HORIZONTAL_SCROLLBAR_AS_NEEDED));
+		GraphicsConfiguration gc = getGraphicsConfiguration();
+		Rectangle screenBounds = gc.getBounds();
+		setMinimumSize(new Dimension(100, 100));
+		setSize(Math.min(server.config.getInt("window.width", 800), screenBounds.width), Math.min(server.config.getInt("window.height", 800), screenBounds.height));
+		setLocation(server.config.getInt("window.posx", screenBounds.x + (screenBounds.width - getSize().width) / 2), screenBounds.y + server.config.getInt("window.posy", (screenBounds.height - getSize().height) / 2));
+		
+		// Resize and close listeners to save position and size betwen launcher starts
+		addComponentListener(new AbstractComponentListener() {
+			@Override
+			public void componentResized(ComponentEvent e) {
+				saveFrameInfo();
+			}
+			
+			@Override
+			public void componentMoved(ComponentEvent e) {
+				saveFrameInfo();
+			}
+		});
+		
+		build();
+	}
+	
+	protected void saveFrameInfo() {
+		Rectangle b = getBounds();
+		server.config.setProperty("window.width", b.width);
+		server.config.setProperty("window.height", b.height);
+		server.config.setProperty("window.posx", b.x);
+		server.config.setProperty("window.posy", b.y);
+		server.saveConfig();
+	}
+	
+	public float getZoom() {
+		return this.zoom;
+	}
+	
+	public void refresh() {
+		// Pack and display
+		//pack();
+		setVisible(true);
+		java.awt.EventQueue.invokeLater(new Runnable() {
+			@Override
+			public void run() {
+				repaint();
+			}
+		});
+	}
+	
+	@AWTThread
+	private void build() {
+		pane.removeAll();
+		
+		pane.add(new EJBoxNoStretch(LINE_AXIS, false, true) {{
+			setBorder(new EmptyBorder(i(5)));
+			
+			add(Box.createHorizontalGlue());
+			add(resetButton = new JButton("RESET") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						reset();
+					}
+				});
+			}});
+			add(Box.createHorizontalStrut(10));
+			add(new JButton("Fast Reset") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						resetFast();
+					}
+				});
+			}});
+			add(Box.createHorizontalGlue());
+			add(new JButton("GUI Zoom (x" + StringUtils.prettyNumber(zoom, 2) + ")") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						guiZoom();
+						setText("GUI Zoom (x" + StringUtils.prettyNumber(zoom, 2) + ")");
+					}
+				});
+			}});
+			add(Box.createHorizontalStrut(10));
+			add(new JButton("WiFi") {{
+				addMouseListener(new MouseInputAdapter() {
+					@SuppressWarnings("unused")
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						new WiFiWindow(VRServerGUI.this);
+					}
+				});
+			}});
+			add(Box.createHorizontalStrut(10));
+		}});
+		
+		pane.add(new EJBox(LINE_AXIS) {{
+			setBorder(new EmptyBorder(i(5)));
+			add(new EJBoxNoStretch(PAGE_AXIS, false, true) {{
+				setAlignmentY(TOP_ALIGNMENT);
+				JLabel l;
+				add(l = new JLabel("Trackers list"));
+				l.setFont(l.getFont().deriveFont(Font.BOLD));
+				l.setAlignmentX(0.5f);
+				add(trackersList);
+				add(Box.createVerticalGlue());
+			}});
+
+			add(new EJBoxNoStretch(PAGE_AXIS, false, true) {{
+				setAlignmentY(TOP_ALIGNMENT);
+				JLabel l;
+				add(l = new JLabel("Body proportions"));
+				l.setFont(l.getFont().deriveFont(Font.BOLD));
+				l.setAlignmentX(0.5f);
+				add(new SkeletonConfig(server, VRServerGUI.this));
+				add(Box.createVerticalStrut(10));
+				if(server.hasBridge(NamedPipeBridge.class)) {
+					NamedPipeBridge br = server.getVRBridge(NamedPipeBridge.class);
+					add(l = new JLabel("SteamVR Trackers"));
+					l.setFont(l.getFont().deriveFont(Font.BOLD));
+					l.setAlignmentX(0.5f);
+					add(l = new JLabel("Changes may require restart of SteamVR"));
+					l.setFont(l.getFont().deriveFont(Font.ITALIC));
+					l.setAlignmentX(0.5f);
+					
+					add(new EJBagNoStretch(false, true) {{
+						JCheckBox waistCb;
+						add(waistCb = new JCheckBox("Waist"), c(1, 1));
+						waistCb.setSelected(br.getShareSetting(TrackerRole.WAIST));
+						waistCb.addActionListener(new ActionListener() {
+							@Override
+							public void actionPerformed(ActionEvent e) {
+								server.queueTask(() -> {
+									br.changeShareSettings(TrackerRole.WAIST, waistCb.isSelected());
+								});
+							}
+						});
+
+						JCheckBox legsCb;
+						add(legsCb = new JCheckBox("Legs"), c(2, 1));
+						legsCb.setSelected(br.getShareSetting(TrackerRole.LEFT_FOOT) && br.getShareSetting(TrackerRole.RIGHT_FOOT));
+						legsCb.addActionListener(new ActionListener() {
+							@Override
+							public void actionPerformed(ActionEvent e) {
+								server.queueTask(() -> {
+									br.changeShareSettings(TrackerRole.LEFT_FOOT, legsCb.isSelected());
+									br.changeShareSettings(TrackerRole.RIGHT_FOOT, legsCb.isSelected());
+								});
+							}
+						});
+
+						JCheckBox chestCb;
+						add(chestCb = new JCheckBox("Chest"), c(1, 2));
+						chestCb.setSelected(br.getShareSetting(TrackerRole.CHEST));
+						chestCb.addActionListener(new ActionListener() {
+							@Override
+							public void actionPerformed(ActionEvent e) {
+								server.queueTask(() -> {
+									br.changeShareSettings(TrackerRole.CHEST, chestCb.isSelected());
+								});
+							}
+						});
+
+						JCheckBox kneesCb;
+						add(kneesCb = new JCheckBox("Knees"), c(2, 2));
+						kneesCb.setSelected(br.getShareSetting(TrackerRole.LEFT_KNEE) && br.getShareSetting(TrackerRole.RIGHT_KNEE));
+						kneesCb.addActionListener(new ActionListener() {
+							@Override
+							public void actionPerformed(ActionEvent e) {
+								server.queueTask(() -> {
+									br.changeShareSettings(TrackerRole.LEFT_KNEE, kneesCb.isSelected());
+									br.changeShareSettings(TrackerRole.RIGHT_KNEE, kneesCb.isSelected());
+								});
+							}
+						});
+						
+					}});
+					
+					
+				add(Box.createVerticalStrut(10));
+				}
+				add(new JLabel("Skeleton data"));
+				add(skeletonList);
+				add(Box.createVerticalGlue());
+			}});
+		}});
+		pane.add(Box.createVerticalGlue());
+		
+		refresh();
+		
+		server.addOnTick(trackersList::updateTrackers);
+		server.addOnTick(skeletonList::updateBones);
+	}
+	
+	// For now only changes font size, but should change fixed components size in the future too
+	private void guiZoom() {
+		if(zoom <= 1.0f) {
+			zoom = 1.5f;
+		} else if(zoom <= 1.5f) {
+			zoom = 1.75f;
+		} else if(zoom <= 1.75f) {
+			zoom = 2.0f;
+		} else if(zoom <= 2.0f) {
+			zoom = 2.5f;
+		} else {
+			zoom = 1.0f;
+		}
+		processNewZoom(zoom / initZoom, pane);
+		refresh();
+		server.config.setProperty("zoom", zoom);
+		server.saveConfig();
+	}
+	
+	private static void processNewZoom(float zoom, Component comp) {
+		if(comp.isFontSet()) {
+			Font newFont = new ScalableFont(comp.getFont(), zoom);
+			comp.setFont(newFont);
+		}
+		if(comp instanceof Container) {
+			Container cont = (Container) comp;
+			for(Component child : cont.getComponents())
+				processNewZoom(zoom, child);
+		}
+	}
+	
+	private static void setDefaultFontSize(float zoom) {
+		java.util.Enumeration<Object> keys = UIManager.getDefaults().keys();
+		while(keys.hasMoreElements()) {
+			Object key = keys.nextElement();
+			Object value = UIManager.get(key);
+			if(value instanceof javax.swing.plaf.FontUIResource) {
+				javax.swing.plaf.FontUIResource f = (javax.swing.plaf.FontUIResource) value;
+				javax.swing.plaf.FontUIResource f2 = new javax.swing.plaf.FontUIResource(f.deriveFont(f.getSize() * zoom));
+				UIManager.put(key, f2);
+			}
+		}
+	}
+	
+	@AWTThread
+	private void resetFast() {
+		server.resetTrackersYaw();
+	}
+	
+	@AWTThread
+	private void reset() {
+		ButtonTimer.runTimer(resetButton, 3, "RESET", server::resetTrackers);
+	}
+}

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

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui;
 
 import java.awt.Container;
 import java.awt.event.MouseEvent;
@@ -26,6 +26,7 @@ import javax.swing.event.MouseInputAdapter;
 
 import com.fazecast.jSerialComm.SerialPort;
 
+import dev.slimevr.gui.swing.EJBox;
 import io.eiren.util.ann.AWTThread;
 
 public class WiFiWindow extends JFrame {
@@ -53,7 +54,7 @@ public class WiFiWindow extends JFrame {
 		
 		SerialPort[] ports = SerialPort.getCommPorts();
 		for(SerialPort port : ports) {
-			if(port.getDescriptivePortName().contains("CH340")) {
+			if(port.getDescriptivePortName().toLowerCase().contains("ch340") || port.getDescriptivePortName().toLowerCase().contains("cp21") || port.getDescriptivePortName().toLowerCase().contains("ch910")) {
 				trackerPort = port;
 				break;
 			}

src/main/java/dev/slimevr/gui/swing/ButtonTimer.java

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui.swing;
 
 import java.util.Timer;
 import java.util.TimerTask;

src/main/java/dev/slimevr/gui/swing/EJBag.java

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui.swing;
 
 import java.awt.GridBagLayout;
 

src/main/java/dev/slimevr/gui/swing/EJBagNoStretch.java

@@ -0,0 +1,33 @@
+package dev.slimevr.gui.swing;
+
+import java.awt.Container;
+import java.awt.Dimension;
+import java.awt.GridBagLayout;
+
+public class EJBagNoStretch extends EJPanel {
+	
+	public EJBagNoStretch(boolean stretchVertical, boolean stretchHorizontal) {
+		super(new EGridBagLayoutNoStretch(stretchVertical, stretchHorizontal));
+	}
+	
+	private static class EGridBagLayoutNoStretch extends GridBagLayout {
+		
+		private final boolean stretchVertical;
+		private final boolean stretchHorizontal;
+		
+		public EGridBagLayoutNoStretch(boolean stretchVertical, boolean stretchHorizontal) {
+			this.stretchVertical = stretchVertical;
+			this.stretchHorizontal = stretchHorizontal;
+		}
+		
+		@Override
+		public Dimension maximumLayoutSize(Container target) {
+			Dimension pref = preferredLayoutSize(target);
+			if(stretchVertical)
+				pref.height = Integer.MAX_VALUE;
+			if(stretchHorizontal)
+				pref.width = Integer.MAX_VALUE;
+			return pref;
+	    }
+	}
+}

src/main/java/dev/slimevr/gui/swing/EJBox.java

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui.swing;
 
 import javax.swing.BoxLayout;
 

src/main/java/dev/slimevr/gui/swing/EJBoxNoStretch.java

@@ -0,0 +1,36 @@
+package dev.slimevr.gui.swing;
+
+import java.awt.Container;
+import java.awt.Dimension;
+
+import javax.swing.BoxLayout;
+
+public class EJBoxNoStretch extends EJPanel {
+	
+	public EJBoxNoStretch(int layout, boolean stretchVertical, boolean stretchHorizontal) {
+		super();
+		setLayout(new BoxLayoutNoStretch(this, layout, stretchVertical, stretchHorizontal));
+	}
+	
+	private static class BoxLayoutNoStretch extends BoxLayout {
+		
+		private final boolean stretchVertical;
+		private final boolean stretchHorizontal;
+		
+		public BoxLayoutNoStretch(Container target, int axis, boolean stretchVertical, boolean stretchHorizontal) {
+			super(target, axis);
+			this.stretchVertical = stretchVertical;
+			this.stretchHorizontal = stretchHorizontal;
+		}
+		
+		@Override
+		public Dimension maximumLayoutSize(Container target) {
+			Dimension pref = preferredLayoutSize(target);
+			if(stretchVertical)
+				pref.height = Integer.MAX_VALUE;
+			if(stretchHorizontal)
+				pref.width = Integer.MAX_VALUE;
+			return pref;
+	    }
+	}
+}

src/main/java/dev/slimevr/gui/swing/EJPanel.java

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui.swing;
 
 import java.awt.Component;
 import java.awt.Dimension;

src/main/java/dev/slimevr/gui/swing/EJlabel.java

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui.swing;
 
 import javax.swing.JLabel;
 

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

@@ -0,0 +1,139 @@
+package dev.slimevr.poserecorder;
+
+import java.io.BufferedInputStream;
+import java.io.BufferedOutputStream;
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.FileOutputStream;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+import io.eiren.vr.trackers.TrackerPosition;
+
+public final class PoseFrameIO {
+	
+	private PoseFrameIO() {
+		// Do not allow instantiating
+	}
+	
+	public static boolean writeFrames(DataOutputStream outputStream, PoseFrames frames) {
+		try {
+			if(frames != null) {
+				outputStream.writeInt(frames.getTrackerCount());
+				for(PoseFrameTracker tracker : frames.getTrackers()) {
+					
+					outputStream.writeUTF(tracker.name);
+					outputStream.writeInt(tracker.getFrameCount());
+					for(int i = 0; i < tracker.getFrameCount(); i++) {
+						TrackerFrame trackerFrame = tracker.safeGetFrame(i);
+						if(trackerFrame == null) {
+							outputStream.writeInt(0);
+							continue;
+						}
+						
+						outputStream.writeInt(trackerFrame.getDataFlags());
+						
+						if(trackerFrame.hasData(TrackerFrameData.DESIGNATION)) {
+							outputStream.writeUTF(trackerFrame.designation.designation);
+						}
+						
+						if(trackerFrame.hasData(TrackerFrameData.ROTATION)) {
+							outputStream.writeFloat(trackerFrame.rotation.getX());
+							outputStream.writeFloat(trackerFrame.rotation.getY());
+							outputStream.writeFloat(trackerFrame.rotation.getZ());
+							outputStream.writeFloat(trackerFrame.rotation.getW());
+						}
+						
+						if(trackerFrame.hasData(TrackerFrameData.POSITION)) {
+							outputStream.writeFloat(trackerFrame.position.getX());
+							outputStream.writeFloat(trackerFrame.position.getY());
+							outputStream.writeFloat(trackerFrame.position.getZ());
+						}
+					}
+				}
+			} else {
+				outputStream.writeInt(0);
+			}
+		} catch(Exception e) {
+			LogManager.log.severe("Error writing frame to stream", e);
+			return false;
+		}
+		
+		return true;
+	}
+	
+	public static boolean writeToFile(File file, PoseFrames frames) {
+		try(DataOutputStream outputStream = new DataOutputStream(new BufferedOutputStream(new FileOutputStream(file)))) {
+			writeFrames(outputStream, frames);
+		} catch(Exception e) {
+			LogManager.log.severe("Error writing frames to file", e);
+			return false;
+		}
+		
+		return true;
+	}
+	
+	public static PoseFrames readFrames(DataInputStream inputStream) {
+		try {
+			
+			int trackerCount = inputStream.readInt();
+			FastList<PoseFrameTracker> trackers = new FastList<PoseFrameTracker>(trackerCount);
+			for(int i = 0; i < trackerCount; i++) {
+				
+				String name = inputStream.readUTF();
+				int trackerFrameCount = inputStream.readInt();
+				FastList<TrackerFrame> trackerFrames = new FastList<TrackerFrame>(trackerFrameCount);
+				for(int j = 0; j < trackerFrameCount; j++) {
+					int dataFlags = inputStream.readInt();
+					
+					TrackerPosition designation = null;
+					if(TrackerFrameData.DESIGNATION.check(dataFlags)) {
+						designation = TrackerPosition.getByDesignation(inputStream.readUTF());
+					}
+					
+					Quaternion rotation = null;
+					if(TrackerFrameData.ROTATION.check(dataFlags)) {
+						float quatX = inputStream.readFloat();
+						float quatY = inputStream.readFloat();
+						float quatZ = inputStream.readFloat();
+						float quatW = inputStream.readFloat();
+						rotation = new Quaternion(quatX, quatY, quatZ, quatW);
+					}
+					
+					Vector3f position = null;
+					if(TrackerFrameData.POSITION.check(dataFlags)) {
+						float posX = inputStream.readFloat();
+						float posY = inputStream.readFloat();
+						float posZ = inputStream.readFloat();
+						position = new Vector3f(posX, posY, posZ);
+					}
+					
+					trackerFrames.add(new TrackerFrame(designation, rotation, position));
+				}
+				
+				trackers.add(new PoseFrameTracker(name, trackerFrames));
+			}
+			
+			return new PoseFrames(trackers);
+		} catch(Exception e) {
+			LogManager.log.severe("Error reading frame from stream", e);
+		}
+		
+		return null;
+	}
+	
+	public static PoseFrames readFromFile(File file) {
+		try {
+			return readFrames(new DataInputStream(new BufferedInputStream(new FileInputStream(file))));
+		} catch(Exception e) {
+			LogManager.log.severe("Error reading frame from file", e);
+		}
+		
+		return null;
+	}
+}

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

@@ -0,0 +1,239 @@
+package dev.slimevr.poserecorder;
+
+import java.util.Iterator;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import io.eiren.util.collections.FastList;
+import io.eiren.vr.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> {
+	
+	public final String name;
+	
+	private final FastList<TrackerFrame> frames;
+	private int frameCursor = 0;
+	private final int trackerId = Tracker.getNextLocalTrackerId();
+	
+	public PoseFrameTracker(String name, FastList<TrackerFrame> frames) {
+		if(frames == null) {
+			throw new NullPointerException("frames must not be null");
+		}
+		
+		this.name = name != null ? name : "";
+		this.frames = frames;
+	}
+	
+	public PoseFrameTracker(String name, int initialCapacity) {
+		this(name, new FastList<TrackerFrame>(initialCapacity));
+	}
+	
+	public PoseFrameTracker(String name) {
+		this(name, 5);
+	}
+	
+	private int limitCursor() {
+		if(frameCursor < 0 || frames.isEmpty()) {
+			frameCursor = 0;
+		} else if(frameCursor >= frames.size()) {
+			frameCursor = frames.size() - 1;
+		}
+		
+		return frameCursor;
+	}
+	
+	public int setCursor(int index) {
+		frameCursor = index;
+		return limitCursor();
+	}
+	
+	public int incrementCursor(int increment) {
+		frameCursor += increment;
+		return limitCursor();
+	}
+	
+	public int incrementCursor() {
+		return incrementCursor(1);
+	}
+	
+	public int getCursor() {
+		return frameCursor;
+	}
+	
+	public int getFrameCount() {
+		return frames.size();
+	}
+	
+	public TrackerFrame addFrame(int index, TrackerFrame trackerFrame) {
+		frames.add(index, trackerFrame);
+		return trackerFrame;
+	}
+	
+	public TrackerFrame addFrame(int index, Tracker tracker) {
+		return addFrame(index, TrackerFrame.fromTracker(tracker));
+	}
+	
+	public TrackerFrame addFrame(TrackerFrame trackerFrame) {
+		frames.add(trackerFrame);
+		return trackerFrame;
+	}
+	
+	public TrackerFrame addFrame(Tracker tracker) {
+		return addFrame(TrackerFrame.fromTracker(tracker));
+	}
+	
+	public TrackerFrame removeFrame(int index) {
+		TrackerFrame trackerFrame = frames.remove(index);
+		limitCursor();
+		return trackerFrame;
+	}
+	
+	public TrackerFrame removeFrame(TrackerFrame trackerFrame) {
+		frames.remove(trackerFrame);
+		limitCursor();
+		return trackerFrame;
+	}
+	
+	public void clearFrames() {
+		frames.clear();
+		limitCursor();
+	}
+	
+	public void fakeClearFrames() {
+		frames.fakeClear();
+		limitCursor();
+	}
+	
+	public TrackerFrame getFrame(int index) {
+		return frames.get(index);
+	}
+	
+	public TrackerFrame getFrame() {
+		return getFrame(frameCursor);
+	}
+	
+	public TrackerFrame safeGetFrame(int index) {
+		try {
+			return getFrame(index);
+		} catch(Exception e) {
+			return null;
+		}
+	}
+	
+	public TrackerFrame safeGetFrame() {
+		return safeGetFrame(frameCursor);
+	}
+	
+	//#region Tracker Interface Implementation
+	@Override
+	public boolean getRotation(Quaternion store) {
+		TrackerFrame frame = safeGetFrame();
+		if(frame != null && frame.hasData(TrackerFrameData.ROTATION)) {
+			store.set(frame.rotation);
+			return true;
+		}
+		
+		store.set(0, 0, 0, 1);
+		return false;
+	}
+	
+	@Override
+	public boolean getPosition(Vector3f store) {
+		TrackerFrame frame = safeGetFrame();
+		if(frame != null && frame.hasData(TrackerFrameData.POSITION)) {
+			store.set(frame.position);
+			return true;
+		}
+		
+		store.set(0, 0, 0);
+		return false;
+	}
+	
+	@Override
+	public String getName() {
+		return name;
+	}
+	
+	@Override
+	public TrackerStatus getStatus() {
+		return TrackerStatus.OK;
+	}
+	
+	@Override
+	public void loadConfig(TrackerConfig config) {
+		throw new UnsupportedOperationException("PoseFrameTracker does not implement configuration");
+	}
+	
+	@Override
+	public void saveConfig(TrackerConfig config) {
+		throw new UnsupportedOperationException("PoseFrameTracker does not implement configuration");
+	}
+	
+	@Override
+	public float getConfidenceLevel() {
+		return 0;
+	}
+	
+	@Override
+	public void resetFull(Quaternion reference) {
+		throw new UnsupportedOperationException("PoseFrameTracker does not implement calibration");
+	}
+	
+	@Override
+	public void resetYaw(Quaternion reference) {
+		throw new UnsupportedOperationException("PoseFrameTracker does not implement calibration");
+	}
+	
+	@Override
+	public void tick() {
+		throw new UnsupportedOperationException("PoseFrameTracker does not implement this method");
+	}
+	
+	@Override
+	public TrackerPosition getBodyPosition() {
+		TrackerFrame frame = safeGetFrame();
+		return frame == null ? null : frame.designation;
+	}
+	
+	@Override
+	public void setBodyPosition(TrackerPosition position) {
+		throw new UnsupportedOperationException("PoseFrameTracker does not allow setting the body position");
+	}
+	
+	@Override
+	public boolean userEditable() {
+		return false;
+	}
+	
+	@Override
+	public boolean hasRotation() {
+		TrackerFrame frame = safeGetFrame();
+		return frame != null && frame.hasData(TrackerFrameData.ROTATION);
+	}
+	
+	@Override
+	public boolean hasPosition() {
+		TrackerFrame frame = safeGetFrame();
+		return frame != null && frame.hasData(TrackerFrameData.POSITION);
+	}
+	
+	@Override
+	public boolean isComputed() {
+		return true;
+	}
+	//#endregion
+	
+	@Override
+	public Iterator<TrackerFrame> iterator() {
+		return frames.iterator();
+	}
+
+	@Override
+	public int getTrackerId() {
+		return this.trackerId;
+	}
+}

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

@@ -0,0 +1,143 @@
+package dev.slimevr.poserecorder;
+
+import java.util.Iterator;
+import java.util.List;
+import java.util.NoSuchElementException;
+
+import io.eiren.util.collections.FastList;
+import io.eiren.vr.trackers.Tracker;
+
+public final class PoseFrames implements Iterable<TrackerFrame[]> {
+	
+	private final FastList<PoseFrameTracker> trackers;
+	
+	public PoseFrames(FastList<PoseFrameTracker> trackers) {
+		this.trackers = trackers;
+	}
+	
+	public PoseFrames(int initialCapacity) {
+		this.trackers = new FastList<PoseFrameTracker>(initialCapacity);
+	}
+	
+	public PoseFrames() {
+		this(5);
+	}
+	
+	public PoseFrameTracker addTracker(PoseFrameTracker tracker) {
+		trackers.add(tracker);
+		return tracker;
+	}
+	
+	public PoseFrameTracker addTracker(Tracker tracker, int initialCapacity) {
+		return addTracker(new PoseFrameTracker(tracker.getName(), initialCapacity));
+	}
+	
+	public PoseFrameTracker addTracker(Tracker tracker) {
+		return addTracker(tracker, 5);
+	}
+	
+	public PoseFrameTracker removeTracker(int index) {
+		return trackers.remove(index);
+	}
+	
+	public PoseFrameTracker removeTracker(PoseFrameTracker tracker) {
+		trackers.remove(tracker);
+		return tracker;
+	}
+	
+	public void clearTrackers() {
+		trackers.clear();
+	}
+	
+	public void fakeClearTrackers() {
+		trackers.fakeClear();
+	}
+	
+	public int getTrackerCount() {
+		return trackers.size();
+	}
+	
+	public List<PoseFrameTracker> getTrackers() {
+		return trackers;
+	}
+	
+	public int getMaxFrameCount() {
+		int maxFrames = 0;
+		
+		for(int i = 0; i < trackers.size(); i++) {
+			PoseFrameTracker tracker = trackers.get(i);
+			if(tracker != null && tracker.getFrameCount() > maxFrames) {
+				maxFrames = tracker.getFrameCount();
+			}
+		}
+		
+		return maxFrames;
+	}
+	
+	public int getFrames(int frameIndex, TrackerFrame[] buffer) {
+		for(int i = 0; i < trackers.size(); i++) {
+			PoseFrameTracker tracker = trackers.get(i);
+			buffer[i] = tracker != null ? tracker.safeGetFrame(frameIndex) : null;
+		}
+		return trackers.size();
+	}
+	
+	public int getFrames(int frameIndex, List<TrackerFrame> buffer) {
+		for(int i = 0; i < trackers.size(); i++) {
+			PoseFrameTracker tracker = trackers.get(i);
+			buffer.add(i, tracker != null ? tracker.safeGetFrame(frameIndex) : null);
+		}
+		return trackers.size();
+	}
+	
+	public TrackerFrame[] getFrames(int frameIndex) {
+		TrackerFrame[] trackerFrames = new TrackerFrame[trackers.size()];
+		getFrames(frameIndex, trackerFrames);
+		return trackerFrames;
+	}
+	
+	@Override
+	public Iterator<TrackerFrame[]> iterator() {
+		return new PoseFrameIterator(this);
+	}
+	
+	public class PoseFrameIterator implements Iterator<TrackerFrame[]> {
+		
+		private final PoseFrames poseFrame;
+		private final TrackerFrame[] trackerFrameBuffer;
+		
+		private int cursor = 0;
+		
+		public PoseFrameIterator(PoseFrames poseFrame) {
+			this.poseFrame = poseFrame;
+			trackerFrameBuffer = new TrackerFrame[poseFrame.getTrackerCount()];
+		}
+		
+		@Override
+		public boolean hasNext() {
+			if(trackers.isEmpty()) {
+				return false;
+			}
+			
+			for(int i = 0; i < trackers.size(); i++) {
+				PoseFrameTracker tracker = trackers.get(i);
+				if(tracker != null && cursor < tracker.getFrameCount()) {
+					return true;
+				}
+			}
+			
+			return false;
+		}
+		
+		@Override
+		public TrackerFrame[] next() {
+			if(!hasNext()) {
+				throw new NoSuchElementException();
+			}
+			
+			poseFrame.getFrames(cursor++, trackerFrameBuffer);
+			
+			return trackerFrameBuffer;
+		}
+	}
+}

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

@@ -0,0 +1,177 @@
+package dev.slimevr.poserecorder;
+
+import java.util.List;
+import java.util.concurrent.CompletableFuture;
+import java.util.concurrent.ExecutionException;
+import java.util.concurrent.Future;
+
+import org.apache.commons.lang3.tuple.Pair;
+
+import io.eiren.util.ann.VRServerThread;
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+import io.eiren.vr.VRServer;
+import io.eiren.vr.trackers.Tracker;
+
+public class PoseRecorder {
+
+	protected 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) {
+			return;
+		}
+
+		PoseFrames poseFrame = this.poseFrame;
+		List<Pair<Tracker, PoseFrameTracker>> trackers = this.trackers;
+		if (poseFrame == null || trackers == null) {
+			return;
+		}
+
+		if (frameCursor >= numFrames) {
+			// If done and hasn't yet, send finished recording
+			stopFrameRecording();
+			return;
+		}
+		
+		long curTime = System.currentTimeMillis();
+		if (curTime < nextFrameTimeMs) {
+			return;
+		}
+
+		nextFrameTimeMs += frameRecordingInterval;
+
+		// To prevent duplicate frames, make sure the frame time is always in the future
+		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) {
+			// 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");
+		}
+		if(intervalMs < 1) {
+			throw new IllegalArgumentException("intervalMs must at least have a value of 1");
+		}
+		if(trackers == null) {
+			throw new IllegalArgumentException("trackers must not be null");
+		}
+		if(trackers.isEmpty()) {
+			throw new IllegalArgumentException("trackers must have at least one entry");
+		}
+		if(!isReadyToRecord()) {
+			throw new IllegalStateException("PoseRecorder isn't ready to record!");
+		}
+
+		cancelFrameRecording();
+
+		poseFrame = new PoseFrames(trackers.size());
+
+		// Update tracker list
+		this.trackers.ensureCapacity(trackers.size());
+		for(Tracker tracker : trackers) {
+			// Ignore null and computed trackers
+			if(tracker == null || tracker.isComputed()) {
+				continue;
+			}
+
+			// Pair tracker with recording
+			this.trackers.add(Pair.of(tracker, poseFrame.addTracker(tracker, numFrames)));
+		}
+
+		this.frameCursor = 0;
+		this.numFrames = numFrames;
+
+		frameRecordingInterval = 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

@@ -0,0 +1,179 @@
+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;
+
+public final class TrackerFrame implements Tracker {
+	
+	private int dataFlags = 0;
+	
+	public final TrackerPosition designation;
+	public final Quaternion rotation;
+	public final Vector3f position;
+	private final int trackerId = Tracker.getNextLocalTrackerId();
+	
+	public TrackerFrame(TrackerPosition designation, Quaternion rotation, Vector3f position) {
+		this.designation = designation;
+		if(designation != null) {
+			dataFlags |= TrackerFrameData.DESIGNATION.flag;
+		}
+		
+		this.rotation = rotation;
+		if(rotation != null) {
+			dataFlags |= TrackerFrameData.ROTATION.flag;
+		}
+		
+		this.position = position;
+		if(position != null) {
+			dataFlags |= TrackerFrameData.POSITION.flag;
+		}
+	}
+	
+	public static TrackerFrame fromTracker(Tracker tracker) {
+		if(tracker == null) {
+			return null;
+		}
+		
+		// If the tracker is not ready
+		if(tracker.getStatus() != TrackerStatus.OK && tracker.getStatus() != TrackerStatus.BUSY && tracker.getStatus() != TrackerStatus.OCCLUDED) {
+			return null;
+		}
+		
+		// If tracker has no data
+		if(tracker.getBodyPosition() == null && !tracker.hasRotation() && !tracker.hasPosition()) {
+			return null;
+		}
+		
+		Quaternion rotation = null;
+		if(tracker.hasRotation()) {
+			rotation = new Quaternion();
+			if(!tracker.getRotation(rotation)) {
+				// If getting the rotation failed, set it back to null
+				rotation = null;
+			}
+		}
+		
+		Vector3f position = null;
+		if(tracker.hasPosition()) {
+			position = new Vector3f();
+			if(!tracker.getPosition(position)) {
+				// If getting the position failed, set it back to null
+				position = null;
+			}
+		}
+		
+		return new TrackerFrame(tracker.getBodyPosition(), rotation, position);
+	}
+	
+	public int getDataFlags() {
+		return dataFlags;
+	}
+	
+	public boolean hasData(TrackerFrameData flag) {
+		return flag.check(dataFlags);
+	}
+	
+	//#region Tracker Interface Implementation
+	@Override
+	public boolean getRotation(Quaternion store) {
+		if(hasData(TrackerFrameData.ROTATION)) {
+			store.set(rotation);
+			return true;
+		}
+		
+		store.set(0, 0, 0, 1);
+		return false;
+	}
+	
+	@Override
+	public boolean getPosition(Vector3f store) {
+		if(hasData(TrackerFrameData.POSITION)) {
+			store.set(position);
+			return true;
+		}
+		
+		store.set(0, 0, 0);
+		return false;
+	}
+	
+	@Override
+	public String getName() {
+		return "TrackerFrame:/" + (designation != null ? designation.designation : "null");
+	}
+	
+	@Override
+	public TrackerStatus getStatus() {
+		return TrackerStatus.OK;
+	}
+	
+	@Override
+	public void loadConfig(TrackerConfig config) {
+		throw new UnsupportedOperationException("TrackerFrame does not implement configuration");
+	}
+	
+	@Override
+	public void saveConfig(TrackerConfig config) {
+		throw new UnsupportedOperationException("TrackerFrame does not implement configuration");
+	}
+	
+	@Override
+	public float getConfidenceLevel() {
+		return 0;
+	}
+	
+	@Override
+	public void resetFull(Quaternion reference) {
+		throw new UnsupportedOperationException("TrackerFrame does not implement calibration");
+	}
+	
+	@Override
+	public void resetYaw(Quaternion reference) {
+		throw new UnsupportedOperationException("TrackerFrame does not implement calibration");
+	}
+	
+	@Override
+	public void tick() {
+		throw new UnsupportedOperationException("TrackerFrame does not implement this method");
+	}
+	
+	@Override
+	public TrackerPosition getBodyPosition() {
+		return designation;
+	}
+	
+	@Override
+	public void setBodyPosition(TrackerPosition position) {
+		throw new UnsupportedOperationException("TrackerFrame does not allow setting the body position");
+	}
+	
+	@Override
+	public boolean userEditable() {
+		return false;
+	}
+	
+	@Override
+	public boolean hasRotation() {
+		return hasData(TrackerFrameData.ROTATION);
+	}
+	
+	@Override
+	public boolean hasPosition() {
+		return hasData(TrackerFrameData.POSITION);
+	}
+	
+	@Override
+	public boolean isComputed() {
+		return true;
+	}
+	//#endregion
+
+	@Override
+	public int getTrackerId() {
+		return this.trackerId;
+	}
+}

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

@@ -0,0 +1,19 @@
+package dev.slimevr.poserecorder;
+
+public enum TrackerFrameData {
+	
+	DESIGNATION(0),
+	ROTATION(1),
+	POSITION(2),
+	;
+	
+	public final int flag;
+	
+	TrackerFrameData(int id) {
+		this.flag = 1 << id;
+	}
+	
+	public boolean check(int dataFlags) {
+		return (dataFlags & this.flag) != 0;
+	}
+}

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

@@ -0,0 +1,276 @@
+package dev.slimevr.posestreamer;
+
+import java.io.BufferedWriter;
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.FileOutputStream;
+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;
+
+public class BVHFileStream extends PoseDataStream {
+
+	private static final int LONG_MAX_VALUE_DIGITS = Long.toString(Long.MAX_VALUE).length();
+	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 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 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()) {
+			writer.write(indentLevel + "End Site\n");
+		} else {
+			writer.write((level > 0 ? indentLevel + "JOINT " : "ROOT ") + node.getName() + "\n");
+		}
+		writer.write(indentLevel + "{\n");
+
+		// Ignore the root offset and original root offset
+		if (level > 0 && node.wrappedNode.getParent() != null) {
+			Vector3f offset = node.localTransform.getTranslation();
+			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()) {
+			// Only give position for root
+			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 (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) {
+			throw new NullPointerException("skeleton must not be null");
+		}
+		if (streamer == null) {
+			throw new NullPointerException("streamer must not be null");
+		}
+
+		writer.write("HIERARCHY\n");
+		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;
+			// 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");
+	}
+
+	// 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");
+		}
+
+		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);
+			}
+		}
+	}
+
+	@Override
+	public void writeFrame(HumanSkeleton skeleton) throws IOException {
+		if (skeleton == null) {
+			throw new NullPointerException("skeleton must not be null");
+		}
+
+		TransformNodeWrapper rootNode = wrapSkeletonIfNew(skeleton);
+
+		Vector3f rootPos = rootNode.worldTransform.getTranslation();
+
+		// Write root position
+		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;
+			// Flush before anything else
+			writer.flush();
+			// Seek to the count offset
+			fileOutputStream.getChannel().position(frameCountOffset);
+			// Overwrite the count with a new value
+			writer.write(Long.toString(frameCount));
+		}
+	}
+
+	@Override
+	public void close() throws IOException {
+		writer.close();
+		super.close();
+	}
+}

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

@@ -0,0 +1,45 @@
+package dev.slimevr.posestreamer;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.FileOutputStream;
+import java.io.IOException;
+import java.io.OutputStream;
+
+import io.eiren.vr.processor.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();
+		closed = true;
+	}
+}

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

@@ -0,0 +1,116 @@
+package dev.slimevr.posestreamer;
+
+import java.io.IOException;
+
+import io.eiren.util.ann.VRServerThread;
+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) {
+			return;
+		}
+
+		HumanSkeleton skeleton = this.skeleton;
+		if (skeleton == null) {
+			return;
+		}
+			
+		long curTime = System.currentTimeMillis();
+		if (curTime < nextFrameTimeMs) {
+			return;
+		}
+			
+		nextFrameTimeMs += frameRecordingInterval;
+
+		// To prevent duplicate frames, make sure the frame time is always in the future
+		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) {
+			// Make sure the stream is open before trying to write
+			if (poseFileStream.isClosed()) {
+				return;
+			}
+			
+			try {
+				poseFileStream.writeFrame(skeleton);
+			} 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) {
+			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 io.eiren.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 io.eiren.util.collections.FastList;
+import io.eiren.vr.processor.TransformNode;
+
+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/io/eiren/gui/SkeletonConfig.java

@@ -1,175 +0,0 @@
-package io.eiren.gui;
-
-import java.awt.event.MouseEvent;
-import java.util.HashMap;
-import java.util.Map;
-
-import javax.swing.JButton;
-import javax.swing.JLabel;
-import javax.swing.event.MouseInputAdapter;
-
-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 EJBag {
-	
-	private final VRServer server;
-	private final VRServerGUI gui;
-	private Map<String, SkeletonLabel> labels = new HashMap<>();
-	
-	public SkeletonConfig(VRServer server, VRServerGUI gui) {
-		super();
-		this.server = server;
-		this.gui = gui;
-
-		setAlignmentY(TOP_ALIGNMENT);
-		server.humanPoseProcessor.addSkeletonUpdatedCallback(this::skeletonUpdated);
-	}
-	
-	@ThreadSafe
-	public void skeletonUpdated(HumanSkeleton newSkeleton) {
-		java.awt.EventQueue.invokeLater(() -> {
-			removeAll();
-			
-			int row = 0;
-			
-			add(new TimedResetButton("Reset All", "All"), s(c(1, row, 1), 3, 1));
-			row++;
-			
-			add(new JLabel("Chest"), c(0, row, 1));
-			add(new AdjButton("+", "Chest", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Chest"), c(2, row, 1));
-			add(new AdjButton("-", "Chest", -0.01f), c(3, row, 1));
-			add(new ResetButton("Reset", "Chest"), c(4, row, 1));
-			row++;
-			
-			add(new JLabel("Waist"), c(0, row, 1));
-			add(new AdjButton("+", "Waist", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Waist"), c(2, row, 1));
-			add(new AdjButton("-", "Waist", -0.01f), c(3, row, 1));
-			add(new TimedResetButton("Reset", "Waist"), c(4, row, 1));
-			row++;
-
-			add(new JLabel("Hips width"), c(0, row, 1));
-			add(new AdjButton("+", "Hips width", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Hips width"), c(2, row, 1));
-			add(new AdjButton("-", "Hips width", -0.01f), c(3, row, 1));
-			add(new ResetButton("Reset", "Hips width"), c(4, row, 1));
-			row++;
-
-			add(new JLabel("Legs length"), c(0, row, 1));
-			add(new AdjButton("+", "Legs length", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Legs length"), c(2, row, 1));
-			add(new AdjButton("-", "Legs length", -0.01f), c(3, row, 1));
-			add(new TimedResetButton("Reset", "Legs length"), c(4, row, 1));
-			row++;
-
-			add(new JLabel("Knee height"), c(0, row, 1));
-			add(new AdjButton("+", "Knee height", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Knee height"), c(2, row, 1));
-			add(new AdjButton("-", "Knee height", -0.01f), c(3, row, 1));
-			add(new TimedResetButton("Reset", "Knee height"), c(4, row, 1));
-			row++;
-
-			add(new JLabel("Foot length"), c(0, row, 1));
-			add(new AdjButton("+", "Foot length", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Foot length"), c(2, row, 1));
-			add(new AdjButton("-", "Foot length", -0.01f), c(3, row, 1));
-			add(new ResetButton("Reset", "Foot length"), c(4, row, 1));
-			row++;
-
-			add(new JLabel("Head offset"), c(0, row, 1));
-			add(new AdjButton("+", "Head", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Head"), c(2, row, 1));
-			add(new AdjButton("-", "Head", -0.01f), c(3, row, 1));
-			add(new ResetButton("Reset", "Head"), c(4, row, 1));
-			row++;
-
-			add(new JLabel("Neck length"), c(0, row, 1));
-			add(new AdjButton("+", "Neck", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Neck"), c(2, row, 1));
-			add(new AdjButton("-", "Neck", -0.01f), c(3, row, 1));
-			add(new ResetButton("Reset", "Neck"), c(4, row, 1));
-			row++;
-			
-			add(new JLabel("Virtual waist"), c(0, row, 1));
-			add(new AdjButton("+", "Virtual waist", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Virtual waist"), c(2, row, 1));
-			add(new AdjButton("-", "Virtual waist", -0.01f), c(3, row, 1));
-			add(new ResetButton("Reset", "Virtual waist"), c(4, row, 1));
-			row++;
-			
-			gui.refresh();
-		});
-	}
-	
-	private void change(String joint, float diff) {
-		float current = server.humanPoseProcessor.getSkeletonConfig(joint);
-		server.humanPoseProcessor.setSkeletonConfig(joint, current + diff);
-		server.saveConfig();
-		labels.get(joint).setText(StringUtils.prettyNumber((current + diff) * 100, 0));
-	}
-	
-	private void reset(String joint) {
-		server.humanPoseProcessor.resetSkeletonConfig(joint);
-		server.saveConfig();
-		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));
-			});
-		}
-	}
-	
-	private class SkeletonLabel extends JLabel {
-		
-		public SkeletonLabel(String joint) {
-			super(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint) * 100, 0));
-			labels.put(joint, this);
-		}
-	}
-	
-	private class AdjButton extends JButton {
-		
-		public AdjButton(String text, String joint, float diff) {
-			super(text);
-			addMouseListener(new MouseInputAdapter() {
-				@Override
-				public void mouseClicked(MouseEvent e) {
-					change(joint, diff);
-				}
-			});
-		}
-	}
-	
-	private class ResetButton extends JButton {
-		
-		public ResetButton(String text, String joint) {
-			super(text);
-			addMouseListener(new MouseInputAdapter() {
-				@Override
-				public void mouseClicked(MouseEvent e) {
-					reset(joint);
-				}
-			});
-		}
-	}
-	
-	private class TimedResetButton extends JButton {
-		
-		public TimedResetButton(String text, String joint) {
-			super(text);
-			addMouseListener(new MouseInputAdapter() {
-				@Override
-				public void mouseClicked(MouseEvent e) {
-					ButtonTimer.runTimer(TimedResetButton.this, 3, text, () -> reset(joint));
-				}
-			});
-		}
-	}
-}

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

@@ -1,251 +0,0 @@
-package io.eiren.gui;
-
-import java.awt.Color;
-import java.awt.GridBagConstraints;
-import java.awt.event.ActionEvent;
-import java.awt.event.ActionListener;
-import java.util.List;
-
-import javax.swing.BorderFactory;
-import javax.swing.Box;
-import javax.swing.BoxLayout;
-import javax.swing.JComboBox;
-import javax.swing.JLabel;
-
-import com.jme3.math.FastMath;
-import com.jme3.math.Quaternion;
-import com.jme3.math.Vector3f;
-
-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.processor.TrackerBodyPosition;
-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.TrackerWithBattery;
-import io.eiren.vr.trackers.TrackerWithTPS;
-
-public class TrackersList extends EJBox {
-	
-	Quaternion q = new Quaternion();
-	Vector3f v = new Vector3f();
-	float[] angles = new float[3];
-	
-	private List<TrackerRow> trackers = new FastList<>();
-	
-	private final VRServer server;
-	private final VRServerGUI gui;
-
-	public TrackersList(VRServer server, VRServerGUI gui) {
-		super(BoxLayout.PAGE_AXIS);
-		this.server = server;
-		this.gui = gui;
-
-		setAlignmentY(TOP_ALIGNMENT);
-		
-		server.addNewTrackerConsumer(this::newTrackerAdded);
-	}
-
-	@AWTThread
-	private void build() {
-		removeAll();
-		
-		trackers.sort((tr1, tr2) -> getTrackerSort(tr1.t) - getTrackerSort(tr2.t));
-		
-		Class<? extends Tracker> currentClass = null;
-		
-		for(int i = 0; i < trackers.size(); ++i) {
-			add(Box.createVerticalStrut(3));
-			TrackerRow tr = trackers.get(i);
-			Tracker t = tr.t;
-			if(currentClass != t.getClass()) {
-				currentClass = t.getClass();
-				add(new JLabel(currentClass.getSimpleName()));
-			}
-
-			tr.build();
-		}
-		validate();
-		gui.refresh();
-	}
-	
-	@ThreadSafe
-	public void updateTrackers() {
-		java.awt.EventQueue.invokeLater(() -> {
-			for(int i = 0; i < trackers.size(); ++i)
-				trackers.get(i).update();
-		});
-	}
-	
-	@ThreadSafe
-	public void newTrackerAdded(Tracker t) {
-		java.awt.EventQueue.invokeLater(() -> {
-			trackers.add(new TrackerRow(t));
-			build();
-		});
-	}
-	
-	private class TrackerRow extends EJBag {
-		
-		final Tracker t;
-		JLabel position;
-		JLabel rotation;
-		JLabel status;
-		JLabel tps;
-		JLabel bat;
-		JLabel ping;
-		JLabel raw;
-		JLabel adj;
-		JLabel adjYaw;
-		
-		@AWTThread
-		public TrackerRow(Tracker t) {
-			super();
-			this.t = t;
-		}
-
-		@SuppressWarnings("unchecked")
-		@AWTThread
-		public TrackerRow build() {
-			removeAll();
-			add(new JLabel(t.getName()), s(c(0, 0, 0, GridBagConstraints.FIRST_LINE_START), 4, 1));
-			if(t.userEditable()) {
-				TrackerConfig cfg = server.getTrackerConfig(t);
-				JComboBox<String> desSelect;
-				add(desSelect = new JComboBox<>(), s(c(0, 1, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
-				for(TrackerBodyPosition p : TrackerBodyPosition.values) {
-					desSelect.addItem(p.name());
-				}
-				if(cfg.designation != null) {
-					TrackerBodyPosition p = TrackerBodyPosition.getByDesignation(cfg.designation);
-					if(p != null)
-						desSelect.setSelectedItem(p.name());
-				}
-				desSelect.addActionListener(new ActionListener() {
-					@Override
-					public void actionPerformed(ActionEvent e) {
-						TrackerBodyPosition p = TrackerBodyPosition.valueOf(String.valueOf(desSelect.getSelectedItem()));
-						t.setBodyPosition(p);
-						server.trackerUpdated(t);
-					}
-				});
-				Tracker realTracker = t;
-				if(t instanceof ReferenceAdjustedTracker<?>)
-					realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
-				if(realTracker instanceof IMUTracker) {
-					IMUTracker imu = (IMUTracker) realTracker;
-					TrackerMountingRotation tr = imu.getMountingRotation();
-					JComboBox<String> mountSelect;
-					add(mountSelect = new JComboBox<>(), s(c(2, 1, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
-					for(TrackerMountingRotation p : TrackerMountingRotation.values) {
-						mountSelect.addItem(p.name());
-					}
-					if(tr != null) {
-						mountSelect.setSelectedItem(tr.name());
-					} else {
-						mountSelect.setSelectedItem(TrackerMountingRotation.BACK.name());
-					}
-					mountSelect.addActionListener(new ActionListener() {
-						@Override
-						public void actionPerformed(ActionEvent e) {
-							TrackerMountingRotation tr = TrackerMountingRotation.valueOf(String.valueOf(mountSelect.getSelectedItem()));
-							imu.setMountingRotation(tr);
-							server.trackerUpdated(t);
-						}
-					});
-				}
-			}
-			add(new JLabel("Rotation"), c(0, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Postion"), c(1, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Ping"), c(2, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("TPS"), c(3, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(rotation = new JLabel("0 0 0"), c(0, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			add(position = new JLabel("0 0 0"), c(1, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			add(ping = new JLabel(""), c(2, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			if(t instanceof TrackerWithTPS) {
-				add(tps = new JLabel("0"), c(3, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			} else {
-				add(new JLabel(""), c(3, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			}
-			add(new JLabel("Status:"), c(0, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Battery:"), c(2, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(bat = new JLabel("0"), c(3, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Raw:"), c(0, 5, 0, GridBagConstraints.FIRST_LINE_START));
-			add(raw = new JLabel("0 0 0 0"), s(c(1, 5, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
-			
-			if(t instanceof ReferenceAdjustedTracker) {
-				add(new JLabel("Adj:"), c(0, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(adj = new JLabel("0 0 0 0"), c(1, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(new JLabel("AdjY:"), c(2, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(adjYaw = new JLabel("0 0 0 0"), c(3, 6, 0, GridBagConstraints.FIRST_LINE_START));
-			}
-
-			setBorder(BorderFactory.createLineBorder(new Color(0x663399), 4, true));
-			TrackersList.this.add(this);
-			return this;
-		}
-
-		@SuppressWarnings("unchecked")
-		@AWTThread
-		public void update() {
-			if(position == null)
-				return;
-			t.getRotation(q);
-			t.getPosition(v);
-			q.toAngles(angles);
-			
-			position.setText(StringUtils.prettyNumber(v.x, 1)
-					+ " " + StringUtils.prettyNumber(v.y, 1)
-					+ " " + StringUtils.prettyNumber(v.z, 1));
-			rotation.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
-					+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
-					+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
-			status.setText(t.getStatus().toString().toLowerCase());
-			
-			if(t instanceof TrackerWithTPS) {
-				tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) t).getTPS(), 1));
-			}
-			if(t instanceof TrackerWithBattery)
-				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) t).getBatteryVoltage(), 1));
-			Tracker t2 = t;
-			if(t instanceof ReferenceAdjustedTracker) {
-				t2 = ((ReferenceAdjustedTracker<Tracker>) t).getTracker();
-				((ReferenceAdjustedTracker<Tracker>) t).adjustmentAttachment.toAngles(angles);
-				adj.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
-						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
-						+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
-				((ReferenceAdjustedTracker<Tracker>) t).adjustmentYaw.toAngles(angles);
-				adjYaw.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
-						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
-						+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
-			}
-			if(t2 instanceof IMUTracker)
-				ping.setText(String.valueOf(((IMUTracker) t2).ping));
-			t2.getRotation(q);
-			raw.setText(StringUtils.prettyNumber(q.getX(), 4)
-					+ " " + StringUtils.prettyNumber(q.getY(), 4)
-					+ " " + StringUtils.prettyNumber(q.getZ(), 4)
-					+ " " + StringUtils.prettyNumber(q.getW(), 4));
-		}
-	}
-	
-	private static int getTrackerSort(Tracker t) {
-		if(t instanceof HMDTracker)
-			return 0;
-		if(t instanceof ComputedTracker)
-			return 1;
-		if(t instanceof IMUTracker)
-			return 2;
-		if(t instanceof ReferenceAdjustedTracker)
-			return 5;
-		return 1000;
-	}
-}

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

@@ -1,200 +0,0 @@
-package io.eiren.gui;
-
-import javax.swing.*;
-import javax.swing.border.EmptyBorder;
-import javax.swing.event.MouseInputAdapter;
-
-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.bridge.NamedPipeVRBridge;
-
-import java.awt.Component;
-import java.awt.Container;
-import java.awt.Font;
-import java.awt.event.MouseEvent;
-
-import static javax.swing.BoxLayout.PAGE_AXIS;
-import static javax.swing.BoxLayout.LINE_AXIS;
-
-public class VRServerGUI extends JFrame {
-	
-	public final VRServer server;
-	private final TrackersList trackersList;
-	private final SkeletonList skeletonList;
-	private JButton resetButton;
-	private JScrollPane scroll;
-	private EJBox pane;
-	
-	private float zoom = 1.5f;
-	private float initZoom = zoom;
-	
-	@AWTThread
-	public VRServerGUI(VRServer server) {
-		super("SlimeVR Server (" + Main.VERSION + ")");
-		//increaseFontSize();
-		
-		this.server = server;
-		
-		this.zoom = server.config.getFloat("zoom", zoom);
-		this.initZoom = zoom;
-		setDefaultFontSize(zoom);
-		// All components should be constructed to the current zoom level by default
-		
-		setDefaultCloseOperation(EXIT_ON_CLOSE);
-		getContentPane().setLayout(new BoxLayout(getContentPane(), PAGE_AXIS));
-		
-		this.trackersList = new TrackersList(server, this);
-		this.skeletonList = new SkeletonList(server, this);
-		
-		add(scroll = new JScrollPane(pane = new EJBox(PAGE_AXIS), ScrollPaneConstants.VERTICAL_SCROLLBAR_AS_NEEDED, ScrollPaneConstants.HORIZONTAL_SCROLLBAR_AS_NEEDED));
-		
-		build();
-	}
-	
-	public float getZoom() {
-		return this.zoom;
-	}
-	
-	public void refresh() {
-		// Pack and display
-		pack();
-		setVisible(true);
-		java.awt.EventQueue.invokeLater(new Runnable() {
-			@Override
-			public void run() {
-				repaint();
-			}
-		});
-	}
-	
-	@AWTThread
-	private void build() {
-		pane.removeAll();
-		
-		NamedPipeVRBridge npvb = server.getVRBridge(NamedPipeVRBridge.class);
-		
-		pane.add(new EJBox(LINE_AXIS) {{
-			setBorder(new EmptyBorder(i(5)));
-			
-			add(Box.createHorizontalGlue());
-			add(resetButton = new JButton("RESET") {{
-				addMouseListener(new MouseInputAdapter() {
-					@Override
-					public void mouseClicked(MouseEvent e) {
-						reset();
-					}
-				});
-			}});
-			add(Box.createHorizontalGlue());
-			if(npvb != null) {
-				add(new JButton(npvb.isOneTrackerMode() ? "Trackers: 1" : "Trackers: 3") {{
-					addMouseListener(new MouseInputAdapter() {
-						@Override
-						public void mouseClicked(MouseEvent e) {
-							npvb.setSpawnOneTracker(!npvb.isOneTrackerMode());
-							setText(npvb.isOneTrackerMode() ? "Trackers: 1" : "Trackers: 3");
-						}
-					});
-				}});
-				add(Box.createHorizontalStrut(10));
-			}
-			add(new JButton("GUI Zoom (x" + StringUtils.prettyNumber(zoom, 2) + ")") {{
-				addMouseListener(new MouseInputAdapter() {
-					@Override
-					public void mouseClicked(MouseEvent e) {
-						guiZoom();
-						setText("GUI Zoom (x" + StringUtils.prettyNumber(zoom, 2) + ")");
-					}
-				});
-			}});
-			add(Box.createHorizontalStrut(10));
-			add(new JButton("WiFi") {{
-				addMouseListener(new MouseInputAdapter() {
-					@SuppressWarnings("unused")
-					@Override
-					public void mouseClicked(MouseEvent e) {
-						new WiFiWindow(VRServerGUI.this);
-					}
-				});
-			}});
-			add(Box.createHorizontalStrut(10));
-		}});
-		
-		pane.add(new EJBox(LINE_AXIS) {{
-			setBorder(new EmptyBorder(i(5)));
-			add(new EJBox(PAGE_AXIS) {{
-				setAlignmentY(TOP_ALIGNMENT);
-				
-				add(new JLabel("Trackers"));
-				add(trackersList);
-				add(Box.createVerticalGlue());
-			}});
-
-			add(new EJBox(PAGE_AXIS) {{
-				setAlignmentY(TOP_ALIGNMENT);
-				add(new JLabel("Body proportions"));
-				add(new SkeletonConfig(server, VRServerGUI.this));
-				add(new JLabel("Skeleton data"));
-				add(skeletonList);
-				add(Box.createVerticalGlue());
-			}});
-		}});
-		
-		refresh();
-		setLocationRelativeTo(null);
-		
-		server.addOnTick(trackersList::updateTrackers);
-		server.addOnTick(skeletonList::updateBones);
-	}
-	
-	// For now only changes font size, but should change fixed components size in the future too
-	private void guiZoom() {
-		if(zoom <= 1.0f) {
-			zoom = 1.5f;
-		} else if(zoom <= 1.5f) {
-			zoom = 1.75f;
-		} else if(zoom <= 1.75f) {
-			zoom = 2.0f;
-		} else if(zoom <= 2.0f) {
-			zoom = 2.5f;
-		} else {
-			zoom = 1.0f;
-		}
-		processNewZoom(zoom / initZoom, pane);
-		refresh();
-		server.config.setProperty("zoom", zoom);
-		server.saveConfig();
-	}
-	
-	private static void processNewZoom(float zoom, Component comp) {
-		if(comp.isFontSet()) {
-			Font newFont = new ScalableFont(comp.getFont(), zoom);
-			comp.setFont(newFont);
-		}
-		if(comp instanceof Container) {
-			Container cont = (Container) comp;
-			for(Component child : cont.getComponents())
-				processNewZoom(zoom, child);
-		}
-	}
-	
-	private static void setDefaultFontSize(float zoom) {
-		java.util.Enumeration<Object> keys = UIManager.getDefaults().keys();
-		while(keys.hasMoreElements()) {
-			Object key = keys.nextElement();
-			Object value = UIManager.get(key);
-			if(value instanceof javax.swing.plaf.FontUIResource) {
-				javax.swing.plaf.FontUIResource f = (javax.swing.plaf.FontUIResource) value;
-				javax.swing.plaf.FontUIResource f2 = new javax.swing.plaf.FontUIResource(f.deriveFont(f.getSize() * zoom));
-				UIManager.put(key, f2);
-			}
-		}
-	}
-	
-	@AWTThread
-	private void reset() {
-		ButtonTimer.runTimer(resetButton, 3, "RESET", server::resetTrackers);
-	}
-}

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

@@ -0,0 +1,63 @@
+package io.eiren.vr;
+
+import com.melloware.jintellitype.JIntellitype;
+import com.melloware.jintellitype.JIntellitypeException;
+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/io/eiren/vr/Main.java

@@ -1,17 +1,24 @@
 package io.eiren.vr;
 
 import java.io.File;
+import java.io.IOException;
+import java.net.ServerSocket;
 
-import io.eiren.gui.VRServerGUI;
+import javax.swing.JOptionPane;
+
+import org.apache.commons.lang3.JavaVersion;
+import org.apache.commons.lang3.SystemUtils;
+
+import dev.slimevr.gui.VRServerGUI;
 import io.eiren.util.logging.LogManager;
 
-
 public class Main {
 	
-	public static String VERSION = "0.0.9";
+	public static String VERSION = "0.1.2";
 	
 	public static VRServer vrServer;
 	
+	@SuppressWarnings("unused")
 	public static void main(String[] args) {
 		System.setProperty("awt.useSystemAAFontSettings", "on");
 		System.setProperty("swing.aatext", "true");
@@ -22,13 +29,36 @@ public class Main {
 		} catch(Exception e1) {
 			e1.printStackTrace();
 		}
+
+		if (!SystemUtils.isJavaVersionAtLeast(JavaVersion.JAVA_11)) {
+			LogManager.log.severe("SlimeVR start-up error! A minimum of Java 11 is required.");
+			JOptionPane.showMessageDialog(null, "SlimeVR start-up error! A minimum of Java 11 is required.", "SlimeVR: Java Runtime Mismatch", JOptionPane.ERROR_MESSAGE);
+			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();
+			new Keybinding(vrServer);
 			new VRServerGUI(vrServer);
 		} catch(Throwable e) {
 			e.printStackTrace();
+			try {
+				Thread.sleep(2000L);
+			} catch(InterruptedException e2) {
+				e.printStackTrace();
+			}
+			System.exit(1); // Exit in case error happened on init and window not appeared, but some thread started
 		} finally {
 			try {
 				Thread.sleep(2000L);

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

@@ -2,6 +2,7 @@ package io.eiren.vr;
 
 import java.io.File;
 import java.io.FileInputStream;
+import java.io.FileNotFoundException;
 import java.io.FileOutputStream;
 import java.io.IOException;
 import java.net.InetAddress;
@@ -14,16 +15,20 @@ import java.util.Queue;
 import java.util.concurrent.LinkedBlockingQueue;
 import java.util.function.Consumer;
 
+import dev.slimevr.bridge.Bridge;
+import dev.slimevr.bridge.NamedPipeBridge;
+import dev.slimevr.bridge.SteamVRPipeInputBridge;
+import dev.slimevr.bridge.VMCBridge;
+import dev.slimevr.bridge.WebSocketVRBridge;
+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.bridge.NamedPipeVRBridge;
-import io.eiren.vr.bridge.VMCBridge;
-import io.eiren.vr.bridge.VRBridge;
 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;
@@ -35,14 +40,15 @@ public class VRServer extends Thread {
 	
 	private final List<Tracker> trackers = new FastList<>();
 	public final HumanPoseProcessor humanPoseProcessor;
-	private final TrackersUDPServer trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker);
-	private final List<VRBridge> bridges = new FastList<>();
+	private final TrackersUDPServer trackersServer;
+	private final List<Bridge> bridges = new FastList<>();
 	private final Queue<Runnable> tasks = new LinkedBlockingQueue<>();
 	private final Map<String, TrackerConfig> configuration = new HashMap<>();
 	public final YamlFile config = new YamlFile();
 	public final HMDTracker hmdTracker;
 	private final List<Consumer<Tracker>> newTrackersConsumers = new FastList<>();
 	private final List<Runnable> onTick = new FastList<>();
+	private final List<? extends ShareableTracker> shareTrackers;
 	
 	public VRServer() {
 		super("VRServer");
@@ -51,17 +57,38 @@ public class VRServer extends Thread {
 		hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier debugging
 		// TODO Multiple processors
 		humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker);
-		List<? extends Tracker> shareTrackers = humanPoseProcessor.getComputedTrackers();
+		shareTrackers = humanPoseProcessor.getComputedTrackers();
 		
-		// Create named pipe bridge for SteamVR driver
-		NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers, this);
-		tasks.add(() -> driverBridge.start());
-		bridges.add(driverBridge);
+		// Start server for SlimeVR trackers
+		trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker);
+		
+		// OpenVR bridge currently only supports Windows
+		if(OperatingSystem.getCurrentPlatform() == OperatingSystem.WINDOWS) {
+			/*
+			// Create named pipe bridge for SteamVR driver
+			NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers, this);
+			tasks.add(() -> driverBridge.startBridge());
+			bridges.add(driverBridge);
+			//*/
+			// Create named pipe bridge for SteamVR input
+			SteamVRPipeInputBridge steamVRInput = new SteamVRPipeInputBridge(this);
+			tasks.add(() -> steamVRInput.startBridge());
+			bridges.add(steamVRInput);
+			//*/
+			NamedPipeBridge driverBridge = new NamedPipeBridge(hmdTracker, "steamvr", "SteamVR Driver Bridge", "\\\\.\\pipe\\SlimeVRDriver", shareTrackers);
+			tasks.add(() -> driverBridge.startBridge());
+			bridges.add(driverBridge);
+		}
+		
+		// Create WebSocket server
+		WebSocketVRBridge wsBridge = new WebSocketVRBridge(hmdTracker, shareTrackers, this);
+		tasks.add(() -> wsBridge.startBridge());
+		bridges.add(wsBridge);
 		
 		// Create VMCBridge
 		try {
 			VMCBridge vmcBridge = new VMCBridge(39539, 39540, InetAddress.getLocalHost());
-			tasks.add(() -> vmcBridge.start());
+			tasks.add(() -> vmcBridge.startBridge());
 			bridges.add(vmcBridge);
 		} catch(UnknownHostException e) {
 			e.printStackTrace();
@@ -72,13 +99,21 @@ public class VRServer extends Thread {
 		for(int i = 0; i < shareTrackers.size(); ++i)
 			registerTracker(shareTrackers.get(i));
 	}
+	
+	public boolean hasBridge(Class<? extends Bridge> bridgeClass) {
+		for(int i = 0; i < bridges.size(); ++i) {
+			if(bridgeClass.isAssignableFrom(bridges.get(i).getClass()))
+				return true;
+		}
+		return false;
+	}
 
 	@ThreadSafe
-	public <E extends VRBridge> E getVRBridge(Class<E> cls) {
+	public <E extends Bridge> E getVRBridge(Class<E> bridgeClass) {
 		for(int i = 0; i < bridges.size(); ++i) {
-			VRBridge b = bridges.get(i);
-			if(cls.isInstance(b))
-				return cls.cast(b);
+			Bridge b = bridges.get(i);
+			if(bridgeClass.isAssignableFrom(b.getClass()))
+				return bridgeClass.cast(b);
 		}
 		return null;
 	}
@@ -88,7 +123,7 @@ public class VRServer extends Thread {
 		synchronized(configuration) {
 			TrackerConfig config = configuration.get(tracker.getName());
 			if(config == null) {
-				config = new TrackerConfig(tracker.getName());
+				config = new TrackerConfig(tracker);
 				configuration.put(tracker.getName(), config);
 			}
 			return config;
@@ -98,8 +133,8 @@ public class VRServer extends Thread {
 	private void loadConfig() {
 		try {
 			config.load(new FileInputStream(new File("vrconfig.yml")));
-		} catch(IOException e) {
-			e.printStackTrace();
+		} catch(FileNotFoundException e) {
+			// Config file didn't exist, is not an error
 		} catch(YamlException e) {
 			e.printStackTrace();
 		}
@@ -143,7 +178,7 @@ public class VRServer extends Thread {
 	}
 
 	@ThreadSafe
-	public void saveConfig() {
+	public synchronized void saveConfig() {
 		List<YamlNode> nodes = config.getNodeList("trackers", null);
 		List<Map<String, Object>> trackersConfig = new FastList<>(nodes.size());
 		for(int i = 0; i < nodes.size(); ++i) {
@@ -189,12 +224,13 @@ public class VRServer extends Thread {
 					break;
 				task.run();
 			} while(true);
-			
 			for(int i = 0; i < onTick.size(); ++i) {
 				this.onTick.get(i).run();
 			}
 			for(int i = 0; i < bridges.size(); ++i)
 				bridges.get(i).dataRead();
+			for(int i = 0; i < trackers.size(); ++i)
+				trackers.get(i).tick();
 			humanPoseProcessor.update();
 			for(int i = 0; i < bridges.size(); ++i)
 				bridges.get(i).dataWrite();
@@ -234,6 +270,12 @@ public class VRServer extends Thread {
 		});
 	}
 	
+	public void resetTrackersYaw() {
+		queueTask(() -> {
+			humanPoseProcessor.resetTrackersYaw();
+		});
+	}
+	
 	public int getTrackersCount() {
 		return trackers.size();
 	}

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

@@ -1,8 +0,0 @@
-package io.eiren.vr.bridge;
-
-public interface VRBridge {
-
-	public void dataRead();
-	
-	public void dataWrite();
-}

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

@@ -2,16 +2,20 @@ package io.eiren.vr.processor;
 
 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 {
+public class ComputedHumanPoseTracker extends ComputedTracker implements TrackerWithTPS, ShareableTracker {
 	
 	public final ComputedHumanPoseTrackerPosition skeletonPosition;
+	protected final TrackerRole trackerRole;
 	protected BufferedTimer timer = new BufferedTimer(1f);
 
-	public ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition skeletonPosition) {
-		super("human://" + skeletonPosition.name());
+	public ComputedHumanPoseTracker(int trackerId, ComputedHumanPoseTrackerPosition skeletonPosition, TrackerRole role) {
+		super(trackerId, "human://" + skeletonPosition.name(), true, true);
 		this.skeletonPosition = skeletonPosition;
+		this.trackerRole = role;
 	}
 	
 	@Override
@@ -23,4 +27,9 @@ public class ComputedHumanPoseTracker extends ComputedTracker implements Tracker
 	public void dataTick() {
 		timer.update();
 	}
+
+	@Override
+	public TrackerRole getTrackerRole() {
+		return trackerRole;
+	}
 }

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

@@ -5,5 +5,7 @@ public enum ComputedHumanPoseTrackerPosition {
 	WAIST,
 	CHEST,
 	LEFT_FOOT,
-	RIGHT_FOOT;
+	RIGHT_FOOT,
+	LEFT_KNEE,
+	RIGHT_KNEE;
 }

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

@@ -8,9 +8,10 @@ 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;
-import io.eiren.vr.trackers.TrackerUtils;
 
 public class HumanPoseProcessor {
 	
@@ -21,9 +22,16 @@ public class HumanPoseProcessor {
 
 	public HumanPoseProcessor(VRServer server, HMDTracker hmd) {
 		this.server = server;
-		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.WAIST));
-		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT));
-		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT));
+		computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.WAIST, TrackerRole.WAIST));
+		computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_FOOT, TrackerRole.LEFT_FOOT));
+		computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_FOOT, TrackerRole.RIGHT_FOOT));
+		computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.CHEST, TrackerRole.CHEST));
+		computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_KNEE, TrackerRole.LEFT_KNEE));
+		computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_KNEE, TrackerRole.RIGHT_KNEE));
+	}
+	
+	public HumanSkeleton getSkeleton() {
+		return skeleton;
 	}
 
 	@VRServerThread
@@ -56,7 +64,7 @@ public class HumanPoseProcessor {
 	}
 	
 	@ThreadSafe
-	public List<? extends Tracker> getComputedTrackers() {
+	public List<? extends ShareableTracker> getComputedTrackers() {
 		return computedTrackers;
 	}
 
@@ -72,31 +80,10 @@ public class HumanPoseProcessor {
 
 	@VRServerThread
 	private void updateSekeltonModel() {
-		boolean hasWaist = false;
-		boolean hasBothLegs = false;
-		List<Tracker> allTrackers = server.getAllTrackers();
-		Tracker waist = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST);
-		Tracker leftAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_ANKLE);
-		Tracker rightAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_ANKLE);
-		Tracker leftLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_LEG);
-		Tracker rightLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_LEG);
-		if(waist != null)
-			hasWaist = true;
-		if(leftAnkle != null && rightAnkle != null && leftLeg != null && rightLeg != null)
-			hasBothLegs = true;
-		if(!hasWaist) {
-			skeleton = null; // Can't track anything without waist
-		} else if(hasBothLegs) {
-			disconnectAllTrackers();
-			skeleton = new HumanSekeletonWithLegs(server, computedTrackers);
-			for(int i = 0; i < onSkeletonUpdated.size(); ++i)
-				onSkeletonUpdated.get(i).accept(skeleton);
-		} else {
-			disconnectAllTrackers();
-			skeleton = new HumanSkeleonWithWaist(server, computedTrackers);
-			for(int i = 0; i < onSkeletonUpdated.size(); ++i)
-				onSkeletonUpdated.get(i).accept(skeleton);
-		}
+		disconnectAllTrackers();
+		skeleton = new HumanSkeletonWithLegs(server, computedTrackers);
+		for(int i = 0; i < onSkeletonUpdated.size(); ++i)
+			onSkeletonUpdated.get(i).accept(skeleton);
 	}
 
 	@VRServerThread
@@ -117,4 +104,10 @@ public class HumanPoseProcessor {
 		if(skeleton != null)
 			skeleton.resetTrackersFull();
 	}
+
+	@VRServerThread
+	public void resetTrackersYaw() {
+		if(skeleton != null)
+			skeleton.resetTrackersYaw();
+	}
 }

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

@@ -24,4 +24,7 @@ public abstract class HumanSkeleton {
 
 	@VRServerThread
 	public abstract void resetTrackersFull();
+	
+	@VRServerThread
+	public abstract void resetTrackersYaw();
 }

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

@@ -9,28 +9,33 @@ 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 HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
+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 float[] kneeAngles = new float[3];
-	protected final float[] hipAngles = new float[3];
 	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);
@@ -46,51 +51,70 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	 */
 	protected float hipsWidth = HIPS_WIDTH_DEFAULT;
 	/**
-	 * Length from waist to knees
+	 * Length from hip to knees
 	 */
 	protected float kneeHeight = 0.42f;
 	/**
-	 * Distance from waist to ankle
+	 * Distance from hip 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 HumanSekeletonWithLegs(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
+	public HumanSkeletonWithLegs(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
 		super(server, computedTrackers);
-		List<Tracker> allTracekrs = server.getAllTrackers();
-		this.leftLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_LEG);
-		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_ANKLE);
-		this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_FOOT);
-		this.rightLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_LEG);
-		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_ANKLE);
-		this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_FOOT);
+		List<Tracker> allTrackers = server.getAllTrackers();
+		this.leftLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.LEFT_LEG, TrackerPosition.LEFT_ANKLE, null);
+		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.LEFT_ANKLE, TrackerPosition.LEFT_LEG, null);
+		this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerPosition.LEFT_FOOT);
+		this.rightLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.RIGHT_LEG, TrackerPosition.RIGHT_ANKLE, null);
+		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.RIGHT_ANKLE, TrackerPosition.RIGHT_LEG, null);
+		this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(allTrackers, 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);
+		hipNode.attachChild(leftHipNode);
 		leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
 		
-		waistNode.attachChild(rightHipNode);
+		hipNode.attachChild(rightHipNode);
 		rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
 		
 		leftHipNode.attachChild(leftKneeNode);
@@ -100,10 +124,10 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
 		
 		leftKneeNode.attachChild(leftAnkleNode);
-		leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+		leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset);
 		
 		rightKneeNode.attachChild(rightAnkleNode);
-		rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+		rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, -footOffset);
 
 		leftAnkleNode.attachChild(leftFootNode);
 		leftFootNode.localTransform.setTranslation(0, 0, -footLength);
@@ -115,6 +139,7 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		configMap.put("Legs length", legsLength);
 		configMap.put("Knee height", kneeHeight);
 		configMap.put("Foot length", footLength);
+		configMap.put("Foot offset", footOffset);
 	}
 	
 	@Override
@@ -125,6 +150,7 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 			// Resets from the parent already performed
 			resetSkeletonConfig("Hips width");
 			resetSkeletonConfig("Foot length");
+			resetSkeletonConfig("Foot offset");
 			resetSkeletonConfig("Legs length");
 			break;
 		case "Hips width":
@@ -133,12 +159,19 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		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);
+				setSkeletonConfig(joint, height - neckLength - torsoLength - DEFAULT_FLOOR_OFFSET);
+			}
+			else //if floor level is incorrect
+			{
+				setSkeletonConfig(joint, 0.84f);
 			}
 			resetSkeletonConfig("Knee height");
 			break;
@@ -161,8 +194,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		case "Knee height":
 			kneeHeight = newLength;
 			server.config.setProperty("body.kneeHeight", kneeHeight);
-			leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
-			rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+			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;
@@ -178,6 +211,40 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 			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;
 		}
 	}
 	
@@ -188,7 +255,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		leftLegTracker.getRotation(hipBuf);
 		leftAnkleTracker.getRotation(kneeBuf);
 
-		//calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
+		if(extendedKneeModel)
+			calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
 		
 		leftHipNode.localTransform.setRotation(hipBuf);
 		leftKneeNode.localTransform.setRotation(kneeBuf);
@@ -205,7 +273,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		rightLegTracker.getRotation(hipBuf);
 		rightAnkleTracker.getRotation(kneeBuf);
 		
-		//calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
+		if(extendedKneeModel)
+			calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
 		
 		rightHipNode.localTransform.setRotation(hipBuf);
 		rightKneeNode.localTransform.setRotation(kneeBuf);
@@ -217,20 +286,39 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 			rightAnkleNode.localTransform.setRotation(kneeBuf);
 			rightFootNode.localTransform.setRotation(kneeBuf);
 		}
-		
-		// TODO Calculate waist node as some function between waist and hip rotations
+
+		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);
+			hipNode.localTransform.setRotation(kneeBuf);
+			//trackerWaistNode.localTransform.setRotation(kneeBuf); // <== 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
+		}
 	}
 	
-	// Knee basically has only 1 DoF (pitch), average yaw between knee and hip
+	// 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) {
-		hipBuf.toAngles(hipAngles);
-		kneeBuf.toAngles(kneeAngles);
+		ankleVector.set(0, -1, 0);
+		hipVector.set(0, -1, 0);
+		hipBuf.multLocal(hipVector);
+		kneeBuf.multLocal(ankleVector);
+		kneeRotation.angleBetweenVectors(hipVector, ankleVector); // Find knee angle
 		
-		hipAngles[1] = kneeAngles[1] = interpolateRadians(kneeLerpFactor, kneeAngles[1], hipAngles[1]);
-		//hipAngles[2] = kneeAngles[2] = interpolateRadians(kneeLerpFactor, kneeAngles[2], hipAngles[2]);
+		// Substract knee angle from knee rotation. With perfect leg and perfect
+		// sensors result should match hip rotation perfectly
+		kneeBuf.multLocal(kneeRotation.inverse());
 		
-		hipBuf.fromAngles(hipAngles);
-		kneeBuf.fromAngles(kneeAngles);
+		// 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) {
@@ -254,13 +342,29 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	protected void updateComputedTrackers() {
 		super.updateComputedTrackers();
 		
-		computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation());
-		computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation());
-		computedLeftFootTracker.dataTick();
+		if(computedLeftFootTracker != null) {
+			computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation());
+			computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation());
+			computedLeftFootTracker.dataTick();
+		}
 		
-		computedRightFootTracker.position.set(rightFootNode.worldTransform.getTranslation());
-		computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation());
-		computedRightFootTracker.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
@@ -271,7 +375,7 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		super.resetTrackersFull();
 		// Start with waist, it was reset in the parent
 		Quaternion referenceRotation = new Quaternion();
-		this.waistTracker.getRotation(referenceRotation);
+		this.hipTracker.getRotation(referenceRotation);
 		
 		this.leftLegTracker.resetFull(referenceRotation);
 		this.rightLegTracker.resetFull(referenceRotation);
@@ -289,8 +393,39 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		this.rightAnkleTracker.resetFull(referenceRotation);
 		this.rightAnkleTracker.getRotation(referenceRotation);
 		
-		if(this.rightAnkleTracker != null) {
-			this.rightAnkleTracker.resetFull(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.hipTracker.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

@@ -11,71 +11,88 @@ 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 HumanSkeleonWithWaist extends HumanSkeleton {
+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 Tracker hipTracker;
 	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 final TransformNode hipNode = new TransformNode("Hip", false);
 	
-	protected float chestDistance = 0.42f;
 	/**
-	 * Distance from eyes to waist
+	 * Distance from shoulders to chest
 	 */
-	protected float waistDistance = 0.85f;
+	protected float chestDistance = 0.35f;
 	/**
-	 * Distance from eyes to waist, defines reported
+	 * Distance from hip to waist
+	 */
+	protected float waistDistance = 0.1f;
+	/**
+	 * Distance from shoulder to hip
+	 */
+	protected float torsoLength = 0.7f;
+	/**
+	 * Distance from eyes to hip, defines reported
 	 * tracker position, if you want to move resulting
-	 * tracker up or down from actual waist
+	 * tracker up or down from actual hip
 	 */
-	protected float trackerWaistDistance = 0.0f;
+	protected float hipOffset = 0.0f;
 	/**
-	 * Distacne from eyes to the base of the neck
+	 * 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 HumanSkeleonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
-		List<Tracker> allTracekrs = server.getAllTrackers();
-		this.waistTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST);
-		this.chestTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.CHEST, TrackerBodyPosition.WAIST);
+	
+	public HumanSkeletonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
+		List<Tracker> allTrackers = server.getAllTrackers();
+		this.waistTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.WAIST, TrackerPosition.CHEST, TrackerPosition.HIP);
+		this.chestTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.CHEST, TrackerPosition.WAIST, TrackerPosition.HIP);
+		this.hipTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.HIP, TrackerPosition.WAIST, TrackerPosition.CHEST);
 		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);
+		torsoLength = server.config.getFloat("body.torsoLength", torsoLength);
+		hipOffset = server.config.getFloat("body.hipOffset", hipOffset);
 		// Build skeleton
 		hmdNode.attachChild(headNode);
 		headNode.localTransform.setTranslation(0, 0, headShift);
@@ -87,16 +104,20 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		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);
+		waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0);
+
+		waistNode.attachChild(hipNode);
+		hipNode.localTransform.setTranslation(0, -waistDistance, 0);
+
+		hipNode.attachChild(trackerWaistNode);
+		trackerWaistNode.localTransform.setTranslation(0, hipOffset, 0);
 		
 		configMap.put("Head", headShift);
 		configMap.put("Neck", neckLength);
 		configMap.put("Chest", chestDistance);
 		configMap.put("Waist", waistDistance);
-		configMap.put("Virtual waist", trackerWaistDistance);
+		configMap.put("Hip offset", hipOffset);
+		configMap.put("Torso", torsoLength);
 	}
 	
 	@Override
@@ -105,29 +126,37 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		case "All": // Reset all joints according to height
 			resetSkeletonConfig("Head");
 			resetSkeletonConfig("Neck");
-			resetSkeletonConfig("Virtual waist");
+			resetSkeletonConfig("Hip offset");
+			resetSkeletonConfig("Torso");
 			resetSkeletonConfig("Waist");
 			resetSkeletonConfig("Chest");
 			break;
-		case "Head": 
+		case "Head":
 			setSkeletonConfig(joint, HEAD_SHIFT_DEFAULT);
 			break;
-		case "Neck": 
+		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
+		case "Torso": // Distance from shoulders to hip (full torso length)
 			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);
+			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) - neckLength);
 			}
+			else// if floor level is incorrect
+			{
+				setSkeletonConfig(joint, 0.7f);
+			}
+			break;
+		case "Chest": //Chest is roughly half of the upper body (shoulders to chest)
+			setSkeletonConfig(joint, torsoLength / 2.0f);
+			break;
+		case "Waist": // waist length is from hips to waist
+			setSkeletonConfig(joint, 0.1f);
+			break;
+		case "Hip offset":
+			setSkeletonConfig(joint, 0.0f);
 			break;
 		}
 	}
@@ -136,7 +165,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	public Map<String, Float> getSkeletonConfig() {
 		return configMap;
 	}
-
+	
 	@Override
 	public void setSkeletonConfig(String joint, float newLength) {
 		configMap.put(joint, newLength);
@@ -151,27 +180,38 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 			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);
+		case "Torso":
+			torsoLength = newLength;
+			server.config.setProperty("body.torsoLength", torsoLength);
+			waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 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);
+			waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0);
 			break;
-		case "Virtual waist":
-			trackerWaistDistance = newLength;
-			server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance);
-			trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
+		case "Waist":
+			waistDistance = newLength;
+			server.config.setProperty("body.waistDistance", waistDistance);
+			waistNode.localTransform.setTranslation(0, (chestDistance - torsoLength + waistDistance), 0);
+			hipNode.localTransform.setTranslation(0, -waistDistance, 0);
+			break;
+		case "Hip offset":
+			hipOffset = newLength;
+			server.config.setProperty("body.hipOffset", hipOffset);
+			trackerWaistNode.localTransform.setTranslation(0, hipOffset, 0);
 			break;
 		}
 	}
 	
+	public boolean getSkeletonConfigBoolean(String config) {
+		return false;
+	}
+	
+	public void setSkeletonConfigBoolean(String config, boolean newState) {
+	}
+	
 	@Override
 	public TransformNode getRootNode() {
 		return hmdNode;
@@ -193,21 +233,32 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 			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);
+		}
+		if(hipTracker.getRotation(qBuf)) {
 			waistNode.localTransform.setRotation(qBuf);
+			trackerWaistNode.localTransform.setRotation(qBuf);
+			hipNode.localTransform.setRotation(qBuf);
 		}
 	}
 	
 	protected void updateComputedTrackers() {
-		computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation());
-		computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation());
-		computedWaistTracker.dataTick();
+		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
@@ -222,5 +273,25 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		this.chestTracker.getRotation(referenceRotation);
 		
 		this.waistTracker.resetFull(referenceRotation);
+		this.waistTracker.getRotation(referenceRotation);
+		
+		this.hipTracker.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);
+		this.waistTracker.getRotation(referenceRotation);
+
+		this.hipTracker.resetYaw(referenceRotation);
 	}
 }

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

@@ -1,37 +0,0 @@
-package io.eiren.vr.processor;
-
-import java.util.HashMap;
-import java.util.Map;
-
-public enum TrackerBodyPosition {
-	
-	NONE(""),
-	HMD("body:HMD"),
-	CHEST("body:chest"),
-	WAIST("body:waist"),
-	LEFT_LEG("body:left_leg"),
-	RIGHT_LEG("body:right_leg"),
-	LEFT_ANKLE("body:left_ankle"),
-	RIGHT_ANKLE("body:right_ankle"),
-	LEFT_FOOT("body:left_foot"),
-	RIGHT_FOOT("body:right_foot"),
-	;
-	
-	public final String designation;
-	
-	public static final TrackerBodyPosition[] values = values();
-	private static final Map<String, TrackerBodyPosition> byDesignation = new HashMap<>();
-	
-	private TrackerBodyPosition(String designation) {
-		this.designation = designation;
-	}
-	
-	public static TrackerBodyPosition getByDesignation(String designation) {
-		return designation == null ? null : byDesignation.get(designation.toLowerCase());
-	}
-	
-	static {
-		for(TrackerBodyPosition tbp : values())
-			byDesignation.put(tbp.designation, tbp);
-	}
-}

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

@@ -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)

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

@@ -0,0 +1,21 @@
+package io.eiren.vr.trackers;
+
+public class BnoTap {
+	
+	public final boolean doubleTap;
+	
+	public BnoTap(int tapBits) {
+		doubleTap = (tapBits & 0x40) > 0;
+	}
+	
+	@Override
+	public String toString() {
+		return "Tap{" + (doubleTap ? "double" : "") + "}";
+	}
+	
+	public enum TapAxis {
+		X,
+		Y,
+		Z;
+	}
+}

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

@@ -3,18 +3,28 @@ package io.eiren.vr.trackers;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
-import io.eiren.vr.processor.TrackerBodyPosition;
-
-public class ComputedTracker implements Tracker {
+public class ComputedTracker implements Tracker, TrackerWithTPS {
 
 	public final Vector3f position = new Vector3f();
 	public final Quaternion rotation = new Quaternion();
 	protected final String name;
+	protected final String serial;
 	protected TrackerStatus status = TrackerStatus.DISCONNECTED;
-	public TrackerBodyPosition bodyPosition = null;
+	public TrackerPosition bodyPosition = null;
+	protected final boolean hasRotation;
+	protected final boolean hasPosition;
+	protected final int trackerId;
 	
-	public ComputedTracker(String name) {
+	public ComputedTracker(int trackerId, String serial, String name, boolean hasRotation, boolean hasPosition) {
 		this.name = name;
+		this.serial = serial;
+		this.hasRotation = hasRotation;
+		this.hasPosition = hasPosition;
+		this.trackerId = trackerId;
+	}
+	
+	public ComputedTracker(int trackerId, String name, boolean hasRotation, boolean hasPosition) {
+		this(trackerId, name, name, hasRotation, hasPosition);
 	}
 	
 	@Override
@@ -24,11 +34,19 @@ public class ComputedTracker implements Tracker {
 	
 	@Override
 	public void loadConfig(TrackerConfig config) {
-		bodyPosition = TrackerBodyPosition.getByDesignation(config.designation);
+		// Loading a config is an act of user editing, therefore it shouldn't not be allowed if editing is not allowed
+		if (userEditable()) {
+			bodyPosition = TrackerPosition.getByDesignation(config.designation);
+		}
 	}
 	
 	@Override
 	public String getName() {
+		return this.serial;
+	}
+	
+	@Override
+	public String getDescriptiveName() {
 		return this.name;
 	}
 	
@@ -67,12 +85,12 @@ public class ComputedTracker implements Tracker {
 	}
 
 	@Override
-	public TrackerBodyPosition getBodyPosition() {
+	public TrackerPosition getBodyPosition() {
 		return bodyPosition;
 	}
 
 	@Override
-	public void setBodyPosition(TrackerBodyPosition position) {
+	public void setBodyPosition(TrackerPosition position) {
 		this.bodyPosition = position;
 	}
 
@@ -80,4 +98,37 @@ public class ComputedTracker implements Tracker {
 	public boolean userEditable() {
 		return false;
 	}
+	
+	@Override
+	public void dataTick() {
+	}
+
+	@Override
+	public void tick() {
+	}
+
+	@Override
+	public boolean hasRotation() {
+		return hasRotation;
+	}
+
+	@Override
+	public boolean hasPosition() {
+		return hasPosition;
+	}
+
+	@Override
+	public boolean isComputed() {
+		return true;
+	}
+
+	@Override
+	public float getTPS() {
+		return -1;
+	}
+
+	@Override
+	public int getTrackerId() {
+		return this.trackerId;
+	}
 }

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

@@ -0,0 +1,9 @@
+package io.eiren.vr.trackers;
+
+public enum DeviceType {
+	HMD,
+    CONTROLLER,
+    TRACKER,
+    TRACKING_REFERENCE,
+    ;
+}

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

@@ -1,15 +1,14 @@
 package io.eiren.vr.trackers;
 
 import io.eiren.util.BufferedTimer;
-import io.eiren.vr.processor.TrackerBodyPosition;
 
 public class HMDTracker extends ComputedTracker implements TrackerWithTPS {
 
 	protected BufferedTimer timer = new BufferedTimer(1f);
 	
 	public HMDTracker(String name) {
-		super(name);
-		setBodyPosition(TrackerBodyPosition.HMD);
+		super(0, name, true, true);
+		setBodyPosition(TrackerPosition.HMD);
 	}
 	
 	@Override
@@ -21,4 +20,9 @@ public class HMDTracker extends ComputedTracker implements TrackerWithTPS {
 	public void dataTick() {
 		timer.update();
 	}
+
+	@Override
+	public boolean isComputed() {
+		return false;
+	}
 }

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

@@ -1,29 +0,0 @@
-package io.eiren.vr.trackers;
-
-public class IMUReferenceAdjustedTracker<T extends IMUTracker & TrackerWithTPS & TrackerWithBattery> extends ReferenceAdjustedTracker<T> implements TrackerWithTPS, TrackerWithBattery {
-	
-	public IMUReferenceAdjustedTracker(T tracker) {
-		super(tracker);
-	}
-
-	@Override
-	public float getBatteryLevel() {
-		return tracker.getBatteryLevel();
-	}
-
-	@Override
-	public float getBatteryVoltage() {
-		return tracker.getBatteryVoltage();
-	}
-
-	@Override
-	public float getTPS() {
-		return tracker.getTPS();
-	}
-
-	@Override
-	public void dataTick() {
-		tracker.dataTick();
-	}
-	
-}

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

@@ -1,37 +1,50 @@
 package io.eiren.vr.trackers;
 
+import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
 import io.eiren.math.FloatMath;
 import io.eiren.util.BufferedTimer;
-import io.eiren.vr.processor.TrackerBodyPosition;
 
 public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	
+	public static final float MAX_MAG_CORRECTION_ACCURACY = 5 * FastMath.RAD_TO_DEG;
+	
 	public final Vector3f gyroVector = new Vector3f();
 	public final Vector3f accelVector = new Vector3f();
 	public final Vector3f magVector = new Vector3f();
 	public final Quaternion rotQuaternion = new Quaternion();
+	public final Quaternion rotMagQuaternion = new Quaternion();
 	protected final Quaternion rotAdjust = new Quaternion();
+	protected final Quaternion correction = new Quaternion();
 	protected TrackerMountingRotation mounting = null;
 	protected TrackerStatus status = TrackerStatus.OK;
+	protected final int trackerId;
 	
 	protected final String name;
+	protected final String descriptiveName;
 	protected final TrackersUDPServer server;
 	protected float confidence = 0;
 	protected float batteryVoltage = 0;
+	public int calibrationStatus = 0;
+	public int magCalibrationStatus = 0;
+	public float magnetometerAccuracy = 0;
+	protected boolean magentometerCalibrated = false;
+	public boolean hasNewCorrectionData = false;
 	
 	protected BufferedTimer timer = new BufferedTimer(1f);
 	public int ping = -1;
 	
 	public StringBuilder serialBuffer = new StringBuilder();
 	long lastSerialUpdate = 0;
-	public TrackerBodyPosition bodyPosition = null;
+	public TrackerPosition bodyPosition = null;
 	
-	public IMUTracker(String name, TrackersUDPServer server) {
+	public IMUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server) {
 		this.name = name;
 		this.server = server;
+		this.trackerId = trackerId;
+		this.descriptiveName = descriptiveName;
 	}
 	
 	@Override
@@ -42,17 +55,20 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	
 	@Override
 	public void loadConfig(TrackerConfig config) {
-		if(config.mountingRotation != null) {
-			mounting = TrackerMountingRotation.valueOf(config.mountingRotation);
-			if(mounting != null) {
-				rotAdjust.set(mounting.quaternion);
+		// Loading a config is an act of user editing, therefore it shouldn't not be allowed if editing is not allowed
+		if (userEditable()) {
+			if(config.mountingRotation != null) {
+				mounting = TrackerMountingRotation.valueOf(config.mountingRotation);
+				if(mounting != null) {
+					rotAdjust.set(mounting.quaternion);
+				} else {
+					rotAdjust.loadIdentity();
+				}
 			} else {
 				rotAdjust.loadIdentity();
 			}
-		} else {
-			rotAdjust.loadIdentity();
+			bodyPosition = TrackerPosition.getByDesignation(config.designation);
 		}
-		bodyPosition = TrackerBodyPosition.getByDesignation(config.designation);
 	}
 	
 	public TrackerMountingRotation getMountingRotation() {
@@ -68,6 +84,18 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 		}
 	}
 	
+	@Override
+	public void tick() {
+		if(magentometerCalibrated && hasNewCorrectionData) {
+			hasNewCorrectionData = false;
+			if(magnetometerAccuracy <= MAX_MAG_CORRECTION_ACCURACY) {
+				// Adjust gyro rotation to match magnetometer rotation only if magnetometer
+				// accuracy is within the parameters
+				calculateLiveMagnetometerCorrection();
+			}
+		}
+	}
+	
 	@Override
 	public String getName() {
 		return this.name;
@@ -82,9 +110,14 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	@Override
 	public boolean getRotation(Quaternion store) {
 		store.set(rotQuaternion);
+		//correction.mult(store, store); // Correction is not used now to preven accidental errors while debugging other things
 		store.multLocal(rotAdjust);
 		return true;
 	}
+	
+	public void getCorrection(Quaternion store) {
+		store.set(correction);
+	}
 
 	@Override
 	public TrackerStatus getStatus() {
@@ -130,19 +163,40 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 
 	@Override
 	public void resetFull(Quaternion reference) {
+		resetYaw(reference);
 	}
 
+	/**
+	 * Does not perform actual gyro reset to reference, that's the task of
+	 * reference adjusted tracker. Only aligns gyro with magnetometer if
+	 * it's reliable
+	 */
 	@Override
 	public void resetYaw(Quaternion reference) {
+		if(magCalibrationStatus >= CalibrationAccuracy.HIGH.status) {
+			magentometerCalibrated = true;
+			// During calibration set correction to match magnetometer readings exactly
+			// TODO : Correct only yaw
+			correction.set(rotQuaternion).inverseLocal().multLocal(rotMagQuaternion);
+		}
+	}
+	
+	/**
+	 * Calculate correction between normal and magnetometer
+	 * readings up to accuracy threshold
+	 */
+	protected void calculateLiveMagnetometerCorrection() {
+		// TODO Magic, correct only yaw
+		// TODO Print "jump" length when correcing if it's more than 1 degree
 	}
 
 	@Override
-	public TrackerBodyPosition getBodyPosition() {
+	public TrackerPosition getBodyPosition() {
 		return bodyPosition;
 	}
 
 	@Override
-	public void setBodyPosition(TrackerBodyPosition position) {
+	public void setBodyPosition(TrackerPosition position) {
 		this.bodyPosition = position;
 	}
 
@@ -150,4 +204,56 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	public boolean userEditable() {
 		return true;
 	}
+
+	@Override
+	public boolean hasRotation() {
+		return true;
+	}
+
+	@Override
+	public boolean hasPosition() {
+		return false;
+	}
+
+	@Override
+	public boolean isComputed() {
+		return false;
+	}
+
+	@Override
+	public int getTrackerId() {
+		return this.trackerId;
+	}
+	
+	@Override
+	public String getDescriptiveName() {
+		return this.descriptiveName;
+	}
+	
+	public enum CalibrationAccuracy {
+		
+		UNRELIABLE(0),
+		LOW(1),
+		MEDIUM(2),
+		HIGH(3),
+		;
+		
+		private static final CalibrationAccuracy[] byStatus = new CalibrationAccuracy[4];
+		public final int status;
+		
+		private CalibrationAccuracy(int status) {
+			this.status = status;
+		}
+		
+		public static CalibrationAccuracy getByStatus(int status) {
+			if(status < 0 || status > 3)
+				return null;
+			return byStatus[status];
+		}
+		
+		static {
+			for(CalibrationAccuracy ca : values())
+				byStatus[ca.status] = ca;
+		}
+	}
 }

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

@@ -6,8 +6,8 @@ public class MPUTracker extends IMUTracker {
 	
 	public ConfigurationData newCalibrationData;
 	
-	public MPUTracker(String name, TrackersUDPServer server) {
-		super(name, server);
+	public MPUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server) {
+		super(trackerId, name, descriptiveName, server);
 	}
 	
 	public static class ConfigurationData {

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

@@ -3,13 +3,12 @@ package io.eiren.vr.trackers;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
-import io.eiren.vr.processor.TrackerBodyPosition;
-
 public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	
 	public final E tracker;
-	public final Quaternion adjustmentYaw = new Quaternion();
-	public final Quaternion adjustmentAttachment = new Quaternion();
+	public final Quaternion yawFix = new Quaternion();
+	public final Quaternion gyroFix = new Quaternion();
+	public final Quaternion attachmentFix = new Quaternion();
 	protected float confidenceMultiplier = 1.0f;
 	
 	public ReferenceAdjustedTracker(E tracker) {
@@ -44,14 +43,15 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetFull(Quaternion reference) {
+		tracker.resetFull(reference);
+		fixGyroscope();
+		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
-		//float[] angles = new float[3];
-		//sensorRotation.toAngles(angles);
-		//sensorRotation.fromAngles(angles[0], 0, angles[2]);
-		adjustmentAttachment.set(sensorRotation).inverseLocal();
+		gyroFix.mult(sensorRotation, sensorRotation);
+		attachmentFix.set(sensorRotation).inverseLocal();
 		
-		resetYaw(reference);
+		fixYaw(reference);
 	}
 
 	/**
@@ -63,6 +63,11 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetYaw(Quaternion reference) {
+		tracker.resetYaw(reference);
+		fixYaw(reference);
+	}
+	
+	private void fixYaw(Quaternion reference) {
 		// Use only yaw HMD rotation
 		Quaternion targetTrackerRotation = new Quaternion(reference);
 		float[] angles = new float[3];
@@ -71,21 +76,31 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
-		adjustmentAttachment.mult(sensorRotation, sensorRotation);
-		//sensorRotation.multLocal(adjustmentAttachment);
+		gyroFix.mult(sensorRotation, sensorRotation);
+		sensorRotation.multLocal(attachmentFix);
 		
 		sensorRotation.toAngles(angles);
 		sensorRotation.fromAngles(0, angles[1], 0);
 		
-		adjustmentYaw.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+		yawFix.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+	}
+	
+	private void fixGyroscope() {
+		float[] angles = new float[3];
 		
-		confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
+		Quaternion sensorRotation = new Quaternion();
+		tracker.getRotation(sensorRotation);
+		
+		sensorRotation.toAngles(angles);
+		sensorRotation.fromAngles(0, angles[1], 0);
+		
+		gyroFix.set(sensorRotation).inverseLocal();
 	}
 	
 	protected void adjustInternal(Quaternion store) {
-		//store.multLocal(adjustmentAttachment);
-		adjustmentAttachment.mult(store, store);
-		adjustmentYaw.mult(store, store);
+		gyroFix.mult(store, store);
+		store.multLocal(attachmentFix);
+		yawFix.mult(store, store);
 	}
 	
 	@Override
@@ -116,12 +131,42 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	}
 
 	@Override
-	public TrackerBodyPosition getBodyPosition() {
+	public TrackerPosition getBodyPosition() {
 		return tracker.getBodyPosition();
 	}
 
 	@Override
-	public void setBodyPosition(TrackerBodyPosition position) {
+	public void setBodyPosition(TrackerPosition position) {
 		tracker.setBodyPosition(position);
 	}
-}
+
+	@Override
+	public void tick() {
+		tracker.tick();
+	}
+
+	@Override
+	public boolean hasRotation() {
+		return tracker.hasRotation();
+	}
+
+	@Override
+	public boolean hasPosition() {
+		return tracker.hasPosition();
+	}
+
+	@Override
+	public boolean isComputed() {
+		return tracker.isComputed();
+	}
+	
+	@Override
+	public int getTrackerId() {
+		return tracker.getTrackerId();
+	}
+	
+	@Override
+	public String getDescriptiveName() {
+		return tracker.getDescriptiveName();
+	}
+}

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

@@ -0,0 +1,6 @@
+package io.eiren.vr.trackers;
+
+public interface ShareableTracker extends Tracker {
+	
+	public TrackerRole getTrackerRole();
+}

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

@@ -1,12 +1,14 @@
 package io.eiren.vr.trackers;
 
+import java.util.concurrent.atomic.AtomicInteger;
+
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
-import io.eiren.vr.processor.TrackerBodyPosition;
-
 public interface Tracker {
 	
+	public static final AtomicInteger nextLocalTrackerId = new AtomicInteger();
+	
 	public boolean getPosition(Vector3f store);
 	
 	public boolean getRotation(Quaternion store);
@@ -25,9 +27,27 @@ public interface Tracker {
 	
 	public void resetYaw(Quaternion reference);
 	
-	public TrackerBodyPosition getBodyPosition();
+	public void tick();
 	
-	public void setBodyPosition(TrackerBodyPosition position);
+	public TrackerPosition getBodyPosition();
+	
+	public void setBodyPosition(TrackerPosition position);
 	
 	public boolean userEditable();
+	
+	public boolean hasRotation();
+	
+	public boolean hasPosition();
+
+	public boolean isComputed();
+	
+	public int getTrackerId();
+	
+	public default String getDescriptiveName() {
+		return getName();
+	}
+	
+	public static int getNextLocalTrackerId() {
+		return nextLocalTrackerId.incrementAndGet();
+	}
 }

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

@@ -8,16 +8,20 @@ public class TrackerConfig {
 	
 	public final String trackerName;
 	public String designation;
+	public String description;
 	public boolean hide;
 	public Quaternion adjustment;
 	public String mountingRotation;
 	
-	public TrackerConfig(String trackerName) {
-		this.trackerName = trackerName;
+	public TrackerConfig(Tracker tracker) {
+		this.trackerName = tracker.getName();
+		this.description = tracker.getDescriptiveName();
+		this.designation = tracker.getBodyPosition() != null ? tracker.getBodyPosition().designation : null;
 	}
 	
 	public TrackerConfig(YamlNode node) {
 		this.trackerName = node.getString("name");
+		this.description = node.getString("description");
 		this.designation = node.getString("designation");
 		this.hide = node.getBoolean("hide", false);
 		this.mountingRotation = node.getString("rotation");
@@ -54,5 +58,10 @@ public class TrackerConfig {
 		} else {
 			configNode.removeProperty("rotation");
 		}
+		if(description != null) {
+			configNode.setProperty("description", description);
+		} else {
+			configNode.removeProperty("description");
+		}
 	}
 }

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

@@ -0,0 +1,55 @@
+package io.eiren.vr.trackers;
+
+import java.util.EnumMap;
+import java.util.HashMap;
+import java.util.Map;
+
+public enum TrackerPosition {
+	
+	NONE("", TrackerRole.NONE),
+	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),
+	RIGHT_ANKLE("body:right_ankle", null),
+	LEFT_FOOT("body:left_foot", TrackerRole.LEFT_FOOT),
+	RIGHT_FOOT("body:right_foot", TrackerRole.RIGHT_FOOT),
+	LEFT_CONTROLLER("body:left_controller", TrackerRole.LEFT_CONTROLLER),
+	RIGHT_CONTROLLER("body:right_conroller", TrackerRole.RIGHT_CONTROLLER),
+	;
+	
+	public final String designation;
+	public final TrackerRole trackerRole;
+	
+	public static final TrackerPosition[] values = values();
+	private static final Map<String, TrackerPosition> byDesignation = new HashMap<>();
+	private static final EnumMap<TrackerRole, TrackerPosition> byRole = new EnumMap<>(TrackerRole.class);
+	
+	private TrackerPosition(String designation, TrackerRole trackerRole) {
+		this.designation = designation;
+		this.trackerRole = trackerRole;
+	}
+	
+	public static TrackerPosition getByDesignation(String designation) {
+		return designation == null ? null : byDesignation.get(designation.toLowerCase());
+	}
+	
+	public static TrackerPosition getByRole(TrackerRole role) {
+		return byRole.get(role);
+	}
+	
+	static {
+		for(TrackerPosition tbp : values()) {
+			byDesignation.put(tbp.designation.toLowerCase(), tbp);
+			if(tbp.trackerRole != null) {
+				TrackerPosition old = byRole.get(tbp.trackerRole);
+				if(old != null)
+					throw new AssertionError("Only one tracker position can match tracker role. " + tbp.trackerRole + " is occupied by " + old + " when adding " + tbp);
+				byRole.put(tbp.trackerRole, tbp);
+			}
+		}
+	}
+}

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

@@ -0,0 +1,55 @@
+package io.eiren.vr.trackers;
+
+public enum TrackerRole {
+	
+	NONE(0, "", "", null),
+    WAIST(1, "vive_tracker_waist", "TrackerRole_Waist", DeviceType.TRACKER),
+    LEFT_FOOT(2, "vive_tracker_left_foot", "TrackerRole_LeftFoot", DeviceType.TRACKER),
+    RIGHT_FOOT(3, "vive_tracker_right_foot", "TrackerRole_RightFoot", DeviceType.TRACKER),
+    CHEST(4, "vive_tracker_chest", "TrackerRole_Chest", DeviceType.TRACKER),
+    LEFT_KNEE(5, "vive_tracker_left_knee", "TrackerRole_LeftKnee", DeviceType.TRACKER),
+    RIGHT_KNEE(6, "vive_tracker_right_knee", "TrackerRole_RightKnee", DeviceType.TRACKER),
+    LEFT_ELBOW(7, "vive_tracker_left_elbow", "TrackerRole_LeftElbow", DeviceType.TRACKER),
+    RIGHT_ELBOW(8, "vive_tracker_right_elbow", "TrackerRole_RightElbow", DeviceType.TRACKER),
+    LEFT_SHOULDER(9, "vive_tracker_left_shoulder", "TrackerRole_LeftShoulder", DeviceType.TRACKER),
+    RIGHT_SHOULDER(10, "vive_tracker_right_shoulder", "TrackerRole_RightShoulder", DeviceType.TRACKER),
+    LEFT_HAND(11, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.TRACKER),
+    RIGHT_HAND(12, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.TRACKER),
+    LEFT_CONTROLLER(13, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.CONTROLLER),
+    RIGHT_CONTROLLER(14, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.CONTROLLER),
+    HEAD(15, "", "", DeviceType.TRACKER),
+    NECK(16, "", "", DeviceType.TRACKER),
+    CAMERA(17, "vive_tracker_camera", "TrackerRole_Camera", DeviceType.TRACKER),
+    KEYBOARD(18, "vive_tracker_keyboard", "TrackerRole_Keyboard", DeviceType.TRACKER),
+    HMD(19, "", "", DeviceType.HMD),
+    BEACON(20, "", "", DeviceType.TRACKING_REFERENCE),
+    GENERIC_CONTROLLER(21, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.CONTROLLER),
+    ;
+	
+	public final int id;
+	public final String roleHint;
+	public final String viveRole;
+	public final DeviceType deviceType;
+	
+	public static final TrackerRole[] values = values();
+	private static final TrackerRole[] byId = new TrackerRole[22];
+	
+	private TrackerRole(int id, String roleHint, String viveRole, DeviceType deviceType) {
+		this.id = id;
+		this.roleHint = roleHint;
+		this.viveRole = viveRole;
+		this.deviceType = deviceType;
+	}
+	
+	public static TrackerRole getById(int id) {
+		return id < 0 || id >= byId.length ? null : byId[id];
+	}
+	
+	static {
+		for(TrackerRole tr : values) {
+			if(byId[tr.id] != null)
+				throw new AssertionError("Tracker role id " + tr.id + " occupied by " + byId[tr.id] + " when adding " + tr);
+			byId[tr.id] = tr;
+		}
+	}
+}

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

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

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

@@ -2,26 +2,70 @@ package io.eiren.vr.trackers;
 
 import java.util.List;
 
-import io.eiren.vr.processor.TrackerBodyPosition;
-
 public class TrackerUtils {
 	
 	private TrackerUtils() {
 	}
-	
-	public static Tracker findTrackerForBodyPosition(List<Tracker> allTrackers, TrackerBodyPosition position) {
-		for(int i = 0; i < allTrackers.size(); ++i) {
-			Tracker t = allTrackers.get(i);
-			if(t.getBodyPosition() == position)
+
+	public static <T extends Tracker> T findTrackerForBodyPosition(T[] allTrackers, 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)
 				return t;
 		}
 		return null;
 	}
-	
-	public static Tracker findTrackerForBodyPosition(List<Tracker> allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) {
-		Tracker t = findTrackerForBodyPosition(allTrackers, position);
+
+	public static <T extends Tracker> T findTrackerForBodyPosition(List<T> allTrackers, 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)
+				return t;
+		}
+		return null;
+	}
+
+	public static <T extends Tracker> T findTrackerForBodyPosition(List<T> allTrackers, TrackerPosition position, TrackerPosition altPosition) {
+		T t = findTrackerForBodyPosition(allTrackers, position);
 		if(t != null)
 			return t;
 		return findTrackerForBodyPosition(allTrackers, 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;
+		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, 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);
+	}
+
+	public static Tracker findTrackerForBodyPositionOrEmpty(Tracker[] allTrackers, TrackerPosition position, TrackerPosition altPosition) {
+		Tracker t = findTrackerForBodyPosition(allTrackers, position);
+		if(t != null)
+			return t;
+		t = findTrackerForBodyPosition(allTrackers, altPosition);
+		if(t != null)
+			return t;
+		return new ComputedTracker(Tracker.getNextLocalTrackerId(), "Empty tracker", false, false);
+	}
 }

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

@@ -4,11 +4,13 @@ import java.io.IOException;
 import java.io.UnsupportedEncodingException;
 import java.net.DatagramPacket;
 import java.net.DatagramSocket;
+import java.net.InetAddress;
 import java.net.SocketAddress;
 import java.net.SocketTimeoutException;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
 import java.util.HashMap;
+import java.util.Iterator;
 import java.util.List;
 import java.util.Map;
 import java.util.Random;
@@ -39,7 +41,7 @@ public class TrackersUDPServer extends Thread {
 	private final Quaternion buf = new Quaternion();
 	private final Random random = new Random();
 	private final List<TrackerConnection> trackers = new FastList<>();
-	private final Map<SocketAddress, TrackerConnection> trackersMap = new HashMap<>();
+	private final Map<InetAddress, TrackerConnection> trackersMap = new HashMap<>();
 	private final Map<Tracker, Consumer<String>> calibrationDataRequests = new HashMap<>();
 	private final Consumer<Tracker> trackersConsumer;
 	private final int port;
@@ -55,22 +57,27 @@ public class TrackersUDPServer extends Thread {
 	
 	private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException {
 		System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort());
-		SocketAddress addr = handshakePacket.getSocketAddress();
+		InetAddress addr = handshakePacket.getAddress();
 		TrackerConnection sensor;
 		synchronized(trackers) {
 			sensor = trackersMap.get(addr);
 		}
 		if(sensor == null) {
+			boolean isOwo = false;
 			data.getLong(); // Skip packet number
 			int boardType = -1;
 			int imuType = -1;
 			int firmwareBuild = -1;
-			StringBuilder sb = new StringBuilder();
+			StringBuilder firmware = new StringBuilder();
+			byte[] mac = new byte[6];
+			String macString = null;
 			if(data.remaining() > 0) {
 				if(data.remaining() > 3)
 					boardType = data.getInt();
 				if(data.remaining() > 3)
 					imuType = data.getInt();
+				if(data.remaining() > 3)
+					data.getInt(); // MCU TYPE
 				if(data.remaining() > 11) {
 					data.getInt(); // IMU info
 					data.getInt();
@@ -78,40 +85,57 @@ public class TrackersUDPServer extends Thread {
 				}
 				if(data.remaining() > 3)
 					firmwareBuild = data.getInt();
-				while(true) {
-					if(data.remaining() == 0)
-						break;
+				int length = 0;
+				if(data.remaining() > 0)
+					length = data.get() & 0xFF; // firmware version length is 1 longer than that because it's nul-terminated
+				while(length > 0 && data.remaining() != 0) {
 					char c = (char) data.get();
 					if(c == 0)
 						break;
-					sb.append(c);
+					firmware.append(c);
+					length--;
+				}
+				if(data.remaining() > mac.length) {
+					data.get(mac);
+					macString = String.format("%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+					if(macString.equals("00:00:00:00:00:00"))
+						macString = null;
 				}
 			}
-			if(sb.length() == 0)
-				sb.append("owoTrack");
-			IMUTracker imu = new IMUTracker("udp:/" + handshakePacket.getAddress().toString(), this);
-			IMUReferenceAdjustedTracker<IMUTracker> adjustedTracker = new IMUReferenceAdjustedTracker<>(imu);
+			if(firmware.length() == 0) {
+				firmware.append("owoTrack");
+				isOwo = true;
+			}
+			String trackerName = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString();
+			String descriptiveName = "udp:/" + handshakePacket.getAddress().toString();
+			IMUTracker imu = new IMUTracker(Tracker.getNextLocalTrackerId(), trackerName, descriptiveName, this);
+			ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
 			trackersConsumer.accept(adjustedTracker);
-			sensor = new TrackerConnection(imu, addr);
+			sensor = new TrackerConnection(imu, handshakePacket.getSocketAddress());
+			sensor.isOwoTrack = isOwo;
 			int i = 0;
 			synchronized(trackers) {
 				i = trackers.size();
 				trackers.add(sensor);
 				trackersMap.put(addr, sensor);
 			}
-			System.out.println("[TrackerServer] Sensor " + i + " added with address " + addr + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + sb + " (" + firmwareBuild + ")");
+			System.out.println("[TrackerServer] Sensor " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + trackerName);
 		}
-		sensor.tracker.setStatus(TrackerStatus.OK);
+		sensor.sensors.get(0).setStatus(TrackerStatus.OK);
         socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort()));
 	}
 	
-	private void setUpAuxialrySensor(TrackerConnection connection) throws IOException {
-		System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.tracker.getName());
-		IMUTracker imu = new IMUTracker(connection.tracker.getName() + "/1", this);
-		connection.secondTracker = imu;
-		IMUReferenceAdjustedTracker<IMUTracker> adjustedTracker = new IMUReferenceAdjustedTracker<>(imu);
-		trackersConsumer.accept(adjustedTracker);
-		System.out.println("[TrackerServer] Sensor added with address " + imu.getName());
+	private void setUpAuxilarySensor(TrackerConnection connection, int trackerId) throws IOException {
+		System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.sensors.get(0).getName());
+		IMUTracker imu = connection.sensors.get(trackerId);
+		if(imu == null) {
+			imu = new IMUTracker(Tracker.getNextLocalTrackerId(), connection.sensors.get(0).getName() + "/" + trackerId, connection.sensors.get(0).getDescriptiveName() + "/" + trackerId, this);
+			connection.sensors.put(trackerId, imu);
+			ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
+			trackersConsumer.accept(adjustedTracker);
+			System.out.println("[TrackerServer] Sensor added with address " + imu.getName());
+		}
+		imu.setStatus(TrackerStatus.OK);
 	}
 	
 	
@@ -129,12 +153,13 @@ public class TrackersUDPServer extends Thread {
 					socket.receive(recieve);
 					bb.rewind();
 
-					TrackerConnection sensor;
+					TrackerConnection connection;
+					IMUTracker tracker = null;
 					synchronized(trackers) {
-						sensor = trackersMap.get(recieve.getSocketAddress());
+						connection = trackersMap.get(recieve.getAddress());
 					}
-					if(sensor != null)
-						sensor.lastPacket = System.currentTimeMillis();
+					if(connection != null)
+						connection.lastPacket = System.currentTimeMillis();
 					int packetId;
 					switch(packetId = bb.getInt()) {
 					case 0:
@@ -144,90 +169,100 @@ public class TrackersUDPServer extends Thread {
 						break;
 					case 1: // PACKET_ROTATION
 					case 16: // PACKET_ROTATION_2
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
 						offset.mult(buf, buf);
-						IMUTracker tracker;
 						if(packetId == 1) {
-							tracker = sensor.tracker;
+							tracker = connection.sensors.get(0);
 						} else {
-							tracker = sensor.secondTracker;
+							tracker = connection.sensors.get(1);
 						}
 						if(tracker == null)
 							break;
 						tracker.rotQuaternion.set(buf);
 						tracker.dataTick();
 						break;
-					case 2:
-						if(sensor == null)
+					case 17: // PACKET_ROTATION_DATA
+						if(connection == null)
+							break;
+						if(connection.isOwoTrack)
 							break;
 						bb.getLong();
-						sensor.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
+						int sensorId = bb.get() & 0xFF;
+						tracker = connection.sensors.get(sensorId);
+						if(tracker == null)
+							break;
+						
+						int dataType = bb.get() & 0xFF;
+						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
+						offset.mult(buf, buf);
+						int calibrationInfo = bb.get() & 0xFF;
+						
+						switch(dataType) {
+						case 1: // DATA_TYPE_NORMAL
+							tracker.rotQuaternion.set(buf);
+							tracker.calibrationStatus = calibrationInfo;
+							tracker.dataTick();
+							break;
+						case 2: // DATA_TYPE_CORRECTION
+							tracker.rotMagQuaternion.set(buf);
+							tracker.magCalibrationStatus = calibrationInfo;
+							tracker.hasNewCorrectionData = true;
+							break;
+						}
 						break;
-					case 4:
-						if(sensor == null)
+					case 18: // PACKET_MAGENTOMETER_ACCURACY
+						if(connection == null)
+							break;
+						if(connection.isOwoTrack)
 							break;
 						bb.getLong();
-						float x = bb.getFloat();
-						float z = bb.getFloat();
-						float y = bb.getFloat();
-						sensor.tracker.accelVector.set(x, y, z);
-						break;
-					case 5:
-						if(sensor == null)
+						sensorId = bb.get() & 0xFF;
+						tracker = connection.sensors.get(sensorId);
+						if(tracker == null)
 							break;
-						bb.getLong();
-						x = bb.getFloat();
-						z = bb.getFloat();
-						y = bb.getFloat();
-						sensor.tracker.magVector.set(x, y, z);
-						break;
-					case 6: // PACKET_RAW_CALIBRATION_DATA
-						if(sensor == null)
-							break;
-						bb.getLong();
-						//sensor.rawCalibrationData.add(new double[] {bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt()});
-						break;
-					case 7: // PACKET_GYRO_CALIBRATION_DATA
-						if(sensor == null)
-							break;
-						bb.getLong();
-						//sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()};
+						float accuracyInfo = bb.getFloat();
+						tracker.magnetometerAccuracy = accuracyInfo;
 						break;
+					case 2: // PACKET_GYRO
+					case 4: // PACKET_ACCEL
+					case 5: // PACKET_MAG
+					case 9: // PACKET_RAW_MAGENTOMETER
+						break; // None of these packets are used by SlimeVR trackers and are deprecated, use more generic PACKET_ROTATION_DATA
 					case 8: // PACKET_CONFIG
-						if(sensor == null)
+						if(connection == null)
+							break;
+						if(connection.isOwoTrack)
 							break;
 						bb.getLong();
 						MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb);
-						Consumer<String> dataConsumer = calibrationDataRequests.remove(sensor.tracker);
+						Consumer<String> dataConsumer = calibrationDataRequests.remove(connection.sensors.get(0));
 						if(dataConsumer != null) {
 							dataConsumer.accept(data.toTextMatrix());
 						}
 						break;
-					case 9: // PACKET_RAW_MAGENTOMETER
-						if(sensor == null)
-							break;
-						bb.getLong();
-						float mx = bb.getFloat();
-						float my = bb.getFloat();
-						float mz = bb.getFloat();
-						sensor.tracker.confidence = (float) Math.sqrt(mx * mx + my * my + mz * mz);
-						break;
 					case 10: // PACKET_PING_PONG:
-						if(sensor == null)
+						if(connection == null)
+							break;
+						if(connection.isOwoTrack)
 							break;
 						int pingId = bb.getInt();
-						if(sensor.lastPingPacketId == pingId) {
-							tracker = sensor.tracker;
-							tracker.ping = (int) (System.currentTimeMillis() - sensor.lastPingPacketTime) / 2;
+						if(connection.lastPingPacketId == pingId) {
+							for(int i = 0; i < connection.sensors.size(); ++i) {
+								tracker = connection.sensors.get(i);
+								tracker.ping = (int) (System.currentTimeMillis() - connection.lastPingPacketTime) / 2;
+								tracker.dataTick();
+							}
 						}
 						break;
 					case 11: // PACKET_SERIAL
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						if(connection.isOwoTrack)
+							break;
+						tracker = connection.sensors.get(0);
 						bb.getLong();
 						int length = bb.getInt();
 						for(int i = 0; i < length; ++i) {
@@ -243,44 +278,53 @@ public class TrackersUDPServer extends Thread {
 						}
 						break;
 					case 12: // PACKET_BATTERY_VOLTAGE
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.sensors.get(0);
 						bb.getLong();
 						tracker.setBatteryVoltage(bb.getFloat());
 						break;
 					case 13: // PACKET_TAP
-						if(sensor == null)
+						if(connection == null)
+							break;
+						if(connection.isOwoTrack)
 							break;
-						tracker = sensor.tracker;
 						bb.getLong();
-						byte tap = bb.get();
-						System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + ": b" + Integer.toBinaryString(tap));
+						sensorId = bb.get() & 0xFF;
+						tracker = connection.sensors.get(sensorId);
+						if(tracker == null)
+							break;
+						int tap = bb.get() & 0xFF;
+						BnoTap tapObj = new BnoTap(tap);
+						System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj  + " (b" + Integer.toBinaryString(tap) + ")");
 						break;
 					case 14: // PACKET_RESET_REASON
 						bb.getLong();
 						byte reason = bb.get();
 						System.out.println("[TrackerServer] Reset recieved from " + recieve.getSocketAddress() + ": " + reason);
-						if(sensor == null)
+						if(connection == null)
+							break;
+						sensorId = bb.get() & 0xFF;
+						tracker = connection.sensors.get(sensorId);
+						if(tracker == null)
 							break;
-						tracker = sensor.tracker;
 						tracker.setStatus(TrackerStatus.ERROR);
 						break;
 					case 15: // PACKET_SENSOR_INFO
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
-						int sensorId = bb.get() & 0xFF;
+						sensorId = bb.get() & 0xFF;
 						int sensorStatus = bb.get() & 0xFF;
-						if(sensorId == 1 && sensorStatus == 1 && sensor.secondTracker == null) {
-							setUpAuxialrySensor(sensor);
+						if(sensorId > 0 && sensorStatus == 1) {
+							setUpAuxilarySensor(connection, sensorId);
 						}
 						bb.rewind();
 						bb.putInt(15);
 						bb.put((byte) sensorId);
 						bb.put((byte) sensorStatus);
-						socket.send(new DatagramPacket(rcvBuffer, bb.position(), sensor.address));
-						System.out.println("[TrackerServer] Sensor info for " + sensor.tracker.getName() + "/" + sensorId + ": " + sensorStatus);
+						socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
+						System.out.println("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus);
 						break;
 					default:
 						System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
@@ -295,19 +339,25 @@ public class TrackersUDPServer extends Thread {
 					synchronized(trackers) {
 						for(int i = 0; i < trackers.size(); ++i) {
 							TrackerConnection conn = trackers.get(i);
-							IMUTracker tracker = conn.tracker;
 							socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, conn.address));
 							if(conn.lastPacket + 1000 < System.currentTimeMillis()) {
-								if(tracker.getStatus() != TrackerStatus.DISCONNECTED) {
-									tracker.setStatus(TrackerStatus.DISCONNECTED);
-									if(conn.secondTracker != null)
-										conn.secondTracker.setStatus(TrackerStatus.DISCONNECTED);
+								Iterator<IMUTracker> iterator = conn.sensors.values().iterator();
+								while(iterator.hasNext()) {
+									IMUTracker tracker = iterator.next();
+									if(tracker.getStatus() == TrackerStatus.OK)
+										tracker.setStatus(TrackerStatus.DISCONNECTED);
+								}
+							} else {
+								Iterator<IMUTracker> iterator = conn.sensors.values().iterator();
+								while(iterator.hasNext()) {
+									IMUTracker tracker = iterator.next();
+									if(tracker.getStatus() == TrackerStatus.DISCONNECTED)
+										tracker.setStatus(TrackerStatus.OK);
 								}
-							} else if(tracker.getStatus() != TrackerStatus.ERROR && tracker.getStatus() != TrackerStatus.BUSY) {
-								tracker.setStatus(TrackerStatus.OK);
-								if(conn.secondTracker != null)
-									conn.secondTracker.setStatus(TrackerStatus.OK);
 							}
+							IMUTracker tracker = conn.sensors.get(0);
+							if(tracker == null)
+								continue;
 							if(tracker.serialBuffer.length() > 0) {
 								if(tracker.lastSerialUpdate + 500L < System.currentTimeMillis()) {
 									serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer);
@@ -337,15 +387,15 @@ public class TrackersUDPServer extends Thread {
 	
 	private class TrackerConnection {
 		
-		IMUTracker tracker;
-		IMUTracker secondTracker;
+		Map<Integer, IMUTracker> sensors = new HashMap<>();
 		SocketAddress address;
 		public long lastPacket = System.currentTimeMillis();
 		public int lastPingPacketId = -1;
 		public long lastPingPacketTime = 0;
+		public boolean isOwoTrack = false;
 		
 		public TrackerConnection(IMUTracker tracker, SocketAddress address) {
-			this.tracker = tracker;
+			this.sensors.put(0, tracker);
 			this.address = address;
 		}
 	}

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

@@ -0,0 +1,36 @@
+package io.eiren.vr.trackers;
+
+import io.eiren.util.BufferedTimer;
+
+public class VRTracker extends ComputedTracker {
+
+	protected BufferedTimer timer = new BufferedTimer(1f);
+	
+	public VRTracker(int id, String serial, String name, boolean hasRotation, boolean hasPosition) {
+		super(id, serial, name, hasRotation, hasPosition);
+	}
+	
+	public VRTracker(int id, String name, boolean hasRotation, boolean hasPosition) {
+		super(id, name, name, hasRotation, hasPosition);
+	}
+	
+	@Override
+	public float getTPS() {
+		return timer.getAverageFPS();
+	}
+	
+	@Override
+	public void dataTick() {
+		timer.update();
+	}
+	
+	@Override
+	public boolean userEditable() {
+		return true;
+	}
+
+	@Override
+	public boolean isComputed() {
+		return false;
+	}
+}

src/test/java/io/eiren/unit/ReferenceAdjustmentsFullTests.java

@@ -1,273 +0,0 @@
-package io.eiren.unit;
-
-import com.jme3.math.FastMath;
-import com.jme3.math.Quaternion;
-
-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 static org.junit.Assert.*;
-
-import java.util.HashSet;
-import java.util.Set;
-
-import org.junit.Test;
-
-/**
- * Tests {@link ReferenceAdjustedTracker#resetFull(Quaternion)}
- */
-public class ReferenceAdjustmentsFullTests {
-	
-	private Set<String> testedTrackerNames = new HashSet<>();
-
-	@Test
-	public void check0to0() {
-		yawTest(0, 0);
-	}
-
-	@Test
-	public void check45to0() {
-		yawTest(0, 45);
-	}
-
-	@Test
-	public void check90to0() {
-		yawTest(0, 90);
-	}
-
-	@Test
-	public void check180to0() {
-		yawTest(0, 180);
-	}
-
-	@Test
-	public void check270to0() {
-		yawTest(0, 270);
-	}
-	
-	@Test
-	public void check0to45() {
-		yawTest(45, 0);
-	}
-
-	@Test
-	public void check45to45() {
-		yawTest(45, 45);
-	}
-
-	@Test
-	public void check90to45() {
-		yawTest(45, 90);
-	}
-
-	@Test
-	public void check180to45() {
-		yawTest(45, 180);
-	}
-
-	@Test
-	public void check270to45() {
-		yawTest(45, 270);
-	}
-	
-	@Test
-	public void check0to90() {
-		yawTest(90, 0);
-	}
-
-	@Test
-	public void check45to90() {
-		yawTest(90, 45);
-	}
-
-	@Test
-	public void check90to90() {
-		yawTest(90, 90);
-	}
-
-	@Test
-	public void check180to90() {
-		yawTest(90, 180);
-	}
-
-	@Test
-	public void check270to90() {
-		yawTest(90, 270);
-	}
-	
-	@Test
-	public void check0to180() {
-		yawTest(180, 0);
-	}
-
-	@Test
-	public void check45to180() {
-		yawTest(180, 45);
-	}
-
-	@Test
-	public void check90to180() {
-		yawTest(180, 90);
-	}
-
-	@Test
-	public void check180to180() {
-		yawTest(180, 180);
-	}
-
-	@Test
-	public void check270to180() {
-		yawTest(180, 270);
-	}
-	
-	private void yawTest(int refYaw, int trackerYaw) {
-		checkReferenceAdjustmentFull(q(0, refYaw, 0), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(15, refYaw, 0), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(0, refYaw, 0), q(15, trackerYaw, 0), refYaw, "Tracker(15," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerYaw, 15), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(15, refYaw, 0), q(15, trackerYaw, 15), refYaw, "Tracker(15," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerYaw, 15), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-	}
-	
-	public void checkReferenceAdjustmentFull(Quaternion referenceQuat, Quaternion trackerQuat, int refYaw, String name) {
-		ComputedTracker tracker = new ComputedTracker("test");
-		tracker.rotation.set(trackerQuat);
-		ReferenceAdjustedTracker<ComputedTracker> adj = new ReferenceAdjustedTracker<>(tracker);
-		adj.resetFull(referenceQuat);
-		Quaternion read = new Quaternion();
-		assertTrue("Adjusted tracker didn't return rotation", adj.getRotation(read));
-
-		// Use only yaw HMD rotation
-		Quaternion targetTrackerRotation = new Quaternion(referenceQuat);
-		float[] angles = new float[3];
-		targetTrackerRotation.toAngles(angles);
-		targetTrackerRotation.fromAngles(0, angles[1], 0);
-		
-		assertEquals("Adjusted quat is not equal to reference quat (" + toDegs(targetTrackerRotation) + " vs " + toDegs(read) + ")", new QuatEqualFullWithEpsilon(targetTrackerRotation), new QuatEqualFullWithEpsilon(read));
-		testAdjustedTracker(tracker, adj, name, refYaw);
-	}
-	
-	private static final boolean PRINT_TEST_RESULTS = false;
-	
-	private static int errors = 0;
-	private static int successes = 0;
-	
-	private void testAdjustedTracker(ComputedTracker tracker, ReferenceAdjustedTracker<ComputedTracker> adj, String name, int refYaw) {
-		if(!testedTrackerNames.add(name))
-			return;
-		
-		final Quaternion trackerBase = new Quaternion();
-		trackerBase.set(tracker.rotation);
-		
-		Quaternion rotation = new Quaternion();
-		Quaternion rotationCompare = new Quaternion();
-		Quaternion read = new Quaternion();
-		Quaternion diff = new Quaternion();
-		float[] angles = new float[3];
-		float[] anglesAdj = new float[3];
-		float[] anglesDiff = new float[3];
-		
-		TransformNode trackerNode = new TransformNode(name, true);
-		TransformNode rotationNode = new TransformNode("Rot", true);
-		trackerNode.attachChild(rotationNode);
-		
-		trackerNode.localTransform.setRotation(trackerBase);
-		
-		for(int yaw = 0; yaw <= 360; yaw += 30) {
-			for(int pitch = -90; pitch <= 90; pitch += 15) {
-				for(int roll = -90; roll <= 90; roll += 15) {
-					rotation.fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-					rotationCompare.fromAngles(pitch * FastMath.DEG_TO_RAD, (yaw + refYaw) * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-					rotationNode.localTransform.setRotation(rotation);
-					trackerNode.update();
-					rotationNode.update();
-					tracker.rotation.set(rotationNode.worldTransform.getRotation());
-					tracker.rotation.toAngles(angles);
-					
-					adj.getRotation(read);
-					read.toAngles(anglesAdj);
-					
-					diff.set(read).inverseLocal().multLocal(rotationCompare);
-					diff.toAngles(anglesDiff);
-					
-					if(!PRINT_TEST_RESULTS) {
-						assertTrue(name(name, yaw, pitch, roll, angles, anglesAdj, anglesDiff),
-								FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]));
-					} else {
-						if(FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]))
-							successes++;
-						else
-							errors++;
-						System.out.println(name(name, yaw, pitch, roll, angles, anglesAdj, anglesDiff));
-					}
-				}
-			}
-		}
-		if(PRINT_TEST_RESULTS)
-			System.out.println("Errors: " + errors + ", successes: " + successes);
-	}
-	
-	private static String name(String name, int yaw, int pitch, int roll, float[] angles, float[] anglesAdj, float[] anglesDiff) {
-		return name + ". Rot: " + yaw + "/" + pitch + ". "
-				+ "Angles: " + StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[0] * FastMath.RAD_TO_DEG, 1) + ", "
-				+ StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[1] * FastMath.RAD_TO_DEG, 1) + ", "
-				+ StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[2] * FastMath.RAD_TO_DEG, 1) + ". Diff: "
-				+ StringUtils.prettyNumber(anglesDiff[0] * FastMath.RAD_TO_DEG, 1) + ", "
-				+ StringUtils.prettyNumber(anglesDiff[1] * FastMath.RAD_TO_DEG, 1) + ", "
-				+ StringUtils.prettyNumber(anglesDiff[2] * FastMath.RAD_TO_DEG, 1);
-	}
-	
-	public static String toDegs(Quaternion q) {
-		float[] degs = new float[3];
-		q.toAngles(degs);
-		return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0);
-	}
-	
-	public static Quaternion q(float pitch, float yaw, float roll) {
-		return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-	}
-	
-	public static class QuatEqualFullWithEpsilon {
-		
-		private final Quaternion q;
-		
-		public QuatEqualFullWithEpsilon(Quaternion q) {
-			this.q = q;
-		}
-		
-		@Override
-		public String toString() {
-			return String.valueOf(q);
-		}
-		
-		@Override
-		public int hashCode() {
-			return q.hashCode();
-		}
-		
-		@Override
-		public boolean equals(Object obj) {
-			if(obj instanceof Quaternion)
-				obj = new QuatEqualFullWithEpsilon((Quaternion) obj);
-			if(!(obj instanceof QuatEqualFullWithEpsilon))
-				return false;
-			Quaternion q2 = ((QuatEqualFullWithEpsilon) obj).q;
-			float[] degs1 = new float[3];
-			q.toAngles(degs1);
-			float[] degs2 = new float[3];
-			q2.toAngles(degs2);
-			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs1[1] += FastMath.TWO_PI;
-			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs2[1] += FastMath.TWO_PI;
-			return FloatMath.equalsWithEpsilon(degs1[0], degs2[0])
-					&& FloatMath.equalsWithEpsilon(degs1[1], degs2[1])
-					&& FloatMath.equalsWithEpsilon(degs1[2], degs2[2]);
-		}
-	}
-}

src/test/java/io/eiren/unit/ReferenceAdjustmentsTests.java

@@ -0,0 +1,266 @@
+package io.eiren.unit;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+
+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;
+
+import java.util.function.Function;
+import java.util.stream.IntStream;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.TestFactory;
+
+/**
+ * Tests {@link ReferenceAdjustedTracker#resetFull(Quaternion)}
+ */
+public class ReferenceAdjustmentsTests {
+
+	private static final int[] yaws = {0, 45, 90, 180, 270};
+	private static final int[] pitches = {0, 15, 35, -15, -35};
+	private static final int[] rolls = {0, 15, 35, -15, -35};
+	private static final boolean PRINT_TEST_RESULTS = false;
+	private static int errors = 0;
+	private static int successes = 0;
+	
+	public static Stream<AnglesSet> getAnglesSet() {
+		return IntStream.of(yaws).mapToObj((yaw) ->
+				IntStream.of(pitches).mapToObj((pitch) ->
+					IntStream.of(rolls).mapToObj((roll) -> new AnglesSet(pitch, yaw, roll)
+			))).flatMap(Function.identity()).flatMap(Function.identity());
+	}
+	
+	@TestFactory
+	Stream<DynamicTest> getTestsYaw() {
+		return getAnglesSet().map((p) -> 
+			dynamicTest("Adjustment Yaw Test of Tracker(" + p.pitch + "," + p.yaw + "," + p.roll + ")",
+					() -> IntStream.of(yaws).forEach((refYaw) ->
+							checkReferenceAdjustmentYaw(q(p.pitch, p.yaw, p.roll), 0, refYaw, 0))
+					));
+	}
+	
+	@TestFactory
+	Stream<DynamicTest> getTestsFull() {
+		return getAnglesSet().map((p) -> 
+		dynamicTest("Adjustment Full Test of Tracker(" + p.pitch + "," + p.yaw + "," + p.roll + ")",
+				() -> getAnglesSet().forEach((ref) ->
+						checkReferenceAdjustmentFull(q(p.pitch, p.yaw, p.roll), ref.pitch, ref.yaw, ref.roll))
+				));
+	}
+	
+	@TestFactory
+	Stream<DynamicTest> getTestsForRotation() {
+		return getAnglesSet().map((p) -> 
+			IntStream.of(yaws).mapToObj((refYaw) ->
+				dynamicTest("Adjustment Rotation Test of Tracker(" + p.pitch + "," + p.yaw + "," + p.roll + "), Ref " + refYaw,
+					() -> testAdjustedTrackerRotation(q(p.pitch, p.yaw, p.roll), 0, refYaw, 0)
+			))).flatMap(Function.identity());
+	}
+	
+	public void checkReferenceAdjustmentFull(Quaternion trackerQuat, int refPitch, int refYaw, int refRoll) {
+		Quaternion referenceQuat = q(refPitch, refYaw, refRoll);
+		ComputedTracker tracker = new ComputedTracker(Tracker.getNextLocalTrackerId(), "test", true, false);
+		tracker.rotation.set(trackerQuat);
+		ReferenceAdjustedTracker<ComputedTracker> adj = new ReferenceAdjustedTracker<>(tracker);
+		adj.resetFull(referenceQuat);
+		Quaternion read = new Quaternion();
+		assertTrue(adj.getRotation(read), "Adjusted tracker didn't return rotation");
+
+		// Use only yaw HMD rotation
+		Quaternion targetTrackerRotation = new Quaternion(referenceQuat);
+		float[] angles = new float[3];
+		targetTrackerRotation.toAngles(angles);
+		targetTrackerRotation.fromAngles(0, angles[1], 0);
+		
+		assertEquals(new QuatEqualFullWithEpsilon(read), new QuatEqualFullWithEpsilon(targetTrackerRotation),
+				"Adjusted quat is not equal to reference quat (" + toDegs(targetTrackerRotation) + " vs " + toDegs(read) + ")");
+	}
+	
+	public void checkReferenceAdjustmentYaw(Quaternion trackerQuat, int refPitch, int refYaw, int refRoll) {
+		Quaternion referenceQuat = q(refPitch, refYaw, refRoll);
+		ComputedTracker tracker = new ComputedTracker(Tracker.getNextLocalTrackerId(), "test", true, false);
+		tracker.rotation.set(trackerQuat);
+		ReferenceAdjustedTracker<ComputedTracker> adj = new ReferenceAdjustedTracker<>(tracker);
+		adj.resetYaw(referenceQuat);
+		Quaternion read = new Quaternion();
+		assertTrue(adj.getRotation(read), "Adjusted tracker didn't return rotation");
+		assertEquals(new QuatEqualYawWithEpsilon(referenceQuat), new QuatEqualYawWithEpsilon(read),
+				"Adjusted quat is not equal to reference quat (" + toDegs(referenceQuat) + " vs " + toDegs(read) + ")");
+	}
+	
+	private void testAdjustedTrackerRotation(Quaternion trackerQuat, int refPitch, int refYaw, int refRoll) {
+		Quaternion referenceQuat = q(refPitch, refYaw, refRoll);
+		ComputedTracker tracker = new ComputedTracker(Tracker.getNextLocalTrackerId(), "test", true, false);
+		tracker.rotation.set(trackerQuat);
+		ReferenceAdjustedTracker<ComputedTracker> adj = new ReferenceAdjustedTracker<>(tracker);
+		adj.resetFull(referenceQuat);
+
+		// Use only yaw HMD rotation
+		Quaternion targetTrackerRotation = new Quaternion(referenceQuat);
+		float[] angles = new float[3];
+		targetTrackerRotation.toAngles(angles);
+		targetTrackerRotation.fromAngles(0, angles[1], 0);
+
+		Quaternion read = new Quaternion();
+		Quaternion rotation = new Quaternion();
+		Quaternion rotationCompare = new Quaternion();
+		Quaternion diff = new Quaternion();
+		float[] anglesAdj = new float[3];
+		float[] anglesDiff = new float[3];
+		
+		TransformNode trackerNode = new TransformNode("Tracker", true);
+		TransformNode rotationNode = new TransformNode("Rot", true);
+		rotationNode.attachChild(trackerNode);
+		
+		trackerNode.localTransform.setRotation(tracker.rotation);
+		
+		for(int yaw = 0; yaw <= 360; yaw += 30) {
+			for(int pitch = -90; pitch <= 90; pitch += 15) {
+				for(int roll = -90; roll <= 90; roll += 15) {
+					rotation.fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
+					rotationCompare.fromAngles(pitch * FastMath.DEG_TO_RAD, (yaw + refYaw) * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
+					rotationNode.localTransform.setRotation(rotation);
+					rotationNode.update();
+					tracker.rotation.set(trackerNode.worldTransform.getRotation());
+					tracker.rotation.toAngles(angles);
+					
+					adj.getRotation(read);
+					read.toAngles(anglesAdj);
+					
+					diff.set(read).inverseLocal().multLocal(rotationCompare);
+					diff.toAngles(anglesDiff);
+					
+					if(!PRINT_TEST_RESULTS) {
+						assertTrue(FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]),
+								name(yaw, pitch, roll, angles, anglesAdj, anglesDiff));
+					} else {
+						if(FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]))
+							successes++;
+						else
+							errors++;
+						System.out.println(name(yaw, pitch, roll, angles, anglesAdj, anglesDiff));
+					}
+				}
+			}
+		}
+		if(PRINT_TEST_RESULTS)
+			System.out.println("Errors: " + errors + ", successes: " + successes);
+	}
+	
+	private static String name(int yaw, int pitch, int roll, float[] angles, float[] anglesAdj, float[] anglesDiff) {
+		return "Rot: " + yaw + "/" + pitch + "/" + roll + ". "
+				+ "Angles: " + StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[0] * FastMath.RAD_TO_DEG, 1) + ", "
+				+ StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[1] * FastMath.RAD_TO_DEG, 1) + ", "
+				+ StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[2] * FastMath.RAD_TO_DEG, 1) + ". Diff: "
+				+ StringUtils.prettyNumber(anglesDiff[0] * FastMath.RAD_TO_DEG, 1) + ", "
+				+ StringUtils.prettyNumber(anglesDiff[1] * FastMath.RAD_TO_DEG, 1) + ", "
+				+ StringUtils.prettyNumber(anglesDiff[2] * FastMath.RAD_TO_DEG, 1);
+	}
+	
+	public static Quaternion q(float pitch, float yaw, float roll) {
+		return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
+	}
+	
+	public static String toDegs(Quaternion q) {
+		float[] degs = new float[3];
+		q.toAngles(degs);
+		return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0);
+	}
+	
+	private static class QuatEqualYawWithEpsilon {
+		
+		private final Quaternion q;
+		
+		public QuatEqualYawWithEpsilon(Quaternion q) {
+			this.q = q;
+		}
+		
+		@Override
+		public String toString() {
+			return String.valueOf(q);
+		}
+		
+		@Override
+		public int hashCode() {
+			return q.hashCode();
+		}
+		
+		@Override
+		public boolean equals(Object obj) {
+			if(obj instanceof Quaternion)
+				obj = new QuatEqualYawWithEpsilon((Quaternion) obj);
+			if(!(obj instanceof QuatEqualYawWithEpsilon))
+				return false;
+			Quaternion q2 = ((QuatEqualYawWithEpsilon) obj).q;
+			float[] degs1 = new float[3];
+			q.toAngles(degs1);
+			float[] degs2 = new float[3];
+			q2.toAngles(degs2);
+			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
+				degs1[1] += FastMath.TWO_PI;
+			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
+				degs2[1] += FastMath.TWO_PI;
+			return FloatMath.equalsWithEpsilon(degs1[1], degs2[1]);
+		}
+	}
+	
+	public static class QuatEqualFullWithEpsilon {
+		
+		private final Quaternion q;
+		
+		public QuatEqualFullWithEpsilon(Quaternion q) {
+			this.q = q;
+		}
+		
+		@Override
+		public String toString() {
+			return String.valueOf(q);
+		}
+		
+		@Override
+		public int hashCode() {
+			return q.hashCode();
+		}
+		
+		@Override
+		public boolean equals(Object obj) {
+			if(obj instanceof Quaternion)
+				obj = new QuatEqualFullWithEpsilon((Quaternion) obj);
+			if(!(obj instanceof QuatEqualFullWithEpsilon))
+				return false;
+			Quaternion q2 = ((QuatEqualFullWithEpsilon) obj).q;
+			float[] degs1 = new float[3];
+			q.toAngles(degs1);
+			float[] degs2 = new float[3];
+			q2.toAngles(degs2);
+			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
+				degs1[1] += FastMath.TWO_PI;
+			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
+				degs2[1] += FastMath.TWO_PI;
+			return FloatMath.equalsWithEpsilon(degs1[0], degs2[0])
+					&& FloatMath.equalsWithEpsilon(degs1[1], degs2[1])
+					&& FloatMath.equalsWithEpsilon(degs1[2], degs2[2]);
+		}
+	}
+	
+	public static class AnglesSet {
+		public final int pitch;
+		public final int yaw;
+		public final int roll;
+		
+		public AnglesSet(int pitch, int yaw, int roll) {
+			this.pitch = pitch;
+			this.yaw = yaw;
+			this.roll = roll;
+		}
+	}
+}

src/test/java/io/eiren/unit/ReferenceAdjustmentsYawTests.java

@@ -1,187 +0,0 @@
-package io.eiren.unit;
-
-import com.jme3.math.FastMath;
-import com.jme3.math.Quaternion;
-
-import io.eiren.math.FloatMath;
-import io.eiren.util.StringUtils;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.ReferenceAdjustedTracker;
-
-import static org.junit.Assert.*;
-
-import org.junit.Test;
-
-/**
- * Tests {@link ReferenceAdjustedTracker#resetYaw(Quaternion)}
- */
-public class ReferenceAdjustmentsYawTests {
-	
-	@Test
-	public void check0to0() {
-		yawTest(0, 0);
-	}
-
-	@Test
-	public void check45to0() {
-		yawTest(0, 45);
-	}
-
-	@Test
-	public void check90to0() {
-		yawTest(0, 90);
-	}
-
-	@Test
-	public void check180to0() {
-		yawTest(0, 180);
-	}
-
-	@Test
-	public void check270to0() {
-		yawTest(0, 270);
-	}
-	
-	@Test
-	public void check0to45() {
-		yawTest(45, 0);
-	}
-
-	@Test
-	public void check45to45() {
-		yawTest(45, 45);
-	}
-
-	@Test
-	public void check90to45() {
-		yawTest(45, 90);
-	}
-
-	@Test
-	public void check180to45() {
-		yawTest(45, 180);
-	}
-
-	@Test
-	public void check270to45() {
-		yawTest(45, 270);
-	}
-	
-	@Test
-	public void check0to90() {
-		yawTest(90, 0);
-	}
-
-	@Test
-	public void check45to90() {
-		yawTest(90, 45);
-	}
-
-	@Test
-	public void check90to90() {
-		yawTest(90, 90);
-	}
-
-	@Test
-	public void check180to90() {
-		yawTest(90, 180);
-	}
-
-	@Test
-	public void check270to90() {
-		yawTest(90, 270);
-	}
-	
-	@Test
-	public void check0to180() {
-		yawTest(180, 0);
-	}
-
-	@Test
-	public void check45to180() {
-		yawTest(180, 45);
-	}
-
-	@Test
-	public void check90to180() {
-		yawTest(180, 90);
-	}
-
-	@Test
-	public void check180to180() {
-		yawTest(180, 180);
-	}
-
-	@Test
-	public void check270to180() {
-		yawTest(180, 270);
-	}
-	
-	private void yawTest(float refYaw, float trackerTaw) {
-		checkReferenceAdjustmentYaw(q(0, refYaw, 0), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(0, refYaw, 15), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(15, refYaw, 0), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(15, refYaw, 15), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(0, refYaw, 0), q(15, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(0, refYaw, 15), q(0, trackerTaw, 15));
-		checkReferenceAdjustmentYaw(q(15, refYaw, 0), q(15, trackerTaw, 15));
-		checkReferenceAdjustmentYaw(q(15, refYaw, 15), q(0, trackerTaw, 15));
-	}
-	
-	public static void checkReferenceAdjustmentYaw(Quaternion referenceQuat, Quaternion trackerQuat) {
-		ComputedTracker tracker = new ComputedTracker("test");
-		tracker.rotation.set(trackerQuat);
-		ReferenceAdjustedTracker adj = new ReferenceAdjustedTracker(tracker);
-		adj.resetYaw(referenceQuat);
-		Quaternion read = new Quaternion();
-		assertTrue("Adjusted tracker didn't return rotation", adj.getRotation(read));
-		assertEquals("Adjusted quat is not equal to reference quat (" + toDegs(referenceQuat) + " vs " + toDegs(read) + ")", new QuatEqualYawWithEpsilon(referenceQuat), new QuatEqualYawWithEpsilon(read));
-	}
-	
-	public static String toDegs(Quaternion q) {
-		float[] degs = new float[3];
-		q.toAngles(degs);
-		return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0);
-	}
-	
-	public static Quaternion q(float pitch, float yaw, float roll) {
-		return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-	}
-	
-	private static class QuatEqualYawWithEpsilon {
-		
-		private final Quaternion q;
-		
-		public QuatEqualYawWithEpsilon(Quaternion q) {
-			this.q = q;
-		}
-		
-		@Override
-		public String toString() {
-			return String.valueOf(q);
-		}
-		
-		@Override
-		public int hashCode() {
-			return q.hashCode();
-		}
-		
-		@Override
-		public boolean equals(Object obj) {
-			if(obj instanceof Quaternion)
-				obj = new QuatEqualYawWithEpsilon((Quaternion) obj);
-			if(!(obj instanceof QuatEqualYawWithEpsilon))
-				return false;
-			Quaternion q2 = ((QuatEqualYawWithEpsilon) obj).q;
-			float[] degs1 = new float[3];
-			q.toAngles(degs1);
-			float[] degs2 = new float[3];
-			q2.toAngles(degs2);
-			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs1[1] += FastMath.TWO_PI;
-			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs2[1] += FastMath.TWO_PI;
-			return FloatMath.equalsWithEpsilon(degs1[1], degs2[1]);
-		}
-	}
-}