From 957d39b0b13e9647f624c6bdd5147445eb1441df Mon Sep 17 00:00:00 2001
From: David Rowe <david@ctrlaltstudio.com>
Date: Sun, 11 Mar 2018 14:49:23 +1300
Subject: [PATCH] Fix rotation and position of meshes attached to eye joints

---
 libraries/animation/src/AnimSkeleton.cpp | 11 +++++++++++
 libraries/animation/src/AnimSkeleton.h   |  1 +
 libraries/animation/src/Rig.cpp          | 21 +++++++++++++++++++++
 libraries/animation/src/Rig.h            |  5 +++++
 4 files changed, 38 insertions(+)

diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp
index 2c37d2d089..3749ad905e 100644
--- a/libraries/animation/src/AnimSkeleton.cpp
+++ b/libraries/animation/src/AnimSkeleton.cpp
@@ -78,6 +78,17 @@ int AnimSkeleton::getParentIndex(int jointIndex) const {
     return _joints[jointIndex].parentIndex;
 }
 
+QVector<int> AnimSkeleton::getChildrenOfJoint(int jointIndex) const {
+    // Children and grandchildren, etc.
+    QVector<int> result;
+    for (int i = jointIndex + 1; i < _joints.size(); i++) {
+        if (_joints[i].parentIndex == jointIndex || result.contains(_joints[i].parentIndex)) {
+            result.push_back(i);
+        }
+    }
+    return result;
+}
+
 const QString& AnimSkeleton::getJointName(int jointIndex) const {
     return _joints[jointIndex].name;
 }
diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h
index 664358f414..52f3ac7182 100644
--- a/libraries/animation/src/AnimSkeleton.h
+++ b/libraries/animation/src/AnimSkeleton.h
@@ -43,6 +43,7 @@ public:
     const AnimPose& getPostRotationPose(int jointIndex) const;
 
     int getParentIndex(int jointIndex) const;
+    QVector<int> getChildrenOfJoint(int jointIndex) const;
 
     AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& relativePoses) const;
 
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index d6791ab0b8..f2f5d48713 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -199,6 +199,8 @@ void Rig::destroyAnimGraph() {
     _internalPoseSet._overridePoses.clear();
     _internalPoseSet._overrideFlags.clear();
     _numOverrides = 0;
+    _leftEyeJointChildren.clear();
+    _rightEyeJointChildren.clear();
 }
 
 void Rig::initJointStates(const FBXGeometry& geometry, const glm::mat4& modelOffset) {
@@ -225,12 +227,17 @@ void Rig::initJointStates(const FBXGeometry& geometry, const glm::mat4& modelOff
     buildAbsoluteRigPoses(_animSkeleton->getRelativeDefaultPoses(), _absoluteDefaultPoses);
 
     _rootJointIndex = geometry.rootJointIndex;
+    _leftEyeJointIndex = geometry.leftEyeJointIndex;
+    _rightEyeJointIndex = geometry.rightEyeJointIndex;
     _leftHandJointIndex = geometry.leftHandJointIndex;
     _leftElbowJointIndex = _leftHandJointIndex >= 0 ? geometry.joints.at(_leftHandJointIndex).parentIndex : -1;
     _leftShoulderJointIndex = _leftElbowJointIndex >= 0 ? geometry.joints.at(_leftElbowJointIndex).parentIndex : -1;
     _rightHandJointIndex = geometry.rightHandJointIndex;
     _rightElbowJointIndex = _rightHandJointIndex >= 0 ? geometry.joints.at(_rightHandJointIndex).parentIndex : -1;
     _rightShoulderJointIndex = _rightElbowJointIndex >= 0 ? geometry.joints.at(_rightElbowJointIndex).parentIndex : -1;
+
+    _leftEyeJointChildren = _animSkeleton->getChildrenOfJoint(geometry.leftEyeJointIndex);
+    _rightEyeJointChildren = _animSkeleton->getChildrenOfJoint(geometry.rightEyeJointIndex);
 }
 
 void Rig::reset(const FBXGeometry& geometry) {
@@ -253,6 +260,8 @@ void Rig::reset(const FBXGeometry& geometry) {
     buildAbsoluteRigPoses(_animSkeleton->getRelativeDefaultPoses(), _absoluteDefaultPoses);
 
     _rootJointIndex = geometry.rootJointIndex;
+    _leftEyeJointIndex = geometry.leftEyeJointIndex;
+    _rightEyeJointIndex = geometry.rightEyeJointIndex;
     _leftHandJointIndex = geometry.leftHandJointIndex;
     _leftElbowJointIndex = _leftHandJointIndex >= 0 ? geometry.joints.at(_leftHandJointIndex).parentIndex : -1;
     _leftShoulderJointIndex = _leftElbowJointIndex >= 0 ? geometry.joints.at(_leftElbowJointIndex).parentIndex : -1;
@@ -260,6 +269,9 @@ void Rig::reset(const FBXGeometry& geometry) {
     _rightElbowJointIndex = _rightHandJointIndex >= 0 ? geometry.joints.at(_rightHandJointIndex).parentIndex : -1;
     _rightShoulderJointIndex = _rightElbowJointIndex >= 0 ? geometry.joints.at(_rightElbowJointIndex).parentIndex : -1;
 
+    _leftEyeJointChildren = _animSkeleton->getChildrenOfJoint(geometry.leftEyeJointIndex);
+    _rightEyeJointChildren = _animSkeleton->getChildrenOfJoint(geometry.rightEyeJointIndex);
+
     if (!_animGraphURL.isEmpty()) {
         _animNode.reset();
         initAnimGraph(_animGraphURL);
@@ -1430,6 +1442,15 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
 
         // directly set absolutePose rotation
         _internalPoseSet._absolutePoses[index].rot() = deltaQuat * headQuat;
+
+        // Update eye joint's children.
+        auto children = index == _leftEyeJointIndex ? _leftEyeJointChildren : _rightEyeJointChildren;
+        for (int i = 0; i < children.size(); i++) {
+            int jointIndex = children[i];
+            int parentIndex = _animSkeleton->getParentIndex(jointIndex);
+            _internalPoseSet._absolutePoses[jointIndex] = 
+                _internalPoseSet._absolutePoses[parentIndex] * _internalPoseSet._relativePoses[jointIndex];
+        }
     }
 }
 
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index 7230d05e2a..72f9f64482 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -267,6 +267,11 @@ protected:
 
     int _rootJointIndex { -1 };
 
+    int _leftEyeJointIndex { -1 };
+    int _rightEyeJointIndex { -1 };
+    QVector<int> _leftEyeJointChildren;
+    QVector<int> _rightEyeJointChildren;
+
     int _leftHandJointIndex { -1 };
     int _leftElbowJointIndex { -1 };
     int _leftShoulderJointIndex { -1 };