mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 17:41:28 -08:00
Initial step mounting
This commit is contained in:
12 files changed
+561
No files matched your search
@@ -12,6 +12,7 @@ import dev.slimevr.routing.BoneRoutingManager
|
||||
import dev.slimevr.serial.SerialServer
|
||||
import dev.slimevr.skeleton.Skeleton
|
||||
import dev.slimevr.solarxr.rpc.ServerInfos
|
||||
import dev.slimevr.stepmounting.StepMountingManager
|
||||
import dev.slimevr.tapdetection.TapDetectionManager
|
||||
import dev.slimevr.trackingchecklist.TrackingChecklist
|
||||
import dev.slimevr.udp.UdpServer
|
||||
@@ -49,6 +50,7 @@ interface AppContextProvider : Phase1ContextProvider {
|
||||
val vrcOscManager: VRCOSCManager
|
||||
val resetsManager: ResetsManager
|
||||
val tapDetectionManager: TapDetectionManager
|
||||
val stepMountingManager: StepMountingManager
|
||||
fun startObserving()
|
||||
suspend fun dispose()
|
||||
}
|
||||
@@ -74,6 +76,7 @@ class AppContext(
|
||||
override val vrcOscManager: VRCOSCManager,
|
||||
override val resetsManager: ResetsManager,
|
||||
override val tapDetectionManager: TapDetectionManager,
|
||||
override val stepMountingManager: StepMountingManager,
|
||||
) : AppContextProvider {
|
||||
override fun startObserving() {
|
||||
keybindManager.startObserving(this)
|
||||
@@ -90,6 +93,7 @@ class AppContext(
|
||||
vrcOscManager.startObserving(this)
|
||||
resetsManager.startObserving()
|
||||
tapDetectionManager.startObserving()
|
||||
stepMountingManager.startObserving()
|
||||
}
|
||||
|
||||
override suspend fun dispose() {
|
||||
|
||||
@@ -32,6 +32,7 @@ object AppLogger {
|
||||
val logging = logger("Logging")
|
||||
val events = logger("Events")
|
||||
val stayAligned = logger("Stay Aligned")
|
||||
val stepMounting = logger("Step Mounting")
|
||||
|
||||
val coroutines = noCoLogger("Coroutines")
|
||||
val console = noCoLogger("Console")
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
package dev.slimevr.stepmounting
|
||||
|
||||
import io.github.axisangles.ktmath.Vector3
|
||||
|
||||
class AccelerationIntegrator {
|
||||
var acceleration = Vector3.ZERO
|
||||
private set
|
||||
var velocity = Vector3.ZERO
|
||||
private set
|
||||
var offset = Vector3.ZERO
|
||||
private set
|
||||
|
||||
fun integrate(acceleration: Vector3, time: Float) {
|
||||
this.acceleration = acceleration
|
||||
offset += (velocity * time) + ((acceleration * time * time) / 2f)
|
||||
velocity += acceleration * time
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
package dev.slimevr.stepmounting
|
||||
|
||||
import dev.slimevr.logging.AppLogger
|
||||
import dev.slimevr.tracker.Tracker
|
||||
import dev.slimevr.tracker.TrackerActions
|
||||
import dev.slimevr.tracker.TrackerState
|
||||
import dev.slimevr.util.ButterworthCoefficients
|
||||
import dev.slimevr.util.Vector3Butterworth
|
||||
import kotlinx.coroutines.ExperimentalCoroutinesApi
|
||||
import kotlinx.coroutines.delay
|
||||
import kotlinx.coroutines.flow.Flow
|
||||
import kotlinx.coroutines.flow.combine
|
||||
import kotlinx.coroutines.flow.distinctUntilChanged
|
||||
import kotlinx.coroutines.flow.first
|
||||
import kotlinx.coroutines.flow.flatMapLatest
|
||||
import kotlinx.coroutines.flow.flow
|
||||
import kotlinx.coroutines.flow.flowOf
|
||||
import kotlinx.coroutines.flow.launchIn
|
||||
import kotlinx.coroutines.flow.onEach
|
||||
import kotlinx.coroutines.flow.stateIn
|
||||
import kotlinx.coroutines.flow.takeWhile
|
||||
import kotlinx.coroutines.withTimeoutOrNull
|
||||
import solarxr_protocol.datatypes.BodyPart
|
||||
import kotlin.time.TimeSource
|
||||
|
||||
internal const val TIMEOUT_MS = 30_000L
|
||||
|
||||
class StepMountingBasicBehaviour : StepMountingBehaviour {
|
||||
fun canCalibrate(trackers: List<TrackerState>): Boolean {
|
||||
val hasPositionalHead = trackers.any { it.bodyPart == BodyPart.HEAD && it.position != null }
|
||||
return hasPositionalHead
|
||||
}
|
||||
|
||||
@OptIn(ExperimentalCoroutinesApi::class)
|
||||
override fun observe(receiver: StepMountingManager) {
|
||||
receiver.server.context.state
|
||||
.flatMapLatest { state ->
|
||||
val trackers = state.trackers.values.toList()
|
||||
if (trackers.isEmpty()) return@flatMapLatest flowOf(false)
|
||||
// React to per-tracker position/bodyPart changes, not just tracker add/remove. canCalibrate
|
||||
// only looks at bodyPart and whether position is set, so dedup on that per tracker rather
|
||||
// than letting every rotation packet resume the combine.
|
||||
combine(
|
||||
trackers.map { tracker ->
|
||||
tracker.context.state.distinctUntilChanged { a, b ->
|
||||
a.bodyPart == b.bodyPart && (a.position == null) == (b.position == null)
|
||||
}
|
||||
},
|
||||
) { states -> canCalibrate(states.toList()) }
|
||||
}
|
||||
.distinctUntilChanged()
|
||||
.onEach { receiver.context.dispatch(StepMountingActions.SetCanCalibrate(it)) }
|
||||
.launchIn(receiver.context.scope)
|
||||
}
|
||||
}
|
||||
|
||||
val coefficients = ButterworthCoefficients(
|
||||
8f,
|
||||
0.02f,
|
||||
)
|
||||
const val startThreshold = 0.4f // in m/s^2
|
||||
const val endThreshold = 0.2f // in m/s^2
|
||||
const val minMovementDurationMs = 3000L
|
||||
|
||||
fun movementDetector(updates: Flow<TrackerSnapshot>) = flow {
|
||||
val lowpass = Vector3Butterworth(coefficients)
|
||||
emit(0f)
|
||||
updates.collect { update ->
|
||||
val lowpassAccel = lowpass.filter(update.acceleration)
|
||||
emit((update.acceleration - lowpassAccel).len())
|
||||
}
|
||||
}
|
||||
|
||||
internal suspend fun runCalibrationSession(
|
||||
context: StepMountingContext,
|
||||
headUpdates: Flow<HeadsetSnapshot>,
|
||||
trackers: List<Pair<Tracker, Flow<TrackerSnapshot>>>,
|
||||
clock: TimeSource.WithComparableMarks = TimeSource.Monotonic,
|
||||
) {
|
||||
val headRecording = ArrayList<HeadRecordingSample>(1024)
|
||||
val trackerRecordings = ArrayList<Pair<Tracker, ArrayList<RecordingSample>>>(trackers.size)
|
||||
|
||||
fun dispatch(status: StepMountingStatus) {
|
||||
context.dispatch(StepMountingActions.Update(status))
|
||||
}
|
||||
|
||||
dispatch(StepMountingStatus.WAITING_FOR_MOVEMENT)
|
||||
AppLogger.stepMounting.info("Waiting for movement...")
|
||||
|
||||
withTimeoutOrNull(TIMEOUT_MS) {
|
||||
val isMoving = combine(
|
||||
trackers.map { (_, updates) ->
|
||||
movementDetector(updates)
|
||||
},
|
||||
) { updates ->
|
||||
// Max tracker movement
|
||||
updates.max()
|
||||
}.stateIn(this)
|
||||
|
||||
// Wait for movement to start
|
||||
isMoving.first { it >= startThreshold }
|
||||
|
||||
dispatch(StepMountingStatus.RECORDING)
|
||||
AppLogger.stepMounting.info("Movement detected, recording...")
|
||||
|
||||
// Collect head and tracker data
|
||||
headUpdates.takeWhile {
|
||||
context.state.value.status == StepMountingStatus.RECORDING
|
||||
}.onEach { update ->
|
||||
headRecording.add(
|
||||
HeadRecordingSample(
|
||||
time = clock.markNow(),
|
||||
position = update.position,
|
||||
),
|
||||
)
|
||||
}.launchIn(this)
|
||||
for ((tracker, updates) in trackers) {
|
||||
val recording = ArrayList<RecordingSample>(1024)
|
||||
trackerRecordings.add(tracker to recording)
|
||||
|
||||
updates.takeWhile {
|
||||
context.state.value.status == StepMountingStatus.RECORDING
|
||||
}.onEach { update ->
|
||||
recording.add(
|
||||
RecordingSample(
|
||||
time = clock.markNow(),
|
||||
accel = update.acceleration,
|
||||
),
|
||||
)
|
||||
}.launchIn(this)
|
||||
}
|
||||
|
||||
// Wait for movement to end
|
||||
delay(minMovementDurationMs)
|
||||
isMoving.first { it < endThreshold }
|
||||
|
||||
dispatch(StepMountingStatus.PROCESSING)
|
||||
AppLogger.stepMounting.info("No more movement detected, processing...")
|
||||
|
||||
if (headRecording.isEmpty() || trackerRecordings.isEmpty()) {
|
||||
dispatch(StepMountingStatus.ERROR_NO_DATA)
|
||||
error("No data found.")
|
||||
}
|
||||
|
||||
val headOffset = headRecording.last().position - headRecording.first().position
|
||||
trackerRecordings.filter {
|
||||
it.second.isNotEmpty()
|
||||
}.map { (tracker, recording) ->
|
||||
tracker to estimateHeadingAlign(recording, headOffset)
|
||||
}.forEach { (tracker, result) ->
|
||||
// TODO: Fail on high error
|
||||
AppLogger.stepMounting.info(result)
|
||||
|
||||
tracker.context.dispatch(
|
||||
TrackerActions.SetStepMounting(
|
||||
result.headingAlignment,
|
||||
),
|
||||
)
|
||||
}
|
||||
|
||||
dispatch(StepMountingStatus.DONE)
|
||||
AppLogger.stepMounting.info("Done!")
|
||||
} ?: dispatch(StepMountingStatus.ERROR_TIMEOUT)
|
||||
}
|
||||
@@ -0,0 +1,146 @@
|
||||
@file:OptIn(kotlinx.coroutines.ExperimentalCoroutinesApi::class)
|
||||
|
||||
package dev.slimevr.stepmounting
|
||||
|
||||
import dev.slimevr.Phase1ContextProvider
|
||||
import dev.slimevr.VRServer
|
||||
import dev.slimevr.config.Settings
|
||||
import dev.slimevr.context.Behaviour
|
||||
import dev.slimevr.context.Context
|
||||
import dev.slimevr.tracker.Tracker
|
||||
import io.github.axisangles.ktmath.Quaternion
|
||||
import io.github.axisangles.ktmath.Vector3
|
||||
import kotlinx.coroutines.CoroutineScope
|
||||
import kotlinx.coroutines.Job
|
||||
import kotlinx.coroutines.flow.Flow
|
||||
import kotlinx.coroutines.flow.emptyFlow
|
||||
import kotlinx.coroutines.flow.flatMapConcat
|
||||
import kotlinx.coroutines.flow.map
|
||||
import kotlinx.coroutines.launch
|
||||
import solarxr_protocol.datatypes.BodyPart
|
||||
import solarxr_protocol.datatypes.TrackerStatus
|
||||
import kotlin.collections.emptyList
|
||||
import kotlin.collections.listOf
|
||||
import kotlin.collections.map
|
||||
import kotlin.time.ComparableTimeMark
|
||||
|
||||
// TODO Put this enum in SolarXR
|
||||
enum class StepMountingStatus(
|
||||
val `value`: UByte,
|
||||
) {
|
||||
NONE(0.toUByte()),
|
||||
WAITING_FOR_MOVEMENT(1.toUByte()),
|
||||
RECORDING(2.toUByte()),
|
||||
PROCESSING(3.toUByte()),
|
||||
DONE(4.toUByte()),
|
||||
ERROR_NO_DATA(5.toUByte()),
|
||||
ERROR_TIMEOUT(6.toUByte()),
|
||||
;
|
||||
|
||||
companion object {
|
||||
fun fromValue(`value`: UByte): StepMountingStatus? = entries.firstOrNull { it.value == value }
|
||||
}
|
||||
}
|
||||
|
||||
data class HeadsetSnapshot(
|
||||
val rotation: Quaternion,
|
||||
val position: Vector3,
|
||||
)
|
||||
|
||||
data class TrackerSnapshot(
|
||||
val rotation: Quaternion,
|
||||
val acceleration: Vector3,
|
||||
)
|
||||
|
||||
data class HeadRecordingSample(
|
||||
val time: ComparableTimeMark,
|
||||
val position: Vector3,
|
||||
)
|
||||
|
||||
data class RecordingSample(
|
||||
val time: ComparableTimeMark,
|
||||
val accel: Vector3,
|
||||
)
|
||||
|
||||
data class StepMountingState(
|
||||
val status: StepMountingStatus,
|
||||
val canDoStepMounting: Boolean,
|
||||
)
|
||||
|
||||
sealed interface StepMountingActions {
|
||||
data class Update(val status: StepMountingStatus) : StepMountingActions
|
||||
data class SetCanCalibrate(val canDo: Boolean) : StepMountingActions
|
||||
}
|
||||
|
||||
typealias StepMountingContext = Context<StepMountingState, StepMountingActions>
|
||||
typealias StepMountingBehaviour = Behaviour<StepMountingManager>
|
||||
|
||||
class StepMountingManager(
|
||||
val context: StepMountingContext,
|
||||
val server: VRServer,
|
||||
val settings: Settings,
|
||||
) {
|
||||
fun startObserving() = context.observeAll(this)
|
||||
|
||||
private var sessionJob: Job? = null
|
||||
|
||||
// These Flows do nothing until the calibration uses collect on it
|
||||
val headUpdates: Flow<HeadsetSnapshot> = server.context.state
|
||||
.flatMapConcat { state ->
|
||||
val positionalHead = state.trackers.values
|
||||
.find {
|
||||
val state = it.context.state.value
|
||||
state.bodyPart == BodyPart.HEAD && state.status == TrackerStatus.OK && state.position != null
|
||||
}
|
||||
?: return@flatMapConcat emptyFlow()
|
||||
positionalHead.context.state.map { s ->
|
||||
HeadsetSnapshot(
|
||||
rotation = s.rotation,
|
||||
position = s.position ?: error("Head will always have a position here"),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
fun getTrackers(): List<Pair<Tracker, Flow<TrackerSnapshot>>> {
|
||||
val activeTrackers = server.context.state.value.trackers.values.filter {
|
||||
val state = it.context.state.value
|
||||
state.status == TrackerStatus.OK && state.acceleration != null
|
||||
}
|
||||
if (activeTrackers.isEmpty()) return emptyList()
|
||||
return activeTrackers.map { controller ->
|
||||
controller to controller.context.state.map { s ->
|
||||
TrackerSnapshot(
|
||||
rotation = s.rotation,
|
||||
acceleration = s.acceleration ?: error("Trackers will always have acceleration in this case"),
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fun start() {
|
||||
sessionJob?.cancel()
|
||||
sessionJob = context.scope.launch { runCalibrationSession(context, headUpdates, getTrackers()) }
|
||||
}
|
||||
|
||||
fun cancel() {
|
||||
sessionJob?.cancel()
|
||||
sessionJob = null
|
||||
context.dispatch(StepMountingActions.Update(StepMountingStatus.NONE))
|
||||
}
|
||||
|
||||
companion object {
|
||||
fun create(ctx: Phase1ContextProvider, scope: CoroutineScope): StepMountingManager {
|
||||
val context = Context.create(
|
||||
initialState = StepMountingState(
|
||||
status = StepMountingStatus.NONE,
|
||||
canDoStepMounting = false,
|
||||
),
|
||||
scope = scope,
|
||||
reducer = ::reduce,
|
||||
behaviours = listOf(StepMountingBasicBehaviour()),
|
||||
name = "StepMountingManager",
|
||||
)
|
||||
return StepMountingManager(context, ctx.server, ctx.config.settings)
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
package dev.slimevr.stepmounting
|
||||
|
||||
import dev.slimevr.tracker.HeadingAlignment
|
||||
import dev.slimevr.util.inFloatingSeconds
|
||||
import io.github.axisangles.ktmath.Quaternion
|
||||
import io.github.axisangles.ktmath.Vector3
|
||||
import kotlin.math.atan2
|
||||
import kotlin.time.ComparableTimeMark
|
||||
import kotlin.time.Duration
|
||||
|
||||
fun accumSample(
|
||||
accum: AccelerationIntegrator,
|
||||
sample: RecordingSample,
|
||||
lastSampleTime: ComparableTimeMark? = null,
|
||||
accelBias: Vector3 = Vector3.ZERO,
|
||||
): Duration {
|
||||
val delta = lastSampleTime?.let { sample.time - it } ?: Duration.ZERO
|
||||
accum.integrate(sample.accel - accelBias, delta.inFloatingSeconds)
|
||||
|
||||
return delta
|
||||
}
|
||||
|
||||
fun processTimeline(
|
||||
accum: AccelerationIntegrator,
|
||||
recording: Iterable<RecordingSample>,
|
||||
lastSampleTime: ComparableTimeMark? = null,
|
||||
accelBias: Vector3 = Vector3.ZERO,
|
||||
): ComparableTimeMark? {
|
||||
var lastTime = lastSampleTime
|
||||
|
||||
for (sample in 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)
|
||||
}
|
||||
|
||||
data class StepMountingResult(
|
||||
val headingAlignment: HeadingAlignment,
|
||||
val errorMeters: Float,
|
||||
val trackerOffset: Vector3,
|
||||
val accelerationBias: Vector3,
|
||||
)
|
||||
|
||||
fun estimateHeadingAlign(
|
||||
recording: Iterable<RecordingSample>,
|
||||
headOffset: Vector3,
|
||||
): StepMountingResult {
|
||||
// Compute the unbiased final velocity
|
||||
val calibAccum = AccelerationIntegrator()
|
||||
processTimeline(calibAccum, recording)
|
||||
|
||||
// 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 = recording.last().time - recording.first().time
|
||||
val bias = calibAccum.velocity / duration.inFloatingSeconds
|
||||
|
||||
// Compute the biased final offset
|
||||
val finalAccum = AccelerationIntegrator()
|
||||
processTimeline(finalAccum, recording, accelBias = bias)
|
||||
|
||||
// Compute the final offsets
|
||||
val trackerOffset = finalAccum.offset
|
||||
val trackerXZ = Vector3(trackerOffset.x, 0f, trackerOffset.z)
|
||||
val hmdXZ = Vector3(headOffset.x, 0f, headOffset.z)
|
||||
|
||||
// Compute mounting to fix the yaw offset from tracker to HMD
|
||||
// OLD: angle(trackerXZ.unit()) * angle(hmdXZ.unit()).inv()
|
||||
return StepMountingResult(
|
||||
Quaternion.fromTo(trackerXZ.unit(), hmdXZ.unit()),
|
||||
trackerXZ.len() - hmdXZ.len(),
|
||||
trackerOffset,
|
||||
bias,
|
||||
)
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
package dev.slimevr.stepmounting
|
||||
|
||||
fun reduce(state: StepMountingState, action: StepMountingActions): StepMountingState = when (action) {
|
||||
is StepMountingActions.Update -> state.copy(
|
||||
status = action.status,
|
||||
)
|
||||
|
||||
is StepMountingActions.SetCanCalibrate -> state.copy(
|
||||
canDoStepMounting = action.canDo,
|
||||
)
|
||||
}
|
||||
@@ -120,6 +120,7 @@ sealed interface TrackerActions {
|
||||
data class SetMotion(val motion: Motion) : TrackerActions
|
||||
data class SetYawCorrection(val yawCorrection: Angle) : TrackerActions
|
||||
data class SetStayAlignedEnabled(val enabled: Boolean) : TrackerActions
|
||||
data class SetStepMounting(val headingAlignment: HeadingAlignment) : TrackerActions
|
||||
}
|
||||
|
||||
typealias TrackerContext = Context<TrackerState, TrackerActions>
|
||||
|
||||
@@ -244,4 +244,14 @@ fun reduce(
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
is TrackerActions.SetStepMounting -> {
|
||||
val newHeadingAlignment = state.sessionCalibration.headingAlignment * action.headingAlignment
|
||||
state.copy(
|
||||
sessionCalibration = state.sessionCalibration.copy(headingAlignment = newHeadingAlignment),
|
||||
lastMountingMethod = MountingMethod.POSE, // TODO More methods?
|
||||
rotationDirty = true,
|
||||
pendingSkeletonResets = state.pendingSkeletonResets + ResetType.POSE_MOUNTING,
|
||||
)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,120 @@
|
||||
package dev.slimevr.util
|
||||
|
||||
import com.jme3.math.FastMath
|
||||
import io.github.axisangles.ktmath.Vector3
|
||||
import kotlin.math.sqrt
|
||||
import kotlin.math.tan
|
||||
|
||||
data class ButterworthCoefficients(
|
||||
/** Corresponds to cutoff frequency (Hz) as `1f / tau`. **/
|
||||
val tau: Float,
|
||||
/** Sampling period in seconds. **/
|
||||
val Ts: Float,
|
||||
) {
|
||||
val b0: Float
|
||||
val b1: Float
|
||||
val b2: Float
|
||||
|
||||
val a1: Float
|
||||
val a2: Float
|
||||
|
||||
init {
|
||||
require(tau > 0f) { "tau must be > 0" }
|
||||
require(Ts > 0f) { "Ts must be > 0" }
|
||||
|
||||
val fc = (sqrt(2f) / FastMath.TWO_PI) / tau
|
||||
val C = tan(FastMath.PI * fc * Ts)
|
||||
val D = (C * C) + (sqrt(2f) * C) + 1f
|
||||
val b0 = (C * C) / D
|
||||
|
||||
this.b0 = b0
|
||||
this.b1 = 2f * b0
|
||||
this.b2 = b0
|
||||
|
||||
// a0 = 1f
|
||||
this.a1 = (2f * ((C * C) - 1f)) / D
|
||||
this.a2 = ((1f - (sqrt(2f) * C)) + (C * C)) / D
|
||||
}
|
||||
}
|
||||
|
||||
class Butterworth(
|
||||
coefficients: ButterworthCoefficients,
|
||||
) {
|
||||
var coefficients = coefficients
|
||||
private set
|
||||
|
||||
private var initialized = false
|
||||
|
||||
private var y: Float = 0f
|
||||
|
||||
private var state0: Float = 0f
|
||||
private var state1: Float = 0f
|
||||
|
||||
fun swapCoefficients(coefficients: ButterworthCoefficients) {
|
||||
// Don't do anything if we're not initialized
|
||||
if (!initialized) {
|
||||
this.coefficients = coefficients
|
||||
return
|
||||
}
|
||||
|
||||
val old = this.coefficients
|
||||
val new = coefficients
|
||||
state0 += (old.b0 - new.b0) * y
|
||||
state1 += (old.b1 - new.b1 - old.a1 + new.a1) * y
|
||||
|
||||
this.coefficients = coefficients
|
||||
}
|
||||
|
||||
fun filter(
|
||||
x: Float,
|
||||
): Float {
|
||||
// Average through duration tau to initialize
|
||||
if (!initialized) {
|
||||
state0 += 1f // Sample count
|
||||
state1 += x // Sum
|
||||
|
||||
val out = state1 / state0
|
||||
|
||||
// If reached tau, initialize
|
||||
if (state0 * coefficients.Ts >= coefficients.tau) {
|
||||
state0 = out * (1f - coefficients.b0)
|
||||
state1 = out * (coefficients.b2 - coefficients.a2)
|
||||
initialized = true
|
||||
}
|
||||
|
||||
return out
|
||||
}
|
||||
|
||||
y = coefficients.b0 * x + state0
|
||||
state0 = coefficients.b1 * x - coefficients.a1 * y + state1
|
||||
state1 = coefficients.b2 * x - coefficients.a2 * y
|
||||
return y
|
||||
}
|
||||
}
|
||||
|
||||
class Vector3Butterworth(
|
||||
coefficients: ButterworthCoefficients,
|
||||
) {
|
||||
private val x = Butterworth(coefficients)
|
||||
private val y = Butterworth(coefficients)
|
||||
private val z = Butterworth(coefficients)
|
||||
|
||||
val coefficients
|
||||
get() = x.coefficients
|
||||
|
||||
fun swapCoefficients(
|
||||
coefficients: ButterworthCoefficients,
|
||||
) {
|
||||
x.swapCoefficients(coefficients)
|
||||
y.swapCoefficients(coefficients)
|
||||
z.swapCoefficients(coefficients)
|
||||
}
|
||||
|
||||
fun filter(
|
||||
x: Vector3,
|
||||
): Vector3 = Vector3(
|
||||
this.x.filter(x.x),
|
||||
this.y.filter(x.y),
|
||||
this.z.filter(x.z),
|
||||
)
|
||||
}
|
||||
@@ -30,6 +30,7 @@ import dev.slimevr.skeleton.ProportionsBehaviour
|
||||
import dev.slimevr.skeleton.Skeleton
|
||||
import dev.slimevr.skeleton.buildBones
|
||||
import dev.slimevr.solarxr.rpc.ServerInfos
|
||||
import dev.slimevr.stepmounting.StepMountingManager
|
||||
import dev.slimevr.tapdetection.TapDetectionManager
|
||||
import dev.slimevr.tracker.Motion
|
||||
import dev.slimevr.tracker.SessionCalibration
|
||||
@@ -256,6 +257,7 @@ abstract class TestAppContext : AppContextProvider {
|
||||
override val resetsManager: ResetsManager get() = error("not used in test")
|
||||
override val tapDetectionManager: TapDetectionManager get() = error("not used in test")
|
||||
override val boneRouting: BoneRoutingManager get() = error("not used in test")
|
||||
override val stepMountingManager: StepMountingManager get() = error("not used in test")
|
||||
override fun startObserving() {}
|
||||
override suspend fun dispose() = Unit
|
||||
}
|
||||
@@ -40,6 +40,7 @@ import dev.slimevr.resolveConfigDirectory
|
||||
import dev.slimevr.routing.BoneRoutingManager
|
||||
import dev.slimevr.skeleton.Skeleton
|
||||
import dev.slimevr.solarxr.rpc.ServerInfos
|
||||
import dev.slimevr.stepmounting.StepMountingManager
|
||||
import dev.slimevr.tapdetection.TapDetectionManager
|
||||
import dev.slimevr.trackingchecklist.TrackingChecklist
|
||||
import dev.slimevr.udp.UdpServer
|
||||
@@ -155,6 +156,7 @@ fun main(args: Array<String>) = runBlocking<Unit>(appCoroutineExceptionHandler +
|
||||
)
|
||||
val resetsManager = ResetsManager.create(ctx = phase1, skeleton = skeleton, scope = this)
|
||||
val tapDetectionManager = TapDetectionManager.create(ctx = phase1, resetsManager = resetsManager, scope = this)
|
||||
val stepMountingManager = StepMountingManager.create(ctx = phase1, scope = this)
|
||||
val keybindManager = KeybindManager.create(scope = this)
|
||||
val serverInfos = ServerInfos(::resolveDesktopLocalIpAddress)
|
||||
|
||||
@@ -179,6 +181,7 @@ fun main(args: Array<String>) = runBlocking<Unit>(appCoroutineExceptionHandler +
|
||||
vrcOscManager = vrcOscManager,
|
||||
resetsManager = resetsManager,
|
||||
tapDetectionManager = tapDetectionManager,
|
||||
stepMountingManager = stepMountingManager,
|
||||
)
|
||||
|
||||
try {
|
||||
|
||||
Reference in new issue
Block a user