fix IK breaking at 180 degree rotations relative to the parent

SlimeVR · Sep 27, 2024 · 42c5f62 · 42c5f62
1 parent af89bc5
commit 42c5f62
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 29 deletions.
diff --git 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
@@ -16,7 +16,7 @@ public enum SkeletonConfigToggles {
 	TOE_SNAP(8, "Toe Snap", "toeSnap", false),
 	FOOT_PLANT(9, "Foot Plant", "footPlant", true),
 	SELF_LOCALIZATION(10, "Self Localization", "selfLocalization", false),
-	USE_POSITION(11, "Use Position", "usePosition", false),
+	USE_POSITION(11, "Use Position", "usePosition", true),
 	ENFORCE_CONSTRAINTS(12, "Enforce Constraints", "enforceConstraints", true),
 	CORRECT_CONSTRAINTS(13, "Correct Constraints", "correctConstraints", true),;
 

diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt
@@ -40,7 +40,7 @@ class IKChain(
 	 */
 	private fun prepBones() {
 		for (i in 0..<bones.size) {
-			if (bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE && bones[i].rotationConstraint.allowModifications) {
+			if (bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE && bones[i].boneType.bodyPart !in IKSolver.LOCK_ROTATION) {
 				bones[i].setRotationRaw(rotations[i])
 			}
 		}
@@ -51,7 +51,6 @@ class IKChain(
 
 		for (i in bones.size - 1 downTo 0) {
 			val currentBone = bones[i]
-			val childBone = if (bones.size - 1 < i) bones[i + 1] else null
 
 			if (currentBone.boneType.bodyPart in IKSolver.LOCK_ROTATION) {
 				rotations[i] = currentBone.getGlobalRotation()
@@ -64,10 +63,17 @@ class IKChain(
 
 			// Compute the axis of rotation and angle for this bone
 			val scalar = IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.allowModifications) 1f else IKSolver.STATIC_DAMPENING
-			val adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit()
-			val correctedRot = (adjustment * currentBone.getGlobalRotation()).unit()
+			var adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit()
 
-			rotations[i] = setBoneRotation(currentBone, childBone, correctedRot, useConstraints)
+			// Bones that are not supposed to be modified should tend towards their origin
+			val rotation = currentBone.getGlobalRotation()
+			if (!currentBone.rotationConstraint.allowModifications) {
+				adjustment *= rotation.interpR(currentBone.rotationConstraint.initialRotation, IKSolver.CORRECTION_FACTOR) * rotation.inv()
+			}
+
+			val correctedRot = (adjustment * rotation).unit()
+
+			rotations[i] = setBoneRotation(currentBone, correctedRot, useConstraints)
 		}
 	}
 
@@ -102,7 +108,7 @@ class IKChain(
 		for (b in bones) {
 			b.rotationConstraint.initialRotation = b.getGlobalRotation()
 		}
-		// prepBones()
+		prepBones()
 
 		for (child in children) {
 			child.resetChain()
@@ -131,11 +137,11 @@ class IKChain(
 	}
 
 	/**
-	 * Sets a bones rotation from a rotation vector after constraining the rotation
-	 * vector with the bone's rotational constraint
+	 * Sets a bones rotation after constraining the rotation
+	 * to the bone's rotational constraint
 	 * returns the constrained rotation
 	 */
-	private fun setBoneRotation(bone: Bone, childBone: Bone?, rotation: Quaternion, useConstraints: Boolean): Quaternion {
+	private fun setBoneRotation(bone: Bone, rotation: Quaternion, useConstraints: Boolean): Quaternion {
 		// Constrain relative to the parent
 		val newRotation = if (bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) {
 			bone.rotationConstraint.applyConstraint(rotation, bone)
@@ -148,14 +154,6 @@ class IKChain(
 		bone.setRotationRaw(newRotation)
 		bone.update()
 
-		// Constrain relative to the child
-		if (childBone != null && useConstraints && !bone.rotationConstraint.hasTrackerRotation) {
-			val newChildRot = childBone.rotationConstraint.applyConstraint(childBone.getGlobalRotation(), childBone)
-			val correctionInv = (newChildRot * rotation.inv()).inv()
-			bone.setRotationRaw(newRotation * correctionInv)
-			bone.update()
-		}
-
-		return bone.getGlobalRotation()
+		return newRotation
 	}
 }
diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt
@@ -13,12 +13,10 @@ import solarxr_protocol.datatypes.BodyPart
 class IKSolver(private val root: Bone) {
 	companion object {
 		const val TOLERANCE_SQR = 1e-8 // == 0.01 cm
-		const val MAX_ITERATIONS = 100
-		const val ANNEALING_STEP = 20
-		const val ANNEALING_ITERATIONS = 5
-		const val ANNEALING_MAX = 60
+		const val MAX_ITERATIONS = 20
 		const val DAMPENING_FACTOR = 0.5f
 		const val STATIC_DAMPENING = 0.1f
+		const val CORRECTION_FACTOR = 0.1f
 
 		// Short limbs positioned on the end of the skeleton are prone to over rotation
 		val LOCK_ROTATION = setOf(BodyPart.LEFT_HAND, BodyPart.RIGHT_HAND, BodyPart.LEFT_FOOT, BodyPart.RIGHT_FOOT)
@@ -243,12 +241,7 @@ class IKSolver(private val root: Bone) {
 		rootChain?.resetChain()
 		root.update()
 
-		for (i in 0 until MAX_ITERATIONS step ANNEALING_STEP) {
-			solve(ANNEALING_ITERATIONS, (i > ANNEALING_MAX))
-			val solved = solve(ANNEALING_STEP - ANNEALING_ITERATIONS)
-
-			if (solved) break
-		}
+		solve(MAX_ITERATIONS)
 
 		root.update()
 	}