diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 5e89093b23..5b5d70be01 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/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) { diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 0267f14650..35845224e2 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/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& targets, const AnimPoseVec& underPoses); void solveWithCyclicCoordinateDescent(const AnimContext& context, const std::vector& 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 diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index add3a461af..6b478da5fc 100644 --- a/libraries/animation/src/Rig.cpp +++ b/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"); - _animVars.unset("leftHandRotation"); - _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); + if (_isLeftHandControlled) { + _leftHandRelaxDuration = RELAX_DURATION; + _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"); - _animVars.unset("rightHandRotation"); - _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); + if (_isRightHandControlled) { + _rightHandRelaxDuration = RELAX_DURATION; + _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); } - - diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index c9d52d8c72..1c7e40a04a 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -355,6 +355,15 @@ private: QMap _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__) */