From 533750b0b6c54d791debf44d9814912bf82a5104 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Tue, 19 Sep 2023 16:49:13 -0700 Subject: [PATCH 01/68] commit starting code --- .../processor/skeleton/HumanSkeleton.kt | 5 + .../tracking/processor/skeleton/IKSolver.kt | 247 ++++++++++++++++++ 2 files changed, 252 insertions(+) create mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index ac8fc42aaa..085fbe8d68 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -147,6 +147,7 @@ class HumanSkeleton( var tapDetectionManager = TapDetectionManager(this) var viveEmulation = ViveEmulation(this) var localizer = Localizer(this) + var ikSolver = IKSolver(headBone) // Constructors init { @@ -304,6 +305,9 @@ class HumanSkeleton( // Update tap detection's trackers tapDetectionManager.updateConfig(trackers) + + // Rebuild Ik Solver + ikSolver.buildChains(trackers) } /** @@ -362,6 +366,7 @@ class HumanSkeleton( updateTransforms() updateBones() + ikSolver.solve() updateComputedTrackers() // Don't run post-processing if the tracking is paused diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt new file mode 100644 index 0000000000..edac9b8e30 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -0,0 +1,247 @@ +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 + +/* + * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) + */ + +class IKSolver(val root: Bone) { + var chainList = mutableListOf() + var rootChain: IKChain? = null + + + fun buildChains(trackers: List) { + chainList = mutableListOf() + + // extract the positional constraints + val constraints = extractConstraints(trackers) + + // build the system of chains + rootChain = chainBuilder(root, null, 0) + chainList.sortBy { -it.level } + + println(chainList.size) + } + + // convert the skeleton in to a system of chains + // a break in a chain is created at any point that has either + // multiple children or is positionally constrained, useless chains are discarded + private fun chainBuilder(root: Bone, parent: IKChain?, level: Int): IKChain { + val boneList = mutableListOf() + + var currentBone = root + boneList.add(currentBone) + while(currentBone.children.size == 1) { + currentBone = currentBone.children[0] + boneList.add(currentBone) + } + + val chain = IKChain(boneList, parent, level, null, null) + + if (currentBone.children.isNotEmpty()) { + val childrenList = mutableListOf() + + // build child chains + for (child in currentBone.children) { + val childChain = chainBuilder(child, chain, level + 1) + if (neededChain(childChain)) { + childrenList.add(childChain) + } + } + chain.children = childrenList + } + + if (chain.children.isNotEmpty() || chain.tailConstraint != null) + chainList.add(chain) + + return chain + } + + // a chain is needed if there is a positional constraint in its children + private fun neededChain(ikChain: IKChain): Boolean { + if (ikChain.children.isEmpty() && ikChain.tailConstraint == null) + return false + + for (c in ikChain.children) { + if (c.tailConstraint != null) + return true + + if (neededChain(c)) + return true + } + + return false + } + + private fun extractConstraints(trackers: List): MutableList { + val constraintList = mutableListOf() + + // each tracker that has position is a positional constraint + for (t in trackers) { + if (t.hasPosition) + constraintList.add(IKConstraint(t)) + } + + return constraintList + } + + fun solve() { + for (chain in chainList) { + chain.updatePositions() + } + + // for now run 10 iterations per tick + for (i in 0..100) { + + //println("iter $i") + for (chain in chainList) { + chain.backwards() + } + rootChain?.forwardsMulti() + + } + + // update transforms + root.update() + + } +} + +class IKChain(val nodes: MutableList, val parent: IKChain?, val level: Int, + val baseConstraint: IKConstraint? , val tailConstraint: IKConstraint?) { + val baseNode = nodes.first() + val tailNode = nodes.last() + var children = mutableListOf() + private var positions = getPositionList() + + var target = Vector3.NULL + val squThreshold = 0.01f + + private fun getPositionList(): MutableList { + val posList = mutableListOf() + for (n in nodes) { + posList.add(n.getPosition()) + } + posList.add(nodes.last().getTailPosition()) + + return posList + } + + fun updatePositions() { + for (i in nodes.indices) { + positions[i] = nodes[i].getPosition() + } + positions[positions.size - 1] = tailNode.getTailPosition() + } + + fun backwards() { + val origin = baseNode.getPosition() + + if (children.size > 1 && tailConstraint == null) { + target /= children.size.toFloat() + } + else { + // TODO set constraints here + target = Vector3.NULL + } + + if ((positions.last() - target).lenSq() > squThreshold) { + //println((positions.last() - target).lenSq()) + + // set the end node to target + positions[positions.size - 1] = target + + for (i in positions.size - 2 downTo 0) { + val direction = (positions[i] - positions[i + 1]).unit() + positions[i] = positions[i + 1] + (direction * nodes[i].length) + } + } + + if (parent != null && parent.tailConstraint == null) { + parent.target += positions[0] + } + + // restore the base position + positions[0] = origin + + } + + private fun forwards() { + for (i in 1 until positions.size - 1) { + val direction = (positions[i] - positions[i - 1]).unit() + + // TODO apply constraints here + setBoneRotation(nodes[i - 1], direction, i - 1) + + positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) + } + + // if there are children and the tail of this chain is not constrained + // we must average the children direction and use this to set the + // starting point for the children + if (children.isNotEmpty() && tailConstraint == null) { + // reset target for next pass + target = Vector3.NULL + + // average the children positions + var direction = Vector3.NULL + for (c in children) { + direction += (c.positions[0] - positions[positions.size - 2]).unit() + } + + // set the last position using direction + direction /= children.size.toFloat() + positions[positions.size - 1] = positions[positions.size - 2] + (direction * tailNode.length) + + // set the children start positions for their forward passes // TODO investigate + for (c in children) { + c.positions[0] = positions[positions.size - 1] + } + + // update the Bone + setBoneRotation(tailNode, direction, nodes.size - 1) + } + else if (children.isEmpty()) { + val direction = (target - positions[positions.size - 2]).unit() + positions[positions.size - 1] = positions[positions.size - 2] + (direction * tailNode.length) + + setBoneRotation(tailNode, direction, nodes.size - 1) + } + + } + + fun forwardsMulti() { + forwards() + + for (c in (children)) { + c.forwardsMulti() + } + + } + + private fun setBoneRotation(bone: Bone, rotationVector: Vector3, nodeIdx: Int) { +// val parentRot = if (nodeIdx == 0) { +// parent?.nodes?.last()?.getGlobalRotation() ?: baseNode.getGlobalRotation() +// } +// else ( +// nodes[nodeIdx - 1].getGlobalRotation() +// ) +// +// val globalRotation = Quaternion.fromRotationVector(rotationVector) +// val localRotation = parentRot.inv() * globalRotation +// +// bone.setRotation(localRotation) + + bone.setRotation(Quaternion.fromRotationVector(rotationVector)) + + } +} + +class IKConstraint(val tracker: Tracker) { + + +} From 624628ce1ea10d71fd085692a3fe640f7a9f2795 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 25 Sep 2023 15:47:56 -0700 Subject: [PATCH 02/68] more bug fixes --- .../tracking/processor/skeleton/IKSolver.kt | 219 ++++++++++-------- 1 file changed, 126 insertions(+), 93 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index edac9b8e30..a8416c91b9 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -13,7 +13,6 @@ class IKSolver(val root: Bone) { var chainList = mutableListOf() var rootChain: IKChain? = null - fun buildChains(trackers: List) { chainList = mutableListOf() @@ -21,33 +20,72 @@ class IKSolver(val root: Bone) { val constraints = extractConstraints(trackers) // build the system of chains - rootChain = chainBuilder(root, null, 0) + rootChain = chainBuilder(root, null, 0, constraints) + populateChainList(rootChain!!) + + // check if there is any constraints (other than the head) in the model + rootChain = if (neededChain(rootChain!!)) rootChain else null chainList.sortBy { -it.level } + print("DEBUG chainList.size = ") println(chainList.size) } + // prints the system of chains + private fun printer(root: IKChain?) { + if (root == null) { + return + } + + for (n in root.nodes) { + println(n.boneType.name) + println(n.length) + } + + println("\n") + var lastPos = Vector3.NULL + for (n in root.positions) { + println((n - lastPos).len()) + println(n) + lastPos = n + } + + println("\n") + println(root.tailConstraint?.tracker?.position) + + for (c in root.children) { + printer(c) + } + } + // convert the skeleton in to a system of chains // a break in a chain is created at any point that has either // multiple children or is positionally constrained, useless chains are discarded - private fun chainBuilder(root: Bone, parent: IKChain?, level: Int): IKChain { + private fun chainBuilder(root: Bone, parent: IKChain?, level: Int, constraints: MutableList): IKChain { val boneList = mutableListOf() - var currentBone = root boneList.add(currentBone) - while(currentBone.children.size == 1) { + + // get constraints + val baseConstraint = if (parent == null) getConstraint(boneList.first(), constraints) + else parent.tailConstraint + var tailConstraint: IKConstraint? = null + + // add bones until there is a reason to make a new chain + while(currentBone.children.size == 1 && tailConstraint == null) { currentBone = currentBone.children[0] boneList.add(currentBone) + tailConstraint = getConstraint(boneList.last(), constraints) } - val chain = IKChain(boneList, parent, level, null, null) + var chain = IKChain(boneList, parent, level, baseConstraint, tailConstraint) if (currentBone.children.isNotEmpty()) { val childrenList = mutableListOf() // build child chains for (child in currentBone.children) { - val childChain = chainBuilder(child, chain, level + 1) + val childChain = chainBuilder(child, chain, level + 1, constraints) if (neededChain(childChain)) { childrenList.add(childChain) } @@ -55,18 +93,44 @@ class IKSolver(val root: Bone) { chain.children = childrenList } - if (chain.children.isNotEmpty() || chain.tailConstraint != null) - chainList.add(chain) + // if the chain has only one child and no tail constraint combine the chains + if (chain.children.size == 1 && chain.tailConstraint == null) + chain = combineChains(chain, chain.children[0]) return chain } + private fun populateChainList(chain: IKChain) { + chainList.add(chain) + for (c in chain.children) { + populateChainList(c) + } + } + + private fun combineChains(chain: IKChain, childChain: IKChain): IKChain { + val boneList = mutableListOf() + boneList.addAll(chain.nodes) + boneList.addAll(childChain.nodes) + + val newChain = IKChain(boneList, chain.parent, chain.level, + chain.baseConstraint, childChain.tailConstraint) + + newChain.children = childChain.children + + for (c in newChain.children) { + c.parent = newChain + } + + return newChain + } + // a chain is needed if there is a positional constraint in its children - private fun neededChain(ikChain: IKChain): Boolean { - if (ikChain.children.isEmpty() && ikChain.tailConstraint == null) - return false + private fun neededChain(chain: IKChain): Boolean { + if (chain.tailConstraint != null) { + return true + } - for (c in ikChain.children) { + for (c in chain.children) { if (c.tailConstraint != null) return true @@ -82,44 +146,51 @@ class IKSolver(val root: Bone) { // each tracker that has position is a positional constraint for (t in trackers) { - if (t.hasPosition) + if (t.hasPosition && !t.isInternal && + !t.status.reset) constraintList.add(IKConstraint(t)) } - return constraintList } - fun solve() { - for (chain in chainList) { - chain.updatePositions() + private fun getConstraint(bone: Bone, constraints: MutableList): IKConstraint? { + for (c in constraints) { + if (bone.boneType.bodyPart == (c.tracker.trackerPosition?.bodyPart ?: 0)) { + constraints.remove(c) + return c + } } + return null + } + fun solve() { // for now run 10 iterations per tick - for (i in 0..100) { - - //println("iter $i") + for (i in 0..10) { for (chain in chainList) { chain.backwards() } rootChain?.forwardsMulti() - } - // update transforms root.update() + // print distances to the root constraints + for (c in chainList) { + println("\n") + println(c.tailConstraint?.tracker?.position) + println(c.positions.last()) + } + } } -class IKChain(val nodes: MutableList, val parent: IKChain?, val level: Int, +class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int, val baseConstraint: IKConstraint? , val tailConstraint: IKConstraint?) { - val baseNode = nodes.first() - val tailNode = nodes.last() var children = mutableListOf() - private var positions = getPositionList() - + var positions = getPositionList() var target = Vector3.NULL - val squThreshold = 0.01f + + val squThreshold = 0.0001f private fun getPositionList(): MutableList { val posList = mutableListOf() @@ -135,23 +206,17 @@ class IKChain(val nodes: MutableList, val parent: IKChain?, val level: Int for (i in nodes.indices) { positions[i] = nodes[i].getPosition() } - positions[positions.size - 1] = tailNode.getTailPosition() + positions[positions.size - 1] = nodes.last().getTailPosition() } fun backwards() { - val origin = baseNode.getPosition() - - if (children.size > 1 && tailConstraint == null) { + // start at the constraint or the centroid of the children + if (tailConstraint == null && children.size > 1) target /= children.size.toFloat() - } - else { - // TODO set constraints here - target = Vector3.NULL - } + else + target = tailConstraint?.tracker?.position ?: Vector3.NULL if ((positions.last() - target).lenSq() > squThreshold) { - //println((positions.last() - target).lenSq()) - // set the end node to target positions[positions.size - 1] = target @@ -161,83 +226,51 @@ class IKChain(val nodes: MutableList, val parent: IKChain?, val level: Int } } - if (parent != null && parent.tailConstraint == null) { - parent.target += positions[0] - } - - // restore the base position - positions[0] = origin - + if (parent != null) + parent!!.target += positions[0] } private fun forwards() { + if (baseConstraint != null) + positions[0] = baseConstraint.tracker.position + else if (parent != null) + positions[0] = parent!!.positions.last() + for (i in 1 until positions.size - 1) { val direction = (positions[i] - positions[i - 1]).unit() // TODO apply constraints here - setBoneRotation(nodes[i - 1], direction, i - 1) + setBoneRotation(nodes[i - 1], direction) positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) } - // if there are children and the tail of this chain is not constrained - // we must average the children direction and use this to set the - // starting point for the children + // set the last node position to point at the centroid of the children + // or the positional constraint if (children.isNotEmpty() && tailConstraint == null) { - // reset target for next pass - target = Vector3.NULL - - // average the children positions - var direction = Vector3.NULL - for (c in children) { - direction += (c.positions[0] - positions[positions.size - 2]).unit() - } - - // set the last position using direction - direction /= children.size.toFloat() - positions[positions.size - 1] = positions[positions.size - 2] + (direction * tailNode.length) - - // set the children start positions for their forward passes // TODO investigate - for (c in children) { - c.positions[0] = positions[positions.size - 1] - } - - // update the Bone - setBoneRotation(tailNode, direction, nodes.size - 1) - } - else if (children.isEmpty()) { val direction = (target - positions[positions.size - 2]).unit() - positions[positions.size - 1] = positions[positions.size - 2] + (direction * tailNode.length) - - setBoneRotation(tailNode, direction, nodes.size - 1) + positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length) + setBoneRotation(nodes.last(), direction) + } + else if (tailConstraint != null) { + val direction = (tailConstraint.tracker.position - positions[positions.size - 2]).unit() + positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length) + setBoneRotation(nodes.last(), direction) } + target = Vector3.NULL } fun forwardsMulti() { forwards() - for (c in (children)) { + for (c in children) { c.forwardsMulti() } - } - private fun setBoneRotation(bone: Bone, rotationVector: Vector3, nodeIdx: Int) { -// val parentRot = if (nodeIdx == 0) { -// parent?.nodes?.last()?.getGlobalRotation() ?: baseNode.getGlobalRotation() -// } -// else ( -// nodes[nodeIdx - 1].getGlobalRotation() -// ) -// -// val globalRotation = Quaternion.fromRotationVector(rotationVector) -// val localRotation = parentRot.inv() * globalRotation -// -// bone.setRotation(localRotation) - - bone.setRotation(Quaternion.fromRotationVector(rotationVector)) - + private fun setBoneRotation(bone: Bone, rotationVector: Vector3) { + bone.setRotation(Quaternion.fromTo(Vector3.NEG_Y, rotationVector)) } } From 607fb3b4f19d787fd2086fdd05e7473545232b69 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Tue, 26 Sep 2023 12:10:20 -0700 Subject: [PATCH 03/68] it works! --- .../dev/slimevr/tracking/processor/Bone.kt | 7 +++ .../tracking/processor/skeleton/IKSolver.kt | 50 ++----------------- 2 files changed, 11 insertions(+), 46 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt index 3a291f49a4..9ea103c740 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt @@ -75,6 +75,13 @@ class Bone(val boneType: BoneType) { headNode.localTransform.rotation = rotation * rotationOffset } + /** + * Sets the global rotation of the bone without applying the rotation offset + */ + fun setRotationRaw(rotation: Quaternion) { + headNode.localTransform.rotation = rotation + } + /** * Returns the global position of the head of the bone */ diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index a8416c91b9..f51997ec07 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -31,33 +31,6 @@ class IKSolver(val root: Bone) { println(chainList.size) } - // prints the system of chains - private fun printer(root: IKChain?) { - if (root == null) { - return - } - - for (n in root.nodes) { - println(n.boneType.name) - println(n.length) - } - - println("\n") - var lastPos = Vector3.NULL - for (n in root.positions) { - println((n - lastPos).len()) - println(n) - lastPos = n - } - - println("\n") - println(root.tailConstraint?.tracker?.position) - - for (c in root.children) { - printer(c) - } - } - // convert the skeleton in to a system of chains // a break in a chain is created at any point that has either // multiple children or is positionally constrained, useless chains are discarded @@ -164,8 +137,8 @@ class IKSolver(val root: Bone) { } fun solve() { - // for now run 10 iterations per tick - for (i in 0..10) { + // for now run 100 iterations per tick + for (i in 0..100) { for (chain in chainList) { chain.backwards() } @@ -173,14 +146,6 @@ class IKSolver(val root: Bone) { } // update transforms root.update() - - // print distances to the root constraints - for (c in chainList) { - println("\n") - println(c.tailConstraint?.tracker?.position) - println(c.positions.last()) - } - } } @@ -190,7 +155,7 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int var positions = getPositionList() var target = Vector3.NULL - val squThreshold = 0.0001f + private val squThreshold = 0.00001f private fun getPositionList(): MutableList { val posList = mutableListOf() @@ -202,13 +167,6 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int return posList } - fun updatePositions() { - for (i in nodes.indices) { - positions[i] = nodes[i].getPosition() - } - positions[positions.size - 1] = nodes.last().getTailPosition() - } - fun backwards() { // start at the constraint or the centroid of the children if (tailConstraint == null && children.size > 1) @@ -270,7 +228,7 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int } private fun setBoneRotation(bone: Bone, rotationVector: Vector3) { - bone.setRotation(Quaternion.fromTo(Vector3.NEG_Y, rotationVector)) + bone.setRotationRaw(Quaternion.fromTo(Vector3.NEG_Y, rotationVector).unit()) } } From 09dbb0d6e58e5ec44554761124de5d3843c8b620 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Tue, 26 Sep 2023 13:41:56 -0700 Subject: [PATCH 04/68] simplify forward pass --- .../tracking/processor/skeleton/IKSolver.kt | 35 +++++++++---------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index f51997ec07..d3602788ec 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -116,8 +116,6 @@ class IKSolver(val root: Bone) { private fun extractConstraints(trackers: List): MutableList { val constraintList = mutableListOf() - - // each tracker that has position is a positional constraint for (t in trackers) { if (t.hasPosition && !t.isInternal && !t.status.reset) @@ -137,12 +135,15 @@ class IKSolver(val root: Bone) { } fun solve() { - // for now run 100 iterations per tick + // run up to 100 iterations per tick for (i in 0..100) { for (chain in chainList) { chain.backwards() } rootChain?.forwardsMulti() + + if (chainList.all {it.solved}) + break } // update transforms root.update() @@ -151,11 +152,13 @@ class IKSolver(val root: Bone) { class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int, val baseConstraint: IKConstraint? , val tailConstraint: IKConstraint?) { + private val squThreshold = 0.00001f + + // state variables var children = mutableListOf() - var positions = getPositionList() var target = Vector3.NULL - - private val squThreshold = 0.00001f + var solved = false + private var positions = getPositionList() private fun getPositionList(): MutableList { val posList = mutableListOf() @@ -182,6 +185,9 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int val direction = (positions[i] - positions[i + 1]).unit() positions[i] = positions[i + 1] + (direction * nodes[i].length) } + solved = false + } else { + solved = true } if (parent != null) @@ -203,19 +209,12 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) } - // set the last node position to point at the centroid of the children - // or the positional constraint - if (children.isNotEmpty() && tailConstraint == null) { - val direction = (target - positions[positions.size - 2]).unit() - positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length) - setBoneRotation(nodes.last(), direction) - } - else if (tailConstraint != null) { - val direction = (tailConstraint.tracker.position - positions[positions.size - 2]).unit() - positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length) - setBoneRotation(nodes.last(), direction) - } + // point the last bone at the target + val direction = (target - positions[positions.size - 2]).unit() + positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length) + setBoneRotation(nodes.last(), direction) + // reset sub-base target target = Vector3.NULL } From d34b1e5de3c7f4ee2f38918d7d16fbc6ab6333b2 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Thu, 19 Oct 2023 11:02:09 -0700 Subject: [PATCH 05/68] remove pointless class --- .../tracking/processor/skeleton/IKSolver.kt | 32 +++++++------------ 1 file changed, 12 insertions(+), 20 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index d3602788ec..d977860e34 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -26,15 +26,12 @@ class IKSolver(val root: Bone) { // check if there is any constraints (other than the head) in the model rootChain = if (neededChain(rootChain!!)) rootChain else null chainList.sortBy { -it.level } - - print("DEBUG chainList.size = ") - println(chainList.size) } // convert the skeleton in to a system of chains // a break in a chain is created at any point that has either // multiple children or is positionally constrained, useless chains are discarded - private fun chainBuilder(root: Bone, parent: IKChain?, level: Int, constraints: MutableList): IKChain { + private fun chainBuilder(root: Bone, parent: IKChain?, level: Int, constraints: MutableList): IKChain { val boneList = mutableListOf() var currentBone = root boneList.add(currentBone) @@ -42,7 +39,7 @@ class IKSolver(val root: Bone) { // get constraints val baseConstraint = if (parent == null) getConstraint(boneList.first(), constraints) else parent.tailConstraint - var tailConstraint: IKConstraint? = null + var tailConstraint: Tracker? = null // add bones until there is a reason to make a new chain while(currentBone.children.size == 1 && tailConstraint == null) { @@ -114,19 +111,19 @@ class IKSolver(val root: Bone) { return false } - private fun extractConstraints(trackers: List): MutableList { - val constraintList = mutableListOf() + private fun extractConstraints(trackers: List): MutableList { + val constraintList = mutableListOf() for (t in trackers) { if (t.hasPosition && !t.isInternal && !t.status.reset) - constraintList.add(IKConstraint(t)) + constraintList.add(t) } return constraintList } - private fun getConstraint(bone: Bone, constraints: MutableList): IKConstraint? { + private fun getConstraint(bone: Bone, constraints: MutableList): Tracker? { for (c in constraints) { - if (bone.boneType.bodyPart == (c.tracker.trackerPosition?.bodyPart ?: 0)) { + if (bone.boneType.bodyPart == (c.trackerPosition?.bodyPart ?: 0)) { constraints.remove(c) return c } @@ -151,7 +148,7 @@ class IKSolver(val root: Bone) { } class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int, - val baseConstraint: IKConstraint? , val tailConstraint: IKConstraint?) { + val baseConstraint: Tracker? , val tailConstraint: Tracker?) { private val squThreshold = 0.00001f // state variables @@ -175,7 +172,7 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int if (tailConstraint == null && children.size > 1) target /= children.size.toFloat() else - target = tailConstraint?.tracker?.position ?: Vector3.NULL + target = tailConstraint?.position ?: Vector3.NULL if ((positions.last() - target).lenSq() > squThreshold) { // set the end node to target @@ -196,14 +193,13 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int private fun forwards() { if (baseConstraint != null) - positions[0] = baseConstraint.tracker.position + positions[0] = baseConstraint.position else if (parent != null) positions[0] = parent!!.positions.last() for (i in 1 until positions.size - 1) { val direction = (positions[i] - positions[i - 1]).unit() - // TODO apply constraints here setBoneRotation(nodes[i - 1], direction) positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) @@ -227,11 +223,7 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int } private fun setBoneRotation(bone: Bone, rotationVector: Vector3) { - bone.setRotationRaw(Quaternion.fromTo(Vector3.NEG_Y, rotationVector).unit()) + val rotation = bone.constraint.applyConstraint(rotationVector, bone.parent) + bone.setRotationRaw(rotation) } } - -class IKConstraint(val tracker: Tracker) { - - -} From 4bcac947b118f4fb02c0d4e6baeeb058fc513022 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Thu, 19 Oct 2023 11:02:26 -0700 Subject: [PATCH 06/68] add Constraint --- .../dev/slimevr/tracking/processor/Bone.kt | 2 +- .../slimevr/tracking/processor/Constraint.kt | 72 +++++++++++++++++++ 2 files changed, 73 insertions(+), 1 deletion(-) create mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt index 9ea103c740..ee993f9e98 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt @@ -76,7 +76,7 @@ class Bone(val boneType: BoneType) { } /** - * Sets the global rotation of the bone without applying the rotation offset + * Sets the global rotation of the bone directly */ fun setRotationRaw(rotation: Quaternion) { headNode.localTransform.rotation = rotation diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt new file mode 100644 index 0000000000..6e368435f7 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -0,0 +1,72 @@ +package dev.slimevr.tracking.processor + +import com.jme3.math.FastMath +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 + +/** + * Represents the rotational limits of a Bone relative to its parent + */ +class Constraint(val swing: Float, val twist: Float) { + val upAxisConstraint = Vector3.POS_Y + val forwardAxisConstraint = Vector3.POS_X + + private fun project(vector: Vector3, onNormal: Vector3): Vector3 { + val sqrMag = onNormal.dot(onNormal) + val dot = vector.dot(onNormal) + + return Vector3(onNormal.x * dot / sqrMag, + onNormal.y * dot / sqrMag, + onNormal.z * dot / sqrMag) + + } + + private fun decompose(rotation: Quaternion, direction: Vector3): Pair { + val vector = Vector3(rotation.x, rotation.y, rotation.z) + val projection = project(vector, direction) + + val twist = Quaternion(rotation.w, projection.x, projection.y, projection.z).unit() + val swing = rotation * twist.inv() + + return Pair(swing, twist) + } + + private fun constrain(rotation: Quaternion, angle: Float): Quaternion { + val length = FastMath.sin(angle) + val sqrLength = length * length + var vector = Vector3(rotation.x, rotation.y, rotation.z) + var rot = rotation + + if (vector.lenSq() > sqrLength) { + vector = vector.unit() * length + rot = Quaternion(FastMath.sqrt(1.0f - sqrLength) * FastMath.sign(rot.w), + vector.x, vector.y, vector.z) + } + + return rot + } + + fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion { + // if there is no parent or no constraint return the direction + if (parent == null || (swing.isNaN() && twist.isNaN())) { + return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() + } + + // get the local rotation + val rotationGlobal = Quaternion.fromTo(Vector3.NEG_Y, direction).unit() + val rotationLocal = parent.getGlobalRotation().inv() * rotationGlobal + + var (swingQ, twistQ) = decompose(rotationLocal, direction) + + // apply the constraints + if (!swing.isNaN()) { + swingQ = constrain(swingQ, swing) + } + + if (!twist.isNaN()) { + twistQ = constrain(twistQ, twist) + } + + return parent.getGlobalRotation() * swingQ * twistQ + } +} From bd41b5ae39e2bbc6fa364ab8182ad4f32a98c45c Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 22 Dec 2023 17:57:44 -0800 Subject: [PATCH 07/68] Make Constraint abstract to allow multiple constraint types to be added --- .../slimevr/tracking/processor/Constraint.kt | 65 +------------------ .../tracking/processor/HingeConstraint.kt | 10 +++ .../processor/TwistSwingConstraint.kt | 63 ++++++++++++++++++ 3 files changed, 75 insertions(+), 63 deletions(-) create mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt create mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 6e368435f7..e3a49c95a2 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -1,72 +1,11 @@ package dev.slimevr.tracking.processor -import com.jme3.math.FastMath import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 /** * Represents the rotational limits of a Bone relative to its parent */ -class Constraint(val swing: Float, val twist: Float) { - val upAxisConstraint = Vector3.POS_Y - val forwardAxisConstraint = Vector3.POS_X - - private fun project(vector: Vector3, onNormal: Vector3): Vector3 { - val sqrMag = onNormal.dot(onNormal) - val dot = vector.dot(onNormal) - - return Vector3(onNormal.x * dot / sqrMag, - onNormal.y * dot / sqrMag, - onNormal.z * dot / sqrMag) - - } - - private fun decompose(rotation: Quaternion, direction: Vector3): Pair { - val vector = Vector3(rotation.x, rotation.y, rotation.z) - val projection = project(vector, direction) - - val twist = Quaternion(rotation.w, projection.x, projection.y, projection.z).unit() - val swing = rotation * twist.inv() - - return Pair(swing, twist) - } - - private fun constrain(rotation: Quaternion, angle: Float): Quaternion { - val length = FastMath.sin(angle) - val sqrLength = length * length - var vector = Vector3(rotation.x, rotation.y, rotation.z) - var rot = rotation - - if (vector.lenSq() > sqrLength) { - vector = vector.unit() * length - rot = Quaternion(FastMath.sqrt(1.0f - sqrLength) * FastMath.sign(rot.w), - vector.x, vector.y, vector.z) - } - - return rot - } - - fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion { - // if there is no parent or no constraint return the direction - if (parent == null || (swing.isNaN() && twist.isNaN())) { - return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() - } - - // get the local rotation - val rotationGlobal = Quaternion.fromTo(Vector3.NEG_Y, direction).unit() - val rotationLocal = parent.getGlobalRotation().inv() * rotationGlobal - - var (swingQ, twistQ) = decompose(rotationLocal, direction) - - // apply the constraints - if (!swing.isNaN()) { - swingQ = constrain(swingQ, swing) - } - - if (!twist.isNaN()) { - twistQ = constrain(twistQ, twist) - } - - return parent.getGlobalRotation() * swingQ * twistQ - } +abstract class Constraint { + abstract fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt new file mode 100644 index 0000000000..85ccacd241 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt @@ -0,0 +1,10 @@ +package dev.slimevr.tracking.processor + +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 + +class HingeConstraint(val min: Float, val max: Float) : Constraint() { + override fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion { + return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt new file mode 100644 index 0000000000..769d30b519 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -0,0 +1,63 @@ +package dev.slimevr.tracking.processor + +import com.jme3.math.FastMath +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 + + +class TwistSwingConstraint(val twist: Float, val swing: Float) : Constraint() { + private fun project(vector: Vector3, onNormal: Vector3): Vector3 { + val sqrMag = onNormal.lenSq() + val dot = vector.dot(onNormal) + return onNormal * (dot / sqrMag) + } + + private fun decompose(rotation: Quaternion, twistAxis: Vector3): Pair { + val vector = Vector3(rotation.x, rotation.y, rotation.z) + val projection = project(vector, twistAxis) + + val twist = Quaternion(rotation.w, projection.x, projection.y, projection.z).unit() + val swing = rotation * twist.inv() + + return Pair(swing, twist) + } + + private fun constrain(rotation: Quaternion, angle: Float): Quaternion { + val length = FastMath.sin(Math.toRadians(angle.toDouble()).toFloat()) + val sqrLength = length * length + var vector = Vector3(rotation.x, rotation.y, rotation.z) + var rot = rotation + + if (vector.lenSq() > sqrLength) { + vector = vector.unit() * length + rot = Quaternion(FastMath.sqrt(1.0f - sqrLength) * FastMath.sign(rot.w), + vector.x, vector.y, vector.z) + } + + return rot + } + + override fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion { + // if there is no parent or no constraint return the direction + if (parent == null || (swing.isNaN() && twist.isNaN())) { + return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() + } + + // get the local rotation + val rotationGlobal = Quaternion.fromTo(Vector3.NEG_Y, direction).unit() + val rotationLocal = parent.getGlobalRotation().inv() * rotationGlobal + + var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) + + // apply the constraints + if (!swing.isNaN()) { + swingQ = constrain(swingQ, swing) + } + + if (!twist.isNaN()) { + twistQ = constrain(twistQ, twist) + } + + return parent.getGlobalRotation() * (swingQ * twistQ) * parent.getGlobalRotation().inv() + } +} From a57191d0d3186414245785b97e52ba5575a8a894 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 22 Dec 2023 17:58:25 -0800 Subject: [PATCH 08/68] bug fixes and move classes to own file --- .../tracking/processor/skeleton/IKChain.kt | 93 +++++++++++++++ .../tracking/processor/skeleton/IKSolver.kt | 106 ++++-------------- 2 files changed, 116 insertions(+), 83 deletions(-) create mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt new file mode 100644 index 0000000000..098150422b --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -0,0 +1,93 @@ +package dev.slimevr.tracking.processor.skeleton + +import dev.slimevr.tracking.processor.Bone +import dev.slimevr.tracking.trackers.Tracker +import io.github.axisangles.ktmath.Vector3 + +/* + * This class implements a chain of Bones for use by the FABRIK solver + */ + +class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int, + val baseConstraint: Tracker?, val tailConstraint: Tracker?) { + // state variables + var children = mutableListOf() + var target = Vector3.NULL + private var positions = getPositionList() + var firstBonePerturbation = 0 + + private fun getPositionList(): MutableList { + val posList = mutableListOf() + 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 + if (tailConstraint == null && children.size > 1) + target /= children.size.toFloat() + else + target = tailConstraint?.position ?: Vector3.NULL + + // set the end node to target + positions[positions.size - 1] = target + + for (i in positions.size - 2 downTo 0) { + val direction = (positions[i] - positions[i + 1]).unit() + positions[i] = positions[i + 1] + (direction * nodes[i].length) + } + + if (parent != null) + parent!!.target += positions[0] + } + + private fun forwards() { + if (baseConstraint != null) + positions[0] = baseConstraint.position + else if (parent != null) + positions[0] = parent!!.positions.last() + + 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) + } + + // point the last bone at the target + 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 + target = Vector3.NULL + } + + fun forwardsMulti() { + forwards() + + for (c in children) { + c.forwardsMulti() + } + } + + fun getTargetDistance(): Float { + return (positions.last() - (tailConstraint?.position ?: Vector3.NULL)).lenSq() + } + + // 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 + private fun setBoneRotation(bone: Bone, rotationVector: Vector3): Vector3 { + // TODO if a bone has a tracker associated with it force that rotation + + + val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone.parent) + bone.setRotationRaw(rotation) + + return rotation.sandwich(Vector3.NEG_Y) + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index d977860e34..318410532b 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -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 /* - * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) + * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) and allows + * positional trackers such as vive/tundra trackers to be used in conjunction with + * IMU trackers */ class IKSolver(val root: Bone) { + companion object { + private const val TOLERANCE = 0.001f + } + var chainList = mutableListOf() var rootChain: IKChain? = null @@ -132,6 +137,11 @@ class IKSolver(val root: Bone) { } fun solve() { + // get the distance from the target + var perturbationStrength = 0 + var distSqrToTarget = Float.POSITIVE_INFINITY + var lastDistSqrToTarget: Float + // run up to 100 iterations per tick for (i in 0..100) { for (chain in chainList) { @@ -139,91 +149,21 @@ class IKSolver(val root: Bone) { } rootChain?.forwardsMulti() - if (chainList.all {it.solved}) + lastDistSqrToTarget = distSqrToTarget + distSqrToTarget = chainList.first().getTargetDistance() + if (distSqrToTarget < TOLERANCE) break - } - // update transforms - root.update() - } -} - -class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int, - val baseConstraint: Tracker? , val tailConstraint: Tracker?) { - private val squThreshold = 0.00001f - - // state variables - var children = mutableListOf() - var target = Vector3.NULL - var solved = false - private var positions = getPositionList() - - private fun getPositionList(): MutableList { - val posList = mutableListOf() - 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 - if (tailConstraint == null && children.size > 1) - target /= children.size.toFloat() - else - target = tailConstraint?.position ?: Vector3.NULL - - if ((positions.last() - target).lenSq() > squThreshold) { - // set the end node to target - positions[positions.size - 1] = target - for (i in positions.size - 2 downTo 0) { - val direction = (positions[i] - positions[i + 1]).unit() - positions[i] = positions[i + 1] + (direction * nodes[i].length) + // If the distance to the target is not changing the algorithm might be in a deadlock + // to solve this introduce some perturbations in the bones closest to the target to allow + // the parent bones to bend more. + if (lastDistSqrToTarget == distSqrToTarget) { + perturbationStrength += 2 + chainList.first().firstBonePerturbation = perturbationStrength } - solved = false - } else { - solved = true } - if (parent != null) - parent!!.target += positions[0] - } - - private fun forwards() { - if (baseConstraint != null) - positions[0] = baseConstraint.position - else if (parent != null) - positions[0] = parent!!.positions.last() - - for (i in 1 until positions.size - 1) { - val direction = (positions[i] - positions[i - 1]).unit() - - setBoneRotation(nodes[i - 1], direction) - - positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) - } - - // point the last bone at the target - val direction = (target - positions[positions.size - 2]).unit() - positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length) - setBoneRotation(nodes.last(), direction) - - // reset sub-base target - target = Vector3.NULL - } - - fun forwardsMulti() { - forwards() - - for (c in children) { - c.forwardsMulti() - } - } - - private fun setBoneRotation(bone: Bone, rotationVector: Vector3) { - val rotation = bone.constraint.applyConstraint(rotationVector, bone.parent) - bone.setRotationRaw(rotation) + // update transforms + root.update() } } From 1ceba555fca6a54f4db03e04fcbfee9dc447da9b Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 22 Dec 2023 17:58:40 -0800 Subject: [PATCH 09/68] add constraints to each bone --- .../dev/slimevr/tracking/processor/Bone.kt | 2 +- .../processor/skeleton/HumanSkeleton.kt | 67 ++++++++++--------- 2 files changed, 35 insertions(+), 34 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt index ee993f9e98..f5a952abca 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt @@ -7,7 +7,7 @@ import java.util.concurrent.CopyOnWriteArrayList /** * Represents a bone composed of 2 joints: headNode and tailNode. */ -class Bone(val boneType: BoneType) { +class Bone(val boneType: BoneType, val rotationConstraint: Constraint) { private val headNode = TransformNode(true) private val tailNode = TransformNode(false) var parent: Bone? = null diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 085fbe8d68..69e40551ac 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -5,6 +5,7 @@ import dev.slimevr.VRServer import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.processor.BoneType import dev.slimevr.tracking.processor.HumanPoseManager +import dev.slimevr.tracking.processor.TwistSwingConstraint import dev.slimevr.tracking.processor.config.SkeletonConfigToggles import dev.slimevr.tracking.processor.config.SkeletonConfigValues import dev.slimevr.tracking.trackers.Tracker @@ -34,45 +35,45 @@ class HumanSkeleton( val humanPoseManager: HumanPoseManager, ) { // Upper body bones - val headBone = Bone(BoneType.HEAD) - val neckBone = Bone(BoneType.NECK) - val upperChestBone = Bone(BoneType.UPPER_CHEST) - val chestBone = Bone(BoneType.CHEST) - val waistBone = Bone(BoneType.WAIST) - val hipBone = Bone(BoneType.HIP) + val headBone = Bone(BoneType.HEAD, TwistSwingConstraint(Float.NaN, Float.NaN)) + val neckBone = Bone(BoneType.NECK, TwistSwingConstraint(Float.NaN, Float.NaN)) + val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(Float.NaN, Float.NaN)) + val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(Float.NaN, Float.NaN)) + val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(Float.NaN, Float.NaN)) + val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(Float.NaN, Float.NaN)) // Lower body bones - val leftHipBone = Bone(BoneType.LEFT_HIP) - val rightHipBone = Bone(BoneType.RIGHT_HIP) - val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG) - val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG) - val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG) - val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG) - val leftFootBone = Bone(BoneType.LEFT_FOOT) - val rightFootBone = Bone(BoneType.RIGHT_FOOT) + val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightHipBone = Bone(BoneType.RIGHT_HIP, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, TwistSwingConstraint(180f, 10f)) + val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftFootBone = Bone(BoneType.LEFT_FOOT, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightFootBone = Bone(BoneType.RIGHT_FOOT, TwistSwingConstraint(Float.NaN, Float.NaN)) // Arm bones - val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER) - val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER) - val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM) - val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM) - val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM) - val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM) - val leftHandBone = Bone(BoneType.LEFT_HAND) - val rightHandBone = Bone(BoneType.RIGHT_HAND) + val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftHandBone = Bone(BoneType.LEFT_HAND, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightHandBone = Bone(BoneType.RIGHT_HAND, TwistSwingConstraint(Float.NaN, Float.NaN)) // Tracker bones - val headTrackerBone = Bone(BoneType.HEAD_TRACKER) - val chestTrackerBone = Bone(BoneType.CHEST_TRACKER) - val hipTrackerBone = Bone(BoneType.HIP_TRACKER) - val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER) - val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER) - val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER) - val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER) - val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER) - val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER) - val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER) - val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER) + val headTrackerBone = Bone(BoneType.HEAD_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val hipTrackerBone = Bone(BoneType.HIP_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) // Buffers var hasSpineTracker = false From 36b69a08a283d47b75bfb32a9ee7947fb3d13c2b Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sun, 24 Dec 2023 12:50:24 -0800 Subject: [PATCH 10/68] bug fixes + use quat lib projection --- .../processor/TwistSwingConstraint.kt | 11 ++---- .../tracking/processor/skeleton/IKChain.kt | 18 ++++++++-- .../tracking/processor/skeleton/IKSolver.kt | 34 +++++++++---------- 3 files changed, 34 insertions(+), 29 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index 769d30b519..321242c944 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -6,15 +6,8 @@ import io.github.axisangles.ktmath.Vector3 class TwistSwingConstraint(val twist: Float, val swing: Float) : Constraint() { - private fun project(vector: Vector3, onNormal: Vector3): Vector3 { - val sqrMag = onNormal.lenSq() - val dot = vector.dot(onNormal) - return onNormal * (dot / sqrMag) - } - private fun decompose(rotation: Quaternion, twistAxis: Vector3): Pair { - val vector = Vector3(rotation.x, rotation.y, rotation.z) - val projection = project(vector, twistAxis) + val projection = rotation.project(twistAxis) val twist = Quaternion(rotation.w, projection.x, projection.y, projection.z).unit() val swing = rotation * twist.inv() @@ -58,6 +51,6 @@ class TwistSwingConstraint(val twist: Float, val swing: Float) : Constraint() { twistQ = constrain(twistQ, twist) } - return parent.getGlobalRotation() * (swingQ * twistQ) * parent.getGlobalRotation().inv() + return parent.getGlobalRotation() * (swingQ * twistQ) } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 098150422b..202b8bd68f 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -13,7 +13,11 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int // state variables var children = mutableListOf() var target = Vector3.NULL + var distToTargetSqr = Float.POSITIVE_INFINITY + var lastDistToTargetSqr = Float.POSITIVE_INFINITY private var positions = getPositionList() + + // deadlock escaping vars var firstBonePerturbation = 0 private fun getPositionList(): MutableList { @@ -74,8 +78,18 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int } } - fun getTargetDistance(): Float { - return (positions.last() - (tailConstraint?.position ?: Vector3.NULL)).lenSq() + // Updates the distance to target and last distance to target variables + // and returns the new distance to the target + fun computeTargetDistance(): Float { + if (tailConstraint != null) { + lastDistToTargetSqr = distToTargetSqr + distToTargetSqr = (positions.last() - (tailConstraint.position)).lenSq() + } else { + lastDistToTargetSqr = distToTargetSqr + distToTargetSqr = 0.0f + } + + return distToTargetSqr } // Sets a bones rotation from a rotation vector after constraining the rotation diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 318410532b..c816e531a4 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -2,7 +2,6 @@ package dev.slimevr.tracking.processor.skeleton import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.trackers.Tracker -import io.github.axisangles.ktmath.Vector3 /* * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) and allows @@ -12,7 +11,8 @@ import io.github.axisangles.ktmath.Vector3 class IKSolver(val root: Bone) { companion object { - private const val TOLERANCE = 0.001f + private const val TOLERANCE_SQR = 1e-8 // == 0.0001 cm + private const val MAX_ITERATIONS = 100 } var chainList = mutableListOf() @@ -137,30 +137,28 @@ class IKSolver(val root: Bone) { } fun solve() { - // get the distance from the target - var perturbationStrength = 0 - var distSqrToTarget = Float.POSITIVE_INFINITY - var lastDistSqrToTarget: Float - - // run up to 100 iterations per tick - for (i in 0..100) { + // run up to MAX_ITERATIONS iterations per tick + for (i in 0..MAX_ITERATIONS) { for (chain in chainList) { chain.backwards() } rootChain?.forwardsMulti() - lastDistSqrToTarget = distSqrToTarget - distSqrToTarget = chainList.first().getTargetDistance() - if (distSqrToTarget < TOLERANCE) - break + // if all chains have reached their target the chain is solved + var solved = true + for (chain in chainList) { + if (chain.computeTargetDistance() > TOLERANCE_SQR) { + solved = false + break + } + } - // If the distance to the target is not changing the algorithm might be in a deadlock + if (solved) break + + // TODO If the distance to the target is not changing the algorithm might be in a deadlock // to solve this introduce some perturbations in the bones closest to the target to allow // the parent bones to bend more. - if (lastDistSqrToTarget == distSqrToTarget) { - perturbationStrength += 2 - chainList.first().firstBonePerturbation = perturbationStrength - } + } // update transforms From 8606c0e417457e8b2ae48f2e0286c695adb2b93e Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 27 Dec 2023 15:41:30 -0800 Subject: [PATCH 11/68] add apply Constraint Inverse --- .../slimevr/tracking/processor/Constraint.kt | 13 ++++++++++++ .../processor/TwistSwingConstraint.kt | 21 +++++++------------ 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index e3a49c95a2..6b9a745382 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -7,5 +7,18 @@ import io.github.axisangles.ktmath.Vector3 * Represents the rotational limits of a Bone relative to its parent */ abstract class Constraint { + /** + * Apply constraints to the direction vector + */ abstract fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion + + /** + * 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, parent: Bone?) : Vector3 { + return -applyConstraint(-direction, parent).sandwich(Vector3.NEG_Y) + } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index 321242c944..65a11fd087 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -5,11 +5,11 @@ import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 -class TwistSwingConstraint(val twist: Float, val swing: Float) : Constraint() { +class TwistSwingConstraint(private val twist: Float, private val swing: Float) : Constraint() { private fun decompose(rotation: Quaternion, twistAxis: Vector3): Pair { val projection = rotation.project(twistAxis) - val twist = Quaternion(rotation.w, projection.x, projection.y, projection.z).unit() + val twist = Quaternion(rotation.w, projection.xyz).unit() val swing = rotation * twist.inv() return Pair(swing, twist) @@ -32,24 +32,19 @@ class TwistSwingConstraint(val twist: Float, val swing: Float) : Constraint() { override fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion { // if there is no parent or no constraint return the direction - if (parent == null || (swing.isNaN() && twist.isNaN())) { - return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() - } + if (parent == null || (swing.isNaN() && twist.isNaN())) + return Quaternion.fromTo(Vector3.NEG_Y, direction) // get the local rotation - val rotationGlobal = Quaternion.fromTo(Vector3.NEG_Y, direction).unit() + val rotationGlobal = Quaternion.fromTo(Vector3.NEG_Y, direction) val rotationLocal = parent.getGlobalRotation().inv() * rotationGlobal + // decompose in to twist and swing var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) // apply the constraints - if (!swing.isNaN()) { - swingQ = constrain(swingQ, swing) - } - - if (!twist.isNaN()) { - twistQ = constrain(twistQ, twist) - } + if (!swing.isNaN()) swingQ = constrain(swingQ, swing) + if (!twist.isNaN()) twistQ = constrain(twistQ, twist) return parent.getGlobalRotation() * (swingQ * twistQ) } From 96638dcb16a637284cb8ec33061f3f2ebbc4b878 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 27 Dec 2023 15:42:16 -0800 Subject: [PATCH 12/68] bug fixes + make vars private --- .../tracking/processor/skeleton/IKChain.kt | 31 +++++++++---------- .../tracking/processor/skeleton/IKSolver.kt | 11 ++----- 2 files changed, 18 insertions(+), 24 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 202b8bd68f..9c8ef3d6d7 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -13,13 +13,9 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int // state variables var children = mutableListOf() var target = Vector3.NULL - var distToTargetSqr = Float.POSITIVE_INFINITY - var lastDistToTargetSqr = Float.POSITIVE_INFINITY + private var distToTargetSqr = Float.POSITIVE_INFINITY private var positions = getPositionList() - // deadlock escaping vars - var firstBonePerturbation = 0 - private fun getPositionList(): MutableList { val posList = mutableListOf() for (n in nodes) { @@ -42,11 +38,14 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int for (i in positions.size - 2 downTo 0) { val direction = (positions[i] - positions[i + 1]).unit() - positions[i] = positions[i + 1] + (direction * nodes[i].length) + val constrainedDirection = nodes[i].rotationConstraint + .applyConstraintInverse(direction, nodes[i].parent) + + + positions[i] = positions[i + 1] + (constrainedDirection * nodes[i].length) } - if (parent != null) - parent!!.target += positions[0] + if (parent != null) parent!!.target += positions[0] } private fun forwards() { @@ -81,13 +80,10 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int // Updates the distance to target and last distance to target variables // and returns the new distance to the target fun computeTargetDistance(): Float { - if (tailConstraint != null) { - lastDistToTargetSqr = distToTargetSqr - distToTargetSqr = (positions.last() - (tailConstraint.position)).lenSq() - } else { - lastDistToTargetSqr = distToTargetSqr - distToTargetSqr = 0.0f - } + distToTargetSqr = if (tailConstraint != null) + (positions.last() - (tailConstraint.position)).lenSq() + else + 0.0f return distToTargetSqr } @@ -98,10 +94,13 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int private fun setBoneRotation(bone: Bone, rotationVector: Vector3): Vector3 { // TODO if a bone has a tracker associated with it force that rotation - val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone.parent) bone.setRotationRaw(rotation) + // TODO optimize (this is required to update the world translation for the next bone as it uses the world + // rotation of the parent) + bone.update() + return rotation.sandwich(Vector3.NEG_Y) } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index c816e531a4..c39f8d7893 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -9,14 +9,14 @@ import dev.slimevr.tracking.trackers.Tracker * IMU trackers */ -class IKSolver(val root: Bone) { +class IKSolver(private val root: Bone) { companion object { private const val TOLERANCE_SQR = 1e-8 // == 0.0001 cm private const val MAX_ITERATIONS = 100 } - var chainList = mutableListOf() - var rootChain: IKChain? = null + private var chainList = mutableListOf() + private var rootChain: IKChain? = null fun buildChains(trackers: List) { chainList = mutableListOf() @@ -154,11 +154,6 @@ class IKSolver(val root: Bone) { } if (solved) break - - // TODO If the distance to the target is not changing the algorithm might be in a deadlock - // to solve this introduce some perturbations in the bones closest to the target to allow - // the parent bones to bend more. - } // update transforms From 17abee48498b224edd77452c6be2ad44e12e7688 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Thu, 28 Dec 2023 14:59:08 -0800 Subject: [PATCH 13/68] fix centroid deadlocking --- .../tracking/processor/skeleton/IKChain.kt | 53 ++++++++++++++++--- .../tracking/processor/skeleton/IKSolver.kt | 7 +++ 2 files changed, 52 insertions(+), 8 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 9c8ef3d6d7..e904b5d3b7 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -14,6 +14,7 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int var children = mutableListOf() var target = Vector3.NULL private var distToTargetSqr = Float.POSITIVE_INFINITY + private var centroidWeight = 1f private var positions = getPositionList() private fun getPositionList(): MutableList { @@ -28,10 +29,10 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int fun backwards() { // start at the constraint or the centroid of the children - if (tailConstraint == null && children.size > 1) - target /= children.size.toFloat() - else - target = tailConstraint?.position ?: Vector3.NULL + if (tailConstraint == null && children.size > 1) { + target /= getChildrenCentroidWeightSum() + } + else target = tailConstraint?.position ?: Vector3.NULL // set the end node to target positions[positions.size - 1] = target @@ -39,13 +40,13 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int for (i in positions.size - 2 downTo 0) { val direction = (positions[i] - positions[i + 1]).unit() val constrainedDirection = nodes[i].rotationConstraint - .applyConstraintInverse(direction, nodes[i].parent) + .applyConstraintInverse(direction, nodes[i]) positions[i] = positions[i + 1] + (constrainedDirection * nodes[i].length) } - if (parent != null) parent!!.target += positions[0] + if (parent != null) parent!!.target += positions[0] * centroidWeight } private fun forwards() { @@ -69,6 +70,7 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int target = Vector3.NULL } + // Run the forward pass fun forwardsMulti() { forwards() @@ -77,7 +79,42 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int } } - // Updates the distance to target and last distance to target variables + // Returns the sum of the children's centroid weights + private fun getChildrenCentroidWeightSum(): Float { + var sum = 0.0f + for (child in children) { + sum += child.centroidWeight + } + + return sum + } + + + // Reset any vars that may be modified from the last solve + fun resetChain() { + centroidWeight = 1f + + for (child in children) { + child.resetChain() + } + } + + // Reduce the centroid weight of the child that is closest to its target + fun updateChildCentroidWeight() { + if (children.size <= 1) return + + var closestToSolved = children.first() + for (child in children) { + if (child.distToTargetSqr < closestToSolved.distToTargetSqr) + closestToSolved = child + } + + // TODO remove nasty magic numbers + if (closestToSolved.centroidWeight > 0.1f) + closestToSolved.centroidWeight -= 0.1f + } + + // Updates the distance to target and centroid weight variables // and returns the new distance to the target fun computeTargetDistance(): Float { distToTargetSqr = if (tailConstraint != null) @@ -94,7 +131,7 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int private fun setBoneRotation(bone: Bone, rotationVector: Vector3): Vector3 { // TODO if a bone has a tracker associated with it force that rotation - val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone.parent) + val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone) bone.setRotationRaw(rotation) // TODO optimize (this is required to update the world translation for the next bone as it uses the world diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index c39f8d7893..e4657ff2f7 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -137,6 +137,8 @@ class IKSolver(private val root: Bone) { } fun solve() { + rootChain?.resetChain() + // run up to MAX_ITERATIONS iterations per tick for (i in 0..MAX_ITERATIONS) { for (chain in chainList) { @@ -154,6 +156,11 @@ class IKSolver(private val root: Bone) { } if (solved) break + + // help the chains out of a deadlock + for (chain in chainList) { + chain.updateChildCentroidWeight() + } } // update transforms From 16fb03e07d777b9934b67876662b1eb4dc808d46 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Thu, 28 Dec 2023 14:59:37 -0800 Subject: [PATCH 14/68] modify applyConstraint + add CompleteConstraint --- .../tracking/processor/CompleteConstraint.kt | 13 +++++++++++++ .../dev/slimevr/tracking/processor/Constraint.kt | 6 +++--- .../slimevr/tracking/processor/HingeConstraint.kt | 2 +- .../tracking/processor/TwistSwingConstraint.kt | 6 ++++-- 4 files changed, 21 insertions(+), 6 deletions(-) create mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt new file mode 100644 index 0000000000..0a7cc81bae --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt @@ -0,0 +1,13 @@ +package dev.slimevr.tracking.processor + +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 + +/** + * A constraint type that allows no modifications to the rotation + */ +class CompleteConstraint : Constraint() { + override fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { + return thisBone.getGlobalRotation() + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 6b9a745382..ee09f9a7b1 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -10,7 +10,7 @@ abstract class Constraint { /** * Apply constraints to the direction vector */ - abstract fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion + abstract fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion /** * Apply constraints to the direction vector such that when @@ -18,7 +18,7 @@ abstract class Constraint { * This is used for constraining direction vectors on the backwards pass * of the FABRIK solver. */ - fun applyConstraintInverse(direction: Vector3, parent: Bone?) : Vector3 { - return -applyConstraint(-direction, parent).sandwich(Vector3.NEG_Y) + fun applyConstraintInverse(direction: Vector3, thisBone: Bone) : Vector3 { + return -applyConstraint(-direction, thisBone).sandwich(Vector3.NEG_Y) } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt index 85ccacd241..656f831512 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt @@ -4,7 +4,7 @@ import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 class HingeConstraint(val min: Float, val max: Float) : Constraint() { - override fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion { + override fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index 65a11fd087..e20764c36a 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -30,11 +30,13 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : return rot } - override fun applyConstraint(direction: Vector3, parent: Bone?): Quaternion { + override fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { // if there is no parent or no constraint return the direction - if (parent == null || (swing.isNaN() && twist.isNaN())) + if (thisBone.parent == null || (swing.isNaN() && twist.isNaN())) return Quaternion.fromTo(Vector3.NEG_Y, direction) + val parent = thisBone.parent!! + // get the local rotation val rotationGlobal = Quaternion.fromTo(Vector3.NEG_Y, direction) val rotationLocal = parent.getGlobalRotation().inv() * rotationGlobal From 20e450592739fba9b18ed2f2b6de4b25b5e6a450 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Thu, 28 Dec 2023 15:03:58 -0800 Subject: [PATCH 15/68] spotless --- .../slimevr/tracking/processor/Constraint.kt | 2 +- .../tracking/processor/HingeConstraint.kt | 2 +- .../processor/TwistSwingConstraint.kt | 14 ++++++--- .../tracking/processor/skeleton/IKChain.kt | 30 +++++++++++------- .../tracking/processor/skeleton/IKSolver.kt | 31 +++++++++++++------ 5 files changed, 52 insertions(+), 27 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index ee09f9a7b1..4387987546 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -18,7 +18,7 @@ abstract class Constraint { * This is used for constraining direction vectors on the backwards pass * of the FABRIK solver. */ - fun applyConstraintInverse(direction: Vector3, thisBone: Bone) : Vector3 { + fun applyConstraintInverse(direction: Vector3, thisBone: Bone): Vector3 { return -applyConstraint(-direction, thisBone).sandwich(Vector3.NEG_Y) } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt index 656f831512..14b60e3a86 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt @@ -3,7 +3,7 @@ package dev.slimevr.tracking.processor import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 -class HingeConstraint(val min: Float, val max: Float) : Constraint() { +class HingeConstraint(val min: Float, val max: Float) : Constraint() { override fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index e20764c36a..0acf0737e0 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -4,8 +4,7 @@ import com.jme3.math.FastMath import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 - -class TwistSwingConstraint(private val twist: Float, private val swing: Float) : Constraint() { +class TwistSwingConstraint(private val twist: Float, private val swing: Float) : Constraint() { private fun decompose(rotation: Quaternion, twistAxis: Vector3): Pair { val projection = rotation.project(twistAxis) @@ -23,8 +22,12 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : if (vector.lenSq() > sqrLength) { vector = vector.unit() * length - rot = Quaternion(FastMath.sqrt(1.0f - sqrLength) * FastMath.sign(rot.w), - vector.x, vector.y, vector.z) + rot = Quaternion( + FastMath.sqrt(1.0f - sqrLength) * FastMath.sign(rot.w), + vector.x, + vector.y, + vector.z + ) } return rot @@ -32,8 +35,9 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : override fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { // if there is no parent or no constraint return the direction - if (thisBone.parent == null || (swing.isNaN() && twist.isNaN())) + if (thisBone.parent == null || (swing.isNaN() && twist.isNaN())) { return Quaternion.fromTo(Vector3.NEG_Y, direction) + } val parent = thisBone.parent!! diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index e904b5d3b7..7a4e4fa397 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -8,8 +8,13 @@ import io.github.axisangles.ktmath.Vector3 * This class implements a chain of Bones for use by the FABRIK solver */ -class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int, - val baseConstraint: Tracker?, val tailConstraint: Tracker?) { +class IKChain( + val nodes: MutableList, + var parent: IKChain?, + val level: Int, + val baseConstraint: Tracker?, + val tailConstraint: Tracker?, +) { // state variables var children = mutableListOf() var target = Vector3.NULL @@ -31,8 +36,9 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int // start at the constraint or the centroid of the children if (tailConstraint == null && children.size > 1) { target /= getChildrenCentroidWeightSum() + } else { + target = tailConstraint?.position ?: Vector3.NULL } - else target = tailConstraint?.position ?: Vector3.NULL // set the end node to target positions[positions.size - 1] = target @@ -42,7 +48,6 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int val constrainedDirection = nodes[i].rotationConstraint .applyConstraintInverse(direction, nodes[i]) - positions[i] = positions[i + 1] + (constrainedDirection * nodes[i].length) } @@ -50,10 +55,11 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int } private fun forwards() { - if (baseConstraint != null) + if (baseConstraint != null) { positions[0] = baseConstraint.position - else if (parent != null) + } else if (parent != null) { positions[0] = parent!!.positions.last() + } for (i in 1 until positions.size - 1) { var direction = (positions[i] - positions[i - 1]).unit() @@ -89,7 +95,6 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int return sum } - // Reset any vars that may be modified from the last solve fun resetChain() { centroidWeight = 1f @@ -105,22 +110,25 @@ class IKChain(val nodes: MutableList, var parent: IKChain?, val level: Int var closestToSolved = children.first() for (child in children) { - if (child.distToTargetSqr < closestToSolved.distToTargetSqr) + if (child.distToTargetSqr < closestToSolved.distToTargetSqr) { closestToSolved = child + } } // TODO remove nasty magic numbers - if (closestToSolved.centroidWeight > 0.1f) + if (closestToSolved.centroidWeight > 0.1f) { closestToSolved.centroidWeight -= 0.1f + } } // Updates the distance to target and centroid weight variables // and returns the new distance to the target fun computeTargetDistance(): Float { - distToTargetSqr = if (tailConstraint != null) + distToTargetSqr = if (tailConstraint != null) { (positions.last() - (tailConstraint.position)).lenSq() - else + } else { 0.0f + } return distToTargetSqr } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index e4657ff2f7..a63899eb15 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -42,12 +42,15 @@ class IKSolver(private val root: Bone) { boneList.add(currentBone) // get constraints - val baseConstraint = if (parent == null) getConstraint(boneList.first(), constraints) - else parent.tailConstraint + val baseConstraint = if (parent == null) { + getConstraint(boneList.first(), constraints) + } else { + parent.tailConstraint + } var tailConstraint: Tracker? = null // add bones until there is a reason to make a new chain - while(currentBone.children.size == 1 && tailConstraint == null) { + while (currentBone.children.size == 1 && tailConstraint == null) { currentBone = currentBone.children[0] boneList.add(currentBone) tailConstraint = getConstraint(boneList.last(), constraints) @@ -69,8 +72,9 @@ class IKSolver(private val root: Bone) { } // if the chain has only one child and no tail constraint combine the chains - if (chain.children.size == 1 && chain.tailConstraint == null) + if (chain.children.size == 1 && chain.tailConstraint == null) { chain = combineChains(chain, chain.children[0]) + } return chain } @@ -87,8 +91,13 @@ class IKSolver(private val root: Bone) { boneList.addAll(chain.nodes) boneList.addAll(childChain.nodes) - val newChain = IKChain(boneList, chain.parent, chain.level, - chain.baseConstraint, childChain.tailConstraint) + val newChain = IKChain( + boneList, + chain.parent, + chain.level, + chain.baseConstraint, + childChain.tailConstraint + ) newChain.children = childChain.children @@ -106,11 +115,13 @@ class IKSolver(private val root: Bone) { } for (c in chain.children) { - if (c.tailConstraint != null) + if (c.tailConstraint != null) { return true + } - if (neededChain(c)) + if (neededChain(c)) { return true + } } return false @@ -120,8 +131,10 @@ class IKSolver(private val root: Bone) { val constraintList = mutableListOf() for (t in trackers) { if (t.hasPosition && !t.isInternal && - !t.status.reset) + !t.status.reset + ) { constraintList.add(t) + } } return constraintList } From a80d7fdf95521473ca9c99e1720fce23ac2b363d Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sat, 30 Dec 2023 18:10:37 -0800 Subject: [PATCH 16/68] add allowModification to Constraint --- .../tracking/processor/CompleteConstraint.kt | 2 +- .../slimevr/tracking/processor/Constraint.kt | 22 +++++++++++++++++-- .../tracking/processor/HingeConstraint.kt | 2 +- .../processor/TwistSwingConstraint.kt | 6 ++++- 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt index 0a7cc81bae..da85cea2a6 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt @@ -7,7 +7,7 @@ import io.github.axisangles.ktmath.Vector3 * A constraint type that allows no modifications to the rotation */ class CompleteConstraint : Constraint() { - override fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { + override fun constraintRotation(direction: Vector3, thisBone: Bone): Quaternion { return thisBone.getGlobalRotation() } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 4387987546..66e7964c1d 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -8,9 +8,27 @@ import io.github.axisangles.ktmath.Vector3 */ abstract class Constraint { /** - * Apply constraints to the direction vector + * If false don't allow the rotation of the bone + * to be modified except to satisfy a constraint */ - abstract fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion + var allowModifications = true + + /** + * Apply rotational constraints to the direction vector + */ + protected abstract fun constraintRotation(direction: Vector3, thisBone: Bone): Quaternion + + /** + * Apply rotational constraints and if applicable force the rotation + * to be unchanged unless it violates the constraints + */ + fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { + if (!allowModifications) { + return constraintRotation(thisBone.getGlobalRotation().sandwich(Vector3.NEG_Y), thisBone) + } + + return constraintRotation(direction, thisBone) + } /** * Apply constraints to the direction vector such that when diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt index 14b60e3a86..afa8756852 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt @@ -4,7 +4,7 @@ import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 class HingeConstraint(val min: Float, val max: Float) : Constraint() { - override fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { + override fun constraintRotation(direction: Vector3, thisBone: Bone): Quaternion { return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index 0acf0737e0..a1231ec5ba 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -4,6 +4,10 @@ import com.jme3.math.FastMath import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 +/** + * A constraint type that allows for a rotation to be constrained relative to the parent + * of the supplied bone + */ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : Constraint() { private fun decompose(rotation: Quaternion, twistAxis: Vector3): Pair { val projection = rotation.project(twistAxis) @@ -33,7 +37,7 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : return rot } - override fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { + override fun constraintRotation(direction: Vector3, thisBone: Bone): Quaternion { // if there is no parent or no constraint return the direction if (thisBone.parent == null || (swing.isNaN() && twist.isNaN())) { return Quaternion.fromTo(Vector3.NEG_Y, direction) From 70f3259309ab7ec312beb1d30631f9fc4b25d4cb Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sat, 30 Dec 2023 18:12:12 -0800 Subject: [PATCH 17/68] add locking of rotation on bones where a rotation tracker is present --- .../processor/skeleton/HumanSkeleton.kt | 29 +++++----- .../tracking/processor/skeleton/IKChain.kt | 2 - .../tracking/processor/skeleton/IKSolver.kt | 55 +++++++++++++++---- 3 files changed, 59 insertions(+), 27 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 69e40551ac..5897b801fd 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -4,6 +4,7 @@ import com.jme3.math.FastMath import dev.slimevr.VRServer import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.processor.BoneType +import dev.slimevr.tracking.processor.CompleteConstraint import dev.slimevr.tracking.processor.HumanPoseManager import dev.slimevr.tracking.processor.TwistSwingConstraint import dev.slimevr.tracking.processor.config.SkeletonConfigToggles @@ -43,24 +44,24 @@ class HumanSkeleton( val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(Float.NaN, Float.NaN)) // Lower body bones - val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightHipBone = Bone(BoneType.RIGHT_HIP, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, TwistSwingConstraint(180f, 10f)) - val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftFootBone = Bone(BoneType.LEFT_FOOT, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightFootBone = Bone(BoneType.RIGHT_FOOT, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftHipBone = Bone(BoneType.LEFT_HIP, CompleteConstraint()) + val rightHipBone = Bone(BoneType.RIGHT_HIP, CompleteConstraint()) + val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, TwistSwingConstraint(180f, 120f)) + val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, TwistSwingConstraint(180f, 120f)) + val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, TwistSwingConstraint(180f, 150f)) + val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, TwistSwingConstraint(180f, 150f)) + val leftFootBone = Bone(BoneType.LEFT_FOOT, TwistSwingConstraint(Float.NaN, 120f)) + val rightFootBone = Bone(BoneType.RIGHT_FOOT, TwistSwingConstraint(Float.NaN, 120f)) // Arm bones - val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, CompleteConstraint()) + val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, CompleteConstraint()) val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftHandBone = Bone(BoneType.LEFT_HAND, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightHandBone = Bone(BoneType.RIGHT_HAND, TwistSwingConstraint(Float.NaN, Float.NaN)) + val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, TwistSwingConstraint(180f, 120f)) + val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, TwistSwingConstraint(180f, 120f)) + val leftHandBone = Bone(BoneType.LEFT_HAND, TwistSwingConstraint(180f, 110f)) + val rightHandBone = Bone(BoneType.RIGHT_HAND, TwistSwingConstraint(180f, 110f)) // Tracker bones val headTrackerBone = Bone(BoneType.HEAD_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 7a4e4fa397..4aa6e2a5d2 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -137,8 +137,6 @@ class IKChain( // vector with the bone's rotational constraint // returns the constrained rotation as a vector private fun setBoneRotation(bone: Bone, rotationVector: Vector3): Vector3 { - // TODO if a bone has a tracker associated with it force that rotation - val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone) bone.setRotationRaw(rotation) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index a63899eb15..4c23d65bf1 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -18,14 +18,19 @@ class IKSolver(private val root: Bone) { private var chainList = mutableListOf() private var rootChain: IKChain? = null + /** + * Any time the skeleton is rebuilt or trackers are assigned / unassigned the chains + * should be rebuilt. + */ fun buildChains(trackers: List) { chainList = mutableListOf() // extract the positional constraints - val constraints = extractConstraints(trackers) + val positionalConstraints = extractPositionalConstraints(trackers) + val rotationalConstraints = extractRotationalConstraints(trackers) // build the system of chains - rootChain = chainBuilder(root, null, 0, constraints) + rootChain = chainBuilder(root, null, 0, positionalConstraints, rotationalConstraints) populateChainList(rootChain!!) // check if there is any constraints (other than the head) in the model @@ -33,17 +38,28 @@ class IKSolver(private val root: Bone) { chainList.sortBy { -it.level } } - // convert the skeleton in to a system of chains - // a break in a chain is created at any point that has either - // multiple children or is positionally constrained, useless chains are discarded - private fun chainBuilder(root: Bone, parent: IKChain?, level: Int, constraints: MutableList): IKChain { + /** + * Convert the skeleton in to a system of chains. + * A break in a chain is created at any point that has either + * multiple children or is positionally constrained, useless chains are discarded + * (useless chains are chains with no positional constraint at their tail). + */ + private fun chainBuilder( + root: Bone, + parent: IKChain?, + level: Int, + positionalConstraints: MutableList, + rotationalConstraints: MutableList, + ): IKChain { val boneList = mutableListOf() var currentBone = root + currentBone.rotationConstraint.allowModifications = + getConstraint(currentBone, rotationalConstraints) == null boneList.add(currentBone) // get constraints val baseConstraint = if (parent == null) { - getConstraint(boneList.first(), constraints) + getConstraint(boneList.first(), positionalConstraints) } else { parent.tailConstraint } @@ -52,8 +68,10 @@ class IKSolver(private val root: Bone) { // add bones until there is a reason to make a new chain while (currentBone.children.size == 1 && tailConstraint == null) { currentBone = currentBone.children[0] + currentBone.rotationConstraint.allowModifications = + getConstraint(currentBone, rotationalConstraints) == null boneList.add(currentBone) - tailConstraint = getConstraint(boneList.last(), constraints) + tailConstraint = getConstraint(boneList.last(), positionalConstraints) } var chain = IKChain(boneList, parent, level, baseConstraint, tailConstraint) @@ -63,7 +81,7 @@ class IKSolver(private val root: Bone) { // build child chains for (child in currentBone.children) { - val childChain = chainBuilder(child, chain, level + 1, constraints) + val childChain = chainBuilder(child, chain, level + 1, positionalConstraints, rotationalConstraints) if (neededChain(childChain)) { childrenList.add(childChain) } @@ -127,10 +145,11 @@ class IKSolver(private val root: Bone) { return false } - private fun extractConstraints(trackers: List): MutableList { + private fun extractPositionalConstraints(trackers: List): MutableList { val constraintList = mutableListOf() for (t in trackers) { - if (t.hasPosition && !t.isInternal && + if (t.hasPosition && + !t.isInternal && !t.status.reset ) { constraintList.add(t) @@ -139,6 +158,20 @@ class IKSolver(private val root: Bone) { return constraintList } + private fun extractRotationalConstraints(trackers: List): MutableList { + val constrainList = mutableListOf() + for (t in trackers) { + if (t.hasRotation && + !t.status.reset && + !t.isInternal + ) { + constrainList.add(t) + } + } + + return constrainList + } + private fun getConstraint(bone: Bone, constraints: MutableList): Tracker? { for (c in constraints) { if (bone.boneType.bodyPart == (c.trackerPosition?.bodyPart ?: 0)) { From cb69dd3733b0718afde6b2601ee30847ddc8997b Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 1 Jan 2024 16:07:26 -0800 Subject: [PATCH 18/68] account for bone.rotationOffset + change input to constrainRotation --- .../slimevr/tracking/processor/CompleteConstraint.kt | 2 +- .../java/dev/slimevr/tracking/processor/Constraint.kt | 9 ++++----- .../dev/slimevr/tracking/processor/HingeConstraint.kt | 4 ++-- .../slimevr/tracking/processor/TwistSwingConstraint.kt | 10 +++++----- 4 files changed, 12 insertions(+), 13 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt index da85cea2a6..b35e2375f8 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt @@ -7,7 +7,7 @@ import io.github.axisangles.ktmath.Vector3 * A constraint type that allows no modifications to the rotation */ class CompleteConstraint : Constraint() { - override fun constraintRotation(direction: Vector3, thisBone: Bone): Quaternion { + override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { return thisBone.getGlobalRotation() } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 66e7964c1d..5f132b8b9a 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -16,18 +16,17 @@ abstract class Constraint { /** * Apply rotational constraints to the direction vector */ - protected abstract fun constraintRotation(direction: Vector3, thisBone: Bone): Quaternion + protected abstract fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion /** * Apply rotational constraints and if applicable force the rotation * to be unchanged unless it violates the constraints */ fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { - if (!allowModifications) { - return constraintRotation(thisBone.getGlobalRotation().sandwich(Vector3.NEG_Y), thisBone) - } + if (!allowModifications) + return constraintRotation(thisBone.getGlobalRotation(), thisBone) - return constraintRotation(direction, thisBone) + return constraintRotation(Quaternion.fromTo(Vector3.NEG_Y, direction), thisBone) } /** diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt index afa8756852..69e7547fff 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt @@ -4,7 +4,7 @@ import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 class HingeConstraint(val min: Float, val max: Float) : Constraint() { - override fun constraintRotation(direction: Vector3, thisBone: Bone): Quaternion { - return Quaternion.fromTo(Vector3.NEG_Y, direction).unit() + override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { + return rotation } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index a1231ec5ba..77ed6b6b8b 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -37,17 +37,17 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : return rot } - override fun constraintRotation(direction: Vector3, thisBone: Bone): Quaternion { + override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { // if there is no parent or no constraint return the direction if (thisBone.parent == null || (swing.isNaN() && twist.isNaN())) { - return Quaternion.fromTo(Vector3.NEG_Y, direction) + return rotation } val parent = thisBone.parent!! // get the local rotation - val rotationGlobal = Quaternion.fromTo(Vector3.NEG_Y, direction) - val rotationLocal = parent.getGlobalRotation().inv() * rotationGlobal + val rotationLocal = (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation + // decompose in to twist and swing var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) @@ -56,6 +56,6 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : if (!swing.isNaN()) swingQ = constrain(swingQ, swing) if (!twist.isNaN()) twistQ = constrain(twistQ, twist) - return parent.getGlobalRotation() * (swingQ * twistQ) + return parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ) } } From 6e3eb0fc10069236f111d1694657942af016bb89 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Thu, 4 Jan 2024 19:05:10 -0800 Subject: [PATCH 19/68] fix constrain function --- .../processor/TwistSwingConstraint.kt | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index 77ed6b6b8b..85990e5dc9 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -9,6 +9,9 @@ import io.github.axisangles.ktmath.Vector3 * of the supplied bone */ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : Constraint() { + private val twistRad = Math.toRadians(twist.toDouble()).toFloat() + private val swingRad = Math.toRadians(swing.toDouble()).toFloat() + private fun decompose(rotation: Quaternion, twistAxis: Vector3): Pair { val projection = rotation.project(twistAxis) @@ -19,15 +22,15 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : } private fun constrain(rotation: Quaternion, angle: Float): Quaternion { - val length = FastMath.sin(Math.toRadians(angle.toDouble()).toFloat()) - val sqrLength = length * length - var vector = Vector3(rotation.x, rotation.y, rotation.z) + val magnitude = FastMath.sin(angle * 0.5f) + val magnitudeSqr = magnitude * magnitude + var vector = rotation.xyz var rot = rotation - if (vector.lenSq() > sqrLength) { - vector = vector.unit() * length + if (vector.lenSq() > magnitudeSqr) { + vector = vector.unit() * magnitude rot = Quaternion( - FastMath.sqrt(1.0f - sqrLength) * FastMath.sign(rot.w), + FastMath.sqrt(1.0f - magnitudeSqr) * FastMath.sign(rot.w), vector.x, vector.y, vector.z @@ -48,13 +51,12 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : // get the local rotation val rotationLocal = (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation - // decompose in to twist and swing var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) // apply the constraints - if (!swing.isNaN()) swingQ = constrain(swingQ, swing) - if (!twist.isNaN()) twistQ = constrain(twistQ, twist) + if (!swing.isNaN()) swingQ = constrain(swingQ, swingRad) + if (!twist.isNaN()) twistQ = constrain(twistQ, twistRad) return parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ) } From fd5363f986a7e54899533130fe7d012c80d00add Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 5 Jan 2024 14:18:21 -0800 Subject: [PATCH 20/68] formatting + add position offset at time of reset --- .../tracking/processor/CompleteConstraint.kt | 1 - .../slimevr/tracking/processor/Constraint.kt | 3 +- .../tracking/processor/skeleton/IKChain.kt | 46 +++++++++++++------ .../tracking/processor/skeleton/IKSolver.kt | 17 ++++++- 4 files changed, 50 insertions(+), 17 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt index b35e2375f8..238d459c94 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt @@ -1,7 +1,6 @@ package dev.slimevr.tracking.processor import io.github.axisangles.ktmath.Quaternion -import io.github.axisangles.ktmath.Vector3 /** * A constraint type that allows no modifications to the rotation diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 5f132b8b9a..9870d62768 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -23,8 +23,9 @@ abstract class Constraint { * to be unchanged unless it violates the constraints */ fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { - if (!allowModifications) + if (!allowModifications) { return constraintRotation(thisBone.getGlobalRotation(), thisBone) + } return constraintRotation(Quaternion.fromTo(Vector3.NEG_Y, direction), thisBone) } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 4aa6e2a5d2..6b721362ae 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -15,12 +15,17 @@ class IKChain( val baseConstraint: Tracker?, val tailConstraint: Tracker?, ) { + companion object { + const val CENTROID_PULL_ADJUSTMENT = 0.1f + } + // state variables var children = mutableListOf() var target = Vector3.NULL private var distToTargetSqr = Float.POSITIVE_INFINITY private var centroidWeight = 1f private var positions = getPositionList() + private var tailConstrainPosOffset = Vector3.NULL private fun getPositionList(): MutableList { val posList = mutableListOf() @@ -37,7 +42,7 @@ class IKChain( if (tailConstraint == null && children.size > 1) { target /= getChildrenCentroidWeightSum() } else { - target = tailConstraint?.position ?: Vector3.NULL + target = (tailConstraint?.position?.plus(tailConstrainPosOffset)) ?: Vector3.NULL } // set the end node to target @@ -76,7 +81,9 @@ class IKChain( target = Vector3.NULL } - // Run the forward pass + /** + * Run the forward pass + */ fun forwardsMulti() { forwards() @@ -85,7 +92,6 @@ class IKChain( } } - // Returns the sum of the children's centroid weights private fun getChildrenCentroidWeightSum(): Float { var sum = 0.0f for (child in children) { @@ -95,7 +101,6 @@ class IKChain( return sum } - // Reset any vars that may be modified from the last solve fun resetChain() { centroidWeight = 1f @@ -104,7 +109,17 @@ class IKChain( } } - // Reduce the centroid weight of the child that is closest to its target + fun resetTrackerOffsets() { + if (tailConstraint == null) { + return + } + tailConstrainPosOffset = nodes.last().getTailPosition() - tailConstraint.position + } + + /** + * Prevent deadlocks where the centroid becomes stuck + * due to two or more chains pulling equally on the centroid + */ fun updateChildCentroidWeight() { if (children.size <= 1) return @@ -115,17 +130,18 @@ class IKChain( } } - // TODO remove nasty magic numbers - if (closestToSolved.centroidWeight > 0.1f) { - closestToSolved.centroidWeight -= 0.1f + if (closestToSolved.centroidWeight > CENTROID_PULL_ADJUSTMENT) { + closestToSolved.centroidWeight -= CENTROID_PULL_ADJUSTMENT } } - // Updates the distance to target and centroid weight variables - // and returns the new distance to the target + /** + * Updates the distance to target and centroid weight variables + * and returns the new distance to the target + */ fun computeTargetDistance(): Float { distToTargetSqr = if (tailConstraint != null) { - (positions.last() - (tailConstraint.position)).lenSq() + (positions.last() - (tailConstraint.position + tailConstrainPosOffset)).lenSq() } else { 0.0f } @@ -133,9 +149,11 @@ class IKChain( return distToTargetSqr } - // 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 + /** + * 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 + */ private fun setBoneRotation(bone: Bone, rotationVector: Vector3): Vector3 { val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone) bone.setRotationRaw(rotation) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 4c23d65bf1..a4ca1ae224 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -17,6 +17,7 @@ class IKSolver(private val root: Bone) { private var chainList = mutableListOf() private var rootChain: IKChain? = null + private var needsReset = false /** * Any time the skeleton is rebuilt or trackers are assigned / unassigned the chains @@ -38,6 +39,14 @@ class IKSolver(private val root: Bone) { chainList.sortBy { -it.level } } + /** + * Reset the offsets of positional trackers. Should only be called right after a full reset + * is performed. + */ + fun resetOffsets() { + needsReset = true + } + /** * Convert the skeleton in to a system of chains. * A break in a chain is created at any point that has either @@ -126,7 +135,6 @@ class IKSolver(private val root: Bone) { return newChain } - // a chain is needed if there is a positional constraint in its children private fun neededChain(chain: IKChain): Boolean { if (chain.tailConstraint != null) { return true @@ -183,6 +191,13 @@ class IKSolver(private val root: Bone) { } fun solve() { + if (needsReset) { + for (c in chainList) { + c.resetTrackerOffsets() + } + needsReset = false + } + rootChain?.resetChain() // run up to MAX_ITERATIONS iterations per tick From e5d237ccbf45f0830069175ede2c8cb3ba2598dc Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 5 Jan 2024 14:18:49 -0800 Subject: [PATCH 21/68] update skeletal constraints --- .../processor/skeleton/HumanSkeleton.kt | 63 ++++++++++--------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 5897b801fd..14e122f1d0 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -36,45 +36,45 @@ class HumanSkeleton( val humanPoseManager: HumanPoseManager, ) { // Upper body bones - val headBone = Bone(BoneType.HEAD, TwistSwingConstraint(Float.NaN, Float.NaN)) - val neckBone = Bone(BoneType.NECK, TwistSwingConstraint(Float.NaN, Float.NaN)) - val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(Float.NaN, Float.NaN)) - val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(Float.NaN, Float.NaN)) - val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(Float.NaN, Float.NaN)) - val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(Float.NaN, Float.NaN)) + val headBone = Bone(BoneType.HEAD, CompleteConstraint()) + val neckBone = Bone(BoneType.NECK, CompleteConstraint()) + val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(180f, 60f)) + val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(180f, 60f)) + val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(180f, 60f)) + val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(180f, 90f)) // Lower body bones - val leftHipBone = Bone(BoneType.LEFT_HIP, CompleteConstraint()) - val rightHipBone = Bone(BoneType.RIGHT_HIP, CompleteConstraint()) + val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(180f, 7f)) + val rightHipBone = Bone(BoneType.RIGHT_HIP, TwistSwingConstraint(180f, 7f)) val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, TwistSwingConstraint(180f, 120f)) val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, TwistSwingConstraint(180f, 120f)) - val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, TwistSwingConstraint(180f, 150f)) - val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, TwistSwingConstraint(180f, 150f)) - val leftFootBone = Bone(BoneType.LEFT_FOOT, TwistSwingConstraint(Float.NaN, 120f)) - val rightFootBone = Bone(BoneType.RIGHT_FOOT, TwistSwingConstraint(Float.NaN, 120f)) + val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, TwistSwingConstraint(110f, 180f)) + val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, TwistSwingConstraint(110f, 180f)) + val leftFootBone = Bone(BoneType.LEFT_FOOT, TwistSwingConstraint(180f, 120f)) + val rightFootBone = Bone(BoneType.RIGHT_FOOT, TwistSwingConstraint(180f, 120f)) // Arm bones - val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, CompleteConstraint()) - val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, CompleteConstraint()) - val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, TwistSwingConstraint(180f, 120f)) - val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, TwistSwingConstraint(180f, 120f)) - val leftHandBone = Bone(BoneType.LEFT_HAND, TwistSwingConstraint(180f, 110f)) - val rightHandBone = Bone(BoneType.RIGHT_HAND, TwistSwingConstraint(180f, 110f)) + val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, TwistSwingConstraint(180f, 7f)) + val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, TwistSwingConstraint(180f, 7f)) + val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, TwistSwingConstraint(180f, 180f)) + val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, TwistSwingConstraint(180f, 180f)) + val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, TwistSwingConstraint(90f, 180f)) + val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, TwistSwingConstraint(90f, 180f)) + val leftHandBone = Bone(BoneType.LEFT_HAND, TwistSwingConstraint(180f, 180f)) + val rightHandBone = Bone(BoneType.RIGHT_HAND, TwistSwingConstraint(180f, 180f)) // Tracker bones - val headTrackerBone = Bone(BoneType.HEAD_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val hipTrackerBone = Bone(BoneType.HIP_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) - val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, TwistSwingConstraint(Float.NaN, Float.NaN)) + val headTrackerBone = Bone(BoneType.HEAD_TRACKER, CompleteConstraint()) + val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, CompleteConstraint()) + val hipTrackerBone = Bone(BoneType.HIP_TRACKER, CompleteConstraint()) + val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, CompleteConstraint()) + val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, CompleteConstraint()) + val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, CompleteConstraint()) + val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, CompleteConstraint()) + val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, CompleteConstraint()) + val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, CompleteConstraint()) + val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, CompleteConstraint()) + val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, CompleteConstraint()) // Buffers var hasSpineTracker = false @@ -1084,6 +1084,7 @@ class HumanSkeleton( } legTweaks.resetBuffer() localizer.reset() + ikSolver.resetOffsets() LogManager.info("[HumanSkeleton] Reset: full ($resetSourceName)") } From 0c60b7a8d4ef7e87d7c330137627e2448ab0bd77 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 5 Jan 2024 16:32:52 -0800 Subject: [PATCH 22/68] spotless --- .../tracking/processor/HingeConstraint.kt | 32 +++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt index 69e7547fff..52c0bdd998 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt @@ -1,10 +1,38 @@ package dev.slimevr.tracking.processor +import com.jme3.math.FastMath import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 -class HingeConstraint(val min: Float, val max: Float) : Constraint() { +/** + * Constrain a rotation to one axis and a min and max value for that axis. + */ +class HingeConstraint(val min: Float, val max: Float, val axis: Vector3) : Constraint() { override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { - return rotation + // if there is no parent or no constraint return the direction + if (thisBone.parent == null) { + return rotation + } + + val parent = thisBone.parent!! + + // get the local rotation + val rotationLocal = (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation + + // project the rotation onto the axis + val projected = axis.dot(rotationLocal.xyz) + + // clamp the angle to the min and max values + val clampedAngle = FastMath.clamp( + projected, + Math.toRadians(min.toDouble()).toFloat(), + Math.toRadians(max.toDouble()).toFloat() + ) + + // reconstruct the rotation + val rot = Quaternion(rotationLocal.w, axis * clampedAngle).unit() + + // return the constrained rotation + return parent.getGlobalRotation() * thisBone.rotationOffset * rot } } From a8ca8f8b2e9afa4b1a19e56de0adb25f1e5d43dc Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 5 Jan 2024 17:04:13 -0800 Subject: [PATCH 23/68] add base constraint offset cause its actually needed --- .../slimevr/tracking/processor/skeleton/IKChain.kt | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 6b721362ae..5ef1b19f98 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -26,6 +26,7 @@ class IKChain( private var centroidWeight = 1f private var positions = getPositionList() private var tailConstrainPosOffset = Vector3.NULL + private var baseConstraintPosOffset = Vector3.NULL private fun getPositionList(): MutableList { val posList = mutableListOf() @@ -61,7 +62,7 @@ class IKChain( private fun forwards() { if (baseConstraint != null) { - positions[0] = baseConstraint.position + positions[0] = baseConstraint.position + baseConstraintPosOffset } else if (parent != null) { positions[0] = parent!!.positions.last() } @@ -110,10 +111,12 @@ class IKChain( } fun resetTrackerOffsets() { - if (tailConstraint == null) { - return + if (tailConstraint != null) { + tailConstrainPosOffset = nodes.last().getTailPosition() - tailConstraint.position + } + if (baseConstraint != null) { + baseConstraintPosOffset = nodes.first().getPosition() - baseConstraint.position } - tailConstrainPosOffset = nodes.last().getTailPosition() - tailConstraint.position } /** From 16771d4070c91be292273882801ae017b72d2eb4 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 12 Jan 2024 16:45:58 -0800 Subject: [PATCH 24/68] fix bug where a constrained sub-base being unable to solve would affect later chain's ability to reach the target. --- .../tracking/processor/skeleton/IKChain.kt | 29 +++++++++++-------- .../tracking/processor/skeleton/IKSolver.kt | 4 ++- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 5ef1b19f98..19dacb3dff 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -22,7 +22,7 @@ class IKChain( // state variables var children = mutableListOf() var target = Vector3.NULL - private var distToTargetSqr = Float.POSITIVE_INFINITY + var distToTargetSqr = Float.POSITIVE_INFINITY private var centroidWeight = 1f private var positions = getPositionList() private var tailConstrainPosOffset = Vector3.NULL @@ -50,21 +50,22 @@ class IKChain( positions[positions.size - 1] = target for (i in positions.size - 2 downTo 0) { - val direction = (positions[i] - positions[i + 1]).unit() - val constrainedDirection = nodes[i].rotationConstraint + var direction = (positions[i] - positions[i + 1]).unit() + direction = nodes[i].rotationConstraint .applyConstraintInverse(direction, nodes[i]) - positions[i] = positions[i + 1] + (constrainedDirection * nodes[i].length) + positions[i] = positions[i + 1] + (direction * nodes[i].length) } - if (parent != null) parent!!.target += positions[0] * centroidWeight + if (parent != null && parent!!.tailConstraint == null) + parent!!.target += positions[0] * centroidWeight } private fun forwards() { - if (baseConstraint != null) { - positions[0] = baseConstraint.position + baseConstraintPosOffset - } else if (parent != null) { + if (parent != null) { positions[0] = parent!!.positions.last() + } else if (baseConstraint != null) { + positions[0] = baseConstraint.position + baseConstraintPosOffset } for (i in 1 until positions.size - 1) { @@ -104,6 +105,8 @@ class IKChain( fun resetChain() { centroidWeight = 1f + distToTargetSqr = Float.POSITIVE_INFINITY + target = Vector3.NULL for (child in children) { child.resetChain() @@ -139,17 +142,19 @@ class IKChain( } /** - * Updates the distance to target and centroid weight variables - * and returns the new distance to the target + * Updates the distance to target and other fields + * Call on the root chain only returns the sum of the + * distances */ - fun computeTargetDistance(): Float { + fun computeTargetDistance() { distToTargetSqr = if (tailConstraint != null) { (positions.last() - (tailConstraint.position + tailConstrainPosOffset)).lenSq() } else { 0.0f } - return distToTargetSqr + for (chain in children) + chain.computeTargetDistance() } /** diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index a4ca1ae224..13c9e722f1 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -207,10 +207,12 @@ class IKSolver(private val root: Bone) { } rootChain?.forwardsMulti() + rootChain?.computeTargetDistance() + // if all chains have reached their target the chain is solved var solved = true for (chain in chainList) { - if (chain.computeTargetDistance() > TOLERANCE_SQR) { + if (chain.distToTargetSqr > TOLERANCE_SQR) { solved = false break } From 7ec4368750d32e0b2715c9b80e554de36a7008f9 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sat, 13 Jan 2024 17:00:06 -0800 Subject: [PATCH 25/68] fix bug where a chain would jitter if in reach of the target but unable to solve due to join restrictions --- .../tracking/processor/skeleton/IKChain.kt | 18 ++++++-- .../tracking/processor/skeleton/IKSolver.kt | 42 ++++++++++++------- 2 files changed, 41 insertions(+), 19 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 19dacb3dff..6b586b9271 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -17,12 +17,15 @@ class IKChain( ) { companion object { const val CENTROID_PULL_ADJUSTMENT = 0.1f + const val CENTROID_RECOVERY = 0.01f } // state variables var children = mutableListOf() var target = Vector3.NULL var distToTargetSqr = Float.POSITIVE_INFINITY + var trySolve = true + var solved = true private var centroidWeight = 1f private var positions = getPositionList() private var tailConstrainPosOffset = Vector3.NULL @@ -39,6 +42,8 @@ class IKChain( } fun backwards() { + if (!trySolve) return + // start at the constraint or the centroid of the children if (tailConstraint == null && children.size > 1) { target /= getChildrenCentroidWeightSum() @@ -57,11 +62,14 @@ class IKChain( positions[i] = positions[i + 1] + (direction * nodes[i].length) } - if (parent != null && parent!!.tailConstraint == null) + if (parent != null && parent!!.tailConstraint == null) { parent!!.target += positions[0] * centroidWeight + } } private fun forwards() { + if (!trySolve) return + if (parent != null) { positions[0] = parent!!.positions.last() } else if (baseConstraint != null) { @@ -104,9 +112,8 @@ class IKChain( } fun resetChain() { - centroidWeight = 1f distToTargetSqr = Float.POSITIVE_INFINITY - target = Vector3.NULL + trySolve = true for (child in children) { child.resetChain() @@ -134,6 +141,11 @@ class IKChain( if (child.distToTargetSqr < closestToSolved.distToTargetSqr) { closestToSolved = child } + child.centroidWeight = if (centroidWeight < 1f) { + child.centroidWeight + CENTROID_RECOVERY + } else { + child.centroidWeight + } } if (closestToSolved.centroidWeight > CENTROID_PULL_ADJUSTMENT) { diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 13c9e722f1..d24bfc8b2a 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -11,7 +11,7 @@ import dev.slimevr.tracking.trackers.Tracker class IKSolver(private val root: Bone) { companion object { - private const val TOLERANCE_SQR = 1e-8 // == 0.0001 cm + private const val TOLERANCE_SQR = 1e-8 // == 0.01 cm private const val MAX_ITERATIONS = 100 } @@ -26,15 +26,15 @@ class IKSolver(private val root: Bone) { fun buildChains(trackers: List) { chainList = mutableListOf() - // extract the positional constraints + // Extract the positional constraints val positionalConstraints = extractPositionalConstraints(trackers) val rotationalConstraints = extractRotationalConstraints(trackers) - // build the system of chains + // Build the system of chains rootChain = chainBuilder(root, null, 0, positionalConstraints, rotationalConstraints) populateChainList(rootChain!!) - // check if there is any constraints (other than the head) in the model + // Check if there is any constraints (other than the head) in the model rootChain = if (neededChain(rootChain!!)) rootChain else null chainList.sortBy { -it.level } } @@ -66,7 +66,7 @@ class IKSolver(private val root: Bone) { getConstraint(currentBone, rotationalConstraints) == null boneList.add(currentBone) - // get constraints + // Get constraints val baseConstraint = if (parent == null) { getConstraint(boneList.first(), positionalConstraints) } else { @@ -74,7 +74,7 @@ class IKSolver(private val root: Bone) { } var tailConstraint: Tracker? = null - // add bones until there is a reason to make a new chain + // Add bones until there is a reason to make a new chain while (currentBone.children.size == 1 && tailConstraint == null) { currentBone = currentBone.children[0] currentBone.rotationConstraint.allowModifications = @@ -88,7 +88,7 @@ class IKSolver(private val root: Bone) { if (currentBone.children.isNotEmpty()) { val childrenList = mutableListOf() - // build child chains + // Build child chains for (child in currentBone.children) { val childChain = chainBuilder(child, chain, level + 1, positionalConstraints, rotationalConstraints) if (neededChain(childChain)) { @@ -98,7 +98,7 @@ class IKSolver(private val root: Bone) { chain.children = childrenList } - // if the chain has only one child and no tail constraint combine the chains + // If the chain has only one child and no tail constraint combine the chains if (chain.children.size == 1 && chain.tailConstraint == null) { chain = combineChains(chain, chain.children[0]) } @@ -191,6 +191,7 @@ class IKSolver(private val root: Bone) { } fun solve() { + var solved: Boolean if (needsReset) { for (c in chainList) { c.resetTrackerOffsets() @@ -200,8 +201,8 @@ class IKSolver(private val root: Bone) { rootChain?.resetChain() - // run up to MAX_ITERATIONS iterations per tick - for (i in 0..MAX_ITERATIONS) { + // run up to MAX_ITERATIONS per tick + for (i in 0 until MAX_ITERATIONS) { for (chain in chainList) { chain.backwards() } @@ -209,24 +210,33 @@ class IKSolver(private val root: Bone) { rootChain?.computeTargetDistance() - // if all chains have reached their target the chain is solved - var solved = true + // Stop solving chains after one iteration for chains that + // failed to solve last tick this prevents some extreme jitter + if (i == 0) { + for (chain in chainList) { + chain.trySolve = chain.solved + } + } + + // If all chains have reached their target the chain is solved + solved = true for (chain in chainList) { - if (chain.distToTargetSqr > TOLERANCE_SQR) { + if (chain.distToTargetSqr > TOLERANCE_SQR && chain.trySolve) { + chain.solved = false solved = false - break + } else if (chain.distToTargetSqr <= TOLERANCE_SQR) { + chain.solved = true } } if (solved) break - // help the chains out of a deadlock + // Help the chains out of a deadlock for (chain in chainList) { chain.updateChildCentroidWeight() } } - // update transforms root.update() } } From a879967de5b89b4e00c2af6f7e47b576ee28d28f Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 19 Feb 2024 13:30:32 -0800 Subject: [PATCH 26/68] small things --- .../dev/slimevr/tracking/processor/Constraint.kt | 3 ++- .../tracking/processor/skeleton/IKChain.kt | 15 +++++---------- .../tracking/processor/skeleton/IKSolver.kt | 2 +- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 9870d62768..b0c1a42f8c 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -23,11 +23,12 @@ abstract class Constraint { * to be unchanged unless it violates the constraints */ fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { + val rotation = Quaternion.fromTo(Vector3.NEG_Y, direction) if (!allowModifications) { return constraintRotation(thisBone.getGlobalRotation(), thisBone) } - return constraintRotation(Quaternion.fromTo(Vector3.NEG_Y, direction), thisBone) + return constraintRotation(rotation, thisBone) } /** diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 6b586b9271..08b356eb22 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -17,10 +17,9 @@ class IKChain( ) { companion object { const val CENTROID_PULL_ADJUSTMENT = 0.1f - const val CENTROID_RECOVERY = 0.01f } - // state variables + // State variables var children = mutableListOf() var target = Vector3.NULL var distToTargetSqr = Float.POSITIVE_INFINITY @@ -44,14 +43,14 @@ class IKChain( fun backwards() { if (!trySolve) return - // start at the constraint or the centroid of the children + // Start at the constraint or the centroid of the children if (tailConstraint == null && children.size > 1) { target /= getChildrenCentroidWeightSum() } else { target = (tailConstraint?.position?.plus(tailConstrainPosOffset)) ?: Vector3.NULL } - // set the end node to target + // Set the end node to target positions[positions.size - 1] = target for (i in positions.size - 2 downTo 0) { @@ -82,7 +81,7 @@ class IKChain( positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) } - // point the last bone at the target + // Point the last bone at the target 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) @@ -113,6 +112,7 @@ class IKChain( fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY + centroidWeight = 1f trySolve = true for (child in children) { @@ -141,11 +141,6 @@ class IKChain( if (child.distToTargetSqr < closestToSolved.distToTargetSqr) { closestToSolved = child } - child.centroidWeight = if (centroidWeight < 1f) { - child.centroidWeight + CENTROID_RECOVERY - } else { - child.centroidWeight - } } if (closestToSolved.centroidWeight > CENTROID_PULL_ADJUSTMENT) { diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index d24bfc8b2a..c675d1f86e 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -11,7 +11,7 @@ import dev.slimevr.tracking.trackers.Tracker class IKSolver(private val root: Bone) { companion object { - private const val TOLERANCE_SQR = 1e-8 // == 0.01 cm + const val TOLERANCE_SQR = 1e-8 // == 0.01 cm private const val MAX_ITERATIONS = 100 } From 66b39677ed65e1e5d4de63978502bdc177b2a89f Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 19 Feb 2024 18:28:06 -0800 Subject: [PATCH 27/68] Add IKConstraint --- .../tracking/processor/skeleton/IKChain.kt | 26 ++++++++----------- .../processor/skeleton/IKConstraint.kt | 19 ++++++++++++++ 2 files changed, 30 insertions(+), 15 deletions(-) create mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKConstraint.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 08b356eb22..f1a4f94080 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -20,6 +20,8 @@ class IKChain( } // State variables + val computedBasePosition = baseConstraint?.let { IKConstraint(it) } + val computedTailPosition = tailConstraint?.let { IKConstraint(it) } var children = mutableListOf() var target = Vector3.NULL var distToTargetSqr = Float.POSITIVE_INFINITY @@ -27,8 +29,6 @@ class IKChain( var solved = true private var centroidWeight = 1f private var positions = getPositionList() - private var tailConstrainPosOffset = Vector3.NULL - private var baseConstraintPosOffset = Vector3.NULL private fun getPositionList(): MutableList { val posList = mutableListOf() @@ -44,10 +44,10 @@ class IKChain( if (!trySolve) return // Start at the constraint or the centroid of the children - if (tailConstraint == null && children.size > 1) { + if (computedTailPosition == null && children.size > 1) { target /= getChildrenCentroidWeightSum() } else { - target = (tailConstraint?.position?.plus(tailConstrainPosOffset)) ?: Vector3.NULL + target = (computedTailPosition?.getPosition()) ?: Vector3.NULL } // Set the end node to target @@ -61,7 +61,7 @@ class IKChain( positions[i] = positions[i + 1] + (direction * nodes[i].length) } - if (parent != null && parent!!.tailConstraint == null) { + if (parent != null && parent!!.computedTailPosition == null) { parent!!.target += positions[0] * centroidWeight } } @@ -71,8 +71,8 @@ class IKChain( if (parent != null) { positions[0] = parent!!.positions.last() - } else if (baseConstraint != null) { - positions[0] = baseConstraint.position + baseConstraintPosOffset + } else if (computedBasePosition != null) { + positions[0] = computedBasePosition.getPosition() } for (i in 1 until positions.size - 1) { @@ -121,12 +121,8 @@ class IKChain( } fun resetTrackerOffsets() { - if (tailConstraint != null) { - tailConstrainPosOffset = nodes.last().getTailPosition() - tailConstraint.position - } - if (baseConstraint != null) { - baseConstraintPosOffset = nodes.first().getPosition() - baseConstraint.position - } + computedTailPosition?.reset(nodes.last().getTailPosition()) + computedBasePosition?.reset(nodes.first().getPosition()) } /** @@ -154,8 +150,8 @@ class IKChain( * distances */ fun computeTargetDistance() { - distToTargetSqr = if (tailConstraint != null) { - (positions.last() - (tailConstraint.position + tailConstrainPosOffset)).lenSq() + distToTargetSqr = if (computedTailPosition != null) { + (positions.last() - computedTailPosition.getPosition()).lenSq() } else { 0.0f } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKConstraint.kt new file mode 100644 index 0000000000..5e58219399 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKConstraint.kt @@ -0,0 +1,19 @@ +package dev.slimevr.tracking.processor.skeleton + +import dev.slimevr.tracking.trackers.Tracker +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 + +class IKConstraint(val tracker: Tracker) { + private var offset = Vector3.NULL + private var rotationOffset = Quaternion.IDENTITY + + fun getPosition(): Vector3 { + return tracker.position + (tracker.getRotation() * rotationOffset).sandwich(offset) + } + + fun reset(nodePosition: Vector3) { + offset = nodePosition - tracker.position + rotationOffset = tracker.getRotation().inv() + } +} From 57e732b8d90feade717c0576e08f22bcd74c273b Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 19 Feb 2024 18:28:43 -0800 Subject: [PATCH 28/68] Reduce IKSolver control in the spine area --- .../slimevr/tracking/processor/skeleton/HumanSkeleton.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 14e122f1d0..4b460018a3 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -38,10 +38,10 @@ class HumanSkeleton( // Upper body bones val headBone = Bone(BoneType.HEAD, CompleteConstraint()) val neckBone = Bone(BoneType.NECK, CompleteConstraint()) - val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(180f, 60f)) - val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(180f, 60f)) - val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(180f, 60f)) - val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(180f, 90f)) + val upperChestBone = Bone(BoneType.UPPER_CHEST, CompleteConstraint()) + val chestBone = Bone(BoneType.CHEST, CompleteConstraint()) + val waistBone = Bone(BoneType.WAIST, CompleteConstraint()) + val hipBone = Bone(BoneType.HIP, CompleteConstraint()) // Lower body bones val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(180f, 7f)) From dfb048ab71d05e0b38fd8fcdc5c830ec80d83a11 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sun, 17 Mar 2024 18:20:54 -0700 Subject: [PATCH 29/68] Allow deviation from rotational constraints if the chains cannot solve --- .../slimevr/tracking/processor/Constraint.kt | 73 ++++++++++++++++++- .../processor/TwistSwingConstraint.kt | 29 -------- .../tracking/processor/skeleton/IKChain.kt | 46 +++++++----- .../tracking/processor/skeleton/IKSolver.kt | 30 ++++---- 4 files changed, 113 insertions(+), 65 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index b0c1a42f8c..ea15716ee1 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -1,5 +1,6 @@ package dev.slimevr.tracking.processor +import com.jme3.math.FastMath import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 @@ -13,6 +14,23 @@ abstract class Constraint { */ var allowModifications = true + /** + * If allowModifications is false this is the + * allowed deviation from the original rotation + */ + var tolerance = 0.0f + set(value) { + field = value + updateComposedField() + } + + private var toleranceRad = 0.0f + var originalRotation = Quaternion.NULL + + private fun updateComposedField() { + toleranceRad = Math.toRadians(tolerance.toDouble()).toFloat() + } + /** * Apply rotational constraints to the direction vector */ @@ -24,11 +42,12 @@ abstract class Constraint { */ fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { val rotation = Quaternion.fromTo(Vector3.NEG_Y, direction) - if (!allowModifications) { - return constraintRotation(thisBone.getGlobalRotation(), thisBone) + if (allowModifications) { + return constraintRotation(rotation, thisBone) } - return constraintRotation(rotation, thisBone) + val constrainedRotation = applyLimits(rotation, originalRotation) + return constraintRotation(constrainedRotation, thisBone) } /** @@ -40,4 +59,52 @@ abstract class Constraint { fun applyConstraintInverse(direction: Vector3, thisBone: Bone): Vector3 { return -applyConstraint(-direction, thisBone).sandwich(Vector3.NEG_Y) } + + /** + * Limit the rotation to tolerance away from the initialRotation + */ + protected fun applyLimits( + rotation: Quaternion, + initialRotation: Quaternion, + ): Quaternion { + val localRotation = initialRotation.inv() * rotation + + var (swingQ, twistQ) = decompose(localRotation, Vector3.NEG_Y) + + twistQ = constrain(twistQ, toleranceRad) + swingQ = constrain(swingQ, toleranceRad) + + return initialRotation * (swingQ * twistQ) + } + + protected fun decompose( + rotation: Quaternion, + twistAxis: Vector3, + ): Pair { + val projection = rotation.project(twistAxis) + + val twist = Quaternion(rotation.w, projection.xyz).unit() + val swing = rotation * twist.inv() + + return Pair(swing, twist) + } + + protected fun constrain(rotation: Quaternion, angle: Float): Quaternion { + val magnitude = FastMath.sin(angle * 0.5f) + val magnitudeSqr = magnitude * magnitude + var vector = rotation.xyz + var rot = rotation + + if (vector.lenSq() > magnitudeSqr) { + vector = vector.unit() * magnitude + rot = Quaternion( + FastMath.sqrt(1.0f - magnitudeSqr) * FastMath.sign(rot.w), + vector.x, + vector.y, + vector.z + ) + } + + return rot + } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index 85990e5dc9..29ba31b382 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -1,6 +1,5 @@ package dev.slimevr.tracking.processor -import com.jme3.math.FastMath import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 @@ -12,34 +11,6 @@ class TwistSwingConstraint(private val twist: Float, private val swing: Float) : private val twistRad = Math.toRadians(twist.toDouble()).toFloat() private val swingRad = Math.toRadians(swing.toDouble()).toFloat() - private fun decompose(rotation: Quaternion, twistAxis: Vector3): Pair { - val projection = rotation.project(twistAxis) - - val twist = Quaternion(rotation.w, projection.xyz).unit() - val swing = rotation * twist.inv() - - return Pair(swing, twist) - } - - private fun constrain(rotation: Quaternion, angle: Float): Quaternion { - val magnitude = FastMath.sin(angle * 0.5f) - val magnitudeSqr = magnitude * magnitude - var vector = rotation.xyz - var rot = rotation - - if (vector.lenSq() > magnitudeSqr) { - vector = vector.unit() * magnitude - rot = Quaternion( - FastMath.sqrt(1.0f - magnitudeSqr) * FastMath.sign(rot.w), - vector.x, - vector.y, - vector.z - ) - } - - return rot - } - override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { // if there is no parent or no constraint return the direction if (thisBone.parent == null || (swing.isNaN() && twist.isNaN())) { diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index f1a4f94080..2a1385b565 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -20,13 +20,12 @@ class IKChain( } // State variables - val computedBasePosition = baseConstraint?.let { IKConstraint(it) } - val computedTailPosition = tailConstraint?.let { IKConstraint(it) } + private val computedBasePosition = baseConstraint?.let { IKConstraint(it) } + private val computedTailPosition = tailConstraint?.let { IKConstraint(it) } var children = mutableListOf() + private var targetSum = Vector3.NULL var target = Vector3.NULL var distToTargetSqr = Float.POSITIVE_INFINITY - var trySolve = true - var solved = true private var centroidWeight = 1f private var positions = getPositionList() @@ -41,13 +40,11 @@ class IKChain( } fun backwards() { - if (!trySolve) return - // Start at the constraint or the centroid of the children - if (computedTailPosition == null && children.size > 1) { - target /= getChildrenCentroidWeightSum() + target = if (computedTailPosition == null && children.size > 1) { + targetSum / getChildrenCentroidWeightSum() } else { - target = (computedTailPosition?.getPosition()) ?: Vector3.NULL + (computedTailPosition?.getPosition()) ?: Vector3.NULL } // Set the end node to target @@ -62,17 +59,15 @@ class IKChain( } if (parent != null && parent!!.computedTailPosition == null) { - parent!!.target += positions[0] * centroidWeight + parent!!.targetSum += positions[0] * centroidWeight } } private fun forwards() { - if (!trySolve) return - - if (parent != null) { - positions[0] = parent!!.positions.last() - } else if (computedBasePosition != null) { - positions[0] = computedBasePosition.getPosition() + positions[0] = if (parent != null) { + parent!!.nodes.last().getTailPosition() + } else { + (computedBasePosition?.getPosition()) ?: positions[0] } for (i in 1 until positions.size - 1) { @@ -87,7 +82,7 @@ class IKChain( positions[positions.size - 1] = positions[positions.size - 2] + (direction * nodes.last().length) // reset sub-base target - target = Vector3.NULL + targetSum = Vector3.NULL } /** @@ -113,7 +108,11 @@ class IKChain( fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY centroidWeight = 1f - trySolve = true + + for (bone in nodes) { + bone.rotationConstraint.tolerance = 0.0f + bone.rotationConstraint.originalRotation = bone.getGlobalRotation() + } for (child in children) { child.resetChain() @@ -144,6 +143,15 @@ class IKChain( } } + /** + * Allow constrained bones to deviate more per step + */ + fun decreaseConstraints() { + for (bone in nodes) { + 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 @@ -170,7 +178,7 @@ class IKChain( bone.setRotationRaw(rotation) // TODO optimize (this is required to update the world translation for the next bone as it uses the world - // rotation of the parent) + // rotation of the parent. We only need to update this bones translation though so this is very wasteful) bone.update() return rotation.sandwich(Vector3.NEG_Y) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index c675d1f86e..cc7a0aa3ec 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -12,7 +12,9 @@ import dev.slimevr.tracking.trackers.Tracker class IKSolver(private val root: Bone) { companion object { const val TOLERANCE_SQR = 1e-8 // == 0.01 cm - private const val MAX_ITERATIONS = 100 + const val MAX_ITERATIONS = 100 + const val ITERATIONS_BEFORE_STEP = MAX_ITERATIONS / 8 + const val TOLERANCE_STEP = 2f } private var chainList = mutableListOf() @@ -24,7 +26,7 @@ class IKSolver(private val root: Bone) { * should be rebuilt. */ fun buildChains(trackers: List) { - chainList = mutableListOf() + chainList.clear() // Extract the positional constraints val positionalConstraints = extractPositionalConstraints(trackers) @@ -191,6 +193,8 @@ class IKSolver(private val root: Bone) { } fun solve() { + if (rootChain == null) return + var solved: Boolean if (needsReset) { for (c in chainList) { @@ -210,22 +214,11 @@ class IKSolver(private val root: Bone) { rootChain?.computeTargetDistance() - // Stop solving chains after one iteration for chains that - // failed to solve last tick this prevents some extreme jitter - if (i == 0) { - for (chain in chainList) { - chain.trySolve = chain.solved - } - } - // If all chains have reached their target the chain is solved solved = true for (chain in chainList) { - if (chain.distToTargetSqr > TOLERANCE_SQR && chain.trySolve) { - chain.solved = false + if (chain.distToTargetSqr > TOLERANCE_SQR) { solved = false - } else if (chain.distToTargetSqr <= TOLERANCE_SQR) { - chain.solved = true } } @@ -235,6 +228,15 @@ class IKSolver(private val root: Bone) { for (chain in chainList) { chain.updateChildCentroidWeight() } + + // Loosen rotational constraints + // TODO only do this if a positional tracker down the chain is actually + // tracking accurately + if (i % ITERATIONS_BEFORE_STEP == 0 && i != 0) { + for (chain in chainList) { + chain.decreaseConstraints() + } + } } root.update() From 1d5a1a58d1e35ec2ed5c991835cb33cee50be84e Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Tue, 26 Mar 2024 15:15:30 -0700 Subject: [PATCH 30/68] Update loosening to be more selective and fix chain builder bug --- .../processor/skeleton/HumanSkeleton.kt | 8 ++-- .../tracking/processor/skeleton/IKChain.kt | 3 ++ .../tracking/processor/skeleton/IKSolver.kt | 47 +++++++++++++------ 3 files changed, 40 insertions(+), 18 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 4b460018a3..3aba42077c 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -38,10 +38,10 @@ class HumanSkeleton( // Upper body bones val headBone = Bone(BoneType.HEAD, CompleteConstraint()) val neckBone = Bone(BoneType.NECK, CompleteConstraint()) - val upperChestBone = Bone(BoneType.UPPER_CHEST, CompleteConstraint()) - val chestBone = Bone(BoneType.CHEST, CompleteConstraint()) - val waistBone = Bone(BoneType.WAIST, CompleteConstraint()) - val hipBone = Bone(BoneType.HIP, CompleteConstraint()) + val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(180f, 180f)) + val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(180f, 180f)) + val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(180f, 180f)) + val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(180f, 180f)) // Lower body bones val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(180f, 7f)) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 2a1385b565..034ba84ffc 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -26,6 +26,7 @@ class IKChain( private var targetSum = Vector3.NULL var target = Vector3.NULL var distToTargetSqr = Float.POSITIVE_INFINITY + var loosense = 0 private var centroidWeight = 1f private var positions = getPositionList() @@ -108,6 +109,7 @@ class IKChain( fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY centroidWeight = 1f + loosense = 0 for (bone in nodes) { bone.rotationConstraint.tolerance = 0.0f @@ -147,6 +149,7 @@ class IKChain( * Allow constrained bones to deviate more per step */ fun decreaseConstraints() { + loosense++ for (bone in nodes) { bone.rotationConstraint.tolerance += IKSolver.TOLERANCE_STEP } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index cc7a0aa3ec..02bbf28d99 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -13,7 +13,9 @@ class IKSolver(private val root: Bone) { companion object { const val TOLERANCE_SQR = 1e-8 // == 0.01 cm const val MAX_ITERATIONS = 100 - const val ITERATIONS_BEFORE_STEP = MAX_ITERATIONS / 8 + const val ITERATIONS_BEFORE_STEP = 20 + const val ITERATIONS_BETWEEN_STEP = 5 + const val MAX_LOOSENS = 5 const val TOLERANCE_STEP = 2f } @@ -35,6 +37,7 @@ class IKSolver(private val root: Bone) { // Build the system of chains rootChain = chainBuilder(root, null, 0, positionalConstraints, rotationalConstraints) populateChainList(rootChain!!) + addConstraints() // Check if there is any constraints (other than the head) in the model rootChain = if (neededChain(rootChain!!)) rootChain else null @@ -74,15 +77,15 @@ class IKSolver(private val root: Bone) { } else { parent.tailConstraint } - var tailConstraint: Tracker? = null + var tailConstraint = getConstraint(currentBone, positionalConstraints) // Add bones until there is a reason to make a new chain while (currentBone.children.size == 1 && tailConstraint == null) { - currentBone = currentBone.children[0] + currentBone = currentBone.children.first() currentBone.rotationConstraint.allowModifications = getConstraint(currentBone, rotationalConstraints) == null boneList.add(currentBone) - tailConstraint = getConstraint(boneList.last(), positionalConstraints) + tailConstraint = getConstraint(currentBone, positionalConstraints) } var chain = IKChain(boneList, parent, level, baseConstraint, tailConstraint) @@ -102,7 +105,7 @@ class IKSolver(private val root: Bone) { // If the chain has only one child and no tail constraint combine the chains if (chain.children.size == 1 && chain.tailConstraint == null) { - chain = combineChains(chain, chain.children[0]) + chain = combineChains(chain, chain.children.first()) } return chain @@ -137,6 +140,12 @@ class IKSolver(private val root: Bone) { return newChain } + private fun addConstraints() { + fun constrainChain(chain: IKChain) = + chain.nodes.forEach { it.rotationConstraint.allowModifications = false } + chainList.forEach { if (it.tailConstraint == null) constrainChain(it) } + } + private fun neededChain(chain: IKChain): Boolean { if (chain.tailConstraint != null) { return true @@ -192,6 +201,24 @@ class IKSolver(private val root: Bone) { return null } + /** + * Loosen rotational constraints gradually + */ + private fun loosenConstraints(iter: Int) { + if (iter < ITERATIONS_BEFORE_STEP && iter % ITERATIONS_BETWEEN_STEP != 0) return + + var maxDist = Float.NEGATIVE_INFINITY + var maxDistChain = chainList.first() + for (chain in chainList) { + if (chain.distToTargetSqr > maxDist) { + maxDist = chain.distToTargetSqr + maxDistChain = chain + } + } + + if (maxDistChain.loosense < MAX_LOOSENS) maxDistChain.decreaseConstraints() + } + fun solve() { if (rootChain == null) return @@ -205,7 +232,6 @@ class IKSolver(private val root: Bone) { rootChain?.resetChain() - // run up to MAX_ITERATIONS per tick for (i in 0 until MAX_ITERATIONS) { for (chain in chainList) { chain.backwards() @@ -229,14 +255,7 @@ class IKSolver(private val root: Bone) { chain.updateChildCentroidWeight() } - // Loosen rotational constraints - // TODO only do this if a positional tracker down the chain is actually - // tracking accurately - if (i % ITERATIONS_BEFORE_STEP == 0 && i != 0) { - for (chain in chainList) { - chain.decreaseConstraints() - } - } + loosenConstraints(i) } root.update() From 025a3acfccf6eb9722c3b467206feb2db8690bac Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 29 Mar 2024 11:59:24 -0700 Subject: [PATCH 31/68] Fix bugs, remove unused code --- .../dev/slimevr/tracking/processor/Bone.kt | 8 ++++ .../tracking/processor/CompleteConstraint.kt | 5 +-- .../slimevr/tracking/processor/Constraint.kt | 11 +++--- .../tracking/processor/HingeConstraint.kt | 38 ------------------- .../tracking/processor/TransformNode.kt | 5 +++ .../processor/skeleton/HumanSkeleton.kt | 18 +++++---- .../tracking/processor/skeleton/IKChain.kt | 13 +++---- .../processor/skeleton/IKConstraint.kt | 5 +-- .../tracking/processor/skeleton/IKSolver.kt | 6 +-- 9 files changed, 41 insertions(+), 68 deletions(-) delete mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt index f5a952abca..43f63b6bab 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt @@ -58,6 +58,14 @@ class Bone(val boneType: BoneType, val rotationConstraint: Constraint) { headNode.update() } + /** + * Computes the rotations and positions of this bone. + */ + fun updateThisNode() { + headNode.updateThisNode() + tailNode.updateThisNode() + } + /** * Returns the world-aligned rotation of the bone */ diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt index 238d459c94..506769d4ff 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt @@ -6,7 +6,6 @@ import io.github.axisangles.ktmath.Quaternion * A constraint type that allows no modifications to the rotation */ class CompleteConstraint : Constraint() { - override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { - return thisBone.getGlobalRotation() - } + override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion = + thisBone.getGlobalRotation() } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index ea15716ee1..c6134be66b 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -43,11 +43,11 @@ abstract class Constraint { fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { val rotation = Quaternion.fromTo(Vector3.NEG_Y, direction) if (allowModifications) { - return constraintRotation(rotation, thisBone) + return constraintRotation(rotation, thisBone).unit() } val constrainedRotation = applyLimits(rotation, originalRotation) - return constraintRotation(constrainedRotation, thisBone) + return constraintRotation(constrainedRotation, thisBone).unit() } /** @@ -56,9 +56,8 @@ abstract class Constraint { * This is used for constraining direction vectors on the backwards pass * of the FABRIK solver. */ - fun applyConstraintInverse(direction: Vector3, thisBone: Bone): Vector3 { - return -applyConstraint(-direction, thisBone).sandwich(Vector3.NEG_Y) - } + fun applyConstraintInverse(direction: Vector3, thisBone: Bone): Vector3 = + -applyConstraint(-direction, thisBone).sandwich(Vector3.NEG_Y) /** * Limit the rotation to tolerance away from the initialRotation @@ -101,7 +100,7 @@ abstract class Constraint { FastMath.sqrt(1.0f - magnitudeSqr) * FastMath.sign(rot.w), vector.x, vector.y, - vector.z + vector.z, ) } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt deleted file mode 100644 index 52c0bdd998..0000000000 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HingeConstraint.kt +++ /dev/null @@ -1,38 +0,0 @@ -package dev.slimevr.tracking.processor - -import com.jme3.math.FastMath -import io.github.axisangles.ktmath.Quaternion -import io.github.axisangles.ktmath.Vector3 - -/** - * Constrain a rotation to one axis and a min and max value for that axis. - */ -class HingeConstraint(val min: Float, val max: Float, val axis: Vector3) : Constraint() { - override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { - // if there is no parent or no constraint return the direction - if (thisBone.parent == null) { - return rotation - } - - val parent = thisBone.parent!! - - // get the local rotation - val rotationLocal = (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation - - // project the rotation onto the axis - val projected = axis.dot(rotationLocal.xyz) - - // clamp the angle to the min and max values - val clampedAngle = FastMath.clamp( - projected, - Math.toRadians(min.toDouble()).toFloat(), - Math.toRadians(max.toDouble()).toFloat() - ) - - // reconstruct the rotation - val rot = Quaternion(rotationLocal.w, axis * clampedAngle).unit() - - // return the constrained rotation - return parent.getGlobalRotation() * thisBone.rotationOffset * rot - } -} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TransformNode.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TransformNode.kt index 773e11e282..626cf35cbe 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TransformNode.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TransformNode.kt @@ -32,6 +32,11 @@ class TransformNode(val localRotation: Boolean) { } } + @ThreadSafe + fun updateThisNode() { + updateWorldTransforms() + } + @Synchronized private fun updateWorldTransforms() { worldTransform.set(localTransform) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 3aba42077c..34889d3b3e 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -38,14 +38,14 @@ class HumanSkeleton( // Upper body bones val headBone = Bone(BoneType.HEAD, CompleteConstraint()) val neckBone = Bone(BoneType.NECK, CompleteConstraint()) - val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(180f, 180f)) - val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(180f, 180f)) - val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(180f, 180f)) - val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(180f, 180f)) + val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(10f, 80f)) + val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(10f, 80f)) + val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(10f, 80f)) + val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(10f, 80f)) // Lower body bones - val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(180f, 7f)) - val rightHipBone = Bone(BoneType.RIGHT_HIP, TwistSwingConstraint(180f, 7f)) + val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(10f, 7f)) + val rightHipBone = Bone(BoneType.RIGHT_HIP, TwistSwingConstraint(10f, 7f)) val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, TwistSwingConstraint(180f, 120f)) val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, TwistSwingConstraint(180f, 120f)) val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, TwistSwingConstraint(110f, 180f)) @@ -58,8 +58,10 @@ class HumanSkeleton( val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, TwistSwingConstraint(180f, 7f)) val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, TwistSwingConstraint(180f, 180f)) val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, TwistSwingConstraint(180f, 180f)) - val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, TwistSwingConstraint(90f, 180f)) - val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, TwistSwingConstraint(90f, 180f)) + val leftLowerArmBone = + Bone(BoneType.LEFT_LOWER_ARM, TwistSwingConstraint(180f, 180f)) + val rightLowerArmBone = + Bone(BoneType.RIGHT_LOWER_ARM, TwistSwingConstraint(180f, 180f)) val leftHandBone = Bone(BoneType.LEFT_HAND, TwistSwingConstraint(180f, 180f)) val rightHandBone = Bone(BoneType.RIGHT_HAND, TwistSwingConstraint(180f, 180f)) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 034ba84ffc..3413d5993b 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -26,7 +26,7 @@ class IKChain( private var targetSum = Vector3.NULL var target = Vector3.NULL var distToTargetSqr = Float.POSITIVE_INFINITY - var loosense = 0 + var loosens = 0 private var centroidWeight = 1f private var positions = getPositionList() @@ -109,7 +109,7 @@ class IKChain( fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY centroidWeight = 1f - loosense = 0 + loosens = 0 for (bone in nodes) { bone.rotationConstraint.tolerance = 0.0f @@ -149,7 +149,7 @@ class IKChain( * Allow constrained bones to deviate more per step */ fun decreaseConstraints() { - loosense++ + loosens++ for (bone in nodes) { bone.rotationConstraint.tolerance += IKSolver.TOLERANCE_STEP } @@ -167,8 +167,9 @@ class IKChain( 0.0f } - for (chain in children) + for (chain in children) { chain.computeTargetDistance() + } } /** @@ -180,9 +181,7 @@ class IKChain( val rotation = bone.rotationConstraint.applyConstraint(rotationVector, bone) bone.setRotationRaw(rotation) - // TODO optimize (this is required to update the world translation for the next bone as it uses the world - // rotation of the parent. We only need to update this bones translation though so this is very wasteful) - bone.update() + bone.updateThisNode() return rotation.sandwich(Vector3.NEG_Y) } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKConstraint.kt index 5e58219399..208583b414 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKConstraint.kt @@ -8,9 +8,8 @@ class IKConstraint(val tracker: Tracker) { private var offset = Vector3.NULL private var rotationOffset = Quaternion.IDENTITY - fun getPosition(): Vector3 { - return tracker.position + (tracker.getRotation() * rotationOffset).sandwich(offset) - } + fun getPosition(): Vector3 = + tracker.position + (tracker.getRotation() * rotationOffset).sandwich(offset) fun reset(nodePosition: Vector3) { offset = nodePosition - tracker.position diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 02bbf28d99..6fff5ae390 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -128,7 +128,7 @@ class IKSolver(private val root: Bone) { chain.parent, chain.level, chain.baseConstraint, - childChain.tailConstraint + childChain.tailConstraint, ) newChain.children = childChain.children @@ -216,7 +216,7 @@ class IKSolver(private val root: Bone) { } } - if (maxDistChain.loosense < MAX_LOOSENS) maxDistChain.decreaseConstraints() + if (maxDistChain.loosens < MAX_LOOSENS) maxDistChain.decreaseConstraints() } fun solve() { @@ -229,7 +229,7 @@ class IKSolver(private val root: Bone) { } needsReset = false } - + rootChain?.resetChain() for (i in 0 until MAX_ITERATIONS) { From ac4fb4652b21b5316a888019cb222bbb43b53bf5 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 29 Mar 2024 14:18:43 -0700 Subject: [PATCH 32/68] change imports --- .../java/dev/slimevr/tracking/processor/Constraint.kt | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index c6134be66b..af9f610d23 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -1,6 +1,8 @@ package dev.slimevr.tracking.processor -import com.jme3.math.FastMath +import com.jme3.math.FastMath.sign +import com.jme3.math.FastMath.sqrt +import com.jme3.math.FastMath.sin import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 @@ -89,7 +91,7 @@ abstract class Constraint { } protected fun constrain(rotation: Quaternion, angle: Float): Quaternion { - val magnitude = FastMath.sin(angle * 0.5f) + val magnitude = sin(angle * 0.5f) val magnitudeSqr = magnitude * magnitude var vector = rotation.xyz var rot = rotation @@ -97,7 +99,7 @@ abstract class Constraint { if (vector.lenSq() > magnitudeSqr) { vector = vector.unit() * magnitude rot = Quaternion( - FastMath.sqrt(1.0f - magnitudeSqr) * FastMath.sign(rot.w), + sqrt(1.0f - magnitudeSqr) * sign(rot.w), vector.x, vector.y, vector.z, From bdedcb0878fc2067dcd6fbb50dc33835b7231a87 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 29 Mar 2024 14:47:53 -0700 Subject: [PATCH 33/68] make spotless happy --- .../src/main/java/dev/slimevr/tracking/processor/Constraint.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index af9f610d23..8e64921665 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -1,8 +1,8 @@ package dev.slimevr.tracking.processor import com.jme3.math.FastMath.sign -import com.jme3.math.FastMath.sqrt import com.jme3.math.FastMath.sin +import com.jme3.math.FastMath.sqrt import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 From 7af8d66b49902425fe7a7039df784a4f6995cded Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sat, 30 Mar 2024 12:09:48 -0700 Subject: [PATCH 34/68] Fix bug causing NULL Quaternion --- .../src/main/java/dev/slimevr/tracking/processor/Constraint.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 8e64921665..cc387bcacf 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -93,13 +93,14 @@ abstract class Constraint { protected 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 var vector = rotation.xyz var rot = rotation if (vector.lenSq() > magnitudeSqr) { vector = vector.unit() * magnitude rot = Quaternion( - sqrt(1.0f - magnitudeSqr) * sign(rot.w), + sqrt(1.0f - magnitudeSqr) * sign, vector.x, vector.y, vector.z, From 597bd286c9c05a46591a99d2e1b5c27713cd5937 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sat, 30 Mar 2024 12:10:08 -0700 Subject: [PATCH 35/68] pause ikSolver --- .../dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 34889d3b3e..e68493d0a4 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -370,7 +370,9 @@ class HumanSkeleton( updateTransforms() updateBones() - ikSolver.solve() + + if (!pauseTracking) ikSolver.solve() + updateComputedTrackers() // Don't run post-processing if the tracking is paused From 97e60a3b602da7d06eb05a2de53bde8b59ca1301 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 3 Apr 2024 17:11:36 -0700 Subject: [PATCH 36/68] small formatting things --- .../tracking/processor/TwistSwingConstraint.kt | 16 ++++------------ .../tracking/processor/skeleton/IKChain.kt | 7 ++++--- .../tracking/processor/skeleton/IKSolver.kt | 12 ++++-------- 3 files changed, 12 insertions(+), 23 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt index 29ba31b382..9588b3dc29 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt @@ -7,27 +7,19 @@ import io.github.axisangles.ktmath.Vector3 * A constraint type that allows for a rotation to be constrained relative to the parent * of the supplied bone */ -class TwistSwingConstraint(private val twist: Float, private val swing: Float) : Constraint() { +class TwistSwingConstraint(twist: Float, swing: Float) : Constraint() { private val twistRad = Math.toRadians(twist.toDouble()).toFloat() private val swingRad = Math.toRadians(swing.toDouble()).toFloat() override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { - // if there is no parent or no constraint return the direction - if (thisBone.parent == null || (swing.isNaN() && twist.isNaN())) { - return rotation - } - + if (thisBone.parent == null) return rotation val parent = thisBone.parent!! - // get the local rotation val rotationLocal = (parent.getGlobalRotation() * thisBone.rotationOffset).inv() * rotation - - // decompose in to twist and swing var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) - // apply the constraints - if (!swing.isNaN()) swingQ = constrain(swingQ, swingRad) - if (!twist.isNaN()) twistQ = constrain(twistQ, twistRad) + swingQ = constrain(swingQ, swingRad) + twistQ = constrain(twistQ, twistRad) return parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ) } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 3413d5993b..42222b5cc7 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -48,7 +48,6 @@ class IKChain( (computedTailPosition?.getPosition()) ?: Vector3.NULL } - // Set the end node to target positions[positions.size - 1] = target for (i in positions.size - 2 downTo 0) { @@ -77,7 +76,6 @@ class IKChain( positions[i] = positions[i - 1] + (direction * nodes[i - 1].length) } - // Point the last bone at the target 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) @@ -106,6 +104,9 @@ class IKChain( return sum } + /** + * Resets the chain to its default state + */ fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY centroidWeight = 1f @@ -146,7 +147,7 @@ class IKChain( } /** - * Allow constrained bones to deviate more per step + * Allow constrained bones to deviate more */ fun decreaseConstraints() { loosens++ diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 6fff5ae390..b93b78d419 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -4,7 +4,7 @@ import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.trackers.Tracker /* - * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) and allows + * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) to allow * positional trackers such as vive/tundra trackers to be used in conjunction with * IMU trackers */ @@ -30,11 +30,9 @@ class IKSolver(private val root: Bone) { fun buildChains(trackers: List) { chainList.clear() - // Extract the positional constraints val positionalConstraints = extractPositionalConstraints(trackers) val rotationalConstraints = extractRotationalConstraints(trackers) - // Build the system of chains rootChain = chainBuilder(root, null, 0, positionalConstraints, rotationalConstraints) populateChainList(rootChain!!) addConstraints() @@ -45,8 +43,7 @@ class IKSolver(private val root: Bone) { } /** - * Reset the offsets of positional trackers. Should only be called right after a full reset - * is performed. + * Reset the offsets of positional trackers. */ fun resetOffsets() { needsReset = true @@ -91,9 +88,8 @@ class IKSolver(private val root: Bone) { var chain = IKChain(boneList, parent, level, baseConstraint, tailConstraint) if (currentBone.children.isNotEmpty()) { - val childrenList = mutableListOf() - // Build child chains + val childrenList = mutableListOf() for (child in currentBone.children) { val childChain = chainBuilder(child, chain, level + 1, positionalConstraints, rotationalConstraints) if (neededChain(childChain)) { @@ -229,7 +225,7 @@ class IKSolver(private val root: Bone) { } needsReset = false } - + rootChain?.resetChain() for (i in 0 until MAX_ITERATIONS) { From e2480e37300cc60e00bb3cf679fb0c85711d0b70 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 3 Apr 2024 17:25:01 -0700 Subject: [PATCH 37/68] change math --- .../main/java/dev/slimevr/tracking/processor/Constraint.kt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index cc387bcacf..d2d939f755 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -1,10 +1,10 @@ package dev.slimevr.tracking.processor -import com.jme3.math.FastMath.sign -import com.jme3.math.FastMath.sin -import com.jme3.math.FastMath.sqrt import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 +import kotlin.math.sign +import kotlin.math.sin +import kotlin.math.sqrt /** * Represents the rotational limits of a Bone relative to its parent From e8a41bba50cb3bce0d3830504871fa2167220093 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Tue, 23 Apr 2024 17:29:53 -0700 Subject: [PATCH 38/68] Add toggle for using position information --- gui/public/i18n/en/translation.ftl | 3 ++ .../settings/pages/GeneralSettings.tsx | 29 +++++++++++++++++++ .../rpc/settings/RPCSettingsBuilder.java | 2 +- .../rpc/settings/RPCSettingsHandler.kt | 1 + .../config/SkeletonConfigToggles.java | 3 +- .../processor/skeleton/HumanSkeleton.kt | 2 ++ .../tracking/processor/skeleton/IKSolver.kt | 3 +- 7 files changed, 40 insertions(+), 3 deletions(-) diff --git a/gui/public/i18n/en/translation.ftl b/gui/public/i18n/en/translation.ftl index 4125361270..6e8165061f 100644 --- a/gui/public/i18n/en/translation.ftl +++ b/gui/public/i18n/en/translation.ftl @@ -396,6 +396,9 @@ settings-general-fk_settings-leg_tweak-foot_plant-description = Foot-plant rotat settings-general-fk_settings-leg_fk = Leg tracking settings-general-fk_settings-leg_fk-reset_mounting_feet-description = Enable feet Mounting Reset by tiptoeing. settings-general-fk_settings-leg_fk-reset_mounting_feet = Feet Mounting Reset +settings-general-fk_settings-ik = Position data +settings-general-fk_settings-ik-use_position = Use Position data +settings-general-fk_settings-ik-use_position-description = Enables the use of position data from trackers that provide it. When enabling this make sure to full reset and recalibrate in game. settings-general-fk_settings-arm_fk = Arm tracking settings-general-fk_settings-arm_fk-description = Force arms to be tracked from the headset (HMD) even if positional hand data is available. settings-general-fk_settings-arm_fk-force_arms = Force arms from HMD diff --git a/gui/src/components/settings/pages/GeneralSettings.tsx b/gui/src/components/settings/pages/GeneralSettings.tsx index d384670e79..8a78258fd0 100644 --- a/gui/src/components/settings/pages/GeneralSettings.tsx +++ b/gui/src/components/settings/pages/GeneralSettings.tsx @@ -69,6 +69,7 @@ interface SettingsForm { toeSnap: boolean; footPlant: boolean; selfLocalization: boolean; + usePosition: boolean; }; ratios: { imputeWaistFromChestHip: number; @@ -128,6 +129,7 @@ const defaultValues: SettingsForm = { toeSnap: false, footPlant: true, selfLocalization: false, + usePosition: true, }, ratios: { imputeWaistFromChestHip: 0.3, @@ -235,6 +237,7 @@ export function GeneralSettings() { toggles.toeSnap = values.toggles.toeSnap; toggles.footPlant = values.toggles.footPlant; toggles.selfLocalization = values.toggles.selfLocalization; + toggles.usePosition = values.toggles.usePosition; modelSettings.toggles = toggles; } @@ -981,6 +984,32 @@ export function GeneralSettings() { )} /> + + +
+ + {l10n.getString('settings-general-fk_settings-ik')} + + + {l10n.getString( + 'settings-general-fk_settings-ik-use_position-description' + )} + +
+
+ +
+ + + {l10n.getString( 'settings-general-fk_settings-arm_fk-reset_mode-description' diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java index c7de6f5512..92116ed99d 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java @@ -191,7 +191,7 @@ public static int createModelSettings( humanPoseManager.getToggle(SkeletonConfigToggles.TOE_SNAP), humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT), humanPoseManager.getToggle(SkeletonConfigToggles.SELF_LOCALIZATION), - false + humanPoseManager.getToggle(SkeletonConfigToggles.USE_POSITION) ); int ratiosOffset = ModelRatios .createModelRatios( diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt index dae1e2b106..8b88e57077 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt @@ -256,6 +256,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) { hpm.setToggle(SkeletonConfigToggles.TOE_SNAP, toggles.toeSnap()) hpm.setToggle(SkeletonConfigToggles.FOOT_PLANT, toggles.footPlant()) hpm.setToggle(SkeletonConfigToggles.SELF_LOCALIZATION, toggles.selfLocalization()) + hpm.setToggle(SkeletonConfigToggles.USE_POSITION, toggles.usePosition()) } if (ratios != null) { diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java index cffec046b0..ac008acf59 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java @@ -15,7 +15,8 @@ public enum SkeletonConfigToggles { VIVE_EMULATION(7, "Vive emulation", "viveEmulation", false), TOE_SNAP(8, "Toe Snap", "toeSnap", false), FOOT_PLANT(9, "Foot Plant", "footPlant", true), - SELF_LOCALIZATION(10, "Self Localization", "selfLocalization", false),; + SELF_LOCALIZATION(10, "Self Localization", "selfLocalization", false), + USE_POSITION(11, "Use Position", "usePosition", true),; public static final SkeletonConfigToggles[] values = values(); private static final Map byStringVal = new HashMap<>(); diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index e68493d0a4..d34eaec2ad 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -877,6 +877,8 @@ class HumanSkeleton( SkeletonConfigToggles.FOOT_PLANT -> legTweaks.footPlantEnabled = newValue SkeletonConfigToggles.SELF_LOCALIZATION -> localizer.setEnabled(newValue) + + SkeletonConfigToggles.USE_POSITION -> ikSolver.enabled = newValue } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index b93b78d419..f9c98969e6 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -19,6 +19,7 @@ class IKSolver(private val root: Bone) { const val TOLERANCE_STEP = 2f } + var enabled = true private var chainList = mutableListOf() private var rootChain: IKChain? = null private var needsReset = false @@ -216,7 +217,7 @@ class IKSolver(private val root: Bone) { } fun solve() { - if (rootChain == null) return + if (rootChain == null || !enabled) return var solved: Boolean if (needsReset) { From 30f0e4b8f3f937bedbce1fbc98bd2327a3cabf55 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 6 May 2024 15:14:58 -0700 Subject: [PATCH 39/68] Fix lint --- gui/src/components/settings/pages/GeneralSettings.tsx | 3 --- 1 file changed, 3 deletions(-) diff --git a/gui/src/components/settings/pages/GeneralSettings.tsx b/gui/src/components/settings/pages/GeneralSettings.tsx index 8a78258fd0..2c664a60b1 100644 --- a/gui/src/components/settings/pages/GeneralSettings.tsx +++ b/gui/src/components/settings/pages/GeneralSettings.tsx @@ -985,7 +985,6 @@ export function GeneralSettings() { /> -
{l10n.getString('settings-general-fk_settings-ik')} @@ -1008,8 +1007,6 @@ export function GeneralSettings() { />
- - {l10n.getString( 'settings-general-fk_settings-arm_fk-reset_mode-description' From 26e6150a99cfa8001ddf96e82e92d38819e8b6f4 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 17 Jun 2024 20:42:25 -0700 Subject: [PATCH 40/68] Refactor and simplify constraints --- .../tracking/processor/CompleteConstraint.kt | 11 -- .../slimevr/tracking/processor/Constraint.kt | 104 +++++++++++------- .../processor/TwistSwingConstraint.kt | 26 ----- .../processor/skeleton/HumanSkeleton.kt | 73 ++++++------ 4 files changed, 103 insertions(+), 111 deletions(-) delete mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt delete mode 100644 server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt deleted file mode 100644 index 506769d4ff..0000000000 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/CompleteConstraint.kt +++ /dev/null @@ -1,11 +0,0 @@ -package dev.slimevr.tracking.processor - -import io.github.axisangles.ktmath.Quaternion - -/** - * A constraint type that allows no modifications to the rotation - */ -class CompleteConstraint : Constraint() { - override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion = - thisBone.getGlobalRotation() -} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index d2d939f755..c4d82b267c 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -6,10 +6,22 @@ import kotlin.math.sign import kotlin.math.sin import kotlin.math.sqrt +/** + * Represents a function that applies a rotational constraint to a direction vector. + */ +typealias ConstraintFunction = (Quaternion, Bone, Float, Float) -> Quaternion + /** * Represents the rotational limits of a Bone relative to its parent */ -abstract class Constraint { +class Constraint( + val constraintFunction: ConstraintFunction, + twist: Float = 0.0f, + swing: Float = 0.0f, +) { + private val twistRad = Math.toRadians(twist.toDouble()).toFloat() + private val swingRad = Math.toRadians(swing.toDouble()).toFloat() + /** * If false don't allow the rotation of the bone * to be modified except to satisfy a constraint @@ -23,21 +35,16 @@ abstract class Constraint { var tolerance = 0.0f set(value) { field = value - updateComposedField() + updateComposedFields() } private var toleranceRad = 0.0f - var originalRotation = Quaternion.NULL + var originalRotation = Quaternion.IDENTITY - private fun updateComposedField() { + private fun updateComposedFields() { toleranceRad = Math.toRadians(tolerance.toDouble()).toFloat() } - /** - * Apply rotational constraints to the direction vector - */ - protected abstract fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion - /** * Apply rotational constraints and if applicable force the rotation * to be unchanged unless it violates the constraints @@ -45,11 +52,11 @@ abstract class Constraint { fun applyConstraint(direction: Vector3, thisBone: Bone): Quaternion { val rotation = Quaternion.fromTo(Vector3.NEG_Y, direction) if (allowModifications) { - return constraintRotation(rotation, thisBone).unit() + return constraintFunction(rotation, thisBone, swingRad, twistRad).unit() } val constrainedRotation = applyLimits(rotation, originalRotation) - return constraintRotation(constrainedRotation, thisBone).unit() + return constraintFunction(constrainedRotation, thisBone, swingRad, twistRad).unit() } /** @@ -64,7 +71,7 @@ abstract class Constraint { /** * Limit the rotation to tolerance away from the initialRotation */ - protected fun applyLimits( + private fun applyLimits( rotation: Quaternion, initialRotation: Quaternion, ): Quaternion { @@ -78,35 +85,58 @@ abstract class Constraint { return initialRotation * (swingQ * twistQ) } - protected fun decompose( - rotation: Quaternion, - twistAxis: Vector3, - ): Pair { - val projection = rotation.project(twistAxis) + companion object { + fun decompose( + rotation: Quaternion, + twistAxis: Vector3, + ): Pair { + val projection = rotation.project(twistAxis) - val twist = Quaternion(rotation.w, projection.xyz).unit() - val swing = rotation * twist.inv() + val twist = Quaternion(rotation.w, projection.xyz).unit() + val swing = rotation * twist.inv() - return Pair(swing, twist) - } + return Pair(swing, twist) + } - protected 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 - var vector = rotation.xyz - var rot = rotation - - if (vector.lenSq() > magnitudeSqr) { - vector = vector.unit() * magnitude - rot = Quaternion( - sqrt(1.0f - magnitudeSqr) * sign, - vector.x, - vector.y, - vector.z, - ) + 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 + var vector = rotation.xyz + var rot = rotation + + if (vector.lenSq() > magnitudeSqr) { + vector = vector.unit() * magnitude + rot = Quaternion( + sqrt(1.0f - magnitudeSqr) * sign, + vector.x, + vector.y, + vector.z, + ) + } + + return rot } - 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 CompleteConstraint + val completeConstraint: ConstraintFunction = { _: Quaternion, thisBone: Bone, _: Float, _: Float -> + thisBone.getGlobalRotation() + } } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt deleted file mode 100644 index 9588b3dc29..0000000000 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/TwistSwingConstraint.kt +++ /dev/null @@ -1,26 +0,0 @@ -package dev.slimevr.tracking.processor - -import io.github.axisangles.ktmath.Quaternion -import io.github.axisangles.ktmath.Vector3 - -/** - * A constraint type that allows for a rotation to be constrained relative to the parent - * of the supplied bone - */ -class TwistSwingConstraint(twist: Float, swing: Float) : Constraint() { - private val twistRad = Math.toRadians(twist.toDouble()).toFloat() - private val swingRad = Math.toRadians(swing.toDouble()).toFloat() - - override fun constraintRotation(rotation: Quaternion, thisBone: Bone): Quaternion { - if (thisBone.parent == null) return rotation - 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) - - return parent.getGlobalRotation() * thisBone.rotationOffset * (swingQ * twistQ) - } -} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index d34eaec2ad..81ffb31c93 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -4,9 +4,10 @@ import com.jme3.math.FastMath import dev.slimevr.VRServer import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.processor.BoneType -import dev.slimevr.tracking.processor.CompleteConstraint +import dev.slimevr.tracking.processor.Constraint +import dev.slimevr.tracking.processor.Constraint.Companion.completeConstraint +import dev.slimevr.tracking.processor.Constraint.Companion.twistSwingConstraint import dev.slimevr.tracking.processor.HumanPoseManager -import dev.slimevr.tracking.processor.TwistSwingConstraint import dev.slimevr.tracking.processor.config.SkeletonConfigToggles import dev.slimevr.tracking.processor.config.SkeletonConfigValues import dev.slimevr.tracking.trackers.Tracker @@ -36,47 +37,45 @@ class HumanSkeleton( val humanPoseManager: HumanPoseManager, ) { // Upper body bones - val headBone = Bone(BoneType.HEAD, CompleteConstraint()) - val neckBone = Bone(BoneType.NECK, CompleteConstraint()) - val upperChestBone = Bone(BoneType.UPPER_CHEST, TwistSwingConstraint(10f, 80f)) - val chestBone = Bone(BoneType.CHEST, TwistSwingConstraint(10f, 80f)) - val waistBone = Bone(BoneType.WAIST, TwistSwingConstraint(10f, 80f)) - val hipBone = Bone(BoneType.HIP, TwistSwingConstraint(10f, 80f)) + val headBone = Bone(BoneType.HEAD, Constraint(completeConstraint)) + val neckBone = Bone(BoneType.NECK, Constraint(completeConstraint)) + val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(twistSwingConstraint, 10f, 80f)) + val chestBone = Bone(BoneType.CHEST, Constraint(twistSwingConstraint, 10f, 80f)) + val waistBone = Bone(BoneType.WAIST, Constraint(twistSwingConstraint, 10f, 80f)) + val hipBone = Bone(BoneType.HIP, Constraint(twistSwingConstraint, 10f, 80f)) // Lower body bones - val leftHipBone = Bone(BoneType.LEFT_HIP, TwistSwingConstraint(10f, 7f)) - val rightHipBone = Bone(BoneType.RIGHT_HIP, TwistSwingConstraint(10f, 7f)) - val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, TwistSwingConstraint(180f, 120f)) - val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, TwistSwingConstraint(180f, 120f)) - val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, TwistSwingConstraint(110f, 180f)) - val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, TwistSwingConstraint(110f, 180f)) - val leftFootBone = Bone(BoneType.LEFT_FOOT, TwistSwingConstraint(180f, 120f)) - val rightFootBone = Bone(BoneType.RIGHT_FOOT, TwistSwingConstraint(180f, 120f)) + val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(twistSwingConstraint, 10f, 7f)) + val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(twistSwingConstraint, 10f, 7f)) + 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 leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(twistSwingConstraint, 180f, 120f)) + val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(twistSwingConstraint, 180f, 120f)) // Arm bones - val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, TwistSwingConstraint(180f, 7f)) - val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, TwistSwingConstraint(180f, 7f)) - val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, TwistSwingConstraint(180f, 180f)) - val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, TwistSwingConstraint(180f, 180f)) - val leftLowerArmBone = - Bone(BoneType.LEFT_LOWER_ARM, TwistSwingConstraint(180f, 180f)) - val rightLowerArmBone = - Bone(BoneType.RIGHT_LOWER_ARM, TwistSwingConstraint(180f, 180f)) - val leftHandBone = Bone(BoneType.LEFT_HAND, TwistSwingConstraint(180f, 180f)) - val rightHandBone = Bone(BoneType.RIGHT_HAND, TwistSwingConstraint(180f, 180f)) + val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(twistSwingConstraint, 180f, 7f)) + val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(twistSwingConstraint, 180f, 7f)) + 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 leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(twistSwingConstraint, 180f, 180f)) + val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(twistSwingConstraint, 180f, 180f)) // Tracker bones - val headTrackerBone = Bone(BoneType.HEAD_TRACKER, CompleteConstraint()) - val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, CompleteConstraint()) - val hipTrackerBone = Bone(BoneType.HIP_TRACKER, CompleteConstraint()) - val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, CompleteConstraint()) - val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, CompleteConstraint()) - val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, CompleteConstraint()) - val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, CompleteConstraint()) - val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, CompleteConstraint()) - val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, CompleteConstraint()) - val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, CompleteConstraint()) - val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, CompleteConstraint()) + val headTrackerBone = Bone(BoneType.HEAD_TRACKER, Constraint(completeConstraint)) + val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, Constraint(completeConstraint)) + val hipTrackerBone = Bone(BoneType.HIP_TRACKER, Constraint(completeConstraint)) + val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, Constraint(completeConstraint)) + val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, Constraint(completeConstraint)) + val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, Constraint(completeConstraint)) + val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, Constraint(completeConstraint)) + val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, Constraint(completeConstraint)) + val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, Constraint(completeConstraint)) + val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, Constraint(completeConstraint)) + val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, Constraint(completeConstraint)) // Buffers var hasSpineTracker = false From d28a209fada22449e4201ec802272f609c3bc2ce Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Tue, 18 Jun 2024 16:22:26 -0700 Subject: [PATCH 41/68] simplify loosening logic --- .../tracking/processor/skeleton/IKChain.kt | 4 ++-- .../tracking/processor/skeleton/IKSolver.kt | 17 +++++------------ 2 files changed, 7 insertions(+), 14 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 42222b5cc7..cc2faafef3 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -110,12 +110,12 @@ class IKChain( fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY centroidWeight = 1f - loosens = 0 for (bone in nodes) { - bone.rotationConstraint.tolerance = 0.0f + if (loosens > 0) bone.rotationConstraint.tolerance -= IKSolver.TOLERANCE_STEP bone.rotationConstraint.originalRotation = bone.getGlobalRotation() } + loosens -= if (loosens > 0) 1 else 0 for (child in children) { child.resetChain() diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index f9c98969e6..de0294204a 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -12,10 +12,10 @@ import dev.slimevr.tracking.trackers.Tracker class IKSolver(private val root: Bone) { companion object { const val TOLERANCE_SQR = 1e-8 // == 0.01 cm - const val MAX_ITERATIONS = 100 + const val MAX_ITERATIONS = 200 const val ITERATIONS_BEFORE_STEP = 20 - const val ITERATIONS_BETWEEN_STEP = 5 - const val MAX_LOOSENS = 5 + const val ITERATIONS_BETWEEN_STEP = 20 + const val MAX_LOOSENS = 10 const val TOLERANCE_STEP = 2f } @@ -202,18 +202,11 @@ class IKSolver(private val root: Bone) { * Loosen rotational constraints gradually */ private fun loosenConstraints(iter: Int) { - if (iter < ITERATIONS_BEFORE_STEP && iter % ITERATIONS_BETWEEN_STEP != 0) return + if (iter < ITERATIONS_BEFORE_STEP || iter % ITERATIONS_BETWEEN_STEP != 0) return - var maxDist = Float.NEGATIVE_INFINITY - var maxDistChain = chainList.first() for (chain in chainList) { - if (chain.distToTargetSqr > maxDist) { - maxDist = chain.distToTargetSqr - maxDistChain = chain - } + if (chain.loosens < MAX_LOOSENS) chain.decreaseConstraints() } - - if (maxDistChain.loosens < MAX_LOOSENS) maxDistChain.decreaseConstraints() } fun solve() { From d78e9ca2392f471c7ba800b2718e3071ffc7cb8f Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 24 Jun 2024 16:16:53 -0700 Subject: [PATCH 42/68] Fix trackers being assigned a bone when they are unassigned --- .../java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index de0294204a..b87c6c66a2 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -190,7 +190,7 @@ class IKSolver(private val root: Bone) { private fun getConstraint(bone: Bone, constraints: MutableList): Tracker? { for (c in constraints) { - if (bone.boneType.bodyPart == (c.trackerPosition?.bodyPart ?: 0)) { + if (c.trackerPosition != null && bone.boneType.bodyPart == (c.trackerPosition?.bodyPart ?: 0)) { constraints.remove(c) return c } From bccaf78f572091507b6a8a6bcdd477386381a230 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sun, 7 Jul 2024 14:00:40 -0700 Subject: [PATCH 43/68] save state before switching it CCDIK --- .../processor/skeleton/HumanSkeleton.kt | 8 +++---- .../tracking/processor/skeleton/IKChain.kt | 9 +++---- .../tracking/processor/skeleton/IKSolver.kt | 24 ++++++++++++------- 3 files changed, 22 insertions(+), 19 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 81ffb31c93..174c7ab26a 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -45,8 +45,8 @@ class HumanSkeleton( val hipBone = Bone(BoneType.HIP, Constraint(twistSwingConstraint, 10f, 80f)) // Lower body bones - val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(twistSwingConstraint, 10f, 7f)) - val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(twistSwingConstraint, 10f, 7f)) + val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(completeConstraint)) + val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(completeConstraint)) 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)) @@ -55,8 +55,8 @@ class HumanSkeleton( val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(twistSwingConstraint, 180f, 120f)) // Arm bones - val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(twistSwingConstraint, 180f, 7f)) - val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(twistSwingConstraint, 180f, 7f)) + val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(twistSwingConstraint, 180f, 8f)) + 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)) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index cc2faafef3..72c39c7150 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -16,7 +16,7 @@ class IKChain( val tailConstraint: Tracker?, ) { companion object { - const val CENTROID_PULL_ADJUSTMENT = 0.1f + const val CENTROID_PULL_ADJUSTMENT = 0.01f } // State variables @@ -42,11 +42,8 @@ class IKChain( fun backwards() { // Start at the constraint or the centroid of the children - target = if (computedTailPosition == null && children.size > 1) { - targetSum / getChildrenCentroidWeightSum() - } else { - (computedTailPosition?.getPosition()) ?: Vector3.NULL - } + //target = computedTailPosition?.getPosition() ?: nodes.last().getTailPosition() + target = computedTailPosition?.getPosition() ?: (targetSum / getChildrenCentroidWeightSum()) positions[positions.size - 1] = target diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index b87c6c66a2..ee18d11acd 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -41,6 +41,12 @@ class IKSolver(private val root: Bone) { // Check if there is any constraints (other than the head) in the model rootChain = if (neededChain(rootChain!!)) rootChain else null chainList.sortBy { -it.level } + + println("ChainList length ${chainList.size}") + for (chain in chainList) { + println("Start = ${chain.nodes.first().boneType.name}") + println("End = ${chain.nodes.last().boneType.name}\n") + } } /** @@ -138,8 +144,9 @@ class IKSolver(private val root: Bone) { } private fun addConstraints() { - fun constrainChain(chain: IKChain) = + fun constrainChain(chain: IKChain) { chain.nodes.forEach { it.rotationConstraint.allowModifications = false } + } chainList.forEach { if (it.tailConstraint == null) constrainChain(it) } } @@ -201,9 +208,7 @@ class IKSolver(private val root: Bone) { /** * Loosen rotational constraints gradually */ - private fun loosenConstraints(iter: Int) { - if (iter < ITERATIONS_BEFORE_STEP || iter % ITERATIONS_BETWEEN_STEP != 0) return - + private fun loosenConstraints() { for (chain in chainList) { if (chain.loosens < MAX_LOOSENS) chain.decreaseConstraints() } @@ -240,12 +245,13 @@ class IKSolver(private val root: Bone) { if (solved) break - // Help the chains out of a deadlock - for (chain in chainList) { - chain.updateChildCentroidWeight() + if (i > ITERATIONS_BEFORE_STEP && i % ITERATIONS_BETWEEN_STEP == 0) { + // Help the chains out of a deadlock + for (chain in chainList) { + chain.updateChildCentroidWeight() + } + loosenConstraints() } - - loosenConstraints(i) } root.update() From 0c003856fa4fdfe2f9a13148295ca86b4070932a Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 10 Jul 2024 20:23:48 -0700 Subject: [PATCH 44/68] switch from FABRIK to CCDIK --- .../slimevr/tracking/processor/Constraint.kt | 86 +++++++++---- .../processor/skeleton/HumanSkeleton.kt | 13 +- .../tracking/processor/skeleton/IKChain.kt | 116 +++++++----------- .../tracking/processor/skeleton/IKSolver.kt | 22 ++-- 4 files changed, 117 insertions(+), 120 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index c4d82b267c..dd89fcacd0 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -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 @@ -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() } @@ -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 */ @@ -86,7 +77,7 @@ class Constraint( } companion object { - fun decompose( + private fun decompose( rotation: Quaternion, twistAxis: Vector3, ): Pair { @@ -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 @@ -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() diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 174c7ab26a..07ef34d513 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -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 @@ -45,12 +46,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)) @@ -59,8 +60,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)) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 72c39c7150..33d5b335c0 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -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, + val bones: MutableList, var parent: IKChain?, val level: Int, val baseConstraint: Tracker?, @@ -23,82 +28,46 @@ class IKChain( private val computedBasePosition = baseConstraint?.let { IKConstraint(it) } private val computedTailPosition = tailConstraint?.let { IKConstraint(it) } var children = mutableListOf() - 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 { - val posList = mutableListOf() - 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 } /** @@ -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() } @@ -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()) } /** @@ -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 } @@ -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 } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index ee18d11acd..9f57596169 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -4,7 +4,7 @@ import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.trackers.Tracker /* - * Implements FABRIK (Forwards And Backwards Reaching Inverse Kinematics) to allow + * Implements CCDIK (Cyclic Coordinate Descent Inverse Kinematics) to allow * positional trackers such as vive/tundra trackers to be used in conjunction with * IMU trackers */ @@ -14,9 +14,9 @@ class IKSolver(private val root: Bone) { const val TOLERANCE_SQR = 1e-8 // == 0.01 cm const val MAX_ITERATIONS = 200 const val ITERATIONS_BEFORE_STEP = 20 - const val ITERATIONS_BETWEEN_STEP = 20 - const val MAX_LOOSENS = 10 - const val TOLERANCE_STEP = 2f + const val ITERATIONS_BETWEEN_STEP = 10 + const val MAX_LOOSENS = 20 + const val TOLERANCE_STEP = 1f } var enabled = true @@ -41,12 +41,6 @@ class IKSolver(private val root: Bone) { // Check if there is any constraints (other than the head) in the model rootChain = if (neededChain(rootChain!!)) rootChain else null chainList.sortBy { -it.level } - - println("ChainList length ${chainList.size}") - for (chain in chainList) { - println("Start = ${chain.nodes.first().boneType.name}") - println("End = ${chain.nodes.last().boneType.name}\n") - } } /** @@ -123,8 +117,8 @@ class IKSolver(private val root: Bone) { private fun combineChains(chain: IKChain, childChain: IKChain): IKChain { val boneList = mutableListOf() - boneList.addAll(chain.nodes) - boneList.addAll(childChain.nodes) + boneList.addAll(chain.bones) + boneList.addAll(childChain.bones) val newChain = IKChain( boneList, @@ -145,7 +139,8 @@ class IKSolver(private val root: Bone) { private fun addConstraints() { fun constrainChain(chain: IKChain) { - chain.nodes.forEach { it.rotationConstraint.allowModifications = false } + chain.locked = true + chain.bones.forEach { it.rotationConstraint.allowModifications = false } } chainList.forEach { if (it.tailConstraint == null) constrainChain(it) } } @@ -231,7 +226,6 @@ class IKSolver(private val root: Bone) { for (chain in chainList) { chain.backwards() } - rootChain?.forwardsMulti() rootChain?.computeTargetDistance() From 0aaab71b3b5c5006856556ab7c968774ff4e5601 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 12 Jul 2024 14:54:18 -0700 Subject: [PATCH 45/68] reduce jitter further --- .../java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 9f57596169..30b6ee7172 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -16,7 +16,7 @@ class IKSolver(private val root: Bone) { const val ITERATIONS_BEFORE_STEP = 20 const val ITERATIONS_BETWEEN_STEP = 10 const val MAX_LOOSENS = 20 - const val TOLERANCE_STEP = 1f + const val TOLERANCE_STEP = 0.2f } var enabled = true From 0a9d3ffb54326c9238c04fdc615f5ace3f671cfd Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sat, 13 Jul 2024 13:04:17 -0700 Subject: [PATCH 46/68] Tuning and better chain control --- .../tracking/processor/skeleton/IKChain.kt | 43 +++++++------------ .../tracking/processor/skeleton/IKSolver.kt | 11 ++--- 2 files changed, 20 insertions(+), 34 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 33d5b335c0..3bbd32dc22 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -20,10 +20,6 @@ class IKChain( val baseConstraint: Tracker?, val tailConstraint: Tracker?, ) { - companion object { - const val CENTROID_PULL_ADJUSTMENT = 0.01f - } - // State variables private val computedBasePosition = baseConstraint?.let { IKConstraint(it) } private val computedTailPosition = tailConstraint?.let { IKConstraint(it) } @@ -42,7 +38,7 @@ class IKChain( val currentBone = bones[i] // 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 endEffectorLocal = (getEndEffectorsAvg() - currentBone.getPosition()).unit() val targetLocal = (target - currentBone.getPosition()).unit() // Compute the axis of rotation and angle for this bone @@ -59,6 +55,17 @@ class IKChain( } } + private fun getEndEffectorsAvg(): Vector3 { + if (children.size == 0) return bones.last().getTailPosition() + + var sum = Vector3.NULL + for (c in children) { + sum += c.getEndEffectorsAvg() + } + + return sum / children.size.toFloat() + } + private fun getWeightedChildTarget(): Vector3 { var weightSum = 0.0f var sum = Vector3.NULL @@ -76,9 +83,10 @@ class IKChain( fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY centroidWeight = 1f + val lockedReduction = if (locked)IKSolver.LOCKED_REDUCTION else 1f for (bone in bones) { - if (loosens > 0) bone.rotationConstraint.tolerance -= IKSolver.TOLERANCE_STEP + if (loosens > 0) bone.rotationConstraint.tolerance -= IKSolver.TOLERANCE_STEP * lockedReduction bone.rotationConstraint.originalRotation = bone.getGlobalRotation() } loosens -= if (loosens > 0) 1 else 0 @@ -93,33 +101,14 @@ class IKChain( computedBasePosition?.reset(bones.first().getPosition()) } - /** - * Prevent deadlocks where the centroid becomes stuck - * due to two or more chains pulling equally on the centroid - */ - fun updateChildCentroidWeight() { - if (children.size <= 1) return - - var closestToSolved = children.first() - for (child in children) { - if (child.distToTargetSqr < closestToSolved.distToTargetSqr) { - closestToSolved = child - } - } - - if (closestToSolved.centroidWeight > CENTROID_PULL_ADJUSTMENT) { - closestToSolved.centroidWeight -= CENTROID_PULL_ADJUSTMENT - } - } - /** * Allow constrained bones to deviate more */ fun decreaseConstraints() { - if (locked) return + val lockedReduction = if (locked)IKSolver.LOCKED_REDUCTION else 1f loosens++ for (bone in bones) { - bone.rotationConstraint.tolerance += IKSolver.TOLERANCE_STEP + bone.rotationConstraint.tolerance += IKSolver.TOLERANCE_STEP * lockedReduction } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 30b6ee7172..86add84b5f 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -12,11 +12,12 @@ import dev.slimevr.tracking.trackers.Tracker class IKSolver(private val root: Bone) { companion object { const val TOLERANCE_SQR = 1e-8 // == 0.01 cm - const val MAX_ITERATIONS = 200 + const val MAX_ITERATIONS = 100 const val ITERATIONS_BEFORE_STEP = 20 const val ITERATIONS_BETWEEN_STEP = 10 - const val MAX_LOOSENS = 20 - const val TOLERANCE_STEP = 0.2f + const val MAX_LOOSENS = 40 + const val TOLERANCE_STEP = 0.5f + const val LOCKED_REDUCTION = 0.1f } var enabled = true @@ -240,10 +241,6 @@ class IKSolver(private val root: Bone) { if (solved) break if (i > ITERATIONS_BEFORE_STEP && i % ITERATIONS_BETWEEN_STEP == 0) { - // Help the chains out of a deadlock - for (chain in chainList) { - chain.updateChildCentroidWeight() - } loosenConstraints() } } From 41e242d84b353237ddb2dfa73c31be83c0a27827 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sun, 14 Jul 2024 12:02:40 -0700 Subject: [PATCH 47/68] fix bugs --- .../src/main/java/dev/slimevr/autobone/AutoBoneStep.kt | 4 +++- .../dev/slimevr/tracking/processor/HumanPoseManager.kt | 5 +++++ .../slimevr/tracking/processor/skeleton/HumanSkeleton.kt | 8 ++++++++ .../dev/slimevr/tracking/processor/skeleton/IKChain.kt | 2 +- 4 files changed, 17 insertions(+), 2 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt b/server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt index a0b802f679..6b07d32205 100644 --- a/server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt +++ b/server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt @@ -40,9 +40,11 @@ class AutoBoneStep( // Load server configs into the skeleton skeleton1.loadFromConfig(serverConfig) skeleton2.loadFromConfig(serverConfig) - // Disable leg tweaks, this will mess with the resulting positions + // Disable leg tweaks and IK solver, these will mess with the resulting positions skeleton1.setLegTweaksEnabled(false) skeleton2.setLegTweaksEnabled(false) + skeleton1.setIKSolverEnabled(false) + skeleton2.setIKSolverEnabled(false) } fun setCursors(cursor1: Int, cursor2: Int, updatePlayerCursors: Boolean) { diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt index 85a6dc2c58..f815bd0808 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt @@ -639,6 +639,11 @@ class HumanPoseManager(val server: VRServer?) { skeleton.setLegTweaksEnabled(value) } + @VRServerThread + fun setIKSolverEnabled(value: Boolean) { + skeleton.setIKSolverEnabled(value) + } + @VRServerThread fun setFloorClipEnabled(value: Boolean) { skeleton.setFloorclipEnabled(value) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 07ef34d513..3db87cdf43 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -1224,6 +1224,14 @@ class HumanSkeleton( legTweaks.enabled = value } + /** + * enable/disable IK solver (for Autobone) + */ + @VRServerThread + fun setIKSolverEnabled(value: Boolean) { + ikSolver.enabled = value + } + @VRServerThread fun setFloorclipEnabled(value: Boolean) { humanPoseManager.setToggle(SkeletonConfigToggles.FLOOR_CLIP, value) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 3bbd32dc22..6e7ab6eaa8 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -56,7 +56,7 @@ class IKChain( } private fun getEndEffectorsAvg(): Vector3 { - if (children.size == 0) return bones.last().getTailPosition() + if (children.size <= 1 || computedTailPosition != null) return bones.last().getTailPosition() var sum = Vector3.NULL for (c in children) { From 9bf1b98b180569666d18a3552b1acc64ae239143 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Thu, 25 Jul 2024 14:52:56 -0700 Subject: [PATCH 48/68] Fix hinge constraint bugs and add annealing --- .../slimevr/tracking/processor/Constraint.kt | 74 ++++++----------- .../processor/skeleton/HumanSkeleton.kt | 70 ++++++++-------- .../tracking/processor/skeleton/IKChain.kt | 80 ++++++++++--------- .../tracking/processor/skeleton/IKSolver.kt | 60 +++++++------- 4 files changed, 129 insertions(+), 155 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index dd89fcacd0..4d981ef81e 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -2,10 +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 +import kotlin.math.* /** * Represents a function that applies a rotational constraint to a direction vector. @@ -16,10 +13,11 @@ typealias ConstraintFunction = (Quaternion, Bone, Float, Float) -> Quaternion * Represents the rotational limits of a Bone relative to its parent */ class Constraint( - val constraintFunction: ConstraintFunction, + val constraintType: ConstraintType, twist: Float = 0.0f, swing: Float = 0.0f, ) { + private val constraintFunction = constraintTypeToFunc(constraintType) private val twistRad = Math.toRadians(twist.toDouble()).toFloat() private val swingRad = Math.toRadians(swing.toDouble()).toFloat() @@ -29,54 +27,26 @@ class Constraint( */ var allowModifications = true - /** - * If allowModifications is false this is the - * allowed deviation from the original rotation - */ - var tolerance = 0.0f - set(value) { - field = value - updateComposedFields() - } - - private var toleranceRad = 0.0f - var originalRotation = Quaternion.IDENTITY - - private fun updateComposedFields() { - toleranceRad = Math.toRadians(tolerance.toDouble()).toFloat() - } - /** * Apply rotational constraints and if applicable force the rotation * to be unchanged unless it violates the constraints */ - fun applyConstraint(rotation: Quaternion, thisBone: Bone): Quaternion { - if (allowModifications) { - return constraintFunction(rotation, thisBone, swingRad, twistRad).unit() - } - - val constrainedRotation = applyLimits(rotation, originalRotation) - return constraintFunction(constrainedRotation, thisBone, swingRad, twistRad).unit() - } + fun applyConstraint(rotation: Quaternion, thisBone: Bone): Quaternion = constraintFunction(rotation, thisBone, swingRad, twistRad).unit() - /** - * Limit the rotation to tolerance away from the initialRotation - */ - private fun applyLimits( - rotation: Quaternion, - initialRotation: Quaternion, - ): Quaternion { - val localRotation = initialRotation.inv() * rotation - - var (swingQ, twistQ) = decompose(localRotation, Vector3.NEG_Y) - - twistQ = constrain(twistQ, toleranceRad) - swingQ = constrain(swingQ, toleranceRad) + companion object { + enum class ConstraintType { + TWIST_SWING, + HINGE, + COMPLETE, + } - return initialRotation * (swingQ * twistQ) - } + private fun constraintTypeToFunc(type: ConstraintType) = + when (type) { + ConstraintType.COMPLETE -> completeConstraint + ConstraintType.TWIST_SWING -> twistSwingConstraint + ConstraintType.HINGE -> hingeConstraint + } - companion object { private fun decompose( rotation: Quaternion, twistAxis: Vector3, @@ -119,9 +89,13 @@ class Constraint( 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 + val distToMin = min(abs(rotMagnitude - magnitudeSqrMin), abs(rotMagnitude + magnitudeSqrMin)) + val distToMax = min(abs(rotMagnitude - magnitudeSqrMax), abs(rotMagnitude + magnitudeSqrMax)) + + val magnitude = if (distToMin < distToMax) magnitudeMin else magnitudeMax + val magnitudeSqr = abs(if (distToMin < distToMax) magnitudeSqrMin else magnitudeSqrMax) + vector = vector.unit() * -magnitude + rot = Quaternion( sqrt(1.0f - magnitudeSqr), vector.x, @@ -130,7 +104,7 @@ class Constraint( ) } - return rot + return rot.unit() } // Constraint function for TwistSwingConstraint diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 3db87cdf43..21a4a61e92 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -5,9 +5,7 @@ import dev.slimevr.VRServer 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.Constraint.Companion.ConstraintType import dev.slimevr.tracking.processor.HumanPoseManager import dev.slimevr.tracking.processor.config.SkeletonConfigToggles import dev.slimevr.tracking.processor.config.SkeletonConfigValues @@ -38,45 +36,45 @@ class HumanSkeleton( val humanPoseManager: HumanPoseManager, ) { // Upper body bones - val headBone = Bone(BoneType.HEAD, Constraint(completeConstraint)) - val neckBone = Bone(BoneType.NECK, Constraint(completeConstraint)) - val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(twistSwingConstraint, 10f, 80f)) - val chestBone = Bone(BoneType.CHEST, Constraint(twistSwingConstraint, 10f, 80f)) - val waistBone = Bone(BoneType.WAIST, Constraint(twistSwingConstraint, 10f, 80f)) - val hipBone = Bone(BoneType.HIP, Constraint(twistSwingConstraint, 10f, 80f)) + 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)) // Lower body bones - 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(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)) + 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 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)) // Arm bones - val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(twistSwingConstraint, 180f, 8f)) - 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(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)) + val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 180f, 20f)) + val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 180f, 20f)) + 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)) // Tracker bones - val headTrackerBone = Bone(BoneType.HEAD_TRACKER, Constraint(completeConstraint)) - val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, Constraint(completeConstraint)) - val hipTrackerBone = Bone(BoneType.HIP_TRACKER, Constraint(completeConstraint)) - val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, Constraint(completeConstraint)) - val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, Constraint(completeConstraint)) - val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, Constraint(completeConstraint)) - val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, Constraint(completeConstraint)) - val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, Constraint(completeConstraint)) - val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, Constraint(completeConstraint)) - val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, Constraint(completeConstraint)) - val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, Constraint(completeConstraint)) + val headTrackerBone = Bone(BoneType.HEAD_TRACKER, Constraint(ConstraintType.COMPLETE)) + val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, Constraint(ConstraintType.COMPLETE)) + val hipTrackerBone = Bone(BoneType.HIP_TRACKER, Constraint(ConstraintType.COMPLETE)) + val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, Constraint(ConstraintType.COMPLETE)) + val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, Constraint(ConstraintType.COMPLETE)) + val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, Constraint(ConstraintType.COMPLETE)) + val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, Constraint(ConstraintType.COMPLETE)) + val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, Constraint(ConstraintType.COMPLETE)) + val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, Constraint(ConstraintType.COMPLETE)) + val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, Constraint(ConstraintType.COMPLETE)) + val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, Constraint(ConstraintType.COMPLETE)) // Buffers var hasSpineTracker = false diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 6e7ab6eaa8..29fe4cc884 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -1,12 +1,12 @@ package dev.slimevr.tracking.processor.skeleton import dev.slimevr.tracking.processor.Bone +import dev.slimevr.tracking.processor.Constraint 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 /* @@ -26,13 +26,30 @@ class IKChain( var children = mutableListOf() var target = Vector3.NULL var distToTargetSqr = Float.POSITIVE_INFINITY - var loosens = 0 - var locked = false - private var centroidWeight = 1f + private var rotations = getRotationsList() - fun backwards() { - // Start at the constraint or the centroid of the children - target = computedTailPosition?.getPosition() ?: getWeightedChildTarget() + private fun getRotationsList(): MutableList { + val rotList = mutableListOf() + for (b in bones) { + rotList.add(b.getGlobalRotation()) + } + + return rotList + } + + /* + * Populate the non-static rotations with the solve angle from the last iteration + */ + private fun prepBones() { + for (i in 0.. 0) bone.rotationConstraint.tolerance -= IKSolver.TOLERANCE_STEP * lockedReduction - bone.rotationConstraint.originalRotation = bone.getGlobalRotation() - } - loosens -= if (loosens > 0) 1 else 0 for (child in children) { child.resetChain() @@ -101,17 +112,6 @@ class IKChain( computedBasePosition?.reset(bones.first().getPosition()) } - /** - * Allow constrained bones to deviate more - */ - fun decreaseConstraints() { - val lockedReduction = if (locked)IKSolver.LOCKED_REDUCTION else 1f - loosens++ - for (bone in bones) { - bone.rotationConstraint.tolerance += IKSolver.TOLERANCE_STEP * lockedReduction - } - } - /** * Updates the distance to target and other fields * Call on the root chain @@ -133,8 +133,12 @@ class IKChain( * vector with the bone's rotational constraint * returns the constrained rotation */ - private fun setBoneRotation(bone: Bone, rotation: Quaternion): Quaternion { - val newRotation = bone.rotationConstraint.applyConstraint(rotation, bone) + private fun setBoneRotation(bone: Bone, rotation: Quaternion, useConstraints: Boolean): Quaternion { + val newRotation = if (useConstraints || bone.rotationConstraint.constraintType == Constraint.Companion.ConstraintType.COMPLETE) { + bone.rotationConstraint.applyConstraint(rotation, bone) + } else { + rotation + } bone.setRotationRaw(newRotation) bone.update() diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 86add84b5f..b3da1ab372 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -13,11 +13,11 @@ class IKSolver(private val root: Bone) { companion object { const val TOLERANCE_SQR = 1e-8 // == 0.01 cm const val MAX_ITERATIONS = 100 - const val ITERATIONS_BEFORE_STEP = 20 - const val ITERATIONS_BETWEEN_STEP = 10 - const val MAX_LOOSENS = 40 - const val TOLERANCE_STEP = 0.5f - const val LOCKED_REDUCTION = 0.1f + const val ANNEALING_STEP = 20 + const val ANNEALING_ITERATIONS = 5 + const val ANNEALING_MAX = 60 + const val DAMPENING_FACTOR = 0.5f + const val STATIC_DAMPENING = 0.25f } var enabled = true @@ -140,7 +140,6 @@ class IKSolver(private val root: Bone) { private fun addConstraints() { fun constrainChain(chain: IKChain) { - chain.locked = true chain.bones.forEach { it.rotationConstraint.allowModifications = false } } chainList.forEach { if (it.tailConstraint == null) constrainChain(it) } @@ -201,19 +200,32 @@ class IKSolver(private val root: Bone) { return null } - /** - * Loosen rotational constraints gradually - */ - private fun loosenConstraints() { - for (chain in chainList) { - if (chain.loosens < MAX_LOOSENS) chain.decreaseConstraints() + private fun solve(iterations: Int, useConstraints: Boolean = true): Boolean { + var solved: Boolean + for (i in 0..iterations) { + for (chain in chainList) { + chain.backwardsCCDIK(useConstraints) + } + + rootChain?.computeTargetDistance() + + // If all chains have reached their target the chain is solved + solved = true + for (chain in chainList) { + if (chain.distToTargetSqr > TOLERANCE_SQR) { + solved = false + } + } + + if (solved && useConstraints) return true } + + return false } fun solve() { if (rootChain == null || !enabled) return - var solved: Boolean if (needsReset) { for (c in chainList) { c.resetTrackerOffsets() @@ -222,27 +234,13 @@ class IKSolver(private val root: Bone) { } rootChain?.resetChain() + root.update() - for (i in 0 until MAX_ITERATIONS) { - for (chain in chainList) { - chain.backwards() - } - - rootChain?.computeTargetDistance() - - // If all chains have reached their target the chain is solved - solved = true - for (chain in chainList) { - if (chain.distToTargetSqr > TOLERANCE_SQR) { - solved = false - } - } + for (i in 0 until MAX_ITERATIONS step ANNEALING_STEP) { + solve(ANNEALING_ITERATIONS, (i > ANNEALING_MAX)) + val solved = solve(ANNEALING_STEP - ANNEALING_ITERATIONS) if (solved) break - - if (i > ITERATIONS_BEFORE_STEP && i % ITERATIONS_BETWEEN_STEP == 0) { - loosenConstraints() - } } root.update() From 7e1d1c69602e3fa9fbd360aceb38d8f2ce9341fe Mon Sep 17 00:00:00 2001 From: Collin Date: Thu, 25 Jul 2024 18:04:00 -0700 Subject: [PATCH 49/68] Update server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt Co-authored-by: Uriel --- .../main/java/dev/slimevr/tracking/processor/Constraint.kt | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 4d981ef81e..037f0b3d77 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -96,12 +96,7 @@ class Constraint( val magnitudeSqr = abs(if (distToMin < distToMax) magnitudeSqrMin else magnitudeSqrMax) vector = vector.unit() * -magnitude - rot = Quaternion( - sqrt(1.0f - magnitudeSqr), - vector.x, - vector.y, - vector.z, - ) + rot = Quaternion(sqrt(1.0f - magnitudeSqr), vector) } return rot.unit() From 1b87235a44caa1fc9d4cd5ddf63678bf85f7a785 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 29 Jul 2024 15:17:36 -0700 Subject: [PATCH 50/68] Format fixes --- .../dev/slimevr/tracking/processor/Constraint.kt | 10 +++++----- .../slimevr/tracking/processor/skeleton/IKChain.kt | 13 +++++-------- 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 037f0b3d77..bbf37dd80b 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -5,9 +5,9 @@ import io.github.axisangles.ktmath.Vector3 import kotlin.math.* /** - * Represents a function that applies a rotational constraint to a direction vector. + * Represents a function that applies a rotational constraint. */ -typealias ConstraintFunction = (Quaternion, Bone, Float, Float) -> Quaternion +typealias ConstraintFunction = (inputRotation: Quaternion, thisBone: Bone, limit1: Float, limit2: Float) -> Quaternion /** * Represents the rotational limits of a Bone relative to its parent @@ -103,7 +103,7 @@ class Constraint( } // Constraint function for TwistSwingConstraint - val twistSwingConstraint: ConstraintFunction = + private val twistSwingConstraint: ConstraintFunction = { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float -> if (thisBone.parent == null) { rotation @@ -121,7 +121,7 @@ class Constraint( } // Constraint function for a hinge constraint with min and max angles - val hingeConstraint: ConstraintFunction = + private val hingeConstraint: ConstraintFunction = { rotation: Quaternion, thisBone: Bone, min: Float, max: Float -> if (thisBone.parent == null) { rotation @@ -138,7 +138,7 @@ class Constraint( } // Constraint function for CompleteConstraint - val completeConstraint: ConstraintFunction = { _: Quaternion, thisBone: Bone, _: Float, _: Float -> + private val completeConstraint: ConstraintFunction = { _: Quaternion, thisBone: Bone, _: Float, _: Float -> thisBone.getGlobalRotation() } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 29fe4cc884..1a87337b5f 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -1,13 +1,11 @@ package dev.slimevr.tracking.processor.skeleton import dev.slimevr.tracking.processor.Bone -import dev.slimevr.tracking.processor.Constraint +import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType 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.sin +import kotlin.math.* /* * This class implements a chain of Bones @@ -42,7 +40,7 @@ class IKChain( */ private fun prepBones() { for (i in 0.. Date: Sun, 18 Aug 2024 14:13:48 -0700 Subject: [PATCH 51/68] Enforce constraints bidirectionally --- .../slimevr/tracking/processor/Constraint.kt | 4 ++-- .../tracking/processor/skeleton/IKChain.kt | 21 ++++++++++++++----- .../processor/skeleton/IKConstraint.kt | 6 +++++- .../tracking/processor/skeleton/IKSolver.kt | 12 ++++++----- 4 files changed, 30 insertions(+), 13 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index bbf37dd80b..3324e15e23 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -20,10 +20,10 @@ class Constraint( private val constraintFunction = constraintTypeToFunc(constraintType) private val twistRad = Math.toRadians(twist.toDouble()).toFloat() private val swingRad = Math.toRadians(swing.toDouble()).toFloat() + var hasTrackerRotation = false /** - * If false don't allow the rotation of the bone - * to be modified except to satisfy a constraint + * If false solve with minimal movement applied to this link */ var allowModifications = true diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 1a87337b5f..229aba6450 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -40,7 +40,7 @@ class IKChain( */ private fun prepBones() { for (i in 0..() var currentBone = root - currentBone.rotationConstraint.allowModifications = - getConstraint(currentBone, rotationalConstraints) == null + var constraint = getConstraint(currentBone, rotationalConstraints) + currentBone.rotationConstraint.allowModifications = constraint == null + currentBone.rotationConstraint.hasTrackerRotation = constraint != null boneList.add(currentBone) // Get constraints @@ -81,8 +82,9 @@ class IKSolver(private val root: Bone) { // Add bones until there is a reason to make a new chain while (currentBone.children.size == 1 && tailConstraint == null) { currentBone = currentBone.children.first() - currentBone.rotationConstraint.allowModifications = - getConstraint(currentBone, rotationalConstraints) == null + constraint = getConstraint(currentBone, rotationalConstraints) + currentBone.rotationConstraint.allowModifications = constraint == null + currentBone.rotationConstraint.hasTrackerRotation = constraint != null boneList.add(currentBone) tailConstraint = getConstraint(currentBone, positionalConstraints) } From 3b059526733fde9aa54f89027d03046254fd08d4 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Thu, 22 Aug 2024 19:12:43 -0700 Subject: [PATCH 52/68] Add upper elbow --- .../slimevr/tracking/processor/BoneType.java | 2 ++ .../processor/config/SkeletonConfigManager.kt | 14 +++++++++++++ .../processor/skeleton/HumanSkeleton.kt | 20 +++++++++++++++---- .../tracking/processor/skeleton/IKChain.kt | 3 +++ .../tracking/processor/skeleton/IKSolver.kt | 4 ++++ 5 files changed, 39 insertions(+), 4 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java b/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java index 146311747d..a7a60edd45 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java @@ -36,6 +36,8 @@ public enum BoneType { RIGHT_UPPER_ARM(BodyPart.RIGHT_UPPER_ARM), LEFT_SHOULDER(BodyPart.LEFT_SHOULDER), RIGHT_SHOULDER(BodyPart.RIGHT_SHOULDER), + LEFT_UPPER_SHOULDER, + RIGHT_UPPER_SHOULDER, LEFT_HAND(BodyPart.LEFT_HAND), RIGHT_HAND(BodyPart.RIGHT_HAND), LEFT_HAND_TRACKER, diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt index f1130d85ad..3e4528400d 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt @@ -245,6 +245,20 @@ class SkeletonConfigManager( -getOffset(SkeletonConfigOffsets.FOOT_LENGTH), ) + BoneType.LEFT_UPPER_SHOULDER -> setNodeOffset( + nodeOffset, + 0f, + 0f, + 0f, + ) + + BoneType.RIGHT_UPPER_SHOULDER -> setNodeOffset( + nodeOffset, + 0f, + 0f, + 0f, + ) + BoneType.LEFT_SHOULDER -> setNodeOffset( nodeOffset, -getOffset(SkeletonConfigOffsets.SHOULDERS_WIDTH) / 2f, diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 21a4a61e92..bae069353c 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -54,8 +54,10 @@ class HumanSkeleton( val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 90f)) // Arm bones - val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 180f, 20f)) - val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 180f, 20f)) + 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 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)) @@ -234,8 +236,10 @@ class HumanSkeleton( } // Shoulders - neckBone.attachChild(leftShoulderBone) - neckBone.attachChild(rightShoulderBone) + neckBone.attachChild(leftUpperShoulderBone) + neckBone.attachChild(rightUpperShoulderBone) + leftUpperShoulderBone.attachChild(leftShoulderBone) + rightUpperShoulderBone.attachChild(rightShoulderBone) // Upper arm leftShoulderBone.attachChild(leftUpperArmBone) @@ -428,6 +432,7 @@ class HumanSkeleton( // Left arm updateArmTransforms( isTrackingLeftArmFromController, + leftUpperShoulderBone, leftShoulderBone, leftUpperArmBone, leftElbowTrackerBone, @@ -442,6 +447,7 @@ class HumanSkeleton( // Right arm updateArmTransforms( isTrackingRightArmFromController, + rightUpperShoulderBone, rightShoulderBone, rightUpperArmBone, rightElbowTrackerBone, @@ -715,6 +721,7 @@ class HumanSkeleton( */ private fun updateArmTransforms( isTrackingFromController: Boolean, + upperShoulderBone: Bone, shoulderBone: Bone, upperArmBone: Bone, elbowTrackerBone: Bone, @@ -747,6 +754,7 @@ class HumanSkeleton( // Get shoulder rotation var armRot = shoulderTracker?.getRotation() ?: upperChestBone.getLocalRotation() // Set shoulder rotation + upperShoulderBone.setRotation(upperChestBone.getLocalRotation()) shoulderBone.setRotation(armRot) if (upperArmTracker != null || lowerArmTracker != null) { @@ -958,6 +966,8 @@ class HumanSkeleton( BoneType.RIGHT_FOOT -> rightFootBone BoneType.LEFT_FOOT_TRACKER -> leftFootTrackerBone BoneType.RIGHT_FOOT_TRACKER -> rightFootTrackerBone + BoneType.LEFT_UPPER_SHOULDER -> leftUpperShoulderBone + BoneType.RIGHT_UPPER_SHOULDER -> rightUpperShoulderBone BoneType.LEFT_SHOULDER -> leftShoulderBone BoneType.RIGHT_SHOULDER -> rightShoulderBone BoneType.LEFT_UPPER_ARM -> leftUpperArmBone @@ -1006,6 +1016,8 @@ class HumanSkeleton( */ private val allArmBones: Array get() = arrayOf( + leftUpperShoulderBone, + rightUpperShoulderBone, leftShoulderBone, rightShoulderBone, leftUpperArmBone, diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 229aba6450..3ae847c52f 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -53,6 +53,8 @@ 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 + // 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() @@ -99,6 +101,7 @@ class IKChain( */ fun resetChain() { distToTargetSqr = Float.POSITIVE_INFINITY + // prepBones() for (child in children) { child.resetChain() diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 7a72863f37..84f1ab1468 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -2,6 +2,7 @@ package dev.slimevr.tracking.processor.skeleton import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.trackers.Tracker +import solarxr_protocol.datatypes.BodyPart /* * Implements CCDIK (Cyclic Coordinate Descent Inverse Kinematics) to allow @@ -12,12 +13,15 @@ import dev.slimevr.tracking.trackers.Tracker 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 const val ANNEALING_MAX = 60 const val DAMPENING_FACTOR = 0.5f const val STATIC_DAMPENING = 0.1f + // Short limbs positioned on the end of the skeleton are prone to over rotation + val LOCK_ROTATION = setOf(BodyPart.LEFT_HAND, BodyPart.RIGHT_HAND, BodyPart.LEFT_FOOT, BodyPart.RIGHT_FOOT) } var enabled = true From b377c99cfb704d6780904569984843552e6785ff Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sat, 31 Aug 2024 12:01:52 -0700 Subject: [PATCH 53/68] small fixes + optimization --- .../slimevr/tracking/processor/Constraint.kt | 33 +++++++++++++++---- .../processor/skeleton/HumanSkeleton.kt | 30 +++++++++-------- .../tracking/processor/skeleton/IKChain.kt | 27 ++++++++------- .../tracking/processor/skeleton/IKSolver.kt | 2 +- 4 files changed, 58 insertions(+), 34 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 3324e15e23..f70e93ac0e 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -16,10 +16,12 @@ 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 /** @@ -27,12 +29,26 @@ class Constraint( */ 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, @@ -51,10 +67,10 @@ class Constraint( rotation: Quaternion, twistAxis: Vector3, ): Pair { - 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) } @@ -76,7 +92,7 @@ class Constraint( ) } - return rot + return rot.unit() } private fun constrain(rotation: Quaternion, minAngle: Float, maxAngle: Float, axis: Vector3): Quaternion { @@ -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() } } @@ -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() } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index bae069353c..a33f54e8d5 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -38,32 +38,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)) @@ -1001,6 +1001,8 @@ class HumanSkeleton( rightLowerLegBone, leftFootBone, rightFootBone, + leftUpperShoulderBone, + rightUpperShoulderBone, leftShoulderBone, rightShoulderBone, leftUpperArmBone, diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 3ae847c52f..dff440f97c 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -40,7 +40,7 @@ class IKChain( */ private fun prepBones() { for (i in 0.. Date: Sat, 31 Aug 2024 13:49:18 -0700 Subject: [PATCH 54/68] Enforce constraints within the skeleton --- .../tracking/processor/skeleton/HumanSkeleton.kt | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index a33f54e8d5..ac4c54f6f6 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -372,6 +372,8 @@ class HumanSkeleton( updateTransforms() updateBones() + enforceConstraints() + updateBones() if (!pauseTracking) ikSolver.solve() @@ -385,6 +387,17 @@ class HumanSkeleton( viveEmulation.update() } + /** + * Enforce rotation constraints on all bones + */ + private fun enforceConstraints() { + for (bone in allHumanBones) { + val initialRot = bone.getGlobalRotation() + val newRot = bone.rotationConstraint.applyConstraint(initialRot, bone) + bone.setRotationRaw(newRot) + } + } + /** * Update all the bones by updating the roots */ From 0bf893ec1a7d719f9c7f1872231d1682e098c091 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sat, 31 Aug 2024 14:02:55 -0700 Subject: [PATCH 55/68] formmating --- .../src/main/java/dev/slimevr/tracking/processor/Constraint.kt | 2 +- .../java/dev/slimevr/tracking/processor/skeleton/IKChain.kt | 2 +- .../java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index f70e93ac0e..2e945a82fb 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -16,7 +16,7 @@ class Constraint( val constraintType: ConstraintType, twist: Float = 0.0f, swing: Float = 0.0f, - allowedDeviation: Float = 0f + allowedDeviation: Float = 0f, ) { private val constraintFunction = constraintTypeToFunc(constraintType) private val twistRad = Math.toRadians(twist.toDouble()).toFloat() diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index dff440f97c..3e0d8297eb 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -102,7 +102,7 @@ class IKChain( for (b in bones) { b.rotationConstraint.initialRotation = b.getGlobalRotation() } - //prepBones() + // prepBones() for (child in children) { child.resetChain() diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index d2d5dbeb83..955029ae60 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -19,6 +19,7 @@ class IKSolver(private val root: Bone) { const val ANNEALING_MAX = 60 const val DAMPENING_FACTOR = 0.5f const val STATIC_DAMPENING = 0.1f + // Short limbs positioned on the end of the skeleton are prone to over rotation val LOCK_ROTATION = setOf(BodyPart.LEFT_HAND, BodyPart.RIGHT_HAND, BodyPart.LEFT_FOOT, BodyPart.RIGHT_FOOT) } From 5f478609bd7b15b8b548e7e29ece954868476452 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Sun, 8 Sep 2024 18:59:42 -0700 Subject: [PATCH 56/68] Correct for constrain violation --- .../filtering/QuaternionMovingAverage.kt | 3 + .../slimevr/tracking/processor/Constraint.kt | 17 ++--- .../processor/skeleton/HumanSkeleton.kt | 70 +++++++++++++++---- .../trackers/TrackerFilteringHandler.kt | 5 ++ .../tracking/trackers/TrackerResetsHandler.kt | 15 ++++ 5 files changed, 87 insertions(+), 23 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt index f31b77f1bb..501844a478 100644 --- a/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt +++ b/server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt @@ -21,6 +21,7 @@ class QuaternionMovingAverage( initialRotation: Quaternion, ) { var filteredQuaternion = IDENTITY + var filteringImpact = 0f private var smoothFactor = 0f private var predictFactor = 0f private lateinit var rotBuffer: CircularArrayList @@ -91,6 +92,8 @@ class QuaternionMovingAverage( // Smooth towards the target rotation by the slerp factor filteredQuaternion = smoothingQuaternion.interpR(latestQuaternion, amt) } + + filteringImpact = latestQuaternion.angleToR(filteredQuaternion) } @Synchronized diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 2e945a82fb..d89502a5fe 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -78,18 +78,13 @@ class Constraint( 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 + val sign = if (rotation.w >= 0f) 1f else -1f var vector = rotation.xyz var rot = rotation if (vector.lenSq() > magnitudeSqr) { vector = vector.unit() * magnitude - rot = Quaternion( - sqrt(1.0f - magnitudeSqr) * sign, - vector.x, - vector.y, - vector.z, - ) + rot = Quaternion(sqrt(1.0f - magnitudeSqr) * sign, vector) } return rot.unit() @@ -98,12 +93,12 @@ class Constraint( 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 + val magnitudeSqrMin = magnitudeMin * magnitudeMin * if (minAngle >= 0f) 1f else -1f + val magnitudeSqrMax = magnitudeMax * magnitudeMax * if (maxAngle >= 0f) 1f else -1f var vector = rotation.xyz var rot = rotation - val rotMagnitude = vector.lenSq() * if (vector.dot(axis) < 0) -1f else 1f + val rotMagnitude = vector.lenSq() * if (vector.dot(axis) * sign(rot.w) < 0) -1f else 1f if (rotMagnitude < magnitudeSqrMin || rotMagnitude > magnitudeSqrMax) { val distToMin = min(abs(rotMagnitude - magnitudeSqrMin), abs(rotMagnitude + magnitudeSqrMin)) val distToMax = min(abs(rotMagnitude - magnitudeSqrMax), abs(rotMagnitude + magnitudeSqrMax)) @@ -128,7 +123,7 @@ class Constraint( val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset val rotationLocal = (parent.getGlobalRotation() * localRotationOffset).inv() * rotation - var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) + var (swingQ, twistQ) = decompose(rotationLocal, Vector3.POS_Y) swingQ = constrain(swingQ, swingRad) twistQ = constrain(twistQ, twistRad) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index ac4c54f6f6..75a8950ff0 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -38,26 +38,26 @@ 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, 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)) + val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(ConstraintType.TWIST_SWING, 15f, 120f)) + val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 15f, 120f)) + val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 15f, 120f)) + val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 20f, 120f)) // Lower body bones - 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 leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) + val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) + val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 90f, 180f)) + val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 180f, 180f)) 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, 50f)) - val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 50f)) + val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) + val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) // 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, 20f, 10f)) - val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 20f, 10f)) + val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 10f)) + val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 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)) @@ -392,9 +392,31 @@ class HumanSkeleton( */ private fun enforceConstraints() { for (bone in allHumanBones) { + // Correct the rotation if it violates a constraint val initialRot = bone.getGlobalRotation() val newRot = bone.rotationConstraint.applyConstraint(initialRot, bone) bone.setRotationRaw(newRot) + bone.updateThisNode() + + if (bone.rotationConstraint.constraintType == ConstraintType.HINGE) continue + + // Apply a correction to the tracker rotation if filtering is not greatly affecting the output rotation + val deltaRot = newRot * initialRot.inv() + + val angle = deltaRot.angleR() + val tracker = getTrackerForBone(bone.boneType) + + val parentTracker = getTrackerForBone(bone.parent?.boneType) + if ((angle > 0.01f) && ( + tracker?.filteringHandler?.getFilteringImpact() + ?: 0f + ) < 0.01f && ( + parentTracker?.filteringHandler?.getFilteringImpact() + ?: 0f + ) < 0.01f + ) { + tracker?.resetsHandler?.updateDynamicFix(deltaRot) + } } } @@ -995,6 +1017,30 @@ class HumanSkeleton( BoneType.RIGHT_HAND_TRACKER -> rightHandTrackerBone } + private fun getTrackerForBone(bone: BoneType?): Tracker? = when (bone) { + BoneType.HEAD -> headTracker + BoneType.NECK -> neckTracker + BoneType.UPPER_CHEST -> upperChestTracker + BoneType.CHEST -> chestTracker + BoneType.WAIST -> waistTracker + BoneType.HIP -> hipTracker + BoneType.LEFT_UPPER_LEG -> leftUpperLegTracker + BoneType.RIGHT_UPPER_LEG -> rightUpperLegTracker + BoneType.LEFT_LOWER_LEG -> leftLowerLegTracker + BoneType.RIGHT_LOWER_LEG -> rightLowerLegTracker + BoneType.LEFT_FOOT -> leftFootTracker + BoneType.RIGHT_FOOT -> rightFootTracker + BoneType.LEFT_SHOULDER -> leftShoulderTracker + BoneType.RIGHT_SHOULDER -> rightShoulderTracker + BoneType.LEFT_UPPER_ARM -> leftUpperArmTracker + BoneType.RIGHT_UPPER_ARM -> rightUpperArmTracker + BoneType.LEFT_LOWER_ARM -> leftLowerArmTracker + BoneType.RIGHT_LOWER_ARM -> rightLowerArmTracker + BoneType.LEFT_HAND -> leftHandTracker + BoneType.RIGHT_HAND -> rightHandTracker + else -> null + } + /** * Returns an array of all the non-tracker bones. */ diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt index 5e7a8ca129..84eb0914c4 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt @@ -51,4 +51,9 @@ class TrackerFilteringHandler { * Get the filtered rotation from the moving average */ fun getFilteredRotation(): Quaternion = movingAverage?.filteredQuaternion ?: Quaternion.IDENTITY + + /** + * Get the impact filtering has on the rotation + */ + fun getFilteringImpact(): Float = movingAverage?.filteringImpact ?: 0f } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index ef0d053192..88cddee612 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -62,6 +62,7 @@ class TrackerResetsHandler(val tracker: Tracker) { var mountRotFix = Quaternion.IDENTITY private set private var yawFix = Quaternion.IDENTITY + private var dynamicFix = Quaternion.IDENTITY // Yaw reset smoothing vars private var yawFixOld = Quaternion.IDENTITY @@ -167,6 +168,7 @@ class TrackerResetsHandler(val tracker: Tracker) { rot = mountRotFix.inv() * (rot * mountRotFix) rot *= tposeDownFix rot = yawFix * rot + rot *= dynamicFix return rot } @@ -213,6 +215,8 @@ class TrackerResetsHandler(val tracker: Tracker) { * 0). This allows the tracker to be strapped to body at any pitch and roll. */ fun resetFull(reference: Quaternion) { + dynamicFix = Quaternion.IDENTITY + // Adjust for T-Pose (down) tposeDownFix = if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN)) { EulerAngles(EulerOrder.YZX, 0f, 0f, -FastMath.HALF_PI).toQuaternion() @@ -289,6 +293,8 @@ class TrackerResetsHandler(val tracker: Tracker) { * position should be corrected in the source. */ fun resetYaw(reference: Quaternion) { + dynamicFix = Quaternion.IDENTITY + // Old rot for drift compensation val oldRot = adjustToReference(tracker.getRawRotation()) lastResetQuaternion = oldRot @@ -323,6 +329,8 @@ class TrackerResetsHandler(val tracker: Tracker) { fun resetMounting(reference: Quaternion) { if (!resetMountingFeet && isFootTracker()) return + dynamicFix = Quaternion.IDENTITY + // Get the current calibrated rotation var rotBuf = adjustToDrift(tracker.getRawRotation() * mountingOrientation) rotBuf = gyroFix * rotBuf @@ -367,6 +375,13 @@ class TrackerResetsHandler(val tracker: Tracker) { if (saveMountingReset) tracker.saveMountingResetOrientation(mountRotFix) } + /** + * Apply a corrective rotation to the gyroFix + */ + fun updateDynamicFix(correctedRotation: Quaternion) { + dynamicFix *= correctedRotation + } + fun clearMounting() { mountRotFix = Quaternion.IDENTITY } From e02461dbb015d1c6c8cd0e1ec6b4dbd8ecfdb5f2 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 16 Sep 2024 13:09:48 -0700 Subject: [PATCH 57/68] fix bugs, add loose hinge constraint --- .../slimevr/tracking/processor/Constraint.kt | 39 +++++++++++++++---- .../config/SkeletonConfigToggles.java | 2 +- .../processor/skeleton/HumanSkeleton.kt | 21 +++++----- .../tracking/trackers/TrackerResetsHandler.kt | 2 +- 4 files changed, 42 insertions(+), 22 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index d89502a5fe..fb49fe1480 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -7,10 +7,11 @@ import kotlin.math.* /** * Represents a function that applies a rotational constraint. */ -typealias ConstraintFunction = (inputRotation: Quaternion, thisBone: Bone, limit1: Float, limit2: Float) -> Quaternion +typealias ConstraintFunction = (inputRotation: Quaternion, thisBone: Bone, limit1: Float, limit2: Float, limit3: Float) -> Quaternion /** - * Represents the rotational limits of a Bone relative to its parent + * Represents the rotational limits of a Bone relative to its parent, + * twist and swing are the max and min when constraintType is a hinge. */ class Constraint( val constraintType: ConstraintType, @@ -35,7 +36,7 @@ class Constraint( * 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() + fun applyConstraint(rotation: Quaternion, thisBone: Bone): Quaternion = constraintFunction(rotation, thisBone, swingRad, twistRad, allowedDeviationRad).unit() /** * Force the given rotation to be within allowedDeviation degrees away from @@ -53,6 +54,7 @@ class Constraint( enum class ConstraintType { TWIST_SWING, HINGE, + LOOSE_HINGE, COMPLETE, } @@ -61,6 +63,7 @@ class Constraint( ConstraintType.COMPLETE -> completeConstraint ConstraintType.TWIST_SWING -> twistSwingConstraint ConstraintType.HINGE -> hingeConstraint + ConstraintType.LOOSE_HINGE -> looseHingeConstraint } private fun decompose( @@ -68,7 +71,6 @@ class Constraint( twistAxis: Vector3, ): Pair { val projection = rotation.project(twistAxis).unit() - val twist = Quaternion(rotation.w, projection.xyz).unit() val swing = (rotation * twist.inv()).unit() @@ -115,7 +117,7 @@ class Constraint( // Constraint function for TwistSwingConstraint private val twistSwingConstraint: ConstraintFunction = - { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float -> + { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float, _ : Float -> if (thisBone.parent == null) { rotation } else { @@ -123,7 +125,7 @@ class Constraint( val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset val rotationLocal = (parent.getGlobalRotation() * localRotationOffset).inv() * rotation - var (swingQ, twistQ) = decompose(rotationLocal, Vector3.POS_Y) + var (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) swingQ = constrain(swingQ, swingRad) twistQ = constrain(twistQ, twistRad) @@ -134,7 +136,7 @@ class Constraint( // Constraint function for a hinge constraint with min and max angles private val hingeConstraint: ConstraintFunction = - { rotation: Quaternion, thisBone: Bone, min: Float, max: Float -> + { rotation: Quaternion, thisBone: Bone, min: Float, max: Float, _: Float -> if (thisBone.parent == null) { rotation } else { @@ -151,8 +153,29 @@ class Constraint( } } + // Constraint function for a hinge constraint with min and max angles that allows nonHingeDeviation + // rotation on all axis but the hinge + private val looseHingeConstraint: ConstraintFunction = + { rotation: Quaternion, thisBone: Bone, min: Float, max: Float, nonHingeDeviation: Float -> + if (thisBone.parent == null) { + rotation + } else { + val parent = thisBone.parent!! + val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset + val rotationLocal = + (parent.getGlobalRotation() * localRotationOffset).inv() * rotation + + var (nonHingeRot, hingeAxisRot) = decompose(rotationLocal, Vector3.NEG_X) + + hingeAxisRot = constrain(hingeAxisRot, min, max, Vector3.NEG_X) + nonHingeRot = constrain(nonHingeRot, nonHingeDeviation) + + (parent.getGlobalRotation() * localRotationOffset * (nonHingeRot * hingeAxisRot)).unit() + } + } + // Constraint function for CompleteConstraint - private val completeConstraint: ConstraintFunction = { _: Quaternion, thisBone: Bone, _: Float, _: Float -> + private val completeConstraint: ConstraintFunction = { _: Quaternion, thisBone: Bone, _: Float, _: Float, _: Float -> thisBone.getGlobalRotation() } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java index ac008acf59..5e7f834bb9 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java @@ -16,7 +16,7 @@ public enum SkeletonConfigToggles { TOE_SNAP(8, "Toe Snap", "toeSnap", false), FOOT_PLANT(9, "Foot Plant", "footPlant", true), SELF_LOCALIZATION(10, "Self Localization", "selfLocalization", false), - USE_POSITION(11, "Use Position", "usePosition", true),; + USE_POSITION(11, "Use Position", "usePosition", false),; public static final SkeletonConfigToggles[] values = values(); private static final Map byStringVal = new HashMap<>(); diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 75a8950ff0..d9d3b4833d 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -38,18 +38,18 @@ 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, 15f, 120f)) - val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 15f, 120f)) - val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 15f, 120f)) + val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(ConstraintType.TWIST_SWING, 90f, 120f)) + val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 30f, 120f)) + val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 30f, 120f)) val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 20f, 120f)) // Lower body bones val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) - val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 90f, 180f)) + val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 180f, 180f)) val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 180f, 180f)) - 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 leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 20f)) + val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 20f)) val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) @@ -60,8 +60,8 @@ class HumanSkeleton( val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 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 leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 20f)) + val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 20f)) val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f)) val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f)) @@ -396,16 +396,13 @@ class HumanSkeleton( val initialRot = bone.getGlobalRotation() val newRot = bone.rotationConstraint.applyConstraint(initialRot, bone) bone.setRotationRaw(newRot) - bone.updateThisNode() - if (bone.rotationConstraint.constraintType == ConstraintType.HINGE) continue + if (bone.rotationConstraint.constraintType == ConstraintType.HINGE || bone.rotationConstraint.constraintType == ConstraintType.LOOSE_HINGE) continue // Apply a correction to the tracker rotation if filtering is not greatly affecting the output rotation val deltaRot = newRot * initialRot.inv() - val angle = deltaRot.angleR() val tracker = getTrackerForBone(bone.boneType) - val parentTracker = getTrackerForBone(bone.parent?.boneType) if ((angle > 0.01f) && ( tracker?.filteringHandler?.getFilteringImpact() diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index 88cddee612..e569583098 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -168,7 +168,7 @@ class TrackerResetsHandler(val tracker: Tracker) { rot = mountRotFix.inv() * (rot * mountRotFix) rot *= tposeDownFix rot = yawFix * rot - rot *= dynamicFix + rot = dynamicFix * rot return rot } From 7e6ec3a26b274d0e5d8b6cf242dcfeb97e4c26bf Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 16 Sep 2024 13:11:50 -0700 Subject: [PATCH 58/68] Formatting --- .../src/main/java/dev/slimevr/tracking/processor/Constraint.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index fb49fe1480..c3a5fed44f 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -117,7 +117,7 @@ class Constraint( // Constraint function for TwistSwingConstraint private val twistSwingConstraint: ConstraintFunction = - { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float, _ : Float -> + { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float, _: Float -> if (thisBone.parent == null) { rotation } else { From 5529474893796c766be55bc013820ee0c5055a07 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 16 Sep 2024 18:58:35 -0700 Subject: [PATCH 59/68] Increase hinge constraint loose-ness --- .../slimevr/tracking/processor/skeleton/HumanSkeleton.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index d9d3b4833d..153a217195 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -48,8 +48,8 @@ class HumanSkeleton( val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 180f, 180f)) val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 180f, 180f)) - val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 20f)) - val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 20f)) + val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 50f)) + val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 50f)) val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) @@ -60,8 +60,8 @@ class HumanSkeleton( val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 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.LOOSE_HINGE, 0f, -180f, 20f)) - val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 20f)) + val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f)) + val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f)) val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f)) val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f)) From cbacad90e5fb86f524264e1f7ecde4899d1e2a6c Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 23 Sep 2024 12:35:39 -0700 Subject: [PATCH 60/68] fix decomposition --- .../main/java/dev/slimevr/tracking/processor/Constraint.kt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index c3a5fed44f..808df90d49 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -70,10 +70,9 @@ class Constraint( rotation: Quaternion, twistAxis: Vector3, ): Pair { - val projection = rotation.project(twistAxis).unit() - val twist = Quaternion(rotation.w, projection.xyz).unit() + val projection = rotation.project(twistAxis) + val twist = Quaternion(sqrt(1.0f - projection.xyz.lenSq()) * if (rotation.w >= 0f) 1f else -1f, projection.xyz).unit() val swing = (rotation * twist.inv()).unit() - return Pair(swing, twist) } From d4450dc8bae9f7a1ebba33fd960773a36817364c Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 23 Sep 2024 12:41:33 -0700 Subject: [PATCH 61/68] Add toggles for enabling skeleton constraints --- gui/public/i18n/en/translation.ftl | 5 ++ .../settings/pages/GeneralSettings.tsx | 90 ++++++++++++++----- .../rpc/settings/RPCSettingsBuilder.java | 4 +- .../rpc/settings/RPCSettingsHandler.kt | 2 + .../config/SkeletonConfigToggles.java | 4 +- .../processor/skeleton/HumanSkeleton.kt | 26 ++++-- .../tracking/trackers/TrackerResetsHandler.kt | 1 + solarxr-protocol | 2 +- 8 files changed, 100 insertions(+), 34 deletions(-) diff --git a/gui/public/i18n/en/translation.ftl b/gui/public/i18n/en/translation.ftl index 6e8165061f..c5bf85dfe9 100644 --- a/gui/public/i18n/en/translation.ftl +++ b/gui/public/i18n/en/translation.ftl @@ -396,6 +396,11 @@ settings-general-fk_settings-leg_tweak-foot_plant-description = Foot-plant rotat settings-general-fk_settings-leg_fk = Leg tracking settings-general-fk_settings-leg_fk-reset_mounting_feet-description = Enable feet Mounting Reset by tiptoeing. settings-general-fk_settings-leg_fk-reset_mounting_feet = Feet Mounting Reset +settings-general-fk_settings-enforce_joint_constraints = Skeletal Limits +settings-general-fk_settings-enforce_joint_constraints-enforce_constraints = Enforce constraints +settings-general-fk_settings-enforce_joint_constraints-enforce_constraints-description = Prevents joints from rotating past their limit +settings-general-fk_settings-enforce_joint_constraints-correct_constraints = Correct with constraints +settings-general-fk_settings-enforce_joint_constraints-correct_constraints-description = Correct joint rotations when they push last their limit settings-general-fk_settings-ik = Position data settings-general-fk_settings-ik-use_position = Use Position data settings-general-fk_settings-ik-use_position-description = Enables the use of position data from trackers that provide it. When enabling this make sure to full reset and recalibrate in game. diff --git a/gui/src/components/settings/pages/GeneralSettings.tsx b/gui/src/components/settings/pages/GeneralSettings.tsx index 2c664a60b1..1bbce39a0a 100644 --- a/gui/src/components/settings/pages/GeneralSettings.tsx +++ b/gui/src/components/settings/pages/GeneralSettings.tsx @@ -70,6 +70,8 @@ interface SettingsForm { footPlant: boolean; selfLocalization: boolean; usePosition: boolean; + enforceConstraints: boolean; + correctConstraints: boolean; }; ratios: { imputeWaistFromChestHip: number; @@ -130,6 +132,8 @@ const defaultValues: SettingsForm = { footPlant: true, selfLocalization: false, usePosition: true, + enforceConstraints: true, + correctConstraints: true, }, ratios: { imputeWaistFromChestHip: 0.3, @@ -238,6 +242,8 @@ export function GeneralSettings() { toggles.footPlant = values.toggles.footPlant; toggles.selfLocalization = values.toggles.selfLocalization; toggles.usePosition = values.toggles.usePosition; + toggles.enforceConstraints = values.toggles.enforceConstraints; + toggles.correctConstraints = values.toggles.correctConstraints; modelSettings.toggles = toggles; } @@ -985,28 +991,6 @@ export function GeneralSettings() { /> -
- - {l10n.getString('settings-general-fk_settings-ik')} - - - {l10n.getString( - 'settings-general-fk_settings-ik-use_position-description' - )} - -
-
- -
- {l10n.getString( 'settings-general-fk_settings-arm_fk-reset_mode-description' @@ -1059,6 +1043,68 @@ export function GeneralSettings() { > +
+ + {l10n.getString('settings-general-fk_settings-enforce_joint_constraints')} + + + {l10n.getString( + 'settings-general-fk_settings-enforce_joint_constraints-enforce_constraints-description' + )} + +
+
+ +
+
+ + {l10n.getString( + 'settings-general-fk_settings-enforce_joint_constraints-correct_constraints-description' + )} + +
+
+ +
+ +
+ + {l10n.getString('settings-general-fk_settings-ik')} + + + {l10n.getString( + 'settings-general-fk_settings-ik-use_position-description' + )} + +
+
+ +
+ {config?.debug && ( <>
diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java index 92116ed99d..5839db94dc 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java @@ -191,7 +191,9 @@ public static int createModelSettings( humanPoseManager.getToggle(SkeletonConfigToggles.TOE_SNAP), humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT), humanPoseManager.getToggle(SkeletonConfigToggles.SELF_LOCALIZATION), - humanPoseManager.getToggle(SkeletonConfigToggles.USE_POSITION) + humanPoseManager.getToggle(SkeletonConfigToggles.USE_POSITION), + humanPoseManager.getToggle(SkeletonConfigToggles.ENFORCE_CONSTRAINTS), + humanPoseManager.getToggle(SkeletonConfigToggles.CORRECT_CONSTRAINTS) ); int ratiosOffset = ModelRatios .createModelRatios( diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt index 8b88e57077..c8dd2b944d 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt @@ -257,6 +257,8 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) { hpm.setToggle(SkeletonConfigToggles.FOOT_PLANT, toggles.footPlant()) hpm.setToggle(SkeletonConfigToggles.SELF_LOCALIZATION, toggles.selfLocalization()) hpm.setToggle(SkeletonConfigToggles.USE_POSITION, toggles.usePosition()) + hpm.setToggle(SkeletonConfigToggles.ENFORCE_CONSTRAINTS, toggles.enforceConstraints()) + hpm.setToggle(SkeletonConfigToggles.CORRECT_CONSTRAINTS, toggles.correctConstraints()) } if (ratios != null) { diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java index 5e7f834bb9..f8b991cf9f 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java @@ -16,7 +16,9 @@ public enum SkeletonConfigToggles { TOE_SNAP(8, "Toe Snap", "toeSnap", false), FOOT_PLANT(9, "Foot Plant", "footPlant", true), SELF_LOCALIZATION(10, "Self Localization", "selfLocalization", false), - USE_POSITION(11, "Use Position", "usePosition", false),; + USE_POSITION(11, "Use Position", "usePosition", false), + ENFORCE_CONSTRAINTS(12, "Enforce Constraints", "enforceConstraints", true), + CORRECT_CONSTRAINTS(13, "Correct Constraints", "correctConstraints", true),; public static final SkeletonConfigToggles[] values = values(); private static final Map byStringVal = new HashMap<>(); diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 153a217195..3cb0c1ed4a 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -39,15 +39,15 @@ class HumanSkeleton( 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, 90f, 120f)) - val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 30f, 120f)) - val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 30f, 120f)) - val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 20f, 120f)) + val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 60f, 120f)) + val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 60f, 120f)) + val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 60f, 120f)) // Lower body bones val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) - val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 180f, 180f)) - val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 180f, 180f)) + val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) + val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 50f)) val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 50f)) val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) @@ -58,8 +58,8 @@ class HumanSkeleton( val rightUpperShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.COMPLETE)) val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 10f)) val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 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 leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) + val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f)) val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f)) val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f)) @@ -133,6 +133,8 @@ class HumanSkeleton( private var extendedPelvisModel = false private var extendedKneeModel = false private var forceArmsFromHMD = true + private var enforceConstraints = true + private var correctConstraints = true // Ratios private var waistFromChestHipAveraging = 0f @@ -373,7 +375,6 @@ class HumanSkeleton( updateTransforms() updateBones() enforceConstraints() - updateBones() if (!pauseTracking) ikSolver.solve() @@ -391,13 +392,16 @@ class HumanSkeleton( * Enforce rotation constraints on all bones */ private fun enforceConstraints() { + if (!enforceConstraints) return + for (bone in allHumanBones) { // Correct the rotation if it violates a constraint val initialRot = bone.getGlobalRotation() val newRot = bone.rotationConstraint.applyConstraint(initialRot, bone) bone.setRotationRaw(newRot) + bone.updateThisNode() - if (bone.rotationConstraint.constraintType == ConstraintType.HINGE || bone.rotationConstraint.constraintType == ConstraintType.LOOSE_HINGE) continue + if (!correctConstraints || bone.rotationConstraint.constraintType == ConstraintType.HINGE || bone.rotationConstraint.constraintType == ConstraintType.LOOSE_HINGE) continue // Apply a correction to the tracker rotation if filtering is not greatly affecting the output rotation val deltaRot = newRot * initialRot.inv() @@ -917,6 +921,10 @@ class HumanSkeleton( SkeletonConfigToggles.SELF_LOCALIZATION -> localizer.setEnabled(newValue) SkeletonConfigToggles.USE_POSITION -> ikSolver.enabled = newValue + + SkeletonConfigToggles.ENFORCE_CONSTRAINTS -> enforceConstraints = newValue + + SkeletonConfigToggles.CORRECT_CONSTRAINTS -> correctConstraints = newValue } } diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index e569583098..4eaf726074 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -181,6 +181,7 @@ class TrackerResetsHandler(val tracker: Tracker) { rot = gyroFixNoMounting * rot rot *= attachmentFixNoMounting rot = yawFixZeroReference * rot + rot = dynamicFix * rot return rot } diff --git a/solarxr-protocol b/solarxr-protocol index 73fca6300d..e4d87c6d99 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit 73fca6300d70ee5e73258ce98a7dbae4d63b9cc8 +Subproject commit e4d87c6d99561d4da4ed93757aa01e203d0fc346 From 69d28ca59c82cb1a04f76c6bbd336d1176fadc5a Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Mon, 23 Sep 2024 13:11:56 -0700 Subject: [PATCH 62/68] lint --- gui/src/components/settings/pages/GeneralSettings.tsx | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gui/src/components/settings/pages/GeneralSettings.tsx b/gui/src/components/settings/pages/GeneralSettings.tsx index 1bbce39a0a..9265f291d3 100644 --- a/gui/src/components/settings/pages/GeneralSettings.tsx +++ b/gui/src/components/settings/pages/GeneralSettings.tsx @@ -1045,7 +1045,9 @@ export function GeneralSettings() {
- {l10n.getString('settings-general-fk_settings-enforce_joint_constraints')} + {l10n.getString( + 'settings-general-fk_settings-enforce_joint_constraints' + )} {l10n.getString( From 7d02d7d71e54a09360ce135f16eafce1f27a6e00 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Fri, 27 Sep 2024 14:56:25 -0700 Subject: [PATCH 63/68] fix IK breaking at 180 degree rotations relative to the parent --- .../config/SkeletonConfigToggles.java | 2 +- .../tracking/processor/skeleton/IKChain.kt | 34 +++++++++---------- .../tracking/processor/skeleton/IKSolver.kt | 13 ++----- 3 files changed, 20 insertions(+), 29 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java index f8b991cf9f..d27021c159 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java @@ -16,7 +16,7 @@ public enum SkeletonConfigToggles { TOE_SNAP(8, "Toe Snap", "toeSnap", false), FOOT_PLANT(9, "Foot Plant", "footPlant", true), SELF_LOCALIZATION(10, "Self Localization", "selfLocalization", false), - USE_POSITION(11, "Use Position", "usePosition", false), + USE_POSITION(11, "Use Position", "usePosition", true), ENFORCE_CONSTRAINTS(12, "Enforce Constraints", "enforceConstraints", true), CORRECT_CONSTRAINTS(13, "Correct Constraints", "correctConstraints", true),; diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index 3e0d8297eb..bd2c96e9b9 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -40,7 +40,7 @@ class IKChain( */ private fun prepBones() { for (i in 0.. ANNEALING_MAX)) - val solved = solve(ANNEALING_STEP - ANNEALING_ITERATIONS) - - if (solved) break - } + solve(MAX_ITERATIONS) root.update() } From c69d3011db337436103a50856e3178e890937ebe Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Tue, 15 Oct 2024 14:33:49 -0700 Subject: [PATCH 64/68] Adjust hand constraints and fix constraint corrections interaction with ik solver --- .../slimevr/tracking/processor/skeleton/HumanSkeleton.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 3cb0c1ed4a..4c0bf9f430 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -62,8 +62,8 @@ class HumanSkeleton( val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f)) val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f)) - val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f)) - val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 90f, 90f)) + val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 120f, 120f)) + val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 120f, 120f)) // Tracker bones val headTrackerBone = Bone(BoneType.HEAD_TRACKER, Constraint(ConstraintType.COMPLETE)) @@ -410,10 +410,10 @@ class HumanSkeleton( val parentTracker = getTrackerForBone(bone.parent?.boneType) if ((angle > 0.01f) && ( tracker?.filteringHandler?.getFilteringImpact() - ?: 0f + ?: 1f ) < 0.01f && ( parentTracker?.filteringHandler?.getFilteringImpact() - ?: 0f + ?: 1f ) < 0.01f ) { tracker?.resetsHandler?.updateDynamicFix(deltaRot) From 60b7b520e48d75a86cb9c9b19aff151cc582ecac Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Tue, 15 Oct 2024 16:02:24 -0700 Subject: [PATCH 65/68] Add target and end effector offset to improve solver stability --- .../tracking/processor/skeleton/IKChain.kt | 21 ++++++++++--------- .../tracking/processor/skeleton/IKSolver.kt | 10 ++++----- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index bd2c96e9b9..a564347647 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -5,7 +5,7 @@ import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType import dev.slimevr.tracking.trackers.Tracker import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 -import kotlin.math.* +import kotlin.math.pow /* * This class implements a chain of Bones @@ -40,7 +40,7 @@ class IKChain( */ private fun prepBones() { for (i in 0.. Date: Tue, 15 Oct 2024 16:32:16 -0700 Subject: [PATCH 66/68] spotless --- .../java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt | 1 - 1 file changed, 1 deletion(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index e0d4f59389..45b9263731 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -2,7 +2,6 @@ package dev.slimevr.tracking.processor.skeleton import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.trackers.Tracker -import solarxr_protocol.datatypes.BodyPart /* * Implements CCDIK (Cyclic Coordinate Descent Inverse Kinematics) to allow From 3701865e862baf9fbcf65c52cdef0e4a0e4e5292 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 30 Oct 2024 12:44:37 -0700 Subject: [PATCH 67/68] Always use constraints in iksolver --- .../slimevr/tracking/processor/Constraint.kt | 6 +++-- .../tracking/processor/skeleton/IKChain.kt | 23 +++++++++---------- .../tracking/processor/skeleton/IKSolver.kt | 12 +++++----- 3 files changed, 21 insertions(+), 20 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt index 808df90d49..7490c85886 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -18,11 +18,13 @@ class Constraint( twist: Float = 0.0f, swing: Float = 0.0f, allowedDeviation: Float = 0f, + maxDeviationFromTracker: Float = 15f, ) { 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() + private val maxDeviationFromTrackerRad = Math.toRadians(maxDeviationFromTracker.toDouble()).toFloat() var hasTrackerRotation = false /** @@ -45,8 +47,8 @@ class Constraint( 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) + swingQ = constrain(swingQ, maxDeviationFromTrackerRad) + twistQ = constrain(twistQ, maxDeviationFromTrackerRad) return initialRotation * (swingQ * twistQ) } diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt index a564347647..b34c930323 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -5,7 +5,7 @@ import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType import dev.slimevr.tracking.trackers.Tracker import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 -import kotlin.math.pow +import kotlin.math.* /* * This class implements a chain of Bones @@ -46,7 +46,7 @@ class IKChain( } } - fun backwardsCCDIK(useConstraints: Boolean) { + fun backwardsCCDIK() { target = computedTailPosition?.getPosition() ?: getChildTargetAvg() var offset = Vector3.NULL @@ -60,17 +60,16 @@ class IKChain( // Compute the axis of rotation and angle for this bone var scalar = IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.hasTrackerRotation) IKSolver.STATIC_DAMPENING else 1f scalar *= ((bones.size - i).toFloat() / bones.size).pow(IKSolver.ANNEALING_EXPONENT) - var adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit() + val adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit() - // Bones that are not supposed to be modified should tend towards their origin val rotation = currentBone.getGlobalRotation() + var correctedRot = (adjustment * rotation).unit() + + // Bones that are not supposed to be modified should tend towards their origin if (!currentBone.rotationConstraint.allowModifications) { - adjustment *= rotation.interpR(currentBone.rotationConstraint.initialRotation, IKSolver.CORRECTION_FACTOR) * rotation.inv() + correctedRot = correctedRot.interpR(currentBone.rotationConstraint.initialRotation, IKSolver.CORRECTION_FACTOR) } - - val correctedRot = (adjustment * rotation).unit() - - rotations[i] = setBoneRotation(currentBone, correctedRot, useConstraints) + rotations[i] = setBoneRotation(currentBone, correctedRot) if (currentBone.rotationConstraint.hasTrackerRotation) { offset += rotations[i].sandwich(Vector3.NEG_Y) * currentBone.length @@ -142,14 +141,14 @@ class IKChain( * to the bone's rotational constraint * returns the constrained rotation */ - private fun setBoneRotation(bone: Bone, rotation: Quaternion, useConstraints: Boolean): Quaternion { + private fun setBoneRotation(bone: Bone, rotation: Quaternion): Quaternion { // Constrain relative to the parent val newRotation = if (bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) { bone.rotationConstraint.applyConstraint(rotation, bone) - } else if (useConstraints && !bone.rotationConstraint.hasTrackerRotation) { + } else if (!bone.rotationConstraint.hasTrackerRotation) { bone.rotationConstraint.applyConstraint(rotation, bone) } else { - rotation + bone.rotationConstraint.constrainToInitialRotation(rotation) } bone.setRotationRaw(newRotation) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt index 45b9263731..ad18c2e90d 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKSolver.kt @@ -201,11 +201,11 @@ class IKSolver(private val root: Bone) { return null } - private fun solve(iterations: Int, useConstraints: Boolean = true): Boolean { - var solved: Boolean + private fun solve(iterations: Int): Boolean { + var solved = false for (i in 0..iterations) { for (chain in chainList) { - chain.backwardsCCDIK(useConstraints) + chain.backwardsCCDIK() } rootChain?.computeTargetDistance() @@ -215,14 +215,14 @@ class IKSolver(private val root: Bone) { for (chain in chainList) { if (chain.distToTargetSqr > TOLERANCE_SQR) { solved = false + break } } - // Terminate if using constraints and the chain is solved - if (solved && useConstraints) return true + if (solved) break } - return false + return solved } fun solve() { From 75c32891d92d1416dd519db3ca1a55cee7db6860 Mon Sep 17 00:00:00 2001 From: Collin Kees Date: Wed, 30 Oct 2024 12:54:40 -0700 Subject: [PATCH 68/68] Update solarxr-protocol --- solarxr-protocol | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/solarxr-protocol b/solarxr-protocol index e4d87c6d99..8c551cf634 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit e4d87c6d99561d4da4ed93757aa01e203d0fc346 +Subproject commit 8c551cf63452c0050372c2a81df31d78b2230812