MyAvatar: Allow user to raise hands directly overhead.

Previously we were using a infinitely tall vertical cylinder
to push hand IK targets out of the body, this had the
side-effect of preventing the hands from being raised over
the head.  Now, we collide against the same 3d capsule
used by the physics system for avatar collisions.

interface/src/avatar/SkeletonModel.cpp

@@ -144,7 +144,10 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         } else {
             handParams.isRightEnabled = false;
         }
+
         handParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
+        handParams.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight();
+        handParams.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset();
 
         _rig->updateFromHandParameters(handParams, deltaTime);
 

libraries/animation/src/Rig.cpp

@@ -17,6 +17,7 @@
 #include <QWriteLocker>
 #include <QReadLocker>
 
+#include <GeometryUtil.h>
 #include <NumericalConstants.h>
 #include <DebugDraw.h>
 
@@ -1073,7 +1074,6 @@ 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.
@@ -1082,23 +1082,20 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
         if (hipsIndex >= 0) {
             hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans;
         }
-        const glm::vec2 bodyCircleCenter(hipsTrans.x, hipsTrans.z);
+        const float bodyCapsuleRadius = params.bodyCapsuleRadius;
+        const glm::vec3 bodyCapsuleCenter = hipsTrans - params.bodyCapsuleLocalOffset;
+        const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, params.bodyCapsuleHalfHeight, 0);
+        const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, params.bodyCapsuleHalfHeight, 0);
 
         if (params.isLeftEnabled) {
 
-            // 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);
+            // prevent the hand IK targets from intersecting the body capsule
+            glm::vec3 handPosition = params.leftPosition;
+            glm::vec3 displacement(glm::vec3::_null);
+            if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
+                handPosition -= displacement;
             }
 
-            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);
@@ -1110,19 +1107,13 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
 
         if (params.isRightEnabled) {
 
-            // 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);
+            // prevent the hand IK targets from intersecting the body capsule
+            glm::vec3 handPosition = params.rightPosition;
+            glm::vec3 displacement(glm::vec3::_null);
+            if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
+                handPosition -= displacement;
             }
 
-            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);

libraries/animation/src/Rig.h

@@ -68,6 +68,8 @@ public:
         bool isLeftEnabled;
         bool isRightEnabled;
         float bodyCapsuleRadius;
+        float bodyCapsuleHalfHeight;
+        glm::vec3 bodyCapsuleLocalOffset;
         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

@@ -79,6 +79,8 @@ public:
     glm::vec3 getLinearVelocity() const;
 
     float getCapsuleRadius() const { return _radius; }
+    float getCapsuleHalfHeight() const { return _halfHeight; }
+    glm::vec3 getCapsuleLocalOffset() const { return _shapeLocalOffset; }
 
     enum class State {
         Ground = 0,

tests/shared/src/GeometryUtilTests.cpp

@@ -212,3 +212,15 @@ void GeometryUtilTests::testTwistSwingDecomposition() {
 }
 
 
+void GeometryUtilTests::testSphereCapsulePenetration() {
+    glm::vec3 sphereCenter(1.5, 0.0, 0.0);
+    float sphereRadius = 1.0f;
+    glm::vec3 capsuleStart(0.0f, -10.0f, 0.0f);
+    glm::vec3 capsuleEnd(0.0f, 10.0f, 0.0f);
+    float capsuleRadius = 1.0f;
+
+    glm::vec3 penetration(glm::vec3::_null);
+    bool hit = findSphereCapsulePenetration(sphereCenter, sphereRadius, capsuleStart, capsuleEnd, capsuleRadius, penetration);
+    QCOMPARE(hit, true);
+    QCOMPARE_WITH_ABS_ERROR(penetration, glm::vec3(-0.5f, 0.0f, 0.0f), EPSILON);
+}

tests/shared/src/GeometryUtilTests.h

@@ -21,6 +21,7 @@ private slots:
     void testLocalRayRectangleIntersection();
     void testWorldRayRectangleIntersection();
     void testTwistSwingDecomposition();
+    void testSphereCapsulePenetration();
 };
 
 float getErrorDifference(const float& a, const float& b);