diff --git a/gui/public/i18n/en/translation.ftl b/gui/public/i18n/en/translation.ftl index c86ac7e403..ac9a714a39 100644 --- a/gui/public/i18n/en/translation.ftl +++ b/gui/public/i18n/en/translation.ftl @@ -450,6 +450,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 past their limit 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..3946b551fc 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,48 @@ 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' + )} + +
+
+ +
+ {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 112121606b..bbcce83814 100644 --- a/server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt +++ b/server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt @@ -36,7 +36,7 @@ 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) } 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 09517e692b..479263ba1f 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,8 @@ class QuaternionMovingAverage( var amount: Float = 0f, initialRotation: Quaternion = IDENTITY, ) { + var filteredQuaternion = IDENTITY + var filteringImpact = 0f private var smoothFactor = 0f private var predictFactor = 0f private var rotBuffer: CircularArrayList? = null @@ -29,7 +31,6 @@ class QuaternionMovingAverage( private val fpsTimer = if (VRServer.instanceInitialized) VRServer.instance.fpsTimer else NanoTimer() private var frameCounter = 0 private var lastAmt = 0f - var filteredQuaternion = IDENTITY init { // amount should range from 0 to 1. @@ -93,6 +94,8 @@ class QuaternionMovingAverage( // No filtering; just keep track of rotations (for going over 180 degrees) filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion) } + + 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 6e0bf02855..5e38d571ec 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 @@ -190,8 +190,8 @@ public static int createModelSettings( humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT), humanPoseManager.getToggle(SkeletonConfigToggles.SELF_LOCALIZATION), false, - true, - true + 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 13bce643ab..9f69fd4052 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 @@ -258,6 +258,8 @@ 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.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..ee2ce87d4c 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 @@ -1,19 +1,23 @@ package dev.slimevr.tracking.processor +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 solarxr_protocol.datatypes.BodyPart 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 private set val children: MutableList = CopyOnWriteArrayList() var rotationOffset = Quaternion.IDENTITY + var attachedTracker: Tracker? = null init { headNode.attachChild(tailNode) @@ -58,6 +62,49 @@ class Bone(val boneType: BoneType) { headNode.update() } + /** + * Computes the rotations and positions of + * this bone and all of its children while + * enforcing rotation constraints. + */ + fun updateWithConstraints() { + val initialRot = getGlobalRotation() + val newRot = rotationConstraint.applyConstraint(initialRot, this) + setRotationRaw(newRot) + updateThisNode() + + // Correct tracker if applicable. Do not adjust correction for hinge constraints + // or the upper chest tracker. + if (rotationConstraint.constraintType != ConstraintType.HINGE && + rotationConstraint.constraintType != ConstraintType.LOOSE_HINGE && + boneType.bodyPart != BodyPart.UPPER_CHEST + ) { + val deltaRot = newRot * initialRot.inv() + val angle = deltaRot.angleR() + + if (angle > Constraint.ANGLE_THRESHOLD && + (attachedTracker?.filteringHandler?.getFilteringImpact() ?: 1f) < Constraint.FILTER_IMPACT_THRESHOLD && + (parent?.attachedTracker?.filteringHandler?.getFilteringImpact() ?: 0f) < Constraint.FILTER_IMPACT_THRESHOLD + ) { + attachedTracker?.resetsHandler?.updateConstraintFix(deltaRot) + } + } + + // Recursively apply constraints and update children. + for (child in children) { + child.updateWithConstraints() + } + } + + /** + * Computes the rotations and positions of this bone. + * Only to be used while traversing bones from top to bottom. + */ + private fun updateThisNode() { + headNode.updateThisNode() + tailNode.updateThisNode() + } + /** * Returns the world-aligned rotation of the bone */ @@ -75,6 +122,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 0e263992bd..b423640103 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..91c6d34ff1 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/Constraint.kt @@ -0,0 +1,191 @@ +package dev.slimevr.tracking.processor + +import com.jme3.math.FastMath +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 = (localRotation: Quaternion, 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. + * Twist, swing, allowedDeviation, and maxDeviationFromTracker represent + * an angle in degrees. + */ +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 = twist * FastMath.DEG_TO_RAD + private val swingRad = swing * FastMath.DEG_TO_RAD + private val allowedDeviationRad = allowedDeviation * FastMath.DEG_TO_RAD + private val maxDeviationFromTrackerRad = maxDeviationFromTracker * FastMath.DEG_TO_RAD + + /** + * allowModification may be false for reasons other than a tracker being on this bone + * while hasTrackerRotation is only true if this bone has a tracker. These values are + * to be used with an IK solver and are not currently set accurately + */ + var allowModifications = true + var hasTrackerRotation = false + + /** + * The rotation before any IK solve takes place. Again this value is not currently set accurately + */ + 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 { + // When constraints are being used during a IK solve the input rotation is not necessarily + // the bones global rotation, thus complete constraints must be specifically handled. + if (constraintType == ConstraintType.COMPLETE) return thisBone.getGlobalRotation() + + // If there is no parent and this is not a complete constraint accept the rotation as is. + if (thisBone.parent == null) return rotation + + val localRotation = getLocalRotation(rotation, thisBone) + val constrainedRotation = constraintFunction(localRotation, swingRad, twistRad, allowedDeviationRad) + return getWorldRotationFromLocal(constrainedRotation, thisBone) + } + + /** + * 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 { + const val ANGLE_THRESHOLD = 0.004f // == 0.25 degrees + const val FILTER_IMPACT_THRESHOLD = 0.0349f // == 2 degrees + + 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 getLocalRotation(rotation: Quaternion, thisBone: Bone): Quaternion { + val parent = thisBone.parent!! + val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset + return (parent.getGlobalRotation() * localRotationOffset).inv() * rotation + } + + private fun getWorldRotationFromLocal(rotation: Quaternion, thisBone: Bone): Quaternion { + val parent = thisBone.parent!! + val localRotationOffset = parent.rotationOffset.inv() * thisBone.rotationOffset + return (parent.getGlobalRotation() * localRotationOffset * rotation).unit() + } + + private fun decompose( + rotation: Quaternion, + twistAxis: Vector3, + ): Pair { + val projection = rotation.project(twistAxis).unit() + 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 { + // Use angle to get the maximum magnitude the vector part of rotation can be + // before it has violated a constraint. + // Multiplying by 0.5 uniquely maps angles 0-180 degrees to 0-1 which works + // nicely with unit quaternions. + 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, swingRad: Float, twistRad: Float, _: Float -> + var (swingQ, twistQ) = decompose(rotation, Vector3.NEG_Y) + swingQ = constrain(swingQ, swingRad) + twistQ = constrain(twistQ, twistRad) + + swingQ * twistQ + } + + // Constraint function for a hinge constraint with min and max angles + private val hingeConstraint: ConstraintFunction = + { rotation: Quaternion, min: Float, max: Float, _: Float -> + val (_, hingeAxisRot) = decompose(rotation, Vector3.NEG_X) + + constrain(hingeAxisRot, min, max, Vector3.NEG_X) + } + + // 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, min: Float, max: Float, nonHingeDeviation: Float -> + var (nonHingeRot, hingeAxisRot) = decompose(rotation, Vector3.NEG_X) + hingeAxisRot = constrain(hingeAxisRot, min, max, Vector3.NEG_X) + nonHingeRot = constrain(nonHingeRot, nonHingeDeviation) + + nonHingeRot * hingeAxisRot + } + + // Constraint function for CompleteConstraint + private val completeConstraint: ConstraintFunction = { rotation: Quaternion, _: Float, _: Float, _: Float -> + rotation + } + } +} 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 66fb43ea17..2d26a447b7 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 c500ea2632..33295f5aa2 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 @@ -35,77 +37,79 @@ 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_UPPER_SHOULDER, Constraint(ConstraintType.COMPLETE)) + val rightUpperShoulderBone = Bone(BoneType.RIGHT_UPPER_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)) // Finger bones - val leftThumbMetacarpalBone = Bone(BoneType.LEFT_THUMB_METACARPAL) - val leftThumbProximalBone = Bone(BoneType.LEFT_THUMB_PROXIMAL) - val leftThumbDistalBone = Bone(BoneType.LEFT_THUMB_DISTAL) - val leftIndexProximalBone = Bone(BoneType.LEFT_INDEX_PROXIMAL) - val leftIndexIntermediateBone = Bone(BoneType.LEFT_INDEX_INTERMEDIATE) - val leftIndexDistalBone = Bone(BoneType.LEFT_INDEX_DISTAL) - val leftMiddleProximalBone = Bone(BoneType.LEFT_MIDDLE_PROXIMAL) - val leftMiddleIntermediateBone = Bone(BoneType.LEFT_MIDDLE_INTERMEDIATE) - val leftMiddleDistalBone = Bone(BoneType.LEFT_MIDDLE_DISTAL) - val leftRingProximalBone = Bone(BoneType.LEFT_RING_PROXIMAL) - val leftRingIntermediateBone = Bone(BoneType.LEFT_RING_INTERMEDIATE) - val leftRingDistalBone = Bone(BoneType.LEFT_RING_DISTAL) - val leftLittleProximalBone = Bone(BoneType.LEFT_LITTLE_PROXIMAL) - val leftLittleIntermediateBone = Bone(BoneType.LEFT_LITTLE_INTERMEDIATE) - val leftLittleDistalBone = Bone(BoneType.LEFT_LITTLE_DISTAL) - val rightThumbMetacarpalBone = Bone(BoneType.RIGHT_THUMB_METACARPAL) - val rightThumbProximalBone = Bone(BoneType.RIGHT_THUMB_PROXIMAL) - val rightThumbDistalBone = Bone(BoneType.RIGHT_THUMB_DISTAL) - val rightIndexProximalBone = Bone(BoneType.RIGHT_INDEX_PROXIMAL) - val rightIndexIntermediateBone = Bone(BoneType.RIGHT_INDEX_INTERMEDIATE) - val rightIndexDistalBone = Bone(BoneType.RIGHT_INDEX_DISTAL) - val rightMiddleProximalBone = Bone(BoneType.RIGHT_MIDDLE_PROXIMAL) - val rightMiddleIntermediateBone = Bone(BoneType.RIGHT_MIDDLE_INTERMEDIATE) - val rightMiddleDistalBone = Bone(BoneType.RIGHT_MIDDLE_DISTAL) - val rightRingProximalBone = Bone(BoneType.RIGHT_RING_PROXIMAL) - val rightRingIntermediateBone = Bone(BoneType.RIGHT_RING_INTERMEDIATE) - val rightRingDistalBone = Bone(BoneType.RIGHT_RING_DISTAL) - val rightLittleProximalBone = Bone(BoneType.RIGHT_LITTLE_PROXIMAL) - val rightLittleIntermediateBone = Bone(BoneType.RIGHT_LITTLE_INTERMEDIATE) - val rightLittleDistalBone = Bone(BoneType.RIGHT_LITTLE_DISTAL) + val leftThumbMetacarpalBone = Bone(BoneType.LEFT_THUMB_METACARPAL, Constraint(ConstraintType.COMPLETE)) + val leftThumbProximalBone = Bone(BoneType.LEFT_THUMB_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val leftThumbDistalBone = Bone(BoneType.LEFT_THUMB_DISTAL, Constraint(ConstraintType.COMPLETE)) + val leftIndexProximalBone = Bone(BoneType.LEFT_INDEX_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val leftIndexIntermediateBone = Bone(BoneType.LEFT_INDEX_INTERMEDIATE, Constraint(ConstraintType.COMPLETE)) + val leftIndexDistalBone = Bone(BoneType.LEFT_INDEX_DISTAL, Constraint(ConstraintType.COMPLETE)) + val leftMiddleProximalBone = Bone(BoneType.LEFT_MIDDLE_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val leftMiddleIntermediateBone = Bone(BoneType.LEFT_MIDDLE_INTERMEDIATE, Constraint(ConstraintType.COMPLETE)) + val leftMiddleDistalBone = Bone(BoneType.LEFT_MIDDLE_DISTAL, Constraint(ConstraintType.COMPLETE)) + val leftRingProximalBone = Bone(BoneType.LEFT_RING_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val leftRingIntermediateBone = Bone(BoneType.LEFT_RING_INTERMEDIATE, Constraint(ConstraintType.COMPLETE)) + val leftRingDistalBone = Bone(BoneType.LEFT_RING_DISTAL, Constraint(ConstraintType.COMPLETE)) + val leftLittleProximalBone = Bone(BoneType.LEFT_LITTLE_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val leftLittleIntermediateBone = Bone(BoneType.LEFT_LITTLE_INTERMEDIATE, Constraint(ConstraintType.COMPLETE)) + val leftLittleDistalBone = Bone(BoneType.LEFT_LITTLE_DISTAL, Constraint(ConstraintType.COMPLETE)) + val rightThumbMetacarpalBone = Bone(BoneType.RIGHT_THUMB_METACARPAL, Constraint(ConstraintType.COMPLETE)) + val rightThumbProximalBone = Bone(BoneType.RIGHT_THUMB_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val rightThumbDistalBone = Bone(BoneType.RIGHT_THUMB_DISTAL, Constraint(ConstraintType.COMPLETE)) + val rightIndexProximalBone = Bone(BoneType.RIGHT_INDEX_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val rightIndexIntermediateBone = Bone(BoneType.RIGHT_INDEX_INTERMEDIATE, Constraint(ConstraintType.COMPLETE)) + val rightIndexDistalBone = Bone(BoneType.RIGHT_INDEX_DISTAL, Constraint(ConstraintType.COMPLETE)) + val rightMiddleProximalBone = Bone(BoneType.RIGHT_MIDDLE_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val rightMiddleIntermediateBone = Bone(BoneType.RIGHT_MIDDLE_INTERMEDIATE, Constraint(ConstraintType.COMPLETE)) + val rightMiddleDistalBone = Bone(BoneType.RIGHT_MIDDLE_DISTAL, Constraint(ConstraintType.COMPLETE)) + val rightRingProximalBone = Bone(BoneType.RIGHT_RING_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val rightRingIntermediateBone = Bone(BoneType.RIGHT_RING_INTERMEDIATE, Constraint(ConstraintType.COMPLETE)) + val rightRingDistalBone = Bone(BoneType.RIGHT_RING_DISTAL, Constraint(ConstraintType.COMPLETE)) + val rightLittleProximalBone = Bone(BoneType.RIGHT_LITTLE_PROXIMAL, Constraint(ConstraintType.COMPLETE)) + val rightLittleIntermediateBone = Bone(BoneType.RIGHT_LITTLE_INTERMEDIATE, Constraint(ConstraintType.COMPLETE)) + val rightLittleDistalBone = Bone(BoneType.RIGHT_LITTLE_DISTAL, Constraint(ConstraintType.COMPLETE)) // 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 @@ -190,6 +194,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 @@ -292,8 +298,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) @@ -442,6 +450,9 @@ class HumanSkeleton( // Update tap detection's trackers tapDetectionManager.updateConfig(trackers) + + // Update bones tracker field + refreshBoneTracker() } /** @@ -500,6 +511,7 @@ class HumanSkeleton( updateTransforms() updateBones() + headBone.updateWithConstraints() updateComputedTrackers() // Don't run post-processing if the tracking is paused @@ -510,6 +522,15 @@ class HumanSkeleton( viveEmulation.update() } + /** + * Refresh the attachedTracker field in each bone + */ + private fun refreshBoneTracker() { + for (bone in allHumanBones) { + bone.attachedTracker = getTrackerForBone(bone.boneType) + } + } + /** * Update all the bones by updating the roots */ @@ -560,6 +581,7 @@ class HumanSkeleton( // Left arm updateArmTransforms( isTrackingLeftArmFromController, + leftUpperShoulderBone, leftShoulderBone, leftUpperArmBone, leftElbowTrackerBone, @@ -575,6 +597,7 @@ class HumanSkeleton( // Right arm updateArmTransforms( isTrackingRightArmFromController, + rightUpperShoulderBone, rightShoulderBone, rightUpperArmBone, rightElbowTrackerBone, @@ -925,6 +948,7 @@ class HumanSkeleton( */ private fun updateArmTransforms( isTrackingFromController: Boolean, + upperShoulderBone: Bone, shoulderBone: Bone, upperArmBone: Bone, elbowTrackerBone: Bone, @@ -957,6 +981,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) { @@ -1136,6 +1161,12 @@ class HumanSkeleton( SkeletonConfigToggles.FOOT_PLANT -> legTweaks.footPlantEnabled = newValue SkeletonConfigToggles.SELF_LOCALIZATION -> localizer.setEnabled(newValue) + + SkeletonConfigToggles.USE_POSITION -> newValue + + SkeletonConfigToggles.ENFORCE_CONSTRAINTS -> enforceConstraints = newValue + + SkeletonConfigToggles.CORRECT_CONSTRAINTS -> correctConstraints = newValue } } @@ -1217,6 +1248,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 @@ -1261,6 +1294,30 @@ class HumanSkeleton( BoneType.RIGHT_LITTLE_DISTAL -> rightLittleDistalBone } + 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. */ @@ -1280,6 +1337,8 @@ class HumanSkeleton( rightLowerLegBone, leftFootBone, rightFootBone, + leftUpperShoulderBone, + rightUpperShoulderBone, leftShoulderBone, rightShoulderBone, leftUpperArmBone, @@ -1325,6 +1384,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/trackers/TrackerFilteringHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFilteringHandler.kt index 87a6e0a805..1ab9c2ecbb 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 @@ -58,4 +58,9 @@ class TrackerFilteringHandler { * Get the filtered rotation from the moving average (either prediction/smoothing or just >180 degs) */ fun getFilteredRotation() = movingAverage.filteredQuaternion + + /** + * Get the impact filtering has on the rotation + */ + fun getFilteringImpact(): Float = movingAverage.filteringImpact } 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 c90414dd65..1e21fbf8f3 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 @@ -63,6 +63,7 @@ class TrackerResetsHandler(val tracker: Tracker) { var mountRotFix = Quaternion.IDENTITY private set private var yawFix = Quaternion.IDENTITY + private var constraintFix = Quaternion.IDENTITY // Yaw reset smoothing vars private var yawFixOld = Quaternion.IDENTITY @@ -168,6 +169,7 @@ class TrackerResetsHandler(val tracker: Tracker) { rot = mountRotFix.inv() * (rot * mountRotFix) rot *= tposeDownFix rot = yawFix * rot + rot = constraintFix * rot return rot } @@ -180,6 +182,7 @@ class TrackerResetsHandler(val tracker: Tracker) { rot = gyroFixNoMounting * rot rot *= attachmentFixNoMounting rot = yawFixZeroReference * rot + rot = constraintFix * rot return rot } @@ -214,6 +217,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) { + constraintFix = Quaternion.IDENTITY + if (tracker.trackerDataType == TrackerDataType.FLEX_RESISTANCE) { tracker.trackerFlexHandler.resetMin() postProcessResetFull() @@ -305,6 +310,8 @@ class TrackerResetsHandler(val tracker: Tracker) { * position should be corrected in the source. */ fun resetYaw(reference: Quaternion) { + constraintFix = Quaternion.IDENTITY + if (tracker.trackerDataType == TrackerDataType.FLEX_RESISTANCE || tracker.trackerDataType == TrackerDataType.FLEX_ANGLE ) { @@ -355,6 +362,8 @@ class TrackerResetsHandler(val tracker: Tracker) { return } + constraintFix = Quaternion.IDENTITY + // Get the current calibrated rotation var rotBuf = adjustToDrift(tracker.getRawRotation() * mountingOrientation) rotBuf = gyroFix * rotBuf @@ -403,6 +412,13 @@ class TrackerResetsHandler(val tracker: Tracker) { tracker.resetFilteringQuats() } + /** + * Apply a corrective rotation to the gyroFix + */ + fun updateConstraintFix(correctedRotation: Quaternion) { + constraintFix *= correctedRotation + } + fun clearMounting() { mountRotFix = Quaternion.IDENTITY }