From 46d23a9f38f84b0fb4730b5afbe4c2ddf1ccfa8f Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 19 Nov 2015 17:30:56 -0800 Subject: [PATCH] head IK and controller IK work again! --- interface/src/avatar/SkeletonModel.cpp | 3 - libraries/animation/src/Rig.cpp | 88 ++++++++++++++------------ libraries/animation/src/Rig.h | 22 ++++--- 3 files changed, 59 insertions(+), 54 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 104bd5b403..570bc0589a 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -114,9 +114,6 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { headParams.leanSideways = head->getFinalLeanSideways(); headParams.leanForward = head->getFinalLeanForward(); headParams.torsoTwist = head->getTorsoTwist(); - headParams.localHeadPitch = head->getFinalPitch(); - headParams.localHeadYaw = head->getFinalYaw(); - headParams.localHeadRoll = head->getFinalRoll(); if (qApp->getAvatarUpdater()->isHMDMode()) { headParams.isInHMD = true; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 56884933d1..a997d358a7 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1014,30 +1014,15 @@ void Rig::updateLeanJoint(int index, float leanSideways, float leanForward, floa } } -static AnimPose avatarToBonePose(AnimPose pose, AnimSkeleton::ConstPointer skeleton) { - AnimPose rootPose = skeleton->getAbsoluteBindPose(skeleton->nameToJointIndex("Hips")); - AnimPose rotY180(glm::vec3(1.0f), glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)), glm::vec3(0)); - return rootPose * rotY180 * pose; -} - -#ifdef DEBUG_RENDERING -static AnimPose boneToAvatarPose(AnimPose pose, AnimSkeleton::ConstPointer skeleton) { - AnimPose rootPose = skeleton->getAbsoluteBindPose(skeleton->nameToJointIndex("Hips")); - AnimPose rotY180(glm::vec3(1.0f), glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)), glm::vec3(0)); - return (rootPose * rotY180).inverse() * pose; -} -#endif - -static void computeHeadNeckAnimVars(AnimSkeleton::ConstPointer skeleton, const AnimPose& avatarHMDPose, +static void computeHeadNeckAnimVars(AnimSkeleton::ConstPointer skeleton, const AnimPose& geometryHMDPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut, glm::vec3& neckPositionOut, glm::quat& neckOrientationOut) { // the input hmd values are in avatar space // we need to transform them into bone space. - AnimPose hmdPose = avatarToBonePose(avatarHMDPose, skeleton); + AnimPose hmdPose = geometryHMDPose; const glm::vec3 hmdPosition = hmdPose.trans; - const glm::quat rotY180 = glm::angleAxis((float)PI, glm::vec3(0.0f, 1.0f, 0.0f)); - const glm::quat hmdOrientation = hmdPose.rot * rotY180; // rotY180 will make z forward not -z + const glm::quat hmdOrientation = hmdPose.rot; // TODO: cache jointIndices int rightEyeIndex = skeleton->nameToJointIndex("RightEye"); @@ -1076,8 +1061,8 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { glm::vec3 headPos, neckPos; glm::quat headRot, neckRot; - AnimPose avatarHMDPose(glm::vec3(1.0f), params.localHeadOrientation, params.localHeadPosition); - computeHeadNeckAnimVars(_animSkeleton, avatarHMDPose, headPos, headRot, neckPos, neckRot); + AnimPose hmdPose(glm::vec3(1.0f), avatarToGeometryNegZForward(params.localHeadOrientation), avatarToGeometry(params.localHeadPosition)); + computeHeadNeckAnimVars(_animSkeleton, hmdPose, headPos, headRot, neckPos, neckRot); // debug rendering #ifdef DEBUG_RENDERING @@ -1086,12 +1071,10 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { // transform from bone into avatar space AnimPose headPose(glm::vec3(1), headRot, headPos); - headPose = boneToAvatarPose(headPose, _animSkeleton); DebugDraw::getInstance().addMyAvatarMarker("headTarget", headPose.rot, headPose.trans, red); // transform from bone into avatar space AnimPose neckPose(glm::vec3(1), neckRot, neckPos); - neckPose = boneToAvatarPose(neckPose, _animSkeleton); DebugDraw::getInstance().addMyAvatarMarker("neckTarget", neckPose.rot, neckPose.trans, green); #endif @@ -1105,13 +1088,21 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { } 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); + + /* + qCDebug(animation) << "AJT: input orientation " << params.localHeadOrientation; + qCDebug(animation) << "AJT: after transform" << avatarToGeometry(params.localHeadOrientation); + */ + + _animVars.set("headRotation", avatarToGeometryNegZForward(params.localHeadOrientation)); _animVars.set("headAndNeckType", (int)IKTarget::Type::RotationOnly); _animVars.set("headType", (int)IKTarget::Type::RotationOnly); _animVars.unset("neckPosition"); @@ -1141,16 +1132,36 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } +glm::vec3 Rig::avatarToGeometry(const glm::vec3& pos) const { + // AJT: TODO cache transform + glm::quat yFlip = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)); + AnimPose geometryToAvatarTransform = AnimPose(glm::vec3(1), yFlip, glm::vec3()) * _modelOffset * _geometryOffset; + glm::vec3 result = geometryToAvatarTransform.inverse() * pos; + return result; +} + +glm::quat Rig::avatarToGeometryNegZForward(const glm::quat& quat) const { + // AJT: TODO cache transform + AnimPose yFlip(glm::vec3(1), glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)), glm::vec3()); + AnimPose geometryToAvatarTransform = yFlip * _modelOffset * _geometryOffset; + return (geometryToAvatarTransform.inverse() * AnimPose(glm::vec3(1), quat, glm::vec3()) * yFlip).rot; +} + +glm::quat Rig::avatarToGeometryZForward(const glm::quat& quat) const { + // AJT: TODO cache transform + AnimPose yFlip(glm::vec3(1), glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)), glm::vec3()); + AnimPose geometryToAvatarTransform = yFlip * _modelOffset * _geometryOffset; + return (geometryToAvatarTransform.inverse() * AnimPose(glm::vec3(1), quat, glm::vec3())).rot; +} + void Rig::updateFromHandParameters(const HandParameters& params, float dt) { if (_animSkeleton && _animNode) { - // TODO: figure out how to obtain the yFlip from where it is actually stored - glm::quat yFlipHACK = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)); AnimPose rootBindPose = _animSkeleton->getRootAbsoluteBindPoseByChildName("LeftHand"); if (params.isLeftEnabled) { - _animVars.set("leftHandPosition", rootBindPose.trans + rootBindPose.rot * yFlipHACK * params.leftPosition); - _animVars.set("leftHandRotation", rootBindPose.rot * yFlipHACK * params.leftOrientation); + _animVars.set("leftHandPosition", avatarToGeometry(params.leftPosition)); + _animVars.set("leftHandRotation", avatarToGeometryZForward(params.leftOrientation)); _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); } else { _animVars.unset("leftHandPosition"); @@ -1158,8 +1169,8 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) { _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); } if (params.isRightEnabled) { - _animVars.set("rightHandPosition", rootBindPose.trans + rootBindPose.rot * yFlipHACK * params.rightPosition); - _animVars.set("rightHandRotation", rootBindPose.rot * yFlipHACK * params.rightOrientation); + _animVars.set("rightHandPosition", avatarToGeometry(params.rightPosition)); + _animVars.set("rightHandRotation", avatarToGeometryZForward(params.rightOrientation)); _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); } else { _animVars.unset("rightHandPosition"); @@ -1187,7 +1198,6 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) { _animVars.set("leftHandOverlayAlpha", _leftHandOverlayAlpha); _animVars.set("leftHandGrabBlend", params.leftTrigger); - // set leftHand grab vars _animVars.set("isRightHandIdle", false); _animVars.set("isRightHandPoint", false); @@ -1307,7 +1317,7 @@ glm::mat4 Rig::getJointTransform(int jointIndex) const { //if (_jointStates.size() == 73) { // check for differences between jointStates and _absolutePoses! - if (false) { + if (true) { glm::mat4 oldMat = _jointStates[jointIndex].getTransform(); AnimPose oldPose(oldMat); @@ -1316,24 +1326,20 @@ glm::mat4 Rig::getJointTransform(int jointIndex) const { AnimPose newPose(newMat); bool badTrans = false; - const float TRANS_EPSILON = 0.01f; - if (glm::length(newPose.trans - oldPose.trans) > TRANS_EPSILON) { + const float TRANS_EPSILON = 0.001f; + if (glm::length(newPose.trans - oldPose.trans) / glm::length(oldPose.trans) > TRANS_EPSILON) { badTrans = true; } bool badScale = false; - const float SCALE_EPSILON = 0.00001f; - if (glm::length(newPose.scale - oldPose.scale) > SCALE_EPSILON) { + const float SCALE_EPSILON = 0.0001f; + if (glm::length(newPose.scale - oldPose.scale) / glm::length(oldPose.scale) > SCALE_EPSILON) { badScale = true; } bool badRot = false; - const float ROT_EPSILON = 0.0001f; - glm::quat oldLog = glm::log(newPose.rot); - glm::quat newLog = glm::log(oldPose.rot); - glm::vec3 oldLogVec(oldLog.x, oldLog.y, oldLog.z); - glm::vec3 newLogVec(newLog.x, newLog.y, newLog.z); - if (glm::length(oldLogVec - newLogVec) > ROT_EPSILON) { + const float ROT_EPSILON = 0.00001f; + if (1.0f - fabsf(glm::dot(newPose.rot, oldPose.rot)) > ROT_EPSILON) { badRot = true; } diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 182b7e632b..0845c5a4a4 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -42,12 +42,9 @@ public: float leanForward = 0.0f; // degrees float torsoTwist = 0.0f; // degrees bool enableLean = false; - glm::quat worldHeadOrientation = glm::quat(); - glm::quat localHeadOrientation = glm::quat(); - float localHeadPitch = 0.0f; // degrees - float localHeadYaw = 0.0f; // degrees - float localHeadRoll = 0.0f; // degrees - glm::vec3 localHeadPosition = glm::vec3(); + glm::quat worldHeadOrientation = glm::quat(); // world space (-z forward) + glm::quat localHeadOrientation = glm::quat(); // avatar space (-z forward) + glm::vec3 localHeadPosition = glm::vec3(); // avatar space bool isInHMD = false; int leanJointIndex = -1; int neckJointIndex = -1; @@ -67,10 +64,10 @@ public: struct HandParameters { bool isLeftEnabled; bool isRightEnabled; - glm::vec3 leftPosition = glm::vec3(); - glm::quat leftOrientation = glm::quat(); - glm::vec3 rightPosition = glm::vec3(); - glm::quat rightOrientation = glm::quat(); + glm::vec3 leftPosition = glm::vec3(); // avatar space + glm::quat leftOrientation = glm::quat(); // avatar space (z forward) + glm::vec3 rightPosition = glm::vec3(); // avatar space + glm::quat rightOrientation = glm::quat(); // avatar space (z forward) float leftTrigger = 0.0f; float rightTrigger = 0.0f; }; @@ -167,6 +164,7 @@ public: const glm::vec3& getEyesInRootFrame() const { return _eyesInRootFrame; } + protected: void updateAnimationStateHandlers(); void applyOverridePoses(); @@ -179,6 +177,10 @@ public: void computeEyesInRootFrame(const AnimPoseVec& poses); + glm::vec3 Rig::avatarToGeometry(const glm::vec3& pos) const; + glm::quat Rig::avatarToGeometryZForward(const glm::quat& quat) const; + glm::quat Rig::avatarToGeometryNegZForward(const glm::quat& quat) const; + // AJT: TODO: LEGACY QVector _jointStates; glm::mat4 _legacyModelOffset;