IK preferred rotation WIP

This commit is contained in:
Erimel committed 2026-11-09 20:01:56 -05:00
1 parent c64fcb1f95
commit 1d8a441d01
4 files changed
+55 -31

No files matched your search

@@ -29,7 +29,7 @@ val BODY_PART_CONSTRAINT_MAP: BodyPartMap<Constraint> = BodyPartMap(
45f * FastMath.DEG_TO_RAD,
),
BodyPart.LEFT_LOWER_ARM to LooseHingeConstraint(
-165f * FastMath.DEG_TO_RAD,
-170f * FastMath.DEG_TO_RAD,
0f * FastMath.DEG_TO_RAD,
55f * FastMath.DEG_TO_RAD,
),
@@ -47,7 +47,7 @@ val BODY_PART_CONSTRAINT_MAP: BodyPartMap<Constraint> = BodyPartMap(
45f * FastMath.DEG_TO_RAD,
),
BodyPart.RIGHT_LOWER_ARM to LooseHingeConstraint(
-165f * FastMath.DEG_TO_RAD,
-170f * FastMath.DEG_TO_RAD,
0f * FastMath.DEG_TO_RAD,
55f * FastMath.DEG_TO_RAD,
),
@@ -79,13 +79,15 @@ val BODY_PART_CONSTRAINT_MAP: BodyPartMap<Constraint> = BodyPartMap(
),
// Left leg
BodyPart.LEFT_UPPER_LEG to TwistSwingConstraint(
120f * FastMath.DEG_TO_RAD,
165f * FastMath.DEG_TO_RAD,
BodyPart.LEFT_UPPER_LEG to LooseHingeConstraint(
-50f * FastMath.DEG_TO_RAD,
170f * FastMath.DEG_TO_RAD,
85f * FastMath.DEG_TO_RAD,
hingeAxis = Vector3.POS_X,
),
BodyPart.LEFT_LOWER_LEG to LooseHingeConstraint(
-6f * FastMath.DEG_TO_RAD,
165f * FastMath.DEG_TO_RAD,
170f * FastMath.DEG_TO_RAD,
50f * FastMath.DEG_TO_RAD,
),
BodyPart.LEFT_FOOT to TwistSwingConstraint(
@@ -94,13 +96,15 @@ val BODY_PART_CONSTRAINT_MAP: BodyPartMap<Constraint> = BodyPartMap(
),
// Right leg
BodyPart.RIGHT_UPPER_LEG to TwistSwingConstraint(
120f * FastMath.DEG_TO_RAD,
165f * FastMath.DEG_TO_RAD,
BodyPart.RIGHT_UPPER_LEG to LooseHingeConstraint(
-50f * FastMath.DEG_TO_RAD,
170f * FastMath.DEG_TO_RAD,
85f * FastMath.DEG_TO_RAD,
hingeAxis = Vector3.POS_X,
),
BodyPart.RIGHT_LOWER_LEG to LooseHingeConstraint(
-6f * FastMath.DEG_TO_RAD,
165f * FastMath.DEG_TO_RAD,
170f * FastMath.DEG_TO_RAD,
50f * FastMath.DEG_TO_RAD,
),
BodyPart.RIGHT_FOOT to TwistSwingConstraint(
@@ -175,6 +179,23 @@ fun constrainSkeleton(
return bones
}
/**
* Used for the initial rotation before doing IK.
*/
val BODY_PART_PREFERRED_ROTATION = BodyPartMap(
mapOf(
BodyPart.LEFT_UPPER_LEG to Quaternion.rotationAroundXAxis(90f * FastMath.DEG_TO_RAD),
BodyPart.RIGHT_UPPER_LEG to Quaternion.rotationAroundXAxis(90f * FastMath.DEG_TO_RAD),
BodyPart.LEFT_LOWER_LEG to Quaternion.rotationAroundXAxis(-45f * FastMath.DEG_TO_RAD),
BodyPart.RIGHT_LOWER_LEG to Quaternion.rotationAroundXAxis(-45f * FastMath.DEG_TO_RAD),
// TODO arms and others
// BodyPart.LEFT_UPPER_ARM to Quaternion.rotationAroundXAxis(-45f * FastMath.DEG_TO_RAD),
// BodyPart.RIGHT_UPPER_ARM to Quaternion.rotationAroundXAxis(-45f * FastMath.DEG_TO_RAD),
// BodyPart.LEFT_LOWER_ARM to Quaternion.rotationAroundXAxis(90f * FastMath.DEG_TO_RAD),
// BodyPart.RIGHT_LOWER_ARM to Quaternion.rotationAroundXAxis(90f * FastMath.DEG_TO_RAD),
),
)
// 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
@@ -216,14 +237,14 @@ val BODY_PART_OFFSET_CONSTRAINT_MAP: BodyPartMap<Constraint> = BodyPartMap(
5f * FastMath.DEG_TO_RAD,
),
BodyPart.LEFT_LOWER_LEG to LooseAxisConstraint(
1f * FastMath.DEG_TO_RAD,
0.2f * FastMath.DEG_TO_RAD,
),
BodyPart.RIGHT_UPPER_LEG to LooseAxisConstraint(
5f * FastMath.DEG_TO_RAD,
),
BodyPart.RIGHT_LOWER_LEG to LooseAxisConstraint(
1f * FastMath.DEG_TO_RAD,
0.2f * FastMath.DEG_TO_RAD,
),
),
)
@@ -28,7 +28,6 @@ fun parentRot(
// 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,
@@ -53,16 +52,7 @@ fun fromChainToTarget(
}
val offset = Quaternion.fromTo(localChainTail.unit(), localTarget.unit())
return if (localTargetLenSq < localChainTailLenSq &&
FastMath.isApproxZero(offset.angleQ())
) {
// TODO Should be based off a preferred axis/direction, currently seems to
// "snap" a little when hitting this fallback
// If the target is closer than the tail and the bone is parallel to the target,
// apply an arbitrary small rotation to promote movement
alignOffset(bone.rotation.inv(), smallRotation)
} else if (FastMath.isApproxEqual(offset.lenSq(), 1f)) {
return if (FastMath.isApproxEqual(offset.lenSq(), 1f)) {
offset
} else {
// When the vectors are exactly opposite, arbitrarily choose an axis
@@ -155,8 +145,7 @@ fun ccdIkIteration(
rotateChain(mutableBoneInputs, chain, constrainedOffset)
// Only build bones for inputs that were changed
// If performance is bad, we can combine buildBones calls for non-overlapping IK
// chains
// If performance is bad, we can combine buildBones calls for non-overlapping IK chains
buildBones(mutableComputedSkeleton, mutableBoneInputs, chain.toSet())
}
@@ -181,8 +170,25 @@ fun ccdIk(
// No goal leaves every bone as the FK pass built it, so nothing below needs to run
if (goals.isEmpty()) return true
// Set each bone in each chain of each goal to its preferred rotation
val changedBodyParts = mutableSetOf<BodyPart>()
for (goal in goals) {
val distanceToTarget = chainDistanceFromTarget(mutableComputedSkeleton, goal.chain, goal.target)
val chainLength = goal.chain.sumOf { requireBone(mutableComputedSkeleton, it).offset.len().toDouble() }.toFloat()
val t = (distanceToTarget / chainLength).coerceAtMost(1f)
for (bodyPart in goal.chain) {
val bone = mutableBoneInputs[bodyPart] ?: continue
val preferredRotation = BODY_PART_PREFERRED_ROTATION[bodyPart] ?: continue
val rotationChange = preferredRotation.pow(t)
mutableBoneInputs[bodyPart] = bone.copy(rotation = bone.rotation * rotationChange)
changedBodyParts.add(bodyPart)
}
}
if (changedBodyParts.isNotEmpty()) buildBones(mutableComputedSkeleton, mutableBoneInputs, changedBodyParts)
var allGoalsMet = true
for (i in 0 until maxIterations) {
var allGoalsMet = true
allGoalsMet = true
for ((chain, target) in goals) {
// Only iterate chains not meeting the threshold
if (chainDistanceFromTarget(mutableComputedSkeleton, chain, target) <= threshold) continue
@@ -205,7 +211,5 @@ fun ccdIk(
if (allGoalsMet) break
}
return goals.all { (chain, target) ->
chainDistanceFromTarget(mutableComputedSkeleton, chain, target) <= threshold
}
return allGoalsMet
}
@@ -16,7 +16,6 @@ class FloorClipTargetProcessor(
BodyPart.RIGHT_LOWER_LEG,
),
) : SkeletonTargetProcessor {
// TODO don't just correct ankles, also correct some of the hip (20% in old code). Probably to be done on IK directly.
override fun process(mutableIkTargets: IKTargets, inputSkeleton: InputSkeleton, fk: ComputedSkeleton, floorLevel: Float) {
if (!skeleton.effectiveFloorClip) return
@@ -47,7 +47,7 @@ const val UNLOCK_WARMUP_MAX = 0.050f
// The distance correction scales with the linear velocity and distance from locked position
const val DISTANCE_MIN = 0.01f
const val DISTANCE_MAX = 0.1f
const val DISTANCE_CORRECTION_MIN = 0f
const val DISTANCE_CORRECTION_MIN = 0.05f
const val DISTANCE_CORRECTION_MAX = 1f
// The correction speed accelerates with time