diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index e4c98215c2..711ed17231 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/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); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 02968e3665..4e7564472c 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -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); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 3d5d44b844..50313d10e7 100644 --- a/libraries/animation/src/Rig.h +++ b/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 diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h index a8543d6070..d3c7405353 100644 --- a/libraries/physics/src/CharacterController.h +++ b/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, diff --git a/tests/shared/src/GeometryUtilTests.cpp b/tests/shared/src/GeometryUtilTests.cpp index 7ba22ec13d..289ac9a0a9 100644 --- a/tests/shared/src/GeometryUtilTests.cpp +++ b/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); +} diff --git a/tests/shared/src/GeometryUtilTests.h b/tests/shared/src/GeometryUtilTests.h index daf740dcd3..b6340cd8cf 100644 --- a/tests/shared/src/GeometryUtilTests.h +++ b/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);