From 73ac47724af70abf2fd7a3739f20adbd1701133b Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Wed, 17 Feb 2016 16:23:18 -0800
Subject: [PATCH] Rig: Prevent the hand IK targets from entering the body.

Use a 2d circle/circle intersection test to keep the hands outside of the body.
---
 interface/src/avatar/SkeletonModel.cpp      |  1 +
 libraries/animation/src/Rig.cpp             | 39 +++++++++++++++++++--
 libraries/animation/src/Rig.h               |  1 +
 libraries/physics/src/CharacterController.h |  2 ++
 4 files changed, 41 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index c77c4fdd78..e4c98215c2 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -144,6 +144,7 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         } else {
             handParams.isRightEnabled = false;
         }
+        handParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
 
         _rig->updateFromHandParameters(handParams, deltaTime);
 
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 817054a9f5..718ae9bb79 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1080,8 +1080,29 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
 
 void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
     if (_animSkeleton && _animNode) {
+
+        const float HAND_RADIUS = 0.05f;
+        const float BODY_RADIUS = params.bodyCapsuleRadius;
+
+        // project the hips onto the xz plane.
+        auto hipsTrans = _internalPoseSet._absolutePoses[_animSkeleton->nameToJointIndex("Hips")].trans;
+        const glm::vec2 bodyCircleCenter(hipsTrans.x, hipsTrans.z);
+
         if (params.isLeftEnabled) {
-            _animVars.set("leftHandPosition", params.leftPosition);
+
+            // project the hand position onto the xz plane.
+            glm::vec2 handCircleCenter(params.leftPosition.x, params.leftPosition.z);
+            auto d = handCircleCenter - bodyCircleCenter;
+
+            // check for 2d overlap of the hand and body circles.
+            float penetrationDistance = HAND_RADIUS + BODY_RADIUS - glm::length(d);
+            if (penetrationDistance > 0) {
+                // push the hands out of the body
+                handCircleCenter += penetrationDistance * glm::normalize(d);
+            }
+
+            glm::vec3 handPosition(handCircleCenter.x, params.leftPosition.y, handCircleCenter.y);
+            _animVars.set("leftHandPosition", handPosition);
             _animVars.set("leftHandRotation", params.leftOrientation);
             _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
         } else {
@@ -1089,8 +1110,22 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
             _animVars.unset("leftHandRotation");
             _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
         }
+
         if (params.isRightEnabled) {
-            _animVars.set("rightHandPosition", params.rightPosition);
+
+            // project the hand position onto the xz plane.
+            glm::vec2 handCircleCenter(params.rightPosition.x, params.rightPosition.z);
+            auto d = handCircleCenter - bodyCircleCenter;
+
+            // check for 2d overlap of the hand and body circles.
+            float penetrationDistance = HAND_RADIUS + BODY_RADIUS - glm::length(d);
+            if (penetrationDistance > 0) {
+                // push the hands out of the body
+                handCircleCenter += penetrationDistance * glm::normalize(d);
+            }
+
+            glm::vec3 handPosition(handCircleCenter.x, params.rightPosition.y, handCircleCenter.y);
+            _animVars.set("rightHandPosition", handPosition);
             _animVars.set("rightHandRotation", params.rightOrientation);
             _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
         } else {
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index e4668d6c2a..9c5b014d55 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -67,6 +67,7 @@ public:
     struct HandParameters {
         bool isLeftEnabled;
         bool isRightEnabled;
+        float bodyCapsuleRadius;
         glm::vec3 leftPosition = glm::vec3();     // rig space
         glm::quat leftOrientation = glm::quat();  // rig space (z forward)
         glm::vec3 rightPosition = glm::vec3();    // rig space
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index 5362ca52e4..a8543d6070 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -78,6 +78,8 @@ public:
 
     glm::vec3 getLinearVelocity() const;
 
+    float getCapsuleRadius() const { return _radius; }
+
     enum class State {
         Ground = 0,
         Takeoff,