Merge pull request #7129 from hyperlogic/tony/no-hands-in-body

Prevent avatar's wrists from entering torso

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);
 

libraries/animation/src/AnimInverseKinematics.cpp

@@ -670,7 +670,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST);
 
             std::vector<float> minDots;
-            const float MAX_SHOULDER_SWING = PI / 20.0f;
+            const float MAX_SHOULDER_SWING = PI / 6.0f;
             minDots.push_back(cosf(MAX_SHOULDER_SWING));
             stConstraint->setSwingLimits(minDots);
 

libraries/animation/src/Rig.cpp

@@ -1080,8 +1080,31 @@ 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;
+        const float MIN_LENGTH = 1.0e-4f;
+
+        // 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);
+
+            // check for 2d overlap of the hand and body circles.
+            auto circleToCircle = handCircleCenter - bodyCircleCenter;
+            const float circleToCircleLength = glm::length(circleToCircle);
+            const float penetrationDistance = HAND_RADIUS + BODY_RADIUS - circleToCircleLength;
+            if (penetrationDistance > 0.0f && circleToCircleLength > MIN_LENGTH) {
+                // push the hands out of the body
+                handCircleCenter += penetrationDistance * glm::normalize(circleToCircle);
+            }
+
+            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 +1112,23 @@ 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);
+
+            // check for 2d overlap of the hand and body circles.
+            auto circleToCircle = handCircleCenter - bodyCircleCenter;
+            const float circleToCircleLength = glm::length(circleToCircle);
+            const float penetrationDistance = HAND_RADIUS + BODY_RADIUS - circleToCircleLength;
+            if (penetrationDistance > 0.0f && circleToCircleLength > MIN_LENGTH) {
+                // push the hands out of the body
+                handCircleCenter += penetrationDistance * glm::normalize(circleToCircle);
+            }
+
+            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 {

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

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,