Implement setting WiFi credentials via GUI

Added version number to the title to reduce confusion

.github/workflows/gradle.yml

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

README.md

@@ -5,7 +5,7 @@ Server orchestrates communication between multiple sensors and integrations, lik
 
 Sensors implementations:
 * [SlimeVR Tracker for ESP](https://github.com/SlimeVR/SlimeVR-Tracker-ESP) - ESP microcontrollers and multiple IMUs are supported
-* [owoTrack Mobile App](https://github.com/abb128/owoTrackVRSyncMobile) - use phone as a tracker (limited functionality and copmatibility)
+* [owoTrack Mobile App](https://github.com/abb128/owoTrackVRSyncMobile) - use phone as a tracker (limited functionality and compatibility)
 
 Integrations:
 * Use [SlimeVR OpenVR Driver](https://github.com/SlimeVR/SlimeVR-OpenVR-Driver) as a driver for SteamVR
@@ -13,4 +13,4 @@ Integrations:
 
 ## How to use
 
-Latest instructions are currently [here](https://gist.github.com/Eirenliel/8c0eefcdbda1076d5c2e1bf634831d20). Will be updated and republished as time goes on.
+Latest instructions are currently [here](https://gist.github.com/Eirenliel/8c0eefcdbda1076d5c2e1bf634831d20). Will be updated and republished as time goes on.

build.gradle

@@ -19,12 +19,15 @@ repositories {
 }
 
 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)'
 
     // This dependency is used internally, and not exposed to consumers on their own compile classpath.
     implementation 'com.google.guava:guava:28.2-jre'
@@ -33,3 +36,15 @@ dependencies {
     // Use JUnit test framework
     testImplementation 'junit:junit:4.12'
 }
+
+subprojects.each { subproject -> evaluationDependsOn(subproject.path) }
+task serverJar (type: Jar, dependsOn: subprojects.tasks['build']) {
+    manifest {
+        attributes 'Main-Class': 'io.eiren.vr.Main'
+    }
+    
+    from {
+        configurations.compile.collect { it.isDirectory() ? it : zipTree(it) }
+    }
+    with jar
+}

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

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

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

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

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

@@ -34,53 +34,71 @@ public class SkeletonConfig extends EJBag {
 			removeAll();
 			
 			int row = 0;
-
+			
+			add(new TimedResetButton("Reset All", "All"), s(c(1, row, 1), 3, 1));
+			row++;
+			
 			add(new JLabel("Chest"), c(0, row, 1));
 			add(new AdjButton("+", "Chest", 0.01f), c(1, row, 1));
 			add(new SkeletonLabel("Chest"), c(2, row, 1));
 			add(new AdjButton("-", "Chest", -0.01f), c(3, row, 1));
+			add(new ResetButton("Reset", "Chest"), c(4, row, 1));
 			row++;
 			
 			add(new JLabel("Waist"), c(0, row, 1));
 			add(new AdjButton("+", "Waist", 0.01f), c(1, row, 1));
 			add(new SkeletonLabel("Waist"), c(2, row, 1));
 			add(new AdjButton("-", "Waist", -0.01f), c(3, row, 1));
-			row++;
-			
-			add(new JLabel("Virtual waist"), c(0, row, 1));
-			add(new AdjButton("+", "Virtual waist", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Virtual waist"), c(2, row, 1));
-			add(new AdjButton("-", "Virtual waist", -0.01f), c(3, row, 1));
+			add(new TimedResetButton("Reset", "Waist"), c(4, row, 1));
 			row++;
 
 			add(new JLabel("Hips width"), c(0, row, 1));
 			add(new AdjButton("+", "Hips width", 0.01f), c(1, row, 1));
 			add(new SkeletonLabel("Hips width"), c(2, row, 1));
 			add(new AdjButton("-", "Hips width", -0.01f), c(3, row, 1));
+			add(new ResetButton("Reset", "Hips width"), c(4, row, 1));
 			row++;
 
-			add(new JLabel("Hip length"), c(0, row, 1));
-			add(new AdjButton("+", "Hip length", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Hip length"), c(2, row, 1));
-			add(new AdjButton("-", "Hip length", -0.01f), c(3, row, 1));
+			add(new JLabel("Legs length"), c(0, row, 1));
+			add(new AdjButton("+", "Legs length", 0.01f), c(1, row, 1));
+			add(new SkeletonLabel("Legs length"), c(2, row, 1));
+			add(new AdjButton("-", "Legs length", -0.01f), c(3, row, 1));
+			add(new TimedResetButton("Reset", "Legs length"), c(4, row, 1));
 			row++;
 
-			add(new JLabel("Ankle length"), c(0, row, 1));
-			add(new AdjButton("+", "Ankle length", 0.01f), c(1, row, 1));
-			add(new SkeletonLabel("Ankle length"), c(2, row, 1));
-			add(new AdjButton("-", "Ankle length", -0.01f), c(3, row, 1));
+			add(new JLabel("Knee height"), c(0, row, 1));
+			add(new AdjButton("+", "Knee height", 0.01f), c(1, row, 1));
+			add(new SkeletonLabel("Knee height"), c(2, row, 1));
+			add(new AdjButton("-", "Knee height", -0.01f), c(3, row, 1));
+			add(new TimedResetButton("Reset", "Knee height"), c(4, row, 1));
+			row++;
+
+			add(new JLabel("Foot length"), c(0, row, 1));
+			add(new AdjButton("+", "Foot length", 0.01f), c(1, row, 1));
+			add(new SkeletonLabel("Foot length"), c(2, row, 1));
+			add(new AdjButton("-", "Foot length", -0.01f), c(3, row, 1));
+			add(new ResetButton("Reset", "Foot length"), c(4, row, 1));
 			row++;
 
 			add(new JLabel("Head offset"), c(0, row, 1));
 			add(new AdjButton("+", "Head", 0.01f), c(1, row, 1));
 			add(new SkeletonLabel("Head"), c(2, row, 1));
 			add(new AdjButton("-", "Head", -0.01f), c(3, row, 1));
+			add(new ResetButton("Reset", "Head"), c(4, row, 1));
 			row++;
 
 			add(new JLabel("Neck length"), c(0, row, 1));
 			add(new AdjButton("+", "Neck", 0.01f), c(1, row, 1));
 			add(new SkeletonLabel("Neck"), c(2, row, 1));
 			add(new AdjButton("-", "Neck", -0.01f), c(3, row, 1));
+			add(new ResetButton("Reset", "Neck"), c(4, row, 1));
+			row++;
+			
+			add(new JLabel("Virtual waist"), c(0, row, 1));
+			add(new AdjButton("+", "Virtual waist", 0.01f), c(1, row, 1));
+			add(new SkeletonLabel("Virtual waist"), c(2, row, 1));
+			add(new AdjButton("-", "Virtual waist", -0.01f), c(3, row, 1));
+			add(new ResetButton("Reset", "Virtual waist"), c(4, row, 1));
 			row++;
 			
 			gui.refresh();
@@ -91,13 +109,27 @@ public class SkeletonConfig extends EJBag {
 		float current = server.humanPoseProcessor.getSkeletonConfig(joint);
 		server.humanPoseProcessor.setSkeletonConfig(joint, current + diff);
 		server.saveConfig();
-		labels.get(joint).setText(StringUtils.prettyNumber(current + diff, 2));
+		labels.get(joint).setText(StringUtils.prettyNumber((current + diff) * 100, 0));
+	}
+	
+	private void reset(String joint) {
+		server.humanPoseProcessor.resetSkeletonConfig(joint);
+		server.saveConfig();
+		if(!"All".equals(joint)) {
+			float current = server.humanPoseProcessor.getSkeletonConfig(joint);
+			labels.get(joint).setText(StringUtils.prettyNumber((current) * 100, 0));
+		} else {
+			labels.forEach((jnt, label) -> {
+				float current = server.humanPoseProcessor.getSkeletonConfig(jnt);
+				label.setText(StringUtils.prettyNumber((current) * 100, 0));
+			});
+		}
 	}
 	
 	private class SkeletonLabel extends JLabel {
 		
 		public SkeletonLabel(String joint) {
-			super(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint), 2));
+			super(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint) * 100, 0));
 			labels.put(joint, this);
 		}
 	}
@@ -114,4 +146,30 @@ public class SkeletonConfig extends EJBag {
 			});
 		}
 	}
+	
+	private class ResetButton extends JButton {
+		
+		public ResetButton(String text, String joint) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					reset(joint);
+				}
+			});
+		}
+	}
+	
+	private class TimedResetButton extends JButton {
+		
+		public TimedResetButton(String text, String joint) {
+			super(text);
+			addMouseListener(new MouseInputAdapter() {
+				@Override
+				public void mouseClicked(MouseEvent e) {
+					ButtonTimer.runTimer(TimedResetButton.this, 3, text, () -> reset(joint));
+				}
+			});
+		}
+	}
 }

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

@@ -11,7 +11,6 @@ import javax.swing.Box;
 import javax.swing.BoxLayout;
 import javax.swing.JComboBox;
 import javax.swing.JLabel;
-import javax.swing.border.BevelBorder;
 
 import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
@@ -20,7 +19,6 @@ import com.jme3.math.Vector3f;
 import io.eiren.util.StringUtils;
 import io.eiren.util.ann.AWTThread;
 import io.eiren.util.ann.ThreadSafe;
-import io.eiren.util.ann.VRServerThread;
 import io.eiren.util.collections.FastList;
 import io.eiren.vr.VRServer;
 import io.eiren.vr.processor.TrackerBodyPosition;
@@ -103,6 +101,9 @@ public class TrackersList extends EJBox {
 		JLabel tps;
 		JLabel bat;
 		JLabel ping;
+		JLabel raw;
+		JLabel adj;
+		JLabel adjYaw;
 		
 		@AWTThread
 		public TrackerRow(Tracker t) {
@@ -177,12 +178,22 @@ public class TrackersList extends EJBox {
 			add(status = new JLabel(t.getStatus().toString().toLowerCase()), c(1, 4, 0, GridBagConstraints.FIRST_LINE_START));
 			add(new JLabel("Battery:"), c(2, 4, 0, GridBagConstraints.FIRST_LINE_START));
 			add(bat = new JLabel("0"), c(3, 4, 0, GridBagConstraints.FIRST_LINE_START));
+			add(new JLabel("Raw:"), c(0, 5, 0, GridBagConstraints.FIRST_LINE_START));
+			add(raw = new JLabel("0 0 0 0"), s(c(1, 5, 0, GridBagConstraints.FIRST_LINE_START), 3, 1));
+			
+			if(t instanceof ReferenceAdjustedTracker) {
+				add(new JLabel("Adj:"), c(0, 6, 0, GridBagConstraints.FIRST_LINE_START));
+				add(adj = new JLabel("0 0 0 0"), c(1, 6, 0, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("AdjY:"), c(2, 6, 0, GridBagConstraints.FIRST_LINE_START));
+				add(adjYaw = new JLabel("0 0 0 0"), c(3, 6, 0, GridBagConstraints.FIRST_LINE_START));
+			}
 
 			setBorder(BorderFactory.createLineBorder(new Color(0x663399), 4, true));
 			TrackersList.this.add(this);
 			return this;
 		}
 
+		@SuppressWarnings("unchecked")
 		@AWTThread
 		public void update() {
 			if(position == null)
@@ -204,8 +215,25 @@ public class TrackersList extends EJBox {
 			}
 			if(t instanceof TrackerWithBattery)
 				bat.setText(StringUtils.prettyNumber(((TrackerWithBattery) t).getBatteryVoltage(), 1));
-			if(t instanceof IMUTracker)
-				ping.setText(String.valueOf(((IMUTracker) t).ping));
+			Tracker t2 = t;
+			if(t instanceof ReferenceAdjustedTracker) {
+				t2 = ((ReferenceAdjustedTracker<Tracker>) t).getTracker();
+				((ReferenceAdjustedTracker<Tracker>) t).adjustmentAttachment.toAngles(angles);
+				adj.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+				((ReferenceAdjustedTracker<Tracker>) t).adjustmentYaw.toAngles(angles);
+				adjYaw.setText(StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+						+ " " + StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0));
+			}
+			if(t2 instanceof IMUTracker)
+				ping.setText(String.valueOf(((IMUTracker) t2).ping));
+			t2.getRotation(q);
+			raw.setText(StringUtils.prettyNumber(q.getX(), 4)
+					+ " " + StringUtils.prettyNumber(q.getY(), 4)
+					+ " " + StringUtils.prettyNumber(q.getZ(), 4)
+					+ " " + StringUtils.prettyNumber(q.getW(), 4));
 		}
 	}
 	

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

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

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().contains("CH340")) {
+				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

@@ -8,7 +8,10 @@ import io.eiren.util.logging.LogManager;
 
 public class Main {
 	
-	@SuppressWarnings("unused")
+	public static String VERSION = "0.0.9";
+	
+	public static VRServer vrServer;
+	
 	public static void main(String[] args) {
 		System.setProperty("awt.useSystemAAFontSettings", "on");
 		System.setProperty("swing.aatext", "true");
@@ -21,7 +24,7 @@ public class Main {
 		}
 		
 		try {
-			VRServer vrServer = new VRServer();
+			vrServer = new VRServer();
 			vrServer.start();
 			new VRServerGUI(vrServer);
 		} catch(Throwable e) {

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

@@ -46,6 +46,7 @@ public class VRServer extends Thread {
 	
 	public VRServer() {
 		super("VRServer");
+		loadConfig();
 		hmdTracker = new HMDTracker("HMD");
 		hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier debugging
 		// TODO Multiple processors
@@ -53,7 +54,7 @@ public class VRServer extends Thread {
 		List<? extends Tracker> shareTrackers = humanPoseProcessor.getComputedTrackers();
 		
 		// Create named pipe bridge for SteamVR driver
-		NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers);
+		NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers, this);
 		tasks.add(() -> driverBridge.start());
 		bridges.add(driverBridge);
 		
@@ -71,6 +72,16 @@ public class VRServer extends Thread {
 		for(int i = 0; i < shareTrackers.size(); ++i)
 			registerTracker(shareTrackers.get(i));
 	}
+
+	@ThreadSafe
+	public <E extends VRBridge> E getVRBridge(Class<E> cls) {
+		for(int i = 0; i < bridges.size(); ++i) {
+			VRBridge b = bridges.get(i);
+			if(cls.isInstance(b))
+				return cls.cast(b);
+		}
+		return null;
+	}
 	
 	@ThreadSafe
 	public TrackerConfig getTrackerConfig(Tracker tracker) {
@@ -169,7 +180,6 @@ public class VRServer extends Thread {
 	@Override
 	@VRServerThread
 	public void run() {
-		loadConfig();
 		trackersServer.start();
 		while(true) {
 			//final long start = System.currentTimeMillis();

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

@@ -14,6 +14,7 @@ import com.sun.jna.ptr.IntByReference;
 
 import io.eiren.util.collections.FastList;
 import io.eiren.util.logging.LogManager;
+import io.eiren.vr.VRServer;
 import io.eiren.vr.trackers.ComputedTracker;
 import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.Tracker;
@@ -32,6 +33,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	private final Quaternion qBuffer = new Quaternion();
 	private final Quaternion qBuffer2 = new Quaternion();
 	
+	private final VRServer server;
 	private Pipe hmdPipe;
 	private final HMDTracker hmd;
 	private final List<Pipe> trackerPipes;
@@ -41,8 +43,11 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	private final HMDTracker internalHMDTracker = new HMDTracker("itnernal://HMD");
 	private final AtomicBoolean newHMDData = new AtomicBoolean(false);
 	
-	public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> shareTrackers) {
+	private boolean spawnOneTracker = false;
+	
+	public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> shareTrackers, VRServer server) {
 		super("Named Pipe VR Bridge");
+		this.server = server;
 		this.hmd = hmd;
 		this.shareTrackers = new FastList<>(shareTrackers);
 		this.trackerPipes = new FastList<>(shareTrackers.size());
@@ -53,6 +58,26 @@ 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
@@ -108,6 +133,8 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 				if(tryOpeningPipe(trackerPipe))
 					initTrackerPipe(trackerPipe, i);
 			}
+			if(spawnOneTracker)
+				break;
 		}
 	}
 	
@@ -167,7 +194,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge {
 	}
 	
 	private void initTrackerPipe(Pipe pipe, int trackerId) {
-		String trackerHello = this.shareTrackers.size() + " 0";
+		String trackerHello = (spawnOneTracker ? "1" : this.shareTrackers.size()) + " 0";
 		System.arraycopy(trackerHello.getBytes(ASCII), 0, buffer, 0, trackerHello.length());
 		buffer[trackerHello.length()] = '\0';
 		IntByReference lpNumberOfBytesWritten = new IntByReference(0);
@@ -225,6 +252,8 @@ 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/HumanPoseProcessor.java

@@ -38,6 +38,12 @@ public class HumanPoseProcessor {
 		if(skeleton != null)
 			skeleton.setSkeletonConfig(key, newLength);
 	}
+
+	@ThreadSafe
+	public void resetSkeletonConfig(String key) {
+		if(skeleton != null)
+			skeleton.resetSkeletonConfig(key);
+	}
 	
 	@ThreadSafe
 	public float getSkeletonConfig(String key) {

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

@@ -4,6 +4,7 @@ import java.util.List;
 
 import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
 
 import io.eiren.util.ann.VRServerThread;
 import io.eiren.vr.VRServer;
@@ -13,6 +14,10 @@ import io.eiren.vr.trackers.TrackerUtils;
 
 public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	
+	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();
@@ -39,15 +44,16 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	/**
 	 * Distance between centers of both hips
 	 */
-	protected float hipsWidth = 0.30f;
+	protected float hipsWidth = HIPS_WIDTH_DEFAULT;
 	/**
 	 * Length from waist to knees
 	 */
-	protected float hipsLength = 0.51f;
+	protected float kneeHeight = 0.42f;
 	/**
 	 * Distance from waist to ankle
 	 */
-	protected float ankleLength = 0.55f;
+	protected float legsLength = 0.84f;
+	protected float footLength = FOOT_LENGTH_DEFAULT;
 	
 	protected float minKneePitch = 0f * FastMath.DEG_TO_RAD;
 	protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD;
@@ -77,8 +83,9 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		lat.setStatus(TrackerStatus.OK);
 		rat.setStatus(TrackerStatus.OK);
 		hipsWidth = server.config.getFloat("body.hipsWidth", hipsWidth);
-		hipsLength = server.config.getFloat("body.hipLength", hipsLength);
-		ankleLength = server.config.getFloat("body.ankleLength", ankleLength);
+		kneeHeight = server.config.getFloat("body.kneeHeight", kneeHeight);
+		legsLength = server.config.getFloat("body.legsLength", legsLength);
+		footLength = server.config.getFloat("body.footLength", footLength);
 		
 		waistNode.attachChild(leftHipNode);
 		leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
@@ -87,26 +94,58 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
 		
 		leftHipNode.attachChild(leftKneeNode);
-		leftKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
+		leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
 		
 		rightHipNode.attachChild(rightKneeNode);
-		rightKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
+		rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
 		
 		leftKneeNode.attachChild(leftAnkleNode);
-		leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
+		leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
 		
 		rightKneeNode.attachChild(rightAnkleNode);
-		rightAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
+		rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
 
 		leftAnkleNode.attachChild(leftFootNode);
-		leftFootNode.localTransform.setTranslation(0, 0, -0.05f);
+		leftFootNode.localTransform.setTranslation(0, 0, -footLength);
 		
 		rightAnkleNode.attachChild(rightFootNode);
-		rightFootNode.localTransform.setTranslation(0, 0, -0.05f);
+		rightFootNode.localTransform.setTranslation(0, 0, -footLength);
 		
 		configMap.put("Hips width", hipsWidth);
-		configMap.put("Hip length", hipsLength);
-		configMap.put("Ankle length", ankleLength);
+		configMap.put("Legs length", legsLength);
+		configMap.put("Knee height", kneeHeight);
+		configMap.put("Foot length", footLength);
+	}
+	
+	@Override
+	public void resetSkeletonConfig(String joint) {
+		super.resetSkeletonConfig(joint);
+		switch(joint) {
+		case "All":
+			// Resets from the parent already performed
+			resetSkeletonConfig("Hips width");
+			resetSkeletonConfig("Foot length");
+			resetSkeletonConfig("Legs length");
+			break;
+		case "Hips width":
+			setSkeletonConfig(joint, HIPS_WIDTH_DEFAULT);
+			break;
+		case "Foot length":
+			setSkeletonConfig(joint, FOOT_LENGTH_DEFAULT);
+			break;
+		case "Legs length": // Set legs length to be 5cm above floor level
+			Vector3f vec = new Vector3f();
+			hmdTracker.getPosition(vec);
+			float height = vec.y;
+			if(height > 0.5f) { // Reset only if floor level is right, todo: read floor level from SteamVR if it's not 0
+				setSkeletonConfig(joint, height - neckLength - waistDistance - DEFAULT_FLOOR_OFFSET);
+			}
+			resetSkeletonConfig("Knee height");
+			break;
+		case "Knee height": // Knees are at 50% of the legs by default
+			setSkeletonConfig(joint, legsLength / 2.0f);
+			break;
+		}
 	}
 	
 	@Override
@@ -116,20 +155,28 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		case "Hips width":
 			hipsWidth = newLength;
 			server.config.setProperty("body.hipsWidth", hipsWidth);
-			leftHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
-			rightHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
+			leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
+			rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
 			break;
-		case "Hip length":
-			hipsLength = newLength;
-			server.config.setProperty("body.hipLength", hipsLength);
-			leftKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
-			rightKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
+		case "Knee height":
+			kneeHeight = newLength;
+			server.config.setProperty("body.kneeHeight", kneeHeight);
+			leftAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+			rightAnkleNode.localTransform.setTranslation(0, -kneeHeight, 0);
+			leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+			rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
 			break;
-		case "Ankle length":
-			ankleLength = newLength;
-			server.config.setProperty("body.ankleLength", ankleLength);
-			leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
-			rightAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
+		case "Legs length":
+			legsLength = newLength;
+			server.config.setProperty("body.legsLength", legsLength);
+			leftKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+			rightKneeNode.localTransform.setTranslation(0, -(legsLength - kneeHeight), 0);
+			break;
+		case "Foot length":
+			footLength = newLength;
+			server.config.setProperty("body.footLength", footLength);
+			leftFootNode.localTransform.setTranslation(0, 0, -footLength);
+			rightFootNode.localTransform.setTranslation(0, 0, -footLength);
 			break;
 		}
 	}
@@ -141,7 +188,7 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		leftLegTracker.getRotation(hipBuf);
 		leftAnkleTracker.getRotation(kneeBuf);
 
-		calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
+		//calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
 		
 		leftHipNode.localTransform.setRotation(hipBuf);
 		leftKneeNode.localTransform.setRotation(kneeBuf);
@@ -158,7 +205,7 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		rightLegTracker.getRotation(hipBuf);
 		rightAnkleTracker.getRotation(kneeBuf);
 		
-		calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
+		//calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
 		
 		rightHipNode.localTransform.setRotation(hipBuf);
 		rightKneeNode.localTransform.setRotation(kneeBuf);

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

@@ -16,6 +16,9 @@ import io.eiren.vr.trackers.TrackerUtils;
 
 public class HumanSkeleonWithWaist extends HumanSkeleton {
 	
+	public static final float HEAD_SHIFT_DEFAULT = 0.1f;
+	public static final float NECK_LENGTH_DEFAULT = 0.1f;
+	
 	protected final Map<String, Float> configMap = new HashMap<>();
 	protected final VRServer server;
 
@@ -48,11 +51,11 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	/**
 	 * Distacne from eyes to the base of the neck
 	 */
-	protected float neckLength = 0.1f;
+	protected float neckLength = NECK_LENGTH_DEFAULT;
 	/**
 	 * Distance from eyes to ear
 	 */
-	protected float headShift = 0.1f;
+	protected float headShift = HEAD_SHIFT_DEFAULT;
 
 	public HumanSkeleonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
 		List<Tracker> allTracekrs = server.getAllTrackers();
@@ -96,6 +99,39 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		configMap.put("Virtual waist", trackerWaistDistance);
 	}
 	
+	@Override
+	public void resetSkeletonConfig(String joint) {
+		switch(joint) {
+		case "All": // Reset all joints according to height
+			resetSkeletonConfig("Head");
+			resetSkeletonConfig("Neck");
+			resetSkeletonConfig("Virtual waist");
+			resetSkeletonConfig("Waist");
+			resetSkeletonConfig("Chest");
+			break;
+		case "Head": 
+			setSkeletonConfig(joint, HEAD_SHIFT_DEFAULT);
+			break;
+		case "Neck": 
+			setSkeletonConfig(joint, NECK_LENGTH_DEFAULT);
+			break;
+		case "Virtual waist": 
+			setSkeletonConfig(joint, 0.0f);
+			break;
+		case "Chest": 
+			setSkeletonConfig(joint, waistDistance / 2.0f);
+			break;
+		case "Waist": // Puts Waist in the middle of the height
+			Vector3f vec = new Vector3f();
+			hmdTracker.getPosition(vec);
+			float height = vec.y;
+			if(height > 0.5f) { // Reset only if floor level is right, todo: read floor level from SteamVR if it's not 0
+				setSkeletonConfig(joint, (height) / 2.0f);
+			}
+			break;
+		}
+	}
+	
 	@Override
 	public Map<String, Float> getSkeletonConfig() {
 		return configMap;
@@ -119,6 +155,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 			waistDistance = newLength;
 			server.config.setProperty("body.waistDistance", waistDistance);
 			waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0);
+			trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
 			break;
 		case "Chest":
 			chestDistance = newLength;

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

@@ -18,6 +18,9 @@ public abstract class HumanSkeleton {
 
 	@ThreadSafe
 	public abstract void setSkeletonConfig(String key, float newLength);
+	
+	@ThreadSafe
+	public abstract void resetSkeletonConfig(String joint);
 
 	@VRServerThread
 	public abstract void resetTrackersFull();

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

@@ -1,6 +1,5 @@
 package io.eiren.vr.trackers;
 
-import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
@@ -9,12 +8,8 @@ import io.eiren.vr.processor.TrackerBodyPosition;
 public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	
 	public final E tracker;
-	private final Quaternion smoothedQuaternion = new Quaternion();
 	public final Quaternion adjustmentYaw = new Quaternion();
 	public final Quaternion adjustmentAttachment = new Quaternion();
-	protected float[] lastAngles = new float[3];
-	public float smooth = 0 * FastMath.DEG_TO_RAD;
-	private final float[] angles = new float[3];
 	protected float confidenceMultiplier = 1.0f;
 	
 	public ReferenceAdjustedTracker(E tracker) {
@@ -49,19 +44,14 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	 */
 	@Override
 	public void resetFull(Quaternion reference) {
-		resetYaw(reference);
-		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
-		adjustmentYaw.mult(sensorRotation, sensorRotation);
-
-		// Use only yaw HMD rotation
-		Quaternion targetTrackerRotation = new Quaternion(reference);
-		float[] angles = new float[3];
-		targetTrackerRotation.toAngles(angles);
-		targetTrackerRotation.fromAngles(0, angles[1], 0);
+		//float[] angles = new float[3];
+		//sensorRotation.toAngles(angles);
+		//sensorRotation.fromAngles(angles[0], 0, angles[2]);
+		adjustmentAttachment.set(sensorRotation).inverseLocal();
 		
-		adjustmentAttachment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+		resetYaw(reference);
 	}
 
 	/**
@@ -81,6 +71,8 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
+		adjustmentAttachment.mult(sensorRotation, sensorRotation);
+		//sensorRotation.multLocal(adjustmentAttachment);
 		
 		sensorRotation.toAngles(angles);
 		sensorRotation.fromAngles(0, angles[1], 0);
@@ -88,26 +80,17 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 		adjustmentYaw.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
 		
 		confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
-		lastAngles[0] = 1000;
 	}
 	
 	protected void adjustInternal(Quaternion store) {
-		store.multLocal(adjustmentAttachment);
+		//store.multLocal(adjustmentAttachment);
+		adjustmentAttachment.mult(store, store);
 		adjustmentYaw.mult(store, store);
 	}
 	
 	@Override
 	public boolean getRotation(Quaternion store) {
 		tracker.getRotation(store);
-		if(smooth > 0) {
-			store.toAngles(angles);
-			if(Math.abs(angles[0] - lastAngles[0]) > smooth || Math.abs(angles[1] - lastAngles[1]) > smooth || Math.abs(angles[2] - lastAngles[2]) > smooth) {
-				smoothedQuaternion.set(store);
-				store.toAngles(lastAngles);
-			} else {
-				store.set(smoothedQuaternion);
-			}
-		}
 		adjustInternal(store);
 		return true;
 	}

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

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

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

@@ -1,4 +1,4 @@
-package io.eiren.unity;
+package io.eiren.unit;
 
 import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;

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

@@ -1,196 +0,0 @@
-package io.eiren.unity;
-
-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#resetFull(Quaternion)}
- */
-public class ReferenceAdjustmentsFullTests {
-
-	@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) {
-		checkReferenceAdjustmentFull(q(0, refYaw, 0), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentFull(q(15, refYaw, 0), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerTaw, 0));
-		checkReferenceAdjustmentFull(q(0, refYaw, 0), q(15, trackerTaw, 0));
-		checkReferenceAdjustmentFull(q(0, refYaw, 15), q(0, trackerTaw, 15));
-		checkReferenceAdjustmentFull(q(15, refYaw, 0), q(15, trackerTaw, 15));
-		checkReferenceAdjustmentFull(q(15, refYaw, 15), q(0, trackerTaw, 15));
-	}
-	
-	public static void checkReferenceAdjustmentFull(Quaternion referenceQuat, Quaternion trackerQuat) {
-		ComputedTracker tracker = new ComputedTracker("test");
-		tracker.rotation.set(trackerQuat);
-		ReferenceAdjustedTracker 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));
-	}
-	
-	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]);
-		}
-	}
-}