From 1233a425ef1a8784979544213e9e438fd2c6cd80 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Mon, 18 Jan 2021 08:28:22 +0300 Subject: [PATCH] Small VR Bridge refactoring Added support for new trackers notification in VR Server Refactor trackers adjustments, fix adjustments bugs, can have different adjustments types (full, yaw, anything in the future) Fixed multiple problems with skeleton computation, added neck node Added support for IMU trackers update rate calculation --- src/main/java/io/eiren/vr/VRServer.java | 16 +++ .../io/eiren/vr/bridge/NamedPipeVRBridge.java | 120 +++++++++--------- .../java/io/eiren/vr/bridge/VRBridge.java | 8 -- .../vr/processor/HumanPoseProcessor.java | 68 ++-------- .../vr/processor/HumanSekeletonWithLegs.java | 31 +++-- .../vr/processor/HumanSkeleonWithWaist.java | 25 +++- .../io/eiren/vr/trackers/AdjustedTracker.java | 47 +++++++ .../eiren/vr/trackers/AdjustedYawTracker.java | 33 +++++ .../eiren/vr/trackers/CalibratingTracker.java | 6 + .../java/io/eiren/vr/trackers/IMUTracker.java | 17 ++- .../io/eiren/vr/trackers/TrackerStatus.java | 15 ++- .../eiren/vr/trackers/TrackersUDPServer.java | 16 +-- 12 files changed, 242 insertions(+), 160 deletions(-) create mode 100644 src/main/java/io/eiren/vr/trackers/AdjustedTracker.java create mode 100644 src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java create mode 100644 src/main/java/io/eiren/vr/trackers/CalibratingTracker.java diff --git a/src/main/java/io/eiren/vr/VRServer.java b/src/main/java/io/eiren/vr/VRServer.java index 5ef9d69ab..ab5f4b91e 100644 --- a/src/main/java/io/eiren/vr/VRServer.java +++ b/src/main/java/io/eiren/vr/VRServer.java @@ -10,6 +10,7 @@ import java.util.List; import java.util.Map; import java.util.Queue; import java.util.concurrent.LinkedBlockingQueue; +import java.util.function.Consumer; import essentia.util.ann.ThreadSafe; import essentia.util.ann.ThreadSecure; @@ -34,6 +35,7 @@ public class VRServer extends Thread { private final Map configuration = new HashMap<>(); public final YamlFile config = new YamlFile(); public final HMDTracker hmdTracker; + private final List> newTrackersConsumers = new FastList<>(); public VRServer() { super("VRServer"); @@ -75,6 +77,16 @@ public class VRServer extends Thread { } } } + + public void addNewTrackerConsumer(Consumer consumer) { + synchronized(newTrackersConsumers) { + newTrackersConsumers.add(consumer); + } + synchronized(trackers) { + for(int i = 0; i < trackers.size(); ++i) + consumer.accept(trackers.get(i)); + } + } @ThreadSafe public void saveConfig() { @@ -151,6 +163,10 @@ public class VRServer extends Thread { trackers.add(tracker); } autoAssignTracker(tracker); + synchronized(newTrackersConsumers) { + for(int i = 0; i < newTrackersConsumers.size(); ++i) + newTrackersConsumers.get(i).accept(tracker); + } } public void calibrate(Tracker tracker) { diff --git a/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java b/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java index 350f90d72..d61183b7c 100644 --- a/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java +++ b/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java @@ -32,7 +32,6 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { private final HMDTracker hmd; private final List trackerPipes; private final List shareTrackers; - protected VRBridgeState bridgeState = VRBridgeState.NOT_STARTED; public NamedPipeVRBridge(HMDTracker hmd, List shareTrackers) { super("Named Pipe VR Bridge"); @@ -41,91 +40,88 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { this.trackerPipes = new FastList<>(shareTrackers.size()); } - @Override - public VRBridgeState getBridgeState() { - return bridgeState; - } - @Override public void run() { try { createPipes(); - bridgeState = VRBridgeState.STARTED; while(true) { - if(bridgeState == VRBridgeState.STARTED) { - if(hmdPipe != null && hmdPipe.state == PipeState.CREATED) { - if(tryOpeningPipe(hmdPipe)) - initHMDPipe(hmdPipe); - } + waitForPipesToOpen(); + if(areAllPipesOpen()) { + boolean hmdUpdated = updateHMD(); // Update at HMDs frequency for(int i = 0; i < trackerPipes.size(); ++i) { - Pipe trackerPipe = trackerPipes.get(i); - if(trackerPipe.state == PipeState.CREATED) - if(tryOpeningPipe(trackerPipe)) - initTrackerPipe(trackerPipe, i); + updateTracker(i, hmdUpdated); } - if(areAllPipesOpen()) { - bridgeState = VRBridgeState.CONNECTED; - LogManager.log.info("[VRBridge] All pipes are connected!"); - } else { - Thread.sleep(200L); - } - } else { - // TODO Handle pipes disconnect, reset state and prepare to send hello again - if(updateHMD()) { // Update at HMDs frequency - for(int i = 0; i < trackerPipes.size(); ++i) { - updateTracker(i); - } - } else { + if(!hmdUpdated) { Thread.sleep(5); // Up to 200Hz } } } } catch(Exception e) { e.printStackTrace(); - bridgeState = VRBridgeState.ERROR; + } + } + + private void waitForPipesToOpen() { + if(hmdPipe.state == PipeState.CREATED) { + if(tryOpeningPipe(hmdPipe)) + initHMDPipe(hmdPipe); + } + for(int i = 0; i < trackerPipes.size(); ++i) { + Pipe trackerPipe = trackerPipes.get(i); + if(trackerPipe.state == PipeState.CREATED) { + if(tryOpeningPipe(trackerPipe)) + initTrackerPipe(trackerPipe, i); + } } } public boolean updateHMD() { - IntByReference bytesAvailable = new IntByReference(0); - if(Kernel32.INSTANCE.PeekNamedPipe(hmdPipe.pipeHandle, null, 0, null, bytesAvailable, null)) { - if(bytesAvailable.getValue() > 0) { - if(Kernel32.INSTANCE.ReadFile(hmdPipe.pipeHandle, buffer, buffer.length, bytesAvailable, null)) { - String str = new String(buffer, 0, bytesAvailable.getValue() - 1, ASCII); - String[] split = str.split("\n")[0].split(" "); - try { - double x = Double.parseDouble(split[0]); - double y = Double.parseDouble(split[1]); - double z = Double.parseDouble(split[2]); - double qw = Double.parseDouble(split[3]); - double qx = Double.parseDouble(split[4]); - double qy = Double.parseDouble(split[5]); - double qz = Double.parseDouble(split[6]); - - hmd.position.set((float) x, (float) y, (float) z); - hmd.rotation.set((float) qx, (float) qy, (float) qz, (float) qw); - } catch(NumberFormatException e) { - e.printStackTrace(); + if(hmdPipe.state == PipeState.OPEN) { + IntByReference bytesAvailable = new IntByReference(0); + if(Kernel32.INSTANCE.PeekNamedPipe(hmdPipe.pipeHandle, null, 0, null, bytesAvailable, null)) { + if(bytesAvailable.getValue() > 0) { + if(Kernel32.INSTANCE.ReadFile(hmdPipe.pipeHandle, buffer, buffer.length, bytesAvailable, null)) { + String str = new String(buffer, 0, bytesAvailable.getValue() - 1, ASCII); + String[] split = str.split("\n")[0].split(" "); + try { + double x = Double.parseDouble(split[0]); + double y = Double.parseDouble(split[1]); + double z = Double.parseDouble(split[2]); + double qw = Double.parseDouble(split[3]); + double qx = Double.parseDouble(split[4]); + double qy = Double.parseDouble(split[5]); + double qz = Double.parseDouble(split[6]); + + hmd.position.set((float) x, (float) y, (float) z); + hmd.rotation.set((float) qx, (float) qy, (float) qz, (float) qw); + } catch(NumberFormatException e) { + e.printStackTrace(); + } } + return true; } - return true; } } return false; } - public void updateTracker(int trackerId) { - sbBuffer.setLength(0); + public void updateTracker(int trackerId, boolean hmdUpdated) { Tracker sensor = shareTrackers.get(trackerId); - sensor.getPosition(vBuffer); - sensor.getRotation(qBuffer); - sbBuffer.append(vBuffer.x).append(' ').append(vBuffer.y).append(' ').append(vBuffer.z).append(' '); - sbBuffer.append(qBuffer.getW()).append(' ').append(qBuffer.getX()).append(' ').append(qBuffer.getY()).append(' ').append(qBuffer.getZ()).append('\n'); - String str = sbBuffer.toString(); - System.arraycopy(str.getBytes(ASCII), 0, buffer, 0, str.length()); - buffer[str.length()] = '\0'; - IntByReference lpNumberOfBytesWritten = new IntByReference(0); - Kernel32.INSTANCE.WriteFile(trackerPipes.get(trackerId).pipeHandle, buffer, str.length() + 1, lpNumberOfBytesWritten, null); + if(sensor.getStatus().sendData) { + Pipe trackerPipe = trackerPipes.get(trackerId); + if(hmdUpdated && trackerPipe.state == PipeState.OPEN) { + sbBuffer.setLength(0); + sensor.getPosition(vBuffer); + sensor.getRotation(qBuffer); + sbBuffer.append(vBuffer.x).append(' ').append(vBuffer.y).append(' ').append(vBuffer.z).append(' '); + sbBuffer.append(qBuffer.getW()).append(' ').append(qBuffer.getX()).append(' ').append(qBuffer.getY()).append(' ').append(qBuffer.getZ()).append('\n'); + String str = sbBuffer.toString(); + System.arraycopy(str.getBytes(ASCII), 0, buffer, 0, str.length()); + buffer[str.length()] = '\0'; + IntByReference lpNumberOfBytesWritten = new IntByReference(0); + Kernel32.INSTANCE.WriteFile(trackerPipe.pipeHandle, buffer, str.length() + 1, lpNumberOfBytesWritten, null); + } + } } private void initHMDPipe(Pipe pipe) { @@ -150,6 +146,8 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { LogManager.log.info("[VRBridge] Pipe " + pipe.name + " is open"); return true; } + + LogManager.log.info("[VRBridge] Error connecting to pipe " + pipe.name + ": " + Kernel32.INSTANCE.GetLastError()); return false; } diff --git a/src/main/java/io/eiren/vr/bridge/VRBridge.java b/src/main/java/io/eiren/vr/bridge/VRBridge.java index 78c9f207d..aadea5111 100644 --- a/src/main/java/io/eiren/vr/bridge/VRBridge.java +++ b/src/main/java/io/eiren/vr/bridge/VRBridge.java @@ -2,12 +2,4 @@ package io.eiren.vr.bridge; public interface VRBridge { - public VRBridgeState getBridgeState(); - - public static enum VRBridgeState { - NOT_STARTED, - STARTED, - CONNECTED, - ERROR - } } diff --git a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java index e19c374e4..6508ed9dc 100644 --- a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java +++ b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java @@ -6,10 +6,11 @@ import java.util.List; import java.util.Map; import com.jme3.math.Quaternion; -import com.jme3.math.Vector3f; import essentia.util.collections.FastList; import io.eiren.vr.VRServer; +import io.eiren.vr.trackers.AdjustedTracker; +import io.eiren.vr.trackers.AdjustedYawTracker; import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.Tracker; import io.eiren.vr.trackers.TrackerConfig; @@ -46,7 +47,7 @@ public class HumanPoseProcessor { } private void addTracker(Tracker tracker, TrackerBodyPosition position) { - AdjustedTracker tt = new AdjustedTracker(tracker, position); + AdjustedTracker tt = new AdjustedYawTracker(tracker); TrackerConfig config = server.getTrackerConfig(tt); if(config.adjustment != null) @@ -74,44 +75,34 @@ public class HumanPoseProcessor { if(skeleton instanceof HumanSekeletonWithLegs) { return; // Proper skeleton applied } + disconnectAllTrackers(); skeleton = new HumanSekeletonWithLegs(server, trackers, computedTrackers); } else { if(skeleton instanceof HumanSkeleonWithWaist) { return; // Proper skeleton applied } + disconnectAllTrackers(); skeleton = new HumanSkeleonWithWaist(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers); } } + private void disconnectAllTrackers() { + for(int i = 0; i < computedTrackers.size(); ++i) { + computedTrackers.get(i).setStatus(TrackerStatus.DISCONNECTED); + } + } + 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.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); - - - tt.position.baseRotation.mult(hmdRotation, targetTrackerRotation); - tt.adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation); + tt.adjust(hmdRotation); TrackerConfig config = server.getTrackerConfig(tt); - config.adjustment = new Quaternion(tt.adjustment); + tt.saveAdjustment(config); } } @@ -119,37 +110,4 @@ public class HumanPoseProcessor { if(skeleton != null) skeleton.updatePose(); } - - private static class AdjustedTracker implements Tracker { - public final Tracker tracker; - public final Quaternion adjustment = new Quaternion(); - public final TrackerBodyPosition position; - - public AdjustedTracker(Tracker tracker, TrackerBodyPosition position) { - this.tracker = tracker; - this.position = position; - } - - @Override - public boolean getRotation(Quaternion store) { - tracker.getRotation(store); - 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/HumanSekeletonWithLegs.java b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java index faafafe19..3851eda5e 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java +++ b/src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java @@ -5,6 +5,7 @@ import java.util.Map; import io.eiren.vr.VRServer; import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerStatus; public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { @@ -15,16 +16,16 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { protected final Tracker rightAnkleTracker; protected final ComputedHumanPoseTracker computedRightAnkleTracker; - protected final TransformNode leftLegNode = new TransformNode(); + protected final TransformNode leftHipNode = new TransformNode(); protected final TransformNode leftKneeNode = new TransformNode(); protected final TransformNode leftAnkleNode = new TransformNode(); - protected final TransformNode rightLegNode = new TransformNode(); + protected final TransformNode rightHipNode = new TransformNode(); protected final TransformNode rightKneeNode = new TransformNode(); protected final TransformNode rightAnkleNode = new TransformNode(); protected float hipsWidth = 0.3f; - protected float kneeLength = 0.5f; - protected float ankleLength = 0.4f; + protected float hipLength = 0.86f; + protected float ankleLength = 0.42f; public HumanSekeletonWithLegs(VRServer server, Map trackers, List computedTrackers) { super(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers); @@ -43,18 +44,20 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { } computedLeftAnkleTracker = lat; computedRightAnkleTracker = rat; + lat.setStatus(TrackerStatus.OK); + rat.setStatus(TrackerStatus.OK); - waistNode.attachChild(leftLegNode); - leftLegNode.localTransform.setTranslation(hipsWidth / 2, 0, 0); + waistNode.attachChild(leftHipNode); + leftHipNode.localTransform.setTranslation(hipsWidth / 2, 0, 0); - waistNode.attachChild(rightLegNode); - rightLegNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0); + waistNode.attachChild(rightHipNode); + rightHipNode.localTransform.setTranslation(-hipsWidth / 2, 0, 0); - leftLegNode.attachChild(leftKneeNode); - leftKneeNode.localTransform.setTranslation(0, -kneeLength, 0); + leftHipNode.attachChild(leftKneeNode); + leftKneeNode.localTransform.setTranslation(0, -hipLength, 0); - rightLegNode.attachChild(rightKneeNode); - rightKneeNode.localTransform.setTranslation(0, -kneeLength, 0); + rightHipNode.attachChild(rightKneeNode); + rightKneeNode.localTransform.setTranslation(0, -hipLength, 0); leftKneeNode.attachChild(leftAnkleNode); leftAnkleNode.localTransform.setTranslation(0, -ankleLength, 0); @@ -67,10 +70,10 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist { public void updateLocalTransforms() { super.updateLocalTransforms(); leftLegTracker.getRotation(qBuf); - leftLegNode.localTransform.setRotation(qBuf); + leftHipNode.localTransform.setRotation(qBuf); rightLegTracker.getRotation(qBuf); - rightLegNode.localTransform.setRotation(qBuf); + rightHipNode.localTransform.setRotation(qBuf); leftAnkleTracker.getRotation(qBuf); leftKneeNode.localTransform.setRotation(qBuf); diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java index 3baebd469..93d3bb6fa 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java +++ b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java @@ -8,6 +8,7 @@ import com.jme3.math.Vector3f; import io.eiren.vr.VRServer; import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerStatus; public class HumanSkeleonWithWaist extends HumanSkeleton { @@ -17,10 +18,13 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { 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 neckNode = new TransformNode(); protected final TransformNode waistNode = new TransformNode(); + + protected float waistDistance = 0.72f; + protected float waistSwingMultiplier = 1f; + protected float neckLength = 0.2f; public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, List computedTrackers) { this.wasitTracker = waistTracker; @@ -32,11 +36,15 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { cwt = t; } computedWaistTracker = cwt; + cwt.setStatus(TrackerStatus.OK); 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); + hmdNode.attachChild(neckNode); + waistNode.localTransform.setTranslation(0, -neckLength, 0); + + neckNode.attachChild(waistNode); + waistNode.localTransform.setTranslation(0, -waistDistance + neckLength, 0); } @Override @@ -47,14 +55,17 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { } protected void updateLocalTransforms() { + hmdTracker.getPosition(vBuf); + hmdTracker.getRotation(qBuf); + hmdNode.localTransform.setTranslation(vBuf); + hmdNode.localTransform.setRotation(qBuf); + 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); + neckNode.localTransform.setRotation(qBuf); waistNode.localTransform.setRotation(qBuf); } diff --git a/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java b/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java new file mode 100644 index 000000000..bd4eabf18 --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/AdjustedTracker.java @@ -0,0 +1,47 @@ +package io.eiren.vr.trackers; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +public class AdjustedTracker implements Tracker { + + public final Tracker tracker; + public final Quaternion adjustment = new Quaternion(); + + public AdjustedTracker(Tracker tracker) { + this.tracker = tracker; + } + + public void saveAdjustment(TrackerConfig cfg) { + cfg.adjustment = new Quaternion(adjustment); + } + + public void adjust(Quaternion reference) { + Quaternion sensorRotation = new Quaternion(); + tracker.getRotation(sensorRotation); + + adjustment.set(sensorRotation).inverseLocal().multLocal(reference); + } + + @Override + public boolean getRotation(Quaternion store) { + tracker.getRotation(store); + 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(); + } +} \ No newline at end of file diff --git a/src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java b/src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java new file mode 100644 index 000000000..1d887ae9f --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java @@ -0,0 +1,33 @@ +package io.eiren.vr.trackers; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +public class AdjustedYawTracker extends AdjustedTracker { + + public AdjustedYawTracker(Tracker tracker) { + super(tracker); + } + + @Override + 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); + + // 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); + + adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation); + } +} diff --git a/src/main/java/io/eiren/vr/trackers/CalibratingTracker.java b/src/main/java/io/eiren/vr/trackers/CalibratingTracker.java new file mode 100644 index 000000000..ae080fde0 --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/CalibratingTracker.java @@ -0,0 +1,6 @@ +package io.eiren.vr.trackers; + +public interface CalibratingTracker { + + public void startCalibration(); +} diff --git a/src/main/java/io/eiren/vr/trackers/IMUTracker.java b/src/main/java/io/eiren/vr/trackers/IMUTracker.java index 22c750a53..a5b867711 100644 --- a/src/main/java/io/eiren/vr/trackers/IMUTracker.java +++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java @@ -3,7 +3,9 @@ package io.eiren.vr.trackers; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; -public class IMUTracker implements Tracker { +import io.eiren.util.BufferedTimer; + +public class IMUTracker implements Tracker, CalibratingTracker { public final Vector3f gyroVector = new Vector3f(); public final Vector3f accelVector = new Vector3f(); @@ -14,6 +16,8 @@ public class IMUTracker implements Tracker { protected final String name; protected final TrackersUDPServer server; + protected BufferedTimer timer = new BufferedTimer(1f); + public IMUTracker(String name, TrackersUDPServer server) { this.name = name; this.server = server; @@ -45,7 +49,16 @@ public class IMUTracker implements Tracker { this.status = status; } + @Override public void startCalibration() { - + server.sendCalibrationCommand(this); + } + + public float getTPS() { + return timer.getAverageFPS(); + } + + public void dataTick() { + timer.update(); } } diff --git a/src/main/java/io/eiren/vr/trackers/TrackerStatus.java b/src/main/java/io/eiren/vr/trackers/TrackerStatus.java index 6c18c9bcc..d5826481c 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackerStatus.java +++ b/src/main/java/io/eiren/vr/trackers/TrackerStatus.java @@ -2,8 +2,15 @@ package io.eiren.vr.trackers; public enum TrackerStatus { - DISCONNECTED, - OK, - BUSY, - ERROR; + DISCONNECTED(false), + OK(true), + BUSY(true), + ERROR(false), + ; + + public final boolean sendData; + + private TrackerStatus(boolean sendData) { + this.sendData = sendData; + } } diff --git a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java index d322f6f58..d3e1cf618 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java @@ -25,13 +25,17 @@ import io.eiren.util.Util; * Recieves trackers data by UDP using extended owoTrack protocol. */ public class TrackersUDPServer extends Thread { + + /** + * Change between IMU axises and OpenGL/SteamVR axises + */ + private static final Quaternion offset = new Quaternion().fromAngleAxis(-FastMath.HALF_PI, Vector3f.UNIT_X); private static final byte[] HANDSHAKE_BUFFER = new byte[64]; private static final byte[] KEEPUP_BUFFER = new byte[64]; private static final byte[] CALIBRATION_BUFFER = new byte[64]; private final Quaternion buf = new Quaternion(); - private final byte[] sendBuffer = new byte[64]; private final List trackers = new FastList<>(); private final Map trackersMap = new HashMap<>(); @@ -151,7 +155,6 @@ public class TrackersUDPServer extends Thread { socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); } - private static final Quaternion offset = new Quaternion().fromAngleAxis(-FastMath.HALF_PI, Vector3f.UNIT_X); @Override public void run() { @@ -180,11 +183,9 @@ public class TrackersUDPServer extends Thread { bb.getLong(); stopCalibration(sensor); buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat()); - //buf.set(buf.getY(), buf.getZ(), buf.getY(), buf.getW()); // Change from sensor rotation to OpenGL/SteamVR rotation - //Quaternion q3 = new Quaternion().fromAngleAxis(FastMath.PI, Vector3f.UNIT_Y).multLocal(q2); offset.mult(buf, buf); sensor.tracker.rotQuaternion.set(buf); - //System.out.println("Rot: " + rotQuaternion.getX() + "," + rotQuaternion.getY() + "," + rotQuaternion.getZ() + "," + rotQuaternion.getW()); + sensor.tracker.dataTick(); break; case 2: if(sensor == null) @@ -192,7 +193,6 @@ public class TrackersUDPServer extends Thread { bb.getLong(); stopCalibration(sensor); sensor.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); - //System.out.println("Gyro: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat()); break; case 4: if(sensor == null) @@ -200,7 +200,6 @@ public class TrackersUDPServer extends Thread { bb.getLong(); stopCalibration(sensor); sensor.tracker.accelVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); - //System.out.println("Accel: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat()); break; case 5: if(sensor == null) @@ -208,7 +207,6 @@ public class TrackersUDPServer extends Thread { bb.getLong(); stopCalibration(sensor); sensor.tracker.magVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); - //System.out.println("Accel: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat()); break; case 6: // PACKET_RAW_CALIBRATION_DATA if(sensor == null) @@ -223,7 +221,7 @@ public class TrackersUDPServer extends Thread { sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()}; break; default: - System.out.println("[TrackerServer] Unknown data recieved: " + packetId + " from " + recieve.getSocketAddress()); + System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress()); break; } if(lastKeepup + 500 < System.currentTimeMillis()) {