Update loosening to be more selective and fix chain builder bug

SlimeVR · Mar 26, 2024 · 8ae8b4f · 8ae8b4f
1 parent 165c7c8
commit 8ae8b4f
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 18 deletions.
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
@@ -36,10 +36,10 @@ class HumanSkeleton(
 	// Upper body bones
 	val headBone = Bone(BoneType.HEAD, CompleteConstraint())
 	val neckBone = Bone(BoneType.NECK, CompleteConstraint())
-	val upperChestBone = Bone(BoneType.UPPER_CHEST, CompleteConstraint())
-	val chestBone = Bone(BoneType.CHEST, CompleteConstraint())
-	val waistBone = Bone(BoneType.WAIST, CompleteConstraint())
-	val hipBone = Bone(BoneType.HIP, CompleteConstraint())
+	val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(180f, 180f))
+	val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(180f, 180f))
+	val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(180f, 180f))
+	val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(180f, 180f))
 
 	// Lower body bones
 	val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(180f, 7f))

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
@@ -26,6 +26,7 @@ class IKChain(
 	private var targetSum = Vector3.NULL
 	var target = Vector3.NULL
 	var distToTargetSqr = Float.POSITIVE_INFINITY
+	var loosense = 0
 	private var centroidWeight = 1f
 	private var positions = getPositionList()
 
@@ -108,6 +109,7 @@ class IKChain(
 	fun resetChain() {
 		distToTargetSqr = Float.POSITIVE_INFINITY
 		centroidWeight = 1f
+		loosense = 0
 
 		for (bone in nodes) {
 			bone.rotationConstraint.tolerance = 0.0f
@@ -147,6 +149,7 @@ class IKChain(
 	 * Allow constrained bones to deviate more per step
 	 */
 	fun decreaseConstraints() {
+		loosense++
 		for (bone in nodes) {
 			bone.rotationConstraint.tolerance += IKSolver.TOLERANCE_STEP
 		}

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,7 +13,9 @@ class IKSolver(private val root: Bone) {
 	companion object {
 		const val TOLERANCE_SQR = 1e-8 // == 0.01 cm
 		const val MAX_ITERATIONS = 100
-		const val ITERATIONS_BEFORE_STEP = MAX_ITERATIONS / 8
+		const val ITERATIONS_BEFORE_STEP = 20
+		const val ITERATIONS_BETWEEN_STEP = 5
+		const val MAX_LOOSENS = 5
 		const val TOLERANCE_STEP = 2f
 	}
 
@@ -35,6 +37,7 @@ class IKSolver(private val root: Bone) {
 		// Build the system of chains
 		rootChain = chainBuilder(root, null, 0, positionalConstraints, rotationalConstraints)
 		populateChainList(rootChain!!)
+		addConstraints()
 
 		// Check if there is any constraints (other than the head) in the model
 		rootChain = if (neededChain(rootChain!!)) rootChain else null
@@ -74,15 +77,15 @@ class IKSolver(private val root: Bone) {
 		} else {
 			parent.tailConstraint
 		}
-		var tailConstraint: Tracker? = null
+		var tailConstraint = getConstraint(currentBone, positionalConstraints)
 
 		// Add bones until there is a reason to make a new chain
 		while (currentBone.children.size == 1 && tailConstraint == null) {
-			currentBone = currentBone.children[0]
+			currentBone = currentBone.children.first()
 			currentBone.rotationConstraint.allowModifications =
 				getConstraint(currentBone, rotationalConstraints) == null
 			boneList.add(currentBone)
-			tailConstraint = getConstraint(boneList.last(), positionalConstraints)
+			tailConstraint = getConstraint(currentBone, positionalConstraints)
 		}
 
 		var chain = IKChain(boneList, parent, level, baseConstraint, tailConstraint)
@@ -102,7 +105,7 @@ class IKSolver(private val root: Bone) {
 
 		// If the chain has only one child and no tail constraint combine the chains
 		if (chain.children.size == 1 && chain.tailConstraint == null) {
-			chain = combineChains(chain, chain.children[0])
+			chain = combineChains(chain, chain.children.first())
 		}
 
 		return chain
@@ -137,6 +140,12 @@ class IKSolver(private val root: Bone) {
 		return newChain
 	}
 
+	private fun addConstraints() {
+		fun constrainChain(chain: IKChain) =
+			chain.nodes.forEach { it.rotationConstraint.allowModifications = false }
+		chainList.forEach { if (it.tailConstraint == null) constrainChain(it) }
+	}
+
 	private fun neededChain(chain: IKChain): Boolean {
 		if (chain.tailConstraint != null) {
 			return true
@@ -192,6 +201,24 @@ class IKSolver(private val root: Bone) {
 		return null
 	}
 
+	/**
+	 * Loosen rotational constraints gradually
+	 */
+	private fun loosenConstraints(iter: Int) {
+		if (iter < ITERATIONS_BEFORE_STEP && iter % ITERATIONS_BETWEEN_STEP != 0) return
+
+		var maxDist = Float.NEGATIVE_INFINITY
+		var maxDistChain = chainList.first()
+		for (chain in chainList) {
+			if (chain.distToTargetSqr > maxDist) {
+				maxDist = chain.distToTargetSqr
+				maxDistChain = chain
+			}
+		}
+
+		if (maxDistChain.loosense < MAX_LOOSENS) maxDistChain.decreaseConstraints()
+	}
+
 	fun solve() {
 		if (rootChain == null) return
 
@@ -205,7 +232,6 @@ class IKSolver(private val root: Bone) {
 
 		rootChain?.resetChain()
 
-		// run up to MAX_ITERATIONS per tick
 		for (i in 0 until MAX_ITERATIONS) {
 			for (chain in chainList) {
 				chain.backwards()
@@ -229,14 +255,7 @@ class IKSolver(private val root: Bone) {
 				chain.updateChildCentroidWeight()
 			}
 
-			// Loosen rotational constraints
-			// TODO only do this if a positional tracker down the chain is actually
-			// tracking accurately
-			if (i % ITERATIONS_BEFORE_STEP == 0 && i != 0) {
-				for (chain in chainList) {
-					chain.decreaseConstraints()
-				}
-			}
+			loosenConstraints(i)
 		}
 
 		root.update()