mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
IK preferred rotation WIP
This commit is contained in:
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
|
||||
|
||||
|
||||
+1
-1
@@ -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
|
||||
|
||||
Reference in new issue
Block a user