Merge pull request #114 from SlimeVR/test

Network refactoring

.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

@@ -0,0 +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: SlimeVR Server
+
+on: [ push, pull_request ]
+
+jobs:
+  test:
+    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: 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

@@ -5,7 +5,7 @@ Server orchestrates communication between multiple sensors and integrations, lik
 
 Sensors implementations:
 * [SlimeVR Tracker for ESP](https://github.com/SlimeVR/SlimeVR-Tracker-ESP) - ESP microcontrollers and multiple IMUs are supported
-* [owoTrack Mobile App](https://github.com/abb128/owoTrackVRSyncMobile) - use phone as a tracker (limited functionality and copmatibility)
+* [owoTrack Mobile App](https://github.com/abb128/owoTrackVRSyncMobile) - use phone as a tracker (limited functionality and compatibility)
 
 Integrations:
 * Use [SlimeVR OpenVR Driver](https://github.com/SlimeVR/SlimeVR-OpenVR-Driver) as a driver for SteamVR
@@ -13,4 +13,39 @@ 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/server-setup/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
+
+## Contributions
+
+By contributing to this project you are placing all your code under MIT or less restricting licenses, and you certify that the code you have used is compatible with those licenses or is authored by you. If you're doing so on your work time, you certify that your employer is okay with this.

build.gradle

@@ -7,29 +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 {
-    // 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'
+	compile project(':slime-java-commons')
 
-    // 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 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'
 
-    // Use JUnit test framework
-    testImplementation 'junit:junit:4.12'
+	// This dependency is used internally, and not exposed to consumers on their own compile classpath.
+	implementation 'com.google.guava:guava:28.2-jre'
+
+	// Use JUnit test framework
+	testImplementation platform('org.junit:junit-bom:5.7.2')
+	testImplementation 'org.junit.jupiter:junit-jupiter'
+	testImplementation 'org.junit.platform:junit-platform-launcher'
+}
+test {
+	useJUnitPlatform()
+}
+
+shadowJar {
+	archiveBaseName.set('slimevr')
+	archiveClassifier.set('')
+	archiveVersion.set('')
+}
+application {
+	mainClassName = 'dev.slimevr.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 Uninstalling 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/Main.java

@@ -0,0 +1,72 @@
+package dev.slimevr;
+
+import java.io.File;
+import java.io.IOException;
+import java.net.ServerSocket;
+
+import javax.swing.JOptionPane;
+
+import org.apache.commons.lang3.JavaVersion;
+import org.apache.commons.lang3.SystemUtils;
+
+import dev.slimevr.gui.Keybinding;
+import dev.slimevr.gui.VRServerGUI;
+import io.eiren.util.logging.LogManager;
+
+public class Main {
+	
+	public static String VERSION = "0.1.4";
+	
+	public static VRServer vrServer;
+	
+	@SuppressWarnings("unused")
+	public static void main(String[] args) {
+		System.setProperty("awt.useSystemAAFontSettings", "on");
+		System.setProperty("swing.aatext", "true");
+		
+		File dir = new File("").getAbsoluteFile();
+		try {
+			LogManager.initialize(new File(dir, "logs/"), dir);
+		} 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);
+			} catch(InterruptedException e) {
+				e.printStackTrace();
+			}
+		}
+	}
+	
+}

src/main/java/dev/slimevr/NetworkProtocol.java

@@ -0,0 +1,9 @@
+package dev.slimevr;
+
+public enum NetworkProtocol {
+	
+	OWO_LEGACY,
+	SLIMEVR_RAW,
+	SLIMEVR_FLATBUFFER,
+	SLIMEVR_WEBSOCKET;
+}

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

@@ -1,7 +1,8 @@
-package io.eiren.vr;
+package dev.slimevr;
 
 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,53 +15,80 @@ 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 dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.HumanPoseProcessor;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerConfig;
+import dev.slimevr.vr.trackers.udp.TrackersUDPServer;
+import io.eiren.util.OperatingSystem;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.util.ann.ThreadSecure;
-import io.eiren.util.ann.VRServerThread;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.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.TrackersUDPServer;
 import io.eiren.yaml.YamlException;
 import io.eiren.yaml.YamlFile;
 import io.eiren.yaml.YamlNode;
-import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerConfig;
 
 public class VRServer extends Thread {
 	
 	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");
+		loadConfig();
 		hmdTracker = new HMDTracker("HMD");
 		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);
-		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,12 +100,30 @@ public class VRServer extends Thread {
 			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 Bridge> E getVRBridge(Class<E> bridgeClass) {
+		for(int i = 0; i < bridges.size(); ++i) {
+			Bridge b = bridges.get(i);
+			if(bridgeClass.isAssignableFrom(b.getClass()))
+				return bridgeClass.cast(b);
+		}
+		return null;
+	}
+	
 	@ThreadSafe
 	public TrackerConfig getTrackerConfig(Tracker tracker) {
 		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;
@@ -87,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();
 		}
@@ -132,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) {
@@ -169,7 +215,6 @@ public class VRServer extends Thread {
 	@Override
 	@VRServerThread
 	public void run() {
-		loadConfig();
 		trackersServer.start();
 		while(true) {
 			//final long start = System.currentTimeMillis();
@@ -179,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();
@@ -224,6 +270,12 @@ public class VRServer extends Thread {
 		});
 	}
 	
+	public void resetTrackersYaw() {
+		queueTask(() -> {
+			humanPoseProcessor.resetTrackersYaw();
+		});
+	}
+	
 	public int getTrackersCount() {
 		return trackers.size();
 	}

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

@@ -0,0 +1,620 @@
+package dev.slimevr.autobone;
+
+import java.util.EnumMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Map.Entry;
+import java.util.function.Consumer;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Vector3f;
+
+import dev.slimevr.VRServer;
+import dev.slimevr.poserecorder.PoseFrameSkeleton;
+import dev.slimevr.poserecorder.PoseFrameTracker;
+import dev.slimevr.poserecorder.PoseFrames;
+import dev.slimevr.poserecorder.TrackerFrame;
+import dev.slimevr.poserecorder.TrackerFrameData;
+import dev.slimevr.vr.processor.HumanPoseProcessor;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.TrackerUtils;
+import io.eiren.util.logging.LogManager;
+import io.eiren.util.collections.FastList;
+
+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 offsetSlideErrorFactor = 0.0f;
+	public float offsetErrorFactor = 0.0f;
+	public float proportionErrorFactor = 0.2f;
+	public float heightErrorFactor = 0.1f;
+	public float positionErrorFactor = 0.0f;
+	public float positionOffsetErrorFactor = 0.0f;
+	
+	// TODO Needs much more work, probably going to rethink how the errors work to avoid this barely functional workaround @ButterscotchVanilla
+	// For scaling distances, since smaller sizes will cause smaller distances
+	//private float totalLengthBase = 2f;
+	
+	// Human average is probably 1.1235 (SD 0.07)
+	public float legBodyRatio = 1.1235f;
+	// SD of 0.07, capture 68% within range
+	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;
+	
+	// This is filled by reloadConfigValues()
+	public final EnumMap<SkeletonConfigValue, Float> configs = new EnumMap<SkeletonConfigValue, Float>(SkeletonConfigValue.class);
+	public final EnumMap<SkeletonConfigValue, Float> staticConfigs = new EnumMap<SkeletonConfigValue, Float>(SkeletonConfigValue.class);
+	
+	public final FastList<SkeletonConfigValue> heightConfigs = new FastList<SkeletonConfigValue>(new SkeletonConfigValue[]{
+			SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, SkeletonConfigValue.LEGS_LENGTH});
+	public final FastList<SkeletonConfigValue> lengthConfigs = new FastList<SkeletonConfigValue>(new SkeletonConfigValue[]{
+			SkeletonConfigValue.HEAD, SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, SkeletonConfigValue.HIPS_WIDTH, SkeletonConfigValue.LEGS_LENGTH});
+	
+	public AutoBone(VRServer server) {
+		this.server = server;
+		reloadConfigValues();
+	}
+	
+	public void reloadConfigValues() {
+		reloadConfigValues(null);
+	}
+	
+	private float readFromConfig(SkeletonConfigValue configValue) {
+		return server.config.getFloat(configValue.configKey, configValue.defaultValue);
+	}
+	
+	public void reloadConfigValues(List<PoseFrameTracker> trackers) {
+		// Load torso configs
+		staticConfigs.put(SkeletonConfigValue.HEAD, readFromConfig(SkeletonConfigValue.HEAD));
+		staticConfigs.put(SkeletonConfigValue.NECK, readFromConfig(SkeletonConfigValue.NECK));
+		configs.put(SkeletonConfigValue.TORSO, readFromConfig(SkeletonConfigValue.TORSO));
+		if(server.config.getBoolean("autobone.forceChestTracker", false) || (trackers != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.CHEST) != null)) {
+			// If force enabled or has a chest tracker
+			staticConfigs.remove(SkeletonConfigValue.CHEST);
+			configs.put(SkeletonConfigValue.CHEST, readFromConfig(SkeletonConfigValue.CHEST));
+		} else {
+			// Otherwise, make sure it's not used
+			configs.remove(SkeletonConfigValue.CHEST);
+			staticConfigs.put(SkeletonConfigValue.CHEST, readFromConfig(SkeletonConfigValue.CHEST));
+		}
+		if(server.config.getBoolean("autobone.forceHipTracker", false) || (trackers != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.HIP) != null && TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.WAIST) != null)) {
+			// If force enabled or has a hip tracker and waist tracker
+			staticConfigs.remove(SkeletonConfigValue.WAIST);
+			configs.put(SkeletonConfigValue.WAIST, readFromConfig(SkeletonConfigValue.WAIST));
+		} else {
+			// Otherwise, make sure it's not used
+			configs.remove(SkeletonConfigValue.WAIST);
+			staticConfigs.put(SkeletonConfigValue.WAIST, readFromConfig(SkeletonConfigValue.WAIST));
+		}
+		
+		// Load leg configs
+		staticConfigs.put(SkeletonConfigValue.HIPS_WIDTH, readFromConfig(SkeletonConfigValue.HIPS_WIDTH));
+		configs.put(SkeletonConfigValue.LEGS_LENGTH, readFromConfig(SkeletonConfigValue.LEGS_LENGTH));
+		configs.put(SkeletonConfigValue.KNEE_HEIGHT, readFromConfig(SkeletonConfigValue.KNEE_HEIGHT));
+		
+		// Keep "feet" at ankles
+		staticConfigs.put(SkeletonConfigValue.FOOT_LENGTH, 0f);
+		staticConfigs.put(SkeletonConfigValue.FOOT_OFFSET, 0f);
+		staticConfigs.put(SkeletonConfigValue.SKELETON_OFFSET, 0f);
+	}
+	
+	/**
+	 * A simple utility method to get the {@link HumanSkeleton} from the {@link VRServer}
+	 * @return The {@link HumanSkeleton} associated with the {@link VRServer}, or null if there is none available
+	 * @see {@link VRServer}, {@link HumanSkeleton}
+	 */
+	private HumanSkeleton getSkeleton() {
+		HumanPoseProcessor humanPoseProcessor = server != null ? server.humanPoseProcessor : null;
+		return humanPoseProcessor != null ? humanPoseProcessor.getSkeleton() : null;
+	}
+
+	public void applyConfig() {
+		if(!applyConfigToSkeleton(getSkeleton())) {
+			// Unable to apply to skeleton, save directly
+			saveConfigs();
+		}
+	}
+	
+	public boolean applyConfigToSkeleton(HumanSkeleton skeleton) {
+		if(skeleton == null) {
+			return false;
+		}
+		
+		SkeletonConfig skeletonConfig = skeleton.getSkeletonConfig();
+		skeletonConfig.setConfigs(configs, null);
+		skeletonConfig.saveToConfig(server.config);
+		server.saveConfig();
+		
+		LogManager.log.info("[AutoBone] Configured skeleton bone lengths");
+		return true;
+	}
+	
+	private void setConfig(SkeletonConfigValue config) {
+		Float value = configs.get(config);
+		if(value != null) {
+			server.config.setProperty(config.configKey, value);
+		}
+	}
+	
+	// This doesn't require a skeleton, therefore can be used if skeleton is null
+	public void saveConfigs() {
+		for(SkeletonConfigValue config : SkeletonConfigValue.values) {
+			setConfig(config);
+		}
+		
+		server.saveConfig();
+	}
+	
+	public Float getConfig(SkeletonConfigValue config) {
+		Float configVal = configs.get(config);
+		return configVal != null ? configVal : staticConfigs.get(config);
+	}
+	
+	public Float getConfig(SkeletonConfigValue config, Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigValue, Float> configsAlt) {
+		if(configs == null) {
+			throw new NullPointerException("Argument \"configs\" must not be null");
+		}
+		
+		Float configVal = configs.get(config);
+		return configVal != null || configsAlt == null ? configVal : configsAlt.get(config);
+	}
+	
+	public float sumSelectConfigs(List<SkeletonConfigValue> selection, Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigValue, Float> configsAlt) {
+		float sum = 0f;
+		
+		for(SkeletonConfigValue config : selection) {
+			Float length = getConfig(config, configs, configsAlt);
+			if(length != null) {
+				sum += length;
+			}
+		}
+		
+		return sum;
+	}
+	
+	public float sumSelectConfigs(List<SkeletonConfigValue> selection, SkeletonConfig skeletonConfig) {
+		float sum = 0f;
+		
+		for(SkeletonConfigValue config : selection) {
+			sum += skeletonConfig.getConfig(config);
+		}
+		
+		return sum;
+	}
+	
+	public float getLengthSum(Map<SkeletonConfigValue, Float> configs) {
+		return getLengthSum(configs, null);
+	}
+	
+	public float getLengthSum(Map<SkeletonConfigValue, Float> configs, Map<SkeletonConfigValue, Float> configsAlt) {
+		float length = 0f;
+		
+		if(configsAlt != null) {
+			for(Entry<SkeletonConfigValue, Float> config : configsAlt.entrySet()) {
+				// If there isn't a duplicate config
+				if(!configs.containsKey(config.getKey())) {
+					length += config.getValue();
+				}
+			}
+		}
+		
+		for(Float boneLength : configs.values()) {
+			length += boneLength;
+		}
+		
+		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();
+		
+		List<PoseFrameTracker> trackers = frames.getTrackers();
+		reloadConfigValues(trackers); // Reload configs and detect chest tracker from the first frame
+		
+		final PoseFrameSkeleton skeleton1 = new PoseFrameSkeleton(trackers, null, configs, staticConfigs);
+		final PoseFrameSkeleton skeleton2 = new PoseFrameSkeleton(trackers, null, configs, staticConfigs);
+		
+		// If target height isn't specified, auto-detect
+		if(targetHeight < 0f) {
+			// Get the current skeleton from the server
+			HumanSkeleton skeleton = getSkeleton();
+			if(skeleton != null) {
+				// If there is a skeleton available, calculate the target height from its configs
+				targetHeight = sumSelectConfigs(heightConfigs, skeleton.getSkeletonConfig());
+				LogManager.log.warning("[AutoBone] Target height loaded from skeleton (Make sure you reset before running!): " + targetHeight);
+			} else {
+				// Otherwise if there is no skeleton available, attempt to get the max HMD height from the recording
+				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;
+			}
+		}
+		
+		// Epoch loop, each epoch is one full iteration over the full dataset
+		for(int epoch = calcInitError ? -1 : 0; epoch < numEpochs; epoch++) {
+			float sumError = 0f;
+			int errorCount = 0;
+			
+			float adjustRate = epoch >= 0 ? (initialAdjustRate / FastMath.pow(adjustRateDecay, epoch)) : 0f;
+			
+			// Iterate over the frames using a cursor and an offset for comparing frames a certain number of frames apart
+			for(int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++) {
+				for(int frameCursor = 0; frameCursor < frameCount - cursorOffset; frameCursor += cursorIncrement) {
+					int frameCursor2 = frameCursor + cursorOffset;
+					
+					skeleton1.skeletonConfig.setConfigs(configs, null);
+					skeleton2.skeletonConfig.setConfigs(configs, null);
+					
+					skeleton1.setCursor(frameCursor);
+					skeleton1.updatePose();
+					
+					skeleton2.setCursor(frameCursor2);
+					skeleton2.updatePose();
+					
+					float totalLength = getLengthSum(configs);
+					float curHeight = sumSelectConfigs(heightConfigs, configs, staticConfigs);
+					//float scaleLength = sumSelectConfigs(lengthConfigs, configs, staticConfigs);
+					float errorDeriv = getErrorDeriv(frames, frameCursor, frameCursor2, skeleton1, skeleton2, targetHeight - curHeight, 1f);
+					float error = errorFunc(errorDeriv);
+					
+					// In case of fire
+					if(Float.isNaN(error) || Float.isInfinite(error)) {
+						// Extinguish
+						LogManager.log.warning("[AutoBone] Error value is invalid, resetting variables to recover");
+						reloadConfigValues(trackers);
+						
+						// 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;
+					
+					// If there is no adjustment whatsoever, skip this
+					if(adjustVal == 0f) {
+						continue;
+					}
+					
+					for(Entry<SkeletonConfigValue, Float> entry : configs.entrySet()) {
+						// Skip adjustment if the epoch is before starting (for logging only)
+						if(epoch < 0) {
+							break;
+						}
+						
+						float originalLength = entry.getValue();
+						
+						// Try positive and negative adjustments
+						boolean isHeightVar = heightConfigs.contains(entry.getKey());
+						//boolean isLengthVar = lengthConfigs.contains(entry.getKey());
+						float minError = errorDeriv;
+						float finalNewLength = -1f;
+						for(int i = 0; i < 2; i++) {
+							// 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 newScaleLength = isLengthVar ? scaleLength + curAdjustVal : scaleLength;
+							float newErrorDeriv = getErrorDeriv(frames, frameCursor, frameCursor2, skeleton1, skeleton2, targetHeight - newHeight, 1f);
+							
+							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 = sumSelectConfigs(heightConfigs, configs, staticConfigs);
+		LogManager.log.info("[AutoBone] Target height: " + targetHeight + " New height: " + finalHeight);
+		
+		return FastMath.abs(finalHeight - targetHeight);
+	}
+	
+	// The change in position of the ankle over time
+	protected float getSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) {
+		float slideLeft = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position.distance(skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position);
+		float slideRight = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position.distance(skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position);
+		
+		// Divide by 4 to halve and average, it's halved because you want to approach a midpoint, not the other point
+		return (slideLeft + slideRight) / 4f;
+	}
+	
+	// The change in distance between both of the ankles over time
+	protected float getOffsetSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) {
+		Vector3f leftFoot1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position;
+		Vector3f rightFoot1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position;
+		
+		Vector3f leftFoot2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position;
+		Vector3f rightFoot2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position;
+		
+		float slideDist1 = leftFoot1.distance(rightFoot1);
+		float slideDist2 = leftFoot2.distance(rightFoot2);
+		
+		float slideDist3 = leftFoot1.distance(rightFoot2);
+		float slideDist4 = leftFoot2.distance(rightFoot1);
+		
+		float dist1 = FastMath.abs(slideDist1 - slideDist2);
+		float dist2 = FastMath.abs(slideDist3 - slideDist4);
+		
+		float dist3 = FastMath.abs(slideDist1 - slideDist3);
+		float dist4 = FastMath.abs(slideDist1 - slideDist4);
+		
+		float dist5 = FastMath.abs(slideDist2 - slideDist3);
+		float dist6 = FastMath.abs(slideDist2 - slideDist4);
+		
+		// Divide by 12 to halve and average, it's halved because you want to approach a midpoint, not the other point
+		return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f;
+	}
+	
+	// The offset between both feet at one instant and over time
+	protected float getOffsetErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) {
+		float leftFoot1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY();
+		float rightFoot1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY();
+		
+		float leftFoot2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY();
+		float rightFoot2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY();
+		
+		float dist1 = FastMath.abs(leftFoot1 - rightFoot1);
+		float dist2 = FastMath.abs(leftFoot2 - rightFoot2);
+		
+		float dist3 = FastMath.abs(leftFoot1 - rightFoot2);
+		float dist4 = FastMath.abs(leftFoot2 - rightFoot1);
+		
+		float dist5 = FastMath.abs(leftFoot1 - leftFoot2);
+		float dist6 = FastMath.abs(rightFoot1 - rightFoot2);
+		
+		// Divide by 12 to halve and average, it's halved because you want to approach a midpoint, not the other point
+		return (dist1 + dist2 + dist3 + dist4 + dist5 + dist6) / 12f;
+	}
+	
+	// The distance from average human proportions
+	protected float getProportionErrorDeriv(SkeletonConfig skeleton) {
+		float neckLength = skeleton.getConfig(SkeletonConfigValue.NECK);
+		float chestLength = skeleton.getConfig(SkeletonConfigValue.CHEST);
+		float torsoLength = skeleton.getConfig(SkeletonConfigValue.TORSO);
+		float legsLength = skeleton.getConfig(SkeletonConfigValue.LEGS_LENGTH);
+		float kneeHeight = skeleton.getConfig(SkeletonConfigValue.KNEE_HEIGHT);
+		
+		float chestTorso = FastMath.abs((chestLength / torsoLength) - chestTorsoRatio);
+		float legBody = FastMath.abs((legsLength / (torsoLength + neckLength)) - legBodyRatio);
+		float kneeLeg = FastMath.abs((kneeHeight / legsLength) - kneeLegRatio);
+		
+		if(legBody <= legBodyRatioRange) {
+			legBody = 0f;
+		} else {
+			legBody -= legBodyRatioRange;
+		}
+		
+		return (chestTorso + legBody + kneeLeg) / 3f;
+	}
+	
+	// The distance of any points to the corresponding absolute position
+	protected float getPositionErrorDeriv(PoseFrames frames, int cursor, PoseFrameSkeleton skeleton) {
+		float offset = 0f;
+		int offsetCount = 0;
+		
+		List<PoseFrameTracker> trackers = frames.getTrackers();
+		for(int i = 0; i < trackers.size(); i++) {
+			PoseFrameTracker tracker = trackers.get(i);
+			
+			TrackerFrame trackerFrame = tracker.safeGetFrame(cursor);
+			if(trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) {
+				continue;
+			}
+			
+			Vector3f nodePos = skeleton.getComputedTracker(trackerFrame.designation.trackerRole).position;
+			if(nodePos != null) {
+				offset += FastMath.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(PoseFrames frames, int cursor1, int cursor2, PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) {
+		float offset = 0f;
+		int offsetCount = 0;
+		
+		List<PoseFrameTracker> trackers = frames.getTrackers();
+		for(int i = 0; i < trackers.size(); i++) {
+			PoseFrameTracker tracker = trackers.get(i);
+			
+			TrackerFrame trackerFrame1 = tracker.safeGetFrame(cursor1);
+			if(trackerFrame1 == null || !trackerFrame1.hasData(TrackerFrameData.POSITION)) {
+				continue;
+			}
+			
+			TrackerFrame trackerFrame2 = tracker.safeGetFrame(cursor2);
+			if(trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) {
+				continue;
+			}
+			
+			Vector3f nodePos1 = skeleton1.getComputedTracker(trackerFrame1.designation.trackerRole).position;
+			if(nodePos1 == null) {
+				continue;
+			}
+			
+			Vector3f nodePos2 = skeleton2.getComputedTracker(trackerFrame2.designation.trackerRole).position;
+			if(nodePos2 == null) {
+				continue;
+			}
+			
+			float dist1 = FastMath.abs(nodePos1.distance(trackerFrame1.position));
+			float dist2 = FastMath.abs(nodePos2.distance(trackerFrame2.position));
+			
+			offset += FastMath.abs(dist2 - dist1);
+			offsetCount++;
+		}
+		
+		return offsetCount > 0 ? offset / offsetCount : 0f;
+	}
+	
+	protected float getErrorDeriv(PoseFrames frames, int cursor1, int cursor2, PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2, float heightChange, float distScale) {
+		float totalError = 0f;
+		float sumWeight = 0f;
+		
+		if(slideErrorFactor > 0f) {
+			// This is the main error function, this calculates the distance between the foot positions on both frames
+			totalError += getSlideErrorDeriv(skeleton1, skeleton2) * distScale * slideErrorFactor;
+			sumWeight += slideErrorFactor;
+		}
+		
+		if(offsetSlideErrorFactor > 0f) {
+			// This error function compares the distance between the feet on each frame and returns the offset between them
+			totalError += getOffsetSlideErrorDeriv(skeleton1, skeleton2) * distScale * offsetSlideErrorFactor;
+			sumWeight += offsetSlideErrorFactor;
+		}
+		
+		if(offsetErrorFactor > 0f) {
+			// This error function compares the height of each foot in each frame
+			totalError += getOffsetErrorDeriv(skeleton1, skeleton2) * distScale * offsetErrorFactor;
+			sumWeight += offsetErrorFactor;
+		}
+		
+		if(proportionErrorFactor > 0f) {
+			// This error function compares the current values to general expected proportions to keep measurements in line
+			// Either skeleton will work fine, skeleton1 is used as a default
+			totalError += getProportionErrorDeriv(skeleton1.skeletonConfig) * proportionErrorFactor;
+			sumWeight += proportionErrorFactor;
+		}
+		
+		if(heightErrorFactor > 0f) {
+			// This error function compares the height change to the actual measured height of the headset
+			totalError += FastMath.abs(heightChange) * heightErrorFactor;
+			sumWeight += heightErrorFactor;
+		}
+		
+		if(positionErrorFactor > 0f) {
+			// This error function compares the position of an assigned tracker with the position on the skeleton
+			totalError += (getPositionErrorDeriv(frames, cursor1, skeleton1) + getPositionErrorDeriv(frames, cursor2, skeleton2) / 2f) * distScale * positionErrorFactor;
+			sumWeight += positionErrorFactor;
+		}
+		
+		if(positionOffsetErrorFactor > 0f) {
+			// This error function compares the offset of the position of an assigned tracker with the position on the skeleton
+			totalError += getPositionOffsetErrorDeriv(frames, cursor1, cursor2, skeleton1, skeleton2) * distScale * positionOffsetErrorFactor;
+			sumWeight += positionOffsetErrorFactor;
+		}
+		
+		return sumWeight > 0f ? totalError / sumWeight : 0f;
+	}
+	
+	// Mean square error function
+	protected static float errorFunc(float errorDeriv) {
+		return 0.5f * (errorDeriv * errorDeriv);
+	}
+	
+	protected void updateSkeletonBoneLength(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2, SkeletonConfigValue config, float newLength) {
+		skeleton1.skeletonConfig.setConfig(config, newLength);
+		skeleton1.updatePoseAffectedByConfig(config);
+		
+		skeleton2.skeletonConfig.setConfig(config, newLength);
+		skeleton2.updatePoseAffectedByConfig(config);
+	}
+}

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

@@ -0,0 +1,43 @@
+package dev.slimevr.bridge;
+
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.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.Main;
+import dev.slimevr.bridge.Pipe.PipeState;
+import dev.slimevr.bridge.ProtobufMessages.ProtobufMessage;
+import dev.slimevr.bridge.ProtobufMessages.TrackerAdded;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.VRTracker;
+import io.eiren.util.logging.LogManager;
+
+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,20 +12,25 @@ import com.sun.jna.platform.win32.WinBase;
 import com.sun.jna.platform.win32.WinNT.HANDLE;
 import com.sun.jna.ptr.IntByReference;
 
+import dev.slimevr.VRServer;
+import dev.slimevr.bridge.Pipe.PipeState;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerStatus;
 import io.eiren.util.collections.FastList;
 import io.eiren.util.logging.LogManager;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.HMDTracker;
-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();
@@ -38,10 +43,10 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	private final List<? extends Tracker> shareTrackers;
 	private final List<ComputedTracker> internalTrackers;
 	
-	private final HMDTracker internalHMDTracker = new HMDTracker("itnernal://HMD");
+	private final HMDTracker internalHMDTracker = new HMDTracker("internal://HMD");
 	private final AtomicBoolean newHMDData = new AtomicBoolean(false);
 	
-	public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> shareTrackers) {
+	public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> shareTrackers, VRServer server) {
 		super("Named Pipe VR Bridge");
 		this.hmd = hmd;
 		this.shareTrackers = new FastList<>(shareTrackers);
@@ -49,7 +54,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 		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);
 		}
@@ -111,30 +116,28 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 		}
 	}
 	
-	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;
 				}
@@ -143,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 received: " + 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) {
@@ -154,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);
 			}
 		}
 	}
@@ -168,11 +195,11 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	
 	private void initTrackerPipe(Pipe pipe, int trackerId) {
 		String trackerHello = this.shareTrackers.size() + " 0";
-		System.arraycopy(trackerHello.getBytes(ASCII), 0, buffer, 0, trackerHello.length());
-		buffer[trackerHello.length()] = '\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);
@@ -180,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;
 		}
@@ -243,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 dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.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.Main;
+import dev.slimevr.bridge.ProtobufMessages.Position;
+import dev.slimevr.bridge.ProtobufMessages.ProtobufMessage;
+import dev.slimevr.bridge.ProtobufMessages.TrackerAdded;
+import dev.slimevr.bridge.ProtobufMessages.TrackerStatus;
+import dev.slimevr.bridge.ProtobufMessages.UserAction;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.VRTracker;
+import io.eiren.util.ann.Synchronize;
+import io.eiren.util.ann.ThreadSafe;
+import io.eiren.util.collections.FastList;
+
+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(dev.slimevr.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(dev.slimevr.vr.trackers.TrackerStatus.DISCONNECTED);
+			}
+		}
+		if(hmdTracker != null) {
+			hmd.setStatus(dev.slimevr.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.VRServer;
+import dev.slimevr.bridge.Pipe.PipeState;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerStatus;
+import dev.slimevr.vr.trackers.VRTracker;
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+
+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 dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.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 dev.slimevr.Main;
+import dev.slimevr.VRServer;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerStatus;
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+
+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,23 @@
+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,439 @@
+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.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 javax.swing.event.MouseInputAdapter;
+
+import org.apache.commons.lang3.tuple.Pair;
+
+import dev.slimevr.VRServer;
+import dev.slimevr.autobone.AutoBone;
+import dev.slimevr.gui.swing.EJBox;
+import dev.slimevr.poserecorder.PoseFrames;
+import dev.slimevr.poserecorder.PoseFrameIO;
+import dev.slimevr.poserecorder.PoseRecorder;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
+
+public class AutoBoneWindow extends JFrame {
+	
+	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 SkeletonConfigGUI 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, SkeletonConfigGUI 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() {
+		final StringBuilder configInfo = new StringBuilder();
+		autoBone.configs.forEach((key, value) -> {
+			if(configInfo.length() > 0) {
+				configInfo.append(", ");
+			}
+			
+			configInfo.append(key.stringVal + ": " + StringUtils.prettyNumber(value * 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.offsetSlideErrorFactor = server.config.getFloat("autobone.offsetSlideErrorFactor", autoBone.offsetSlideErrorFactor);
+		autoBone.offsetErrorFactor = server.config.getFloat("autobone.offsetErrorFactor", autoBone.offsetErrorFactor);
+		autoBone.proportionErrorFactor = server.config.getFloat("autobone.proportionErrorFactor", autoBone.proportionErrorFactor);
+		autoBone.heightErrorFactor = server.config.getFloat("autobone.heightErrorFactor", autoBone.heightErrorFactor);
+		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(SkeletonConfigValue.NECK);
+												Float chestDistance = autoBone.getConfig(SkeletonConfigValue.CHEST);
+												Float torsoLength = autoBone.getConfig(SkeletonConfigValue.TORSO);
+												Float hipWidth = autoBone.getConfig(SkeletonConfigValue.HIPS_WIDTH);
+												Float legsLength = autoBone.getConfig(SkeletonConfigValue.LEGS_LENGTH);
+												Float kneeHeight = autoBone.getConfig(SkeletonConfigValue.KNEE_HEIGHT);
+												
+												float 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,9 +12,10 @@ import javax.swing.JTextArea;
 import javax.swing.border.EmptyBorder;
 import javax.swing.event.MouseInputAdapter;
 
+import dev.slimevr.gui.swing.EJBox;
+import dev.slimevr.vr.trackers.CalibratingTracker;
+import dev.slimevr.vr.trackers.Tracker;
 import io.eiren.util.ann.AWTThread;
-import io.eiren.vr.trackers.CalibratingTracker;
-import io.eiren.vr.trackers.Tracker;
 
 public class CalibrationWindow extends JFrame {
 	

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

@@ -0,0 +1,64 @@
+package dev.slimevr.gui;
+
+import com.melloware.jintellitype.HotkeyListener;
+import com.melloware.jintellitype.JIntellitype;
+
+import dev.slimevr.VRServer;
+import io.eiren.util.OperatingSystem;
+import io.eiren.util.ann.AWTThread;
+import io.eiren.util.logging.LogManager;
+
+public class Keybinding implements HotkeyListener {
+	public final VRServer server;
+	private static final int RESET = 1;
+	private static final int QUICK_RESET = 2;
+	
+	@AWTThread
+	public Keybinding(VRServer server) {
+		this.server = server;
+		
+		if (OperatingSystem.getCurrentPlatform() != OperatingSystem.WINDOWS) {
+			LogManager.log.info("[Keybinding] Currently only supported on Windows. Keybindings will be disabled.");
+			return;
+		}
+		
+		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(Throwable e) {
+			LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer.");
+		}
+	}
+	
+	@AWTThread
+	@Override
+	public void onHotKey(int identifier) {
+		switch(identifier) {
+		case RESET:
+			LogManager.log.info("[Keybinding] Reset pressed");
+			server.resetTrackers();
+			break;
+		case QUICK_RESET:
+			LogManager.log.info("[Keybinding] Quick reset pressed");
+			server.resetTrackersYaw();
+			break;
+		}
+	}
+}

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

@@ -0,0 +1,90 @@
+package dev.slimevr.gui;
+
+import java.awt.Font;
+import java.text.AttributedCharacterIterator.Attribute;
+import java.util.Map;
+
+public class ScalableFont extends Font {
+	
+	protected float scale = 1.0f;
+	
+	protected int initSize;
+	protected float initPointSize;
+	
+	public ScalableFont(Map<? extends Attribute, ?> attributes) {
+		super(attributes);
+		
+		this.initSize = this.size;
+		this.initPointSize = this.pointSize;
+	}
+	
+	public ScalableFont(Font font) {
+		super(font);
+		
+		if(font instanceof ScalableFont) {
+			ScalableFont sourceFont = (ScalableFont) font;
+			
+			this.initSize = sourceFont.getInitSize();
+			this.initPointSize = sourceFont.getInitSize2D();
+			
+			this.size = this.initSize;
+			this.pointSize = this.initPointSize;
+		} else {
+			this.initSize = this.size;
+			this.initPointSize = this.pointSize;
+		}
+	}
+	
+	public ScalableFont(Font font, float scale) {
+		super(font);
+		
+		if(font instanceof ScalableFont) {
+			ScalableFont sourceFont = (ScalableFont) font;
+			
+			this.initSize = sourceFont.getInitSize();
+			this.initPointSize = sourceFont.getInitSize2D();
+		} else {
+			this.initSize = this.size;
+			this.initPointSize = this.pointSize;
+		}
+		
+		setScale(scale);
+	}
+	
+	public ScalableFont(String name, int style, int size) {
+		super(name, style, size);
+		
+		this.initSize = this.size;
+		this.initPointSize = this.pointSize;
+	}
+	
+	public ScalableFont(String name, int style, int size, float scale) {
+		super(name, style, size);
+		
+		this.initSize = this.size;
+		this.initPointSize = this.pointSize;
+		
+		setScale(scale);
+	}
+	
+	public int getInitSize() {
+		return initSize;
+	}
+	
+	public float getInitSize2D() {
+		return initPointSize;
+	}
+	
+	public float getScale() {
+		return scale;
+	}
+	
+	private void setScale(float scale) {
+		this.scale = scale;
+		
+		float newPointSize = initPointSize * scale;
+		
+		this.size = (int) (newPointSize + 0.5);
+		this.pointSize = newPointSize;
+	}
+}

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

@@ -0,0 +1,228 @@
+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.VRServer;
+import dev.slimevr.gui.swing.ButtonTimer;
+import dev.slimevr.gui.swing.EJBagNoStretch;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
+import io.eiren.util.StringUtils;
+import io.eiren.util.ann.ThreadSafe;
+
+public class SkeletonConfigGUI extends EJBagNoStretch {
+
+	private final VRServer server;
+	private final VRServerGUI gui;
+	private final AutoBoneWindow autoBone;
+	private Map<SkeletonConfigValue, SkeletonLabel> labels = new HashMap<>();
+
+	public SkeletonConfigGUI(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"), 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++;
+
+			for (SkeletonConfigValue config : SkeletonConfigValue.values) {
+				add(new JLabel(config.label), c(0, row, 2));
+				add(new AdjButton("+", config, 0.01f), c(1, row, 2));
+				add(new SkeletonLabel(config), c(2, row, 2));
+				add(new AdjButton("-", config, -0.01f), c(3, row, 2));
+
+				// Only use a timer on configs that need time to get into position for
+				switch (config) {
+				case TORSO:
+				case LEGS_LENGTH:
+					add(new TimedResetButton("Reset", config), c(4, row, 2));
+					break;
+				default:
+					add(new ResetButton("Reset", config), c(4, row, 2));
+					break;
+				}
+
+				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(SkeletonConfigValue joint, float diff) {
+		// Update config value
+		float current = server.humanPoseProcessor.getSkeletonConfig(joint);
+		server.humanPoseProcessor.setSkeletonConfig(joint, current + diff);
+		server.humanPoseProcessor.getSkeletonConfig().saveToConfig(server.config);
+		server.saveConfig();
+
+		// Update GUI
+		labels.get(joint).setText(StringUtils.prettyNumber((current + diff) * 100, 0));
+	}
+
+	private void reset(SkeletonConfigValue joint) {
+		// Update config value
+		server.humanPoseProcessor.resetSkeletonConfig(joint);
+		server.humanPoseProcessor.getSkeletonConfig().saveToConfig(server.config);
+		server.saveConfig();
+
+		// Update GUI
+		float current = server.humanPoseProcessor.getSkeletonConfig(joint);
+		labels.get(joint).setText(StringUtils.prettyNumber((current) * 100, 0));
+	}
+
+	private void resetAll() {
+		// Update config value
+		server.humanPoseProcessor.resetAllSkeletonConfigs();
+		server.humanPoseProcessor.getSkeletonConfig().saveToConfig(server.config);
+		server.saveConfig();
+
+		// Update GUI
+		refreshAll();
+	}
+
+	private class SkeletonLabel extends JLabel {
+
+		public SkeletonLabel(SkeletonConfigValue joint) {
+			super(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint) * 100, 0));
+			labels.put(joint, this);
+		}
+	}
+
+	private class AdjButton extends JButton {
+
+		public AdjButton(String text, SkeletonConfigValue 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, SkeletonConfigValue joint) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					reset(joint);
+				}
+			});
+		}
+	}
+
+	private class TimedResetButton extends JButton {
+
+		public TimedResetButton(String text, SkeletonConfigValue joint) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					ButtonTimer.runTimer(TimedResetButton.this, 3, text, () -> reset(joint));
+				}
+			});
+		}
+
+		public TimedResetButton(String text) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					ButtonTimer.runTimer(TimedResetButton.this, 3, text, () -> resetAll());
+				}
+			});
+		}
+	}
+}

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

@@ -1,4 +1,4 @@
-package io.eiren.gui;
+package dev.slimevr.gui;
 
 import java.awt.GridBagConstraints;
 import java.util.List;
@@ -9,27 +9,29 @@ import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.VRServer;
+import dev.slimevr.gui.swing.EJBagNoStretch;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.TransformNode;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.ThreadSafe;
-import io.eiren.util.ann.VRServerThread;
 import io.eiren.util.collections.FastList;
-import io.eiren.vr.VRServer;
-import io.eiren.vr.processor.HumanSkeleton;
-import io.eiren.vr.processor.TransformNode;
 
-public class SkeletonList extends 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,421 @@
+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.VRServer;
+import dev.slimevr.gui.swing.EJBagNoStretch;
+import dev.slimevr.gui.swing.EJBoxNoStretch;
+import dev.slimevr.vr.trackers.ComputedTracker;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.IMUTracker;
+import dev.slimevr.vr.trackers.ReferenceAdjustedTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerConfig;
+import dev.slimevr.vr.trackers.TrackerMountingRotation;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerWithBattery;
+import dev.slimevr.vr.trackers.TrackerWithTPS;
+import io.eiren.util.StringUtils;
+import io.eiren.util.ann.AWTThread;
+import io.eiren.util.ann.ThreadSafe;
+import io.eiren.util.collections.FastList;
+
+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;
+	private boolean debug = false;
+
+	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
+	public void setDebug(boolean debug) {
+		this.debug = debug;
+		build();
+	}
+
+	@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 adjGyro;
+		JLabel correction;
+		JLabel signalStrength;
+		JLabel rotQuat;
+		JLabel rotAdj;
+		JLabel temperature;
+		
+		@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));
+				add(new JLabel("Signal"), c(4, 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));
+				add(signalStrength = new JLabel(""), c(4, 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));
+			
+			if(debug && realTracker instanceof IMUTracker) {
+				add(new JLabel("Quat:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(rotQuat = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+			}
+			row++;
+			
+			if(debug && 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));
+				add(new JLabel("Gyro fix:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel(String.format("0x%8x", realTracker.hashCode())), s(c(3, 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));
+				add(new JLabel("RotAdj:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(rotAdj = new JLabel("0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+				row++;
+			}
+			
+			if(debug && t instanceof ReferenceAdjustedTracker) {	
+				add(new JLabel("Att fix:"), 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("Yaw Fix:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(adjYaw = new JLabel("0 0 0 0"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START));
+				row++;
+				add(new JLabel("Gyro Fix:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(adjGyro = new JLabel("0 0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("Temp:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(temperature = new JLabel("?"), 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(String.format("%d%% (%sV)", Math.round(((TrackerWithBattery) realTracker).getBatteryLevel()), StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 2)));
+			if(t instanceof ReferenceAdjustedTracker) {
+				ReferenceAdjustedTracker<Tracker> rat = (ReferenceAdjustedTracker<Tracker>) t;
+				if(adj != null) {
+					rat.attachmentFix.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));
+				}
+				if(adjYaw != null) {
+					rat.yawFix.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(adjGyro != null) {
+					rat.gyroFix.toAngles(angles);
+					adjGyro.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));
+				if(signalStrength != null) {
+					int signal = ((IMUTracker) realTracker).signalStrength;
+					if (signal == -1) {
+						signalStrength.setText("N/A");
+					} else {
+						// -40 dBm is excellent, -95 dBm is very poor
+						int percentage = (signal - -95) * (100 - 0) / (-40 - -95) + 0;
+						percentage = Math.max(Math.min(percentage, 100), 0);
+						signalStrength.setText(String.valueOf(percentage) + "% " + "(" + String.valueOf(signal) + " dBm" + ")");
+					}
+				}
+			}
+			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 imu = (IMUTracker) realTracker;
+				if(rawMag != null) {
+					imu.rotMagQuaternion.toAngles(angles);
+					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(imu.calibrationStatus + " / " + imu.magCalibrationStatus);
+				if(magAccuracy != null)
+					magAccuracy.setText(StringUtils.prettyNumber(imu.magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°");
+				if(correction != null) {
+					imu.getCorrection(q);
+					q.toAngles(angles);
+					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));
+				}
+				if(rotQuat != null) {
+					imu.rotQuaternion.toAngles(angles);
+					rotQuat.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(rotAdj != null) {
+					imu.rotAdjust.toAngles(angles);
+					rotAdj.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(temperature != null) {
+					if(imu.temperature == 0.0f) {
+						// Can't be exact 0, so no info received
+						temperature.setText("?");
+					} else {
+						temperature.setText(StringUtils.prettyNumber(imu.temperature, 1) + "∘C");
+					}
+				}
+			}
+		}
+	}
+	
+	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,349 @@
+package dev.slimevr.gui;
+
+import javax.imageio.ImageIO;
+import javax.swing.*;
+import javax.swing.border.EmptyBorder;
+import javax.swing.event.MouseInputAdapter;
+
+import dev.slimevr.Main;
+import dev.slimevr.VRServer;
+import dev.slimevr.bridge.NamedPipeBridge;
+import dev.slimevr.gui.swing.ButtonTimer;
+import dev.slimevr.gui.swing.EJBagNoStretch;
+import dev.slimevr.gui.swing.EJBox;
+import dev.slimevr.gui.swing.EJBoxNoStretch;
+import dev.slimevr.vr.trackers.TrackerRole;
+import io.eiren.util.MacOSX;
+import io.eiren.util.OperatingSystem;
+import io.eiren.util.StringUtils;
+import io.eiren.util.ann.AWTThread;
+
+import 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);
+				
+				JCheckBox debugCb;
+				add(debugCb = new JCheckBox("Show debug information"));
+				debugCb.setSelected(false);
+				debugCb.addActionListener(new ActionListener() {
+					@Override
+					public void actionPerformed(ActionEvent e) {
+						trackersList.setDebug(debugCb.isSelected());
+					}
+				});
+				
+				JLabel l;
+				add(l = new JLabel("Body proportions"));
+				l.setFont(l.getFont().deriveFont(Font.BOLD));
+				l.setAlignmentX(0.5f);
+				add(new SkeletonConfigGUI(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

@@ -0,0 +1,172 @@
+package dev.slimevr.gui;
+
+import java.awt.Container;
+import java.awt.event.MouseEvent;
+import java.awt.event.WindowAdapter;
+import java.awt.event.WindowEvent;
+import java.io.IOException;
+import java.io.InputStream;
+import java.io.InputStreamReader;
+import java.io.OutputStream;
+import java.io.OutputStreamWriter;
+import java.io.Reader;
+import java.util.Timer;
+import java.util.TimerTask;
+
+import javax.swing.BoxLayout;
+import javax.swing.JButton;
+import javax.swing.JFrame;
+import javax.swing.JLabel;
+import javax.swing.JScrollPane;
+import javax.swing.JTextArea;
+import javax.swing.JTextField;
+import javax.swing.ScrollPaneConstants;
+import javax.swing.WindowConstants;
+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 {
+	
+	private static final Timer timer = new Timer();
+	private static String savedSSID = "";
+	private static String savedPassword = "";
+	JTextField ssidField;
+	JTextField passwdField;
+	SerialPort trackerPort = null;
+	JTextArea log;
+	TimerTask readTask;
+	
+	public WiFiWindow(VRServerGUI gui) {
+		super("WiFi Settings");
+		getContentPane().setLayout(new BoxLayout(getContentPane(), BoxLayout.LINE_AXIS));
+		
+		build();
+	}
+	
+	@AWTThread
+	private void build() {
+		Container pane = getContentPane();
+		
+		
+		SerialPort[] ports = SerialPort.getCommPorts();
+		for(SerialPort port : ports) {
+			if(port.getDescriptivePortName().toLowerCase().contains("ch340") || port.getDescriptivePortName().toLowerCase().contains("cp21") || port.getDescriptivePortName().toLowerCase().contains("ch910")) {
+				trackerPort = port;
+				break;
+			}
+		}
+		pane.add(new EJBox(BoxLayout.PAGE_AXIS) {{
+			if(trackerPort == null) {
+				add(new JLabel("No trackers connected, connect tracker to USB and reopen window"));
+				timer.schedule(new TimerTask() {
+					@Override
+					public void run() {
+						WiFiWindow.this.dispose();
+					}
+				}, 5000);
+			} else {
+				add(new JLabel("Tracker connected to " + trackerPort.getSystemPortName() + " (" + trackerPort.getDescriptivePortName() + ")"));
+				JScrollPane scroll;
+				add(scroll = new JScrollPane(log = new JTextArea(10, 20), ScrollPaneConstants.VERTICAL_SCROLLBAR_ALWAYS, ScrollPaneConstants.HORIZONTAL_SCROLLBAR_NEVER));
+				log.setLineWrap(true);
+				scroll.setAutoscrolls(true);
+				if(trackerPort.openPort()) {
+					trackerPort.setBaudRate(115200);
+					log.append("[OK] Port opened\n");
+					readTask = new ReadTask();
+					timer.schedule(readTask, 500, 500);
+				} else {
+					log.append("ERROR: Can't open port");
+				}
+				
+				add(new JLabel("Enter WiFi credentials:"));
+				add(new EJBox(BoxLayout.LINE_AXIS) {{
+					add(new JLabel("Network name:"));
+					add(ssidField = new JTextField(savedSSID));
+				}});
+				add(new EJBox(BoxLayout.LINE_AXIS) {{
+					add(new JLabel("Network password:"));
+					add(passwdField = new JTextField(savedPassword));
+				}});
+				add(new JButton("Send") {{
+					addMouseListener(new MouseInputAdapter() {
+						@Override
+						public void mouseClicked(MouseEvent e) {
+							send(ssidField.getText(), passwdField.getText());
+						}
+					});
+				}});
+			}
+		}});
+		
+		
+		// Pack and display
+		pack();
+		setLocationRelativeTo(null);
+		setVisible(true);
+		java.awt.EventQueue.invokeLater(new Runnable() {
+			@Override
+			public void run() {
+				toFront();
+				repaint();
+			}
+		});
+		setDefaultCloseOperation(WindowConstants.DISPOSE_ON_CLOSE);
+		addWindowListener(new WindowAdapter() {
+	        @Override
+	        public void windowClosing(WindowEvent windowEvent) {
+	            if(trackerPort != null)
+	            	trackerPort.closePort();
+	            if(readTask != null)
+	            	readTask.cancel();
+	            System.out.println("Port closed okay");
+	            dispose();
+	        }
+	    });
+	}
+	
+	protected void send(String ssid, String passwd) {
+		savedSSID = ssid;
+		savedPassword = passwd;
+		OutputStream os = trackerPort.getOutputStream();
+		OutputStreamWriter writer = new OutputStreamWriter(os);
+		try {
+			writer.append("SET WIFI \"" + ssid + "\" \"" + passwd + "\"\n");
+			writer.flush();
+		} catch(IOException e) {
+			log.append(e.toString() + "\n");
+			e.printStackTrace();
+		}
+	}
+	
+	private class ReadTask extends TimerTask {
+		
+		final InputStream is;
+		final Reader reader;
+		StringBuffer sb = new StringBuffer();
+		
+		public ReadTask() {
+			is = trackerPort.getInputStreamWithSuppressedTimeoutExceptions();
+			reader = new InputStreamReader(is);
+		}
+
+		@Override
+		public void run() {
+			try {
+				while(reader.ready())
+					sb.appendCodePoint(reader.read());
+				if(sb.length() > 0)
+					log.append(sb.toString());
+				sb.setLength(0);
+			} catch(Exception e) {
+				log.append(e.toString() + "\n");
+				e.printStackTrace();
+			}
+		}
+		
+	}
+}

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

@@ -0,0 +1,42 @@
+package dev.slimevr.gui.swing;
+
+import java.util.Timer;
+import java.util.TimerTask;
+
+import javax.swing.AbstractButton;
+
+public class ButtonTimer {
+
+	private static Timer timer = new Timer();
+	
+	public static void runTimer(AbstractButton button, int seconds, String defaultText, Runnable runnable) {
+		if(seconds <= 0) {
+			button.setText(defaultText);
+			runnable.run();
+		} else {
+			button.setText(String.valueOf(seconds));
+			timer.schedule(new ButtonTimerTask(button, seconds - 1, defaultText, runnable), 1000);
+		}
+	}
+	
+	private static class ButtonTimerTask extends TimerTask {
+		
+		private final AbstractButton button;
+		private final int seconds;
+		private final String defaultText;
+		private final Runnable runnable;
+		
+		private ButtonTimerTask(AbstractButton button, int seconds, String defaultText, Runnable runnable) {
+			this.button = button;
+			this.seconds = seconds;
+			this.defaultText = defaultText;
+			this.runnable = runnable;
+		}
+
+		@Override
+		public void run() {
+			runTimer(button, seconds, defaultText, runnable);
+		}
+		
+	}
+}

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/hardware/magentometer/Magneto.java

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

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

@@ -0,0 +1,138 @@
+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 dev.slimevr.vr.trackers.TrackerPosition;
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+
+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/PoseFrameSkeleton.java

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

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

@@ -0,0 +1,239 @@
+package dev.slimevr.poserecorder;
+
+import java.util.Iterator;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerConfig;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerStatus;
+import io.eiren.util.collections.FastList;
+
+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(Quaternion.IDENTITY);
+		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(Vector3f.ZERO);
+		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 1f;
+	}
+	
+	@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 dev.slimevr.vr.trackers.Tracker;
+import io.eiren.util.collections.FastList;
+
+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 dev.slimevr.VRServer;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.trackers.Tracker;
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+
+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 dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerConfig;
+import dev.slimevr.vr.trackers.TrackerPosition;
+import dev.slimevr.vr.trackers.TrackerStatus;
+
+public final class TrackerFrame implements Tracker {
+	
+	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(Quaternion.IDENTITY);
+		return false;
+	}
+	
+	@Override
+	public boolean getPosition(Vector3f store) {
+		if(hasData(TrackerFrameData.POSITION)) {
+			store.set(position);
+			return true;
+		}
+		
+		store.set(Vector3f.ZERO);
+		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 1f;
+	}
+	
+	@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 dev.slimevr.vr.processor.TransformNode;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+
+public class BVHFileStream extends PoseDataStream {
+	
+	private static final int LONG_MAX_VALUE_DIGITS = Long.toString(Long.MAX_VALUE).length();
+	private static final float 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 dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+
+public abstract class PoseDataStream implements AutoCloseable {
+	
+	protected boolean closed = false;
+	protected final OutputStream outputStream;
+	
+	protected PoseDataStream(OutputStream outputStream) {
+		this.outputStream = outputStream;
+	}
+	
+	protected PoseDataStream(File file) throws FileNotFoundException {
+		this(new FileOutputStream(file));
+	}
+	
+	protected PoseDataStream(String file) throws FileNotFoundException {
+		this(new FileOutputStream(file));
+	}
+	
+	public void writeHeader(HumanSkeleton skeleton, PoseStreamer streamer) throws IOException {
+	}
+	
+	abstract void writeFrame(HumanSkeleton skeleton) throws IOException;
+	
+	public void writeFooter(HumanSkeleton skeleton) throws IOException {
+	}
+	
+	public boolean isClosed() {
+		return closed;
+	}
+	
+	@Override
+	public void close() throws IOException {
+		outputStream.close();
+		closed = true;
+	}
+}

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

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

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

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

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

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

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

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

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

@@ -1,9 +1,11 @@
-package io.eiren.vr.processor;
+package dev.slimevr.vr.processor;
 
 public enum ComputedHumanPoseTrackerPosition {
 	
 	WAIST,
 	CHEST,
 	LEFT_FOOT,
-	RIGHT_FOOT;
+	RIGHT_FOOT,
+	LEFT_KNEE,
+	RIGHT_KNEE;
 }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,134 @@
+package dev.slimevr.vr.trackers;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+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 TrackerPosition bodyPosition = null;
+	protected final boolean hasRotation;
+	protected final boolean hasPosition;
+	protected final int trackerId;
+	
+	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
+	public void saveConfig(TrackerConfig config) {
+		config.setDesignation(bodyPosition == null ? null : bodyPosition.designation);
+	}
+	
+	@Override
+	public void loadConfig(TrackerConfig config) {
+		// 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;
+	}
+	
+	@Override
+	public boolean getPosition(Vector3f store) {
+		store.set(position);
+		return true;
+	}
+
+	@Override
+	public boolean getRotation(Quaternion store) {
+		store.set(rotation);
+		return true;
+	}
+
+	@Override
+	public TrackerStatus getStatus() {
+		return status;
+	}
+	
+	public void setStatus(TrackerStatus status) {
+		this.status = status;
+	}
+	
+	@Override
+	public float getConfidenceLevel() {
+		return 1.0f;
+	}
+
+	@Override
+	public void resetFull(Quaternion reference) {
+	}
+
+	@Override
+	public void resetYaw(Quaternion reference) {
+	}
+
+	@Override
+	public TrackerPosition getBodyPosition() {
+		return bodyPosition;
+	}
+
+	@Override
+	public void setBodyPosition(TrackerPosition position) {
+		this.bodyPosition = position;
+	}
+
+	@Override
+	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/dev/slimevr/vr/trackers/DeviceType.java

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

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

@@ -1,15 +1,14 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.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/dev/slimevr/vr/trackers/IMUTracker.java

@@ -0,0 +1,264 @@
+package dev.slimevr.vr.trackers;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import dev.slimevr.vr.trackers.udp.TrackersUDPServer;
+import io.eiren.util.BufferedTimer;
+
+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();
+	public 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;
+	protected float batteryLevel = 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 int signalStrength = -1;
+	public float temperature = 0;
+	
+	public TrackerPosition bodyPosition = null;
+	
+	public IMUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server) {
+		this.name = name;
+		this.server = server;
+		this.trackerId = trackerId;
+		this.descriptiveName = descriptiveName;
+	}
+	
+	@Override
+	public void saveConfig(TrackerConfig config) {
+		config.setDesignation(bodyPosition == null ? null : bodyPosition.designation);
+		config.mountingRotation = mounting != null ? mounting.name() : null;
+	}
+	
+	@Override
+	public void loadConfig(TrackerConfig config) {
+		// 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();
+			}
+			bodyPosition = TrackerPosition.getByDesignation(config.designation);
+		}
+	}
+	
+	public TrackerMountingRotation getMountingRotation() {
+		return mounting;
+	}
+	
+	public void setMountingRotation(TrackerMountingRotation mr) {
+		mounting = mr;
+		if(mounting != null) {
+			rotAdjust.set(mounting.quaternion);
+		} else {
+			rotAdjust.loadIdentity();
+		}
+	}
+	
+	@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;
+	}
+	
+	@Override
+	public boolean getPosition(Vector3f store) {
+		store.set(0, 0, 0);
+		return false;
+	}
+	
+	@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() {
+		return status;
+	}
+	
+	public void setStatus(TrackerStatus status) {
+		this.status = status;
+	}
+	
+	@Override
+	public float getTPS() {
+		return timer.getAverageFPS();
+	}
+	
+	@Override
+	public void dataTick() {
+		timer.update();
+	}
+	
+	@Override
+	public float getConfidenceLevel() {
+		return confidence;
+	}
+	
+	public void setConfidence(float newConf) {
+		this.confidence = newConf;
+	}
+	
+	@Override
+	public float getBatteryLevel() {
+		return batteryLevel;
+	}
+	
+	@Override
+	public float getBatteryVoltage() {
+		return batteryVoltage;
+	}
+	
+	public void setBatteryLevel(float level) {
+		this.batteryLevel = level;
+	}
+
+	public void setBatteryVoltage(float voltage) {
+		this.batteryVoltage = voltage;
+	}
+
+	@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 TrackerPosition getBodyPosition() {
+		return bodyPosition;
+	}
+
+	@Override
+	public void setBodyPosition(TrackerPosition position) {
+		this.bodyPosition = position;
+	}
+
+	@Override
+	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/dev/slimevr/vr/trackers/MPUTracker.java

@@ -1,13 +1,15 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import java.nio.ByteBuffer;
 
+import dev.slimevr.vr.trackers.udp.TrackersUDPServer;
+
 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/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java

@@ -1,20 +1,14 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
-import com.jme3.math.FastMath;
 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;
-	private final Quaternion smoothedQuaternion = new Quaternion();
-	public final Quaternion adjustmentYaw = new Quaternion();
-	public final Quaternion adjustmentAttachment = new Quaternion();
-	protected float[] lastAngles = new float[3];
-	public float smooth = 0 * FastMath.DEG_TO_RAD;
-	private final float[] angles = new float[3];
+	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) {
@@ -49,19 +43,15 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetFull(Quaternion reference) {
-		resetYaw(reference);
+		tracker.resetFull(reference);
+		fixGyroscope();
 		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
-		adjustmentYaw.mult(sensorRotation, sensorRotation);
-
-		// Use only yaw HMD rotation
-		Quaternion targetTrackerRotation = new Quaternion(reference);
-		float[] angles = new float[3];
-		targetTrackerRotation.toAngles(angles);
-		targetTrackerRotation.fromAngles(0, angles[1], 0);
+		gyroFix.mult(sensorRotation, sensorRotation);
+		attachmentFix.set(sensorRotation).inverseLocal();
 		
-		adjustmentAttachment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+		fixYaw(reference);
 	}
 
 	/**
@@ -73,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];
@@ -81,33 +76,36 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
+		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();
-		lastAngles[0] = 1000;
+		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);
-		adjustmentYaw.mult(store, store);
+		gyroFix.mult(store, store);
+		store.multLocal(attachmentFix);
+		yawFix.mult(store, store);
 	}
 	
 	@Override
 	public boolean getRotation(Quaternion store) {
 		tracker.getRotation(store);
-		if(smooth > 0) {
-			store.toAngles(angles);
-			if(Math.abs(angles[0] - lastAngles[0]) > smooth || Math.abs(angles[1] - lastAngles[1]) > smooth || Math.abs(angles[2] - lastAngles[2]) > smooth) {
-				smoothedQuaternion.set(store);
-				store.toAngles(lastAngles);
-			} else {
-				store.set(smoothedQuaternion);
-			}
-		}
 		adjustInternal(store);
 		return true;
 	}
@@ -133,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/dev/slimevr/vr/trackers/SensorTap.java

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

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

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

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

@@ -0,0 +1,53 @@
+package dev.slimevr.vr.trackers;
+
+import java.util.concurrent.atomic.AtomicInteger;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+public interface Tracker {
+	
+	public static final AtomicInteger nextLocalTrackerId = new AtomicInteger();
+	
+	public boolean getPosition(Vector3f store);
+	
+	public boolean getRotation(Quaternion store);
+	
+	public String getName();
+	
+	public TrackerStatus getStatus();
+	
+	public void loadConfig(TrackerConfig config);
+	
+	public void saveConfig(TrackerConfig config);
+	
+	public float getConfidenceLevel();
+	
+	public void resetFull(Quaternion reference);
+	
+	public void resetYaw(Quaternion reference);
+	
+	public void tick();
+	
+	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/dev/slimevr/vr/trackers/TrackerConfig.java

@@ -1,4 +1,4 @@
-package io.eiren.vr.trackers;
+package dev.slimevr.vr.trackers;
 
 import com.jme3.math.Quaternion;
 
@@ -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/dev/slimevr/vr/trackers/TrackerMountingRotation.java

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

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

@@ -0,0 +1,55 @@
+package dev.slimevr.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/dev/slimevr/vr/trackers/TrackerRole.java

@@ -0,0 +1,55 @@
+package dev.slimevr.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/dev/slimevr/vr/trackers/TrackerStatus.java

@@ -0,0 +1,32 @@
+package dev.slimevr.vr.trackers;
+
+public enum TrackerStatus {
+	
+	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(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/dev/slimevr/vr/trackers/TrackerUtils.java

@@ -0,0 +1,71 @@
+package dev.slimevr.vr.trackers;
+
+import java.util.List;
+
+public class TrackerUtils {
+	
+	private TrackerUtils() {
+	}
+
+	public static <T extends Tracker> T findTrackerForBodyPosition(T[] allTrackers, TrackerPosition position) {
+		if(position == null)
+			return null;
+		for(int i = 0; i < allTrackers.length; ++i) {
+			T t = allTrackers[i];
+			if(t != null && t.getBodyPosition() == position)
+				return t;
+		}
+		return null;
+	}
+
+	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/dev/slimevr/vr/trackers/TrackerWithBattery.java

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

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

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

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

@@ -0,0 +1,36 @@
+package dev.slimevr.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/main/java/dev/slimevr/vr/trackers/udp/SensorSpecificPacket.java

@@ -0,0 +1,15 @@
+package dev.slimevr.vr.trackers.udp;
+
+public interface SensorSpecificPacket {
+	
+	public int getSensorId();
+	
+	/**
+	 * Sensor with id 255 is "global" representing a whole device
+	 * @param sensorId
+	 * @return
+	 */
+	public static boolean isGlobal(int sensorId) {
+		return sensorId == 255;
+	}
+}

src/main/java/dev/slimevr/vr/trackers/udp/TrackerUDPConnection.java

@@ -0,0 +1,43 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.net.InetAddress;
+import java.net.SocketAddress;
+import java.util.HashMap;
+import java.util.Map;
+
+import dev.slimevr.NetworkProtocol;
+import dev.slimevr.vr.trackers.IMUTracker;
+
+public class TrackerUDPConnection {
+	
+	public Map<Integer, IMUTracker> sensors = new HashMap<>();
+	public SocketAddress address;
+	public InetAddress ipAddress;
+	public long lastPacket = System.currentTimeMillis();
+	public int lastPingPacketId = -1;
+	public long lastPingPacketTime = 0;
+	public String name;
+	public String descriptiveName;
+	public StringBuilder serialBuffer = new StringBuilder();
+	public long lastSerialUpdate = 0;
+	public long lastPacketNumber = -1;
+	public NetworkProtocol protocol = null;
+	public int firmwareBuild = 0;
+	
+	public TrackerUDPConnection(SocketAddress address, InetAddress ipAddress) {
+		this.address = address;
+		this.ipAddress = ipAddress;
+	}
+	
+	public boolean isNextPacket(long packetId) {
+		if(packetId != 0 && packetId <= lastPacketNumber)
+			return false;
+		lastPacketNumber = packetId;
+		return true;
+	}
+	
+	@Override
+	public String toString() {
+		return "udp:/" + ipAddress;
+	}
+}

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

@@ -0,0 +1,429 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.Inet6Address;
+import java.net.InetAddress;
+import java.net.InetSocketAddress;
+import java.net.NetworkInterface;
+import java.net.SocketAddress;
+import java.net.SocketTimeoutException;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.Enumeration;
+import java.util.HashMap;
+import java.util.Iterator;
+import java.util.List;
+import java.util.Map;
+import java.util.Random;
+import java.util.function.Consumer;
+
+import org.apache.commons.lang3.ArrayUtils;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import dev.slimevr.NetworkProtocol;
+import dev.slimevr.vr.trackers.IMUTracker;
+import dev.slimevr.vr.trackers.ReferenceAdjustedTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerStatus;
+import io.eiren.util.Util;
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+
+/**
+ * Recieves trackers data by UDP using extended owoTrack protocol.
+ */
+public class TrackersUDPServer extends Thread {
+	
+	/**
+	 * Change between IMU axises and OpenGL/SteamVR axises
+	 */
+	private static final Quaternion offset = new Quaternion().fromAngleAxis(-FastMath.HALF_PI, Vector3f.UNIT_X);
+	
+	private final Quaternion buf = new Quaternion();
+	private final Random random = new Random();
+	private final List<TrackerUDPConnection> connections = new FastList<>();
+	private final Map<InetAddress, TrackerUDPConnection> connectionsByAddress = new HashMap<>();
+	private final Map<String, TrackerUDPConnection> connectionsByMAC = new HashMap<>();
+	private final Consumer<Tracker> trackersConsumer;
+	private final int port;
+	private final ArrayList<SocketAddress> broadcastAddresses = new ArrayList<>();
+	private final UDPProtocolParser parser = new UDPProtocolParser();
+	private final byte[] rcvBuffer = new byte[512];
+	private final ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN);
+	
+	protected DatagramSocket socket = null;
+	protected long lastKeepup = System.currentTimeMillis();
+	
+	public TrackersUDPServer(int port, String name, Consumer<Tracker> trackersConsumer) {
+		super(name);
+		this.port = port;
+		this.trackersConsumer = trackersConsumer;
+		try {
+		Enumeration<NetworkInterface> ifaces = NetworkInterface.getNetworkInterfaces();
+		while(ifaces.hasMoreElements()) {
+			NetworkInterface iface = ifaces.nextElement();
+			// Ignore loopback, PPP, virtual and disabled devices
+			if(iface.isLoopback() || !iface.isUp() || iface.isPointToPoint() || iface.isVirtual()) {
+				continue;
+			}
+			Enumeration<InetAddress> iaddrs = iface.getInetAddresses();
+			while(iaddrs.hasMoreElements()) {
+				InetAddress iaddr = iaddrs.nextElement();
+				// Ignore IPv6 addresses
+				if(iaddr instanceof Inet6Address) {
+					continue;
+				}
+				String[] iaddrParts = iaddr.getHostAddress().split("\\.");
+				broadcastAddresses.add(new InetSocketAddress(String.format("%s.%s.%s.255", iaddrParts[0], iaddrParts[1], iaddrParts[2]), port));
+			}
+		}
+		} catch(Exception e) {
+			LogManager.log.severe("[TrackerServer] Can't enumerate network interfaces", e);
+		}
+	}
+	
+	private void setUpNewConnection(DatagramPacket handshakePacket, UDPPacket3Handshake handshake) throws IOException {
+		LogManager.log.info("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort());
+		InetAddress addr = handshakePacket.getAddress();
+		TrackerUDPConnection connection;
+		synchronized(connections) {
+			connection = connectionsByAddress.get(addr);
+		}
+		if(connection == null) {
+			connection = new TrackerUDPConnection(handshakePacket.getSocketAddress(), addr);
+			connection.firmwareBuild = handshake.firmwareBuild;
+			if(handshake.firmware == null || handshake.firmware.length() == 0) {
+				// Only old owoTrack doesn't report firmware and have differenet packet IDs with SlimeVR
+				connection.protocol = NetworkProtocol.OWO_LEGACY;
+			} else {
+				connection.protocol = NetworkProtocol.SLIMEVR_RAW;
+			}
+			connection.name = handshake.macString != null ? "udp://" + handshake.macString : "udp:/" + handshakePacket.getAddress().toString();
+			connection.descriptiveName = "udp:/" + handshakePacket.getAddress().toString();
+			int i = 0;
+			synchronized(connections) {
+				if(handshake.macString != null && connectionsByMAC.containsKey(handshake.macString)) {
+					TrackerUDPConnection previousConnection = connectionsByMAC.get(handshake.macString);
+					i = connections.indexOf(previousConnection);
+					connectionsByAddress.remove(previousConnection.ipAddress);
+					previousConnection.lastPacketNumber = 0;
+					previousConnection.ipAddress = addr;
+					previousConnection.address = handshakePacket.getSocketAddress();
+					previousConnection.name = connection.name;
+					previousConnection.descriptiveName = connection.descriptiveName;
+					connectionsByAddress.put(addr, previousConnection);
+					LogManager.log.info("[TrackerServer] Tracker " + i + " handed over to address " + handshakePacket.getSocketAddress() + ". Board type: " + handshake.boardType + ", imu type: " + handshake.imuType + ", firmware: " + handshake.firmware + " (" + connection.firmwareBuild + "), mac: " + handshake.macString + ", name: " + previousConnection.name);
+				} else {
+					i = connections.size();
+					connections.add(connection);
+					connectionsByAddress.put(addr, connection);
+					if(handshake.macString != null) {
+						connectionsByMAC.put(handshake.macString, connection);
+					}
+					LogManager.log.info("[TrackerServer] Tracker " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + handshake.boardType + ", imu type: " + handshake.imuType + ", firmware: " + handshake.firmware + " (" + connection.firmwareBuild + "), mac: " + handshake.macString + ", name: " + connection.name);
+				}
+			}
+			if(connection.protocol == NetworkProtocol.OWO_LEGACY || connection.firmwareBuild < 8) {
+				// Set up new sensor for older firmware
+				// Firmware after 7 should send sensor status packet and sensor will be created when it's received
+				setUpSensor(connection, 0, handshake.imuType, 1);
+			}
+		}
+		bb.limit(bb.capacity());
+		bb.rewind();
+		parser.writeHandshakeResponse(bb, connection);
+		socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
+	}
+	
+	private void setUpSensor(TrackerUDPConnection connection, int trackerId, int sensorType, int sensorStatus) throws IOException {
+		LogManager.log.info("[TrackerServer] Sensor " + trackerId + " for " + connection.name + " status: " + sensorStatus);
+		IMUTracker imu = connection.sensors.get(trackerId);
+		if(imu == null) {
+			imu = new IMUTracker(Tracker.getNextLocalTrackerId(), connection.name + "/" + trackerId, connection.descriptiveName + "/" + trackerId, this);
+			connection.sensors.put(trackerId, imu);
+			ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
+			trackersConsumer.accept(adjustedTracker);
+			LogManager.log.info("[TrackerServer] Added sensor " + trackerId + " for " + connection.name + ", type " + sensorType);
+		}
+		TrackerStatus status = UDPPacket15SensorInfo.getStatus(sensorStatus);
+		if(status != null)
+			imu.setStatus(status);
+	}
+	
+	@Override
+	public void run() {
+		StringBuilder serialBuffer2 = new StringBuilder();
+		try {
+			socket = new DatagramSocket(port);
+			
+			long prevPacketTime = System.currentTimeMillis();
+			socket.setSoTimeout(250);
+			while(true) {
+				DatagramPacket received = null;
+				try {
+					boolean hasActiveTrackers = false;
+					for(TrackerUDPConnection tracker : connections) {
+						if(tracker.sensors.size() > 0) {
+							hasActiveTrackers = true;
+							break;
+						}
+					}
+					if(!hasActiveTrackers) {
+						long discoveryPacketTime = System.currentTimeMillis();
+						if((discoveryPacketTime - prevPacketTime) >= 2000) {
+							for(SocketAddress addr : broadcastAddresses) {
+								bb.limit(bb.capacity());
+								bb.rewind();
+								parser.write(bb, null, new UDPPacket0Heartbeat());
+								socket.send(new DatagramPacket(rcvBuffer, bb.position(), addr));
+							}
+							prevPacketTime = discoveryPacketTime;
+						}
+					}
+					
+					received = new DatagramPacket(rcvBuffer, rcvBuffer.length);
+					socket.receive(received);
+					bb.limit(received.getLength());
+					bb.rewind();
+					
+					TrackerUDPConnection connection;
+					
+					synchronized(connections) {
+						connection = connectionsByAddress.get(received.getAddress());
+					}
+					UDPPacket packet = parser.parse(bb, connection);
+					if(packet != null) {
+						processPacket(received, packet, connection);
+					}
+				} catch(SocketTimeoutException e) {
+				} catch(Exception e) {
+					LogManager.log.warning("Error parsing packet " + packetToString(received), e);
+				}
+				if(lastKeepup + 500 < System.currentTimeMillis()) {
+					lastKeepup = System.currentTimeMillis();
+					synchronized(connections) {
+						for(int i = 0; i < connections.size(); ++i) {
+							TrackerUDPConnection conn = connections.get(i);
+							bb.limit(bb.capacity());
+							bb.rewind();
+							parser.write(bb, conn, new UDPPacket1Heartbeat());
+							socket.send(new DatagramPacket(rcvBuffer, bb.position(), conn.address));
+							if(conn.lastPacket + 1000 < System.currentTimeMillis()) {
+								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);
+								}
+							}
+							if(conn.serialBuffer.length() > 0) {
+								if(conn.lastSerialUpdate + 500L < System.currentTimeMillis()) {
+									serialBuffer2.append('[').append(conn.name).append("] ").append(conn.serialBuffer);
+									System.out.println(serialBuffer2.toString());
+									serialBuffer2.setLength(0);
+									conn.serialBuffer.setLength(0);
+								}
+							}
+							if(conn.lastPingPacketTime + 500 < System.currentTimeMillis()) {
+								conn.lastPingPacketId = random.nextInt();
+								conn.lastPingPacketTime = System.currentTimeMillis();
+								bb.limit(bb.capacity());
+								bb.rewind();
+								bb.putInt(10);
+								bb.putLong(0);
+								bb.putInt(conn.lastPingPacketId);
+								socket.send(new DatagramPacket(rcvBuffer, bb.position(), conn.address));
+							}
+						}
+					}
+				}
+			}
+		} catch(Exception e) {
+			e.printStackTrace();
+		} finally {
+			Util.close(socket);
+		}
+	}
+	
+	protected void processPacket(DatagramPacket received, UDPPacket packet, TrackerUDPConnection connection) throws IOException {
+		IMUTracker tracker = null;
+		switch(packet.getPacketId()) {
+		case UDPProtocolParser.PACKET_HEARTBEAT:
+			break;
+		case UDPProtocolParser.PACKET_HANDSHAKE:
+			setUpNewConnection(received, (UDPPacket3Handshake) packet);
+			break;
+		case UDPProtocolParser.PACKET_ROTATION:
+		case UDPProtocolParser.PACKET_ROTATION_2:
+			if(connection == null)
+				break;
+			UDPPacket1Rotation rotationPacket = (UDPPacket1Rotation) packet;
+			buf.set(rotationPacket.rotation);
+			offset.mult(buf, buf);
+			tracker = connection.sensors.get(rotationPacket.getSensorId());
+			if(tracker == null)
+				break;
+			tracker.rotQuaternion.set(buf);
+			tracker.dataTick();
+			break;
+		case UDPProtocolParser.PACKET_ROTATION_DATA:
+			if(connection == null)
+				break;
+			UDPPacket17RotationData rotationData = (UDPPacket17RotationData) packet;
+			tracker = connection.sensors.get(rotationData.getSensorId());
+			if(tracker == null)
+				break;
+			buf.set(rotationData.rotation);
+			offset.mult(buf, buf);
+			
+			switch(rotationData.dataType) {
+			case UDPPacket17RotationData.DATA_TYPE_NORMAL:
+				tracker.rotQuaternion.set(buf);
+				tracker.calibrationStatus = rotationData.calibrationInfo;
+				tracker.dataTick();
+				break;
+			case UDPPacket17RotationData.DATA_TYPE_CORRECTION:
+				tracker.rotMagQuaternion.set(buf);
+				tracker.magCalibrationStatus = rotationData.calibrationInfo;
+				tracker.hasNewCorrectionData = true;
+				break;
+			}
+			break;
+		case UDPProtocolParser.PACKET_MAGNETOMETER_ACCURACY:
+			if(connection == null)
+				break;
+			UDPPacket18MagnetometerAccuracy magAccuracy = (UDPPacket18MagnetometerAccuracy) packet;
+			tracker = connection.sensors.get(magAccuracy.getSensorId());
+			if(tracker == null)
+				break;
+			tracker.magnetometerAccuracy = magAccuracy.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(connection == null)
+				break;
+			break;
+		case UDPProtocolParser.PACKET_PING_PONG: // PACKET_PING_PONG:
+			if(connection == null)
+				break;
+			UDPPacket10PingPong ping = (UDPPacket10PingPong) packet;
+			if(connection.lastPingPacketId == ping.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();
+				}
+			} else {
+				LogManager.log.debug("[TrackerServer] Wrog ping id " + ping.pingId + " != " + connection.lastPingPacketId);
+			}
+			break;
+		case UDPProtocolParser.PACKET_SERIAL:
+			if(connection == null)
+				break;
+			UDPPacket11Serial serial = (UDPPacket11Serial) packet;
+			System.out.println("[" + connection.name + "] " + serial.serial);
+			break;
+		case UDPProtocolParser.PACKET_BATTERY_LEVEL:
+			if(connection == null)
+				break;
+			UDPPacket12BatteryLevel battery = (UDPPacket12BatteryLevel) packet;
+			if(connection.sensors.size() > 0) {
+				Collection<IMUTracker> trackers = connection.sensors.values();
+				Iterator<IMUTracker> iterator = trackers.iterator();
+				while(iterator.hasNext()) {
+					IMUTracker tr = iterator.next();
+					tr.setBatteryVoltage(battery.voltage);
+					tr.setBatteryLevel(battery.level * 100);
+				}
+			}
+			break;
+		case UDPProtocolParser.PACKET_TAP:
+			if(connection == null)
+				break;
+			UDPPacket13Tap tap = (UDPPacket13Tap) packet;
+			tracker = connection.sensors.get(tap.getSensorId());
+			if(tracker == null)
+				break;
+			LogManager.log.info("[TrackerServer] Tap packet received from " + tracker.getName() + ": " + tap.tap);
+			break;
+		case UDPProtocolParser.PACKET_ERROR:
+			UDPPacket14Error error = (UDPPacket14Error) packet;
+			LogManager.log.severe("[TrackerServer] Error recieved from " + received.getSocketAddress() + ": " + error.errorNumber);
+			if(connection == null)
+				break;
+			tracker = connection.sensors.get(error.getSensorId());
+			if(tracker == null)
+				break;
+			tracker.setStatus(TrackerStatus.ERROR);
+			break;
+		case UDPProtocolParser.PACKET_SENSOR_INFO:
+			if(connection == null)
+				break;
+			UDPPacket15SensorInfo info = (UDPPacket15SensorInfo) packet;
+			setUpSensor(connection, info.getSensorId(), info.sensorType, info.sensorStatus);
+			// Send ack
+			bb.limit(bb.capacity());
+			bb.rewind();
+			parser.writeSensorInfoResponse(bb, connection, info);
+			socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
+			LogManager.log.info("[TrackerServer] Sensor info for " + connection.descriptiveName + "/" + info.getSensorId() + ": " + info.sensorStatus);
+			break;
+		case UDPProtocolParser.PACKET_SIGNAL_STRENGTH:
+			if(connection == null)
+				break;
+			UDPPacket19SignalStrength signalStrength = (UDPPacket19SignalStrength) packet;
+			if(connection.sensors.size() > 0) {
+				Collection<IMUTracker> trackers = connection.sensors.values();
+				Iterator<IMUTracker> iterator = trackers.iterator();
+				while(iterator.hasNext()) {
+					IMUTracker tr = iterator.next();
+					tr.signalStrength = signalStrength.signalStrength;
+				}
+			}
+			break;
+		case UDPProtocolParser.PACKET_TEMPERATURE:
+			if(connection == null)
+				break;
+			UDPPacket20Temperature temp = (UDPPacket20Temperature) packet;
+			tracker = connection.sensors.get(temp.getSensorId());
+			if(tracker == null)
+				break;
+			tracker.temperature = temp.temperature;
+			break;
+		default:
+			LogManager.log.warning("[TrackerServer] Skipped packet " + packet);
+			break;
+		}
+	}
+	
+	private static String packetToString(DatagramPacket packet) {
+		StringBuilder sb = new StringBuilder();
+		sb.append("DatagramPacket{");
+		sb.append(packet.getAddress().toString());
+		sb.append(packet.getPort());
+		sb.append(',');
+		sb.append(packet.getLength());
+		sb.append(',');
+		sb.append(ArrayUtils.toString(packet.getData()));
+		sb.append('}');
+		return sb.toString();
+	}
+}

src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket.java

@@ -0,0 +1,70 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+public abstract class UDPPacket {
+	
+	public abstract int getPacketId();
+	
+	public abstract void readData(ByteBuffer buf) throws IOException;
+	
+	public abstract void writeData(ByteBuffer buf) throws IOException;
+	
+	@Override
+	public String toString() {
+		StringBuilder sb = new StringBuilder();
+		sb.append(getClass().getSimpleName());
+		sb.append('{');
+		sb.append(getPacketId());
+		if(this instanceof SensorSpecificPacket) {
+			sb.append(",sensor:");
+			sb.append(((SensorSpecificPacket) this).getSensorId());
+		}
+		sb.append('}');
+		return sb.toString();
+	}
+	
+	/**
+	 * Naively read null-terminated ASCII string from the byte buffer
+	 * @param buf
+	 * @return
+	 * @throws IOException
+	 */
+	public static String readASCIIString(ByteBuffer buf) throws IOException {
+		StringBuilder sb = new StringBuilder();
+		while(true) {
+			char c = (char) (buf.get() & 0xFF);
+			if(c == 0)
+				break;
+			sb.append(c);
+		}
+		return sb.toString();
+	}
+	
+	public static String readASCIIString(ByteBuffer buf, int length) throws IOException {
+		StringBuilder sb = new StringBuilder();
+		while(length-- > 0) {
+			char c = (char) (buf.get() & 0xFF);
+			if(c == 0)
+				break;
+			sb.append(c);
+		}
+		return sb.toString();
+	}
+	
+	/**
+	 * Naively write null-terminated ASCII string to byte buffer
+	 * @param str
+	 * @param buf
+	 * @throws IOException
+	 */
+	public static void writeASCIIString(String str, ByteBuffer buf) throws IOException {
+		for(int i = 0; i < str.length(); ++i) {
+			char c = str.charAt(i);
+			buf.put((byte) (c & 0xFF));
+		}
+		buf.put((byte) 0);
+	}
+	
+}

src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket0Heartbeat.java

@@ -0,0 +1,25 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+public class UDPPacket0Heartbeat extends UDPPacket {
+	
+	public UDPPacket0Heartbeat() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 0;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		// Empty packet
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		// Empty packet
+	}
+}

src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket10PingPong.java

@@ -0,0 +1,32 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+public class UDPPacket10PingPong extends UDPPacket {
+	
+	public int pingId;
+	
+	public UDPPacket10PingPong() {
+	}
+	
+	public UDPPacket10PingPong(int pingId) {
+		this.pingId = pingId;
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 10;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		pingId = buf.getInt();
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		buf.putInt(pingId);
+	}
+	
+}

src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket11Serial.java

@@ -0,0 +1,34 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+public class UDPPacket11Serial extends UDPPacket {
+	
+	public String serial;
+	
+	public UDPPacket11Serial() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 11;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		int length = buf.getInt();
+		StringBuilder sb = new StringBuilder(length);
+		for(int i = 0; i < length; ++i) {
+			char ch = (char) buf.get();
+			sb.append(ch);
+		}
+		serial = sb.toString();
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		// Never sent back in current protocol
+	}
+	
+}

src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket12BatteryLevel.java

@@ -0,0 +1,30 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+public class UDPPacket12BatteryLevel extends UDPPacket {
+	
+	public float voltage;
+	public float level;
+	
+	public UDPPacket12BatteryLevel() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 12;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		voltage = buf.getFloat();
+		if(buf.remaining() > 3)
+			level = buf.getFloat();
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		// Never sent back in current protocol
+	}
+}

src/main/java/dev/slimevr/vr/trackers/udp/UDPPacket13Tap.java

@@ -0,0 +1,36 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+import dev.slimevr.vr.trackers.SensorTap;
+
+public class UDPPacket13Tap extends UDPPacket implements SensorSpecificPacket {
+	
+	public int sensorId;
+	public SensorTap tap;
+	
+	public UDPPacket13Tap() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 13;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		sensorId = buf.get() & 0xFF;
+		tap = new SensorTap(buf.get() & 0xFF);
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		// Never sent back in current protocol
+	}
+
+	@Override
+	public int getSensorId() {
+		return sensorId;
+	}
+}