Merge pull request #2720 from ey6es/master

Added option to render joint constraints, changed safeEulerAngles so that z (rather than y) is in [-pi/2, pi/2], added ability to set end effector rotation in IK, added option to avoid aligning forearms with wrists.

interface/src/Menu.cpp

@@ -335,6 +335,8 @@ Menu::Menu() :
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::PlaySlaps, 0, false);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::HandsCollideWithSelf, 0, false);
+    addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::ShowIKConstraints, 0, false);
+    addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::AlignForearmsWithWrists, 0, true);
 
     addDisabledActionAndSeparator(developerMenu, "Testing");
 

interface/src/Menu.h

@@ -256,6 +256,7 @@ private:
 
 namespace MenuOption {
     const QString AboutApp = "About Interface";
+    const QString AlignForearmsWithWrists = "Align Forearms with Wrists";
     const QString AmbientOcclusion = "Ambient Occlusion";
     const QString Atmosphere = "Atmosphere";
     const QString AudioNoiseReduction = "Audio Noise Reduction";
@@ -354,6 +355,7 @@ namespace MenuOption {
     const QString SettingsImport = "Import Settings";
     const QString Shadows = "Shadows";
     const QString ShowCulledSharedFaces = "Show Culled Shared Voxel Faces";
+    const QString ShowIKConstraints = "Show IK Constraints";
     const QString Stars = "Stars";
     const QString Stats = "Stats";
     const QString StopAllScripts = "Stop All Scripts";

interface/src/avatar/MyAvatar.cpp

@@ -462,6 +462,9 @@ void MyAvatar::render(const glm::vec3& cameraPosition, RenderMode renderMode) {
         return; // exit early
     }
     Avatar::render(cameraPosition, renderMode);
+    if (Menu::getInstance()->isOptionChecked(MenuOption::ShowIKConstraints)) {
+        _skeletonModel.renderIKConstraints();
+    }
 }
 
 void MyAvatar::renderHeadMouse() const {

interface/src/avatar/SkeletonModel.cpp

@@ -103,6 +103,11 @@ void SkeletonModel::getBodyShapes(QVector<const Shape*>& shapes) const {
     shapes.push_back(&_boundingShape);
 }
 
+void SkeletonModel::renderIKConstraints() {
+    renderJointConstraints(getRightHandJointIndex());
+    renderJointConstraints(getLeftHandJointIndex());
+}
+
 class IndexValue {
 public:
     int index;
@@ -133,7 +138,7 @@ void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position)
 
     // align hand with forearm
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
-    applyRotationDelta(jointIndex, rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), false);
+    applyRotationDelta(jointIndex, rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector));
 }
 
 void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,
@@ -148,12 +153,15 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
         return;
     }
     
-    // rotate forearm to align with palm direction
+    // rotate palm to align with palm direction
     glm::quat palmRotation;
-    getJointRotation(parentJointIndex, palmRotation, true);
+    if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
-    applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), false);
+        getJointRotation(parentJointIndex, palmRotation, true);
-    getJointRotation(parentJointIndex, palmRotation, true);
+    } else {
-
+        getJointRotation(jointIndex, palmRotation, true);
+    }
+    palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
+    
     // sort the finger indices by raw x, get the average direction
     QVector<IndexValue> fingerIndices;
     glm::vec3 direction;
@@ -173,17 +181,20 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     float directionLength = glm::length(direction);
     const unsigned int MIN_ROTATION_FINGERS = 3;
     if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
-        applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction), false);
+        palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
-        getJointRotation(parentJointIndex, palmRotation, true);
     }
 
-    // let wrist inherit forearm rotation
+    // set hand position, rotation
-    _jointStates[jointIndex].rotation = glm::quat();
+    if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
-
+        glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
-    // set elbow position from wrist position
+        setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
-    glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
+            geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale));
-    setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
+        setJointRotation(parentJointIndex, palmRotation, true);
-        geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale));
+        _jointStates[jointIndex].rotation = glm::quat();
+        
+    } else {
+        setJointPosition(jointIndex, palm.getPosition(), palmRotation, true);
+    }
 }
 
 void SkeletonModel::updateJointState(int index) {
@@ -210,3 +221,59 @@ void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const
         glm::normalize(inverse * axes[0])) * joint.rotation;
 }
 
+void SkeletonModel::renderJointConstraints(int jointIndex) {
+    if (jointIndex == -1) {
+        return;
+    }
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    const float BASE_DIRECTION_SIZE = 300.0f;
+    float directionSize = BASE_DIRECTION_SIZE * extractUniformScale(_scale);
+    glLineWidth(3.0f);
+    do {
+        const FBXJoint& joint = geometry.joints.at(jointIndex);
+        const JointState& jointState = _jointStates.at(jointIndex);
+        glm::vec3 position = extractTranslation(jointState.transform) + _translation;
+        
+        glPushMatrix();
+        glTranslatef(position.x, position.y, position.z);
+        glm::quat parentRotation = (joint.parentIndex == -1) ? _rotation : _jointStates.at(joint.parentIndex).combinedRotation;
+        glm::vec3 rotationAxis = glm::axis(parentRotation);
+        glRotatef(glm::degrees(glm::angle(parentRotation)), rotationAxis.x, rotationAxis.y, rotationAxis.z);
+        float fanScale = directionSize * 0.75f;
+        glScalef(fanScale, fanScale, fanScale);
+        const int AXIS_COUNT = 3;
+        for (int i = 0; i < AXIS_COUNT; i++) {
+            if (joint.rotationMin[i] <= -PI + EPSILON && joint.rotationMax[i] >= PI - EPSILON) {
+                continue; // unconstrained
+            }
+            glm::vec3 axis;
+            axis[i] = 1.0f;
+            
+            glm::vec3 otherAxis;
+            if (i == 0) {
+                otherAxis.y = 1.0f;
+            } else {
+                otherAxis.x = 1.0f;
+            }
+            glColor4f(otherAxis.r, otherAxis.g, otherAxis.b, 0.75f);
+        
+            glBegin(GL_TRIANGLE_FAN);
+            glVertex3f(0.0f, 0.0f, 0.0f);
+            const int FAN_SEGMENTS = 16;
+            for (int j = 0; j < FAN_SEGMENTS; j++) {
+                glm::vec3 rotated = glm::angleAxis(glm::mix(joint.rotationMin[i], joint.rotationMax[i],
+                    (float)j / (FAN_SEGMENTS - 1)), axis) * otherAxis;
+                glVertex3f(rotated.x, rotated.y, rotated.z);
+            }
+            glEnd();
+        }
+        glPopMatrix();
+        
+        renderOrientationDirections(position, jointState.combinedRotation, directionSize);
+        jointIndex = joint.parentIndex;
+        
+    } while (jointIndex != -1 && geometry.joints.at(jointIndex).isFree);
+    
+    glLineWidth(1.0f);
+}
+

interface/src/avatar/SkeletonModel.h

@@ -33,6 +33,8 @@ public:
     ///  \param shapes[out] list of shapes for body collisions
     void getBodyShapes(QVector<const Shape*>& shapes) const;
 
+    void renderIKConstraints();
+
 protected:
 
     void applyHandPosition(int jointIndex, const glm::vec3& position);
@@ -46,6 +48,8 @@ protected:
     virtual void maybeUpdateLeanRotation(const JointState& parentState, const FBXJoint& joint, JointState& state);
     
 private:
+
+    void renderJointConstraints(int jointIndex);
     
     Avatar* _owningAvatar;
 };

interface/src/renderer/Model.cpp

@@ -882,12 +882,12 @@ bool Model::getJointRotation(int jointIndex, glm::quat& rotation, bool fromBind)
     return true;
 }
 
-bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int lastFreeIndex,
+bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const glm::quat& rotation, bool useRotation,
-        bool allIntermediatesFree, const glm::vec3& alignment) {
+       int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
-    glm::vec3 relativePosition = position - _translation;
+    glm::vec3 relativePosition = translation - _translation;
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
     if (freeLineage.isEmpty()) {
@@ -896,13 +896,21 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
     if (lastFreeIndex == -1) {
         lastFreeIndex = freeLineage.last();
     }
-
+    
     // this is a cyclic coordinate descent algorithm: see
     // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
     const int ITERATION_COUNT = 1;
     glm::vec3 worldAlignment = _rotation * alignment;
     for (int i = 0; i < ITERATION_COUNT; i++) {
-        // first, we go from the joint upwards, rotating the end as close as possible to the target
+        // first, try to rotate the end effector as close as possible to the target rotation, if any
+        glm::quat endRotation;
+        if (useRotation) {
+            getJointRotation(jointIndex, endRotation, true);
+            applyRotationDelta(jointIndex, rotation * glm::inverse(endRotation));
+            getJointRotation(jointIndex, endRotation, true);
+        }    
+        
+        // then, we go from the joint upwards, rotating the end as close as possible to the target
         glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].transform);
         for (int j = 1; freeLineage.at(j - 1) != lastFreeIndex; j++) {
             int index = freeLineage.at(j);
@@ -914,8 +922,17 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
             glm::vec3 jointPosition = extractTranslation(state.transform);
             glm::vec3 jointVector = endPosition - jointPosition;
             glm::quat oldCombinedRotation = state.combinedRotation;
-            applyRotationDelta(index, rotationBetween(jointVector, relativePosition - jointPosition));
+            glm::quat combinedDelta;
-            endPosition = state.combinedRotation * glm::inverse(oldCombinedRotation) * jointVector + jointPosition;
+            float combinedWeight;
+            if (useRotation) {
+                combinedDelta = safeMix(rotation * glm::inverse(endRotation),
+                    rotationBetween(jointVector, relativePosition - jointPosition), 0.5f);
+                combinedWeight = 2.0f;
+                
+            } else {
+                combinedDelta = rotationBetween(jointVector, relativePosition - jointPosition);
+                combinedWeight = 1.0f;
+            }
             if (alignment != glm::vec3() && j > 1) {
                 jointVector = endPosition - jointPosition;
                 glm::vec3 positionSum;
@@ -929,9 +946,16 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
                 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) {
-                    applyRotationDelta(index, rotationBetween(projectedCenterOfMass, projectedAlignment));
+                    combinedDelta = safeMix(combinedDelta, rotationBetween(projectedCenterOfMass, projectedAlignment),
+                        1.0f / (combinedWeight + 1.0f));
                 }
             }
+            applyRotationDelta(index, combinedDelta);
+            glm::quat actualDelta = state.combinedRotation * glm::inverse(oldCombinedRotation);
+            endPosition = actualDelta * jointVector + jointPosition;
+            if (useRotation) {
+                endRotation = actualDelta * endRotation;
+            }
         }       
     }
      
@@ -1012,8 +1036,9 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
         state.combinedRotation = delta * state.combinedRotation;
         return;
     }
-    glm::quat newRotation = glm::quat(glm::clamp(safeEulerAngles(state.rotation *
+    glm::quat targetRotation = delta * state.combinedRotation;
-        glm::inverse(state.combinedRotation) * delta * state.combinedRotation), joint.rotationMin, joint.rotationMax));
+    glm::vec3 eulers = safeEulerAngles(state.rotation * glm::inverse(state.combinedRotation) * targetRotation);
+    glm::quat newRotation = glm::quat(glm::clamp(eulers, joint.rotationMin, joint.rotationMax));
     state.combinedRotation = state.combinedRotation * glm::inverse(state.rotation) * newRotation;
     state.rotation = newRotation;
 }
@@ -1141,7 +1166,7 @@ void Model::applyCollision(CollisionInfo& collision) {
                 getJointPosition(jointIndex, end);
                 glm::vec3 newEnd = start + glm::angleAxis(angle, axis) * (end - start);
                 // try to move it
-                setJointPosition(jointIndex, newEnd, -1, true);
+                setJointPosition(jointIndex, newEnd, glm::quat(), false, -1, true);
             }
         }
     }

interface/src/renderer/Model.h

@@ -242,8 +242,9 @@ protected:
     bool getJointPosition(int jointIndex, glm::vec3& position) const;
     bool getJointRotation(int jointIndex, glm::quat& rotation, bool fromBind = false) const;
     
-    bool setJointPosition(int jointIndex, const glm::vec3& position, int lastFreeIndex = -1,
+    bool setJointPosition(int jointIndex, const glm::vec3& translation, const glm::quat& rotation = glm::quat(),
-        bool allIntermediatesFree = false, const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f));
+        bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false,
+        const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f));
     bool setJointRotation(int jointIndex, const glm::quat& rotation, bool fromBind = false);
     
     void setJointTranslation(int jointIndex, const glm::vec3& translation);

libraries/shared/src/SharedUtil.cpp

@@ -656,27 +656,59 @@ void debug::checkDeadBeef(void* memoryVoid, int size) {
 // https://github.com/threerings/clyde/blob/master/src/main/java/com/threerings/math/Quaternion.java)
 glm::vec3 safeEulerAngles(const glm::quat& q) {
     float sy = 2.0f * (q.y * q.w - q.x * q.z);
+    glm::vec3 eulers;
     if (sy < 1.0f - EPSILON) {
         if (sy > -1.0f + EPSILON) {
-            return glm::vec3(
+            eulers = glm::vec3(
                 atan2f(q.y * q.z + q.x * q.w, 0.5f - (q.x * q.x + q.y * q.y)),
                 asinf(sy),
                 atan2f(q.x * q.y + q.z * q.w, 0.5f - (q.y * q.y + q.z * q.z)));
 
         } else {
             // not a unique solution; x + z = atan2(-m21, m11)
-            return glm::vec3(
+            eulers = glm::vec3(
                 0.0f,
                 - PI_OVER_TWO,
                 atan2f(q.x * q.w - q.y * q.z, 0.5f - (q.x * q.x + q.z * q.z)));
         }
     } else {
         // not a unique solution; x - z = atan2(-m21, m11)
-        return glm::vec3(
+        eulers = glm::vec3(
             0.0f,
             PI_OVER_TWO,
             -atan2f(q.x * q.w - q.y * q.z, 0.5f - (q.x * q.x + q.z * q.z)));
     }
+    
+    // adjust so that z, rather than y, is in [-pi/2, pi/2]
+    if (eulers.z < -PI_OVER_TWO) {
+        if (eulers.x < 0.0f) {
+            eulers.x += PI;
+        } else {
+            eulers.x -= PI;
+        }
+        eulers.y = -eulers.y;
+        if (eulers.y < 0.0f) {
+            eulers.y += PI;
+        } else {
+            eulers.y -= PI;
+        }
+        eulers.z += PI;
+        
+    } else if (eulers.z > PI_OVER_TWO) {
+        if (eulers.x < 0.0f) {
+            eulers.x += PI;
+        } else {
+            eulers.x -= PI;
+        }
+        eulers.y = -eulers.y;
+        if (eulers.y < 0.0f) {
+            eulers.y += PI;
+        } else {
+            eulers.y -= PI;
+        }
+        eulers.z -= PI;
+    }
+    return eulers;
 }
 
 //  Helper function returns the positive angle (in radians) between two 3D vectors