Fix version number

Update build.gradle

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

build.gradle

@@ -22,28 +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'
+    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
 }

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
+    }
+}

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

@@ -179,7 +179,7 @@ public class TrackersList extends EJBox {
 			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));
+			add(raw = new JLabel("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));
@@ -218,11 +218,11 @@ public class TrackersList extends EJBox {
 			Tracker t2 = t;
 			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));
@@ -230,10 +230,10 @@ public class TrackersList extends EJBox {
 			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));
+			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));
 		}
 	}
 	

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)));
 			
@@ -88,18 +92,6 @@ public class VRServerGUI extends JFrame {
 				});
 			}});
 			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
@@ -110,14 +102,73 @@ public class VRServerGUI extends JFrame {
 				});
 			}});
 			add(Box.createHorizontalStrut(10));
+			add(new JButton("WiFi") {{
+				addMouseListener(new MouseInputAdapter() {
+					@SuppressWarnings("unused")
+					@Override
+					public void mouseClicked(MouseEvent e) {
+						new WiFiWindow(VRServerGUI.this);
+					}
+				});
+			}});
+			add(Box.createHorizontalStrut(10));
 		}});
 		
 		pane.add(new EJBox(LINE_AXIS) {{
 			setBorder(new EmptyBorder(i(5)));
 			add(new EJBox(PAGE_AXIS) {{
 				setAlignmentY(TOP_ALIGNMENT);
+				add(new JLabel("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("vitrualtrackers", 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("vitrualtrackers", 1);
+							break;
+						case 1:
+							server.config.setProperty("vitrualtrackers", 3);
+							break;
+						case 2:
+							server.config.setProperty("vitrualtrackers", 4);
+							break;
+						case 3:
+							server.config.setProperty("vitrualtrackers", 5);
+							break;
+						case 4:
+							server.config.setProperty("vitrualtrackers", 6);
+							break;
+						}
+						server.saveConfig();
+					}
+				});
+				add(Box.createHorizontalStrut(10));
 				
-				add(new JLabel("Trackers"));
+				add(new JLabel("Trackers list"));
 				add(trackersList);
 				add(Box.createVerticalGlue());
 			}});

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

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

src/main/java/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.7 DEV 1";
+	public static String VERSION = "0.0.13";
 	
 	public static VRServer vrServer;
 	
+	@SuppressWarnings("unused")
 	public static void main(String[] args) {
 		System.setProperty("awt.useSystemAAFontSettings", "on");
 		System.setProperty("swing.aatext", "true");

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("vitrualtrackers", 3));
 		List<? extends Tracker> shareTrackers = humanPoseProcessor.getComputedTrackers();
 		
 		// Create named pipe bridge for SteamVR driver

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

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

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

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

@@ -19,11 +19,20 @@ 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));
+			}
+		}
 	}
 
 	@VRServerThread

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

@@ -27,10 +27,12 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	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);
@@ -71,15 +73,23 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		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);
@@ -254,13 +264,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 +315,8 @@ 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);
 		}
 	}
 }

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

@@ -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);
@@ -64,12 +65,16 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		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

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,12 +45,12 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetFull(Quaternion 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);
 	}
@@ -71,21 +72,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

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

@@ -1,257 +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));
-		if(refYaw == 0)
-			testAdjustedTracker(tracker, adj, name, refYaw);
-	}
-	
-	//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 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 += 90) {
-			for(int pitch = -90; pitch <= 90; pitch += 30) {
-				for(int roll = -90; roll <= 90; roll += 30) {
-					rotation.fromAngles(pitch, yaw, roll);
-					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(rotation);
-					
-					diff.toAngles(anglesDiff);
-					assertTrue(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),
-							FloatMath.equalsToZero(anglesDiff[0]) && FloatMath.equalsToZero(anglesDiff[1]) && FloatMath.equalsToZero(anglesDiff[2]));
-				}
-			}
-		}
-		//System.out.println("Errors: " + errors + ", successes: " + successes);
-	}
-	
-	public static String toDegs(Quaternion q) {
-		float[] degs = new float[3];
-		q.toAngles(degs);
-		return StringUtils.prettyNumber(degs[0] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[1] * FastMath.RAD_TO_DEG, 0) + "," + StringUtils.prettyNumber(degs[2] * FastMath.RAD_TO_DEG, 0);
-	}
-	
-	public static Quaternion q(float pitch, float yaw, float roll) {
-		return new Quaternion().fromAngles(pitch * FastMath.DEG_TO_RAD, yaw * FastMath.DEG_TO_RAD, roll * FastMath.DEG_TO_RAD);
-	}
-	
-	public static class QuatEqualFullWithEpsilon {
-		
-		private final Quaternion q;
-		
-		public QuatEqualFullWithEpsilon(Quaternion q) {
-			this.q = q;
-		}
-		
-		@Override
-		public String toString() {
-			return String.valueOf(q);
-		}
-		
-		@Override
-		public int hashCode() {
-			return q.hashCode();
-		}
-		
-		@Override
-		public boolean equals(Object obj) {
-			if(obj instanceof Quaternion)
-				obj = new QuatEqualFullWithEpsilon((Quaternion) obj);
-			if(!(obj instanceof QuatEqualFullWithEpsilon))
-				return false;
-			Quaternion q2 = ((QuatEqualFullWithEpsilon) obj).q;
-			float[] degs1 = new float[3];
-			q.toAngles(degs1);
-			float[] degs2 = new float[3];
-			q2.toAngles(degs2);
-			if(degs1[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs1[1] += FastMath.TWO_PI;
-			if(degs2[1] < -FloatMath.ANGLE_EPSILON_RAD)
-				degs2[1] += FastMath.TWO_PI;
-			return FloatMath.equalsWithEpsilon(degs1[0], degs2[0])
-					&& FloatMath.equalsWithEpsilon(degs1[1], degs2[1])
-					&& FloatMath.equalsWithEpsilon(degs1[2], degs2[2]);
-		}
-	}
-}

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

@@ -0,0 +1,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;
+		}
+	}
+}

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]);
-		}
-	}
-}