fix bug where a chain would jitter if in reach of the target but unab…

…le to solve due to join restrictions

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt

@@ -17,12 +17,15 @@ class IKChain(
 ) {
 	companion object {
 		const val CENTROID_PULL_ADJUSTMENT = 0.1f
+		const val CENTROID_RECOVERY = 0.01f
 	}
 
 	// state variables
 	var children = mutableListOf<IKChain>()
 	var target = Vector3.NULL
 	var distToTargetSqr = Float.POSITIVE_INFINITY
+	var trySolve = true
+	var solved = true
 	private var centroidWeight = 1f
 	private var positions = getPositionList()
 	private var tailConstrainPosOffset = Vector3.NULL
@@ -39,6 +42,8 @@ class IKChain(
 	}
 
 	fun backwards() {
+		if (!trySolve) return
+
 		// start at the constraint or the centroid of the children
 		if (tailConstraint == null && children.size > 1) {
 			target /= getChildrenCentroidWeightSum()
@@ -57,11 +62,14 @@ class IKChain(
 			positions[i] = positions[i + 1] + (direction * nodes[i].length)
 		}
 
-		if (parent != null && parent!!.tailConstraint == null)
+		if (parent != null && parent!!.tailConstraint == null) {
 			parent!!.target += positions[0] * centroidWeight
+		}
 	}
 
 	private fun forwards() {
+		if (!trySolve) return
+
 		if (parent != null) {
 			positions[0] = parent!!.positions.last()
 		} else if (baseConstraint != null) {
@@ -104,9 +112,8 @@ class IKChain(
 	}
 
 	fun resetChain() {
-		centroidWeight = 1f
 		distToTargetSqr = Float.POSITIVE_INFINITY
-		target = Vector3.NULL
+		trySolve = true
 
 		for (child in children) {
 			child.resetChain()
@@ -134,6 +141,11 @@ class IKChain(
 			if (child.distToTargetSqr < closestToSolved.distToTargetSqr) {
 				closestToSolved = child
 			}
+			child.centroidWeight = if (centroidWeight < 1f) {
+				child.centroidWeight + CENTROID_RECOVERY
+			} else {
+				child.centroidWeight
+			}
 		}
 
 		if (closestToSolved.centroidWeight > CENTROID_PULL_ADJUSTMENT) {

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt

@@ -11,7 +11,7 @@ import dev.slimevr.tracking.trackers.Tracker
 
 class IKSolver(private val root: Bone) {
 	companion object {
-		private const val TOLERANCE_SQR = 1e-8 // == 0.0001 cm
+		private const val TOLERANCE_SQR = 1e-8 // == 0.01 cm
 		private const val MAX_ITERATIONS = 100
 	}
 
@@ -26,15 +26,15 @@ class IKSolver(private val root: Bone) {
 	fun buildChains(trackers: List<Tracker>) {
 		chainList = mutableListOf()
 
-		// extract the positional constraints
+		// Extract the positional constraints
 		val positionalConstraints = extractPositionalConstraints(trackers)
 		val rotationalConstraints = extractRotationalConstraints(trackers)
 
-		// build the system of chains
+		// Build the system of chains
 		rootChain = chainBuilder(root, null, 0, positionalConstraints, rotationalConstraints)
 		populateChainList(rootChain!!)
 
-		// check if there is any constraints (other than the head) in the model
+		// Check if there is any constraints (other than the head) in the model
 		rootChain = if (neededChain(rootChain!!)) rootChain else null
 		chainList.sortBy { -it.level }
 	}
@@ -66,15 +66,15 @@ class IKSolver(private val root: Bone) {
 			getConstraint(currentBone, rotationalConstraints) == null
 		boneList.add(currentBone)
 
-		// get constraints
+		// Get constraints
 		val baseConstraint = if (parent == null) {
 			getConstraint(boneList.first(), positionalConstraints)
 		} else {
 			parent.tailConstraint
 		}
 		var tailConstraint: Tracker? = null
 
-		// add bones until there is a reason to make a new chain
+		// Add bones until there is a reason to make a new chain
 		while (currentBone.children.size == 1 && tailConstraint == null) {
 			currentBone = currentBone.children[0]
 			currentBone.rotationConstraint.allowModifications =
@@ -88,7 +88,7 @@ class IKSolver(private val root: Bone) {
 		if (currentBone.children.isNotEmpty()) {
 			val childrenList = mutableListOf<IKChain>()
 
-			// build child chains
+			// Build child chains
 			for (child in currentBone.children) {
 				val childChain = chainBuilder(child, chain, level + 1, positionalConstraints, rotationalConstraints)
 				if (neededChain(childChain)) {
@@ -98,7 +98,7 @@ class IKSolver(private val root: Bone) {
 			chain.children = childrenList
 		}
 
-		// if the chain has only one child and no tail constraint combine the chains
+		// 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])
 		}
@@ -191,6 +191,7 @@ class IKSolver(private val root: Bone) {
 	}
 
 	fun solve() {
+		var solved: Boolean
 		if (needsReset) {
 			for (c in chainList) {
 				c.resetTrackerOffsets()
@@ -200,33 +201,42 @@ class IKSolver(private val root: Bone) {
 
 		rootChain?.resetChain()
 
-		// run up to MAX_ITERATIONS iterations per tick
-		for (i in 0..MAX_ITERATIONS) {
+		// run up to MAX_ITERATIONS per tick
+		for (i in 0 until MAX_ITERATIONS) {
 			for (chain in chainList) {
 				chain.backwards()
 			}
 			rootChain?.forwardsMulti()
 
 			rootChain?.computeTargetDistance()
 
-			// if all chains have reached their target the chain is solved
-			var solved = true
+			// Stop solving chains after one iteration for chains that
+			// failed to solve last tick this prevents some extreme jitter
+			if (i == 0) {
+				for (chain in chainList) {
+					chain.trySolve = chain.solved
+				}
+			}
+
+			// If all chains have reached their target the chain is solved
+			solved = true
 			for (chain in chainList) {
-				if (chain.distToTargetSqr > TOLERANCE_SQR) {
+				if (chain.distToTargetSqr > TOLERANCE_SQR && chain.trySolve) {
+					chain.solved = false
 					solved = false
-					break
+				} else if (chain.distToTargetSqr <= TOLERANCE_SQR) {
+					chain.solved = true
 				}
 			}
 
 			if (solved) break
 
-			// help the chains out of a deadlock
+			// Help the chains out of a deadlock
 			for (chain in chainList) {
 				chain.updateChildCentroidWeight()
 			}
 		}
 
-		// update transforms
 		root.update()
 	}
 }