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 index 728faedf1d..8f5568936b 100644 --- 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() }