remove world-frame hand/palm set methods

interface/src/avatar/SkeletonModel.cpp

@@ -70,7 +70,7 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
 
     } else if (leftPalmIndex == rightPalmIndex) {
         // right hand only
-        applyPalmData(geometry.rightHandJointIndex, hand->getPalms()[leftPalmIndex]);
+        applyPalmDataInModelFrame(geometry.rightHandJointIndex, hand->getPalms()[leftPalmIndex]);
         restoreLeftHandPosition(HAND_RESTORATION_RATE, PALM_PRIORITY);
 
     } else {
@@ -132,29 +132,6 @@ bool operator<(const IndexValue& firstIndex, const IndexValue& secondIndex) {
     return firstIndex.value < secondIndex.value;
 }
 
-void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) {
-    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
-        return;
-    }
-    setJointPosition(jointIndex, position, glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
-
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
-    glm::vec3 handPosition, elbowPosition;
-    getJointPosition(jointIndex, handPosition);
-    getJointPosition(geometry.joints.at(jointIndex).parentIndex, elbowPosition);
-    glm::vec3 forearmVector = handPosition - elbowPosition;
-    float forearmLength = glm::length(forearmVector);
-    if (forearmLength < EPSILON) {
-        return;
-    }
-    JointState& state = _jointStates[jointIndex];
-    glm::quat handRotation = state.getJointRotation();
-
-    // align hand with forearm
-    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
-    state.applyRotationDelta(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY);
-}
-
 void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec3& position) {
     if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
         return;
@@ -178,52 +155,6 @@ void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec
     state.applyRotationDeltaInModelFrame(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY);
 }
 
-void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
-    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
-        return;
-    }
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
-    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
-    int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
-    if (parentJointIndex == -1) {
-        return;
-    }
-    
-    // rotate palm to align with its normal (normal points out of hand's palm)
-    glm::quat palmRotation;
-    if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) &&
-            Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
-        JointState parentState = _jointStates[parentJointIndex];
-        palmRotation = parentState.getJointRotation();
-    } else {
-        JointState state = _jointStates[jointIndex];
-        palmRotation = state.getJointRotation();
-    }
-    palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
-    
-    // rotate palm to align with finger direction
-    glm::vec3 direction = palm.getFingerDirection();
-    palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
-
-    // set hand position, rotation
-    if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) {
-        setHandPosition(jointIndex, palm.getPosition(), palmRotation);  
-        
-    } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
-        glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
-        setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
-            geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale),
-            glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
-        JointState& parentState = _jointStates[parentJointIndex];
-        parentState.setRotation(palmRotation, PALM_PRIORITY);
-        // slam parent-relative rotation to identity
-        _jointStates[jointIndex]._rotation = glm::quat();
-    } else {
-        setJointPosition(jointIndex, palm.getPosition(), palmRotation,
-            true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
-    }
-}
-
 void SkeletonModel::applyPalmDataInModelFrame(int jointIndex, PalmData& palm) {
     if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
         return;
@@ -377,70 +308,6 @@ void SkeletonModel::renderJointConstraints(int jointIndex) {
     glLineWidth(1.0f);
 }
 
-void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) {
-    // this algorithm is from sample code from sixense
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
-    int elbowJointIndex = geometry.joints.at(jointIndex).parentIndex;
-    if (elbowJointIndex == -1) {
-        return;
-    }
-    int shoulderJointIndex = geometry.joints.at(elbowJointIndex).parentIndex;
-    glm::vec3 shoulderPosition;
-    if (!getJointPosition(shoulderJointIndex, shoulderPosition)) {
-        return;
-    }
-    // precomputed lengths
-    float scale = extractUniformScale(_scale);
-    float upperArmLength = geometry.joints.at(elbowJointIndex).distanceToParent * scale;
-    float lowerArmLength = geometry.joints.at(jointIndex).distanceToParent * scale;
-    
-    // first set wrist position
-    glm::vec3 wristPosition = position;
-    
-    glm::vec3 shoulderToWrist = wristPosition - shoulderPosition;
-    float distanceToWrist = glm::length(shoulderToWrist);
-    
-    // prevent gimbal lock
-    if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) {
-        distanceToWrist = upperArmLength + lowerArmLength - EPSILON;
-        shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist;
-        wristPosition = shoulderPosition + shoulderToWrist;
-    }
-    
-    // cosine of angle from upper arm to hand vector 
-    float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) / 
-        (2 * upperArmLength * distanceToWrist);
-    float mid = upperArmLength * cosA;
-    float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA);
-    
-    // direction of the elbow
-    glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist
-    glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down
-    const float NORMAL_WEIGHT = 0.5f;
-    glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT);
-    
-    bool rightHand = (jointIndex == geometry.rightHandJointIndex);
-    if (rightHand ? (finalNormal.y > 0.0f) : (finalNormal.y < 0.0f)) {
-        finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis)
-    }
-    
-    glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal));
-    
-    // ik solution
-    glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height;
-    glm::vec3 forwardVector(rightHand ? -1.0f : 1.0f, 0.0f, 0.0f);
-    glm::quat shoulderRotation = rotationBetween(forwardVector, elbowPosition - shoulderPosition);
-
-    JointState& shoulderState = _jointStates[shoulderJointIndex];
-    shoulderState.setRotation(shoulderRotation, PALM_PRIORITY);
-    
-    JointState& elbowState = _jointStates[elbowJointIndex];
-    elbowState.setRotation(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY);
-    
-    JointState& handState = _jointStates[jointIndex];
-    handState.setRotation(rotation, PALM_PRIORITY);
-}
-
 void SkeletonModel::setHandPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation) {
     // this algorithm is from sample code from sixense
     const FBXGeometry& geometry = _geometry->getFBXGeometry();

interface/src/avatar/SkeletonModel.h

@@ -91,10 +91,8 @@ public:
     
 protected:
 
-    void applyHandPosition(int jointIndex, const glm::vec3& position);
     void applyHandPositionInModelFrame(int jointIndex, const glm::vec3& position);
     
-    void applyPalmData(int jointIndex, PalmData& palm);
     void applyPalmDataInModelFrame(int jointIndex, PalmData& palm);
     
     /// Updates the state of the joint at the specified index.
@@ -107,7 +105,6 @@ protected:
 private:
 
     void renderJointConstraints(int jointIndex);
-    void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation);
     void setHandPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation);
     
     Avatar* _owningAvatar;

interface/src/renderer/Model.cpp

@@ -1288,95 +1288,6 @@ void Model::updateJointState(int index) {
     }
 }
 
-bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const glm::quat& rotation, bool useRotation,
-       int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment, float priority) {
-    if (jointIndex == -1 || _jointStates.isEmpty()) {
-        return false;
-    }
-    glm::vec3 relativePosition = translation - _translation;
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
-    const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
-    if (freeLineage.isEmpty()) {
-        return false;
-    }
-    if (lastFreeIndex == -1) {
-        lastFreeIndex = freeLineage.last();
-    }
-    
-    // this is a cyclic coordinate descent algorithm: see
-    // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
-    const int ITERATION_COUNT = 1;
-    glm::vec3 worldAlignment = _rotation * alignment;
-    for (int i = 0; i < ITERATION_COUNT; i++) {
-        // first, try to rotate the end effector as close as possible to the target rotation, if any
-        glm::quat endRotation;
-        if (useRotation) {
-            JointState& state = _jointStates[jointIndex];
-
-            // TODO: figure out what this is trying to do and combine it into one JointState method
-            endRotation = state.getJointRotation();
-            state.applyRotationDelta(rotation * glm::inverse(endRotation), true, priority);
-            endRotation = state.getJointRotation();
-        }    
-        
-        // then, we go from the joint upwards, rotating the end as close as possible to the target
-        glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].getHybridTransform());
-        for (int j = 1; freeLineage.at(j - 1) != lastFreeIndex; j++) {
-            int index = freeLineage.at(j);
-            JointState& state = _jointStates[index];
-            const FBXJoint& joint = state.getFBXJoint();
-            if (!(joint.isFree || allIntermediatesFree)) {
-                continue;
-            }
-            glm::vec3 jointPosition = extractTranslation(state.getHybridTransform());
-            glm::vec3 jointVector = endPosition - jointPosition;
-            glm::quat oldCombinedRotation = _rotation * state.getRotationInModelFrame();
-            glm::quat combinedDelta;
-            float combinedWeight;
-            if (useRotation) {
-                combinedDelta = safeMix(rotation * glm::inverse(endRotation),
-                    rotationBetween(jointVector, relativePosition - jointPosition), 0.5f);
-                combinedWeight = 2.0f;
-                
-            } else {
-                combinedDelta = rotationBetween(jointVector, relativePosition - jointPosition);
-                combinedWeight = 1.0f;
-            }
-            if (alignment != glm::vec3() && j > 1) {
-                jointVector = endPosition - jointPosition;
-                glm::vec3 positionSum;
-                for (int k = j - 1; k > 0; k--) {
-                    int index = freeLineage.at(k);
-                    updateJointState(index);
-                    positionSum += extractTranslation(_jointStates.at(index).getHybridTransform());
-                }
-                glm::vec3 projectedCenterOfMass = glm::cross(jointVector,
-                    glm::cross(positionSum / (j - 1.0f) - jointPosition, jointVector));
-                glm::vec3 projectedAlignment = glm::cross(jointVector, glm::cross(worldAlignment, jointVector));
-                const float LENGTH_EPSILON = 0.001f;
-                if (glm::length(projectedCenterOfMass) > LENGTH_EPSILON && glm::length(projectedAlignment) > LENGTH_EPSILON) {
-                    combinedDelta = safeMix(combinedDelta, rotationBetween(projectedCenterOfMass, projectedAlignment),
-                        1.0f / (combinedWeight + 1.0f));
-                }
-            }
-            state.applyRotationDelta(combinedDelta, true, priority);
-            glm::quat actualDelta = _rotation * state.getRotationInModelFrame() * glm::inverse(oldCombinedRotation);
-            endPosition = actualDelta * jointVector + jointPosition;
-            if (useRotation) {
-                endRotation = actualDelta * endRotation;
-            }
-        }       
-    }
-     
-    // now update the joint states from the top
-    for (int j = freeLineage.size() - 1; j >= 0; j--) {
-        updateJointState(freeLineage.at(j));
-    }
-    _shapesAreDirty = true;
-        
-    return true;
-}
-
 bool Model::setJointPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation, bool useRotation,
        int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment, float priority) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
@@ -1614,9 +1525,10 @@ void Model::applyCollision(CollisionInfo& collision) {
                 axis = glm::normalize(axis);
                 glm::vec3 end;
                 getJointPosition(jointIndex, end);
-                glm::vec3 newEnd = start + glm::angleAxis(angle, axis) * (end - start);
+                // transform into model-frame
+                glm::vec3 newEnd = glm::inverse(_rotation) * (start + glm::angleAxis(angle, axis) * (end - start) - _translation);
                 // try to move it
-                setJointPosition(jointIndex, newEnd, glm::quat(), false, -1, true);
+                setJointPositionInModelFrame(jointIndex, newEnd, glm::quat(), false, -1, true);
             }
         }
     }

interface/src/renderer/Model.h

@@ -176,6 +176,7 @@ public:
     int getLastFreeJointIndex(int jointIndex) const;
     
     bool getJointPosition(int jointIndex, glm::vec3& position) const;
+
     bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const;
     bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const;
 
@@ -268,10 +269,6 @@ protected:
     /// Updates the state of the joint at the specified index.
     virtual void updateJointState(int index);
     
-    bool setJointPosition(int jointIndex, const glm::vec3& translation, const glm::quat& rotation = glm::quat(),
-        bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false,
-        const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f);
-
     bool setJointPositionInModelFrame(int jointIndex, const glm::vec3& translation, const glm::quat& rotation = glm::quat(),
         bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false,
         const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f);