From 6564cfd5d1d44425335296eeab3cbea75ca69fbd Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 5 Jun 2017 18:33:32 -0700 Subject: [PATCH 01/20] WIP --- .../animation/src/AnimInverseKinematics.cpp | 138 ++++++++++++++---- .../animation/src/AnimInverseKinematics.h | 17 ++- libraries/animation/src/IKTarget.h | 10 +- 3 files changed, 126 insertions(+), 39 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 77437e79b9..41dae120fd 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -23,6 +23,25 @@ #include "CubicHermiteSpline.h" #include "AnimUtil.h" +static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointChainInfos, size_t numJointChainInfos, + int indexA, int indexB, + AnimInverseKinematics::JointChainInfo** jointChainInfoA, + AnimInverseKinematics::JointChainInfo** jointChainInfoB) { + *jointChainInfoA = nullptr; + *jointChainInfoB = nullptr; + for (size_t i = 0; i < numJointChainInfos; i++) { + if (jointChainInfos[i].jointIndex == indexA) { + *jointChainInfoA = jointChainInfos + i; + } + if (jointChainInfos[i].jointIndex == indexB) { + *jointChainInfoB = jointChainInfos + i; + } + if (*jointChainInfoA && *jointChainInfoB) { + break; + } + } +} + AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn) : jointName(jointNameIn), @@ -148,6 +167,12 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); + // AJT: HACK REMOVE manually set pole vector. + if (targetVar.jointName == "RightHand") { + target.setPoleVector(glm::normalize(glm::vec3(-1, 0, 0))); + target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); + } + targets.push_back(target); if (targetVar.jointIndex > _maxTargetIndex) { @@ -298,7 +323,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const // the tip's parent-relative as we proceed up the chain glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot(); - std::map debugJointMap; + const size_t MAX_CHAIN_DEPTH = 30; + JointChainInfo jointChainInfos[MAX_CHAIN_DEPTH]; // NOTE: if this code is removed, the head will remain rigid, causing the spine/hips to thrust forward backward // as the head is nodded. @@ -326,15 +352,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } } - // store the relative rotation change in the accumulator - _rotationAccumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); - glm::vec3 tipRelativeTranslation = _relativePoses[target.getIndex()].trans(); - _translationAccumulators[tipIndex].add(tipRelativeTranslation); - - if (debug) { - debugJointMap[tipIndex] = DebugJoint(tipRelativeRotation, tipRelativeTranslation, constrained); - } + jointChainInfos[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, target.getWeight(), tipIndex, constrained }; } // cache tip absolute position @@ -344,6 +363,9 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const // descend toward root, pivoting each joint to get tip closer to target position while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) { + + assert(chainDepth < MAX_CHAIN_DEPTH); + // compute the two lines that should be aligned glm::vec3 jointPosition = absolutePoses[pivotIndex].trans(); glm::vec3 leverArm = tipPosition - jointPosition; @@ -440,15 +462,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } } - // store the relative rotation change in the accumulator - _rotationAccumulators[pivotIndex].add(newRot, target.getWeight()); - glm::vec3 newTrans = _relativePoses[pivotIndex].trans(); - _translationAccumulators[pivotIndex].add(newTrans); - - if (debug) { - debugJointMap[pivotIndex] = DebugJoint(newRot, newTrans, constrained); - } + jointChainInfos[chainDepth] = { newRot, newTrans, target.getWeight(), pivotIndex, constrained }; // keep track of tip's new transform as we descend towards root tipPosition = jointPosition + deltaRotation * (tipPosition - jointPosition); @@ -461,8 +476,73 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const chainDepth++; } + if (target.getPoleIndex() != -1) { + + int topIndex = target.getPoleIndex(); + int midIndex = _skeleton->getParentIndex(topIndex); + if (midIndex != -1) { + int baseIndex = _skeleton->getParentIndex(midIndex); + if (baseIndex != -1) { + int baseParentIndex = _skeleton->getParentIndex(baseIndex); + AnimPose topPose, midPose, basePose, baseParentPose; + AnimPose postAbsPoses[MAX_CHAIN_DEPTH]; + AnimPose accum = absolutePoses[_hipsIndex]; + for (int i = (int)chainDepth - 1; i >= 0; i--) { + accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfos[i].relRot, jointChainInfos[i].relTrans); + postAbsPoses[i] = accum; + if (jointChainInfos[i].jointIndex == topIndex) { + topPose = accum; + } + if (jointChainInfos[i].jointIndex == midIndex) { + midPose = accum; + } + if (jointChainInfos[i].jointIndex == baseIndex) { + basePose = accum; + } + if (jointChainInfos[i].jointIndex == baseParentIndex) { + baseParentPose = accum; + } + } + + // AJT: TODO: check for parallel edge cases. + glm::vec3 d = glm::normalize(basePose.trans() - topPose.trans()); + glm::vec3 e = midPose.trans() - topPose.trans(); + glm::vec3 dHat = glm::dot(e, d) * d + topPose.trans(); + glm::vec3 eHat = glm::normalize(midPose.trans() - dHat); + float theta = acos(glm::dot(eHat, target.getPoleVector())); + glm::vec3 axis = glm::normalize(glm::cross(eHat, target.getPoleVector())); + glm::quat poleRot = glm::angleAxis(-theta, axis); + + if (debug) { + + const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); + const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f); + const vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f); + + AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); + + const float POLE_VECTOR_LEN = 10.0f; + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()), geomToWorldPose.xformPoint(topPose.trans()), GREEN); + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(dHat), geomToWorldPose.xformPoint(dHat + POLE_VECTOR_LEN * eHat), RED); + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(dHat), geomToWorldPose.xformPoint(dHat + POLE_VECTOR_LEN * target.getPoleVector()), BLUE); + } + + glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot() * glm::inverse(jointChainInfos[baseIndex].relRot); + jointChainInfos[baseIndex].relRot = glm::quat(); //newBaseRelRot * jointChainInfos[baseIndex].relRot; + + glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot() * glm::inverse(jointChainInfos[topIndex].relRot); + jointChainInfos[topIndex].relRot = glm::quat(); //newTopRelRot * jointChainInfos[topIndex].relRot; + } + } + } + + for (size_t i = 0; i < chainDepth; i++) { + _rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight); + _translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight); + } + if (debug) { - debugDrawIKChain(debugJointMap, context); + debugDrawIKChain(jointChainInfos, chainDepth, context); } } @@ -1338,13 +1418,14 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c } } -void AnimInverseKinematics::debugDrawIKChain(std::map& debugJointMap, const AnimContext& context) const { +void AnimInverseKinematics::debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, const AnimContext& context) const { AnimPoseVec poses = _relativePoses; // copy debug joint rotations into the relative poses - for (auto& debugJoint : debugJointMap) { - poses[debugJoint.first].rot() = debugJoint.second.relRot; - poses[debugJoint.first].trans() = debugJoint.second.relTrans; + for (size_t i = 0; i < numJointChainInfos; i++) { + const JointChainInfo& info = jointChainInfos[i]; + poses[info.jointIndex].rot() = info.relRot; + poses[info.jointIndex].trans() = info.relTrans; } // convert relative poses to absolute @@ -1360,11 +1441,11 @@ void AnimInverseKinematics::debugDrawIKChain(std::map& debugJoi // draw each pose for (int i = 0; i < (int)poses.size(); i++) { - - // only draw joints that are actually in debugJointMap, or their parents - auto iter = debugJointMap.find(i); - auto parentIter = debugJointMap.find(_skeleton->getParentIndex(i)); - if (iter != debugJointMap.end() || parentIter != debugJointMap.end()) { + int parentIndex = _skeleton->getParentIndex(i); + JointChainInfo* jointInfo = nullptr; + JointChainInfo* parentJointInfo = nullptr; + lookupJointChainInfo(jointChainInfos, numJointChainInfos, i, parentIndex, &jointInfo, &parentJointInfo); + if (jointInfo && parentJointInfo) { // transform local axes into world space. auto pose = poses[i]; @@ -1377,13 +1458,12 @@ void AnimInverseKinematics::debugDrawIKChain(std::map& debugJoi DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * zAxis, BLUE); // draw line to parent - int parentIndex = _skeleton->getParentIndex(i); if (parentIndex != -1) { glm::vec3 parentPos = transformPoint(geomToWorldMatrix, poses[parentIndex].trans()); glm::vec4 color = GRAY; // draw constrained joints with a RED link to their parent. - if (parentIter != debugJointMap.end() && parentIter->second.constrained) { + if (parentJointInfo->constrained) { color = RED; } DebugDraw::getInstance().drawRay(pos, parentPos, color); diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index cc919c1684..2ed96ed6ed 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -26,6 +26,14 @@ class RotationConstraint; class AnimInverseKinematics : public AnimNode { public: + struct JointChainInfo { + glm::quat relRot; + glm::vec3 relTrans; + float weight; + int jointIndex; + bool constrained; + }; + explicit AnimInverseKinematics(const QString& id); virtual ~AnimInverseKinematics() override; @@ -67,14 +75,7 @@ protected: void solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug); void solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug); virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override; - struct DebugJoint { - DebugJoint() : relRot(), constrained(false) {} - DebugJoint(const glm::quat& relRotIn, const glm::vec3& relTransIn, bool constrainedIn) : relRot(relRotIn), relTrans(relTransIn), constrained(constrainedIn) {} - glm::quat relRot; - glm::vec3 relTrans; - bool constrained; - }; - void debugDrawIKChain(std::map& debugJointMap, const AnimContext& context) const; + void debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, const AnimContext& context) const; void debugDrawRelativePoses(const AnimContext& context) const; void debugDrawConstraints(const AnimContext& context) const; void debugDrawSpineSplines(const AnimContext& context, const std::vector& targets) const; diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index 011175aedf..c476e9c9da 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -30,10 +30,14 @@ public: const glm::vec3& getTranslation() const { return _pose.trans(); } const glm::quat& getRotation() const { return _pose.rot(); } const AnimPose& getPose() const { return _pose; } + glm::vec3 getPoleVector() const { return _poleVector; } + int getPoleIndex() const { return _poleIndex; } int getIndex() const { return _index; } Type getType() const { return _type; } void setPose(const glm::quat& rotation, const glm::vec3& translation); + void setPoleVector(const glm::vec3& poleVector) { _poleVector = poleVector; } + void setPoleIndex(int poleIndex) { _poleIndex = poleIndex; } void setIndex(int index) { _index = index; } void setType(int); void setFlexCoefficients(size_t numFlexCoefficientsIn, const float* flexCoefficientsIn); @@ -46,8 +50,10 @@ public: private: AnimPose _pose; - int _index{-1}; - Type _type{Type::RotationAndPosition}; + glm::vec3 _poleVector; + int _poleIndex { -1 }; + int _index { -1 }; + Type _type { Type::RotationAndPosition }; float _weight; float _flexCoefficients[MAX_FLEX_COEFFICIENTS]; size_t _numFlexCoefficients; From 02f06d4d4eed054943916b152bb0cef2efdb1d40 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 6 Jun 2017 17:05:06 -0700 Subject: [PATCH 02/20] WIP pole vectors work.. but still have issues... --- .../animation/src/AnimInverseKinematics.cpp | 100 +++++++++++++----- 1 file changed, 72 insertions(+), 28 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 41dae120fd..1fdeaef483 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -169,7 +169,11 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: // AJT: HACK REMOVE manually set pole vector. if (targetVar.jointName == "RightHand") { - target.setPoleVector(glm::normalize(glm::vec3(-1, 0, 0))); + target.setPoleVector(glm::normalize(glm::vec3(-1, -1, 0))); + target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); + } + if (targetVar.jointName == "LeftHand") { + target.setPoleVector(glm::normalize(glm::vec3(1, -1, 0))); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } @@ -478,60 +482,99 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const if (target.getPoleIndex() != -1) { - int topIndex = target.getPoleIndex(); - int midIndex = _skeleton->getParentIndex(topIndex); - if (midIndex != -1) { - int baseIndex = _skeleton->getParentIndex(midIndex); - if (baseIndex != -1) { - int baseParentIndex = _skeleton->getParentIndex(baseIndex); + int topJointIndex = target.getPoleIndex(); + int midJointIndex = _skeleton->getParentIndex(topJointIndex); + if (midJointIndex != -1) { + int baseJointIndex = _skeleton->getParentIndex(midJointIndex); + if (baseJointIndex != -1) { + int baseParentJointIndex = _skeleton->getParentIndex(baseJointIndex); AnimPose topPose, midPose, basePose, baseParentPose; + int topChainIndex = -1, baseChainIndex = -1; AnimPose postAbsPoses[MAX_CHAIN_DEPTH]; AnimPose accum = absolutePoses[_hipsIndex]; for (int i = (int)chainDepth - 1; i >= 0; i--) { accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfos[i].relRot, jointChainInfos[i].relTrans); postAbsPoses[i] = accum; - if (jointChainInfos[i].jointIndex == topIndex) { + if (jointChainInfos[i].jointIndex == topJointIndex) { + topChainIndex = i; topPose = accum; } - if (jointChainInfos[i].jointIndex == midIndex) { + if (jointChainInfos[i].jointIndex == midJointIndex) { midPose = accum; } - if (jointChainInfos[i].jointIndex == baseIndex) { + if (jointChainInfos[i].jointIndex == baseJointIndex) { + baseChainIndex = i; basePose = accum; } - if (jointChainInfos[i].jointIndex == baseParentIndex) { + if (jointChainInfos[i].jointIndex == baseParentJointIndex) { baseParentPose = accum; } } - // AJT: TODO: check for parallel edge cases. - glm::vec3 d = glm::normalize(basePose.trans() - topPose.trans()); - glm::vec3 e = midPose.trans() - topPose.trans(); - glm::vec3 dHat = glm::dot(e, d) * d + topPose.trans(); - glm::vec3 eHat = glm::normalize(midPose.trans() - dHat); - float theta = acos(glm::dot(eHat, target.getPoleVector())); - glm::vec3 axis = glm::normalize(glm::cross(eHat, target.getPoleVector())); - glm::quat poleRot = glm::angleAxis(-theta, axis); + glm::quat poleRot = Quaternions::IDENTITY; + glm::vec3 d = basePose.trans() - topPose.trans(); + float dLen = glm::length(d); + if (dLen > EPSILON) { + glm::vec3 dUnit = d / dLen; + glm::vec3 e = midPose.trans() - basePose.trans(); + glm::vec3 p = target.getPoleVector(); + glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; + glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; + float eProjLen = glm::length(eProj); + float pProjLen = glm::length(pProj); + if (eProjLen > EPSILON && pProjLen > EPSILON) { + float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); + float theta = acos(dot); + glm::vec3 cross = glm::cross(eProj, pProj); + float crossLen = glm::length(cross); + if (crossLen > EPSILON) { + glm::vec3 axis = cross / crossLen; + poleRot = glm::angleAxis(theta, axis); + } + } + } if (debug) { - const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f); const vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f); + const vec4 YELLOW(1.0f, 1.0f, 0.0f, 1.0f); + const vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f); AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); + glm::vec3 dUnit = d / dLen; + glm::vec3 e = midPose.trans() - basePose.trans(); + glm::vec3 p = target.getPoleVector(); + glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; + glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; + float eProjLen = glm::length(eProj); + float pProjLen = glm::length(pProj); + const float POLE_VECTOR_LEN = 10.0f; - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()), geomToWorldPose.xformPoint(topPose.trans()), GREEN); - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(dHat), geomToWorldPose.xformPoint(dHat + POLE_VECTOR_LEN * eHat), RED); - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(dHat), geomToWorldPose.xformPoint(dHat + POLE_VECTOR_LEN * target.getPoleVector()), BLUE); + glm::vec3 midPoint = (basePose.trans() + topPose.trans()) * 0.5f; + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()), + geomToWorldPose.xformPoint(topPose.trans()), + YELLOW); + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(eProj)), + RED); + /* + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), + BLUE); + */ + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(pProj)), + GREEN); } - glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot() * glm::inverse(jointChainInfos[baseIndex].relRot); - jointChainInfos[baseIndex].relRot = glm::quat(); //newBaseRelRot * jointChainInfos[baseIndex].relRot; - glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot() * glm::inverse(jointChainInfos[topIndex].relRot); - jointChainInfos[topIndex].relRot = glm::quat(); //newTopRelRot * jointChainInfos[topIndex].relRot; + glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot(); + jointChainInfos[baseChainIndex].relRot = newBaseRelRot; + + glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot(); + jointChainInfos[topChainIndex].relRot = newTopRelRot; } } } @@ -1222,7 +1265,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z; - const float MIN_ELBOW_ANGLE = 0.05f; + const float MIN_ELBOW_ANGLE = 0.15f; // ~8.6 deg (ajt-rad-to-deg 0.15) const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f; glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y; @@ -1388,6 +1431,7 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c // convert relative poses to absolute _skeleton->convertRelativePosesToAbsolute(poses); + mat4 geomToWorldMatrix = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix(); const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); From bb45fe03888d1e2f60b7366038e059d545f61b70 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 7 Jun 2017 10:49:27 -0700 Subject: [PATCH 03/20] WIP, straight arm still rotates.. --- libraries/animation/src/AnimInverseKinematics.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 1fdeaef483..09cee28794 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -169,11 +169,11 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: // AJT: HACK REMOVE manually set pole vector. if (targetVar.jointName == "RightHand") { - target.setPoleVector(glm::normalize(glm::vec3(-1, -1, 0))); + target.setPoleVector(glm::normalize(glm::vec3(-1, -2, -1))); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } if (targetVar.jointName == "LeftHand") { - target.setPoleVector(glm::normalize(glm::vec3(1, -1, 0))); + target.setPoleVector(glm::normalize(glm::vec3(1, -2, -1))); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } @@ -522,7 +522,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; float eProjLen = glm::length(eProj); float pProjLen = glm::length(pProj); - if (eProjLen > EPSILON && pProjLen > EPSILON) { + const float MIN_E_PROJ_LEN = 0.2f; // cm + if (eProjLen > MIN_E_PROJ_LEN && pProjLen > EPSILON) { float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); float theta = acos(dot); glm::vec3 cross = glm::cross(eProj, pProj); @@ -1265,7 +1266,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z; - const float MIN_ELBOW_ANGLE = 0.15f; // ~8.6 deg (ajt-rad-to-deg 0.15) + const float MIN_ELBOW_ANGLE = 0.05f; // ~2.8 deg (ajt-rad-to-deg 0.05) const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f; glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y; From e8ca1a30602f050b121e7fa47d6e4619f658948d Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 9 Jun 2017 18:06:56 -0700 Subject: [PATCH 04/20] WIP: added magnitude to damping rotation near singularities. Also knee pole constraints don't work.. why? --- .../animation/src/AnimInverseKinematics.cpp | 85 ++++++++++++++----- 1 file changed, 64 insertions(+), 21 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 09cee28794..ff9a6d0d6a 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -42,6 +42,10 @@ static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointCha } } +float easeOutExpo(float t) { + return 1.0f - powf(2, -10.0f * t); +} + AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn) : jointName(jointNameIn), @@ -166,14 +170,50 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setIndex(targetVar.jointIndex); target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); + target.setPoleIndex(-1); + /* // AJT: HACK REMOVE manually set pole vector. if (targetVar.jointName == "RightHand") { - target.setPoleVector(glm::normalize(glm::vec3(-1, -2, -1))); + int armJointIndex = _skeleton->nameToJointIndex("RightArm"); + glm::vec3 armPosition = _skeleton->getAbsolutePose(armJointIndex, _relativePoses).trans(); + glm::vec3 d = glm::normalize(translation - armPosition); + + // project hand y-axis onto d. + glm::vec3 handY = rotation * Vectors::UNIT_Y; + glm::vec3 handYProj = handY - glm::dot(handY, d) * d; + + // project negative pole vector on to d. + glm::vec3 p = glm::normalize(glm::vec3(-1, -3, -3)); + glm::vec3 pProj = p - glm::dot(p, d) * d; + + // compute a rotation around d that will bring the pProj to to yProj + float theta = acosf(glm::dot(glm::normalize(-pProj), glm::normalize(handYProj))); + glm::vec3 axis = glm::normalize(glm::cross(-pProj, handYProj)); + + if (glm::dot(axis, d) < 0.0f) { + // as eProjLen and pProjLen become orthognal to d, reduce the amount of rotation. + float magnitude = easeOutExpo(glm::min(glm::length(handYProj), glm::length(pProj))); + glm::quat poleAdjustRot = angleAxis(magnitude * (theta / 4.0f), axis); + target.setPoleVector(poleAdjustRot * p); + } else { + target.setPoleVector(p); + } target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } if (targetVar.jointName == "LeftHand") { - target.setPoleVector(glm::normalize(glm::vec3(1, -2, -1))); + target.setPoleVector(glm::normalize(glm::vec3(1, -3, -3))); + target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); + } + */ + + if (targetVar.jointName == "LeftFoot") { + glm::vec3 footY = rotation * Vectors::UNIT_Y; + target.setPoleVector(footY); + target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); + } else if (targetVar.jointName == "RightFoot") { + glm::vec3 footY = rotation * Vectors::UNIT_Y; + target.setPoleVector(footY); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } @@ -524,13 +564,17 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const float pProjLen = glm::length(pProj); const float MIN_E_PROJ_LEN = 0.2f; // cm if (eProjLen > MIN_E_PROJ_LEN && pProjLen > EPSILON) { + + // as eProjLen and pProjLen become orthognal to d, reduce the amount of rotation. + float magnitude = easeOutExpo(glm::min(eProjLen, pProjLen)); + float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); - float theta = acos(dot); + float theta = acosf(dot); glm::vec3 cross = glm::cross(eProj, pProj); float crossLen = glm::length(cross); if (crossLen > EPSILON) { glm::vec3 axis = cross / crossLen; - poleRot = glm::angleAxis(theta, axis); + poleRot = glm::angleAxis(magnitude * theta, axis); } } } @@ -552,25 +596,23 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const float eProjLen = glm::length(eProj); float pProjLen = glm::length(pProj); - const float POLE_VECTOR_LEN = 10.0f; + const float PROJ_VECTOR_LEN = 10.0f; + const float POLE_VECTOR_LEN = 100.0f; glm::vec3 midPoint = (basePose.trans() + topPose.trans()) * 0.5f; DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()), geomToWorldPose.xformPoint(topPose.trans()), YELLOW); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(eProj)), + geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(eProj)), RED); - /* + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(pProj)), + GREEN); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), BLUE); - */ - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(pProj)), - GREEN); } - glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot(); jointChainInfos[baseChainIndex].relRot = newBaseRelRot; @@ -672,7 +714,8 @@ const std::vector* AnimInverseKinematics void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug) { - std::map debugJointMap; + const size_t MAX_CHAIN_DEPTH = 30; + JointChainInfo jointChainInfos[MAX_CHAIN_DEPTH]; const int baseIndex = _hipsIndex; @@ -732,7 +775,6 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co ::blend(1, &absolutePoses[splineJointInfo.jointIndex], &desiredAbsPose, target.getFlexCoefficient(i), &flexedAbsPose); AnimPose relPose = parentAbsPose.inverse() * flexedAbsPose; - _rotationAccumulators[splineJointInfo.jointIndex].add(relPose.rot(), target.getWeight()); bool constrained = false; if (splineJointInfo.jointIndex != _hipsIndex) { @@ -756,18 +798,19 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co } } - _translationAccumulators[splineJointInfo.jointIndex].add(relPose.trans(), target.getWeight()); - - if (debug) { - debugJointMap[splineJointInfo.jointIndex] = DebugJoint(relPose.rot(), relPose.trans(), constrained); - } + jointChainInfos[i] = { relPose.rot(), relPose.trans(), target.getWeight(), splineJointInfo.jointIndex, constrained }; parentAbsPose = flexedAbsPose; } } + for (size_t i = 0; i < splineJointInfoVec->size(); i++) { + _rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight); + _translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight); + } + if (debug) { - debugDrawIKChain(debugJointMap, context); + debugDrawIKChain(jointChainInfos, splineJointInfoVec->size(), context); } } @@ -1611,7 +1654,7 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con glm::vec3 worldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * swungAxis); glm::vec3 swingTip = pos + SWING_LENGTH * worldSwungAxis; - float prevPhi = acos(swingTwistConstraint->getMinDots()[j]); + float prevPhi = acosf(swingTwistConstraint->getMinDots()[j]); float prevTheta = theta - D_THETA; glm::vec3 prevSwungAxis = sphericalToCartesian(prevPhi, prevTheta - PI_2); glm::vec3 prevWorldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * prevSwungAxis); From 0cde22d937175822583236e4f71787eba50d6fa5 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 12 Jun 2017 12:13:03 -0700 Subject: [PATCH 05/20] Bug fix for pole constraint on legs --- libraries/animation/src/AnimInverseKinematics.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index ff9a6d0d6a..7de6626cd5 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -528,10 +528,11 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const int baseJointIndex = _skeleton->getParentIndex(midJointIndex); if (baseJointIndex != -1) { int baseParentJointIndex = _skeleton->getParentIndex(baseJointIndex); - AnimPose topPose, midPose, basePose, baseParentPose; + AnimPose topPose, midPose, basePose; int topChainIndex = -1, baseChainIndex = -1; AnimPose postAbsPoses[MAX_CHAIN_DEPTH]; AnimPose accum = absolutePoses[_hipsIndex]; + AnimPose baseParentPose = absolutePoses[_hipsIndex]; for (int i = (int)chainDepth - 1; i >= 0; i--) { accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfos[i].relRot, jointChainInfos[i].relTrans); postAbsPoses[i] = accum; @@ -1341,7 +1342,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment const float MIN_KNEE_ANGLE = 0.097f; // ~5 deg - const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f; + const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f; // 157.5 deg glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y; glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y; From 7521d6124e3502ebf4097489d304eccabfccd7bd Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 12 Jun 2017 17:23:23 -0700 Subject: [PATCH 06/20] WIP: added blend between hips and foot for knee pole vector --- .../animation/src/AnimInverseKinematics.cpp | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 7de6626cd5..0ff236d1b1 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -161,9 +161,9 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: IKTarget target; target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition)); if (target.getType() != IKTarget::Type::Unknown) { - AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses); - glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, defaultPose.rot()); - glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, defaultPose.trans()); + AnimPose absPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses); + glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, absPose.rot()); + glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, absPose.trans()); float weight = animVars.lookup(targetVar.weightVar, targetVar.weight); target.setPose(rotation, translation); @@ -207,13 +207,24 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: } */ - if (targetVar.jointName == "LeftFoot") { - glm::vec3 footY = rotation * Vectors::UNIT_Y; - target.setPoleVector(footY); - target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); - } else if (targetVar.jointName == "RightFoot") { - glm::vec3 footY = rotation * Vectors::UNIT_Y; - target.setPoleVector(footY); + if (targetVar.jointName == "LeftFoot" || targetVar.jointName == "RightFoot") { + + // compute the forward direction of the foot. + AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(targetVar.jointIndex); + glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; + glm::vec3 footForward = rotation * localForward; + + // compute the forward direction of the hips. + glm::quat hipsRotation = _skeleton->getAbsolutePose(_hipsIndex, _relativePoses).rot(); + glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; + + // blend between the hips and the foot. + const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; + glm::vec3 poleVector = glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); + + // TODO: smooth toward desired pole vector from previous pole vector... to reduce jitter + + target.setPoleVector(poleVector); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } From f20c03fa6e033bad78278c827137ceba05f7d72d Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 13 Jun 2017 15:36:05 -0700 Subject: [PATCH 07/20] Pole vectors can be controlled via anim vars. --- .../resources/avatar/avatar-animation.json | 8 +- .../animation/src/AnimInverseKinematics.cpp | 73 ++++------------- .../animation/src/AnimInverseKinematics.h | 8 +- libraries/animation/src/AnimNodeLoader.cpp | 4 +- libraries/animation/src/Rig.cpp | 80 ++++++++++++++----- libraries/animation/src/Rig.h | 12 ++- libraries/render-utils/src/Model.cpp | 4 - libraries/render-utils/src/Model.h | 1 - 8 files changed, 96 insertions(+), 94 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 1412b45968..914fe8ba69 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -86,7 +86,9 @@ "typeVar": "rightFootType", "weightVar": "rightFootWeight", "weight": 1.0, - "flexCoefficients": [1, 0.45, 0.45] + "flexCoefficients": [1, 0.45, 0.45], + "poleJointNameVar": "rightFootPoleJointName", + "poleVectorVar": "rightFootPoleVector" }, { "jointName": "LeftFoot", @@ -95,7 +97,9 @@ "typeVar": "leftFootType", "weightVar": "leftFootWeight", "weight": 1.0, - "flexCoefficients": [1, 0.45, 0.45] + "flexCoefficients": [1, 0.45, 0.45], + "poleJointNameVar": "leftFootPoleJointName", + "poleVectorVar": "leftFootPoleVector" }, { "jointName": "Spine2", diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 0ff236d1b1..57fa6fd1e3 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -47,12 +47,15 @@ float easeOutExpo(float t) { } AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, - const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn) : + const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, + const QString& poleJointNameVarIn, const QString& poleVectorVarIn) : jointName(jointNameIn), positionVar(positionVarIn), rotationVar(rotationVarIn), typeVar(typeVarIn), weightVar(weightVarIn), + poleJointNameVar(poleJointNameVarIn), + poleVectorVar(poleVectorVarIn), weight(weightIn), numFlexCoefficients(flexCoefficientsIn.size()), jointIndex(-1) @@ -69,6 +72,8 @@ AnimInverseKinematics::IKTargetVar::IKTargetVar(const IKTargetVar& orig) : rotationVar(orig.rotationVar), typeVar(orig.typeVar), weightVar(orig.weightVar), + poleJointNameVar(orig.poleJointNameVar), + poleVectorVar(orig.poleVectorVar), weight(orig.weight), numFlexCoefficients(orig.numFlexCoefficients), jointIndex(orig.jointIndex) @@ -122,8 +127,9 @@ void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) con } void AnimInverseKinematics::setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, - const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients) { - IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients); + const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients, + const QString& poleJointNameVar, const QString& poleVectorVar) { + IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleJointNameVar, poleVectorVar); // if there are dups, last one wins. bool found = false; @@ -170,63 +176,14 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setIndex(targetVar.jointIndex); target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); - target.setPoleIndex(-1); - /* - // AJT: HACK REMOVE manually set pole vector. - if (targetVar.jointName == "RightHand") { - int armJointIndex = _skeleton->nameToJointIndex("RightArm"); - glm::vec3 armPosition = _skeleton->getAbsolutePose(armJointIndex, _relativePoses).trans(); - glm::vec3 d = glm::normalize(translation - armPosition); + // AJT: TODO: cache the pole joint index. + QString poleJointName = animVars.lookup(targetVar.poleJointNameVar, QString("")); + int poleJointIndex = _skeleton->nameToJointIndex(poleJointName); + target.setPoleIndex(poleJointIndex); - // project hand y-axis onto d. - glm::vec3 handY = rotation * Vectors::UNIT_Y; - glm::vec3 handYProj = handY - glm::dot(handY, d) * d; - - // project negative pole vector on to d. - glm::vec3 p = glm::normalize(glm::vec3(-1, -3, -3)); - glm::vec3 pProj = p - glm::dot(p, d) * d; - - // compute a rotation around d that will bring the pProj to to yProj - float theta = acosf(glm::dot(glm::normalize(-pProj), glm::normalize(handYProj))); - glm::vec3 axis = glm::normalize(glm::cross(-pProj, handYProj)); - - if (glm::dot(axis, d) < 0.0f) { - // as eProjLen and pProjLen become orthognal to d, reduce the amount of rotation. - float magnitude = easeOutExpo(glm::min(glm::length(handYProj), glm::length(pProj))); - glm::quat poleAdjustRot = angleAxis(magnitude * (theta / 4.0f), axis); - target.setPoleVector(poleAdjustRot * p); - } else { - target.setPoleVector(p); - } - target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); - } - if (targetVar.jointName == "LeftHand") { - target.setPoleVector(glm::normalize(glm::vec3(1, -3, -3))); - target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); - } - */ - - if (targetVar.jointName == "LeftFoot" || targetVar.jointName == "RightFoot") { - - // compute the forward direction of the foot. - AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(targetVar.jointIndex); - glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; - glm::vec3 footForward = rotation * localForward; - - // compute the forward direction of the hips. - glm::quat hipsRotation = _skeleton->getAbsolutePose(_hipsIndex, _relativePoses).rot(); - glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; - - // blend between the hips and the foot. - const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; - glm::vec3 poleVector = glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); - - // TODO: smooth toward desired pole vector from previous pole vector... to reduce jitter - - target.setPoleVector(poleVector); - target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); - } + glm::vec3 poleVector = animVars.lookupRigToGeometryVector(targetVar.poleVectorVar, Vectors::UNIT_Z); + target.setPoleVector(glm::normalize(poleVector)); targets.push_back(target); diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 2ed96ed6ed..946c23883e 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -42,7 +42,8 @@ public: void computeAbsolutePoses(AnimPoseVec& absolutePoses) const; void setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, - const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients); + const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients, + const QString& poleJointNameVar, const QString& poleVectorVar); virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) override; virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; @@ -108,7 +109,8 @@ protected: enum FlexCoefficients { MAX_FLEX_COEFFICIENTS = 10 }; struct IKTargetVar { IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, - const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn); + const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, + const QString& poleJointNameVar, const QString& poleVectorVar); IKTargetVar(const IKTargetVar& orig); QString jointName; @@ -116,6 +118,8 @@ protected: QString rotationVar; QString typeVar; QString weightVar; + QString poleJointNameVar; + QString poleVectorVar; float weight; float flexCoefficients[MAX_FLEX_COEFFICIENTS]; size_t numFlexCoefficients; diff --git a/libraries/animation/src/AnimNodeLoader.cpp b/libraries/animation/src/AnimNodeLoader.cpp index 44ed8c6053..c4a149a35b 100644 --- a/libraries/animation/src/AnimNodeLoader.cpp +++ b/libraries/animation/src/AnimNodeLoader.cpp @@ -479,6 +479,8 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS READ_OPTIONAL_STRING(typeVar, targetObj); READ_OPTIONAL_STRING(weightVar, targetObj); READ_OPTIONAL_FLOAT(weight, targetObj, 1.0f); + READ_OPTIONAL_STRING(poleJointNameVar, targetObj); + READ_OPTIONAL_STRING(poleVectorVar, targetObj); auto flexCoefficientsValue = targetObj.value("flexCoefficients"); if (!flexCoefficientsValue.isArray()) { @@ -491,7 +493,7 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS flexCoefficients.push_back((float)value.toDouble()); } - node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients); + node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleJointNameVar, poleVectorVar); }; READ_OPTIONAL_STRING(solutionSource, jsonObj); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 20a2aab2b6..2966e882d2 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -479,12 +479,6 @@ bool Rig::getAbsoluteJointPoseInRigFrame(int jointIndex, AnimPose& returnPose) c } } -bool Rig::getJointCombinedRotation(int jointIndex, glm::quat& result, const glm::quat& rotation) const { - // AJT: TODO: used by attachments - ASSERT(false); - return false; -} - void Rig::calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const { ASSERT(referenceSpeeds.size() > 0); @@ -950,6 +944,7 @@ void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, cons // evaluate the animation AnimNode::Triggers triggersOut; + _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, context, deltaTime, triggersOut); if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) { // animations haven't fully loaded yet. @@ -1145,6 +1140,21 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } +glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { + + AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex); + glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; + glm::vec3 footForward = footTargetOrientation * localForward; + + // compute the forward direction of the hips. + glm::quat hipsRotation = _externalPoseSet._absolutePoses[hipsIndex].rot(); + glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; + + // blend between the hips and the foot. + const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; + return glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); +} + void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt) { if (_animSkeleton && _animNode) { const float HAND_RADIUS = 0.05f; @@ -1255,16 +1265,46 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _animVars.set("leftFootPosition", params.leftFootPosition); _animVars.set("leftFootRotation", params.leftFootOrientation); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, params.leftFootOrientation, hipsIndex); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftFootPoleVectorValid) { + _prevLeftFootPoleVectorValid = true; + _prevLeftFootPoleVector = poleVector; + } + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + _prevLeftFootPoleVector = lerp(poleVector, _prevLeftFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector); + _animVars.set("leftFootPoleJointName", QString("LeftFoot")); + } else { _animVars.unset("leftFootPosition"); _animVars.unset("leftFootRotation"); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + _prevLeftFootPoleVectorValid = false; } if (params.isRightFootEnabled) { _animVars.set("rightFootPosition", params.rightFootPosition); _animVars.set("rightFootRotation", params.rightFootOrientation); _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, params.rightFootOrientation, hipsIndex); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightFootPoleVectorValid) { + _prevRightFootPoleVectorValid = true; + _prevRightFootPoleVector = poleVector; + } + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + _prevRightFootPoleVector = lerp(poleVector, _prevRightFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("rightFootPoleVector", _prevRightFootPoleVector); + _animVars.set("rightFootPoleJointName", QString("RightFoot")); } else { _animVars.unset("rightFootPosition"); _animVars.unset("rightFootRotation"); @@ -1455,22 +1495,18 @@ void Rig::computeAvatarBoundingCapsule( AnimInverseKinematics ikNode("boundingShape"); ikNode.setSkeleton(_animSkeleton); - ikNode.setTargetVars("LeftHand", - "leftHandPosition", - "leftHandRotation", - "leftHandType", "leftHandWeight", 1.0f, {}); - ikNode.setTargetVars("RightHand", - "rightHandPosition", - "rightHandRotation", - "rightHandType", "rightHandWeight", 1.0f, {}); - ikNode.setTargetVars("LeftFoot", - "leftFootPosition", - "leftFootRotation", - "leftFootType", "leftFootWeight", 1.0f, {}); - ikNode.setTargetVars("RightFoot", - "rightFootPosition", - "rightFootRotation", - "rightFootType", "rightFootWeight", 1.0f, {}); + ikNode.setTargetVars("LeftHand", "leftHandPosition", "leftHandRotation", + "leftHandType", "leftHandWeight", 1.0f, {}, + "leftHandPoleJointName", "leftHandPoleVector"); + ikNode.setTargetVars("RightHand", "rightHandPosition", "rightHandRotation", + "rightHandType", "rightHandWeight", 1.0f, {}, + "rightHandPoleJointName", "rightHandPoleVector"); + ikNode.setTargetVars("LeftFoot", "leftFootPosition", "leftFootRotation", + "leftFootType", "leftFootWeight", 1.0f, {}, + "leftFootPoleJointName", "leftFootPoleVector"); + ikNode.setTargetVars("RightFoot", "rightFootPosition", "rightFootRotation", + "rightFootType", "rightFootWeight", 1.0f, {}, + "rightFootPoleJointName", "rightFootPoleVector"); AnimPose geometryToRig = _modelOffset * _geometryOffset; diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 994bd4b074..4fc5f76146 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -153,9 +153,6 @@ public: bool getAbsoluteJointTranslationInRigFrame(int jointIndex, glm::vec3& translation) const; bool getAbsoluteJointPoseInRigFrame(int jointIndex, AnimPose& returnPose) const; - // legacy - bool getJointCombinedRotation(int jointIndex, glm::quat& result, const glm::quat& rotation) const; - // rig space glm::mat4 getJointTransform(int jointIndex) const; @@ -252,6 +249,8 @@ protected: void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; + glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; + AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) AnimPose _invGeometryOffset; @@ -347,7 +346,6 @@ protected: bool _enableDebugDrawIKConstraints { false }; bool _enableDebugDrawIKChains { false }; -private: QMap _stateHandlers; int _nextStateHandlerId { 0 }; QMutex _stateMutex; @@ -358,6 +356,12 @@ private: float _rightHandRelaxDuration { 0.0f }; AnimPose _lastLeftHandControlledPose; AnimPose _lastRightHandControlledPose; + + glm::vec3 _prevRightFootPoleVector = { Vectors::UNIT_Z }; + bool _prevRightFootPoleVectorValid = { false }; + + glm::vec3 _prevLeftFootPoleVector = { Vectors::UNIT_Z }; + bool _prevLeftFootPoleVectorValid = { false }; }; #endif /* defined(__hifi__Rig__) */ diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index 447d0e37bd..8953ea94f0 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -866,10 +866,6 @@ bool Model::getRelativeDefaultJointTranslation(int jointIndex, glm::vec3& transl return _rig.getRelativeDefaultJointTranslation(jointIndex, translationOut); } -bool Model::getJointCombinedRotation(int jointIndex, glm::quat& rotation) const { - return _rig.getJointCombinedRotation(jointIndex, rotation, _rotation); -} - QStringList Model::getJointNames() const { if (QThread::currentThread() != thread()) { QStringList result; diff --git a/libraries/render-utils/src/Model.h b/libraries/render-utils/src/Model.h index 53d446d306..a0da8a7a8c 100644 --- a/libraries/render-utils/src/Model.h +++ b/libraries/render-utils/src/Model.h @@ -176,7 +176,6 @@ public: int getJointStateCount() const { return (int)_rig.getJointStateCount(); } bool getJointPositionInWorldFrame(int jointIndex, glm::vec3& position) const; bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const; - bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const; /// \param jointIndex index of joint in model structure /// \param rotation[out] rotation of joint in model-frame From 10f94c2d605e8d7eddf38b57e82ba68afe30022f Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 14 Jun 2017 11:35:53 -0700 Subject: [PATCH 08/20] Fixed pole vectors on straight limbs by using reference vector --- .../resources/avatar/avatar-animation.json | 6 ++- .../animation/src/AnimInverseKinematics.cpp | 37 ++++++++++--------- .../animation/src/AnimInverseKinematics.h | 7 ++-- libraries/animation/src/AnimNodeLoader.cpp | 5 ++- libraries/animation/src/IKTarget.h | 9 +++-- libraries/animation/src/Rig.cpp | 21 ++++++----- 6 files changed, 48 insertions(+), 37 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 914fe8ba69..c9552a3ead 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -87,7 +87,8 @@ "weightVar": "rightFootWeight", "weight": 1.0, "flexCoefficients": [1, 0.45, 0.45], - "poleJointNameVar": "rightFootPoleJointName", + "poleVectorEnabledVar": "rightFootPoleVectorEnabled", + "poleReferenceVectorVar": "rightFootPoleReferenceVector", "poleVectorVar": "rightFootPoleVector" }, { @@ -98,7 +99,8 @@ "weightVar": "leftFootWeight", "weight": 1.0, "flexCoefficients": [1, 0.45, 0.45], - "poleJointNameVar": "leftFootPoleJointName", + "poleVectorEnabledVar": "leftFootPoleVectorEnabled", + "poleReferenceVectorVar": "leftFootPoleReferenceVector", "poleVectorVar": "leftFootPoleVector" }, { diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 57fa6fd1e3..2a493a96ab 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -48,13 +48,14 @@ float easeOutExpo(float t) { AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, - const QString& poleJointNameVarIn, const QString& poleVectorVarIn) : + const QString& poleVectorEnabledVarIn, const QString& poleReferenceVectorVarIn, const QString& poleVectorVarIn) : jointName(jointNameIn), positionVar(positionVarIn), rotationVar(rotationVarIn), typeVar(typeVarIn), weightVar(weightVarIn), - poleJointNameVar(poleJointNameVarIn), + poleVectorEnabledVar(poleVectorEnabledVarIn), + poleReferenceVectorVar(poleReferenceVectorVarIn), poleVectorVar(poleVectorVarIn), weight(weightIn), numFlexCoefficients(flexCoefficientsIn.size()), @@ -72,7 +73,8 @@ AnimInverseKinematics::IKTargetVar::IKTargetVar(const IKTargetVar& orig) : rotationVar(orig.rotationVar), typeVar(orig.typeVar), weightVar(orig.weightVar), - poleJointNameVar(orig.poleJointNameVar), + poleVectorEnabledVar(orig.poleVectorEnabledVar), + poleReferenceVectorVar(orig.poleReferenceVectorVar), poleVectorVar(orig.poleVectorVar), weight(orig.weight), numFlexCoefficients(orig.numFlexCoefficients), @@ -128,8 +130,8 @@ void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) con void AnimInverseKinematics::setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients, - const QString& poleJointNameVar, const QString& poleVectorVar) { - IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleJointNameVar, poleVectorVar); + const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar) { + IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleVectorEnabledVar, poleReferenceVectorVar, poleVectorVar); // if there are dups, last one wins. bool found = false; @@ -177,14 +179,15 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); - // AJT: TODO: cache the pole joint index. - QString poleJointName = animVars.lookup(targetVar.poleJointNameVar, QString("")); - int poleJointIndex = _skeleton->nameToJointIndex(poleJointName); - target.setPoleIndex(poleJointIndex); + bool poleVectorEnabled = animVars.lookup(targetVar.poleVectorEnabledVar, false); + target.setPoleVectorEnabled(poleVectorEnabled); glm::vec3 poleVector = animVars.lookupRigToGeometryVector(targetVar.poleVectorVar, Vectors::UNIT_Z); target.setPoleVector(glm::normalize(poleVector)); + glm::vec3 poleReferenceVector = animVars.lookupRigToGeometryVector(targetVar.poleReferenceVectorVar, Vectors::UNIT_Z); + target.setPoleReferenceVector(glm::normalize(poleReferenceVector)); + targets.push_back(target); if (targetVar.jointIndex > _maxTargetIndex) { @@ -488,9 +491,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const chainDepth++; } - if (target.getPoleIndex() != -1) { - - int topJointIndex = target.getPoleIndex(); + if (target.getPoleVectorEnabled()) { + int topJointIndex = target.getIndex(); int midJointIndex = _skeleton->getParentIndex(topJointIndex); if (midJointIndex != -1) { int baseJointIndex = _skeleton->getParentIndex(midJointIndex); @@ -525,17 +527,16 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const float dLen = glm::length(d); if (dLen > EPSILON) { glm::vec3 dUnit = d / dLen; - glm::vec3 e = midPose.trans() - basePose.trans(); + glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; float eProjLen = glm::length(eProj); float pProjLen = glm::length(pProj); - const float MIN_E_PROJ_LEN = 0.2f; // cm - if (eProjLen > MIN_E_PROJ_LEN && pProjLen > EPSILON) { + if (eProjLen > EPSILON && pProjLen > EPSILON) { - // as eProjLen and pProjLen become orthognal to d, reduce the amount of rotation. - float magnitude = easeOutExpo(glm::min(eProjLen, pProjLen)); + // as pProjLen become orthognal to d, reduce the amount of rotation. + float magnitude = easeOutExpo(pProjLen); float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); float theta = acosf(dot); @@ -558,7 +559,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); glm::vec3 dUnit = d / dLen; - glm::vec3 e = midPose.trans() - basePose.trans(); + glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 946c23883e..016317aee3 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -43,7 +43,7 @@ public: void setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients, - const QString& poleJointNameVar, const QString& poleVectorVar); + const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar); virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) override; virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; @@ -110,7 +110,7 @@ protected: struct IKTargetVar { IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, - const QString& poleJointNameVar, const QString& poleVectorVar); + const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar); IKTargetVar(const IKTargetVar& orig); QString jointName; @@ -118,7 +118,8 @@ protected: QString rotationVar; QString typeVar; QString weightVar; - QString poleJointNameVar; + QString poleVectorEnabledVar; + QString poleReferenceVectorVar; QString poleVectorVar; float weight; float flexCoefficients[MAX_FLEX_COEFFICIENTS]; diff --git a/libraries/animation/src/AnimNodeLoader.cpp b/libraries/animation/src/AnimNodeLoader.cpp index c4a149a35b..2a1978127d 100644 --- a/libraries/animation/src/AnimNodeLoader.cpp +++ b/libraries/animation/src/AnimNodeLoader.cpp @@ -479,7 +479,8 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS READ_OPTIONAL_STRING(typeVar, targetObj); READ_OPTIONAL_STRING(weightVar, targetObj); READ_OPTIONAL_FLOAT(weight, targetObj, 1.0f); - READ_OPTIONAL_STRING(poleJointNameVar, targetObj); + READ_OPTIONAL_STRING(poleVectorEnabledVar, targetObj); + READ_OPTIONAL_STRING(poleReferenceVectorVar, targetObj); READ_OPTIONAL_STRING(poleVectorVar, targetObj); auto flexCoefficientsValue = targetObj.value("flexCoefficients"); @@ -493,7 +494,7 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS flexCoefficients.push_back((float)value.toDouble()); } - node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleJointNameVar, poleVectorVar); + node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleVectorEnabledVar, poleReferenceVectorVar, poleVectorVar); }; READ_OPTIONAL_STRING(solutionSource, jsonObj); diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index c476e9c9da..5567539659 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -31,13 +31,15 @@ public: const glm::quat& getRotation() const { return _pose.rot(); } const AnimPose& getPose() const { return _pose; } glm::vec3 getPoleVector() const { return _poleVector; } - int getPoleIndex() const { return _poleIndex; } + glm::vec3 getPoleReferenceVector() const { return _poleReferenceVector; } + bool getPoleVectorEnabled() const { return _poleVectorEnabled; } int getIndex() const { return _index; } Type getType() const { return _type; } void setPose(const glm::quat& rotation, const glm::vec3& translation); void setPoleVector(const glm::vec3& poleVector) { _poleVector = poleVector; } - void setPoleIndex(int poleIndex) { _poleIndex = poleIndex; } + void setPoleReferenceVector(const glm::vec3& poleReferenceVector) { _poleReferenceVector = poleReferenceVector; } + void setPoleVectorEnabled(bool poleVectorEnabled) { _poleVectorEnabled = poleVectorEnabled; } void setIndex(int index) { _index = index; } void setType(int); void setFlexCoefficients(size_t numFlexCoefficientsIn, const float* flexCoefficientsIn); @@ -51,7 +53,8 @@ public: private: AnimPose _pose; glm::vec3 _poleVector; - int _poleIndex { -1 }; + glm::vec3 _poleReferenceVector; + bool _poleVectorEnabled { false }; int _index { -1 }; Type _type { Type::RotationAndPosition }; float _weight; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 2966e882d2..1e6965faf5 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1261,6 +1261,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f } } + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + if (params.isLeftFootEnabled) { _animVars.set("leftFootPosition", params.leftFootPosition); _animVars.set("leftFootRotation", params.leftFootOrientation); @@ -1274,16 +1276,16 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevLeftFootPoleVectorValid = true; _prevLeftFootPoleVector = poleVector; } - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; _prevLeftFootPoleVector = lerp(poleVector, _prevLeftFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + _animVars.set("leftFootPoleVectorEnabled", true); + _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z); _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector); - _animVars.set("leftFootPoleJointName", QString("LeftFoot")); - } else { _animVars.unset("leftFootPosition"); _animVars.unset("leftFootRotation"); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("leftFootPoleVectorEnabled", false); _prevLeftFootPoleVectorValid = false; } @@ -1300,14 +1302,15 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevRightFootPoleVectorValid = true; _prevRightFootPoleVector = poleVector; } - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; _prevRightFootPoleVector = lerp(poleVector, _prevRightFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + _animVars.set("rightFootPoleVectorEnabled", true); + _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z); _animVars.set("rightFootPoleVector", _prevRightFootPoleVector); - _animVars.set("rightFootPoleJointName", QString("RightFoot")); } else { _animVars.unset("rightFootPosition"); _animVars.unset("rightFootRotation"); + _animVars.set("rightFootPoleVectorEnabled", false); _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); } } @@ -1497,16 +1500,16 @@ void Rig::computeAvatarBoundingCapsule( ikNode.setTargetVars("LeftHand", "leftHandPosition", "leftHandRotation", "leftHandType", "leftHandWeight", 1.0f, {}, - "leftHandPoleJointName", "leftHandPoleVector"); + QString(), QString(), QString()); ikNode.setTargetVars("RightHand", "rightHandPosition", "rightHandRotation", "rightHandType", "rightHandWeight", 1.0f, {}, - "rightHandPoleJointName", "rightHandPoleVector"); + QString(), QString(), QString()); ikNode.setTargetVars("LeftFoot", "leftFootPosition", "leftFootRotation", "leftFootType", "leftFootWeight", 1.0f, {}, - "leftFootPoleJointName", "leftFootPoleVector"); + QString(), QString(), QString()); ikNode.setTargetVars("RightFoot", "rightFootPosition", "rightFootRotation", "rightFootType", "rightFootWeight", 1.0f, {}, - "rightFootPoleJointName", "rightFootPoleVector"); + QString(), QString(), QString()); AnimPose geometryToRig = _modelOffset * _geometryOffset; From fac21033e738890368ce485cc90f192d7d86c9bd Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 14 Jun 2017 11:53:54 -0700 Subject: [PATCH 09/20] Fixed matthew sitting pose by opening up UpLeg twist constraint to +- 90 degrees. --- libraries/animation/src/AnimInverseKinematics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 2a493a96ab..eb256497b7 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1137,7 +1137,7 @@ void AnimInverseKinematics::initConstraints() { } else if (0 == baseName.compare("UpLeg", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f); + stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f); std::vector swungDirections; float deltaTheta = PI / 4.0f; From cdfba52488f5cea72a71968d5721ccb1fca5c819 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 15 Jun 2017 15:02:42 -0700 Subject: [PATCH 10/20] precondition initial solution before solving to reduce limb locking. --- .../animation/src/AnimInverseKinematics.cpp | 54 +++++++++++++++++-- .../animation/src/AnimInverseKinematics.h | 1 + libraries/animation/src/AnimSkeleton.cpp | 6 +-- libraries/animation/src/AnimSkeleton.h | 2 +- 4 files changed, 56 insertions(+), 7 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index eb256497b7..f55537b3d6 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -394,6 +394,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const const float MIN_AXIS_LENGTH = 1.0e-4f; RotationConstraint* constraint = getConstraint(pivotIndex); + // only allow swing on lowerSpine if there is a hips IK target. if (_hipsTargetIndex < 0 && constraint && constraint->isLowerSpine() && tipIndex != _headIndex) { // for these types of targets we only allow twist at the lower-spine @@ -419,6 +420,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const float cosAngle = glm::clamp(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)), -1.0f, 1.0f); float angle = acosf(cosAngle); const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f; + if (angle > MIN_ADJUSTMENT_ANGLE) { // reduce angle by a flexCoefficient angle *= target.getFlexCoefficient(chainDepth); @@ -791,6 +793,7 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar return _relativePoses; } + //virtual const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { // allows solutionSource to be overridden by an animVar @@ -904,6 +907,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); + preconditionRelativePosesToAvoidLimbLock(context, targets); solve(context, targets); } @@ -1279,7 +1283,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z; - const float MIN_ELBOW_ANGLE = 0.05f; // ~2.8 deg (ajt-rad-to-deg 0.05) + const float MIN_ELBOW_ANGLE = 0.0f; const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f; glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y; @@ -1310,7 +1314,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment - const float MIN_KNEE_ANGLE = 0.097f; // ~5 deg + const float MIN_KNEE_ANGLE = 0.0f; const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f; // 157.5 deg glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y; @@ -1659,6 +1663,50 @@ void AnimInverseKinematics::blendToPoses(const AnimPoseVec& targetPoses, const A } } +void AnimInverseKinematics::preconditionRelativePosesToAvoidLimbLock(const AnimContext& context, const std::vector& targets) { + const int NUM_LIMBS = 4; + std::pair limbs[NUM_LIMBS] = { + {_skeleton->nameToJointIndex("LeftHand"), _skeleton->nameToJointIndex("LeftArm")}, + {_skeleton->nameToJointIndex("RightHand"), _skeleton->nameToJointIndex("RightArm")}, + {_skeleton->nameToJointIndex("LeftFoot"), _skeleton->nameToJointIndex("LeftUpLeg")}, + {_skeleton->nameToJointIndex("RightFoot"), _skeleton->nameToJointIndex("RightUpLeg")} + }; + const float MIN_AXIS_LENGTH = 1.0e-4f; + + for (auto& target : targets) { + if (target.getIndex() != -1) { + for (int i = 0; i < NUM_LIMBS; i++) { + if (limbs[i].first == target.getIndex()) { + int tipIndex = limbs[i].first; + int baseIndex = limbs[i].second; + + // TODO: as an optimization, these poses can be computed in one pass down the chain, instead of three. + AnimPose tipPose = _skeleton->getAbsolutePose(tipIndex, _relativePoses); + AnimPose basePose = _skeleton->getAbsolutePose(baseIndex, _relativePoses); + AnimPose baseParentPose = _skeleton->getAbsolutePose(_skeleton->getParentIndex(baseIndex), _relativePoses); + + // to help reduce limb locking, and to help the CCD solver converge faster + // rotate the limbs leverArm over the targetLine. + glm::vec3 targetLine = target.getTranslation() - basePose.trans(); + glm::vec3 leverArm = tipPose.trans() - basePose.trans(); + glm::vec3 axis = glm::cross(leverArm, targetLine); + float axisLength = glm::length(axis); + if (axisLength > MIN_AXIS_LENGTH) { + // compute angle of rotation that brings tip to target + axis /= axisLength; + float cosAngle = glm::clamp(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)), -1.0f, 1.0f); + float angle = acosf(cosAngle); + glm::quat newBaseRotation = glm::angleAxis(angle, axis) * basePose.rot(); + + // convert base rotation into relative space of base. + _relativePoses[baseIndex].rot() = glm::inverse(baseParentPose.rot()) * newBaseRotation; + } + } + } + } + } +} + void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPoses) { const float RELAX_BLEND_FACTOR = (1.0f / 16.0f); const float COPY_BLEND_FACTOR = 1.0f; @@ -1678,7 +1726,7 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s break; case SolutionSource::LimitCenterPoses: // essentially copy limitCenterPoses over to _relativePoses. - blendToPoses(_limitCenterPoses, underPoses, COPY_BLEND_FACTOR); + blendToPoses(underPoses, _limitCenterPoses, COPY_BLEND_FACTOR); break; } } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 016317aee3..d473ae3698 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -82,6 +82,7 @@ protected: void debugDrawSpineSplines(const AnimContext& context, const std::vector& targets) const; void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose); void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor); + void preconditionRelativePosesToAvoidLimbLock(const AnimContext& context, const std::vector& targets); // used to pre-compute information about each joint influeced by a spline IK target. struct SplineJointInfo { diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp index 350fe8a534..062e016660 100644 --- a/libraries/animation/src/AnimSkeleton.cpp +++ b/libraries/animation/src/AnimSkeleton.cpp @@ -76,11 +76,11 @@ const QString& AnimSkeleton::getJointName(int jointIndex) const { return _joints[jointIndex].name; } -AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const { - if (jointIndex < 0 || jointIndex >= (int)poses.size() || jointIndex >= _jointsSize) { +AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& relativePoses) const { + if (jointIndex < 0 || jointIndex >= (int)relativePoses.size() || jointIndex >= _jointsSize) { return AnimPose::identity; } else { - return getAbsolutePose(_joints[jointIndex].parentIndex, poses) * poses[jointIndex]; + return getAbsolutePose(_joints[jointIndex].parentIndex, relativePoses) * relativePoses[jointIndex]; } } diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h index 0988c26bdb..6315f2d62b 100644 --- a/libraries/animation/src/AnimSkeleton.h +++ b/libraries/animation/src/AnimSkeleton.h @@ -50,7 +50,7 @@ public: int getParentIndex(int jointIndex) const; - AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const; + AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& relativePoses) const; void convertRelativePosesToAbsolute(AnimPoseVec& poses) const; void convertAbsolutePosesToRelative(AnimPoseVec& poses) const; From e7991579ef6f9021cb01cf5f3aee5681c8cdeec1 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 16 Jun 2017 17:29:56 -0700 Subject: [PATCH 11/20] Enabled elbow pole vectors There are still some issues with rotations of the elbow pole vectors. * When the (hand - shoulder) vector approaches the normal vector used in Rig::calculateElbowPoleVector() unexpected twists can occur. * Also, when the (hand - shoulder) vector approaches zero, the IK system starts to flutter between two states. * The shoulder twist constraint probably needs to be opened up for more natural range of motion. --- .../resources/avatar/avatar-animation.json | 10 +- .../animation/src/AnimInverseKinematics.cpp | 2 +- libraries/animation/src/Rig.cpp | 95 ++++++++++++++++++- libraries/animation/src/Rig.h | 9 +- 4 files changed, 110 insertions(+), 6 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index c9552a3ead..018987b58b 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -68,7 +68,10 @@ "typeVar": "rightHandType", "weightVar": "rightHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0], + "poleVectorEnabledVar": "rightHandPoleVectorEnabled", + "poleReferenceVectorVar": "rightHandPoleReferenceVector", + "poleVectorVar": "rightHandPoleVector" }, { "jointName": "LeftHand", @@ -77,7 +80,10 @@ "typeVar": "leftHandType", "weightVar": "leftHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0], + "poleVectorEnabledVar": "leftHandPoleVectorEnabled", + "poleReferenceVectorVar": "leftHandPoleReferenceVector", + "poleVectorVar": "leftHandPoleVector" }, { "jointName": "RightFoot", diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index f55537b3d6..080e6449d9 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -42,7 +42,7 @@ static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointCha } } -float easeOutExpo(float t) { +static float easeOutExpo(float t) { return 1.0f - powf(2, -10.0f * t); } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 1e6965faf5..99678eaa28 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1140,6 +1140,47 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } +static float easeOutExpo(float t) { + return 1.0f - powf(2, -10.0f * t); +} + +static glm::quat quatLerp(const glm::quat& q1, const glm::quat& q2, float alpha) { + float dot = glm::dot(q1, q2); + glm::quat temp; + if (dot < 0.0f) { + temp = -q2; + } else { + temp = q2; + } + return glm::normalize(glm::lerp(q1, temp, alpha)); +} + +glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const { + AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex]; + AnimPose handPose = _externalPoseSet._absolutePoses[handIndex]; + AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex]; + AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; + glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); + + glm::quat elbowToHandDelta = handPose.rot() * glm::inverse(elbowPose.rot()); + const float WRIST_POLE_ADJUST_FACTOR = 0.5f; + glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, elbowToHandDelta, WRIST_POLE_ADJUST_FACTOR); + + glm::vec3 n; + if (isLeft) { + n = hipsPose.rot() * glm::normalize(glm::vec3(1.0f, 1.5f, -0.1f)); + } else { + n = hipsPose.rot() * -glm::normalize(glm::vec3(-1.0f, 1.5f, -0.1f)); + } + + // project d onto n. + glm::vec3 dProj = d - glm::dot(d, n) * n; + glm::vec3 dProjRot = glm::angleAxis(PI / 2.0f, n) * dProj; + glm::vec3 pole = glm::normalize(dProjRot); + + return glm::normalize(poleAdjust * pole); +} + glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex); @@ -1175,6 +1216,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f const float RELAX_DURATION = 0.6f; + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + if (params.isLeftEnabled) { glm::vec3 handPosition = params.leftPosition; if (!bodySensorTrackingEnabled) { @@ -1191,7 +1234,32 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _isLeftHandControlled = true; _lastLeftHandControlledPose = AnimPose(glm::vec3(1.0f), params.leftOrientation, handPosition); + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftHandPoleVectorValid) { + _prevLeftHandPoleVectorValid = true; + _prevLeftHandPoleVector = poleVector; + } + _prevLeftHandPoleVector = lerp(poleVector, _prevLeftHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("leftHandPoleVectorEnabled", true); + _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); + _animVars.set("leftHandPoleVector", _prevLeftHandPoleVector); + } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + } } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + if (_isLeftHandControlled) { _leftHandRelaxDuration = RELAX_DURATION; _isLeftHandControlled = false; @@ -1234,7 +1302,32 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _isRightHandControlled = true; _lastRightHandControlledPose = AnimPose(glm::vec3(1.0f), params.rightOrientation, handPosition); + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightHandPoleVectorValid) { + _prevRightHandPoleVectorValid = true; + _prevRightHandPoleVector = poleVector; + } + _prevRightHandPoleVector = lerp(poleVector, _prevRightHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("rightHandPoleVectorEnabled", true); + _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); + _animVars.set("rightHandPoleVector", _prevRightHandPoleVector); + } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + } } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + if (_isRightHandControlled) { _rightHandRelaxDuration = RELAX_DURATION; _isRightHandControlled = false; @@ -1261,8 +1354,6 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f } } - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; - if (params.isLeftFootEnabled) { _animVars.set("leftFootPosition", params.leftFootPosition); _animVars.set("leftFootRotation", params.leftFootOrientation); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 4fc5f76146..06822d666e 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -249,7 +249,8 @@ protected: void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; - glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; + glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; + glm::vec3 calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) @@ -362,6 +363,12 @@ protected: glm::vec3 _prevLeftFootPoleVector = { Vectors::UNIT_Z }; bool _prevLeftFootPoleVectorValid = { false }; + + glm::vec3 _prevRightHandPoleVector = { -Vectors::UNIT_Z }; + bool _prevRightHandPoleVectorValid = { false }; + + glm::vec3 _prevLeftHandPoleVector = { -Vectors::UNIT_Z }; + bool _prevLeftHandPoleVectorValid = { false }; }; #endif /* defined(__hifi__Rig__) */ From 81852cd91c744e81f9296d09ec04f9b8590d6fcc Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 16 Jun 2017 18:15:41 -0700 Subject: [PATCH 12/20] warning fixes --- libraries/animation/src/AnimInverseKinematics.cpp | 2 -- libraries/animation/src/Rig.cpp | 4 ---- 2 files changed, 6 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 080e6449d9..3ea8937eec 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -565,8 +565,6 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; - float eProjLen = glm::length(eProj); - float pProjLen = glm::length(pProj); const float PROJ_VECTOR_LEN = 10.0f; const float POLE_VECTOR_LEN = 100.0f; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 5ad664614e..43ce209420 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1140,10 +1140,6 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } -static float easeOutExpo(float t) { - return 1.0f - powf(2, -10.0f * t); -} - static glm::quat quatLerp(const glm::quat& q1, const glm::quat& q2, float alpha) { float dot = glm::dot(q1, q2); glm::quat temp; From 3f5aba26557be43e31c98c73d395ca028ca0dbd6 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Jun 2017 13:21:12 -0700 Subject: [PATCH 13/20] improved elbow pole vector calculation Also, pole vectors are blended spherical linearly, this might help fast moving pole vectors from rotating too quickly. --- libraries/animation/src/Rig.cpp | 57 ++++++++++++++++++++------------- 1 file changed, 35 insertions(+), 22 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 43ce209420..b1d0dca9e6 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1158,23 +1158,27 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); + float sign = isLeft ? 1.0f : -1.0f; + + glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; + glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; + // project d onto n. + glm::vec3 dProj = d - glm::dot(d, n) * n; + const float LATERAL_OFFSET = 0.333f; + const float VERTICAL_OFFSET = -0.333f; + + // give dProj a bit of offset away from the body. + glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; + + // rotate dProj by 90 degrees to get the poleVector. + glm::vec3 poleVector = glm::angleAxis(PI / 2.0f, n) * dProjWithOffset; + + // blend the wrist oreintation into the pole vector to reduce the painfully bent wrist problem. glm::quat elbowToHandDelta = handPose.rot() * glm::inverse(elbowPose.rot()); const float WRIST_POLE_ADJUST_FACTOR = 0.5f; glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, elbowToHandDelta, WRIST_POLE_ADJUST_FACTOR); - glm::vec3 n; - if (isLeft) { - n = hipsPose.rot() * glm::normalize(glm::vec3(1.0f, 1.5f, -0.1f)); - } else { - n = hipsPose.rot() * -glm::normalize(glm::vec3(-1.0f, 1.5f, -0.1f)); - } - - // project d onto n. - glm::vec3 dProj = d - glm::dot(d, n) * n; - glm::vec3 dProjRot = glm::angleAxis(PI / 2.0f, n) * dProj; - glm::vec3 pole = glm::normalize(dProjRot); - - return glm::normalize(poleAdjust * pole); + return glm::normalize(poleAdjust * poleVector); } glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { @@ -1217,7 +1221,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f const bool LEFT_HAND = true; const bool RIGHT_HAND = false; - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.9f; + const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.9f; if (params.isLeftEnabled) { if (!_isLeftHandControlled) { @@ -1232,8 +1237,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f // Move hand from non-controlled position to controlled position. _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f); AnimPose handPose(Vectors::ONE, handRotation, handPosition); - if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, LEFT_HAND, TO_CONTROLLED, - handPose)) { + if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, + LEFT_HAND, TO_CONTROLLED, handPose)) { handPosition = handPose.trans(); handRotation = handPose.rot(); } @@ -1268,7 +1273,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevLeftHandPoleVectorValid = true; _prevLeftHandPoleVector = poleVector; } - _prevLeftHandPoleVector = lerp(poleVector, _prevLeftHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector; _animVars.set("leftHandPoleVectorEnabled", true); _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); @@ -1290,8 +1297,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f // Move hand from controlled position to non-controlled position. _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f); AnimPose handPose; - if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, LEFT_HAND, - FROM_CONTROLLED, handPose)) { + if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, + LEFT_HAND, FROM_CONTROLLED, handPose)) { _animVars.set("leftHandPosition", handPose.trans()); _animVars.set("leftHandRotation", handPose.rot()); _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); @@ -1352,7 +1359,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevRightHandPoleVectorValid = true; _prevRightHandPoleVector = poleVector; } - _prevRightHandPoleVector = lerp(poleVector, _prevRightHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector; _animVars.set("rightHandPoleVectorEnabled", true); _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); @@ -1401,7 +1410,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevLeftFootPoleVectorValid = true; _prevLeftFootPoleVector = poleVector; } - _prevLeftFootPoleVector = lerp(poleVector, _prevLeftFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + glm::quat deltaRot = rotationBetween(_prevLeftFootPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); + _prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector; _animVars.set("leftFootPoleVectorEnabled", true); _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z); @@ -1427,7 +1438,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevRightFootPoleVectorValid = true; _prevRightFootPoleVector = poleVector; } - _prevRightFootPoleVector = lerp(poleVector, _prevRightFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + glm::quat deltaRot = rotationBetween(_prevRightFootPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); + _prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector; _animVars.set("rightFootPoleVectorEnabled", true); _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z); From 6bbc5bfbea17c16fde201d2a655aacdaf17e4617 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Jun 2017 13:42:39 -0700 Subject: [PATCH 14/20] formatting on initializers --- libraries/animation/src/Rig.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index c9033b6e6e..d101425a77 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -351,8 +351,8 @@ protected: int _nextStateHandlerId { 0 }; QMutex _stateMutex; - bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand, - bool isToControlled, AnimPose& returnHandPose); + bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand, + bool isToControlled, AnimPose& returnHandPose); bool _isLeftHandControlled { false }; bool _isRightHandControlled { false }; @@ -363,17 +363,17 @@ protected: AnimPose _lastLeftHandControlledPose; AnimPose _lastRightHandControlledPose; - glm::vec3 _prevRightFootPoleVector = { Vectors::UNIT_Z }; - bool _prevRightFootPoleVectorValid = { false }; + glm::vec3 _prevRightFootPoleVector { Vectors::UNIT_Z }; + bool _prevRightFootPoleVectorValid { false }; - glm::vec3 _prevLeftFootPoleVector = { Vectors::UNIT_Z }; - bool _prevLeftFootPoleVectorValid = { false }; + glm::vec3 _prevLeftFootPoleVector { Vectors::UNIT_Z }; + bool _prevLeftFootPoleVectorValid { false }; - glm::vec3 _prevRightHandPoleVector = { -Vectors::UNIT_Z }; - bool _prevRightHandPoleVectorValid = { false }; + glm::vec3 _prevRightHandPoleVector { -Vectors::UNIT_Z }; + bool _prevRightHandPoleVectorValid { false }; - glm::vec3 _prevLeftHandPoleVector = { -Vectors::UNIT_Z }; - bool _prevLeftHandPoleVectorValid = { false }; + glm::vec3 _prevLeftHandPoleVector { -Vectors::UNIT_Z }; + bool _prevLeftHandPoleVectorValid { false }; }; #endif /* defined(__hifi__Rig__) */ From c236afe68c9ccf08ffed40f81e95d38f2db9300d Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Jun 2017 16:54:39 -0700 Subject: [PATCH 15/20] Simplify passing data from MySkeletonModel to Rig --- interface/src/avatar/MySkeletonModel.cpp | 125 ++--- libraries/animation/src/AnimPose.h | 2 + libraries/animation/src/Rig.cpp | 607 ++++++++++++----------- libraries/animation/src/Rig.h | 63 +-- 4 files changed, 407 insertions(+), 390 deletions(-) diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index e74df4cf0f..89b6c36c63 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -47,110 +47,113 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { MyAvatar* myAvatar = static_cast(_owningAvatar); - Rig::HeadParameters headParams; + Rig::ControllerParameters params; + + AnimPose avatarToRigPose(glm::vec3(1.0f), Quaternions::Y_180, glm::vec3(0.0f)); // input action is the highest priority source for head orientation. auto avatarHeadPose = myAvatar->getHeadControllerPoseInAvatarFrame(); if (avatarHeadPose.isValid()) { - glm::mat4 rigHeadMat = Matrices::Y_180 * - createMatFromQuatAndPos(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation()); - headParams.rigHeadPosition = extractTranslation(rigHeadMat); - headParams.rigHeadOrientation = glmExtractRotation(rigHeadMat); - headParams.headEnabled = true; + AnimPose pose(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_Head] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_Head] = true; } else { // even though full head IK is disabled, the rig still needs the head orientation to rotate the head up and // down in desktop mode. // preMult 180 is necessary to convert from avatar to rig coordinates. // postMult 180 is necessary to convert head from -z forward to z forward. - headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180; - headParams.headEnabled = false; + glm::quat headRot = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180; + params.controllerPoses[Rig::ControllerType_Head] = AnimPose(glm::vec3(1.0f), headRot, glm::vec3(0.0f)); + params.controllerActiveFlags[Rig::ControllerType_Head] = false; } auto avatarHipsPose = myAvatar->getHipsControllerPoseInAvatarFrame(); if (avatarHipsPose.isValid()) { - glm::mat4 rigHipsMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation()); - headParams.hipsMatrix = rigHipsMat; - headParams.hipsEnabled = true; + AnimPose pose(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_Hips] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_Hips] = true; } else { - headParams.hipsEnabled = false; + params.controllerPoses[Rig::ControllerType_Hips] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_Hips] = false; } auto avatarSpine2Pose = myAvatar->getSpine2ControllerPoseInAvatarFrame(); if (avatarSpine2Pose.isValid()) { - glm::mat4 rigSpine2Mat = Matrices::Y_180 * createMatFromQuatAndPos(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation()); - headParams.spine2Matrix = rigSpine2Mat; - headParams.spine2Enabled = true; + AnimPose pose(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation()); + params.controllerPoses[Rig::ControllerType_Spine2] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_Spine2] = true; } else { - headParams.spine2Enabled = false; + params.controllerPoses[Rig::ControllerType_Spine2] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_Spine2] = false; } auto avatarRightArmPose = myAvatar->getRightArmControllerPoseInAvatarFrame(); if (avatarRightArmPose.isValid()) { - glm::mat4 rightArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation()); - headParams.rightArmPosition = extractTranslation(rightArmMat); - headParams.rightArmRotation = glmExtractRotation(rightArmMat); - headParams.rightArmEnabled = true; + AnimPose pose(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_RightArm] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_RightArm] = true; } else { - headParams.rightArmEnabled = false; + params.controllerPoses[Rig::ControllerType_RightArm] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_RightArm] = false; } - + auto avatarLeftArmPose = myAvatar->getLeftArmControllerPoseInAvatarFrame(); if (avatarLeftArmPose.isValid()) { - glm::mat4 leftArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation()); - headParams.leftArmPosition = extractTranslation(leftArmMat); - headParams.leftArmRotation = glmExtractRotation(leftArmMat); - headParams.leftArmEnabled = true; + AnimPose pose(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_LeftArm] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_LeftArm] = true; } else { - headParams.leftArmEnabled = false; + params.controllerPoses[Rig::ControllerType_LeftArm] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_LeftArm] = false; } - headParams.isTalking = head->getTimeWithoutTalking() <= 1.5f; - - _rig.updateFromHeadParameters(headParams, deltaTime); - - Rig::HandAndFeetParameters handAndFeetParams; - - auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); - if (leftPose.isValid()) { - handAndFeetParams.isLeftEnabled = true; - handAndFeetParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation(); - handAndFeetParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation(); + auto avatarLeftHandPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); + if (avatarLeftHandPose.isValid()) { + AnimPose pose(avatarLeftHandPose.getRotation(), avatarLeftHandPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_LeftHand] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_LeftHand] = true; } else { - handAndFeetParams.isLeftEnabled = false; + params.controllerPoses[Rig::ControllerType_LeftHand] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_LeftHand] = false; } - auto rightPose = myAvatar->getRightHandControllerPoseInAvatarFrame(); - if (rightPose.isValid()) { - handAndFeetParams.isRightEnabled = true; - handAndFeetParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation(); - handAndFeetParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation(); + auto avatarRightHandPose = myAvatar->getRightHandControllerPoseInAvatarFrame(); + if (avatarRightHandPose.isValid()) { + AnimPose pose(avatarRightHandPose.getRotation(), avatarRightHandPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_RightHand] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_RightHand] = true; } else { - handAndFeetParams.isRightEnabled = false; + params.controllerPoses[Rig::ControllerType_RightHand] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_RightHand] = false; } - auto leftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame(); - if (leftFootPose.isValid()) { - handAndFeetParams.isLeftFootEnabled = true; - handAndFeetParams.leftFootPosition = Quaternions::Y_180 * leftFootPose.getTranslation(); - handAndFeetParams.leftFootOrientation = Quaternions::Y_180 * leftFootPose.getRotation(); + auto avatarLeftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame(); + if (avatarLeftFootPose.isValid()) { + AnimPose pose(avatarLeftFootPose.getRotation(), avatarLeftFootPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_LeftFoot] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = true; } else { - handAndFeetParams.isLeftFootEnabled = false; + params.controllerPoses[Rig::ControllerType_LeftFoot] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = false; } - auto rightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame(); - if (rightFootPose.isValid()) { - handAndFeetParams.isRightFootEnabled = true; - handAndFeetParams.rightFootPosition = Quaternions::Y_180 * rightFootPose.getTranslation(); - handAndFeetParams.rightFootOrientation = Quaternions::Y_180 * rightFootPose.getRotation(); + auto avatarRightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame(); + if (avatarRightFootPose.isValid()) { + AnimPose pose(avatarRightFootPose.getRotation(), avatarRightFootPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_RightFoot] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_RightFoot] = true; } else { - handAndFeetParams.isRightFootEnabled = false; + params.controllerPoses[Rig::ControllerType_RightFoot] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_RightFoot] = false; } - handAndFeetParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius(); - handAndFeetParams.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight(); - handAndFeetParams.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset(); + params.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius(); + params.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight(); + params.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset(); - _rig.updateFromHandAndFeetParameters(handAndFeetParams, deltaTime); + params.isTalking = head->getTimeWithoutTalking() <= 1.5f; + + _rig.updateFromControllerParameters(params, deltaTime); Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState()); diff --git a/libraries/animation/src/AnimPose.h b/libraries/animation/src/AnimPose.h index a2e22a24be..2df3d1f2e4 100644 --- a/libraries/animation/src/AnimPose.h +++ b/libraries/animation/src/AnimPose.h @@ -21,6 +21,8 @@ class AnimPose { public: AnimPose() {} explicit AnimPose(const glm::mat4& mat); + explicit AnimPose(const glm::quat& rotIn) : _scale(1.0f), _rot(rotIn), _trans(0.0f) {} + AnimPose(const glm::quat& rotIn, const glm::vec3& transIn) : _scale(1.0f), _rot(rotIn), _trans(transIn) {} AnimPose(const glm::vec3& scaleIn, const glm::quat& rotIn, const glm::vec3& transIn) : _scale(scaleIn), _rot(rotIn), _trans(transIn) {} static const AnimPose identity; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index b1d0dca9e6..0ee42372b1 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1010,46 +1010,6 @@ glm::quat Rig::getJointDefaultRotationInParentFrame(int jointIndex) { return glm::quat(); } -void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { - updateHeadAnimVars(params); - - _animVars.set("isTalking", params.isTalking); - _animVars.set("notIsTalking", !params.isTalking); - - if (params.hipsEnabled) { - _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToLimitCenterPoses); - _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("hipsPosition", extractTranslation(params.hipsMatrix)); - _animVars.set("hipsRotation", glmExtractRotation(params.hipsMatrix)); - } else { - _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToUnderPoses); - _animVars.set("hipsType", (int)IKTarget::Type::Unknown); - } - - if (params.hipsEnabled && params.spine2Enabled) { - _animVars.set("spine2Type", (int)IKTarget::Type::Spline); - _animVars.set("spine2Position", extractTranslation(params.spine2Matrix)); - _animVars.set("spine2Rotation", glmExtractRotation(params.spine2Matrix)); - } else { - _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); - } - - if (params.leftArmEnabled) { - _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("leftArmPosition", params.leftArmPosition); - _animVars.set("leftArmRotation", params.leftArmRotation); - } else { - _animVars.set("leftArmType", (int)IKTarget::Type::Unknown); - } - - if (params.rightArmEnabled) { - _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("rightArmPosition", params.rightArmPosition); - _animVars.set("rightArmRotation", params.rightArmRotation); - } else { - _animVars.set("rightArmType", (int)IKTarget::Type::Unknown); - } -} void Rig::updateFromEyeParameters(const EyeParameters& params) { updateEyeJoint(params.leftEyeJointIndex, params.modelTranslation, params.modelRotation, params.eyeLookAt, params.eyeSaccade); @@ -1081,12 +1041,12 @@ void Rig::computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut headOrientationOut = hmdOrientation; } -void Rig::updateHeadAnimVars(const HeadParameters& params) { +void Rig::updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headPose) { if (_animSkeleton) { - if (params.headEnabled) { - _animVars.set("headPosition", params.rigHeadPosition); - _animVars.set("headRotation", params.rigHeadOrientation); - if (params.hipsEnabled) { + if (headEnabled) { + _animVars.set("headPosition", headPose.trans()); + _animVars.set("headRotation", headPose.rot()); + if (hipsEnabled) { // Since there is an explicit hips ik target, switch the head to use the more flexible Spline IK chain type. // this will allow the spine to compress/expand and bend more natrually, ensuring that it can reach the head target position. _animVars.set("headType", (int)IKTarget::Type::Spline); @@ -1099,12 +1059,271 @@ void Rig::updateHeadAnimVars(const HeadParameters& params) { } } else { _animVars.unset("headPosition"); - _animVars.set("headRotation", params.rigHeadOrientation); + _animVars.set("headRotation", headPose.rot()); _animVars.set("headType", (int)IKTarget::Type::RotationOnly); } } } +void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, float dt, + const AnimPose& leftHandPose, const AnimPose& rightHandPose, + float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset) { + + // Use this capsule to represent the avatar body. + int hipsIndex = indexOfJoint("Hips"); + glm::vec3 hipsTrans; + if (hipsIndex >= 0) { + hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans(); + } + + const glm::vec3 bodyCapsuleCenter = hipsTrans - bodyCapsuleLocalOffset; + const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, bodyCapsuleHalfHeight, 0); + const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, bodyCapsuleHalfHeight, 0); + + const float HAND_RADIUS = 0.05f; + + const float RELAX_DURATION = 0.6f; + const float CONTROL_DURATION = 0.4f; + const bool TO_CONTROLLED = true; + const bool FROM_CONTROLLED = false; + const bool LEFT_HAND = true; + const bool RIGHT_HAND = false; + + const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.9f; + + if (leftHandEnabled) { + if (!_isLeftHandControlled) { + _leftHandControlTimeRemaining = CONTROL_DURATION; + _isLeftHandControlled = true; + } + + glm::vec3 handPosition = leftHandPose.trans(); + glm::quat handRotation = leftHandPose.rot(); + + if (_leftHandControlTimeRemaining > 0.0f) { + // Move hand from non-controlled position to controlled position. + _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f); + AnimPose handPose(Vectors::ONE, handRotation, handPosition); + if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, + LEFT_HAND, TO_CONTROLLED, handPose)) { + handPosition = handPose.trans(); + handRotation = handPose.rot(); + } + } + + if (!hipsEnabled) { + // prevent the hand IK targets from intersecting the body capsule + glm::vec3 displacement; + if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { + handPosition -= displacement; + } + } + + _animVars.set("leftHandPosition", handPosition); + _animVars.set("leftHandRotation", handRotation); + _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); + + _lastLeftHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); + _isLeftHandControlled = true; + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftHandPoleVectorValid) { + _prevLeftHandPoleVectorValid = true; + _prevLeftHandPoleVector = poleVector; + } + glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector; + + _animVars.set("leftHandPoleVectorEnabled", true); + _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); + _animVars.set("leftHandPoleVector", _prevLeftHandPoleVector); + } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + } + } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + + if (_isLeftHandControlled) { + _leftHandRelaxTimeRemaining = RELAX_DURATION; + _isLeftHandControlled = false; + } + + if (_leftHandRelaxTimeRemaining > 0.0f) { + // Move hand from controlled position to non-controlled position. + _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f); + AnimPose handPose; + if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, + LEFT_HAND, FROM_CONTROLLED, handPose)) { + _animVars.set("leftHandPosition", handPose.trans()); + _animVars.set("leftHandRotation", handPose.rot()); + _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); + } + } else { + _animVars.unset("leftHandPosition"); + _animVars.unset("leftHandRotation"); + _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); + } + } + + if (rightHandEnabled) { + if (!_isRightHandControlled) { + _rightHandControlTimeRemaining = CONTROL_DURATION; + _isRightHandControlled = true; + } + + glm::vec3 handPosition = rightHandPose.trans(); + glm::quat handRotation = rightHandPose.rot(); + + if (_rightHandControlTimeRemaining > 0.0f) { + // Move hand from non-controlled position to controlled position. + _rightHandControlTimeRemaining = std::max(_rightHandControlTimeRemaining - dt, 0.0f); + AnimPose handPose(Vectors::ONE, handRotation, handPosition); + if (transitionHandPose(_rightHandControlTimeRemaining, CONTROL_DURATION, handPose, RIGHT_HAND, TO_CONTROLLED, handPose)) { + handPosition = handPose.trans(); + handRotation = handPose.rot(); + } + } + + if (!hipsEnabled) { + // prevent the hand IK targets from intersecting the body capsule + glm::vec3 displacement; + if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { + handPosition -= displacement; + } + } + + _animVars.set("rightHandPosition", handPosition); + _animVars.set("rightHandRotation", handRotation); + _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); + + _lastRightHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); + _isRightHandControlled = true; + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightHandPoleVectorValid) { + _prevRightHandPoleVectorValid = true; + _prevRightHandPoleVector = poleVector; + } + glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector; + + _animVars.set("rightHandPoleVectorEnabled", true); + _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); + _animVars.set("rightHandPoleVector", _prevRightHandPoleVector); + } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + } + } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + + if (_isRightHandControlled) { + _rightHandRelaxTimeRemaining = RELAX_DURATION; + _isRightHandControlled = false; + } + + if (_rightHandRelaxTimeRemaining > 0.0f) { + // Move hand from controlled position to non-controlled position. + _rightHandRelaxTimeRemaining = std::max(_rightHandRelaxTimeRemaining - dt, 0.0f); + AnimPose handPose; + if (transitionHandPose(_rightHandRelaxTimeRemaining, RELAX_DURATION, _lastRightHandControlledPose, RIGHT_HAND, FROM_CONTROLLED, handPose)) { + _animVars.set("rightHandPosition", handPose.trans()); + _animVars.set("rightHandRotation", handPose.rot()); + _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); + } + } else { + _animVars.unset("rightHandPosition"); + _animVars.unset("rightHandRotation"); + _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); + } + } +} + +void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose) { + + const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.9f; + + int hipsIndex = indexOfJoint("Hips"); + + if (leftFootEnabled) { + glm::vec3 footPosition = leftFootPose.trans(); + glm::quat footRotation = leftFootPose.rot(); + _animVars.set("leftFootPosition", footPosition); + _animVars.set("leftFootRotation", footRotation); + _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftFootPoleVectorValid) { + _prevLeftFootPoleVectorValid = true; + _prevLeftFootPoleVector = poleVector; + } + glm::quat deltaRot = rotationBetween(_prevLeftFootPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); + _prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector; + + _animVars.set("leftFootPoleVectorEnabled", true); + _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z); + _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector); + } else { + _animVars.unset("leftFootPosition"); + _animVars.unset("leftFootRotation"); + _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("leftFootPoleVectorEnabled", false); + _prevLeftFootPoleVectorValid = false; + } + + if (rightFootEnabled) { + glm::vec3 footPosition = rightFootPose.trans(); + glm::quat footRotation = rightFootPose.rot(); + _animVars.set("rightFootPosition", footPosition); + _animVars.set("rightFootRotation", footRotation); + _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightFootPoleVectorValid) { + _prevRightFootPoleVectorValid = true; + _prevRightFootPoleVector = poleVector; + } + glm::quat deltaRot = rotationBetween(_prevRightFootPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); + _prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector; + + _animVars.set("rightFootPoleVectorEnabled", true); + _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z); + _animVars.set("rightFootPoleVector", _prevRightFootPoleVector); + } else { + _animVars.unset("rightFootPosition"); + _animVars.unset("rightFootRotation"); + _animVars.set("rightFootPoleVectorEnabled", false); + _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); + } +} + void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAtSpot, const glm::vec3& saccade) { // TODO: does not properly handle avatar scale. @@ -1196,261 +1415,65 @@ glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& foot return glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); } -void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt) { - if (_animSkeleton && _animNode) { - const float HAND_RADIUS = 0.05f; - int hipsIndex = indexOfJoint("Hips"); - glm::vec3 hipsTrans; - if (hipsIndex >= 0) { - hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans(); - } +void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) { + if (!_animSkeleton || !_animNode) { + return; + } - // Use this capsule to represent the avatar body. - 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); + _animVars.set("isTalking", params.isTalking); + _animVars.set("notIsTalking", !params.isTalking); - // TODO: add isHipsEnabled - bool bodySensorTrackingEnabled = params.isLeftFootEnabled || params.isRightFootEnabled; + bool headEnabled = params.controllerActiveFlags[ControllerType_Head]; + bool leftHandEnabled = params.controllerActiveFlags[ControllerType_LeftHand]; + bool rightHandEnabled = params.controllerActiveFlags[ControllerType_RightHand]; + bool hipsEnabled = params.controllerActiveFlags[ControllerType_Hips]; + bool leftFootEnabled = params.controllerActiveFlags[ControllerType_LeftFoot]; + bool rightFootEnabled = params.controllerActiveFlags[ControllerType_RightFoot]; + bool leftArmEnabled = params.controllerActiveFlags[ControllerType_LeftArm]; + bool rightArmEnabled = params.controllerActiveFlags[ControllerType_RightArm]; + bool spine2Enabled = params.controllerActiveFlags[ControllerType_Spine2]; - const float RELAX_DURATION = 0.6f; - const float CONTROL_DURATION = 0.4f; - const bool TO_CONTROLLED = true; - const bool FROM_CONTROLLED = false; - const bool LEFT_HAND = true; - const bool RIGHT_HAND = false; + updateHead(headEnabled, hipsEnabled, params.controllerPoses[ControllerType_Head]); - const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.9f; - const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.9f; + updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, dt, + params.controllerPoses[ControllerType_LeftHand], params.controllerPoses[ControllerType_RightHand], + params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset); - if (params.isLeftEnabled) { - if (!_isLeftHandControlled) { - _leftHandControlTimeRemaining = CONTROL_DURATION; - _isLeftHandControlled = true; - } + updateFeet(leftFootEnabled, rightFootEnabled, + params.controllerPoses[ControllerType_LeftFoot], params.controllerPoses[ControllerType_RightFoot]); - glm::vec3 handPosition = params.leftPosition; - glm::quat handRotation = params.leftOrientation; + if (hipsEnabled) { + _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToLimitCenterPoses); + _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("hipsPosition", params.controllerPoses[ControllerType_Hips].trans()); + _animVars.set("hipsRotation", params.controllerPoses[ControllerType_Hips].rot()); + } else { + _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToUnderPoses); + _animVars.set("hipsType", (int)IKTarget::Type::Unknown); + } - if (_leftHandControlTimeRemaining > 0.0f) { - // Move hand from non-controlled position to controlled position. - _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f); - AnimPose handPose(Vectors::ONE, handRotation, handPosition); - if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, - LEFT_HAND, TO_CONTROLLED, handPose)) { - handPosition = handPose.trans(); - handRotation = handPose.rot(); - } - } + if (hipsEnabled && spine2Enabled) { + _animVars.set("spine2Type", (int)IKTarget::Type::Spline); + _animVars.set("spine2Position", params.controllerPoses[ControllerType_Spine2].trans()); + _animVars.set("spine2Rotation", params.controllerPoses[ControllerType_Spine2].rot()); + } else { + _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); + } - if (!bodySensorTrackingEnabled) { - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } - } + if (leftArmEnabled) { + _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("leftArmPosition", params.controllerPoses[ControllerType_LeftArm].trans()); + _animVars.set("leftArmRotation", params.controllerPoses[ControllerType_LeftArm].rot()); + } else { + _animVars.set("leftArmType", (int)IKTarget::Type::Unknown); + } - _animVars.set("leftHandPosition", handPosition); - _animVars.set("leftHandRotation", handRotation); - _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); - - _lastLeftHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); - - _isLeftHandControlled = true; - _lastLeftHandControlledPose = AnimPose(glm::vec3(1.0f), params.leftOrientation, handPosition); - - // compute pole vector - int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); - int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); - int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); - if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { - glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevLeftHandPoleVectorValid) { - _prevLeftHandPoleVectorValid = true; - _prevLeftHandPoleVector = poleVector; - } - glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, poleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); - _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector; - - _animVars.set("leftHandPoleVectorEnabled", true); - _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); - _animVars.set("leftHandPoleVector", _prevLeftHandPoleVector); - } else { - _prevLeftHandPoleVectorValid = false; - _animVars.set("leftHandPoleVectorEnabled", false); - } - } else { - _prevLeftHandPoleVectorValid = false; - _animVars.set("leftHandPoleVectorEnabled", false); - - if (_isLeftHandControlled) { - _leftHandRelaxTimeRemaining = RELAX_DURATION; - _isLeftHandControlled = false; - } - - if (_leftHandRelaxTimeRemaining > 0.0f) { - // Move hand from controlled position to non-controlled position. - _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f); - AnimPose handPose; - if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, - LEFT_HAND, FROM_CONTROLLED, handPose)) { - _animVars.set("leftHandPosition", handPose.trans()); - _animVars.set("leftHandRotation", handPose.rot()); - _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); - } - } else { - _animVars.unset("leftHandPosition"); - _animVars.unset("leftHandRotation"); - _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); - } - } - - if (params.isRightEnabled) { - if (!_isRightHandControlled) { - _rightHandControlTimeRemaining = CONTROL_DURATION; - _isRightHandControlled = true; - } - - glm::vec3 handPosition = params.rightPosition; - glm::quat handRotation = params.rightOrientation; - - if (_rightHandControlTimeRemaining > 0.0f) { - // Move hand from non-controlled position to controlled position. - _rightHandControlTimeRemaining = std::max(_rightHandControlTimeRemaining - dt, 0.0f); - AnimPose handPose(Vectors::ONE, handRotation, handPosition); - if (transitionHandPose(_rightHandControlTimeRemaining, CONTROL_DURATION, handPose, RIGHT_HAND, TO_CONTROLLED, - handPose)) { - handPosition = handPose.trans(); - handRotation = handPose.rot(); - } - } - - if (!bodySensorTrackingEnabled) { - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } - } - - _animVars.set("rightHandPosition", handPosition); - _animVars.set("rightHandRotation", handRotation); - _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); - - _lastRightHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); - - _isRightHandControlled = true; - _lastRightHandControlledPose = AnimPose(glm::vec3(1.0f), params.rightOrientation, handPosition); - - // compute pole vector - int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); - int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); - int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); - if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { - glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevRightHandPoleVectorValid) { - _prevRightHandPoleVectorValid = true; - _prevRightHandPoleVector = poleVector; - } - glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, poleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); - _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector; - - _animVars.set("rightHandPoleVectorEnabled", true); - _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); - _animVars.set("rightHandPoleVector", _prevRightHandPoleVector); - } else { - _prevRightHandPoleVectorValid = false; - _animVars.set("rightHandPoleVectorEnabled", false); - } - - } else { - _prevRightHandPoleVectorValid = false; - _animVars.set("rightHandPoleVectorEnabled", false); - - if (_isRightHandControlled) { - _rightHandRelaxTimeRemaining = RELAX_DURATION; - _isRightHandControlled = false; - } - - if (_rightHandRelaxTimeRemaining > 0.0f) { - // Move hand from controlled position to non-controlled position. - _rightHandRelaxTimeRemaining = std::max(_rightHandRelaxTimeRemaining - dt, 0.0f); - AnimPose handPose; - if (transitionHandPose(_rightHandRelaxTimeRemaining, RELAX_DURATION, _lastRightHandControlledPose, RIGHT_HAND, - FROM_CONTROLLED, handPose)) { - _animVars.set("rightHandPosition", handPose.trans()); - _animVars.set("rightHandRotation", handPose.rot()); - _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); - } - } else { - _animVars.unset("rightHandPosition"); - _animVars.unset("rightHandRotation"); - _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); - } - } - - if (params.isLeftFootEnabled) { - _animVars.set("leftFootPosition", params.leftFootPosition); - _animVars.set("leftFootRotation", params.leftFootOrientation); - _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); - - int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, params.leftFootOrientation, hipsIndex); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevLeftFootPoleVectorValid) { - _prevLeftFootPoleVectorValid = true; - _prevLeftFootPoleVector = poleVector; - } - glm::quat deltaRot = rotationBetween(_prevLeftFootPoleVector, poleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); - _prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector; - - _animVars.set("leftFootPoleVectorEnabled", true); - _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z); - _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector); - } else { - _animVars.unset("leftFootPosition"); - _animVars.unset("leftFootRotation"); - _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("leftFootPoleVectorEnabled", false); - _prevLeftFootPoleVectorValid = false; - } - - if (params.isRightFootEnabled) { - _animVars.set("rightFootPosition", params.rightFootPosition); - _animVars.set("rightFootRotation", params.rightFootOrientation); - _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); - - int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, params.rightFootOrientation, hipsIndex); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevRightFootPoleVectorValid) { - _prevRightFootPoleVectorValid = true; - _prevRightFootPoleVector = poleVector; - } - glm::quat deltaRot = rotationBetween(_prevRightFootPoleVector, poleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); - _prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector; - - _animVars.set("rightFootPoleVectorEnabled", true); - _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z); - _animVars.set("rightFootPoleVector", _prevRightFootPoleVector); - } else { - _animVars.unset("rightFootPosition"); - _animVars.unset("rightFootRotation"); - _animVars.set("rightFootPoleVectorEnabled", false); - _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); - } + if (rightArmEnabled) { + _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("rightArmPosition", params.controllerPoses[ControllerType_RightArm].trans()); + _animVars.set("rightArmRotation", params.controllerPoses[ControllerType_RightArm].rot()); + } else { + _animVars.set("rightArmType", (int)IKTarget::Type::Unknown); } } diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index d101425a77..d876b8c22c 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -41,21 +41,26 @@ public: bool useNames; }; - struct HeadParameters { - glm::mat4 hipsMatrix = glm::mat4(); // rig space - glm::mat4 spine2Matrix = glm::mat4(); // rig space - glm::quat rigHeadOrientation = glm::quat(); // rig space (-z forward) - glm::vec3 rigHeadPosition = glm::vec3(); // rig space - glm::vec3 rightArmPosition = glm::vec3(); // rig space - glm::quat rightArmRotation = glm::quat(); // rig space - glm::vec3 leftArmPosition = glm::vec3(); // rig space - glm::quat leftArmRotation = glm::quat(); // rig space - bool hipsEnabled = false; - bool headEnabled = false; - bool spine2Enabled = false; - bool leftArmEnabled = false; - bool rightArmEnabled = false; - bool isTalking = false; + enum ControllerType { + ControllerType_Head = 0, + ControllerType_LeftHand, + ControllerType_RightHand, + ControllerType_Hips, + ControllerType_LeftFoot, + ControllerType_RightFoot, + ControllerType_LeftArm, + ControllerType_RightArm, + ControllerType_Spine2, + NumControllerTypes + }; + + struct ControllerParameters { + AnimPose controllerPoses[NumControllerTypes]; // rig space + bool controllerActiveFlags[NumControllerTypes]; + bool isTalking; + float bodyCapsuleRadius; + float bodyCapsuleHalfHeight; + glm::vec3 bodyCapsuleLocalOffset; }; struct EyeParameters { @@ -67,25 +72,6 @@ public: int rightEyeJointIndex = -1; }; - struct HandAndFeetParameters { - 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 - glm::quat rightOrientation = glm::quat(); // rig space (z forward) - - bool isLeftFootEnabled; - bool isRightFootEnabled; - glm::vec3 leftFootPosition = glm::vec3(); // rig space - glm::quat leftFootOrientation = glm::quat(); // rig space (z forward) - glm::vec3 rightFootPosition = glm::vec3(); // rig space - glm::quat rightFootOrientation = glm::quat(); // rig space (z forward) - }; - enum class CharacterControllerState { Ground = 0, Takeoff, @@ -192,9 +178,8 @@ public: // legacy void clearJointStatePriorities(); - void updateFromHeadParameters(const HeadParameters& params, float dt); + void updateFromControllerParameters(const ControllerParameters& params, float dt); void updateFromEyeParameters(const EyeParameters& params); - void updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt); void initAnimGraph(const QUrl& url); @@ -244,7 +229,11 @@ protected: void applyOverridePoses(); void buildAbsoluteRigPoses(const AnimPoseVec& relativePoses, AnimPoseVec& absolutePosesOut); - void updateHeadAnimVars(const HeadParameters& params); + void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix); + void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, float dt, + const AnimPose& leftHandPose, const AnimPose& rightHandPose, + float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset); + void updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose); void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; From 1979ed7f3a08557d8fae5942b55d7d109befd057 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Jun 2017 16:58:11 -0700 Subject: [PATCH 16/20] Disable elbow pole vector if arm/shoulder pucks are enabled --- libraries/animation/src/Rig.cpp | 8 ++++---- libraries/animation/src/Rig.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 0ee42372b1..ef5bd3c032 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1065,7 +1065,7 @@ void Rig::updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headPos } } -void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, float dt, +void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, const AnimPose& leftHandPose, const AnimPose& rightHandPose, float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset) { @@ -1130,7 +1130,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); - if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + if (!leftArmEnabled && elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); // smooth toward desired pole vector from previous pole vector... to reduce jitter @@ -1213,7 +1213,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); - if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + if (!rightArmEnabled && elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); // smooth toward desired pole vector from previous pole vector... to reduce jitter @@ -1435,7 +1435,7 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo updateHead(headEnabled, hipsEnabled, params.controllerPoses[ControllerType_Head]); - updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, dt, + updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, leftArmEnabled, rightArmEnabled, dt, params.controllerPoses[ControllerType_LeftHand], params.controllerPoses[ControllerType_RightHand], params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index d876b8c22c..413b598079 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -230,7 +230,7 @@ protected: void buildAbsoluteRigPoses(const AnimPoseVec& relativePoses, AnimPoseVec& absolutePosesOut); void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix); - void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, float dt, + void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, const AnimPose& leftHandPose, const AnimPose& rightHandPose, float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset); void updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose); From 0f51236fb06103773c8a0aac3210f95b9ad17506 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 21 Jun 2017 14:36:42 -0700 Subject: [PATCH 17/20] Rig.cpp: take avatar scale into account when computing elbow pole vector --- libraries/animation/src/Rig.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index ef5bd3c032..51480e8ef1 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1383,8 +1383,10 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; // project d onto n. glm::vec3 dProj = d - glm::dot(d, n) * n; - const float LATERAL_OFFSET = 0.333f; - const float VERTICAL_OFFSET = -0.333f; + + float avatarScale = extractUniformScale(_modelOffset); + const float LATERAL_OFFSET = 0.333f * avatarScale; + const float VERTICAL_OFFSET = -0.333f * avatarScale; // give dProj a bit of offset away from the body. glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; From 54af6af651117bbc0c4703abda38b14ff56b505a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Jun 2017 10:38:21 -0700 Subject: [PATCH 18/20] Fix for pole vector stability and knee pole vector computation --- .../animation/src/AnimInverseKinematics.cpp | 45 +++++++++----- libraries/animation/src/Rig.cpp | 62 ++++++++++++------- libraries/animation/src/Rig.h | 2 +- 3 files changed, 71 insertions(+), 38 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 3ea8937eec..e2e0d72d66 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -530,22 +530,33 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const if (dLen > EPSILON) { glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); - glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; - glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; float eProjLen = glm::length(eProj); - float pProjLen = glm::length(pProj); - if (eProjLen > EPSILON && pProjLen > EPSILON) { + const float MIN_EPROJ_LEN = 0.5f; + if (eProjLen < MIN_EPROJ_LEN) { + glm::vec3 midPoint = topPose.trans() + d * 0.5f; + e = midPose.trans() - midPoint; + eProj = e - glm::dot(e, dUnit) * dUnit; + eProjLen = glm::length(eProj); + } + + glm::vec3 p = target.getPoleVector(); + glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; + float pProjLen = glm::length(pProj); + + if (eProjLen > EPSILON && pProjLen > EPSILON) { // as pProjLen become orthognal to d, reduce the amount of rotation. float magnitude = easeOutExpo(pProjLen); - float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); float theta = acosf(dot); glm::vec3 cross = glm::cross(eProj, pProj); - float crossLen = glm::length(cross); - if (crossLen > EPSILON) { - glm::vec3 axis = cross / crossLen; + const float MIN_ADJUSTMENT_ANGLE = 0.001745f; // 0.1 degree + if (theta > MIN_ADJUSTMENT_ANGLE) { + glm::vec3 axis = dUnit; + if (glm::dot(cross, dUnit) < 0) { + axis = -dUnit; + } poleRot = glm::angleAxis(magnitude * theta, axis); } } @@ -562,8 +573,17 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); - glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; + float eProjLen = glm::length(eProj); + const float MIN_EPROJ_LEN = 0.5f; + if (eProjLen < MIN_EPROJ_LEN) { + glm::vec3 midPoint = topPose.trans() + d * 0.5f; + e = midPose.trans() - midPoint; + eProj = e - glm::dot(e, dUnit) * dUnit; + eProjLen = glm::length(eProj); + } + + glm::vec3 p = target.getPoleVector(); glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; const float PROJ_VECTOR_LEN = 10.0f; @@ -573,11 +593,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const geomToWorldPose.xformPoint(topPose.trans()), YELLOW); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(eProj)), + geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(e)), RED); - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(pProj)), - GREEN); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), BLUE); @@ -1060,7 +1077,7 @@ void AnimInverseKinematics::initConstraints() { // y | // | | // | O---O---O RightUpLeg - // z | | Hips2 | + // z | | Hips | // \ | | | // \| | | // x -----+ O O RightLeg diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 51480e8ef1..8466b79d1d 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1265,14 +1265,14 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose int hipsIndex = indexOfJoint("Hips"); if (leftFootEnabled) { - glm::vec3 footPosition = leftFootPose.trans(); - glm::quat footRotation = leftFootPose.rot(); - _animVars.set("leftFootPosition", footPosition); - _animVars.set("leftFootRotation", footRotation); + _animVars.set("leftFootPosition", leftFootPose.trans()); + _animVars.set("leftFootRotation", leftFootPose.rot()); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + int kneeJointIndex = _animSkeleton->nameToJointIndex("LeftLeg"); + int upLegJointIndex = _animSkeleton->nameToJointIndex("LeftUpLeg"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, leftFootPose); // smooth toward desired pole vector from previous pole vector... to reduce jitter if (!_prevLeftFootPoleVectorValid) { @@ -1295,14 +1295,14 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose } if (rightFootEnabled) { - glm::vec3 footPosition = rightFootPose.trans(); - glm::quat footRotation = rightFootPose.rot(); - _animVars.set("rightFootPosition", footPosition); - _animVars.set("rightFootRotation", footRotation); + _animVars.set("rightFootPosition", rightFootPose.trans()); + _animVars.set("rightFootRotation", rightFootPose.rot()); _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + int kneeJointIndex = _animSkeleton->nameToJointIndex("RightLeg"); + int upLegJointIndex = _animSkeleton->nameToJointIndex("RightUpLeg"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, rightFootPose); // smooth toward desired pole vector from previous pole vector... to reduce jitter if (!_prevRightFootPoleVectorValid) { @@ -1375,20 +1375,23 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn AnimPose handPose = _externalPoseSet._absolutePoses[handIndex]; AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex]; AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; + + // ray from hand to arm. glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); float sign = isLeft ? 1.0f : -1.0f; + // form a plane normal to the hips x-axis. glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; - // project d onto n. + + // project d onto this plane glm::vec3 dProj = d - glm::dot(d, n) * n; + // give dProj a bit of offset away from the body. float avatarScale = extractUniformScale(_modelOffset); const float LATERAL_OFFSET = 0.333f * avatarScale; const float VERTICAL_OFFSET = -0.333f * avatarScale; - - // give dProj a bit of offset away from the body. glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; // rotate dProj by 90 degrees to get the poleVector. @@ -1402,19 +1405,32 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn return glm::normalize(poleAdjust * poleVector); } -glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { +glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const { - AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex); - glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; - glm::vec3 footForward = footTargetOrientation * localForward; + AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex]; + AnimPose footPose = targetFootPose; + AnimPose kneePose = _externalPoseSet._absolutePoses[kneeIndex]; + AnimPose upLegPose = _externalPoseSet._absolutePoses[upLegIndex]; - // compute the forward direction of the hips. - glm::quat hipsRotation = _externalPoseSet._absolutePoses[hipsIndex].rot(); - glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; + // ray from foot to upLeg + glm::vec3 d = glm::normalize(footPose.trans() - upLegPose.trans()); - // blend between the hips and the foot. - const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; - return glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); + // form a plane normal to the hips x-axis + glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; + glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; + + // project d onto this plane + glm::vec3 dProj = d - glm::dot(d, n) * n; + + // rotate dProj by 90 degrees to get the poleVector. + glm::vec3 poleVector = glm::angleAxis(-PI / 2.0f, n) * dProj; + + // blend the foot oreintation into the pole vector + glm::quat kneeToFootDelta = footPose.rot() * glm::inverse(kneePose.rot()); + const float WRIST_POLE_ADJUST_FACTOR = 0.5f; + glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, kneeToFootDelta, WRIST_POLE_ADJUST_FACTOR); + + return glm::normalize(poleAdjust * poleVector); } void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 413b598079..c17a7b9c8f 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -239,7 +239,7 @@ protected: void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; - glm::vec3 calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; + glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const; AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) From f3206106812ceb2e376f6dd41c7d9aa3d8a8da64 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Jun 2017 14:06:00 -0700 Subject: [PATCH 19/20] opened up shoulder constraint + other fixes * bent elbows will be away from the body a bit more. * sped up smoothing of pole vectors --- libraries/animation/src/AnimInverseKinematics.cpp | 6 ++++-- libraries/animation/src/Rig.cpp | 6 +++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index e2e0d72d66..0395d4d391 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1122,7 +1122,9 @@ void AnimInverseKinematics::initConstraints() { if (0 == baseName.compare("Arm", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f); + //stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f); + const float TWIST_LIMIT = 5.0f * PI / 8.0f; + stConstraint->setTwistLimits(-TWIST_LIMIT, TWIST_LIMIT); /* KEEP THIS CODE for future experimentation // these directions are approximate swing limits in root-frame @@ -1148,7 +1150,7 @@ void AnimInverseKinematics::initConstraints() { // simple cone std::vector minDots; - const float MAX_HAND_SWING = PI / 2.0f; + const float MAX_HAND_SWING = 5.0f * PI / 8.0f; minDots.push_back(cosf(MAX_HAND_SWING)); stConstraint->setSwingLimits(minDots); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 8466b79d1d..53e3f3ddc0 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1089,7 +1089,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab const bool LEFT_HAND = true; const bool RIGHT_HAND = false; - const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.9f; + const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f; if (leftHandEnabled) { if (!_isLeftHandControlled) { @@ -1260,7 +1260,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose) { - const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.9f; + const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.95f; int hipsIndex = indexOfJoint("Hips"); @@ -1390,7 +1390,7 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn // give dProj a bit of offset away from the body. float avatarScale = extractUniformScale(_modelOffset); - const float LATERAL_OFFSET = 0.333f * avatarScale; + const float LATERAL_OFFSET = 1.0f * avatarScale; const float VERTICAL_OFFSET = -0.333f * avatarScale; glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; From b8c638b2b7214d5e7f508366443a33761dcf8ca1 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Jun 2017 14:36:59 -0700 Subject: [PATCH 20/20] warning fixes --- libraries/animation/src/AnimInverseKinematics.cpp | 2 -- libraries/animation/src/Rig.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 0395d4d391..3e948a4f5b 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -584,8 +584,6 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } glm::vec3 p = target.getPoleVector(); - glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; - const float PROJ_VECTOR_LEN = 10.0f; const float POLE_VECTOR_LEN = 100.0f; glm::vec3 midPoint = (basePose.trans() + topPose.trans()) * 0.5f; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 53e3f3ddc0..3f63f226e7 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1417,7 +1417,6 @@ glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int up // form a plane normal to the hips x-axis glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; - glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; // project d onto this plane glm::vec3 dProj = d - glm::dot(d, n) * n;