From 1e69c12e87c1eb20d38cf5cdb80200e252bfb889 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Mon, 1 Feb 2016 18:47:26 -0800
Subject: [PATCH] move grab position to front of palm

---
 interface/src/avatar/SkeletonModel.cpp | 46 ++++++++++++++++++++++----
 1 file changed, 40 insertions(+), 6 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 7a871236c5..1163b4dff0 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -236,17 +236,51 @@ void SkeletonModel::applyPalmData(int jointIndex, const PalmData& palm) {
 }
 
 bool SkeletonModel::getLeftGrabPosition(glm::vec3& position) const {
-    int index = _rig->indexOfJoint("LeftHandMiddle1");
-    if (index >= 0) {
-        return getJointPositionInWorldFrame(index, position);
+    int knuckleIndex = _rig->indexOfJoint("LeftHandMiddle1");
+    int handIndex = _rig->indexOfJoint("LeftHand");
+    if (knuckleIndex >= 0 && handIndex >= 0) {
+        glm::quat handRotation;
+        glm::vec3 knucklePosition;
+        glm::vec3 handPosition;
+        if (!getJointPositionInWorldFrame(knuckleIndex, knucklePosition)) {
+            return false;
+        }
+        if (!getJointPositionInWorldFrame(handIndex, handPosition)) {
+            return false;
+        }
+        if (!getJointRotationInWorldFrame(handIndex, handRotation)) {
+            return false;
+        }
+        float halfPalmLength = glm::distance(knucklePosition, handPosition) * 0.5f;
+        // z azis is standardized to be out of the palm.  move from the knuckle-joint away from the palm
+        // by 1/2 the palm length.
+        position = knucklePosition + handRotation * (glm::vec3(0.0f, 0.0f, 1.0f) * halfPalmLength);
+        return true;
     }
     return false;
 }
 
 bool SkeletonModel::getRightGrabPosition(glm::vec3& position) const {
-    int index = _rig->indexOfJoint("RightHandMiddle1");
-    if (index >= 0) {
-        return getJointPositionInWorldFrame(index, position);
+    int knuckleIndex = _rig->indexOfJoint("RightHandMiddle1");
+    int handIndex = _rig->indexOfJoint("RightHand");
+    if (knuckleIndex >= 0 && handIndex >= 0) {
+        glm::quat handRotation;
+        glm::vec3 knucklePosition;
+        glm::vec3 handPosition;
+        if (!getJointPositionInWorldFrame(knuckleIndex, knucklePosition)) {
+            return false;
+        }
+        if (!getJointPositionInWorldFrame(handIndex, handPosition)) {
+            return false;
+        }
+        if (!getJointRotationInWorldFrame(handIndex, handRotation)) {
+            return false;
+        }
+        float halfPalmLength = glm::distance(knucklePosition, handPosition) * 0.5f;
+        // z azis is standardized to be out of the palm.  move from the knuckle-joint away from the palm
+        // by 1/2 the palm length.
+        position = knucklePosition + handRotation * (glm::vec3(0.0f, 0.0f, 1.0f) * halfPalmLength);
+        return true;
     }
     return false;
 }