Merge branch 'main' of github.com:SlimeVR/SlimeVR-Server into merge-gui-and-server

server/src/main/java/com/jme3/math/FastMath.java

@@ -70,9 +70,59 @@ final public class FastMath {
 	public static final float DEG_TO_RAD = PI / 180.0f;
 	/** A value to multiply a radian value by, to convert it to degrees. */
 	public static final float RAD_TO_DEG = 180.0f / PI;
-	/** A precreated random object for random numbers. */
+	/** A premade random object for random numbers. */
 	public static final Random rand = new Random(System.currentTimeMillis());
 
+	/**
+	 * Returns true if the number is within the specified {@code tolerance}
+	 * value.
+	 * 
+	 * @param value The number to check.
+	 * @param tolerance The tolerance to zero (must be positive).
+	 * @return True if the number is within the specified {@code tolerance}
+	 * value.
+	 */
+	public static boolean isApproxZero(float value, float tolerance) {
+		return value < tolerance && value > tolerance;
+	}
+
+	/**
+	 * Returns true if the number is within the {@link #ZERO_TOLERANCE} value.
+	 * 
+	 * @param value The number to check.
+	 * @return True if the number is within the {@link #ZERO_TOLERANCE} value.
+	 */
+	public static boolean isApproxZero(float value) {
+		return isApproxZero(value, ZERO_TOLERANCE);
+	}
+
+	/**
+	 * Returns true if the two numbers are equal within the specified
+	 * {@code tolerance} value.
+	 * 
+	 * @param valueOne The first number to check.
+	 * @param valueTwo The second number to check.
+	 * @param tolerance The tolerance to zero (must be positive).
+	 * @return True if the numbers are approximately equal within the specified
+	 * {@code tolerance} value.
+	 */
+	public static boolean isApproxEqual(float valueOne, float valueTwo, float tolerance) {
+		return isApproxZero(valueTwo - valueOne, tolerance);
+	}
+
+	/**
+	 * Returns true if the two numbers are equal within the
+	 * {@link #ZERO_TOLERANCE} value.
+	 * 
+	 * @param valueOne The first number to check.
+	 * @param valueTwo The second number to check.
+	 * @return True if the numbers are approximately equal within the
+	 * {@link #ZERO_TOLERANCE} value.
+	 */
+	public static boolean isApproxEqual(float valueOne, float valueTwo) {
+		return isApproxEqual(valueOne, valueTwo, ZERO_TOLERANCE);
+	}
+
 	/**
 	 * Returns true if the number is a power of 2 (2,4,8,16...)
 	 * 
@@ -99,7 +149,7 @@ final public class FastMath {
 	 * 
 	 * @param scale scale value to use. if 1, use endValue, if 0, use
 	 * startValue.
-	 * @param startValue Begining value. 0% of f
+	 * @param startValue Beginning value. 0% of f
 	 * @param endValue ending value. 100% of f
 	 * @return The interpolated value between startValue and endValue.
 	 */
@@ -122,7 +172,7 @@ final public class FastMath {
 	 *
 	 * @param scale scale value to use. if 1, use endValue, if 0, use
 	 * startValue.
-	 * @param startValue Begining value. 0% of f
+	 * @param startValue Beginning value. 0% of f
 	 * @param endValue ending value. 100% of f
 	 * @param store a vector3f to store the result
 	 * @return The interpolated value between startValue and endValue.
@@ -148,7 +198,7 @@ final public class FastMath {
 	 *
 	 * @param scale scale value to use. if 1, use endValue, if 0, use
 	 * startValue.
-	 * @param startValue Begining value. 0% of f
+	 * @param startValue Beginning value. 0% of f
 	 * @param endValue ending value. 100% of f
 	 * @return The interpolated value between startValue and endValue.
 	 */
@@ -392,7 +442,7 @@ final public class FastMath {
 	}
 
 	/**
-	 * Compute the lenght on a catmull rom spline between control point 1 and 2
+	 * Compute the length on a catmull rom spline between control point 1 and 2
 	 * 
 	 * @param p0 control point 0
 	 * @param p1 control point 1
@@ -437,7 +487,7 @@ final public class FastMath {
 	}
 
 	/**
-	 * Compute the lenght on a bezier spline between control point 1 and 2
+	 * Compute the length on a bezier spline between control point 1 and 2
 	 * 
 	 * @param p0 control point 0
 	 * @param p1 control point 1
@@ -657,7 +707,7 @@ final public class FastMath {
 	/**
 	 * Returns the value squared. fValue ^ 2
 	 * 
-	 * @param fValue The vaule to square.
+	 * @param fValue The value to square.
 	 * @return The square of the given value.
 	 */
 	public static float sqr(float fValue) {

server/src/main/java/com/jme3/math/Quaternion.java

@@ -279,7 +279,7 @@ public final class Quaternion implements Cloneable, java.io.Serializable {
 
 	/**
 	 * <code>toAngles</code> returns this quaternion converted to Euler rotation
-	 * angles (yaw,roll,pitch).<br/>
+	 * angles (pitch, yaw, roll).<br/>
 	 * Note that the result is not always 100% accurate due to the implications
 	 * of euler angles.
 	 * 
@@ -288,7 +288,7 @@ public final class Quaternion implements Cloneable, java.io.Serializable {
 	 * 
 	 * @param angles the float[] in which the angles should be stored, or null
 	 * if you want a new float[] to be created
-	 * @return the float[] in which the angles are stored.
+	 * @return the float[] in which the angles are stored (pitch, yaw, roll).
 	 */
 	public float[] toAngles(float[] angles) {
 		if (angles == null) {

server/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;
@@ -28,6 +29,8 @@ import java.net.InetAddress;
 import java.net.UnknownHostException;
 import java.util.List;
 import java.util.Queue;
+import java.util.Timer;
+import java.util.TimerTask;
 import java.util.concurrent.LinkedBlockingQueue;
 import java.util.function.Consumer;
 
@@ -43,12 +46,14 @@ 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;
 	private final AutoBoneHandler autoBoneHandler;
 	private final ProtocolAPI protocolAPI;
 	private final ConfigManager configManager;
+	private final Timer timer = new Timer();
 	private final NanoTimer fpsTimer = new NanoTimer();
 
 	/**
@@ -74,17 +79,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 +129,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 +199,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 +220,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
@@ -242,6 +258,43 @@ public class VRServer extends Thread {
 		queueTask(humanPoseProcessor::resetTrackersYaw);
 	}
 
+	public void resetTrackersMounting() {
+		queueTask(humanPoseProcessor::resetTrackersMounting);
+	}
+
+	public void scheduleResetTrackers(long delay) {
+		TimerTask resetTask = new resetTask();
+		timer.schedule(resetTask, delay);
+	}
+
+	public void scheduleResetTrackersYaw(long delay) {
+		TimerTask yawResetTask = new yawResetTask();
+		timer.schedule(yawResetTask, delay);
+	}
+
+	public void scheduleResetTrackersMounting(long delay) {
+		TimerTask resetMountingTask = new resetMountingTask();
+		timer.schedule(resetMountingTask, delay);
+	}
+
+	class resetTask extends TimerTask {
+		public void run() {
+			queueTask(humanPoseProcessor::resetTrackers);
+		}
+	}
+
+	class yawResetTask extends TimerTask {
+		public void run() {
+			queueTask(humanPoseProcessor::resetTrackersYaw);
+		}
+	}
+
+	class resetMountingTask extends TimerTask {
+		public void run() {
+			queueTask(humanPoseProcessor::resetTrackersMounting);
+		}
+	}
+
 	public void setLegTweaksEnabled(boolean value) {
 		queueTask(() -> humanPoseProcessor.setLegTweaksEnabled(value));
 	}
@@ -306,6 +359,10 @@ public class VRServer extends Thread {
 		return trackersServer;
 	}
 
+	public VRCOSCHandler getVRCOSCHandler() {
+		return VRCOSCHandler;
+	}
+
 	public DeviceManager getDeviceManager() {
 		return deviceManager;
 	}

server/src/main/java/dev/slimevr/config/KeybindingsConfig.java

@@ -6,6 +6,15 @@ public class KeybindingsConfig {
 
 	private String quickResetBinding = "CTRL+ALT+SHIFT+U";
 
+	private String resetMountingBinding = "CTRL+ALT+SHIFT+I";
+
+	private long resetDelay = 0L;
+
+	private long quickResetDelay = 0L;
+
+	private long resetMountingDelay = 0L;
+
+
 	public KeybindingsConfig() {
 	}
 
@@ -16,4 +25,32 @@ public class KeybindingsConfig {
 	public String getQuickResetBinding() {
 		return quickResetBinding;
 	}
+
+	public String getResetMountingBinding() {
+		return resetMountingBinding;
+	}
+
+	public long getResetDelay() {
+		return resetDelay;
+	}
+
+	public void setResetDelay(long delay) {
+		resetDelay = delay;
+	}
+
+	public long getQuickResetDelay() {
+		return quickResetDelay;
+	}
+
+	public void setQuickResetDelay(long delay) {
+		quickResetDelay = delay;
+	}
+
+	public long getResetMountingDelay() {
+		return resetMountingDelay;
+	}
+
+	public void setResetMountingDelay(long delay) {
+		resetMountingDelay = delay;
+	}
 }

server/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);
+	}
+}

server/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;
 	}

server/src/main/java/dev/slimevr/gui/Keybinding.java

@@ -12,9 +12,8 @@ import io.eiren.util.logging.LogManager;
 public class Keybinding implements HotkeyListener {
 	private static final int RESET = 1;
 	private static final int QUICK_RESET = 2;
+	private static final int RESET_MOUNTING = 3;
 	public final VRServer server;
-
-
 	public final KeybindingsConfig config;
 
 	@AWTThread
@@ -42,10 +41,14 @@ public class Keybinding implements HotkeyListener {
 				String quickResetBinding = this.config.getQuickResetBinding();
 				JIntellitype.getInstance().registerHotKey(QUICK_RESET, quickResetBinding);
 				LogManager.info("[Keybinding] Bound quick reset to " + quickResetBinding);
+
+				String resetMountingBinding = this.config.getResetMountingBinding();
+				JIntellitype.getInstance().registerHotKey(RESET_MOUNTING, resetMountingBinding);
+				LogManager.info("[Keybinding] Bound reset mounting to " + resetMountingBinding);
 			}
 		} catch (Throwable e) {
 			LogManager
-				.info(
+				.warning(
 					"[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer."
 				);
 		}
@@ -57,11 +60,15 @@ public class Keybinding implements HotkeyListener {
 		switch (identifier) {
 			case RESET -> {
 				LogManager.info("[Keybinding] Reset pressed");
-				server.resetTrackers();
+				server.scheduleResetTrackers(this.config.getResetDelay());
 			}
 			case QUICK_RESET -> {
 				LogManager.info("[Keybinding] Quick reset pressed");
-				server.resetTrackersYaw();
+				server.scheduleResetTrackersYaw(this.config.getQuickResetDelay());
+			}
+			case RESET_MOUNTING -> {
+				LogManager.info("[Keybinding] Reset mounting pressed");
+				server.scheduleResetTrackersMounting(this.config.getResetMountingDelay());
 			}
 		}
 	}

server/src/main/java/dev/slimevr/gui/TrackersList.java

@@ -142,6 +142,7 @@ public class TrackersList extends EJBoxNoStretch {
 		JLabel adj;
 		JLabel adjYaw;
 		JLabel adjGyro;
+		JLabel adjMounting;
 		JLabel correction;
 		JLabel signalStrength;
 		JLabel rotQuat;
@@ -345,10 +346,15 @@ public class TrackersList extends EJBoxNoStretch {
 					adjGyro = new JLabel("0 0 0 0"),
 					c(1, row, 2, GridBagConstraints.FIRST_LINE_START)
 				);
-				add(new JLabel("Temp:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(new JLabel("Mount Fix:"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START));
+				add(
+					adjMounting = new JLabel("0 0 0 0"),
+					c(3, row, 2, GridBagConstraints.FIRST_LINE_START)
+				);
+				add(new JLabel("Temp:"), c(4, row, 2, GridBagConstraints.FIRST_LINE_START));
 				add(
 					temperature = new JLabel("?"),
-					c(3, row, 2, GridBagConstraints.FIRST_LINE_START)
+					c(5, row, 2, GridBagConstraints.FIRST_LINE_START)
 				);
 			}
 
@@ -446,6 +452,17 @@ public class TrackersList extends EJBoxNoStretch {
 								+ StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)
 						);
 				}
+				if (adjMounting != null) {
+					rat.mountRotFix.toAngles(angles);
+					adjMounting
+						.setText(
+							StringUtils.prettyNumber(angles[0] * FastMath.RAD_TO_DEG, 0)
+								+ " "
+								+ StringUtils.prettyNumber(angles[1] * FastMath.RAD_TO_DEG, 0)
+								+ " "
+								+ StringUtils.prettyNumber(angles[2] * FastMath.RAD_TO_DEG, 0)
+						);
+				}
 			}
 
 			if (tracker instanceof TrackerWithWireless) {

server/src/main/java/dev/slimevr/gui/VRServerGUI.java

@@ -43,6 +43,7 @@ public class VRServerGUI extends JFrame {
 	private final SkeletonList skeletonList;
 	private final EJBox pane;
 	private JButton resetButton;
+	private JButton resetMountingButton;
 	private JButton floorClipButton;
 	private JButton skatingCorrectionButton;
 
@@ -199,6 +200,17 @@ public class VRServerGUI extends JFrame {
 					}
 				});
 				add(Box.createHorizontalStrut(10));
+				add(resetMountingButton = new JButton("Reset Mounting") {
+					{
+						addMouseListener(new MouseInputAdapter() {
+							@Override
+							public void mouseClicked(MouseEvent e) {
+								resetMounting();
+							}
+						});
+					}
+				});
+				add(Box.createHorizontalStrut(10));
 				add(new JButton("Fast Reset") {
 					{
 						addMouseListener(new MouseInputAdapter() {
@@ -544,6 +556,12 @@ public class VRServerGUI extends JFrame {
 		ButtonTimer.runTimer(resetButton, 3, "RESET", server::resetTrackers);
 	}
 
+	@AWTThread
+	private void resetMounting() {
+		ButtonTimer
+			.runTimer(resetMountingButton, 3, "Reset Mounting", server::resetTrackersMounting);
+	}
+
 	@AWTThread
 	private void setSkatingReductionEnabled(Boolean value) {
 		if (value == null)

server/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);
+			}
+		}
+	}
+}

server/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;
+	}
 }

server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java

@@ -207,6 +207,11 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
 		throw new UnsupportedOperationException("PoseFrameTracker does not implement calibration");
 	}
 
+	@Override
+	public void resetMounting(boolean reverseYaw) {
+		throw new UnsupportedOperationException("PoseFrameTracker does not implement calibration");
+	}
+
 	@Override
 	public void tick() {
 		throw new UnsupportedOperationException("PoseFrameTracker does not implement this method");

server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java

@@ -146,6 +146,11 @@ public final class TrackerFrame implements Tracker {
 		throw new UnsupportedOperationException("TrackerFrame does not implement calibration");
 	}
 
+	@Override
+	public void resetMounting(boolean reverseYaw) {
+		throw new UnsupportedOperationException("TrackerFrame does not implement calibration");
+	}
+
 	@Override
 	public void tick() {
 		throw new UnsupportedOperationException("TrackerFrame does not implement this method");

server/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;
@@ -243,6 +247,8 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
 			this.api.server.resetTrackersYaw();
 		if (req.resetType() == ResetType.Full)
 			this.api.server.resetTrackers();
+		if (req.resetType() == ResetType.Mounting)
+			this.api.server.resetTrackersMounting();
 		LogManager.severe("[WebSocketAPI] Reset performed");
 	}
 
@@ -313,6 +319,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 +355,54 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
 		}
 
 		int settings = SettingsResponse
-			.createSettingsResponse(fbb, steamvrTrackerSettings, filterSettings, 0, 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
@@ -382,6 +439,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();

server/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,

server/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,16 @@ public class HumanPoseProcessor {
 
 	@VRServerThread
 	public void resetTrackers() {
-		if (skeleton != null)
+		if (skeleton != null) {
 			skeleton.resetTrackersFull();
+			server.getVRCOSCHandler().yawAlign();
+		}
+	}
+
+	@VRServerThread
+	public void resetTrackersMounting() {
+		if (skeleton != null)
+			skeleton.resetTrackersMounting();
 	}
 
 	@VRServerThread
@@ -236,4 +251,12 @@ public class HumanPoseProcessor {
 			server.getConfigManager().saveConfig();
 		}
 	}
+
+	@VRServerThread
+	public float getUserHeightFromConfig() {
+		if (skeleton != null) {
+			return getSkeletonConfig().getUserHeightFromOffsets();
+		}
+		return 0f;
+	}
 }

server/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,
@@ -1760,6 +1781,38 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
 		this.legTweaks.resetBuffer();
 	}
 
+	private boolean shouldResetMounting(TrackerPosition position) {
+		return position != null
+			// TODO: Feet can't currently be reset using this method, maybe
+			// they'll need a separate step just for them?
+			&& position != TrackerPosition.LEFT_FOOT
+			&& position != TrackerPosition.RIGHT_FOOT;
+	}
+
+	private boolean shouldReverseYaw(TrackerPosition position) {
+		return position != null
+			&& (position == TrackerPosition.LEFT_UPPER_LEG
+				|| position == TrackerPosition.RIGHT_UPPER_LEG
+				|| position == TrackerPosition.LEFT_LOWER_ARM
+				|| position == TrackerPosition.RIGHT_LOWER_ARM
+				|| position == TrackerPosition.LEFT_HAND
+				|| position == TrackerPosition.RIGHT_HAND);
+	}
+
+	@Override
+	@VRServerThread
+	public void resetTrackersMounting() {
+		// Pass all trackers through trackerPreUpdate
+		Tracker[] trackersToReset = getTrackersToReset();
+
+		for (Tracker tracker : trackersToReset) {
+			if (tracker != null && shouldResetMounting(tracker.getBodyPosition())) {
+				tracker.resetMounting(shouldReverseYaw(tracker.getBodyPosition()));
+			}
+		}
+		this.legTweaks.resetBuffer();
+	}
+
 	@Override
 	@VRServerThread
 	public void resetTrackersYaw() {

server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweakBuffer.java

@@ -2,6 +2,7 @@ package dev.slimevr.vr.processor.skeleton;
 
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
+import com.jme3.math.FastMath;
 
 
 /**
@@ -12,7 +13,7 @@ import com.jme3.math.Vector3f;
  * rules: The conditions for an unlock are as follows: 1. the foot is to far
  * from its correct position 2. a velocity higher than a threashold is achived
  * 3. a large acceleration is applied to the foot 4. angular velocity of the
- * foot goes higher than a threashold The conditions for a lock are the opposite
+ * foot goes higher than a threshold. The conditions for a lock are the opposite
  * of the above but require a lower value for all of the above conditions
  */
 
@@ -24,40 +25,41 @@ public class LegTweakBuffer {
 	public static final int ANKLE_ACCEL = 4;
 
 	public static final float NS_CONVERT = 1000000000.0f;
-	private static final Vector3f placeHolderVec = new Vector3f();
-	private static final Quaternion placeHolderQuat = new Quaternion();
+	private static final Vector3f GRAVITY = new Vector3f(0, -9.81f, 0);
+	private static final float GRAVITY_MAGNITUDE = GRAVITY.length();
+	private static final int BUFFER_LEN = 10;
 
 	// states for the legs
 	private int leftLegState = STATE_UNKNOWN;
 	private int rightLegState = STATE_UNKNOWN;
 
 	// positions and rotations
-	private Vector3f leftFootPosition = placeHolderVec;
-	private Vector3f rightFootPosition = placeHolderVec;
-	private Vector3f leftKneePosition = placeHolderVec;
-	private Vector3f rightKneePosition = placeHolderVec;
-	private Vector3f waistPosition = placeHolderVec;
-	private Quaternion leftFootRotation = placeHolderQuat;
-	private Quaternion rightFootRotation = placeHolderQuat;
+	private Vector3f leftFootPosition = new Vector3f();
+	private Vector3f rightFootPosition = new Vector3f();
+	private Vector3f leftKneePosition = new Vector3f();
+	private Vector3f rightKneePosition = new Vector3f();
+	private Vector3f waistPosition = new Vector3f();
+	private Quaternion leftFootRotation = new Quaternion();
+	private Quaternion rightFootRotation = new Quaternion();
 
-	private Vector3f leftFootPositionCorrected = placeHolderVec;
-	private Vector3f rightFootPositionCorrected = placeHolderVec;
-	private Vector3f leftKneePositionCorrected = placeHolderVec;
-	private Vector3f rightKneePositionCorrected = placeHolderVec;
-	private Vector3f waistPositionCorrected = placeHolderVec;
+	private Vector3f leftFootPositionCorrected = new Vector3f();
+	private Vector3f rightFootPositionCorrected = new Vector3f();
+	private Vector3f leftKneePositionCorrected = new Vector3f();
+	private Vector3f rightKneePositionCorrected = new Vector3f();
+	private Vector3f waistPositionCorrected = new Vector3f();
 
 	// velocities
-	private Vector3f leftFootVelocity = placeHolderVec;
+	private Vector3f leftFootVelocity = new Vector3f();
 	private float leftFootVelocityMagnitude = 0;
-	private Vector3f rightFootVelocity = placeHolderVec;
+	private Vector3f rightFootVelocity = new Vector3f();
 	private float rightFootVelocityMagnitude = 0;
 	private float leftFootAngleDiff = 0;
 	private float rightFootAngleDiff = 0;
 
 	// acceleration
-	private Vector3f leftFootAcceleration = placeHolderVec;
+	private Vector3f leftFootAcceleration = new Vector3f();
 	private float leftFootAccelerationMagnitude = 0;
-	private Vector3f rightFootAcceleration = placeHolderVec;
+	private Vector3f rightFootAcceleration = new Vector3f();
 	private float rightFootAccelerationMagnitude = 0;
 
 	// other data
@@ -67,36 +69,46 @@ public class LegTweakBuffer {
 	private int detectionMode = ANKLE_ACCEL; // detection mode
 	private boolean accelerationAboveThresholdLeft = true;
 	private boolean accelerationAboveThresholdRight = true;
+	private Vector3f centerOfMass = new Vector3f();
+	private Vector3f centerOfMassVelocity = new Vector3f();
+	private Vector3f centerOfMassAcceleration = new Vector3f();
 	private float leftFloorLevel;
 	private float rightFloorLevel;
 
 	// hyperparameters
-	public static final float SKATING_DISTANCE_CUTOFF = 0.225f;
-	private static final float SKATING_VELOCITY_THRESHOLD = 4.25f;
-	private static final float SKATING_ACCELERATION_THRESHOLD = 1.15f;
+	public static final float SKATING_DISTANCE_CUTOFF = 0.5f;
+	private static final float SKATING_VELOCITY_THRESHOLD = 3.25f;
+	private static final float SKATING_ACCELERATION_THRESHOLD = 1.00f;
 	private static final float SKATING_ROTVELOCITY_THRESHOLD = 4.5f;
 	private static final float SKATING_LOCK_ENGAGE_PERCENT = 0.85f;
 	private static final float SKATING_ACCELERATION_Y_USE_PERCENT = 0.25f;
 	private static final float FLOOR_DISTANCE_CUTOFF = 0.125f;
-	private static final float SIX_TRACKER_TOLLERANCE = 0.10f;
+	private static final float SIX_TRACKER_TOLLERANCE = -0.10f;
+	private static final Vector3f FORCE_VECTOR_TO_PRESSURE = new Vector3f(0.25f, 1.0f, 0.25f);
+	private static final float FORCE_ERROR_TOLLERANCE = 4.0f;
+	private static final float[] FORCE_VECTOR_FALLBACK = new float[] { 0.1f, 0.1f };
 
-	private static final float PARAM_SCALAR_MAX = 2.5f;
-	private static final float PARAM_SCALAR_MIN = 0.5f;
+	private static final float PARAM_SCALAR_MAX = 3.2f;
+	private static final float PARAM_SCALAR_MIN = 0.25f;
 	private static final float PARAM_SCALAR_MID = 1.0f;
 
 	// the point at which the scalar is at the max or min depending on accel
-	private static final float MAX_SCALAR_ACCEL = 0.3f;
+	private static final float MAX_SCALAR_ACCEL = 0.2f;
 	private static final float MIN_SCALAR_ACCEL = 0.9f;
 
 	// the point at which the scalar is at it max or min in a double locked foot
 	// situation
-	private static final float MAX_SCALAR_DORMANT = 0.6f;
-	private static final float MIN_SCALAR_DORMANT = 2.0f;
+	private static final float MAX_SCALAR_DORMANT = 0.2f;
+	private static final float MIN_SCALAR_DORMANT = 1.50f;
 
 	// the point at which the scalar is at it max or min in a single locked foot
 	// situation
-	private static final float MIN_SCALAR_ACTIVE = 3.0f;
-	private static final float MAX_SCALAR_ACTIVE = 0.2f;
+	private static final float MIN_SCALAR_ACTIVE = 1.75f;
+	private static final float MAX_SCALAR_ACTIVE = 0.1f;
+
+	// maximum scalers for the pressure on each foot
+	private static final float PRESSURE_SCALER_MIN = 0.1f;
+	private static final float PRESSURE_SCALER_MAX = 1.9f;
 
 	private float leftFootSensitivityVel = 1.0f;
 	private float rightFootSensitivityVel = 1.0f;
@@ -121,7 +133,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setLeftFootPosition(Vector3f leftFootPosition) {
-		this.leftFootPosition = leftFootPosition.clone();
+		this.leftFootPosition.set(leftFootPosition);
 	}
 
 	public Vector3f getRightFootPosition(Vector3f vec) {
@@ -132,7 +144,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setRightFootPosition(Vector3f rightFootPosition) {
-		this.rightFootPosition = rightFootPosition.clone();
+		this.rightFootPosition.set(rightFootPosition);
 	}
 
 	public Vector3f getLeftKneePosition(Vector3f vec) {
@@ -143,7 +155,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setLeftKneePosition(Vector3f leftKneePosition) {
-		this.leftKneePosition = leftKneePosition.clone();
+		this.leftKneePosition.set(leftKneePosition);
 	}
 
 	public Vector3f getRightKneePosition(Vector3f vec) {
@@ -153,7 +165,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setRightKneePosition(Vector3f rightKneePosition) {
-		this.rightKneePosition = rightKneePosition.clone();
+		this.rightKneePosition.set(rightKneePosition);
 	}
 
 	public Vector3f getWaistPosition(Vector3f vec) {
@@ -164,7 +176,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setWaistPosition(Vector3f waistPosition) {
-		this.waistPosition = waistPosition.clone();
+		this.waistPosition.set(waistPosition);
 	}
 
 	public Quaternion getLeftFootRotation(Quaternion quat) {
@@ -175,7 +187,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setLeftFootRotation(Quaternion leftFootRotation) {
-		this.leftFootRotation = leftFootRotation.clone();
+		this.leftFootRotation.set(leftFootRotation);
 	}
 
 	public Quaternion getRightFootRotation(Quaternion quat) {
@@ -186,7 +198,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setRightFootRotation(Quaternion rightFootRotation) {
-		this.rightFootRotation = rightFootRotation.clone();
+		this.rightFootRotation.set(rightFootRotation);
 	}
 
 	public Vector3f getLeftFootPositionCorrected(Vector3f vec) {
@@ -197,7 +209,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setLeftFootPositionCorrected(Vector3f leftFootPositionCorrected) {
-		this.leftFootPositionCorrected = leftFootPositionCorrected.clone();
+		this.leftFootPositionCorrected.set(leftFootPositionCorrected);
 	}
 
 	public Vector3f getRightFootPositionCorrected(Vector3f vec) {
@@ -208,7 +220,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setRightFootPositionCorrected(Vector3f rightFootPositionCorrected) {
-		this.rightFootPositionCorrected = rightFootPositionCorrected.clone();
+		this.rightFootPositionCorrected.set(rightFootPositionCorrected);
 	}
 
 	public Vector3f getLeftKneePositionCorrected(Vector3f vec) {
@@ -219,7 +231,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setLeftKneePositionCorrected(Vector3f leftKneePositionCorrected) {
-		this.leftKneePositionCorrected = leftKneePositionCorrected.clone();
+		this.leftKneePositionCorrected.set(leftKneePositionCorrected);
 	}
 
 	public Vector3f getRightKneePositionCorrected(Vector3f vec) {
@@ -230,7 +242,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setRightKneePositionCorrected(Vector3f rightKneePositionCorrected) {
-		this.rightKneePositionCorrected = rightKneePositionCorrected.clone();
+		this.rightKneePositionCorrected.set(rightKneePositionCorrected);
 	}
 
 	public Vector3f getWaistPositionCorrected(Vector3f vec) {
@@ -241,7 +253,7 @@ public class LegTweakBuffer {
 	}
 
 	public void setWaistPositionCorrected(Vector3f waistPositionCorrected) {
-		this.waistPositionCorrected = waistPositionCorrected.clone();
+		this.waistPositionCorrected.set(waistPositionCorrected);
 	}
 
 	public Vector3f getLeftFootVelocity(Vector3f vec) {
@@ -258,6 +270,17 @@ public class LegTweakBuffer {
 		return vec.set(rightFootVelocity);
 	}
 
+	public Vector3f getCenterOfMass(Vector3f vec) {
+		if (vec == null)
+			vec = new Vector3f();
+
+		return vec.set(centerOfMass);
+	}
+
+	public void setCenterOfMass(Vector3f centerOfMass) {
+		this.centerOfMass.set(centerOfMass);
+	}
+
 	public void setLeftFloorLevel(float leftFloorLevel) {
 		this.leftFloorLevel = leftFloorLevel;
 	}
@@ -291,11 +314,11 @@ public class LegTweakBuffer {
 	}
 
 	public void setLeftFootAcceleration(Vector3f leftFootAcceleration) {
-		this.leftFootAcceleration = leftFootAcceleration.clone();
+		this.leftFootAcceleration.set(leftFootAcceleration);
 	}
 
 	public void setRightFootAcceleration(Vector3f rightFootAcceleration) {
-		this.rightFootAcceleration = rightFootAcceleration.clone();
+		this.rightFootAcceleration.set(rightFootAcceleration);
 	}
 
 	public Vector3f getLeftFootAcceleration(Vector3f vec) {
@@ -328,6 +351,22 @@ public class LegTweakBuffer {
 		return this.rightFootAcceleration.y;
 	}
 
+	public float getLeftFloorLevel() {
+		return leftFloorLevel;
+	}
+
+	public float getRightFloorLevel() {
+		return rightFloorLevel;
+	}
+
+	// returns 1 / delta time
+	public float getTimeDelta() {
+		if (parent == null)
+			return 0.0f;
+
+		return 1.0f / ((timeOfFrame - parent.timeOfFrame) / NS_CONVERT);
+	}
+
 	public void setDetectionMode(int mode) {
 		this.detectionMode = mode;
 	}
@@ -339,6 +378,7 @@ public class LegTweakBuffer {
 		// compute attributes of the legs
 		computeVelocity();
 		computeAccelerationMagnitude();
+		computeComAtributes();
 
 		// check if the acceleration triggers forced unlock
 		if (detectionMode == FOOT_ACCEL) {
@@ -359,19 +399,15 @@ public class LegTweakBuffer {
 		}
 	}
 
-	// update the frame number of all the frames
+	// update the frame number and discard frames older than BUFFER_LEN
 	public void updateFrameNumber(int frameNumber) {
 		this.frameNumber = frameNumber;
 
-		if (this.frameNumber >= 10) {
-			this.parent = null; // once a frame is 10 frames old, it is no
-								// longer
-								// needed
-		}
+		if (this.frameNumber >= BUFFER_LEN)
+			this.parent = null;
 
-		if (parent != null) {
+		if (parent != null)
 			parent.updateFrameNumber(frameNumber + 1);
-		}
 	}
 
 	// compute the state of the legs
@@ -384,8 +420,7 @@ public class LegTweakBuffer {
 
 	// check if a locked foot should stay locked or be released
 	private int checkStateLeft() {
-		float timeStep = 1.0f / ((timeOfFrame - parent.timeOfFrame) / NS_CONVERT);
-
+		float timeStep = getTimeDelta();
 		if (parent.leftLegState == UNLOCKED) {
 			if (
 				parent.getLeftFootHorizantalDifference() > SKATING_CUTOFF_ENGAGE
@@ -419,7 +454,7 @@ public class LegTweakBuffer {
 
 	// check if a locked foot should stay locked or be released
 	private int checkStateRight() {
-		float timeStep = 1.0f / ((timeOfFrame - parent.timeOfFrame) / NS_CONVERT);
+		float timeStep = getTimeDelta();
 
 		if (parent.rightLegState == UNLOCKED) {
 			if (
@@ -511,6 +546,12 @@ public class LegTweakBuffer {
 			.length();
 	}
 
+	// compute the velocity and acceleration of the center of mass
+	private void computeComAtributes() {
+		centerOfMassVelocity = centerOfMass.subtract(parent.centerOfMass);
+		centerOfMassAcceleration = centerOfMassVelocity.subtract(parent.centerOfMassVelocity);
+	}
+
 	// for 8 trackers the data from the imus is enough to determine lock/unlock
 	private void computeAccelerationAboveThresholdFootTrackers() {
 		accelerationAboveThresholdLeft = leftFootAccelerationMagnitude
@@ -544,11 +585,16 @@ public class LegTweakBuffer {
 		float leftFootScalarVel = getLeftFootLockLiklyHood();
 		float rightFootScalarVel = getRightFootLockLiklyHood();
 
-		// combine these two sets of scalars to get the final scalars
-		// ( -1 just makes is so if both scalars are 1.0 the final scalar is 1.0
-		// not 2)
-		leftFootSensitivityVel = leftFootScalarAccel + leftFootScalarVel - 1.0f;
-		rightFootSensitivityVel = rightFootScalarAccel + rightFootScalarVel - 1.0f;
+		// get the third set of scalars that is based on where the COM is
+		float[] pressureScalars = getPressurePrediction();
+
+		// combine the scalars to get the final scalars
+		leftFootSensitivityVel = (leftFootScalarAccel
+			+ leftFootScalarVel / 2.0f)
+			* FastMath.clamp(pressureScalars[0] * 2.0f, PRESSURE_SCALER_MIN, PRESSURE_SCALER_MAX);
+		rightFootSensitivityVel = (rightFootScalarAccel
+			+ rightFootScalarVel / 2.0f)
+			* FastMath.clamp(pressureScalars[1] * 2.0f, PRESSURE_SCALER_MIN, PRESSURE_SCALER_MAX);
 
 		leftFootSensitivityAccel = leftFootScalarVel;
 		rightFootSensitivityAccel = rightFootScalarVel;
@@ -654,4 +700,130 @@ public class LegTweakBuffer {
 			/ (MAX_SCALAR_ACTIVE - MIN_SCALAR_ACTIVE)
 			- PARAM_SCALAR_MID;
 	}
+
+	// get the pressure prediction for the feet based of the center of mass
+	// (assume mass is 1)
+	// for understanding in the future this is assuming that the mass is one and
+	// the force of gravity
+	// is 9.8 m/s^2 this allows for the force sum to map directly to the
+	// acceleration of the center of mass
+	// since F = ma and if m is 1 then F = a
+	private float[] getPressurePrediction() {
+		float leftFootPressure = 0;
+		float rightFootPressure = 0;
+
+		// get the vectors from the com to each foot
+		Vector3f leftFootVector = leftFootPosition.subtract(centerOfMass).normalizeLocal();
+		Vector3f rightFootVector = rightFootPosition.subtract(centerOfMass).normalizeLocal();
+
+		// get the magnitude of the force on each foot
+		float leftFootMagnitude = GRAVITY_MAGNITUDE * leftFootVector.y / leftFootVector.length();
+		float rightFootMagnitude = GRAVITY_MAGNITUDE * rightFootVector.y / rightFootVector.length();
+
+		// get the force vector each foot could apply to the com
+		Vector3f leftFootForce = leftFootVector.mult(leftFootMagnitude / 2.0f);
+		Vector3f rightFootForce = rightFootVector.mult(rightFootMagnitude / 2.0f);
+
+		// based off the acceleration of the com, get the force each foot is
+		// likely applying (the expected force sum should be equal to
+		// centerOfMassAcceleration since the mass is 1)
+		findForceVectors(leftFootForce, rightFootForce);
+
+		// see if the force vectors found a reasonable solution
+		// if they did not we assume there is another force acting on the com
+		// and fall back to a low pressure prediction
+		if (detectOutsideForces(leftFootForce, rightFootForce))
+			return FORCE_VECTOR_FALLBACK;
+
+		// set the pressure to the force on each foot times the force to
+		// pressure scalar
+		leftFootPressure = leftFootForce.mult(FORCE_VECTOR_TO_PRESSURE).length();
+		rightFootPressure = rightFootForce.mult(FORCE_VECTOR_TO_PRESSURE).length();
+
+		// distance from the ground is a factor in the pressure
+		// using the inverse of the distance to the ground scale the
+		// pressure
+		float leftDistance = (leftFootPosition.y > leftFloorLevel)
+			? (leftFootPosition.y - leftFloorLevel)
+			: LegTweaks.NEARLY_ZERO;
+		leftFootPressure *= 1.0f / (leftDistance);
+		float rightDistance = (rightFootPosition.y > rightFloorLevel)
+			? (rightFootPosition.y - rightFloorLevel)
+			: LegTweaks.NEARLY_ZERO;
+		rightFootPressure *= 1.0f / (rightDistance);
+
+		// normalize the pressure values
+		float pressureSum = leftFootPressure + rightFootPressure;
+		leftFootPressure /= pressureSum;
+		rightFootPressure /= pressureSum;
+
+		return new float[] { leftFootPressure, rightFootPressure };
+	}
+
+	// perform a gradient descent to find the force vectors that best match the
+	// acceleration of the com
+	private void findForceVectors(Vector3f leftFootForce, Vector3f rightFootForce) {
+		int iterations = 100;
+		float stepSize = 0.01f;
+		// setup the temporary variables
+		Vector3f tempLeftFootForce1 = leftFootForce.clone();
+		Vector3f tempLeftFootForce2 = leftFootForce.clone();
+		Vector3f tempRightFootForce1 = rightFootForce.clone();
+		Vector3f tempRightFootForce2 = rightFootForce.clone();
+		Vector3f error;
+		Vector3f error1;
+		Vector3f error2;
+		Vector3f error3;
+		Vector3f error4;
+
+		for (int i = 0; i < iterations; i++) {
+			tempLeftFootForce1.set(leftFootForce);
+			tempLeftFootForce2.set(leftFootForce);
+			tempRightFootForce1.set(rightFootForce);
+			tempRightFootForce2.set(rightFootForce);
+
+			// get the error at the current position
+			error = centerOfMassAcceleration
+				.subtract(leftFootForce.add(rightFootForce).add(GRAVITY));
+
+			// add and subtract the error to the force vectors
+			tempLeftFootForce1 = tempLeftFootForce1.mult(1.0f + stepSize);
+			tempLeftFootForce2 = tempLeftFootForce2.mult(1.0f - stepSize);
+			tempRightFootForce1 = tempRightFootForce1.mult(1.0f + stepSize);
+			tempRightFootForce2 = tempRightFootForce2.mult(1.0f - stepSize);
+
+			// get the error at the new position
+			error1 = getForceVectorError(tempLeftFootForce1, rightFootForce);
+			error2 = getForceVectorError(tempLeftFootForce2, rightFootForce);
+			error3 = getForceVectorError(tempRightFootForce1, leftFootForce);
+			error4 = getForceVectorError(tempRightFootForce2, leftFootForce);
+
+			// set the new force vectors
+			if (error1.length() < error.length()) {
+				leftFootForce.set(tempLeftFootForce1);
+			} else if (error2.length() < error.length()) {
+				leftFootForce.set(tempLeftFootForce2);
+			}
+
+			if (error3.length() < error.length()) {
+				rightFootForce.set(tempRightFootForce1);
+			} else if (error4.length() < error.length()) {
+				rightFootForce.set(tempRightFootForce2);
+			}
+		}
+	}
+
+	// detect any outside forces on the body such
+	// as a wall or a chair. returns true if there is a outside force
+	private boolean detectOutsideForces(Vector3f f1, Vector3f f2) {
+		Vector3f force = GRAVITY.add(f1).add(f2);
+		Vector3f error = centerOfMassAcceleration.subtract(force);
+		return error.lengthSquared() > FastMath.sqr(FORCE_ERROR_TOLLERANCE);
+	}
+
+	// simple error function for the force vector gradient descent
+	private Vector3f getForceVectorError(Vector3f testForce, Vector3f otherForce) {
+		return centerOfMassAcceleration
+			.subtract(testForce.add(otherForce).add(GRAVITY));
+	}
 }

server/src/main/java/dev/slimevr/vr/processor/skeleton/LegTweaks.java

@@ -1,8 +1,11 @@
 package dev.slimevr.vr.processor.skeleton;
 
+import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 
+import dev.slimevr.vr.processor.TransformNode;
+
 
 public class LegTweaks {
 	// state variables
@@ -28,8 +31,6 @@ public class LegTweaks {
 	private Vector3f waistPosition = new Vector3f();
 	private Quaternion leftFootRotation = new Quaternion();
 	private Quaternion rightFootRotation = new Quaternion();
-	private Vector3f leftWaistUpperLegOffset = new Vector3f();
-	private Vector3f rightWaistUpperLegOffset = new Vector3f();
 
 	private Vector3f leftFootAcceleration = new Vector3f();
 	private Vector3f rightFootAcceleration = new Vector3f();
@@ -59,30 +60,44 @@ public class LegTweaks {
 	 */
 
 	// hyperparameters (clip correction)
-	private static final float DYNAMIC_DISPLACEMENT_CUTOFF = 0.8f;
-	private static final float MAX_DYNAMIC_DISPLACEMENT = 0.08f;
-	private static final float FLOOR_CALIBRATION_OFFSET = 0.02f;
+	private static final float DYNAMIC_DISPLACEMENT_CUTOFF = 1.0f;
+	private static final float MAX_DYNAMIC_DISPLACEMENT = 0.06f;
+	private static final float FLOOR_CALIBRATION_OFFSET = 0.015f;
 
 	// hyperparameters (skating correction)
-	private static final float MIN_ACCEPTABLE_ERROR = 0.05f;
-	private static final float MAX_ACCEPTABLE_ERROR = LegTweakBuffer.SKATING_DISTANCE_CUTOFF;
-	private static final float CORRECTION_WEIGHT_MIN = 0.25f;
+	private static final float MIN_ACCEPTABLE_ERROR = 0.01f;
+	private static final float MAX_ACCEPTABLE_ERROR = 0.225f;
+	private static final float CORRECTION_WEIGHT_MIN = 0.40f;
 	private static final float CORRECTION_WEIGHT_MAX = 0.70f;
-
-	// hyperparameters (floating feet correction)
-	private static final float FOOT_Y_CORRECTION_WEIGHT = 0.45f;
-	private static final float FOOT_Y_MAX_ACCELERATION = 0.40f;
-	private static final float FOOT_Y_DIFF_CUTOFF = 0.15f;
+	private static final float CONTINUOUS_CORRECTION_DIST = 0.5f;
+	private static final int CONTINUOUS_CORRECTION_WARMUP = 175;
 
 	// hyperparameters (knee / waist correction)
 	private static final float KNEE_CORRECTION_WEIGHT = 0.00f;
-	private static final float KNEE_LATERAL_WEIGHT = 0.9f;
+	private static final float KNEE_LATERAL_WEIGHT = 0.8f;
 	private static final float WAIST_PUSH_WEIGHT = 0.2f;
 
+	// hyperparameters (COM calculation)
+	// mass percentages of the body
+	private static final float HEAD_MASS = 0.082f;
+	private static final float CHEST_MASS = 0.25f;
+	private static final float WAIST_MASS = 0.209f;
+	private static final float THIGH_MASS = 0.128f;
+	private static final float CALF_MASS = 0.0535f;
+	private static final float UPPER_ARM_MASS = 0.031f;
+	private static final float FOREARM_MASS = 0.017f;
+
 	// hyperparameters (misc)
-	private static final float NEARLY_ZERO = 0.005f;
+	static final float NEARLY_ZERO = 0.001f;
 	private static final float STANDING_CUTOFF_VERTICAL = 0.65f;
 	private static final float MAX_DISENGAGMENT_OFFSET = 0.30f;
+	private static final float DEFAULT_ARM_DISTANCE = 0.15f;
+
+	// counters
+	private int leftFramesLocked = 0;
+	private int rightFramesLocked = 0;
+	private int leftFramesUnlocked = 0;
+	private int rightFramesUnlocked = 0;
 
 	// buffer for holding previus frames of data
 	private LegTweakBuffer bufferHead = new LegTweakBuffer();
@@ -92,14 +107,6 @@ public class LegTweaks {
 		this.skeleton = skeleton;
 	}
 
-	// update the offsets for the waist and upper leg
-	// this is used for correcting the knee tracker position
-	public void updateOffsets(Vector3f upperLeftLeg, Vector3f upperRightLeg, Vector3f waist) {
-		// update the relevant leg data
-		this.leftWaistUpperLegOffset = upperLeftLeg.subtract(waist);
-		this.rightWaistUpperLegOffset = upperRightLeg.subtract(waist);
-	}
-
 	public Vector3f getLeftFootPosition() {
 		return leftFootPosition;
 	}
@@ -270,7 +277,7 @@ public class LegTweaks {
 		// tweaks
 		active = isStanding();
 
-		// if the buffer is invalid set it up
+		// if the buffer is invalid add all the extra info
 		if (bufferInvalid) {
 			bufferHead.setLeftFootPositionCorrected(leftFootPosition);
 			bufferHead.setRightFootPositionCorrected(rightFootPosition);
@@ -284,22 +291,18 @@ public class LegTweaks {
 			bufferHead.setWaistPosition(waistPosition);
 			bufferHead.setLeftLegState(LegTweakBuffer.UNLOCKED);
 			bufferHead.setRightLegState(LegTweakBuffer.UNLOCKED);
+
+			// if the system is active propulate the buffer with corrected floor
+			// clip feet positions
+			if (active && isStanding()) {
+				correctClipping();
+				bufferHead.setLeftFootPositionCorrected(leftFootPosition);
+				bufferHead.setRightFootPositionCorrected(rightFootPosition);
+			}
+
 			bufferInvalid = false;
 		}
 
-		// update offsets for knee correction if the knees are not null
-		if (kneesActive) {
-			Vector3f temp1 = new Vector3f();
-			Vector3f temp2 = new Vector3f();
-			Vector3f temp3 = new Vector3f();
-
-			// get offsets from the waist to the upper legs
-			skeleton.leftHipNode.localTransform.getTranslation(temp1);
-			skeleton.rightHipNode.localTransform.getTranslation(temp2);
-			skeleton.trackerWaistNode.localTransform.getTranslation(temp3);
-			updateOffsets(temp1, temp2, temp3);
-		}
-
 		// update the buffer
 		LegTweakBuffer currentFrame = new LegTweakBuffer();
 		currentFrame.setLeftFootPosition(leftFootPosition);
@@ -309,14 +312,15 @@ public class LegTweaks {
 		currentFrame.setRightFootRotation(rightFootRotation);
 		currentFrame.setRightKneePosition(rightKneePosition);
 		currentFrame.setWaistPosition(waistPosition);
+		currentFrame.setCenterOfMass(computeCenterOfMass());
 
-		this.bufferHead
+		currentFrame
 			.setLeftFloorLevel(
 				(floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getLeftFootOffset()))
 					- currentDisengagementOffset
 			);
 
-		this.bufferHead
+		currentFrame
 			.setRightFloorLevel(
 				(floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getRightFootOffset()))
 					- currentDisengagementOffset
@@ -335,10 +339,14 @@ public class LegTweaks {
 			currentFrame.setDetectionMode(LegTweakBuffer.ANKLE_ACCEL);
 		}
 
+		// update the buffer head and compute the current state of the legs
 		currentFrame.setParent(bufferHead);
 		this.bufferHead = currentFrame;
 		this.bufferHead.calculateFootAttributes(active);
 
+		// update the lock duration counters
+		updateLockStateCounters();
+
 		return true;
 	}
 
@@ -354,12 +362,13 @@ public class LegTweaks {
 		if (floorclipEnabled)
 			correctClipping();
 
-		// calculate acceleration and velocity of the feet using the buffer
-		// (only needed if skating correction is enabled)
-		if (skatingCorrectionEnabled) {
+		// correct for skating if needed
+		if (skatingCorrectionEnabled)
 			correctSkating();
-			correctFloat();
-		}
+
+		// currently scuffed
+		// if (skatingCorrectionEnabled && floorclipEnabled)
+		// correctFloat();
 
 		// determine if either leg is in a position to activate or deactivate
 		// (use the buffer to get the positions before corrections)
@@ -532,22 +541,36 @@ public class LegTweaks {
 				// if velocity and dif are pointing in the same direction,
 				// add a small amount of velocity to the dif
 				// else subtract a small amount of velocity from the dif
-				// calculate the correction weight
+				// calculate the correction weight.
+				// it is also right here where the constant correction is
+				// applied
 				float weight = calculateCorrectionWeight(
 					leftFootPosition,
 					bufferHead.getParent().getLeftFootPositionCorrected(null)
 				);
 
 				if (velocity.x * leftFootDif.x > 0) {
-					leftFootPosition.x += velocity.x * weight;
-				} else {
-					leftFootPosition.x -= velocity.x * weight;
+					leftFootPosition.x += (velocity.x * weight)
+						+ (getConstantCorrectionQuantityLeft()
+							* (velocity.x > 0 ? 1 : -1)
+							/ bufferHead.getTimeDelta());
+				} else if (velocity.x * leftFootDif.x < 0) {
+					leftFootPosition.x -= (velocity.x * weight)
+						+ (getConstantCorrectionQuantityLeft()
+							* (velocity.x > 0 ? 1 : -1)
+							/ bufferHead.getTimeDelta());
 				}
 
 				if (velocity.z * leftFootDif.z > 0) {
-					leftFootPosition.z += velocity.z * weight;
-				} else {
-					leftFootPosition.z -= velocity.z * weight;
+					leftFootPosition.z += (velocity.z * weight)
+						+ (getConstantCorrectionQuantityLeft()
+							* (velocity.z > 0 ? 1 : -1)
+							/ bufferHead.getTimeDelta());
+				} else if (velocity.z * leftFootDif.z < 0) {
+					leftFootPosition.z -= (velocity.z * weight)
+						+ (getConstantCorrectionQuantityLeft()
+							* (velocity.z > 0 ? 1 : -1)
+							/ bufferHead.getTimeDelta());
 				}
 
 				// if the foot overshot the target, move it back to the target
@@ -610,15 +633,27 @@ public class LegTweaks {
 				);
 
 				if (velocity.x * rightFootDif.x > 0) {
-					rightFootPosition.x += velocity.x * weight;
-				} else {
-					rightFootPosition.x -= velocity.x * weight;
+					rightFootPosition.x += (velocity.x * weight)
+						+ (getConstantCorrectionQuantityRight()
+							* (velocity.x > 0 ? 1 : -1)
+							/ bufferHead.getTimeDelta());
+				} else if (velocity.x * rightFootDif.x < 0) {
+					rightFootPosition.x -= (velocity.x * weight)
+						+ (getConstantCorrectionQuantityRight()
+							* (velocity.x > 0 ? 1 : -1)
+							/ bufferHead.getTimeDelta());
 				}
 
 				if (velocity.z * rightFootDif.z > 0) {
-					rightFootPosition.z += velocity.z * weight;
-				} else {
-					rightFootPosition.z -= velocity.z * weight;
+					rightFootPosition.z += (velocity.z * weight)
+						+ (getConstantCorrectionQuantityRight()
+							* (velocity.z > 0 ? 1 : -1)
+							/ bufferHead.getTimeDelta());
+				} else if (velocity.z * rightFootDif.z < 0) {
+					rightFootPosition.z -= (velocity.z * weight)
+						+ (getConstantCorrectionQuantityRight()
+							* (velocity.z > 0 ? 1 : -1)
+							/ bufferHead.getTimeDelta());
 				}
 
 				// if the foot overshot the target, move it back to the target
@@ -645,135 +680,6 @@ public class LegTweaks {
 		}
 	}
 
-	// check if the foot is begining to float over the floor when it should not
-	// be if it is move it back down to the floor
-	// note: very small corrections are all that is allowed to prevent momemnt
-	// that looks unrealistic
-	private void correctFloat() {
-		boolean correctLeft = true;
-		boolean correctRight = true;
-
-		if (bufferHead.getLeftLegState() == LegTweakBuffer.LOCKED) {
-			float lastPositionY = bufferHead
-				.getParent()
-				.getLeftFootPositionCorrected(null).y;
-			if (
-				Math.abs(leftFootPosition.y - lastPositionY) < FOOT_Y_DIFF_CUTOFF
-					&& leftFootPosition.y > lastPositionY
-					&& bufferHead.getLeftFootAccelerationY() < FOOT_Y_MAX_ACCELERATION
-			) {
-				leftFootPosition.y = lastPositionY;
-				correctLeft = false;
-			}
-		}
-
-		if (bufferHead.getRightLegState() == LegTweakBuffer.LOCKED) {
-			float lastPositionY = bufferHead
-				.getParent()
-				.getRightFootPositionCorrected(null).y;
-
-			if (
-				Math.abs(rightFootPosition.y - lastPositionY) < FOOT_Y_DIFF_CUTOFF
-					&& rightFootPosition.y > lastPositionY
-					&& bufferHead.getRightFootAccelerationY() < FOOT_Y_MAX_ACCELERATION
-			) {
-				rightFootPosition.y = lastPositionY;
-				correctRight = false;
-			}
-		}
-
-		// using the velocity correct the position
-		if (correctLeft)
-			correctLeftFootTrackerY();
-
-		if (correctRight)
-			correctRightFootTrackerY();
-	}
-
-	private void correctLeftFootTrackerY() {
-		Vector3f temp;
-		Vector3f leftFootDif = leftFootPosition
-			.subtract(bufferHead.getParent().getLeftFootPositionCorrected(null));
-
-		if (Math.abs(leftFootDif.y) > NEARLY_ZERO) {
-			temp = bufferHead
-				.getParent()
-				.getLeftFootPositionCorrected(null)
-				.subtract(
-					bufferHead
-						.getParent()
-						.getLeftFootPosition(null)
-						.subtract(bufferHead.getLeftFootPosition(null))
-				);
-
-			float leftFloor = (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getLeftFootOffset()))
-				- currentDisengagementOffset;
-
-			leftFootPosition.y = Math.max(temp.y, leftFloor);
-
-			Vector3f velocity = bufferHead.getLeftFootVelocity(null);
-
-			if (velocity.y * leftFootDif.y > 0) {
-				leftFootPosition.y += velocity.y * FOOT_Y_CORRECTION_WEIGHT;
-			} else {
-				leftFootPosition.y -= velocity.y * FOOT_Y_CORRECTION_WEIGHT;
-			}
-
-			// check for overshoot and correct if necessary
-			if (
-				checkOverShoot(
-					this.bufferHead.getLeftFootPosition(null).y,
-					this.bufferHead.getParent().getLeftFootPositionCorrected(null).y,
-					leftFootPosition.y
-				)
-			) {
-				leftFootPosition.y = bufferHead.getLeftFootPosition(null).y;
-			}
-		}
-	}
-
-	private void correctRightFootTrackerY() {
-		Vector3f temp;
-		Vector3f rightFootDif = rightFootPosition
-			.subtract(bufferHead.getParent().getRightFootPositionCorrected(null));
-
-		if (Math.abs(rightFootDif.y) > NEARLY_ZERO) {
-			temp = bufferHead
-				.getParent()
-				.getRightFootPositionCorrected(null)
-				.subtract(
-					bufferHead
-						.getParent()
-						.getRightFootPosition(null)
-						.subtract(bufferHead.getRightFootPosition(null))
-				);
-
-			float rightFloor = (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getRightFootOffset()))
-				- currentDisengagementOffset;
-
-			rightFootPosition.y = Math.max(temp.y, rightFloor);
-
-			Vector3f velocity = bufferHead.getRightFootVelocity(null);
-
-			if (velocity.y * rightFootDif.y > 0) {
-				rightFootPosition.y += velocity.y * FOOT_Y_CORRECTION_WEIGHT;
-			} else {
-				rightFootPosition.y -= velocity.y * FOOT_Y_CORRECTION_WEIGHT;
-			}
-
-			// check for overshoot and correct if necessary
-			if (
-				checkOverShoot(
-					this.bufferHead.getRightFootPosition(null).y,
-					this.bufferHead.getParent().getRightFootPositionCorrected(null).y,
-					rightFootPosition.y
-				)
-			) {
-				rightFootPosition.y = bufferHead.getRightFootPosition(null).y;
-			}
-		}
-	}
-
 	// returns true if it is likely the user is standing
 	public boolean isStanding() {
 		// if the waist is below the vertical cutoff, user is not standing
@@ -796,8 +702,8 @@ public class LegTweaks {
 	// move the knees in to a position that is closer to the truth
 	private void solveLowerBody() {
 		// calculate the left and right waist nodes in standing space
-		Vector3f leftWaist = waistPosition;// .add(leftWaistUpperLegOffset);
-		Vector3f rightWaist = waistPosition;// .add(rightWaistUpperLegOffset);
+		Vector3f leftWaist = waistPosition;
+		Vector3f rightWaist = waistPosition;
 
 		Vector3f tempLeft;
 		Vector3f tempRight;
@@ -843,12 +749,12 @@ public class LegTweaks {
 
 	private float getLeftFootOffset() {
 		float offset = computeUnitVector(this.leftFootRotation).y;
-		return clamp(0, DYNAMIC_DISPLACEMENT_CUTOFF, offset);
+		return FastMath.clamp(offset, 0, DYNAMIC_DISPLACEMENT_CUTOFF);
 	}
 
 	private float getRightFootOffset() {
 		float offset = computeUnitVector(this.rightFootRotation).y;
-		return clamp(0, DYNAMIC_DISPLACEMENT_CUTOFF, offset);
+		return FastMath.clamp(offset, 0, DYNAMIC_DISPLACEMENT_CUTOFF);
 	}
 
 	// calculate the weight of foot correction
@@ -870,6 +776,129 @@ public class LegTweaks {
 				* (CORRECTION_WEIGHT_MAX - CORRECTION_WEIGHT_MIN);
 	}
 
+	// calculate the center of mass of the user for the current frame
+	// returns a vector representing the center of mass position
+	private Vector3f computeCenterOfMass() {
+		// perform a check to see if the needed data is available
+		if (
+			skeleton.headNode == null
+				|| skeleton.chestNode == null
+				|| skeleton.waistNode == null
+				|| skeleton.leftFootNode == null
+				|| skeleton.rightFootNode == null
+				|| skeleton.leftKneeNode == null
+				|| skeleton.rightKneeNode == null
+				|| skeleton.leftHipNode == null
+				|| skeleton.rightHipNode == null
+		) {
+			return null;
+		}
+
+		// check if arm data is available
+		boolean armsAvailable = skeleton.hasLeftArmTracker
+			&& skeleton.hasRightArmTracker;
+
+		Vector3f centerOfMass = new Vector3f();
+
+		// compute the center of mass of smaller body parts and then sum them up
+		// with their respective weights
+		Vector3f head = skeleton.headNode.worldTransform.getTranslation();
+		Vector3f chest = skeleton.chestNode.worldTransform.getTranslation();
+		Vector3f waist = skeleton.waistNode.worldTransform.getTranslation();
+		Vector3f leftCalf = getCenterOfJoint(skeleton.leftAnkleNode, skeleton.leftKneeNode);
+		Vector3f rightCalf = getCenterOfJoint(skeleton.rightAnkleNode, skeleton.rightKneeNode);
+		Vector3f leftThigh = getCenterOfJoint(skeleton.leftKneeNode, skeleton.leftHipNode);
+		Vector3f rightThigh = getCenterOfJoint(skeleton.rightKneeNode, skeleton.rightHipNode);
+		centerOfMass = centerOfMass.add(head.mult(HEAD_MASS));
+		centerOfMass = centerOfMass.add(chest.mult(CHEST_MASS));
+		centerOfMass = centerOfMass.add(waist.mult(WAIST_MASS));
+		centerOfMass = centerOfMass.add(leftCalf.mult(CALF_MASS));
+		centerOfMass = centerOfMass.add(rightCalf.mult(CALF_MASS));
+		centerOfMass = centerOfMass.add(leftThigh.mult(THIGH_MASS));
+		centerOfMass = centerOfMass.add(rightThigh.mult(THIGH_MASS));
+
+		if (armsAvailable) {
+			Vector3f leftUpperArm = getCenterOfJoint(
+				skeleton.leftElbowNode,
+				skeleton.leftShoulderTailNode
+			);
+			Vector3f rightUpperArm = getCenterOfJoint(
+				skeleton.rightElbowNode,
+				skeleton.rightShoulderTailNode
+			);
+			Vector3f leftForearm = getCenterOfJoint(skeleton.leftElbowNode, skeleton.leftHandNode);
+			Vector3f rightForearm = getCenterOfJoint(
+				skeleton.rightElbowNode,
+				skeleton.rightHandNode
+			);
+			centerOfMass = centerOfMass.add(leftUpperArm.mult(UPPER_ARM_MASS));
+			centerOfMass = centerOfMass.add(rightUpperArm.mult(UPPER_ARM_MASS));
+			centerOfMass = centerOfMass.add(leftForearm.mult(FOREARM_MASS));
+			centerOfMass = centerOfMass.add(rightForearm.mult(FOREARM_MASS));
+		} else {
+			// if the arms are not avaliable put them slightly in front
+			// of the chest.
+			Vector3f chestUnitVector = computeUnitVector(
+				skeleton.chestNode.worldTransform.getRotation()
+			);
+			Vector3f armLocation = chest.add(chestUnitVector.mult(DEFAULT_ARM_DISTANCE));
+			centerOfMass = centerOfMass.add(armLocation.mult(UPPER_ARM_MASS * 2.0f));
+			centerOfMass = centerOfMass.add(armLocation.mult(FOREARM_MASS * 2.0f));
+		}
+
+		// finally translate in to tracker space
+		centerOfMass = waistPosition
+			.add(
+				centerOfMass.subtract(skeleton.trackerWaistNode.worldTransform.getTranslation(null))
+			);
+
+		return centerOfMass;
+	}
+
+	// get the center of two joints
+	private Vector3f getCenterOfJoint(TransformNode node1, TransformNode node2) {
+		return node1.worldTransform
+			.getTranslation(null)
+			.add(node2.worldTransform.getTranslation(null))
+			.mult(0.5f);
+	}
+
+	// get the amount of the constant correction to apply.
+	private float getConstantCorrectionQuantityLeft() {
+		if (leftFramesUnlocked >= CONTINUOUS_CORRECTION_WARMUP)
+			return CONTINUOUS_CORRECTION_DIST;
+
+		return CONTINUOUS_CORRECTION_DIST
+			* ((float) leftFramesUnlocked / CONTINUOUS_CORRECTION_WARMUP);
+	}
+
+	private float getConstantCorrectionQuantityRight() {
+		if (rightFramesUnlocked >= CONTINUOUS_CORRECTION_WARMUP)
+			return CONTINUOUS_CORRECTION_DIST;
+
+		return CONTINUOUS_CORRECTION_DIST
+			* ((float) rightFramesUnlocked / CONTINUOUS_CORRECTION_WARMUP);
+	}
+
+	// update counters for the lock state of the feet
+	private void updateLockStateCounters() {
+		if (bufferHead.getLeftLegState() == LegTweakBuffer.LOCKED) {
+			leftFramesUnlocked = 0;
+			leftFramesLocked++;
+		} else {
+			leftFramesLocked = 0;
+			leftFramesUnlocked++;
+		}
+
+		if (bufferHead.getRightLegState() == LegTweakBuffer.LOCKED) {
+			rightFramesUnlocked = 0;
+			rightFramesLocked++;
+		} else {
+			rightFramesLocked = 0;
+			rightFramesUnlocked++;
+		}
+	}
+
 	// check if the difference between two floats flipped after correction
 	private boolean checkOverShoot(float trueVal, float valBefore, float valAfter) {
 		return (trueVal - valBefore) * (trueVal - valAfter) < 0;
@@ -879,9 +908,4 @@ public class LegTweaks {
 	private Vector3f computeUnitVector(Quaternion quaternion) {
 		return quaternion.getRotationColumn(2).normalize();
 	}
-
-	// clamp a float between two values
-	private float clamp(float min, float max, float val) {
-		return Math.min(max, Math.max(min, val));
-	}
 }

server/src/main/java/dev/slimevr/vr/processor/skeleton/Skeleton.java

@@ -37,6 +37,9 @@ public abstract class Skeleton {
 	@VRServerThread
 	public abstract void resetTrackersFull();
 
+	@VRServerThread
+	public abstract void resetTrackersMounting();
+
 	@VRServerThread
 	public abstract void resetTrackersYaw();
 

server/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)

server/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java

@@ -108,6 +108,10 @@ public class ComputedTracker implements Tracker, TrackerWithTPS {
 	public void resetYaw(Quaternion reference) {
 	}
 
+	@Override
+	public void resetMounting(boolean reverseYaw) {
+	}
+
 	@Override
 	public TrackerPosition getBodyPosition() {
 		return bodyPosition;

server/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java

@@ -277,6 +277,10 @@ public class IMUTracker
 		// TODO Print "jump" length when correcting if it's more than 1 degree
 	}
 
+	@Override
+	public void resetMounting(boolean reverseYaw) {
+	}
+
 	@Override
 	public TrackerPosition getBodyPosition() {
 		return bodyPosition;

server/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java

@@ -1,5 +1,6 @@
 package dev.slimevr.vr.trackers;
 
+import com.jme3.math.FastMath;
 import com.jme3.math.Quaternion;
 import com.jme3.math.Vector3f;
 import dev.slimevr.config.TrackerConfig;
@@ -9,9 +10,15 @@ import dev.slimevr.vr.Device;
 public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 
 	public final E tracker;
-	public final Quaternion yawFix = new Quaternion();
+
+	// Vector for mounting rotation correction (tilt direction)
+	public final Vector3f rotVector = new Vector3f();
+
 	public final Quaternion gyroFix = new Quaternion();
 	public final Quaternion attachmentFix = new Quaternion();
+	public final Quaternion mountRotFix = new Quaternion();
+	public final Quaternion yawFix = new Quaternion();
+
 	protected float confidenceMultiplier = 1.0f;
 
 	public ReferenceAdjustedTracker(E tracker) {
@@ -34,8 +41,8 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	}
 
 	/**
-	 * Reset the tracker so that it's current rotation is counted as (0, <HMD
-	 * Yaw>, 0). This allows tracker to be strapped to body at any pitch and
+	 * Reset the tracker so that its current rotation is counted as (0, <HMD
+	 * Yaw>, 0). This allows the tracker to be strapped to body at any pitch and
 	 * roll.
 	 * <p>
 	 * Performs {@link #resetYaw(Quaternion)} for yaw drift correction.
@@ -43,12 +50,12 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 	@Override
 	public void resetFull(Quaternion reference) {
 		tracker.resetFull(reference);
-		fixGyroscope();
 
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
-		gyroFix.mult(sensorRotation, sensorRotation);
-		attachmentFix.set(sensorRotation).inverseLocal();
+
+		fixGyroscope(sensorRotation.clone());
+		fixAttachment(sensorRotation.clone());
 
 		fixYaw(reference);
 	}
@@ -65,39 +72,69 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
 		fixYaw(reference);
 	}
 
+	private void fixGyroscope(Quaternion sensorRotation) {
+		sensorRotation.fromAngles(0, sensorRotation.getYaw(), 0);
+		gyroFix.set(sensorRotation).inverseLocal();
+	}
+
+	private void fixAttachment(Quaternion sensorRotation) {
+		gyroFix.mult(sensorRotation, sensorRotation);
+		attachmentFix.set(sensorRotation).inverseLocal();
+	}
+
+	@Override
+	public void resetMounting(boolean reverseYaw) {
+		tracker.resetMounting(reverseYaw);
+
+		// Get the current calibrated rotation
+		Quaternion buffer = new Quaternion();
+		tracker.getRotation(buffer);
+		gyroFix.mult(buffer, buffer);
+		buffer.multLocal(attachmentFix);
+
+		// Reset the vector for the rotation to point straight up
+		rotVector.set(0f, 1f, 0f);
+		// Rotate the vector by the quat, then flatten and normalize the vector
+		buffer.multLocal(rotVector).setY(0f).normalizeLocal();
+
+		// Calculate the yaw angle using tan
+		// Just use an angle offset of zero for unsolvable circumstances
+		float yawAngle = FastMath.isApproxZero(rotVector.x) && FastMath.isApproxZero(rotVector.z)
+			? 0f
+			: FastMath.atan2(rotVector.x, rotVector.z);
+
+		// Make an adjustment quaternion from the angle
+		buffer.fromAngles(0f, reverseYaw ? yawAngle : yawAngle - FastMath.PI, 0f);
+
+		Quaternion lastRotAdjust = new Quaternion(mountRotFix);
+		mountRotFix.set(buffer);
+
+		// Get the difference from the last adjustment
+		buffer.multLocal(lastRotAdjust.inverseLocal());
+		// Apply the yaw rotation difference to the yaw fix quaternion
+		yawFix.multLocal(buffer.inverseLocal());
+	}
+
 	private void fixYaw(Quaternion reference) {
 		// Use only yaw HMD rotation
-		Quaternion targetTrackerRotation = new Quaternion(reference);
-		float[] angles = new float[3];
-		targetTrackerRotation.toAngles(angles);
-		targetTrackerRotation.fromAngles(0, angles[1], 0);
+		Quaternion targetRotation = new Quaternion(reference);
+		targetRotation.fromAngles(0, targetRotation.getYaw(), 0);
 
 		Quaternion sensorRotation = new Quaternion();
 		tracker.getRotation(sensorRotation);
 		gyroFix.mult(sensorRotation, sensorRotation);
 		sensorRotation.multLocal(attachmentFix);
+		sensorRotation.multLocal(mountRotFix);
 
-		sensorRotation.toAngles(angles);
-		sensorRotation.fromAngles(0, angles[1], 0);
+		sensorRotation.fromAngles(0, sensorRotation.getYaw(), 0);
 
-		yawFix.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
-	}
-
-	private void fixGyroscope() {
-		float[] angles = new float[3];
-
-		Quaternion sensorRotation = new Quaternion();
-		tracker.getRotation(sensorRotation);
-
-		sensorRotation.toAngles(angles);
-		sensorRotation.fromAngles(0, angles[1], 0);
-
-		gyroFix.set(sensorRotation).inverseLocal();
+		yawFix.set(sensorRotation).inverseLocal().multLocal(targetRotation);
 	}
 
 	protected void adjustInternal(Quaternion store) {
 		gyroFix.mult(store, store);
 		store.multLocal(attachmentFix);
+		store.multLocal(mountRotFix);
 		yawFix.mult(store, store);
 	}
 

server/src/main/java/dev/slimevr/vr/trackers/Tracker.java

@@ -36,6 +36,8 @@ public interface Tracker {
 
 	void resetYaw(Quaternion reference);
 
+	void resetMounting(boolean reverseYaw);
+
 	void tick();
 
 	TrackerPosition getBodyPosition();