From cc5967e8f99d01e5cc13fcfd63af82ad1655c775 Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Wed, 8 Feb 2017 12:38:34 -0800
Subject: [PATCH] Pull part of the Out Of Body Exp code

---
 .../animation/src/AnimInverseKinematics.cpp   | 52 +++++++++++++++----
 .../animation/src/AnimInverseKinematics.h     |  3 ++
 2 files changed, 45 insertions(+), 10 deletions(-)

diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index adeede17ad..3dc80fc764 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -486,7 +486,13 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                 // measure new _hipsOffset for next frame
                 // by looking for discrepancies between where a targeted endEffector is
                 // and where it wants to be (after IK solutions are done)
-                glm::vec3 newHipsOffset = Vectors::ZERO;
+
+                // OUTOFBODY_HACK:use weighted average between HMD and other targets
+                float HMD_WEIGHT = 10.0f;
+                float OTHER_WEIGHT = 1.0f;
+                float totalWeight = 0.0f;
+
+                glm::vec3 additionalHipsOffset = Vectors::ZERO;
                 for (auto& target: targets) {
                     int targetIndex = target.getIndex();
                     if (targetIndex == _headIndex && _headIndex != -1) {
@@ -497,32 +503,58 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                             glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans();
                             glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans();
                             const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
-                            newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under);
+                            additionalHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (under- actual);
+                            totalWeight += OTHER_WEIGHT;
                         } else if (target.getType() == IKTarget::Type::HmdHead) {
-                            // 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 thisOffset = target.getTranslation() - actual;
+                            glm::vec3 futureHipsOffset = _hipsOffset + thisOffset;
+                            if (glm::length(glm::vec2(futureHipsOffset.x, futureHipsOffset.z)) < _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
+                                additionalHipsOffset = futureHipsOffset - _hipsOffset;
+                                totalWeight = 0.0f;
+                                break;
+                            } else {
+                                additionalHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual);
+                                totalWeight += HMD_WEIGHT;
+                            }
                         }
                     } else if (target.getType() == IKTarget::Type::RotationAndPosition) {
                         glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans();
                         glm::vec3 targetPosition = target.getTranslation();
-                        newHipsOffset += targetPosition - actualPosition;
+                        additionalHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition);
+                        totalWeight += OTHER_WEIGHT;
                     }
                 }
+                if (totalWeight > 1.0f) {
+                    additionalHipsOffset /= 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;
-                _hipsOffset += (newHipsOffset - _hipsOffset) * tau;
+                _hipsOffset += additionalHipsOffset * tau;
+
+                // clamp the horizontal component of the hips offset
+                float hipsOffsetLength2D = glm::length(glm::vec2(_hipsOffset.x, _hipsOffset.z));
+                if (hipsOffsetLength2D > _maxHipsOffsetLength) {
+                    _hipsOffset.x *= _maxHipsOffsetLength / hipsOffsetLength2D;
+                    _hipsOffset.z *= _maxHipsOffsetLength / hipsOffsetLength2D;
+                }
+
             }
         }
     }
     return _relativePoses;
 }
 
+void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) {
+    // OUTOFBODY_HACK: manually adjust scale here
+    const float METERS_TO_CENTIMETERS = 100.0f;
+    _maxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength;
+}
+
 void AnimInverseKinematics::clearIKJointLimitHistory() {
     for (auto& pair : _constraints) {
         pair.second->clearHistory();
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index c9560c7383..7e4a7e5473 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -39,6 +39,8 @@ public:
 
     void clearIKJointLimitHistory();
 
+    void setMaxHipsOffsetLength(float maxLength);
+
 protected:
     void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
     void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
@@ -83,6 +85,7 @@ protected:
 
     // experimental data for moving hips during IK
     glm::vec3 _hipsOffset { Vectors::ZERO };
+    float _maxHipsOffsetLength { 1.0f };
     int _headIndex { -1 };
     int _hipsIndex { -1 };
     int _hipsParentIndex { -1 };