Implemented human pose resolve with wasit tracker

Added configuration

build.gradle

@@ -15,11 +15,13 @@ repositories {
     // Use jcenter for resolving dependencies.
     // You can declare any Maven/Ivy/file repository here.
     jcenter()
+    mavenCentral()
 }
 
 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'
 
     // This dependency is used internally, and not exposed to consumers on their own compile classpath.
     implementation 'com.google.guava:guava:28.2-jre'

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

@@ -1,14 +1,29 @@
 package io.eiren.vr;
 
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.FileNotFoundException;
+import java.io.FileOutputStream;
+import java.io.IOException;
+import java.util.HashMap;
+import java.util.Iterator;
 import java.util.List;
+import java.util.Map;
+import java.util.Queue;
+import java.util.concurrent.LinkedBlockingQueue;
 
+import essentia.util.ann.ThreadSafe;
 import essentia.util.ann.ThreadSecure;
 import essentia.util.collections.FastList;
 import io.eiren.vr.bridge.NamedPipeVRBridge;
 import io.eiren.vr.processor.HumanPoseProcessor;
 import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.TrackersUDPServer;
+import io.eiren.yaml.YamlException;
+import io.eiren.yaml.YamlFile;
+import io.eiren.yaml.YamlNode;
 import io.eiren.vr.trackers.Tracker;
+import io.eiren.vr.trackers.TrackerConfig;
 
 public class VRServer extends Thread {
 	
@@ -16,25 +31,86 @@ public class VRServer extends Thread {
 	public final HumanPoseProcessor humanPoseProcessor;
 	private final TrackersUDPServer trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker);
 	private final NamedPipeVRBridge driverBridge;
+	private final Queue<Runnable> tasks = new LinkedBlockingQueue<>();
+	private final Map<String, TrackerConfig> configuration = new HashMap<>();
+	public final YamlFile config = new YamlFile();
+	public final HMDTracker hmdTracker;
 	
 	public VRServer() {
 		super("VRServer");
-		HMDTracker hmd = new HMDTracker("HMD");
-		humanPoseProcessor = new HumanPoseProcessor(hmd);
+		hmdTracker = new HMDTracker("HMD");
+		humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker);
 		List<? extends Tracker> shareTrackers = humanPoseProcessor.getComputedTrackers();
-		driverBridge = new NamedPipeVRBridge(hmd, shareTrackers);
+		driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers);
 		
-		registerTracker(hmd);
+		registerTracker(hmdTracker);
 		for(int i = 0; i < shareTrackers.size(); ++i)
 			registerTracker(shareTrackers.get(i));
 	}
 	
+	@ThreadSafe
+	public TrackerConfig getTrackerConfig(Tracker tracker) {
+		synchronized(configuration) {
+			TrackerConfig config = configuration.get(tracker.getName());
+			if(config == null) {
+				config = new TrackerConfig(tracker.getName());
+				configuration.put(tracker.getName(), config);
+			}
+			return config;
+		}
+	}
+	
+	private void loadConfig() {
+		try {
+			config.load(new FileInputStream(new File("vrconfig.yml")));
+		} catch(IOException e) {
+			e.printStackTrace();
+		} catch(YamlException e) {
+			e.printStackTrace();
+		}
+		List<YamlNode> trackersConfig = config.getNodeList("trackers", null);
+		for(int i = 0; i < trackersConfig.size(); ++i) {
+			TrackerConfig cfg = new TrackerConfig(trackersConfig.get(i));
+			synchronized(configuration) {
+				configuration.put(cfg.trackerName, cfg);
+			}
+		}
+	}
+
+	@ThreadSafe
+	public void saveConfig() {
+		List<Object> trackersConfig = new FastList<>();
+		config.setProperty("trackers", trackersConfig);
+		synchronized(configuration) {
+			Iterator<TrackerConfig> iterator = configuration.values().iterator();
+			while(iterator.hasNext()) {
+				TrackerConfig tc = iterator.next();
+				Map<String, Object> cfg = new HashMap<>();
+				trackersConfig.add(cfg);
+				tc.saveConfig(new YamlNode(cfg));
+			}
+		}
+		File cfgFile = new File("vrconfig.yml");
+		try {
+			config.save(new FileOutputStream(cfgFile));
+		} catch(IOException e) {
+			e.printStackTrace();
+		}
+	}
+	
 	@Override
 	public void run() {
+		loadConfig();
 		trackersServer.start();
 		driverBridge.start();
 		while(true) {
 			final long start = System.currentTimeMillis();
+			do {
+				Runnable task = tasks.poll();
+				if(task == null)
+					break;
+				task.run();
+			} while(true);
 			
 			humanPoseProcessor.update();
 			
@@ -46,8 +122,14 @@ public class VRServer extends Thread {
 		}
 	}
 	
+	public void queueTask(Runnable r) {
+		tasks.add(r);
+	}
+	
 	private void autoAssignTracker(Tracker tracker) {
-		//
+		queueTask(() -> {
+			humanPoseProcessor.trackerAdded(tracker);
+		});
 	}
 	
 	@ThreadSecure
@@ -57,4 +139,28 @@ public class VRServer extends Thread {
 		}
 		autoAssignTracker(tracker);
 	}
+	
+	public void calibrate(Tracker tracker) {
+		if(tracker.getName().startsWith("udp://")) {
+			trackersServer.sendCalibrationCommand(tracker);
+		}
+	}
+	
+	public void resetTrackers() {
+		queueTask(() -> {
+			humanPoseProcessor.resetTrackers();
+		});
+	}
+	
+	public int getTrackersCount() {
+		synchronized(trackers) {
+			return trackers.size();
+		}
+	}
+	
+	public List<Tracker> getAllTrackers() {
+		synchronized(trackers) {
+			return new FastList<>(trackers);
+		}
+	}
 }

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

@@ -0,0 +1,13 @@
+package io.eiren.vr.processor;
+
+import io.eiren.vr.trackers.ComputedTracker;
+
+public class ComputedHumanPoseTracker extends ComputedTracker {
+	
+	public final ComputedHumanPoseTrackerPosition skeletonPosition;
+
+	public ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition skeletonPosition) {
+		super("human://" + skeletonPosition.name());
+		this.skeletonPosition = skeletonPosition;
+	}
+}

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

@@ -0,0 +1,8 @@
+package io.eiren.vr.processor;
+
+public enum ComputedHumanPoseTrackerPosition {
+	
+	WAIST,
+	LEFT_FOOT,
+	RIGHT_FOOT;
+}

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

@@ -4,83 +4,147 @@ import java.util.EnumMap;
 import java.util.Iterator;
 import java.util.List;
 
-import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
 
 import essentia.util.collections.FastList;
-import io.eiren.vr.trackers.ComputedTracker;
+import io.eiren.vr.VRServer;
 import io.eiren.vr.trackers.HMDTracker;
 import io.eiren.vr.trackers.Tracker;
+import io.eiren.vr.trackers.TrackerConfig;
+import io.eiren.vr.trackers.TrackerStatus;
 
 public class HumanPoseProcessor {
 	
+	private final VRServer server;
 	private final HMDTracker hmd;
-	private final List<ComputedTracker> computedTrackers = new FastList<>();
-	private final EnumMap<TrackerPosition, TransformedTracker> trackers = new EnumMap<>(TrackerPosition.class);
-	private final ComputedTracker waist;
-	private final ComputedTracker leftFoot;
-	private final ComputedTracker rightFoot;
-	
-	public HumanPoseProcessor(HMDTracker hmd) {
+	private final List<ComputedHumanPoseTracker> computedTrackers = new FastList<>();
+	private final EnumMap<TrackerBodyPosition, AdjustedTracker> trackers = new EnumMap<>(TrackerBodyPosition.class);
+	private HumanSkeleton skeleton;
+
+	public HumanPoseProcessor(VRServer server, HMDTracker hmd) {
+		this.server = server;
 		this.hmd = hmd;
-		computedTrackers.add(waist = new ComputedTracker("Waist"));
-		computedTrackers.add(leftFoot = new ComputedTracker("Left Foot"));
-		computedTrackers.add(rightFoot = new ComputedTracker("Right Foot"));
+		computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.WAIST));
+		//computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT));
+		//computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT));
 	}
 	
 	public List<? extends Tracker> getComputedTrackers() {
 		return computedTrackers;
 	}
 	
-	public void addTracker(Tracker tracker, TrackerPosition position) {
-		TransformedTracker tt = new TransformedTracker(tracker);
-		synchronized(trackers) {
-			trackers.put(position, tt);
-		}
-	}
-	
-	public void resetTrackers() {
-		Quaternion buff = new Quaternion();
-		Quaternion targetRotation = new Quaternion();
-		hmd.getRotation(targetRotation);
-		
-		// TODO
-		
-		synchronized(trackers) {
-			Iterator<TransformedTracker> iterator = trackers.values().iterator();
-			while(iterator.hasNext()) {
-				TransformedTracker tt = iterator.next();
-				tt.getRotation(buff);
-				// TODO : Set offset
+	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);
 			}
 		}
 	}
 	
+	private void addTracker(Tracker tracker, TrackerBodyPosition position) {
+		AdjustedTracker tt = new AdjustedTracker(tracker, position);
+		
+		TrackerConfig config = server.getTrackerConfig(tt);
+		if(config.adjustment != null)
+			tt.adjustment.set(config.adjustment);
+		
+		trackers.put(position, tt);
+		server.registerTracker(tt);
+		updateSekeltonModel();
+	}
+	
+	private void updateSekeltonModel() {
+		boolean hasWaist = false;
+		//boolean hasBothLegs = false;
+		//boolean hasChest = false;
+		if(trackers.get(TrackerBodyPosition.WAIST) != null)
+			hasWaist = true;
+		//if(trackers.get(TrackerBodyPosition.CHEST) != null)
+		//	hasChest = true;
+		//if(trackers.get(TrackerBodyPosition.LEFT_FOOT) != null && trackers.get(TrackerBodyPosition.LEFT_LEG) != null
+		//		&& trackers.get(TrackerBodyPosition.RIGHT_FOOT) != null && trackers.get(TrackerBodyPosition.RIGHT_LEG) != null)
+		//	hasBothLegs = true;
+		if(!hasWaist) {
+			skeleton = null; // Can't track anything without waist
+		} else {
+			// TODO : Add legs and chest support
+			if(skeleton instanceof HumanSkeleonWithWaist) {
+				return; // Proper skeleton applied
+			}
+			skeleton = new HumanSkeleonWithWaist(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers);
+		}
+	}
+	
+	public void resetTrackers() {
+		Quaternion sensorRotation = new Quaternion();
+		Quaternion hmdRotation = new Quaternion();
+		Quaternion targetTrackerRotation = new Quaternion();
+		hmd.getRotation(hmdRotation);
+
+		// Adjust only yaw rotation
+		Vector3f hmdFront = new Vector3f(0, 0, 1);
+		hmdRotation.multLocal(hmdFront);
+		hmdFront.multLocal(1, 0, 1).normalizeLocal();
+		hmdRotation.lookAt(hmdFront, Vector3f.UNIT_Y);
+		
+		Iterator<AdjustedTracker> iterator = trackers.values().iterator();
+		while(iterator.hasNext()) {
+			AdjustedTracker tt = iterator.next();
+			tt.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);
+			
+			
+			tt.position.baseRotation.mult(hmdRotation, targetTrackerRotation);
+			tt.adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
+			
+			TrackerConfig config = server.getTrackerConfig(tt);
+			config.adjustment = new Quaternion(tt.adjustment);
+		}
+	}
+	
 	public void update() {
-		
+		if(skeleton != null)
+			skeleton.updatePose();
 	}
 	
-	public enum TrackerPosition {
-		NECK,
-		CHEST,
-		WAIST,
-		LEFT_LEG,
-		RIGHT_LEG,
-		LEFT_FOOT,
-		RIGHT_FOOT
-	}
-	
-	private static class TransformedTracker {
+	private static class AdjustedTracker implements Tracker {
 		public final Tracker tracker;
-		public final Quaternion transformation = new Quaternion();
+		public final Quaternion adjustment = new Quaternion();
+		public final TrackerBodyPosition position;
 		
-		public TransformedTracker(Tracker tracker) {
+		public AdjustedTracker(Tracker tracker, TrackerBodyPosition position) {
 			this.tracker = tracker;
+			this.position = position;
 		}
 		
-		public void getRotation(Quaternion store) {
+		@Override
+		public boolean getRotation(Quaternion store) {
 			tracker.getRotation(store);
-			store.multLocal(transformation);
+			adjustment.mult(store, store);
+			return true;
+		}
+
+		@Override
+		public boolean getPosition(Vector3f store) {
+			return tracker.getPosition(store);
+		}
+
+		@Override
+		public String getName() {
+			return tracker.getName() + "/adj";
+		}
+
+		@Override
+		public TrackerStatus getStatus() {
+			return tracker.getStatus();
 		}
 	}
 }

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

@@ -0,0 +1,62 @@
+package io.eiren.vr.processor;
+
+import java.util.List;
+
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+
+import io.eiren.vr.VRServer;
+import io.eiren.vr.trackers.HMDTracker;
+import io.eiren.vr.trackers.Tracker;
+
+public class HumanSkeleonWithWaist extends HumanSkeleton {
+	
+	protected final Quaternion qBuf = new Quaternion();
+	protected final Vector3f vBuf = new Vector3f();
+	
+	protected final Tracker wasitTracker;
+	protected final HMDTracker hmdTracker;
+	protected final ComputedHumanPoseTracker computedWaistTracker;
+	protected float waistDistance = 0.63f;
+	protected float waistSwingMultiplier = 1f;
+	protected final TransformNode hmdNode = new TransformNode();
+	protected final TransformNode waistNode = new TransformNode();
+
+	public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, List<ComputedHumanPoseTracker> computedTrackers) {
+		this.wasitTracker = waistTracker;
+		this.hmdTracker = server.hmdTracker;
+		ComputedHumanPoseTracker cwt = null;
+		for(int i = 0; i < computedTrackers.size(); ++i) {
+			ComputedHumanPoseTracker t = computedTrackers.get(i);
+			if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.WAIST)
+				cwt = t;
+		}
+		computedWaistTracker = cwt;
+		waistDistance = server.config.getFloat("body.waistDistance", waistDistance);
+		waistSwingMultiplier = server.config.getFloat("body.waistSwingMultiplier", waistSwingMultiplier);
+		// Build skeleton
+		hmdNode.attachChild(waistNode);
+		waistNode.localTransform.setTranslation(0, -waistDistance, 0);
+	}
+	
+	@Override
+	public void updatePose() {
+		wasitTracker.getRotation(qBuf);
+		if(waistSwingMultiplier != 1.0) {
+			// TODO : Adjust waist swing if swing multiplier != 0
+		}
+		
+		hmdTracker.getPosition(vBuf);
+		hmdNode.localTransform.setTranslation(vBuf);
+		hmdNode.localTransform.setRotation(qBuf);
+		
+		hmdNode.update();
+		
+		updateTrackers();
+	}
+	
+	protected void updateTrackers() {
+		computedWaistTracker.position.set(waistNode.worldTransform.getTranslation());
+		computedWaistTracker.rotation.set(waistNode.worldTransform.getRotation());
+	}
+}

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

@@ -0,0 +1,8 @@
+package io.eiren.vr.processor;
+
+public abstract class HumanSkeleton {
+	
+	public abstract void updatePose();
+	
+	
+}

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

@@ -0,0 +1,36 @@
+package io.eiren.vr.processor;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import com.jme3.math.Quaternion;
+
+public enum TrackerBodyPosition {
+	
+	CHEST(Quaternion.IDENTITY, "body:chest"),
+	WAIST(Quaternion.IDENTITY, "body:waist"),
+	LEFT_LEG(Quaternion.IDENTITY, "body:left_leg"),
+	RIGHT_LEG(Quaternion.IDENTITY, "body:right_leg"),
+	LEFT_FOOT(Quaternion.IDENTITY, "body:left_foot"),
+	RIGHT_FOOT(Quaternion.IDENTITY, "body:right_foot"),
+	;
+	
+	public final Quaternion baseRotation;
+	public final String designation;
+	
+	private static final Map<String, TrackerBodyPosition> byDesignation = new HashMap<>();
+	
+	private TrackerBodyPosition(Quaternion base, String designation) {
+		this.baseRotation = base;
+		this.designation = designation;
+	}
+	
+	public static TrackerBodyPosition getByDesignation(String designation) {
+		return byDesignation.get(designation.toLowerCase());
+	}
+	
+	static {
+		for(TrackerBodyPosition tbp : values())
+			byDesignation.put(tbp.designation, tbp);
+	}
+}

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

@@ -0,0 +1,35 @@
+package io.eiren.vr.processor;
+
+import java.util.List;
+
+import com.jme3.math.Transform;
+
+import essentia.util.collections.FastList;
+
+public class TransformNode {
+	
+	public final Transform localTransform = new Transform();
+	public final Transform worldTransform = new Transform();
+	public final List<TransformNode> children = new FastList<>();
+	private TransformNode parent;
+	
+	public void attachChild(TransformNode node) {
+		this.children.add(node);
+		node.parent = this;
+	}
+	
+	public void update() {
+		updateWorldTransforms(); // Call update on each frame because we have relatively few nodes
+		for(int i = 0; i < children.size(); ++i)
+			children.get(i).update();
+	}
+	
+	protected synchronized void updateWorldTransforms() {
+		if(parent == null) {
+			worldTransform.set(localTransform);
+		} else {
+			worldTransform.set(localTransform);
+			worldTransform.combineWithParent(parent.worldTransform);
+		}
+	}
+}

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

@@ -0,0 +1,51 @@
+package io.eiren.vr.trackers;
+
+import com.jme3.math.Quaternion;
+
+import io.eiren.yaml.YamlNode;
+
+public class TrackerConfig {
+	
+	public final String trackerName;
+	public String designation;
+	public boolean hide;
+	public Quaternion adjustment;
+	
+	public TrackerConfig(String trackerName) {
+		this.trackerName = trackerName;
+	}
+	
+	public TrackerConfig(YamlNode node) {
+		this.trackerName = node.getString("name");
+		this.designation = node.getString("designation");
+		this.hide = node.getBoolean("hide", false);
+		YamlNode adjNode = node.getNode("adjustment");
+		if(adjNode != null) {
+			adjustment = new Quaternion(adjNode.getFloat("x", 0), adjNode.getFloat("y", 0), adjNode.getFloat("z", 0), adjNode.getFloat("w", 0));
+		}
+	}
+	
+	public void setDesignation(String newDesignation) {
+		this.designation = newDesignation;
+	}
+	
+	public void saveConfig(YamlNode configNode) {
+		configNode.setProperty("name", trackerName);
+		if(designation != null)
+			configNode.setProperty("designation", designation);
+		else
+			configNode.removeProperty("designation");
+		if(hide)
+			configNode.setProperty("hide", hide);
+		else
+			configNode.removeProperty("hide");
+		if(adjustment != null) {
+			configNode.setProperty("adj.x", adjustment.getX());
+			configNode.setProperty("adj.y", adjustment.getY());
+			configNode.setProperty("adj.z", adjustment.getZ());
+			configNode.setProperty("adj.w", adjustment.getW());
+		} else {
+			configNode.removeProperty("adj");
+		}
+	}
+}