From c4a7f4843a03d0ac65d9321879c5afe35d56c3dd Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Tue, 12 Dec 2017 17:02:07 -0800
Subject: [PATCH] Much more stable hips IK target.

Target is now estimated in sensor space from the head, which is very deterministic and not prone to feedback from the IK system.
Previous the hip was estimated from accumulated IK error deltas, which was not stable, as the error would tend to accumulate if the IK targets could not be reached.
---
 interface/src/avatar/MySkeletonModel.cpp | 69 ++++++++++++++++++++++++
 interface/src/avatar/MySkeletonModel.h   |  3 ++
 2 files changed, 72 insertions(+)

diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp
index a707031167..f249be33ea 100644
--- a/interface/src/avatar/MySkeletonModel.cpp
+++ b/interface/src/avatar/MySkeletonModel.cpp
@@ -9,9 +9,12 @@
 #include "MySkeletonModel.h"
 
 #include <avatars-renderer/Avatar.h>
+#include <DebugDraw.h>
 
 #include "Application.h"
 #include "InterfaceLogging.h"
+#include "AnimUtil.h"
+
 
 MySkeletonModel::MySkeletonModel(Avatar* owningAvatar, QObject* parent) : SkeletonModel(owningAvatar, parent) {
 }
@@ -30,6 +33,39 @@ Rig::CharacterControllerState convertCharacterControllerState(CharacterControlle
     };
 }
 
+static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) {
+    glm::mat4 hipsMat = myAvatar->deriveBodyFromHMDSensor();
+    glm::vec3 hipsPos = extractTranslation(hipsMat);
+    glm::quat hipsRot = glmExtractRotation(hipsMat);
+
+    glm::mat4 avatarToWorldMat = myAvatar->getTransform().getMatrix();
+    glm::mat4 worldToSensorMat = glm::inverse(myAvatar->getSensorToWorldMatrix());
+    glm::mat4 avatarToSensorMat = worldToSensorMat * avatarToWorldMat;
+
+    // dampen hips rotation, by mixing it with the avatar orientation in sensor space
+    const float MIX_RATIO = 0.5f;
+    hipsRot = safeLerp(glmExtractRotation(avatarToSensorMat), hipsRot, MIX_RATIO);
+
+    if (isFlying) {
+        // rotate the hips back to match the flying animation.
+
+        const float TILT_ANGLE = 0.523f;
+        const glm::quat tiltRot = glm::angleAxis(TILT_ANGLE, transformVectorFast(avatarToSensorMat, -Vectors::UNIT_X));
+
+        glm::vec3 headPos;
+        int headIndex = myAvatar->getJointIndex("Head");
+        if (headIndex != -1) {
+            headPos = transformPoint(avatarToSensorMat, myAvatar->getAbsoluteJointTranslationInObjectFrame(headIndex));
+        } else {
+            headPos = transformPoint(myAvatar->getSensorToWorldMatrix(), myAvatar->getHMDSensorPosition());
+        }
+        hipsRot = tiltRot * hipsRot;
+        hipsPos = headPos + tiltRot * (hipsPos - headPos);
+    }
+
+    return AnimPose(hipsRot * Quaternions::Y_180, hipsPos);
+}
+
 // Called within Model::simulate call, below.
 void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
     const FBXGeometry& geometry = getFBXGeometry();
@@ -124,6 +160,39 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         }
     }
 
+    // if hips are not under direct control, estimate the hips position.
+    if (avatarHeadPose.isValid() && !params.primaryControllerActiveFlags[Rig::PrimaryControllerType_Hips]) {
+        bool isFlying = (myAvatar->getCharacterController()->getState() == CharacterController::State::Hover || myAvatar->getCharacterController()->computeCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS);
+
+        if (!_prevHipsValid) {
+            AnimPose hips = computeHipsInSensorFrame(myAvatar, isFlying);
+            _prevHips = hips;
+        }
+
+        AnimPose hips = computeHipsInSensorFrame(myAvatar, isFlying);
+
+        // smootly lerp hips, in sensorframe, with different coeff for horiz and vertical translation.
+        const float ROT_ALPHA = 0.9f;
+        const float TRANS_HORIZ_ALPHA = 0.9f;
+        const float TRANS_VERT_ALPHA = 0.1f;
+        float hipsY = hips.trans().y;
+        hips.trans() = lerp(hips.trans(), _prevHips.trans(), TRANS_HORIZ_ALPHA);
+        hips.trans().y = lerp(hipsY, _prevHips.trans().y, TRANS_VERT_ALPHA);
+        hips.rot() = safeLerp(hips.rot(), _prevHips.rot(), ROT_ALPHA);
+
+        _prevHips = hips;
+        _prevHipsValid = true;
+
+        glm::mat4 invRigMat = glm::inverse(myAvatar->getTransform().getMatrix() * Matrices::Y_180);
+        AnimPose sensorToRigPose(invRigMat * myAvatar->getSensorToWorldMatrix());
+
+        params.primaryControllerPoses[Rig::PrimaryControllerType_Hips] = sensorToRigPose * hips;
+        params.primaryControllerActiveFlags[Rig::PrimaryControllerType_Hips] = true;
+
+    } else {
+        _prevHipsValid = false;
+    }
+
     params.isTalking = head->getTimeWithoutTalking() <= 1.5f;
 
     // pass detailed torso k-dops to rig.
diff --git a/interface/src/avatar/MySkeletonModel.h b/interface/src/avatar/MySkeletonModel.h
index ad0ae1b8e9..d9f57a439a 100644
--- a/interface/src/avatar/MySkeletonModel.h
+++ b/interface/src/avatar/MySkeletonModel.h
@@ -25,6 +25,9 @@ public:
 
 private:
     void updateFingers();
+
+    AnimPose _prevHips;  // sensor frame
+    bool _prevHipsValid { false };
 };
 
 #endif // hifi_MySkeletonModel_h