diff --git a/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt b/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt index 9844ff738..579475ce6 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/behaviours.kt @@ -314,7 +314,7 @@ class ComputedSkeletonBehaviour( for (processor in targetProcessors) processor.process(ikTargets, mutableBoneInputs, mutableComputedSkeleton, targetState.floorLevel) // Run IK - ccdIk( + val ikSuccess = ccdIk( mutableBoneInputs, mutableComputedSkeleton, ikTargets.map { (bodyPart, target) -> @@ -323,10 +323,15 @@ class ComputedSkeletonBehaviour( target, ) }, + BODY_PART_OFFSET_CONSTRAINT_MAP, BODY_PART_CONSTRAINT_MAP, IK_THRESHOLD, IK_MAX_ITERATIONS, ) + // TODO Make this not spam logs on repeated failures +// if (!ikSuccess) { +// AppLogger.skeleton.warn("IK step did not meet all requested targets within $IK_THRESHOLD meters") +// } // Save or restore paused tracking if (targetState.pausedProcessedBoneInputs != null) { diff --git a/server/core/src/main/java/dev/slimevr/skeleton/bodypart-constraints.kt b/server/core/src/main/java/dev/slimevr/skeleton/bodypart-constraints.kt index c85b24c34..d59f4f4b8 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/bodypart-constraints.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/bodypart-constraints.kt @@ -158,36 +158,6 @@ val BODY_PART_CONSTRAINT_MAP: BodyPartMap = BodyPartMap( ), ) -fun constrainOffset( - constraint: Constraint, - parent: Quaternion, - bone: Quaternion, - offset: Quaternion, -): Quaternion { - // TODO: Ensure the quaternion multiplication order is correct here - return constraint.apply(parent, offset * bone) / bone -} - -fun constrainOffsetWithSkeleton( - bodyPart: BodyPart, - offset: Quaternion, - bones: ComputedSkeleton, - constraints: BodyPartMap, -): Quaternion { - val constraint = constraints[bodyPart] ?: return offset - val boneRot = bones[bodyPart]?.rotation ?: return offset - val parentRot = parentOf(bodyPart)?.let { parent -> - bones[parent]?.rotation - } ?: Quaternion.IDENTITY - - return constrainOffset( - constraint, - parentRot, - boneRot, - offset, - ) -} - fun constrainSkeleton( bones: InputSkeleton, constraints: BodyPartMap, @@ -206,3 +176,41 @@ fun constrainSkeleton( } return bones } + +// Constraints to apply to a local offset to best fit a bone's natural movement +// Basically the max angle change per IK iteration within local axes +// TODO May need to decay constraint over iterations +val BODY_PART_OFFSET_CONSTRAINT_MAP: BodyPartMap = BodyPartMap( + mapOf( + // TODO Figure out arms + // BodyPart.LEFT_UPPER_ARM to AxisConstraint(), + // BodyPart.RIGHT_UPPER_ARM to AxisConstraint(), + + BodyPart.UPPER_CHEST to LooseAxisConstraint( + 10f * FastMath.DEG_TO_RAD, + ), + BodyPart.LOWER_CHEST to LooseAxisConstraint( + 10f * FastMath.DEG_TO_RAD, + ), + BodyPart.UPPER_WAIST to LooseAxisConstraint( + 10f * FastMath.DEG_TO_RAD, + ), + BodyPart.LOWER_WAIST to LooseAxisConstraint( + 10f * FastMath.DEG_TO_RAD, + ), + + BodyPart.LEFT_UPPER_LEG to LooseAxisConstraint( + 5f * FastMath.DEG_TO_RAD, + ), + BodyPart.LEFT_LOWER_LEG to LooseAxisConstraint( + 0.1f * FastMath.DEG_TO_RAD, + ), + + BodyPart.RIGHT_UPPER_LEG to LooseAxisConstraint( + 5f * FastMath.DEG_TO_RAD, + ), + BodyPart.RIGHT_LOWER_LEG to LooseAxisConstraint( + 0.1f * FastMath.DEG_TO_RAD, + ), + ), +) diff --git a/server/core/src/main/java/dev/slimevr/skeleton/constraint.kt b/server/core/src/main/java/dev/slimevr/skeleton/constraint.kt index 516e2fb16..fa1ae91c9 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/constraint.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/constraint.kt @@ -12,9 +12,9 @@ fun applyLocalOperation( parentRotation: Quaternion, childRotation: Quaternion, operation: (localRotation: Quaternion) -> Quaternion, -): Quaternion = parentRotation * operation(parentRotation.inv() * childRotation) +): Quaternion = operation(childRotation.localAbout(parentRotation)).globalAbout(parentRotation) -// TODO Actually rewrite, currently still using original server code +// TODO Create unit tests to validate behaviour interface Constraint { /** * Applies the constraint to a local rotation. @@ -40,7 +40,7 @@ data class TwistSwingConstraint( /** * Swing range in radians. */ - val swing: Float = 0f, + val swing: Float, ) : Constraint { override fun apply(localRotation: Quaternion): Quaternion { var (swingQ, twistQ) = decomposeToSwingTwist(localRotation, Vector3.NEG_Y) @@ -58,8 +58,7 @@ data class HingeConstraint( /** * Maximum rotation in radians. */ - val max: Float = 0f, - val maxDeviationFromTracker: Float = 15f, + val max: Float, val hingeAxis: Vector3 = Vector3.NEG_X, ) : Constraint { override fun apply(localRotation: Quaternion): Quaternion { @@ -76,9 +75,11 @@ data class LooseHingeConstraint( /** * Maximum rotation in radians. */ - val max: Float = 0f, - val allowedDeviation: Float = 0f, - val maxDeviationFromTracker: Float = 15f, + val max: Float, + /** + * Maximum non-hinge axis rotation in radians. + */ + val allowedDeviation: Float, val hingeAxis: Vector3 = Vector3.NEG_X, ) : Constraint { override fun apply(localRotation: Quaternion): Quaternion { @@ -88,13 +89,46 @@ data class LooseHingeConstraint( } } +data class AxisConstraint( + val axis: Vector3 = Vector3.NEG_X, +) : Constraint { + override fun apply(localRotation: Quaternion): Quaternion = localRotation.project(axis).unit() + + // Optimized to reduce quaternion operations + override fun apply( + parentRotation: Quaternion, + childRotation: Quaternion, + ): Quaternion { + val globalAxis = parentRotation.sandwich(axis) + return childRotation.project(globalAxis).unit() + } +} + +data class LooseAxisConstraint( + /** + * Maximum non-axis rotation in radians. + */ + val allowedDeviation: Float, + val axis: Vector3 = Vector3.NEG_X, +) : Constraint { + override fun apply(localRotation: Quaternion): Quaternion { + val (nonAxisRot, axisRot) = decomposeToSwingTwist(localRotation, axis) + return constrainAngle(nonAxisRot, allowedDeviation) * axisRot + } +} + +data class SwingTwist( + val swing: Quaternion, + val twist: Quaternion, +) + fun decomposeToSwingTwist( rotation: Quaternion, twistAxis: Vector3, -): Pair { +): SwingTwist { val twist = rotation.project(twistAxis).unit() val swing = rotation / twist - return Pair(swing, twist) + return SwingTwist(swing, twist) } fun constrainAngle(rotation: Quaternion, angle: Float): Quaternion { diff --git a/server/core/src/main/java/dev/slimevr/skeleton/ik.kt b/server/core/src/main/java/dev/slimevr/skeleton/ik.kt index 179427df6..d4d4dbfd3 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/ik.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/ik.kt @@ -1,6 +1,7 @@ package dev.slimevr.skeleton import com.jme3.math.FastMath +import dev.slimevr.util.subListOfLast import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 import solarxr_protocol.datatypes.BodyPart @@ -18,7 +19,9 @@ fun chainDistanceFromTarget( return (target - chainTail).len() } -private val oppositeRotation = Quaternion.rotationAroundZAxis(FastMath.PI) +// Bones are most likely opposite along the X axis +private val oppositeRotation = Quaternion.rotationAroundXAxis(FastMath.PI) +private val smallRotation = Quaternion.rotationAroundXAxis(1f * FastMath.DEG_TO_RAD) fun fromChainToTarget( bodyPart: BodyPart, bones: ComputedSkeleton, @@ -28,19 +31,32 @@ fun fromChainToTarget( val boneHead = requireBone(bones, bodyPart).headPosition val chainTail = requireBone(bones, chain.last()).tailPosition - val localChainTail = (chainTail - boneHead).unit() - if (FastMath.isApproxEqual(localChainTail.lenSq(), 0f)) { + val localChainTail = chainTail - boneHead + val localChainTailLenSq = localChainTail.lenSq() + if (FastMath.isApproxZero(localChainTailLenSq)) { // Chain tail is at the origin return null } - val localTarget = (target - boneHead).unit() - if (FastMath.isApproxEqual(localTarget.lenSq(), 0f)) { + val localTarget = target - boneHead + val localTargetLenSq = localTarget.lenSq() + if (FastMath.isApproxZero(localTargetLenSq)) { // Target is at the origin return null } - val offset = Quaternion.fromTo(localChainTail, localTarget) + val offset = Quaternion.fromTo(localChainTail.unit(), localTarget.unit()) + + if (localTargetLenSq < localChainTailLenSq && + FastMath.isApproxZero(offset.angleQ()) + ) { + // TODO This is not ideal whatsoever, this should be based on a preferred axis + // and aligned to global space using the bone's parent. + // If the target is closer than the tail and the bone is parallel to the target, + // apply an arbitrary small rotation to promote movement + return smallRotation + } + return if (FastMath.isApproxEqual(offset.lenSq(), 1f)) { offset } else { @@ -49,6 +65,37 @@ fun fromChainToTarget( } } +// Apply offset in global space to rotation +fun applyOffset( + rotation: Quaternion, + offset: Quaternion, +) = offset * rotation + +fun computeOffset( + rotation: Quaternion, + rotationWithOffset: Quaternion, +) = rotationWithOffset / rotation + +fun constrainOffsetWithSkeleton( + bodyPart: BodyPart, + offset: Quaternion, + bones: ComputedSkeleton, + constraint: Constraint, +): Quaternion { + val boneRot = bones[bodyPart]?.rotation ?: return offset + val parentRot = parentOf(bodyPart)?.let { parent -> + bones[parent]?.rotation + } ?: Quaternion.IDENTITY + + return computeOffset( + boneRot, + constraint.apply( + parentRot, + applyOffset(boneRot, offset), + ), + ) +} + fun rotateChain( boneInputs: InputSkeleton, chain: IKChain, @@ -59,7 +106,7 @@ fun rotateChain( requireNotNull(boneInput) { "The provided bone inputs are missing \"${bodyPart}\" from the IK chain." }.copy( - rotation = rotation * boneInput.rotation, + rotation = applyOffset(boneInput.rotation, rotation), ) } } @@ -70,22 +117,32 @@ fun ccdIkIteration( mutableComputedSkeleton: ComputedSkeleton, chain: IKChain, target: Vector3, - constraints: BodyPartMap?, + offsetConstraint: Constraint?, + constraint: Constraint?, ) { // TODO: Do we need annealing and/or dampening? // The first bone in the chain is the one we are adjusting in this iteration val bodyPart = chain.first() val offset = fromChainToTarget(bodyPart, mutableComputedSkeleton, chain, target) ?: return + // TODO Constrain the local offset + val localConstrainedOffset = offset +// val localConstrainedOffset = offsetConstraint?.let { +// val parentRot = parentOf(bodyPart)?.let { parent -> +// mutableComputedSkeleton[parent]?.rotation +// } ?: Quaternion.IDENTITY +// it.apply(parentRot, offset) +// } ?: offset + // We only need to constrain the bone that we are adjusting - val constrainedOffset = constraints?.let { + val constrainedOffset = constraint?.let { constrainOffsetWithSkeleton( bodyPart, - offset, + localConstrainedOffset, mutableComputedSkeleton, it, ) - } ?: offset + } ?: localConstrainedOffset // Mutate the input skeleton rotateChain(mutableBoneInputs, chain, constrainedOffset) @@ -100,34 +157,46 @@ data class IKChainGoal( val target: Vector3, ) +/** + * @return True if all goals were met, false if any goal was not met. + */ fun ccdIk( mutableBoneInputs: InputSkeleton, mutableComputedSkeleton: ComputedSkeleton, goals: List, + offsetConstraints: BodyPartMap?, constraints: BodyPartMap?, threshold: Float, maxIterations: Int, -) { +): Boolean { // No goal leaves every bone as the FK pass built it, so nothing below needs to run - if (goals.isEmpty()) return + if (goals.isEmpty()) return true - for (i in 0..maxIterations) { + for (i in 0 until maxIterations) { var allGoalsMet = true - for (goal in goals) { + for ((chain, target) in goals) { // Only iterate chains not meeting the threshold - if (chainDistanceFromTarget(mutableComputedSkeleton, goal.chain, goal.target) <= threshold) continue + if (chainDistanceFromTarget(mutableComputedSkeleton, chain, target) <= threshold) continue allGoalsMet = false // The chain from the current bone to the end, iterating backwards - val iterationChain = goal.chain.takeLast((i % goal.chain.size) + 1) + // TODO We can avoid making any objects or just cache this if it's slow, + // check performance to see if needed + val iterationChain = chain.subListOfLast((i % chain.size) + 1) + val bodyPart = iterationChain.first() ccdIkIteration( mutableBoneInputs, mutableComputedSkeleton, iterationChain, - goal.target, - constraints, + target, + offsetConstraints?.get(bodyPart), + constraints?.get(bodyPart), ) } if (allGoalsMet) break } + + return goals.all { (chain, target) -> + chainDistanceFromTarget(mutableComputedSkeleton, chain, target) <= threshold + } } diff --git a/server/core/src/main/java/dev/slimevr/skeleton/module.kt b/server/core/src/main/java/dev/slimevr/skeleton/module.kt index a959d1a81..550a84a8b 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/module.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/module.kt @@ -83,7 +83,7 @@ data class BoneState( get() = rotation * orientationOffset val localRotation: Quaternion - get() = parentBone?.let { it.rotation.inv() * rotation } ?: rotation + get() = parentBone?.let { rotation.localAbout(it.rotation) } ?: rotation val localHeadPosition: Vector3 get() = parentBone?.let { headPosition - it.tailPosition } ?: headPosition val localTailPosition: Vector3 diff --git a/server/core/src/main/java/dev/slimevr/util/sublist.kt b/server/core/src/main/java/dev/slimevr/util/sublist.kt new file mode 100644 index 000000000..e82b1bcbc --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/util/sublist.kt @@ -0,0 +1,15 @@ +package dev.slimevr.util + +/** + * Returns a sub-list containing last n elements (similar to [takeLast]). + * @throws IllegalArgumentException if [n] is negative. + */ +fun List.subListOfLast( + n: Int, +): List { + require(n >= 0) { "Requested element count $n is less than zero." } + if (n == 0) return emptyList() + val size = size + if (n >= size) return this + return subList(size - n, size) +} diff --git a/server/core/src/test/java/dev/slimevr/skeleton/IKTest.kt b/server/core/src/test/java/dev/slimevr/skeleton/IKTest.kt index dc942b1e4..2c18f88de 100644 --- a/server/core/src/test/java/dev/slimevr/skeleton/IKTest.kt +++ b/server/core/src/test/java/dev/slimevr/skeleton/IKTest.kt @@ -1,6 +1,9 @@ package dev.slimevr.skeleton +import com.jme3.math.FastMath +import dev.slimevr.quaternionAssertEquals import io.github.axisangles.ktmath.EulerOrder +import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 import solarxr_protocol.datatypes.BodyPart import kotlin.test.Test @@ -49,13 +52,95 @@ class IKTest { target, ) - val ikOut = ccdIk(boneInputs, bones, listOf(goal), BODY_PART_CONSTRAINT_MAP, 0.01f, 100) - assert(ikOut.goalsReached.all { it.value }) { - val boneRots = ikOut.bones.values.joinToString { + val goalsMet = ccdIk( + boneInputs, + bones, + listOf(goal), + BODY_PART_OFFSET_CONSTRAINT_MAP, + BODY_PART_CONSTRAINT_MAP, + 0.01f, + 100, + ) + assert(goalsMet) { + val boneRots = bones.values.joinToString { "${it.bodyPart}: ${it.rotation.toEulerAngles(EulerOrder.YZX)}" } - val targetDist = chainDistanceFromTarget(ikOut.bones, goal.chain, goal.target) + val targetDist = chainDistanceFromTarget(bones, goal.chain, goal.target) "Failed to reach target:\nDistance from target: $targetDist\nBone rotations: $boneRots" } } + + @Test + fun testRotateChain() { + val boneInputs: InputSkeleton = bodyPartMap() + boneInputs[BodyPart.NECK] = DEFAULT_BONE_INPUT.copy( + bodyPart = BodyPart.NECK, + offset = Vector3.NEG_Y, + rotation = Quaternion.rotationAroundXAxis(FastMath.QUARTER_PI), + ) + boneInputs[BodyPart.UPPER_CHEST] = DEFAULT_BONE_INPUT.copy( + bodyPart = BodyPart.UPPER_CHEST, + offset = Vector3.NEG_Y, + rotation = Quaternion.rotationAroundYAxis(FastMath.QUARTER_PI), + ) + boneInputs[BodyPart.LOWER_CHEST] = DEFAULT_BONE_INPUT.copy( + bodyPart = BodyPart.LOWER_CHEST, + offset = Vector3.NEG_Y, + rotation = Quaternion.rotationAroundZAxis(FastMath.QUARTER_PI), + ) + boneInputs[BodyPart.UPPER_WAIST] = DEFAULT_BONE_INPUT.copy( + bodyPart = BodyPart.UPPER_WAIST, + offset = Vector3.NEG_Y, + rotation = Quaternion.rotationAroundXAxis(FastMath.HALF_PI), + ) + boneInputs[BodyPart.LOWER_WAIST] = DEFAULT_BONE_INPUT.copy( + bodyPart = BodyPart.LOWER_WAIST, + offset = Vector3.NEG_Y, + rotation = Quaternion.rotationAroundYAxis(FastMath.HALF_PI), + ) + boneInputs[BodyPart.HIP] = DEFAULT_BONE_INPUT.copy( + bodyPart = BodyPart.HIP, + offset = Vector3.NEG_Y, + rotation = Quaternion.rotationAroundZAxis(FastMath.HALF_PI), + ) + + val rotatedBoneInputs = boneInputs.clone() + val chain = listOf( + BodyPart.NECK, + BodyPart.UPPER_CHEST, + BodyPart.LOWER_CHEST, + BodyPart.UPPER_WAIST, + BodyPart.LOWER_WAIST, + BodyPart.HIP, + ) + rotateChain( + rotatedBoneInputs, + chain, + Quaternion.rotationAroundYAxis(FastMath.QUARTER_PI), + ) + + assert(boneInputs != rotatedBoneInputs) { + "Bones were not rotated" + } + + fun getLocalRotation( + bones: InputSkeleton, + bone: BodyPart, + parent: BodyPart, + ) = bones[bone]!!.rotation.localAbout(bones[parent]!!.rotation) + + for (i in 1 until chain.size) { + val bone = chain[i] + val parent = chain[i - 1] + + val originalLocal = getLocalRotation(boneInputs, bone, parent) + val newLocal = getLocalRotation(rotatedBoneInputs, bone, parent) + + quaternionAssertEquals( + originalLocal, + newLocal, + message = "Local rotations are not matching after chain rotation", + ) + } + } }