github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-06 02:01:58 +02:00
Code Issues 159 Packages Projects Releases Wiki Activity

Fixes and future changes for pose processing

Browse Source
This commit is contained in:
Eiren Rain
2021-01-12 19:30:57 +03:00
parent 4da945e09b
commit 6f2d76c687
3 changed files with 47 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/io/eiren/vr/VRServer.java
View File

@@ -13,7 +13,7 @@ import io.eiren.vr.trackers.Tracker;
public class VRServer extends Thread {
private final List<Tracker> 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;

40
src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java
View File

@@ -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<ComputedTracker> computedTrackers = new FastList<>();
private final EnumMap<TrackerPosition, Tracker> trackers = new EnumMap<>(TrackerPosition.class);
private final EnumMap<TrackerPosition, TransformedTracker> 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<TransformedTracker> 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);
}
}
}

10
src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java
View File

@@ -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;