From 1dafe6cbcd8229d4090f77d502f2f1f8c3fa465c Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 10 Jul 2024 20:23:48 -0700 Subject: [PATCH] switch from FABRIK to CCDIK --- .../slimevr/tracking/processor/Constraint.kt | 86 +++++++++---- .../processor/skeleton/HumanSkeleton.kt | 13 +- .../tracking/processor/skeleton/IKChain.kt | 116 +++++++----------- .../tracking/processor/skeleton/IKSolver.kt | 22 ++-- 4 files changed, 117 insertions(+), 120 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 c4d82b267c..dd89fcacd0 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 @@ -2,6 +2,7 @@ package dev.slimevr.tracking.processor import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 +import kotlin.math.abs import kotlin.math.sign import kotlin.math.sin import kotlin.math.sqrt @@ -49,8 +50,7 @@ class Constraint( * Apply rotational constraints and if applicable force the rotation * to be unchanged unless it violates the constraints */ - fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { - val rotation = Quaternion.fromTo(Vector3.NEG_Y, direction) + fun applyConstraint(rotation: Quaternion, thisBone: Bone): Quaternion { if (allowModifications) { return constraintFunction(rotation, thisBone, swingRad, twistRad).unit() } @@ -59,15 +59,6 @@ class Constraint( return constraintFunction(constrainedRotation, thisBone, swingRad, twistRad).unit() } - /** - * Apply constraints to the direction vector such that when - * inverted the vector would be within the constraints. - * This is used for constraining direction vectors on the backwards pass - * of the FABRIK solver. - */ - fun applyConstraintInverse(direction: Vector3, thisBone: Bone): Vector3 = - -applyConstraint(-direction, thisBone).sandwich(Vector3.NEG_Y) - /** * Limit the rotation to tolerance away from the initialRotation */ @@ -86,7 +77,7 @@ class Constraint( } companion object { - fun decompose( + private fun decompose( rotation: Quaternion, twistAxis: Vector3, ): Pair { @@ -98,7 +89,7 @@ class Constraint( return Pair(swing, twist) } - fun constrain(rotation: Quaternion, angle: Float): Quaternion { + private fun constrain(rotation: Quaternion, angle: Float): Quaternion { val magnitude = sin(angle * 0.5f) val magnitudeSqr = magnitude * magnitude val sign = if (rotation.w != 0f) sign(rotation.w) else 1f @@ -118,22 +109,65 @@ class Constraint( return rot } - // Constraint function for TwistSwingConstraint - val twistSwingConstraint: ConstraintFunction = { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float -> - if (thisBone.parent == null) { - rotation - } else { - val parent = thisBone.parent!! - val rotationLocal = (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation - var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) - - swingQ = constrain(swingQ, swingRad) - twistQ = constrain(twistQ, twistRad) - - parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ) + private fun constrain(rotation: Quaternion, minAngle: Float, maxAngle: Float, axis: Vector3): Quaternion { + val magnitudeMin = sin(minAngle * 0.5f) + val magnitudeMax = sin(maxAngle * 0.5f) + val magnitudeSqrMin = magnitudeMin * magnitudeMin * if (minAngle != 0f) sign(minAngle) else 1f + val magnitudeSqrMax = magnitudeMax * magnitudeMax * if (maxAngle != 0f) sign(maxAngle) else 1f + var vector = rotation.xyz + var rot = rotation + + val rotMagnitude = vector.lenSq() * if (vector.dot(axis) < 0) -1f else 1f + if (rotMagnitude < magnitudeSqrMin || rotMagnitude > magnitudeSqrMax) { + val magnitude = if (rotMagnitude < magnitudeSqrMin) magnitudeMin else magnitudeMax + val magnitudeSqr = abs(if (rotMagnitude < magnitudeSqrMin) magnitudeSqrMin else magnitudeSqrMax) + vector = vector.unit() * magnitude + rot = Quaternion( + sqrt(1.0f - magnitudeSqr), + vector.x, + vector.y, + vector.z, + ) } + + return rot } + // Constraint function for TwistSwingConstraint + val twistSwingConstraint: ConstraintFunction = + { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float -> + if (thisBone.parent == null) { + rotation + } else { + val parent = thisBone.parent!! + val rotationLocal = + (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation + var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) + + swingQ = constrain(swingQ, swingRad) + twistQ = constrain(twistQ, twistRad) + + parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ) + } + } + + // Constraint function for a hinge constraint with min and max angles + val hingeConstraint: ConstraintFunction = + { rotation: Quaternion, thisBone: Bone, min: Float, max: Float -> + if (thisBone.parent == null) { + rotation + } else { + val parent = thisBone.parent!! + val rotationLocal = + (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation + var (_, hingeAxisRot) = decompose(rotationLocal, Vector3.NEG_X) + + hingeAxisRot = constrain(hingeAxisRot, min, max, Vector3.NEG_X) + + parent.getGlobalRotation() * thisBone.rotationOffset * hingeAxisRot + } + } + // Constraint function for CompleteConstraint val completeConstraint: ConstraintFunction = { _: Quaternion, thisBone: Bone, _: Float, _: Float -> thisBone.getGlobalRotation() diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index ef432bd4c3..1b79b41226 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -6,6 +6,7 @@ import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.processor.BoneType import dev.slimevr.tracking.processor.Constraint import dev.slimevr.tracking.processor.Constraint.Companion.completeConstraint +import dev.slimevr.tracking.processor.Constraint.Companion.hingeConstraint import dev.slimevr.tracking.processor.Constraint.Companion.twistSwingConstraint import dev.slimevr.tracking.processor.HumanPoseManager import dev.slimevr.tracking.processor.config.SkeletonConfigToggles @@ -43,12 +44,12 @@ class HumanSkeleton( val hipBone = Bone(BoneType.HIP, Constraint(twistSwingConstraint, 10f, 80f)) // Lower body bones - val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(completeConstraint)) - val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(completeConstraint)) + val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(twistSwingConstraint, 180f, 8f)) + val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(twistSwingConstraint, 180f, 8f)) val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(twistSwingConstraint, 180f, 120f)) val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(twistSwingConstraint, 180f, 120f)) - val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(twistSwingConstraint, 110f, 180f)) - val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(twistSwingConstraint, 110f, 180f)) + val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(hingeConstraint, 180f, 0f)) + val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(hingeConstraint, 180f, 0f)) val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(twistSwingConstraint, 180f, 120f)) val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(twistSwingConstraint, 180f, 120f)) @@ -57,8 +58,8 @@ class HumanSkeleton( val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(twistSwingConstraint, 180f, 8f)) val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, Constraint(twistSwingConstraint, 180f, 180f)) val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, Constraint(twistSwingConstraint, 180f, 180f)) - val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(twistSwingConstraint, 180f, 180f)) - val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(twistSwingConstraint, 180f, 180f)) + val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(hingeConstraint, 0f, -180f)) + val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(hingeConstraint, 0f, -180f)) val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(twistSwingConstraint, 180f, 180f)) val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(twistSwingConstraint, 180f, 180f)) 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 72c39c7150..33d5b335c0 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 @@ -2,14 +2,19 @@ package dev.slimevr.tracking.processor.skeleton import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.trackers.Tracker +import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 +import kotlin.math.acos +import kotlin.math.cos +import kotlin.math.sign +import kotlin.math.sin /* - * This class implements a chain of Bones for use by the FABRIK solver + * This class implements a chain of Bones */ class IKChain( - val nodes: MutableList, + val bones: MutableList, var parent: IKChain?, val level: Int, val baseConstraint: Tracker?, @@ -23,82 +28,46 @@ class IKChain( private val computedBasePosition = baseConstraint?.let { IKConstraint(it) } private val computedTailPosition = tailConstraint?.let { IKConstraint(it) } var children = mutableListOf() - private var targetSum = Vector3.NULL var target = Vector3.NULL var distToTargetSqr = Float.POSITIVE_INFINITY var loosens = 0 + var locked = false private var centroidWeight = 1f - private var positions = getPositionList() - - private fun getPositionList(): MutableList { - val posList = mutableListOf() - for (n in nodes) { - posList.add(n.getPosition()) - } - posList.add(nodes.last().getTailPosition()) - - return posList - } fun backwards() { // Start at the constraint or the centroid of the children - //target = computedTailPosition?.getPosition() ?: nodes.last().getTailPosition() - target = computedTailPosition?.getPosition() ?: (targetSum / getChildrenCentroidWeightSum()) - - positions[positions.size - 1] = target + target = computedTailPosition?.getPosition() ?: getWeightedChildTarget() - for (i in positions.size - 2 downTo 0) { - var direction = (positions[i] - positions[i + 1]).unit() - direction = nodes[i].rotationConstraint - .applyConstraintInverse(direction, nodes[i]) + for (i in bones.size - 1 downTo 0) { + val currentBone = bones[i] - positions[i] = positions[i + 1] + (direction * nodes[i].length) - } + // Get the local position of the end effector and the target relative to the current node + val endEffectorLocal = (bones.last().getTailPosition() - currentBone.getPosition()).unit() + val targetLocal = (target - currentBone.getPosition()).unit() - if (parent != null && parent!!.computedTailPosition == null) { - parent!!.targetSum += positions[0] * centroidWeight - } - } + // Compute the axis of rotation and angle for this bone + val cross = endEffectorLocal.cross(targetLocal).unit() + if (cross.lenSq() == 0.0f) continue + val baseAngle = acos(endEffectorLocal.dot(targetLocal).coerceIn(-1.0f, 1.0f)) + val angle = baseAngle * sign(cross.dot(cross)) - private fun forwards() { - positions[0] = if (parent != null) { - parent!!.nodes.last().getTailPosition() - } else { - (computedBasePosition?.getPosition()) ?: positions[0] - } + val sinHalfAngle = sin(angle / 2) + val adjustment = Quaternion(cos(angle / 2), cross.x * sinHalfAngle, cross.y * sinHalfAngle, cross.z * sinHalfAngle) + val correctedRot = (adjustment * currentBone.getGlobalRotation()).unit() - for (i in 1 until positions.size - 1) { - var direction = (positions[i] - positions[i - 1]).unit() - direction = setBoneRotation(nodes[i - 1], direction) - positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) + setBoneRotation(currentBone, correctedRot) } - - var direction = (target - positions[positions.size - 2]).unit() - direction = setBoneRotation(nodes.last(), direction) - positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length) - - // reset sub-base target - targetSum = Vector3.NULL } - /** - * Run the forward pass - */ - fun forwardsMulti() { - forwards() - - for (c in children) { - c.forwardsMulti() - } - } - - private fun getChildrenCentroidWeightSum(): Float { - var sum = 0.0f + private fun getWeightedChildTarget(): Vector3 { + var weightSum = 0.0f + var sum = Vector3.NULL for (child in children) { - sum += child.centroidWeight + weightSum += child.centroidWeight + sum += child.target } - return sum + return sum / weightSum } /** @@ -108,7 +77,7 @@ class IKChain( distToTargetSqr = Float.POSITIVE_INFINITY centroidWeight = 1f - for (bone in nodes) { + for (bone in bones) { if (loosens > 0) bone.rotationConstraint.tolerance -= IKSolver.TOLERANCE_STEP bone.rotationConstraint.originalRotation = bone.getGlobalRotation() } @@ -120,8 +89,8 @@ class IKChain( } fun resetTrackerOffsets() { - computedTailPosition?.reset(nodes.last().getTailPosition()) - computedBasePosition?.reset(nodes.first().getPosition()) + computedTailPosition?.reset(bones.last().getTailPosition()) + computedBasePosition?.reset(bones.first().getPosition()) } /** @@ -147,20 +116,20 @@ class IKChain( * Allow constrained bones to deviate more */ fun decreaseConstraints() { + if (locked) return loosens++ - for (bone in nodes) { + for (bone in bones) { 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 - * distances + * Call on the root chain */ fun computeTargetDistance() { distToTargetSqr = if (computedTailPosition != null) { - (positions.last() - computedTailPosition.getPosition()).lenSq() + (bones.last().getTailPosition() - computedTailPosition.getPosition()).lenSq() } else { 0.0f } @@ -173,14 +142,13 @@ class IKChain( /** * Sets a bones rotation from a rotation vector after constraining the rotation * vector with the bone's rotational constraint - * returns the constrained rotation as a vector + * returns the constrained rotation */ - private fun setBoneRotation(bone: Bone, rotationVector: Vector3): Vector3 { - val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone) - bone.setRotationRaw(rotation) - - bone.updateThisNode() + private fun setBoneRotation(bone: Bone, rotation: Quaternion): Quaternion { + val newRotation = bone.rotationConstraint.applyConstraint(rotation, bone) + bone.setRotationRaw(newRotation) + bone.update() - return rotation.sandwich(Vector3.NEG_Y) + return 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 ee18d11acd..9f57596169 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 @@ -4,7 +4,7 @@ import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.trackers.Tracker /* - * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) to allow + * Implements CCDIK (Cyclic Coordinate Descent Inverse Kinematics) to allow * positional trackers such as vive/tundra trackers to be used in conjunction with * IMU trackers */ @@ -14,9 +14,9 @@ class IKSolver(private val root: Bone) { const val TOLERANCE_SQR = 1e-8 // == 0.01 cm const val MAX_ITERATIONS = 200 const val ITERATIONS_BEFORE_STEP = 20 - const val ITERATIONS_BETWEEN_STEP = 20 - const val MAX_LOOSENS = 10 - const val TOLERANCE_STEP = 2f + const val ITERATIONS_BETWEEN_STEP = 10 + const val MAX_LOOSENS = 20 + const val TOLERANCE_STEP = 1f } var enabled = true @@ -41,12 +41,6 @@ class IKSolver(private val root: Bone) { // Check if there is any constraints (other than the head) in the model rootChain = if (neededChain(rootChain!!)) rootChain else null chainList.sortBy { -it.level } - - println("ChainList length ${chainList.size}") - for (chain in chainList) { - println("Start = ${chain.nodes.first().boneType.name}") - println("End = ${chain.nodes.last().boneType.name}\n") - } } /** @@ -123,8 +117,8 @@ class IKSolver(private val root: Bone) { private fun combineChains(chain: IKChain, childChain: IKChain): IKChain { val boneList = mutableListOf() - boneList.addAll(chain.nodes) - boneList.addAll(childChain.nodes) + boneList.addAll(chain.bones) + boneList.addAll(childChain.bones) val newChain = IKChain( boneList, @@ -145,7 +139,8 @@ class IKSolver(private val root: Bone) { private fun addConstraints() { fun constrainChain(chain: IKChain) { - chain.nodes.forEach { it.rotationConstraint.allowModifications = false } + chain.locked = true + chain.bones.forEach { it.rotationConstraint.allowModifications = false } } chainList.forEach { if (it.tailConstraint == null) constrainChain(it) } } @@ -231,7 +226,6 @@ class IKSolver(private val root: Bone) { for (chain in chainList) { chain.backwards() } - rootChain?.forwardsMulti() rootChain?.computeTargetDistance()