From 4da945e09b9c61e48eee28939aadaf2120603fdb Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Sun, 10 Jan 2021 01:05:39 +0300 Subject: [PATCH] Base for trackers server --- src/main/java/io/eiren/vr/VRServer.java | 60 ++++++++ .../io/eiren/vr/bridge/NamedPipeVRBridge.java | 77 +++++++---- .../vr/processor/HumanPoseProcessor.java | 50 +++++++ .../io/eiren/vr/sensors/RotationSensor.java | 11 -- .../io/eiren/vr/trackers/ComputedTracker.java | 42 ++++++ .../java/io/eiren/vr/trackers/HMDTracker.java | 9 ++ .../java/io/eiren/vr/trackers/IMUTracker.java | 51 +++++++ .../java/io/eiren/vr/trackers/Tracker.java | 15 ++ .../io/eiren/vr/trackers/TrackerStatus.java | 9 ++ .../TrackersUDPServer.java} | 129 ++++++++++-------- 10 files changed, 357 insertions(+), 96 deletions(-) create mode 100644 src/main/java/io/eiren/vr/VRServer.java create mode 100644 src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java delete mode 100644 src/main/java/io/eiren/vr/sensors/RotationSensor.java create mode 100644 src/main/java/io/eiren/vr/trackers/ComputedTracker.java create mode 100644 src/main/java/io/eiren/vr/trackers/HMDTracker.java create mode 100644 src/main/java/io/eiren/vr/trackers/IMUTracker.java create mode 100644 src/main/java/io/eiren/vr/trackers/Tracker.java create mode 100644 src/main/java/io/eiren/vr/trackers/TrackerStatus.java rename src/main/java/io/eiren/vr/{sensors/SensorUDPServer.java => trackers/TrackersUDPServer.java} (64%) diff --git a/src/main/java/io/eiren/vr/VRServer.java b/src/main/java/io/eiren/vr/VRServer.java new file mode 100644 index 000000000..438485f0d --- /dev/null +++ b/src/main/java/io/eiren/vr/VRServer.java @@ -0,0 +1,60 @@ +package io.eiren.vr; + +import java.util.List; + +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.vr.trackers.Tracker; + +public class VRServer extends Thread { + + private final List trackers = new FastList<>(); + private final HumanPoseProcessor humanPoseProcessor; + private final TrackersUDPServer trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker); + private final NamedPipeVRBridge driverBridge; + + public VRServer() { + super("VRServer"); + HMDTracker hmd = new HMDTracker("HMD"); + humanPoseProcessor = new HumanPoseProcessor(hmd); + List shareTrackers = humanPoseProcessor.getComputedTrackers(); + driverBridge = new NamedPipeVRBridge(hmd, shareTrackers); + + registerTracker(hmd); + for(int i = 0; i < shareTrackers.size(); ++i) + registerTracker(shareTrackers.get(i)); + } + + @Override + public void run() { + trackersServer.start(); + driverBridge.start(); + while(true) { + final long start = System.currentTimeMillis(); + + humanPoseProcessor.update(); + + final long time = System.currentTimeMillis() - start; + try { + Thread.sleep(Math.max(1, 10L - time)); // 100Hz + } catch(InterruptedException e) { + } + } + } + + private void autoAssignTracker(Tracker tracker) { + // + } + + @ThreadSecure + public void registerTracker(Tracker tracker) { + synchronized(trackers) { + trackers.add(tracker); + } + autoAssignTracker(tracker); + } +} diff --git a/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java b/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java index 83540c6e9..350f90d72 100644 --- a/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java +++ b/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java @@ -4,29 +4,41 @@ import java.io.IOException; import java.nio.charset.Charset; import java.util.List; +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; import com.sun.jna.platform.win32.Kernel32; import com.sun.jna.platform.win32.WinBase; import com.sun.jna.platform.win32.WinNT.HANDLE; import com.sun.jna.ptr.IntByReference; import essentia.util.collections.FastList; -import io.eiren.util.StringUtils; import io.eiren.util.logging.LogManager; +import io.eiren.vr.trackers.HMDTracker; +import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerStatus; public class NamedPipeVRBridge extends Thread implements VRBridge { public static final String HMDPipeName = "\\\\.\\pipe\\HMDPipe"; public static final String TrackersPipeName = "\\\\.\\pipe\\TrackPipe"; + public static final Charset ASCII = Charset.forName("ASCII"); - public static final int TRACKERS = 3; - private static final byte[] buffer = new byte[1024]; + private final byte[] buffer = new byte[1024]; + private final StringBuilder sbBuffer = new StringBuilder(1024); + private final Vector3f vBuffer = new Vector3f(); + private final Quaternion qBuffer = new Quaternion(); private Pipe hmdPipe; - private List trackerPipes = new FastList<>(); + private final HMDTracker hmd; + private final List trackerPipes; + private final List shareTrackers; protected VRBridgeState bridgeState = VRBridgeState.NOT_STARTED; - public NamedPipeVRBridge() { + public NamedPipeVRBridge(HMDTracker hmd, List shareTrackers) { super("Named Pipe VR Bridge"); + this.hmd = hmd; + this.shareTrackers = new FastList<>(shareTrackers); + this.trackerPipes = new FastList<>(shareTrackers.size()); } @Override @@ -58,9 +70,13 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { Thread.sleep(200L); } } else { - updateHMD(); - for(int i = 0; i < trackerPipes.size(); ++i) { - updateTracker(trackerPipes.get(i), i); + // 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 { + Thread.sleep(5); // Up to 200Hz } } } @@ -70,12 +86,12 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { } } - public void updateHMD() { + 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, Charset.forName("ASCII")); + String str = new String(buffer, 0, bytesAvailable.getValue() - 1, ASCII); String[] split = str.split("\n")[0].split(" "); try { double x = Double.parseDouble(split[0]); @@ -85,38 +101,45 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { double qx = Double.parseDouble(split[4]); double qy = Double.parseDouble(split[5]); double qz = Double.parseDouble(split[6]); - LogManager.log.info("[VRBridge] New HMD position:" - + " " + StringUtils.prettyNumber((float) x, 2) - + " " + StringUtils.prettyNumber((float) y, 2) - + " " + StringUtils.prettyNumber((float) z, 2) - + " " + StringUtils.prettyNumber((float) qw, 2) - + " " + StringUtils.prettyNumber((float) qx, 2) - + " " + StringUtils.prettyNumber((float) qy, 2) - + " " + StringUtils.prettyNumber((float) qz, 2)); + + 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 false; } - public void updateTracker(Pipe pipe, int trackerId) { - + public void updateTracker(int trackerId) { + sbBuffer.setLength(0); + 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); } private void initHMDPipe(Pipe pipe) { - + hmd.setStatus(TrackerStatus.OK); } private void initTrackerPipe(Pipe pipe, int trackerId) { - String trackerHello = TRACKERS + " 0"; - byte[] buff = new byte[trackerHello.length() + 1];// = length of string + terminating '\0' !!! - System.arraycopy(trackerHello.getBytes(Charset.forName("ASCII")), 0, buff, 0, trackerHello.length()); + String trackerHello = this.shareTrackers.size() + " 0"; + System.arraycopy(trackerHello.getBytes(ASCII), 0, buffer, 0, trackerHello.length()); + buffer[trackerHello.length()] = '\0'; IntByReference lpNumberOfBytesWritten = new IntByReference(0); Kernel32.INSTANCE.WriteFile(pipe.pipeHandle, - buff, - buff.length, + buffer, + trackerHello.length() + 1, lpNumberOfBytesWritten, null); } @@ -153,7 +176,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { LogManager.log.info("[VRBridge] Pipe " + hmdPipe.name + " created"); if(WinBase.INVALID_HANDLE_VALUE.equals(hmdPipe.pipeHandle)) throw new IOException("Can't open " + HMDPipeName + " pipe: " + Kernel32.INSTANCE.GetLastError()); - for(int i = 0; i < TRACKERS; ++i) { + for(int i = 0; i < this.shareTrackers.size(); ++i) { String pipeName = TrackersPipeName + i; HANDLE pipeHandle = Kernel32.INSTANCE.CreateNamedPipe(pipeName, WinBase.PIPE_ACCESS_DUPLEX, // dwOpenMode WinBase.PIPE_TYPE_BYTE | WinBase.PIPE_READMODE_BYTE | WinBase.PIPE_WAIT, // dwPipeMode diff --git a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java new file mode 100644 index 000000000..bb3f56161 --- /dev/null +++ b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java @@ -0,0 +1,50 @@ +package io.eiren.vr.processor; + +import java.util.EnumMap; +import java.util.List; + +import essentia.util.collections.FastList; +import io.eiren.vr.trackers.ComputedTracker; +import io.eiren.vr.trackers.HMDTracker; +import io.eiren.vr.trackers.Tracker; + +public class HumanPoseProcessor { + + 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) { + this.hmd = hmd; + computedTrackers.add(waist = new ComputedTracker("Waist")); + computedTrackers.add(leftFoot = new ComputedTracker("Left Foot")); + computedTrackers.add(rightFoot = new ComputedTracker("Right Foot")); + } + + public List getComputedTrackers() { + return computedTrackers; + } + + public void addTracker(Tracker tracker, TrackerPosition position) { + synchronized(trackers) { + trackers.put(position, tracker); + } + } + + public void update() { + + } + + public enum TrackerPosition { + NECK, + CHEST, + WAIST, + LEFT_LEG, + RIGHT_LEG, + LEFT_FOOT, + RIGHT_FOOT + } +} diff --git a/src/main/java/io/eiren/vr/sensors/RotationSensor.java b/src/main/java/io/eiren/vr/sensors/RotationSensor.java deleted file mode 100644 index 60d9ee2a6..000000000 --- a/src/main/java/io/eiren/vr/sensors/RotationSensor.java +++ /dev/null @@ -1,11 +0,0 @@ -package io.eiren.vr.sensors; - -import com.jme3.math.Quaternion; -import com.jme3.math.Vector3f; - -public class RotationSensor { - public final Vector3f gyroVector = new Vector3f(); - public final Vector3f accelVector = new Vector3f(); - public final Vector3f magVector = new Vector3f(); - public final Quaternion rotQuaternion = new Quaternion(); -} diff --git a/src/main/java/io/eiren/vr/trackers/ComputedTracker.java b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java new file mode 100644 index 000000000..4292b4a77 --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/ComputedTracker.java @@ -0,0 +1,42 @@ +package io.eiren.vr.trackers; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +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 ComputedTracker(String name) { + this.name = name; + } + + @Override + public String getName() { + return this.name; + } + + @Override + public boolean getPosition(Vector3f store) { + store.set(position); + return true; + } + + @Override + public boolean getRotation(Quaternion store) { + store.set(rotation); + return true; + } + + @Override + public TrackerStatus getStatus() { + return status; + } + + public void setStatus(TrackerStatus status) { + this.status = status; + } +} diff --git a/src/main/java/io/eiren/vr/trackers/HMDTracker.java b/src/main/java/io/eiren/vr/trackers/HMDTracker.java new file mode 100644 index 000000000..ab86abb7f --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/HMDTracker.java @@ -0,0 +1,9 @@ +package io.eiren.vr.trackers; + +public class HMDTracker extends ComputedTracker { + + public HMDTracker(String name) { + super(name); + } + +} diff --git a/src/main/java/io/eiren/vr/trackers/IMUTracker.java b/src/main/java/io/eiren/vr/trackers/IMUTracker.java new file mode 100644 index 000000000..22c750a53 --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java @@ -0,0 +1,51 @@ +package io.eiren.vr.trackers; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +public class IMUTracker implements Tracker { + + public final Vector3f gyroVector = new Vector3f(); + public final Vector3f accelVector = new Vector3f(); + public final Vector3f magVector = new Vector3f(); + public final Quaternion rotQuaternion = new Quaternion(); + protected TrackerStatus status = TrackerStatus.OK; + + protected final String name; + protected final TrackersUDPServer server; + + public IMUTracker(String name, TrackersUDPServer server) { + this.name = name; + this.server = server; + } + + @Override + public String getName() { + return this.name; + } + + @Override + public boolean getPosition(Vector3f store) { + store.set(0, 0, 0); + return false; + } + + @Override + public boolean getRotation(Quaternion store) { + store.set(rotQuaternion); + return true; + } + + @Override + public TrackerStatus getStatus() { + return status; + } + + public void setStatus(TrackerStatus status) { + this.status = status; + } + + public void startCalibration() { + + } +} diff --git a/src/main/java/io/eiren/vr/trackers/Tracker.java b/src/main/java/io/eiren/vr/trackers/Tracker.java new file mode 100644 index 000000000..0d9cae39f --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/Tracker.java @@ -0,0 +1,15 @@ +package io.eiren.vr.trackers; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +public interface Tracker { + + public boolean getPosition(Vector3f store); + + public boolean getRotation(Quaternion store); + + public String getName(); + + public TrackerStatus getStatus(); +} diff --git a/src/main/java/io/eiren/vr/trackers/TrackerStatus.java b/src/main/java/io/eiren/vr/trackers/TrackerStatus.java new file mode 100644 index 000000000..6c18c9bcc --- /dev/null +++ b/src/main/java/io/eiren/vr/trackers/TrackerStatus.java @@ -0,0 +1,9 @@ +package io.eiren.vr.trackers; + +public enum TrackerStatus { + + DISCONNECTED, + OK, + BUSY, + ERROR; +} diff --git a/src/main/java/io/eiren/vr/sensors/SensorUDPServer.java b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java similarity index 64% rename from src/main/java/io/eiren/vr/sensors/SensorUDPServer.java rename to src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java index 4e60b2656..2dc4ded50 100644 --- a/src/main/java/io/eiren/vr/sensors/SensorUDPServer.java +++ b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java @@ -1,4 +1,4 @@ -package io.eiren.vr.sensors; +package io.eiren.vr.trackers; import java.io.IOException; import java.io.UnsupportedEncodingException; @@ -8,73 +8,83 @@ import java.net.SocketAddress; import java.net.SocketTimeoutException; import java.nio.ByteBuffer; import java.nio.ByteOrder; -import java.nio.DoubleBuffer; import java.util.HashMap; import java.util.List; import java.util.Map; -import java.util.function.Supplier; +import java.util.function.Consumer; -import org.apache.commons.math3.util.DoubleArray; - -import com.sun.jna.ptr.DoubleByReference; +import com.jme3.math.Quaternion; import essentia.util.collections.FastList; import io.eiren.hardware.magentometer.Magneto; import io.eiren.util.Util; -public class SensorUDPServer extends Thread { +/** + * Recieves trackers data by UDP using extended owoTrack protocol. + */ +public class TrackersUDPServer extends Thread { 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]; - DatagramSocket socket = null; - byte[] sendBuffer = new byte[64]; - long lastKeepup = System.currentTimeMillis(); - private final Supplier sensorSupplier; + 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<>(); + private final Consumer trackersConsumer; private final int port; - private List sensors = new FastList<>(); - private Map sensorsMap = new HashMap<>(); - public SensorUDPServer(int port, String name, Supplier sensorSupplier) { + protected DatagramSocket socket = null; + protected long lastKeepup = System.currentTimeMillis(); + + public TrackersUDPServer(int port, String name, Consumer trackersConsumer) { super(name); this.port = port; - this.sensorSupplier = sensorSupplier; + this.trackersConsumer = trackersConsumer; } - public void sendCalibrationCommand(int sensorId) { - SensorConnection sensor; - synchronized(sensors) { - if(sensors.size() < sensorId + 1) - return; - sensor = sensors.get(sensorId); + public void sendCalibrationCommand(Tracker tracker) { + TrackerConnection connection = null; + synchronized(trackers) { + for(int i = 0; i < trackers.size(); ++i) { + if(trackers.get(i).tracker == tracker) { + connection = trackers.get(i); + break; + } + } } - synchronized(sensor) { - if(sensor.isCalibrating) + if(connection == null) + return; + synchronized(connection) { + if(connection.isCalibrating) return; - sensor.isCalibrating = true; - sensor.rawCalibrationData.clear(); + connection.tracker.setStatus(TrackerStatus.BUSY); + connection.isCalibrating = true; + connection.rawCalibrationData.clear(); } try { - socket.send(new DatagramPacket(CALIBRATION_BUFFER, CALIBRATION_BUFFER.length, sensor.address)); - System.out.println("Calibrating sensor on " + sensor.address); + socket.send(new DatagramPacket(CALIBRATION_BUFFER, CALIBRATION_BUFFER.length, connection.address)); + System.out.println("[TrackerServer] Calibrating sensor on " + connection.address); } catch(IOException e) { e.printStackTrace(); } } - private void stopCalibration(SensorConnection sensor) { + private void stopCalibration(TrackerConnection sensor) { synchronized(sensor) { if(!sensor.isCalibrating) return; if(sensor.gyroCalibrationData == null || sensor.rawCalibrationData.size() == 0) return; // Calibration not started yet + sensor.tracker.setStatus(TrackerStatus.OK); sensor.isCalibrating = false; } if(sensor.rawCalibrationData.size() > 50 && sensor.gyroCalibrationData != null) { - System.out.println("Gathered " + sensor.address + " calibrration data, processing..."); + System.out.println("[TrackerServer] Gathered " + sensor.address + " calibrration data, processing..."); } else { - System.out.println("Can't gather enough calibration data, aboring..."); + System.out.println("[TrackerServer] Can't gather enough calibration data, aboring..."); return; } double[] accelBasis = new double[3]; @@ -96,9 +106,9 @@ public class SensorUDPServer extends Thread { } - System.out.println("Accelerometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(accelData, sensor.rawCalibrationData.size())); + System.out.println("[TrackerServer] Accelerometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(accelData, sensor.rawCalibrationData.size())); Magneto.INSTANCE.calculate(accelData, sensor.rawCalibrationData.size(), 2, 10000, accelBasis, accelAInv); - System.out.println("Magentometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size())); + System.out.println("[TrackerServer] Magentometer Hnorm: " + Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size())); Magneto.INSTANCE.calculate(magData, sensor.rawCalibrationData.size(), 2, 100, magBasis, magAInv); System.out.println("float G_off[3] ="); @@ -118,21 +128,23 @@ public class SensorUDPServer extends Thread { } private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { - System.out.println("Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); + System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); SocketAddress addr = handshakePacket.getSocketAddress(); - SensorConnection sensor; - synchronized(sensors) { - sensor = sensorsMap.get(addr); + TrackerConnection sensor; + synchronized(trackers) { + sensor = trackersMap.get(addr); } if(sensor == null) { - sensor = new SensorConnection(sensorSupplier.get(), addr); + IMUTracker imu = new IMUTracker(handshakePacket.getSocketAddress().toString(), this); + trackersConsumer.accept(imu); + sensor = new TrackerConnection(imu, addr); int i = 0; - synchronized(sensors) { - i = sensors.size(); - sensors.add(sensor); - sensorsMap.put(addr, sensor); + synchronized(trackers) { + i = trackers.size(); + trackers.add(sensor); + trackersMap.put(addr, sensor); } - System.out.println("Sensor " + i + " added with address " + addr); + System.out.println("[TrackerServer] Sensor " + i + " added with address " + addr); } socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort())); } @@ -149,9 +161,9 @@ public class SensorUDPServer extends Thread { socket.receive(recieve); bb.rewind(); - SensorConnection sensor; - synchronized(sensors) { - sensor = sensorsMap.get(recieve.getSocketAddress()); + TrackerConnection sensor; + synchronized(trackers) { + sensor = trackersMap.get(recieve.getSocketAddress()); } int packetId; switch(packetId = bb.getInt()) { @@ -163,8 +175,9 @@ public class SensorUDPServer extends Thread { break; bb.getLong(); stopCalibration(sensor); - sensor.sensor.rotQuaternion.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat()); - //rotQuaternion.set(-rotQuaternion.getY(), rotQuaternion.getX(), rotQuaternion.getZ(), rotQuaternion.getW()); + buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat()); + buf.set(-buf.getX(), buf.getZ(), buf.getY(), buf.getW()); // Change from sensor rotation to OpenGL/SteamVR rotation + sensor.tracker.rotQuaternion.set(buf); //System.out.println("Rot: " + rotQuaternion.getX() + "," + rotQuaternion.getY() + "," + rotQuaternion.getZ() + "," + rotQuaternion.getW()); break; case 2: @@ -172,7 +185,7 @@ public class SensorUDPServer extends Thread { break; bb.getLong(); stopCalibration(sensor); - sensor.sensor.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); + sensor.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); //System.out.println("Gyro: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat()); break; case 4: @@ -180,7 +193,7 @@ public class SensorUDPServer extends Thread { break; bb.getLong(); stopCalibration(sensor); - sensor.sensor.accelVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); + sensor.tracker.accelVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); //System.out.println("Accel: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat()); break; case 5: @@ -188,7 +201,7 @@ public class SensorUDPServer extends Thread { break; bb.getLong(); stopCalibration(sensor); - sensor.sensor.magVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); + 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 @@ -204,14 +217,14 @@ public class SensorUDPServer extends Thread { sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()}; break; default: - System.out.println("Unknown data recieved: " + packetId); + System.out.println("[TrackerServer] Unknown data recieved: " + packetId + " from " + recieve.getSocketAddress()); break; } if(lastKeepup + 500 < System.currentTimeMillis()) { lastKeepup = System.currentTimeMillis(); - synchronized(sensors) { - for(int i = 0; i < sensors.size(); ++i) - socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, sensors.get(i).address)); + synchronized(trackers) { + for(int i = 0; i < trackers.size(); ++i) + socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, trackers.get(i).address)); } } } catch(SocketTimeoutException e) { @@ -226,16 +239,16 @@ public class SensorUDPServer extends Thread { } } - private class SensorConnection { + private class TrackerConnection { - RotationSensor sensor; + IMUTracker tracker; SocketAddress address; boolean isCalibrating; private List rawCalibrationData = new FastList<>(); private double[] gyroCalibrationData; - public SensorConnection(RotationSensor sensor, SocketAddress address) { - this.sensor = sensor; + public TrackerConnection(IMUTracker tracker, SocketAddress address) { + this.tracker = tracker; this.address = address; } }