From 7bee0478be2f53fdb8016f5576c0a45fa3d458e7 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 15 May 2014 11:30:09 -0700
Subject: [PATCH] More work on integrating IK solution from Sixense.

---
 interface/src/avatar/SkeletonModel.cpp | 52 +++++++++++++++++++++++++-
 1 file changed, 51 insertions(+), 1 deletion(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 1e57481c66..1921094a0f 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -164,7 +164,8 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
     
     // rotate palm to align with its normal (normal points out of hand's palm)
     glm::quat palmRotation;
-    if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
+    if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) &&
+            Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
         getJointRotation(parentJointIndex, palmRotation, true);
     } else {
         getJointRotation(jointIndex, palmRotation, true);
@@ -280,6 +281,55 @@ void SkeletonModel::renderJointConstraints(int jointIndex) {
 }
 
 void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) {
+    // this algorithm is from sample code from sixense
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    int elbowJointIndex = geometry.joints.at(jointIndex).parentIndex;
+    if (elbowJointIndex == -1) {
+        return;
+    }
+    int shoulderJointIndex = geometry.joints.at(elbowJointIndex).parentIndex;
+    glm::vec3 shoulderPosition;
+    if (!getJointPosition(shoulderJointIndex, shoulderPosition)) {
+        return;
+    }
+    // precomputed lengths
+    float scale = extractUniformScale(_scale);
+    float upperArmLength = geometry.joints.at(elbowJointIndex).distanceToParent * scale;
+    float lowerArmLength = geometry.joints.at(jointIndex).distanceToParent * scale;
     
+    // first set wrist position
+    glm::vec3 wristPosition = position;
+    
+    glm::vec3 shoulderToWrist = wristPosition - shoulderPosition;
+    float distanceToWrist = glm::length(shoulderToWrist);
+    
+    // prevent gimbal lock
+    if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) {
+        distanceToWrist = upperArmLength + lowerArmLength - EPSILON;
+        shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist;
+        wristPosition = shoulderPosition + shoulderToWrist;
+    }
+    
+    // cosine of angle from upper arm to hand vector 
+    float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) / 
+        (2 * upperArmLength * distanceToWrist);
+    float mid = upperArmLength * cosA;
+    float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA);
+    
+    // direction of the elbow
+    glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist
+    glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down
+    const float NORMAL_WEIGHT = 0.5f;
+	glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT);
+    
+    if((jointIndex == geometry.rightHandJointIndex && finalNormal.y > 0.0f) ||
+            (jointIndex == geometry.leftHandJointIndex && finalNormal.y < 0)) {
+        finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis)
+    }
+    
+	glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal));
+	
+	// ik solution
+    glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height;
 }