diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 1412b45968..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", @@ -86,7 +92,10 @@ "typeVar": "rightFootType", "weightVar": "rightFootWeight", "weight": 1.0, - "flexCoefficients": [1, 0.45, 0.45] + "flexCoefficients": [1, 0.45, 0.45], + "poleVectorEnabledVar": "rightFootPoleVectorEnabled", + "poleReferenceVectorVar": "rightFootPoleReferenceVector", + "poleVectorVar": "rightFootPoleVector" }, { "jointName": "LeftFoot", @@ -95,7 +104,10 @@ "typeVar": "leftFootType", "weightVar": "leftFootWeight", "weight": 1.0, - "flexCoefficients": [1, 0.45, 0.45] + "flexCoefficients": [1, 0.45, 0.45], + "poleVectorEnabledVar": "leftFootPoleVectorEnabled", + "poleReferenceVectorVar": "leftFootPoleReferenceVector", + "poleVectorVar": "leftFootPoleVector" }, { "jointName": "Spine2", 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/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 77437e79b9..3e948a4f5b 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -23,13 +23,40 @@ #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; + } + } +} + +static 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) : + const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, + const QString& poleVectorEnabledVarIn, const QString& poleReferenceVectorVarIn, const QString& poleVectorVarIn) : jointName(jointNameIn), positionVar(positionVarIn), rotationVar(rotationVarIn), typeVar(typeVarIn), weightVar(weightVarIn), + poleVectorEnabledVar(poleVectorEnabledVarIn), + poleReferenceVectorVar(poleReferenceVectorVarIn), + poleVectorVar(poleVectorVarIn), weight(weightIn), numFlexCoefficients(flexCoefficientsIn.size()), jointIndex(-1) @@ -46,6 +73,9 @@ AnimInverseKinematics::IKTargetVar::IKTargetVar(const IKTargetVar& orig) : rotationVar(orig.rotationVar), typeVar(orig.typeVar), weightVar(orig.weightVar), + poleVectorEnabledVar(orig.poleVectorEnabledVar), + poleReferenceVectorVar(orig.poleReferenceVectorVar), + poleVectorVar(orig.poleVectorVar), weight(orig.weight), numFlexCoefficients(orig.numFlexCoefficients), jointIndex(orig.jointIndex) @@ -99,8 +129,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& 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; @@ -138,9 +169,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); @@ -148,6 +179,15 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); + 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) { @@ -298,7 +338,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 +367,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 +378,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; @@ -357,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 @@ -382,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); @@ -440,15 +479,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 +493,127 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const chainDepth++; } + if (target.getPoleVectorEnabled()) { + int topJointIndex = target.getIndex(); + 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; + 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; + if (jointChainInfos[i].jointIndex == topJointIndex) { + topChainIndex = i; + topPose = accum; + } + if (jointChainInfos[i].jointIndex == midJointIndex) { + midPose = accum; + } + if (jointChainInfos[i].jointIndex == baseJointIndex) { + baseChainIndex = i; + basePose = accum; + } + if (jointChainInfos[i].jointIndex == baseParentJointIndex) { + baseParentPose = accum; + } + } + + 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.xformVector(target.getPoleReferenceVector()); + 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; + 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); + 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); + } + } + } + + 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.xformVector(target.getPoleReferenceVector()); + 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(); + 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 + PROJ_VECTOR_LEN * glm::normalize(e)), + RED); + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), + BLUE); + } + + 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; + } + } + } + + 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); } } @@ -548,7 +699,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; @@ -608,7 +760,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) { @@ -632,18 +783,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); } } @@ -654,6 +806,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 @@ -767,6 +920,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); + preconditionRelativePosesToAvoidLimbLock(context, targets); solve(context, targets); } @@ -921,7 +1075,7 @@ void AnimInverseKinematics::initConstraints() { // y | // | | // | O---O---O RightUpLeg - // z | | Hips2 | + // z | | Hips | // \ | | | // \| | | // x -----+ O O RightLeg @@ -966,7 +1120,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 @@ -992,7 +1148,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); @@ -1000,7 +1156,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; @@ -1142,7 +1298,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.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; @@ -1173,8 +1329,8 @@ 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 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; glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y; @@ -1308,6 +1464,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); @@ -1338,13 +1495,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 +1518,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 +1535,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); @@ -1486,7 +1643,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); @@ -1521,6 +1678,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; @@ -1540,7 +1741,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 cc919c1684..d473ae3698 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; @@ -34,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& 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; @@ -67,19 +76,13 @@ 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; 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 { @@ -107,7 +110,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& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar); IKTargetVar(const IKTargetVar& orig); QString jointName; @@ -115,6 +119,9 @@ protected: QString rotationVar; QString typeVar; QString weightVar; + QString poleVectorEnabledVar; + QString poleReferenceVectorVar; + 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..2a1978127d 100644 --- a/libraries/animation/src/AnimNodeLoader.cpp +++ b/libraries/animation/src/AnimNodeLoader.cpp @@ -479,6 +479,9 @@ 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(poleVectorEnabledVar, targetObj); + READ_OPTIONAL_STRING(poleReferenceVectorVar, targetObj); + READ_OPTIONAL_STRING(poleVectorVar, targetObj); auto flexCoefficientsValue = targetObj.value("flexCoefficients"); if (!flexCoefficientsValue.isArray()) { @@ -491,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); + node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleVectorEnabledVar, poleReferenceVectorVar, poleVectorVar); }; READ_OPTIONAL_STRING(solutionSource, jsonObj); 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/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; diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index 011175aedf..5567539659 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -30,10 +30,16 @@ 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; } + 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 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); @@ -46,8 +52,11 @@ public: private: AnimPose _pose; - int _index{-1}; - Type _type{Type::RotationAndPosition}; + glm::vec3 _poleVector; + glm::vec3 _poleReferenceVector; + bool _poleVectorEnabled { false }; + int _index { -1 }; + Type _type { Type::RotationAndPosition }; float _weight; float _flexCoefficients[MAX_FLEX_COEFFICIENTS]; size_t _numFlexCoefficients; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index fbb3d24298..3f63f226e7 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. @@ -1015,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); @@ -1086,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); @@ -1104,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, bool leftArmEnabled, bool rightArmEnabled, 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.95f; + + 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 (!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 + 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 (!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 + 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.95f; + + int hipsIndex = indexOfJoint("Hips"); + + if (leftFootEnabled) { + _animVars.set("leftFootPosition", leftFootPose.trans()); + _animVars.set("leftFootRotation", leftFootPose.rot()); + _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); + 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) { + _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) { + _animVars.set("rightFootPosition", rightFootPose.trans()); + _animVars.set("rightFootRotation", rightFootPose.rot()); + _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); + 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) { + _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. @@ -1145,162 +1359,138 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } -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(); - } +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)); +} - // 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); +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]; - // TODO: add isHipsEnabled - bool bodySensorTrackingEnabled = params.isLeftFootEnabled || params.isRightFootEnabled; + // ray from hand to arm. + glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); - 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; + float sign = isLeft ? 1.0f : -1.0f; - if (params.isLeftEnabled) { - if (!_isLeftHandControlled) { - _leftHandControlTimeRemaining = CONTROL_DURATION; - _isLeftHandControlled = true; - } + // 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; - glm::vec3 handPosition = params.leftPosition; - glm::quat handRotation = params.leftOrientation; + // project d onto this plane + glm::vec3 dProj = d - glm::dot(d, n) * n; - 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(); - } - } + // give dProj a bit of offset away from the body. + float avatarScale = extractUniformScale(_modelOffset); + 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; - 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; - } - } + // rotate dProj by 90 degrees to get the poleVector. + glm::vec3 poleVector = glm::angleAxis(PI / 2.0f, n) * dProjWithOffset; - _animVars.set("leftHandPosition", handPosition); - _animVars.set("leftHandRotation", handRotation); - _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); + // 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); - _lastLeftHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); - } else { - if (_isLeftHandControlled) { - _leftHandRelaxTimeRemaining = RELAX_DURATION; - _isLeftHandControlled = false; - } + return glm::normalize(poleAdjust * poleVector); +} - 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); - } - } +glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const { - if (params.isRightEnabled) { - if (!_isRightHandControlled) { - _rightHandControlTimeRemaining = CONTROL_DURATION; - _isRightHandControlled = true; - } + AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex]; + AnimPose footPose = targetFootPose; + AnimPose kneePose = _externalPoseSet._absolutePoses[kneeIndex]; + AnimPose upLegPose = _externalPoseSet._absolutePoses[upLegIndex]; - glm::vec3 handPosition = params.rightPosition; - glm::quat handRotation = params.rightOrientation; + // ray from foot to upLeg + glm::vec3 d = glm::normalize(footPose.trans() - upLegPose.trans()); - 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(); - } - } + // form a plane normal to the hips x-axis + glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; - 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; - } - } + // project d onto this plane + glm::vec3 dProj = d - glm::dot(d, n) * n; - _animVars.set("rightHandPosition", handPosition); - _animVars.set("rightHandRotation", handRotation); - _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); + // rotate dProj by 90 degrees to get the poleVector. + glm::vec3 poleVector = glm::angleAxis(-PI / 2.0f, n) * dProj; - _lastRightHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); - } else { - if (_isRightHandControlled) { - _rightHandRelaxTimeRemaining = RELAX_DURATION; - _isRightHandControlled = false; - } + // 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); - 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); - } - } + return glm::normalize(poleAdjust * poleVector); +} - if (params.isLeftFootEnabled) { - _animVars.set("leftFootPosition", params.leftFootPosition); - _animVars.set("leftFootRotation", params.leftFootOrientation); - _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); - } else { - _animVars.unset("leftFootPosition"); - _animVars.unset("leftFootRotation"); - _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); - } +void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) { + if (!_animSkeleton || !_animNode) { + return; + } - if (params.isRightFootEnabled) { - _animVars.set("rightFootPosition", params.rightFootPosition); - _animVars.set("rightFootRotation", params.rightFootOrientation); - _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); - } else { - _animVars.unset("rightFootPosition"); - _animVars.unset("rightFootRotation"); - _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); - } + _animVars.set("isTalking", params.isTalking); + _animVars.set("notIsTalking", !params.isTalking); + + 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]; + + updateHead(headEnabled, hipsEnabled, params.controllerPoses[ControllerType_Head]); + + updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, leftArmEnabled, rightArmEnabled, dt, + params.controllerPoses[ControllerType_LeftHand], params.controllerPoses[ControllerType_RightHand], + params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset); + + updateFeet(leftFootEnabled, rightFootEnabled, + params.controllerPoses[ControllerType_LeftFoot], params.controllerPoses[ControllerType_RightFoot]); + + 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 (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 (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); + } + + 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); } } @@ -1486,22 +1676,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, {}, + QString(), QString(), QString()); + ikNode.setTargetVars("RightHand", "rightHandPosition", "rightHandRotation", + "rightHandType", "rightHandWeight", 1.0f, {}, + QString(), QString(), QString()); + ikNode.setTargetVars("LeftFoot", "leftFootPosition", "leftFootRotation", + "leftFootType", "leftFootWeight", 1.0f, {}, + QString(), QString(), QString()); + ikNode.setTargetVars("RightFoot", "rightFootPosition", "rightFootRotation", + "rightFootType", "rightFootWeight", 1.0f, {}, + QString(), QString(), QString()); AnimPose geometryToRig = _modelOffset * _geometryOffset; diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index b5b69fc018..c17a7b9c8f 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, @@ -153,9 +139,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; @@ -195,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); @@ -247,11 +229,18 @@ 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, 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); 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 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) 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) AnimPose _invGeometryOffset; @@ -347,13 +336,12 @@ protected: bool _enableDebugDrawIKConstraints { false }; bool _enableDebugDrawIKChains { false }; -private: QMap _stateHandlers; 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,6 +351,18 @@ private: float _rightHandRelaxTimeRemaining { 0.0f }; AnimPose _lastLeftHandControlledPose; AnimPose _lastRightHandControlledPose; + + glm::vec3 _prevRightFootPoleVector { Vectors::UNIT_Z }; + bool _prevRightFootPoleVectorValid { false }; + + 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__) */ diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index 73f0e37d6c..67452c5d33 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -867,10 +867,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 adee4d57b1..3eb796b763 100644 --- a/libraries/render-utils/src/Model.h +++ b/libraries/render-utils/src/Model.h @@ -177,7 +177,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