Always use constraints in iksolver

SlimeVR · Oct 30, 2024 · 3701865 · 3701865
1 parent da67cd8
commit 3701865
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 20 deletions.
diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt
@@ -18,11 +18,13 @@ class Constraint(
 	twist: Float = 0.0f,
 	swing: Float = 0.0f,
 	allowedDeviation: Float = 0f,
+	maxDeviationFromTracker: Float = 15f,
 ) {
 	private val constraintFunction = constraintTypeToFunc(constraintType)
 	private val twistRad = Math.toRadians(twist.toDouble()).toFloat()
 	private val swingRad = Math.toRadians(swing.toDouble()).toFloat()
 	private val allowedDeviationRad = Math.toRadians(allowedDeviation.toDouble()).toFloat()
+	private val maxDeviationFromTrackerRad = Math.toRadians(maxDeviationFromTracker.toDouble()).toFloat()
 	var hasTrackerRotation = false
 
 	/**
@@ -45,8 +47,8 @@ class Constraint(
 	fun constrainToInitialRotation(rotation: Quaternion): Quaternion {
 		val rotationLocal = rotation * initialRotation.inv()
 		var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y)
-		swingQ = constrain(swingQ, allowedDeviationRad)
-		twistQ = constrain(twistQ, allowedDeviationRad)
+		swingQ = constrain(swingQ, maxDeviationFromTrackerRad)
+		twistQ = constrain(twistQ, maxDeviationFromTrackerRad)
 		return initialRotation * (swingQ * twistQ)
 	}
 

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
@@ -5,7 +5,7 @@ import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType
 import dev.slimevr.tracking.trackers.Tracker
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Vector3
-import kotlin.math.pow
+import kotlin.math.*
 
 /*
  * This class implements a chain of Bones
@@ -46,7 +46,7 @@ class IKChain(
 		}
 	}
 
-	fun backwardsCCDIK(useConstraints: Boolean) {
+	fun backwardsCCDIK() {
 		target = computedTailPosition?.getPosition() ?: getChildTargetAvg()
 		var offset = Vector3.NULL
 
@@ -60,17 +60,16 @@ class IKChain(
 			// Compute the axis of rotation and angle for this bone
 			var scalar = IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.hasTrackerRotation) IKSolver.STATIC_DAMPENING else 1f
 			scalar *= ((bones.size - i).toFloat() / bones.size).pow(IKSolver.ANNEALING_EXPONENT)
-			var adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit()
+			val adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit()
 
-			// Bones that are not supposed to be modified should tend towards their origin
 			val rotation = currentBone.getGlobalRotation()
+			var correctedRot = (adjustment * rotation).unit()
+
+			// Bones that are not supposed to be modified should tend towards their origin
 			if (!currentBone.rotationConstraint.allowModifications) {
-				adjustment *= rotation.interpR(currentBone.rotationConstraint.initialRotation, IKSolver.CORRECTION_FACTOR) * rotation.inv()
+				correctedRot = correctedRot.interpR(currentBone.rotationConstraint.initialRotation, IKSolver.CORRECTION_FACTOR)
 			}
-
-			val correctedRot = (adjustment * rotation).unit()
-
-			rotations[i] = setBoneRotation(currentBone, correctedRot, useConstraints)
+			rotations[i] = setBoneRotation(currentBone, correctedRot)
 
 			if (currentBone.rotationConstraint.hasTrackerRotation) {
 				offset += rotations[i].sandwich(Vector3.NEG_Y) * currentBone.length
@@ -142,14 +141,14 @@ class IKChain(
 	 * to the bone's rotational constraint
 	 * returns the constrained rotation
 	 */
-	private fun setBoneRotation(bone: Bone, rotation: Quaternion, useConstraints: Boolean): Quaternion {
+	private fun setBoneRotation(bone: Bone, rotation: Quaternion): Quaternion {
 		// Constrain relative to the parent
 		val newRotation = if (bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) {
 			bone.rotationConstraint.applyConstraint(rotation, bone)
-		} else if (useConstraints && !bone.rotationConstraint.hasTrackerRotation) {
+		} else if (!bone.rotationConstraint.hasTrackerRotation) {
 			bone.rotationConstraint.applyConstraint(rotation, bone)
 		} else {
-			rotation
+			bone.rotationConstraint.constrainToInitialRotation(rotation)
 		}
 
 		bone.setRotationRaw(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
@@ -201,11 +201,11 @@ class IKSolver(private val root: Bone) {
 		return null
 	}
 
-	private fun solve(iterations: Int, useConstraints: Boolean = true): Boolean {
-		var solved: Boolean
+	private fun solve(iterations: Int): Boolean {
+		var solved = false
 		for (i in 0..iterations) {
 			for (chain in chainList) {
-				chain.backwardsCCDIK(useConstraints)
+				chain.backwardsCCDIK()
 			}
 
 			rootChain?.computeTargetDistance()
@@ -215,14 +215,14 @@ class IKSolver(private val root: Bone) {
 			for (chain in chainList) {
 				if (chain.distToTargetSqr > TOLERANCE_SQR) {
 					solved = false
+					break
 				}
 			}
 
-			// Terminate if using constraints and the chain is solved
-			if (solved && useConstraints) return true
+			if (solved) break
 		}
 
-		return false
+		return solved
 	}
 
 	fun solve() {