Enforce constraints bidirectionally

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

@@ -20,10 +20,10 @@ class Constraint(
 	private val constraintFunction = constraintTypeToFunc(constraintType)
 	private val twistRad = Math.toRadians(twist.toDouble()).toFloat()
 	private val swingRad = Math.toRadians(swing.toDouble()).toFloat()
+	var hasTrackerRotation = false
 
 	/**
-	 * If false don't allow the rotation of the bone
-	 * to be modified except to satisfy a constraint
+	 * If false solve with minimal movement applied to this link
 	 */
 	var allowModifications = true
 

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.allowModifications && bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE) {
+			if (!bones[i].rotationConstraint.hasTrackerRotation && bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE) {
 				bones[i].setRotationRaw(rotations[i])
 			}
 		}
@@ -51,6 +51,7 @@ 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
 
 			// Get the local position of the end effector and the target relative to the current node
 			val endEffectorLocal = (getEndEffectorsAvg() - currentBone.getPosition()).unit()
@@ -67,7 +68,7 @@ class IKChain(
 			val adjustment = Quaternion(cos(angle / 2), cross * sinHalfAngle)
 			val correctedRot = (adjustment * currentBone.getGlobalRotation()).unit()
 
-			rotations[i] = setBoneRotation(currentBone, correctedRot, useConstraints)
+			rotations[i] = setBoneRotation(currentBone, childBone, correctedRot, useConstraints)
 		}
 	}
 
@@ -130,15 +131,25 @@ class IKChain(
 	 * vector with the bone's rotational constraint
 	 * returns the constrained rotation
 	 */
-	private fun setBoneRotation(bone: Bone, rotation: Quaternion, useConstraints: Boolean): Quaternion {
-		val newRotation = if (useConstraints || bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) {
+	private fun setBoneRotation(bone: Bone, childBone: Bone?, rotation: Quaternion, useConstraints: Boolean): Quaternion {
+		// Constrain relative to the parent
+		val newRotation = if ((useConstraints || bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) && !bone.rotationConstraint.hasTrackerRotation) {
 			bone.rotationConstraint.applyConstraint(rotation, bone)
 		} else {
 			rotation
 		}
+
 		bone.setRotationRaw(newRotation)
 		bone.update()
 
-		return newRotation
+		// 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()
 	}
 }

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

@@ -3,6 +3,7 @@ package dev.slimevr.tracking.processor.skeleton
 import dev.slimevr.tracking.trackers.Tracker
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Vector3
+import solarxr_protocol.datatypes.BodyPart
 
 class IKConstraint(val tracker: Tracker) {
 	private var offset = Vector3.NULL
@@ -12,7 +13,10 @@ class IKConstraint(val tracker: Tracker) {
 		tracker.position + (tracker.getRotation() * rotationOffset).sandwich(offset)
 
 	fun reset(nodePosition: Vector3) {
-		offset = nodePosition - tracker.position
+		val bodyPartsToSkip = setOf(BodyPart.LEFT_HAND, BodyPart.RIGHT_HAND)
+
 		rotationOffset = tracker.getRotation().inv()
+		if (tracker.trackerPosition?.bodyPart in bodyPartsToSkip) return
+		offset = nodePosition - tracker.position
 	}
 }

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

@@ -17,7 +17,7 @@ class IKSolver(private val root: Bone) {
 		const val ANNEALING_ITERATIONS = 5
 		const val ANNEALING_MAX = 60
 		const val DAMPENING_FACTOR = 0.5f
-		const val STATIC_DAMPENING = 0.25f
+		const val STATIC_DAMPENING = 0.1f
 	}
 
 	var enabled = true
@@ -66,8 +66,9 @@ class IKSolver(private val root: Bone) {
 	): IKChain {
 		val boneList = mutableListOf<Bone>()
 		var currentBone = root
-		currentBone.rotationConstraint.allowModifications =
-			getConstraint(currentBone, rotationalConstraints) == null
+		var constraint = getConstraint(currentBone, rotationalConstraints)
+		currentBone.rotationConstraint.allowModifications = constraint == null
+		currentBone.rotationConstraint.hasTrackerRotation = constraint != null
 		boneList.add(currentBone)
 
 		// Get constraints
@@ -81,8 +82,9 @@ class IKSolver(private val root: Bone) {
 		// Add bones until there is a reason to make a new chain
 		while (currentBone.children.size == 1 && tailConstraint == null) {
 			currentBone = currentBone.children.first()
-			currentBone.rotationConstraint.allowModifications =
-				getConstraint(currentBone, rotationalConstraints) == null
+			constraint = getConstraint(currentBone, rotationalConstraints)
+			currentBone.rotationConstraint.allowModifications = constraint == null
+			currentBone.rotationConstraint.hasTrackerRotation = constraint != null
 			boneList.add(currentBone)
 			tailConstraint = getConstraint(currentBone, positionalConstraints)
 		}