Refactor trackers chaining and reset

Pose processor now resets trackers accoring to its own tracker hierarchy
Each tracker can now have body part designation independently of pose processor, pose processor scans all trackers to build skeleton hierarchy
Trackers requiring adjustments are now created on the tracker supplier side
Tracker waist distance is now diff with waist position (default is now 0)
Model can now be updated live after tracker body position changed

build.gradle

@@ -22,11 +22,13 @@ dependencies {
     // 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'
 
     // This dependency is used internally, and not exposed to consumers on their own compile classpath.
     implementation 'com.google.guava:guava:28.2-jre'
-    implementation 'net.java.dev.jna:jna:5.6.0'
-    implementation 'net.java.dev.jna:jna-platform:5.6.0'
+    
 
     // Use JUnit test framework
     testImplementation 'junit:junit:4.12'

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

@@ -1,12 +1,9 @@
 package io.eiren.gui;
 
 import java.awt.GridBagConstraints;
-import java.awt.event.MouseEvent;
 import java.util.List;
 
-import javax.swing.JButton;
 import javax.swing.JLabel;
-import javax.swing.event.MouseInputAdapter;
 
 import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
@@ -18,8 +15,7 @@ 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.ReferenceAdjustedTracker;
 import io.eiren.vr.trackers.ComputedTracker;
 import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.IMUTracker;
@@ -248,7 +244,7 @@ public class TrackersList extends EJBag {
 			return 1;
 		if(t instanceof IMUTracker)
 			return 2;
-		if(t instanceof AdjustedTracker)
+		if(t instanceof ReferenceAdjustedTracker)
 			return 5;
 		return 1000;
 	}

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

@@ -1,35 +1,26 @@
 package io.eiren.vr.processor;
 
-import java.util.EnumMap;
-import java.util.Iterator;
 import java.util.List;
-import java.util.Map;
 import java.util.function.Consumer;
 
-import com.jme3.math.Quaternion;
-
 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.HMDTracker;
 import io.eiren.vr.trackers.Tracker;
-import io.eiren.vr.trackers.TrackerConfig;
 import io.eiren.vr.trackers.TrackerStatus;
+import io.eiren.vr.trackers.TrackerUtils;
 
 public class HumanPoseProcessor {
 	
 	private final VRServer server;
-	private final HMDTracker hmd;
 	private final List<ComputedHumanPoseTracker> computedTrackers = new FastList<>();
-	private final Map<TrackerBodyPosition, AdjustedTracker> trackers = new EnumMap<>(TrackerBodyPosition.class);
 	private final List<Consumer<HumanSkeleton>> onSkeletonUpdated = new FastList<>();
 	private HumanSkeleton skeleton;
 
 	public HumanPoseProcessor(VRServer server, HMDTracker hmd) {
 		this.server = server;
-		this.hmd = hmd;
 		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.WAIST));
 		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT));
 		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT));
@@ -62,21 +53,11 @@ public class HumanPoseProcessor {
 
 	@VRServerThread
 	public void trackerAdded(Tracker tracker) {
-		TrackerConfig config = server.getTrackerConfig(tracker);
-		if(config.designation != null) {
-			TrackerBodyPosition pos = TrackerBodyPosition.getByDesignation(config.designation);
-			if(pos != null) {
-				addTracker(tracker, pos);
-			}
-		}
+		updateSekeltonModel();
 	}
 
 	@VRServerThread
-	private void addTracker(Tracker tracker, TrackerBodyPosition position) {
-		AdjustedTracker tt = new AdjustedTracker(tracker);
-		
-		trackers.put(position, tt);
-		server.registerTracker(tt);
+	public void trackerUpdated(Tracker tracker) {
 		updateSekeltonModel();
 	}
 
@@ -84,36 +65,26 @@ public class HumanPoseProcessor {
 	private void updateSekeltonModel() {
 		boolean hasWaist = false;
 		boolean hasBothLegs = false;
-		boolean hasChest = false;
-		if(trackers.get(TrackerBodyPosition.WAIST) != null)
+		List<Tracker> allTrackers = server.getAllTrackers();
+		Tracker waist = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST);
+		Tracker leftAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_ANKLE);
+		Tracker rightAnkle = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_ANKLE);
+		Tracker leftLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.LEFT_LEG);
+		Tracker rightLeg = TrackerUtils.findTrackerForBodyPosition(allTrackers, TrackerBodyPosition.RIGHT_LEG);
+		if(waist != null)
 			hasWaist = true;
-		if(trackers.get(TrackerBodyPosition.CHEST) != null)
-			hasChest = true;
-		if(trackers.get(TrackerBodyPosition.LEFT_ANKLE) != null && trackers.get(TrackerBodyPosition.LEFT_LEG) != null
-				&& trackers.get(TrackerBodyPosition.RIGHT_ANKLE) != null && trackers.get(TrackerBodyPosition.RIGHT_LEG) != null)
+		if(leftAnkle != null && rightAnkle != null && leftLeg != null && rightLeg != null)
 			hasBothLegs = true;
-		if(!hasWaist && !hasChest) {
+		if(!hasWaist) {
 			skeleton = null; // Can't track anything without waist
 		} else if(hasBothLegs) {
 			disconnectAllTrackers();
-			AdjustedTracker waist = trackers.get(TrackerBodyPosition.WAIST);
-			if(waist == null)
-				waist = trackers.get(TrackerBodyPosition.CHEST);
-			AdjustedTracker chest = trackers.get(TrackerBodyPosition.CHEST);
-			if(chest == null)
-				chest = trackers.get(TrackerBodyPosition.WAIST);
-			skeleton = new HumanSekeletonWithLegs(server, waist, chest, trackers, computedTrackers);
+			skeleton = new HumanSekeletonWithLegs(server, computedTrackers);
 			for(int i = 0; i < onSkeletonUpdated.size(); ++i)
 				onSkeletonUpdated.get(i).accept(skeleton);
 		} else {
-			AdjustedTracker waist = trackers.get(TrackerBodyPosition.WAIST);
-			if(waist == null)
-				waist = trackers.get(TrackerBodyPosition.CHEST);
-			AdjustedTracker chest = trackers.get(TrackerBodyPosition.CHEST);
-			if(chest == null)
-				chest = trackers.get(TrackerBodyPosition.WAIST);
 			disconnectAllTrackers();
-			skeleton = new HumanSkeleonWithWaist(server, waist, chest, computedTrackers);
+			skeleton = new HumanSkeleonWithWaist(server, computedTrackers);
 			for(int i = 0; i < onSkeletonUpdated.size(); ++i)
 				onSkeletonUpdated.get(i).accept(skeleton);
 		}
@@ -126,24 +97,15 @@ public class HumanPoseProcessor {
 		}
 	}
 
-	@VRServerThread
-	public void resetTrackers() {
-		Quaternion hmdRotation = new Quaternion();
-		hmd.getRotation(hmdRotation);
-		
-		Iterator<AdjustedTracker> iterator = trackers.values().iterator();
-		while(iterator.hasNext()) {
-			AdjustedTracker tt = iterator.next();
-			tt.adjustFull(hmdRotation);
-			
-			TrackerConfig config = server.getTrackerConfig(tt);
-			tt.saveConfig(config);
-		}
-	}
-
 	@VRServerThread
 	public void update() {
 		if(skeleton != null)
 			skeleton.updatePose();
 	}
+
+	@VRServerThread
+	public void resetTrackers() {
+		if(skeleton != null)
+			skeleton.resetTrackersFull();
+	}
 }

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

@@ -1,14 +1,15 @@
 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.util.ann.VRServerThread;
 import io.eiren.vr.VRServer;
 import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerStatus;
+import io.eiren.vr.trackers.TrackerUtils;
 
 public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	
@@ -53,14 +54,15 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 	
 	protected float kneeLerpFactor = 0.5f;
 
-	public HumanSekeletonWithLegs(VRServer server, Tracker waistTracker, Tracker chestTracker, Map<TrackerBodyPosition, ? extends Tracker> trackers, List<ComputedHumanPoseTracker> computedTrackers) {
-		super(server, waistTracker, chestTracker, computedTrackers);
-		this.leftLegTracker = trackers.get(TrackerBodyPosition.LEFT_LEG);
-		this.leftAnkleTracker = trackers.get(TrackerBodyPosition.LEFT_ANKLE);
-		this.leftFootTracker = trackers.get(TrackerBodyPosition.LEFT_FOOT);
-		this.rightLegTracker = trackers.get(TrackerBodyPosition.RIGHT_LEG);
-		this.rightAnkleTracker = trackers.get(TrackerBodyPosition.RIGHT_ANKLE);
-		this.rightFootTracker = trackers.get(TrackerBodyPosition.RIGHT_FOOT);
+	public HumanSekeletonWithLegs(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
+		super(server, computedTrackers);
+		List<Tracker> allTracekrs = server.getAllTrackers();
+		this.leftLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_LEG);
+		this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_ANKLE);
+		this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.LEFT_FOOT);
+		this.rightLegTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_LEG);
+		this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_ANKLE);
+		this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.RIGHT_FOOT);
 		ComputedHumanPoseTracker lat = null;
 		ComputedHumanPoseTracker rat = null;
 		for(int i = 0; i < computedTrackers.size(); ++i) {
@@ -213,4 +215,35 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
 		computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation());
 		computedRightFootTracker.dataTick();
 	}
+	
+	@Override
+	@VRServerThread
+	public void resetTrackersFull() {
+		// Each tracker uses the tracker before it to adjust iteself,
+		// so trackers that don't need adjustments could be used too
+		super.resetTrackersFull();
+		// Start with waist, it was reset in the parent
+		Quaternion referenceRotation = new Quaternion();
+		this.waistTracker.getRotation(referenceRotation);
+		
+		this.leftLegTracker.resetFull(referenceRotation);
+		this.rightLegTracker.resetFull(referenceRotation);
+		this.leftLegTracker.getRotation(referenceRotation);
+		
+		this.leftAnkleTracker.resetFull(referenceRotation);
+		this.leftAnkleTracker.getRotation(referenceRotation);
+		
+		if(this.leftFootTracker != null) {
+			this.leftFootTracker.resetFull(referenceRotation);
+		}
+
+		this.rightLegTracker.getRotation(referenceRotation);
+		
+		this.rightAnkleTracker.resetFull(referenceRotation);
+		this.rightAnkleTracker.getRotation(referenceRotation);
+		
+		if(this.rightAnkleTracker != null) {
+			this.rightAnkleTracker.resetFull(referenceRotation);
+		}
+	}
 }

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

@@ -12,6 +12,7 @@ import io.eiren.vr.VRServer;
 import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.Tracker;
 import io.eiren.vr.trackers.TrackerStatus;
+import io.eiren.vr.trackers.TrackerUtils;
 
 public class HumanSkeleonWithWaist extends HumanSkeleton {
 	
@@ -43,7 +44,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	 * tracker position, if you want to move resulting
 	 * tracker up or down from actual waist
 	 */
-	protected float trackerWaistDistance = 0.57f;
+	protected float trackerWaistDistance = 0.0f;
 	/**
 	 * Distacne from eyes to the base of the neck
 	 */
@@ -53,9 +54,10 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 	 */
 	protected float headShift = 0.1f;
 
-	public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, Tracker chestTracker, List<ComputedHumanPoseTracker> computedTrackers) {
-		this.waistTracker = waistTracker;
-		this.chestTracker = chestTracker;
+	public HumanSkeleonWithWaist(VRServer server, List<ComputedHumanPoseTracker> computedTrackers) {
+		List<Tracker> allTracekrs = server.getAllTrackers();
+		this.waistTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.WAIST, TrackerBodyPosition.CHEST);
+		this.chestTracker = TrackerUtils.findTrackerForBodyPosition(allTracekrs, TrackerBodyPosition.CHEST, TrackerBodyPosition.WAIST);
 		this.hmdTracker = server.hmdTracker;
 		this.server = server;
 		ComputedHumanPoseTracker cwt = null;
@@ -85,7 +87,7 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0);
 		
 		chestNode.attachChild(trackerWaistNode);
-		trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - chestDistance), 0);
+		trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
 		
 		configMap.put("Head", headShift);
 		configMap.put("Neck", neckLength);
@@ -123,12 +125,12 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 			server.config.setProperty("body.chestDistance", chestDistance);
 			chestNode.localTransform.setTranslation(0, -chestDistance, 0);
 			waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0);
-			trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - chestDistance), 0);
+			trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
 			break;
 		case "Virtual waist":
 			trackerWaistDistance = newLength;
 			server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance);
-			trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - chestDistance), 0);
+			trackerWaistNode.localTransform.setTranslation(0, -(waistDistance + trackerWaistDistance - chestDistance), 0);
 			break;
 		}
 	}
@@ -170,4 +172,18 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
 		computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation());
 		computedWaistTracker.dataTick();
 	}
+	
+	@Override
+	@VRServerThread
+	public void resetTrackersFull() {
+		// Each tracker uses the tracker before it to adjust iteself,
+		// so trackers that don't need adjustments could be used too
+		Quaternion referenceRotation = new Quaternion();
+		server.hmdTracker.getRotation(referenceRotation);
+		
+		this.chestTracker.resetFull(referenceRotation);
+		this.chestTracker.getRotation(referenceRotation);
+		
+		this.waistTracker.resetFull(referenceRotation);
+	}
 }

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

@@ -18,4 +18,7 @@ public abstract class HumanSkeleton {
 
 	@ThreadSafe
 	public abstract void setSkeletonConfig(String key, float newLength);
+
+	@VRServerThread
+	public abstract void resetTrackersFull();
 }

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

@@ -6,6 +6,7 @@ import java.util.Map;
 public enum TrackerBodyPosition {
 	
 	NONE(""),
+	HMD("body:HMD"),
 	CHEST("body:chest"),
 	WAIST("body:waist"),
 	LEFT_LEG("body:left_leg"),

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

@@ -3,12 +3,15 @@ package io.eiren.vr.trackers;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import io.eiren.vr.processor.TrackerBodyPosition;
+
 public class ComputedTracker implements Tracker {
 
 	public final Vector3f position = new Vector3f();
 	public final Quaternion rotation = new Quaternion();
 	protected final String name;
 	protected TrackerStatus status = TrackerStatus.DISCONNECTED;
+	public TrackerBodyPosition bodyPosition = null;
 	
 	public ComputedTracker(String name) {
 		this.name = name;
@@ -16,10 +19,12 @@ public class ComputedTracker implements Tracker {
 	
 	@Override
 	public void saveConfig(TrackerConfig config) {
+		config.setDesignation(bodyPosition == null ? null : bodyPosition.designation);
 	}
 	
 	@Override
 	public void loadConfig(TrackerConfig config) {
+		bodyPosition = TrackerBodyPosition.getByDesignation(config.designation);
 	}
 	
 	@Override
@@ -52,4 +57,22 @@ public class ComputedTracker implements Tracker {
 	public float getConfidenceLevel() {
 		return 1.0f;
 	}
+
+	@Override
+	public void resetFull(Quaternion reference) {
+	}
+
+	@Override
+	public void resetYaw(Quaternion reference) {
+	}
+
+	@Override
+	public TrackerBodyPosition getBodyPosition() {
+		return bodyPosition;
+	}
+
+	@Override
+	public void setBodyPosition(TrackerBodyPosition position) {
+		this.bodyPosition = position;
+	}
 }

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

@@ -1,6 +1,7 @@
 package io.eiren.vr.trackers;
 
 import io.eiren.util.BufferedTimer;
+import io.eiren.vr.processor.TrackerBodyPosition;
 
 public class HMDTracker extends ComputedTracker implements TrackerWithTPS {
 
@@ -8,6 +9,7 @@ public class HMDTracker extends ComputedTracker implements TrackerWithTPS {
 	
 	public HMDTracker(String name) {
 		super(name);
+		setBodyPosition(TrackerBodyPosition.HMD);
 	}
 	
 	@Override

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

@@ -0,0 +1,33 @@
+package io.eiren.vr.trackers;
+
+public class IMUReferenceAdjustedTracker<T extends IMUTracker & TrackerWithTPS & TrackerWithBattery> extends ReferenceAdjustedTracker implements TrackerWithTPS, TrackerWithBattery {
+	
+	public IMUReferenceAdjustedTracker(T tracker) {
+		super(tracker);
+	}
+
+	@SuppressWarnings("unchecked")
+	@Override
+	public float getBatteryLevel() {
+		return ((T) tracker).getBatteryLevel();
+	}
+
+	@SuppressWarnings("unchecked")
+	@Override
+	public float getBatteryVoltage() {
+		return ((T) tracker).getBatteryVoltage();
+	}
+
+	@SuppressWarnings("unchecked")
+	@Override
+	public float getTPS() {
+		return ((T) tracker).getTPS();
+	}
+
+	@SuppressWarnings("unchecked")
+	@Override
+	public void dataTick() {
+		((T) tracker).dataTick();
+	}
+	
+}

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

@@ -1,16 +1,14 @@
 package io.eiren.vr.trackers;
 
-import java.nio.ByteBuffer;
-import java.util.function.Consumer;
-
 import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
 import io.eiren.math.FloatMath;
 import io.eiren.util.BufferedTimer;
+import io.eiren.vr.processor.TrackerBodyPosition;
 
-public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, TrackerWithBattery {
+public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
 	
 	public final Vector3f gyroVector = new Vector3f();
 	public final Vector3f accelVector = new Vector3f();
@@ -25,11 +23,11 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS,
 	protected float batteryVoltage = 0;
 	
 	protected BufferedTimer timer = new BufferedTimer(1f);
-	public ConfigurationData newCalibrationData;
 	public int ping = -1;
 	
 	public StringBuilder serialBuffer = new StringBuilder();
 	long lastSerialUpdate = 0;
+	public TrackerBodyPosition bodyPosition = null;
 	
 	public IMUTracker(String name, TrackersUDPServer server) {
 		this.name = name;
@@ -38,13 +36,17 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS,
 	
 	@Override
 	public void saveConfig(TrackerConfig config) {
+		config.setDesignation(bodyPosition == null ? null : bodyPosition.designation);
 	}
 	
 	@Override
 	public void loadConfig(TrackerConfig config) {
 		if(!FloatMath.equalsToZero(config.trackerRotation)) {
 			rotAdjust.fromAngles(0, config.trackerRotation * FastMath.DEG_TO_RAD, 0);
+		} else {
+			rotAdjust.loadIdentity();
 		}
+		bodyPosition = TrackerBodyPosition.getByDesignation(config.designation);
 	}
 	
 	@Override
@@ -74,11 +76,6 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS,
 		this.status = status;
 	}
 	
-	@Override
-	public void startCalibration(Consumer<String> calibrationDataConsumer) {
-		//server.sendCalibrationCommand(this, calibrationDataConsumer);
-	}
-	
 	@Override
 	public float getTPS() {
 		return timer.getAverageFPS();
@@ -89,16 +86,6 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS,
 		timer.update();
 	}
 	
-	@Override
-	public void requestCalibrationData(Consumer<String> calibrationDataConsumer) {
-		//server.requestCalibrationData(this, calibrationDataConsumer);
-	}
-	
-	@Override
-	public void uploadNewClibrationData() {
-		//server.uploadNewCalibrationData(this, newCalibrationData);
-	}
-	
 	@Override
 	public float getConfidenceLevel() {
 		return confidence;
@@ -121,104 +108,22 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS,
 	public void setBatteryVoltage(float voltage) {
 		this.batteryVoltage = voltage;
 	}
-	
-	public static class ConfigurationData {
 
-	    //acel offsets and correction matrix
-		float[] A_B = new float[3];
-	    float[][] A_Ainv = new float[3][3];
-	    // mag offsets and correction matrix
-	    float[] M_B = new float[3];
-	    float[][] M_Ainv = new float[3][3];
-	    //raw offsets, determined for gyro at rest
-	    float[] G_off = new float[3];
-	    int deviceId = -1;
-	    int deviceMode = -1;
-	    
-	    public ConfigurationData(double[] accelBasis, double[] accelAInv, double[] magBasis, double[] magAInv, double[] gyroOffset) {
-	    	A_B[0] = (float) accelBasis[0];
-	    	A_B[1] = (float) accelBasis[1];
-	    	A_B[2] = (float) accelBasis[2];
-	    	
-	    	A_Ainv[0][0] = (float) accelAInv[0];
-	    	A_Ainv[0][1] = (float) accelAInv[1];
-	    	A_Ainv[0][2] = (float) accelAInv[2];
-	    	A_Ainv[1][0] = (float) accelAInv[3];
-	    	A_Ainv[1][1] = (float) accelAInv[4];
-	    	A_Ainv[1][2] = (float) accelAInv[5];
-	    	A_Ainv[2][0] = (float) accelAInv[6];
-	    	A_Ainv[2][1] = (float) accelAInv[7];
-	    	A_Ainv[2][2] = (float) accelAInv[8];
-	    	
-	    	M_B[0] = (float) magBasis[0];
-	    	M_B[1] = (float) magBasis[1];
-	    	M_B[2] = (float) magBasis[2];
-	    	
-	    	M_Ainv[0][0] = (float) magAInv[0];
-	    	M_Ainv[0][1] = (float) magAInv[1];
-	    	M_Ainv[0][2] = (float) magAInv[2];
-	    	M_Ainv[1][0] = (float) magAInv[3];
-	    	M_Ainv[1][1] = (float) magAInv[4];
-	    	M_Ainv[1][2] = (float) magAInv[5];
-	    	M_Ainv[2][0] = (float) magAInv[6];
-	    	M_Ainv[2][1] = (float) magAInv[7];
-	    	M_Ainv[2][2] = (float) magAInv[8];
-	    	
-	    	G_off[0] = (float) gyroOffset[0];
-	    	G_off[1] = (float) gyroOffset[1];
-	    	G_off[2] = (float) gyroOffset[2];
-	    }
-		
-		public ConfigurationData(ByteBuffer buffer) {
-			deviceMode = buffer.getInt();
-			deviceId = buffer.getInt();
-			// Data is read in reverse, because it was reversed when sending
-			G_off[2] = buffer.getFloat();
-			G_off[1] = buffer.getFloat();
-			G_off[0] = buffer.getFloat();
-			
-			M_Ainv[2][2] = buffer.getFloat();
-			M_Ainv[2][1] = buffer.getFloat();
-			M_Ainv[2][0] = buffer.getFloat();
-			M_Ainv[1][2] = buffer.getFloat();
-			M_Ainv[1][1] = buffer.getFloat();
-			M_Ainv[1][0] = buffer.getFloat();
-			M_Ainv[0][2] = buffer.getFloat();
-			M_Ainv[0][1] = buffer.getFloat();
-			M_Ainv[0][0] = buffer.getFloat();
-			
-			M_B[2] = buffer.getFloat();
-			M_B[1] = buffer.getFloat();
-			M_B[0] = buffer.getFloat();
-			
-			A_Ainv[2][2] = buffer.getFloat();
-			A_Ainv[2][1] = buffer.getFloat();
-			A_Ainv[2][0] = buffer.getFloat();
-			A_Ainv[1][2] = buffer.getFloat();
-			A_Ainv[1][1] = buffer.getFloat();
-			A_Ainv[1][0] = buffer.getFloat();
-			A_Ainv[0][2] = buffer.getFloat();
-			A_Ainv[0][1] = buffer.getFloat();
-			A_Ainv[0][0] = buffer.getFloat();
-			
-			A_B[2] = buffer.getFloat();
-			A_B[1] = buffer.getFloat();
-			A_B[0] = buffer.getFloat();
-		}
-		
-		public String toTextMatrix() {
-			StringBuilder sb = new StringBuilder();
-			sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", A_B[0], A_B[1], A_B[2]));
-			sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", A_Ainv[0][0], A_Ainv[0][1], A_Ainv[0][2]));
-			sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", A_Ainv[1][0], A_Ainv[1][1], A_Ainv[1][2]));
-			sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", A_Ainv[2][0], A_Ainv[2][1], A_Ainv[2][2]));
-			sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", M_B[0], M_B[1], M_B[2]));
-			sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", M_Ainv[0][0], M_Ainv[0][1], M_Ainv[0][2]));
-			sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", M_Ainv[1][0], M_Ainv[1][1], M_Ainv[1][2]));
-			sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", M_Ainv[2][0], M_Ainv[2][1], M_Ainv[2][2]));
-			sb.append(String.format("{%8.2f, %8.2f, %8.2f}};\n", G_off[0], G_off[1], G_off[2]));
-			
-			return sb.toString();
-		}
+	@Override
+	public void resetFull(Quaternion reference) {
+	}
+
+	@Override
+	public void resetYaw(Quaternion reference) {
+	}
+
+	@Override
+	public TrackerBodyPosition getBodyPosition() {
+		return bodyPosition;
+	}
+
+	@Override
+	public void setBodyPosition(TrackerBodyPosition position) {
+		this.bodyPosition = position;
 	}
 }

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

@@ -0,0 +1,112 @@
+package io.eiren.vr.trackers;
+
+import java.nio.ByteBuffer;
+
+public class MPUTracker extends IMUTracker {
+	
+	public ConfigurationData newCalibrationData;
+	
+	public MPUTracker(String name, TrackersUDPServer server) {
+		super(name, server);
+	}
+	
+	public static class ConfigurationData {
+
+	    //acel offsets and correction matrix
+		float[] A_B = new float[3];
+	    float[][] A_Ainv = new float[3][3];
+	    // mag offsets and correction matrix
+	    float[] M_B = new float[3];
+	    float[][] M_Ainv = new float[3][3];
+	    //raw offsets, determined for gyro at rest
+	    float[] G_off = new float[3];
+	    int deviceId = -1;
+	    int deviceMode = -1;
+	    
+	    public ConfigurationData(double[] accelBasis, double[] accelAInv, double[] magBasis, double[] magAInv, double[] gyroOffset) {
+	    	A_B[0] = (float) accelBasis[0];
+	    	A_B[1] = (float) accelBasis[1];
+	    	A_B[2] = (float) accelBasis[2];
+	    	
+	    	A_Ainv[0][0] = (float) accelAInv[0];
+	    	A_Ainv[0][1] = (float) accelAInv[1];
+	    	A_Ainv[0][2] = (float) accelAInv[2];
+	    	A_Ainv[1][0] = (float) accelAInv[3];
+	    	A_Ainv[1][1] = (float) accelAInv[4];
+	    	A_Ainv[1][2] = (float) accelAInv[5];
+	    	A_Ainv[2][0] = (float) accelAInv[6];
+	    	A_Ainv[2][1] = (float) accelAInv[7];
+	    	A_Ainv[2][2] = (float) accelAInv[8];
+	    	
+	    	M_B[0] = (float) magBasis[0];
+	    	M_B[1] = (float) magBasis[1];
+	    	M_B[2] = (float) magBasis[2];
+	    	
+	    	M_Ainv[0][0] = (float) magAInv[0];
+	    	M_Ainv[0][1] = (float) magAInv[1];
+	    	M_Ainv[0][2] = (float) magAInv[2];
+	    	M_Ainv[1][0] = (float) magAInv[3];
+	    	M_Ainv[1][1] = (float) magAInv[4];
+	    	M_Ainv[1][2] = (float) magAInv[5];
+	    	M_Ainv[2][0] = (float) magAInv[6];
+	    	M_Ainv[2][1] = (float) magAInv[7];
+	    	M_Ainv[2][2] = (float) magAInv[8];
+	    	
+	    	G_off[0] = (float) gyroOffset[0];
+	    	G_off[1] = (float) gyroOffset[1];
+	    	G_off[2] = (float) gyroOffset[2];
+	    }
+		
+		public ConfigurationData(ByteBuffer buffer) {
+			deviceMode = buffer.getInt();
+			deviceId = buffer.getInt();
+			// Data is read in reverse, because it was reversed when sending
+			G_off[2] = buffer.getFloat();
+			G_off[1] = buffer.getFloat();
+			G_off[0] = buffer.getFloat();
+			
+			M_Ainv[2][2] = buffer.getFloat();
+			M_Ainv[2][1] = buffer.getFloat();
+			M_Ainv[2][0] = buffer.getFloat();
+			M_Ainv[1][2] = buffer.getFloat();
+			M_Ainv[1][1] = buffer.getFloat();
+			M_Ainv[1][0] = buffer.getFloat();
+			M_Ainv[0][2] = buffer.getFloat();
+			M_Ainv[0][1] = buffer.getFloat();
+			M_Ainv[0][0] = buffer.getFloat();
+			
+			M_B[2] = buffer.getFloat();
+			M_B[1] = buffer.getFloat();
+			M_B[0] = buffer.getFloat();
+			
+			A_Ainv[2][2] = buffer.getFloat();
+			A_Ainv[2][1] = buffer.getFloat();
+			A_Ainv[2][0] = buffer.getFloat();
+			A_Ainv[1][2] = buffer.getFloat();
+			A_Ainv[1][1] = buffer.getFloat();
+			A_Ainv[1][0] = buffer.getFloat();
+			A_Ainv[0][2] = buffer.getFloat();
+			A_Ainv[0][1] = buffer.getFloat();
+			A_Ainv[0][0] = buffer.getFloat();
+			
+			A_B[2] = buffer.getFloat();
+			A_B[1] = buffer.getFloat();
+			A_B[0] = buffer.getFloat();
+		}
+		
+		public String toTextMatrix() {
+			StringBuilder sb = new StringBuilder();
+			sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", A_B[0], A_B[1], A_B[2]));
+			sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", A_Ainv[0][0], A_Ainv[0][1], A_Ainv[0][2]));
+			sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", A_Ainv[1][0], A_Ainv[1][1], A_Ainv[1][2]));
+			sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", A_Ainv[2][0], A_Ainv[2][1], A_Ainv[2][2]));
+			sb.append(String.format("{%8.2f,%8.2f,%8.2f},\n", M_B[0], M_B[1], M_B[2]));
+			sb.append(String.format("{{%9.5f,%9.5f,%9.5f},\n", M_Ainv[0][0], M_Ainv[0][1], M_Ainv[0][2]));
+			sb.append(String.format(" {%9.5f,%9.5f,%9.5f},\n", M_Ainv[1][0], M_Ainv[1][1], M_Ainv[1][2]));
+			sb.append(String.format(" {%9.5f,%9.5f,%9.5f}},\n", M_Ainv[2][0], M_Ainv[2][1], M_Ainv[2][2]));
+			sb.append(String.format("{%8.2f, %8.2f, %8.2f}};\n", G_off[0], G_off[1], G_off[2]));
+			
+			return sb.toString();
+		}
+	}
+}

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

@@ -4,7 +4,9 @@ import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
-public class AdjustedTracker implements Tracker {
+import io.eiren.vr.processor.TrackerBodyPosition;
+
+public class ReferenceAdjustedTracker implements Tracker {
 	
 	public final Tracker tracker;
 	private final Quaternion smoothedQuaternion = new Quaternion();
@@ -13,12 +15,9 @@ public class AdjustedTracker implements Tracker {
 	protected float[] lastAngles = new float[3];
 	public float smooth = 0 * FastMath.DEG_TO_RAD;
 	private final float[] angles = new float[3];
-	private float pitchCorrection = 0;
-	private float rollCorrection = 0;
-	
 	protected float confidenceMultiplier = 1.0f;
 	
-	public AdjustedTracker(Tracker tracker) {
+	public ReferenceAdjustedTracker(Tracker tracker) {
 		this.tracker = tracker;
 	}
 
@@ -30,8 +29,9 @@ public class AdjustedTracker implements Tracker {
 	public void saveConfig(TrackerConfig config) {
 	}
 
-	public void adjustFull(Quaternion reference) {
-		adjustYaw(reference);
+	@Override
+	public void resetFull(Quaternion reference) {
+		resetYaw(reference);
 		
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
@@ -40,7 +40,8 @@ public class AdjustedTracker implements Tracker {
 		adjustmentAttachment.set(sensorRotation).inverseLocal();
 	}
 	
-	public void adjustYaw(Quaternion reference) {
+	@Override
+	public void resetYaw(Quaternion reference) {
 		Quaternion targetTrackerRotation = new Quaternion(reference);
 
 		// Use only yaw HMD rotation
@@ -100,4 +101,14 @@ public class AdjustedTracker implements Tracker {
 	public float getConfidenceLevel() {
 		return tracker.getConfidenceLevel() * confidenceMultiplier;
 	}
+
+	@Override
+	public TrackerBodyPosition getBodyPosition() {
+		return tracker.getBodyPosition();
+	}
+
+	@Override
+	public void setBodyPosition(TrackerBodyPosition position) {
+		tracker.setBodyPosition(position);
+	}
 }

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

@@ -3,6 +3,8 @@ package io.eiren.vr.trackers;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import io.eiren.vr.processor.TrackerBodyPosition;
+
 public interface Tracker {
 	
 	public boolean getPosition(Vector3f store);
@@ -18,4 +20,12 @@ public interface Tracker {
 	public void saveConfig(TrackerConfig config);
 	
 	public float getConfidenceLevel();
+	
+	public void resetFull(Quaternion reference);
+	
+	public void resetYaw(Quaternion reference);
+	
+	public TrackerBodyPosition getBodyPosition();
+	
+	public void setBodyPosition(TrackerBodyPosition position);
 }

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

@@ -0,0 +1,27 @@
+package io.eiren.vr.trackers;
+
+import java.util.List;
+
+import io.eiren.vr.processor.TrackerBodyPosition;
+
+public class TrackerUtils {
+	
+	private TrackerUtils() {
+	}
+	
+	public static Tracker findTrackerForBodyPosition(List<Tracker> allTrackers, TrackerBodyPosition position) {
+		for(int i = 0; i < allTrackers.size(); ++i) {
+			Tracker t = allTrackers.get(i);
+			if(t.getBodyPosition() == position)
+				return t;
+		}
+		return null;
+	}
+	
+	public static Tracker findTrackerForBodyPosition(List<Tracker> allTrackers, TrackerBodyPosition position, TrackerBodyPosition altPosition) {
+		Tracker t = findTrackerForBodyPosition(allTrackers, position);
+		if(t != null)
+			return t;
+		return findTrackerForBodyPosition(allTrackers, altPosition);
+	}
+}

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

@@ -41,13 +41,13 @@ public class TrackersUDPServer extends Thread {
 	private final List<TrackerConnection> trackers = new FastList<>();
 	private final Map<SocketAddress, TrackerConnection> trackersMap = new HashMap<>();
 	private final Map<Tracker, Consumer<String>> calibrationDataRequests = new HashMap<>();
-	private final Consumer<IMUTracker> trackersConsumer;
+	private final Consumer<Tracker> trackersConsumer;
 	private final int port;
 	
 	protected DatagramSocket socket = null;
 	protected long lastKeepup = System.currentTimeMillis();
 	
-	public TrackersUDPServer(int port, String name, Consumer<IMUTracker> trackersConsumer) {
+	public TrackersUDPServer(int port, String name, Consumer<Tracker> trackersConsumer) {
 		super(name);
 		this.port = port;
 		this.trackersConsumer = trackersConsumer;
@@ -90,7 +90,8 @@ public class TrackersUDPServer extends Thread {
 			if(sb.length() == 0)
 				sb.append("owoTrack");
 			IMUTracker imu = new IMUTracker("udp:/" + handshakePacket.getAddress().toString(), this);
-			trackersConsumer.accept(imu);
+			IMUReferenceAdjustedTracker<IMUTracker> adjustedTracker = new IMUReferenceAdjustedTracker<>(imu);
+			trackersConsumer.accept(adjustedTracker);
 			sensor = new TrackerConnection(imu, addr);
 			int i = 0;
 			synchronized(trackers) {
@@ -108,7 +109,8 @@ public class TrackersUDPServer extends Thread {
 		System.out.println("[TrackerServer] Setting up auxilary sensor for " + connection.tracker.getName());
 		IMUTracker imu = new IMUTracker(connection.tracker.getName() + "/1", this);
 		connection.secondTracker = imu;
-		trackersConsumer.accept(imu);
+		IMUReferenceAdjustedTracker<IMUTracker> adjustedTracker = new IMUReferenceAdjustedTracker<>(imu);
+		trackersConsumer.accept(adjustedTracker);
 		System.out.println("[TrackerServer] Sensor added with address " + imu.getName());
 	}
 	
@@ -135,6 +137,8 @@ public class TrackersUDPServer extends Thread {
 						sensor.lastPacket = System.currentTimeMillis();
 					int packetId;
 					switch(packetId = bb.getInt()) {
+					case 0:
+						break;
 					case 3:
 						setUpNewSensor(recieve, bb);
 						break;
@@ -196,7 +200,7 @@ public class TrackersUDPServer extends Thread {
 						if(sensor == null)
 							break;
 						bb.getLong();
-						IMUTracker.ConfigurationData data = new IMUTracker.ConfigurationData(bb);
+						MPUTracker.ConfigurationData data = new MPUTracker.ConfigurationData(bb);
 						Consumer<String> dataConsumer = calibrationDataRequests.remove(sensor.tracker);
 						if(dataConsumer != null) {
 							dataConsumer.accept(data.toTextMatrix());