Add Velocity calculation logic and GUI toggle (end-to-end MVP).

gui/public/i18n/en/translation.ftl

@@ -635,6 +635,9 @@ settings-general-fk_settings-enforce_joint_constraints-correct_constraints-descr
 settings-general-fk_settings-ik = Position data
 settings-general-fk_settings-ik-use_position = Use Position data
 settings-general-fk_settings-ik-use_position-description = Enables the use of position data from trackers that provide it. When enabling this make sure to full reset and recalibrate in game.
+settings-general-fk_settings-velocity_settings = Velocity Settings
+settings-general-fk_settings-velocity_settings-description = Send derived velocity data to SteamVR. Required for Natural Locomotion support. May cause jitter in FBT.
+settings-general-fk_settings-velocity_settings-send_derived_velocity = Send derived velocity to driver
 settings-general-fk_settings-arm_fk = Arm tracking
 settings-general-fk_settings-arm_fk-description = Force arms to be tracked from the headset (HMD) even if positional hand data is available.
 settings-general-fk_settings-arm_fk-force_arms = Force arms from HMD

gui/src/components/settings/pages/GeneralSettings.tsx

@@ -15,6 +15,7 @@ import {
   SteamVRTrackersSettingT,
   TapDetectionSettingsT,
   HIDSettingsT,
+  VelocitySettingsT,
   BodyPart,
 } from 'solarxr-protocol';
 import { useConfig } from '@/hooks/config';
@@ -114,6 +115,9 @@ export type SettingsForm = {
   hidSettings: {
     trackersOverHID: boolean;
   };
+  velocitySettings: {
+    sendDerivedVelocity: boolean;
+  };
 };
 
 const defaultValues: SettingsForm = {
@@ -173,6 +177,7 @@ const defaultValues: SettingsForm = {
   resetsSettings: defaultResetSettings,
   stayAligned: defaultStayAlignedSettings,
   hidSettings: { trackersOverHID: false },
+  velocitySettings: { sendDerivedVelocity: false },
 };
 
 const settingsAtom = atom(new SettingsResponseT());
@@ -350,6 +355,11 @@ export function GeneralSettings() {
     hidSettings.trackersOverHid = values.hidSettings.trackersOverHID;
     settingsReq.hidSettings = hidSettings;
 
+    const velocitySettings = new VelocitySettingsT();
+    velocitySettings.sendDerivedVelocity =
+      values.velocitySettings.sendDerivedVelocity;
+    settingsReq.velocitySettings = velocitySettings;
+
     if (values.resetsSettings) {
       settingsReq.resetsSettings = loadResetSettings(values.resetsSettings);
     }
@@ -483,6 +493,12 @@ export function GeneralSettings() {
       };
     }
 
+    if (settings.velocitySettings) {
+      formData.velocitySettings = {
+        sendDerivedVelocity: settings.velocitySettings.sendDerivedVelocity,
+      };
+    }
+
     reset({ ...getValues(), ...formData });
   }, [settings]);
 
@@ -1053,6 +1069,32 @@ export function GeneralSettings() {
               />
             </div>
 
+            <div className="flex flex-col pt-2 pb-1">
+              <Typography variant="section-title">
+                {l10n.getString(
+                  'settings-general-fk_settings-velocity_settings'
+                )}
+              </Typography>
+              <div className="pt-2">
+                <Typography>
+                  {l10n.getString(
+                    'settings-general-fk_settings-velocity_settings-description'
+                  )}
+                </Typography>
+              </div>
+            </div>
+            <div className="grid sm:grid-cols-1 pb-3">
+              <CheckBox
+                variant="toggle"
+                outlined
+                control={control}
+                name="velocitySettings.sendDerivedVelocity"
+                label={l10n.getString(
+                  'settings-general-fk_settings-velocity_settings-send_derived_velocity'
+                )}
+              />
+            </div>
+
             {config?.debug && (
               <>
                 <div className="flex flex-col pt-2 pb-3">

server/core/src/main/java/dev/slimevr/config/VRConfig.kt

@@ -58,6 +58,8 @@ class VRConfig {
 
 	val trackingChecklist: TrackingChecklistConfig = TrackingChecklistConfig()
 
+	val velocityConfig: VelocityConfig = VelocityConfig()
+
 	val vrcConfig: VRCConfig = VRCConfig()
 
 	init {
@@ -119,6 +121,7 @@ class VRConfig {
 					.readFilteringConfig(filters, tracker.getRotation())
 			}
 		}
+		tracker.allowVelocity = velocityConfig.sendDerivedVelocity
 	}
 
 	fun writeTrackerConfig(tracker: Tracker?) {

server/core/src/main/java/dev/slimevr/config/VelocityConfig.kt

@@ -0,0 +1,19 @@
+package dev.slimevr.config
+
+import dev.slimevr.VRServer
+
+/**
+ * Allows to enable/disable sending of optional derived velocity data via Protobuf.
+ * Enables Natural Locomotion Support
+ * May create overprediction in certain titles causing excessive jitter when moving upper body.
+ */
+class VelocityConfig {
+	// Disables derived velocity for all trackers. Driver zeroes out velocity if nothing is returned in protobuf message.
+	var sendDerivedVelocity: Boolean = false
+
+	fun updateTrackersVelocitySettings() {
+		for (t in VRServer.instance.allTrackers) {
+			t.allowVelocity = sendDerivedVelocity
+		}
+	}
+}

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.kt

@@ -15,6 +15,7 @@ import dev.slimevr.config.StayAlignedConfig
 import dev.slimevr.config.TapDetectionConfig
 import dev.slimevr.config.VMCConfig
 import dev.slimevr.config.VRCOSCConfig
+import dev.slimevr.config.VelocityConfig
 import dev.slimevr.filtering.TrackerFilters.Companion.getByConfigkey
 import dev.slimevr.tracking.processor.HumanPoseManager
 import dev.slimevr.tracking.processor.config.SkeletonConfigToggles
@@ -34,6 +35,7 @@ import solarxr_protocol.rpc.SteamVRTrackersSetting
 import solarxr_protocol.rpc.TapDetectionSettings
 import solarxr_protocol.rpc.VMCOSCSettings
 import solarxr_protocol.rpc.VRCOSCSettings
+import solarxr_protocol.rpc.VelocitySettings
 import solarxr_protocol.rpc.settings.LegTweaksSettings
 import solarxr_protocol.rpc.settings.ModelRatios
 import solarxr_protocol.rpc.settings.ModelSettings
@@ -427,6 +429,7 @@ fun createSettingsResponse(fbb: FlatBufferBuilder, server: VRServer): Int {
 			),
 			createHIDSettings(fbb, server.configManager.vrConfig.hidConfig),
 			0,
+			createVelocitySettings(fbb, server.configManager.vrConfig.velocityConfig),
 		)
 }
 
@@ -462,3 +465,12 @@ fun createHIDSettings(
 		fbb,
 		config.trackersOverHID,
 	)
+
+fun createVelocitySettings(
+	fbb: FlatBufferBuilder,
+	config: VelocityConfig,
+): Int = VelocitySettings
+	.createVelocitySettings(
+		fbb,
+		config.sendDerivedVelocity,
+	)

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt

@@ -367,6 +367,12 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 			config.trackersOverHID = requestConfig.trackersOverHid()
 		}
 
+		if (req.velocitySettings() != null) {
+			val velocityConfig = api.server.configManager.vrConfig.velocityConfig
+			velocityConfig.sendDerivedVelocity = req.velocitySettings().sendDerivedVelocity()
+			velocityConfig.updateTrackersVelocitySettings()
+		}
+
 		api.server.configManager.saveConfig()
 	}
 
@@ -384,7 +390,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 			val settings = SettingsResponse
 				.createSettingsResponse(
 					fbb,
-					createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+					createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 				)
 			val outbound =
 				rpcHandler.createRPCMessage(fbb, RpcMessage.SettingsResponse, settings)

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt

@@ -1171,6 +1171,7 @@ class HumanSkeleton(
 			it.position = trackerBone.getTailPosition()
 			it.setRotation(trackerBone.getGlobalRotation() * trackerBone.rotationOffset.inv())
 			it.dataTick()
+			it.updateDerivedVelocity()
 		}
 	}
 

server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt

@@ -12,6 +12,10 @@ import io.eiren.util.BufferedTimer
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Vector3
 import kotlin.properties.Delegates
+import kotlin.time.Duration.Companion.microseconds
+import kotlin.time.Duration.Companion.milliseconds
+import kotlin.time.DurationUnit
+import kotlin.time.TimeSource
 
 const val TIMEOUT_MS = 2_000L
 const val DISCONNECT_MS = 3_000L + TIMEOUT_MS
@@ -75,6 +79,11 @@ class Tracker @JvmOverloads constructor(
 	 */
 	val allowMounting: Boolean = false,
 
+	/**
+	 * If true, the tracker will send Derived Velocity.
+	 */
+	var allowVelocity: Boolean = false,
+
 	val isHmd: Boolean = false,
 
 	/**
@@ -111,7 +120,19 @@ class Tracker @JvmOverloads constructor(
 	// IMU: +z forward, +x left, +y up
 	// SlimeVR: +z backward, +x right, +y up
 	private var _acceleration = Vector3.NULL
+	private var _velocity = Vector3.NULL
 	private var _magVector = Vector3.NULL
+
+	/**
+	 * Velocity state server-side differentiation based on sent poses
+	 */
+	private data class VelocityState(
+		var prevMark: TimeSource.Monotonic.ValueTimeMark? = null,
+		var prevPos: Vector3 = Vector3(0f, 0f, 0f),
+	)
+
+	private val velocityState = VelocityState()
+
 	var position = Vector3.NULL
 	val resetsHandler: TrackerResetsHandler = TrackerResetsHandler(this)
 	val filteringHandler: TrackerFilteringHandler = TrackerFilteringHandler()
@@ -345,6 +366,39 @@ class Tracker @JvmOverloads constructor(
 		}
 	}
 
+	/**
+	 * If derived velocity is enabled, updates the derived velocity of the tracker,
+	 * calculating it based on the position displacement since the last update,
+	 * applying a sanity check on the time delta to filter out noise and ensure data stability.
+	 */
+	fun updateDerivedVelocity() {
+		if (!allowVelocity || !hasPosition) {
+			velocityState.prevMark = null
+			_velocity = Vector3.NULL
+			return
+		}
+
+		val pos = position
+		val now = TimeSource.Monotonic.markNow()
+
+		val prevMark = velocityState.prevMark
+		if (prevMark != null) {
+			val dt = now - prevMark
+			if (dt in 100.microseconds..250.milliseconds) {
+				val dtSeconds = dt.toDouble(DurationUnit.SECONDS)
+				_velocity = Vector3(
+					((pos.x - velocityState.prevPos.x) / dtSeconds).toFloat(),
+					((pos.y - velocityState.prevPos.y) / dtSeconds).toFloat(),
+					((pos.z - velocityState.prevPos.z) / dtSeconds).toFloat(),
+				)
+			} else {
+				_velocity = Vector3.NULL
+			}
+		}
+		velocityState.prevMark = now
+		velocityState.prevPos = pos
+	}
+
 	/**
 	 * Gets the identity-adjusted tracker rotation after the resetsHandler's corrections
 	 * (identity reset, drift and identity mounting).
@@ -421,6 +475,24 @@ class Tracker @JvmOverloads constructor(
 		this._acceleration = vec
 	}
 
+	/**
+	 * Sets the derived velocity of the tracker.
+	 */
+	fun setVelocity(vec: Vector3) {
+		this._velocity = if (allowVelocity) vec else Vector3.NULL
+	}
+
+	/**
+	 * Gets the derived velocity of the tracker.
+	 */
+	fun getVelocity(): Vector3 = _velocity
+
+	/**
+	 * True if the tracker has valid velocity data.
+	 */
+	val hasVelocity: Boolean
+		get() = allowVelocity && _velocity != Vector3.NULL
+
 	/**
 	 * True if the raw rotation is coming directly from an IMU (no cameras or lighthouses)
 	 * For example, flex sensor trackers are not considered as IMU trackers (see TrackerDataType)

server/desktop/src/main/java/dev/slimevr/desktop/platform/ProtobufBridge.kt

@@ -82,6 +82,7 @@ abstract class ProtobufBridge(@JvmField protected val bridgeName: String) : ISte
 		target.position = source.position
 		target.setRotation(source.getRotation())
 		target.status = source.status
+		target.setVelocity(source.getVelocity())
 		target.batteryLevel = source.batteryLevel
 		target.batteryVoltage = source.batteryVoltage
 		target.dataTick()
@@ -102,24 +103,37 @@ abstract class ProtobufBridge(@JvmField protected val bridgeName: String) : ISte
 	}
 
 	@VRServerThread
-	protected fun writeTrackerUpdate(localTracker: Tracker?) {
-		val builder = ProtobufMessages.Position.newBuilder().setTrackerId(
-			localTracker!!.id,
-		)
+	protected fun writeTrackerUpdate(localTracker: Tracker) {
+		val builder = ProtobufMessages.Position.newBuilder()
+			.setTrackerId(localTracker.id)
+
 		if (localTracker.hasPosition) {
 			val pos = localTracker.position
 			builder.setX(pos.x)
 			builder.setY(pos.y)
 			builder.setZ(pos.z)
 		}
+
 		if (localTracker.hasRotation) {
 			val rot = localTracker.getRotation()
 			builder.setQx(rot.x)
 			builder.setQy(rot.y)
 			builder.setQz(rot.z)
 			builder.setQw(rot.w)
 		}
-		sendMessage(ProtobufMessage.newBuilder().setPosition(builder).build())
+
+		if (localTracker.hasVelocity) {
+			val vel = localTracker.getVelocity()
+			builder.setVx(vel.x)
+			builder.setVy(vel.y)
+			builder.setVz(vel.z)
+		}
+
+		sendMessage(
+			ProtobufMessage.newBuilder()
+				.setPosition(builder)
+				.build(),
+		)
 	}
 
 	@VRServerThread
@@ -169,6 +183,16 @@ abstract class ProtobufBridge(@JvmField protected val bridgeName: String) : ISte
 					),
 
 				)
+			if (positionMessage.hasVx()) {
+				tracker
+					.setVelocity(
+						Vector3(
+							positionMessage.vx,
+							positionMessage.vy,
+							positionMessage.vz,
+						),
+					)
+			}
 			tracker.dataTick()
 		}
 	}