From 25610c75876ba7db1de50f26855582d05a4a9ac2 Mon Sep 17 00:00:00 2001
From: Angus Antley <amantley@googlemail.com>
Date: Fri, 24 Aug 2018 11:04:36 +0100
Subject: [PATCH] got the hands turning the shoulders,  needs error checking

---
 interface/src/avatar/MyAvatar.cpp        | 15 +++++++++++++++
 interface/src/avatar/MyAvatar.h          |  2 ++
 interface/src/avatar/MySkeletonModel.cpp |  5 ++---
 3 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 2771f567b3..97179f5d22 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -3349,6 +3349,21 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     return createMatFromQuatAndPos(headOrientationYawOnly, bodyPos);
 }
 
+glm::mat4 MyAvatar::getSpine2RotationRigSpace() const {
+
+    static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180;
+    glm::vec3 hipToHandRigSpace = RIG_CHANGE_OF_BASIS * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y);
+
+    //to do: check for zero before normalizing.
+    glm::vec3 u, v, w;
+    generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w);
+    glm::mat4 spine2RigSpace(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f));
+
+    //glm::quat spine2RigSpace = glm::quat_cast(glm::lookAt(glm::vec3(0.0f, 0.0f, 0.0f), glm::normalize(hipToHandRigSpace), glm::vec3(0.0f, 1.0f, 0.0f)));
+
+    return spine2RigSpace;
+}
+
 // ease in function for dampening cg movement
 static float slope(float num) {
     const float CURVE_CONSTANT = 1.0f;
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 6b7c4e83a1..3c951264e8 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -1050,6 +1050,8 @@ public:
     // results are in sensor frame (-z forward)
     glm::mat4 deriveBodyFromHMDSensor() const;
 
+    glm::mat4 getSpine2RotationRigSpace() const;
+
     glm::vec3 computeCounterBalance();
 
     // derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous
diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp
index 491fab466d..5272b1499d 100644
--- a/interface/src/avatar/MySkeletonModel.cpp
+++ b/interface/src/avatar/MySkeletonModel.cpp
@@ -240,9 +240,8 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
 
         // spine 2 hack to be improved.
-        AnimPose newSpinePose;
-        bool fred = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), newSpinePose);
-        newSpinePose.rot() = glm::quat(0.7071f, 0.0f, 0.0f, 0.7071f)*newSpinePose.rot();
+        AnimPose newSpinePose(myAvatar->getSpine2RotationRigSpace());
+        //newSpinePose.rot() = myAvatar->getSpine2RotationRigSpace();// *newSpinePose.rot();
         params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = newSpinePose;
         params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated;