|
| 1 | +package dev.slimevr.tracking.processor.skeleton |
| 2 | + |
| 3 | +import dev.slimevr.tracking.processor.Bone |
| 4 | +import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType |
| 5 | +import dev.slimevr.tracking.trackers.Tracker |
| 6 | +import io.github.axisangles.ktmath.Quaternion |
| 7 | +import io.github.axisangles.ktmath.Vector3 |
| 8 | +import kotlin.math.* |
| 9 | + |
| 10 | +/* |
| 11 | + * This class implements a chain of Bones |
| 12 | + */ |
| 13 | + |
| 14 | +class IKChain( |
| 15 | + val bones: MutableList<Bone>, |
| 16 | + var parent: IKChain?, |
| 17 | + val level: Int, |
| 18 | + val baseConstraint: Tracker?, |
| 19 | + val tailConstraint: Tracker?, |
| 20 | +) { |
| 21 | + // State variables |
| 22 | + private val computedBasePosition = baseConstraint?.let { IKConstraint(it) } |
| 23 | + private val computedTailPosition = tailConstraint?.let { IKConstraint(it) } |
| 24 | + var children = mutableListOf<IKChain>() |
| 25 | + var target = Vector3.NULL |
| 26 | + var distToTargetSqr = Float.POSITIVE_INFINITY |
| 27 | + private var rotations = getRotationsList() |
| 28 | + |
| 29 | + private fun getRotationsList(): MutableList<Quaternion> { |
| 30 | + val rotList = mutableListOf<Quaternion>() |
| 31 | + for (b in bones) { |
| 32 | + rotList.add(b.getGlobalRotation()) |
| 33 | + } |
| 34 | + |
| 35 | + return rotList |
| 36 | + } |
| 37 | + |
| 38 | + /* |
| 39 | + * Populate the non-static rotations with the solve angle from the last iteration |
| 40 | + */ |
| 41 | + private fun prepBones() { |
| 42 | + for (i in 0..<bones.size) { |
| 43 | + if (bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE) { |
| 44 | + bones[i].setRotationRaw(rotations[i]) |
| 45 | + } |
| 46 | + } |
| 47 | + } |
| 48 | + |
| 49 | + fun backwardsCCDIK() { |
| 50 | + target = computedTailPosition?.getPosition() ?: getChildTargetAvg() |
| 51 | + var offset = Vector3.NULL |
| 52 | + |
| 53 | + for (i in bones.size - 1 downTo 0) { |
| 54 | + val currentBone = bones[i] |
| 55 | + |
| 56 | + // Get the local position of the end effector and the target relative to the current node |
| 57 | + val endEffectorLocal = ((getEndEffectorsAvg() - offset) - currentBone.getPosition()).unit() |
| 58 | + val targetLocal = ((target - offset) - currentBone.getPosition()).unit() |
| 59 | + |
| 60 | + // Compute the axis of rotation and angle for this bone |
| 61 | + var scalar = IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.hasTrackerRotation) IKSolver.STATIC_DAMPENING else 1f |
| 62 | + scalar *= ((bones.size - i).toFloat() / bones.size).pow(IKSolver.ANNEALING_EXPONENT) |
| 63 | + val adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit() |
| 64 | + |
| 65 | + val rotation = currentBone.getGlobalRotation() |
| 66 | + var correctedRot = (adjustment * rotation).unit() |
| 67 | + |
| 68 | + // Bones that are not supposed to be modified should tend towards their origin |
| 69 | + if (!currentBone.rotationConstraint.allowModifications) { |
| 70 | + correctedRot = correctedRot.interpR(currentBone.rotationConstraint.initialRotation, IKSolver.CORRECTION_FACTOR) |
| 71 | + } |
| 72 | + rotations[i] = setBoneRotation(currentBone, correctedRot) |
| 73 | + |
| 74 | + if (currentBone.rotationConstraint.hasTrackerRotation) { |
| 75 | + offset += rotations[i].sandwich(Vector3.NEG_Y) * currentBone.length |
| 76 | + } |
| 77 | + } |
| 78 | + } |
| 79 | + |
| 80 | + private fun getEndEffectorsAvg(): Vector3 { |
| 81 | + if (children.size < 1 || computedTailPosition != null) return bones.last().getTailPosition() |
| 82 | + |
| 83 | + var sum = Vector3.NULL |
| 84 | + for (c in children) { |
| 85 | + sum += c.getEndEffectorsAvg() |
| 86 | + } |
| 87 | + |
| 88 | + return sum / children.size.toFloat() |
| 89 | + } |
| 90 | + |
| 91 | + private fun getChildTargetAvg(): Vector3 { |
| 92 | + if (computedTailPosition != null) return computedTailPosition.getPosition() |
| 93 | + |
| 94 | + var sum = Vector3.NULL |
| 95 | + for (c in children) { |
| 96 | + sum += c.getChildTargetAvg() |
| 97 | + } |
| 98 | + |
| 99 | + return sum / children.size.toFloat() |
| 100 | + } |
| 101 | + |
| 102 | + /** |
| 103 | + * Resets the chain to its default state |
| 104 | + */ |
| 105 | + fun resetChain() { |
| 106 | + distToTargetSqr = Float.POSITIVE_INFINITY |
| 107 | + |
| 108 | + for (b in bones) { |
| 109 | + b.rotationConstraint.initialRotation = b.getGlobalRotation() |
| 110 | + } |
| 111 | + prepBones() |
| 112 | + |
| 113 | + for (child in children) { |
| 114 | + child.resetChain() |
| 115 | + } |
| 116 | + } |
| 117 | + |
| 118 | + fun resetTrackerOffsets() { |
| 119 | + computedTailPosition?.reset(bones.last().getTailPosition()) |
| 120 | + computedBasePosition?.reset(bones.first().getPosition()) |
| 121 | + } |
| 122 | + |
| 123 | + /** |
| 124 | + * Updates the distance to target and other fields |
| 125 | + * Call on the root chain |
| 126 | + */ |
| 127 | + fun computeTargetDistance() { |
| 128 | + distToTargetSqr = if (computedTailPosition != null) { |
| 129 | + (bones.last().getTailPosition() - computedTailPosition.getPosition()).lenSq() |
| 130 | + } else { |
| 131 | + 0.0f |
| 132 | + } |
| 133 | + |
| 134 | + for (chain in children) { |
| 135 | + chain.computeTargetDistance() |
| 136 | + } |
| 137 | + } |
| 138 | + |
| 139 | + /** |
| 140 | + * Sets a bones rotation after constraining the rotation |
| 141 | + * to the bone's rotational constraint |
| 142 | + * returns the constrained rotation |
| 143 | + */ |
| 144 | + private fun setBoneRotation(bone: Bone, rotation: Quaternion): Quaternion { |
| 145 | + // Constrain relative to the parent |
| 146 | + val newRotation = if (bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) { |
| 147 | + bone.rotationConstraint.applyConstraint(rotation, bone) |
| 148 | + } else if (!bone.rotationConstraint.hasTrackerRotation) { |
| 149 | + bone.rotationConstraint.applyConstraint(rotation, bone) |
| 150 | + } else { |
| 151 | + bone.rotationConstraint.constrainToInitialRotation(rotation) |
| 152 | + } |
| 153 | + |
| 154 | + bone.setRotationRaw(newRotation) |
| 155 | + bone.update() |
| 156 | + |
| 157 | + return newRotation |
| 158 | + } |
| 159 | +} |
0 commit comments