From 9d2d7c3aa4c71d0ddde7d740d31becff637e17eb Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Mon, 23 Jul 2018 13:16:11 -0700 Subject: [PATCH 1/6] Redesign and enable hand's Pole Vectors for elbow simulation --- interface/src/avatar/MyAvatar.h | 2 + libraries/animation/src/Rig.cpp | 156 +++++++++++++++++++------------- libraries/animation/src/Rig.h | 10 +- 3 files changed, 99 insertions(+), 69 deletions(-) diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 9b5ddd360d..f3492db9a4 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -612,6 +612,8 @@ public: const MyHead* getMyHead() const; + Q_INVOKABLE void toggleHandPoleVectors() { _skeletonModel->getRig().toggleHandPoleVectors(); } + /**jsdoc * Get the current position of the avatar's "Head" joint. * @function MyAvatar.getHeadPosition diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 549989778e..ce9d803574 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1247,8 +1247,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo, const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix) { - const bool ENABLE_POLE_VECTORS = false; - const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f; + float avatarScale = extractUniformScale(_modelOffset); int hipsIndex = indexOfJoint("Hips"); @@ -1270,34 +1269,28 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); - if (ENABLE_POLE_VECTORS && !leftArmEnabled && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { - glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); - glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevLeftHandPoleVectorValid) { - _prevLeftHandPoleVectorValid = true; - _prevLeftHandPoleVector = sensorPoleVector; + int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("RightArm"); + if (_enablePoleVectors && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { + glm::vec3 poleVector; + bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector); + if (usePoleVector) { + glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector); + _animVars.set("leftHandPoleVectorEnabled", true); + _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); + _animVars.set("leftHandPoleVector", transformVectorFast(sensorToRigMatrix, sensorPoleVector)); + } else { + _animVars.set("leftHandPoleVectorEnabled", false); } - glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, sensorPoleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); - _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector; - _animVars.set("leftHandPoleVectorEnabled", true); - _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); - _animVars.set("leftHandPoleVector", transformVectorFast(sensorToRigMatrix, _prevLeftHandPoleVector)); } else { - _prevLeftHandPoleVectorValid = false; _animVars.set("leftHandPoleVectorEnabled", false); } } else { - _prevLeftHandPoleVectorValid = false; _animVars.set("leftHandPoleVectorEnabled", false); _animVars.unset("leftHandPosition"); _animVars.unset("leftHandRotation"); _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); - } if (rightHandEnabled) { @@ -1318,28 +1311,24 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); - if (ENABLE_POLE_VECTORS && !rightArmEnabled && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { - glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); - glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector); + int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevRightHandPoleVectorValid) { - _prevRightHandPoleVectorValid = true; - _prevRightHandPoleVector = sensorPoleVector; + if (_enablePoleVectors && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { + glm::vec3 poleVector; + bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector); + if (usePoleVector) { + glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector); + + _animVars.set("rightHandPoleVectorEnabled", true); + _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); + _animVars.set("rightHandPoleVector", transformVectorFast(sensorToRigMatrix, sensorPoleVector)); + } else { + _animVars.set("rightHandPoleVectorEnabled", false); } - glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, sensorPoleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); - _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector; - - _animVars.set("rightHandPoleVectorEnabled", true); - _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); - _animVars.set("rightHandPoleVector", transformVectorFast(sensorToRigMatrix, _prevRightHandPoleVector)); } else { - _prevRightHandPoleVectorValid = false; _animVars.set("rightHandPoleVectorEnabled", false); } } else { - _prevRightHandPoleVectorValid = false; _animVars.set("rightHandPoleVectorEnabled", false); _animVars.unset("rightHandPosition"); @@ -1472,39 +1461,80 @@ static glm::quat quatLerp(const glm::quat& q1, const glm::quat& q2, float alpha) 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]; +bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& poleVector) const { + // The resulting Pole Vector is calculated as the sum of a three vectors. + // The first is the vector with direction shoulder-hand. The module of this vector is inversely proportional to the strength of the resulting Pole Vector. + // The second vector is always perpendicular to previous vector and is part of the plane that contains a point located on the horizontal line, + // pointing forward and with height aprox to the avatar head. The position of the horizontal point will be determined by the hands Y component. + // The third vector apply a weighted correction to the resulting pole vector to avoid interpenetration and force a more natural pose. - // ray from hand to arm. - glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); - - float sign = isLeft ? 1.0f : -1.0f; - - // form a plane normal to the hips x-axis. - glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; - glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; - - // project d onto this plane - glm::vec3 dProj = d - glm::dot(d, n) * n; - - // give dProj a bit of offset away from the body. float avatarScale = extractUniformScale(_modelOffset); - const float LATERAL_OFFSET = 1.0f * avatarScale; - const float VERTICAL_OFFSET = -0.333f * avatarScale; - glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; - // rotate dProj by 90 degrees to get the poleVector. - glm::vec3 poleVector = glm::angleAxis(PI / 2.0f, n) * dProjWithOffset; + AnimPose oppositeArmPose = _externalPoseSet._absolutePoses[oppositeArmIndex]; + AnimPose handPose = _externalPoseSet._absolutePoses[handIndex]; + AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; + AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex]; - // blend the wrist oreintation into the pole vector to reduce the painfully bent wrist problem. - 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 armToHand = handPose.trans() - armPose.trans(); + glm::vec3 armToElbow = elbowPose.trans() - armPose.trans(); + glm::vec3 elbowToHand = handPose.trans() - elbowPose.trans(); - return glm::normalize(poleAdjust * poleVector); + glm::vec3 backVector = oppositeArmPose.trans() - armPose.trans(); + glm::vec3 backCenter = armPose.trans() + 0.5f * backVector; + + const float OVER_BACK_HEAD_PERCENTAGE = 0.2f; + + glm::vec3 headCenter = backCenter + glm::vec3(0, OVER_BACK_HEAD_PERCENTAGE * backVector.length(), 0); + glm::vec3 frontVector = glm::normalize(glm::cross(backVector, glm::vec3(0, 1, 0))); + // Make sure is pointing forward + frontVector = frontVector.z < 0 ? -frontVector : frontVector; + + float horizontalModule = glm::dot(armToHand, glm::vec3(0, -1, 0)); + glm::vec3 headForward = headCenter + horizontalModule * frontVector; + + glm::vec3 armToHead = headForward - armPose.trans(); + + float armToHandDistance = glm::length(armToHand); + float armToElbowDistance = glm::length(armToElbow); + float armToHeadDistance = glm::length(armToHead); + float elbowToHandDistance = glm::length(elbowToHand); + + float armTotalDistance = armToElbowDistance + elbowToHandDistance; + + // Don't use pole vector when the hands are behind + if (glm::dot(frontVector, armToHand) < 0) { + return false; + } + + glm::vec3 armToHandDir = armToHand / armToHandDistance; + glm::vec3 armToElbowDir = armToElbow / armToElbowDistance; + glm::vec3 armToHeadDir = armToHead / armToHeadDistance; + + glm::vec3 armToHandPlaneNormal = glm::cross(armToHandDir, armToElbowDir); + glm::vec3 currentPoleVector = glm::normalize(glm::cross(armToHandPlaneNormal, armToHandDir)); + + glm::vec3 armToHeadPlaneNormal = glm::cross(armToHead, armToHandDir); + + // The strenght of the resulting pole determined by the arm flex. + float armFlexCoeficient = armToHandDistance / armTotalDistance; + glm::vec3 attenuationVector = armFlexCoeficient * armToHandDir; + // Pole vector is perpendicular to the shoulder-hand direction and located on the plane that contains the head-forward line + glm::vec3 fullPoleVector = glm::normalize(glm::cross(armToHeadPlaneNormal, armToHandDir)); + + // Push elbow forward when hand reaches opposite side + glm::vec3 correctionVector = glm::vec3(0, 0, 0); + + const float FORWARD_TRIGGER_PERCENTAGE = 0.2f; + const float FORWARD_CORRECTOR_WEIGHT = 3.0f; + + float elbowForwardTrigger = FORWARD_TRIGGER_PERCENTAGE * armToHandDistance; + float oppositeProjection = glm::dot(armToHandDir, glm::normalize(backVector)); + if (oppositeProjection > -elbowForwardTrigger) { + float forwardAmount = FORWARD_CORRECTOR_WEIGHT * (elbowForwardTrigger + oppositeProjection); + correctionVector = forwardAmount * frontVector; + } + poleVector = glm::normalize(attenuationVector + fullPoleVector + correctionVector); + return true; } glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index ffa3a128b9..9cd8368db7 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -218,6 +218,8 @@ public: // input assumed to be in rig space void computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const; + void toggleHandPoleVectors() { _enablePoleVectors = !_enablePoleVectors; }; + signals: void onLoadComplete(); @@ -240,7 +242,7 @@ 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 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; + bool calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& outPoleVector) const; glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const; glm::vec3 deflectHandFromTorso(const glm::vec3& handPosition, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) const; @@ -368,11 +370,7 @@ protected: glm::vec3 _prevLeftFootPoleVector { Vectors::UNIT_Z }; // sensor space bool _prevLeftFootPoleVectorValid { false }; - glm::vec3 _prevRightHandPoleVector { -Vectors::UNIT_Z }; // sensor space - bool _prevRightHandPoleVectorValid { false }; - - glm::vec3 _prevLeftHandPoleVector { -Vectors::UNIT_Z }; // sensor space - bool _prevLeftHandPoleVectorValid { false }; + bool _enablePoleVectors{ true }; int _rigId; }; From 2adb334034936e727c9bdc5efc969c381efc54ba Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Mon, 23 Jul 2018 14:25:30 -0700 Subject: [PATCH 2/6] unused code deleted --- interface/src/avatar/MyAvatar.h | 2 -- libraries/animation/src/Rig.cpp | 11 ++--------- libraries/animation/src/Rig.h | 4 ---- 3 files changed, 2 insertions(+), 15 deletions(-) diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index f3492db9a4..9b5ddd360d 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -612,8 +612,6 @@ public: const MyHead* getMyHead() const; - Q_INVOKABLE void toggleHandPoleVectors() { _skeletonModel->getRig().toggleHandPoleVectors(); } - /**jsdoc * Get the current position of the avatar's "Head" joint. * @function MyAvatar.getHeadPosition diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index ce9d803574..62b3efb495 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1247,10 +1247,6 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo, const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix) { - float avatarScale = extractUniformScale(_modelOffset); - - int hipsIndex = indexOfJoint("Hips"); - if (leftHandEnabled) { glm::vec3 handPosition = leftHandPose.trans(); @@ -1270,7 +1266,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("RightArm"); - if (_enablePoleVectors && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { + if (handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { glm::vec3 poleVector; bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector); if (usePoleVector) { @@ -1313,7 +1309,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); - if (_enablePoleVectors && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { + if (handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { glm::vec3 poleVector; bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector); if (usePoleVector) { @@ -1508,11 +1504,8 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, glm::vec3 armToHandDir = armToHand / armToHandDistance; glm::vec3 armToElbowDir = armToElbow / armToElbowDistance; - glm::vec3 armToHeadDir = armToHead / armToHeadDistance; glm::vec3 armToHandPlaneNormal = glm::cross(armToHandDir, armToElbowDir); - glm::vec3 currentPoleVector = glm::normalize(glm::cross(armToHandPlaneNormal, armToHandDir)); - glm::vec3 armToHeadPlaneNormal = glm::cross(armToHead, armToHandDir); // The strenght of the resulting pole determined by the arm flex. diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 9cd8368db7..d5da02d052 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -218,8 +218,6 @@ public: // input assumed to be in rig space void computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const; - void toggleHandPoleVectors() { _enablePoleVectors = !_enablePoleVectors; }; - signals: void onLoadComplete(); @@ -370,8 +368,6 @@ protected: glm::vec3 _prevLeftFootPoleVector { Vectors::UNIT_Z }; // sensor space bool _prevLeftFootPoleVectorValid { false }; - bool _enablePoleVectors{ true }; - int _rigId; }; From 4a27189913ac3fb852d45d3422284f24d36e0d5b Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Mon, 23 Jul 2018 14:27:55 -0700 Subject: [PATCH 3/6] function consistancy --- libraries/animation/src/Rig.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index d5da02d052..ccbb19a4d3 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -240,7 +240,7 @@ 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; - bool calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& outPoleVector) const; + bool calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& poleVector) const; glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const; glm::vec3 deflectHandFromTorso(const glm::vec3& handPosition, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) const; From 94bb8fc917b1050836a8c214c99e10a80d752fb2 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Mon, 23 Jul 2018 15:29:42 -0700 Subject: [PATCH 4/6] More unused code --- libraries/animation/src/Rig.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 62b3efb495..c854b9dae3 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1464,8 +1464,6 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, // pointing forward and with height aprox to the avatar head. The position of the horizontal point will be determined by the hands Y component. // The third vector apply a weighted correction to the resulting pole vector to avoid interpenetration and force a more natural pose. - float avatarScale = extractUniformScale(_modelOffset); - AnimPose oppositeArmPose = _externalPoseSet._absolutePoses[oppositeArmIndex]; AnimPose handPose = _externalPoseSet._absolutePoses[handIndex]; AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; @@ -1492,7 +1490,6 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, float armToHandDistance = glm::length(armToHand); float armToElbowDistance = glm::length(armToElbow); - float armToHeadDistance = glm::length(armToHead); float elbowToHandDistance = glm::length(elbowToHand); float armTotalDistance = armToElbowDistance + elbowToHandDistance; @@ -1504,8 +1501,7 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, glm::vec3 armToHandDir = armToHand / armToHandDistance; glm::vec3 armToElbowDir = armToElbow / armToElbowDistance; - - glm::vec3 armToHandPlaneNormal = glm::cross(armToHandDir, armToElbowDir); + glm::vec3 armToHeadPlaneNormal = glm::cross(armToHead, armToHandDir); // The strenght of the resulting pole determined by the arm flex. From 8f547d9bdd20f27f84668db211732d34b59f5988 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Mon, 23 Jul 2018 16:37:25 -0700 Subject: [PATCH 5/6] fix warnings --- libraries/animation/src/Rig.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index c854b9dae3..b0a4020c60 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1500,8 +1500,6 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, } glm::vec3 armToHandDir = armToHand / armToHandDistance; - glm::vec3 armToElbowDir = armToElbow / armToElbowDistance; - glm::vec3 armToHeadPlaneNormal = glm::cross(armToHead, armToHandDir); // The strenght of the resulting pole determined by the arm flex. From e7d70a832d828e57fce679fb4a8dc6a2bed8e8f1 Mon Sep 17 00:00:00 2001 From: luiscuenca Date: Tue, 24 Jul 2018 08:58:17 -0700 Subject: [PATCH 6/6] Pole Vectors always calculated when arms are crossed --- interface/src/avatar/MyAvatar.h | 2 ++ libraries/animation/src/Rig.cpp | 61 ++++++++++++++++++++++++++------- libraries/animation/src/Rig.h | 11 +++++- 3 files changed, 61 insertions(+), 13 deletions(-) diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 9b5ddd360d..e2f4fecde7 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -612,6 +612,8 @@ public: const MyHead* getMyHead() const; + Q_INVOKABLE void toggleSmoothPoleVectors() { _skeletonModel->getRig().toggleSmoothPoleVectors(); }; + /**jsdoc * Get the current position of the avatar's "Head" joint. * @function MyAvatar.getHeadPosition diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index b0a4020c60..87e33ed95d 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1247,6 +1247,9 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo, const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix) { + const bool ENABLE_POLE_VECTORS = true; + const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f; + if (leftHandEnabled) { glm::vec3 handPosition = leftHandPose.trans(); @@ -1266,22 +1269,38 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("RightArm"); - if (handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { + if (ENABLE_POLE_VECTORS && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { glm::vec3 poleVector; bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector); if (usePoleVector) { glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector); + + if (_smoothPoleVectors) { + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftHandPoleVectorValid) { + _prevLeftHandPoleVectorValid = true; + _prevLeftHandPoleVector = sensorPoleVector; + } + glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, sensorPoleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector; + } else { + _prevLeftHandPoleVector = sensorPoleVector; + } _animVars.set("leftHandPoleVectorEnabled", true); _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); - _animVars.set("leftHandPoleVector", transformVectorFast(sensorToRigMatrix, sensorPoleVector)); + _animVars.set("leftHandPoleVector", transformVectorFast(sensorToRigMatrix, _prevLeftHandPoleVector)); } else { + _prevLeftHandPoleVectorValid = false; _animVars.set("leftHandPoleVectorEnabled", false); } } else { + _prevLeftHandPoleVectorValid = false; _animVars.set("leftHandPoleVectorEnabled", false); } } else { + _prevLeftHandPoleVectorValid = false; _animVars.set("leftHandPoleVectorEnabled", false); _animVars.unset("leftHandPosition"); @@ -1309,22 +1328,38 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); - if (handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { + if (ENABLE_POLE_VECTORS && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) { glm::vec3 poleVector; bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector); if (usePoleVector) { glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector); + if (_smoothPoleVectors) { + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightHandPoleVectorValid) { + _prevRightHandPoleVectorValid = true; + _prevRightHandPoleVector = sensorPoleVector; + } + glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, sensorPoleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector; + } else { + _prevRightHandPoleVector = sensorPoleVector; + } + _animVars.set("rightHandPoleVectorEnabled", true); _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); - _animVars.set("rightHandPoleVector", transformVectorFast(sensorToRigMatrix, sensorPoleVector)); + _animVars.set("rightHandPoleVector", transformVectorFast(sensorToRigMatrix, _prevRightHandPoleVector)); } else { + _prevRightHandPoleVectorValid = false; _animVars.set("rightHandPoleVectorEnabled", false); } } else { + _prevRightHandPoleVectorValid = false; _animVars.set("rightHandPoleVectorEnabled", false); } } else { + _prevRightHandPoleVectorValid = false; _animVars.set("rightHandPoleVectorEnabled", false); _animVars.unset("rightHandPosition"); @@ -1487,21 +1522,23 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, glm::vec3 headForward = headCenter + horizontalModule * frontVector; glm::vec3 armToHead = headForward - armPose.trans(); - + float armToHandDistance = glm::length(armToHand); float armToElbowDistance = glm::length(armToElbow); float elbowToHandDistance = glm::length(elbowToHand); - float armTotalDistance = armToElbowDistance + elbowToHandDistance; - // Don't use pole vector when the hands are behind - if (glm::dot(frontVector, armToHand) < 0) { - return false; - } - glm::vec3 armToHandDir = armToHand / armToHandDistance; glm::vec3 armToHeadPlaneNormal = glm::cross(armToHead, armToHandDir); + // How much the hand is reaching for the opposite side + float oppositeProjection = glm::dot(armToHandDir, glm::normalize(backVector)); + + // Don't use pole vector when the hands are behind + if (glm::dot(frontVector, armToHand) < 0 && oppositeProjection < 0.5f * armTotalDistance) { + return false; + } + // The strenght of the resulting pole determined by the arm flex. float armFlexCoeficient = armToHandDistance / armTotalDistance; glm::vec3 attenuationVector = armFlexCoeficient * armToHandDir; @@ -1515,7 +1552,7 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, const float FORWARD_CORRECTOR_WEIGHT = 3.0f; float elbowForwardTrigger = FORWARD_TRIGGER_PERCENTAGE * armToHandDistance; - float oppositeProjection = glm::dot(armToHandDir, glm::normalize(backVector)); + if (oppositeProjection > -elbowForwardTrigger) { float forwardAmount = FORWARD_CORRECTOR_WEIGHT * (elbowForwardTrigger + oppositeProjection); correctionVector = forwardAmount * frontVector; diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index ccbb19a4d3..1a1337fa84 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -217,7 +217,7 @@ public: // input assumed to be in rig space void computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const; - + void toggleSmoothPoleVectors() { _smoothPoleVectors = !_smoothPoleVectors; }; signals: void onLoadComplete(); @@ -245,6 +245,7 @@ protected: glm::vec3 deflectHandFromTorso(const glm::vec3& handPosition, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) const; + AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) AnimPose _invGeometryOffset; @@ -368,6 +369,14 @@ protected: glm::vec3 _prevLeftFootPoleVector { Vectors::UNIT_Z }; // sensor space bool _prevLeftFootPoleVectorValid { false }; + glm::vec3 _prevRightHandPoleVector{ -Vectors::UNIT_Z }; // sensor space + bool _prevRightHandPoleVectorValid{ false }; + + glm::vec3 _prevLeftHandPoleVector{ -Vectors::UNIT_Z }; // sensor space + bool _prevLeftHandPoleVectorValid{ false }; + + bool _smoothPoleVectors { false }; + int _rigId; };