Fixed typo

Added important comment
Parse handshake properly
2026-04-06 02:01:58 +02:00 · 2021-08-10 16:51:42 +03:00 · 2021-08-10 09:20:12 +03:00 · 2021-08-10 08:58:57 +03:00 · 2021-08-10 08:31:03 +03:00 · 2021-08-10 07:26:32 +03:00
24 changed files with 983 additions and 693 deletions
--- a/README.md
+++ b/README.md
@@ -14,3 +14,21 @@ Integrations:
 ## How to use

 Latest instructions are currently [here](https://gist.github.com/Eirenliel/8c0eefcdbda1076d5c2e1bf634831d20). Will be updated and republished as time goes on.
+
+## How to build
+
+You need to execute these commands in the folder where you want this project.
+
+```bash
+# Clone repositories
+git clone https://github.com/SlimeVR/SlimeVR-Server.git
+git clone https://github.com/Eirenliel/slime-java-commons.git
+
+# Enter the directory and build the runnable server JAR
+cd SlimeVR-Server
+gradlew serverJar
+```
+
+Open Slime VR Server project in Eclipse or Intellij Idea
+
+run gradle command `serverJar` to build a runnable server JAR
--- a/build.gradle
+++ b/build.gradle
@@ -22,29 +22,38 @@ dependencies {
 	compile project(':Slime Java Commons')

    // This dependency is exported to consumers, that is to say found on their compile classpath.
-    api 'org.apache.commons:commons-math3:3.6.1'
-    api 'org.yaml:snakeyaml:1.25'
-    api 'net.java.dev.jna:jna:5.6.0'
-    api 'net.java.dev.jna:jna-platform:5.6.0'
-    api 'com.illposed.osc:javaosc-core:0.8'
-    api 'com.fazecast:jSerialComm:[2.0.0,3.0.0)'
+    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)'

    // This dependency is used internally, and not exposed to consumers on their own compile classpath.
    implementation 'com.google.guava:guava:28.2-jre'
    

    // Use JUnit test framework
-    testImplementation 'junit:junit:4.12'
+	testImplementation platform('org.junit:junit-bom:5.7.2')
+	testImplementation 'org.junit.jupiter:junit-jupiter'
+	testImplementation 'org.junit.platform:junit-platform-launcher'
+}
+test {
+    useJUnitPlatform()
 }

 subprojects.each { subproject -> evaluationDependsOn(subproject.path) }
 task serverJar (type: Jar, dependsOn: subprojects.tasks['build']) {
+    // Make the JAR runnable
    manifest {
        attributes 'Main-Class': 'io.eiren.vr.Main'
    }
    
+    // Pack all dependencies within the JAR
    from {
        configurations.compile.collect { it.isDirectory() ? it : zipTree(it) }
    }
+
+    // Add this project's classes in the JAR
    with jar
 }
--- a/settings.gradle
+++ b/settings.gradle
@@ -8,4 +8,19 @@
 */

 rootProject.name = 'SlimeVR Server'
-include('Slime Java Commons')
+include('Slime Java Commons')
+
+def commonsDirs = [
+    new File(settingsDir, 'Slime Java Commons'),
+    new File(settingsDir, 'slime-java-commons'),
+    new File(settingsDir, '../Slime Java Commons'),
+    new File(settingsDir, '../slime-java-commons')
+]
+
+for (commonsDir in commonsDirs) {
+    if (commonsDir.isDirectory()) {
+        logger.info('\"Slime Java Commons\" subproject detected at \"{}\"', commonsDir.getCanonicalPath())
+        project(':Slime Java Commons').projectDir = commonsDir
+        break
+    }
+}
--- a/src/main/java/io/eiren/gui/SkeletonConfig.java
+++ b/src/main/java/io/eiren/gui/SkeletonConfig.java
@@ -1,16 +1,20 @@
 package io.eiren.gui;

+import java.awt.event.ItemEvent;
+import java.awt.event.ItemListener;
 import java.awt.event.MouseEvent;
 import java.util.HashMap;
 import java.util.Map;

 import javax.swing.JButton;
+import javax.swing.JCheckBox;
 import javax.swing.JLabel;
 import javax.swing.event.MouseInputAdapter;

 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.vr.VRServer;
+import io.eiren.vr.processor.HumanSkeletonWithLegs;
 import io.eiren.vr.processor.HumanSkeleton;

 public class SkeletonConfig extends EJBag {
@@ -26,6 +30,7 @@ public class SkeletonConfig extends EJBag {

 		setAlignmentY(TOP_ALIGNMENT);
 		server.humanPoseProcessor.addSkeletonUpdatedCallback(this::skeletonUpdated);
+		skeletonUpdated(null);
 	}
 	
 	@ThreadSafe
@@ -34,6 +39,56 @@ public class SkeletonConfig extends EJBag {
 			removeAll();
 			
 			int row = 0;
+
+			add(new JCheckBox("Extended pelvis model") {{
+				addItemListener(new ItemListener() {
+				    @Override
+				    public void itemStateChanged(ItemEvent e) {
+				        if(e.getStateChange() == ItemEvent.SELECTED) {//checkbox has been selected
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended pelvis model", true);
+				        	}
+				        } else {
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended pelvis model", false);
+				        	}
+				        }
+				    }
+				});
+				if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+	        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+	        		setSelected(hswl.getSkeletonConfigBoolean("Extended pelvis model"));
+				}
+			}}, s(c(0, row, 1), 3, 1));
+			row++;
+			
+			/*
+			add(new JCheckBox("Extended knee model") {{
+				addItemListener(new ItemListener() {
+				    @Override
+				    public void itemStateChanged(ItemEvent e) {
+				        if(e.getStateChange() == ItemEvent.SELECTED) {//checkbox has been selected
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended knee model", true);
+				        	}
+				        } else {
+				        	if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+				        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+				        		hswl.setSkeletonConfigBoolean("Extended knee model", false);
+				        	}
+				        }
+				    }
+				});
+				if(newSkeleton != null && newSkeleton instanceof HumanSkeletonWithLegs) {
+	        		HumanSkeletonWithLegs hswl = (HumanSkeletonWithLegs) newSkeleton;
+	        		setSelected(hswl.getSkeletonConfigBoolean("Extended knee model"));
+				}
+			}}, s(c(0, row, 1), 3, 1));
+			row++;
+			//*/
 			
 			add(new TimedResetButton("Reset All", "All"), s(c(1, row, 1), 3, 1));
 			row++;
--- a/src/main/java/io/eiren/gui/TrackersList.java
+++ b/src/main/java/io/eiren/gui/TrackersList.java
@@ -102,8 +102,12 @@ public class TrackersList extends EJBox {
 		JLabel bat;
 		JLabel ping;
 		JLabel raw;
+		JLabel rawMag;
+		JLabel calibration;
+		JLabel magAccuracy;
 		JLabel adj;
 		JLabel adjYaw;
+		JLabel correction;
 		
 		@AWTThread
 		public TrackerRow(Tracker t) {
@@ -114,12 +118,19 @@ public class TrackersList extends EJBox {
 		@SuppressWarnings("unchecked")
 		@AWTThread
 		public TrackerRow build() {
+			int row = 0;
+			
+			Tracker realTracker = t;
+			if(t instanceof ReferenceAdjustedTracker)
+				realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
 			removeAll();
-			add(new JLabel(t.getName()), s(c(0, 0, 0, GridBagConstraints.FIRST_LINE_START), 4, 1));
+			add(new JLabel(t.getName()), s(c(0, row, 0, GridBagConstraints.FIRST_LINE_START), 4, 1));
+			row++;
+			
 			if(t.userEditable()) {
 				TrackerConfig cfg = server.getTrackerConfig(t);
 				JComboBox<String> desSelect;
-				add(desSelect = new JComboBox<>(), s(c(0, 1, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
+				add(desSelect = new JComboBox<>(), s(c(0, row, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
 				for(TrackerBodyPosition p : TrackerBodyPosition.values) {
 					desSelect.addItem(p.name());
 				}
@@ -136,14 +147,11 @@ public class TrackersList extends EJBox {
 						server.trackerUpdated(t);
 					}
 				});
-				Tracker realTracker = t;
-				if(t instanceof ReferenceAdjustedTracker<?>)
-					realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
 				if(realTracker instanceof IMUTracker) {
 					IMUTracker imu = (IMUTracker) realTracker;
 					TrackerMountingRotation tr = imu.getMountingRotation();
 					JComboBox<String> mountSelect;
-					add(mountSelect = new JComboBox<>(), s(c(2, 1, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
+					add(mountSelect = new JComboBox<>(), s(c(2, row, 0, GridBagConstraints.FIRST_LINE_START), 2, 1));
 					for(TrackerMountingRotation p : TrackerMountingRotation.values) {
 						mountSelect.addItem(p.name());
 					}
@@ -161,31 +169,49 @@ public class TrackersList extends EJBox {
 						}
 					});
 				}
+				row++;
 			}
-			add(new JLabel("Rotation"), c(0, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Postion"), c(1, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Ping"), c(2, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("TPS"), c(3, 2, 0, GridBagConstraints.FIRST_LINE_START));
-			add(rotation = new JLabel("0 0 0"), c(0, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			add(position = new JLabel("0 0 0"), c(1, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			add(ping = new JLabel(""), c(2, 3, 0, GridBagConstraints.FIRST_LINE_START));
-			if(t instanceof TrackerWithTPS) {
-				add(tps = new JLabel("0"), c(3, 3, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Rotation"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Position"), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Ping"), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("TPS"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
+			row++;
+			add(rotation = new JLabel("0 0 0"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(position = new JLabel("0 0 0"), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(ping = new JLabel(""), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+			if(realTracker instanceof TrackerWithTPS) {
+				add(tps = new JLabel("0"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
 			} else {
-				add(new JLabel(""), c(3, 3, 0, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel(""), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
+			}
+			row++;
+			add(new JLabel("Status:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Battery:"), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(bat = new JLabel("0"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
+			row++;
+			add(new JLabel("Raw:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+			add(raw = new JLabel("0 0 0"), s(c(1, row, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
+			row++;
+			if(realTracker instanceof IMUTracker) {
+				add(new JLabel("Raw mag:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(rawMag = new JLabel("0 0 0"), s(c(1, row, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
+				row++;
+				add(new JLabel("Cal:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(calibration = new JLabel("0"), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("Mag acc:"), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(magAccuracy = new JLabel("0°"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
+				row++;
+				add(new JLabel("Correction:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(correction = new JLabel("0 0 0"), s(c(1, row, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
+				row++;
 			}
-			add(new JLabel("Status:"), c(0, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Battery:"), c(2, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(bat = new JLabel("0"), c(3, 4, 0, GridBagConstraints.FIRST_LINE_START));
-			add(new JLabel("Raw:"), c(0, 5, 0, GridBagConstraints.FIRST_LINE_START));
-			add(raw = new JLabel("0 0 0 0"), s(c(1, 5, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
 			
-			if(t instanceof ReferenceAdjustedTracker) {
-				add(new JLabel("Adj:"), c(0, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(adj = new JLabel("0 0 0 0"), c(1, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(new JLabel("AdjY:"), c(2, 6, 0, GridBagConstraints.FIRST_LINE_START));
-				add(adjYaw = new JLabel("0 0 0 0"), c(3, 6, 0, GridBagConstraints.FIRST_LINE_START));
+			if(t instanceof ReferenceAdjustedTracker) {	
+				add(new JLabel("Adj:"), c(0, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(adj = new JLabel("0 0 0 0"), c(1, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("AdjY:"), c(2, row, 0, GridBagConstraints.FIRST_LINE_START));
+				add(adjYaw = new JLabel("0 0 0 0"), c(3, row, 0, GridBagConstraints.FIRST_LINE_START));
 			}

 			setBorder(BorderFactory.createLineBorder(new Color(0x663399), 4, true));
@@ -198,6 +224,9 @@ public class TrackersList extends EJBox {
 		public void update() {
 			if(position == null)
 				return;
+			Tracker realTracker = t;
+			if(t instanceof ReferenceAdjustedTracker)
+				realTracker = ((ReferenceAdjustedTracker<? extends Tracker>) t).getTracker();
 			t.getRotation(q);
 			t.getPosition(v);
 			q.toAngles(angles);
@@ -210,30 +239,42 @@ public class TrackersList extends EJBox {
 					+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
 			status.setText(t.getStatus().toString().toLowerCase());
 			
-			if(t instanceof TrackerWithTPS) {
-				tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) t).getTPS(), 1));
+			if(realTracker instanceof TrackerWithTPS) {
+				tps.setText(StringUtils.prettyNumber(((TrackerWithTPS) realTracker).getTPS(), 1));
 			}
-			if(t instanceof TrackerWithBattery)
-				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) t).getBatteryVoltage(), 1));
-			Tracker t2 = t;
+			if(realTracker instanceof TrackerWithBattery)
+				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) realTracker).getBatteryVoltage(), 1));
 			if(t instanceof ReferenceAdjustedTracker) {
-				t2 = ((ReferenceAdjustedTracker<Tracker>) t).getTracker();
-				((ReferenceAdjustedTracker<Tracker>) t).adjustmentAttachment.toAngles(angles);
+				((ReferenceAdjustedTracker<Tracker>) t).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).adjustmentYaw.toAngles(angles);
+				((ReferenceAdjustedTracker<Tracker>) t).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(t2 instanceof IMUTracker)
-				ping.setText(String.valueOf(((IMUTracker) t2).ping));
-			t2.getRotation(q);
-			raw.setText(StringUtils.prettyNumber(q.getX(), 4)
-					+ " " + StringUtils.prettyNumber(q.getY(), 4)
-					+ " " + StringUtils.prettyNumber(q.getZ(), 4)
-					+ " " + StringUtils.prettyNumber(q.getW(), 4));
+			if(realTracker instanceof IMUTracker) {
+				ping.setText(String.valueOf(((IMUTracker) realTracker).ping));
+			}
+			realTracker.getRotation(q);
+			q.toAngles(angles);
+			raw.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+					+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+					+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			if(realTracker instanceof IMUTracker) {
+				((IMUTracker) realTracker).rotMagQuaternion.toAngles(angles);
+				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));
+				calibration.setText(((IMUTracker) realTracker).calibrationStatus + " / " + ((IMUTracker) realTracker).magCalibrationStatus);
+				magAccuracy.setText(StringUtils.prettyNumber(((IMUTracker) realTracker).magnetometerAccuracy * FastMath.RAD_TO_DEG, 1) + "°");
+				((IMUTracker) realTracker).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));
+			}
 		}
 	}
 	
--- a/src/main/java/io/eiren/gui/VRServerGUI.java
+++ b/src/main/java/io/eiren/gui/VRServerGUI.java
@@ -8,11 +8,12 @@ import io.eiren.util.StringUtils;
 import io.eiren.util.ann.AWTThread;
 import io.eiren.vr.Main;
 import io.eiren.vr.VRServer;
-import io.eiren.vr.bridge.NamedPipeVRBridge;

 import java.awt.Component;
 import java.awt.Container;
 import java.awt.Font;
+import java.awt.event.ActionEvent;
+import java.awt.event.ActionListener;
 import java.awt.event.MouseEvent;

 import static javax.swing.BoxLayout.PAGE_AXIS;
@@ -33,6 +34,11 @@ public class VRServerGUI extends JFrame {
 	@AWTThread
 	public VRServerGUI(VRServer server) {
 		super("SlimeVR Server (" + Main.VERSION + ")");
+		try {
+			UIManager.setLookAndFeel(UIManager.getSystemLookAndFeelClassName());
+		} catch(Exception e) {
+			e.printStackTrace();
+		}
 		//increaseFontSize();
 		
 		this.server = server;
@@ -73,8 +79,6 @@ public class VRServerGUI extends JFrame {
 	private void build() {
 		pane.removeAll();
 		
-		NamedPipeVRBridge npvb = server.getVRBridge(NamedPipeVRBridge.class);
-		
 		pane.add(new EJBox(LINE_AXIS) {{
 			setBorder(new EmptyBorder(i(5)));
 			
@@ -87,19 +91,16 @@ public class VRServerGUI extends JFrame {
 					}
 				});
 			}});
+			add(Box.createHorizontalStrut(10));
+			add(new JButton("Fast Reset") {{
+				addMouseListener(new MouseInputAdapter() {
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						resetFast();
+					}
+				});
+			}});
 			add(Box.createHorizontalGlue());
-			if(npvb != null) {
-				add(new JButton(npvb.isOneTrackerMode() ? "Trackers: 1" : "Trackers: 3") {{
-					addMouseListener(new MouseInputAdapter() {
-						@Override
-						public void mouseClicked(MouseEvent e) {
-							npvb.setSpawnOneTracker(!npvb.isOneTrackerMode());
-							setText(npvb.isOneTrackerMode() ? "Trackers: 1" : "Trackers: 3");
-						}
-					});
-				}});
-				add(Box.createHorizontalStrut(10));
-			}
 			add(new JButton("GUI Zoom (x" + StringUtils.prettyNumber(zoom, 2) + ")") {{
 				addMouseListener(new MouseInputAdapter() {
 					@Override
@@ -126,8 +127,57 @@ public class VRServerGUI extends JFrame {
 			setBorder(new EmptyBorder(i(5)));
 			add(new EJBox(PAGE_AXIS) {{
 				setAlignmentY(TOP_ALIGNMENT);
+				add(new JLabel("SteamVR Trackers:"));
+				JComboBox<String> trackersSelect;
+				add(trackersSelect = new JComboBox<>());
+				trackersSelect.addItem("Waist");
+				trackersSelect.addItem("Waist + Feet");
+				trackersSelect.addItem("Waist + Feet + Chest");
+				trackersSelect.addItem("Waist + Feet + Knees");
+				trackersSelect.addItem("Waist + Feet + Chest + Knees");
+				switch(server.config.getInt("virtualtrackers", 3)) {
+				case 1:
+					trackersSelect.setSelectedIndex(0);
+					break;
+				case 3:
+					trackersSelect.setSelectedIndex(1);
+					break;
+				case 4:
+					trackersSelect.setSelectedIndex(2);
+					break;
+				case 5:
+					trackersSelect.setSelectedIndex(3);
+					break;
+				case 6:
+					trackersSelect.setSelectedIndex(4);
+					break;
+				}
+				trackersSelect.addActionListener(new ActionListener() {
+					@Override
+					public void actionPerformed(ActionEvent e) {
+						switch(trackersSelect.getSelectedIndex()) {
+						case 0:
+							server.config.setProperty("virtualtrackers", 1);
+							break;
+						case 1:
+							server.config.setProperty("virtualtrackers", 3);
+							break;
+						case 2:
+							server.config.setProperty("virtualtrackers", 4);
+							break;
+						case 3:
+							server.config.setProperty("virtualtrackers", 5);
+							break;
+						case 4:
+							server.config.setProperty("virtualtrackers", 6);
+							break;
+						}
+						server.saveConfig();
+					}
+				});
+				add(Box.createHorizontalStrut(10));
 				
-				add(new JLabel("Trackers"));
+				add(new JLabel("Trackers list"));
 				add(trackersList);
 				add(Box.createVerticalGlue());
 			}});
@@ -193,6 +243,11 @@ public class VRServerGUI extends JFrame {
 		}
 	}
 	
+	@AWTThread
+	private void resetFast() {
+		server.resetTrackersYaw();
+	}
+	
 	@AWTThread
 	private void reset() {
 		ButtonTimer.runTimer(resetButton, 3, "RESET", server::resetTrackers);
--- a/src/main/java/io/eiren/vr/Main.java
+++ b/src/main/java/io/eiren/vr/Main.java
@@ -5,13 +5,13 @@ import java.io.File;
 import io.eiren.gui.VRServerGUI;
 import io.eiren.util.logging.LogManager;

-
 public class Main {
 	
-	public static String VERSION = "0.0.10";
+	public static String VERSION = "0.0.14";
 	
 	public static VRServer vrServer;
 	
+	@SuppressWarnings("unused")
 	public static void main(String[] args) {
 		System.setProperty("awt.useSystemAAFontSettings", "on");
 		System.setProperty("swing.aatext", "true");
--- a/src/main/java/io/eiren/vr/VRServer.java
+++ b/src/main/java/io/eiren/vr/VRServer.java
@@ -50,7 +50,7 @@ public class VRServer extends Thread {
 		hmdTracker = new HMDTracker("HMD");
 		hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier debugging
 		// TODO Multiple processors
-		humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker);
+		humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker, config.getInt("virtualtrackers", 3));
 		List<? extends Tracker> shareTrackers = humanPoseProcessor.getComputedTrackers();
 		
 		// Create named pipe bridge for SteamVR driver
@@ -189,6 +189,8 @@ public class VRServer extends Thread {
 					break;
 				task.run();
 			} while(true);
+			for(int i = 0; i < trackers.size(); ++i)
+				trackers.get(i).tick();
 			
 			for(int i = 0; i < onTick.size(); ++i) {
 				this.onTick.get(i).run();
@@ -234,6 +236,12 @@ public class VRServer extends Thread {
 		});
 	}
 	
+	public void resetTrackersYaw() {
+		queueTask(() -> {
+			humanPoseProcessor.resetTrackersYaw();
+		});
+	}
+	
 	public int getTrackersCount() {
 		return trackers.size();
 	}
--- a/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java
+++ b/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java
@@ -43,8 +43,6 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	private final HMDTracker internalHMDTracker = new HMDTracker("itnernal://HMD");
 	private final AtomicBoolean newHMDData = new AtomicBoolean(false);
 	
-	private boolean spawnOneTracker = false;
-	
 	public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> shareTrackers, VRServer server) {
 		super("Named Pipe VR Bridge");
 		this.server = server;
@@ -58,26 +56,6 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 			ct.setStatus(TrackerStatus.OK);
 			this.internalTrackers.add(ct);
 		}
-		this.spawnOneTracker = server.config.getBoolean("openvr.onetracker", spawnOneTracker);
-	}
-	
-	public boolean isOneTrackerMode() {
-		return this.spawnOneTracker;
-	}
-	
-	/**
-	 * Makes OpenVR bridge spawn only 1 tracker instead of 3, for
-	 * use with only waist/chest tracking. Requires restart.
-	 */
-	public void setSpawnOneTracker(boolean spawnOneTracker) {
-		if(spawnOneTracker == this.spawnOneTracker)
-			return;
-		this.spawnOneTracker = spawnOneTracker;
-		if(this.spawnOneTracker)
-			this.server.config.setProperty("openvr.onetracker", true);
-		else
-			this.server.config.removeProperty("openvr.onetracker");
-		this.server.saveConfig();
 	}
 	
 	@Override
@@ -133,8 +111,6 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 				if(tryOpeningPipe(trackerPipe))
 					initTrackerPipe(trackerPipe, i);
 			}
-			if(spawnOneTracker)
-				break;
 		}
 	}
 	
@@ -194,7 +170,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	}
 	
 	private void initTrackerPipe(Pipe pipe, int trackerId) {
-		String trackerHello = (spawnOneTracker ? "1" : this.shareTrackers.size()) + " 0";
+		String trackerHello = this.shareTrackers.size() + " 0";
 		System.arraycopy(trackerHello.getBytes(ASCII), 0, buffer, 0, trackerHello.length());
 		buffer[trackerHello.length()] = '\0';
 		IntByReference lpNumberOfBytesWritten = new IntByReference(0);
@@ -252,8 +228,6 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 					throw new IOException("Can't open " + pipeName + " pipe: " + Kernel32.INSTANCE.GetLastError());
 				LogManager.log.info("[VRBridge] Pipe " + pipeName + " created");
 				trackerPipes.add(new Pipe(pipeHandle, pipeName));
-				if(spawnOneTracker)
-					break;
 			}
 			LogManager.log.info("[VRBridge] Pipes are open");
 		} catch(IOException e) {
--- a/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTrackerPosition.java
+++ b/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTrackerPosition.java
@@ -5,5 +5,7 @@ public enum ComputedHumanPoseTrackerPosition {
 	WAIST,
 	CHEST,
 	LEFT_FOOT,
-	RIGHT_FOOT;
+	RIGHT_FOOT,
+	LEFT_KNEE,
+	RIGHT_KNEE;
 }
--- a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java
+++ b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java
@@ -19,11 +19,24 @@ public class HumanPoseProcessor {
 	private final List<Consumer<HumanSkeleton>> onSkeletonUpdated = new FastList<>();
 	private HumanSkeleton skeleton;

-	public HumanPoseProcessor(VRServer server, HMDTracker hmd) {
+	public HumanPoseProcessor(VRServer server, HMDTracker hmd, int trackersAmount) {
 		this.server = server;
 		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.WAIST));
-		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT));
-		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT));
+		if(trackersAmount > 2) {
+			computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT));
+			computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT));
+			if(trackersAmount == 4 || trackersAmount >= 6) {
+				computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.CHEST));
+			}
+			if(trackersAmount >= 5) {
+				computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_KNEE));
+				computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_KNEE));
+			}
+		}
+	}
+	
+	public HumanSkeleton getSkeleton() {
+		return skeleton;
 	}

 	@VRServerThread
@@ -76,24 +89,22 @@ public class HumanPoseProcessor {
 		boolean hasBothLegs = false;
 		List<Tracker> allTrackers = server.getAllTrackers();
 		Tracker waist = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST);
-		Tracker leftAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_ANKLE);
-		Tracker rightAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_ANKLE);
-		Tracker leftLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_LEG);
-		Tracker rightLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_LEG);
+		Tracker leftAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_ANKLE, TrackerBodyPosition.LEFT_LEG);
+		Tracker rightAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_ANKLE, TrackerBodyPosition.RIGHT_LEG);
 		if(waist != null)
 			hasWaist = true;
-		if(leftAnkle != null && rightAnkle != null && leftLeg != null && rightLeg != null)
+		if(leftAnkle != null && rightAnkle != null)
 			hasBothLegs = true;
 		if(!hasWaist) {
 			skeleton = null; // Can't track anything without waist
 		} else if(hasBothLegs) {
 			disconnectAllTrackers();
-			skeleton = new HumanSekeletonWithLegs(server, computedTrackers);
+			skeleton = new HumanSkeletonWithLegs(server, computedTrackers);
 			for(int i = 0; i < onSkeletonUpdated.size(); ++i)
 				onSkeletonUpdated.get(i).accept(skeleton);
 		} else {
 			disconnectAllTrackers();
-			skeleton = new HumanSkeleonWithWaist(server, computedTrackers);
+			skeleton = new HumanSkeletonWithWaist(server, computedTrackers);
 			for(int i = 0; i < onSkeletonUpdated.size(); ++i)
 				onSkeletonUpdated.get(i).accept(skeleton);
 		}
@@ -117,4 +128,10 @@ public class HumanPoseProcessor {
 		if(skeleton != null)
 			skeleton.resetTrackersFull();
 	}
+
+	@VRServerThread
+	public void resetTrackersYaw() {
+		if(skeleton != null)
+			skeleton.resetTrackersYaw();
+	}
 }
--- a/src/main/java/io/eiren/vr/processor/HumanSkeleton.java
+++ b/src/main/java/io/eiren/vr/processor/HumanSkeleton.java
@@ -24,4 +24,7 @@ public abstract class HumanSkeleton {

 	@VRServerThread
 	public abstract void resetTrackersFull();
+	
+	@VRServerThread
+	public abstract void resetTrackersYaw();
 }
--- a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java
+++ b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java
@@ -12,25 +12,28 @@ import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerStatus;
 import io.eiren.vr.trackers.TrackerUtils;

-public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
+public class HumanSkeletonWithLegs extends HumanSkeletonWithWaist {
 	
 	public static final float HIPS_WIDTH_DEFAULT = 0.3f;
 	public static final float FOOT_LENGTH_DEFAULT = 0.05f;
 	public static final float DEFAULT_FLOOR_OFFSET = 0.05f;
 	
-	protected final float[] kneeAngles = new float[3];
-	protected final float[] hipAngles = new float[3];
 	protected final Quaternion hipBuf = new Quaternion();
 	protected final Quaternion kneeBuf = new Quaternion();
+	protected final Vector3f hipVector = new Vector3f();
+	protected final Vector3f ankleVector = new Vector3f();
+	protected final Quaternion kneeRotation = new Quaternion();
 	
 	protected final Tracker leftLegTracker;
 	protected final Tracker leftAnkleTracker;
 	protected final Tracker leftFootTracker;
 	protected final ComputedHumanPoseTracker computedLeftFootTracker;
+	protected final ComputedHumanPoseTracker computedLeftKneeTracker;
 	protected final Tracker rightLegTracker;
 	protected final Tracker rightAnkleTracker;
 	protected final Tracker rightFootTracker;
 	protected final ComputedHumanPoseTracker computedRightFootTracker;
+	protected final ComputedHumanPoseTracker computedRightKneeTracker;
 	
 	protected final TransformNode leftHipNode = new TransformNode("Left-Hip", false);
 	protected final TransformNode leftKneeNode = new TransformNode("Left-Knee", false);
@@ -59,33 +62,46 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD;
 	
 	protected float kneeLerpFactor = 0.5f;
+	
+	protected boolean extendedPelvisModel = true;
+	protected boolean extendedKneeModel = false;

-	public HumanSekeletonWithLegs(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
+	public HumanSkeletonWithLegs(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
 		super(server, computedTrackers);
 		List<Tracker> allTracekrs = server.getAllTrackers();
-		this.leftLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_LEG);
-		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_ANKLE);
+		this.leftLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_LEG, TrackerBodyPosition.LEFT_ANKLE);
+		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_ANKLE, TrackerBodyPosition.LEFT_LEG);
 		this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_FOOT);
-		this.rightLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_LEG);
-		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_ANKLE);
+		this.rightLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_LEG, TrackerBodyPosition.RIGHT_ANKLE);
+		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_ANKLE, TrackerBodyPosition.RIGHT_LEG);
 		this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_FOOT);
 		ComputedHumanPoseTracker lat = null;
 		ComputedHumanPoseTracker rat = null;
+		ComputedHumanPoseTracker rkt = null;
+		ComputedHumanPoseTracker lkt = null;
 		for(int i = 0; i < computedTrackers.size(); ++i) {
 			ComputedHumanPoseTracker t = computedTrackers.get(i);
 			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.LEFT_FOOT)
 				lat = t;
 			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.RIGHT_FOOT)
 				rat = t;
+			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.LEFT_KNEE)
+				lkt = t;
+			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.RIGHT_KNEE)
+				rkt = t;
 		}
 		computedLeftFootTracker = lat;
 		computedRightFootTracker = rat;
+		computedLeftKneeTracker = lkt;
+		computedRightKneeTracker = rkt;
 		lat.setStatus(TrackerStatus.OK);
 		rat.setStatus(TrackerStatus.OK);
 		hipsWidth = server.config.getFloat("body.hipsWidth", hipsWidth);
 		kneeHeight = server.config.getFloat("body.kneeHeight", kneeHeight);
 		legsLength = server.config.getFloat("body.legsLength", legsLength);
 		footLength = server.config.getFloat("body.footLength", footLength);
+		extendedPelvisModel = server.config.getBoolean("body.model.extendedPelvis", extendedPelvisModel);
+		extendedKneeModel = server.config.getBoolean("body.model.extendedKnee", extendedKneeModel);
 		
 		waistNode.attachChild(leftHipNode);
 		leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
@@ -181,6 +197,28 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		}
 	}
 	
+	public boolean getSkeletonConfigBoolean(String config) {
+		switch(config) {
+		case "Extended pelvis model":
+			return extendedPelvisModel;
+		case "Extended knee model":
+			return extendedKneeModel;
+		}
+		return false;
+	}
+	
+	public void setSkeletonConfigBoolean(String config, boolean newState) {
+		switch(config) {
+		case "Extended pelvis model":
+			extendedPelvisModel = newState;
+			server.config.setProperty("body.model.extendedPelvis", newState);
+			break;
+		case "Extended knee model":
+			extendedKneeModel = newState;
+			server.config.setProperty("body.model.extendedKnee", newState);
+		}
+	}
+	
 	@Override
 	public void updateLocalTransforms() {
 		super.updateLocalTransforms();
@@ -188,7 +226,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		leftLegTracker.getRotation(hipBuf);
 		leftAnkleTracker.getRotation(kneeBuf);

-		//calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
+		if(extendedKneeModel)
+			calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
 		
 		leftHipNode.localTransform.setRotation(hipBuf);
 		leftKneeNode.localTransform.setRotation(kneeBuf);
@@ -205,7 +244,8 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		rightLegTracker.getRotation(hipBuf);
 		rightAnkleTracker.getRotation(kneeBuf);
 		
-		//calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
+		if(extendedKneeModel)
+			calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
 		
 		rightHipNode.localTransform.setRotation(hipBuf);
 		rightKneeNode.localTransform.setRotation(kneeBuf);
@@ -217,20 +257,35 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 			rightAnkleNode.localTransform.setRotation(kneeBuf);
 			rightFootNode.localTransform.setRotation(kneeBuf);
 		}
-		
-		// TODO Calculate waist node as some function between waist and hip rotations
+
+		if(extendedPelvisModel) {
+			// Average pelvis between two legs
+			leftHipNode.localTransform.getRotation(hipBuf);
+			rightHipNode.localTransform.getRotation(kneeBuf);
+			kneeBuf.slerp(hipBuf, 0.5f);
+			waistNode.localTransform.setRotation(kneeBuf);
+			// TODO : Use vectors to add like 50% of wasit tracker yaw to waist node to reduce drift and let user take weird poses
+		}
 	}
 	
-	// Knee basically has only 1 DoF (pitch), average yaw between knee and hip
+	// Knee basically has only 1 DoF (pitch), average yaw and roll between knee and hip
 	protected void calculateKneeLimits(Quaternion hipBuf, Quaternion kneeBuf, float hipConfidense, float kneeConfidense) {
-		hipBuf.toAngles(hipAngles);
-		kneeBuf.toAngles(kneeAngles);
+		ankleVector.set(0, -1, 0);
+		hipVector.set(0, -1, 0);
+		hipBuf.multLocal(hipVector);
+		kneeBuf.multLocal(ankleVector);
+		kneeRotation.angleBetweenVectors(hipVector, ankleVector); // Find knee angle
 		
-		hipAngles[1] = kneeAngles[1] = interpolateRadians(kneeLerpFactor, kneeAngles[1], hipAngles[1]);
-		//hipAngles[2] = kneeAngles[2] = interpolateRadians(kneeLerpFactor, kneeAngles[2], hipAngles[2]);
+		// Substract knee angle from knee rotation. With perfect leg and perfect
+		// sensors result should match hip rotation perfectly
+		kneeBuf.multLocal(kneeRotation.inverse());
 		
-		hipBuf.fromAngles(hipAngles);
-		kneeBuf.fromAngles(kneeAngles);
+		// Average knee and hip with a slerp
+		hipBuf.slerp(kneeBuf, 0.5f); // TODO : Use confidence to calculate changeAmt
+		kneeBuf.set(hipBuf);
+
+		// Return knee angle into knee rotation
+		kneeBuf.multLocal(kneeRotation);
 	}
 	
 	public static float normalizeRad(float angle) {
@@ -254,13 +309,29 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	protected void updateComputedTrackers() {
 		super.updateComputedTrackers();
 		
-		computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation());
-		computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation());
-		computedLeftFootTracker.dataTick();
+		if(computedLeftFootTracker != null) {
+			computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation());
+			computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation());
+			computedLeftFootTracker.dataTick();
+		}
 		
-		computedRightFootTracker.position.set(rightFootNode.worldTransform.getTranslation());
-		computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation());
-		computedRightFootTracker.dataTick();
+		if(computedLeftKneeTracker != null) {
+			computedLeftKneeTracker.position.set(leftKneeNode.worldTransform.getTranslation());
+			computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation());
+			computedLeftKneeTracker.dataTick();
+		}
+		
+		if(computedRightFootTracker != null) {
+			computedRightFootTracker.position.set(rightFootNode.worldTransform.getTranslation());
+			computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation());
+			computedRightFootTracker.dataTick();
+		}
+		
+		if(computedRightKneeTracker != null) {
+			computedRightKneeTracker.position.set(rightKneeNode.worldTransform.getTranslation());
+			computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation());
+			computedRightKneeTracker.dataTick();
+		}
 	}
 	
 	@Override
@@ -289,8 +360,39 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		this.rightAnkleTracker.resetFull(referenceRotation);
 		this.rightAnkleTracker.getRotation(referenceRotation);
 		
-		if(this.rightAnkleTracker != null) {
-			this.rightAnkleTracker.resetFull(referenceRotation);
+		if(this.rightFootTracker != null) {
+			this.rightFootTracker.resetFull(referenceRotation);
+		}
+	}
+	
+	@Override
+	@VRServerThread
+	public void resetTrackersYaw() {
+		// Each tracker uses the tracker before it to adjust iteself,
+		// so trackers that don't need adjustments could be used too
+		super.resetTrackersYaw();
+		// Start with waist, it was reset in the parent
+		Quaternion referenceRotation = new Quaternion();
+		this.waistTracker.getRotation(referenceRotation);
+		
+		this.leftLegTracker.resetYaw(referenceRotation);
+		this.rightLegTracker.resetYaw(referenceRotation);
+		this.leftLegTracker.getRotation(referenceRotation);
+		
+		this.leftAnkleTracker.resetYaw(referenceRotation);
+		this.leftAnkleTracker.getRotation(referenceRotation);
+		
+		if(this.leftFootTracker != null) {
+			this.leftFootTracker.resetYaw(referenceRotation);
+		}
+
+		this.rightLegTracker.getRotation(referenceRotation);
+		
+		this.rightAnkleTracker.resetYaw(referenceRotation);
+		this.rightAnkleTracker.getRotation(referenceRotation);
+		
+		if(this.rightFootTracker != null) {
+			this.rightFootTracker.resetYaw(referenceRotation);
 		}
 	}
 }
--- a/src/main/java/io/eiren/vr/processor/HumanSkeletonWithWaist.java
+++ b/src/main/java/io/eiren/vr/processor/HumanSkeletonWithWaist.java
@@ -14,7 +14,7 @@ import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerStatus;
 import io.eiren.vr.trackers.TrackerUtils;

-public class HumanSkeleonWithWaist extends HumanSkeleton {
+public class HumanSkeletonWithWaist extends HumanSkeleton {
 	
 	public static final float HEAD_SHIFT_DEFAULT = 0.1f;
 	public static final float NECK_LENGTH_DEFAULT = 0.1f;
@@ -30,6 +30,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	protected final Tracker chestTracker;
 	protected final HMDTracker hmdTracker;
 	protected final ComputedHumanPoseTracker computedWaistTracker;
+	protected final ComputedHumanPoseTracker computedChestTracker;
 	protected final TransformNode hmdNode = new TransformNode("HMD", false);
 	protected final TransformNode headNode = new TransformNode("Head", false);
 	protected final TransformNode neckNode = new TransformNode("Neck", false);
@@ -57,19 +58,23 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	 */
 	protected float headShift = HEAD_SHIFT_DEFAULT;

-	public HumanSkeleonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
+	public HumanSkeletonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
 		List<Tracker> allTracekrs = server.getAllTrackers();
 		this.waistTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST);
 		this.chestTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.CHEST, TrackerBodyPosition.WAIST);
 		this.hmdTracker = server.hmdTracker;
 		this.server = server;
 		ComputedHumanPoseTracker cwt = null;
+		ComputedHumanPoseTracker cct = null;
 		for(int i = 0; i < computedTrackers.size(); ++i) {
 			ComputedHumanPoseTracker t = computedTrackers.get(i);
 			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.WAIST)
 				cwt = t;
+			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.CHEST)
+				cct = t;
 		}
 		computedWaistTracker = cwt;
+		computedChestTracker = cct;
 		cwt.setStatus(TrackerStatus.OK);
 		headShift = server.config.getFloat("body.headShift", headShift);
 		neckLength = server.config.getFloat("body.neckLength", neckLength);
@@ -205,9 +210,17 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	}
 	
 	protected void updateComputedTrackers() {
-		computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation());
-		computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation());
-		computedWaistTracker.dataTick();
+		if(computedWaistTracker != null) {
+			computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation());
+			computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation());
+			computedWaistTracker.dataTick();
+		}
+		
+		if(computedChestTracker != null) {
+			computedChestTracker.position.set(chestNode.worldTransform.getTranslation());
+			computedChestTracker.rotation.set(neckNode.worldTransform.getRotation());
+			computedChestTracker.dataTick();
+		}
 	}
 	
 	@Override
@@ -223,4 +236,18 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		
 		this.waistTracker.resetFull(referenceRotation);
 	}
+	
+	@Override
+	@VRServerThread
+	public void resetTrackersYaw() {
+		// Each tracker uses the tracker before it to adjust iteself,
+		// so trackers that don't need adjustments could be used too
+		Quaternion referenceRotation = new Quaternion();
+		server.hmdTracker.getRotation(referenceRotation);
+		
+		this.chestTracker.resetYaw(referenceRotation);
+		this.chestTracker.getRotation(referenceRotation);
+		
+		this.waistTracker.resetYaw(referenceRotation);
+	}
 }
--- a/src/main/java/io/eiren/vr/trackers/BnoTap.java
+++ b/src/main/java/io/eiren/vr/trackers/BnoTap.java
@@ -0,0 +1,21 @@
+package io.eiren.vr.trackers;
+
+public class BnoTap {
+	
+	public final boolean doubleTap;
+	
+	public BnoTap(int tapBits) {
+		doubleTap = (tapBits & 0x40) > 0;
+	}
+	
+	@Override
+	public String toString() {
+		return "Tap{" + (doubleTap ? "double" : "") + "}";
+	}
+	
+	public enum TapAxis {
+		X,
+		Y,
+		Z;
+	}
+}
--- a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java
+++ b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java
@@ -80,4 +80,8 @@ public class ComputedTracker implements Tracker {
 	public boolean userEditable() {
 		return false;
 	}
+
+	@Override
+	public void tick() {
+	}
 }
--- a/src/main/java/io/eiren/vr/trackers/IMUReferenceAdjustedTracker.java
+++ b/src/main/java/io/eiren/vr/trackers/IMUReferenceAdjustedTracker.java
@@ -1,29 +0,0 @@
-package io.eiren.vr.trackers;
-
-public class IMUReferenceAdjustedTracker<T extends IMUTracker & TrackerWithTPS & TrackerWithBattery> extends ReferenceAdjustedTracker<T> implements TrackerWithTPS, TrackerWithBattery {
-	
-	public IMUReferenceAdjustedTracker(T tracker) {
-		super(tracker);
-	}
-
-	@Override
-	public float getBatteryLevel() {
-		return tracker.getBatteryLevel();
-	}
-
-	@Override
-	public float getBatteryVoltage() {
-		return tracker.getBatteryVoltage();
-	}
-
-	@Override
-	public float getTPS() {
-		return tracker.getTPS();
-	}
-
-	@Override
-	public void dataTick() {
-		tracker.dataTick();
-	}
-	
-}
--- a/src/main/java/io/eiren/vr/trackers/IMUTracker.java
+++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java
@@ -1,5 +1,6 @@
 package io.eiren.vr.trackers;

+import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;

@@ -9,11 +10,15 @@ import io.eiren.vr.processor.TrackerBodyPosition;

 public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	
+	public static final float MAX_MAG_CORRECTION_ACCURACY = 5 * FastMath.RAD_TO_DEG;
+	
 	public final Vector3f gyroVector = new Vector3f();
 	public final Vector3f accelVector = new Vector3f();
 	public final Vector3f magVector = new Vector3f();
 	public final Quaternion rotQuaternion = new Quaternion();
+	public final Quaternion rotMagQuaternion = new Quaternion();
 	protected final Quaternion rotAdjust = new Quaternion();
+	protected final Quaternion correction = new Quaternion();
 	protected TrackerMountingRotation mounting = null;
 	protected TrackerStatus status = TrackerStatus.OK;
 	
@@ -21,6 +26,11 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	protected final TrackersUDPServer server;
 	protected float confidence = 0;
 	protected float batteryVoltage = 0;
+	public int calibrationStatus = 0;
+	public int magCalibrationStatus = 0;
+	public float magnetometerAccuracy = 0;
+	protected boolean magentometerCalibrated = false;
+	public boolean hasNewCorrectionData = false;
 	
 	protected BufferedTimer timer = new BufferedTimer(1f);
 	public int ping = -1;
@@ -68,6 +78,18 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 		}
 	}
 	
+	@Override
+	public void tick() {
+		if(magentometerCalibrated && hasNewCorrectionData) {
+			hasNewCorrectionData = false;
+			if(magnetometerAccuracy <= MAX_MAG_CORRECTION_ACCURACY) {
+				// Adjust gyro rotation to match magnetometer rotation only if magnetometer
+				// accuracy is within the parameters
+				calculateLiveMagnetometerCorrection();
+			}
+		}
+	}
+	
 	@Override
 	public String getName() {
 		return this.name;
@@ -82,9 +104,14 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	@Override
 	public boolean getRotation(Quaternion store) {
 		store.set(rotQuaternion);
+		//correction.mult(store, store); // Correction is not used now to preven accidental errors while debugging other things
 		store.multLocal(rotAdjust);
 		return true;
 	}
+	
+	public void getCorrection(Quaternion store) {
+		store.set(correction);
+	}

 	@Override
 	public TrackerStatus getStatus() {
@@ -130,10 +157,31 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {

 	@Override
 	public void resetFull(Quaternion reference) {
+		resetYaw(reference);
 	}

+	/**
+	 * Does not perform actual gyro reset to reference, that's the task of
+	 * reference adjusted tracker. Only aligns gyro with magnetometer if
+	 * it's reliable
+	 */
 	@Override
 	public void resetYaw(Quaternion reference) {
+		if(magCalibrationStatus >= CalibrationAccuracy.HIGH.status) {
+			magentometerCalibrated = true;
+			// During calibration set correction to match magnetometer readings exactly
+			// TODO : Correct only yaw
+			correction.set(rotQuaternion).inverseLocal().multLocal(rotMagQuaternion);
+		}
+	}
+	
+	/**
+	 * Calculate correction between normal and magnetometer
+	 * readings up to accuracy threshold
+	 */
+	protected void calculateLiveMagnetometerCorrection() {
+		// TODO Magic, correct only yaw
+		// TODO Print "jump" length when correcing if it's more than 1 degree
 	}

 	@Override
@@ -150,4 +198,31 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	public boolean userEditable() {
 		return true;
 	}
+	
+	public enum CalibrationAccuracy {
+		
+		UNRELIABLE(0),
+		LOW(1),
+		MEDIUM(2),
+		HIGH(3),
+		;
+		
+		private static final CalibrationAccuracy[] byStatus = new CalibrationAccuracy[4];
+		public final int status;
+		
+		private CalibrationAccuracy(int status) {
+			this.status = status;
+		}
+		
+		public static CalibrationAccuracy getByStatus(int status) {
+			if(status < 0 || status > 3)
+				return null;
+			return byStatus[status];
+		}
+		
+		static {
+			for(CalibrationAccuracy ca : values())
+				byStatus[ca.status] = ca;
+		}
+	}
 }
--- a/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java
+++ b/src/main/java/io/eiren/vr/trackers/ReferenceAdjustedTracker.java
@@ -8,8 +8,9 @@ import io.eiren.vr.processor.TrackerBodyPosition;
 public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	
 	public final E tracker;
-	public final Quaternion adjustmentYaw = new Quaternion();
-	public final Quaternion adjustmentAttachment = new Quaternion();
+	public final Quaternion yawFix = new Quaternion();
+	public final Quaternion gyroFix = new Quaternion();
+	public final Quaternion attachmentFix = new Quaternion();
 	protected float confidenceMultiplier = 1.0f;
 	
 	public ReferenceAdjustedTracker(E tracker) {
@@ -44,14 +45,15 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetFull(Quaternion reference) {
+		tracker.resetFull(reference);
+		fixGyroscope();
+		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
-		//float[] angles = new float[3];
-		//sensorRotation.toAngles(angles);
-		//sensorRotation.fromAngles(angles[0], 0, angles[2]);
-		adjustmentAttachment.set(sensorRotation).inverseLocal();
+		gyroFix.mult(sensorRotation, sensorRotation);
+		attachmentFix.set(sensorRotation).inverseLocal();
 		
-		resetYaw(reference);
+		fixYaw(reference);
 	}

 	/**
@@ -63,6 +65,11 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetYaw(Quaternion reference) {
+		tracker.resetYaw(reference);
+		fixYaw(reference);
+	}
+	
+	private void fixYaw(Quaternion reference) {
 		// Use only yaw HMD rotation
 		Quaternion targetTrackerRotation = new Quaternion(reference);
 		float[] angles = new float[3];
@@ -71,21 +78,31 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
-		adjustmentAttachment.mult(sensorRotation, sensorRotation);
-		//sensorRotation.multLocal(adjustmentAttachment);
+		gyroFix.mult(sensorRotation, sensorRotation);
+		sensorRotation.multLocal(attachmentFix);
 		
 		sensorRotation.toAngles(angles);
 		sensorRotation.fromAngles(0, angles[1], 0);
 		
-		adjustmentYaw.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+		yawFix.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+	}
+	
+	private void fixGyroscope() {
+		float[] angles = new float[3];
 		
-		confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
+		Quaternion sensorRotation = new Quaternion();
+		tracker.getRotation(sensorRotation);
+		
+		sensorRotation.toAngles(angles);
+		sensorRotation.fromAngles(0, angles[1], 0);
+		
+		gyroFix.set(sensorRotation).inverseLocal();
 	}
 	
 	protected void adjustInternal(Quaternion store) {
-		//store.multLocal(adjustmentAttachment);
-		adjustmentAttachment.mult(store, store);
-		adjustmentYaw.mult(store, store);
+		gyroFix.mult(store, store);
+		store.multLocal(attachmentFix);
+		yawFix.mult(store, store);
 	}
 	
 	@Override
@@ -124,4 +141,9 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	public void setBodyPosition(TrackerBodyPosition position) {
 		tracker.setBodyPosition(position);
 	}
+
+	@Override
+	public void tick() {
+		tracker.tick();
+	}
 }
--- a/src/main/java/io/eiren/vr/trackers/Tracker.java
+++ b/src/main/java/io/eiren/vr/trackers/Tracker.java
@@ -25,6 +25,8 @@ public interface Tracker {
 	
 	public void resetYaw(Quaternion reference);
 	
+	public void tick();
+	
 	public TrackerBodyPosition getBodyPosition();
 	
 	public void setBodyPosition(TrackerBodyPosition position);
--- a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java
+++ b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java
@@ -65,12 +65,16 @@ public class TrackersUDPServer extends Thread {
 			int boardType = -1;
 			int imuType = -1;
 			int firmwareBuild = -1;
-			StringBuilder sb = new StringBuilder();
+			StringBuilder firmware = new StringBuilder();
+			byte[] mac = new byte[6];
+			String macString = null;
 			if(data.remaining() > 0) {
 				if(data.remaining() > 3)
 					boardType = data.getInt();
 				if(data.remaining() > 3)
 					imuType = data.getInt();
+				if(data.remaining() > 3)
+					data.getInt(); // MCU TYPE
 				if(data.remaining() > 11) {
 					data.getInt(); // IMU info
 					data.getInt();
@@ -78,19 +82,25 @@ public class TrackersUDPServer extends Thread {
 				}
 				if(data.remaining() > 3)
 					firmwareBuild = data.getInt();
-				while(true) {
-					if(data.remaining() == 0)
-						break;
+				int length = 0;
+				if(data.remaining() > 0)
+					length = data.get() & 0xFF; // firmware version length is 1 longer than that because it's nul-terminated
+				while(length > 0 && data.remaining() != 0) {
 					char c = (char) data.get();
 					if(c == 0)
 						break;
-					sb.append(c);
+					firmware.append(c);
+					length--;
+				}
+				if(data.remaining() > mac.length) {
+					data.get(mac);
+					macString = String.format("%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
 				}
 			}
-			if(sb.length() == 0)
-				sb.append("owoTrack");
+			if(firmware.length() == 0)
+				firmware.append("owoTrack");
 			IMUTracker imu = new IMUTracker("udp:/" + handshakePacket.getAddress().toString(), this);
-			IMUReferenceAdjustedTracker<IMUTracker> adjustedTracker = new IMUReferenceAdjustedTracker<>(imu);
+			ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
 			trackersConsumer.accept(adjustedTracker);
 			sensor = new TrackerConnection(imu, addr);
 			int i = 0;
@@ -99,7 +109,7 @@ public class TrackersUDPServer extends Thread {
 				trackers.add(sensor);
 				trackersMap.put(addr, sensor);
 			}
-			System.out.println("[TrackerServer] Sensor " + i + " added with address " + addr + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + sb + " (" + firmwareBuild + ")");
+			System.out.println("[TrackerServer] Sensor " + i + " added with address " + addr + ". Board type: " + boardType + ", imu type: " + imuType + ", firmware: " + firmware + " (" + firmwareBuild + "), mac: " + macString);
 		}
 		sensor.tracker.setStatus(TrackerStatus.OK);
        socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort()));
@@ -109,7 +119,7 @@ public class TrackersUDPServer extends Thread {
 		System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.tracker.getName());
 		IMUTracker imu = new IMUTracker(connection.tracker.getName() + "/1", this);
 		connection.secondTracker = imu;
-		IMUReferenceAdjustedTracker<IMUTracker> adjustedTracker = new IMUReferenceAdjustedTracker<>(imu);
+		ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
 		trackersConsumer.accept(adjustedTracker);
 		System.out.println("[TrackerServer] Sensor added with address " + imu.getName());
 	}
@@ -129,12 +139,13 @@ public class TrackersUDPServer extends Thread {
 					socket.receive(recieve);
 					bb.rewind();

-					TrackerConnection sensor;
+					TrackerConnection connection;
+					IMUTracker tracker = null;
 					synchronized(trackers) {
-						sensor = trackersMap.get(recieve.getSocketAddress());
+						connection = trackersMap.get(recieve.getSocketAddress());
 					}
-					if(sensor != null)
-						sensor.lastPacket = System.currentTimeMillis();
+					if(connection != null)
+						connection.lastPacket = System.currentTimeMillis();
 					int packetId;
 					switch(packetId = bb.getInt()) {
 					case 0:
@@ -144,90 +155,136 @@ public class TrackersUDPServer extends Thread {
 						break;
 					case 1: // PACKET_ROTATION
 					case 16: // PACKET_ROTATION_2
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat());
 						offset.mult(buf, buf);
-						IMUTracker tracker;
 						if(packetId == 1) {
-							tracker = sensor.tracker;
+							tracker = connection.tracker;
 						} else {
-							tracker = sensor.secondTracker;
+							tracker = connection.secondTracker;
 						}
 						if(tracker == null)
 							break;
 						tracker.rotQuaternion.set(buf);
 						tracker.dataTick();
 						break;
-					case 2:
-						if(sensor == null)
+					case 17: // PACKET_ROTATION_DATA
+						if(connection == null)
 							break;
 						bb.getLong();
-						sensor.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
+						int sensorId = bb.get() & 0xFF;
+						if(sensorId == 0) {
+							tracker = connection.tracker;
+						} else if(sensorId == 1) {
+							tracker = connection.secondTracker;
+						}
+						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;
+						bb.getLong();
+						sensorId = bb.get() & 0xFF;
+						if(sensorId == 0) {
+							tracker = connection.tracker;
+						} else if(sensorId == 1) {
+							tracker = connection.secondTracker;
+						}
+						if(tracker == null)
+							break;
+						float accuracyInfo = bb.getFloat();
+						tracker.magnetometerAccuracy = accuracyInfo;
+						// TODO
+						break;
+					case 2:
+						if(connection == null)
+							break;
+						bb.getLong();
+						connection.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat());
 						break;
 					case 4:
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						float x = bb.getFloat();
 						float z = bb.getFloat();
 						float y = bb.getFloat();
-						sensor.tracker.accelVector.set(x, y, z);
+						connection.tracker.accelVector.set(x, y, z);
 						break;
 					case 5:
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						x = bb.getFloat();
 						z = bb.getFloat();
 						y = bb.getFloat();
-						sensor.tracker.magVector.set(x, y, z);
+						connection.tracker.magVector.set(x, y, z);
 						break;
 					case 6: // PACKET_RAW_CALIBRATION_DATA
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						//sensor.rawCalibrationData.add(new double[] {bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt()});
 						break;
 					case 7: // PACKET_GYRO_CALIBRATION_DATA
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						//sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()};
 						break;
 					case 8: // PACKET_CONFIG
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb);
-						Consumer<String> dataConsumer = calibrationDataRequests.remove(sensor.tracker);
+						Consumer<String> dataConsumer = calibrationDataRequests.remove(connection.tracker);
 						if(dataConsumer != null) {
 							dataConsumer.accept(data.toTextMatrix());
 						}
 						break;
 					case 9: // PACKET_RAW_MAGENTOMETER
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
 						float mx = bb.getFloat();
 						float my = bb.getFloat();
 						float mz = bb.getFloat();
-						sensor.tracker.confidence = (float) Math.sqrt(mx * mx + my * my + mz * mz);
+						connection.tracker.confidence = (float) Math.sqrt(mx * mx + my * my + mz * mz);
 						break;
 					case 10: // PACKET_PING_PONG:
-						if(sensor == null)
+						if(connection == null)
 							break;
 						int pingId = bb.getInt();
-						if(sensor.lastPingPacketId == pingId) {
-							tracker = sensor.tracker;
-							tracker.ping = (int) (System.currentTimeMillis() - sensor.lastPingPacketTime) / 2;
+						if(connection.lastPingPacketId == pingId) {
+							tracker = connection.tracker;
+							tracker.ping = (int) (System.currentTimeMillis() - connection.lastPingPacketTime) / 2;
 						}
 						break;
 					case 11: // PACKET_SERIAL
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.tracker;
 						bb.getLong();
 						int length = bb.getInt();
 						for(int i = 0; i < length; ++i) {
@@ -243,44 +300,51 @@ public class TrackersUDPServer extends Thread {
 						}
 						break;
 					case 12: // PACKET_BATTERY_VOLTAGE
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.tracker;
 						bb.getLong();
 						tracker.setBatteryVoltage(bb.getFloat());
 						break;
 					case 13: // PACKET_TAP
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.tracker;
 						bb.getLong();
-						byte tap = bb.get();
-						System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + ": b" + Integer.toBinaryString(tap));
+						sensorId = bb.get() & 0xFF;
+						if(sensorId == 0) {
+							tracker = connection.tracker;
+						} else if(sensorId == 1) {
+							tracker = connection.secondTracker;
+						}
+						int tap = bb.get() & 0xFF;
+						BnoTap tapObj = new BnoTap(tap);
+						System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + "/" + sensorId + ": " + tapObj  + " (b" + Integer.toBinaryString(tap) + ")");
 						break;
 					case 14: // PACKET_RESET_REASON
 						bb.getLong();
 						byte reason = bb.get();
 						System.out.println("[TrackerServer] Reset recieved from " + recieve.getSocketAddress() + ": " + reason);
-						if(sensor == null)
+						if(connection == null)
 							break;
-						tracker = sensor.tracker;
+						tracker = connection.tracker;
 						tracker.setStatus(TrackerStatus.ERROR);
 						break;
 					case 15: // PACKET_SENSOR_INFO
-						if(sensor == null)
+						if(connection == null)
 							break;
 						bb.getLong();
-						int sensorId = bb.get() & 0xFF;
+						sensorId = bb.get() & 0xFF;
 						int sensorStatus = bb.get() & 0xFF;
-						if(sensorId == 1 && sensorStatus == 1 && sensor.secondTracker == null) {
-							setUpAuxialrySensor(sensor);
+						if(sensorId == 1 && sensorStatus == 1 && connection.secondTracker == null) {
+							setUpAuxialrySensor(connection);
 						}
 						bb.rewind();
 						bb.putInt(15);
 						bb.put((byte) sensorId);
 						bb.put((byte) sensorStatus);
-						socket.send(new DatagramPacket(rcvBuffer, bb.position(), sensor.address));
-						System.out.println("[TrackerServer] Sensor info for " + sensor.tracker.getName() + "/" + sensorId + ": " + sensorStatus);
+						socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
+						System.out.println("[TrackerServer] Sensor info for " + connection.tracker.getName() + "/" + sensorId + ": " + sensorStatus);
 						break;
 					default:
 						System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
--- a/src/test/java/io/eiren/unit/ReferenceAdjustmentsFullTests.java
+++ b/src/test/java/io/eiren/unit/ReferenceAdjustmentsFullTests.java
@@ -1,273 +0,0 @@
-package io.eiren.unit;
-
-import com.jme3.math.FastMath;
-import com.jme3.math.Quaternion;
-
-import io.eiren.math.FloatMath;
-import io.eiren.util.StringUtils;
-import io.eiren.vr.processor.TransformNode;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.ReferenceAdjustedTracker;
-
-import static org.junit.Assert.*;
-
-import java.util.HashSet;
-import java.util.Set;
-
-import org.junit.Test;
-
-/**
- * Tests {@link ReferenceAdjustedTracker#resetFull(Quaternion)}
- */
-public class ReferenceAdjustmentsFullTests {
-	
-	private Set<String> testedTrackerNames = new HashSet<>();
-
-	@Test
-	public void check0to0() {
-		yawTest(0, 0);
-	}
-
-	@Test
-	public void check45to0() {
-		yawTest(0, 45);
-	}
-
-	@Test
-	public void check90to0() {
-		yawTest(0, 90);
-	}
-
-	@Test
-	public void check180to0() {
-		yawTest(0, 180);
-	}
-
-	@Test
-	public void check270to0() {
-		yawTest(0, 270);
-	}
-	
-	@Test
-	public void check0to45() {
-		yawTest(45, 0);
-	}
-
-	@Test
-	public void check45to45() {
-		yawTest(45, 45);
-	}
-
-	@Test
-	public void check90to45() {
-		yawTest(45, 90);
-	}
-
-	@Test
-	public void check180to45() {
-		yawTest(45, 180);
-	}
-
-	@Test
-	public void check270to45() {
-		yawTest(45, 270);
-	}
-	
-	@Test
-	public void check0to90() {
-		yawTest(90, 0);
-	}
-
-	@Test
-	public void check45to90() {
-		yawTest(90, 45);
-	}
-
-	@Test
-	public void check90to90() {
-		yawTest(90, 90);
-	}
-
-	@Test
-	public void check180to90() {
-		yawTest(90, 180);
-	}
-
-	@Test
-	public void check270to90() {
-		yawTest(90, 270);
-	}
-	
-	@Test
-	public void check0to180() {
-		yawTest(180, 0);
-	}
-
-	@Test
-	public void check45to180() {
-		yawTest(180, 45);
-	}
-
-	@Test
-	public void check90to180() {
-		yawTest(180, 90);
-	}
-
-	@Test
-	public void check180to180() {
-		yawTest(180, 180);
-	}
-
-	@Test
-	public void check270to180() {
-		yawTest(180, 270);
-	}
-	
-	private void yawTest(int refYaw, int trackerYaw) {
-		checkReferenceAdjustmentFull(q(0, refYaw, 0), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(15, refYaw, 0), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerYaw, 0), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(0, refYaw, 0), q(15, trackerYaw, 0), refYaw, "Tracker(15," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerYaw, 15), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(15, refYaw, 0), q(15, trackerYaw, 15), refYaw, "Tracker(15," + trackerYaw + ",0/" + refYaw + ")");
-		checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerYaw, 15), refYaw, "Tracker(0," + trackerYaw + ",0/" + refYaw + ")");
-	}
-	
-	public void checkReferenceAdjustmentFull(Quaternion referenceQuat, Quaternion trackerQuat, int refYaw, String name) {
-		ComputedTracker tracker = new ComputedTracker("test");
-		tracker.rotation.set(trackerQuat);
-		ReferenceAdjustedTracker<ComputedTracker> adj = new ReferenceAdjustedTracker<>(tracker);
-		adj.resetFull(referenceQuat);
-		Quaternion read = new Quaternion();
-		assertTrue("Adjusted tracker didn't return rotation", adj.getRotation(read));
-
-		// Use only yaw HMD rotation
-		Quaternion targetTrackerRotation = new Quaternion(referenceQuat);
-		float[] angles = new float[3];
-		targetTrackerRotation.toAngles(angles);
-		targetTrackerRotation.fromAngles(0, angles[1], 0);
-		
-		assertEquals("Adjusted quat is not equal to reference quat (" + toDegs(targetTrackerRotation) + " vs " + toDegs(read) + ")", new QuatEqualFullWithEpsilon(targetTrackerRotation), new QuatEqualFullWithEpsilon(read));
-		testAdjustedTracker(tracker, adj, name, refYaw);
-	}
-	
-	private static final boolean PRINT_TEST_RESULTS = false;
-	
-	private static int errors = 0;
-	private static int successes = 0;
-	
-	private void testAdjustedTracker(ComputedTracker tracker, ReferenceAdjustedTracker<ComputedTracker> adj, String name, int refYaw) {
-		if(!testedTrackerNames.add(name))
-			return;
-		
-		final Quaternion trackerBase = new Quaternion();
-		trackerBase.set(tracker.rotation);
-		
-		Quaternion rotation = new Quaternion();
-		Quaternion rotationCompare = new Quaternion();
-		Quaternion read = new Quaternion();
-		Quaternion diff = new Quaternion();
-		float[] angles = new float[3];
-		float[] anglesAdj = new float[3];
-		float[] anglesDiff = new float[3];
-		
-		TransformNode trackerNode = new TransformNode(name, true);
-		TransformNode rotationNode = new TransformNode("Rot", true);
-		trackerNode.attachChild(rotationNode);
-		
-		trackerNode.localTransform.setRotation(trackerBase);
-		
-		for(int yaw = 0; yaw <= 360; yaw += 30) {
-			for(int pitch = -90; pitch <= 90; pitch += 15) {
-				for(int roll = -90; roll <= 90; roll += 15) {
-					rotation.fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-					rotationCompare.fromAngles(pitch * FastMath.DEG_TO_RAD, (yaw + refYaw) * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-					rotationNode.localTransform.setRotation(rotation);
-					trackerNode.update();
-					rotationNode.update();
-					tracker.rotation.set(rotationNode.worldTransform.getRotation());
-					tracker.rotation.toAngles(angles);
-					
-					adj.getRotation(read);
-					read.toAngles(anglesAdj);
-					
-					diff.set(read).inverseLocal().multLocal(rotationCompare);
-					diff.toAngles(anglesDiff);
-					
-					if(!PRINT_TEST_RESULTS) {
-						assertTrue(name(name, yaw, pitch, roll, angles, anglesAdj, anglesDiff),
-								FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]));
-					} else {
-						if(FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]))
-							successes++;
-						else
-							errors++;
-						System.out.println(name(name, yaw, pitch, roll, angles, anglesAdj, anglesDiff));
-					}
-				}
-			}
-		}
-		if(PRINT_TEST_RESULTS)
-			System.out.println("Errors: " + errors + ", successes: " + successes);
-	}
-	
-	private static String name(String name, int yaw, int pitch, int roll, float[] angles, float[] anglesAdj, float[] anglesDiff) {
-		return name + ". Rot: " + yaw + "/" + pitch + ". "
-				+ "Angles: " + StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[0] * FastMath.RAD_TO_DEG, 1) + ", "
-				+ StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[1] * FastMath.RAD_TO_DEG, 1) + ", "
-				+ StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[2] * FastMath.RAD_TO_DEG, 1) + ". Diff: "
-				+ StringUtils.prettyNumber(anglesDiff[0] * FastMath.RAD_TO_DEG, 1) + ", "
-				+ StringUtils.prettyNumber(anglesDiff[1] * FastMath.RAD_TO_DEG, 1) + ", "
-				+ StringUtils.prettyNumber(anglesDiff[2] * FastMath.RAD_TO_DEG, 1);
-	}
-	
-	public static String toDegs(Quaternion q) {
-		float[] degs = new float[3];
-		q.toAngles(degs);
-		return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0);
-	}
-	
-	public static Quaternion q(float pitch, float yaw, float roll) {
-		return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-	}
-	
-	public static class QuatEqualFullWithEpsilon {
-		
-		private final Quaternion q;
-		
-		public QuatEqualFullWithEpsilon(Quaternion q) {
-			this.q = q;
-		}
-		
-		@Override
-		public String toString() {
-			return String.valueOf(q);
-		}
-		
-		@Override
-		public int hashCode() {
-			return q.hashCode();
-		}
-		
-		@Override
-		public boolean equals(Object obj) {
-			if(obj instanceof Quaternion)
-				obj = new QuatEqualFullWithEpsilon((Quaternion) obj);
-			if(!(obj instanceof QuatEqualFullWithEpsilon))
-				return false;
-			Quaternion q2 = ((QuatEqualFullWithEpsilon) obj).q;
-			float[] degs1 = new float[3];
-			q.toAngles(degs1);
-			float[] degs2 = new float[3];
-			q2.toAngles(degs2);
-			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs1[1] += FastMath.TWO_PI;
-			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs2[1] += FastMath.TWO_PI;
-			return FloatMath.equalsWithEpsilon(degs1[0], degs2[0])
-					&& FloatMath.equalsWithEpsilon(degs1[1], degs2[1])
-					&& FloatMath.equalsWithEpsilon(degs1[2], degs2[2]);
-		}
-	}
-}
--- a/src/test/java/io/eiren/unit/ReferenceAdjustmentsTests.java
+++ b/src/test/java/io/eiren/unit/ReferenceAdjustmentsTests.java
@@ -0,0 +1,265 @@
+package io.eiren.unit;
+
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+
+import io.eiren.math.FloatMath;
+import io.eiren.util.StringUtils;
+import io.eiren.vr.processor.TransformNode;
+import io.eiren.vr.trackers.ComputedTracker;
+import io.eiren.vr.trackers.ReferenceAdjustedTracker;
+
+import static org.junit.jupiter.api.Assertions.*;
+import static org.junit.jupiter.api.DynamicTest.dynamicTest;
+
+import java.util.function.Function;
+import java.util.stream.IntStream;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.TestFactory;
+
+/**
+ * Tests {@link ReferenceAdjustedTracker#resetFull(Quaternion)}
+ */
+public class ReferenceAdjustmentsTests {
+
+	private static final int[] yaws = {0, 45, 90, 180, 270};
+	private static final int[] pitches = {0, 15, 35, -15, -35};
+	private static final int[] rolls = {0, 15, 35, -15, -35};
+	private static final boolean PRINT_TEST_RESULTS = false;
+	private static int errors = 0;
+	private static int successes = 0;
+	
+	public static Stream<AnglesSet> getAnglesSet() {
+		return IntStream.of(yaws).mapToObj((yaw) ->
+				IntStream.of(pitches).mapToObj((pitch) ->
+					IntStream.of(rolls).mapToObj((roll) -> new AnglesSet(pitch, yaw, roll)
+			))).flatMap(Function.identity()).flatMap(Function.identity());
+	}
+	
+	@TestFactory
+	Stream<DynamicTest> getTestsYaw() {
+		return getAnglesSet().map((p) -> 
+			dynamicTest("Adjustment Yaw Test of Tracker(" + p.pitch + "," + p.yaw + "," + p.roll + ")",
+					() -> IntStream.of(yaws).forEach((refYaw) ->
+							checkReferenceAdjustmentYaw(q(p.pitch, p.yaw, p.roll), 0, refYaw, 0))
+					));
+	}
+	
+	@TestFactory
+	Stream<DynamicTest> getTestsFull() {
+		return getAnglesSet().map((p) -> 
+		dynamicTest("Adjustment Full Test of Tracker(" + p.pitch + "," + p.yaw + "," + p.roll + ")",
+				() -> getAnglesSet().forEach((ref) ->
+						checkReferenceAdjustmentFull(q(p.pitch, p.yaw, p.roll), ref.pitch, ref.yaw, ref.roll))
+				));
+	}
+	
+	@TestFactory
+	Stream<DynamicTest> getTestsForRotation() {
+		return getAnglesSet().map((p) -> 
+			IntStream.of(yaws).mapToObj((refYaw) ->
+				dynamicTest("Adjustment Rotation Test of Tracker(" + p.pitch + "," + p.yaw + "," + p.roll + "), Ref " + refYaw,
+					() -> testAdjustedTrackerRotation(q(p.pitch, p.yaw, p.roll), 0, refYaw, 0)
+			))).flatMap(Function.identity());
+	}
+	
+	public void checkReferenceAdjustmentFull(Quaternion trackerQuat, int refPitch, int refYaw, int refRoll) {
+		Quaternion referenceQuat = q(refPitch, refYaw, refRoll);
+		ComputedTracker tracker = new ComputedTracker("test");
+		tracker.rotation.set(trackerQuat);
+		ReferenceAdjustedTracker<ComputedTracker> adj = new ReferenceAdjustedTracker<>(tracker);
+		adj.resetFull(referenceQuat);
+		Quaternion read = new Quaternion();
+		assertTrue(adj.getRotation(read), "Adjusted tracker didn't return rotation");
+
+		// Use only yaw HMD rotation
+		Quaternion targetTrackerRotation = new Quaternion(referenceQuat);
+		float[] angles = new float[3];
+		targetTrackerRotation.toAngles(angles);
+		targetTrackerRotation.fromAngles(0, angles[1], 0);
+		
+		assertEquals(new QuatEqualFullWithEpsilon(read), new QuatEqualFullWithEpsilon(targetTrackerRotation),
+				"Adjusted quat is not equal to reference quat (" + toDegs(targetTrackerRotation) + " vs " + toDegs(read) + ")");
+	}
+	
+	public void checkReferenceAdjustmentYaw(Quaternion trackerQuat, int refPitch, int refYaw, int refRoll) {
+		Quaternion referenceQuat = q(refPitch, refYaw, refRoll);
+		ComputedTracker tracker = new ComputedTracker("test");
+		tracker.rotation.set(trackerQuat);
+		ReferenceAdjustedTracker<ComputedTracker> adj = new ReferenceAdjustedTracker<>(tracker);
+		adj.resetYaw(referenceQuat);
+		Quaternion read = new Quaternion();
+		assertTrue(adj.getRotation(read), "Adjusted tracker didn't return rotation");
+		assertEquals(new QuatEqualYawWithEpsilon(referenceQuat), new QuatEqualYawWithEpsilon(read),
+				"Adjusted quat is not equal to reference quat (" + toDegs(referenceQuat) + " vs " + toDegs(read) + ")");
+	}
+	
+	private void testAdjustedTrackerRotation(Quaternion trackerQuat, int refPitch, int refYaw, int refRoll) {
+		Quaternion referenceQuat = q(refPitch, refYaw, refRoll);
+		ComputedTracker tracker = new ComputedTracker("test");
+		tracker.rotation.set(trackerQuat);
+		ReferenceAdjustedTracker<ComputedTracker> adj = new ReferenceAdjustedTracker<>(tracker);
+		adj.resetFull(referenceQuat);
+
+		// Use only yaw HMD rotation
+		Quaternion targetTrackerRotation = new Quaternion(referenceQuat);
+		float[] angles = new float[3];
+		targetTrackerRotation.toAngles(angles);
+		targetTrackerRotation.fromAngles(0, angles[1], 0);
+
+		Quaternion read = new Quaternion();
+		Quaternion rotation = new Quaternion();
+		Quaternion rotationCompare = new Quaternion();
+		Quaternion diff = new Quaternion();
+		float[] anglesAdj = new float[3];
+		float[] anglesDiff = new float[3];
+		
+		TransformNode trackerNode = new TransformNode("Tracker", true);
+		TransformNode rotationNode = new TransformNode("Rot", true);
+		rotationNode.attachChild(trackerNode);
+		
+		trackerNode.localTransform.setRotation(tracker.rotation);
+		
+		for(int yaw = 0; yaw <= 360; yaw += 30) {
+			for(int pitch = -90; pitch <= 90; pitch += 15) {
+				for(int roll = -90; roll <= 90; roll += 15) {
+					rotation.fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
+					rotationCompare.fromAngles(pitch * FastMath.DEG_TO_RAD, (yaw + refYaw) * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
+					rotationNode.localTransform.setRotation(rotation);
+					rotationNode.update();
+					tracker.rotation.set(trackerNode.worldTransform.getRotation());
+					tracker.rotation.toAngles(angles);
+					
+					adj.getRotation(read);
+					read.toAngles(anglesAdj);
+					
+					diff.set(read).inverseLocal().multLocal(rotationCompare);
+					diff.toAngles(anglesDiff);
+					
+					if(!PRINT_TEST_RESULTS) {
+						assertTrue(FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]),
+								name(yaw, pitch, roll, angles, anglesAdj, anglesDiff));
+					} else {
+						if(FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]))
+							successes++;
+						else
+							errors++;
+						System.out.println(name(yaw, pitch, roll, angles, anglesAdj, anglesDiff));
+					}
+				}
+			}
+		}
+		if(PRINT_TEST_RESULTS)
+			System.out.println("Errors: " + errors + ", successes: " + successes);
+	}
+	
+	private static String name(int yaw, int pitch, int roll, float[] angles, float[] anglesAdj, float[] anglesDiff) {
+		return "Rot: " + yaw + "/" + pitch + "/" + roll + ". "
+				+ "Angles: " + StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[0] * FastMath.RAD_TO_DEG, 1) + ", "
+				+ StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[1] * FastMath.RAD_TO_DEG, 1) + ", "
+				+ StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 1) + "/" + StringUtils.prettyNumber(anglesAdj[2] * FastMath.RAD_TO_DEG, 1) + ". Diff: "
+				+ StringUtils.prettyNumber(anglesDiff[0] * FastMath.RAD_TO_DEG, 1) + ", "
+				+ StringUtils.prettyNumber(anglesDiff[1] * FastMath.RAD_TO_DEG, 1) + ", "
+				+ StringUtils.prettyNumber(anglesDiff[2] * FastMath.RAD_TO_DEG, 1);
+	}
+	
+	public static Quaternion q(float pitch, float yaw, float roll) {
+		return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
+	}
+	
+	public static String toDegs(Quaternion q) {
+		float[] degs = new float[3];
+		q.toAngles(degs);
+		return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0);
+	}
+	
+	private static class QuatEqualYawWithEpsilon {
+		
+		private final Quaternion q;
+		
+		public QuatEqualYawWithEpsilon(Quaternion q) {
+			this.q = q;
+		}
+		
+		@Override
+		public String toString() {
+			return String.valueOf(q);
+		}
+		
+		@Override
+		public int hashCode() {
+			return q.hashCode();
+		}
+		
+		@Override
+		public boolean equals(Object obj) {
+			if(obj instanceof Quaternion)
+				obj = new QuatEqualYawWithEpsilon((Quaternion) obj);
+			if(!(obj instanceof QuatEqualYawWithEpsilon))
+				return false;
+			Quaternion q2 = ((QuatEqualYawWithEpsilon) obj).q;
+			float[] degs1 = new float[3];
+			q.toAngles(degs1);
+			float[] degs2 = new float[3];
+			q2.toAngles(degs2);
+			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
+				degs1[1] += FastMath.TWO_PI;
+			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
+				degs2[1] += FastMath.TWO_PI;
+			return FloatMath.equalsWithEpsilon(degs1[1], degs2[1]);
+		}
+	}
+	
+	public static class QuatEqualFullWithEpsilon {
+		
+		private final Quaternion q;
+		
+		public QuatEqualFullWithEpsilon(Quaternion q) {
+			this.q = q;
+		}
+		
+		@Override
+		public String toString() {
+			return String.valueOf(q);
+		}
+		
+		@Override
+		public int hashCode() {
+			return q.hashCode();
+		}
+		
+		@Override
+		public boolean equals(Object obj) {
+			if(obj instanceof Quaternion)
+				obj = new QuatEqualFullWithEpsilon((Quaternion) obj);
+			if(!(obj instanceof QuatEqualFullWithEpsilon))
+				return false;
+			Quaternion q2 = ((QuatEqualFullWithEpsilon) obj).q;
+			float[] degs1 = new float[3];
+			q.toAngles(degs1);
+			float[] degs2 = new float[3];
+			q2.toAngles(degs2);
+			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
+				degs1[1] += FastMath.TWO_PI;
+			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
+				degs2[1] += FastMath.TWO_PI;
+			return FloatMath.equalsWithEpsilon(degs1[0], degs2[0])
+					&& FloatMath.equalsWithEpsilon(degs1[1], degs2[1])
+					&& FloatMath.equalsWithEpsilon(degs1[2], degs2[2]);
+		}
+	}
+	
+	public static class AnglesSet {
+		public final int pitch;
+		public final int yaw;
+		public final int roll;
+		
+		public AnglesSet(int pitch, int yaw, int roll) {
+			this.pitch = pitch;
+			this.yaw = yaw;
+			this.roll = roll;
+		}
+	}
+}
--- a/src/test/java/io/eiren/unit/ReferenceAdjustmentsYawTests.java
+++ b/src/test/java/io/eiren/unit/ReferenceAdjustmentsYawTests.java
@@ -1,187 +0,0 @@
-package io.eiren.unit;
-
-import com.jme3.math.FastMath;
-import com.jme3.math.Quaternion;
-
-import io.eiren.math.FloatMath;
-import io.eiren.util.StringUtils;
-import io.eiren.vr.trackers.ComputedTracker;
-import io.eiren.vr.trackers.ReferenceAdjustedTracker;
-
-import static org.junit.Assert.*;
-
-import org.junit.Test;
-
-/**
- * Tests {@link ReferenceAdjustedTracker#resetYaw(Quaternion)}
- */
-public class ReferenceAdjustmentsYawTests {
-	
-	@Test
-	public void check0to0() {
-		yawTest(0, 0);
-	}
-
-	@Test
-	public void check45to0() {
-		yawTest(0, 45);
-	}
-
-	@Test
-	public void check90to0() {
-		yawTest(0, 90);
-	}
-
-	@Test
-	public void check180to0() {
-		yawTest(0, 180);
-	}
-
-	@Test
-	public void check270to0() {
-		yawTest(0, 270);
-	}
-	
-	@Test
-	public void check0to45() {
-		yawTest(45, 0);
-	}
-
-	@Test
-	public void check45to45() {
-		yawTest(45, 45);
-	}
-
-	@Test
-	public void check90to45() {
-		yawTest(45, 90);
-	}
-
-	@Test
-	public void check180to45() {
-		yawTest(45, 180);
-	}
-
-	@Test
-	public void check270to45() {
-		yawTest(45, 270);
-	}
-	
-	@Test
-	public void check0to90() {
-		yawTest(90, 0);
-	}
-
-	@Test
-	public void check45to90() {
-		yawTest(90, 45);
-	}
-
-	@Test
-	public void check90to90() {
-		yawTest(90, 90);
-	}
-
-	@Test
-	public void check180to90() {
-		yawTest(90, 180);
-	}
-
-	@Test
-	public void check270to90() {
-		yawTest(90, 270);
-	}
-	
-	@Test
-	public void check0to180() {
-		yawTest(180, 0);
-	}
-
-	@Test
-	public void check45to180() {
-		yawTest(180, 45);
-	}
-
-	@Test
-	public void check90to180() {
-		yawTest(180, 90);
-	}
-
-	@Test
-	public void check180to180() {
-		yawTest(180, 180);
-	}
-
-	@Test
-	public void check270to180() {
-		yawTest(180, 270);
-	}
-	
-	private void yawTest(float refYaw, float trackerTaw) {
-		checkReferenceAdjustmentYaw(q(0, refYaw, 0), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(0, refYaw, 15), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(15, refYaw, 0), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(15, refYaw, 15), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(0, refYaw, 0), q(15, trackerTaw, 0));
-		checkReferenceAdjustmentYaw(q(0, refYaw, 15), q(0, trackerTaw, 15));
-		checkReferenceAdjustmentYaw(q(15, refYaw, 0), q(15, trackerTaw, 15));
-		checkReferenceAdjustmentYaw(q(15, refYaw, 15), q(0, trackerTaw, 15));
-	}
-	
-	public static void checkReferenceAdjustmentYaw(Quaternion referenceQuat, Quaternion trackerQuat) {
-		ComputedTracker tracker = new ComputedTracker("test");
-		tracker.rotation.set(trackerQuat);
-		ReferenceAdjustedTracker adj = new ReferenceAdjustedTracker(tracker);
-		adj.resetYaw(referenceQuat);
-		Quaternion read = new Quaternion();
-		assertTrue("Adjusted tracker didn't return rotation", adj.getRotation(read));
-		assertEquals("Adjusted quat is not equal to reference quat (" + toDegs(referenceQuat) + " vs " + toDegs(read) + ")", new QuatEqualYawWithEpsilon(referenceQuat), new QuatEqualYawWithEpsilon(read));
-	}
-	
-	public static String toDegs(Quaternion q) {
-		float[] degs = new float[3];
-		q.toAngles(degs);
-		return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0);
-	}
-	
-	public static Quaternion q(float pitch, float yaw, float roll) {
-		return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-	}
-	
-	private static class QuatEqualYawWithEpsilon {
-		
-		private final Quaternion q;
-		
-		public QuatEqualYawWithEpsilon(Quaternion q) {
-			this.q = q;
-		}
-		
-		@Override
-		public String toString() {
-			return String.valueOf(q);
-		}
-		
-		@Override
-		public int hashCode() {
-			return q.hashCode();
-		}
-		
-		@Override
-		public boolean equals(Object obj) {
-			if(obj instanceof Quaternion)
-				obj = new QuatEqualYawWithEpsilon((Quaternion) obj);
-			if(!(obj instanceof QuatEqualYawWithEpsilon))
-				return false;
-			Quaternion q2 = ((QuatEqualYawWithEpsilon) obj).q;
-			float[] degs1 = new float[3];
-			q.toAngles(degs1);
-			float[] degs2 = new float[3];
-			q2.toAngles(degs2);
-			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs1[1] += FastMath.TWO_PI;
-			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs2[1] += FastMath.TWO_PI;
-			return FloatMath.equalsWithEpsilon(degs1[1], degs2[1]);
-		}
-	}
-}
Author	SHA1	Message	Date
Eiren Rain	52f59fbfb3	Fixed typo	2021-08-10 16:51:42 +03:00
Eiren Rain	4a59017269	Added important comment	2021-08-10 09:20:12 +03:00
Eiren Rain	5c6d02de30	Parse handshake properly	2021-08-10 08:58:57 +03:00
Eiren Rain	83b0e78b9e	Recieve correction and tap data, currently not used Fix some typos	2021-08-10 08:31:03 +03:00
Eiren Rain	ac192e2416	Display magentometer calibration separately	2021-08-10 07:26:32 +03:00
Eiren Rain	52932d63d3	Display data from new protocol and firmware version, including calibration statis and magentometer correction	2021-08-09 17:46:00 +03:00
Eiren Rain	6a45e5d32c	Another attempt at new knee model Waist rotation now takes into account knee model	2021-08-08 08:04:28 +03:00
Eiren Rain	6f09598243	Added toggle for extended pelvis model Extended aknle model is added but not used Added 3-tracker rudementary support for people that want to hurt themselves Fixed typos	2021-08-05 07:50:49 +03:00
Eiren Rain	467e79d1c0	Added Fast Reset button that resets only Yaw of the trackers	2021-08-04 07:48:54 +03:00
Eiren Rain	fa66c94ec3	Implemented new pelvis model as average between legs rotation SlimeVR/SlimeVR-Server#9	2021-08-04 01:03:58 +03:00
Eiren Rain	2b4ce4b920	Fix version number	2021-08-02 23:08:22 +03:00
Eiren Rain	4e7585b87e	Added support for different SteamVR trackers configuration	2021-08-02 23:07:25 +03:00
Eiren Rain	de13db4627	Fix right foot wasn't resetting	2021-07-31 03:05:08 +03:00
Eiren Rain	ca8ceb428b	Set version to 0.0.12	2021-07-26 02:01:18 +03:00
Eiren Rain	c18597387a	Merge pull request #20 from adigyran/patch-5 Update build.gradle	2021-07-26 02:00:25 +03:00
Yury	962504b788	Update build.gradle useJUnitPlatform	2021-07-26 01:27:15 +03:00
Eiren Rain	8d1886d045	Display raw tracker data in degrees not in quats	2021-07-26 00:55:01 +03:00
Eiren Rain	1c5167bb7c	Another tracker adjustment fix, doesn't pass all tests, but works better	2021-07-26 00:48:34 +03:00
Eiren Rain	e248cca4e7	Adjustmed trackers pass all tests	2021-07-25 23:04:34 +03:00
Eiren Rain	89ee97872d	Streams go brrrr in unit tests	2021-07-25 22:45:54 +03:00
Eiren Rain	b22a2368d4	Refactor tests, generate tests for each angle dynamically, separate 3 test types	2021-07-25 22:19:04 +03:00
Eiren Rain	9ecfc57e44	Use JUnit 5 framework for testing	2021-07-25 20:57:11 +03:00
Eiren Rain	cd141258c5	Merge pull request #19 from adigyran/patch-4 Update build.gradle	2021-07-23 17:51:47 +03:00
Yury	5dc027a9e2	Update build.gradle fix gradle dependencies	2021-07-23 17:46:29 +03:00
Eiren Rain	3e55b0e417	Merge pull request #18 from adigyran/patch-3 Update README.md	2021-07-22 12:36:20 +03:00
Eiren Rain	9ca6b21c61	Merge pull request #17 from ButterscotchVanilla/main Automatically detect and set the Slime Java Commons subproject location	2021-07-22 12:36:01 +03:00
Yury	8ec528d4a0	Update README.md formatting, thanks Butterscotch for some changes	2021-07-22 02:56:25 +03:00
Yury	961946bd29	Update README.md formatting	2021-07-22 02:53:35 +03:00
Yury	da5fc860cf	Update README.md formatting	2021-07-22 02:50:54 +03:00
Yury	fdd39c4010	Update README.md How to build instructions, this is for ButterscotchVanilla's PR #17	2021-07-22 02:46:39 +03:00
ButterscotchVanilla	900e96a3a6	Announce subproject location before setting it	2021-07-21 17:39:45 -04:00
ButterscotchVanilla	6a9f42f126	Auto-detect Slime Java Commons subproject location	2021-07-21 17:35:09 -04:00
ButterscotchVanilla	72ea196359	Update gradle.yml	2021-07-21 17:11:01 -04:00
ButterscotchVanilla	90a8abeed2	Add comments to build.gradle and add path to subproject	2021-07-21 17:07:16 -04:00