AutoBone bone contribution fix & cleanup (#1249)

server/core/src/main/java/dev/slimevr/autobone/AutoBone.kt

@@ -6,6 +6,7 @@ import dev.slimevr.autobone.errors.*
 import dev.slimevr.config.AutoBoneConfig
 import dev.slimevr.poseframeformat.PoseFrameIO
 import dev.slimevr.poseframeformat.PoseFrames
+import dev.slimevr.tracking.processor.BoneType
 import dev.slimevr.tracking.processor.HumanPoseManager
 import dev.slimevr.tracking.processor.config.SkeletonConfigManager
 import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets
@@ -94,38 +95,73 @@ class AutoBone(server: VRServer) {
 		}
 	}
 
-	fun getBoneDirection(
+	/**
+	 * Computes the local tail position of the bone after rotation.
+	 */
+	fun getBoneLocalTail(
 		skeleton: HumanPoseManager,
-		configOffset: SkeletonConfigOffsets,
-		rightSide: Boolean,
+		boneType: BoneType,
 	): Vector3 {
-		// IMPORTANT: This assumption for acquiring BoneType only works if
-		// SkeletonConfigOffsets is set up to only affect one BoneType, make sure no
-		// changes to SkeletonConfigOffsets goes against this assumption, please!
-		val boneType = when (configOffset) {
-			SkeletonConfigOffsets.HIPS_WIDTH, SkeletonConfigOffsets.SHOULDERS_WIDTH,
-			SkeletonConfigOffsets.SHOULDERS_DISTANCE, SkeletonConfigOffsets.UPPER_ARM,
-			SkeletonConfigOffsets.LOWER_ARM, SkeletonConfigOffsets.UPPER_LEG,
-			SkeletonConfigOffsets.LOWER_LEG, SkeletonConfigOffsets.FOOT_LENGTH,
-			->
-				if (rightSide) configOffset.affectedOffsets[1] else configOffset.affectedOffsets[0]
-
-			else -> configOffset.affectedOffsets[0]
-		}
-		return skeleton.getBone(boneType).getGlobalRotation().toRotationVector()
+		val bone = skeleton.getBone(boneType)
+		return bone.getTailPosition() - bone.getPosition()
 	}
 
-	fun getDotProductDiff(
+	/**
+	 * Computes the direction of the bone tail's movement between skeletons 1 and 2.
+	 */
+	fun getBoneLocalTailDir(
 		skeleton1: HumanPoseManager,
 		skeleton2: HumanPoseManager,
-		configOffset: SkeletonConfigOffsets,
-		rightSide: Boolean,
-		offset: Vector3,
+		boneType: BoneType,
+	): Vector3? {
+		val boneOff = getBoneLocalTail(skeleton2, boneType) - getBoneLocalTail(skeleton1, boneType)
+		val boneOffLen = boneOff.len()
+		return if (boneOffLen > MIN_SLIDE_DIST) boneOff / boneOffLen else null
+	}
+
+	/**
+	 * Predicts how much the provided config should be affecting the slide offsets
+	 * of the left and right ankles.
+	 */
+	fun getSlideDot(
+		skeleton1: HumanPoseManager,
+		skeleton2: HumanPoseManager,
+		config: SkeletonConfigOffsets,
+		slideL: Vector3?,
+		slideR: Vector3?,
 	): Float {
-		val normalizedOffset = offset.unit()
-		val dot1 = normalizedOffset.dot(getBoneDirection(skeleton1, configOffset, rightSide))
-		val dot2 = normalizedOffset.dot(getBoneDirection(skeleton2, configOffset, rightSide))
-		return dot2 - dot1
+		var slideDot = 0f
+		// Used for right offset if not a symmetric bone
+		var boneOffL: Vector3? = null
+
+		if (slideL != null) {
+			boneOffL = getBoneLocalTailDir(skeleton1, skeleton2, config.affectedOffsets[0])
+
+			if (boneOffL != null) {
+				slideDot += slideL.dot(boneOffL)
+			}
+		}
+
+		if (slideR != null) {
+			// IMPORTANT: This assumption for acquiring BoneType only works if
+			// SkeletonConfigOffsets is set up to only affect one BoneType, make sure no
+			// changes to SkeletonConfigOffsets goes against this assumption, please!
+			val boneOffR = if (SYMM_CONFIGS.contains(config)) {
+				getBoneLocalTailDir(skeleton1, skeleton2, config.affectedOffsets[1])
+			} else if (slideL != null) {
+				// Use cached offset if slideL was used
+				boneOffL
+			} else {
+				// Compute offset if missing because of slideL
+				getBoneLocalTailDir(skeleton1, skeleton2, config.affectedOffsets[0])
+			}
+
+			if (boneOffR != null) {
+				slideDot += slideR.dot(boneOffR)
+			}
+		}
+
+		return slideDot / 2f
 	}
 
 	fun applyConfig(
@@ -488,13 +524,15 @@ class AutoBone(server: VRServer) {
 			return
 		}
 
-		val slideLeft = skeleton2
-			.getComputedTracker(TrackerRole.LEFT_FOOT).position -
+		val slideL = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position -
 			skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position
+		val slideLLen = slideL.len()
+		val slideLUnit: Vector3? = if (slideLLen > MIN_SLIDE_DIST) slideL / slideLLen else null
 
-		val slideRight = skeleton2
-			.getComputedTracker(TrackerRole.RIGHT_FOOT).position -
+		val slideR = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position -
 			skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position
+		val slideRLen = slideR.len()
+		val slideRUnit: Vector3? = if (slideRLen > MIN_SLIDE_DIST) slideR / slideRLen else null
 
 		val intermediateOffsets = EnumMap(offsets)
 		for (entry in intermediateOffsets.entries) {
@@ -505,28 +543,23 @@ class AutoBone(server: VRServer) {
 			}
 			val originalLength = entry.value
 
-			val leftDotProduct = getDotProductDiff(
-				skeleton1,
-				skeleton2,
-				entry.key,
-				false,
-				slideLeft,
-			)
-			val rightDotProduct = getDotProductDiff(
-				skeleton1,
-				skeleton2,
-				entry.key,
-				true,
-				slideRight,
-			)
-
 			// Calculate the total effect of the bone based on change in rotation
-			val dotLength = originalLength * ((leftDotProduct + rightDotProduct) / 2f)
+			val slideDot = getSlideDot(
+				skeleton1,
+				skeleton2,
+				entry.key,
+				slideLUnit,
+				slideRUnit,
+			)
+			val dotLength = originalLength * slideDot
 
 			// Scale by the total effect of the bone
 			val curAdjustVal = adjustVal * -dotLength
-			val newLength = originalLength + curAdjustVal
+			if (curAdjustVal == 0f) {
+				continue
+			}
 
+			val newLength = originalLength + curAdjustVal
 			// No small or negative numbers!!! Bad algorithm!
 			if (newLength < 0.01f) {
 				continue
@@ -754,6 +787,7 @@ class AutoBone(server: VRServer) {
 
 	companion object {
 		const val MIN_HEIGHT = 0.4f
+		const val MIN_SLIDE_DIST = 0.002f
 		const val AUTOBONE_FOLDER = "AutoBone Recordings"
 		const val LOADAUTOBONE_FOLDER = "Load AutoBone Recordings"
 
@@ -773,5 +807,16 @@ class AutoBone(server: VRServer) {
 		private fun errorFunc(errorDeriv: Float): Float = 0.5f * (errorDeriv * errorDeriv)
 
 		private fun decayFunc(initialAdjustRate: Float, adjustRateDecay: Float, epoch: Int): Float = if (epoch >= 0) initialAdjustRate / (1 + (adjustRateDecay * epoch)) else 0.0f
+
+		private val SYMM_CONFIGS = arrayOf(
+			SkeletonConfigOffsets.HIPS_WIDTH,
+			SkeletonConfigOffsets.SHOULDERS_WIDTH,
+			SkeletonConfigOffsets.SHOULDERS_DISTANCE,
+			SkeletonConfigOffsets.UPPER_ARM,
+			SkeletonConfigOffsets.LOWER_ARM,
+			SkeletonConfigOffsets.UPPER_LEG,
+			SkeletonConfigOffsets.LOWER_LEG,
+			SkeletonConfigOffsets.FOOT_LENGTH,
+		)
 	}
 }

server/core/src/main/java/dev/slimevr/autobone/errors/PositionError.kt

@@ -39,10 +39,14 @@ class PositionError : IAutoBoneError {
 				val position = trackerFrame.tryGetPosition() ?: continue
 				val trackerRole = trackerFrame.tryGetTrackerPosition()?.trackerRole ?: continue
 
-				val computedTracker = skeleton.getComputedTracker(trackerRole) ?: continue
+				try {
+					val computedTracker = skeleton.getComputedTracker(trackerRole)
 
-				offset += (position - computedTracker.position).len()
-				offsetCount++
+					offset += (position - computedTracker.position).len()
+					offsetCount++
+				} catch (_: Exception) {
+					// Ignore unsupported positions
+				}
 			}
 			return if (offsetCount > 0) offset / offsetCount else 0f
 		}

server/core/src/main/java/dev/slimevr/autobone/errors/PositionOffsetError.kt

@@ -37,13 +37,17 @@ class PositionOffsetError : IAutoBoneError {
 			val position2 = trackerFrame2.tryGetPosition() ?: continue
 			val trackerRole2 = trackerFrame2.tryGetTrackerPosition()?.trackerRole ?: continue
 
-			val computedTracker1 = skeleton1.getComputedTracker(trackerRole1) ?: continue
-			val computedTracker2 = skeleton2.getComputedTracker(trackerRole2) ?: continue
+			try {
+				val computedTracker1 = skeleton1.getComputedTracker(trackerRole1)
+				val computedTracker2 = skeleton2.getComputedTracker(trackerRole2)
 
-			val dist1 = (position1 - computedTracker1.position).len()
-			val dist2 = (position2 - computedTracker2.position).len()
-			offset += abs(dist2 - dist1)
-			offsetCount++
+				val dist1 = (position1 - computedTracker1.position).len()
+				val dist2 = (position2 - computedTracker2.position).len()
+				offset += abs(dist2 - dist1)
+				offsetCount++
+			} catch (_: Exception) {
+				// Ignore unsupported positions
+			}
 		}
 		return if (offsetCount > 0) offset / offsetCount else 0f
 	}

server/core/src/main/java/dev/slimevr/config/AutoBoneConfig.kt

@@ -4,14 +4,14 @@ class AutoBoneConfig {
 	var cursorIncrement = 2
 	var minDataDistance = 1
 	var maxDataDistance = 1
-	var numEpochs = 100
+	var numEpochs = 50
 	var printEveryNumEpochs = 25
 	var initialAdjustRate = 10.0f
 	var adjustRateDecay = 1.0f
-	var slideErrorFactor = 0.0f
-	var offsetSlideErrorFactor = 1.0f
+	var slideErrorFactor = 1.0f
+	var offsetSlideErrorFactor = 0.0f
 	var footHeightOffsetErrorFactor = 0.0f
-	var bodyProportionErrorFactor = 0.25f
+	var bodyProportionErrorFactor = 0.05f
 	var heightErrorFactor = 0.0f
 	var positionErrorFactor = 0.0f
 	var positionOffsetErrorFactor = 0.0f

server/core/src/main/java/dev/slimevr/config/CurrentVRConfigConverter.java

@@ -304,6 +304,32 @@ public class CurrentVRConfigConverter implements VersionedModelConverter {
 					}
 				}
 			}
+
+			if (version < 14) {
+				// Update AutoBone defaults
+				ObjectNode autoBoneNode = (ObjectNode) modelData.get("autoBone");
+				if (autoBoneNode != null) {
+					JsonNode offsetSlideNode = autoBoneNode.get("offsetSlideErrorFactor");
+					JsonNode slideNode = autoBoneNode.get("slideErrorFactor");
+					if (
+						offsetSlideNode != null
+							&& slideNode != null
+							&& offsetSlideNode.floatValue() == 1.0f
+							&& slideNode.floatValue() == 0.0f
+					) {
+						autoBoneNode.set("offsetSlideErrorFactor", new FloatNode(0.0f));
+						autoBoneNode.set("slideErrorFactor", new FloatNode(1.0f));
+					}
+					JsonNode bodyProportionsNode = autoBoneNode.get("bodyProportionErrorFactor");
+					if (bodyProportionsNode != null && bodyProportionsNode.floatValue() == 0.25f) {
+						autoBoneNode.set("bodyProportionErrorFactor", new FloatNode(0.05f));
+					}
+					JsonNode numEpochsNode = autoBoneNode.get("numEpochs");
+					if (numEpochsNode != null && numEpochsNode.intValue() == 100) {
+						autoBoneNode.set("numEpochs", new IntNode(50));
+					}
+				}
+			}
 		} catch (Exception e) {
 			LogManager.severe("Error during config migration: " + e);
 		}

server/core/src/main/java/dev/slimevr/config/VRConfig.kt

@@ -10,8 +10,8 @@ import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.TrackerRole
 
 @JsonVersionedModel(
-	currentVersion = "13",
-	defaultDeserializeToVersion = "13",
+	currentVersion = "14",
+	defaultDeserializeToVersion = "14",
 	toCurrentConverterClass = CurrentVRConfigConverter::class,
 )
 class VRConfig {