From 314434e0689098d567f76e2cb2c92b586e9c5dc3 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Thu, 30 Jul 2015 12:27:28 -0700
Subject: [PATCH] Re-added neck and eye procedural animation to FaceModel.

This is only used in the case of split head/body avatars.
---
 interface/src/avatar/FaceModel.cpp | 61 +++++++++++++++++++++++++++++-
 interface/src/avatar/FaceModel.h   |  4 ++
 2 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp
index d5d4da8665..f909489dc9 100644
--- a/interface/src/avatar/FaceModel.cpp
+++ b/interface/src/avatar/FaceModel.cpp
@@ -25,6 +25,7 @@ FaceModel::FaceModel(Head* owningHead, RigPointer rig) :
 
 void FaceModel::simulate(float deltaTime, bool fullUpdate) {
     updateGeometry();
+
     Avatar* owningAvatar = static_cast<Avatar*>(_owningHead->_owningAvatar);
     glm::vec3 neckPosition;
     if (!owningAvatar->getSkeletonModel().getNeckPosition(neckPosition)) {
@@ -37,19 +38,75 @@ void FaceModel::simulate(float deltaTime, bool fullUpdate) {
     }
     setRotation(neckParentRotation);
     setScale(glm::vec3(1.0f, 1.0f, 1.0f) * _owningHead->getScale());
-    
+
     setPupilDilation(_owningHead->getPupilDilation());
     setBlendshapeCoefficients(_owningHead->getBlendshapeCoefficients());
-    
+
     // FIXME - this is very expensive, we shouldn't do it if we don't have to
     //invalidCalculatedMeshBoxes();
 
     if (isActive()) {
         setOffset(-_geometry->getFBXGeometry().neckPivot);
+
+        for (int i = 0; i < _rig->getJointStateCount(); i++) {
+            updateJointState(i);
+        }
+
         Model::simulateInternal(deltaTime);
     }
 }
 
+void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, int index) {
+    // get the rotation axes in joint space and use them to adjust the rotation
+    glm::mat3 axes = glm::mat3_cast(glm::quat());
+    glm::mat3 inverse = glm::mat3(glm::inverse(parentState.getTransform() *
+                                               glm::translate(_rig->getJointDefaultTranslationInConstrainedFrame(index)) *
+                                               joint.preTransform * glm::mat4_cast(joint.preRotation)));
+    glm::vec3 pitchYawRoll = safeEulerAngles(_owningHead->getFinalOrientationInLocalFrame());
+    glm::vec3 lean = glm::radians(glm::vec3(_owningHead->getFinalLeanForward(),
+                                            _owningHead->getTorsoTwist(),
+                                            _owningHead->getFinalLeanSideways()));
+    pitchYawRoll -= lean;
+    _rig->setJointRotationInConstrainedFrame(index,
+                                             glm::angleAxis(-pitchYawRoll.z, glm::normalize(inverse * axes[2]))
+                                             * glm::angleAxis(pitchYawRoll.y, glm::normalize(inverse * axes[1]))
+                                             * glm::angleAxis(-pitchYawRoll.x, glm::normalize(inverse * axes[0]))
+                                             * joint.rotation, DEFAULT_PRIORITY);
+}
+
+void FaceModel::maybeUpdateEyeRotation(Model* model, const JointState& parentState, const FBXJoint& joint, int index) {
+    // likewise with the eye joints
+    // NOTE: at the moment we do the math in the world-frame, hence the inverse transform is more complex than usual.
+    glm::mat4 inverse = glm::inverse(glm::mat4_cast(model->getRotation()) * parentState.getTransform() *
+                                     glm::translate(_rig->getJointDefaultTranslationInConstrainedFrame(index)) *
+                                     joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation));
+    glm::vec3 front = glm::vec3(inverse * glm::vec4(_owningHead->getFinalOrientationInWorldFrame() * IDENTITY_FRONT, 0.0f));
+    glm::vec3 lookAtDelta = _owningHead->getCorrectedLookAtPosition() - model->getTranslation();
+    glm::vec3 lookAt = glm::vec3(inverse * glm::vec4(lookAtDelta + glm::length(lookAtDelta) * _owningHead->getSaccade(), 1.0f));
+    glm::quat between = rotationBetween(front, lookAt);
+    const float MAX_ANGLE = 30.0f * RADIANS_PER_DEGREE;
+    _rig->setJointRotationInConstrainedFrame(index, glm::angleAxis(glm::clamp(glm::angle(between),
+                                                                              -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
+                                             joint.rotation, DEFAULT_PRIORITY);
+}
+
+void FaceModel::updateJointState(int index) {
+    const JointState& state = _rig->getJointState(index);
+    const FBXJoint& joint = state.getFBXJoint();
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+
+    // guard against out-of-bounds access to _jointStates
+    if (joint.parentIndex != -1 && joint.parentIndex >= 0 && joint.parentIndex < _rig->getJointStateCount()) {
+        const JointState& parentState = _rig->getJointState(joint.parentIndex);
+        if (index == geometry.neckJointIndex) {
+            maybeUpdateNeckRotation(parentState, joint, index);
+
+        } else if (index == geometry.leftEyeJointIndex || index == geometry.rightEyeJointIndex) {
+            maybeUpdateEyeRotation(this, parentState, joint, index);
+        }
+    }
+}
+
 bool FaceModel::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const {
     if (!isActive()) {
         return false;
diff --git a/interface/src/avatar/FaceModel.h b/interface/src/avatar/FaceModel.h
index 5a19a8ea29..40f502f2d3 100644
--- a/interface/src/avatar/FaceModel.h
+++ b/interface/src/avatar/FaceModel.h
@@ -26,6 +26,10 @@ public:
 
     virtual void simulate(float deltaTime, bool fullUpdate = true);
 
+    void maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, int index);
+    void maybeUpdateEyeRotation(Model* model, const JointState& parentState, const FBXJoint& joint, int index);
+    void updateJointState(int index);
+
     /// Retrieve the positions of up to two eye meshes.
     /// \return whether or not both eye meshes were found
     bool getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const;