IK goal condition fix & basic unit test

This commit is contained in:
Butterscotch! committed 2026-08-05 12:52:46 -04:00
1 parent 8a51cef017
commit ea348b65bb
2 files changed
+74 -1

No files matched your search

@@ -73,7 +73,7 @@ fun ccdIk(boneInputs: InputSkeleton, bones: ComputedSkeleton, goals: List<IKChai
return IKOutput(
curBones,
goals.associateWith {
chainDistanceFromTarget(bones, it.chain, it.target) <= threshold
chainDistanceFromTarget(curBones, it.chain, it.target) <= threshold
},
)
}
@@ -0,0 +1,73 @@
package dev.slimevr.skeleton
import io.github.axisangles.ktmath.EulerOrder
import io.github.axisangles.ktmath.Quaternion
import io.github.axisangles.ktmath.Vector3
import org.junit.jupiter.api.Test
import solarxr_protocol.datatypes.BodyPart
class IKTest {
@Test
fun testCcdIk() {
val boneInputs: InputSkeleton = bodyPartMap()
boneInputs[BodyPart.NECK] = BoneInput(
bodyPart = BodyPart.NECK,
offset = Vector3.NEG_Y,
rawRotation = Quaternion.IDENTITY,
rawPosition = Vector3.NULL,
isActive = false,
)
boneInputs[BodyPart.UPPER_CHEST] = BoneInput(
bodyPart = BodyPart.UPPER_CHEST,
offset = Vector3.NEG_Y,
rawRotation = Quaternion.IDENTITY,
rawPosition = Vector3.NULL,
isActive = false,
)
boneInputs[BodyPart.CHEST] = BoneInput(
bodyPart = BodyPart.CHEST,
offset = Vector3.NEG_Y,
rawRotation = Quaternion.IDENTITY,
rawPosition = Vector3.NULL,
isActive = false,
)
boneInputs[BodyPart.WAIST] = BoneInput(
bodyPart = BodyPart.WAIST,
offset = Vector3.NEG_Y,
rawRotation = Quaternion.IDENTITY,
rawPosition = Vector3.NULL,
isActive = false,
)
boneInputs[BodyPart.HIP] = BoneInput(
bodyPart = BodyPart.HIP,
offset = Vector3.NEG_Y,
rawRotation = Quaternion.IDENTITY,
rawPosition = Vector3.NULL,
isActive = false,
)
val bones = buildBones(boneInputs)
val target = Vector3.POS_X * 4f
val goals = listOf(
IKChainGoal(
listOf(
BodyPart.NECK,
BodyPart.UPPER_CHEST,
BodyPart.CHEST,
BodyPart.WAIST,
BodyPart.HIP,
),
target,
),
)
val ikOut = ccdIk(boneInputs, bones, goals, 0.01f, 100)
assert(ikOut.goalsReached.all { it.value }) {
val boneRots = ikOut.bones.values.joinToString {
"${it.bodyPart}: ${it.rotation.toEulerAngles(EulerOrder.YZX)}"
}
val targetDist = (target - ikOut.bones[BodyPart.HIP]!!.tailPosition).len()
"Failed to reach target:\nDistance from target: $targetDist\nBone rotations: $boneRots"
}
}
}