diff --git a/.classpath b/.classpath index 8b3d57e2b..2f30af680 100644 --- a/.classpath +++ b/.classpath @@ -28,5 +28,6 @@ + diff --git a/.gitignore b/.gitignore index 1b6985c00..a83967dcc 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,8 @@ # Ignore Gradle build output directory build + +/bin/ + +# Syncthing ignore file +.stignore \ No newline at end of file diff --git a/.project b/.project index 6e12a0074..314b26780 100644 --- a/.project +++ b/.project @@ -1,6 +1,6 @@ - SlimeVR-Server + SlimeVR Server SlimeVR Server diff --git a/settings.gradle b/settings.gradle index 09d513dc5..d807a5906 100644 --- a/settings.gradle +++ b/settings.gradle @@ -7,4 +7,5 @@ * in the user manual at https://docs.gradle.org/6.3/userguide/multi_project_builds.html */ -rootProject.name = 'SlimeVR-Server' +rootProject.name = 'SlimeVR Server' +include('Slime Java Commons') \ No newline at end of file diff --git a/src/main/java/eiren/io/vr/bridge/NamedPipeVRBridge.java b/src/main/java/eiren/io/vr/bridge/NamedPipeVRBridge.java new file mode 100644 index 000000000..2ded99a75 --- /dev/null +++ b/src/main/java/eiren/io/vr/bridge/NamedPipeVRBridge.java @@ -0,0 +1,204 @@ +package eiren.io.vr.bridge; + +import java.io.IOException; +import java.nio.charset.Charset; +import java.util.List; + +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; + +public class NamedPipeVRBridge extends Thread implements VRBridge { + + public static final String HMDPipeName = "\\\\.\\pipe\\HMDPipe"; + public static final String TrackersPipeName = "\\\\.\\pipe\\TrackPipe"; + + public static final int TRACKERS = 3; + private static final byte[] buffer = new byte[1024]; + + private Pipe hmdPipe; + private List trackerPipes = new FastList<>(); + protected VRBridgeState bridgeState = VRBridgeState.NOT_STARTED; + + public NamedPipeVRBridge() { + super("Named Pipe VR Bridge"); + } + + @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); + } + for(int i = 0; i < trackerPipes.size(); ++i) { + Pipe trackerPipe = trackerPipes.get(i); + if(trackerPipe.state == PipeState.CREATED) + if(tryOpeningPipe(trackerPipe)) + initTrackerPipe(trackerPipe, i); + } + if(areAllPipesOpen()) { + bridgeState = VRBridgeState.CONNECTED; + LogManager.log.info("[VRBridge] All pipes are connected!"); + } else { + Thread.sleep(200L); + } + } else { + updateHMD(); + for(int i = 0; i < trackerPipes.size(); ++i) { + updateTracker(trackerPipes.get(i), i); + } + } + } + } catch(Exception e) { + e.printStackTrace(); + bridgeState = VRBridgeState.ERROR; + } + } + + public void 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[] 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]); + 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)); + } catch(NumberFormatException e) { + e.printStackTrace(); + } + } + } + } + } + + public void updateTracker(Pipe pipe, int trackerId) { + + } + + private void initHMDPipe(Pipe pipe) { + + } + + 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()); + IntByReference lpNumberOfBytesWritten = new IntByReference(0); + Kernel32.INSTANCE.WriteFile(pipe.pipeHandle, + buff, + buff.length, + lpNumberOfBytesWritten, + null); + } + + private boolean tryOpeningPipe(Pipe pipe) { + if(Kernel32.INSTANCE.ConnectNamedPipe(pipe.pipeHandle, null)) { + pipe.state = NamedPipeVRBridge.PipeState.OPEN; + LogManager.log.info("[VRBridge] Pipe " + pipe.name + " is open"); + return true; + } + return false; + } + + private boolean areAllPipesOpen() { + if(hmdPipe == null || hmdPipe.state == PipeState.CREATED) { + return false; + } + for(int i = 0; i < trackerPipes.size(); ++i) { + if(trackerPipes.get(i).state == PipeState.CREATED) + return false; + } + return true; + } + + private void createPipes() throws IOException { + try { + hmdPipe = new Pipe(Kernel32.INSTANCE.CreateNamedPipe(HMDPipeName, WinBase.PIPE_ACCESS_DUPLEX, // dwOpenMode + WinBase.PIPE_TYPE_BYTE | WinBase.PIPE_READMODE_BYTE | WinBase.PIPE_WAIT, // dwPipeMode + 1, // nMaxInstances, + 1024 * 16, // nOutBufferSize, + 1024 * 16, // nInBufferSize, + 0, // nDefaultTimeOut, + null), HMDPipeName); // lpSecurityAttributes + 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) { + 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 + 1, // nMaxInstances, + 1024 * 16, // nOutBufferSize, + 1024 * 16, // nInBufferSize, + 0, // nDefaultTimeOut, + null); // lpSecurityAttributes + if(WinBase.INVALID_HANDLE_VALUE.equals(pipeHandle)) + throw new IOException("Can't open " + pipeName + " pipe: " + Kernel32.INSTANCE.GetLastError()); + LogManager.log.info("[VRBridge] Pipe " + pipeName + " created"); + trackerPipes.add(new Pipe(pipeHandle, pipeName)); + } + LogManager.log.info("[VRBridge] Pipes are open"); + } catch(IOException e) { + safeDisconnect(hmdPipe); + for(int i = 0; i < trackerPipes.size(); ++i) + safeDisconnect(trackerPipes.get(i)); + trackerPipes.clear(); + throw e; + } + } + + public static void safeDisconnect(Pipe pipe) { + try { + if(pipe != null && pipe.pipeHandle != null) + Kernel32.INSTANCE.DisconnectNamedPipe(pipe.pipeHandle); + } catch(Exception e) { + } + } + + private static class Pipe { + final String name; + final HANDLE pipeHandle; + PipeState state = PipeState.CREATED; + + public Pipe(HANDLE pipeHandle, String name) { + this.pipeHandle = pipeHandle; + this.name = name; + } + } + + private static enum PipeState { + CREATED, + OPEN, + ERROR; + } +} diff --git a/src/main/java/eiren/io/vr/bridge/VRBridge.java b/src/main/java/eiren/io/vr/bridge/VRBridge.java new file mode 100644 index 000000000..8bcf680ff --- /dev/null +++ b/src/main/java/eiren/io/vr/bridge/VRBridge.java @@ -0,0 +1,13 @@ +package eiren.io.vr.bridge; + +public interface VRBridge { + + public VRBridgeState getBridgeState(); + + public static enum VRBridgeState { + NOT_STARTED, + STARTED, + CONNECTED, + ERROR + } +} diff --git a/src/main/java/eiren/io/vr/sensors/RotationSensor.java b/src/main/java/eiren/io/vr/sensors/RotationSensor.java new file mode 100644 index 000000000..2206328dd --- /dev/null +++ b/src/main/java/eiren/io/vr/sensors/RotationSensor.java @@ -0,0 +1,11 @@ +package eiren.io.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/eiren/io/vr/sensors/SensorUDPServer.java b/src/main/java/eiren/io/vr/sensors/SensorUDPServer.java new file mode 100644 index 000000000..71c2aad8a --- /dev/null +++ b/src/main/java/eiren/io/vr/sensors/SensorUDPServer.java @@ -0,0 +1,89 @@ +package eiren.io.vr.sensors; + +import java.net.DatagramPacket; +import java.net.DatagramSocket; +import java.net.SocketTimeoutException; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; + +import io.eiren.util.Util; + +public class SensorUDPServer extends Thread { + + DatagramSocket socket = null; + byte[] sendBuffer = new byte[64]; + long lastKeepup = System.currentTimeMillis(); + boolean connected = false; + private final RotationSensor sensor; + private final int port; + + public SensorUDPServer(int port, String name, RotationSensor sensor) { + super(name); + this.sensor = sensor; + this.port = port; + } + + @Override + public void run() { + try { + socket = new DatagramSocket(port); + socket.setSoTimeout(250); + while(true) { + try { + byte[] rcvBuffer = new byte[64]; + ByteBuffer bb = ByteBuffer.wrap(rcvBuffer).order(ByteOrder.BIG_ENDIAN); + //ByteBuffer bb2 = bb.asReadOnlyBuffer().order(ByteOrder.BIG_ENDIAN); + DatagramPacket recieve = new DatagramPacket(rcvBuffer, 64); + socket.receive(recieve); + bb.rewind(); + //System.out.println(StringUtils.toHexString(rcvBuffer)); + switch(bb.getInt()) { + case 3: + System.out.println("Handshake"); + sendBuffer[0] = 3; + byte[] str = "Hey OVR =D 5".getBytes("ASCII"); + System.arraycopy(str, 0, sendBuffer, 1, str.length); + socket.send(new DatagramPacket(sendBuffer, 64, recieve.getAddress(), recieve.getPort())); + connected = true; + break; + case 1: + bb.getLong(); + sensor.rotQuaternion.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat()); + //rotQuaternion.set(-rotQuaternion.getY(), rotQuaternion.getX(), rotQuaternion.getZ(), rotQuaternion.getW()); + //System.out.println("Rot: " + rotQuaternion.getX() + "," + rotQuaternion.getY() + "," + rotQuaternion.getZ() + "," + rotQuaternion.getW()); + break; + case 2: + bb.getLong(); + sensor.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); + //System.out.println("Gyro: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat()); + break; + case 4: + bb.getLong(); + sensor.accelVector.set(bb.get(), bb.getFloat(), bb.getFloat()); + //System.out.println("Accel: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat()); + break; + case 5: + bb.getLong(); + sensor.magVector.set(bb.get(), bb.getFloat(), bb.getFloat()); + //System.out.println("Accel: " + bb.getFloat() + "," + bb.getFloat() + "," + bb.getFloat()); + break; + } + if(lastKeepup + 500 < System.currentTimeMillis()) { + lastKeepup = System.currentTimeMillis(); + if(connected) { + sendBuffer[0] = 1; + socket.send(new DatagramPacket(sendBuffer, 64, recieve.getAddress(), recieve.getPort())); + } + } + } catch(SocketTimeoutException e) { + } catch(Exception e) { + e.printStackTrace(); + } + } + } catch(Exception e) { + e.printStackTrace(); + } finally { + Util.close(socket); + } + } +}