github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-06 02:01:58 +02:00
Code Issues 161 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Erimel
2025-02-09 17:43:14 -05:00
committed by GitHub
parent f89f9ae35b
commit 9e666a74e1
1 changed files with 46 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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()
}