diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index 29ba31b382..9588b3dc29 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -7,27 +7,19 @@ import io.github.axisangles.ktmath.Vector3 * A constraint type that allows for a rotation to be constrained relative to the parent * of the supplied bone */ -class TwistSwingConstraint(private val twist: Float, private val swing: Float) : Constraint() { +class TwistSwingConstraint(twist: Float, swing: Float) : Constraint() { private val twistRad = Math.toRadians(twist.toDouble()).toFloat() private val swingRad = Math.toRadians(swing.toDouble()).toFloat() 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())) { - return rotation - } - + if (thisBone.parent == null) return rotation val parent = thisBone.parent!! - // get the local rotation val rotationLocal = (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation - - // decompose in to twist and swing var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) - // apply the constraints - if (!swing.isNaN()) swingQ = constrain(swingQ, swingRad) - if (!twist.isNaN()) twistQ = constrain(twistQ, twistRad) + swingQ = constrain(swingQ, swingRad) + twistQ = constrain(twistQ, twistRad) return parent.getGlobalRotation() * thisBone.rotationOffset * (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 3413d5993b..42222b5cc7 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 @@ -48,7 +48,6 @@ class IKChain( (computedTailPosition?.getPosition()) ?: Vector3.NULL } - // Set the end node to target positions[positions.size - 1] = target for (i in positions.size - 2 downTo 0) { @@ -77,7 +76,6 @@ class IKChain( positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) } - // Point the last bone at the target 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) @@ -106,6 +104,9 @@ class IKChain( return sum } + /** + * Resets the chain to its default state + */ fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY centroidWeight = 1f @@ -146,7 +147,7 @@ class IKChain( } /** - * Allow constrained bones to deviate more per step + * Allow constrained bones to deviate more */ fun decreaseConstraints() { loosens++ 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 6fff5ae390..b93b78d419 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) and allows + * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) to allow * positional trackers such as vive/tundra trackers to be used in conjunction with * IMU trackers */ @@ -30,11 +30,9 @@ class IKSolver(private val root: Bone) { fun buildChains(trackers: List) { chainList.clear() - // Extract the positional constraints val positionalConstraints = extractPositionalConstraints(trackers) val rotationalConstraints = extractRotationalConstraints(trackers) - // Build the system of chains rootChain = chainBuilder(root, null, 0, positionalConstraints, rotationalConstraints) populateChainList(rootChain!!) addConstraints() @@ -45,8 +43,7 @@ class IKSolver(private val root: Bone) { } /** - * Reset the offsets of positional trackers. Should only be called right after a full reset - * is performed. + * Reset the offsets of positional trackers. */ fun resetOffsets() { needsReset = true @@ -91,9 +88,8 @@ class IKSolver(private val root: Bone) { var chain = IKChain(boneList, parent, level, baseConstraint, tailConstraint) if (currentBone.children.isNotEmpty()) { - val childrenList = mutableListOf() - // Build child chains + val childrenList = mutableListOf() for (child in currentBone.children) { val childChain = chainBuilder(child, chain, level + 1, positionalConstraints, rotationalConstraints) if (neededChain(childChain)) { @@ -229,7 +225,7 @@ class IKSolver(private val root: Bone) { } needsReset = false } - + rootChain?.resetChain() for (i in 0 until MAX_ITERATIONS) {