diff --git a/gui/public/i18n/en/translation.ftl b/gui/public/i18n/en/translation.ftl index 4125361270..c5bf85dfe9 100644 --- a/gui/public/i18n/en/translation.ftl +++ b/gui/public/i18n/en/translation.ftl @@ -396,6 +396,14 @@ 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. 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..9265f291d3 100644 --- a/gui/src/components/settings/pages/GeneralSettings.tsx +++ b/gui/src/components/settings/pages/GeneralSettings.tsx @@ -69,6 +69,9 @@ interface SettingsForm { toeSnap: boolean; footPlant: boolean; selfLocalization: boolean; + usePosition: boolean; + enforceConstraints: boolean; + correctConstraints: boolean; }; ratios: { imputeWaistFromChestHip: number; @@ -128,6 +131,9 @@ const defaultValues: SettingsForm = { toeSnap: false, footPlant: true, selfLocalization: false, + usePosition: true, + enforceConstraints: true, + correctConstraints: true, }, ratios: { imputeWaistFromChestHip: 0.3, @@ -235,6 +241,9 @@ export function GeneralSettings() { toggles.toeSnap = values.toggles.toeSnap; 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; } @@ -981,6 +990,7 @@ export function GeneralSettings() { )} /> + {l10n.getString( 'settings-general-fk_settings-arm_fk-reset_mode-description' @@ -1033,6 +1043,70 @@ 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/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/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/protocol/rpc/settings/RPCSettingsBuilder.java b/server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java index c7de6f5512..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), - false + 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 dae1e2b106..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 @@ -256,6 +256,9 @@ 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()) + 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/Bone.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt index 3a291f49a4..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 @@ -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 @@ -58,6 +58,14 @@ class Bone(val boneType: BoneType) { headNode.update() } + /** + * Computes the rotations and positions of this bone. + */ + fun updateThisNode() { + headNode.updateThisNode() + tailNode.updateThisNode() + } + /** * Returns the world-aligned rotation of the bone */ @@ -75,6 +83,13 @@ class Bone(val boneType: BoneType) { headNode.localTransform.rotation = rotation * rotationOffset } + /** + * Sets the global rotation of the bone directly + */ + 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/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/Constraint.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt new file mode 100644 index 0000000000..7490c85886 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -0,0 +1,183 @@ +package dev.slimevr.tracking.processor + +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import kotlin.math.* + +/** + * Represents a function that applies a rotational constraint. + */ +typealias ConstraintFunction = (inputRotation: Quaternion, thisBone: Bone, limit1: Float, limit2: Float, limit3: Float) -> Quaternion + +/** + * 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, + 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 + + /** + * If false solve with minimal movement applied to this link + */ + 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, allowedDeviationRad).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, maxDeviationFromTrackerRad) + twistQ = constrain(twistQ, maxDeviationFromTrackerRad) + return initialRotation * (swingQ * twistQ) + } + + companion object { + enum class ConstraintType { + TWIST_SWING, + HINGE, + LOOSE_HINGE, + COMPLETE, + } + + private fun constraintTypeToFunc(type: ConstraintType) = + when (type) { + ConstraintType.COMPLETE -> completeConstraint + ConstraintType.TWIST_SWING -> twistSwingConstraint + ConstraintType.HINGE -> hingeConstraint + ConstraintType.LOOSE_HINGE -> looseHingeConstraint + } + + private fun decompose( + rotation: Quaternion, + twistAxis: Vector3, + ): Pair { + 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) + } + + private fun constrain(rotation: Quaternion, angle: Float): Quaternion { + val magnitude = sin(angle * 0.5f) + val magnitudeSqr = magnitude * magnitude + 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) + } + + return rot.unit() + } + + 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) 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) * 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)) + + 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) + } + + return rot.unit() + } + + // Constraint function for TwistSwingConstraint + private val twistSwingConstraint: ConstraintFunction = + { rotation: Quaternion, thisBone: Bone, swingRad: Float, twistRad: Float, _: 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 (swingQ, twistQ) = decompose(rotationLocal, Vector3.NEG_Y) + + swingQ = constrain(swingQ, swingRad) + twistQ = constrain(twistQ, twistRad) + + (parent.getGlobalRotation() * localRotationOffset * (swingQ * twistQ)).unit() + } + } + + // Constraint function for a hinge constraint with min and max angles + private val hingeConstraint: ConstraintFunction = + { rotation: Quaternion, thisBone: Bone, min: Float, max: Float, _: 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 (_, hingeAxisRot) = decompose(rotationLocal, Vector3.NEG_X) + + hingeAxisRot = constrain(hingeAxisRot, min, max, Vector3.NEG_X) + + (parent.getGlobalRotation() * localRotationOffset * hingeAxisRot).unit() + } + } + + // 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, _: Float -> + thisBone.getGlobalRotation() + } + } +} 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/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/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/config/SkeletonConfigToggles.java b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java index cffec046b0..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 @@ -15,7 +15,10 @@ 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), + 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 ac8fc42aaa..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 @@ -4,6 +4,8 @@ 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.Constraint +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 @@ -34,45 +36,47 @@ 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, 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, 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) - 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, 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, 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)) + val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) // 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 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, 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, 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, 120f, 120f)) + val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 120f, 120f)) // 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, 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 @@ -129,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 @@ -147,6 +153,7 @@ class HumanSkeleton( var tapDetectionManager = TapDetectionManager(this) var viveEmulation = ViveEmulation(this) var localizer = Localizer(this) + var ikSolver = IKSolver(headBone) // Constructors init { @@ -231,8 +238,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) @@ -304,6 +313,9 @@ class HumanSkeleton( // Update tap detection's trackers tapDetectionManager.updateConfig(trackers) + + // Rebuild Ik Solver + ikSolver.buildChains(trackers) } /** @@ -362,6 +374,10 @@ class HumanSkeleton( updateTransforms() updateBones() + enforceConstraints() + + if (!pauseTracking) ikSolver.solve() + updateComputedTrackers() // Don't run post-processing if the tracking is paused @@ -372,6 +388,39 @@ class HumanSkeleton( viveEmulation.update() } + /** + * 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 (!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() + val angle = deltaRot.angleR() + val tracker = getTrackerForBone(bone.boneType) + val parentTracker = getTrackerForBone(bone.parent?.boneType) + if ((angle > 0.01f) && ( + tracker?.filteringHandler?.getFilteringImpact() + ?: 1f + ) < 0.01f && ( + parentTracker?.filteringHandler?.getFilteringImpact() + ?: 1f + ) < 0.01f + ) { + tracker?.resetsHandler?.updateDynamicFix(deltaRot) + } + } + } + /** * Update all the bones by updating the roots */ @@ -419,6 +468,7 @@ class HumanSkeleton( // Left arm updateArmTransforms( isTrackingLeftArmFromController, + leftUpperShoulderBone, leftShoulderBone, leftUpperArmBone, leftElbowTrackerBone, @@ -433,6 +483,7 @@ class HumanSkeleton( // Right arm updateArmTransforms( isTrackingRightArmFromController, + rightUpperShoulderBone, rightShoulderBone, rightUpperArmBone, rightElbowTrackerBone, @@ -706,6 +757,7 @@ class HumanSkeleton( */ private fun updateArmTransforms( isTrackingFromController: Boolean, + upperShoulderBone: Bone, shoulderBone: Bone, upperArmBone: Bone, elbowTrackerBone: Bone, @@ -738,6 +790,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) { @@ -866,6 +919,12 @@ class HumanSkeleton( SkeletonConfigToggles.FOOT_PLANT -> legTweaks.footPlantEnabled = newValue SkeletonConfigToggles.SELF_LOCALIZATION -> localizer.setEnabled(newValue) + + SkeletonConfigToggles.USE_POSITION -> ikSolver.enabled = newValue + + SkeletonConfigToggles.ENFORCE_CONSTRAINTS -> enforceConstraints = newValue + + SkeletonConfigToggles.CORRECT_CONSTRAINTS -> correctConstraints = newValue } } @@ -947,6 +1006,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 @@ -961,6 +1022,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. */ @@ -980,6 +1065,8 @@ class HumanSkeleton( rightLowerLegBone, leftFootBone, rightFootBone, + leftUpperShoulderBone, + rightUpperShoulderBone, leftShoulderBone, rightShoulderBone, leftUpperArmBone, @@ -995,6 +1082,8 @@ class HumanSkeleton( */ private val allArmBones: Array get() = arrayOf( + leftUpperShoulderBone, + rightUpperShoulderBone, leftShoulderBone, rightShoulderBone, leftUpperArmBone, @@ -1077,6 +1166,7 @@ class HumanSkeleton( } legTweaks.resetBuffer() localizer.reset() + ikSolver.resetOffsets() LogManager.info("[HumanSkeleton] Reset: full ($resetSourceName)") } @@ -1210,6 +1300,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 new file mode 100644 index 0000000000..b34c930323 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/IKChain.kt @@ -0,0 +1,159 @@ +package dev.slimevr.tracking.processor.skeleton + +import dev.slimevr.tracking.processor.Bone +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.* + +/* + * This class implements a chain of Bones + */ + +class IKChain( + val bones: MutableList, + var parent: IKChain?, + val level: Int, + val baseConstraint: Tracker?, + val tailConstraint: Tracker?, +) { + // State variables + private val computedBasePosition = baseConstraint?.let { IKConstraint(it) } + private val computedTailPosition = tailConstraint?.let { IKConstraint(it) } + var children = mutableListOf() + var target = Vector3.NULL + var distToTargetSqr = Float.POSITIVE_INFINITY + private var rotations = getRotationsList() + + 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..() + private var rootChain: IKChain? = null + private var needsReset = false + + /** + * Any time the skeleton is rebuilt or trackers are assigned / unassigned the chains + * should be rebuilt. + */ + fun buildChains(trackers: List) { + chainList.clear() + + val positionalConstraints = extractPositionalConstraints(trackers) + val rotationalConstraints = extractRotationalConstraints(trackers) + + 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 + chainList.sortBy { -it.level } + } + + /** + * Reset the offsets of positional trackers. + */ + 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 + * 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 + var constraint = getConstraint(currentBone, rotationalConstraints) + currentBone.rotationConstraint.allowModifications = constraint == null + currentBone.rotationConstraint.hasTrackerRotation = constraint != null + boneList.add(currentBone) + + // Get constraints + val baseConstraint = if (parent == null) { + getConstraint(boneList.first(), positionalConstraints) + } else { + parent.tailConstraint + } + 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.first() + constraint = getConstraint(currentBone, rotationalConstraints) + currentBone.rotationConstraint.allowModifications = constraint == null + currentBone.rotationConstraint.hasTrackerRotation = constraint != null + boneList.add(currentBone) + tailConstraint = getConstraint(currentBone, positionalConstraints) + } + + var chain = IKChain(boneList, parent, level, baseConstraint, tailConstraint) + + if (currentBone.children.isNotEmpty()) { + // Build child chains + val childrenList = mutableListOf() + for (child in currentBone.children) { + val childChain = chainBuilder(child, chain, level + 1, positionalConstraints, rotationalConstraints) + if (neededChain(childChain)) { + childrenList.add(childChain) + } + } + chain.children = childrenList + } + + // 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.first()) + } + + 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.bones) + boneList.addAll(childChain.bones) + + 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 + } + + private fun addConstraints() { + fun constrainChain(chain: IKChain) { + chain.bones.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 + } + + for (c in chain.children) { + if (c.tailConstraint != null) { + return true + } + + if (neededChain(c)) { + return true + } + } + + return false + } + + private fun extractPositionalConstraints(trackers: List): MutableList { + val constraintList = mutableListOf() + for (t in trackers) { + if (t.hasPosition && + !t.isInternal && + !t.status.reset + ) { + constraintList.add(t) + } + } + 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 (c.trackerPosition != null && bone.boneType.bodyPart == (c.trackerPosition?.bodyPart ?: 0)) { + constraints.remove(c) + return c + } + } + return null + } + + private fun solve(iterations: Int): Boolean { + var solved = false + for (i in 0..iterations) { + for (chain in chainList) { + chain.backwardsCCDIK() + } + + 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 + break + } + } + + if (solved) break + } + + return solved + } + + fun solve() { + if (rootChain == null || !enabled) return + + if (needsReset) { + for (c in chainList) { + c.resetTrackerOffsets() + } + needsReset = false + } + + rootChain?.resetChain() + root.update() + + solve(MAX_ITERATIONS) + + root.update() + } +} 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..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 @@ -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 * rot return rot } @@ -179,6 +181,7 @@ class TrackerResetsHandler(val tracker: Tracker) { rot = gyroFixNoMounting * rot rot *= attachmentFixNoMounting rot = yawFixZeroReference * rot + rot = dynamicFix * rot return rot } @@ -213,6 +216,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 +294,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 +330,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 +376,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 } diff --git a/solarxr-protocol b/solarxr-protocol index 73fca6300d..8c551cf634 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit 73fca6300d70ee5e73258ce98a7dbae4d63b9cc8 +Subproject commit 8c551cf63452c0050372c2a81df31d78b2230812