Merge pull request #1250 from ey6es/master

Back to CCR for IK (with rotational alignment to replace "gravity"), read/apply constraints correctly, Leap tweaks.

interface/src/avatar/Avatar.cpp

@@ -390,6 +390,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
         }
     }
     
+    _hand.simulate(deltaTime, false);
     _skeletonModel.simulate(deltaTime);
     _head.setBodyRotation(glm::vec3(_bodyPitch, _bodyYaw, _bodyRoll));
     glm::vec3 headPosition;
@@ -399,7 +400,6 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     _head.setPosition(headPosition);
     _head.setSkinColor(glm::vec3(SKIN_COLOR[0], SKIN_COLOR[1], SKIN_COLOR[2]));
     _head.simulate(deltaTime, false);
-    _hand.simulate(deltaTime, false);
 
     // use speed and angular velocity to determine walking vs. standing
     if (_speed + fabs(_bodyYawDelta) > 0.2) {
@@ -665,14 +665,15 @@ void Avatar::updateArmIKAndConstraints(float deltaTime, AvatarJointID fingerTipJ
     float distance = glm::length(armVector);
     
     // don't let right hand get dragged beyond maximum arm length...
+    float armLength = _skeletonModel.isActive() ? _skeletonModel.getRightArmLength() : _skeleton.getArmLength();
     const float ARM_RETRACTION = 0.75f;
-    float armLength = _maxArmLength * ARM_RETRACTION;
+    float retractedArmLength = armLength * ARM_RETRACTION;
-    if (distance > armLength) {
+    if (distance > retractedArmLength) {
         // reset right hand to be constrained to maximum arm length
         fingerJoint.position = shoulderJoint.position;
         glm::vec3 armNormal = armVector / distance;
-        armVector = armNormal * armLength;
+        armVector = armNormal * retractedArmLength;
-        distance = armLength;
+        distance = retractedArmLength;
         glm::vec3 constrainedPosition = shoulderJoint.position;
         constrainedPosition += armVector;
         fingerJoint.position = constrainedPosition;

interface/src/avatar/Hand.cpp

@@ -52,6 +52,8 @@ void Hand::reset() {
 
 void Hand::simulate(float deltaTime, bool isMine) {
     
+    calculateGeometry();
+
     if (_isRaveGloveActive) {
         if (_raveGloveEffectsModeChanged && _raveGloveInitialized) {
             activateNewRaveGloveMode();
@@ -66,8 +68,8 @@ void Hand::calculateGeometry() {
     const glm::vec3 leapHandsOffsetFromFace(0.0, -0.2, -0.3);  // place the hand in front of the face where we can see it
     
     Head& head = _owningAvatar->getHead();
-    _basePosition = head.getPosition() + head.getOrientation() * leapHandsOffsetFromFace;
+    _baseOrientation = _owningAvatar->getOrientation();
-    _baseOrientation = head.getOrientation();
+    _basePosition = head.calculateAverageEyePosition() + _baseOrientation * leapHandsOffsetFromFace * head.getScale();
     
     // generate finger tip balls....
     _leapFingerTipBalls.clear();
@@ -136,8 +138,6 @@ void Hand::render(bool lookingInMirror) {
     _renderAlpha = 1.0;
     _lookingInMirror = lookingInMirror;
     
-    calculateGeometry();
-
     if (Menu::getInstance()->isOptionChecked(MenuOption::DisplayLeapHands)) {
         if (!isRaveGloveActive()) {
             renderLeapFingerTrails();

interface/src/avatar/MyAvatar.cpp

@@ -320,6 +320,7 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter) {
         }
     }
     
+    _hand.simulate(deltaTime, true);
     _skeletonModel.simulate(deltaTime);
     _head.setBodyRotation(glm::vec3(_bodyPitch, _bodyYaw, _bodyRoll));
     glm::vec3 headPosition;
@@ -330,7 +331,6 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter) {
     _head.setScale(_scale);
     _head.setSkinColor(glm::vec3(SKIN_COLOR[0], SKIN_COLOR[1], SKIN_COLOR[2]));
     _head.simulate(deltaTime, true);
-    _hand.simulate(deltaTime, true);
 
     const float WALKING_SPEED_THRESHOLD = 0.2f;
     // use speed and angular velocity to determine walking vs. standing

interface/src/avatar/SkeletonModel.cpp

@@ -141,7 +141,8 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
     glm::quat palmRotation;
     getJointRotation(jointIndex, palmRotation, true);
-    palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
+    applyRotationDelta(jointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()));
+    getJointRotation(jointIndex, palmRotation, true);
     
     // sort the finger indices by raw x, get the average direction
     QVector<IndexValue> fingerIndices;
@@ -160,10 +161,11 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     
     // rotate palm according to average finger direction
     float directionLength = glm::length(direction);
-    if (directionLength > EPSILON) {
+    const int MIN_ROTATION_FINGERS = 3;
-        palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
+    if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
+        applyRotationDelta(jointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction));
+        getJointRotation(jointIndex, palmRotation, true);
     }
-    setJointRotation(jointIndex, palmRotation, true);
     
     // no point in continuing if there are no fingers
     if (palm.getNumFingers() == 0 || fingerJointIndices.isEmpty()) {

interface/src/renderer/FBXReader.cpp

@@ -875,9 +875,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                     glm::vec3 preRotation, rotation, postRotation;
                     glm::vec3 scale = glm::vec3(1.0f, 1.0f, 1.0f);
                     glm::vec3 scalePivot, rotationPivot;
+                    bool rotationMinX = false, rotationMinY = false, rotationMinZ = false;
+                    bool rotationMaxX = false, rotationMaxY = false, rotationMaxZ = false;
+                    glm::vec3 rotationMin, rotationMax;
                     FBXModel model = { name, -1 };
-                    model.rotationMin = glm::vec3(-180.0f, -180.0f, -180.0f);
-                    model.rotationMax = glm::vec3(180.0f, 180.0f, 180.0f);
                     foreach (const FBXNode& subobject, object.children) {
                         bool properties = false;
                         QByteArray propertyName;
@@ -920,10 +921,28 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                                         scale = getVec3(property.properties, index);
                                         
                                     } else if (property.properties.at(0) == "RotationMin") {
-                                        model.rotationMin = getVec3(property.properties, index);
+                                        rotationMin = getVec3(property.properties, index);
                                         
                                     } else if (property.properties.at(0) == "RotationMax") {
-                                        model.rotationMax = getVec3(property.properties, index);
+                                        rotationMax = getVec3(property.properties, index);
+                                    
+                                    } else if (property.properties.at(0) == "RotationMinX") {
+                                        rotationMinX = property.properties.at(index).toBool();
+                                        
+                                    } else if (property.properties.at(0) == "RotationMinY") {
+                                        rotationMinY = property.properties.at(index).toBool();
+                                    
+                                    } else if (property.properties.at(0) == "RotationMinZ") {
+                                        rotationMinZ = property.properties.at(index).toBool();
+                                    
+                                    } else if (property.properties.at(0) == "RotationMaxX") {
+                                        rotationMaxX = property.properties.at(index).toBool();
+                                        
+                                    } else if (property.properties.at(0) == "RotationMaxY") {
+                                        rotationMaxY = property.properties.at(index).toBool();
+                                    
+                                    } else if (property.properties.at(0) == "RotationMaxZ") {
+                                        rotationMaxZ = property.properties.at(index).toBool();
                                     }
                                 }
                             }
@@ -940,6 +959,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                     model.postRotation = glm::quat(glm::radians(postRotation));
                     model.postTransform = glm::translate(-rotationPivot) * glm::translate(scalePivot) *
                         glm::scale(scale) * glm::translate(-scalePivot);
+                    model.rotationMin = glm::vec3(rotationMinX ? rotationMin.x : -180.0f,
+                        rotationMinY ? rotationMin.y : -180.0f, rotationMinZ ? rotationMin.z : -180.0f);
+                    model.rotationMax = glm::vec3(rotationMaxX ? rotationMax.x : 180.0f,
+                        rotationMaxY ? rotationMax.y : 180.0f, rotationMaxZ ? rotationMax.z : 180.0f);
                     models.insert(getID(object.properties), model);
 
                 } else if (object.name == "Texture") {

interface/src/renderer/Model.cpp

@@ -584,7 +584,8 @@ 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& position, int lastFreeIndex,
+        bool allIntermediatesFree, const glm::vec3& alignment) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
@@ -595,48 +596,41 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
         lastFreeIndex = freeLineage.last();
     }
 
-    // this is a constraint relaxation algorithm: see
+    // this is a cyclic coordinate descent algorithm: see
-    // http://www.ryanjuckett.com/programming/animation/22-constraint-relaxation-ik-in-2d
+    // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
-    
+    const int ITERATION_COUNT = 1;
-    // the influence of gravity; lowers the potential energy of our configurations
+    glm::vec3 worldAlignment = _rotation * alignment;
-    glm::vec3 gravity = _rotation * IDENTITY_UP * -0.01f;
-    
-    // over one or more iterations, apply the length constraints and update the rotations accordingly
-    float uniformScale = (_scale.x + _scale.y + _scale.z) / 3.0f;
-    const int ITERATION_COUNT = 3;
     for (int i = 0; i < ITERATION_COUNT; i++) {
-        // start by optimistically setting the position of the end joint to our target
+        // first, we go from the joint upwards, rotating the end as close as possible to the target
-        setJointTranslation(jointIndex, freeLineage.at(1), -1, relativePosition);
+        glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].transform);
-    
         for (int j = 1; freeLineage.at(j - 1) != lastFreeIndex; j++) {
-            int sourceIndex = freeLineage.at(j);
+            int index = freeLineage.at(j);
-            int destIndex = freeLineage.at(j - 1);
+            const FBXJoint& joint = geometry.joints.at(index);
-            JointState& sourceState = _jointStates[sourceIndex];
+            if (!(joint.isFree || allIntermediatesFree)) {
-            JointState& destState = _jointStates[destIndex];
-            glm::vec3 sourceTranslation = extractTranslation(sourceState.transform);
-            glm::vec3 destTranslation = extractTranslation(destState.transform);
-            glm::vec3 boneVector = destTranslation - sourceTranslation;
-            float boneLength = glm::length(boneVector);
-            if (boneLength < EPSILON) {
                 continue;
             }
-            float extension = geometry.joints.at(destIndex).distanceToParent * uniformScale / boneLength - 1.0f;
+            JointState& state = _jointStates[index];
-            if (fabs(extension) < EPSILON) {
+            glm::vec3 jointPosition = extractTranslation(state.transform);
-                continue;
+            glm::vec3 jointVector = endPosition - jointPosition;
+            glm::quat oldCombinedRotation = state.combinedRotation;
+            applyRotationDelta(index, rotationBetween(jointVector, relativePosition - jointPosition));
+            endPosition = state.combinedRotation * glm::inverse(oldCombinedRotation) * jointVector + jointPosition;
+            if (alignment != glm::vec3() && j > 1) {
+                jointVector = endPosition - jointPosition;
+                glm::vec3 positionSum;
+                for (int k = j - 1; k > 0; k--) {
+                    int index = freeLineage.at(k);
+                    updateJointState(index);
+                    positionSum += extractTranslation(_jointStates.at(index).transform);
+                }
+                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) {
+                    applyRotationDelta(index, rotationBetween(projectedCenterOfMass, projectedAlignment));
+                }
             }
-            if (j == 1) {
-                setJointTranslation(sourceIndex, freeLineage.at(j + 1), -1,
-                    sourceTranslation - boneVector * extension + gravity);
-            
-            } else if (sourceIndex == lastFreeIndex) {
-                setJointTranslation(destIndex, -1, freeLineage.at(j - 2),
-                    destTranslation + boneVector * extension + gravity);
-
-            } else {
-                setJointTranslation(sourceIndex, freeLineage.at(j + 1), -1,
-                    sourceTranslation - boneVector * extension * 0.5f + gravity);
-                setJointTranslation(destIndex, -1, freeLineage.at(j - 2),
-                    destTranslation + boneVector * extension * 0.5f + gravity);
         }       
     }
      
@@ -644,7 +638,6 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
     for (int j = freeLineage.size() - 1; j >= 0; j--) {
         updateJointState(freeLineage.at(j));
     }
-    }
         
     return true;
 }
@@ -679,13 +672,29 @@ float Model::getLimbLength(int jointIndex) const {
     }
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
-    int length = 0.0f;
+    float length = 0.0f;
+    float lengthScale = (_scale.x + _scale.y + _scale.z) / 3.0f;
     for (int i = freeLineage.size() - 2; i >= 0; i--) {
-        length += geometry.joints.at(freeLineage.at(i)).distanceToParent;
+        length += geometry.joints.at(freeLineage.at(i)).distanceToParent * lengthScale;
     }
     return length;
 }
 
+void Model::applyRotationDelta(int jointIndex, const glm::quat& delta) {
+    JointState& state = _jointStates[jointIndex];
+    const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex];
+    if (joint.rotationMin == glm::vec3(-180.0f, -180.0f, -180.0f) && joint.rotationMax == glm::vec3(180.0f, 180.0f, 180.0f)) {
+        // no constraints
+        state.rotation = state.rotation * glm::inverse(state.combinedRotation) * delta * state.combinedRotation;
+        state.combinedRotation = delta * state.combinedRotation;
+        return;
+    }
+    glm::quat newRotation = glm::quat(glm::radians(glm::clamp(safeEulerAngles(state.rotation *
+        glm::inverse(state.combinedRotation) * delta * state.combinedRotation), joint.rotationMin, joint.rotationMax)));
+    state.combinedRotation = state.combinedRotation * glm::inverse(state.rotation) * newRotation;
+    state.rotation = newRotation;
+}
+
 void Model::setJointTranslation(int jointIndex, int parentIndex, int childIndex, const glm::vec3& translation) {
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     JointState& state = _jointStates[jointIndex];
@@ -705,21 +714,6 @@ void Model::setJointTranslation(int jointIndex, int parentIndex, int childIndex,
     ::setTranslation(state.transform, translation);
 }
 
-void Model::applyRotationDelta(int jointIndex, const glm::quat& delta) {
-    JointState& state = _jointStates[jointIndex];
-    const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex];
-    if (joint.rotationMin == glm::vec3(-180.0f, -180.0f, -180.0f) && joint.rotationMax == glm::vec3(180.0f, 180.0f, 180.0f)) {
-        // no constraints
-        state.rotation = state.rotation * glm::inverse(state.combinedRotation) * delta * state.combinedRotation;
-        state.combinedRotation = delta * state.combinedRotation;
-        return;
-    }
-    glm::quat newRotation = glm::quat(glm::radians(glm::clamp(safeEulerAngles(state.rotation *
-        glm::inverse(state.combinedRotation) * delta * state.combinedRotation), joint.rotationMin, joint.rotationMax)));
-    state.combinedRotation = state.combinedRotation * glm::inverse(state.rotation) * newRotation;
-    state.rotation = newRotation;
-}
-
 void Model::deleteGeometry() {
     foreach (Model* attachment, _attachments) {
         delete attachment;

interface/src/renderer/Model.h

@@ -143,7 +143,8 @@ 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& position, 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);
     
     /// Restores the indexed joint to its default position.
@@ -156,10 +157,11 @@ protected:
     /// first free ancestor.
     float getLimbLength(int jointIndex) const;
     
+    void applyRotationDelta(int jointIndex, const glm::quat& delta);
+    
 private:
     
     void setJointTranslation(int jointIndex, int parentIndex, int childIndex, const glm::vec3& translation);
-    void applyRotationDelta(int jointIndex, const glm::quat& delta);
     
     void deleteGeometry();