Skip to content

Commit

Permalink
switch from FABRIK to CCDIK
Browse files Browse the repository at this point in the history
  • Loading branch information
@Stermere
Stermere committed Jul 11, 2024
1 parent 499535c commit 1dafe6c
Show file tree
Hide file tree
Showing 4 changed files with 117 additions and 120 deletions.
86 changes: 60 additions & 26 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 @@ -2,6 +2,7 @@ package dev.slimevr.tracking.processor

import io.github.axisangles.ktmath.Quaternion
import io.github.axisangles.ktmath.Vector3
import kotlin.math.abs
import kotlin.math.sign
import kotlin.math.sin
import kotlin.math.sqrt
Expand Down Expand Up @@ -49,8 +50,7 @@ class Constraint(
* Apply rotational constraints and if applicable force the rotation
* to be unchanged unless it violates the constraints
*/
fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion {
val rotation = Quaternion.fromTo(Vector3.NEG_Y, direction)
fun applyConstraint(rotation: Quaternion, thisBone: Bone): Quaternion {
if (allowModifications) {
return constraintFunction(rotation, thisBone, swingRad, twistRad).unit()
}
Expand All @@ -59,15 +59,6 @@ class Constraint(
return constraintFunction(constrainedRotation, thisBone, swingRad, twistRad).unit()
}

/**
* Apply constraints to the direction vector such that when
* inverted the vector would be within the constraints.
* This is used for constraining direction vectors on the backwards pass
* of the FABRIK solver.
*/
fun applyConstraintInverse(direction: Vector3, thisBone: Bone): Vector3 =
-applyConstraint(-direction, thisBone).sandwich(Vector3.NEG_Y)

/**
* Limit the rotation to tolerance away from the initialRotation
*/
Expand All @@ -86,7 +77,7 @@ class Constraint(
}

companion object {
fun decompose(
private fun decompose(
rotation: Quaternion,
twistAxis: Vector3,
): Pair<Quaternion, Quaternion> {
Expand All @@ -98,7 +89,7 @@ class Constraint(
return Pair(swing, twist)
}

fun constrain(rotation: Quaternion, angle: Float): Quaternion {
private fun constrain(rotation: Quaternion, angle: Float): Quaternion {
val magnitude = sin(angle * 0.5f)
val magnitudeSqr = magnitude * magnitude
val sign = if (rotation.w != 0f) sign(rotation.w) else 1f
Expand All @@ -118,22 +109,65 @@ class Constraint(
return rot
}

// Constraint function for TwistSwingConstraint
val twistSwingConstraint: ConstraintFunction = { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float ->
if (thisBone.parent == null) {
rotation
} else {
val parent = thisBone.parent!!
val rotationLocal = (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation
var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y)

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

parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ)
private fun constrain(rotation: Quaternion, minAngle: Float, maxAngle: Float, axis: Vector3): Quaternion {
val magnitudeMin = sin(minAngle * 0.5f)
val magnitudeMax = sin(maxAngle * 0.5f)
val magnitudeSqrMin = magnitudeMin * magnitudeMin * if (minAngle != 0f) sign(minAngle) else 1f
val magnitudeSqrMax = magnitudeMax * magnitudeMax * if (maxAngle != 0f) sign(maxAngle) else 1f
var vector = rotation.xyz
var rot = rotation

val rotMagnitude = vector.lenSq() * if (vector.dot(axis) < 0) -1f else 1f
if (rotMagnitude < magnitudeSqrMin || rotMagnitude > magnitudeSqrMax) {
val magnitude = if (rotMagnitude < magnitudeSqrMin) magnitudeMin else magnitudeMax
val magnitudeSqr = abs(if (rotMagnitude < magnitudeSqrMin) magnitudeSqrMin else magnitudeSqrMax)
vector = vector.unit() * magnitude
rot = Quaternion(
sqrt(1.0f - magnitudeSqr),
vector.x,
vector.y,
vector.z,
)
}

return rot
}

// Constraint function for TwistSwingConstraint
val twistSwingConstraint: ConstraintFunction =
{ rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float ->
if (thisBone.parent == null) {
rotation
} else {
val parent = thisBone.parent!!
val rotationLocal =
(parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation
var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y)

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

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

// Constraint function for a hinge constraint with min and max angles
val hingeConstraint: ConstraintFunction =
{ rotation: Quaternion, thisBone: Bone, min: Float, max: Float ->
if (thisBone.parent == null) {
rotation
} else {
val parent = thisBone.parent!!
val rotationLocal =
(parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation
var (_, hingeAxisRot) = decompose(rotationLocal, Vector3.NEG_X)

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

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

// Constraint function for CompleteConstraint
val completeConstraint: ConstraintFunction = { _: Quaternion, thisBone: Bone, _: Float, _: Float ->
thisBone.getGlobalRotation()
Expand Down
13 changes: 7 additions & 6 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 @@ -6,6 +6,7 @@ import dev.slimevr.tracking.processor.Bone
import dev.slimevr.tracking.processor.BoneType
import dev.slimevr.tracking.processor.Constraint
import dev.slimevr.tracking.processor.Constraint.Companion.completeConstraint
import dev.slimevr.tracking.processor.Constraint.Companion.hingeConstraint
import dev.slimevr.tracking.processor.Constraint.Companion.twistSwingConstraint
import dev.slimevr.tracking.processor.HumanPoseManager
import dev.slimevr.tracking.processor.config.SkeletonConfigToggles
Expand Down Expand Up @@ -43,12 +44,12 @@ class HumanSkeleton(
val hipBone = Bone(BoneType.HIP, Constraint(twistSwingConstraint, 10f, 80f))

// Lower body bones
val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(completeConstraint))
val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(completeConstraint))
val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(twistSwingConstraint, 180f, 8f))
val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(twistSwingConstraint, 180f, 8f))
val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(twistSwingConstraint, 180f, 120f))
val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(twistSwingConstraint, 180f, 120f))
val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(twistSwingConstraint, 110f, 180f))
val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(twistSwingConstraint, 110f, 180f))
val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(hingeConstraint, 180f, 0f))
val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(hingeConstraint, 180f, 0f))
val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(twistSwingConstraint, 180f, 120f))
val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(twistSwingConstraint, 180f, 120f))

Expand All @@ -57,8 +58,8 @@ class HumanSkeleton(
val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(twistSwingConstraint, 180f, 8f))
val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, Constraint(twistSwingConstraint, 180f, 180f))
val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, Constraint(twistSwingConstraint, 180f, 180f))
val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(twistSwingConstraint, 180f, 180f))
val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(twistSwingConstraint, 180f, 180f))
val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(hingeConstraint, 0f, -180f))
val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(hingeConstraint, 0f, -180f))
val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(twistSwingConstraint, 180f, 180f))
val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(twistSwingConstraint, 180f, 180f))

Expand Down
116 changes: 42 additions & 74 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 @@ -2,14 +2,19 @@ package dev.slimevr.tracking.processor.skeleton

import dev.slimevr.tracking.processor.Bone
import dev.slimevr.tracking.trackers.Tracker
import io.github.axisangles.ktmath.Quaternion
import io.github.axisangles.ktmath.Vector3
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sign
import kotlin.math.sin

/*
* This class implements a chain of Bones for use by the FABRIK solver
* This class implements a chain of Bones
*/

class IKChain(
val nodes: MutableList<Bone>,
val bones: MutableList<Bone>,
var parent: IKChain?,
val level: Int,
val baseConstraint: Tracker?,
Expand All @@ -23,82 +28,46 @@ class IKChain(
private val computedBasePosition = baseConstraint?.let { IKConstraint(it) }
private val computedTailPosition = tailConstraint?.let { IKConstraint(it) }
var children = mutableListOf<IKChain>()
private var targetSum = Vector3.NULL
var target = Vector3.NULL
var distToTargetSqr = Float.POSITIVE_INFINITY
var loosens = 0
var locked = false
private var centroidWeight = 1f
private var positions = getPositionList()

private fun getPositionList(): MutableList<Vector3> {
val posList = mutableListOf<Vector3>()
for (n in nodes) {
posList.add(n.getPosition())
}
posList.add(nodes.last().getTailPosition())

return posList
}

fun backwards() {
// Start at the constraint or the centroid of the children
//target = computedTailPosition?.getPosition() ?: nodes.last().getTailPosition()
target = computedTailPosition?.getPosition() ?: (targetSum / getChildrenCentroidWeightSum())

positions[positions.size - 1] = target
target = computedTailPosition?.getPosition() ?: getWeightedChildTarget()

for (i in positions.size - 2 downTo 0) {
var direction = (positions[i] - positions[i + 1]).unit()
direction = nodes[i].rotationConstraint
.applyConstraintInverse(direction, nodes[i])
for (i in bones.size - 1 downTo 0) {
val currentBone = bones[i]

positions[i] = positions[i + 1] + (direction * nodes[i].length)
}
// Get the local position of the end effector and the target relative to the current node
val endEffectorLocal = (bones.last().getTailPosition() - currentBone.getPosition()).unit()
val targetLocal = (target - currentBone.getPosition()).unit()

if (parent != null && parent!!.computedTailPosition == null) {
parent!!.targetSum += positions[0] * centroidWeight
}
}
// Compute the axis of rotation and angle for this bone
val cross = endEffectorLocal.cross(targetLocal).unit()
if (cross.lenSq() == 0.0f) continue
val baseAngle = acos(endEffectorLocal.dot(targetLocal).coerceIn(-1.0f, 1.0f))
val angle = baseAngle * sign(cross.dot(cross))

private fun forwards() {
positions[0] = if (parent != null) {
parent!!.nodes.last().getTailPosition()
} else {
(computedBasePosition?.getPosition()) ?: positions[0]
}
val sinHalfAngle = sin(angle / 2)
val adjustment = Quaternion(cos(angle / 2), cross.x * sinHalfAngle, cross.y * sinHalfAngle, cross.z * sinHalfAngle)
val correctedRot = (adjustment * currentBone.getGlobalRotation()).unit()

for (i in 1 until positions.size - 1) {
var direction = (positions[i] - positions[i - 1]).unit()
direction = setBoneRotation(nodes[i - 1], direction)
positions[i] = positions[i - 1] + (direction * nodes[i - 1].length)
setBoneRotation(currentBone, correctedRot)
}

var direction = (target - positions[positions.size - 2]).unit()
direction = setBoneRotation(nodes.last(), direction)
positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length)

// reset sub-base target
targetSum = Vector3.NULL
}

/**
* Run the forward pass
*/
fun forwardsMulti() {
forwards()

for (c in children) {
c.forwardsMulti()
}
}

private fun getChildrenCentroidWeightSum(): Float {
var sum = 0.0f
private fun getWeightedChildTarget(): Vector3 {
var weightSum = 0.0f
var sum = Vector3.NULL
for (child in children) {
sum += child.centroidWeight
weightSum += child.centroidWeight
sum += child.target
}

return sum
return sum / weightSum
}

/**
Expand All @@ -108,7 +77,7 @@ class IKChain(
distToTargetSqr = Float.POSITIVE_INFINITY
centroidWeight = 1f

for (bone in nodes) {
for (bone in bones) {
if (loosens > 0) bone.rotationConstraint.tolerance -= IKSolver.TOLERANCE_STEP
bone.rotationConstraint.originalRotation = bone.getGlobalRotation()
}
Expand All @@ -120,8 +89,8 @@ class IKChain(
}

fun resetTrackerOffsets() {
computedTailPosition?.reset(nodes.last().getTailPosition())
computedBasePosition?.reset(nodes.first().getPosition())
computedTailPosition?.reset(bones.last().getTailPosition())
computedBasePosition?.reset(bones.first().getPosition())
}

/**
Expand All @@ -147,20 +116,20 @@ class IKChain(
* Allow constrained bones to deviate more
*/
fun decreaseConstraints() {
if (locked) return
loosens++
for (bone in nodes) {
for (bone in bones) {
bone.rotationConstraint.tolerance += IKSolver.TOLERANCE_STEP
}
}

/**
* Updates the distance to target and other fields
* Call on the root chain only returns the sum of the
* distances
* Call on the root chain
*/
fun computeTargetDistance() {
distToTargetSqr = if (computedTailPosition != null) {
(positions.last() - computedTailPosition.getPosition()).lenSq()
(bones.last().getTailPosition() - computedTailPosition.getPosition()).lenSq()
} else {
0.0f
}
Expand All @@ -173,14 +142,13 @@ class IKChain(
/**
* Sets a bones rotation from a rotation vector after constraining the rotation
* vector with the bone's rotational constraint
* returns the constrained rotation as a vector
* returns the constrained rotation
*/
private fun setBoneRotation(bone: Bone, rotationVector: Vector3): Vector3 {
val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone)
bone.setRotationRaw(rotation)

bone.updateThisNode()
private fun setBoneRotation(bone: Bone, rotation: Quaternion): Quaternion {
val newRotation = bone.rotationConstraint.applyConstraint(rotation, bone)
bone.setRotationRaw(newRotation)
bone.update()

return rotation.sandwich(Vector3.NEG_Y)
return newRotation
}
}
Loading

0 comments on commit 1dafe6c

Please sign in to comment.