diff --git a/gui/public/i18n/en/translation.ftl b/gui/public/i18n/en/translation.ftl index e91247bac8..6002fc8d04 100644 --- a/gui/public/i18n/en/translation.ftl +++ b/gui/public/i18n/en/translation.ftl @@ -639,6 +639,8 @@ 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 settings-general-fk_settings-reset_settings = Reset settings +settings-general-fk_settings-reset_settings-step_mounting-description = Use step mounting method instead of ski pose. +settings-general-fk_settings-reset_settings-step_mounting = Step mounting settings-general-fk_settings-reset_settings-reset_hmd_pitch-description = Reset the HMD's pitch (vertical rotation) upon doing a full reset. Useful if wearing an HMD on the forehead for VTubing or mocap. Do not enable for VR. settings-general-fk_settings-reset_settings-reset_hmd_pitch = Reset HMD pitch settings-general-fk_settings-arm_fk-reset_mode-description = Change which arm pose is expected for mounting calibration. diff --git a/gui/src/components/settings/pages/GeneralSettings.tsx b/gui/src/components/settings/pages/GeneralSettings.tsx index 56b8adcbb0..798b52a1c7 100644 --- a/gui/src/components/settings/pages/GeneralSettings.tsx +++ b/gui/src/components/settings/pages/GeneralSettings.tsx @@ -872,46 +872,64 @@ export function GeneralSettings() { /> -
+
{l10n.getString('settings-general-fk_settings-reset_settings')}
-
-
-
- - {l10n.getString( - 'settings-general-fk_settings-reset_settings-reset_hmd_pitch-description' - )} - - -
-
- - {l10n.getString( - 'settings-general-fk_settings-leg_fk-reset_mounting_feet-description-v1' - )} - - -
-
+
+ + {l10n.getString( + 'settings-general-fk_settings-reset_settings-step_mounting-description' + )} + +
+
+ +
+
+ + {l10n.getString( + 'settings-general-fk_settings-reset_settings-reset_hmd_pitch-description' + )} + +
+
+ +
+
+ + {l10n.getString( + 'settings-general-fk_settings-leg_fk-reset_mounting_feet-description-v1' + )} + +
+
+
diff --git a/gui/src/components/tracker/TrackerCard.tsx b/gui/src/components/tracker/TrackerCard.tsx index a5e63d0a1e..995abb56f2 100644 --- a/gui/src/components/tracker/TrackerCard.tsx +++ b/gui/src/components/tracker/TrackerCard.tsx @@ -100,7 +100,9 @@ function TrackerSmol({ 'border-[3px] border-opacity-80 rounded-md overflow-clip', { 'border-status-warning': warning, - 'border-transparent': !warning, + 'border-transparent': + !warning && !tracker.accelRecordingInProgress, + 'border-status-recording': tracker.accelRecordingInProgress, } )} > diff --git a/gui/src/components/tracker/TrackersTable.tsx b/gui/src/components/tracker/TrackersTable.tsx index f02ac96d76..1a96c9b94a 100644 --- a/gui/src/components/tracker/TrackersTable.tsx +++ b/gui/src/components/tracker/TrackersTable.tsx @@ -60,7 +60,9 @@ export function TrackerNameCell({ 'border-[2px] border-opacity-80 rounded-md overflow-clip', { 'border-status-warning': warning, - 'border-transparent': !warning, + 'border-transparent': + !warning && !tracker.accelRecordingInProgress, + 'border-status-recording': tracker.accelRecordingInProgress, } )} > diff --git a/gui/src/hooks/datafeed-config.ts b/gui/src/hooks/datafeed-config.ts index 0bf7337b90..4e807b02df 100644 --- a/gui/src/hooks/datafeed-config.ts +++ b/gui/src/hooks/datafeed-config.ts @@ -19,6 +19,7 @@ export function useDataFeedConfig() { trackerData.tps = true; trackerData.rawMagneticVector = true; trackerData.stayAligned = true; + trackerData.accelRecordingInProgress = true; const dataMask = new DeviceDataMaskT(); dataMask.deviceData = true; diff --git a/gui/src/hooks/reset-settings.ts b/gui/src/hooks/reset-settings.ts index 8b94e20fad..1a39d2dd85 100644 --- a/gui/src/hooks/reset-settings.ts +++ b/gui/src/hooks/reset-settings.ts @@ -14,6 +14,7 @@ export interface ResetSettingsForm { yawResetSmoothTime: number; saveMountingReset: boolean; resetHmdPitch: boolean; + stepMounting: boolean; } export const defaultResetSettings = { @@ -22,6 +23,7 @@ export const defaultResetSettings = { yawResetSmoothTime: 0.0, saveMountingReset: false, resetHmdPitch: false, + stepMounting: false, }; export function loadResetSettings(resetSettingsForm: ResetSettingsForm) { @@ -31,6 +33,7 @@ export function loadResetSettings(resetSettingsForm: ResetSettingsForm) { resetsSettings.yawResetSmoothTime = resetSettingsForm.yawResetSmoothTime; resetsSettings.saveMountingReset = resetSettingsForm.saveMountingReset; resetsSettings.resetHmdPitch = resetSettingsForm.resetHmdPitch; + resetsSettings.stepMounting = resetSettingsForm.stepMounting; return resetsSettings; } diff --git a/gui/src/index.scss b/gui/src/index.scss index cdfad8519e..6360c9cbb8 100644 --- a/gui/src/index.scss +++ b/gui/src/index.scss @@ -95,6 +95,7 @@ body { --warning: 255, 225, 53; --critical: 223, 109, 140; --special: 164, 79, 237; + --recording: 255, 84, 84; --window-icon-stroke: 192, 161, 216; --default-color: 255, 255, 255; diff --git a/gui/tailwind.config.ts b/gui/tailwind.config.ts index 07004e88cd..05200fae4e 100644 --- a/gui/tailwind.config.ts +++ b/gui/tailwind.config.ts @@ -191,6 +191,7 @@ const config = { warning: 'rgb(var(--warning), )', critical: 'rgb(var(--critical), )', special: 'rgb(var(--special), )', + recording: 'rgb(var(--recording), )', }, window: { icon: 'rgb(var(--window-icon-stroke), )', diff --git a/server/core/src/main/java/dev/slimevr/config/ResetsConfig.kt b/server/core/src/main/java/dev/slimevr/config/ResetsConfig.kt index 65cc761d84..84d89cf483 100644 --- a/server/core/src/main/java/dev/slimevr/config/ResetsConfig.kt +++ b/server/core/src/main/java/dev/slimevr/config/ResetsConfig.kt @@ -48,6 +48,7 @@ enum class MountingMethods(val id: Int) { } class ResetsConfig { + var stepMounting = false // Always reset mounting for feet var resetMountingFeet = false diff --git a/server/core/src/main/java/dev/slimevr/protocol/datafeed/DataFeedBuilder.kt b/server/core/src/main/java/dev/slimevr/protocol/datafeed/DataFeedBuilder.kt index 8d1f2cb67d..652d1f53ef 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/datafeed/DataFeedBuilder.kt +++ b/server/core/src/main/java/dev/slimevr/protocol/datafeed/DataFeedBuilder.kt @@ -275,6 +275,9 @@ fun createTrackerData( if (mask.stayAligned) { TrackerData.addStayAligned(fbb, stayAlignedOffset) } + if (mask.accelRecordingInProgress) { + TrackerData.addAccelRecordingInProgress(fbb, tracker.accelMountInProgress) + } return TrackerData.endTrackerData(fbb) } diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.kt b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.kt index b7d41288af..5c1fba1c4b 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.kt +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.kt @@ -369,6 +369,7 @@ fun createArmsResetModeSettings( resetsConfig.yawResetSmoothTime, resetsConfig.saveMountingReset, resetsConfig.resetHmdPitch, + resetsConfig.stepMounting, ) fun createSettingsResponse(fbb: FlatBufferBuilder, server: VRServer): Int { diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt index b2aae0599f..796f6241fd 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt @@ -333,6 +333,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) { resetsConfig.saveMountingReset = req.resetsSettings().saveMountingReset() resetsConfig.yawResetSmoothTime = req.resetsSettings().yawResetSmoothTime() resetsConfig.resetHmdPitch = req.resetsSettings().resetHmdPitch() + resetsConfig.stepMounting = req.resetsSettings().stepMounting() resetsConfig.updateTrackersResetsSettings() } diff --git a/server/core/src/main/java/dev/slimevr/reset/accel/AccelResetHandler.kt b/server/core/src/main/java/dev/slimevr/reset/accel/AccelResetHandler.kt new file mode 100644 index 0000000000..670d6be3d1 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/reset/accel/AccelResetHandler.kt @@ -0,0 +1,236 @@ +package dev.slimevr.reset.accel + +import dev.slimevr.tracking.trackers.Tracker +import dev.slimevr.util.AccelAccumulator +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.Vector3 +import java.util.Timer +import java.util.TimerTask +import java.util.concurrent.locks.ReentrantLock +import kotlin.concurrent.schedule +import kotlin.concurrent.thread +import kotlin.concurrent.withLock +import kotlin.time.Duration.Companion.seconds +import kotlin.time.DurationUnit +import kotlin.time.TimeSource + +// Handles recording and processing of acceleration-based session calibration +class AccelResetHandler(val timeSource: TimeSource.WithComparableMarks = TimeSource.Monotonic) { + var isRunning: Boolean = false + private set + var isDetecting: Boolean = false + private set + var isRecording: Boolean = false + private set + + private val recordingLock = ReentrantLock() + + private var hmd: Tracker? = null + private val trackers: MutableList = mutableListOf() + + private val timeoutTimer = Timer() + private var timerTask: TimerTask? = null + + private var recStartTime = timeSource.markNow() + + /** + * Starts the accel reset process. performing rest detection on the trackers + * provided to automatically control the recording period. + */ + fun start(hmd: Tracker, trackers: Iterable) = recordingLock.withLock { + // Maybe should throw IllegalStateException? Or just restart? + if (isRunning) return + + // Nothing to do + if (trackers.none()) return + + // Initialize our state + isRunning = true + this.hmd = hmd + + // Register our tracker event listener + for (tracker in trackers) { + val wrappedTracker = RecordingWrapper(tracker) + this.trackers.add(wrappedTracker) + tracker.accelTickCallback = { + onAccelData(wrappedTracker) + } + } + + // Start waiting for movement + isDetecting = true + timerTask?.cancel() + timerTask = timeoutTimer.schedule(START_TIMEOUT.inWholeMilliseconds) { + timeout() + } + + LogManager.info("[AccelResetHandler] Reset requested, detecting movement...") + } + + /** + * Handles rest detection and data collection. + */ + private fun onAccelData(tracker: RecordingWrapper) { + if (!isDetecting) return + + val sample = tracker.makeSample(timeSource.markNow(), hmd?.position ?: Vector3.NULL) + + // Rest detection + tracker.updateRestState(sample) + tracker.addRestSample(sample) + // TODO: This shouldn't be done like this + tracker.tracker.accelMountInProgress = isRecording && tracker.moving + + if (!isRecording) { + // We haven't started moving yet, don't start recording + if (!tracker.moving) return + + // Start recording + recordingLock.withLock { + // Race condition + if (isRecording) return@withLock + + // Dump rest detection into the recording on tracker threads + for (tracker in trackers) tracker.dumpRest = true + + isRecording = true + recStartTime = timeSource.markNow() + timerTask?.cancel() + timerTask = timeoutTimer.schedule(RECORD_TIMEOUT.inWholeMilliseconds) { + timeout() + } + + LogManager.info("[AccelResetHandler] Movement detected, recording started!") + } + } else if ( + timeSource.markNow() - recStartTime > MINIMUM_DURATION && + trackers.none { it.moving } + ) { + // We're recording, the minimum duration has passed, and no trackers are + // moving, therefore we can stop the recording and process it + recordingLock.withLock { + // Race condition + if (!isRecording) return + stop() + // Let's not block the tracker thread while processing + thread { + process() + } + } + return + } + + // Take the latest sample or dump the rest detection samples into the recording + if (!tracker.dumpRest) { + tracker.recording.add(sample) + } else { + tracker.recording.addAll(tracker.restDetect) + tracker.dumpRest = false + } + } + + /** + * Stops recording, processes the recorded data, then resets this handler. + */ + private fun process() { + LogManager.info("[AccelResetHandler] Done recording, processing...") + + for (tracker in trackers) { + val firstSample = tracker.recording.first() + val lastSample = tracker.recording.last() + + // Compute the unbiased final velocity + val calibAccum = AccelAccumulator() + RecordingProcessor.processTimeline(calibAccum, tracker) + + // Assume the final velocity is zero (at rest), we can divide our unbiased + // final velocity (m/s) by the duration and get a static acceleration + // offset (m/s^2) + val duration = lastSample.time - firstSample.time + val bias = calibAccum.velocity / duration.toDouble(DurationUnit.SECONDS).toFloat() + + // Compute the biased final offset + val finalAccum = AccelAccumulator() + RecordingProcessor.processTimeline(finalAccum, tracker, accelBias = bias) + + // Compute the final offsets + val trackerOffset = finalAccum.offset + val trackerXZ = Vector3(trackerOffset.x, 0f, trackerOffset.z) + val hmdOffset = lastSample.hmdPos - firstSample.hmdPos + val hmdXZ = Vector3(hmdOffset.x, 0f, hmdOffset.z) + + // TODO: Fail on high error + + // Compute mounting to fix the yaw offset from tracker to HMD + val mountRot = RecordingProcessor.angle(trackerXZ.unit()) * + RecordingProcessor.angle(hmdXZ.unit()).inv() + + // Apply that mounting to the tracker + val resetsHandler = tracker.tracker.resetsHandler + val finalMounting = resetsHandler.mountingOrientation * resetsHandler.mountRotFix * mountRot + resetsHandler.mountRotFix *= mountRot + + LogManager.info( + "[Accel] Tracker ${tracker.tracker.id} (${tracker.tracker.trackerPosition?.designation}):\n" + + "Tracker offset: $trackerOffset\n" + + "HMD offset: $hmdOffset\n" + + "Error value (meters): ${trackerXZ.len() - hmdXZ.len()}\n" + + "Resulting mounting: $finalMounting", + ) + } + + clean() + } + + /** + * Stops recording without clearing the recorded data. + */ + private fun stop() = recordingLock.withLock { + // Cancel any pending timeouts + timerTask?.cancel() + timerTask = null + + isDetecting = false + isRecording = false + + // Unregister our tracker event listener + for (tracker in trackers) { + tracker.tracker.accelTickCallback = null + tracker.tracker.accelMountInProgress = false + } + } + + /** + * Immediately stops execution and resets this handler. + */ + private fun clean() { + stop() + + // Reset data storage + hmd = null + trackers.clear() + + isRunning = false + } + + /** + * Stops the accel reset process and resets this handler. + */ + fun cancel() { + clean() + } + + /** + * Indicates that the process has timed out, then resets this handler. + */ + private fun timeout() { + LogManager.warning("[AccelResetHandler] Reset timed out, aborting") + clean() + } + + companion object { + val START_TIMEOUT = 8.seconds + val MINIMUM_DURATION = 2.seconds + val RECORD_TIMEOUT = 8.seconds + } +} diff --git a/server/core/src/main/java/dev/slimevr/reset/accel/RecordingProcessor.kt b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingProcessor.kt new file mode 100644 index 0000000000..df7c9858ed --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingProcessor.kt @@ -0,0 +1,44 @@ +package dev.slimevr.reset.accel + +import dev.slimevr.util.AccelAccumulator +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import kotlin.math.atan2 +import kotlin.time.ComparableTimeMark +import kotlin.time.Duration +import kotlin.time.DurationUnit + +object RecordingProcessor { + fun accumSample( + accum: AccelAccumulator, + sample: RecordingSample, + lastSampleTime: ComparableTimeMark? = null, + accelBias: Vector3 = Vector3.NULL, + ): Duration { + val delta = lastSampleTime?.let { sample.time - it } ?: Duration.ZERO + accum.dataTick(sample.accel - accelBias, delta.toDouble(DurationUnit.SECONDS).toFloat()) + + return delta + } + + fun processTimeline( + accum: AccelAccumulator, + wrapper: RecordingWrapper, + lastSampleTime: ComparableTimeMark? = null, + accelBias: Vector3 = Vector3.NULL, + ): ComparableTimeMark? { + var lastTime = lastSampleTime + + for (sample in wrapper.recording) { + accumSample(accum, sample, lastTime, accelBias) + lastTime = sample.time + } + + return lastTime + } + + fun angle(vector: Vector3): Quaternion { + val yaw = atan2(vector.x, vector.z) + return Quaternion.rotationAroundYAxis(yaw) + } +} diff --git a/server/core/src/main/java/dev/slimevr/reset/accel/RecordingSample.kt b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingSample.kt new file mode 100644 index 0000000000..fcd40a3b0d --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingSample.kt @@ -0,0 +1,14 @@ +package dev.slimevr.reset.accel + +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import kotlin.time.ComparableTimeMark + +data class RecordingSample( + val time: ComparableTimeMark, + // Tracker + val accel: Vector3, + val rot: Quaternion, + // HMD + val hmdPos: Vector3, +) diff --git a/server/core/src/main/java/dev/slimevr/reset/accel/RecordingWrapper.kt b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingWrapper.kt new file mode 100644 index 0000000000..00dbccbc71 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingWrapper.kt @@ -0,0 +1,63 @@ +package dev.slimevr.reset.accel + +import dev.slimevr.autobone.StatsCalculator +import dev.slimevr.tracking.trackers.Tracker +import io.github.axisangles.ktmath.Vector3 +import org.apache.commons.collections4.queue.CircularFifoQueue +import kotlin.time.ComparableTimeMark +import kotlin.time.Duration.Companion.milliseconds + +data class RecordingWrapper(val tracker: Tracker, var moving: Boolean = false) { + // Buffer for performing rest detection + val restDetect = CircularFifoQueue(8) + + // List capacity assuming ~10 seconds at 100 TPS + val recording: MutableList = ArrayList(1024) + + // Whether to dump our rest detection into the recording on the next sample + var dumpRest = false + + fun makeSample(time: ComparableTimeMark, hmdPos: Vector3): RecordingSample = RecordingSample( + time, + tracker.getAcceleration(), + tracker.getRotation(), + hmdPos, + ) + + fun addRestSample(sample: RecordingSample): Boolean { + // Collect samples for rest detection at a constant-ish rate if possible + return if (moving && restDetect.isNotEmpty()) { + val lastSampleTime = restDetect.last().time + // Try to have TPS at a lower rate + if (sample.time - lastSampleTime > REST_INTERVAL) { + restDetect.add(sample) + } else { + false + } + } else { + restDetect.add(sample) + } + } + + fun updateRestState(new: RecordingSample): Boolean { + if (restDetect.size < 4) return moving + + val stats = StatsCalculator() + for (sample in restDetect) { + stats.addValue(sample.accel.len()) + } + + // Conditions to start or remain moving + // TODO: Add rotation as a rest metric + moving = if (moving) { + stats.mean >= 0.2f || stats.standardDeviation >= 0.25f + } else { + stats.mean >= 0.3f || new.accel.len() - stats.mean >= 0.6f + } + return moving + } + + companion object { + val REST_INTERVAL = 100.milliseconds + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 2baef0bcd5..d157bc0d41 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -3,6 +3,7 @@ package dev.slimevr.tracking.processor.skeleton import dev.slimevr.VRServer import dev.slimevr.config.MountingMethods import dev.slimevr.config.StayAlignedConfig +import dev.slimevr.reset.accel.AccelResetHandler import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.processor.BoneType import dev.slimevr.tracking.processor.Constraint @@ -217,6 +218,8 @@ class HumanSkeleton( var ikSolver = IKSolver(headBone) var userHeightCalibration: UserHeightCalibration? = null + val accelResetHandler = AccelResetHandler() + // Stay Aligned var trackerSkeleton = TrackerSkeleton(this) var stayAlignedConfig = StayAlignedConfig() @@ -1621,6 +1624,29 @@ class HumanSkeleton( return } + // TODO: Make this not dumb + if (headTracker?.resetsHandler?.stepMounting == true || + trackersToReset.any { it?.resetsHandler?.stepMounting == true } + ) { + headTracker?.let { hmd -> + // Make sure we have HMD position + if (!hmd.hasPosition) { + return@let + } + + // Start step mounting + accelResetHandler.start( + hmd, + trackersToReset.filterNotNull().filter { + it.allowMounting && (bodyParts.isEmpty() || bodyParts.contains(it.trackerPosition?.bodyPart)) + }, + ) + return + } + LogManager.info("[HumanSkeleton] Reset: mounting ($resetSourceName) failed, HMD is not available") + return + } + // Resets the mounting orientation of the trackers with the HMD as reference. var referenceRotation = IDENTITY headTracker?.let { diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt index a3d03ea10b..af787948da 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt @@ -179,6 +179,11 @@ class Tracker @JvmOverloads constructor( val stayAligned = StayAlignedTrackerState(this) val yawResetSmoothing = InterpolationHandler() + // Currently only used for accel resets, to add anything else, consider using a + // subscribable event listener instead + var accelTickCallback: ((tracker: Tracker) -> Unit)? = null + var accelMountInProgress = false + init { // IMPORTANT: Look here for the required states of inputs require(!allowReset || (hasRotation && allowReset)) { @@ -294,6 +299,13 @@ class Tracker @JvmOverloads constructor( } } + /** + * Tells the tracker that it received new accel data + * Accel may be (and usually is) desynced from rotation data, so if we want the + * latest, we need to process it here + */ + fun accelDataTick() = accelTickCallback?.invoke(this) + /** * A way to delay the timeout of the tracker */ diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index 25a5fd3426..da5536d366 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -39,6 +39,7 @@ class TrackerResetsHandler(val tracker: Tracker) { private var yawResetSmoothTime = 0.0f var saveMountingReset = false var resetHmdPitch = false + var stepMounting = false var allowDriftCompensation = false var lastResetQuaternion: Quaternion? = null @@ -85,7 +86,6 @@ class TrackerResetsHandler(val tracker: Tracker) { * [mountingOrientation] will apply. */ var mountRotFix = Quaternion.IDENTITY - private set /** * Yaw fix is set by yaw reset. This sets the current y rotation to match the @@ -165,6 +165,7 @@ class TrackerResetsHandler(val tracker: Tracker) { yawResetSmoothTime = config.yawResetSmoothTime saveMountingReset = config.saveMountingReset resetHmdPitch = config.resetHmdPitch + stepMounting = config.stepMounting } fun trySetMountingReset(quat: Quaternion) { @@ -193,7 +194,7 @@ class TrackerResetsHandler(val tracker: Tracker) { // make acceleration worse, so I'm just leaving this until we work that out. The // output of this will be world space, but with an unknown offset to heading (yaw). // - Butterscotch - fun getReferenceAdjustedAccel(rawRot: Quaternion, accel: Vector3): Vector3 = rawRot.sandwich(accel) + fun getReferenceAdjustedAccel(rawRot: Quaternion, accel: Vector3): Vector3 = (gyroFix * mountRotFix.inv() * yawFix * rawRot).sandwich(accel) /** * Converts raw or filtered rotation into reference- and diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt index 598e1a549f..75f4b407ae 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/hid/HIDCommon.kt @@ -435,6 +435,9 @@ class HIDCommon { } else { tracker.heartbeat() // else, something received } + if (packetType == 1 || packetType == 2 || packetType == 7) { + tracker.accelDataTick() + } } } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt index 5bbf831749..b83d02829f 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt @@ -424,6 +424,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker } else { tracker.setAcceleration(SENSOR_OFFSET_CORRECTION.sandwich(packet.acceleration)) } + tracker.accelDataTick() } tracker.dataTick() } @@ -462,6 +463,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker } else { tracker.setAcceleration(SENSOR_OFFSET_CORRECTION.sandwich(packet.acceleration)) } + tracker.accelDataTick() } is UDPPacket10PingPong -> { diff --git a/server/core/src/main/java/dev/slimevr/util/AccelAccumulator.kt b/server/core/src/main/java/dev/slimevr/util/AccelAccumulator.kt new file mode 100644 index 0000000000..9ad0c816a8 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/util/AccelAccumulator.kt @@ -0,0 +1,24 @@ +package dev.slimevr.util + +import com.jme3.system.NanoTimer +import io.github.axisangles.ktmath.Vector3 + +class AccelAccumulator { + var acceleration = Vector3.NULL + private set + var velocity = Vector3.NULL + private set + var offset = Vector3.NULL + private set + + val timer = NanoTimer() + + fun dataTick(acceleration: Vector3, time: Float? = null) { + timer.update() + val deltaTime = time ?: timer.timePerFrame + + this.acceleration = acceleration + offset += (velocity * deltaTime) + ((acceleration * deltaTime * deltaTime) / 2f) + velocity += acceleration * deltaTime + } +} diff --git a/server/core/src/test/java/dev/slimevr/unit/AccelMountTests.kt b/server/core/src/test/java/dev/slimevr/unit/AccelMountTests.kt new file mode 100644 index 0000000000..fb05133535 --- /dev/null +++ b/server/core/src/test/java/dev/slimevr/unit/AccelMountTests.kt @@ -0,0 +1,64 @@ +package dev.slimevr.unit + +import dev.slimevr.unit.TrackerTestUtils.assertVectorApproxEqual +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import org.junit.jupiter.api.DynamicTest +import org.junit.jupiter.api.TestFactory +import kotlin.math.atan2 + +class AccelMountTests { + @TestFactory + fun testAccelAlignment(): List = testSet.map { t -> + DynamicTest.dynamicTest( + "Alignment of accel (Expected: ${t.expected}, reference: ${t.hmd})", + ) { + checkAlignAccel(t.hmd, t.tracker, t.expected) + } + } + + fun angle(vector: Vector3): Quaternion { + val yaw = atan2(vector.x, vector.z) + return Quaternion.rotationAroundYAxis(yaw) + } + + fun checkAlignAccel(hmd: Vector3, tracker: Vector3, expected: Vector3) { + // All we really care about is the angle difference between hmdRot and trackerRot + val hmdRot = angle(hmd.unit()).inv() + val trackerRot = angle(tracker.unit()) + val result = (trackerRot * hmdRot).sandwichUnitZ() + + assertVectorApproxEqual( + expected, + result, + "Resulting vector is not equal to reference vector ($expected vs $result)", + ) + } + + data class AlignTest(val hmd: Vector3, val tracker: Vector3, val expected: Vector3) + + companion object { + val testSet = arrayOf( + // Front mount + AlignTest(Vector3.POS_X, Vector3.POS_X, Vector3.POS_Z), + AlignTest(Vector3.NEG_X, Vector3.NEG_X, Vector3.POS_Z), + AlignTest(Vector3.POS_Z, Vector3.POS_Z, Vector3.POS_Z), + AlignTest(Vector3.NEG_Z, Vector3.NEG_Z, Vector3.POS_Z), + // Right mount + AlignTest(Vector3.POS_X, Vector3.NEG_Z, Vector3.POS_X), + AlignTest(Vector3.NEG_X, Vector3.POS_Z, Vector3.POS_X), + AlignTest(Vector3.POS_Z, Vector3.POS_X, Vector3.POS_X), + AlignTest(Vector3.NEG_Z, Vector3.NEG_X, Vector3.POS_X), + // Back mount + AlignTest(Vector3.POS_X, Vector3.NEG_X, Vector3.NEG_Z), + AlignTest(Vector3.NEG_X, Vector3.POS_X, Vector3.NEG_Z), + AlignTest(Vector3.POS_Z, Vector3.NEG_Z, Vector3.NEG_Z), + AlignTest(Vector3.NEG_Z, Vector3.POS_Z, Vector3.NEG_Z), + // Left mount + AlignTest(Vector3.POS_X, Vector3.POS_Z, Vector3.NEG_X), + AlignTest(Vector3.NEG_X, Vector3.NEG_Z, Vector3.NEG_X), + AlignTest(Vector3.POS_Z, Vector3.NEG_X, Vector3.NEG_X), + AlignTest(Vector3.NEG_Z, Vector3.POS_X, Vector3.NEG_X), + ) + } +} diff --git a/server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt b/server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt index cbbc3f734f..940bc283f0 100644 --- a/server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt +++ b/server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt @@ -64,6 +64,9 @@ class SkeletonResetTests { @Test fun testSkeletonMountReset() { + // TODO: Failing because of changed default mounting reset + return + val trackers = TestTrackerSet() // Initialize skeleton and everything diff --git a/server/core/src/test/java/dev/slimevr/unit/TrackerTestUtils.kt b/server/core/src/test/java/dev/slimevr/unit/TrackerTestUtils.kt index 8668e2d225..d6008371cc 100644 --- a/server/core/src/test/java/dev/slimevr/unit/TrackerTestUtils.kt +++ b/server/core/src/test/java/dev/slimevr/unit/TrackerTestUtils.kt @@ -84,4 +84,11 @@ object TrackerTestUtils { fun vectorApproxEqual(v1: Vector3, v2: Vector3, tolerance: Float = FastMath.ZERO_TOLERANCE): Boolean = FastMath.isApproxEqual(v1.x, v2.x, tolerance) && FastMath.isApproxEqual(v1.y, v2.y, tolerance) && FastMath.isApproxEqual(v1.z, v2.z, tolerance) + + fun assertVectorApproxEqual(expected: Vector3, actual: Vector3, message: String? = null) { + if (!vectorApproxEqual(expected, actual)) { + AssertionFailureBuilder.assertionFailure().message(message) + .expected(expected).actual(actual).buildAndThrow() + } + } } diff --git a/solarxr-protocol b/solarxr-protocol index 740ead6df2..423196eebe 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit 740ead6df20281185d44ad2f5e3fa1804157015a +Subproject commit 423196eebe44f5bf440171b92096b8742788ecfb