IK cleanup, test, docs & quat parent space fix

This commit is contained in:
Butterscotch! committed 2026-10-28 19:53:36 -04:00
1 parent 546101defa
commit c911a29d25
7 files changed
+281 -65

No files matched your search

@@ -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) {
@@ -158,36 +158,6 @@ val BODY_PART_CONSTRAINT_MAP: BodyPartMap<Constraint> = 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<Constraint>,
): 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<Constraint>,
@@ -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<Constraint> = 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,
),
),
)
@@ -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<Quaternion, Quaternion> {
): 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 {
@@ -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<Constraint>?,
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<IKChainGoal>,
offsetConstraints: BodyPartMap<Constraint>?,
constraints: BodyPartMap<Constraint>?,
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
}
}
@@ -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
@@ -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 <T> List<T>.subListOfLast(
n: Int,
): List<T> {
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)
}
@@ -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",
)
}
}
}