Compare commits

...
Author SHA1 Message Date
Sapphire 7a06e71cca Call stop() while we have the lock
Fixes rare ConcurrentModificationException with step mounting
2026-05-24 05:26:28 -04:00
Butterscotch! 608e1c698f Fix accel & make rest detection more lenient 2026-05-24 05:26:28 -04:00
Butterscotch! 948b2a41d7 Separate step mounting from Tracker 2026-05-24 05:26:28 -04:00
Butterscotch! 6cdc1cdfbc Add accelDataTick 2026-05-24 05:26:28 -04:00
Sapphire 36becd5cd8 Basic step mounting ui 2026-05-24 05:26:28 -04:00
Sapphire ca894a5506 Add setting for step mounting 2026-05-24 05:26:28 -04:00
Butterscotch! 2b5761ef8d Acceleration based mounting (step mounting) 2026-05-24 05:22:32 -04:00
26 changed files with 576 additions and 41 deletions

No files matched your search

+2
View File
@@ -642,6 +642,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.
@@ -923,46 +923,64 @@ export function GeneralSettings() {
/>
</div>
<div className="flex flex-col pt-2">
<div className="flex flex-col pt-2 pb-2">
<Typography variant="section-title">
{l10n.getString('settings-general-fk_settings-reset_settings')}
</Typography>
</div>
<div className="flex flex-col pt-2 pb-3">
<div className="grid grid-cols-2 gap-2">
<div className="flex flex-col gap-2">
<Typography>
{l10n.getString(
'settings-general-fk_settings-reset_settings-reset_hmd_pitch-description'
)}
</Typography>
<CheckBox
variant="toggle"
outlined
control={control}
name="resetsSettings.resetHmdPitch"
label={l10n.getString(
'settings-general-fk_settings-reset_settings-reset_hmd_pitch'
)}
/>
</div>
<div className="flex flex-col gap-2 justify-end">
<Typography>
{l10n.getString(
'settings-general-fk_settings-leg_fk-reset_mounting_feet-description-v1'
)}
</Typography>
<CheckBox
variant="toggle"
outlined
control={control}
name="resetsSettings.resetMountingFeet"
label={l10n.getString(
'settings-general-fk_settings-leg_fk-reset_mounting_feet-v1'
)}
/>
</div>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<Typography>
{l10n.getString(
'settings-general-fk_settings-reset_settings-step_mounting-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-1 gap-2 pb-3">
<CheckBox
variant="toggle"
outlined
control={control}
name="resetsSettings.stepMounting"
label={l10n.getString(
'settings-general-fk_settings-reset_settings-step_mounting'
)}
/>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<Typography>
{l10n.getString(
'settings-general-fk_settings-reset_settings-reset_hmd_pitch-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<CheckBox
variant="toggle"
outlined
control={control}
name="resetsSettings.resetHmdPitch"
label={l10n.getString(
'settings-general-fk_settings-reset_settings-reset_hmd_pitch'
)}
/>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<Typography>
{l10n.getString(
'settings-general-fk_settings-leg_fk-reset_mounting_feet-description-v1'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<CheckBox
variant="toggle"
outlined
control={control}
name="resetsSettings.resetMountingFeet"
label={l10n.getString(
'settings-general-fk_settings-leg_fk-reset_mounting_feet-v1'
)}
/>
</div>
<div>
+3 -1
View File
@@ -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,
}
)}
>
+3 -1
View File
@@ -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,
}
)}
>
+1
View File
@@ -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;
+3
View File
@@ -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;
}
+1
View File
@@ -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;
+1
View File
@@ -191,6 +191,7 @@ const config = {
warning: 'rgb(var(--warning), <alpha-value>)',
critical: 'rgb(var(--critical), <alpha-value>)',
special: 'rgb(var(--special), <alpha-value>)',
recording: 'rgb(var(--recording), <alpha-value>)',
},
window: {
icon: 'rgb(var(--window-icon-stroke), <alpha-value>)',
@@ -48,6 +48,7 @@ enum class MountingMethods(val id: Int) {
}
class ResetsConfig {
var stepMounting = false
// Always reset mounting for feet
var resetMountingFeet = false
@@ -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)
}
@@ -383,6 +383,7 @@ fun createArmsResetModeSettings(
resetsConfig.yawResetSmoothTime,
resetsConfig.saveMountingReset,
resetsConfig.resetHmdPitch,
resetsConfig.stepMounting,
)
fun createSettingsResponse(fbb: FlatBufferBuilder, server: VRServer): Int {
@@ -347,6 +347,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()
}
@@ -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<RecordingWrapper> = 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<Tracker>) = 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
}
}
@@ -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)
}
}
@@ -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,
)
@@ -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<RecordingSample>(8)
// List capacity assuming ~10 seconds at 100 TPS
val recording: MutableList<RecordingSample> = 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
}
}
@@ -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()
@@ -1622,6 +1625,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 {
@@ -200,6 +200,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)) {
@@ -315,6 +320,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
*/
@@ -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
@@ -435,6 +435,9 @@ class HIDCommon {
} else {
tracker.heartbeat() // else, something received
}
if (packetType == 1 || packetType == 2 || packetType == 7) {
tracker.accelDataTick()
}
}
}
}
@@ -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 -> {
@@ -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
}
}
@@ -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<DynamicTest> = 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),
)
}
}
@@ -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
@@ -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()
}
}
}