diff --git a/src/main/java/io/eiren/vr/VRServer.java b/src/main/java/io/eiren/vr/VRServer.java index 438485f0d..229ad2536 100644 --- a/src/main/java/io/eiren/vr/VRServer.java +++ b/src/main/java/io/eiren/vr/VRServer.java @@ -13,7 +13,7 @@ import io.eiren.vr.trackers.Tracker; public class VRServer extends Thread { private final List trackers = new FastList<>(); - private final HumanPoseProcessor humanPoseProcessor; + public final HumanPoseProcessor humanPoseProcessor; private final TrackersUDPServer trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker); private final NamedPipeVRBridge driverBridge; diff --git a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java index bb3f56161..d13b6601e 100644 --- a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java +++ b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java @@ -1,8 +1,12 @@ package io.eiren.vr.processor; import java.util.EnumMap; +import java.util.Iterator; import java.util.List; +import com.jme3.math.FastMath; +import com.jme3.math.Quaternion; + import essentia.util.collections.FastList; import io.eiren.vr.trackers.ComputedTracker; import io.eiren.vr.trackers.HMDTracker; @@ -12,7 +16,7 @@ public class HumanPoseProcessor { private final HMDTracker hmd; private final List computedTrackers = new FastList<>(); - private final EnumMap trackers = new EnumMap<>(TrackerPosition.class); + private final EnumMap trackers = new EnumMap<>(TrackerPosition.class); private final ComputedTracker waist; private final ComputedTracker leftFoot; private final ComputedTracker rightFoot; @@ -29,8 +33,26 @@ public class HumanPoseProcessor { } public void addTracker(Tracker tracker, TrackerPosition position) { + TransformedTracker tt = new TransformedTracker(tracker); synchronized(trackers) { - trackers.put(position, tracker); + 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 + } } } @@ -47,4 +69,18 @@ public class HumanPoseProcessor { LEFT_FOOT, RIGHT_FOOT } + + private static class TransformedTracker { + public final Tracker tracker; + public final Quaternion transformation = new Quaternion(); + + public TransformedTracker(Tracker tracker) { + this.tracker = tracker; + } + + public void getRotation(Quaternion store) { + tracker.getRotation(store); + store.multLocal(transformation); + } + } } diff --git a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java index 2dc4ded50..bec36e990 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java @@ -13,7 +13,9 @@ import java.util.List; import java.util.Map; import java.util.function.Consumer; +import com.jme3.math.FastMath; import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; import essentia.util.collections.FastList; import io.eiren.hardware.magentometer.Magneto; @@ -135,7 +137,7 @@ public class TrackersUDPServer extends Thread { sensor = trackersMap.get(addr); } if(sensor == null) { - IMUTracker imu = new IMUTracker(handshakePacket.getSocketAddress().toString(), this); + IMUTracker imu = new IMUTracker("udp://" + handshakePacket.getAddress().toString(), this); trackersConsumer.accept(imu); sensor = new TrackerConnection(imu, addr); int i = 0; @@ -149,6 +151,8 @@ 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() { byte[] rcvBuffer = new byte[64]; @@ -176,7 +180,9 @@ public class TrackersUDPServer extends Thread { bb.getLong(); stopCalibration(sensor); 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 + //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()); break;