Working on testing the Leap/Hydra hands against the avatar body.

interface/src/Util.cpp

@@ -229,7 +229,10 @@ glm::vec3 extractScale(const glm::mat4& matrix) {
 }
 
 float extractUniformScale(const glm::mat4& matrix) {
-    glm::vec3 scale = extractScale(matrix);
+    return extractUniformScale(extractScale(matrix));
+}
+
+float extractUniformScale(const glm::vec3& scale) {
     return (scale.x + scale.y + scale.z) / 3.0f;
 }
 

interface/src/Util.h

@@ -65,6 +65,8 @@ glm::vec3 extractScale(const glm::mat4& matrix);
 
 float extractUniformScale(const glm::mat4& matrix);
 
+float extractUniformScale(const glm::vec3& scale);
+
 double diffclock(timeval *clock1,timeval *clock2);
 
 void renderMouseVoxelGrid(const float& mouseVoxelX, const float& mouseVoxelY, const float& mouseVoxelZ, const float& mouseVoxelS);

interface/src/avatar/Hand.cpp

@@ -97,6 +97,26 @@ void Hand::calculateGeometry() {
     _baseOrientation = _owningAvatar->getOrientation();
     _basePosition = head.calculateAverageEyePosition() + _baseOrientation * leapHandsOffsetFromFace * head.getScale();
     
+    // use position to obtain the left and right palm indices
+    int leftPalmIndex, rightPalmIndex;   
+    getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
+    
+    // check for collisions
+    for (int i = 0; i < getNumPalms(); i++) {
+        PalmData& palm = getPalms()[i];
+        if (!palm.isActive()) {
+            continue;
+        }
+        const float PALM_RADIUS = 0.01f;
+        glm::vec3 penetration;
+        int skipIndex = (i == leftPalmIndex) ? _owningAvatar->getSkeletonModel().getLeftHandJointIndex() :
+            (i == rightPalmIndex) ? _owningAvatar->getSkeletonModel().getRightHandJointIndex() : -1;
+        if (_owningAvatar->getSkeletonModel().findSpherePenetration(palm.getPosition(),
+                PALM_RADIUS * _owningAvatar->getScale(), penetration, skipIndex)) {
+            palm.addToPosition(-penetration);
+        }
+    }
+    
     // generate finger tip balls....
     _leapFingerTipBalls.clear();
     for (size_t i = 0; i < getNumPalms(); ++i) {

interface/src/avatar/SkeletonModel.cpp

@@ -29,24 +29,9 @@ void SkeletonModel::simulate(float deltaTime) {
     Model::simulate(deltaTime);
 
     // find the left and rightmost active Leap palms
-    HandData& hand = _owningAvatar->getHand();
+    int leftPalmIndex, rightPalmIndex;   
-    int leftPalmIndex = -1;
+    HandData& hand = _owningAvatar->getHand(); 
-    float leftPalmX = FLT_MAX;
+    hand.getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
-    int rightPalmIndex = -1;    
-    float rightPalmX = -FLT_MAX;
-    for (int i = 0; i < hand.getNumPalms(); i++) {
-        if (hand.getPalms()[i].isActive()) {
-            float x = hand.getPalms()[i].getRawPosition().x;
-            if (x < leftPalmX) {
-                leftPalmIndex = i;
-                leftPalmX = x;
-            }
-            if (x > rightPalmX) {
-                rightPalmIndex = i;
-                rightPalmX = x;
-            }
-        }
-    }
     
     const float HAND_RESTORATION_RATE = 0.25f;
     

interface/src/renderer/Model.cpp

@@ -8,6 +8,8 @@
 
 #include <glm/gtx/transform.hpp>
 
+#include <GeometryUtil.h>
+
 #include "Application.h"
 #include "Model.h"
 
@@ -474,35 +476,35 @@ bool Model::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePos
 }
 
 bool Model::setLeftHandPosition(const glm::vec3& position) {
-    return isActive() && setJointPosition(_geometry->getFBXGeometry().leftHandJointIndex, position);
+    return setJointPosition(getLeftHandJointIndex(), position);
 }
 
 bool Model::restoreLeftHandPosition(float percent) {
-    return isActive() && restoreJointPosition(_geometry->getFBXGeometry().leftHandJointIndex, percent);
+    return restoreJointPosition(getLeftHandJointIndex(), percent);
 }
 
 bool Model::setLeftHandRotation(const glm::quat& rotation) {
-    return isActive() && setJointRotation(_geometry->getFBXGeometry().leftHandJointIndex, rotation);
+    return setJointRotation(getLeftHandJointIndex(), rotation);
 }
 
 float Model::getLeftArmLength() const {
-    return isActive() ? getLimbLength(_geometry->getFBXGeometry().leftHandJointIndex) : 0.0f;
+    return getLimbLength(getLeftHandJointIndex());
 }
 
 bool Model::setRightHandPosition(const glm::vec3& position) {
-    return isActive() && setJointPosition(_geometry->getFBXGeometry().rightHandJointIndex, position);
+    return setJointPosition(getRightHandJointIndex(), position);
 }
 
 bool Model::restoreRightHandPosition(float percent) {
-    return isActive() && restoreJointPosition(_geometry->getFBXGeometry().rightHandJointIndex, percent);
+    return restoreJointPosition(getRightHandJointIndex(), percent);
 }
 
 bool Model::setRightHandRotation(const glm::quat& rotation) {
-    return isActive() && setJointRotation(_geometry->getFBXGeometry().rightHandJointIndex, rotation);
+    return setJointRotation(getRightHandJointIndex(), rotation);
 }
 
 float Model::getRightArmLength() const {
-    return isActive() ? getLimbLength(_geometry->getFBXGeometry().rightHandJointIndex) : 0.0f;
+    return getLimbLength(getRightHandJointIndex());
 }
 
 void Model::setURL(const QUrl& url) {
@@ -523,6 +525,48 @@ glm::vec4 Model::computeAverageColor() const {
     return _geometry ? _geometry->computeAverageColor() : glm::vec4(1.0f, 1.0f, 1.0f, 1.0f);
 }
 
+bool Model::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+        glm::vec3& penetration, int skipIndex) const {
+    const glm::vec3 relativeCenter = penetratorCenter - _translation;
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    bool didPenetrate = false;
+    glm::vec3 totalPenetration;
+    float radiusScale = extractUniformScale(_scale);
+    for (int i = 0; i < _jointStates.size(); i++) {
+        const FBXJoint& joint = geometry.joints[i];
+        glm::vec3 end = extractTranslation(_jointStates[i].transform);
+        float endRadius = joint.boneRadius * radiusScale;
+        glm::vec3 start = end;
+        float startRadius = joint.boneRadius * radiusScale;
+        glm::vec3 bonePenetration;
+        if (joint.parentIndex != -1) {
+            if (skipIndex != -1) {
+                int ancestorIndex = joint.parentIndex;
+                do {
+                    if (ancestorIndex == skipIndex) {
+                        goto outerContinue;
+                    }
+                    ancestorIndex = geometry.joints[ancestorIndex].parentIndex;
+                    
+                } while (ancestorIndex != -1);
+            }
+            start = extractTranslation(_jointStates[joint.parentIndex].transform);
+            startRadius = geometry.joints[joint.parentIndex].boneRadius;
+        }
+        if (findSphereCapsulePenetration(relativeCenter, penetratorRadius, start, end,
+                (startRadius + endRadius) / 2.0f, bonePenetration)) {
+            totalPenetration = addPenetrations(totalPenetration, bonePenetration);
+            didPenetrate = true; 
+        }
+        outerContinue: ;
+    }
+    if (didPenetrate) {
+        penetration = totalPenetration;
+        return true;
+    }
+    return false;
+}
+
 void Model::updateJointState(int index) {
     JointState& state = _jointStates[index];
     const FBXGeometry& geometry = _geometry->getFBXGeometry();

interface/src/renderer/Model.h

@@ -54,6 +54,12 @@ public:
     Q_INVOKABLE void setURL(const QUrl& url);
     const QUrl& getURL() const { return _url; }
     
+    /// Returns the index of the left hand joint, or -1 if not found.
+    int getLeftHandJointIndex() const { return isActive() ? _geometry->getFBXGeometry().leftHandJointIndex : -1; }
+    
+    /// Returns the index of the right hand joint, or -1 if not found.
+    int getRightHandJointIndex() const { return isActive() ? _geometry->getFBXGeometry().rightHandJointIndex : -1; }
+    
     /// Returns the position of the head joint.
     /// \return whether or not the head was found
     bool getHeadPosition(glm::vec3& headPosition) const;
@@ -105,6 +111,9 @@ public:
     /// Returns the average color of all meshes in the geometry.
     glm::vec4 computeAverageColor() const;
 
+    bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+        glm::vec3& penetration, int skipIndex = -1) const;
+
 protected:
 
     QSharedPointer<NetworkGeometry> _geometry;

libraries/avatars/src/HandData.cpp

@@ -29,11 +29,40 @@ HandData::HandData(AvatarData* owningAvatar) :
     addNewPalm();
 }
 
+glm::vec3 HandData::worldPositionToLeapPosition(const glm::vec3& worldPosition) const {
+    return glm::inverse(_baseOrientation) * (worldPosition - _basePosition) / LEAP_UNIT_SCALE;
+}
+
+glm::vec3 HandData::worldVectorToLeapVector(const glm::vec3& worldVector) const {
+    return glm::inverse(_baseOrientation) * worldVector / LEAP_UNIT_SCALE;
+}
+
 PalmData& HandData::addNewPalm()  {
     _palms.push_back(PalmData(this));
     return _palms.back();
 }
 
+void HandData::getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex) const {
+    leftPalmIndex = -1;
+    float leftPalmX = FLT_MAX;
+    rightPalmIndex = -1;    
+    float rightPalmX = -FLT_MAX;
+    for (int i = 0; i < _palms.size(); i++) {
+        const PalmData& palm = _palms[i];
+        if (palm.isActive()) {
+            float x = palm.getRawPosition().x;
+            if (x < leftPalmX) {
+                leftPalmIndex = i;
+                leftPalmX = x;
+            }
+            if (x > rightPalmX) {
+                rightPalmIndex = i;
+                rightPalmX = x;
+            }
+        }
+    }
+}
+
 PalmData::PalmData(HandData* owningHandData) :
 _rawPosition(0, 0, 0),
 _rawNormal(0, 1, 0),
@@ -49,6 +78,19 @@ _owningHandData(owningHandData)
     }
 }
 
+void PalmData::addToPosition(const glm::vec3& delta) {
+    // convert to Leap coordinates, then add to palm and finger positions
+    glm::vec3 leapDelta = _owningHandData->worldVectorToLeapVector(delta);
+    _rawPosition += leapDelta;
+    for (int i = 0; i < getNumFingers(); i++) {
+        FingerData& finger = _fingers[i];
+        if (finger.isActive()) {
+            finger.setRawTipPosition(finger.getTipRawPosition() + leapDelta);
+            finger.setRawRootPosition(finger.getRootRawPosition() + leapDelta);
+        }
+    }
+}
+
 FingerData::FingerData(PalmData* owningPalmData, HandData* owningHandData) :
 _tipRawPosition(0, 0, 0),
 _rootRawPosition(0, 0, 0),

libraries/avatars/src/HandData.h

@@ -47,6 +47,8 @@ const int BUTTON_3 = 8;
 const int BUTTON_4 = 16;
 const int BUTTON_FWD = 128;
 
+const float LEAP_UNIT_SCALE = 0.001f; ///< convert mm to meters
+
 class HandData {
 public:
     HandData(AvatarData* owningAvatar);
@@ -54,20 +56,25 @@ public:
     
     // These methods return the positions in Leap-relative space.
     // To convert to world coordinates, use Hand::leapPositionToWorldPosition.
-    
+
     // position conversion
     glm::vec3 leapPositionToWorldPosition(const glm::vec3& leapPosition) {
-        const float unitScale = 0.001;            // convert mm to meters
+        return _basePosition + _baseOrientation * (leapPosition * LEAP_UNIT_SCALE);
-        return _basePosition + _baseOrientation * (leapPosition * unitScale);
     }
     glm::vec3 leapDirectionToWorldDirection(const glm::vec3& leapDirection) {
         return glm::normalize(_baseOrientation * leapDirection);
     }
+    glm::vec3 worldPositionToLeapPosition(const glm::vec3& worldPosition) const;
+    glm::vec3 worldVectorToLeapVector(const glm::vec3& worldVector) const;
 
     std::vector<PalmData>& getPalms()    { return _palms; }
     size_t                 getNumPalms() { return _palms.size(); }
     PalmData&              addNewPalm();
 
+    /// Finds the indices of the left and right palms according to their locations, or -1 if either or
+    /// both is not found.
+    void getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex) const;
+
     void setFingerTrailLength(unsigned int length);
     void updateFingerTrails();
 
@@ -153,6 +160,8 @@ public:
     void setVelocity(const glm::vec3& velocity) { _velocity = velocity; }
     const glm::vec3& getVelocity()  const { return _velocity; }
 
+    void addToPosition(const glm::vec3& delta);
+
     void incrementFramesWithoutData()          { _numFramesWithoutData++; }
     void resetFramesWithoutData()              { _numFramesWithoutData = 0; }
     int  getFramesWithoutData()          const { return _numFramesWithoutData; }