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()) {