mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-20 09:01:20 -08:00
IK goal condition fix & basic unit test
This commit is contained in:
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"
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user