Merge remote-tracking branch 'upstream/master'

domain-server/src/DomainServer.cpp

@@ -1744,13 +1744,26 @@ void DomainServer::addStaticAssignmentsToQueue() {
 
     // if the domain-server has just restarted,
     // check if there are static assignments that we need to throw into the assignment queue
-    QHash<QUuid, SharedAssignmentPointer> staticHashCopy = _allAssignments;
-    QHash<QUuid, SharedAssignmentPointer>::iterator staticAssignment = staticHashCopy.begin();
-    while (staticAssignment != staticHashCopy.end()) {
+    auto sharedAssignments = _allAssignments.values();
+    
+    // sort the assignments to put the server/mixer assignments first
+    qSort(sharedAssignments.begin(), sharedAssignments.end(), [](SharedAssignmentPointer a, SharedAssignmentPointer b){
+        if (a->getType() == b->getType()) {
+            return true;
+        } else if (a->getType() != Assignment::AgentType && b->getType() != Assignment::AgentType) {
+            return a->getType() < b->getType();
+        } else {
+            return a->getType() != Assignment::AgentType;
+        }
+    });
+    
+    auto staticAssignment = sharedAssignments.begin();
+    
+    while (staticAssignment != sharedAssignments.end()) {
         // add any of the un-matched static assignments to the queue
 
         // enumerate the nodes and check if there is one with an attached assignment with matching UUID
-        if (!DependencyManager::get<LimitedNodeList>()->nodeWithUUID(staticAssignment->data()->getUUID())) {
+        if (!DependencyManager::get<LimitedNodeList>()->nodeWithUUID((*staticAssignment)->getUUID())) {
             // this assignment has not been fulfilled - reset the UUID and add it to the assignment queue
             refreshStaticAssignmentAndAddToQueue(*staticAssignment);
         }

examples/faceBlendCoefficients.js

@@ -0,0 +1,98 @@
+//
+//  faceBlendCoefficients.js
+//
+//  version 2.0
+//
+//  Created by Bob Long, 9/14/2015
+//  A simple panel that can select and display the blending coefficient of the Avatar's face model.
+//  
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+Script.include('utilities/tools/cookies.js')
+
+var panel;
+var coeff;
+var interval;
+var item = 0;
+var DEVELOPER_MENU = "Developer";
+var AVATAR_MENU = DEVELOPER_MENU + " > Avatar";
+var SHOW_FACE_BLEND_COEFFICIENTS = "Show face blend coefficients"
+
+function MenuConnect(menuItem) {
+    if (menuItem == SHOW_FACE_BLEND_COEFFICIENTS) {
+        if(Menu.isOptionChecked(SHOW_FACE_BLEND_COEFFICIENTS)) {
+           panel.show();
+           Overlays.editOverlay(coeff, { visible : true });
+        } else {
+           panel.hide();
+           Overlays.editOverlay(coeff, { visible : false });
+        }        
+    }
+}
+
+// Add a menu item to show/hide the coefficients
+function setupMenu() {   
+   if (!Menu.menuExists(DEVELOPER_MENU)) {
+     Menu.addMenu(DEVELOPER_MENU);
+   }
+   
+   if (!Menu.menuExists(AVATAR_MENU)) {
+     Menu.addMenu(AVATAR_MENU);
+   }
+   
+   Menu.addMenuItem({ menuName: AVATAR_MENU, menuItemName: SHOW_FACE_BLEND_COEFFICIENTS, isCheckable: true, isChecked: true });
+   Menu.menuItemEvent.connect(MenuConnect);
+}
+
+function setupPanel() {
+   panel = new Panel(10, 400);   
+   
+   // Slider to select which coefficient to display
+   panel.newSlider("Select Coefficient Index",
+       0,
+       100,
+       function(value) { item = value.toFixed(0); },
+       function() { return item; },
+       function(value) { return "index = " + item; }
+   );
+   
+   // The raw overlay used to show the actual coefficient value
+   coeff = Overlays.addOverlay("text", {
+       x: 10,
+       y: 420,
+       width: 300,
+       height: 50,
+       color: { red: 255, green: 255, blue: 255 },
+       alpha: 1.0,
+       backgroundColor: { red: 127, green: 127, blue: 127 },
+       backgroundAlpha: 0.5,
+       topMargin: 15,
+       leftMargin: 20,
+       text: "Coefficient: 0.0"
+   });
+    
+    // Set up the interval (0.5 sec) to update the coefficient.
+   interval = Script.setInterval(function() {
+      Overlays.editOverlay(coeff, { text: "Coefficient: " + MyAvatar.getFaceBlendCoef(item).toFixed(4) });
+   }, 500);
+   
+   // Mouse event setup
+   Controller.mouseMoveEvent.connect(function panelMouseMoveEvent(event) { return panel.mouseMoveEvent(event); });
+   Controller.mousePressEvent.connect( function panelMousePressEvent(event) { return panel.mousePressEvent(event); });
+   Controller.mouseReleaseEvent.connect(function(event) { return panel.mouseReleaseEvent(event); });
+}
+
+// Clean up
+function scriptEnding() {   
+   panel.destroy();
+   Overlays.deleteOverlay(coeff);
+   Script.clearInterval(interval);
+   
+   Menu.removeMenuItem(AVATAR_MENU, SHOW_FACE_BLEND_COEFFICIENTS);
+}
+
+setupMenu();
+setupPanel();
+Script.scriptEnding.connect(scriptEnding);

interface/src/Application.cpp

@@ -605,7 +605,6 @@ Application::Application(int& argc, char** argv, QElapsedTimer &startup_time) :
     _overlays.init(); // do this before scripts load
 
     _runningScriptsWidget->setRunningScripts(getRunningScripts());
-    connect(_runningScriptsWidget, &RunningScriptsWidget::stopScriptName, this, &Application::stopScript);
 
     connect(this, SIGNAL(aboutToQuit()), this, SLOT(saveScripts()));
     connect(this, SIGNAL(aboutToQuit()), this, SLOT(aboutToQuit()));
@@ -2615,20 +2614,19 @@ void Application::updateMyAvatarLookAtPosition() {
     bool isLookingAtSomeone = false;
     bool isHMD = _avatarUpdate->isHMDMode();
     glm::vec3 lookAtSpot;
-    if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
-        //  When I am in mirror mode, just look right at the camera (myself); don't switch gaze points because when physically
-        //  looking in a mirror one's eyes appear steady.
-        lookAtSpot = _myCamera.getPosition();
-    } else if (eyeTracker->isTracking() && (isHMD || eyeTracker->isSimulating())) {
+    if (eyeTracker->isTracking() && (isHMD || eyeTracker->isSimulating())) {
         //  Look at the point that the user is looking at.
+        glm::vec3 lookAtPosition = eyeTracker->getLookAtPosition();
+        if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
+            lookAtPosition.x = -lookAtPosition.x;
+        }
         if (isHMD) {
             glm::mat4 headPose = getActiveDisplayPlugin()->getHeadPose();
             glm::quat hmdRotation = glm::quat_cast(headPose);
-            lookAtSpot = _myCamera.getPosition() +
-                _myAvatar->getOrientation() * (hmdRotation * eyeTracker->getLookAtPosition());
+            lookAtSpot = _myCamera.getPosition() + _myAvatar->getOrientation() * (hmdRotation * lookAtPosition);
         } else {
-            lookAtSpot = _myAvatar->getHead()->getEyePosition() +
-                (_myAvatar->getHead()->getFinalOrientationInWorldFrame() * eyeTracker->getLookAtPosition());
+            lookAtSpot = _myAvatar->getHead()->getEyePosition()
+                + (_myAvatar->getHead()->getFinalOrientationInWorldFrame() * lookAtPosition);
         }
     } else {
         AvatarSharedPointer lookingAt = _myAvatar->getLookAtTargetAvatar().lock();
@@ -4332,17 +4330,18 @@ void Application::stopAllScripts(bool restart) {
     _myAvatar->clearScriptableSettings();
 }
 
-void Application::stopScript(const QString &scriptName, bool restart) {
-    const QString& scriptURLString = QUrl(scriptName).toString();
-    if (_scriptEnginesHash.contains(scriptURLString)) {
-        ScriptEngine* scriptEngine = _scriptEnginesHash[scriptURLString];
+bool Application::stopScript(const QString& scriptHash, bool restart) {
+    bool stoppedScript = false;
+    if (_scriptEnginesHash.contains(scriptHash)) {
+        ScriptEngine* scriptEngine = _scriptEnginesHash[scriptHash];
         if (restart) {
             auto scriptCache = DependencyManager::get<ScriptCache>();
-            scriptCache->deleteScript(scriptName);
+            scriptCache->deleteScript(QUrl(scriptHash));
             connect(scriptEngine, SIGNAL(finished(const QString&)), SLOT(reloadScript(const QString&)));
         }
         scriptEngine->stop();
-        qCDebug(interfaceapp) << "stopping script..." << scriptName;
+        stoppedScript = true;
+        qCDebug(interfaceapp) << "stopping script..." << scriptHash;
         // HACK: ATM scripts cannot set/get their animation priorities, so we clear priorities
         // whenever a script stops in case it happened to have been setting joint rotations.
         // TODO: expose animation priorities and provide a layered animation control system.
@@ -4351,6 +4350,7 @@ void Application::stopScript(const QString &scriptName, bool restart) {
     if (_scriptEnginesHash.empty()) {
         _myAvatar->clearScriptableSettings();
     }
+    return stoppedScript;
 }
 
 void Application::reloadAllScripts() {

interface/src/Application.h

@@ -292,7 +292,7 @@ public:
     NodeToJurisdictionMap& getEntityServerJurisdictions() { return _entityServerJurisdictions; }
 
     QStringList getRunningScripts() { return _scriptEnginesHash.keys(); }
-    ScriptEngine* getScriptEngine(QString scriptHash) { return _scriptEnginesHash.contains(scriptHash) ? _scriptEnginesHash[scriptHash] : NULL; }
+    ScriptEngine* getScriptEngine(const QString& scriptHash) { return _scriptEnginesHash.value(scriptHash, NULL); }
     
     bool isLookingAtMyAvatar(AvatarSharedPointer avatar);
 
@@ -392,7 +392,7 @@ public slots:
     void reloadScript(const QString& scriptName, bool isUserLoaded = true);
     void scriptFinished(const QString& scriptName);
     void stopAllScripts(bool restart = false);
-    void stopScript(const QString& scriptName, bool restart = false);
+    bool stopScript(const QString& scriptHash, bool restart = false);
     void reloadAllScripts();
     void reloadOneScript(const QString& scriptName);
     void loadDefaultScripts();

interface/src/avatar/MyAvatar.h

@@ -110,6 +110,8 @@ public:
     Q_INVOKABLE float getHeadFinalRoll() const { return getHead()->getFinalRoll(); }
     Q_INVOKABLE float getHeadFinalPitch() const { return getHead()->getFinalPitch(); }
     Q_INVOKABLE float getHeadDeltaPitch() const { return getHead()->getDeltaPitch(); }
+    Q_INVOKABLE int getFaceBlendCoefNum() const { return getHead()->getFaceModel().getBlendshapeCoefficientsNum(); }
+    Q_INVOKABLE float getFaceBlendCoef(int index) const { return getHead()->getFaceModel().getBlendshapeCoefficient(index); }
 
     Q_INVOKABLE glm::vec3 getEyePosition() const { return getHead()->getEyePosition(); }
 

interface/src/ui/RunningScriptsWidget.cpp

@@ -57,16 +57,15 @@ RunningScriptsWidget::RunningScriptsWidget(QWidget* parent) :
     connect(ui->filterLineEdit, &QLineEdit::textChanged, this, &RunningScriptsWidget::updateFileFilter);
     connect(ui->scriptTreeView, &QTreeView::doubleClicked, this, &RunningScriptsWidget::loadScriptFromList);
 
-    connect(ui->reloadAllButton, &QPushButton::clicked,
-            Application::getInstance(), &Application::reloadAllScripts);
-    connect(ui->stopAllButton, &QPushButton::clicked,
-            this, &RunningScriptsWidget::allScriptsStopped);
-    connect(ui->loadScriptFromDiskButton, &QPushButton::clicked,
-            Application::getInstance(), &Application::loadDialog);
-    connect(ui->loadScriptFromURLButton, &QPushButton::clicked,
-            Application::getInstance(), &Application::loadScriptURLDialog);
-    connect(&_reloadSignalMapper, SIGNAL(mapped(QString)), Application::getInstance(), SLOT(reloadOneScript(const QString&)));
-    connect(&_stopSignalMapper, SIGNAL(mapped(QString)), Application::getInstance(), SLOT(stopScript(const QString&)));
+    connect(ui->reloadAllButton, &QPushButton::clicked, Application::getInstance(), &Application::reloadAllScripts);
+    connect(ui->stopAllButton, &QPushButton::clicked, this, &RunningScriptsWidget::allScriptsStopped);
+    connect(ui->loadScriptFromDiskButton, &QPushButton::clicked, Application::getInstance(), &Application::loadDialog);
+    connect(ui->loadScriptFromURLButton, &QPushButton::clicked, Application::getInstance(), &Application::loadScriptURLDialog);
+    connect(&_reloadSignalMapper, static_cast<void(QSignalMapper::*)(const QString&)>(&QSignalMapper::mapped),
+            Application::getInstance(), &Application::reloadOneScript);
+
+    connect(&_stopSignalMapper, static_cast<void(QSignalMapper::*)(const QString&)>(&QSignalMapper::mapped),
+            [](const QString& script) { Application::getInstance()->stopScript(script); });
 
     UIUtil::scaleWidgetFontSizes(this);
 }
@@ -217,9 +216,6 @@ void RunningScriptsWidget::keyPressEvent(QKeyEvent *keyEvent) {
     }
 }
 
-void RunningScriptsWidget::scriptStopped(const QString& scriptName) {
-}
-
 void RunningScriptsWidget::allScriptsStopped() {
     Application::getInstance()->stopAllScripts();
 }
@@ -227,15 +223,16 @@ void RunningScriptsWidget::allScriptsStopped() {
 QVariantList RunningScriptsWidget::getRunning() {
     const int WINDOWS_DRIVE_LETTER_SIZE = 1;
     QVariantList result;
-    QStringList runningScripts = Application::getInstance()->getRunningScripts();
-    for (int i = 0; i < runningScripts.size(); i++) {
-        QUrl runningScriptURL = QUrl(runningScripts.at(i));
+    foreach(const QString& runningScript, Application::getInstance()->getRunningScripts()) {
+        QUrl runningScriptURL = QUrl(runningScript);
         if (runningScriptURL.scheme().size() <= WINDOWS_DRIVE_LETTER_SIZE) {
             runningScriptURL = QUrl::fromLocalFile(runningScriptURL.toDisplayString(QUrl::FormattingOptions(QUrl::FullyEncoded)));
         }
         QVariantMap resultNode;
         resultNode.insert("name", runningScriptURL.fileName());
         resultNode.insert("url", runningScriptURL.toDisplayString(QUrl::FormattingOptions(QUrl::FullyEncoded)));
+        // The path contains the exact path/URL of the script, which also is used in the stopScript function.
+        resultNode.insert("path", runningScript);
         resultNode.insert("local", runningScriptURL.isLocalFile());
         result.append(resultNode);
     }
@@ -294,3 +291,16 @@ QVariantList RunningScriptsWidget::getLocal() {
     }
     return result;
 }
+
+bool RunningScriptsWidget::stopScriptByName(const QString& name) {
+    foreach (const QString& runningScript, Application::getInstance()->getRunningScripts()) {
+        if (QUrl(runningScript).fileName().toLower() == name.trimmed().toLower()) {
+            return Application::getInstance()->stopScript(runningScript, false);
+        }
+    }
+    return false;
+}
+
+bool RunningScriptsWidget::stopScript(const QString& name, bool restart) {
+    return Application::getInstance()->stopScript(name, restart);
+}

interface/src/ui/RunningScriptsWidget.h

@@ -36,7 +36,7 @@ public:
     const ScriptsModel* getScriptsModel() { return &_scriptsModel; }
 
 signals:
-    void stopScriptName(const QString& name, bool restart = false);
+    void scriptStopped(const QString& scriptName);
 
 protected:
     virtual bool eventFilter(QObject* sender, QEvent* event);
@@ -45,10 +45,11 @@ protected:
     virtual void showEvent(QShowEvent* event);
 
 public slots:
-    void scriptStopped(const QString& scriptName);
     QVariantList getRunning();
     QVariantList getPublic();
     QVariantList getLocal();
+    bool stopScript(const QString& name, bool restart = false);
+    bool stopScriptByName(const QString& name);
     
 private slots:
     void allScriptsStopped();
@@ -63,9 +64,6 @@ private:
     QSignalMapper _stopSignalMapper;
     ScriptsModelFilter _scriptsModelFilter;
     ScriptsModel _scriptsModel;
-    ScriptsTableWidget* _recentlyLoadedScriptsTable;
-    QStringList _recentlyLoadedScripts;
-    QString _lastStoppedScript;
 
     QVariantList getPublicChildNodes(TreeNodeFolder* parent);
 };

libraries/animation/src/AnimInverseKinematics.cpp

@@ -82,22 +82,8 @@ static int findRootJointInSkeleton(AnimSkeleton::ConstPointer skeleton, int inde
     return rootIndex;
 }
 
-struct IKTarget {
-    AnimPose pose;
-    int index;
-    int rootIndex;
-};
-
-//virtual
-const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
-
-    // NOTE: we assume that _relativePoses are up to date (e.g. loadPoses() was just called)
-    if (_relativePoses.empty()) {
-        return _relativePoses;
-    }
-
-    // build a list of targets from _targetVarVec
-    std::vector<IKTarget> targets;
+void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets) {
+    // build a list of valid targets from _targetVarVec and animVars
     bool removeUnfoundJoints = false;
     for (auto& targetVar : _targetVarVec) {
         if (targetVar.jointIndex == -1) {
@@ -113,15 +99,13 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
             }
         } else {
             // TODO: get this done without a double-lookup of each var in animVars
-            if (animVars.hasKey(targetVar.positionVar) || animVars.hasKey(targetVar.rotationVar)) {
-                IKTarget target;
-                AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, _relativePoses);
-                target.pose.trans = animVars.lookup(targetVar.positionVar, defaultPose.trans);
-                target.pose.rot = animVars.lookup(targetVar.rotationVar, defaultPose.rot);
-                target.rootIndex = targetVar.rootIndex;
-                target.index = targetVar.jointIndex;
-                targets.push_back(target);
-            }
+            IKTarget target;
+            AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, _relativePoses);
+            target.pose.trans = animVars.lookup(targetVar.positionVar, defaultPose.trans);
+            target.pose.rot = animVars.lookup(targetVar.rotationVar, defaultPose.rot);
+            target.rootIndex = targetVar.rootIndex;
+            target.index = targetVar.jointIndex;
+            targets.push_back(target);
         }
     }
 
@@ -141,154 +125,160 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
             }
         }
     }
+}
 
-    if (targets.empty()) {
-        // no IK targets but still need to enforce constraints
-        std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
-        while (constraintItr != _constraints.end()) {
-            int index = constraintItr->first;
-            glm::quat rotation = _relativePoses[index].rot;
-            constraintItr->second->apply(rotation);
-            _relativePoses[index].rot = rotation;
-            ++constraintItr;
-        }
-    } else {
-        // clear the accumulators before we start the IK solver
-        for (auto& accumulatorPair: _accumulators) {
-            accumulatorPair.second.clear();
-        }
+void AnimInverseKinematics::solveWithCyclicCoordinateDescent(std::vector<IKTarget>& targets) {
+    // compute absolute poses that correspond to relative target poses
+    AnimPoseVec absolutePoses;
+    computeAbsolutePoses(absolutePoses);
 
-        // compute absolute poses that correspond to relative target poses
-        AnimPoseVec absolutePoses;
-        computeAbsolutePoses(absolutePoses);
+    // clear the accumulators before we start the IK solver
+    for (auto& accumulatorPair: _accumulators) {
+        accumulatorPair.second.clear();
+    }
 
-        float largestError = 0.0f;
-        const float ACCEPTABLE_RELATIVE_ERROR = 1.0e-3f;
-        int numLoops = 0;
-        const int MAX_IK_LOOPS = 16;
-        const quint64 MAX_IK_TIME = 10 * USECS_PER_MSEC;
-        quint64 expiry = usecTimestampNow() + MAX_IK_TIME;
-        do {
-            largestError = 0.0f;
-            int lowestMovedIndex = _relativePoses.size();
-            for (auto& target: targets) {
-                int tipIndex = target.index;
-                AnimPose targetPose = target.pose;
-                int rootIndex = target.rootIndex;
-                if (rootIndex != -1) {
-                    // transform targetPose into skeleton's absolute frame
-                    AnimPose& rootPose = _relativePoses[rootIndex];
-                    targetPose.trans = rootPose.trans + rootPose.rot * targetPose.trans;
-                    targetPose.rot = rootPose.rot * targetPose.rot;
-                }
-
-                glm::vec3 tip = absolutePoses[tipIndex].trans;
-                float error = glm::length(targetPose.trans - tip);
-
-                // descend toward root, pivoting each joint to get tip closer to target
-                int pivotIndex = _skeleton->getParentIndex(tipIndex);
-                while (pivotIndex != -1 && error > ACCEPTABLE_RELATIVE_ERROR) {
-                    // compute the two lines that should be aligned
-                    glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
-                    glm::vec3 leverArm = tip - jointPosition;
-                    glm::vec3 targetLine = targetPose.trans - jointPosition;
-
-                    // compute the axis of the rotation that would align them
-                    glm::vec3 axis = glm::cross(leverArm, targetLine);
-                    float axisLength = glm::length(axis);
-                    if (axisLength > EPSILON) {
-                        // compute deltaRotation for alignment (brings tip closer to target)
-                        axis /= axisLength;
-                        float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)));
-
-                        // NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is
-                        // still possible for the angle to be zero so we also check that to avoid unnecessary calculations.
-                        if (angle > EPSILON) {
-                            // reduce angle by half: slows convergence but adds stability to IK solution
-                            angle = 0.5f * angle;
-                            glm::quat deltaRotation = glm::angleAxis(angle, axis);
-
-                            int parentIndex = _skeleton->getParentIndex(pivotIndex);
-                            if (parentIndex == -1) {
-                                // TODO? apply constraints to root?
-                                // TODO? harvest the root's transform as movement of entire skeleton?
-                            } else {
-                                // compute joint's new parent-relative rotation
-                                // Q' = dQ * Q   and   Q = Qp * q   -->   q' = Qp^ * dQ * Q
-                                glm::quat newRot = glm::normalize(glm::inverse(
-                                        absolutePoses[parentIndex].rot) *
-                                        deltaRotation *
-                                        absolutePoses[pivotIndex].rot);
-                                RotationConstraint* constraint = getConstraint(pivotIndex);
-                                if (constraint) {
-                                    bool constrained = constraint->apply(newRot);
-                                    if (constrained) {
-                                        // the constraint will modify the movement of the tip so we have to compute the modified
-                                        // model-frame deltaRotation
-                                        // Q' = Qp^ * dQ * Q  -->  dQ =   Qp * Q' * Q^
-                                        deltaRotation = absolutePoses[parentIndex].rot *
-                                            newRot *
-                                            glm::inverse(absolutePoses[pivotIndex].rot);
-                                    }
-                                }
-                                // store the rotation change in the accumulator
-                                _accumulators[pivotIndex].add(newRot);
-                            }
-                            // this joint has been changed so we check to see if it has the lowest index
-                            if (pivotIndex < lowestMovedIndex) {
-                                lowestMovedIndex = pivotIndex;
-                            }
-
-                            // keep track of tip's new position as we descend towards root
-                            tip = jointPosition + deltaRotation * leverArm;
-                            error = glm::length(targetPose.trans - tip);
-                        }
-                    }
-                    pivotIndex = _skeleton->getParentIndex(pivotIndex);
-                }
-                if (largestError < error) {
-                    largestError = error;
-                }
-            }
-            ++numLoops;
-
-            // harvest accumulated rotations and apply the average
-            for (auto& accumulatorPair: _accumulators) {
-                RotationAccumulator& accumulator = accumulatorPair.second;
-                if (accumulator.size() > 0) {
-                    _relativePoses[accumulatorPair.first].rot = accumulator.getAverage();
-                    accumulator.clear();
-                }
-            }
-
-            // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex
-            for (int i = lowestMovedIndex; i <= _maxTargetIndex; ++i) {
-                int parentIndex = _skeleton->getParentIndex(i);
-                if (parentIndex != -1) {
-                    absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
-                }
-            }
-        } while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry);
-
-        // finally set the relative rotation of each tip to agree with absolute target rotation
+    float largestError = 0.0f;
+    const float ACCEPTABLE_RELATIVE_ERROR = 1.0e-3f;
+    int numLoops = 0;
+    const int MAX_IK_LOOPS = 16;
+    const quint64 MAX_IK_TIME = 10 * USECS_PER_MSEC;
+    quint64 expiry = usecTimestampNow() + MAX_IK_TIME;
+    do {
+        largestError = 0.0f;
+        int lowestMovedIndex = _relativePoses.size();
         for (auto& target: targets) {
             int tipIndex = target.index;
-            int parentIndex = _skeleton->getParentIndex(tipIndex);
-            if (parentIndex != -1) {
-                AnimPose targetPose = target.pose;
-                // compute tip's new parent-relative rotation
-                // Q = Qp * q   -->   q' = Qp^ * Q
-                glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetPose.rot;
-                RotationConstraint* constraint = getConstraint(tipIndex);
-                if (constraint) {
-                    constraint->apply(newRelativeRotation);
-                    // TODO: ATM the final rotation target just fails but we need to provide
-                    // feedback to the IK system so that it can adjust the bones up the skeleton
-                    // to help this rotation target get met.
+            AnimPose targetPose = target.pose;
+
+            glm::vec3 tip = absolutePoses[tipIndex].trans;
+            float error = glm::length(targetPose.trans - tip);
+
+            // descend toward root, pivoting each joint to get tip closer to target
+            int pivotIndex = _skeleton->getParentIndex(tipIndex);
+            while (pivotIndex != -1 && error > ACCEPTABLE_RELATIVE_ERROR) {
+                // compute the two lines that should be aligned
+                glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
+                glm::vec3 leverArm = tip - jointPosition;
+                glm::vec3 targetLine = targetPose.trans - jointPosition;
+
+                // compute the axis of the rotation that would align them
+                glm::vec3 axis = glm::cross(leverArm, targetLine);
+                float axisLength = glm::length(axis);
+                if (axisLength > EPSILON) {
+                    // compute deltaRotation for alignment (brings tip closer to target)
+                    axis /= axisLength;
+                    float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)));
+
+                    // NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is
+                    // still possible for the angle to be zero so we also check that to avoid unnecessary calculations.
+                    if (angle > EPSILON) {
+                        // reduce angle by half: slows convergence but adds stability to IK solution
+                        angle = 0.5f * angle;
+                        glm::quat deltaRotation = glm::angleAxis(angle, axis);
+
+                        int parentIndex = _skeleton->getParentIndex(pivotIndex);
+                        if (parentIndex == -1) {
+                            // TODO? apply constraints to root?
+                            // TODO? harvest the root's transform as movement of entire skeleton?
+                        } else {
+                            // compute joint's new parent-relative rotation
+                            // Q' = dQ * Q   and   Q = Qp * q   -->   q' = Qp^ * dQ * Q
+                            glm::quat newRot = glm::normalize(glm::inverse(
+                                    absolutePoses[parentIndex].rot) *
+                                    deltaRotation *
+                                    absolutePoses[pivotIndex].rot);
+                            RotationConstraint* constraint = getConstraint(pivotIndex);
+                            if (constraint) {
+                                bool constrained = constraint->apply(newRot);
+                                if (constrained) {
+                                    // the constraint will modify the movement of the tip so we have to compute the modified
+                                    // model-frame deltaRotation
+                                    // Q' = Qp^ * dQ * Q  -->  dQ =   Qp * Q' * Q^
+                                    deltaRotation = absolutePoses[parentIndex].rot *
+                                        newRot *
+                                        glm::inverse(absolutePoses[pivotIndex].rot);
+                                }
+                            }
+                            // store the rotation change in the accumulator
+                            _accumulators[pivotIndex].add(newRot);
+                        }
+                        // this joint has been changed so we check to see if it has the lowest index
+                        if (pivotIndex < lowestMovedIndex) {
+                            lowestMovedIndex = pivotIndex;
+                        }
+
+                        // keep track of tip's new position as we descend towards root
+                        tip = jointPosition + deltaRotation * leverArm;
+                        error = glm::length(targetPose.trans - tip);
+                    }
                 }
-                _relativePoses[tipIndex].rot = newRelativeRotation;
-                absolutePoses[tipIndex].rot = targetPose.rot;
+                pivotIndex = _skeleton->getParentIndex(pivotIndex);
             }
+            if (largestError < error) {
+                largestError = error;
+            }
+        }
+        ++numLoops;
+
+        // harvest accumulated rotations and apply the average
+        for (auto& accumulatorPair: _accumulators) {
+            RotationAccumulator& accumulator = accumulatorPair.second;
+            if (accumulator.size() > 0) {
+                _relativePoses[accumulatorPair.first].rot = accumulator.getAverage();
+                accumulator.clear();
+            }
+        }
+
+        // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex
+        for (int i = lowestMovedIndex; i <= _maxTargetIndex; ++i) {
+            int parentIndex = _skeleton->getParentIndex(i);
+            if (parentIndex != -1) {
+                absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
+            }
+        }
+    } while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry);
+
+    // finally set the relative rotation of each tip to agree with absolute target rotation
+    for (auto& target: targets) {
+        int tipIndex = target.index;
+        int parentIndex = _skeleton->getParentIndex(tipIndex);
+        if (parentIndex != -1) {
+            AnimPose targetPose = target.pose;
+            // compute tip's new parent-relative rotation
+            // Q = Qp * q   -->   q' = Qp^ * Q
+            glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetPose.rot;
+            RotationConstraint* constraint = getConstraint(tipIndex);
+            if (constraint) {
+                constraint->apply(newRelativeRotation);
+                // TODO: ATM the final rotation target just fails but we need to provide
+                // feedback to the IK system so that it can adjust the bones up the skeleton
+                // to help this rotation target get met.
+            }
+            _relativePoses[tipIndex].rot = newRelativeRotation;
+            absolutePoses[tipIndex].rot = targetPose.rot;
+        }
+    }
+}
+
+//virtual
+const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
+    if (!_relativePoses.empty()) {
+        // build a list of targets from _targetVarVec
+        std::vector<IKTarget> targets;
+        computeTargets(animVars, targets);
+    
+        if (targets.empty()) {
+            // no IK targets but still need to enforce constraints
+            std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
+            while (constraintItr != _constraints.end()) {
+                int index = constraintItr->first;
+                glm::quat rotation = _relativePoses[index].rot;
+                constraintItr->second->apply(rotation);
+                _relativePoses[index].rot = rotation;
+                ++constraintItr;
+            }
+        } else {
+            solveWithCyclicCoordinateDescent(targets);
         }
     }
     return _relativePoses;

libraries/animation/src/AnimInverseKinematics.h

@@ -37,6 +37,14 @@ public:
     virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override;
 
 protected:
+    struct IKTarget {
+        AnimPose pose;
+        int index;
+        int rootIndex;
+    };
+
+    void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets);
+    void solveWithCyclicCoordinateDescent(std::vector<IKTarget>& targets);
     virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton);
 
     // for AnimDebugDraw rendering
@@ -64,7 +72,7 @@ protected:
     };
 
     std::map<int, RotationConstraint*> _constraints;
-    std::map<int, RotationAccumulator> _accumulators;
+    std::map<int, RotationAccumulator> _accumulators; // class-member to exploit temporal coherency
     std::vector<IKTargetVar> _targetVarVec;
     AnimPoseVec _defaultRelativePoses; // poses of the relaxed state
     AnimPoseVec _relativePoses; // current relative poses

libraries/animation/src/AnimNode.cpp

@@ -0,0 +1,54 @@
+//
+//  AnimNode.cpp
+//
+//  Created by Anthony J. Thibault on 9/2/15.
+//  Copyright (c) 2015 High Fidelity, Inc. All rights reserved.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "AnimNode.h"
+
+void AnimNode::removeChild(AnimNode::Pointer child) {
+    auto iter = std::find(_children.begin(), _children.end(), child);
+    if (iter != _children.end()) {
+        _children.erase(iter);
+    }
+}
+
+AnimNode::Pointer AnimNode::getChild(int i) const {
+    assert(i >= 0 && i < (int)_children.size());
+    return _children[i];
+}
+
+void AnimNode::setSkeleton(const AnimSkeleton::Pointer skeleton) {
+    setSkeletonInternal(skeleton);
+    for (auto&& child : _children) {
+        child->setSkeleton(skeleton);
+    }
+}
+
+const AnimPose AnimNode::getRootPose(int jointIndex) const {
+    AnimPose pose = AnimPose::identity;
+    if (_skeleton && jointIndex != -1) {
+        const AnimPoseVec& poses = getPosesInternal();
+        int numJoints = (int)(poses.size());
+        if (jointIndex < numJoints) {
+            int parentIndex = _skeleton->getParentIndex(jointIndex);
+            while (parentIndex != -1 && parentIndex < numJoints) {
+                jointIndex = parentIndex;
+                parentIndex = _skeleton->getParentIndex(jointIndex);
+            }
+            pose = poses[jointIndex];
+        }
+    }
+    return pose;
+}
+
+void AnimNode::setCurrentFrame(float frame) {
+    setCurrentFrameInternal(frame);
+    for (auto&& child : _children) {
+        child->setCurrentFrameInternal(frame);
+    }
+}

libraries/animation/src/AnimNode.h

@@ -60,25 +60,13 @@ public:
 
     // hierarchy accessors
     void addChild(Pointer child) { _children.push_back(child); }
-    void removeChild(Pointer child) {
-        auto iter = std::find(_children.begin(), _children.end(), child);
-        if (iter != _children.end()) {
-            _children.erase(iter);
-        }
-    }
-    Pointer getChild(int i) const {
-        assert(i >= 0 && i < (int)_children.size());
-        return _children[i];
-    }
+    void removeChild(Pointer child);
+    
+    Pointer getChild(int i) const;
     int getChildCount() const { return (int)_children.size(); }
 
     // pair this AnimNode graph with a skeleton.
-    void setSkeleton(const AnimSkeleton::Pointer skeleton) {
-        setSkeletonInternal(skeleton);
-        for (auto&& child : _children) {
-            child->setSkeleton(skeleton);
-        }
-    }
+    void setSkeleton(const AnimSkeleton::Pointer skeleton);
 
     AnimSkeleton::ConstPointer getSkeleton() const { return _skeleton; }
 
@@ -87,19 +75,14 @@ public:
         return evaluate(animVars, dt, triggersOut);
     }
 
+    const AnimPose getRootPose(int jointIndex) const;
+
 protected:
 
-    void setCurrentFrame(float frame) {
-        setCurrentFrameInternal(frame);
-        for (auto&& child : _children) {
-            child->setCurrentFrameInternal(frame);
-        }
-    }
+    void setCurrentFrame(float frame);
 
     virtual void setCurrentFrameInternal(float frame) {}
-    virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
-        _skeleton = skeleton;
-    }
+    virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) { _skeleton = skeleton; }
 
     // for AnimDebugDraw rendering
     virtual const AnimPoseVec& getPosesInternal() const = 0;

libraries/animation/src/Rig.cpp

@@ -986,16 +986,20 @@ void Rig::updateLeanJoint(int index, float leanSideways, float leanForward, floa
 
 void Rig::updateNeckJoint(int index, const HeadParameters& params) {
     if (index >= 0 && _jointStates[index].getParentIndex() >= 0) {
-        if (_enableAnimGraph && _animSkeleton) {
+        if (_enableAnimGraph && _animSkeleton && _animNode) {
             // the params.localHeadOrientation is composed incorrectly, so re-compose it correctly from pitch, yaw and roll.
             glm::quat realLocalHeadOrientation = (glm::angleAxis(glm::radians(-params.localHeadRoll), Z_AXIS) *
                                                   glm::angleAxis(glm::radians(params.localHeadYaw), Y_AXIS) *
                                                   glm::angleAxis(glm::radians(-params.localHeadPitch), X_AXIS));
             _animVars.set("headRotation", realLocalHeadOrientation);
 
-            // There's a theory that when not in hmd, we should _animVars.unset("headPosition").
-            // However, until that works well, let's always request head be positioned where requested by hmd, camera, or default.
-            _animVars.set("headPosition", params.localHeadPosition);
+            if (params.isInHMD) {
+                int headIndex = _animSkeleton->nameToJointIndex("Head");
+                AnimPose rootPose = _animNode->getRootPose(headIndex);
+                _animVars.set("headPosition", rootPose.trans + params.localHeadPosition); // rootPose.rot is handled in params?d
+            } else {
+                _animVars.unset("headPosition");
+            }
         } else if (!_enableAnimGraph) {
 
             auto& state = _jointStates[index];
@@ -1044,20 +1048,22 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
 
 void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
 
-    if (_enableAnimGraph && _animSkeleton) {
+    if (_enableAnimGraph && _animSkeleton && _animNode) {
 
         // TODO: figure out how to obtain the yFlip from where it is actually stored
         glm::quat yFlipHACK = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
+        int leftHandIndex = _animSkeleton->nameToJointIndex("LeftHand");
+        AnimPose rootPose = _animNode->getRootPose(leftHandIndex);
         if (params.isLeftEnabled) {
-            _animVars.set("leftHandPosition", yFlipHACK * params.leftPosition);
-            _animVars.set("leftHandRotation", yFlipHACK * params.leftOrientation);
+            _animVars.set("leftHandPosition", rootPose.trans + rootPose.rot * yFlipHACK * params.leftPosition);
+            _animVars.set("leftHandRotation", rootPose.rot * yFlipHACK * params.leftOrientation);
         } else {
             _animVars.unset("leftHandPosition");
             _animVars.unset("leftHandRotation");
         }
         if (params.isRightEnabled) {
-            _animVars.set("rightHandPosition", yFlipHACK * params.rightPosition);
-            _animVars.set("rightHandRotation", yFlipHACK * params.rightOrientation);
+            _animVars.set("rightHandPosition", rootPose.trans + rootPose.rot * yFlipHACK * params.rightPosition);
+            _animVars.set("rightHandRotation", rootPose.rot * yFlipHACK * params.rightOrientation);
         } else {
             _animVars.unset("rightHandPosition");
             _animVars.unset("rightHandRotation");

libraries/render-utils/src/Model.h

@@ -191,6 +191,11 @@ public:
     const std::unordered_set<int>& getCauterizeBoneSet() const { return _cauterizeBoneSet; }
     void setCauterizeBoneSet(const std::unordered_set<int>& boneSet) { _cauterizeBoneSet = boneSet; }
 
+    int getBlendshapeCoefficientsNum() const { return _blendshapeCoefficients.size(); }
+    float getBlendshapeCoefficient(unsigned int index) const {
+        return index >= _blendshapeCoefficients.size() ? 0.0f : _blendshapeCoefficients.at(index);
+     }
+
 protected:
 
     void setPupilDilation(float dilation) { _pupilDilation = dilation; }