From cac62bd2db54834fe0473e8c8dceb545a8c9f9d3 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 30 Oct 2024 12:44:37 -0700 Subject: [PATCH] Always use constraints in iksolver --- .../slimevr/tracking/processor/Constraint.kt | 6 +++-- .../tracking/processor/skeleton/IKChain.kt | 23 +++++++++---------- .../tracking/processor/skeleton/IKSolver.kt | 12 +++++----- 3 files changed, 21 insertions(+), 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 index 808df90d49..7490c85886 100644 --- 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 index a564347647..b34c930323 100644 --- 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 index 45b9263731..ad18c2e90d 100644 --- 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() {