Redesign and enable hand's Pole Vectors for elbow simulation

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

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;
+    float avatarScale = extractUniformScale(_modelOffset);
-    const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f;
 
     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) {
+        int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("RightArm");
-            glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true);
+        if (_enablePoleVectors && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) {
-            glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector);
+            glm::vec3 poleVector;
-
+            bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector);
-            // smooth toward desired pole vector from previous pole vector...  to reduce jitter
+            if (usePoleVector) {
-            if (!_prevLeftHandPoleVectorValid) {
+                glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector);
-                _prevLeftHandPoleVectorValid = true;
+                _animVars.set("leftHandPoleVectorEnabled", true);
-                _prevLeftHandPoleVector = sensorPoleVector;
+                _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) {
+        int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("LeftArm");
-            glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false);
-            glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector);
 
-            // smooth toward desired pole vector from previous pole vector...  to reduce jitter
+        if (_enablePoleVectors && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) {
-            if (!_prevRightHandPoleVectorValid) {
+            glm::vec3 poleVector;
-                _prevRightHandPoleVectorValid = true;
+            bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector);
-                _prevRightHandPoleVector = sensorPoleVector;
+            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 {
+bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& poleVector) const {
-    AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex];
+    // The resulting Pole Vector is calculated as the sum of a three vectors.
-    AnimPose handPose = _externalPoseSet._absolutePoses[handIndex];
+    // 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.
-    AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex];
+    // The second vector is always perpendicular to previous vector and is part of the plane that contains a point located on the horizontal line,
-    AnimPose armPose = _externalPoseSet._absolutePoses[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.
 
-    // 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.
+    AnimPose oppositeArmPose = _externalPoseSet._absolutePoses[oppositeArmIndex];
-    glm::vec3 poleVector = glm::angleAxis(PI / 2.0f, n) * dProjWithOffset;
+    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::vec3 armToHand = handPose.trans() - armPose.trans();
-    glm::quat elbowToHandDelta = handPose.rot() * glm::inverse(elbowPose.rot());
+    glm::vec3 armToElbow = elbowPose.trans() - armPose.trans();
-    const float WRIST_POLE_ADJUST_FACTOR = 0.5f;
+    glm::vec3 elbowToHand = handPose.trans() - elbowPose.trans();
-    glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, elbowToHandDelta, WRIST_POLE_ADJUST_FACTOR);
 
-    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 {

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<float>& 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 _enablePoleVectors{ true };
-    bool _prevRightHandPoleVectorValid { false };
-
-    glm::vec3 _prevLeftHandPoleVector { -Vectors::UNIT_Z }; // sensor space
-    bool _prevLeftHandPoleVectorValid { false };
 
     int _rigId;
 };