Skeleton Constraints (#1222)

Co-authored-by: Butterscotch! <bscotchvanilla@gmail.com>
2026-04-06 02:01:58 +02:00 · 2025-01-26 07:36:10 -08:00
parent 4ad9d5cfca
commit bfb30c472b
15 changed files with 484 additions and 71 deletions
--- a/gui/public/i18n/en/translation.ftl
+++ b/gui/public/i18n/en/translation.ftl
@@ -450,6 +450,11 @@ settings-general-fk_settings-leg_tweak-foot_plant-description = Foot-plant rotat
 settings-general-fk_settings-leg_fk = Leg tracking
 settings-general-fk_settings-leg_fk-reset_mounting_feet-description = Enable feet Mounting Reset by tiptoeing.
 settings-general-fk_settings-leg_fk-reset_mounting_feet = Feet Mounting Reset
+settings-general-fk_settings-enforce_joint_constraints = Skeletal Limits
+settings-general-fk_settings-enforce_joint_constraints-enforce_constraints = Enforce constraints
+settings-general-fk_settings-enforce_joint_constraints-enforce_constraints-description = Prevents joints from rotating past their limit
+settings-general-fk_settings-enforce_joint_constraints-correct_constraints = Correct with constraints
+settings-general-fk_settings-enforce_joint_constraints-correct_constraints-description = Correct joint rotations when they push past their limit
 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
--- a/gui/src/components/settings/pages/GeneralSettings.tsx
+++ b/gui/src/components/settings/pages/GeneralSettings.tsx
@@ -69,6 +69,9 @@ interface SettingsForm {
    toeSnap: boolean;
    footPlant: boolean;
    selfLocalization: boolean;
+    usePosition: boolean;
+    enforceConstraints: boolean;
+    correctConstraints: boolean;
  };
  ratios: {
    imputeWaistFromChestHip: number;
@@ -128,6 +131,9 @@ const defaultValues: SettingsForm = {
    toeSnap: false,
    footPlant: true,
    selfLocalization: false,
+    usePosition: true,
+    enforceConstraints: true,
+    correctConstraints: true,
  },
  ratios: {
    imputeWaistFromChestHip: 0.3,
@@ -235,6 +241,9 @@ export function GeneralSettings() {
      toggles.toeSnap = values.toggles.toeSnap;
      toggles.footPlant = values.toggles.footPlant;
      toggles.selfLocalization = values.toggles.selfLocalization;
+      toggles.usePosition = values.toggles.usePosition;
+      toggles.enforceConstraints = values.toggles.enforceConstraints;
+      toggles.correctConstraints = values.toggles.correctConstraints;
      modelSettings.toggles = toggles;
    }

@@ -981,6 +990,7 @@ export function GeneralSettings() {
                )}
              />
            </div>
+
            <Typography color="secondary">
              {l10n.getString(
                'settings-general-fk_settings-arm_fk-reset_mode-description'
@@ -1033,6 +1043,48 @@ export function GeneralSettings() {
              ></Radio>
            </div>

+            <div className="flex flex-col pt-2 pb-3">
+              <Typography bold>
+                {l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints'
+                )}
+              </Typography>
+              <Typography color="secondary">
+                {l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints-enforce_constraints-description'
+                )}
+              </Typography>
+            </div>
+            <div className="grid sm:grid-cols-1 pb-3">
+              <CheckBox
+                variant="toggle"
+                outlined
+                control={control}
+                name="toggles.enforceConstraints"
+                label={l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints-enforce_constraints'
+                )}
+              />
+            </div>
+            <div className="flex flex-col pt-2 pb-3">
+              <Typography color="secondary">
+                {l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints-correct_constraints-description'
+                )}
+              </Typography>
+            </div>
+            <div className="grid sm:grid-cols-1 pb-3">
+              <CheckBox
+                variant="toggle"
+                outlined
+                control={control}
+                name="toggles.correctConstraints"
+                label={l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints-correct_constraints'
+                )}
+              />
+            </div>
+
            {config?.debug && (
              <>
                <div className="flex flex-col pt-2 pb-3">
--- a/server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt
+++ b/server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt
@@ -36,7 +36,7 @@ class AutoBoneStep(
 		// Load server configs into the skeleton
 		skeleton1.loadFromConfig(serverConfig)
 		skeleton2.loadFromConfig(serverConfig)
-		// Disable leg tweaks, this will mess with the resulting positions
+		// Disable leg tweaks and IK solver, these will mess with the resulting positions
 		skeleton1.setLegTweaksEnabled(false)
 		skeleton2.setLegTweaksEnabled(false)
 	}
--- a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt
+++ b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt
@@ -21,6 +21,8 @@ class QuaternionMovingAverage(
 	var amount: Float = 0f,
 	initialRotation: Quaternion = IDENTITY,
 ) {
+	var filteredQuaternion = IDENTITY
+	var filteringImpact = 0f
 	private var smoothFactor = 0f
 	private var predictFactor = 0f
 	private var rotBuffer: CircularArrayList<Quaternion>? = null
@@ -29,7 +31,6 @@ class QuaternionMovingAverage(
 	private val fpsTimer = if (VRServer.instanceInitialized) VRServer.instance.fpsTimer else NanoTimer()
 	private var frameCounter = 0
 	private var lastAmt = 0f
-	var filteredQuaternion = IDENTITY

 	init {
 		// amount should range from 0 to 1.
@@ -93,6 +94,8 @@ class QuaternionMovingAverage(
 			// No filtering; just keep track of rotations (for going over 180 degrees)
 			filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion)
 		}
+
+		filteringImpact = latestQuaternion.angleToR(filteredQuaternion)
 	}

 	@Synchronized
--- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java
+++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java
@@ -190,8 +190,8 @@ public class RPCSettingsBuilder {
 				humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT),
 				humanPoseManager.getToggle(SkeletonConfigToggles.SELF_LOCALIZATION),
 				false,
-				true,
-				true
+				humanPoseManager.getToggle(SkeletonConfigToggles.ENFORCE_CONSTRAINTS),
+				humanPoseManager.getToggle(SkeletonConfigToggles.CORRECT_CONSTRAINTS)
 			);
 		int ratiosOffset = ModelRatios
 			.createModelRatios(
--- 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
@@ -258,6 +258,8 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 				hpm.setToggle(SkeletonConfigToggles.TOE_SNAP, toggles.toeSnap())
 				hpm.setToggle(SkeletonConfigToggles.FOOT_PLANT, toggles.footPlant())
 				hpm.setToggle(SkeletonConfigToggles.SELF_LOCALIZATION, toggles.selfLocalization())
+				hpm.setToggle(SkeletonConfigToggles.ENFORCE_CONSTRAINTS, toggles.enforceConstraints())
+				hpm.setToggle(SkeletonConfigToggles.CORRECT_CONSTRAINTS, toggles.correctConstraints())
 			}

 			if (ratios != null) {
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt
@@ -1,19 +1,23 @@
 package dev.slimevr.tracking.processor

+import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType
+import dev.slimevr.tracking.trackers.Tracker
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Vector3
+import solarxr_protocol.datatypes.BodyPart
 import java.util.concurrent.CopyOnWriteArrayList

 /**
 * Represents a bone composed of 2 joints: headNode and tailNode.
 */
-class Bone(val boneType: BoneType) {
+class Bone(val boneType: BoneType, val rotationConstraint: Constraint) {
 	private val headNode = TransformNode(true)
 	private val tailNode = TransformNode(false)
 	var parent: Bone? = null
 		private set
 	val children: MutableList<Bone> = CopyOnWriteArrayList()
 	var rotationOffset = Quaternion.IDENTITY
+	var attachedTracker: Tracker? = null

 	init {
 		headNode.attachChild(tailNode)
@@ -58,6 +62,49 @@ class Bone(val boneType: BoneType) {
 		headNode.update()
 	}

+	/**
+	 * Computes the rotations and positions of
+	 * this bone and all of its children while
+	 * enforcing rotation constraints.
+	 */
+	fun updateWithConstraints() {
+		val initialRot = getGlobalRotation()
+		val newRot = rotationConstraint.applyConstraint(initialRot, this)
+		setRotationRaw(newRot)
+		updateThisNode()
+
+		// Correct tracker if applicable. Do not adjust correction for hinge constraints
+		// or the upper chest tracker.
+		if (rotationConstraint.constraintType != ConstraintType.HINGE &&
+			rotationConstraint.constraintType != ConstraintType.LOOSE_HINGE &&
+			boneType.bodyPart != BodyPart.UPPER_CHEST
+		) {
+			val deltaRot = newRot * initialRot.inv()
+			val angle = deltaRot.angleR()
+
+			if (angle > Constraint.ANGLE_THRESHOLD &&
+				(attachedTracker?.filteringHandler?.getFilteringImpact() ?: 1f) < Constraint.FILTER_IMPACT_THRESHOLD &&
+				(parent?.attachedTracker?.filteringHandler?.getFilteringImpact() ?: 0f) < Constraint.FILTER_IMPACT_THRESHOLD
+			) {
+				attachedTracker?.resetsHandler?.updateConstraintFix(deltaRot)
+			}
+		}
+
+		// Recursively apply constraints and update children.
+		for (child in children) {
+			child.updateWithConstraints()
+		}
+	}
+
+	/**
+	 * Computes the rotations and positions of this bone.
+	 * Only to be used while traversing bones from top to bottom.
+	 */
+	private fun updateThisNode() {
+		headNode.updateThisNode()
+		tailNode.updateThisNode()
+	}
+
 	/**
 	 * Returns the world-aligned rotation of the bone
 	 */
@@ -75,6 +122,13 @@ class Bone(val boneType: BoneType) {
 		headNode.localTransform.rotation = rotation * rotationOffset
 	}

+	/**
+	 * Sets the global rotation of the bone directly
+	 */
+	fun setRotationRaw(rotation: Quaternion) {
+		headNode.localTransform.rotation = rotation
+	}
+
 	/**
 	 * Returns the global position of the head of the bone
 	 */
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java
@@ -36,6 +36,8 @@ public enum BoneType {
 	RIGHT_UPPER_ARM(BodyPart.RIGHT_UPPER_ARM),
 	LEFT_SHOULDER(BodyPart.LEFT_SHOULDER),
 	RIGHT_SHOULDER(BodyPart.RIGHT_SHOULDER),
+	LEFT_UPPER_SHOULDER,
+	RIGHT_UPPER_SHOULDER,
 	LEFT_HAND(BodyPart.LEFT_HAND),
 	RIGHT_HAND(BodyPart.RIGHT_HAND),
 	LEFT_HAND_TRACKER,
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt
@@ -0,0 +1,191 @@
+package dev.slimevr.tracking.processor
+
+import com.jme3.math.FastMath
+import io.github.axisangles.ktmath.Quaternion
+import io.github.axisangles.ktmath.Vector3
+import kotlin.math.*
+
+/**
+ * Represents a function that applies a rotational constraint.
+ */
+typealias ConstraintFunction = (localRotation: Quaternion, limit1: Float, limit2: Float, limit3: Float) -> Quaternion
+
+/**
+ * Represents the rotational limits of a Bone relative to its parent,
+ * twist and swing are the max and min when constraintType is a hinge.
+ * Twist, swing, allowedDeviation, and maxDeviationFromTracker represent
+ * an angle in degrees.
+ */
+class Constraint(
+	val constraintType: ConstraintType,
+	twist: Float = 0.0f,
+	swing: Float = 0.0f,
+	allowedDeviation: Float = 0f,
+	maxDeviationFromTracker: Float = 15f,
+) {
+	private val constraintFunction = constraintTypeToFunc(constraintType)
+	private val twistRad = twist * FastMath.DEG_TO_RAD
+	private val swingRad = swing * FastMath.DEG_TO_RAD
+	private val allowedDeviationRad = allowedDeviation * FastMath.DEG_TO_RAD
+	private val maxDeviationFromTrackerRad = maxDeviationFromTracker * FastMath.DEG_TO_RAD
+
+	/**
+	 * allowModification may be false for reasons other than a tracker being on this bone
+	 * while hasTrackerRotation is only true if this bone has a tracker. These values are
+	 * to be used with an IK solver and are not currently set accurately
+	 */
+	var allowModifications = true
+	var hasTrackerRotation = false
+
+	/**
+	 * The rotation before any IK solve takes place. Again this value is not currently set accurately
+	 */
+	var initialRotation = Quaternion.IDENTITY
+
+	/**
+	 * Apply rotational constraints and if applicable force the rotation
+	 * to be unchanged unless it violates the constraints
+	 */
+	fun applyConstraint(rotation: Quaternion, thisBone: Bone): Quaternion {
+		// When constraints are being used during a IK solve the input rotation is not necessarily
+		// the bones global rotation, thus complete constraints must be specifically handled.
+		if (constraintType == ConstraintType.COMPLETE) return thisBone.getGlobalRotation()
+
+		// If there is no parent and this is not a complete constraint accept the rotation as is.
+		if (thisBone.parent == null) return rotation
+
+		val localRotation = getLocalRotation(rotation, thisBone)
+		val constrainedRotation = constraintFunction(localRotation, swingRad, twistRad, allowedDeviationRad)
+		return getWorldRotationFromLocal(constrainedRotation, thisBone)
+	}
+
+	/**
+	 * Force the given rotation to be within allowedDeviation degrees away from
+	 * initialRotation on both the twist and swing axis
+	 */
+	fun constrainToInitialRotation(rotation: Quaternion): Quaternion {
+		val rotationLocal = rotation * initialRotation.inv()
+		var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y)
+		swingQ = constrain(swingQ, maxDeviationFromTrackerRad)
+		twistQ = constrain(twistQ, maxDeviationFromTrackerRad)
+		return initialRotation * (swingQ * twistQ)
+	}
+
+	companion object {
+		const val ANGLE_THRESHOLD = 0.004f // == 0.25 degrees
+		const val FILTER_IMPACT_THRESHOLD = 0.0349f // == 2 degrees
+
+		enum class ConstraintType {
+			TWIST_SWING,
+			HINGE,
+			LOOSE_HINGE,
+			COMPLETE,
+		}
+
+		private fun constraintTypeToFunc(type: ConstraintType) =
+			when (type) {
+				ConstraintType.COMPLETE -> completeConstraint
+				ConstraintType.TWIST_SWING -> twistSwingConstraint
+				ConstraintType.HINGE -> hingeConstraint
+				ConstraintType.LOOSE_HINGE -> looseHingeConstraint
+			}
+
+		private fun getLocalRotation(rotation: Quaternion, thisBone: Bone): Quaternion {
+			val parent = thisBone.parent!!
+			val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset
+			return (parent.getGlobalRotation() * localRotationOffset).inv() * rotation
+		}
+
+		private fun getWorldRotationFromLocal(rotation: Quaternion, thisBone: Bone): Quaternion {
+			val parent = thisBone.parent!!
+			val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset
+			return (parent.getGlobalRotation() * localRotationOffset * rotation).unit()
+		}
+
+		private fun decompose(
+			rotation: Quaternion,
+			twistAxis: Vector3,
+		): Pair<Quaternion, Quaternion> {
+			val projection = rotation.project(twistAxis).unit()
+			val twist = Quaternion(sqrt(1.0f - projection.xyz.lenSq()) * if (rotation.w >= 0f) 1f else -1f, projection.xyz).unit()
+			val swing = (rotation * twist.inv()).unit()
+			return Pair(swing, twist)
+		}
+
+		private fun constrain(rotation: Quaternion, angle: Float): Quaternion {
+			// Use angle to get the maximum magnitude the vector part of rotation can be
+			// before it has violated a constraint.
+			// Multiplying by 0.5 uniquely maps angles 0-180 degrees to 0-1 which works
+			// nicely with unit quaternions.
+			val magnitude = sin(angle * 0.5f)
+			val magnitudeSqr = magnitude * magnitude
+			val sign = if (rotation.w >= 0f) 1f else -1f
+			var vector = rotation.xyz
+			var rot = rotation
+
+			if (vector.lenSq() > magnitudeSqr) {
+				vector = vector.unit() * magnitude
+				rot = Quaternion(sqrt(1.0f - magnitudeSqr) * sign, vector)
+			}
+
+			return rot.unit()
+		}
+
+		private fun constrain(rotation: Quaternion, minAngle: Float, maxAngle: Float, axis: Vector3): Quaternion {
+			val magnitudeMin = sin(minAngle * 0.5f)
+			val magnitudeMax = sin(maxAngle * 0.5f)
+			val magnitudeSqrMin = magnitudeMin * magnitudeMin * if (minAngle >= 0f) 1f else -1f
+			val magnitudeSqrMax = magnitudeMax * magnitudeMax * if (maxAngle >= 0f) 1f else -1f
+			var vector = rotation.xyz
+			var rot = rotation
+
+			val rotMagnitude = vector.lenSq() * if (vector.dot(axis) * sign(rot.w) < 0) -1f else 1f
+			if (rotMagnitude < magnitudeSqrMin || rotMagnitude > magnitudeSqrMax) {
+				val distToMin = min(abs(rotMagnitude - magnitudeSqrMin), abs(rotMagnitude + magnitudeSqrMin))
+				val distToMax = min(abs(rotMagnitude - magnitudeSqrMax), abs(rotMagnitude + magnitudeSqrMax))
+
+				val magnitude = if (distToMin < distToMax) magnitudeMin else magnitudeMax
+				val magnitudeSqr = abs(if (distToMin < distToMax) magnitudeSqrMin else magnitudeSqrMax)
+				vector = vector.unit() * -magnitude
+
+				rot = Quaternion(sqrt(1.0f - magnitudeSqr), vector)
+			}
+
+			return rot.unit()
+		}
+
+		// Constraint function for TwistSwingConstraint
+		private val twistSwingConstraint: ConstraintFunction =
+			{ rotation: Quaternion, swingRad: Float, twistRad: Float, _: Float ->
+				var (swingQ, twistQ) = decompose(rotation, Vector3.NEG_Y)
+				swingQ = constrain(swingQ, swingRad)
+				twistQ = constrain(twistQ, twistRad)
+
+				swingQ * twistQ
+			}
+
+		// Constraint function for a hinge constraint with min and max angles
+		private val hingeConstraint: ConstraintFunction =
+			{ rotation: Quaternion, min: Float, max: Float, _: Float ->
+				val (_, hingeAxisRot) = decompose(rotation, Vector3.NEG_X)
+
+				constrain(hingeAxisRot, min, max, Vector3.NEG_X)
+			}
+
+		// Constraint function for a hinge constraint with min and max angles that allows nonHingeDeviation
+		// rotation on all axis but the hinge
+		private val looseHingeConstraint: ConstraintFunction =
+			{ rotation: Quaternion, min: Float, max: Float, nonHingeDeviation: Float ->
+				var (nonHingeRot, hingeAxisRot) = decompose(rotation, Vector3.NEG_X)
+				hingeAxisRot = constrain(hingeAxisRot, min, max, Vector3.NEG_X)
+				nonHingeRot = constrain(nonHingeRot, nonHingeDeviation)
+
+				nonHingeRot * hingeAxisRot
+			}
+
+		// Constraint function for CompleteConstraint
+		private val completeConstraint: ConstraintFunction = { rotation: Quaternion, _: Float, _: Float, _: Float ->
+			rotation
+		}
+	}
+}
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/TransformNode.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TransformNode.kt
@@ -32,6 +32,11 @@ class TransformNode(val localRotation: Boolean) {
 		}
 	}

+	@ThreadSafe
+	fun updateThisNode() {
+		updateWorldTransforms()
+	}
+
 	@Synchronized
 	private fun updateWorldTransforms() {
 		worldTransform.set(localTransform)
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt
@@ -245,6 +245,20 @@ class SkeletonConfigManager(
 				-getOffset(SkeletonConfigOffsets.FOOT_LENGTH),
 			)

+			BoneType.LEFT_UPPER_SHOULDER -> setNodeOffset(
+				nodeOffset,
+				0f,
+				0f,
+				0f,
+			)
+
+			BoneType.RIGHT_UPPER_SHOULDER -> setNodeOffset(
+				nodeOffset,
+				0f,
+				0f,
+				0f,
+			)
+
 			BoneType.LEFT_SHOULDER -> setNodeOffset(
 				nodeOffset,
 				-getOffset(SkeletonConfigOffsets.SHOULDERS_WIDTH) / 2f,
--- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java
+++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java
@@ -15,7 +15,10 @@ public enum SkeletonConfigToggles {
 	VIVE_EMULATION(7, "Vive emulation", "viveEmulation", false),
 	TOE_SNAP(8, "Toe Snap", "toeSnap", false),
 	FOOT_PLANT(9, "Foot Plant", "footPlant", true),
-	SELF_LOCALIZATION(10, "Self Localization", "selfLocalization", false),;
+	SELF_LOCALIZATION(10, "Self Localization", "selfLocalization", false),
+	USE_POSITION(11, "Use Position", "usePosition", true),
+	ENFORCE_CONSTRAINTS(12, "Enforce Constraints", "enforceConstraints", true),
+	CORRECT_CONSTRAINTS(13, "Correct Constraints", "correctConstraints", true),;

 	public static final SkeletonConfigToggles[] values = values();
 	private static final Map<String, SkeletonConfigToggles> byStringVal = new HashMap<>();
--- 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
@@ -4,6 +4,8 @@ import com.jme3.math.FastMath
 import dev.slimevr.VRServer
 import dev.slimevr.tracking.processor.Bone
 import dev.slimevr.tracking.processor.BoneType
+import dev.slimevr.tracking.processor.Constraint
+import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType
 import dev.slimevr.tracking.processor.HumanPoseManager
 import dev.slimevr.tracking.processor.config.SkeletonConfigToggles
 import dev.slimevr.tracking.processor.config.SkeletonConfigValues
@@ -35,77 +37,79 @@ class HumanSkeleton(
 	val humanPoseManager: HumanPoseManager,
 ) {
 	// Upper body bones
-	val headBone = Bone(BoneType.HEAD)
-	val neckBone = Bone(BoneType.NECK)
-	val upperChestBone = Bone(BoneType.UPPER_CHEST)
-	val chestBone = Bone(BoneType.CHEST)
-	val waistBone = Bone(BoneType.WAIST)
-	val hipBone = Bone(BoneType.HIP)
+	val headBone = Bone(BoneType.HEAD, Constraint(ConstraintType.COMPLETE))
+	val neckBone = Bone(BoneType.NECK, Constraint(ConstraintType.COMPLETE))
+	val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(ConstraintType.TWIST_SWING, 90f, 120f))
+	val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 60f, 120f))
+	val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 60f, 120f))
+	val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 60f, 120f))

 	// Lower body bones
-	val leftHipBone = Bone(BoneType.LEFT_HIP)
-	val rightHipBone = Bone(BoneType.RIGHT_HIP)
-	val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG)
-	val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG)
-	val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG)
-	val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG)
-	val leftFootBone = Bone(BoneType.LEFT_FOOT)
-	val rightFootBone = Bone(BoneType.RIGHT_FOOT)
+	val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f))
+	val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f))
+	val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 120f, 180f))
+	val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 120f, 180f))
+	val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 50f))
+	val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 50f))
+	val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f))
+	val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f))

 	// Arm bones
-	val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER)
-	val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER)
-	val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM)
-	val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM)
-	val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM)
-	val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM)
-	val leftHandBone = Bone(BoneType.LEFT_HAND)
-	val rightHandBone = Bone(BoneType.RIGHT_HAND)
+	val leftUpperShoulderBone = Bone(BoneType.LEFT_UPPER_SHOULDER, Constraint(ConstraintType.COMPLETE))
+	val rightUpperShoulderBone = Bone(BoneType.RIGHT_UPPER_SHOULDER, Constraint(ConstraintType.COMPLETE))
+	val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 10f))
+	val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 10f))
+	val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 120f, 180f))
+	val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 120f, 180f))
+	val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f))
+	val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f))
+	val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 120f, 120f))
+	val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 120f, 120f))

 	// Finger bones
-	val leftThumbMetacarpalBone = Bone(BoneType.LEFT_THUMB_METACARPAL)
-	val leftThumbProximalBone = Bone(BoneType.LEFT_THUMB_PROXIMAL)
-	val leftThumbDistalBone = Bone(BoneType.LEFT_THUMB_DISTAL)
-	val leftIndexProximalBone = Bone(BoneType.LEFT_INDEX_PROXIMAL)
-	val leftIndexIntermediateBone = Bone(BoneType.LEFT_INDEX_INTERMEDIATE)
-	val leftIndexDistalBone = Bone(BoneType.LEFT_INDEX_DISTAL)
-	val leftMiddleProximalBone = Bone(BoneType.LEFT_MIDDLE_PROXIMAL)
-	val leftMiddleIntermediateBone = Bone(BoneType.LEFT_MIDDLE_INTERMEDIATE)
-	val leftMiddleDistalBone = Bone(BoneType.LEFT_MIDDLE_DISTAL)
-	val leftRingProximalBone = Bone(BoneType.LEFT_RING_PROXIMAL)
-	val leftRingIntermediateBone = Bone(BoneType.LEFT_RING_INTERMEDIATE)
-	val leftRingDistalBone = Bone(BoneType.LEFT_RING_DISTAL)
-	val leftLittleProximalBone = Bone(BoneType.LEFT_LITTLE_PROXIMAL)
-	val leftLittleIntermediateBone = Bone(BoneType.LEFT_LITTLE_INTERMEDIATE)
-	val leftLittleDistalBone = Bone(BoneType.LEFT_LITTLE_DISTAL)
-	val rightThumbMetacarpalBone = Bone(BoneType.RIGHT_THUMB_METACARPAL)
-	val rightThumbProximalBone = Bone(BoneType.RIGHT_THUMB_PROXIMAL)
-	val rightThumbDistalBone = Bone(BoneType.RIGHT_THUMB_DISTAL)
-	val rightIndexProximalBone = Bone(BoneType.RIGHT_INDEX_PROXIMAL)
-	val rightIndexIntermediateBone = Bone(BoneType.RIGHT_INDEX_INTERMEDIATE)
-	val rightIndexDistalBone = Bone(BoneType.RIGHT_INDEX_DISTAL)
-	val rightMiddleProximalBone = Bone(BoneType.RIGHT_MIDDLE_PROXIMAL)
-	val rightMiddleIntermediateBone = Bone(BoneType.RIGHT_MIDDLE_INTERMEDIATE)
-	val rightMiddleDistalBone = Bone(BoneType.RIGHT_MIDDLE_DISTAL)
-	val rightRingProximalBone = Bone(BoneType.RIGHT_RING_PROXIMAL)
-	val rightRingIntermediateBone = Bone(BoneType.RIGHT_RING_INTERMEDIATE)
-	val rightRingDistalBone = Bone(BoneType.RIGHT_RING_DISTAL)
-	val rightLittleProximalBone = Bone(BoneType.RIGHT_LITTLE_PROXIMAL)
-	val rightLittleIntermediateBone = Bone(BoneType.RIGHT_LITTLE_INTERMEDIATE)
-	val rightLittleDistalBone = Bone(BoneType.RIGHT_LITTLE_DISTAL)
+	val leftThumbMetacarpalBone = Bone(BoneType.LEFT_THUMB_METACARPAL, Constraint(ConstraintType.COMPLETE))
+	val leftThumbProximalBone = Bone(BoneType.LEFT_THUMB_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val leftThumbDistalBone = Bone(BoneType.LEFT_THUMB_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val leftIndexProximalBone = Bone(BoneType.LEFT_INDEX_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val leftIndexIntermediateBone = Bone(BoneType.LEFT_INDEX_INTERMEDIATE, Constraint(ConstraintType.COMPLETE))
+	val leftIndexDistalBone = Bone(BoneType.LEFT_INDEX_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val leftMiddleProximalBone = Bone(BoneType.LEFT_MIDDLE_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val leftMiddleIntermediateBone = Bone(BoneType.LEFT_MIDDLE_INTERMEDIATE, Constraint(ConstraintType.COMPLETE))
+	val leftMiddleDistalBone = Bone(BoneType.LEFT_MIDDLE_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val leftRingProximalBone = Bone(BoneType.LEFT_RING_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val leftRingIntermediateBone = Bone(BoneType.LEFT_RING_INTERMEDIATE, Constraint(ConstraintType.COMPLETE))
+	val leftRingDistalBone = Bone(BoneType.LEFT_RING_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val leftLittleProximalBone = Bone(BoneType.LEFT_LITTLE_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val leftLittleIntermediateBone = Bone(BoneType.LEFT_LITTLE_INTERMEDIATE, Constraint(ConstraintType.COMPLETE))
+	val leftLittleDistalBone = Bone(BoneType.LEFT_LITTLE_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val rightThumbMetacarpalBone = Bone(BoneType.RIGHT_THUMB_METACARPAL, Constraint(ConstraintType.COMPLETE))
+	val rightThumbProximalBone = Bone(BoneType.RIGHT_THUMB_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val rightThumbDistalBone = Bone(BoneType.RIGHT_THUMB_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val rightIndexProximalBone = Bone(BoneType.RIGHT_INDEX_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val rightIndexIntermediateBone = Bone(BoneType.RIGHT_INDEX_INTERMEDIATE, Constraint(ConstraintType.COMPLETE))
+	val rightIndexDistalBone = Bone(BoneType.RIGHT_INDEX_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val rightMiddleProximalBone = Bone(BoneType.RIGHT_MIDDLE_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val rightMiddleIntermediateBone = Bone(BoneType.RIGHT_MIDDLE_INTERMEDIATE, Constraint(ConstraintType.COMPLETE))
+	val rightMiddleDistalBone = Bone(BoneType.RIGHT_MIDDLE_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val rightRingProximalBone = Bone(BoneType.RIGHT_RING_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val rightRingIntermediateBone = Bone(BoneType.RIGHT_RING_INTERMEDIATE, Constraint(ConstraintType.COMPLETE))
+	val rightRingDistalBone = Bone(BoneType.RIGHT_RING_DISTAL, Constraint(ConstraintType.COMPLETE))
+	val rightLittleProximalBone = Bone(BoneType.RIGHT_LITTLE_PROXIMAL, Constraint(ConstraintType.COMPLETE))
+	val rightLittleIntermediateBone = Bone(BoneType.RIGHT_LITTLE_INTERMEDIATE, Constraint(ConstraintType.COMPLETE))
+	val rightLittleDistalBone = Bone(BoneType.RIGHT_LITTLE_DISTAL, Constraint(ConstraintType.COMPLETE))

 	// Tracker bones
-	val headTrackerBone = Bone(BoneType.HEAD_TRACKER)
-	val chestTrackerBone = Bone(BoneType.CHEST_TRACKER)
-	val hipTrackerBone = Bone(BoneType.HIP_TRACKER)
-	val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER)
-	val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER)
-	val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER)
-	val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER)
-	val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER)
-	val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER)
-	val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER)
-	val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER)
+	val headTrackerBone = Bone(BoneType.HEAD_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val hipTrackerBone = Bone(BoneType.HIP_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, Constraint(ConstraintType.COMPLETE))
+	val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, Constraint(ConstraintType.COMPLETE))

 	// Buffers
 	var hasSpineTracker = false
@@ -190,6 +194,8 @@ class HumanSkeleton(
 	private var extendedPelvisModel = false
 	private var extendedKneeModel = false
 	private var forceArmsFromHMD = true
+	private var enforceConstraints = true
+	private var correctConstraints = true

 	// Ratios
 	private var waistFromChestHipAveraging = 0f
@@ -292,8 +298,10 @@ class HumanSkeleton(
 		}

 		// Shoulders
-		neckBone.attachChild(leftShoulderBone)
-		neckBone.attachChild(rightShoulderBone)
+		neckBone.attachChild(leftUpperShoulderBone)
+		neckBone.attachChild(rightUpperShoulderBone)
+		leftUpperShoulderBone.attachChild(leftShoulderBone)
+		rightUpperShoulderBone.attachChild(rightShoulderBone)

 		// Upper arm
 		leftShoulderBone.attachChild(leftUpperArmBone)
@@ -442,6 +450,9 @@ class HumanSkeleton(

 		// Update tap detection's trackers
 		tapDetectionManager.updateConfig(trackers)
+
+		// Update bones tracker field
+		refreshBoneTracker()
 	}

 	/**
@@ -500,6 +511,7 @@ class HumanSkeleton(

 		updateTransforms()
 		updateBones()
+		headBone.updateWithConstraints()
 		updateComputedTrackers()

 		// Don't run post-processing if the tracking is paused
@@ -510,6 +522,15 @@ class HumanSkeleton(
 		viveEmulation.update()
 	}

+	/**
+	 * Refresh the attachedTracker field in each bone
+	 */
+	private fun refreshBoneTracker() {
+		for (bone in allHumanBones) {
+			bone.attachedTracker = getTrackerForBone(bone.boneType)
+		}
+	}
+
 	/**
 	 * Update all the bones by updating the roots
 	 */
@@ -560,6 +581,7 @@ class HumanSkeleton(
 		// Left arm
 		updateArmTransforms(
 			isTrackingLeftArmFromController,
+			leftUpperShoulderBone,
 			leftShoulderBone,
 			leftUpperArmBone,
 			leftElbowTrackerBone,
@@ -575,6 +597,7 @@ class HumanSkeleton(
 		// Right arm
 		updateArmTransforms(
 			isTrackingRightArmFromController,
+			rightUpperShoulderBone,
 			rightShoulderBone,
 			rightUpperArmBone,
 			rightElbowTrackerBone,
@@ -925,6 +948,7 @@ class HumanSkeleton(
 	 */
 	private fun updateArmTransforms(
 		isTrackingFromController: Boolean,
+		upperShoulderBone: Bone,
 		shoulderBone: Bone,
 		upperArmBone: Bone,
 		elbowTrackerBone: Bone,
@@ -957,6 +981,7 @@ class HumanSkeleton(
 			// Get shoulder rotation
 			var armRot = shoulderTracker?.getRotation() ?: upperChestBone.getLocalRotation()
 			// Set shoulder rotation
+			upperShoulderBone.setRotation(upperChestBone.getLocalRotation())
 			shoulderBone.setRotation(armRot)

 			if (upperArmTracker != null || lowerArmTracker != null) {
@@ -1136,6 +1161,12 @@ class HumanSkeleton(
 			SkeletonConfigToggles.FOOT_PLANT -> legTweaks.footPlantEnabled = newValue

 			SkeletonConfigToggles.SELF_LOCALIZATION -> localizer.setEnabled(newValue)
+
+			SkeletonConfigToggles.USE_POSITION -> newValue
+
+			SkeletonConfigToggles.ENFORCE_CONSTRAINTS -> enforceConstraints = newValue
+
+			SkeletonConfigToggles.CORRECT_CONSTRAINTS -> correctConstraints = newValue
 		}
 	}

@@ -1217,6 +1248,8 @@ class HumanSkeleton(
 		BoneType.RIGHT_FOOT -> rightFootBone
 		BoneType.LEFT_FOOT_TRACKER -> leftFootTrackerBone
 		BoneType.RIGHT_FOOT_TRACKER -> rightFootTrackerBone
+		BoneType.LEFT_UPPER_SHOULDER -> leftUpperShoulderBone
+		BoneType.RIGHT_UPPER_SHOULDER -> rightUpperShoulderBone
 		BoneType.LEFT_SHOULDER -> leftShoulderBone
 		BoneType.RIGHT_SHOULDER -> rightShoulderBone
 		BoneType.LEFT_UPPER_ARM -> leftUpperArmBone
@@ -1261,6 +1294,30 @@ class HumanSkeleton(
 		BoneType.RIGHT_LITTLE_DISTAL -> rightLittleDistalBone
 	}

+	private fun getTrackerForBone(bone: BoneType?): Tracker? = when (bone) {
+		BoneType.HEAD -> headTracker
+		BoneType.NECK -> neckTracker
+		BoneType.UPPER_CHEST -> upperChestTracker
+		BoneType.CHEST -> chestTracker
+		BoneType.WAIST -> waistTracker
+		BoneType.HIP -> hipTracker
+		BoneType.LEFT_UPPER_LEG -> leftUpperLegTracker
+		BoneType.RIGHT_UPPER_LEG -> rightUpperLegTracker
+		BoneType.LEFT_LOWER_LEG -> leftLowerLegTracker
+		BoneType.RIGHT_LOWER_LEG -> rightLowerLegTracker
+		BoneType.LEFT_FOOT -> leftFootTracker
+		BoneType.RIGHT_FOOT -> rightFootTracker
+		BoneType.LEFT_SHOULDER -> leftShoulderTracker
+		BoneType.RIGHT_SHOULDER -> rightShoulderTracker
+		BoneType.LEFT_UPPER_ARM -> leftUpperArmTracker
+		BoneType.RIGHT_UPPER_ARM -> rightUpperArmTracker
+		BoneType.LEFT_LOWER_ARM -> leftLowerArmTracker
+		BoneType.RIGHT_LOWER_ARM -> rightLowerArmTracker
+		BoneType.LEFT_HAND -> leftHandTracker
+		BoneType.RIGHT_HAND -> rightHandTracker
+		else -> null
+	}
+
 	/**
 	 * Returns an array of all the non-tracker bones.
 	 */
@@ -1280,6 +1337,8 @@ class HumanSkeleton(
 			rightLowerLegBone,
 			leftFootBone,
 			rightFootBone,
+			leftUpperShoulderBone,
+			rightUpperShoulderBone,
 			leftShoulderBone,
 			rightShoulderBone,
 			leftUpperArmBone,
@@ -1325,6 +1384,8 @@ class HumanSkeleton(
 	 */
 	private val allArmBones: Array<Bone>
 		get() = arrayOf(
+			leftUpperShoulderBone,
+			rightUpperShoulderBone,
 			leftShoulderBone,
 			rightShoulderBone,
 			leftUpperArmBone,
--- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt
@@ -58,4 +58,9 @@ class TrackerFilteringHandler {
 	 * Get the filtered rotation from the moving average (either prediction/smoothing or just >180 degs)
 	 */
 	fun getFilteredRotation() = movingAverage.filteredQuaternion
+
+	/**
+	 * Get the impact filtering has on the rotation
+	 */
+	fun getFilteringImpact(): Float = movingAverage.filteringImpact
 }
--- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt
+++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt
@@ -63,6 +63,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	var mountRotFix = Quaternion.IDENTITY
 		private set
 	private var yawFix = Quaternion.IDENTITY
+	private var constraintFix = Quaternion.IDENTITY

 	// Yaw reset smoothing vars
 	private var yawFixOld = Quaternion.IDENTITY
@@ -168,6 +169,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		rot = mountRotFix.inv() * (rot * mountRotFix)
 		rot *= tposeDownFix
 		rot = yawFix * rot
+		rot = constraintFix * rot
 		return rot
 	}

@@ -180,6 +182,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		rot = gyroFixNoMounting * rot
 		rot *= attachmentFixNoMounting
 		rot = yawFixZeroReference * rot
+		rot = constraintFix * rot
 		return rot
 	}

@@ -214,6 +217,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	 * 0). This allows the tracker to be strapped to body at any pitch and roll.
 	 */
 	fun resetFull(reference: Quaternion) {
+		constraintFix = Quaternion.IDENTITY
+
 		if (tracker.trackerDataType == TrackerDataType.FLEX_RESISTANCE) {
 			tracker.trackerFlexHandler.resetMin()
 			postProcessResetFull()
@@ -305,6 +310,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
 	 * position should be corrected in the source.
 	 */
 	fun resetYaw(reference: Quaternion) {
+		constraintFix = Quaternion.IDENTITY
+
 		if (tracker.trackerDataType == TrackerDataType.FLEX_RESISTANCE ||
 			tracker.trackerDataType == TrackerDataType.FLEX_ANGLE
 		) {
@@ -355,6 +362,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
 			return
 		}

+		constraintFix = Quaternion.IDENTITY
+
 		// Get the current calibrated rotation
 		var rotBuf = adjustToDrift(tracker.getRawRotation() * mountingOrientation)
 		rotBuf = gyroFix * rotBuf
@@ -403,6 +412,13 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		tracker.resetFilteringQuats()
 	}

+	/**
+	 * Apply a corrective rotation to the gyroFix
+	 */
+	fun updateConstraintFix(correctedRotation: Quaternion) {
+		constraintFix *= correctedRotation
+	}
+
 	fun clearMounting() {
 		mountRotFix = Quaternion.IDENTITY
 	}