OSC Support (#274)

src/main/java/dev/slimevr/VRServer.java

@@ -5,6 +5,7 @@ import dev.slimevr.autobone.AutoBoneHandler;
 import dev.slimevr.bridge.Bridge;
 import dev.slimevr.bridge.VMCBridge;
 import dev.slimevr.config.ConfigManager;
+import dev.slimevr.osc.VRCOSCHandler;
 import dev.slimevr.platform.windows.WindowsNamedPipeBridge;
 import dev.slimevr.poserecorder.BVHRecorder;
 import dev.slimevr.protocol.ProtocolAPI;
@@ -43,6 +44,7 @@ public class VRServer extends Thread {
 	private final List<Consumer<Tracker>> newTrackersConsumers = new FastList<>();
 	private final List<Runnable> onTick = new FastList<>();
 	private final List<? extends ShareableTracker> shareTrackers;
+	private final VRCOSCHandler VRCOSCHandler;
 	private final DeviceManager deviceManager;
 	private final BVHRecorder bvhRecorder;
 	private final SerialHandler serialHandler;
@@ -74,17 +76,18 @@ public class VRServer extends Thread {
 		hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier
 												// debugging
 		// TODO Multiple processors
-		humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker);
+		humanPoseProcessor = new HumanPoseProcessor(this);
 		shareTrackers = humanPoseProcessor.getComputedTrackers();
 
 		// Start server for SlimeVR trackers
 		trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker);
 
 		// OpenVR bridge currently only supports Windows
+		WindowsNamedPipeBridge driverBridge = null;
 		if (OperatingSystem.getCurrentPlatform() == OperatingSystem.WINDOWS) {
 
 			// Create named pipe bridge for SteamVR driver
-			WindowsNamedPipeBridge driverBridge = new WindowsNamedPipeBridge(
+			driverBridge = new WindowsNamedPipeBridge(
 				this,
 				hmdTracker,
 				"steamvr",
@@ -123,6 +126,15 @@ public class VRServer extends Thread {
 			e.printStackTrace();
 		}
 
+		// Initialize OSC
+		VRCOSCHandler = new VRCOSCHandler(
+			hmdTracker,
+			humanPoseProcessor,
+			driverBridge,
+			getConfigManager().getVrConfig().getVrcOSC(),
+			shareTrackers
+		);
+
 		bvhRecorder = new BVHRecorder(this);
 
 		registerTracker(hmdTracker);
@@ -184,8 +196,8 @@ public class VRServer extends Thread {
 	public void run() {
 		trackersServer.start();
 		while (true) {
-			fpsTimer.update();
 			// final long start = System.currentTimeMillis();
+			fpsTimer.update();
 			do {
 				Runnable task = tasks.poll();
 				if (task == null)
@@ -205,6 +217,7 @@ public class VRServer extends Thread {
 			for (Bridge bridge : bridges) {
 				bridge.dataWrite();
 			}
+			VRCOSCHandler.update();
 			// final long time = System.currentTimeMillis() - start;
 			try {
 				Thread.sleep(1); // 1000Hz
@@ -306,6 +319,10 @@ public class VRServer extends Thread {
 		return trackersServer;
 	}
 
+	public VRCOSCHandler getVRCOSCHandler() {
+		return VRCOSCHandler;
+	}
+
 	public DeviceManager getDeviceManager() {
 		return deviceManager;
 	}

src/main/java/dev/slimevr/config/OSCConfig.java

@@ -0,0 +1,91 @@
+package dev.slimevr.config;
+
+
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.ser.std.StdKeySerializers;
+import dev.slimevr.config.serializers.BooleanMapDeserializer;
+import dev.slimevr.vr.trackers.TrackerRole;
+
+import java.net.InetAddress;
+import java.net.UnknownHostException;
+import java.util.HashMap;
+import java.util.Map;
+
+
+public class OSCConfig {
+
+	// Is OSC enabled for the app
+	private boolean enabled = false;
+
+	// Port to receive OSC messages from
+	private int portIn = 9001;
+
+	// Port to send out OSC messages at
+	private int portOut = 9000;
+
+	// Address to send out OSC messages at
+	private String address = "127.0.0.1";
+
+	// Which trackers' data to send
+	@JsonDeserialize(using = BooleanMapDeserializer.class)
+	@JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer.class)
+	public Map<String, Boolean> trackers = new HashMap<>();
+	private final TrackerRole[] defaultRoles = new TrackerRole[] { TrackerRole.WAIST,
+		TrackerRole.LEFT_FOOT, TrackerRole.RIGHT_FOOT };
+
+	public OSCConfig() {
+		// Initialize default tracker role settings
+		for (TrackerRole role : defaultRoles) {
+			setOSCTrackerRole(
+				role,
+				getOSCTrackerRole(role, true)
+			);
+		}
+
+	}
+
+	public boolean getEnabled() {
+		return enabled;
+	}
+
+	public void setEnabled(boolean value) {
+		enabled = value;
+	}
+
+	public int getPortIn() {
+		return portIn;
+	}
+
+	public void setPortIn(int portIn) {
+		this.portIn = portIn;
+	}
+
+	public int getPortOut() {
+		return portOut;
+	}
+
+	public void setPortOut(int portOut) {
+		this.portOut = portOut;
+	}
+
+	public InetAddress getAddress() {
+		try {
+			return InetAddress.getByName(address);
+		} catch (UnknownHostException e) {
+			throw new RuntimeException(e);
+		}
+	}
+
+	public void setAddress(String address) {
+		this.address = address;
+	}
+
+	public boolean getOSCTrackerRole(TrackerRole role, boolean def) {
+		return trackers.getOrDefault(role.name().toLowerCase(), def);
+	}
+
+	public void setOSCTrackerRole(TrackerRole role, boolean val) {
+		this.trackers.put(role.name().toLowerCase(), val);
+	}
+}

src/main/java/dev/slimevr/config/VRConfig.java

@@ -17,25 +17,27 @@ import java.util.Map;
 )
 public class VRConfig {
 
-	private WindowConfig window = new WindowConfig();
+	private final WindowConfig window = new WindowConfig();
 
-	private FiltersConfig filters = new FiltersConfig();
+	private final FiltersConfig filters = new FiltersConfig();
 
-	private AutoBoneConfig autobone = new AutoBoneConfig();
+	private final OSCConfig vrcOSC = new OSCConfig();
 
-	private KeybindingsConfig keybindings = new KeybindingsConfig();
+	private final AutoBoneConfig autobone = new AutoBoneConfig();
 
-	private SkeletonConfig skeleton = new SkeletonConfig();
+	private final KeybindingsConfig keybindings = new KeybindingsConfig();
+
+	private final SkeletonConfig skeleton = new SkeletonConfig();
 
 	@JsonDeserialize(using = TrackerConfigMapDeserializer.class)
 	@JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer.class)
-	private Map<String, TrackerConfig> trackers = new HashMap<>();
+	private final Map<String, TrackerConfig> trackers = new HashMap<>();
 
 	@JsonDeserialize(using = BridgeConfigMapDeserializer.class)
 	@JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer.class)
-	private Map<String, BridgeConfig> bridges = new HashMap<>();
+	private final Map<String, BridgeConfig> bridges = new HashMap<>();
 
-	private OverlayConfig overlay = new OverlayConfig();
+	private final OverlayConfig overlay = new OverlayConfig();
 
 	public WindowConfig getWindow() {
 		return window;
@@ -45,6 +47,10 @@ public class VRConfig {
 		return filters;
 	}
 
+	public OSCConfig getVrcOSC() {
+		return vrcOSC;
+	}
+
 	public AutoBoneConfig getAutoBone() {
 		return autobone;
 	}

src/main/java/dev/slimevr/osc/VRCOSCHandler.java

@@ -0,0 +1,261 @@
+package dev.slimevr.osc;
+
+import com.illposed.osc.*;
+import com.illposed.osc.messageselector.OSCPatternAddressMessageSelector;
+import com.illposed.osc.transport.OSCPortIn;
+import com.illposed.osc.transport.OSCPortOut;
+import com.jme3.math.FastMath;
+import com.jme3.math.Quaternion;
+import com.jme3.math.Vector3f;
+import dev.slimevr.config.OSCConfig;
+import dev.slimevr.platform.windows.WindowsNamedPipeBridge;
+import dev.slimevr.vr.processor.HumanPoseProcessor;
+import dev.slimevr.vr.trackers.HMDTracker;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.TrackerStatus;
+import io.eiren.util.collections.FastList;
+import io.eiren.util.logging.LogManager;
+
+import java.io.IOException;
+import java.net.InetAddress;
+import java.util.List;
+
+
+/**
+ * VRChat OSCTracker documentation: https://docs.vrchat.com/docs/osc-trackers
+ */
+public class VRCOSCHandler {
+	private OSCPortIn oscReceiver;
+	private OSCPortOut oscSender;
+	private OSCMessage oscMessage;
+	private final OSCConfig config;
+	private final HMDTracker hmd;
+	private final WindowsNamedPipeBridge steamvrBridge;
+	private final HumanPoseProcessor humanPoseProcessor;
+	private final List<? extends ShareableTracker> shareableTrackers;
+	private final FastList<Float> oscArgs = new FastList<>(3);
+	private final Vector3f vec = new Vector3f();
+	private final Quaternion quatBuf = new Quaternion();
+	private final float[] floatBuf = new float[3];
+	private final boolean[] trackersEnabled;
+	private long timeAtLastOSCMessageReceived;
+	private static final long HMD_TIMEOUT = 15000;
+
+	public VRCOSCHandler(
+		HMDTracker hmd,
+		HumanPoseProcessor humanPoseProcessor,
+		WindowsNamedPipeBridge steamvrBridge,
+		OSCConfig oscConfig,
+		List<? extends ShareableTracker> shareableTrackers
+	) {
+		this.hmd = hmd;
+		this.humanPoseProcessor = humanPoseProcessor;
+		this.steamvrBridge = steamvrBridge;
+		this.config = oscConfig;
+		this.shareableTrackers = shareableTrackers;
+
+		trackersEnabled = new boolean[shareableTrackers.size()];
+
+		refreshSettings();
+	}
+
+	public void refreshSettings() {
+		// Sets which trackers are enabled and force HEAD to false
+		for (int i = 0; i < shareableTrackers.size(); i++) {
+			if (shareableTrackers.get(i).getTrackerRole() != TrackerRole.HEAD) {
+				trackersEnabled[i] = config
+					.getOSCTrackerRole(shareableTrackers.get(i).getTrackerRole(), false);
+			}
+		}
+
+		// Stops listening and closes OSC port
+		if (oscReceiver != null && oscReceiver.isListening()) {
+			oscReceiver.stopListening();
+		}
+		if (oscSender != null && oscSender.isConnected()) {
+			try {
+				oscSender.close();
+			} catch (IOException e) {
+				LogManager.severe("[VRCOSCHandler] Error closing the OSC sender: " + e);
+			}
+		}
+
+		if (config.getEnabled()) {
+			// Instantiates the OSC receiver
+			try {
+				int port = config.getPortIn();
+				oscReceiver = new OSCPortIn(
+					port
+				);
+				LogManager.info("[VRCOSCHandler] Listening to port " + port);
+			} catch (IOException e) {
+				LogManager
+					.severe("[VRCOSCHandler] Error listening to the port " + config.getPortIn());
+			}
+
+			// Starts listening for the Upright parameter from VRC
+			OSCMessageListener listener = this::handleReceivedMessage;
+			MessageSelector selector = new OSCPatternAddressMessageSelector(
+				"/avatar/parameters/Upright"
+			);
+			oscReceiver.getDispatcher().addListener(selector, listener);
+			oscReceiver.startListening();
+
+			// Instantiate the OSC sender
+			try {
+				InetAddress address = config.getAddress();
+				int port = config.getPortOut();
+				oscSender = new OSCPortOut(
+					address,
+					port
+				);
+				oscSender.connect();
+				LogManager
+					.info(
+						"[VRCOSCHandler] Sending to port "
+							+ port
+							+ " at address "
+							+ address.toString()
+					);
+			} catch (IOException e) {
+				LogManager
+					.severe(
+						"[VRCOSCHandler] Error connecting to port "
+							+ config.getPortOut()
+							+ " at the address "
+							+ config.getAddress()
+					);
+			}
+		}
+	}
+
+	void handleReceivedMessage(OSCMessageEvent event) {
+		timeAtLastOSCMessageReceived = System.currentTimeMillis();
+
+		if (steamvrBridge != null && !steamvrBridge.isConnected()) {
+			// Sets HMD status to OK
+			if (hmd.getStatus() != TrackerStatus.OK) {
+				hmd.setStatus(TrackerStatus.OK);
+			}
+
+			// Sets the HMD y position to
+			// the vrc Upright parameter (0-1) * the user's height
+			hmd.position
+				.set(
+					0f,
+					(float) event
+						.getMessage()
+						.getArguments()
+						.get(0) * humanPoseProcessor.getUserHeightFromConfig(),
+					0f
+				);
+			hmd.rotation.set(Quaternion.IDENTITY);
+
+			hmd.dataTick();
+		}
+	}
+
+	public void update() {
+		// Manage HMD state with timeout
+		if (oscReceiver != null) {
+			if (
+				((steamvrBridge != null
+					&& steamvrBridge.isConnected())
+					||
+					System.currentTimeMillis() - timeAtLastOSCMessageReceived > HMD_TIMEOUT
+					||
+					!oscReceiver.isListening())
+					&& hmd.getStatus() == TrackerStatus.OK
+			) {
+				hmd.setStatus(TrackerStatus.DISCONNECTED);
+			}
+		}
+
+		// Send OSC data
+		if (oscSender != null && oscSender.isConnected()) {
+			for (int i = 0; i < shareableTrackers.size(); i++) {
+				if (trackersEnabled[i]) {
+					// Send regular trackers' positions
+					shareableTrackers.get(i).getPosition(vec);
+					oscArgs.clear();
+					oscArgs.add(-vec.x);
+					oscArgs.add(vec.y);
+					oscArgs.add(vec.z);
+					oscMessage = new OSCMessage(
+						"/tracking/trackers/" + (i + 1) + "/position",
+						oscArgs
+					);
+					try {
+						oscSender.send(oscMessage);
+					} catch (IOException | OSCSerializeException e) {
+						LogManager
+							.warning(
+								"[VRCOSCHandler] Error sending tracker positions to VRChat: " + e
+							);
+					}
+
+					// Send regular trackers' rotations
+					shareableTrackers.get(i).getRotation(quatBuf);
+					quatBuf.toAngles(floatBuf);
+					oscArgs.clear();
+					oscArgs.add(floatBuf[0] * FastMath.RAD_TO_DEG);
+					oscArgs.add(-floatBuf[1] * FastMath.RAD_TO_DEG);
+					oscArgs.add(-floatBuf[2] * FastMath.RAD_TO_DEG);
+					oscMessage = new OSCMessage(
+						"/tracking/trackers/" + (i + 1) + "/rotation",
+						oscArgs
+					);
+					try {
+						oscSender.send(oscMessage);
+					} catch (IOException | OSCSerializeException e) {
+						LogManager
+							.warning(
+								"[VRCOSCHandler] Error sending tracker rotations to VRChat: " + e
+							);
+					}
+				}
+
+				if (shareableTrackers.get(i).getTrackerRole() == TrackerRole.HEAD) {
+					// Send HMD position
+					shareableTrackers.get(i).getPosition(vec);
+					oscArgs.clear();
+					oscArgs.add(-vec.x);
+					oscArgs.add(vec.y);
+					oscArgs.add(vec.z);
+					oscMessage = new OSCMessage(
+						"/tracking/trackers/head/position",
+						oscArgs
+					);
+					try {
+						oscSender.send(oscMessage);
+					} catch (IOException | OSCSerializeException e) {
+						LogManager
+							.warning("[VRCOSCHandler] Error sending head position to VRChat: " + e);
+					}
+				}
+			}
+		}
+	}
+
+	/**
+	 * Sends the expected HMD rotation upon reset to align the trackers in VRC
+	 */
+	public void yawAlign() {
+		if (oscSender != null && oscSender.isConnected()) {
+			oscArgs.clear();
+			oscArgs.add(0f);
+			oscArgs.add(180f);
+			oscArgs.add(0f);
+			oscMessage = new OSCMessage(
+				"/tracking/trackers/head/rotation",
+				oscArgs
+			);
+			try {
+				oscSender.send(oscMessage);
+			} catch (IOException | OSCSerializeException e) {
+				LogManager.warning("[VRCOSCHandler] Error sending HMD rotation to VRChat: " + e);
+			}
+		}
+	}
+}

src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java

@@ -304,4 +304,9 @@ public class WindowsNamedPipeBridge extends ProtobufBridge<VRTracker> implements
 			);
 		return false;
 	}
+
+	public boolean isConnected() {
+		return pipe != null
+			&& pipe.state == PipeState.OPEN;
+	}
 }

src/main/java/dev/slimevr/protocol/RPCHandler.java

@@ -7,8 +7,10 @@ import dev.slimevr.autobone.AutoBone.Epoch;
 import dev.slimevr.autobone.AutoBoneListener;
 import dev.slimevr.autobone.AutoBoneProcessType;
 import dev.slimevr.config.FiltersConfig;
+import dev.slimevr.config.OSCConfig;
 import dev.slimevr.config.OverlayConfig;
 import dev.slimevr.filtering.TrackerFilters;
+import dev.slimevr.osc.VRCOSCHandler;
 import dev.slimevr.platform.windows.WindowsNamedPipeBridge;
 import dev.slimevr.poserecorder.PoseFrames;
 import dev.slimevr.serial.SerialListener;
@@ -21,12 +23,14 @@ import dev.slimevr.vr.trackers.TrackerPosition;
 import dev.slimevr.vr.trackers.TrackerRole;
 import io.eiren.util.logging.LogManager;
 import solarxr_protocol.MessageBundle;
+import solarxr_protocol.datatypes.Ipv4Address;
 import solarxr_protocol.datatypes.TransactionId;
 import solarxr_protocol.rpc.*;
 import solarxr_protocol.rpc.settings.ModelRatios;
 import solarxr_protocol.rpc.settings.ModelSettings;
 import solarxr_protocol.rpc.settings.ModelToggles;
 
+import java.nio.ByteBuffer;
 import java.util.EnumMap;
 import java.util.Map.Entry;
 import java.util.function.BiConsumer;
@@ -313,6 +317,15 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
 				filtersConfig.getAmount()
 			);
 
+		OSCConfig vrcOSCConfig = this.api.server
+			.getConfigManager()
+			.getVrConfig()
+			.getVrcOSC();
+		int vrcOSCSettings = createOSCSettings(
+			fbb,
+			vrcOSCConfig
+		);
+
 		int modelSettings;
 		{
 			var config = this.api.server.humanPoseProcessor.getSkeletonConfig();
@@ -340,12 +353,54 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
 		}
 
 		int settings = SettingsResponse
-			.createSettingsResponse(fbb, steamvrTrackerSettings, filterSettings, modelSettings);
+			.createSettingsResponse(
+				fbb,
+				steamvrTrackerSettings,
+				filterSettings,
+				vrcOSCSettings,
+				modelSettings
+			);
 		int outbound = createRPCMessage(fbb, RpcMessage.SettingsResponse, settings);
 		fbb.finish(outbound);
 		conn.send(fbb.dataBuffer());
 	}
 
+	private static int createOSCSettings(
+		FlatBufferBuilder fbb,
+		OSCConfig config
+	) {
+		VRCOSCSettings.startVRCOSCSettings(fbb);
+		VRCOSCSettings.addEnabled(fbb, config.getEnabled());
+		VRCOSCSettings.addPortIn(fbb, config.getPortIn());
+		VRCOSCSettings.addPortOut(fbb, config.getPortOut());
+		VRCOSCSettings
+			.addAddress(
+				fbb,
+				Ipv4Address
+					.createIpv4Address(
+						fbb,
+						ByteBuffer.wrap(config.getAddress().getAddress()).getInt()
+					)
+			);
+		VRCOSCSettings
+			.addTrackers(
+				fbb,
+				OSCTrackersSetting
+					.createOSCTrackersSetting(
+						fbb,
+						config.getOSCTrackerRole(TrackerRole.HEAD, false),
+						config.getOSCTrackerRole(TrackerRole.CHEST, false),
+						config.getOSCTrackerRole(TrackerRole.WAIST, false),
+						config.getOSCTrackerRole(TrackerRole.LEFT_KNEE, false),
+						config.getOSCTrackerRole(TrackerRole.LEFT_FOOT, false),
+						config.getOSCTrackerRole(TrackerRole.LEFT_ELBOW, false),
+						config.getOSCTrackerRole(TrackerRole.LEFT_HAND, false)
+					)
+			);
+
+		return VRCOSCSettings.endVRCOSCSettings(fbb);
+	}
+
 	public void onChangeSettingsRequest(GenericConnection conn, RpcMessageHeader messageHeader) {
 
 		ChangeSettingsRequest req = (ChangeSettingsRequest) messageHeader
@@ -358,8 +413,8 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
 				.getVRBridge(WindowsNamedPipeBridge.class);
 			bridge.changeShareSettings(TrackerRole.WAIST, req.steamVrTrackers().waist());
 			bridge.changeShareSettings(TrackerRole.CHEST, req.steamVrTrackers().chest());
-			bridge.changeShareSettings(TrackerRole.LEFT_FOOT, req.steamVrTrackers().legs());
-			bridge.changeShareSettings(TrackerRole.RIGHT_FOOT, req.steamVrTrackers().legs());
+			bridge.changeShareSettings(TrackerRole.LEFT_FOOT, req.steamVrTrackers().feet());
+			bridge.changeShareSettings(TrackerRole.RIGHT_FOOT, req.steamVrTrackers().feet());
 			bridge.changeShareSettings(TrackerRole.LEFT_KNEE, req.steamVrTrackers().knees());
 			bridge.changeShareSettings(TrackerRole.RIGHT_KNEE, req.steamVrTrackers().knees());
 			bridge.changeShareSettings(TrackerRole.LEFT_ELBOW, req.steamVrTrackers().elbows());
@@ -382,6 +437,31 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
 			}
 		}
 
+		if (req.vrcOsc() != null) {
+			OSCConfig vrcOSCConfig = this.api.server
+				.getConfigManager()
+				.getVrConfig()
+				.getVrcOSC();
+			VRCOSCHandler VRCOSCHandler = this.api.server.getVRCOSCHandler();
+			var trackers = req.vrcOsc().trackers();
+
+			vrcOSCConfig.setEnabled(req.vrcOsc().enabled());
+			vrcOSCConfig.setPortIn(req.vrcOsc().portIn());
+			vrcOSCConfig.setPortOut(req.vrcOsc().portOut());
+			vrcOSCConfig.setAddress(req.vrcOsc().address().toString());
+			vrcOSCConfig.setOSCTrackerRole(TrackerRole.HEAD, trackers.head());
+			vrcOSCConfig.setOSCTrackerRole(TrackerRole.CHEST, trackers.chest());
+			vrcOSCConfig.setOSCTrackerRole(TrackerRole.WAIST, trackers.waist());
+			vrcOSCConfig.setOSCTrackerRole(TrackerRole.LEFT_KNEE, trackers.knees());
+			vrcOSCConfig.setOSCTrackerRole(TrackerRole.RIGHT_KNEE, trackers.knees());
+			vrcOSCConfig.setOSCTrackerRole(TrackerRole.HEAD, trackers.feet());
+			vrcOSCConfig.setOSCTrackerRole(TrackerRole.HEAD, trackers.elbows());
+			vrcOSCConfig.setOSCTrackerRole(TrackerRole.HEAD, trackers.hands());
+
+
+			VRCOSCHandler.refreshSettings();
+		}
+
 		var modelSettings = req.modelSettings();
 		if (modelSettings != null) {
 			var cfg = this.api.server.humanPoseProcessor.getSkeletonConfig();

src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTrackerPosition.java

@@ -2,6 +2,7 @@ package dev.slimevr.vr.processor;
 
 public enum ComputedHumanPoseTrackerPosition {
 
+	HEAD,
 	WAIST,
 	CHEST,
 	LEFT_FOOT,

src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java

@@ -2,12 +2,11 @@ package dev.slimevr.vr.processor;
 
 import dev.slimevr.VRServer;
 import dev.slimevr.util.ann.VRServerThread;
-import dev.slimevr.vr.processor.skeleton.Skeleton;
-import dev.slimevr.vr.processor.skeleton.HumanSkeleton;
-import dev.slimevr.vr.processor.skeleton.SkeletonConfig;
-import dev.slimevr.vr.processor.skeleton.SkeletonConfigOffsets;
-import dev.slimevr.vr.processor.skeleton.SkeletonConfigToggles;
-import dev.slimevr.vr.trackers.*;
+import dev.slimevr.vr.processor.skeleton.*;
+import dev.slimevr.vr.trackers.ShareableTracker;
+import dev.slimevr.vr.trackers.Tracker;
+import dev.slimevr.vr.trackers.TrackerRole;
+import dev.slimevr.vr.trackers.TrackerStatus;
 import io.eiren.util.ann.ThreadSafe;
 import io.eiren.util.collections.FastList;
 
@@ -22,8 +21,24 @@ public class HumanPoseProcessor {
 	private final List<Consumer<Skeleton>> onSkeletonUpdated = new FastList<>();
 	private Skeleton skeleton;
 
-	public HumanPoseProcessor(VRServer server, HMDTracker hmd) {
+	public HumanPoseProcessor(VRServer server) {
 		this.server = server;
+		computedTrackers
+			.add(
+				new ComputedHumanPoseTracker(
+					Tracker.getNextLocalTrackerId(),
+					ComputedHumanPoseTrackerPosition.HEAD,
+					TrackerRole.HEAD
+				)
+			);
+		computedTrackers
+			.add(
+				new ComputedHumanPoseTracker(
+					Tracker.getNextLocalTrackerId(),
+					ComputedHumanPoseTrackerPosition.CHEST,
+					TrackerRole.CHEST
+				)
+			);
 		computedTrackers
 			.add(
 				new ComputedHumanPoseTracker(
@@ -48,14 +63,6 @@ public class HumanPoseProcessor {
 					TrackerRole.RIGHT_FOOT
 				)
 			);
-		computedTrackers
-			.add(
-				new ComputedHumanPoseTracker(
-					Tracker.getNextLocalTrackerId(),
-					ComputedHumanPoseTrackerPosition.CHEST,
-					TrackerRole.CHEST
-				)
-			);
 		computedTrackers
 			.add(
 				new ComputedHumanPoseTracker(
@@ -155,16 +162,16 @@ public class HumanPoseProcessor {
 
 	@VRServerThread
 	public void trackerAdded(Tracker tracker) {
-		updateSekeltonModel();
+		updateSkeletonModel();
 	}
 
 	@VRServerThread
 	public void trackerUpdated(Tracker tracker) {
-		updateSekeltonModel();
+		updateSkeletonModel();
 	}
 
 	@VRServerThread
-	private void updateSekeltonModel() {
+	private void updateSkeletonModel() {
 		disconnectAllTrackers();
 		skeleton = new HumanSkeleton(server, computedTrackers);
 		for (Consumer<Skeleton> sc : onSkeletonUpdated)
@@ -186,8 +193,10 @@ public class HumanPoseProcessor {
 
 	@VRServerThread
 	public void resetTrackers() {
-		if (skeleton != null)
+		if (skeleton != null) {
 			skeleton.resetTrackersFull();
+			server.getVRCOSCHandler().yawAlign();
+		}
 	}
 
 	@VRServerThread
@@ -236,4 +245,12 @@ public class HumanPoseProcessor {
 			server.getConfigManager().saveConfig();
 		}
 	}
+
+	@VRServerThread
+	public float getUserHeightFromConfig() {
+		if (skeleton != null) {
+			return getSkeletonConfig().getUserHeightFromOffsets();
+		}
+		return 0f;
+	}
 }

src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java

@@ -23,6 +23,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 	// @formatter:off
 	protected final TransformNode hmdNode = new TransformNode("HMD", false);
 	protected final TransformNode headNode = new TransformNode("Head", false);
+	protected final TransformNode trackerHeadNode = new TransformNode("Head-Tracker", false);
 	protected final TransformNode neckNode = new TransformNode("Neck", false);
 	protected final TransformNode chestNode = new TransformNode("Chest", false);
 	protected final TransformNode trackerChestNode = new TransformNode("Chest-Tracker", false);
@@ -102,6 +103,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 	protected Tracker rightShoulderTracker;
 	// #endregion
 	// #region Tracker Output
+	protected ComputedHumanPoseTracker computedHeadTracker;
 	protected ComputedHumanPoseTracker computedChestTracker;
 	protected ComputedHumanPoseTracker computedWaistTracker;
 	protected ComputedHumanPoseTracker computedLeftKneeTracker;
@@ -226,6 +228,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 		// #endregion
 
 		// #region Attach tracker nodes for tracker offsets
+		neckNode.attachChild(trackerHeadNode);
 		chestNode.attachChild(trackerChestNode);
 		hipNode.attachChild(trackerWaistNode);
 
@@ -618,6 +621,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 
 	public void setComputedTracker(ComputedHumanPoseTracker tracker) {
 		switch (tracker.getTrackerRole()) {
+			case HEAD -> computedHeadTracker = tracker;
 			case CHEST -> computedChestTracker = tracker;
 			case WAIST -> computedWaistTracker = tracker;
 			case LEFT_KNEE -> computedLeftKneeTracker = tracker;
@@ -641,6 +645,22 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 	// #endregion
 
 	public void fillNullComputedTrackers() {
+		if (computedHeadTracker == null) {
+			computedHeadTracker = new ComputedHumanPoseTracker(
+				Tracker.getNextLocalTrackerId(),
+				ComputedHumanPoseTrackerPosition.HEAD,
+				TrackerRole.HEAD
+			);
+			computedHeadTracker.setStatus(TrackerStatus.OK);
+		}
+		if (computedChestTracker == null) {
+			computedChestTracker = new ComputedHumanPoseTracker(
+				Tracker.getNextLocalTrackerId(),
+				ComputedHumanPoseTrackerPosition.CHEST,
+				TrackerRole.CHEST
+			);
+			computedChestTracker.setStatus(TrackerStatus.OK);
+		}
 		if (computedWaistTracker == null) {
 			computedWaistTracker = new ComputedHumanPoseTracker(
 				Tracker.getNextLocalTrackerId(),
@@ -665,14 +685,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 			);
 			computedRightFootTracker.setStatus(TrackerStatus.OK);
 		}
-		if (computedChestTracker == null) {
-			computedChestTracker = new ComputedHumanPoseTracker(
-				Tracker.getNextLocalTrackerId(),
-				ComputedHumanPoseTrackerPosition.CHEST,
-				TrackerRole.CHEST
-			);
-			computedChestTracker.setStatus(TrackerStatus.OK);
-		}
 		if (computedLeftKneeTracker == null) {
 			computedLeftKneeTracker = new ComputedHumanPoseTracker(
 				Tracker.getNextLocalTrackerId(),
@@ -726,6 +738,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 	// #region Get Trackers
 	public ComputedHumanPoseTracker getComputedTracker(TrackerRole trackerRole) {
 		switch (trackerRole) {
+			case HEAD:
+				return computedHeadTracker;
 			case CHEST:
 				return computedChestTracker;
 			case WAIST:
@@ -824,6 +838,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 
 			hmdTracker.getRotation(rotBuf1);
 			hmdNode.localTransform.setRotation(rotBuf1);
+			trackerHeadNode.localTransform.setRotation(rotBuf1);
 
 			if (neckTracker != null)
 				neckTracker.getRotation(rotBuf1);
@@ -832,6 +847,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 			// Set to zero
 			hmdNode.localTransform.setTranslation(Vector3f.ZERO);
 			hmdNode.localTransform.setRotation(Quaternion.IDENTITY);
+			trackerHeadNode.localTransform.setRotation(Quaternion.IDENTITY);
 			headNode.localTransform.setRotation(Quaternion.IDENTITY);
 		}
 
@@ -1219,6 +1235,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 
 	// #region Update the output trackers
 	protected void updateComputedTrackers() {
+		if (computedHeadTracker != null) {
+			computedHeadTracker.position.set(trackerHeadNode.worldTransform.getTranslation());
+			computedHeadTracker.rotation.set(trackerHeadNode.worldTransform.getRotation());
+			computedHeadTracker.dataTick();
+		}
+
 		if (computedChestTracker != null) {
 			computedChestTracker.position.set(trackerChestNode.worldTransform.getTranslation());
 			computedChestTracker.rotation.set(trackerChestNode.worldTransform.getRotation());
@@ -1544,6 +1566,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 		return new TransformNode[] {
 			hmdNode,
 			headNode,
+			trackerHeadNode,
 			neckNode,
 			chestNode,
 			trackerChestNode,
@@ -1624,7 +1647,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 				hmdTracker.getPosition(vec);
 				height = vec.y;
 				if (height > 0.5f) { // Reset only if floor level seems right,
-					// TODO: read floor level from SteamVR
 					skeletonConfig
 						.setOffset(
 							SkeletonConfigOffsets.TORSO,
@@ -1656,7 +1678,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 				hmdTracker.getPosition(vec);
 				height = vec.y;
 				if (height > 0.5f) { // Reset only if floor level seems right,
-					// TODO: read floor level from SteamVR
 					skeletonConfig
 						.setOffset(
 							SkeletonConfigOffsets.LEGS_LENGTH,

src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java

@@ -27,6 +27,7 @@ public class SkeletonConfig {
 
 	protected final boolean autoUpdateOffsets;
 	protected final SkeletonConfigCallback callback;
+	protected float userHeight;
 
 	public SkeletonConfig(boolean autoUpdateOffsets) {
 		this.autoUpdateOffsets = autoUpdateOffsets;
@@ -105,6 +106,7 @@ public class SkeletonConfig {
 			}
 		}
 
+		// Calls callback
 		if (callback != null) {
 			try {
 				callback
@@ -114,6 +116,11 @@ public class SkeletonConfig {
 			}
 		}
 
+		// Re-calculate user height
+		userHeight = getOffset(SkeletonConfigOffsets.NECK)
+			+ getOffset(SkeletonConfigOffsets.TORSO)
+			+ getOffset(SkeletonConfigOffsets.LEGS_LENGTH);
+
 		return origVal;
 	}
 
@@ -130,6 +137,10 @@ public class SkeletonConfig {
 		return val != null ? val : config.defaultValue;
 	}
 
+	public float getUserHeightFromOffsets() {
+		return userHeight;
+	}
+
 	public Boolean setToggle(SkeletonConfigToggles config, Boolean newValue) {
 		Boolean origVal = newValue != null
 			? configToggles.put(config, newValue)