Remove tracker polarity tracking

server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt

@@ -50,7 +50,10 @@ class QuaternionMovingAverage(
 			predictFactor = PREDICT_MULTIPLIER * amount + PREDICT_MIN
 			rotBuffer = CircularArrayList(PREDICT_BUFFER)
 		}
-		resetQuats(initialRotation)
+
+		latestQuaternion = initialRotation
+		filteredQuaternion = initialRotation
+		addQuaternion(initialRotation)
 	}
 
 	// Runs at up to 1000hz. We use a timer to make it framerate-independent
@@ -91,8 +94,7 @@ class QuaternionMovingAverage(
 			// Smooth towards the target rotation by the slerp factor
 			filteredQuaternion = smoothingQuaternion.interpR(latestQuaternion, amt)
 		} else {
-			// No filtering; just keep track of rotations (for going over 180 degrees)
-			filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion)
+			return
 		}
 
 		filteringImpact = latestQuaternion.angleToR(filteredQuaternion)
@@ -112,18 +114,9 @@ class QuaternionMovingAverage(
 			lastAmt = 0f
 			smoothingQuaternion = filteredQuaternion
 		} else {
-			smoothingQuaternion = filteredQuaternion
+			filteredQuaternion = q
 		}
 
 		latestQuaternion = q
 	}
-
-	fun resetQuats(q: Quaternion) {
-		if (type == TrackerFilters.PREDICTION) {
-			rotBuffer?.clear()
-			latestQuaternion = q
-		}
-		filteredQuaternion = q
-		addQuaternion(q)
-	}
 }

server/core/src/main/java/dev/slimevr/poseframeformat/trackerdata/TrackerFrames.kt

@@ -41,7 +41,6 @@ data class TrackerFrames(var name: String = "", val frames: FastList<TrackerFram
 			// Make sure this is false!! Otherwise HumanSkeleton ignores it
 			isInternal = false,
 			isComputed = true,
-			trackRotDirection = false,
 		)
 
 		tracker.status = TrackerStatus.OK

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt

@@ -30,7 +30,6 @@ import io.github.axisangles.ktmath.Vector3.Companion.NEG_Y
 import io.github.axisangles.ktmath.Vector3.Companion.NULL
 import io.github.axisangles.ktmath.Vector3.Companion.POS_Y
 import solarxr_protocol.rpc.StatusData
-import java.lang.IllegalArgumentException
 import kotlin.properties.Delegates
 
 class HumanSkeleton(
@@ -805,106 +804,98 @@ class HumanSkeleton(
 			// Tries to guess missing lower spine trackers by interpolating rotations
 			if (waistTracker == null) {
 				getFirstAvailableTracker(chestTracker, upperChestTracker)?.let { chest ->
+					val chestRot = chest.getRotation()
+
 					hipTracker?.let {
 						// Calculates waist from chest + hip
-						var hipRot = it.getRotation()
-						var chestRot = chest.getRotation()
-
-						// Get the rotation relative to where we expect the hip to be
-						if (chestRot.times(FORWARD_QUATERNION).dot(hipRot) < 0.0f) {
-							hipRot = hipRot.unaryMinus()
-						}
+						val hipRot = signExtendedRot(chestRot, it.getRotation())
 
 						// Interpolate between the chest and the hip
-						chestRot = chestRot.interpQ(hipRot, waistFromChestHipAveraging)
+						val interp = chestRot.interpQ(hipRot, waistFromChestHipAveraging)
 
 						// Set waist's rotation
-						waistBone.setRotation(chestRot)
+						waistBone.setRotation(interp)
 					} ?: run {
 						if (hasKneeTrackers) {
 							// Calculates waist from chest + legs
-							var leftLegRot = leftUpperLegTracker?.getRotation() ?: IDENTITY
-							var rightLegRot = rightUpperLegTracker?.getRotation() ?: IDENTITY
-							var chestRot = chest.getRotation()
-
-							// Get the rotation relative to where we expect the upper legs to be
-							val expectedUpperLegsRot = chestRot.times(FORWARD_QUATERNION)
-							if (expectedUpperLegsRot.dot(leftLegRot) < 0.0f) {
-								leftLegRot = leftLegRot.unaryMinus()
-							}
-							if (expectedUpperLegsRot.dot(rightLegRot) < 0.0f) {
-								rightLegRot = rightLegRot.unaryMinus()
-							}
+							val leftLegRot = signExtendedRot(
+								chestRot,
+								leftUpperLegTracker?.getRotation(),
+							)
+							val rightLegRot = signExtendedRot(
+								chestRot,
+								rightUpperLegTracker?.getRotation(),
+							)
 
 							// Interpolate between the pelvis, averaged from the legs, and the chest
-							chestRot = chestRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), waistFromChestLegsAveraging).unit()
+							val interp = chestRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), waistFromChestLegsAveraging).unit()
 
 							// Set waist's rotation
-							waistBone.setRotation(chestRot)
+							waistBone.setRotation(interp)
 						}
 					}
 				}
 			}
 			if (hipTracker == null && hasKneeTrackers) {
 				waistTracker?.let {
 					// Calculates hip from waist + legs
-					var leftLegRot = leftUpperLegTracker?.getRotation() ?: IDENTITY
-					var rightLegRot = rightUpperLegTracker?.getRotation() ?: IDENTITY
-					var waistRot = it.getRotation()
-
-					// Get the rotation relative to where we expect the upper legs to be
-					val expectedUpperLegsRot = waistRot.times(FORWARD_QUATERNION)
-					if (expectedUpperLegsRot.dot(leftLegRot) < 0.0f) {
-						leftLegRot = leftLegRot.unaryMinus()
-					}
-					if (expectedUpperLegsRot.dot(rightLegRot) < 0.0f) {
-						rightLegRot = rightLegRot.unaryMinus()
-					}
+					val waistRot = it.getRotation()
+					val leftLegRot = signExtendedRot(
+						waistRot,
+						leftUpperLegTracker?.getRotation(),
+					)
+					val rightLegRot = signExtendedRot(
+						waistRot,
+						rightUpperLegTracker?.getRotation(),
+					)
 
 					// Interpolate between the pelvis, averaged from the legs, and the chest
-					waistRot = waistRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), hipFromWaistLegsAveraging).unit()
+					val interp = waistRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), hipFromWaistLegsAveraging).unit()
 
 					// Set hip rotation
-					hipBone.setRotation(waistRot)
-					hipTrackerBone.setRotation(waistRot)
+					hipBone.setRotation(interp)
+					hipTrackerBone.setRotation(interp)
 				} ?: run {
 					getFirstAvailableTracker(chestTracker, upperChestTracker)?.let {
 						// Calculates hip from chest + legs
-						var leftLegRot = leftUpperLegTracker?.getRotation() ?: IDENTITY
-						var rightLegRot = rightUpperLegTracker?.getRotation() ?: IDENTITY
-						var chestRot = it.getRotation()
-
-						// Get the rotation relative to where we expect the upper legs to be
-						val expectedUpperLegsRot = chestRot.times(FORWARD_QUATERNION)
-						if (expectedUpperLegsRot.dot(leftLegRot) < 0.0f) {
-							leftLegRot = leftLegRot.unaryMinus()
-						}
-						if (expectedUpperLegsRot.dot(rightLegRot) < 0.0f) {
-							rightLegRot = rightLegRot.unaryMinus()
-						}
+						val chestRot = it.getRotation()
+						val leftLegRot = signExtendedRot(
+							chestRot,
+							leftUpperLegTracker?.getRotation(),
+						)
+						val rightLegRot = signExtendedRot(
+							chestRot,
+							rightUpperLegTracker?.getRotation(),
+						)
 
 						// Interpolate between the pelvis, averaged from the legs, and the chest
-						chestRot = chestRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), hipFromChestLegsAveraging).unit()
+						val interp = chestRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), hipFromChestLegsAveraging).unit()
 
 						// Set hip rotation
-						hipBone.setRotation(chestRot)
-						hipTrackerBone.setRotation(chestRot)
+						hipBone.setRotation(interp)
+						hipTrackerBone.setRotation(interp)
 					}
 				}
 			}
 		}
 
 		// Extended pelvis model
 		if (extendedPelvisModel && hasKneeTrackers && hipTracker == null) {
-			val leftLegRot = leftUpperLegTracker?.getRotation() ?: IDENTITY
-			val rightLegRot = rightUpperLegTracker?.getRotation() ?: IDENTITY
 			val hipRot = hipBone.getLocalRotation()
+			val leftLegRot = signExtendedRot(
+				hipRot,
+				leftUpperLegTracker?.getRotation(),
+			)
+			val rightLegRot = signExtendedRot(
+				hipRot,
+				rightUpperLegTracker?.getRotation(),
+			)
 
 			val extendedPelvisRot = extendedPelvisYawRoll(leftLegRot, rightLegRot, hipRot)
 
 			// Interpolate between the hipRot and extendedPelvisRot
 			val newHipRot = hipRot.interpR(
-				if (extendedPelvisRot.lenSq() != 0.0f) extendedPelvisRot else IDENTITY,
+				if (extendedPelvisRot.lenSq() != 0.0f) extendedPelvisRot else hipRot,
 				hipLegsAveraging,
 			)
 
@@ -1127,8 +1118,8 @@ class HumanSkeleton(
 	 * Rotates the third Quaternion to match its yaw and roll to the rotation of
 	 * the average of the first and second quaternions.
 	 *
-	 * @param leftKnee the first Quaternion
-	 * @param rightKnee the second Quaternion
+	 * @param leftKnee the first Quaternion (signed to the hip)
+	 * @param rightKnee the second Quaternion (signed to the hip)
 	 * @param hip the third Quaternion
 	 * @return the rotated Quaternion
 	 */
@@ -1137,24 +1128,11 @@ class HumanSkeleton(
 		rightKnee: Quaternion,
 		hip: Quaternion,
 	): Quaternion {
-		// Get the knees' rotation relative to where we expect them to be.
-		// The angle between your knees and hip can be over 180 degrees...
-		var leftKneeRot = leftKnee
-		var rightKneeRot = rightKnee
-
-		val kneeRot = hip.times(FORWARD_QUATERNION)
-		if (kneeRot.dot(leftKneeRot) < 0.0f) {
-			leftKneeRot = leftKneeRot.unaryMinus()
-		}
-		if (kneeRot.dot(rightKneeRot) < 0.0f) {
-			rightKneeRot = rightKneeRot.unaryMinus()
-		}
-
 		// R = InverseHip * (LeftLeft + RightLeg)
 		// C = Quaternion(R.w, -R.x, 0, 0)
 		// Pelvis = Hip * R * C
 		// normalize(Pelvis)
-		val r = hip.inv() * (leftKneeRot + rightKneeRot)
+		val r = hip.inv() * (leftKnee + rightKnee)
 		val c = Quaternion(r.w, -r.x, 0f, 0f)
 		return (hip * r * c).unit()
 	}
@@ -1736,11 +1714,39 @@ class HumanSkeleton(
 	}
 
 	companion object {
-		val FORWARD_QUATERNION = EulerAngles(
+		/**
+		 * Used to rotate an identity quaternion to face up.
+		 */
+		val UP_ADJ = EulerAngles(
 			EulerOrder.YZX,
-			FastMath.HALF_PI,
+			-FastMath.HALF_PI,
 			0f,
 			0f,
 		).toQuaternion()
+
+		/**
+		 * Similar to twinNearest, but uses the longest rotation for the lower back
+		 * quadrant. This handles the thigh extending behind the torso to face
+		 * downwards, and the hip extending behind the chest. The thigh cannot bend to
+		 * the back away from the torso and the spine hopefully can't bend back that far,
+		 * so we can fairly safely assume the rotation is towards the torso.
+		 */
+		fun signExtendedRot(ref: Quaternion, extended: Quaternion?): Quaternion {
+			// Handle null here for convenience
+			if (extended == null) {
+				return ref
+			}
+
+			val isBack = ref.angleToR(extended) > FastMath.HALF_PI
+			val isLower = (ref * UP_ADJ).angleToR(extended) > FastMath.HALF_PI
+
+			return if (isBack && isLower) {
+				// Select longest rotation
+				if (extended.dot(ref) >= 0f) -extended else extended
+			} else {
+				// Select shortest rotation
+				extended.twinNearest(ref)
+			}
+		}
 	}
 }

server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt

@@ -67,12 +67,6 @@ class Tracker @JvmOverloads constructor(
 	val needsReset: Boolean = false,
 	val needsMounting: Boolean = false,
 	val isHmd: Boolean = false,
-	/**
-	 * Whether to track the direction of the tracker's rotation
-	 * (positive vs negative rotation). This needs to be disabled for AutoBone and
-	 * unit tests, where the rotation is absolute and not temporal.
-	 */
-	val trackRotDirection: Boolean = true,
 	magStatus: MagnetometerStatus = MagnetometerStatus.NOT_SUPPORTED,
 	/**
 	 * Rotation by default.
@@ -306,7 +300,9 @@ class Tracker @JvmOverloads constructor(
 				status = TrackerStatus.TIMED_OUT
 			}
 		}
-		filteringHandler.update()
+		if (filteringHandler.filteringEnabled) {
+			filteringHandler.update()
+		}
 		resetsHandler.update()
 	}
 
@@ -316,7 +312,7 @@ class Tracker @JvmOverloads constructor(
 	fun dataTick() {
 		timer.update()
 		timeAtLastUpdate = System.currentTimeMillis()
-		if (trackRotDirection) {
+		if (filteringHandler.filteringEnabled) {
 			filteringHandler.dataTick(_rotation)
 		}
 	}
@@ -328,7 +324,7 @@ class Tracker @JvmOverloads constructor(
 		timeAtLastUpdate = System.currentTimeMillis()
 	}
 
-	private fun getFilteredRotation(): Quaternion = if (trackRotDirection) {
+	private fun getFilteredRotation(): Quaternion = if (filteringHandler.filteringEnabled) {
 		filteringHandler.getFilteredRotation()
 	} else {
 		// Get raw rotation
@@ -422,11 +418,4 @@ class Tracker @JvmOverloads constructor(
 	 */
 	val tps: Float
 		get() = timer.averageFPS
-
-	/**
-	 * Call when doing a full reset to reset the tracking of rotations >180 degrees
-	 */
-	fun resetFilteringQuats() {
-		filteringHandler.resetMovingAverage(_rotation)
-	}
 }

server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt

@@ -37,7 +37,9 @@ class TrackerFilteringHandler {
 	 * Update the moving average to make it smooth
 	 */
 	fun update() {
-		movingAverage.update()
+		if (filteringEnabled) {
+			movingAverage.update()
+		}
 	}
 
 	/**
@@ -47,13 +49,6 @@ class TrackerFilteringHandler {
 		movingAverage.addQuaternion(currentRawRotation)
 	}
 
-	/**
-	 * Call when doing a full reset to reset the tracking of rotations >180 degrees
-	 */
-	fun resetMovingAverage(currentRawRotation: Quaternion) {
-		movingAverage.resetQuats(currentRawRotation)
-	}
-
 	/**
 	 * Get the filtered rotation from the moving average (either prediction/smoothing or just >180 degs)
 	 */