diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 49ff7feda6..863970ccaa 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -9,6 +9,7 @@ #include "AnimInverseKinematics.h" +#include #include #include @@ -54,7 +55,20 @@ void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) con void AnimInverseKinematics::setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar) { // if there are dups, last one wins. - _targetVarVec.push_back(IKTargetVar(jointName, positionVar.toStdString(), rotationVar.toStdString())); + bool found = false; + for (auto& targetVar: _targetVarVec) { + if (targetVar.jointName == jointName) { + // update existing targetVar + targetVar.positionVar = positionVar.toStdString(); + targetVar.rotationVar = rotationVar.toStdString(); + found = true; + break; + } + } + if (!found) { + // create a new entry + _targetVarVec.push_back(IKTargetVar(jointName, positionVar.toStdString(), rotationVar.toStdString())); + } } static int findRootJointInSkeleton(AnimSkeleton::ConstPointer skeleton, int index) { @@ -68,6 +82,12 @@ static int findRootJointInSkeleton(AnimSkeleton::ConstPointer skeleton, int inde return rootIndex; } +struct IKTarget { + AnimPose pose; + int index; + int rootIndex; +}; + //virtual const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) { @@ -76,43 +96,53 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar return _relativePoses; } - // evaluate target vars + // build a list of targets from _targetVarVec + std::vector targets; + bool removeUnfoundJoints = false; for (auto& targetVar : _targetVarVec) { - - // lazy look up of jointIndices and insertion into _absoluteTargets map - if (!targetVar.hasPerformedJointLookup) { - targetVar.jointIndex = _skeleton->nameToJointIndex(targetVar.jointName); - if (targetVar.jointIndex >= 0) { - // insert into _absoluteTargets map - IKTarget target; - target.pose = AnimPose::identity; - target.rootIndex = findRootJointInSkeleton(_skeleton, targetVar.jointIndex); - _absoluteTargets[targetVar.jointIndex] = target; - - if (targetVar.jointIndex > _maxTargetIndex) { - _maxTargetIndex = targetVar.jointIndex; - } + if (targetVar.jointIndex == -1) { + // this targetVar hasn't been validated yet... + int jointIndex = _skeleton->nameToJointIndex(targetVar.jointName); + if (jointIndex >= 0) { + // this targetVar has a valid joint --> cache the indices + targetVar.jointIndex = jointIndex; + targetVar.rootIndex = findRootJointInSkeleton(_skeleton, jointIndex); } else { qCWarning(animation) << "AnimInverseKinematics could not find jointName" << targetVar.jointName << "in skeleton"; + removeUnfoundJoints = true; } - targetVar.hasPerformedJointLookup = true; - } - - if (targetVar.jointIndex >= 0) { - // update pose in _absoluteTargets map - auto iter = _absoluteTargets.find(targetVar.jointIndex); - if (iter != _absoluteTargets.end()) { + } else { + // TODO: get this done without a double-lookup of each var in animVars + if (animVars.hasKey(targetVar.positionVar) || animVars.hasKey(targetVar.rotationVar)) { + IKTarget target; AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, _relativePoses); - iter->second.pose.trans = animVars.lookup(targetVar.positionVar, defaultPose.trans); - iter->second.pose.rot = animVars.lookup(targetVar.rotationVar, defaultPose.rot); + target.pose.trans = animVars.lookup(targetVar.positionVar, defaultPose.trans); + target.pose.rot = animVars.lookup(targetVar.rotationVar, defaultPose.rot); + target.rootIndex = targetVar.rootIndex; + target.index = targetVar.jointIndex; + targets.push_back(target); } } } - // RELAX! Don't do it. - // relaxTowardDefaults(dt); + if (removeUnfoundJoints) { + int numVars = _targetVarVec.size(); + int i = 0; + while (i < numVars) { + if (_targetVarVec[i].jointIndex == -1) { + if (numVars > 1) { + // swap i for last element + _targetVarVec[i] = _targetVarVec[numVars - 1]; + } + _targetVarVec.pop_back(); + --numVars; + } else { + ++i; + } + } + } - if (_absoluteTargets.empty()) { + if (targets.empty()) { // no IK targets but still need to enforce constraints std::map::iterator constraintItr = _constraints.begin(); while (constraintItr != _constraints.end()) { @@ -135,11 +165,11 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar quint64 expiry = usecTimestampNow() + MAX_IK_TIME; do { largestError = 0.0f; - for (auto& targetPair: _absoluteTargets) { + for (auto& target: targets) { int lowestMovedIndex = _relativePoses.size() - 1; - int tipIndex = targetPair.first; - AnimPose targetPose = targetPair.second.pose; - int rootIndex = targetPair.second.rootIndex; + int tipIndex = target.index; + AnimPose targetPose = target.pose; + int rootIndex = target.rootIndex; if (rootIndex != -1) { // transform targetPose into skeleton's absolute frame AnimPose& rootPose = _relativePoses[rootIndex]; @@ -150,11 +180,11 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar glm::vec3 tip = absolutePoses[tipIndex].trans; float error = glm::length(targetPose.trans - tip); - // descend toward root, rotating each joint to get tip closer to target - int index = _skeleton->getParentIndex(tipIndex); - while (index != -1 && error > ACCEPTABLE_RELATIVE_ERROR) { + // descend toward root, pivoting each joint to get tip closer to target + int pivotIndex = _skeleton->getParentIndex(tipIndex); + while (pivotIndex != -1 && error > ACCEPTABLE_RELATIVE_ERROR) { // compute the two lines that should be aligned - glm::vec3 jointPosition = absolutePoses[index].trans; + glm::vec3 jointPosition = absolutePoses[pivotIndex].trans; glm::vec3 leverArm = tip - jointPosition; glm::vec3 targetLine = targetPose.trans - jointPosition; @@ -173,29 +203,34 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar angle = 0.5f * angle; glm::quat deltaRotation = glm::angleAxis(angle, axis); - int parentIndex = _skeleton->getParentIndex(index); + int parentIndex = _skeleton->getParentIndex(pivotIndex); if (parentIndex == -1) { // TODO? apply constraints to root? // TODO? harvest the root's transform as movement of entire skeleton? } else { // compute joint's new parent-relative rotation // Q' = dQ * Q and Q = Qp * q --> q' = Qp^ * dQ * Q - glm::quat newRot = glm::normalize(glm::inverse(absolutePoses[parentIndex].rot) * deltaRotation * absolutePoses[index].rot); - RotationConstraint* constraint = getConstraint(index); + glm::quat newRot = glm::normalize(glm::inverse( + absolutePoses[parentIndex].rot) * + deltaRotation * + absolutePoses[pivotIndex].rot); + RotationConstraint* constraint = getConstraint(pivotIndex); if (constraint) { bool constrained = constraint->apply(newRot); if (constrained) { // the constraint will modify the movement of the tip so we have to compute the modified // model-frame deltaRotation // Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^ - deltaRotation = absolutePoses[parentIndex].rot * newRot * glm::inverse(absolutePoses[index].rot); + deltaRotation = absolutePoses[parentIndex].rot * + newRot * + glm::inverse(absolutePoses[pivotIndex].rot); } } - _relativePoses[index].rot = newRot; + _relativePoses[pivotIndex].rot = newRot; } // this joint has been changed so we check to see if it has the lowest index - if (index < lowestMovedIndex) { - lowestMovedIndex = index; + if (pivotIndex < lowestMovedIndex) { + lowestMovedIndex = pivotIndex; } // keep track of tip's new position as we descend towards root @@ -203,7 +238,7 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar error = glm::length(targetPose.trans - tip); } } - index = _skeleton->getParentIndex(index); + pivotIndex = _skeleton->getParentIndex(pivotIndex); } if (largestError < error) { largestError = error; @@ -248,12 +283,16 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars loadPoses(underPoses); } else { // relax toward underpose - const float RELAXATION_TIMESCALE = 0.125f; - const float alpha = glm::clamp(dt / RELAXATION_TIMESCALE, 0.0f, 1.0f); + // HACK: this relaxation needs to be constant per-frame rather than per-realtime + // in order to prevent IK "flutter" for bad FPS. The bad news is that the good parts + // of this relaxation will be FPS dependent (low FPS will make the limbs align slower + // in real-time), however most people will not notice this and this problem is less + // annoying than the flutter. + const float blend = (1.0f / 60.0f) / (0.25f); // effectively: dt / RELAXATION_TIMESCALE int numJoints = (int)_relativePoses.size(); for (int i = 0; i < numJoints; ++i) { float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot, underPoses[i].rot)); - _relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * underPoses[i].rot, alpha)); + _relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * underPoses[i].rot, blend)); } } return evaluate(animVars, dt, triggersOut); @@ -277,10 +316,6 @@ void AnimInverseKinematics::clearConstraints() { _constraints.clear(); } -const glm::vec3 xAxis(1.0f, 0.0f, 0.0f); -const glm::vec3 yAxis(0.0f, 1.0f, 0.0f); -const glm::vec3 zAxis(0.0f, 0.0f, 1.0f); - void AnimInverseKinematics::initConstraints() { if (!_skeleton) { return; @@ -288,7 +323,7 @@ void AnimInverseKinematics::initConstraints() { // We create constraints for the joints shown here // (and their Left counterparts if applicable). // - // + // // O RightHand // Head / // O / @@ -306,7 +341,7 @@ void AnimInverseKinematics::initConstraints() { // y | // | | // | O---O---O RightUpLeg - // z | | | + // z | | Hips2 | // \ | | | // \| | | // x -----+ O O RightLeg @@ -334,7 +369,7 @@ void AnimInverseKinematics::initConstraints() { _constraints.clear(); for (int i = 0; i < numJoints; ++i) { - // compute the joint's baseName and remember if it was Left or not + // compute the joint's baseName and remember whether its prefix was "Left" or not QString baseName = _skeleton->getJointName(i); bool isLeft = baseName.startsWith("Left", Qt::CaseInsensitive); float mirror = isLeft ? -1.0f : 1.0f; @@ -442,7 +477,7 @@ void AnimInverseKinematics::initConstraints() { const float MAX_HAND_SWING = PI / 2.0f; minDots.push_back(cosf(MAX_HAND_SWING)); stConstraint->setSwingLimits(minDots); - + constraint = static_cast(stConstraint); } else if (baseName.startsWith("Shoulder", Qt::CaseInsensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); @@ -467,6 +502,18 @@ void AnimInverseKinematics::initConstraints() { minDots.push_back(cosf(MAX_SPINE_SWING)); stConstraint->setSwingLimits(minDots); + constraint = static_cast(stConstraint); + } else if (baseName.startsWith("Hips2", Qt::CaseInsensitive)) { + SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); + stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); + const float MAX_SPINE_TWIST = PI / 8.0f; + stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST); + + std::vector minDots; + const float MAX_SPINE_SWING = PI / 14.0f; + minDots.push_back(cosf(MAX_SPINE_SWING)); + stConstraint->setSwingLimits(minDots); + constraint = static_cast(stConstraint); } else if (0 == baseName.compare("Neck", Qt::CaseInsensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); @@ -488,18 +535,18 @@ 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 * zAxis; + glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z; const float MIN_ELBOW_ANGLE = 0.05f; 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) * yAxis; - glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_ELBOW_ANGLE, hingeAxis) * yAxis; + glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y; + glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y; // for the rest of the math we rotate hingeAxis into the child frame hingeAxis = referenceRotation * hingeAxis; eConstraint->setHingeAxis(hingeAxis); - glm::vec3 projectedYAxis = glm::normalize(yAxis - glm::dot(yAxis, hingeAxis) * hingeAxis); + glm::vec3 projectedYAxis = glm::normalize(Vectors::UNIT_Y - glm::dot(Vectors::UNIT_Y, hingeAxis) * hingeAxis); float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis)); if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) { minAngle = - minAngle; @@ -516,21 +563,21 @@ void AnimInverseKinematics::initConstraints() { ElbowConstraint* eConstraint = new ElbowConstraint(); glm::quat referenceRotation = _defaultRelativePoses[i].rot; eConstraint->setReferenceRotation(referenceRotation); - glm::vec3 hingeAxis = -1.0f * xAxis; + glm::vec3 hingeAxis = -1.0f * Vectors::UNIT_X; // 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.0f; const float MAX_KNEE_ANGLE = 3.0f * PI / 4.0f; glm::quat invReferenceRotation = glm::inverse(referenceRotation); - glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * yAxis; - glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_KNEE_ANGLE, hingeAxis) * yAxis; + 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; // for the rest of the math we rotate hingeAxis into the child frame hingeAxis = referenceRotation * hingeAxis; eConstraint->setHingeAxis(hingeAxis); - glm::vec3 projectedYAxis = glm::normalize(yAxis - glm::dot(yAxis, hingeAxis) * hingeAxis); + glm::vec3 projectedYAxis = glm::normalize(Vectors::UNIT_Y - glm::dot(Vectors::UNIT_Y, hingeAxis) * hingeAxis); float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis)); if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) { minAngle = - minAngle; @@ -550,8 +597,8 @@ void AnimInverseKinematics::initConstraints() { // these directions are approximate swing limits in parent-frame // NOTE: they don't need to be normalized std::vector swungDirections; - swungDirections.push_back(yAxis); - swungDirections.push_back(xAxis); + swungDirections.push_back(Vectors::UNIT_Y); + swungDirections.push_back(Vectors::UNIT_X); swungDirections.push_back(glm::vec3(1.0f, 1.0f, 1.0f)); swungDirections.push_back(glm::vec3(1.0f, 1.0f, -1.0f)); @@ -575,13 +622,10 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele AnimNode::setSkeletonInternal(skeleton); // invalidate all targetVars - for (auto& targetVar : _targetVarVec) { - targetVar.hasPerformedJointLookup = false; + for (auto& targetVar: _targetVarVec) { + targetVar.jointIndex = -1; } - // invalidate all targets - _absoluteTargets.clear(); - _maxTargetIndex = 0; if (skeleton) { diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 41e380f64c..eabc3448de 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -50,23 +50,17 @@ protected: rotationVar(rotationVarIn), jointName(jointNameIn), jointIndex(-1), - hasPerformedJointLookup(false) {} + rootIndex(-1) {} std::string positionVar; std::string rotationVar; QString jointName; int jointIndex; // cached joint index - bool hasPerformedJointLookup = false; - }; - - struct IKTarget { - AnimPose pose; - int rootIndex; + int rootIndex; // cached root index }; std::map _constraints; std::vector _targetVarVec; - std::map _absoluteTargets; // IK targets of end-points AnimPoseVec _defaultRelativePoses; // poses of the relaxed state AnimPoseVec _relativePoses; // current relative poses diff --git a/libraries/animation/src/AnimVariant.h b/libraries/animation/src/AnimVariant.h index ac84aafc32..bcff4b2a9b 100644 --- a/libraries/animation/src/AnimVariant.h +++ b/libraries/animation/src/AnimVariant.h @@ -154,6 +154,8 @@ public: void setTrigger(const std::string& key) { _triggers.insert(key); } void clearTriggers() { _triggers.clear(); } + bool hasKey(const std::string& key) const { return _map.find(key) != _map.end(); } + protected: std::map _map; std::set _triggers;