From e8b6338de9026938a34ac35191a4abb1d906ecc6 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 9 Jun 2014 15:40:29 -0700
Subject: [PATCH] Fix IK for hydra hands.

---
 interface/src/avatar/SkeletonModel.cpp | 45 +++++++++-----------------
 interface/src/renderer/Model.cpp       |  4 +--
 2 files changed, 17 insertions(+), 32 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index ca40ba1d64..d5a13b0381 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -144,9 +144,9 @@ void SkeletonModel::getBodyShapes(QVector<const Shape*>& shapes) const {
 void SkeletonModel::renderIKConstraints() {
     renderJointConstraints(getRightHandJointIndex());
     renderJointConstraints(getLeftHandJointIndex());
-    if (isActive() && _owningAvatar->isMyAvatar()) {
-        renderRagDoll();
-    }
+    //if (isActive() && _owningAvatar->isMyAvatar()) {
+    //    renderRagDoll();
+    //}
 }
 
 class IndexValue {
@@ -193,45 +193,30 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
     if (parentJointIndex == -1) {
         return;
     }
-    
+  
     // rotate palm to align with its normal (normal points out of hand's palm)
-    glm::quat palmRotation;
-    glm::quat r0, r1;
-    if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) &&
-            Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
-        JointState parentState = _jointStates[parentJointIndex];
-        palmRotation = parentState.getRotationFromBindToModelFrame();
-        r0 = palmRotation;
-    } else {
-        JointState state = _jointStates[jointIndex];
-        palmRotation = state.getRotationFromBindToModelFrame();
-    }
     glm::quat inverseRotation = glm::inverse(_rotation);
-    glm::vec3 palmNormal = inverseRotation * palm.getNormal();
-    palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palmNormal) * palmRotation;
-    r1 = palmRotation;
-    
-    // rotate palm to align with finger direction
-    glm::vec3 direction = inverseRotation * palm.getFingerDirection();
-    palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
-
-    // set hand position, rotation
     glm::vec3 palmPosition = inverseRotation * (palm.getPosition() - _translation);
+    glm::vec3 palmNormal = inverseRotation * palm.getNormal();
+    glm::vec3 fingerDirection = inverseRotation * palm.getFingerDirection();
+
+    glm::quat palmRotation = rotationBetween(glm::vec3(0.0f, -1.0f, 0.0f), palmNormal);
+    palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), fingerDirection) * palmRotation;
+
     if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) {
         setHandPosition(jointIndex, palmPosition, palmRotation);  
-        
     } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
-        glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
-        setJointPosition(parentJointIndex, palmPosition + forearmVector *
-            geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale),
+        float forearmLength = geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale);
+        glm::vec3 forearm = palmRotation * glm::vec3(sign * forearmLength, 0.0f, 0.0f);
+        setJointPosition(parentJointIndex, palmPosition + forearm,
             glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
         JointState& parentState = _jointStates[parentJointIndex];
         parentState.setRotationFromBindFrame(palmRotation, PALM_PRIORITY);
         // lock hand to forearm by slamming its rotation (in parent-frame) to identity
         _jointStates[jointIndex]._rotationInParentFrame = glm::quat();
     } else {
-        setJointPosition(jointIndex, palmPosition, palmRotation,
-            true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
+        setJointPosition(jointIndex, palmPosition, palmRotation, 
+                true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
     }
 }
 
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 105301054b..aff023c2a0 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -1275,8 +1275,8 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, const gl
         if (useRotation) {
             JointState& state = _jointStates[jointIndex];
 
-            state.setRotation(rotation, true, priority);
-            endRotation = state.getRotation();
+            state.setRotationFromBindFrame(rotation, priority);
+            endRotation = state.getRotationFromBindToModelFrame();
         }    
         
         // then, we go from the joint upwards, rotating the end as close as possible to the target