Lots of imporvements

Added tracker confidense level
Better tracker list
Fixed tracker adjustments
Better config loading
Added WIP knee/hip yaw interpolation

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

@@ -4,7 +4,6 @@ import java.awt.GridBagConstraints;
 import java.awt.event.MouseEvent;
 import java.util.List;
 
-import javax.swing.BoxLayout;
 import javax.swing.JButton;
 import javax.swing.JLabel;
 import javax.swing.event.MouseInputAdapter;
@@ -19,7 +18,10 @@ 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.trackers.AdjustedTracker;
 import io.eiren.vr.trackers.CalibratingTracker;
+import io.eiren.vr.trackers.ComputedTracker;
+import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.IMUTracker;
 import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerConfig;
@@ -41,6 +43,15 @@ public class TrackersList extends EJBag {
 		this.gui = gui;
 
 		setAlignmentY(TOP_ALIGNMENT);
+		
+		
+		server.addNewTrackerConsumer(this::newTrackerAdded);
+	}
+
+	@AWTThread
+	private void build() {
+		removeAll();
+		
 		add(new JLabel("Tracker"), c(0, 0, 2));
 		add(new JLabel("Designation"), c(1, 0, 2));
 		add(new JLabel("X"), c(2, 0, 2));
@@ -51,8 +62,41 @@ public class TrackersList extends EJBag {
 		add(new JLabel("Roll"), c(7, 0, 2));
 		add(new JLabel("Status"), c(8, 0, 2));
 		add(new JLabel("TPS"), c(9, 0, 2));
+		add(new JLabel("Conf"), c(10, 0, 2));
 		
-		server.addNewTrackerConsumer(this::newTrackerAdded);
+		trackers.sort((tr1, tr2) -> getTrackerSort(tr1.t) - getTrackerSort(tr2.t));
+		
+		Class<? extends Tracker> currentClass = null;
+		
+		int n = 1;
+		
+		for(int i = 0; i < trackers.size(); ++i) {
+			TrackerRow tr = trackers.get(i);
+			Tracker t = tr.t;
+			if(currentClass != t.getClass()) {
+				currentClass = t.getClass();
+				add(new JLabel(currentClass.getSimpleName()), c(0, n, 5, GridBagConstraints.CENTER));
+				n++;
+			}
+
+			tr.build(n);
+			TrackerConfig cfg = server.getTrackerConfig(t);
+			
+			if(cfg.designation != null)
+				add(new JLabel(cfg.designation), c(1, n, 2));
+			if(t instanceof CalibratingTracker) {
+				add(new JButton("Calibrate") {{
+					addMouseListener(new MouseInputAdapter() {
+						@Override
+						public void mouseClicked(MouseEvent e) {
+							new CalibrationWindow(t);
+						}
+					});
+				}}, c(11, n, 2));
+			}
+			n++;
+		}
+		gui.refresh();
 	}
 	
 	@VRServerThread
@@ -66,30 +110,14 @@ public class TrackersList extends EJBag {
 	@ThreadSafe
 	public void newTrackerAdded(Tracker t) {
 		java.awt.EventQueue.invokeLater(() -> {
-			final int n = trackers.size();
-			TrackerConfig cfg = server.getTrackerConfig(t);
-			
-			trackers.add(new TrackerRow(t, n + 1));
-			if(cfg.designation != null)
-				add(new JLabel(cfg.designation), c(1, n + 1, 2));
-			if(t instanceof CalibratingTracker) {
-				add(new JButton("Calibrate") {{
-					addMouseListener(new MouseInputAdapter() {
-						@Override
-						public void mouseClicked(MouseEvent e) {
-							new CalibrationWindow(t);
-						}
-					});
-				}}, c(10, n + 1, 2));
-			}
-			
-			gui.refresh();
+			trackers.add(new TrackerRow(t));
+			build();
 		});
 	}
 	
 	private class TrackerRow {
 		
-		Tracker t;
+		final Tracker t;
 		JLabel x;
 		JLabel y;
 		JLabel z;
@@ -98,10 +126,15 @@ public class TrackersList extends EJBag {
 		JLabel a3;
 		JLabel status;
 		JLabel tps;
+		JLabel conf;
 		
 		@AWTThread
-		public TrackerRow(Tracker t, int n) {
+		public TrackerRow(Tracker t) {
 			this.t = t;
+		}
+
+		@AWTThread
+		public TrackerRow build(int n) {
 			add(new JLabel(t.getName()), c(0, n, 2, GridBagConstraints.FIRST_LINE_START));
 			add(x = new JLabel("0"), c(2, n, 2, GridBagConstraints.FIRST_LINE_START));
 			add(y = new JLabel("0"), c(3, n, 2, GridBagConstraints.FIRST_LINE_START));
@@ -112,11 +145,17 @@ public class TrackersList extends EJBag {
 			add(status = new JLabel(t.getStatus().toString()), c(8, n, 2, GridBagConstraints.FIRST_LINE_START));
 			if(t instanceof IMUTracker) {
 				add(tps = new JLabel("0"), c(9, n, 2, GridBagConstraints.FIRST_LINE_START));
+			} else {
+				add(new JLabel(""), c(9, n, 2, GridBagConstraints.FIRST_LINE_START));
 			}
+			add(conf = new JLabel("0"), c(10, n, 2, GridBagConstraints.FIRST_LINE_START));
+			return this;
 		}
 
 		@AWTThread
 		public void update() {
+			if(x == null)
+				return;
 			t.getRotation(q);
 			t.getPosition(v);
 			q.toAngles(angles);
@@ -132,6 +171,19 @@ public class TrackersList extends EJBag {
 			if(t instanceof IMUTracker) {
 				tps.setText(StringUtils.prettyNumber(((IMUTracker) t).getTPS(), 1));
 			}
+			conf.setText(StringUtils.prettyNumber(t.getConfidenceLevel(), 1));
 		}
 	}
+	
+	private static int getTrackerSort(Tracker t) {
+		if(t instanceof HMDTracker)
+			return 0;
+		if(t instanceof ComputedTracker)
+			return 1;
+		if(t instanceof IMUTracker)
+			return 2;
+		if(t instanceof AdjustedTracker)
+			return 5;
+		return 1000;
+	}
 }

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

@@ -81,6 +81,7 @@ public class VRServerGUI extends JFrame {
 				setAlignmentY(TOP_ALIGNMENT);
 				add(new JLabel("Sekelton"));
 				add(skeletonList);
+				add(Box.createVerticalGlue());
 			}});
 		}});
 		

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

@@ -178,6 +178,8 @@ public class VRServer extends Thread {
 	
 	@ThreadSecure
 	public void registerTracker(Tracker tracker) {
+		TrackerConfig config = getTrackerConfig(tracker);
+		tracker.loadConfig(config);
 		queueTask(() -> {
 			trackers.add(tracker);
 			autoAssignTracker(tracker);

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

@@ -63,10 +63,6 @@ public class HumanPoseProcessor {
 	private void addTracker(Tracker tracker, TrackerBodyPosition position) {
 		AdjustedTracker tt = new AdjustedYawTracker(tracker);
 		
-		TrackerConfig config = server.getTrackerConfig(tt);
-		if(config.adjustment != null)
-			tt.adjustment.set(config.adjustment);
-		
 		trackers.put(position, tt);
 		server.registerTracker(tt);
 		updateSekeltonModel();
@@ -123,7 +119,7 @@ public class HumanPoseProcessor {
 			tt.adjust(hmdRotation);
 			
 			TrackerConfig config = server.getTrackerConfig(tt);
-			tt.saveAdjustment(config);
+			tt.saveConfig(config);
 		}
 	}
 

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

@@ -3,12 +3,21 @@ package io.eiren.vr.processor;
 import java.util.List;
 import java.util.Map;
 
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+
+import io.eiren.math.FloatMath;
 import io.eiren.vr.VRServer;
 import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerStatus;
 
 public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	
+	protected final float[] kneeAngles = new float[3];
+	protected final float[] hipAngles = new float[3];
+	protected final Quaternion hipBuf = new Quaternion();
+	protected final Quaternion kneeBuf = new Quaternion();
+	
 	protected final Tracker leftLegTracker;
 	protected final Tracker leftAnkleTracker;
 	protected final ComputedHumanPoseTracker computedLeftAnkleTracker;
@@ -26,7 +35,7 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	/**
 	 * Distance between centers of both hips
 	 */
-	protected float hipsWidth = 0.22f;
+	protected float hipsWidth = 0.33f;
 	/**
 	 * Length from waist to knees
 	 */
@@ -35,6 +44,11 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	 * Distance from waist to ankle
 	 */
 	protected float ankleLength = 0.4f;
+	
+	protected float minKneePitch = 0f * FastMath.DEG_TO_RAD;
+	protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD;
+	
+	protected float kneeLerpFactor = 0.5f;
 
 	public HumanSekeletonWithLegs(VRServer server, Map<TrackerBodyPosition, ? extends Tracker> trackers, List<ComputedHumanPoseTracker> computedTrackers) {
 		super(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers);
@@ -60,10 +74,10 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		ankleLength = server.config.getFloat("body.ankleLength", ankleLength);
 		
 		waistNode.attachChild(leftHipNode);
-		leftHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
+		leftHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
 		
 		waistNode.attachChild(rightHipNode);
-		rightHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0);
+		rightHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0);
 		
 		leftHipNode.attachChild(leftKneeNode);
 		leftKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
@@ -77,28 +91,28 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		rightKneeNode.attachChild(rightAnkleNode);
 		rightAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
 		
-		jointsMap.put(HumanJoint.HIPS_WIDTH, hipsWidth);
-		jointsMap.put(HumanJoint.HIPS_LENGTH, hipsLength);
-		jointsMap.put(HumanJoint.LEGS_LENGTH, ankleLength);
+		configMap.put("Hips width", hipsWidth);
+		configMap.put("Hips length", hipsLength);
+		configMap.put("Legs length", ankleLength);
 	}
 	
 	@Override
-	public void sentJointLength(HumanJoint joint, float newLength) {
-		super.sentJointLength(joint, newLength);
+	public void setSkeletonConfig(String joint, float newLength) {
+		super.setSkeletonConfig(joint, newLength);
 		switch(joint) {
-		case HIPS_WIDTH:
+		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);
 			break;
-		case HIPS_LENGTH:
+		case "Hips length":
 			hipsLength = newLength;
 			server.config.setProperty("body.hipsLength", hipsLength);
 			leftKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
 			rightKneeNode.localTransform.setTranslation(0, -hipsLength, 0);
 			break;
-		case LEGS_LENGTH:
+		case "Legs length":
 			ankleLength = newLength;
 			server.config.setProperty("body.ankleLength", ankleLength);
 			leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0);
@@ -110,21 +124,60 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	@Override
 	public void updateLocalTransforms() {
 		super.updateLocalTransforms();
-		leftLegTracker.getRotation(qBuf);
-		leftHipNode.localTransform.setRotation(qBuf);
-		
-		rightLegTracker.getRotation(qBuf);
-		rightHipNode.localTransform.setRotation(qBuf);
-		
-		leftAnkleTracker.getRotation(qBuf);
-		leftKneeNode.localTransform.setRotation(qBuf);
-		leftAnkleNode.localTransform.setRotation(qBuf);
-		
-		rightAnkleTracker.getRotation(qBuf);
-		rightKneeNode.localTransform.setRotation(qBuf);
-		rightAnkleNode.localTransform.setRotation(qBuf);
-	}
+		// Left Leg
+		leftLegTracker.getRotation(hipBuf);
+		leftAnkleTracker.getRotation(kneeBuf);
 
+		calculateKneeLimits(hipBuf, kneeBuf, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel());
+		
+		leftHipNode.localTransform.setRotation(hipBuf);
+		leftKneeNode.localTransform.setRotation(kneeBuf);
+		leftAnkleNode.localTransform.setRotation(kneeBuf);
+
+		// Right Leg
+		rightLegTracker.getRotation(hipBuf);
+		rightAnkleTracker.getRotation(kneeBuf);
+		
+		calculateKneeLimits(hipBuf, kneeBuf, rightLegTracker.getConfidenceLevel(), rightAnkleTracker.getConfidenceLevel());
+		
+		rightHipNode.localTransform.setRotation(hipBuf);
+		rightKneeNode.localTransform.setRotation(kneeBuf);
+		rightAnkleNode.localTransform.setRotation(kneeBuf);
+	}
+	
+	// Knee basically has only 1 DoF (pitch), average yaw between knee and hip
+	protected void calculateKneeLimits(Quaternion hipBuf, Quaternion kneeBuf, float hipConfidense, float kneeConfidense) {
+		hipBuf.toAngles(hipAngles);
+		kneeBuf.toAngles(kneeAngles);
+		
+		hipConfidense = Math.abs(1 - hipConfidense);
+		kneeConfidense = Math.abs(1 - kneeConfidense);
+		float confidenseDiff = hipConfidense < kneeConfidense ? 1 : 0;//FloatMath.equalsToZero(hipConfidense) ? 1.0f : FastMath.clamp(0.5f * (kneeConfidense / hipConfidense), 0f, 1f);
+		
+		hipAngles[1] = kneeAngles[1] = interpolateRadians(confidenseDiff, kneeAngles[1], hipAngles[1]);
+		//hipAngles[2] = kneeAngles[2] = interpolateRadians(kneeLerpFactor, kneeAngles[2], hipAngles[2]);
+		
+		hipBuf.fromAngles(hipAngles[0], hipAngles[1], hipAngles[2]);
+		kneeBuf.fromAngles(kneeAngles[0], kneeAngles[1], kneeAngles[2]);
+	}
+	
+	public static float normalizeRad(float angle) {
+		return FastMath.normalize(angle, -FastMath.PI, FastMath.PI);
+	}
+	
+	public static float interpolateRadians(float factor, float start, float end) {
+		float angle = Math.abs(end - start);
+		if(angle > FastMath.PI) {
+			if(end > start) {
+				start += FastMath.TWO_PI;
+			} else {
+				end += FastMath.TWO_PI;
+			}
+		}
+		float val = start + (end - start) * factor;
+		return normalizeRad(val);
+	}
+	
 	@Override
 	protected void updateComputedTrackers() {
 		super.updateComputedTrackers();

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

@@ -15,7 +15,7 @@ import io.eiren.vr.trackers.TrackerStatus;
 
 public class HumanSkeleonWithWaist extends HumanSkeleton {
 	
-	protected final Map<HumanJoint, Float> jointsMap = new HashMap<>();
+	protected final Map<String, Float> configMap = new HashMap<>();
 	protected final VRServer server;
 	
 	protected final Quaternion qBuf = new Quaternion();
@@ -47,7 +47,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	/**
 	 * Distance from eyes to ear
 	 */
-	protected float headShift = 0.00f;
+	protected float headShift = 0.05f;
 
 	public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, List<ComputedHumanPoseTracker> computedTrackers) {
 		this.wasitTracker = waistTracker;
@@ -78,37 +78,37 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		neckNode.attachChild(trackerWaistNode);
 		trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance, 0);
 		
-		jointsMap.put(HumanJoint.HEAD, headShift);
-		jointsMap.put(HumanJoint.NECK, neckLength);
-		jointsMap.put(HumanJoint.WAIST, waistDistance);
-		jointsMap.put(HumanJoint.WASIT_VIRTUAL, trackerWaistDistance);
+		configMap.put("Head", headShift);
+		configMap.put("Neck", neckLength);
+		configMap.put("Waist", waistDistance);
+		configMap.put("Virtual waist", trackerWaistDistance);
 	}
 	
 	@Override
-	public Map<HumanJoint, Float> getJointsMap() {
-		return jointsMap;
+	public Map<String, Float> getSkeletonConfig() {
+		return configMap;
 	}
 
 	@Override
-	public void sentJointLength(HumanJoint joint, float newLength) {
-		jointsMap.put(joint, newLength);
+	public void setSkeletonConfig(String joint, float newLength) {
+		configMap.put(joint, newLength);
 		switch(joint) {
-		case HEAD:
+		case "Head":
 			headShift = newLength;
 			server.config.setProperty("body.headShift", headShift);
 			headNode.localTransform.setTranslation(0, 0, headShift);
 			break;
-		case NECK:
+		case "Neck":
 			neckLength = newLength;
 			server.config.setProperty("body.neckLength", neckLength);
 			neckNode.localTransform.setTranslation(0, -neckLength, 0);
 			break;
-		case WAIST:
+		case "Waist":
 			waistDistance = newLength;
 			server.config.setProperty("body.waistDistance", waistDistance);
 			waistNode.localTransform.setTranslation(0, -waistDistance, 0);
 			break;
-		case WASIT_VIRTUAL:
+		case "Virtual waist":
 			trackerWaistDistance = newLength;
 			server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance);
 			trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance, 0);
@@ -144,6 +144,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		// Pelvic bone doesn't tilt when humans tilt, unless they really try.
 		// Can't calculate tilt without additional sensors, so just remove it
 		// completely.
+		vBuf.set(0, 0, 1);
 		qBuf.multLocal(vBuf);
 		vBuf.multLocal(1, 0, 1); // Keep only yaw / Don't normalize, it's done by lookAt()
 		qBuf.lookAt(vBuf, Vector3f.UNIT_Y);

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

@@ -14,28 +14,8 @@ public abstract class HumanSkeleton {
 	public abstract TransformNode getRootNode();
 
 	@ThreadSafe
-	public abstract Map<HumanJoint, Float> getJointsMap();
+	public abstract Map<String, Float> getSkeletonConfig();
 
 	@ThreadSafe
-	public abstract void sentJointLength(HumanJoint joint, float newLength);
-	
-	public enum HumanJoint {
-		
-		HEAD("Head", ""),
-		NECK("Neck", ""),
-		WAIST("Waist", ""),
-		WASIT_VIRTUAL("Virtual waist", ""),
-		HIPS_WIDTH("Hips width", ""),
-		HIPS_LENGTH("Hips length", ""),
-		LEGS_LENGTH("Legs length", ""),
-		;
-		
-		public final String name;
-		public final String description;
-		
-		private HumanJoint(String name, String description) {
-			this.name = name;
-			this.description = description;
-		}
-	}
+	public abstract void setSkeletonConfig(String key, float newLength);
 }

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

@@ -10,22 +10,42 @@ public class AdjustedTracker implements Tracker {
 	public final Quaternion adjustment = new Quaternion();
 	private final Quaternion smoothedQuaternion = new Quaternion();
 	private float[] angles = new float[3];
-	private float[] lastAngles = new float[3];
-	public float smooth = 2 * FastMath.DEG_TO_RAD;
+	protected float[] lastAngles = new float[3];
+	public float smooth = 3 * FastMath.DEG_TO_RAD;
+	
+	protected float confidenceMultiplier = 1.0f;
 	
 	public AdjustedTracker(Tracker tracker) {
 		this.tracker = tracker;
 	}
 	
-	public void saveAdjustment(TrackerConfig cfg) {
+	@Override
+	public void loadConfig(TrackerConfig config) {
+		if(config.adjustment != null)
+			adjustment.set(config.adjustment);
+	}
+	
+	@Override
+	public void saveConfig(TrackerConfig cfg) {
 		cfg.adjustment = new Quaternion(adjustment);
 	}
 	
 	public void adjust(Quaternion reference) {
+		Quaternion targetTrackerRotation = new Quaternion(reference);
+		
+		// Use only yaw rotation
+		Vector3f hmdFront = new Vector3f(0, 0, 1);
+		targetTrackerRotation.multLocal(hmdFront);
+		hmdFront.multLocal(1, 0, 1).normalizeLocal();
+		targetTrackerRotation.lookAt(hmdFront, Vector3f.UNIT_Y);
+		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
 		
-		adjustment.set(sensorRotation).inverseLocal().multLocal(reference);
+		adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+		
+		confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
+		lastAngles[0] = 1000;
 	}
 	
 	@Override
@@ -57,4 +77,9 @@ public class AdjustedTracker implements Tracker {
 	public TrackerStatus getStatus() {
 		return tracker.getStatus();
 	}
+	
+	@Override
+	public float getConfidenceLevel() {
+		return tracker.getConfidenceLevel() * confidenceMultiplier;
+	}
 }

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

@@ -1,7 +1,6 @@
 package io.eiren.vr.trackers;
 
 import com.jme3.math.Quaternion;
-import com.jme3.math.Vector3f;
 
 public class AdjustedYawTracker extends AdjustedTracker {
 
@@ -13,21 +12,19 @@ public class AdjustedYawTracker extends AdjustedTracker {
 	public void adjust(Quaternion reference) {
 		Quaternion targetTrackerRotation = new Quaternion(reference);
 		
-		// Use only yaw rotation
-		Vector3f hmdFront = new Vector3f(0, 0, 1);
-		targetTrackerRotation.multLocal(hmdFront);
-		hmdFront.multLocal(1, 0, 1).normalizeLocal();
-		targetTrackerRotation.lookAt(hmdFront, Vector3f.UNIT_Y);
+		float[] angles = new float[3];
+		targetTrackerRotation.toAngles(angles);
+		targetTrackerRotation.fromAngles(0, angles[1], 0);
 		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
 		
-		// Adjust only yaw rotation
-		Vector3f sensorFront = new Vector3f(0, 0, 1);
-		sensorRotation.multLocal(sensorFront);
-		sensorFront.multLocal(1, 0, 1).normalizeLocal();
-		sensorRotation.lookAt(sensorFront, Vector3f.UNIT_Y);
+		sensorRotation.toAngles(angles);
+		sensorRotation.fromAngles(0, angles[1], 0);
 		
 		adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+		
+		confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
+		lastAngles[0] = 1000;
 	}
 }

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

@@ -14,6 +14,14 @@ public class ComputedTracker implements Tracker {
 		this.name = name;
 	}
 	
+	@Override
+	public void saveConfig(TrackerConfig config) {
+	}
+	
+	@Override
+	public void loadConfig(TrackerConfig config) {
+	}
+	
 	@Override
 	public String getName() {
 		return this.name;
@@ -39,4 +47,9 @@ public class ComputedTracker implements Tracker {
 	public void setStatus(TrackerStatus status) {
 		this.status = status;
 	}
+	
+	@Override
+	public float getConfidenceLevel() {
+		return 1.0f;
+	}
 }

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

@@ -18,6 +18,7 @@ public class IMUTracker implements Tracker, CalibratingTracker {
 	
 	protected final String name;
 	protected final TrackersUDPServer server;
+	protected float confidence = 0;
 	
 	protected BufferedTimer timer = new BufferedTimer(1f);
 	public CalibrationData newCalibrationData;
@@ -27,6 +28,14 @@ public class IMUTracker implements Tracker, CalibratingTracker {
 		this.server = server;
 	}
 	
+	@Override
+	public void saveConfig(TrackerConfig config) {
+	}
+	
+	@Override
+	public void loadConfig(TrackerConfig config) {
+	}
+	
 	@Override
 	public String getName() {
 		return this.name;
@@ -76,6 +85,15 @@ public class IMUTracker implements Tracker, CalibratingTracker {
 		server.uploadNewCalibrationData(this, newCalibrationData);
 	}
 	
+	@Override
+	public float getConfidenceLevel() {
+		return confidence;
+	}
+	
+	public void setConfidence(float newConf) {
+		this.confidence = newConf;
+	}
+	
 	public static class CalibrationData {
 
 	    //acel offsets and correction matrix
@@ -122,6 +140,7 @@ public class IMUTracker implements Tracker, CalibratingTracker {
 	    }
 		
 		public CalibrationData(ByteBuffer buffer) {
+			buffer.getFloat(); // TODO : WHY???
 			// Data is read in reverse, because it was reversed when sending
 			G_off[2] = buffer.getFloat();
 			G_off[1] = buffer.getFloat();

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

@@ -12,4 +12,10 @@ public interface Tracker {
 	public String getName();
 	
 	public TrackerStatus getStatus();
+	
+	public void loadConfig(TrackerConfig config);
+	
+	public void saveConfig(TrackerConfig config);
+	
+	public float getConfidenceLevel();
 }

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

@@ -159,11 +159,13 @@ public class TrackersUDPServer extends Thread {
 			magData[i * 3 + 2] = line[5];
 		}
 		
+		double accelHnorm = 10000;
+		double magentometerHnorm = 100;
 		
-		System.out.println("[TrackerServer] Accelerometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(accelData, sensor.rawCalibrationData.size()));
-		Magneto.INSTANCE.calculate(accelData, sensor.rawCalibrationData.size(), 2, 10000, accelBasis, accelAInv);
-		System.out.println("[TrackerServer] Magentometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size()));
-		Magneto.INSTANCE.calculate(magData, sensor.rawCalibrationData.size(), 2, 100, magBasis, magAInv);
+		System.out.println("[TrackerServer] Accelerometer Hnorm: " + (accelHnorm = Magneto.INSTANCE.calculateHnorm(accelData, sensor.rawCalibrationData.size())));
+		Magneto.INSTANCE.calculate(accelData, sensor.rawCalibrationData.size(), 0, accelHnorm, accelBasis, accelAInv);
+		System.out.println("[TrackerServer] Magentometer Hnorm: " + (magentometerHnorm = Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size())));
+		Magneto.INSTANCE.calculate(magData, sensor.rawCalibrationData.size(), 0, magentometerHnorm, magBasis, magAInv);
 		
 		System.out.println("float A_B[3] =");
 		System.out.println(String.format("  {%8.2f,%8.2f,%8.2f},", accelBasis[0], accelBasis[1], accelBasis[2]));
@@ -286,6 +288,15 @@ public class TrackersUDPServer extends Thread {
 							dataConsumer.accept(data.toTextMatrix());
 						}
 						break;
+					case 9: // PACKET_RAW_MAGENTOMETER
+						if(sensor == null)
+							break;
+						bb.getLong();
+						float mx = bb.getFloat();
+						float my = bb.getFloat();
+						float mz = bb.getFloat();
+						sensor.tracker.confidence = (float) Math.sqrt(mx * mx + my * my + mz * mz);
+						break;
 					default:
 						System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
 						break;