diff --git a/build.gradle b/build.gradle index 05e579cf3..cb47fb551 100644 --- a/build.gradle +++ b/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' diff --git a/src/main/java/io/eiren/vr/VRServer.java b/src/main/java/io/eiren/vr/VRServer.java index 229ad2536..b955f4f18 100644 --- a/src/main/java/io/eiren/vr/VRServer.java +++ b/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 tasks = new LinkedBlockingQueue<>(); + private final Map 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 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 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 trackersConfig = new FastList<>(); + config.setProperty("trackers", trackersConfig); + synchronized(configuration) { + Iterator iterator = configuration.values().iterator(); + while(iterator.hasNext()) { + TrackerConfig tc = iterator.next(); + Map 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 getAllTrackers() { + synchronized(trackers) { + return new FastList<>(trackers); + } + } } diff --git a/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTracker.java b/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTracker.java new file mode 100644 index 000000000..48db0ce25 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTrackerPosition.java b/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTrackerPosition.java new file mode 100644 index 000000000..a7c887620 --- /dev/null +++ b/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; +} diff --git a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java index d13b6601e..c214899bb 100644 --- a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java +++ b/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 computedTrackers = new FastList<>(); - private final EnumMap trackers = new EnumMap<>(TrackerPosition.class); - private final ComputedTracker waist; - private final ComputedTracker leftFoot; - private final ComputedTracker rightFoot; - - public HumanPoseProcessor(HMDTracker hmd) { + private final List computedTrackers = new FastList<>(); + private final EnumMap 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 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 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 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(); } } } diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java new file mode 100644 index 000000000..d82111a7a --- /dev/null +++ b/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 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()); + } +} diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleton.java b/src/main/java/io/eiren/vr/processor/HumanSkeleton.java new file mode 100644 index 000000000..d1a63196e --- /dev/null +++ b/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(); + + +} diff --git a/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java b/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java new file mode 100644 index 000000000..bb0684975 --- /dev/null +++ b/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 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); + } +} \ No newline at end of file diff --git a/src/main/java/io/eiren/vr/processor/TransformNode.java b/src/main/java/io/eiren/vr/processor/TransformNode.java new file mode 100644 index 000000000..b088bd565 --- /dev/null +++ b/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 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); + } + } +} diff --git a/src/main/java/io/eiren/vr/trackers/TrackerConfig.java b/src/main/java/io/eiren/vr/trackers/TrackerConfig.java new file mode 100644 index 000000000..0283ddb78 --- /dev/null +++ b/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"); + } + } +}