Extremely basic IK based on implementation of the cyclic coordinate descent

algorithm.  Actually seems to work pretty well.

interface/src/avatar/SkeletonModel.cpp

@@ -26,6 +26,8 @@ void SkeletonModel::simulate(float deltaTime) {
     setScale(glm::vec3(1.0f, 1.0f, 1.0f) * _owningAvatar->getScale() * MODEL_SCALE);
     
     Model::simulate(deltaTime);
+    
+    setRightHandPosition(_owningAvatar->getHandPosition());
 }
 
 bool SkeletonModel::render(float alpha) {

interface/src/renderer/FBXReader.cpp

@@ -326,11 +326,11 @@ QVariantHash parseMapping(QIODevice* device) {
         }
         QByteArray name = sections.at(0).trimmed();
         if (sections.size() == 2) {
-            properties.insert(name, sections.at(1).trimmed());
+            properties.insertMulti(name, sections.at(1).trimmed());
         
         } else if (sections.size() == 3) {
             QVariantHash heading = properties.value(name).toHash();
-            heading.insert(sections.at(1).trimmed(), sections.at(2).trimmed());
+            heading.insertMulti(sections.at(1).trimmed(), sections.at(2).trimmed());
             properties.insert(name, heading);
             
         } else if (sections.size() >= 4) {
@@ -696,12 +696,14 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
     QString jointRootName = processID(joints.value("jointRoot", "jointRoot").toString());
     QString jointLeanName = processID(joints.value("jointLean", "jointLean").toString());
     QString jointHeadName = processID(joints.value("jointHead", "jointHead").toString());
+    QString jointRightHandName = processID(joints.value("jointRightHand", "jointRightHand").toString());
     QString jointEyeLeftID;
     QString jointEyeRightID;
     QString jointNeckID;
     QString jointRootID;
     QString jointLeanID;
     QString jointHeadID;
+    QString jointRightHandID;
     
     QVariantHash blendshapeMappings = mapping.value("bs").toHash();
     QHash<QByteArray, QPair<int, float> > blendshapeIndices;
@@ -775,6 +777,9 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                         
                     } else if (name == jointHeadName) {
                         jointHeadID = getID(object.properties);
+                        
+                    } else if (name == jointRightHandName) {
+                        jointRightHandID = getID(object.properties);
                     }
                     glm::vec3 translation;
                     glm::vec3 rotationOffset;
@@ -978,10 +983,24 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
     }
     
     // convert the models to joints
+    QVariantList freeJoints = mapping.values("freeJoint");
     foreach (const QString& modelID, modelIDs) {
         const FBXModel& model = models[modelID];
         FBXJoint joint;
+        joint.isFree = freeJoints.contains(model.name);
         joint.parentIndex = model.parentIndex;
+        
+        // get the indices of all ancestors starting with the first free one (if any)
+        joint.freeLineage.append(geometry.joints.size());
+        int lastFreeIndex = joint.isFree ? 0 : -1;
+        for (int index = joint.parentIndex; index != -1; index = geometry.joints.at(index).parentIndex) {
+            if (geometry.joints.at(index).isFree) {
+                lastFreeIndex = joint.freeLineage.size();
+            }
+            joint.freeLineage.append(index);
+        }
+        joint.freeLineage.remove(lastFreeIndex + 1, joint.freeLineage.size() - lastFreeIndex - 1);
+        
         joint.preTransform = model.preTransform;
         joint.preRotation = model.preRotation;
         joint.rotation = model.rotation;
@@ -1009,6 +1028,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
     geometry.rootJointIndex = modelIDs.indexOf(jointRootID);
     geometry.leanJointIndex = modelIDs.indexOf(jointLeanID);
     geometry.headJointIndex = modelIDs.indexOf(jointHeadID);
+    geometry.rightHandJointIndex = modelIDs.indexOf(jointRightHandID);
     
     // extract the translation component of the neck transform
     if (geometry.neckJointIndex != -1) {

interface/src/renderer/FBXReader.h

@@ -43,6 +43,8 @@ public:
 class FBXJoint {
 public:
 
+    bool isFree;
+    QVector<int> freeLineage;
     int parentIndex;
     glm::mat4 preTransform;
     glm::quat preRotation;
@@ -128,6 +130,7 @@ public:
     int rootJointIndex;
     int leanJointIndex;
     int headJointIndex;
+    int rightHandJointIndex;
     
     glm::vec3 neckPivot;
     

interface/src/renderer/Model.cpp

@@ -413,6 +413,10 @@ bool Model::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePos
         getJointPosition(geometry.rightEyeJointIndex, secondEyePosition);
 }
 
+bool Model::setRightHandPosition(const glm::vec3& position) {
+    return isActive() && setJointPosition(_geometry->getFBXGeometry().rightHandJointIndex, position);
+}
+
 void Model::setURL(const QUrl& url) {
     // don't recreate the geometry if it's the same URL
     if (_url == url) {
@@ -492,6 +496,45 @@ bool Model::getJointRotation(int jointIndex, glm::quat& rotation) const {
     return true;
 }
 
+bool Model::setJointPosition(int jointIndex, const glm::vec3& position) {
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return false;
+    }
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
+    
+    // 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;
+    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
+        glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].transform);
+        for (int j = 1; j < freeLineage.size(); j++) {
+            int index = freeLineage.at(j);
+            if (glm::distance(endPosition, position) < EPSILON) {
+                return true; // close enough to target position
+            }
+            const FBXJoint& joint = geometry.joints.at(index);
+            if (!joint.isFree) {
+                continue;
+            }
+            JointState& state = _jointStates[index];
+            glm::vec3 jointPosition = extractTranslation(state.transform);
+            glm::vec3 jointVector = endPosition - jointPosition;
+            glm::quat deltaRotation = rotationBetween(jointVector, position - jointPosition);
+            state.rotation = state.rotation * glm::inverse(state.combinedRotation) * deltaRotation * state.combinedRotation;
+            endPosition = deltaRotation * jointVector + jointPosition;
+        }
+        
+        // then, from the first free joint downwards, update the transforms again
+        for (int j = freeLineage.size() - 1; j >= 0; j--) {
+            updateJointState(freeLineage.at(j));
+        }
+    }
+    
+    return true;
+}
+
 void Model::deleteGeometry() {
     foreach (Model* attachment, _attachments) {
         delete attachment;

interface/src/renderer/Model.h

@@ -70,6 +70,10 @@ public:
     /// \return whether or not both eye meshes were found
     bool getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const;
     
+    /// Sets the position of the right hand using inverse kinematics.
+    /// \return whether or not the right hand joint was found
+    bool setRightHandPosition(const glm::vec3& position);
+    
     /// Returns the average color of all meshes in the geometry.
     glm::vec4 computeAverageColor() const;
 
@@ -101,6 +105,8 @@ protected:
     bool getJointPosition(int jointIndex, glm::vec3& position) const;
     bool getJointRotation(int jointIndex, glm::quat& rotation) const;
     
+    bool setJointPosition(int jointIndex, const glm::vec3& position);
+    
 private:
     
     void deleteGeometry();