Update version to 0.1.5

Add simple BVH recording button

README.md

@@ -15,7 +15,7 @@ Integrations:
 
 It's recommended to download installer from here: https://github.com/SlimeVR/SlimeVR-Installer/releases/latest/download/slimevr_web_installer.exe
 
-Latest instructions are [on our site](https://docs.slimevr.dev/slimevr-setup.html).
+Latest instructions are [on our site](https://docs.slimevr.dev/server-setup/slimevr-setup.html).
 
 ## How to build
 
@@ -45,3 +45,7 @@ run gradle command `shadowJar` to build a runnable server JAR
 * 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

@@ -8,7 +8,7 @@
 
 plugins {
 	id 'application'
-	id "com.github.johnrengelman.shadow" version "6.1.0"
+	id "com.github.johnrengelman.shadow" version "7.1.2"
 }
 
 sourceCompatibility = 1.8
@@ -22,9 +22,7 @@ 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'])
+		options.release = 8
 	}
 }
 tasks.withType(Test) {
@@ -38,30 +36,25 @@ allprojects {
 	repositories {
 		// Use jcenter for resolving dependencies.
 		// You can declare any Maven/Ivy/file repository here.
-		jcenter()
 		mavenCentral()
 	}
 }
 
 dependencies {
-	compile project(':slime-java-commons')
-
-	// 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'
+	implementation 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'
+	implementation 'org.apache.commons:commons-math3:3.6.1'
+	implementation 'net.java.dev.jna:jna:5.10.0'
+	implementation 'net.java.dev.jna:jna-platform:5.10.0'
+	implementation 'com.illposed.osc:javaosc-core:0.8'
+	implementation 'com.fazecast:jSerialComm:2.9.0'
+	implementation 'com.google.protobuf:protobuf-java:3.19.4'
+	implementation "org.java-websocket:Java-WebSocket:1.5.2"
+	implementation 'com.melloware:jintellitype:1.4.0'
 
 	// Use JUnit test framework
-	testImplementation platform('org.junit:junit-bom:5.7.2')
+	testImplementation platform('org.junit:junit-bom:5.8.2')
 	testImplementation 'org.junit.jupiter:junit-jupiter'
 	testImplementation 'org.junit.platform:junit-platform-launcher'
 }
@@ -75,5 +68,5 @@ shadowJar {
 	archiveVersion.set('')
 }
 application {
-	mainClassName = 'io.eiren.vr.Main'
+	getMainClass().set('dev.slimevr.Main')
 }

gradle/wrapper/gradle-wrapper.properties

@@ -1,5 +1,5 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.3-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists

gradlew

@@ -1,7 +1,7 @@
-#!/usr/bin/env sh
+#!/bin/sh
 
 #
-# Copyright 2015 the original author or authors.
+# Copyright © 2015-2021 the original authors.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -17,78 +17,113 @@
 #
 
 ##############################################################################
-##
-##  Gradle start up script for UN*X
-##
+#
+#   Gradle start up script for POSIX generated by Gradle.
+#
+#   Important for running:
+#
+#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+#       noncompliant, but you have some other compliant shell such as ksh or
+#       bash, then to run this script, type that shell name before the whole
+#       command line, like:
+#
+#           ksh Gradle
+#
+#       Busybox and similar reduced shells will NOT work, because this script
+#       requires all of these POSIX shell features:
+#         * functions;
+#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+#         * compound commands having a testable exit status, especially «case»;
+#         * various built-in commands including «command», «set», and «ulimit».
+#
+#   Important for patching:
+#
+#   (2) This script targets any POSIX shell, so it avoids extensions provided
+#       by Bash, Ksh, etc; in particular arrays are avoided.
+#
+#       The "traditional" practice of packing multiple parameters into a
+#       space-separated string is a well documented source of bugs and security
+#       problems, so this is (mostly) avoided, by progressively accumulating
+#       options in "$@", and eventually passing that to Java.
+#
+#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+#       see the in-line comments for details.
+#
+#       There are tweaks for specific operating systems such as AIX, CygWin,
+#       Darwin, MinGW, and NonStop.
+#
+#   (3) This script is generated from the Groovy template
+#       https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
 ##############################################################################
 
 # Attempt to set APP_HOME
+
 # Resolve links: $0 may be a link
-PRG="$0"
-# Need this for relative symlinks.
-while [ -h "$PRG" ] ; do
-    ls=`ls -ld "$PRG"`
-    link=`expr "$ls" : '.*-> \(.*\)$'`
-    if expr "$link" : '/.*' > /dev/null; then
-        PRG="$link"
-    else
-        PRG=`dirname "$PRG"`"/$link"
-    fi
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
+    [ -h "$app_path" ]
+do
+    ls=$( ls -ld "$app_path" )
+    link=${ls#*' -> '}
+    case $link in             #(
+      /*)   app_path=$link ;; #(
+      *)    app_path=$APP_HOME$link ;;
+    esac
 done
-SAVED="`pwd`"
-cd "`dirname \"$PRG\"`/" >/dev/null
-APP_HOME="`pwd -P`"
-cd "$SAVED" >/dev/null
+
+APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
 
 APP_NAME="Gradle"
-APP_BASE_NAME=`basename "$0"`
+APP_BASE_NAME=${0##*/}
 
 # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
 DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
 
 # Use the maximum available, or set MAX_FD != -1 to use that value.
-MAX_FD="maximum"
+MAX_FD=maximum
 
 warn () {
     echo "$*"
-}
+} >&2
 
 die () {
     echo
     echo "$*"
     echo
     exit 1
-}
+} >&2
 
 # OS specific support (must be 'true' or 'false').
 cygwin=false
 msys=false
 darwin=false
 nonstop=false
-case "`uname`" in
-  CYGWIN* )
-    cygwin=true
-    ;;
-  Darwin* )
-    darwin=true
-    ;;
-  MINGW* )
-    msys=true
-    ;;
-  NONSTOP* )
-    nonstop=true
-    ;;
+case "$( uname )" in                #(
+  CYGWIN* )         cygwin=true  ;; #(
+  Darwin* )         darwin=true  ;; #(
+  MSYS* | MINGW* )  msys=true    ;; #(
+  NONSTOP* )        nonstop=true ;;
 esac
 
 CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
 
+
 # Determine the Java command to use to start the JVM.
 if [ -n "$JAVA_HOME" ] ; then
     if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
         # IBM's JDK on AIX uses strange locations for the executables
-        JAVACMD="$JAVA_HOME/jre/sh/java"
+        JAVACMD=$JAVA_HOME/jre/sh/java
     else
-        JAVACMD="$JAVA_HOME/bin/java"
+        JAVACMD=$JAVA_HOME/bin/java
     fi
     if [ ! -x "$JAVACMD" ] ; then
         die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
@@ -97,7 +132,7 @@ Please set the JAVA_HOME variable in your environment to match the
 location of your Java installation."
     fi
 else
-    JAVACMD="java"
+    JAVACMD=java
     which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
 
 Please set the JAVA_HOME variable in your environment to match the
@@ -105,79 +140,95 @@ location of your Java installation."
 fi
 
 # Increase the maximum file descriptors if we can.
-if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then
-    MAX_FD_LIMIT=`ulimit -H -n`
-    if [ $? -eq 0 ] ; then
-        if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
-            MAX_FD="$MAX_FD_LIMIT"
-        fi
-        ulimit -n $MAX_FD
-        if [ $? -ne 0 ] ; then
-            warn "Could not set maximum file descriptor limit: $MAX_FD"
-        fi
-    else
-        warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
-    fi
-fi
-
-# For Darwin, add options to specify how the application appears in the dock
-if $darwin; then
-    GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
-fi
-
-# For Cygwin or MSYS, switch paths to Windows format before running java
-if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then
-    APP_HOME=`cygpath --path --mixed "$APP_HOME"`
-    CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
-    JAVACMD=`cygpath --unix "$JAVACMD"`
-
-    # We build the pattern for arguments to be converted via cygpath
-    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
-    SEP=""
-    for dir in $ROOTDIRSRAW ; do
-        ROOTDIRS="$ROOTDIRS$SEP$dir"
-        SEP="|"
-    done
-    OURCYGPATTERN="(^($ROOTDIRS))"
-    # Add a user-defined pattern to the cygpath arguments
-    if [ "$GRADLE_CYGPATTERN" != "" ] ; then
-        OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
-    fi
-    # Now convert the arguments - kludge to limit ourselves to /bin/sh
-    i=0
-    for arg in "$@" ; do
-        CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
-        CHECK2=`echo "$arg"|egrep -c "^-"`                                 ### Determine if an option
-
-        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition
-            eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
-        else
-            eval `echo args$i`="\"$arg\""
-        fi
-        i=`expr $i + 1`
-    done
-    case $i in
-        0) set -- ;;
-        1) set -- "$args0" ;;
-        2) set -- "$args0" "$args1" ;;
-        3) set -- "$args0" "$args1" "$args2" ;;
-        4) set -- "$args0" "$args1" "$args2" "$args3" ;;
-        5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
-        6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
-        7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
-        8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
-        9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
     esac
 fi
 
-# Escape application args
-save () {
-    for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
-    echo " "
-}
-APP_ARGS=`save "$@"`
+# Collect all arguments for the java command, stacking in reverse order:
+#   * args from the command line
+#   * the main class name
+#   * -classpath
+#   * -D...appname settings
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
 
-# Collect all arguments for the java command, following the shell quoting and substitution rules
-eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
 
 exec "$JAVACMD" "$@"

gradlew.bat

@@ -40,7 +40,7 @@ if defined JAVA_HOME goto findJavaFromJavaHome
 
 set JAVA_EXE=java.exe
 %JAVA_EXE% -version >NUL 2>&1
-if "%ERRORLEVEL%" == "0" goto init
+if "%ERRORLEVEL%" == "0" goto execute
 
 echo.
 echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
@@ -54,7 +54,7 @@ goto fail
 set JAVA_HOME=%JAVA_HOME:"=%
 set JAVA_EXE=%JAVA_HOME%/bin/java.exe
 
-if exist "%JAVA_EXE%" goto init
+if exist "%JAVA_EXE%" goto execute
 
 echo.
 echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
@@ -64,28 +64,14 @@ echo location of your Java installation.
 
 goto fail
 
-:init
-@rem Get command-line arguments, handling Windows variants
-
-if not "%OS%" == "Windows_NT" goto win9xME_args
-
-:win9xME_args
-@rem Slurp the command line arguments.
-set CMD_LINE_ARGS=
-set _SKIP=2
-
-:win9xME_args_slurp
-if "x%~1" == "x" goto execute
-
-set CMD_LINE_ARGS=%*
-
 :execute
 @rem Setup the command line
 
 set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
 
+
 @rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
 
 :end
 @rem End local scope for the variables with windows NT shell

resources/firewall_uninstall.bat

@@ -1,5 +1,5 @@
 @echo off
-echo Installing firewall rules...
+echo Uninstalling firewall rules...
 
 rem Discovery defauly port
 netsh advfirewall firewall delete rule name="SlimeVR UDP 35903 incoming"

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

@@ -15,7 +15,7 @@ import io.eiren.util.logging.LogManager;
 
 public class Main {
 	
-	public static String VERSION = "0.1.3";
+	public static String VERSION = "0.1.5";
 	
 	public static VRServer vrServer;
 	
@@ -49,7 +49,7 @@ public class Main {
 
 		try {
 			vrServer = new VRServer();
-			vrServer.start();
+			vrServer.start(); 
 			new Keybinding(vrServer);
 			new VRServerGUI(vrServer);
 		} catch(Throwable e) {

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

@@ -27,7 +27,7 @@ import dev.slimevr.vr.trackers.HMDTracker;
 import dev.slimevr.vr.trackers.ShareableTracker;
 import dev.slimevr.vr.trackers.Tracker;
 import dev.slimevr.vr.trackers.TrackerConfig;
-import dev.slimevr.vr.trackers.TrackersUDPServer;
+import dev.slimevr.vr.trackers.udp.TrackersUDPServer;
 import io.eiren.util.OperatingSystem;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.util.ann.ThreadSecure;

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

@@ -15,14 +15,13 @@ 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.SimpleSkeleton;
 import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
 import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue;
 import dev.slimevr.vr.trackers.TrackerPosition;
 import dev.slimevr.vr.trackers.TrackerRole;
 import dev.slimevr.vr.trackers.TrackerUtils;
-import io.eiren.util.ann.ThreadSafe;
 import io.eiren.util.logging.LogManager;
 import io.eiren.util.collections.FastList;
 
@@ -77,8 +76,6 @@ public class AutoBone {
 	
 	protected final VRServer server;
 	
-	protected SimpleSkeleton skeleton = null;
-	
 	// 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);
@@ -90,10 +87,7 @@ public class AutoBone {
 	
 	public AutoBone(VRServer server) {
 		this.server = server;
-		
 		reloadConfigValues();
-		
-		server.addSkeletonUpdatedCallback(this::skeletonUpdated);
 	}
 	
 	public void reloadConfigValues() {
@@ -136,19 +130,21 @@ public class AutoBone {
 		// Keep "feet" at ankles
 		staticConfigs.put(SkeletonConfigValue.FOOT_LENGTH, 0f);
 		staticConfigs.put(SkeletonConfigValue.FOOT_OFFSET, 0f);
+		staticConfigs.put(SkeletonConfigValue.SKELETON_OFFSET, 0f);
 	}
 	
-	@ThreadSafe
-	public void skeletonUpdated(HumanSkeleton newSkeleton) {
-		if(newSkeleton instanceof SimpleSkeleton) {
-			skeleton = (SimpleSkeleton) newSkeleton;
-			applyConfigToSkeleton(newSkeleton);
-			LogManager.log.info("[AutoBone] Received updated skeleton");
-		}
+	/**
+	 * 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(skeleton)) {
+		if(!applyConfigToSkeleton(getSkeleton())) {
 			// Unable to apply to skeleton, save directly
 			saveConfigs();
 		}
@@ -286,10 +282,14 @@ public class AutoBone {
 		
 		// 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);
@@ -302,12 +302,14 @@ public class AutoBone {
 			}
 		}
 		
+		// 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;
@@ -558,42 +560,48 @@ public class AutoBone {
 		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;
 		}
 		
-		// Minimize sliding, minimize foot height offset, minimize change in total height
 		return sumWeight > 0f ? totalError / sumWeight : 0f;
 	}
 	

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

@@ -43,7 +43,7 @@ public class NamedPipeVRBridge extends Thread implements Bridge {
 	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, VRServer server) {
@@ -149,7 +149,7 @@ public class NamedPipeVRBridge extends Thread implements Bridge {
 	private void executeHMDInput() throws IOException {
 		String[] split = commandBuilder.toString().split(" ");
 		if(split.length < 7) {
-			LogManager.log.severe("[VRBridge] Short HMD data recieved: " + commandBuilder.toString());
+			LogManager.log.severe("[VRBridge] Short HMD data received: " + commandBuilder.toString());
 			return;
 		}
 		try {

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

@@ -1,11 +1,10 @@
 package dev.slimevr.gui;
 
+import com.melloware.jintellitype.HotkeyListener;
 import com.melloware.jintellitype.JIntellitype;
-import com.melloware.jintellitype.JIntellitypeException;
 
 import dev.slimevr.VRServer;
-
-import com.melloware.jintellitype.HotkeyListener;
+import io.eiren.util.OperatingSystem;
 import io.eiren.util.ann.AWTThread;
 import io.eiren.util.logging.LogManager;
 
@@ -18,6 +17,11 @@ public class Keybinding implements HotkeyListener {
 	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);
@@ -38,9 +42,7 @@ public class Keybinding implements HotkeyListener {
 				JIntellitype.getInstance().registerHotKey(QUICK_RESET, quickResetBinding);
 				LogManager.log.info("[Keybinding] Bound quick reset to " + quickResetBinding);
 			}
-		} catch(JIntellitypeException je) {
-			LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer.");
-		} catch(ExceptionInInitializerError e) {
+		} catch(Throwable e) {
 			LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer.");
 		}
 	}

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

@@ -48,6 +48,7 @@ public class TrackersList extends EJBoxNoStretch {
 	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);
@@ -59,6 +60,12 @@ public class TrackersList extends EJBoxNoStretch {
 		server.addNewTrackerConsumer(this::newTrackerAdded);
 	}
 
+	@AWTThread
+	public void setDebug(boolean debug) {
+		this.debug = debug;
+		build();
+	}
+
 	@AWTThread
 	private void build() {
 		removeAll();
@@ -142,7 +149,12 @@ public class TrackersList extends EJBoxNoStretch {
 		JLabel magAccuracy;
 		JLabel adj;
 		JLabel adjYaw;
+		JLabel adjGyro;
 		JLabel correction;
+		JLabel signalStrength;
+		JLabel rotQuat;
+		JLabel rotAdj;
+		JLabel temperature;
 		
 		@AWTThread
 		public TrackerPanel(Tracker t) {
@@ -216,6 +228,7 @@ public class TrackersList extends EJBoxNoStretch {
 			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())
@@ -224,6 +237,7 @@ public class TrackersList extends EJBoxNoStretch {
 				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));
@@ -240,11 +254,18 @@ public class TrackersList extends EJBoxNoStretch {
 			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(realTracker instanceof IMUTracker) {
+			
+			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));
@@ -253,18 +274,22 @@ public class TrackersList extends EJBoxNoStretch {
 				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(t instanceof ReferenceAdjustedTracker) {	
-				add(new JLabel("Adj:"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START));
+			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("AdjY:"), c(2, 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);
@@ -296,23 +321,53 @@ public class TrackersList extends EJBoxNoStretch {
 			if(realTracker instanceof TrackerWithTPS) {
 				tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1));
 			}
-			if(realTracker instanceof TrackerWithBattery)
-				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 1));
+			if(realTracker instanceof TrackerWithBattery) {
+				TrackerWithBattery twb = (TrackerWithBattery) realTracker;
+				float level = twb.getBatteryLevel();
+				float voltage = twb.getBatteryVoltage();
+				if(level == 0.0f) {
+					bat.setText(String.format("%sV", StringUtils.prettyNumber(voltage, 2)));
+				} else if(voltage == 0.0f) {
+					bat.setText(String.format("%d%%", Math.round(level)));
+				} else {
+					bat.setText(String.format("%d%% (%sV)", Math.round(level), StringUtils.prettyNumber(voltage, 2)));
+				}
+			}
 			if(t instanceof ReferenceAdjustedTracker) {
-				((ReferenceAdjustedTracker<Tracker>) t).attachmentFix.toAngles(angles);
-				if(adj != null)
+				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));
-				((ReferenceAdjustedTracker<Tracker>) t).yawFix.toAngles(angles);
-				if(adjYaw != null)
+				}
+				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);
@@ -320,21 +375,44 @@ public class TrackersList extends EJBoxNoStretch {
 					+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
 					+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
 			if(realTracker instanceof IMUTracker) {
-				((IMUTracker) realTracker).rotMagQuaternion.toAngles(angles);
-				if(rawMag != null)
+				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(((IMUTracker) realTracker).calibrationStatus + " / " + ((IMUTracker) realTracker).magCalibrationStatus);
+					calibration.setText(imu.calibrationStatus + " / " + imu.magCalibrationStatus);
 				if(magAccuracy != null)
-					magAccuracy.setText(StringUtils.prettyNumber(((IMUTracker) realTracker).magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°");
-				((IMUTracker) realTracker).getCorrection(q);
-				q.toAngles(angles);
-				if(correction != null)
+					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");
+					}
+				}
 			}
 		}
 	}

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

@@ -13,10 +13,14 @@ 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 dev.slimevr.posestreamer.BVHFileStream;
+import dev.slimevr.posestreamer.PoseDataStream;
+import dev.slimevr.posestreamer.ServerPoseStreamer;
 import io.eiren.util.MacOSX;
 import io.eiren.util.OperatingSystem;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.AWTThread;
+import io.eiren.util.logging.LogManager;
 
 import java.awt.Component;
 import java.awt.Container;
@@ -29,6 +33,8 @@ import java.awt.event.ActionListener;
 import java.awt.event.ComponentEvent;
 import java.awt.event.MouseEvent;
 import java.awt.image.BufferedImage;
+import java.io.File;
+import java.io.FileNotFoundException;
 import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
@@ -45,6 +51,10 @@ public class VRServerGUI extends JFrame {
 	private final SkeletonList skeletonList;
 	private JButton resetButton;
 	private EJBox pane;
+
+	private static File bvhSaveDir = new File("BVH Recordings");
+	private final ServerPoseStreamer poseStreamer;
+	private PoseDataStream poseDataStream = null;
 	
 	private float zoom = 1.5f;
 	private float initZoom = zoom;
@@ -88,6 +98,8 @@ public class VRServerGUI extends JFrame {
 		this.trackersList = new TrackersList(server, this);
 		this.skeletonList = new SkeletonList(server, this);
 		
+		this.poseStreamer = new ServerPoseStreamer(server);
+
 		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();
@@ -136,6 +148,22 @@ public class VRServerGUI extends JFrame {
 		});
 	}
 	
+	private File getBvhFile() {
+		if (bvhSaveDir.isDirectory() || bvhSaveDir.mkdirs()) {
+			File saveRecording;
+			int recordingIndex = 1;
+			do {
+				saveRecording = new File(bvhSaveDir, "BVH-Recording" + recordingIndex++ + ".bvh");
+			} while(saveRecording.exists());
+			
+			return saveRecording;
+		} else {
+			LogManager.log.severe("[BVH] Failed to create the recording directory \"" + bvhSaveDir.getPath() + "\".");
+		}
+
+		return null;
+	}
+
 	@AWTThread
 	private void build() {
 		pane.removeAll();
@@ -162,6 +190,37 @@ public class VRServerGUI extends JFrame {
 				});
 			}});
 			add(Box.createHorizontalGlue());
+			add(new JButton("Record BVH") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						if (poseDataStream == null) {
+							File bvhFile = getBvhFile();
+							if (bvhFile != null) {
+								try {
+									poseDataStream = new BVHFileStream(bvhFile);
+									setText("Stop Recording BVH...");
+									poseStreamer.setOutput(poseDataStream, 1000L / 100L);
+								} catch (IOException e1) {
+									LogManager.log.severe("[BVH] Failed to create the recording file \"" + bvhFile.getPath() + "\".");
+								}
+							} else {
+								LogManager.log.severe("[BVH] Unable to get file to save to");
+							}
+						} else {
+							try {
+								poseStreamer.closeOutput(poseDataStream);
+							} catch (Exception e1) {
+								LogManager.log.severe("[BVH] Exception while closing poseDataStream", e1);
+							} finally {
+								poseDataStream = null;
+								setText("Record BVH");
+							}
+						}
+					}
+				});
+			}});
+			add(Box.createHorizontalGlue());
 			add(new JButton("GUI Zoom (x" + StringUtils.prettyNumber(zoom, 2) + ")") {{
 				addMouseListener(new MouseInputAdapter() {
 					@Override
@@ -198,6 +257,17 @@ public class VRServerGUI extends JFrame {
 
 			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));

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

@@ -154,7 +154,7 @@ public class BVHFileStream extends PoseDataStream {
 		writer.write(getBufferedFrameCount(frameCount) + "\n");
 		
 		// Frame time in seconds
-		writer.write("Frame Time: " + (streamer.frameRecordingInterval / 1000d) + "\n");
+		writer.write("Frame Time: " + (streamer.getFrameInterval() / 1000d) + "\n");
 	}
 	
 	// Roughly based off code from https://github.com/TrackLab/ViRe/blob/50a987eff4db31036b2ebaeb5a28983cd473f267/Assets/Scripts/BVH/BVHRecorder.cs
@@ -207,7 +207,7 @@ public class BVHFileStream extends PoseDataStream {
 		
 		// Adjust to local rotation
 		if(inverseRootRot != null) {
-			rotBuf = rotBuf.multLocal(inverseRootRot);
+			rotBuf = inverseRootRot.mult(rotBuf, rotBuf);
 		}
 		
 		// Yaw (Z), roll (X), pitch (Y) (intrinsic)

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

@@ -0,0 +1,38 @@
+package dev.slimevr.posestreamer;
+
+import java.io.File;
+
+import dev.slimevr.poserecorder.PoseFrameIO;
+import dev.slimevr.poserecorder.PoseFrameSkeleton;
+import dev.slimevr.poserecorder.PoseFrames;
+
+public class PoseFrameStreamer extends PoseStreamer {
+
+	private PoseFrames frames;
+
+	public PoseFrameStreamer(String path) {
+		this(new File(path));
+	}
+
+	public PoseFrameStreamer(File file) {
+		this(PoseFrameIO.readFromFile(file));
+	}
+
+	public PoseFrameStreamer(PoseFrames frames) {
+		super(new PoseFrameSkeleton(frames.getTrackers(), null));
+		this.frames = frames;
+	}
+
+	public PoseFrames getFrames() {
+		return frames;
+	}
+
+	public synchronized void streamAllFrames() {
+		PoseFrameSkeleton skeleton = (PoseFrameSkeleton) this.skeleton;
+		for (int i = 0; i < frames.getMaxFrameCount(); i++) {
+			skeleton.setCursor(i);
+			skeleton.updatePose();
+			captureFrame();
+		}
+	}
+}

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

@@ -2,72 +2,31 @@ 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;
+
+	protected HumanSkeleton skeleton;
+	protected PoseDataStream poseFileStream;
 	
-	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) {
+	public PoseStreamer(HumanSkeleton skeleton) {
 		this.skeleton = skeleton;
 	}
 	
-	@VRServerThread
-	public void onTick() {
-		PoseDataStream poseFileStream = this.poseFileStream;
-		if(poseFileStream == null) {
+	public synchronized void captureFrame() {
+		// Make sure the stream is open before trying to write
+		if(poseFileStream.isClosed()) {
 			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);
-			}
+		try {
+			poseFileStream.writeFrame(skeleton);
+		} catch(Exception e) {
+			// Handle any exceptions without crashing the program
+			LogManager.log.severe("[PoseStreamer] Exception while saving frame", e);
 		}
 	}
 	
@@ -82,13 +41,16 @@ public class PoseStreamer {
 	public synchronized long getFrameInterval() {
 		return frameRecordingInterval;
 	}
-	
+
+	public synchronized HumanSkeleton getSkeleton() {
+		return skeleton;
+	}
+
 	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);

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

@@ -0,0 +1,30 @@
+package dev.slimevr.posestreamer;
+
+
+import dev.slimevr.VRServer;
+import dev.slimevr.util.ann.VRServerThread;
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+
+public class ServerPoseStreamer extends TickPoseStreamer {
+	
+	protected final VRServer server;
+	
+	public ServerPoseStreamer(VRServer server) {
+		super(null); // Skeleton is registered later
+		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() {
+		super.doTick();
+	}
+}

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

@@ -27,23 +27,7 @@ public class StdBVHFileStream extends BVHFileStream {
 			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;
+		return TransformNodeWrapper.wrapFullHierarchy(newRoot);
 	}
 	
 	private TransformNode getNodeFromHierarchy(TransformNode node, String name) {

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

@@ -0,0 +1,46 @@
+package dev.slimevr.posestreamer;
+
+import java.io.IOException;
+
+import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
+
+public class TickPoseStreamer extends PoseStreamer {
+	
+	protected long nextFrameTimeMs = -1L;
+	
+	public TickPoseStreamer(HumanSkeleton skeleton) {
+		super(skeleton);
+	}
+	
+	public void doTick() {
+		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;
+		}
+		
+		captureFrame();
+	}
+
+	@Override
+	public synchronized void setOutput(PoseDataStream poseFileStream) throws IOException {
+		super.setOutput(poseFileStream);
+		nextFrameTimeMs = -1L; // Reset the frame timing
+	}
+}

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

@@ -58,25 +58,38 @@ public class TransformNodeWrapper {
 	public TransformNodeWrapper(TransformNode nodeToWrap) {
 		this(nodeToWrap, nodeToWrap.getName());
 	}
+
+	public static TransformNodeWrapper wrapFullHierarchyWithFakeRoot(TransformNode root) {
+		// Allocate a "fake" root with appropriate size depending on connections the root has
+		TransformNodeWrapper fakeRoot = new TransformNodeWrapper(root, root.getParent() != null ? 2 : 1);
+
+		// Attach downwards hierarchy to the fake root
+		wrapNodeHierarchyDown(root, fakeRoot);
+
+		// Attach upwards hierarchy to the fake root
+		fakeRoot.attachChild(wrapHierarchyUp(root));
+
+		return fakeRoot;
+	}
 	
 	public static TransformNodeWrapper wrapFullHierarchy(TransformNode root) {
 		return wrapNodeHierarchyUp(wrapHierarchyDown(root));
 	}
 	
 	public static TransformNodeWrapper wrapHierarchyDown(TransformNode root) {
-		return wrapNodeHierarchyDown(new TransformNodeWrapper(root, root.children.size()));
+		return wrapNodeHierarchyDown(root, new TransformNodeWrapper(root, root.children.size()));
 	}
 	
-	public static TransformNodeWrapper wrapNodeHierarchyDown(TransformNodeWrapper root) {
-		for(TransformNode child : root.wrappedNode.children) {
-			root.attachChild(wrapHierarchyDown(child));
+	public static TransformNodeWrapper wrapNodeHierarchyDown(TransformNode root, TransformNodeWrapper target) {
+		for(TransformNode child : root.children) {
+			target.attachChild(wrapHierarchyDown(child));
 		}
 		
-		return root;
+		return target;
 	}
 	
 	public static TransformNodeWrapper wrapHierarchyUp(TransformNode root) {
-		return wrapNodeHierarchyUp(new TransformNodeWrapper(root, root.getParent() != null ? 1 : 0));
+		return wrapNodeHierarchyUp(new TransformNodeWrapper(root, true, root.getParent() != null ? 1 : 0));
 	}
 	
 	public static TransformNodeWrapper wrapNodeHierarchyUp(TransformNodeWrapper root) {
@@ -132,7 +145,7 @@ public class TransformNodeWrapper {
 			result = new Quaternion();
 		}
 		
-		return worldTransform.getRotation().mult(inverseRelativeTo, result);
+		return inverseRelativeTo.mult(worldTransform.getRotation(), result);
 	}
 	
 	public void attachChild(TransformNodeWrapper node) {

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

@@ -28,6 +28,7 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 	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);
@@ -36,13 +37,17 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 	//#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;
@@ -101,7 +106,6 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 		neckNode.attachChild(chestNode);
 		chestNode.attachChild(waistNode);
 		waistNode.attachChild(hipNode);
-		hipNode.attachChild(trackerWaistNode);
 		//#endregion
 		
 		//#region Assemble skeleton to feet
@@ -117,6 +121,17 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 		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);
@@ -335,6 +350,7 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 		}
 		if(waistTracker.getRotation(rotBuf1)) {
 			chestNode.localTransform.setRotation(rotBuf1);
+			trackerChestNode.localTransform.setRotation(rotBuf1);
 		}
 		if(hipTracker.getRotation(rotBuf1)) {
 			waistNode.localTransform.setRotation(rotBuf1);
@@ -353,11 +369,15 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 		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
@@ -371,11 +391,15 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 		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) {
@@ -435,7 +459,7 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 	//#region Update the output trackers
 	protected void updateComputedTrackers() {
 		if(computedChestTracker != null) {
-			computedChestTracker.position.set(chestNode.worldTransform.getTranslation());
+			computedChestTracker.position.set(trackerChestNode.worldTransform.getTranslation());
 			computedChestTracker.rotation.set(neckNode.worldTransform.getRotation());
 			computedChestTracker.dataTick();
 		}
@@ -447,26 +471,26 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 		}
 		
 		if(computedLeftKneeTracker != null) {
-			computedLeftKneeTracker.position.set(leftKneeNode.worldTransform.getTranslation());
+			computedLeftKneeTracker.position.set(trackerLeftKneeNode.worldTransform.getTranslation());
 			computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation());
 			computedLeftKneeTracker.dataTick();
 		}
 		
 		if(computedLeftFootTracker != null) {
-			computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation());
-			computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation());
+			computedLeftFootTracker.position.set(trackerLeftFootNode.worldTransform.getTranslation());
+			computedLeftFootTracker.rotation.set(trackerLeftFootNode.worldTransform.getRotation());
 			computedLeftFootTracker.dataTick();
 		}
 		
 		if(computedRightKneeTracker != null) {
-			computedRightKneeTracker.position.set(rightKneeNode.worldTransform.getTranslation());
+			computedRightKneeTracker.position.set(trackerRightKneeNode.worldTransform.getTranslation());
 			computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation());
 			computedRightKneeTracker.dataTick();
 		}
 		
 		if(computedRightFootTracker != null) {
-			computedRightFootTracker.position.set(rightFootNode.worldTransform.getTranslation());
-			computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation());
+			computedRightFootTracker.position.set(trackerRightFootNode.worldTransform.getTranslation());
+			computedRightFootTracker.rotation.set(trackerRightFootNode.worldTransform.getRotation());
 			computedRightFootTracker.dataTick();
 		}
 	}
@@ -512,6 +536,9 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 		case CHEST:
 			chestNode.localTransform.setTranslation(offset);
 			break;
+		case CHEST_TRACKER:
+			trackerChestNode.localTransform.setTranslation(offset);
+			break;
 		case WAIST:
 			waistNode.localTransform.setTranslation(offset);
 			break;
@@ -533,6 +560,10 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 			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);
@@ -541,6 +572,10 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 			leftFootNode.localTransform.setTranslation(offset);
 			rightFootNode.localTransform.setTranslation(offset);
 			break;
+		case FOOT_TRACKER:
+			trackerLeftFootNode.localTransform.setTranslation(offset);
+			trackerRightFootNode.localTransform.setTranslation(offset);
+			break;
 		}
 	}
 	
@@ -594,6 +629,15 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 			rightAnkleNode.update();
 			updateComputedTrackers();
 			break;
+		case SKELETON_OFFSET:
+			trackerChestNode.update();
+			trackerWaistNode.update();
+			trackerLeftKneeNode.update();
+			trackerRightKneeNode.update();
+			trackerLeftFootNode.update();
+			trackerRightFootNode.update();
+			updateComputedTrackers();
+			break;
 		}
 	}
 	//#endregion
@@ -652,6 +696,9 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb
 		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);

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

@@ -216,6 +216,9 @@ public class SkeletonConfig {
 		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;
@@ -223,7 +226,7 @@ public class SkeletonConfig {
 			setNodeOffset(nodeOffset, 0, -getConfig(SkeletonConfigValue.WAIST), 0);
 			break;
 		case HIP_TRACKER:
-			setNodeOffset(nodeOffset, 0, getConfig(SkeletonConfigValue.HIP_OFFSET), 0);
+			setNodeOffset(nodeOffset, 0, getConfig(SkeletonConfigValue.HIP_OFFSET), -getConfig(SkeletonConfigValue.SKELETON_OFFSET));
 			break;
 		
 		case LEFT_HIP:
@@ -236,12 +239,18 @@ public class SkeletonConfig {
 		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;
 		}
 	}
 	

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

@@ -7,15 +7,16 @@ public enum SkeletonConfigValue {
 	
 	HEAD("Head", "headShift", "Head shift", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HEAD}),
 	NECK("Neck", "neckLength", "Neck length", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.NECK}),
-	TORSO("Torso", "torsoLength", "Torso length", 0.64f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}),
-	CHEST("Chest", "chestDistance", "Chest distance", 0.32f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}),
+	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.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}),
-	LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.86f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}),
-	KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.43f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}),
+	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.";

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

@@ -5,14 +5,17 @@ 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/IMUTracker.java

@@ -4,19 +4,19 @@ import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
-import io.eiren.math.FloatMath;
+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 gyroVector = new Vector3f();
+	//public final Vector3f accelVector = new Vector3f();
 	public final Vector3f magVector = new Vector3f();
 	public final Quaternion rotQuaternion = new Quaternion();
 	public final Quaternion rotMagQuaternion = new Quaternion();
-	protected final Quaternion rotAdjust = new Quaternion();
+	public final Quaternion rotAdjust = new Quaternion();
 	protected final Quaternion correction = new Quaternion();
 	protected TrackerMountingRotation mounting = null;
 	protected TrackerStatus status = TrackerStatus.OK;
@@ -27,6 +27,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	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;
@@ -35,9 +36,9 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	
 	protected BufferedTimer timer = new BufferedTimer(1f);
 	public int ping = -1;
+	public int signalStrength = -1;
+	public float temperature = 0;
 	
-	public StringBuilder serialBuffer = new StringBuilder();
-	long lastSerialUpdate = 0;
 	public TrackerPosition bodyPosition = null;
 	
 	public IMUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server) {
@@ -149,7 +150,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	
 	@Override
 	public float getBatteryLevel() {
-		return FloatMath.mapValue(getBatteryVoltage(), 3.6f, 4.2f, 0f, 1f);
+		return batteryLevel;
 	}
 	
 	@Override
@@ -157,6 +158,10 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 		return batteryVoltage;
 	}
 	
+	public void setBatteryLevel(float level) {
+		this.batteryLevel = level;
+	}
+
 	public void setBatteryVoltage(float voltage) {
 		this.batteryVoltage = voltage;
 	}

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

@@ -2,6 +2,8 @@ package dev.slimevr.vr.trackers;
 
 import java.nio.ByteBuffer;
 
+import dev.slimevr.vr.trackers.udp.TrackersUDPServer;
+
 public class MPUTracker extends IMUTracker {
 	
 	public ConfigurationData newCalibrationData;

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

@@ -1,10 +1,10 @@
 package dev.slimevr.vr.trackers;
 
-public class BnoTap {
+public class SensorTap {
 	
 	public final boolean doubleTap;
 	
-	public BnoTap(int tapBits) {
+	public SensorTap(int tapBits) {
 		doubleTap = (tapBits & 0x40) > 0;
 	}
 	

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

@@ -1,464 +0,0 @@
-package dev.slimevr.vr.trackers;
-
-import java.io.IOException;
-import java.io.UnsupportedEncodingException;
-import java.net.DatagramPacket;
-import java.net.DatagramSocket;
-import java.net.Inet6Address;
-import java.net.InetAddress;
-import java.net.InetSocketAddress;
-import java.net.NetworkInterface;
-import java.net.SocketAddress;
-import java.net.SocketTimeoutException;
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-import java.util.ArrayList;
-import java.util.Enumeration;
-import java.util.HashMap;
-import java.util.Iterator;
-import java.util.List;
-import java.util.Map;
-import java.util.Random;
-import java.util.function.Consumer;
-
-import com.jme3.math.FastMath;
-import com.jme3.math.Quaternion;
-import com.jme3.math.Vector3f;
-
-import io.eiren.util.Util;
-import io.eiren.util.collections.FastList;
-
-/**
- * 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 static final byte[] HANDSHAKE_BUFFER = new byte[64];
-	private static final byte[] KEEPUP_BUFFER = new byte[64];
-	private static final byte[] CALIBRATION_BUFFER = new byte[64];
-	private static final byte[] CALIBRATION_REQUEST_BUFFER = new byte[64];
-
-	private final Quaternion buf = new Quaternion();
-	private final Random random = new Random();
-	private final List<TrackerConnection> trackers = new FastList<>();
-	private final Map<InetAddress, TrackerConnection> trackersMap = new HashMap<>();
-	private final Map<Tracker, Consumer<String>> calibrationDataRequests = new HashMap<>();
-	private final Consumer<Tracker> trackersConsumer;
-	private final int port;
-	
-	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;
-	}
-	
-	private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException {
-		System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort());
-		InetAddress addr = handshakePacket.getAddress();
-		TrackerConnection sensor;
-		synchronized(trackers) {
-			sensor = trackersMap.get(addr);
-		}
-		if(sensor == null) {
-			boolean isOwo = false;
-			data.getLong(); // Skip packet number
-			int boardType = -1;
-			int imuType = -1;
-			int firmwareBuild = -1;
-			StringBuilder firmware = new StringBuilder();
-			byte[] mac = new byte[6];
-			String macString = null;
-			if(data.remaining() > 0) {
-				if(data.remaining() > 3)
-					boardType = data.getInt();
-				if(data.remaining() > 3)
-					imuType = data.getInt();
-				if(data.remaining() > 3)
-					data.getInt(); // MCU TYPE
-				if(data.remaining() > 11) {
-					data.getInt(); // IMU info
-					data.getInt();
-					data.getInt();
-				}
-				if(data.remaining() > 3)
-					firmwareBuild = data.getInt();
-				int length = 0;
-				if(data.remaining() > 0)
-					length = data.get() & 0xFF; // firmware version length is 1 longer than that because it's nul-terminated
-				while(length > 0 && data.remaining() != 0) {
-					char c = (char) data.get();
-					if(c == 0)
-						break;
-					firmware.append(c);
-					length--;
-				}
-				if(data.remaining() > mac.length) {
-					data.get(mac);
-					macString = String.format("%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
-					if(macString.equals("00:00:00:00:00:00"))
-						macString = null;
-				}
-			}
-			if(firmware.length() == 0) {
-				firmware.append("owoTrack");
-				isOwo = true;
-			}
-			String trackerName = macString != null ? "udp://" + macString : "udp:/" + handshakePacket.getAddress().toString();
-			String descriptiveName = "udp:/" + handshakePacket.getAddress().toString();
-			IMUTracker imu = new IMUTracker(Tracker.getNextLocalTrackerId(), trackerName, descriptiveName, this);
-			ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
-			trackersConsumer.accept(adjustedTracker);
-			sensor = new TrackerConnection(imu, handshakePacket.getSocketAddress());
-			sensor.isOwoTrack = isOwo;
-			int i = 0;
-			synchronized(trackers) {
-				i = trackers.size();
-				trackers.add(sensor);
-				trackersMap.put(addr, sensor);
-			}
-			System.out.println("[TrackerServer] Sensor " + i + " added with address " + handshakePacket.getSocketAddress() + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString + ", name: " + trackerName);
-		}
-		sensor.sensors.get(0).setStatus(TrackerStatus.OK);
-        socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort()));
-	}
-	
-	private void setUpAuxilarySensor(TrackerConnection connection, int trackerId) throws IOException {
-		System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.sensors.get(0).getName());
-		IMUTracker imu = connection.sensors.get(trackerId);
-		if(imu == null) {
-			imu = new IMUTracker(Tracker.getNextLocalTrackerId(), connection.sensors.get(0).getName() + "/" + trackerId, connection.sensors.get(0).getDescriptiveName() + "/" + trackerId, this);
-			connection.sensors.put(trackerId, imu);
-			ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
-			trackersConsumer.accept(adjustedTracker);
-			System.out.println("[TrackerServer] Sensor added with address " + imu.getName());
-		}
-		imu.setStatus(TrackerStatus.OK);
-	}
-	
-	
-	@Override
-	public void run() {
-		byte[] rcvBuffer = new byte[512];
-		ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN);
-		StringBuilder serialBuffer2 = new StringBuilder();
-		try {
-			socket = new DatagramSocket(port);
-
-			// Why not just 255.255.255.255? Because Windows.
-			// https://social.technet.microsoft.com/Forums/windows/en-US/72e7387a-9f2c-4bf4-a004-c89ddde1c8aa/how-to-fix-the-global-broadcast-address-255255255255-behavior-on-windows
-			ArrayList<SocketAddress> addresses = new ArrayList<SocketAddress>();
-			Enumeration<NetworkInterface> ifaces = NetworkInterface.getNetworkInterfaces();
-			while (ifaces.hasMoreElements()) {
-				NetworkInterface iface = ifaces.nextElement();
-				// Ignore loopback, PPP, virtual and disabled devices
-				if (iface.isLoopback() || !iface.isUp() || iface.isPointToPoint() || iface.isVirtual()) {
-					continue;
-				}
-				Enumeration<InetAddress> iaddrs = iface.getInetAddresses();
-				while (iaddrs.hasMoreElements()) {
-					InetAddress iaddr = iaddrs.nextElement();
-					// Ignore IPv6 addresses
-					if (iaddr instanceof Inet6Address) {
-						continue;
-					}
-					String[] iaddrParts = iaddr.getHostAddress().split("\\.");
-					addresses.add(new InetSocketAddress(String.format("%s.%s.%s.255", iaddrParts[0], iaddrParts[1], iaddrParts[2]), port));
-				}
-			}
-			byte[] dummyPacket = new byte[] {0x0};
-
-			long prevPacketTime = System.currentTimeMillis();
-			socket.setSoTimeout(250);
-			while(true) {
-				try {
-					boolean hasActiveTrackers = false;
-					for (TrackerConnection tracker: trackers) {
-						if (tracker.sensors.get(0).getStatus() == TrackerStatus.OK) {
-							hasActiveTrackers = true;
-							break;
-						}
-					}
-					if (!hasActiveTrackers) {
-						long discoveryPacketTime = System.currentTimeMillis();
-						if ((discoveryPacketTime - prevPacketTime) >= 2000) {
-							for (SocketAddress addr: addresses) {
-								socket.send(new DatagramPacket(dummyPacket, dummyPacket.length, addr));
-							}
-							prevPacketTime = discoveryPacketTime;
-						}
-					}
-
-					DatagramPacket recieve = new DatagramPacket(rcvBuffer, rcvBuffer.length);
-					socket.receive(recieve);
-					bb.rewind();
-
-					TrackerConnection connection;
-					IMUTracker tracker = null;
-					synchronized(trackers) {
-						connection = trackersMap.get(recieve.getAddress());
-					}
-					if(connection != null)
-						connection.lastPacket = System.currentTimeMillis();
-					int packetId;
-					switch(packetId = bb.getInt()) {
-					case 0:
-						break;
-					case 3:
-						setUpNewSensor(recieve, bb);
-						break;
-					case 1: // PACKET_ROTATION
-					case 16: // PACKET_ROTATION_2
-						if(connection == null)
-							break;
-						bb.getLong();
-						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
-						offset.mult(buf, buf);
-						if(packetId == 1) {
-							tracker = connection.sensors.get(0);
-						} else {
-							tracker = connection.sensors.get(1);
-						}
-						if(tracker == null)
-							break;
-						tracker.rotQuaternion.set(buf);
-						tracker.dataTick();
-						break;
-					case 17: // PACKET_ROTATION_DATA
-						if(connection == null)
-							break;
-						if(connection.isOwoTrack)
-							break;
-						bb.getLong();
-						int sensorId = bb.get() & 0xFF;
-						tracker = connection.sensors.get(sensorId);
-						if(tracker == null)
-							break;
-						
-						int dataType = bb.get() & 0xFF;
-						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
-						offset.mult(buf, buf);
-						int calibrationInfo = bb.get() & 0xFF;
-						
-						switch(dataType) {
-						case 1: // DATA_TYPE_NORMAL
-							tracker.rotQuaternion.set(buf);
-							tracker.calibrationStatus = calibrationInfo;
-							tracker.dataTick();
-							break;
-						case 2: // DATA_TYPE_CORRECTION
-							tracker.rotMagQuaternion.set(buf);
-							tracker.magCalibrationStatus = calibrationInfo;
-							tracker.hasNewCorrectionData = true;
-							break;
-						}
-						break;
-					case 18: // PACKET_MAGENTOMETER_ACCURACY
-						if(connection == null)
-							break;
-						if(connection.isOwoTrack)
-							break;
-						bb.getLong();
-						sensorId = bb.get() & 0xFF;
-						tracker = connection.sensors.get(sensorId);
-						if(tracker == null)
-							break;
-						float accuracyInfo = bb.getFloat();
-						tracker.magnetometerAccuracy = accuracyInfo;
-						break;
-					case 2: // PACKET_GYRO
-					case 4: // PACKET_ACCEL
-					case 5: // PACKET_MAG
-					case 9: // PACKET_RAW_MAGENTOMETER
-						break; // None of these packets are used by SlimeVR trackers and are deprecated, use more generic PACKET_ROTATION_DATA
-					case 8: // PACKET_CONFIG
-						if(connection == null)
-							break;
-						if(connection.isOwoTrack)
-							break;
-						bb.getLong();
-						MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb);
-						Consumer<String> dataConsumer = calibrationDataRequests.remove(connection.sensors.get(0));
-						if(dataConsumer != null) {
-							dataConsumer.accept(data.toTextMatrix());
-						}
-						break;
-					case 10: // PACKET_PING_PONG:
-						if(connection == null)
-							break;
-						if(connection.isOwoTrack)
-							break;
-						int pingId = bb.getInt();
-						if(connection.lastPingPacketId == pingId) {
-							for(int i = 0; i < connection.sensors.size(); ++i) {
-								tracker = connection.sensors.get(i);
-								tracker.ping = (int) (System.currentTimeMillis() - connection.lastPingPacketTime) / 2;
-								tracker.dataTick();
-							}
-						}
-						break;
-					case 11: // PACKET_SERIAL
-						if(connection == null)
-							break;
-						if(connection.isOwoTrack)
-							break;
-						tracker = connection.sensors.get(0);
-						bb.getLong();
-						int length = bb.getInt();
-						for(int i = 0; i < length; ++i) {
-							char ch = (char) bb.get();
-							if(ch == '\n') {
-								serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer);
-								System.out.println(serialBuffer2.toString());
-								serialBuffer2.setLength(0);
-								tracker.serialBuffer.setLength(0);
-							} else {
-								tracker.serialBuffer.append(ch);
-							}
-						}
-						break;
-					case 12: // PACKET_BATTERY_VOLTAGE
-						if(connection == null)
-							break;
-						tracker = connection.sensors.get(0);
-						bb.getLong();
-						tracker.setBatteryVoltage(bb.getFloat());
-						break;
-					case 13: // PACKET_TAP
-						if(connection == null)
-							break;
-						if(connection.isOwoTrack)
-							break;
-						bb.getLong();
-						sensorId = bb.get() & 0xFF;
-						tracker = connection.sensors.get(sensorId);
-						if(tracker == null)
-							break;
-						int tap = bb.get() & 0xFF;
-						BnoTap tapObj = new BnoTap(tap);
-						System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj  + " (b" + Integer.toBinaryString(tap) + ")");
-						break;
-					case 14: // PACKET_RESET_REASON
-						bb.getLong();
-						byte reason = bb.get();
-						System.out.println("[TrackerServer] Reset recieved from " + recieve.getSocketAddress() + ": " + reason);
-						if(connection == null)
-							break;
-						sensorId = bb.get() & 0xFF;
-						tracker = connection.sensors.get(sensorId);
-						if(tracker == null)
-							break;
-						tracker.setStatus(TrackerStatus.ERROR);
-						break;
-					case 15: // PACKET_SENSOR_INFO
-						if(connection == null)
-							break;
-						bb.getLong();
-						sensorId = bb.get() & 0xFF;
-						int sensorStatus = bb.get() & 0xFF;
-						if(sensorId > 0 && sensorStatus == 1) {
-							setUpAuxilarySensor(connection, sensorId);
-						}
-						bb.rewind();
-						bb.putInt(15);
-						bb.put((byte) sensorId);
-						bb.put((byte) sensorStatus);
-						socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
-						System.out.println("[TrackerServer] Sensor info for " + connection.sensors.get(0).getName() + "/" + sensorId + ": " + sensorStatus);
-						break;
-					default:
-						System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
-						break;
-					}
-				} catch(SocketTimeoutException e) {
-				} catch(Exception e) {
-					e.printStackTrace();
-				}
-				if(lastKeepup + 500 < System.currentTimeMillis()) {
-					lastKeepup = System.currentTimeMillis();
-					synchronized(trackers) {
-						for(int i = 0; i < trackers.size(); ++i) {
-							TrackerConnection conn = trackers.get(i);
-							socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, 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);
-								}
-							}
-							IMUTracker tracker = conn.sensors.get(0);
-							if(tracker == null)
-								continue;
-							if(tracker.serialBuffer.length() > 0) {
-								if(tracker.lastSerialUpdate + 500L < System.currentTimeMillis()) {
-									serialBuffer2.append('[').append(tracker.getName()).append("] ").append(tracker.serialBuffer);
-									System.out.println(serialBuffer2.toString());
-									serialBuffer2.setLength(0);
-									tracker.serialBuffer.setLength(0);
-								}
-							}
-							if(conn.lastPingPacketTime + 500 < System.currentTimeMillis()) {
-								conn.lastPingPacketId = random.nextInt();
-								conn.lastPingPacketTime = System.currentTimeMillis();
-								bb.rewind();
-								bb.putInt(10);
-								bb.putInt(conn.lastPingPacketId);
-								socket.send(new DatagramPacket(rcvBuffer, bb.position(), conn.address));
-							}
-						}
-					}
-				}
-			}
-		} catch(Exception e) {
-			e.printStackTrace();
-		} finally {
-			Util.close(socket);
-		}
-	}
-	
-	private class TrackerConnection {
-		
-		Map<Integer, IMUTracker> sensors = new HashMap<>();
-		SocketAddress address;
-		public long lastPacket = System.currentTimeMillis();
-		public int lastPingPacketId = -1;
-		public long lastPingPacketTime = 0;
-		public boolean isOwoTrack = false;
-		
-		public TrackerConnection(IMUTracker tracker, SocketAddress address) {
-			this.sensors.put(0, tracker);
-			this.address = address;
-		}
-	}
-	
-	static {
-		try {
-			HANDSHAKE_BUFFER[0] = 3;
-			byte[] str = "Hey OVR =D 5".getBytes("ASCII");
-	        System.arraycopy(str, 0, HANDSHAKE_BUFFER, 1, str.length);
-		} catch(UnsupportedEncodingException e) {
-			throw new AssertionError(e);
-		}
-		KEEPUP_BUFFER[3] = 1;
-		CALIBRATION_BUFFER[3] = 4;
-		CALIBRATION_BUFFER[4] = 1;
-		CALIBRATION_REQUEST_BUFFER[3] = 4;
-		CALIBRATION_REQUEST_BUFFER[4] = 2;
-	}
-}

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 < 9) {
+				// 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,34 @@
+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();
+		} else {
+			level = voltage;
+			voltage = 0.0f;
+		}
+	}
+	
+	@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;
+	}
+}

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

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

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

@@ -0,0 +1,52 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+import dev.slimevr.vr.trackers.TrackerStatus;
+
+public class UDPPacket15SensorInfo extends UDPPacket implements SensorSpecificPacket {
+	
+	public int sensorId;
+	public int sensorStatus;
+	public int sensorType;
+	
+	public UDPPacket15SensorInfo() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 15;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		sensorId = buf.get() & 0xFF;
+		sensorStatus = buf.get() & 0xFF;
+		if(buf.remaining() > 0)
+			sensorType = buf.get() & 0xFF;
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		// Never sent back in current protocol
+	}
+
+	@Override
+	public int getSensorId() {
+		return sensorId;
+	}
+	
+	public static TrackerStatus getStatus(int sensorStatus) {
+		switch(sensorStatus) {
+		case 0:
+			return TrackerStatus.DISCONNECTED;
+		case 1:
+			return TrackerStatus.OK;
+		case 2:
+			return TrackerStatus.ERROR;
+		}
+		return null;
+	}
+	
+}

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

@@ -0,0 +1,17 @@
+package dev.slimevr.vr.trackers.udp;
+
+public class UDPPacket16Rotation2 extends UDPPacket1Rotation {
+	
+	public UDPPacket16Rotation2() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 16;
+	}
+
+	@Override
+	public int getSensorId() {
+		return 1;
+	}
+}

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

@@ -0,0 +1,43 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+import com.jme3.math.Quaternion;
+
+public class UDPPacket17RotationData extends UDPPacket implements SensorSpecificPacket {
+
+	public static final int DATA_TYPE_NORMAL = 1;
+	public static final int DATA_TYPE_CORRECTION = 2;
+	
+	public int sensorId;
+	public int dataType;
+	public final Quaternion rotation = new Quaternion();
+	public int calibrationInfo;
+	
+	public UDPPacket17RotationData() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 17;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		sensorId = buf.get() & 0xFF;
+		dataType = buf.get() & 0xFF;
+		rotation.set(buf.getFloat(), buf.getFloat(), buf.getFloat(), buf.getFloat());
+		calibrationInfo = buf.get() & 0xFF;
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		// Never sent back in current protocol
+	}
+
+	@Override
+	public int getSensorId() {
+		return sensorId;
+	}
+}

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

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

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

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

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

@@ -0,0 +1,9 @@
+package dev.slimevr.vr.trackers.udp;
+
+public class UDPPacket1Heartbeat extends UDPPacket0Heartbeat {
+	
+	@Override
+	public int getPacketId() {
+		return 1;
+	}
+}

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

@@ -0,0 +1,34 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+import com.jme3.math.Quaternion;
+
+public class UDPPacket1Rotation extends UDPPacket implements SensorSpecificPacket {
+	
+	public final Quaternion rotation = new Quaternion();
+	
+	public UDPPacket1Rotation() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 1;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		rotation.set(buf.getFloat(), buf.getFloat(), buf.getFloat(), buf.getFloat());
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		// Never sent back in current protocol
+	}
+
+	@Override
+	public int getSensorId() {
+		return 0;
+	}
+}

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

@@ -0,0 +1,31 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+public class UDPPacket200ProtocolChange extends UDPPacket {
+	
+	public int targetProtocol;
+	public int targetProtocolVersion;
+	
+	public UDPPacket200ProtocolChange() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 200;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		targetProtocol = buf.get() & 0xFF;
+		targetProtocolVersion = buf.get() & 0xFF;
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		buf.put((byte) targetProtocol);
+		buf.put((byte) targetProtocolVersion);
+	}
+	
+}

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

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

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

@@ -0,0 +1,59 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+
+public class UDPPacket3Handshake extends UDPPacket {
+	
+	public int boardType;
+	public int imuType;
+	public int mcuType;
+	public int firmwareBuild;
+	public String firmware;
+	public String macString;
+	
+	public UDPPacket3Handshake() {
+	}
+	
+	@Override
+	public int getPacketId() {
+		return 3;
+	}
+	
+	@Override
+	public void readData(ByteBuffer buf) throws IOException {
+		if(buf.remaining() > 0) {
+			byte[] mac = new byte[6];
+			if(buf.remaining() > 3)
+				boardType = buf.getInt();
+			if(buf.remaining() > 3)
+				imuType = buf.getInt();
+			if(buf.remaining() > 3)
+				mcuType = buf.getInt(); // MCU TYPE
+			if(buf.remaining() > 11) {
+				buf.getInt(); // IMU info
+				buf.getInt();
+				buf.getInt();
+			}
+			if(buf.remaining() > 3)
+				firmwareBuild = buf.getInt();
+			int length = 0;
+			if(buf.remaining() > 0)
+				length = buf.get(); // firmware version length is 1 longer than that because it's nul-terminated
+			firmware = readASCIIString(buf, length);
+			if(buf.remaining() >= mac.length) {
+				buf.get(mac);
+				macString = String.format("%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+				if(macString.equals("00:00:00:00:00:00"))
+					macString = null;
+			}
+		}
+	}
+	
+	@Override
+	public void writeData(ByteBuffer buf) throws IOException {
+		// Never sent back in current protocol
+		// Handshake for RAW SlimeVR and legacy owoTrack has different packet id byte order from normal packets
+		// So it's handled by raw protocol call
+	}
+}

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

@@ -0,0 +1,120 @@
+package dev.slimevr.vr.trackers.udp;
+
+import java.io.IOException;
+import java.io.UnsupportedEncodingException;
+import java.nio.ByteBuffer;
+
+import io.eiren.util.logging.LogManager;
+
+public class UDPProtocolParser {
+	
+	public static final int PACKET_HEARTBEAT = 0;
+	public static final int PACKET_ROTATION = 1; // Deprecated
+	//public static final int PACKET_GYRO = 2; // Deprecated
+	public static final int PACKET_HANDSHAKE = 3;
+	//public static final int PACKET_ACCEL = 4; // Not parsed by server
+	//public static final int PACKET_MAG = 5; // Deprecated
+	//public static final int PACKET_RAW_CALIBRATION_DATA = 6; // Not parsed by server
+	//public static final int PACKET_CALIBRATION_FINISHED = 7; // Not parsed by server
+	//public static final int PACKET_CONFIG = 8; // Not parsed by server
+	//public static final int PACKET_RAW_MAGNETOMETER = 9 // Deprecated
+	public static final int PACKET_PING_PONG = 10;
+	public static final int PACKET_SERIAL = 11;
+	public static final int PACKET_BATTERY_LEVEL = 12;
+	public static final int PACKET_TAP = 13;
+	public static final int PACKET_ERROR = 14;
+	public static final int PACKET_SENSOR_INFO = 15;
+	public static final int PACKET_ROTATION_2 = 16; // Deprecated
+	public static final int PACKET_ROTATION_DATA = 17;
+	public static final int PACKET_MAGNETOMETER_ACCURACY = 18;
+	public static final int PACKET_SIGNAL_STRENGTH = 19;
+	public static final int PACKET_TEMPERATURE = 20;
+	
+	public static final int PACKET_PROTOCOL_CHANGE = 200;
+	
+	private static final byte[] HANDSHAKE_BUFFER = new byte[64];
+	
+	public UDPProtocolParser() {
+	}
+	
+	public UDPPacket parse(ByteBuffer buf, TrackerUDPConnection connection) throws IOException {
+		int packetId = buf.getInt();
+		long packetNumber = buf.getLong();
+		if(connection != null) {
+			if(!connection.isNextPacket(packetNumber)) {
+				// Skip packet because it's not next
+				throw new IOException("Out of order packet received: id " + packetId + ", number " + packetNumber + ", last " + connection.lastPacketNumber + ", from " + connection);
+			}
+			connection.lastPacket = System.currentTimeMillis();
+		}
+		UDPPacket newPacket = getNewPacket(packetId);
+		if(newPacket != null) {
+			newPacket.readData(buf);
+		} else {
+			//LogManager.log.debug("[UDPPorotocolParser] Skipped packet id " + packetId + " from " + connection);
+		}
+		return newPacket;
+	}
+	
+	public void write(ByteBuffer buf, TrackerUDPConnection connection, UDPPacket packet) throws IOException {
+		buf.putInt(packet.getPacketId());
+		buf.putLong(0); // Packet number is always 0 when sending data to trackers
+		packet.writeData(buf);
+	}
+	
+	public void writeHandshakeResponse(ByteBuffer buf, TrackerUDPConnection connection) throws IOException {
+		buf.put(HANDSHAKE_BUFFER);
+	}
+	
+	public void writeSensorInfoResponse(ByteBuffer buf, TrackerUDPConnection connection, UDPPacket15SensorInfo packet) throws IOException {
+		buf.putInt(packet.getPacketId());
+		buf.put((byte) packet.sensorId);
+		buf.put((byte) packet.sensorStatus);
+	}
+	
+	protected UDPPacket getNewPacket(int packetId) {
+		switch(packetId) {
+		case PACKET_HEARTBEAT:
+			return new UDPPacket0Heartbeat();
+		case PACKET_ROTATION:
+			return new UDPPacket1Rotation();
+		case PACKET_HANDSHAKE:
+			return new UDPPacket3Handshake();
+		case PACKET_PING_PONG:
+			return new UDPPacket10PingPong();
+		case PACKET_SERIAL:
+			return new UDPPacket11Serial();
+		case PACKET_BATTERY_LEVEL:
+			return new UDPPacket12BatteryLevel();
+		case PACKET_TAP:
+			return new UDPPacket13Tap();
+		case PACKET_ERROR:
+			return new UDPPacket14Error();
+		case PACKET_SENSOR_INFO:
+			return new UDPPacket15SensorInfo();
+		case PACKET_ROTATION_2:
+			return new UDPPacket16Rotation2();
+		case PACKET_ROTATION_DATA:
+			return new UDPPacket17RotationData();
+		case PACKET_MAGNETOMETER_ACCURACY:
+			return new UDPPacket18MagnetometerAccuracy();
+		case PACKET_SIGNAL_STRENGTH:
+			return new UDPPacket19SignalStrength();
+		case PACKET_TEMPERATURE:
+			return new UDPPacket20Temperature();
+		case PACKET_PROTOCOL_CHANGE:
+			return new UDPPacket200ProtocolChange();
+		}
+		return null;
+	}
+	
+	static {
+		try {
+			HANDSHAKE_BUFFER[0] = 3;
+			byte[] str = "Hey OVR =D 5".getBytes("ASCII");
+			System.arraycopy(str, 0, HANDSHAKE_BUFFER, 1, str.length);
+		} catch(UnsupportedEncodingException e) {
+			throw new AssertionError(e);
+		}
+	}
+}