From 1d8a441d01d4e7cd155404859374e03619efc419 Mon Sep 17 00:00:00 2001 From: Erimel Date: Mon, 9 Nov 2026 19:12:22 -0500 Subject: [PATCH] IK preferred rotation WIP --- .../slimevr/skeleton/bodypart-constraints.kt | 45 ++++++++++++++----- .../src/main/java/dev/slimevr/skeleton/ik.kt | 38 +++++++++------- .../skeleton/targetprocessors/floor-clip.kt | 1 - .../targetprocessors/skating-correction.kt | 2 +- 4 files changed, 55 insertions(+), 31 deletions(-) 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 37fc1d88c..23437488d 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 @@ -29,7 +29,7 @@ val BODY_PART_CONSTRAINT_MAP: BodyPartMap = 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 = 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 = 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 = 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 = 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, ), ), ) 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 1ad9e1f4e..bd39c2617 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/ik.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/ik.kt @@ -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() + 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 } diff --git a/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/floor-clip.kt b/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/floor-clip.kt index 8e18e141e..355707fff 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/floor-clip.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/floor-clip.kt @@ -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 diff --git a/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/skating-correction.kt b/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/skating-correction.kt index 59c7e7d9a..9de0ff69d 100644 --- a/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/skating-correction.kt +++ b/server/core/src/main/java/dev/slimevr/skeleton/targetprocessors/skating-correction.kt @@ -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