From 2bb23256a548a9d565b7100e8ad65a95668b45db Mon Sep 17 00:00:00 2001 From: Erimel Date: Mon, 5 Oct 2026 19:25:07 -0400 Subject: [PATCH] tracker-yaw-correction.kt side refactor --- .../stayaligned/tracker-yaw-correction.kt | 76 ++++++++++--------- 1 file changed, 40 insertions(+), 36 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracker/stayaligned/tracker-yaw-correction.kt b/server/core/src/main/java/dev/slimevr/tracker/stayaligned/tracker-yaw-correction.kt index 670f77db4..8de26f661 100644 --- a/server/core/src/main/java/dev/slimevr/tracker/stayaligned/tracker-yaw-correction.kt +++ b/server/core/src/main/java/dev/slimevr/tracker/stayaligned/tracker-yaw-correction.kt @@ -148,10 +148,8 @@ object TrackerYawCorrection { private fun getNeighbourError(yawCorrectedRotation: Quaternion, trackerState: TrackerState, relaxedPose: RelaxedPose, trackerStates: List): Angle { fun neighbourError(rotation: Quaternion) = trackerYaw(rotation) - trackerYaw(yawCorrectedRotation) val upperBodyTrackers = trackerStates.getAllActiveFor(StayAlignedBodyParts.upperBodyGroup).sortedBy { StayAlignedBodyParts.upperBodyOrder[it.bodyPart] } - val bodyPart = trackerState.bodyPart - val side = bodyPart?.side ?: Side.LEFT - return when (bodyPart) { + return when (trackerState.bodyPart) { StayAlignedBodyParts.head if (upperBodyTrackers.isNotEmpty()) -> neighbourError(upperBodyTrackers.first().rotation) in StayAlignedBodyParts.upperBodyGroup -> { @@ -189,57 +187,63 @@ object TrackerYawCorrection { StayAlignedBodyParts.leftUpperLeg, StayAlignedBodyParts.rightUpperLeg, -> { - val lastUpperBodyTracker = upperBodyTrackers.lastOrNull() - val upperBodyError = if (lastUpperBodyTracker != null) { - neighbourError(lastUpperBodyTracker.rotation) + sideYaw(Side.LEFT, relaxedPose.upperLeg) - } else { - Angle.ZERO - } + trackerState.bodyPart.side?.let { side -> + val lastUpperBodyTracker = upperBodyTrackers.lastOrNull() + val upperBodyError = if (lastUpperBodyTracker != null) { + neighbourError(lastUpperBodyTracker.rotation) + sideYaw(Side.LEFT, relaxedPose.upperLeg) + } else { + Angle.ZERO + } - val lowerLeg = trackerStates.getFirstActiveFor(StayAlignedBodyParts.lowerLeg(side)) - val lowerLegError = if (lowerLeg != null) { - neighbourError(lowerLeg.rotation) - sideYaw(side, relaxedPose.lowerLeg) + sideYaw(side, relaxedPose.upperLeg) - } else { - Angle.ZERO - } + val lowerLeg = trackerStates.getFirstActiveFor(StayAlignedBodyParts.lowerLeg(side)) + val lowerLegError = if (lowerLeg != null) { + neighbourError(lowerLeg.rotation) - sideYaw(side, relaxedPose.lowerLeg) + sideYaw(side, relaxedPose.upperLeg) + } else { + Angle.ZERO + } - upperBodyError + lowerLegError + upperBodyError + lowerLegError + } } StayAlignedBodyParts.leftLowerLeg, StayAlignedBodyParts.rightLowerLeg, -> { - val upperLeg = trackerStates.getFirstActiveFor(StayAlignedBodyParts.upperLeg(side)) - val upperLegError = if (upperLeg != null) { - neighbourError(upperLeg.rotation) - sideYaw(side, relaxedPose.upperLeg) + sideYaw(side, relaxedPose.lowerLeg) - } else { - Angle.ZERO - } + trackerState.bodyPart.side?.let { side -> + val upperLeg = trackerStates.getFirstActiveFor(StayAlignedBodyParts.upperLeg(side)) + val upperLegError = if (upperLeg != null) { + neighbourError(upperLeg.rotation) - sideYaw(side, relaxedPose.upperLeg) + sideYaw(side, relaxedPose.lowerLeg) + } else { + Angle.ZERO + } - val foot = trackerStates.getFirstActiveFor(StayAlignedBodyParts.foot(side)) - val footError = if (foot != null) { - neighbourError(foot.rotation) - sideYaw(side, relaxedPose.foot) + sideYaw(side, relaxedPose.lowerLeg) - } else { - Angle.ZERO - } + val foot = trackerStates.getFirstActiveFor(StayAlignedBodyParts.foot(side)) + val footError = if (foot != null) { + neighbourError(foot.rotation) - sideYaw(side, relaxedPose.foot) + sideYaw(side, relaxedPose.lowerLeg) + } else { + Angle.ZERO + } - upperLegError + footError + upperLegError + footError + } } StayAlignedBodyParts.leftFoot, StayAlignedBodyParts.rightFoot, -> { - val lowerLeg = trackerStates.getFirstActiveFor(StayAlignedBodyParts.lowerLeg(side)) - if (lowerLeg != null) { - neighbourError(lowerLeg.rotation) - sideYaw(side, relaxedPose.lowerLeg) + sideYaw(side, relaxedPose.foot) - } else { - Angle.ZERO + trackerState.bodyPart.side?.let { side -> + val lowerLeg = trackerStates.getFirstActiveFor(StayAlignedBodyParts.lowerLeg(side)) + if (lowerLeg != null) { + neighbourError(lowerLeg.rotation) - sideYaw(side, relaxedPose.lowerLeg) + sideYaw(side, relaxedPose.foot) + } else { + Angle.ZERO + } } } // No error for others - else -> Angle.ZERO - } + else -> null + } ?: Angle.ZERO } /**