Gradually relax hands from controlled positions when lose tracking

libraries/animation/src/AnimInverseKinematics.cpp

@@ -453,7 +453,6 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
 
 //virtual
 const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
-
     // allows solutionSource to be overridden by an animVar
     auto solutionSource = animVars.lookup(_solutionSourceVar, (int)_solutionSource);
 
@@ -581,6 +580,16 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
         }
     }
 
+    if (_leftHandIndex > -1) {
+        _uncontrolledLeftHandPose = _skeleton->getAbsolutePose(_leftHandIndex, underPoses);
+    }
+    if (_rightHandIndex > -1) {
+        _uncontrolledRightHandPose = _skeleton->getAbsolutePose(_rightHandIndex, underPoses);
+    }
+    if (_hipsIndex > -1) {
+        _uncontrolledHipsPose = _skeleton->getAbsolutePose(_hipsIndex, underPoses);
+    }
+
     return _relativePoses;
 }
 
@@ -1064,12 +1073,21 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
         } else {
             _hipsParentIndex = -1;
         }
+
+        _leftHandIndex = _skeleton->nameToJointIndex("LeftHand");
+        _rightHandIndex = _skeleton->nameToJointIndex("RightHand");
     } else {
         clearConstraints();
         _headIndex = -1;
         _hipsIndex = -1;
         _hipsParentIndex = -1;
+        _leftHandIndex = -1;
+        _rightHandIndex = -1;
     }
+
+    _uncontrolledLeftHandPose = AnimPose();
+    _uncontrolledRightHandPose = AnimPose();
+    _uncontrolledHipsPose = AnimPose();
 }
 
 static glm::vec3 sphericalToCartesian(float phi, float theta) {

libraries/animation/src/AnimInverseKinematics.h

@@ -56,6 +56,10 @@ public:
     void setSolutionSource(SolutionSource solutionSource) { _solutionSource = solutionSource; }
     void setSolutionSourceVar(const QString& solutionSourceVar) { _solutionSourceVar = solutionSourceVar; }
 
+    const AnimPose& getUncontrolledLeftHandPose() { return _uncontrolledLeftHandPose; }
+    const AnimPose& getUncontrolledRightHandPose() { return _uncontrolledRightHandPose; }
+    const AnimPose& getUncontrolledHipPose() { return _uncontrolledHipsPose; }
+
 protected:
     void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
     void solveWithCyclicCoordinateDescent(const AnimContext& context, const std::vector<IKTarget>& targets);
@@ -118,6 +122,8 @@ protected:
     int _hipsIndex { -1 };
     int _hipsParentIndex { -1 };
     int _hipsTargetIndex { -1 };
+    int _leftHandIndex { -1 };
+    int _rightHandIndex { -1 };
 
     // _maxTargetIndex is tracked to help optimize the recalculation of absolute poses
     // during the the cyclic coordinate descent algorithm
@@ -127,6 +133,10 @@ protected:
     bool _previousEnableDebugIKTargets { false };
     SolutionSource _solutionSource { SolutionSource::RelaxToUnderPoses };
     QString _solutionSourceVar;
+
+    AnimPose _uncontrolledLeftHandPose { AnimPose() };
+    AnimPose _uncontrolledRightHandPose { AnimPose() };
+    AnimPose _uncontrolledHipsPose { AnimPose() };
 };
 
 #endif // hifi_AnimInverseKinematics_h

libraries/animation/src/Rig.cpp

@@ -1172,10 +1172,10 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
         // TODO: add isHipsEnabled
         bool bodySensorTrackingEnabled = params.isLeftFootEnabled || params.isRightFootEnabled;
 
+        const float RELAX_DURATION = 0.6f;
+
         if (params.isLeftEnabled) {
-
             glm::vec3 handPosition = params.leftPosition;
-
             if (!bodySensorTrackingEnabled) {
                 // prevent the hand IK targets from intersecting the body capsule
                 glm::vec3 displacement;
@@ -1187,16 +1187,46 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
             _animVars.set("leftHandPosition", handPosition);
             _animVars.set("leftHandRotation", params.leftOrientation);
             _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
+
+            _isLeftHandControlled = true;
+            _lastLeftHandControlledPosition = handPosition;
+            _lastLeftHandControlledOrientation = params.leftOrientation;
         } else {
-            _animVars.unset("leftHandPosition");
+            if (_isLeftHandControlled) {
-            _animVars.unset("leftHandRotation");
+                _leftHandRelaxDuration = RELAX_DURATION;
-            _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+                _isLeftHandControlled = false;
+            }
+
+            if (_leftHandRelaxDuration > 0) {
+                // Move hand from controlled position to non-controlled position.
+                _leftHandRelaxDuration = std::max(_leftHandRelaxDuration - dt, 0.0f);
+
+                auto ikNode = getAnimInverseKinematicsNode();
+                if (ikNode) {
+                    float alpha = _leftHandRelaxDuration / RELAX_DURATION;
+                    auto uncontrolledHandPose = ikNode->getUncontrolledLeftHandPose();
+                    auto uncontrolledHipsPose = ikNode->getUncontrolledHipPose();
+
+                    glm::vec3 relaxedPosition = _geometryOffset * (uncontrolledHandPose.trans() - uncontrolledHipsPose.trans());
+                    glm::vec3 handPosition = alpha * _lastLeftHandControlledPosition + (1.0f - alpha) * relaxedPosition;
+
+                    const AnimPose geometryToRigPose(_geometryToRigTransform);
+                    glm::quat handOrientation = geometryToRigPose.rot() * uncontrolledHandPose.rot();
+                    handOrientation = slerp(handOrientation, _lastLeftHandControlledOrientation, alpha);
+
+                    _animVars.set("leftHandPosition", handPosition);
+                    _animVars.set("leftHandRotation", handOrientation);
+                    _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
+                }
+            } else {
+                _animVars.unset("leftHandPosition");
+                _animVars.unset("leftHandRotation");
+                _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+            }
         }
 
         if (params.isRightEnabled) {
-
             glm::vec3 handPosition = params.rightPosition;
-
             if (!bodySensorTrackingEnabled) {
                 // prevent the hand IK targets from intersecting the body capsule
                 glm::vec3 displacement;
@@ -1208,10 +1238,42 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
             _animVars.set("rightHandPosition", handPosition);
             _animVars.set("rightHandRotation", params.rightOrientation);
             _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
+
+            _isRightHandControlled = true;
+            _lastRightHandControlledPosition = handPosition;
+            _lastRightHandControlledOrientation = params.rightOrientation;
         } else {
-            _animVars.unset("rightHandPosition");
+            if (_isRightHandControlled) {
-            _animVars.unset("rightHandRotation");
+                _rightHandRelaxDuration = RELAX_DURATION;
-            _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+                _isRightHandControlled = false;
+            }
+
+            if (_rightHandRelaxDuration > 0) {
+                // Move hand from controlled position to non-controlled position.
+                _rightHandRelaxDuration = std::max(_rightHandRelaxDuration - dt, 0.0f);
+
+                auto ikNode = getAnimInverseKinematicsNode();
+                if (ikNode) {
+                    float alpha = _rightHandRelaxDuration / RELAX_DURATION;
+                    auto uncontrolledHandPose = ikNode->getUncontrolledRightHandPose();
+                    auto uncontrolledHipsPose = ikNode->getUncontrolledHipPose();
+
+                    glm::vec3 relaxedPosition = _geometryOffset * (uncontrolledHandPose.trans() - uncontrolledHipsPose.trans());
+                    glm::vec3 handPosition = alpha * _lastRightHandControlledPosition + (1.0f - alpha) * relaxedPosition;
+
+                    const AnimPose geometryToRigPose(_geometryToRigTransform);
+                    glm::quat handOrientation = geometryToRigPose.rot() * uncontrolledHandPose.rot();
+                    handOrientation = slerp(handOrientation, _lastRightHandControlledOrientation, alpha);
+
+                    _animVars.set("rightHandPosition", handPosition);
+                    _animVars.set("rightHandRotation", handOrientation);
+                    _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
+                }
+            } else {
+                _animVars.unset("rightHandPosition");
+                _animVars.unset("rightHandRotation");
+                _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+            }
         }
 
         if (params.isLeftFootEnabled) {
@@ -1233,7 +1295,6 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
             _animVars.unset("rightFootRotation");
             _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
         }
-
     }
 }
 
@@ -1509,5 +1570,3 @@ void Rig::computeAvatarBoundingCapsule(
     glm::vec3 rigCenter = (geometryToRig * (0.5f * (totalExtents.maximum + totalExtents.minimum)));
     localOffsetOut = rigCenter - (geometryToRig * rootPosition);
 }
-
-

libraries/animation/src/Rig.h

@@ -355,6 +355,15 @@ private:
     QMap<int, StateHandler> _stateHandlers;
     int _nextStateHandlerId { 0 };
     QMutex _stateMutex;
+
+    bool _isLeftHandControlled { false };
+    bool _isRightHandControlled { false };
+    float _leftHandRelaxDuration { 0.0f };
+    float _rightHandRelaxDuration { 0.0f };
+    glm::vec3 _lastLeftHandControlledPosition;
+    glm::vec3 _lastRightHandControlledPosition;
+    glm::quat _lastLeftHandControlledOrientation;
+    glm::quat _lastRightHandControlledOrientation;
 };
 
 #endif /* defined(__hifi__Rig__) */