Skip to content

Commit

Permalink
small fixes + optimization
Browse files Browse the repository at this point in the history
  • Loading branch information
@Stermere
Stermere committed Sep 23, 2024
1 parent be971cc commit 59b1060
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 34 deletions.
33 changes: 26 additions & 7 deletions server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,23 +16,39 @@ class Constraint(
val constraintType: ConstraintType,
twist: Float = 0.0f,
swing: Float = 0.0f,
allowedDeviation: Float = 0f
) {
private val constraintFunction = constraintTypeToFunc(constraintType)
private val twistRad = Math.toRadians(twist.toDouble()).toFloat()
private val swingRad = Math.toRadians(swing.toDouble()).toFloat()
private val allowedDeviationRad = Math.toRadians(allowedDeviation.toDouble()).toFloat()
var hasTrackerRotation = false

/**
* If false solve with minimal movement applied to this link
*/
var allowModifications = true

var initialRotation = Quaternion.IDENTITY

/**
* Apply rotational constraints and if applicable force the rotation
* to be unchanged unless it violates the constraints
*/
fun applyConstraint(rotation: Quaternion, thisBone: Bone): Quaternion = constraintFunction(rotation, thisBone, swingRad, twistRad).unit()

/**
* Force the given rotation to be within allowedDeviation degrees away from
* initialRotation on both the twist and swing axis
*/
fun constrainToInitialRotation(rotation: Quaternion): Quaternion {
val rotationLocal = rotation * initialRotation.inv()
var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y)
swingQ = constrain(swingQ, allowedDeviationRad)
twistQ = constrain(twistQ, allowedDeviationRad)
return initialRotation * (swingQ * twistQ)
}

companion object {
enum class ConstraintType {
TWIST_SWING,
Expand All @@ -51,10 +67,10 @@ class Constraint(
rotation: Quaternion,
twistAxis: Vector3,
): Pair<Quaternion, Quaternion> {
val projection = rotation.project(twistAxis)
val projection = rotation.project(twistAxis).unit()

val twist = Quaternion(rotation.w, projection.xyz).unit()
val swing = rotation * twist.inv()
val swing = (rotation * twist.inv()).unit()

return Pair(swing, twist)
}
Expand All @@ -76,7 +92,7 @@ class Constraint(
)
}

return rot
return rot.unit()
}

private fun constrain(rotation: Quaternion, minAngle: Float, maxAngle: Float, axis: Vector3): Quaternion {
Expand Down Expand Up @@ -109,14 +125,15 @@ class Constraint(
rotation
} else {
val parent = thisBone.parent!!
val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset
val rotationLocal =
(parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation
(parent.getGlobalRotation() * localRotationOffset).inv() * rotation
var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y)

swingQ = constrain(swingQ, swingRad)
twistQ = constrain(twistQ, twistRad)

parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ)
(parent.getGlobalRotation() * localRotationOffset * (swingQ * twistQ)).unit()
}
}

Expand All @@ -127,13 +144,15 @@ class Constraint(
rotation
} else {
val parent = thisBone.parent!!
val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset
val rotationLocal =
(parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation
(parent.getGlobalRotation() * localRotationOffset).inv() * rotation

var (_, hingeAxisRot) = decompose(rotationLocal, Vector3.NEG_X)

hingeAxisRot = constrain(hingeAxisRot, min, max, Vector3.NEG_X)

parent.getGlobalRotation() * thisBone.rotationOffset * hingeAxisRot
(parent.getGlobalRotation() * localRotationOffset * hingeAxisRot).unit()
}
}

Expand Down
30 changes: 16 additions & 14 deletions server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,32 +37,32 @@ class HumanSkeleton(
// Upper body bones
val headBone = Bone(BoneType.HEAD, Constraint(ConstraintType.COMPLETE))
val neckBone = Bone(BoneType.NECK, Constraint(ConstraintType.COMPLETE))
val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(ConstraintType.TWIST_SWING, 10f, 80f))
val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 10f, 80f))
val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 10f, 80f))
val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 10f, 80f))
val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(ConstraintType.TWIST_SWING, 15f, 80f))
val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 15f, 80f))
val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 15f, 80f))
val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 20f, 80f))

// Lower body bones
val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 180f, 15f))
val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 180f, 15f))
val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 110f, 120f))
val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 110f, 120f))
val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 60f, 15f))
val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 60f, 15f))
val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 90f, 170f))
val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 90f, 170f))
val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(ConstraintType.HINGE, 180f, 0f))
val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(ConstraintType.HINGE, 180f, 0f))
val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 90f))
val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 90f))
val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 50f))
val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 50f))

// Arm bones
val leftUpperShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.COMPLETE))
val rightUpperShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.COMPLETE))
val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 180f, 10f))
val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 180f, 10f))
val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 20f, 10f))
val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 20f, 10f))
val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 180f, 180f))
val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 180f, 180f))
val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(ConstraintType.HINGE, 0f, -180f))
val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.HINGE, 0f, -180f))
val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 180f, 180f))
val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 180f, 180f))
val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f))
val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f))

// Tracker bones
val headTrackerBone = Bone(BoneType.HEAD_TRACKER, Constraint(ConstraintType.COMPLETE))
Expand Down Expand Up @@ -1000,6 +1000,8 @@ class HumanSkeleton(
rightLowerLegBone,
leftFootBone,
rightFootBone,
leftUpperShoulderBone,
rightUpperShoulderBone,
leftShoulderBone,
rightShoulderBone,
leftUpperArmBone,
Expand Down
27 changes: 15 additions & 12 deletions server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class IKChain(
*/
private fun prepBones() {
for (i in 0..<bones.size) {
if (!bones[i].rotationConstraint.hasTrackerRotation && bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE) {
if (bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE && bones[i].rotationConstraint.allowModifications) {
bones[i].setRotationRaw(rotations[i])
}
}
Expand All @@ -53,21 +53,18 @@ class IKChain(
val currentBone = bones[i]
val childBone = if (bones.size - 1 < i) bones[i + 1] else null

if (currentBone.boneType.bodyPart in IKSolver.LOCK_ROTATION) continue
if (currentBone.boneType.bodyPart in IKSolver.LOCK_ROTATION) {
rotations[i] = currentBone.getGlobalRotation()
continue
}

// Get the local position of the end effector and the target relative to the current node
val endEffectorLocal = (getEndEffectorsAvg() - currentBone.getPosition()).unit()
val targetLocal = (target - currentBone.getPosition()).unit()

// Compute the axis of rotation and angle for this bone
val cross = endEffectorLocal.cross(targetLocal).unit()
if (cross.lenSq() < 1e-9) continue
val baseAngle = acos(endEffectorLocal.dot(targetLocal).coerceIn(-1.0f, 1.0f))

val angle = baseAngle * IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.allowModifications) 1f else IKSolver.STATIC_DAMPENING

val sinHalfAngle = sin(angle / 2)
val adjustment = Quaternion(cos(angle / 2), cross * sinHalfAngle)
val scalar = IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.allowModifications) 1f else IKSolver.STATIC_DAMPENING
val adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit()
val correctedRot = (adjustment * currentBone.getGlobalRotation()).unit()

rotations[i] = setBoneRotation(currentBone, childBone, correctedRot, useConstraints)
Expand Down Expand Up @@ -101,7 +98,11 @@ class IKChain(
*/
fun resetChain() {
distToTargetSqr = Float.POSITIVE_INFINITY
// prepBones()

for (b in bones) {
b.rotationConstraint.initialRotation = b.getGlobalRotation()
}
//prepBones()

for (child in children) {
child.resetChain()
Expand Down Expand Up @@ -136,7 +137,9 @@ class IKChain(
*/
private fun setBoneRotation(bone: Bone, childBone: Bone?, rotation: Quaternion, useConstraints: Boolean): Quaternion {
// Constrain relative to the parent
val newRotation = if ((useConstraints || bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) && !bone.rotationConstraint.hasTrackerRotation) {
val newRotation = if (bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) {
bone.rotationConstraint.applyConstraint(rotation, bone)
} else if (useConstraints && !bone.rotationConstraint.hasTrackerRotation) {
bone.rotationConstraint.applyConstraint(rotation, bone)
} else {
rotation
Expand Down
2 changes: 1 addition & 1 deletion server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@ import solarxr_protocol.datatypes.BodyPart
class IKSolver(private val root: Bone) {
companion object {
const val TOLERANCE_SQR = 1e-8 // == 0.01 cm
const val ROTATIONAL_TOLERANCE = 0.000745329 // == 0.5 degrees
const val MAX_ITERATIONS = 100
const val ANNEALING_STEP = 20
const val ANNEALING_ITERATIONS = 5
Expand Down Expand Up @@ -223,6 +222,7 @@ class IKSolver(private val root: Bone) {
}
}

// Terminate if using constraints and the chain is solved
if (solved && useConstraints) return true
}

Expand Down

0 comments on commit 59b1060

Please sign in to comment.