Merge pull request #5786 from AndrewMeadows/ik-with-constraints

inverse kinematics works with rotation constraints

interface/src/Menu.cpp

@@ -467,7 +467,6 @@ Menu::Menu() {
                                            SLOT(toggleConnexion(bool)));
 
     MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands");
-    addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHands, 0, true);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::HandMouseInput, 0, true);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::LowVelocityFilter, 0, true,

interface/src/Menu.h

@@ -175,7 +175,6 @@ namespace MenuOption {
     const QString DisableNackPackets = "Disable Entity NACK Packets";
     const QString DiskCacheEditor = "Disk Cache Editor";
     const QString DisplayCrashOptions = "Display Crash Options";
-    const QString DisplayHands = "Show Hand Info";
     const QString DisplayHandTargets = "Show Hand Targets";
     const QString DisplayModelBounds = "Display Model Bounds";
     const QString DisplayModelTriangles = "Display Model Triangles";

interface/src/avatar/Avatar.cpp

@@ -602,7 +602,10 @@ void Avatar::renderBody(RenderArgs* renderArgs, ViewFrustum* renderFrustum, floa
             getHead()->render(renderArgs, 1.0f, renderFrustum);
         }
 
-        getHand()->render(renderArgs, false);
+        if (renderArgs->_renderMode != RenderArgs::SHADOW_RENDER_MODE &&
+                Menu::getInstance()->isOptionChecked(MenuOption::DisplayHandTargets)) {
+            getHand()->renderHandTargets(renderArgs, false);
+        }
     }
     getHead()->renderLookAts(renderArgs);
 }

interface/src/avatar/Hand.cpp

@@ -8,22 +8,21 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
-#include <QImage>
+#include "Hand.h"
+
+#include <glm/glm.hpp>
 
 #include <GeometryUtil.h>
-#include <NodeList.h>
+#include <RenderArgs.h>
 
+#include "Avatar.h"
 #include "AvatarManager.h"
-#include "Hand.h"
-#include "Menu.h"
 #include "MyAvatar.h"
 #include "Util.h"
+#include "world.h"
 
 using namespace std;
 
-const float PALM_COLLISION_RADIUS = 0.03f;
-
-
 Hand::Hand(Avatar* owningAvatar) :
     HandData((AvatarData*)owningAvatar),
     _owningAvatar(owningAvatar)
@@ -40,76 +39,67 @@ void Hand::simulate(float deltaTime, bool isMine) {
     }
 }
 
-void Hand::render(RenderArgs* renderArgs, bool isMine) {
+void Hand::renderHandTargets(RenderArgs* renderArgs, bool isMine) {
+    float avatarScale = 1.0f;
+    if (_owningAvatar) {
+        avatarScale = _owningAvatar->getScale();
+    }
+
+    const float alpha = 1.0f;
+    const glm::vec3 redColor(1.0f, 0.0f, 0.0f); //  Color the hand targets red to be different than skin
+    const glm::vec3 greenColor(0.0f, 1.0f, 0.0f); //  Color the hand targets red to be different than skin
+    const glm::vec3 blueColor(0.0f, 0.0f, 1.0f); //  Color the hand targets red to be different than skin
+    const glm::vec3 grayColor(0.5f);
+    const int NUM_FACETS = 8;
+    const float SPHERE_RADIUS = 0.03f * avatarScale;
+
     gpu::Batch& batch = *renderArgs->_batch;
-    if (renderArgs->_renderMode != RenderArgs::SHADOW_RENDER_MODE &&
+    if (isMine) {
-                Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionShapes)) {
-        // draw a green sphere at hand joint location, which is actually near the wrist)
         for (size_t i = 0; i < getNumPalms(); i++) {
             PalmData& palm = getPalms()[i];
             if (!palm.isActive()) {
                 continue;
             }
+            // draw a gray sphere at the target position of the "Hand" joint
             glm::vec3 position = palm.getPosition();
             Transform transform = Transform();
             transform.setTranslation(position);
+            transform.setRotation(palm.getRotation());
             batch.setModelTransform(transform);
-            DependencyManager::get<GeometryCache>()->renderSphere(batch, PALM_COLLISION_RADIUS * _owningAvatar->getScale(), 10, 10, glm::vec3(0.0f, 1.0f, 0.0f));
+            DependencyManager::get<GeometryCache>()->renderSphere(batch, SPHERE_RADIUS,
+                    NUM_FACETS, NUM_FACETS, grayColor);
+    
+            // draw a green sphere at the old "finger tip"
+            position = palm.getTipPosition();
+            transform.setTranslation(position);
+            batch.setModelTransform(transform);
+            DependencyManager::get<GeometryCache>()->renderSphere(batch, SPHERE_RADIUS,
+                    NUM_FACETS, NUM_FACETS, greenColor, false);
         }
     }
     
-    if (renderArgs->_renderMode != RenderArgs::SHADOW_RENDER_MODE && Menu::getInstance()->isOptionChecked(MenuOption::DisplayHands)) {
+    const float AXIS_RADIUS = 0.1f * SPHERE_RADIUS;
-        renderHandTargets(renderArgs, isMine);
+    const float AXIS_LENGTH = 10.0f * SPHERE_RADIUS;
-    }
 
-}
+    // Draw the coordinate frames of the hand targets
-
-void Hand::renderHandTargets(RenderArgs* renderArgs, bool isMine) {
-    gpu::Batch& batch = *renderArgs->_batch;
-    const float avatarScale = DependencyManager::get<AvatarManager>()->getMyAvatar()->getScale();
-
-    const float alpha = 1.0f;
-    const glm::vec3 handColor(1.0, 0.0, 0.0); //  Color the hand targets red to be different than skin
-
-    if (isMine && Menu::getInstance()->isOptionChecked(MenuOption::DisplayHandTargets)) {
-        for (size_t i = 0; i < getNumPalms(); ++i) {
-            PalmData& palm = getPalms()[i];
-            if (!palm.isActive()) {
-                continue;
-            }
-            glm::vec3 targetPosition = palm.getTipPosition();
-            Transform transform = Transform();
-            transform.setTranslation(targetPosition);
-            batch.setModelTransform(transform);
-        
-            const float collisionRadius = 0.05f;
-            DependencyManager::get<GeometryCache>()->renderSphere(batch, collisionRadius, 10, 10, glm::vec4(0.5f,0.5f,0.5f, alpha), false);
-        }
-    }
-    
-    const float PALM_BALL_RADIUS = 0.03f * avatarScale;
-    const float PALM_DISK_RADIUS = 0.06f * avatarScale;
-    const float PALM_DISK_THICKNESS = 0.01f * avatarScale;
-    const float PALM_FINGER_ROD_RADIUS = 0.003f * avatarScale;
-    
-    // Draw the palm ball and disk
     for (size_t i = 0; i < getNumPalms(); ++i) {
         PalmData& palm = getPalms()[i];
         if (palm.isActive()) {
-            glm::vec3 tip = palm.getTipPosition();
             glm::vec3 root = palm.getPosition();
+
+            const glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
+            glm::quat palmRotation = palm.getRotation();
             Transform transform = Transform();
             transform.setTranslation(glm::vec3());
             batch.setModelTransform(transform);
-            Avatar::renderJointConnectingCone(batch, root, tip, PALM_FINGER_ROD_RADIUS, PALM_FINGER_ROD_RADIUS, glm::vec4(handColor.r, handColor.g, handColor.b, alpha));
+            glm::vec3 tip = root + palmRotation * glm::vec3(AXIS_LENGTH, 0.0f, 0.0f);
+            Avatar::renderJointConnectingCone(batch, root, tip, AXIS_RADIUS, AXIS_RADIUS, glm::vec4(redColor.r, redColor.g, redColor.b, alpha));
 
-            //  Render sphere at palm/finger root
+            tip = root + palmRotation * glm::vec3(0.0f, AXIS_LENGTH, 0.0f);
-            glm::vec3 offsetFromPalm = root + palm.getNormal() * PALM_DISK_THICKNESS;
+            Avatar::renderJointConnectingCone(batch, root, tip, AXIS_RADIUS, AXIS_RADIUS, glm::vec4(greenColor.r, greenColor.g, greenColor.b, alpha));
-            Avatar::renderJointConnectingCone(batch, root, offsetFromPalm, PALM_DISK_RADIUS, 0.0f, glm::vec4(handColor.r, handColor.g, handColor.b, alpha));
+
-            transform = Transform();
+            tip = root + palmRotation * glm::vec3(0.0f, 0.0f, AXIS_LENGTH);
-            transform.setTranslation(root);
+            Avatar::renderJointConnectingCone(batch, root, tip, AXIS_RADIUS, AXIS_RADIUS, glm::vec4(blueColor.r, blueColor.g, blueColor.b, alpha));
-            batch.setModelTransform(transform);
-            DependencyManager::get<GeometryCache>()->renderSphere(batch, PALM_BALL_RADIUS, 20.0f, 20.0f, glm::vec4(handColor.r, handColor.g, handColor.b, alpha));
         }
     }
 }

interface/src/avatar/Hand.h

@@ -11,41 +11,26 @@
 #ifndef hifi_Hand_h
 #define hifi_Hand_h
 
-#include <vector>
-
-#include <QAction>
-
-#include <glm/glm.hpp>
-
-#include <SharedUtil.h>
-
-#include <AvatarData.h>
-#include <AudioScriptingInterface.h>
 #include <HandData.h>
-#include <Model.h>
-
-#include "world.h"
-
 
 class Avatar;
+class RenderArgs;
 
 class Hand : public HandData {
 public:
     Hand(Avatar* owningAvatar);
     
     void simulate(float deltaTime, bool isMine);
-    void render(RenderArgs* renderArgs, bool isMine);
+    void renderHandTargets(RenderArgs* renderArgs, bool isMine);
 
 private:
     // disallow copies of the Hand, copy of owning Avatar is disallowed too
     Hand(const Hand&);
     Hand& operator= (const Hand&);
         
-    int _controllerButtons;             ///  Button states read from hand-held controllers
+    int _controllerButtons; ///  Button states read from hand-held controllers
 
-    Avatar*        _owningAvatar;
+    Avatar* _owningAvatar;
-    
-    void renderHandTargets(RenderArgs* renderArgs, bool isMine);
 };
 
 #endif // hifi_Hand_h

interface/src/avatar/MyAvatar.cpp

@@ -1251,7 +1251,10 @@ void MyAvatar::renderBody(RenderArgs* renderArgs, ViewFrustum* renderFrustum, fl
         getHead()->renderLookAts(renderArgs);
     }
 
-    getHand()->render(renderArgs, true);
+    if (renderArgs->_renderMode != RenderArgs::SHADOW_RENDER_MODE && 
+            Menu::getInstance()->isOptionChecked(MenuOption::DisplayHandTargets)) {
+        getHand()->renderHandTargets(renderArgs, true);
+    }
 }
 
 void MyAvatar::setVisibleInSceneIfReady(Model* model, render::ScenePointer scene, bool visible) {

libraries/animation/src/AnimInverseKinematics.cpp

@@ -178,15 +178,15 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
                     // compute the two lines that should be aligned
                     glm::vec3 jointPosition = absolutePoses[index].trans;
                     glm::vec3 leverArm = tip - jointPosition;
-                    glm::vec3 pivotLine = targetPose.trans - jointPosition;
+                    glm::vec3 targetLine = targetPose.trans - jointPosition;
 
                     // compute the axis of the rotation that would align them
-                    glm::vec3 axis = glm::cross(leverArm, pivotLine);
+                    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, pivotLine) / (glm::length(leverArm) * glm::length(pivotLine)));
+                        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.
@@ -203,7 +203,13 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
                                 glm::quat newRot = glm::normalize(glm::inverse(absolutePoses[parentIndex].rot) * deltaRotation * absolutePoses[index].rot);
                                 RotationConstraint* constraint = getConstraint(index);
                                 if (constraint) {
-                                    constraint->apply(newRot);
+                                    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[index].rot);
+                                    }
                                 }
                                 _relativePoses[index].rot = newRot;
                             }
@@ -258,7 +264,18 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
 
 //virtual
 const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
-    loadPoses(underPoses);
+    if (_relativePoses.size() != underPoses.size()) {
+        loadPoses(underPoses);
+    } else {
+        // relax toward underpose
+        const float RELAXATION_TIMESCALE = 0.25f;
+        const float alpha = glm::clamp(dt / RELAXATION_TIMESCALE, 0.0f, 1.0f);
+        int numJoints = (int)_relativePoses.size();
+        for (int i = 0; i < numJoints; ++i) {
+            float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot, underPoses[i].rot));
+            _relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * underPoses[i].rot, alpha));
+        }
+    }
     return evaluate(animVars, dt, triggersOut);
 }
 
@@ -291,34 +308,34 @@ void AnimInverseKinematics::initConstraints() {
     // We create constraints for the joints shown here
     // (and their Left counterparts if applicable).
     //
-    //                                     O RightHand
+    //                           
-    //                                    /
+    //                                    O RightHand
-    //                          O        /
+    //                      Head         /
-    //                          |       O RightForeArm
+    //                          O       /
-    //                     Neck O      /
+    //                      Neck|      O RightForeArm
-    //                          |     /
+    //                          O     /
-    //     O-------O-------O----O----O RightArm
+    //                        O | O  / RightShoulder
-    //                    Spine2|
+    //      O-------O-------O' \|/ 'O
-    //                          |
+    //                   Spine2 O  RightArm
-    //                          O Spine1
     //                          |
     //                          |
-    //                          O Spine
+    //                   Spine1 O
     //                          |
     //                          |
-    //                      O---O---O RightUpLeg
+    //                    Spine O
+    //         y                |
+    //         |                |
+    //         |            O---O---O RightUpLeg
+    //      z  |            |       |
+    //       \ |            |       |
+    //        \|            |       |
+    //  x -----+            O       O RightLeg
     //                      |       |
     //                      |       |
     //                      |       |
-    //                      O       O RightLeg
+    //                      O       O RightFoot
-    //                      |       |
+    //                     /       /
-    //        y             |       |
+    //                 O--O    O--O
-    //        |             |       |
-    //        |             O       O RightFoot
-    //     z  |            /       /
-    //      \ |        O--O    O--O
-    //       \|
-    // x -----+
 
     loadDefaultPoses(_skeleton->getRelativeBindPoses());
 
@@ -337,21 +354,23 @@ void AnimInverseKinematics::initConstraints() {
 
     _constraints.clear();
     for (int i = 0; i < numJoints; ++i) {
-        QString name = _skeleton->getJointName(i);
+        // compute the joint's baseName and remember if it was Left or not
-        bool isLeft = name.startsWith("Left", Qt::CaseInsensitive);
+        QString baseName = _skeleton->getJointName(i);
+        bool isLeft = baseName.startsWith("Left", Qt::CaseInsensitive);
         float mirror = isLeft ? -1.0f : 1.0f;
         if (isLeft) {
-            //name.remove(0, 4);
+            baseName.remove(0, 4);
-        } else if (name.startsWith("Right", Qt::CaseInsensitive)) {
+        } else if (baseName.startsWith("Right", Qt::CaseInsensitive)) {
-            //name.remove(0, 5);
+            baseName.remove(0, 5);
         }
 
         RotationConstraint* constraint = nullptr;
-        if (0 == name.compare("Arm", Qt::CaseInsensitive)) {
+        if (0 == baseName.compare("Arm", Qt::CaseInsensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f);
 
+            /* KEEP THIS CODE for future experimentation
             // these directions are approximate swing limits in root-frame
             // NOTE: they don't need to be normalized
             std::vector<glm::vec3> swungDirections;
@@ -371,9 +390,16 @@ void AnimInverseKinematics::initConstraints() {
                 swungDirections[j] = invAbsoluteRotation * swungDirections[j];
             }
             stConstraint->setSwingLimits(swungDirections);
+            */
+
+            // simple cone
+            std::vector<float> minDots;
+            const float MAX_HAND_SWING = PI / 2.0f;
+            minDots.push_back(cosf(MAX_HAND_SWING));
+            stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == name.compare("UpLeg", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("UpLegXXX", Qt::CaseInsensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
@@ -399,12 +425,19 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(swungDirections);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == name.compare("RightHand", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("Hand", Qt::CaseInsensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
-            const float MAX_HAND_TWIST = PI / 2.0f;
+            const float MAX_HAND_TWIST = PI;
-            stConstraint->setTwistLimits(-MAX_HAND_TWIST, MAX_HAND_TWIST);
+            const float MIN_HAND_TWIST = -PI / 2.0f;
+            if (isLeft) {
+                stConstraint->setTwistLimits(-MAX_HAND_TWIST, -MIN_HAND_TWIST);
+            } else {
+                stConstraint->setTwistLimits(MIN_HAND_TWIST, MAX_HAND_TWIST);
+            }
 
+            /* KEEP THIS CODE for future experimentation
+             * a more complicated wrist with asymmetric cone
             // these directions are approximate swing limits in parent-frame
             // NOTE: they don't need to be normalized
             std::vector<glm::vec3> swungDirections;
@@ -422,15 +455,28 @@ void AnimInverseKinematics::initConstraints() {
                 swungDirections[j] = invRelativeRotation * swungDirections[j];
             }
             stConstraint->setSwingLimits(swungDirections);
+            */
 
-            /*
+            // simple cone
             std::vector<float> minDots;
-            const float MAX_HAND_SWING = PI / 3.0f;
+            const float MAX_HAND_SWING = PI / 2.0f;
             minDots.push_back(cosf(MAX_HAND_SWING));
             stConstraint->setSwingLimits(minDots);
-            */
+            
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (name.startsWith("SpineXXX", Qt::CaseInsensitive)) {
+        } else if (baseName.startsWith("Shoulder", Qt::CaseInsensitive)) {
+            SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
+            stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
+            const float MAX_SHOULDER_TWIST = PI / 8.0f;
+            stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST);
+
+            std::vector<float> minDots;
+            const float MAX_SHOULDER_SWING = PI / 14.0f;
+            minDots.push_back(cosf(MAX_SHOULDER_SWING));
+            stConstraint->setSwingLimits(minDots);
+
+            constraint = static_cast<RotationConstraint*>(stConstraint);
+        } else if (baseName.startsWith("Spine", Qt::CaseInsensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             const float MAX_SPINE_TWIST = PI / 8.0f;
@@ -442,7 +488,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == name.compare("NeckXXX", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("Neck", Qt::CaseInsensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             const float MAX_NECK_TWIST = PI / 2.0f;
@@ -454,7 +500,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == name.compare("ForeArm", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("ForeArm", Qt::CaseInsensitive)) {
             // The elbow joint rotates about the parent-frame's zAxis (-zAxis) for the Right (Left) arm.
             ElbowConstraint* eConstraint = new ElbowConstraint();
             glm::quat referenceRotation = _defaultRelativePoses[i].rot;
@@ -464,7 +510,7 @@ void AnimInverseKinematics::initConstraints() {
             // then measure the angles to swing the yAxis into alignment
             glm::vec3 hingeAxis = - mirror * zAxis;
             const float MIN_ELBOW_ANGLE = 0.0f;
-            const float MAX_ELBOW_ANGLE = 7.0f * PI / 8.0f;
+            const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f;
             glm::quat invReferenceRotation = glm::inverse(referenceRotation);
             glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * yAxis;
             glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_ELBOW_ANGLE, hingeAxis) * yAxis;
@@ -485,7 +531,7 @@ void AnimInverseKinematics::initConstraints() {
             eConstraint->setAngleLimits(minAngle, maxAngle);
 
             constraint = static_cast<RotationConstraint*>(eConstraint);
-        } else if (0 == name.compare("Leg", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("LegXXX", Qt::CaseInsensitive)) {
             // The knee joint rotates about the parent-frame's -xAxis.
             ElbowConstraint* eConstraint = new ElbowConstraint();
             glm::quat referenceRotation = _defaultRelativePoses[i].rot;
@@ -516,7 +562,7 @@ void AnimInverseKinematics::initConstraints() {
             eConstraint->setAngleLimits(minAngle, maxAngle);
 
             constraint = static_cast<RotationConstraint*>(eConstraint);
-        } else if (0 == name.compare("Foot", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("FootXXX", Qt::CaseInsensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
@@ -558,14 +604,11 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
 
     _maxTargetIndex = 0;
 
-    // No constraints!
-    /*
     if (skeleton) {
         initConstraints();
     } else {
         clearConstraints();
     }
-    */
 }
 
 void AnimInverseKinematics::relaxTowardDefaults(float dt) {

libraries/animation/src/AnimInverseKinematics.h

@@ -46,9 +46,9 @@ protected:
 
     struct IKTargetVar {
         IKTargetVar(const QString& jointNameIn, const std::string& positionVarIn, const std::string& rotationVarIn) :
-            jointName(jointNameIn),
             positionVar(positionVarIn),
             rotationVar(rotationVarIn),
+            jointName(jointNameIn),
             jointIndex(-1),
             hasPerformedJointLookup(false) {}
 

libraries/animation/src/SwingTwistConstraint.cpp

@@ -19,6 +19,10 @@
 const float MIN_MINDOT = -0.999f;
 const float MAX_MINDOT = 1.0f;
 
+const int LAST_CLAMP_LOW_BOUNDARY = -1;
+const int LAST_CLAMP_NO_BOUNDARY = 0;
+const int LAST_CLAMP_HIGH_BOUNDARY = 1;
+
 SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() {
     setCone(PI);
 }
@@ -67,7 +71,8 @@ SwingTwistConstraint::SwingTwistConstraint() :
         RotationConstraint(),
         _swingLimitFunction(),
         _minTwist(-PI),
-        _maxTwist(PI)
+        _maxTwist(PI),
+        _lastTwistBoundary(0)
 {
 }
 
@@ -158,10 +163,11 @@ void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) {
     // NOTE: min/maxTwist angles should be in the range [-PI, PI]
     _minTwist = glm::min(minTwist, maxTwist);
     _maxTwist = glm::max(minTwist, maxTwist);
+
+    _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
 }
 
 bool SwingTwistConstraint::apply(glm::quat& rotation) const {
-
     // decompose the rotation into first twist about yAxis, then swing about something perp
     const glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
     // NOTE: rotation = postRotation * referenceRotation
@@ -176,9 +182,43 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
     glm::vec3 twistedX = twistRotation * xAxis;
     twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis));
 
+    // adjust measured twistAngle according to clamping history
+    switch (_lastTwistBoundary) {
+        case LAST_CLAMP_LOW_BOUNDARY:
+            // clamp to min
+            if (twistAngle > _maxTwist) {
+                twistAngle -= TWO_PI;
+            }
+            break;
+        case LAST_CLAMP_HIGH_BOUNDARY:
+            // clamp to max
+            if (twistAngle < _minTwist) {
+                twistAngle += TWO_PI;
+            }
+            break;
+        default: // LAST_CLAMP_NO_BOUNDARY
+            // clamp to nearest boundary
+            float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI);
+            if (twistAngle > midBoundary) {
+                // lower boundary is closer --> phase down one cycle
+                twistAngle -= TWO_PI;
+            } else if (twistAngle < midBoundary - TWO_PI) {
+                // higher boundary is closer --> phase up one cycle
+                twistAngle += TWO_PI;
+            }
+            break;
+    }
+
     // clamp twistAngle
     float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
     bool twistWasClamped = (twistAngle != clampedTwistAngle);
+
+    // remember twist's clamp boundary history
+    if (twistWasClamped) {
+        _lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY;
+    } else {
+        _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
+    }
     
     // clamp the swing
     // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).
@@ -201,6 +241,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
     }
 
     if (swingWasClamped || twistWasClamped) {
+        // update the rotation
         twistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
         rotation = swingRotation * twistRotation * _referenceRotation;
         return true;

libraries/animation/src/SwingTwistConstraint.h

@@ -77,6 +77,10 @@ protected:
     SwingLimitFunction _swingLimitFunction;
     float _minTwist;
     float _maxTwist;
+
+    // We want to remember the LAST clamped boundary, so we an use it even when the far boundary is closer.
+    // This reduces "pops" when the input twist angle goes far beyond and wraps around toward the far boundary.
+    mutable int _lastTwistBoundary;
 };
 
 #endif // hifi_SwingTwistConstraint_h

libraries/avatars/src/HandData.h

@@ -19,6 +19,7 @@
 #include <glm/gtc/quaternion.hpp>
 
 #include <NumericalConstants.h>
+#include <SharedUtil.h>
 
 class AvatarData;
 class PalmData;

libraries/physics/src/DynamicCharacterController.h

@@ -12,7 +12,6 @@ class btCollisionWorld;
 
 const int NUM_CHARACTER_CONTROLLER_RAYS = 2;
 
-///DynamicCharacterController is obsolete/unsupported at the moment
 class DynamicCharacterController : public btCharacterControllerInterface
 {
 protected: