whenever Model class uses _jointState, use the Rig version if there is a Rig.

interface/src/avatar/SkeletonModel.cpp

@@ -269,21 +269,22 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
     }
 }
 
+
 void SkeletonModel::updateJointState(int index) {
-    if (index > _jointStates.size()) {
+    if (index < 0 && index >= _jointStates.size()) {
         return; // bail
     }
     JointState& state = _jointStates[index];
     const FBXJoint& joint = state.getFBXJoint();
-    if (joint.parentIndex != -1 && joint.parentIndex <= _jointStates.size()) {
+    if (joint.parentIndex >= 0 && joint.parentIndex < _jointStates.size()) {
         const JointState& parentState = _jointStates.at(joint.parentIndex);
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
         if (index == geometry.leanJointIndex) {
             maybeUpdateLeanRotation(parentState, state);
-        
+
         } else if (index == geometry.neckJointIndex) {
-            maybeUpdateNeckRotation(parentState, joint, state);    
-                
+            maybeUpdateNeckRotation(parentState, joint, state);
+
         } else if (index == geometry.leftEyeJointIndex || index == geometry.rightEyeJointIndex) {
             maybeUpdateEyeRotation(parentState, joint, state);
         }
@@ -296,6 +297,7 @@ void SkeletonModel::updateJointState(int index) {
     }
 }
 
+
 void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, JointState& state) {
     if (!_owningAvatar->isMyAvatar()) {
         return;

interface/src/avatar/SkeletonModel.h

@@ -161,7 +161,7 @@ private:
     void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation);
 
     bool getEyeModelPositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const;
-    
+
     Avatar* _owningAvatar;
 
     CapsuleShape _boundingShape;

libraries/animation/src/Rig.cpp

@@ -35,7 +35,7 @@ bool Rig::isRunningAnimation(AnimationHandlePointer animationHandle) {
     return _runningAnimations.contains(animationHandle);
 }
 
-void Rig::initJointStates(glm::vec3 scale, glm::vec3 offset, QVector<JointState> states) {
+float Rig::initJointStates(glm::vec3 scale, glm::vec3 offset, QVector<JointState> states) {
     _jointStates = states;
     initJointTransforms(scale, offset);
 
@@ -53,6 +53,7 @@ void Rig::initJointStates(glm::vec3 scale, glm::vec3 offset, QVector<JointState>
     }
 
     // XXX update AnimationHandles from here?
+    return radius;
 }
 
 void Rig::initJointTransforms(glm::vec3 scale, glm::vec3 offset) {
@@ -86,8 +87,360 @@ void Rig::resetJoints() {
     }
 }
 
+bool Rig::getJointState(int index, glm::quat& rotation) const {
+    if (index == -1 || index >= _jointStates.size()) {
+        return false;
+    }
+    const JointState& state = _jointStates.at(index);
+    rotation = state.getRotationInConstrainedFrame();
+    return !state.rotationIsDefault(rotation);
+}
+
+bool Rig::getVisibleJointState(int index, glm::quat& rotation) const {
+    if (index == -1 || index >= _jointStates.size()) {
+        return false;
+    }
+    const JointState& state = _jointStates.at(index);
+    rotation = state.getVisibleRotationInConstrainedFrame();
+    return !state.rotationIsDefault(rotation);
+}
+
+void Rig::updateVisibleJointStates() {
+    for (int i = 0; i < _jointStates.size(); i++) {
+        _jointStates[i].slaveVisibleTransform();
+    }
+}
+
+void Rig::clearJointState(int index) {
+    if (index != -1 && index < _jointStates.size()) {
+        JointState& state = _jointStates[index];
+        state.setRotationInConstrainedFrame(glm::quat(), 0.0f);
+    }
+}
+
+void Rig::clearJointStates() {
+    _jointStates.clear();
+}
+
+void Rig::setJointState(int index, bool valid, const glm::quat& rotation, float priority) {
+    if (index != -1 && index < _jointStates.size()) {
+        JointState& state = _jointStates[index];
+        if (valid) {
+            state.setRotationInConstrainedFrame(rotation, priority);
+        } else {
+            state.restoreRotation(1.0f, priority);
+        }
+    }
+}
+
+
+void Rig::clearJointAnimationPriority(int index) {
+    if (index != -1 && index < _jointStates.size()) {
+        _jointStates[index]._animationPriority = 0.0f;
+    }
+}
+
 AnimationHandlePointer Rig::createAnimationHandle() {
     AnimationHandlePointer handle(new AnimationHandle(getRigPointer()));
     _animationHandles.insert(handle);
     return handle;
 }
+
+bool Rig::getJointStateAtIndex(int jointIndex, JointState& jointState) const {
+    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
+        return false;
+    }
+    jointState = _jointStates[jointIndex];
+    return true;
+}
+
+void Rig::updateJointStates(glm::mat4 parentTransform) {
+    for (int i = 0; i < _jointStates.size(); i++) {
+        updateJointState(i, parentTransform);
+    }
+}
+
+void Rig::updateJointState(int index, glm::mat4 parentTransform) {
+    JointState& state = _jointStates[index];
+    const FBXJoint& joint = state.getFBXJoint();
+
+    // compute model transforms
+    int parentIndex = joint.parentIndex;
+    if (parentIndex == -1) {
+        // glm::mat4 parentTransform = glm::scale(scale) * glm::translate(offset) * geometryOffset;
+        state.computeTransform(parentTransform);
+    } else {
+        // guard against out-of-bounds access to _jointStates
+        if (joint.parentIndex >= 0 && joint.parentIndex < _jointStates.size()) {
+            const JointState& parentState = _jointStates.at(parentIndex);
+            state.computeTransform(parentState.getTransform(), parentState.getTransformChanged());
+        }
+    }
+}
+
+void Rig::resetAllTransformsChanged() {
+    for (int i = 0; i < _jointStates.size(); i++) {
+        _jointStates[i].resetTransformChanged();
+    }
+}
+
+glm::quat Rig::setJointRotationInBindFrame(int jointIndex, const glm::quat& rotation, float priority) {
+    glm::quat endRotation;
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return endRotation;
+    }
+    JointState& state = _jointStates[jointIndex];
+    state.setRotationInBindFrame(rotation, priority);
+    endRotation = state.getRotationInBindFrame();
+    return endRotation;
+}
+
+void Rig::applyJointRotationDelta(int jointIndex, const glm::quat& delta, bool constrain, float priority) {
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return;
+    }
+    _jointStates[jointIndex].applyRotationDelta(delta, constrain, priority);
+}
+
+bool Rig::setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation,
+                           bool useRotation, int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment,
+                           float priority, glm::mat4 parentTransform) {
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return false;
+    }
+
+    // const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    // const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
+    const FBXJoint& fbxJoint = _jointStates[jointIndex].getFBXJoint();
+    const QVector<int>& freeLineage = fbxJoint.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 = 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];
+
+            state.setRotationInBindFrame(rotation, priority);
+            endRotation = state.getRotationInBindFrame();
+        }
+
+        // then, we go from the joint upwards, rotating the end as close as possible to the target
+        glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].getTransform());
+        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.getTransform());
+            glm::vec3 jointVector = endPosition - jointPosition;
+            glm::quat oldCombinedRotation = state.getRotation();
+            glm::quat combinedDelta;
+            float combinedWeight;
+            if (useRotation) {
+                combinedDelta = safeMix(rotation * glm::inverse(endRotation),
+                    rotationBetween(jointVector, position - jointPosition), 0.5f);
+                combinedWeight = 2.0f;
+
+            } else {
+                combinedDelta = rotationBetween(jointVector, position - 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, parentTransform);
+                    positionSum += extractTranslation(_jointStates.at(index).getTransform());
+                }
+                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 = state.getRotation() * 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), parentTransform);
+    }
+
+    return true;
+}
+
+void Rig::inverseKinematics(int endIndex, glm::vec3 targetPosition,
+                            const glm::quat& targetRotation, float priority, glm::mat4 parentTransform) {
+    // NOTE: targetRotation is from bind- to model-frame
+
+    if (endIndex == -1 || _jointStates.isEmpty()) {
+        return;
+    }
+
+    // const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    // const QVector<int>& freeLineage = geometry.joints.at(endIndex).freeLineage;
+    const FBXJoint& fbxJoint = _jointStates[endIndex].getFBXJoint();
+    const QVector<int>& freeLineage = fbxJoint.freeLineage;
+
+    if (freeLineage.isEmpty()) {
+        return;
+    }
+    int numFree = freeLineage.size();
+
+    // store and remember topmost parent transform
+    glm::mat4 topParentTransform;
+    {
+        int index = freeLineage.last();
+        const JointState& state = _jointStates.at(index);
+        const FBXJoint& joint = state.getFBXJoint();
+        int parentIndex = joint.parentIndex;
+        if (parentIndex == -1) {
+            topParentTransform = parentTransform;
+        } else {
+            topParentTransform = _jointStates[parentIndex].getTransform();
+        }
+    }
+
+    // this is a cyclic coordinate descent algorithm: see
+    // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
+
+    // keep track of the position of the end-effector
+    JointState& endState = _jointStates[endIndex];
+    glm::vec3 endPosition = endState.getPosition();
+    float distanceToGo = glm::distance(targetPosition, endPosition);
+
+    const int MAX_ITERATION_COUNT = 2;
+    const float ACCEPTABLE_IK_ERROR = 0.005f; // 5mm
+    int numIterations = 0;
+    do {
+        ++numIterations;
+        // moving up, rotate each free joint to get endPosition closer to target
+        for (int j = 1; j < numFree; j++) {
+            int nextIndex = freeLineage.at(j);
+            JointState& nextState = _jointStates[nextIndex];
+            FBXJoint nextJoint = nextState.getFBXJoint();
+            if (! nextJoint.isFree) {
+                continue;
+            }
+
+            glm::vec3 pivot = nextState.getPosition();
+            glm::vec3 leverArm = endPosition - pivot;
+            float leverLength = glm::length(leverArm);
+            if (leverLength < EPSILON) {
+                continue;
+            }
+            glm::quat deltaRotation = rotationBetween(leverArm, targetPosition - pivot);
+
+            // We want to mix the shortest rotation with one that will pull the system down with gravity
+            // so that limbs don't float unrealistically.  To do this we compute a simplified center of mass
+            // where each joint has unit mass and we don't bother averaging it because we only need direction.
+            if (j > 1) {
+
+                glm::vec3 centerOfMass(0.0f);
+                for (int k = 0; k < j; ++k) {
+                    int massIndex = freeLineage.at(k);
+                    centerOfMass += _jointStates[massIndex].getPosition() - pivot;
+                }
+                // the gravitational effect is a rotation that tends to align the two cross products
+                const glm::vec3 worldAlignment = glm::vec3(0.0f, -1.0f, 0.0f);
+                glm::quat gravityDelta = rotationBetween(glm::cross(centerOfMass, leverArm),
+                    glm::cross(worldAlignment, leverArm));
+
+                float gravityAngle = glm::angle(gravityDelta);
+                const float MIN_GRAVITY_ANGLE = 0.1f;
+                float mixFactor = 0.5f;
+                if (gravityAngle < MIN_GRAVITY_ANGLE) {
+                    // the final rotation is a mix of the two
+                    mixFactor = 0.5f * gravityAngle / MIN_GRAVITY_ANGLE;
+                }
+                deltaRotation = safeMix(deltaRotation, gravityDelta, mixFactor);
+            }
+
+            // Apply the rotation, but use mixRotationDelta() which blends a bit of the default pose
+            // in the process.  This provides stability to the IK solution for most models.
+            glm::quat oldNextRotation = nextState.getRotation();
+            float mixFactor = 0.03f;
+            nextState.mixRotationDelta(deltaRotation, mixFactor, priority);
+
+            // measure the result of the rotation which may have been modified by
+            // blending and constraints
+            glm::quat actualDelta = nextState.getRotation() * glm::inverse(oldNextRotation);
+            endPosition = pivot + actualDelta * leverArm;
+        }
+
+        // recompute transforms from the top down
+        glm::mat4 parentTransform = topParentTransform;
+        for (int j = numFree - 1; j >= 0; --j) {
+            JointState& freeState = _jointStates[freeLineage.at(j)];
+            freeState.computeTransform(parentTransform);
+            parentTransform = freeState.getTransform();
+        }
+
+        // measure our success
+        endPosition = endState.getPosition();
+        distanceToGo = glm::distance(targetPosition, endPosition);
+    } while (numIterations < MAX_ITERATION_COUNT && distanceToGo < ACCEPTABLE_IK_ERROR);
+
+    // set final rotation of the end joint
+    endState.setRotationInBindFrame(targetRotation, priority, true);
+}
+
+bool Rig::restoreJointPosition(int jointIndex, float fraction, float priority) {
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return false;
+    }
+    // const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    // const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
+    const FBXJoint& fbxJoint = _jointStates[jointIndex].getFBXJoint();
+    const QVector<int>& freeLineage = fbxJoint.freeLineage;
+
+    foreach (int index, freeLineage) {
+        JointState& state = _jointStates[index];
+        state.restoreRotation(fraction, priority);
+    }
+    return true;
+}
+
+float Rig::getLimbLength(int jointIndex, glm::vec3 scale) const {
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return 0.0f;
+    }
+
+    // const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    // const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
+    const FBXJoint& fbxJoint = _jointStates[jointIndex].getFBXJoint();
+    const QVector<int>& freeLineage = fbxJoint.freeLineage;
+
+    float length = 0.0f;
+    float lengthScale = (scale.x + scale.y + scale.z) / 3.0f;
+    for (int i = freeLineage.size() - 2; i >= 0; i--) {
+        int something = freeLineage.at(i);
+        const FBXJoint& fbxJointI = _jointStates[something].getFBXJoint();
+        length += fbxJointI.distanceToParent * lengthScale;
+    }
+    return length;
+}

libraries/animation/src/Rig.h

@@ -36,14 +36,39 @@ public:
     bool isRunningAnimation(AnimationHandlePointer animationHandle);
     const QList<AnimationHandlePointer>& getRunningAnimations() const { return _runningAnimations; }
 
-    void initJointStates(glm::vec3 scale, glm::vec3 offset, QVector<JointState> states);
+    float initJointStates(glm::vec3 scale, glm::vec3 offset, QVector<JointState> states);
     void initJointTransforms(glm::vec3 scale, glm::vec3 offset);
     void resetJoints();
+    bool jointStatesEmpty() { return _jointStates.isEmpty(); };
+    int jointStateCount() const { return _jointStates.size(); }
+    bool getJointStateAtIndex(int jointIndex, JointState& jointState) const;
+
+    void updateJointStates(glm::mat4 parentTransform);
+    void updateJointState(int index, glm::mat4 parentTransform);
+    void resetAllTransformsChanged();
+
+    bool getJointState(int index, glm::quat& rotation) const;
+    bool getVisibleJointState(int index, glm::quat& rotation) const;
+    void updateVisibleJointStates();
+    void clearJointState(int index);
+    void clearJointStates();
+    void setJointState(int index, bool valid, const glm::quat& rotation, float priority);
+    void clearJointAnimationPriority(int index);
+    glm::quat setJointRotationInBindFrame(int jointIndex, const glm::quat& rotation, float priority);
+    void applyJointRotationDelta(int jointIndex, const glm::quat& delta, bool constrain, float priority);
 
     QVector<JointState> getJointStates() { return _jointStates; }
 
     AnimationHandlePointer createAnimationHandle();
 
+    bool setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation,
+                          bool useRotation, int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment,
+                          float priority, glm::mat4 parentTransform);
+    void inverseKinematics(int endIndex, glm::vec3 targetPosition,
+                           const glm::quat& targetRotation, float priority, glm::mat4 parentTransform);
+    bool restoreJointPosition(int jointIndex, float fraction, float priority);
+    float getLimbLength(int jointIndex, glm::vec3 scale) const;
+
 protected:
     QVector<JointState> _jointStates;
 

libraries/render-utils/src/Model.h

@@ -262,6 +262,7 @@ protected:
     
     bool _showTrueJointTransforms;
     
+    bool getJointStateAtIndex(int jointIndex, JointState& jointState) const;
     QVector<JointState> _jointStates;
 
     class MeshState {
@@ -283,10 +284,13 @@ protected:
     void simulateInternal(float deltaTime);
 
     /// Updates the state of the joint at the specified index.
+    void updateJointStates();
     virtual void updateJointState(int index);
 
     virtual void updateVisibleJointStates();
-    
+
+    glm::quat setJointRotationInBindFrame(int jointIndex, const glm::quat& rotation, float priority);
+
     /// \param jointIndex index of joint in model structure
     /// \param position position of joint in model-frame
     /// \param rotation rotation of joint in model-frame
@@ -525,6 +529,7 @@ private:
     bool _readyWhenAdded = false;
     bool _needsReload = true;
 
+protected:
     RigPointer _rig;
 };