From e7991579ef6f9021cb01cf5f3aee5681c8cdeec1 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 16 Jun 2017 17:29:56 -0700 Subject: [PATCH] Enabled elbow pole vectors There are still some issues with rotations of the elbow pole vectors. * When the (hand - shoulder) vector approaches the normal vector used in Rig::calculateElbowPoleVector() unexpected twists can occur. * Also, when the (hand - shoulder) vector approaches zero, the IK system starts to flutter between two states. * The shoulder twist constraint probably needs to be opened up for more natural range of motion. --- .../resources/avatar/avatar-animation.json | 10 +- .../animation/src/AnimInverseKinematics.cpp | 2 +- libraries/animation/src/Rig.cpp | 95 ++++++++++++++++++- libraries/animation/src/Rig.h | 9 +- 4 files changed, 110 insertions(+), 6 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index c9552a3ead..018987b58b 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -68,7 +68,10 @@ "typeVar": "rightHandType", "weightVar": "rightHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0], + "poleVectorEnabledVar": "rightHandPoleVectorEnabled", + "poleReferenceVectorVar": "rightHandPoleReferenceVector", + "poleVectorVar": "rightHandPoleVector" }, { "jointName": "LeftHand", @@ -77,7 +80,10 @@ "typeVar": "leftHandType", "weightVar": "leftHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0], + "poleVectorEnabledVar": "leftHandPoleVectorEnabled", + "poleReferenceVectorVar": "leftHandPoleReferenceVector", + "poleVectorVar": "leftHandPoleVector" }, { "jointName": "RightFoot", diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index f55537b3d6..080e6449d9 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -42,7 +42,7 @@ static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointCha } } -float easeOutExpo(float t) { +static float easeOutExpo(float t) { return 1.0f - powf(2, -10.0f * t); } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 1e6965faf5..99678eaa28 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1140,6 +1140,47 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } +static float easeOutExpo(float t) { + return 1.0f - powf(2, -10.0f * t); +} + +static glm::quat quatLerp(const glm::quat& q1, const glm::quat& q2, float alpha) { + float dot = glm::dot(q1, q2); + glm::quat temp; + if (dot < 0.0f) { + temp = -q2; + } else { + temp = q2; + } + return glm::normalize(glm::lerp(q1, temp, alpha)); +} + +glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const { + AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex]; + AnimPose handPose = _externalPoseSet._absolutePoses[handIndex]; + AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex]; + AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; + glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); + + glm::quat elbowToHandDelta = handPose.rot() * glm::inverse(elbowPose.rot()); + const float WRIST_POLE_ADJUST_FACTOR = 0.5f; + glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, elbowToHandDelta, WRIST_POLE_ADJUST_FACTOR); + + glm::vec3 n; + if (isLeft) { + n = hipsPose.rot() * glm::normalize(glm::vec3(1.0f, 1.5f, -0.1f)); + } else { + n = hipsPose.rot() * -glm::normalize(glm::vec3(-1.0f, 1.5f, -0.1f)); + } + + // project d onto n. + glm::vec3 dProj = d - glm::dot(d, n) * n; + glm::vec3 dProjRot = glm::angleAxis(PI / 2.0f, n) * dProj; + glm::vec3 pole = glm::normalize(dProjRot); + + return glm::normalize(poleAdjust * pole); +} + glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex); @@ -1175,6 +1216,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f const float RELAX_DURATION = 0.6f; + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + if (params.isLeftEnabled) { glm::vec3 handPosition = params.leftPosition; if (!bodySensorTrackingEnabled) { @@ -1191,7 +1234,32 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _isLeftHandControlled = true; _lastLeftHandControlledPose = AnimPose(glm::vec3(1.0f), params.leftOrientation, handPosition); + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftHandPoleVectorValid) { + _prevLeftHandPoleVectorValid = true; + _prevLeftHandPoleVector = poleVector; + } + _prevLeftHandPoleVector = lerp(poleVector, _prevLeftHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("leftHandPoleVectorEnabled", true); + _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); + _animVars.set("leftHandPoleVector", _prevLeftHandPoleVector); + } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + } } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + if (_isLeftHandControlled) { _leftHandRelaxDuration = RELAX_DURATION; _isLeftHandControlled = false; @@ -1234,7 +1302,32 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _isRightHandControlled = true; _lastRightHandControlledPose = AnimPose(glm::vec3(1.0f), params.rightOrientation, handPosition); + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightHandPoleVectorValid) { + _prevRightHandPoleVectorValid = true; + _prevRightHandPoleVector = poleVector; + } + _prevRightHandPoleVector = lerp(poleVector, _prevRightHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("rightHandPoleVectorEnabled", true); + _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); + _animVars.set("rightHandPoleVector", _prevRightHandPoleVector); + } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + } } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + if (_isRightHandControlled) { _rightHandRelaxDuration = RELAX_DURATION; _isRightHandControlled = false; @@ -1261,8 +1354,6 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f } } - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; - if (params.isLeftFootEnabled) { _animVars.set("leftFootPosition", params.leftFootPosition); _animVars.set("leftFootRotation", params.leftFootOrientation); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 4fc5f76146..06822d666e 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -249,7 +249,8 @@ protected: void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; - glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; + glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; + glm::vec3 calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) @@ -362,6 +363,12 @@ protected: glm::vec3 _prevLeftFootPoleVector = { Vectors::UNIT_Z }; bool _prevLeftFootPoleVectorValid = { false }; + + glm::vec3 _prevRightHandPoleVector = { -Vectors::UNIT_Z }; + bool _prevRightHandPoleVectorValid = { false }; + + glm::vec3 _prevLeftHandPoleVector = { -Vectors::UNIT_Z }; + bool _prevLeftHandPoleVectorValid = { false }; }; #endif /* defined(__hifi__Rig__) */