Add upper elbow

SlimeVR · Sep 1, 2024 · 4567816 · 4567816
1 parent c193a2d
commit 4567816
Show file tree

Hide file tree

Showing 5 changed files with 39 additions and 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
@@ -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
@@ -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
@@ -53,8 +53,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))
@@ -233,8 +235,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)
@@ -427,6 +431,7 @@ class HumanSkeleton(
 		// Left arm
 		updateArmTransforms(
 			isTrackingLeftArmFromController,
+			leftUpperShoulderBone,
 			leftShoulderBone,
 			leftUpperArmBone,
 			leftElbowTrackerBone,
@@ -441,6 +446,7 @@ class HumanSkeleton(
 		// Right arm
 		updateArmTransforms(
 			isTrackingRightArmFromController,
+			rightUpperShoulderBone,
 			rightShoulderBone,
 			rightUpperArmBone,
 			rightElbowTrackerBone,
@@ -714,6 +720,7 @@ class HumanSkeleton(
 	 */
 	private fun updateArmTransforms(
 		isTrackingFromController: Boolean,
+		upperShoulderBone: Bone,
 		shoulderBone: Bone,
 		upperArmBone: Bone,
 		elbowTrackerBone: Bone,
@@ -746,6 +753,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) {
@@ -957,6 +965,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
@@ -1005,6 +1015,8 @@ class HumanSkeleton(
 	 */
 	private val allArmBones: Array<Bone>
 		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
@@ -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
@@ -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