From 570ec8457d0f1e24dce30ac1e20e2984b6ad9406 Mon Sep 17 00:00:00 2001
From: David Rowe <david@ctrlaltstudio.com>
Date: Wed, 14 Jun 2017 10:45:30 +1200
Subject: [PATCH] Refactor

---
 libraries/animation/src/Rig.cpp | 66 +++++++++++++++++++--------------
 libraries/animation/src/Rig.h   |  3 ++
 2 files changed, 41 insertions(+), 28 deletions(-)

diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 0362c42b39..fbb3d24298 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1165,6 +1165,10 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
 
         const float RELAX_DURATION = 0.6f;
         const float CONTROL_DURATION = 0.4f;
+        const bool TO_CONTROLLED = true;
+        const bool FROM_CONTROLLED = false;
+        const bool LEFT_HAND = true;
+        const bool RIGHT_HAND = false;
 
         if (params.isLeftEnabled) {
             if (!_isLeftHandControlled) {
@@ -1178,13 +1182,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
             if (_leftHandControlTimeRemaining > 0.0f) {
                 // Move hand from non-controlled position to controlled position.
                 _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f);
-                auto ikNode = getAnimInverseKinematicsNode();
-                if (ikNode) {
-                    AnimPose handPose(Vectors::ONE, handRotation, handPosition);
-                    float alpha = 1.0f - _leftHandControlTimeRemaining / CONTROL_DURATION;
-                    const AnimPose geometryToRigTransform(_geometryToRigTransform);
-                    AnimPose uncontrolledHandPose = geometryToRigTransform * ikNode->getUncontrolledLeftHandPose();
-                    ::blend(1, &uncontrolledHandPose, &handPose, alpha, &handPose);
+                AnimPose handPose(Vectors::ONE, handRotation, handPosition);
+                if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, LEFT_HAND, TO_CONTROLLED, 
+                        handPose)) {
                     handPosition = handPose.trans();
                     handRotation = handPose.rot();
                 }
@@ -1212,13 +1212,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
             if (_leftHandRelaxTimeRemaining > 0.0f) {
                 // Move hand from controlled position to non-controlled position.
                 _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f);
-                auto ikNode = getAnimInverseKinematicsNode();
-                if (ikNode) {
-                    float alpha = 1.0f - _leftHandRelaxTimeRemaining / RELAX_DURATION;
-                    const AnimPose geometryToRigTransform(_geometryToRigTransform);
-                    AnimPose uncontrolledHandPose = geometryToRigTransform * ikNode->getUncontrolledLeftHandPose();
-                    AnimPose handPose;
-                    ::blend(1, &_lastLeftHandControlledPose, &uncontrolledHandPose, alpha, &handPose);
+                AnimPose handPose;
+                if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, LEFT_HAND, 
+                        FROM_CONTROLLED, handPose)) {
                     _animVars.set("leftHandPosition", handPose.trans());
                     _animVars.set("leftHandRotation", handPose.rot());
                     _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
@@ -1242,13 +1238,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
             if (_rightHandControlTimeRemaining > 0.0f) {
                 // Move hand from non-controlled position to controlled position.
                 _rightHandControlTimeRemaining = std::max(_rightHandControlTimeRemaining - dt, 0.0f);
-                auto ikNode = getAnimInverseKinematicsNode();
-                if (ikNode) {
-                    AnimPose handPose(Vectors::ONE, handRotation, handPosition);
-                    float alpha = 1.0f - _rightHandControlTimeRemaining / CONTROL_DURATION;
-                    const AnimPose geometryToRigTransform(_geometryToRigTransform);
-                    AnimPose uncontrolledHandPose = geometryToRigTransform * ikNode->getUncontrolledRightHandPose();
-                    ::blend(1, &uncontrolledHandPose, &handPose, alpha, &handPose);
+                AnimPose handPose(Vectors::ONE, handRotation, handPosition);
+                if (transitionHandPose(_rightHandControlTimeRemaining, CONTROL_DURATION, handPose, RIGHT_HAND, TO_CONTROLLED, 
+                        handPose)) {
                     handPosition = handPose.trans();
                     handRotation = handPose.rot();
                 }
@@ -1276,13 +1268,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
             if (_rightHandRelaxTimeRemaining > 0.0f) {
                 // Move hand from controlled position to non-controlled position.
                 _rightHandRelaxTimeRemaining = std::max(_rightHandRelaxTimeRemaining - dt, 0.0f);
-                auto ikNode = getAnimInverseKinematicsNode();
-                if (ikNode) {
-                    float alpha = 1.0f - _rightHandRelaxTimeRemaining / RELAX_DURATION;
-                    const AnimPose geometryToRigTransform(_geometryToRigTransform);
-                    AnimPose uncontrolledHandPose = geometryToRigTransform * ikNode->getUncontrolledRightHandPose();
-                    AnimPose handPose;
-                    ::blend(1, &_lastRightHandControlledPose, &uncontrolledHandPose, alpha, &handPose);
+                AnimPose handPose;
+                if (transitionHandPose(_rightHandRelaxTimeRemaining, RELAX_DURATION, _lastRightHandControlledPose, RIGHT_HAND, 
+                        FROM_CONTROLLED, handPose)) {
                     _animVars.set("rightHandPosition", handPose.trans());
                     _animVars.set("rightHandRotation", handPose.rot());
                     _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
@@ -1588,3 +1576,25 @@ void Rig::computeAvatarBoundingCapsule(
     glm::vec3 rigCenter = (geometryToRig * (0.5f * (totalExtents.maximum + totalExtents.minimum)));
     localOffsetOut = rigCenter - (geometryToRig * rootPosition);
 }
+
+bool Rig::transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand,
+                             bool isToControlled, AnimPose& returnHandPose) {
+    auto ikNode = getAnimInverseKinematicsNode();
+    if (ikNode) {
+        float alpha = 1.0f - deltaTime / totalDuration;
+        const AnimPose geometryToRigTransform(_geometryToRigTransform);
+        AnimPose uncontrolledHandPose;
+        if (isLeftHand) {
+            uncontrolledHandPose = geometryToRigTransform * ikNode->getUncontrolledLeftHandPose();
+        } else {
+            uncontrolledHandPose = geometryToRigTransform * ikNode->getUncontrolledRightHandPose();
+        }
+        if (isToControlled) {
+            ::blend(1, &uncontrolledHandPose, &controlledHandPose, alpha, &returnHandPose);
+        } else {
+            ::blend(1, &controlledHandPose, &uncontrolledHandPose, alpha, &returnHandPose);
+        }
+        return true;
+    }
+    return false;
+}
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index 1cb1dac4f3..b5b69fc018 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -352,6 +352,9 @@ private:
     int _nextStateHandlerId { 0 };
     QMutex _stateMutex;
 
+    bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand, 
+        bool isToControlled, AnimPose& returnHandPose);
+
     bool _isLeftHandControlled { false };
     bool _isRightHandControlled { false };
     float _leftHandControlTimeRemaining { 0.0f };