Allow deviation from rotational constraints if the chains cannot solve

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

@@ -1,5 +1,6 @@
 package dev.slimevr.tracking.processor
 
+import com.jme3.math.FastMath
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Vector3
 
@@ -13,6 +14,23 @@ abstract class Constraint {
 	 */
 	var allowModifications = true
 
+	/**
+	 * If allowModifications is false this is the
+	 * allowed deviation from the original rotation
+	 */
+	var tolerance = 0.0f
+		set(value) {
+			field = value
+			updateComposedField()
+		}
+
+	private var toleranceRad = 0.0f
+	var originalRotation = Quaternion.NULL
+
+	private fun updateComposedField() {
+		toleranceRad = Math.toRadians(tolerance.toDouble()).toFloat()
+	}
+
 	/**
 	 * Apply rotational constraints to the direction vector
 	 */
@@ -24,11 +42,12 @@ abstract class Constraint {
 	 */
 	fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion {
 		val rotation = Quaternion.fromTo(Vector3.NEG_Y, direction)
-		if (!allowModifications) {
-			return constraintRotation(thisBone.getGlobalRotation(), thisBone)
+		if (allowModifications) {
+			return constraintRotation(rotation, thisBone)
 		}
 
-		return constraintRotation(rotation, thisBone)
+		val constrainedRotation = applyLimits(rotation, originalRotation)
+		return constraintRotation(constrainedRotation, thisBone)
 	}
 
 	/**
@@ -40,4 +59,52 @@ abstract class Constraint {
 	fun applyConstraintInverse(direction: Vector3, thisBone: Bone): Vector3 {
 		return -applyConstraint(-direction, thisBone).sandwich(Vector3.NEG_Y)
 	}
+
+	/**
+	 * Limit the rotation to tolerance away from the initialRotation
+	 */
+	protected fun applyLimits(
+		rotation: Quaternion,
+		initialRotation: Quaternion,
+	): Quaternion {
+		val localRotation = initialRotation.inv() * rotation
+
+		var (swingQ, twistQ) = decompose(localRotation, Vector3.NEG_Y)
+
+		twistQ = constrain(twistQ, toleranceRad)
+		swingQ = constrain(swingQ, toleranceRad)
+
+		return initialRotation * (swingQ * twistQ)
+	}
+
+	protected fun decompose(
+		rotation: Quaternion,
+		twistAxis: Vector3,
+	): Pair<Quaternion, Quaternion> {
+		val projection = rotation.project(twistAxis)
+
+		val twist = Quaternion(rotation.w, projection.xyz).unit()
+		val swing = rotation * twist.inv()
+
+		return Pair(swing, twist)
+	}
+
+	protected fun constrain(rotation: Quaternion, angle: Float): Quaternion {
+		val magnitude = FastMath.sin(angle * 0.5f)
+		val magnitudeSqr = magnitude * magnitude
+		var vector = rotation.xyz
+		var rot = rotation
+
+		if (vector.lenSq() > magnitudeSqr) {
+			vector = vector.unit() * magnitude
+			rot = Quaternion(
+				FastMath.sqrt(1.0f - magnitudeSqr) * FastMath.sign(rot.w),
+				vector.x,
+				vector.y,
+				vector.z
+			)
+		}
+
+		return rot
+	}
 }

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

@@ -1,6 +1,5 @@
 package dev.slimevr.tracking.processor
 
-import com.jme3.math.FastMath
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Vector3
 
@@ -12,34 +11,6 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) :
 	private val twistRad = Math.toRadians(twist.toDouble()).toFloat()
 	private val swingRad = Math.toRadians(swing.toDouble()).toFloat()
 
-	private fun decompose(rotation: Quaternion, twistAxis: Vector3): Pair<Quaternion, Quaternion> {
-		val projection = rotation.project(twistAxis)
-
-		val twist = Quaternion(rotation.w, projection.xyz).unit()
-		val swing = rotation * twist.inv()
-
-		return Pair(swing, twist)
-	}
-
-	private fun constrain(rotation: Quaternion, angle: Float): Quaternion {
-		val magnitude = FastMath.sin(angle * 0.5f)
-		val magnitudeSqr = magnitude * magnitude
-		var vector = rotation.xyz
-		var rot = rotation
-
-		if (vector.lenSq() > magnitudeSqr) {
-			vector = vector.unit() * magnitude
-			rot = Quaternion(
-				FastMath.sqrt(1.0f - magnitudeSqr) * FastMath.sign(rot.w),
-				vector.x,
-				vector.y,
-				vector.z
-			)
-		}
-
-		return rot
-	}
-
 	override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion {
 		// if there is no parent or no constraint return the direction
 		if (thisBone.parent == null || (swing.isNaN() && twist.isNaN())) {

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

@@ -20,13 +20,12 @@ class IKChain(
 	}
 
 	// State variables
-	val computedBasePosition = baseConstraint?.let { IKConstraint(it) }
-	val computedTailPosition = tailConstraint?.let { IKConstraint(it) }
+	private val computedBasePosition = baseConstraint?.let { IKConstraint(it) }
+	private val computedTailPosition = tailConstraint?.let { IKConstraint(it) }
 	var children = mutableListOf<IKChain>()
+	private var targetSum = Vector3.NULL
 	var target = Vector3.NULL
 	var distToTargetSqr = Float.POSITIVE_INFINITY
-	var trySolve = true
-	var solved = true
 	private var centroidWeight = 1f
 	private var positions = getPositionList()
 
@@ -41,13 +40,11 @@ class IKChain(
 	}
 
 	fun backwards() {
-		if (!trySolve) return
-
 		// Start at the constraint or the centroid of the children
-		if (computedTailPosition == null && children.size > 1) {
-			target /= getChildrenCentroidWeightSum()
+		target = if (computedTailPosition == null && children.size > 1) {
+			targetSum / getChildrenCentroidWeightSum()
 		} else {
-			target = (computedTailPosition?.getPosition()) ?: Vector3.NULL
+			(computedTailPosition?.getPosition()) ?: Vector3.NULL
 		}
 
 		// Set the end node to target
@@ -62,17 +59,15 @@ class IKChain(
 		}
 
 		if (parent != null && parent!!.computedTailPosition == null) {
-			parent!!.target += positions[0] * centroidWeight
+			parent!!.targetSum += positions[0] * centroidWeight
 		}
 	}
 
 	private fun forwards() {
-		if (!trySolve) return
-
-		if (parent != null) {
-			positions[0] = parent!!.positions.last()
-		} else if (computedBasePosition != null) {
-			positions[0] = computedBasePosition.getPosition()
+		positions[0] = if (parent != null) {
+			parent!!.nodes.last().getTailPosition()
+		} else {
+			(computedBasePosition?.getPosition()) ?: positions[0]
 		}
 
 		for (i in 1 until positions.size - 1) {
@@ -87,7 +82,7 @@ class IKChain(
 		positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length)
 
 		// reset sub-base target
-		target = Vector3.NULL
+		targetSum = Vector3.NULL
 	}
 
 	/**
@@ -113,7 +108,11 @@ class IKChain(
 	fun resetChain() {
 		distToTargetSqr = Float.POSITIVE_INFINITY
 		centroidWeight = 1f
-		trySolve = true
+
+		for (bone in nodes) {
+			bone.rotationConstraint.tolerance = 0.0f
+			bone.rotationConstraint.originalRotation = bone.getGlobalRotation()
+		}
 
 		for (child in children) {
 			child.resetChain()
@@ -144,6 +143,15 @@ class IKChain(
 		}
 	}
 
+	/**
+	 * Allow constrained bones to deviate more per step
+	 */
+	fun decreaseConstraints() {
+		for (bone in nodes) {
+			bone.rotationConstraint.tolerance += IKSolver.TOLERANCE_STEP
+		}
+	}
+
 	/**
 	 * Updates the distance to target and other fields
 	 * Call on the root chain only returns the sum of the
@@ -170,7 +178,7 @@ class IKChain(
 		bone.setRotationRaw(rotation)
 
 		// TODO optimize (this is required to update the world translation for the next bone as it uses the world
-		//  rotation of the parent)
+		//  rotation of the parent. We only need to update this bones translation though so this is very wasteful)
 		bone.update()
 
 		return rotation.sandwich(Vector3.NEG_Y)

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

@@ -12,7 +12,9 @@ import dev.slimevr.tracking.trackers.Tracker
 class IKSolver(private val root: Bone) {
 	companion object {
 		const val TOLERANCE_SQR = 1e-8 // == 0.01 cm
-		private const val MAX_ITERATIONS = 100
+		const val MAX_ITERATIONS = 100
+		const val ITERATIONS_BEFORE_STEP = MAX_ITERATIONS / 8
+		const val TOLERANCE_STEP = 2f
 	}
 
 	private var chainList = mutableListOf<IKChain>()
@@ -24,7 +26,7 @@ class IKSolver(private val root: Bone) {
 	 * should be rebuilt.
 	 */
 	fun buildChains(trackers: List<Tracker>) {
-		chainList = mutableListOf()
+		chainList.clear()
 
 		// Extract the positional constraints
 		val positionalConstraints = extractPositionalConstraints(trackers)
@@ -191,6 +193,8 @@ class IKSolver(private val root: Bone) {
 	}
 
 	fun solve() {
+		if (rootChain == null) return
+
 		var solved: Boolean
 		if (needsReset) {
 			for (c in chainList) {
@@ -210,22 +214,11 @@ class IKSolver(private val root: Bone) {
 
 			rootChain?.computeTargetDistance()
 
-			// 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 && chain.trySolve) {
-					chain.solved = false
+				if (chain.distToTargetSqr > TOLERANCE_SQR) {
 					solved = false
-				} else if (chain.distToTargetSqr <= TOLERANCE_SQR) {
-					chain.solved = true
 				}
 			}
 
@@ -235,6 +228,15 @@ class IKSolver(private val root: Bone) {
 			for (chain in chainList) {
 				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()
+				}
+			}
 		}
 
 		root.update()