Revert "Fix over 180 degrees rotations in HumanSkeleton" (#1302)

SlimeVR · Feb 9, 2025 · 9e666a7 · 9e666a7
1 parent f89f9ae
commit 9e666a7
Showing 1 changed file with 46 additions and 1 deletion.
diff --git 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
@@ -810,6 +810,11 @@ class HumanSkeleton(
 						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()
+						}
+
 						// Interpolate between the chest and the hip
 						chestRot = chestRot.interpQ(hipRot, waistFromChestHipAveraging)
 
@@ -822,6 +827,15 @@ class HumanSkeleton(
 							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()
+							}
+
 							// Interpolate between the pelvis, averaged from the legs, and the chest
 							chestRot = chestRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), waistFromChestLegsAveraging).unit()
 
@@ -838,6 +852,15 @@ class HumanSkeleton(
 					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()
+					}
+
 					// Interpolate between the pelvis, averaged from the legs, and the chest
 					waistRot = waistRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), hipFromWaistLegsAveraging).unit()
 
@@ -851,6 +874,15 @@ class HumanSkeleton(
 						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()
+						}
+
 						// Interpolate between the pelvis, averaged from the legs, and the chest
 						chestRot = chestRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), hipFromChestLegsAveraging).unit()
 
@@ -1105,11 +1137,24 @@ 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() * (leftKnee + rightKnee)
+		val r = hip.inv() * (leftKneeRot + rightKneeRot)
 		val c = Quaternion(r.w, -r.x, 0f, 0f)
 		return (hip * r * c).unit()
 	}