Merge pull request #1057 from machinelevel/dev4

Leap interface and Avatar: left-hand IK works for Leap interface.

interface/src/avatar/Avatar.cpp

@@ -463,7 +463,8 @@ void Avatar::updateHandMovementAndTouching(float deltaTime, bool enableHandMovem
     //constrain right arm length and re-adjust elbow position as it bends
     // NOTE - the following must be called on all avatars - not just _isMine
     if (enableHandMovement) {
-        updateArmIKAndConstraints(deltaTime);
+        updateArmIKAndConstraints(deltaTime, AVATAR_JOINT_RIGHT_FINGERTIPS);
+        updateArmIKAndConstraints(deltaTime, AVATAR_JOINT_LEFT_FINGERTIPS);
     }
 }
 
@@ -640,11 +641,15 @@ void Avatar::updateBodyBalls(float deltaTime) {
         _bodyBall[BODY_BALL_HEAD_TOP].rotation * _skeleton.joint[BODY_BALL_HEAD_TOP].bindPosePosition;
 }
 
-void Avatar::updateArmIKAndConstraints(float deltaTime) {
+void Avatar::updateArmIKAndConstraints(float deltaTime, AvatarJointID fingerTipJointID) {
+    Skeleton::AvatarJoint& fingerJoint = _skeleton.joint[fingerTipJointID];
+    Skeleton::AvatarJoint& wristJoint = _skeleton.joint[fingerJoint.parent];
+    Skeleton::AvatarJoint& elbowJoint = _skeleton.joint[wristJoint.parent];
+    Skeleton::AvatarJoint& shoulderJoint = _skeleton.joint[elbowJoint.parent];
     
     // determine the arm vector
-    glm::vec3 armVector = _skeleton.joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].position;
+    glm::vec3 armVector = fingerJoint.position;
-    armVector -= _skeleton.joint[ AVATAR_JOINT_RIGHT_SHOULDER ].position;
+    armVector -= shoulderJoint.position;
     
     // test to see if right hand is being dragged beyond maximum arm length
     float distance = glm::length(armVector);
@@ -652,28 +657,26 @@ void Avatar::updateArmIKAndConstraints(float deltaTime) {
     // don't let right hand get dragged beyond maximum arm length...
     if (distance > _maxArmLength) {
         // reset right hand to be constrained to maximum arm length
-        _skeleton.joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].position = _skeleton.joint[ AVATAR_JOINT_RIGHT_SHOULDER ].position;
+        fingerJoint.position = shoulderJoint.position;
         glm::vec3 armNormal = armVector / distance;
         armVector = armNormal * _maxArmLength;
         distance = _maxArmLength;
-        glm::vec3 constrainedPosition = _skeleton.joint[ AVATAR_JOINT_RIGHT_SHOULDER ].position;
+        glm::vec3 constrainedPosition = shoulderJoint.position;
         constrainedPosition += armVector;
-        _skeleton.joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].position = constrainedPosition;
+        fingerJoint.position = constrainedPosition;
     }
     
     // set elbow position
-    glm::vec3 newElbowPosition = _skeleton.joint[ AVATAR_JOINT_RIGHT_SHOULDER ].position + armVector * ONE_HALF;
+    glm::vec3 newElbowPosition = shoulderJoint.position + armVector * ONE_HALF;
     
     glm::vec3 perpendicular = glm::cross(getBodyRightDirection(),  armVector);
     
     newElbowPosition += perpendicular * (1.0f - (_maxArmLength / distance)) * ONE_HALF;
-    _skeleton.joint[ AVATAR_JOINT_RIGHT_ELBOW ].position = newElbowPosition;
+    elbowJoint.position = newElbowPosition;
     
     // set wrist position
-    glm::vec3 vv(_skeleton.joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].position);
+    const float wristPosRatio = 0.7f;
-    vv -= _skeleton.joint[ AVATAR_JOINT_RIGHT_ELBOW ].position;
+    wristJoint.position = elbowJoint.position + (fingerJoint.position - elbowJoint.position) * wristPosRatio;
-    glm::vec3 newWristPosition = _skeleton.joint[ AVATAR_JOINT_RIGHT_ELBOW ].position + vv * 0.7f;
-    _skeleton.joint[ AVATAR_JOINT_RIGHT_WRIST ].position = newWristPosition;
 }
 
 glm::quat Avatar::computeRotationFromBodyToWorldUp(float proportion) const {

interface/src/avatar/Avatar.h

@@ -231,7 +231,7 @@ protected:
     glm::vec3 getBodyFrontDirection() const { return getOrientation() * IDENTITY_FRONT; }
     glm::quat computeRotationFromBodyToWorldUp(float proportion = 1.0f) const;
     void updateBodyBalls(float deltaTime);
-    void updateArmIKAndConstraints(float deltaTime);
+    void updateArmIKAndConstraints(float deltaTime, AvatarJointID fingerTipJointID);
     void setScale(const float scale);
 
 

interface/src/avatar/MyAvatar.cpp

@@ -912,24 +912,49 @@ void MyAvatar::updateHandMovementAndTouching(float deltaTime, bool enableHandMov
         _avatarTouch.setHasInteractingOther(false);
     }
     
-    // If there's a leap-interaction hand visible, use that as the endpoint
+    // If there are leap-interaction hands visible, see if we can use them as the endpoints for IK
-    glm::vec3 rightMostHand;
+    if (getHand().getPalms().size() > 0) {
-    bool anyHandsFound = false;
+        PalmData const* leftLeapHand = NULL;
+        PalmData const* rightLeapHand = NULL;
+        // Look through all of the palms available (there may be more than two), and pick
+        // the leftmost and rightmost. If there's only one, we'll use a heuristic below
+        // to decode whether it's the left or right.
         for (size_t i = 0; i < getHand().getPalms().size(); ++i) {
             PalmData& palm = getHand().getPalms()[i];
             if (palm.isActive()) {
-            if (!anyHandsFound || palm.getRawPosition().x > rightMostHand.x) {
+                if (!rightLeapHand || !leftLeapHand) {
-                _skeleton.joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].position = palm.getPosition();
+                    rightLeapHand = leftLeapHand = &palm;
-                rightMostHand = palm.getRawPosition();
                 }
-            anyHandsFound = true;
+                else if (palm.getRawPosition().x > rightLeapHand->getRawPosition().x) {
+                    rightLeapHand = &palm;
+                }
+                else if (palm.getRawPosition().x < leftLeapHand->getRawPosition().x) {
+                    leftLeapHand = &palm;
+                }
+            }
+        }
+        // If there's only one palm visible. Decide if it's the left or right
+        if (leftLeapHand == rightLeapHand && leftLeapHand) {
+            if (leftLeapHand->getRawPosition().x > 0) {
+                leftLeapHand = NULL;
+            }
+            else {
+                rightLeapHand = NULL;
+            }
+        }
+        if (leftLeapHand) {
+            _skeleton.joint[ AVATAR_JOINT_LEFT_FINGERTIPS ].position = leftLeapHand->getPosition();
+        }
+        if (rightLeapHand) {
+            _skeleton.joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].position = rightLeapHand->getPosition();
         }
     }
     
     //constrain right arm length and re-adjust elbow position as it bends
     // NOTE - the following must be called on all avatars - not just _isMine
     if (enableHandMovement) {
-        updateArmIKAndConstraints(deltaTime);
+        updateArmIKAndConstraints(deltaTime, AVATAR_JOINT_RIGHT_FINGERTIPS);
+        updateArmIKAndConstraints(deltaTime, AVATAR_JOINT_LEFT_FINGERTIPS);
     }
     
     //Set right hand position and state to be transmitted, and also tell AvatarTouch about it