From e9316d30d0705b9d693ac54049e4cfccd3a4bc3d Mon Sep 17 00:00:00 2001
From: Atlante45 <clement.brisset@gmail.com>
Date: Fri, 24 Feb 2017 18:40:36 -0800
Subject: [PATCH] More IK smoothing experimentation

---
 interface/src/avatar/MyAvatar.cpp             |  2 +-
 .../animation/src/AnimInverseKinematics.cpp   | 20 +++++++++++--------
 scripts/tutorials/entity_scripts/sit.js       |  2 +-
 3 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 0b69544527..a91cd33827 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2496,7 +2496,7 @@ bool MyAvatar::pinJoint(int index, const glm::vec3& position, const glm::quat& o
     setPosition(targetPosition);
     setOrientation(targetOrientation);
 
-    _rig->setMaxHipsOffsetLength(0.0f);
+    _rig->setMaxHipsOffsetLength(0.05f);
 
     auto it = std::find(_pinnedJoints.begin(), _pinnedJoints.end(), index);
     if (it == _pinnedJoints.end()) {
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 1cf70bcc14..c22819f22d 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -262,14 +262,14 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
     glm::vec3 tipPosition = absolutePoses[tipIndex].trans();
 
     // descend toward root, pivoting each joint to get tip closer to target position
-    while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) {
+    while (pivotIndex != -1) {
         // compute the two lines that should be aligned
         glm::vec3 jointPosition = absolutePoses[pivotIndex].trans();
         glm::vec3 leverArm = tipPosition - jointPosition;
 
         glm::quat deltaRotation;
         if (targetType == IKTarget::Type::RotationAndPosition ||
-                targetType == IKTarget::Type::HipsRelativeRotationAndPosition) {
+            targetType == IKTarget::Type::HipsRelativeRotationAndPosition) {
             // compute the swing that would get get tip closer
             glm::vec3 targetLine = target.getTranslation() - jointPosition;
 
@@ -338,10 +338,15 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
             deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR));
         }
 
+        glm::quat parentRotation;
+        if (pivotsParentIndex != -1) {
+            parentRotation = absolutePoses[pivotsParentIndex].rot();
+        }
+
         // compute joint's new parent-relative rotation after swing
         // Q' = dQ * Q   and   Q = Qp * q   -->   q' = Qp^ * dQ * Q
         glm::quat newRot = glm::normalize(glm::inverse(
-                absolutePoses[pivotsParentIndex].rot()) *
+                parentRotation) *
                 deltaRotation *
                 absolutePoses[pivotIndex].rot());
 
@@ -353,7 +358,7 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
                 // the constraint will modify the local rotation of the tip so we must
                 // compute the corresponding model-frame deltaRotation
                 // Q' = Qp^ * dQ * Q  -->  dQ =   Qp * Q' * Q^
-                deltaRotation = absolutePoses[pivotsParentIndex].rot() * newRot * glm::inverse(absolutePoses[pivotIndex].rot());
+                deltaRotation = parentRotation * newRot * glm::inverse(absolutePoses[pivotIndex].rot());
             }
         }
 
@@ -538,10 +543,9 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                 _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;
+                float hipsOffsetLength = glm::length(_hipsOffset);
+                if (hipsOffsetLength > _maxHipsOffsetLength) {
+                    _hipsOffset *= _maxHipsOffsetLength / hipsOffsetLength;
                 }
 
             }
diff --git a/scripts/tutorials/entity_scripts/sit.js b/scripts/tutorials/entity_scripts/sit.js
index aab5d27800..174024f5d8 100644
--- a/scripts/tutorials/entity_scripts/sit.js
+++ b/scripts/tutorials/entity_scripts/sit.js
@@ -12,7 +12,7 @@
     var MAX_IK_ERROR = 20;
     var DESKTOP_UI_CHECK_INTERVAL = 250;
     var DESKTOP_MAX_DISTANCE = 5;
-    var SIT_DELAY = 20
+    var SIT_DELAY = 25
 
     this.entityID = null;
     this.timers = {};