save state before switching it CCDIK

SlimeVR · Jul 11, 2024 · 499535c · 499535c
1 parent 7bc1ec5
commit 499535c
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 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
@@ -43,8 +43,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))
@@ -53,8 +53,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
@@ -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
@@ -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()