From 61a05eb4d103e7d318f60e9b3f36d892b7d01198 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 16 Sep 2016 10:16:18 -0700
Subject: [PATCH] failed experiment for getting hip lean over table

---
 .../animation/src/AnimInverseKinematics.cpp   | 41 ++++++++++++-------
 .../animation/src/AnimInverseKinematics.h     |  1 +
 2 files changed, 28 insertions(+), 14 deletions(-)

diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 68fc0b45dc..059f192f3c 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -244,6 +244,11 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
     // the tip's parent-relative as we proceed up the chain
     glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot;
 
+    /* OUTOFBODY_HACK -- experimental override target type when HmdHead pushes outside hipsOffset limit
+    if (targetType == IKTarget::Type::HmdHead && _hipsAreOver) {
+        targetType = IKTarget::Type::RotationAndPosition;
+    }
+    */
     if (targetType == IKTarget::Type::HmdHead) {
         // rotate tip directly to target orientation
         tipOrientation = target.getRotation();
@@ -525,17 +530,19 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                             newHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (actual - under);
                             totalWeight += OTHER_WEIGHT;
                         } else if (target.getType() == IKTarget::Type::HmdHead) {
-                            /* OUTOFBODY_HACK: keep this old code to remind us of what changed
-                            // we want to shift the hips to bring the head to its designated position
                             glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
-                            _hipsOffset += target.getTranslation() - actual;
-                            // and ignore all other targets
-                            newHipsOffset = _hipsOffset;
-                            break;
-                            */
-                            glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
-                            newHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual);
-                            totalWeight += HMD_WEIGHT;
+                            glm::vec3 thisOffset = target.getTranslation() - actual;
+                            glm::vec3 futureHipsOffset = _hipsOffset + thisOffset;
+                            if (glm::length(futureHipsOffset) < _maxHipsOffsetLength) {
+                                // it is imperative to shift the hips and bring the head to its designated position
+                                // so we slam newHipsOffset here and ignore all other targets
+                                newHipsOffset = futureHipsOffset;
+                                totalWeight = 0.0f;
+                                break;
+                            } else {
+                                newHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual);
+                                totalWeight += HMD_WEIGHT;
+                            }
                         }
                     } else if (target.getType() == IKTarget::Type::RotationAndPosition) {
                         glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans;
@@ -544,13 +551,12 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                         totalWeight += OTHER_WEIGHT;
                     }
                 }
-                if (totalWeight == 0.0f) {
-                    totalWeight = 1.0f;
+                if (totalWeight > 1.0f) {
+                    newHipsOffset /= totalWeight;
                 }
-                newHipsOffset /= totalWeight;
 
                 // smooth transitions by relaxing _hipsOffset toward the new value
-                const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f;
+                const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f;
                 float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ?  dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
                 float newOffsetLength = glm::length(newHipsOffset);
                 if (newOffsetLength > _maxHipsOffsetLength) {
@@ -558,6 +564,13 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                     newHipsOffset *= _maxHipsOffsetLength / newOffsetLength;
                 }
                 _hipsOffset += (newHipsOffset - _hipsOffset) * tau;
+                /* OUTOFBODY_HACK: experimental code for disabling HmdHead IK behavior when hips over limit
+                if (_hipsAreOver) {
+                    _hipsAreOver = glm::length(newHipsOffset) - _maxHipsOffsetLength > -1.0f;
+                } else {
+                    _hipsAreOver = glm::length(newHipsOffset) - _maxHipsOffsetLength > 1.0f;
+                }
+                */
             }
         }
     }
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index 7e4a7e5473..5b83b9734e 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -86,6 +86,7 @@ protected:
     // experimental data for moving hips during IK
     glm::vec3 _hipsOffset { Vectors::ZERO };
     float _maxHipsOffsetLength { 1.0f };
+    bool _hipsAreOver { false }; // OUTOFBODY_HACK: experimental
     int _headIndex { -1 };
     int _hipsIndex { -1 };
     int _hipsParentIndex { -1 };