From 9ce43a57f19816e580de0964107e4d903682cfe4 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 22 Sep 2015 19:58:21 -0700 Subject: [PATCH] Better head IK when in an HMD. Because the current IK system doesn't quite handle what we need for the head and neck IK, we do it procedurally in the rig, and manually set both neck and head IK targets. --- interface/src/avatar/SkeletonModel.cpp | 34 ++++++++--- libraries/animation/src/Rig.cpp | 79 +++++++++++++++++++++++--- tests/animation/src/data/avatar.json | 5 ++ 3 files changed, 102 insertions(+), 16 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 6b56e92d80..9aa83e453c 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -21,6 +21,7 @@ #include "SkeletonModel.h" #include "Util.h" #include "InterfaceLogging.h" +#include "AnimDebugDraw.h" SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) : Model(rig, parent), @@ -126,19 +127,36 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { headParams.leanSideways = head->getFinalLeanSideways(); headParams.leanForward = head->getFinalLeanForward(); headParams.torsoTwist = head->getTorsoTwist(); - headParams.localHeadOrientation = head->getFinalOrientationInLocalFrame(); headParams.localHeadPitch = head->getFinalPitch(); headParams.localHeadYaw = head->getFinalYaw(); headParams.localHeadRoll = head->getFinalRoll(); - headParams.isInHMD = qApp->getAvatarUpdater()->isHMDMode(); - // get HMD position from sensor space into world space, and back into model space - glm::mat4 worldToModel = glm::inverse(createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition())); - glm::vec3 yAxis(0.0f, 1.0f, 0.0f); - glm::vec3 hmdPosition = glm::angleAxis((float)M_PI, yAxis) * transformPoint(worldToModel * myAvatar->getSensorToWorldMatrix(), myAvatar->getHMDSensorPosition()); - headParams.localHeadPosition = hmdPosition; - headParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame(); + if (qApp->getAvatarUpdater()->isHMDMode()) { + headParams.isInHMD = true; + + // get HMD position from sensor space into world space, and back into model space + AnimPose avatarToWorld(glm::vec3(1), myAvatar->getOrientation(), myAvatar->getPosition()); + glm::mat4 worldToAvatar = (glm::mat4)avatarToWorld.inverse(); + glm::mat4 worldHMDMat = myAvatar->getSensorToWorldMatrix() * myAvatar->getHMDSensorMatrix(); + glm::mat4 hmdMat = worldToAvatar * worldHMDMat; + + // in local avatar space (i.e. relative to avatar pos/orientation.) + glm::vec3 hmdPosition = extractTranslation(hmdMat); + glm::quat hmdOrientation = extractRotation(hmdMat); + + headParams.localHeadPosition = hmdPosition; + headParams.localHeadOrientation = hmdOrientation; + + headParams.worldHeadOrientation = extractRotation(worldHMDMat); + } else { + headParams.isInHMD = false; + + // We don't have a valid localHeadPosition. + headParams.localHeadOrientation = head->getFinalOrientationInLocalFrame(); + headParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame(); + } + headParams.eyeLookAt = head->getLookAtPosition(); headParams.eyeSaccade = head->getSaccade(); headParams.leanJointIndex = geometry.leanJointIndex; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index b0ffd081c2..27bfb35787 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -14,9 +14,11 @@ #include #include +#include "NumericalConstants.h" #include "AnimationHandle.h" #include "AnimationLogging.h" #include "AnimSkeleton.h" +#include "DebugDraw.h" #include "Rig.h" @@ -983,19 +985,80 @@ void Rig::updateLeanJoint(int index, float leanSideways, float leanForward, floa } } } +static void hmdHeadNeckModel(AnimSkeleton::ConstPointer skeleton, const glm::vec3& hmdPosition, const glm::quat& origHMDOrientation, + glm::vec3& headPositionOut, glm::quat& headOrientationOut, + glm::vec3& neckPositionOut, glm::quat& neckOrientationOut) { + + // hmd orientation comes in as -z forward, we need z forward for the translations to work correctly. + glm::quat rotY180 = glm::angleAxis((float)PI, glm::vec3(0.0f, 1.0f, 0.0f)); + glm::quat hmdOrientation = origHMDOrientation * rotY180; + + // TODO: cache jointIndices + // 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 absCenterEyePos = (absRightEyePos + absLeftEyePos) / 2.0f; + glm::vec3 eyeOffset = absCenterEyePos - absHeadPos; + glm::vec3 headOffset = absHeadPos - absNeckPos; + + // apply simplistic head/neck model + + // head + headPositionOut = hmdPosition - hmdOrientation * eyeOffset; + headOrientationOut = hmdOrientation; + + // neck + neckPositionOut = hmdPosition - hmdOrientation * (headOffset + eyeOffset); + + // slerp between bind orientation and hmdOrientation + neckOrientationOut = safeMix(hmdOrientation, skeleton->getRelativeBindPose(skeleton->nameToJointIndex("Neck")).rot, 0.5f); +} void Rig::updateNeckJoint(int index, const HeadParameters& params) { if (index >= 0 && _jointStates[index].getParentIndex() >= 0) { if (_enableAnimGraph && _animSkeleton) { - // the params.localHeadOrientation is composed incorrectly, so re-compose it correctly from pitch, yaw and roll. - glm::quat realLocalHeadOrientation = (glm::angleAxis(glm::radians(-params.localHeadRoll), Z_AXIS) * - glm::angleAxis(glm::radians(params.localHeadYaw), Y_AXIS) * - glm::angleAxis(glm::radians(-params.localHeadPitch), X_AXIS)); - _animVars.set("headRotation", realLocalHeadOrientation); - // There's a theory that when not in hmd, we should _animVars.unset("headPosition"). - // However, until that works well, let's always request head be positioned where requested by hmd, camera, or default. - _animVars.set("headPosition", params.localHeadPosition); + if (params.isInHMD) { + glm::vec3 headPos, neckPos; + glm::quat headRot, neckRot; + + // the input hmd values are in avatar space + // apply rotY180 to values to get them into bone space which has z forward. + glm::quat rotY180 = glm::angleAxis((float)PI, glm::vec3(0.0f, 1.0f, 0.0f)); + glm::vec3 hmdPos = rotY180 * params.localHeadPosition; + glm::quat hmdRot = rotY180 * params.localHeadOrientation; + hmdHeadNeckModel(_animSkeleton, hmdPos, hmdRot, headPos, headRot, neckPos, neckRot); + + // debug rendering + /* + // apply rotY180 again to transform into avatar space. + glm::vec4 red(1.0f, 0.0f, 0.0f, 1.0f); + glm::vec4 green(0.0f, 1.0f, 0.0f, 1.0f); + DebugDraw::getInstance().addMyAvatarMarker("headTarget", rotY180 * headRot, rotY180 * headPos, red); + DebugDraw::getInstance().addMyAvatarMarker("neckTarget", rotY180 * neckRot, rotY180 * neckPos, green); + */ + + _animVars.set("headPosition", headPos); + _animVars.set("headRotation", headRot); + _animVars.set("neckPosition", neckPos); + _animVars.set("neckRotation", neckRot); + + } else { + + // the params.localHeadOrientation is composed incorrectly, so re-compose it correctly from pitch, yaw and roll. + glm::quat realLocalHeadOrientation = (glm::angleAxis(glm::radians(-params.localHeadRoll), Z_AXIS) * + glm::angleAxis(glm::radians(params.localHeadYaw), Y_AXIS) * + glm::angleAxis(glm::radians(-params.localHeadPitch), X_AXIS)); + + _animVars.unset("headPosition"); + _animVars.set("headRotation", realLocalHeadOrientation); + _animVars.unset("neckPosition"); + _animVars.unset("neckRotation"); + } + } else if (!_enableAnimGraph) { auto& state = _jointStates[index]; diff --git a/tests/animation/src/data/avatar.json b/tests/animation/src/data/avatar.json index 9c357ac845..550a95e980 100644 --- a/tests/animation/src/data/avatar.json +++ b/tests/animation/src/data/avatar.json @@ -23,6 +23,11 @@ "positionVar": "leftHandPosition", "rotationVar": "leftHandRotation" }, + { + "jointName": "Neck", + "positionVar": "neckPosition", + "rotationVar": "neckRotation" + }, { "jointName": "Head", "positionVar": "headPosition",