diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 8f7af63a81..a57740a691 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -271,6 +271,25 @@ glm::mat4 MyAvatar::getSensorToWorldMatrix() const { return _sensorToWorldMatrix; } +// returns true if pos is OUTSIDE of the vertical capsule +// where the middle cylinder length is defined by capsuleLen and the radius by capsuleRad. +static bool capsuleCheck(const glm::vec3& pos, float capsuleLen, float capsuleRad) { + const float halfCapsuleLen = capsuleLen / 2.0f; + if (fabs(pos.y) <= halfCapsuleLen) { + // cylinder check for middle capsule + glm::vec2 horizPos(pos.x, pos.z); + return glm::length(horizPos) > capsuleRad; + } else if (pos.y > halfCapsuleLen) { + glm::vec3 center(0.0f, halfCapsuleLen, 0.0f); + return glm::length(center - pos) > capsuleRad; + } else if (pos.y < halfCapsuleLen) { + glm::vec3 center(0.0f, -halfCapsuleLen, 0.0f); + return glm::length(center - pos) > capsuleRad; + } else { + return false; + } +} + // Pass a recent sample of the HMD to the avatar. // This can also update the avatar's position to follow the HMD // as it moves through the world. @@ -289,11 +308,14 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { _hmdSensorOrientation = glm::quat_cast(hmdSensorMatrix); const float STRAIGHTING_LEAN_DURATION = 0.5f; // seconds - const float STRAIGHTING_LEAN_THRESHOLD = 0.2f; // meters + + // define a vertical capsule + const float STRAIGHTING_LEAN_CAPSULE_RADIUS = 0.2f; // meters + const float STRAIGHTING_LEAN_CAPSULE_LENGTH = 0.05f; // length of the cylinder part of the capsule in meters. auto newBodySensorMatrix = deriveBodyFromHMDSensor(); glm::vec3 diff = extractTranslation(newBodySensorMatrix) - extractTranslation(_bodySensorMatrix); - if (!_straightingLean && glm::length(diff) > STRAIGHTING_LEAN_THRESHOLD) { + if (!_straightingLean && capsuleCheck(diff, STRAIGHTING_LEAN_CAPSULE_LENGTH, STRAIGHTING_LEAN_CAPSULE_RADIUS)) { // begin homing toward derived body position. _straightingLean = true; @@ -1854,13 +1876,39 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { const glm::quat hmdOrientation = getHMDSensorOrientation(); const glm::quat hmdOrientationYawOnly = cancelOutRollAndPitch(hmdOrientation); - // In sensor space, figure out where the avatar body should be, - // by applying offsets from the avatar's neck & head joints. - vec3 localEyes = _skeletonModel.getDefaultEyeModelPosition(); - vec3 localNeck(0.0f, 0.48f, 0.0f); // start with some kind of guess if the skeletonModel is not loaded yet. - _skeletonModel.getLocalNeckPosition(localNeck); + const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 1.6f, 0.0f); + const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 1.6f, 0.0f); + const glm::vec3 DEFAULT_NECK_POS(0.0f, 1.5f, 0.0f); + const glm::vec3 DEFAULT_HIPS_POS(0.0f, 1.0f, 0.0f); + + vec3 localEyes, localNeck; + if (!_debugDrawSkeleton) { + const glm::quat rotY180 = glm::angleAxis((float)PI, glm::vec3(0.0f, 1.0f, 0.0f)); + localEyes = rotY180 * (((DEFAULT_RIGHT_EYE_POS + DEFAULT_LEFT_EYE_POS) / 2.0f) - DEFAULT_HIPS_POS); + localNeck = rotY180 * (DEFAULT_NECK_POS - DEFAULT_HIPS_POS); + } else { + // TODO: At the moment MyAvatar does not have access to the rig, which has the skeleton, which has the bind poses. + // for now use the _debugDrawSkeleton, which is initialized with the same FBX model as the rig. + + // TODO: cache these indices. + int rightEyeIndex = _debugDrawSkeleton->nameToJointIndex("RightEye"); + int leftEyeIndex = _debugDrawSkeleton->nameToJointIndex("LeftEye"); + int neckIndex = _debugDrawSkeleton->nameToJointIndex("Neck"); + int hipsIndex = _debugDrawSkeleton->nameToJointIndex("Hips"); + + glm::vec3 absRightEyePos = rightEyeIndex != -1 ? _debugDrawSkeleton->getAbsoluteBindPose(rightEyeIndex).trans : DEFAULT_RIGHT_EYE_POS; + glm::vec3 absLeftEyePos = leftEyeIndex != -1 ? _debugDrawSkeleton->getAbsoluteBindPose(leftEyeIndex).trans : DEFAULT_LEFT_EYE_POS; + glm::vec3 absNeckPos = neckIndex != -1 ? _debugDrawSkeleton->getAbsoluteBindPose(neckIndex).trans : DEFAULT_NECK_POS; + glm::vec3 absHipsPos = neckIndex != -1 ? _debugDrawSkeleton->getAbsoluteBindPose(hipsIndex).trans : DEFAULT_HIPS_POS; + + const glm::quat rotY180 = glm::angleAxis((float)PI, glm::vec3(0.0f, 1.0f, 0.0f)); + localEyes = rotY180 * (((absRightEyePos + absLeftEyePos) / 2.0f) - absHipsPos); + localNeck = rotY180 * (absNeckPos - absHipsPos); + } // apply simplistic head/neck model + // figure out where the avatar body should be by applying offsets from the avatar's neck & head joints. + // eyeToNeck offset is relative full HMD orientation. // while neckToRoot offset is only relative to HMDs yaw. glm::vec3 eyeToNeck = hmdOrientation * (localNeck - localEyes); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 0022749d51..1e512ee767 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1026,12 +1026,21 @@ static void computeHeadNeckAnimVars(AnimSkeleton::ConstPointer skeleton, const A const glm::quat hmdOrientation = hmdPose.rot * rotY180; // rotY180 will make z forward not -z // TODO: cache jointIndices + int rightEyeIndex = skeleton->nameToJointIndex("RightEye"); + int leftEyeIndex = skeleton->nameToJointIndex("LeftEye"); + int headIndex = skeleton->nameToJointIndex("Head"); + int neckIndex = skeleton->nameToJointIndex("Neck"); + + const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 1.6f, 0.0f); + const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 1.6f, 0.0f); + const glm::vec3 DEFAULT_HEAD_POS(0.0f, 1.55f, 0.0f); + const glm::vec3 DEFAULT_NECK_POS(0.0f, 1.5f, 0.0f); // Use absolute bindPose positions just in case the relBindPose have rotations we don't expect. - glm::vec3 absRightEyePos = skeleton->getAbsoluteBindPose(skeleton->nameToJointIndex("RightEye")).trans; - glm::vec3 absLeftEyePos = skeleton->getAbsoluteBindPose(skeleton->nameToJointIndex("LeftEye")).trans; - glm::vec3 absHeadPos = skeleton->getAbsoluteBindPose(skeleton->nameToJointIndex("Head")).trans; - glm::vec3 absNeckPos = skeleton->getAbsoluteBindPose(skeleton->nameToJointIndex("Neck")).trans; + glm::vec3 absRightEyePos = rightEyeIndex != -1 ? skeleton->getAbsoluteBindPose(rightEyeIndex).trans : DEFAULT_RIGHT_EYE_POS; + glm::vec3 absLeftEyePos = leftEyeIndex != -1 ? skeleton->getAbsoluteBindPose(leftEyeIndex).trans : DEFAULT_LEFT_EYE_POS; + glm::vec3 absHeadPos = headIndex != -1 ? skeleton->getAbsoluteBindPose(headIndex).trans : DEFAULT_HEAD_POS; + glm::vec3 absNeckPos = neckIndex != -1 ? skeleton->getAbsoluteBindPose(neckIndex).trans : DEFAULT_NECK_POS; glm::vec3 absCenterEyePos = (absRightEyePos + absLeftEyePos) / 2.0f; glm::vec3 eyeOffset = absCenterEyePos - absHeadPos;