From e6776ef5ebe41cbe121ce672faa7f9197ba0d83c Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 21 Sep 2015 17:29:39 -0700 Subject: [PATCH 1/4] split AnimIK::evaluate() into sub-functions also IK targets now in model-frame instead of root-frame --- .../animation/src/AnimInverseKinematics.cpp | 306 +++++++++--------- .../animation/src/AnimInverseKinematics.h | 10 +- libraries/animation/src/AnimNode.h | 17 + libraries/animation/src/Rig.cpp | 18 +- 4 files changed, 186 insertions(+), 165 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 6af58e89a1..1ccf984815 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -82,22 +82,8 @@ 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) { - - // NOTE: we assume that _relativePoses are up to date (e.g. loadPoses() was just called) - if (_relativePoses.empty()) { - return _relativePoses; - } - - // build a list of targets from _targetVarVec - std::vector targets; +void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector& targets) { + // build a list of valid targets from _targetVarVec and animVars bool removeUnfoundJoints = false; for (auto& targetVar : _targetVarVec) { if (targetVar.jointIndex == -1) { @@ -141,154 +127,160 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar } } } +} - if (targets.empty()) { - // no IK targets but still need to enforce constraints - std::map::iterator constraintItr = _constraints.begin(); - while (constraintItr != _constraints.end()) { - int index = constraintItr->first; - glm::quat rotation = _relativePoses[index].rot; - constraintItr->second->apply(rotation); - _relativePoses[index].rot = rotation; - ++constraintItr; - } - } else { - // clear the accumulators before we start the IK solver - for (auto& accumulatorPair: _accumulators) { - accumulatorPair.second.clear(); - } +void AnimInverseKinematics::solveWithCyclicCoordinateDescent(std::vector& targets) { + // compute absolute poses that correspond to relative target poses + AnimPoseVec absolutePoses; + computeAbsolutePoses(absolutePoses); - // compute absolute poses that correspond to relative target poses - AnimPoseVec absolutePoses; - computeAbsolutePoses(absolutePoses); + // clear the accumulators before we start the IK solver + for (auto& accumulatorPair: _accumulators) { + accumulatorPair.second.clear(); + } - float largestError = 0.0f; - const float ACCEPTABLE_RELATIVE_ERROR = 1.0e-3f; - int numLoops = 0; - const int MAX_IK_LOOPS = 16; - const quint64 MAX_IK_TIME = 10 * USECS_PER_MSEC; - quint64 expiry = usecTimestampNow() + MAX_IK_TIME; - do { - largestError = 0.0f; - int lowestMovedIndex = _relativePoses.size(); - for (auto& target: targets) { - 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]; - targetPose.trans = rootPose.trans + rootPose.rot * targetPose.trans; - targetPose.rot = rootPose.rot * targetPose.rot; - } - - glm::vec3 tip = absolutePoses[tipIndex].trans; - float error = glm::length(targetPose.trans - tip); - - // 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[pivotIndex].trans; - glm::vec3 leverArm = tip - jointPosition; - glm::vec3 targetLine = targetPose.trans - jointPosition; - - // compute the axis of the rotation that would align them - glm::vec3 axis = glm::cross(leverArm, targetLine); - float axisLength = glm::length(axis); - if (axisLength > EPSILON) { - // compute deltaRotation for alignment (brings tip closer to target) - axis /= axisLength; - float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine))); - - // NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is - // still possible for the angle to be zero so we also check that to avoid unnecessary calculations. - if (angle > EPSILON) { - // reduce angle by half: slows convergence but adds stability to IK solution - angle = 0.5f * angle; - glm::quat deltaRotation = glm::angleAxis(angle, axis); - - 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[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[pivotIndex].rot); - } - } - // store the rotation change in the accumulator - _accumulators[pivotIndex].add(newRot); - } - // this joint has been changed so we check to see if it has the lowest index - if (pivotIndex < lowestMovedIndex) { - lowestMovedIndex = pivotIndex; - } - - // keep track of tip's new position as we descend towards root - tip = jointPosition + deltaRotation * leverArm; - error = glm::length(targetPose.trans - tip); - } - } - pivotIndex = _skeleton->getParentIndex(pivotIndex); - } - if (largestError < error) { - largestError = error; - } - } - ++numLoops; - - // harvest accumulated rotations and apply the average - for (auto& accumulatorPair: _accumulators) { - RotationAccumulator& accumulator = accumulatorPair.second; - if (accumulator.size() > 0) { - _relativePoses[accumulatorPair.first].rot = accumulator.getAverage(); - accumulator.clear(); - } - } - - // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex - for (int i = lowestMovedIndex; i <= _maxTargetIndex; ++i) { - int parentIndex = _skeleton->getParentIndex(i); - if (parentIndex != -1) { - absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i]; - } - } - } while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry); - - // finally set the relative rotation of each tip to agree with absolute target rotation + float largestError = 0.0f; + const float ACCEPTABLE_RELATIVE_ERROR = 1.0e-3f; + int numLoops = 0; + const int MAX_IK_LOOPS = 16; + const quint64 MAX_IK_TIME = 10 * USECS_PER_MSEC; + quint64 expiry = usecTimestampNow() + MAX_IK_TIME; + do { + largestError = 0.0f; + int lowestMovedIndex = _relativePoses.size(); for (auto& target: targets) { int tipIndex = target.index; - int parentIndex = _skeleton->getParentIndex(tipIndex); - if (parentIndex != -1) { - AnimPose targetPose = target.pose; - // compute tip's new parent-relative rotation - // Q = Qp * q --> q' = Qp^ * Q - glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetPose.rot; - RotationConstraint* constraint = getConstraint(tipIndex); - if (constraint) { - constraint->apply(newRelativeRotation); - // TODO: ATM the final rotation target just fails but we need to provide - // feedback to the IK system so that it can adjust the bones up the skeleton - // to help this rotation target get met. + AnimPose targetPose = target.pose; + + glm::vec3 tip = absolutePoses[tipIndex].trans; + float error = glm::length(targetPose.trans - tip); + + // 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[pivotIndex].trans; + glm::vec3 leverArm = tip - jointPosition; + glm::vec3 targetLine = targetPose.trans - jointPosition; + + // compute the axis of the rotation that would align them + glm::vec3 axis = glm::cross(leverArm, targetLine); + float axisLength = glm::length(axis); + if (axisLength > EPSILON) { + // compute deltaRotation for alignment (brings tip closer to target) + axis /= axisLength; + float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine))); + + // NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is + // still possible for the angle to be zero so we also check that to avoid unnecessary calculations. + if (angle > EPSILON) { + // reduce angle by half: slows convergence but adds stability to IK solution + angle = 0.5f * angle; + glm::quat deltaRotation = glm::angleAxis(angle, axis); + + 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[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[pivotIndex].rot); + } + } + // store the rotation change in the accumulator + _accumulators[pivotIndex].add(newRot); + } + // this joint has been changed so we check to see if it has the lowest index + if (pivotIndex < lowestMovedIndex) { + lowestMovedIndex = pivotIndex; + } + + // keep track of tip's new position as we descend towards root + tip = jointPosition + deltaRotation * leverArm; + error = glm::length(targetPose.trans - tip); + } } - _relativePoses[tipIndex].rot = newRelativeRotation; - absolutePoses[tipIndex].rot = targetPose.rot; + pivotIndex = _skeleton->getParentIndex(pivotIndex); } + if (largestError < error) { + largestError = error; + } + } + ++numLoops; + + // harvest accumulated rotations and apply the average + for (auto& accumulatorPair: _accumulators) { + RotationAccumulator& accumulator = accumulatorPair.second; + if (accumulator.size() > 0) { + _relativePoses[accumulatorPair.first].rot = accumulator.getAverage(); + accumulator.clear(); + } + } + + // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex + for (int i = lowestMovedIndex; i <= _maxTargetIndex; ++i) { + int parentIndex = _skeleton->getParentIndex(i); + if (parentIndex != -1) { + absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i]; + } + } + } while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry); + + // finally set the relative rotation of each tip to agree with absolute target rotation + for (auto& target: targets) { + int tipIndex = target.index; + int parentIndex = _skeleton->getParentIndex(tipIndex); + if (parentIndex != -1) { + AnimPose targetPose = target.pose; + // compute tip's new parent-relative rotation + // Q = Qp * q --> q' = Qp^ * Q + glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetPose.rot; + RotationConstraint* constraint = getConstraint(tipIndex); + if (constraint) { + constraint->apply(newRelativeRotation); + // TODO: ATM the final rotation target just fails but we need to provide + // feedback to the IK system so that it can adjust the bones up the skeleton + // to help this rotation target get met. + } + _relativePoses[tipIndex].rot = newRelativeRotation; + absolutePoses[tipIndex].rot = targetPose.rot; + } + } +} + +//virtual +const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) { + if (!_relativePoses.empty()) { + // build a list of targets from _targetVarVec + std::vector targets; + computeTargets(animVars, targets); + + if (targets.empty()) { + // no IK targets but still need to enforce constraints + std::map::iterator constraintItr = _constraints.begin(); + while (constraintItr != _constraints.end()) { + int index = constraintItr->first; + glm::quat rotation = _relativePoses[index].rot; + constraintItr->second->apply(rotation); + _relativePoses[index].rot = rotation; + ++constraintItr; + } + } else { + solveWithCyclicCoordinateDescent(targets); } } return _relativePoses; diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index c4bda1be89..f2073c01b8 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -37,6 +37,14 @@ public: virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; protected: + struct IKTarget { + AnimPose pose; + int index; + int rootIndex; + }; + + void computeTargets(const AnimVariantMap& animVars, std::vector& targets); + void solveWithCyclicCoordinateDescent(std::vector& targets); virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton); // for AnimDebugDraw rendering @@ -64,7 +72,7 @@ protected: }; std::map _constraints; - std::map _accumulators; + std::map _accumulators; // class-member to exploit temporal coherency std::vector _targetVarVec; AnimPoseVec _defaultRelativePoses; // poses of the relaxed state AnimPoseVec _relativePoses; // current relative poses diff --git a/libraries/animation/src/AnimNode.h b/libraries/animation/src/AnimNode.h index 9325ef3835..b6f9987f33 100644 --- a/libraries/animation/src/AnimNode.h +++ b/libraries/animation/src/AnimNode.h @@ -87,6 +87,23 @@ public: return evaluate(animVars, dt, triggersOut); } + const AnimPose getRootPose(int jointIndex) const { + AnimPose pose = AnimPose::identity; + if (_skeleton && jointIndex != -1) { + const AnimPoseVec& poses = getPosesInternal(); + int numJoints = (int)(poses.size()); + if (jointIndex < numJoints) { + int parentIndex = _skeleton->getParentIndex(jointIndex); + while (parentIndex != -1 && parentIndex < numJoints) { + jointIndex = parentIndex; + parentIndex = _skeleton->getParentIndex(jointIndex); + } + pose = poses[jointIndex]; + } + } + return pose; + } + protected: void setCurrentFrame(float frame) { diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index b0ffd081c2..29d0bc011b 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -986,7 +986,7 @@ void Rig::updateLeanJoint(int index, float leanSideways, float leanForward, floa void Rig::updateNeckJoint(int index, const HeadParameters& params) { if (index >= 0 && _jointStates[index].getParentIndex() >= 0) { - if (_enableAnimGraph && _animSkeleton) { + if (_enableAnimGraph && _animSkeleton && _animNode) { // the params.localHeadOrientation is composed incorrectly, so re-compose it correctly from pitch, yaw and roll. glm::quat realLocalHeadOrientation = (glm::angleAxis(glm::radians(-params.localHeadRoll), Z_AXIS) * glm::angleAxis(glm::radians(params.localHeadYaw), Y_AXIS) * @@ -995,7 +995,9 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { // There's a theory that when not in hmd, we should _animVars.unset("headPosition"). // However, until that works well, let's always request head be positioned where requested by hmd, camera, or default. - _animVars.set("headPosition", params.localHeadPosition); + int headIndex = _animSkeleton->nameToJointIndex("Head"); + AnimPose rootPose = _animNode->getRootPose(headIndex); + _animVars.set("headPosition", rootPose.trans + rootPose.rot * params.localHeadPosition); } else if (!_enableAnimGraph) { auto& state = _jointStates[index]; @@ -1044,20 +1046,22 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm void Rig::updateFromHandParameters(const HandParameters& params, float dt) { - if (_enableAnimGraph && _animSkeleton) { + if (_enableAnimGraph && _animSkeleton && _animNode) { // TODO: figure out how to obtain the yFlip from where it is actually stored glm::quat yFlipHACK = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)); + int leftHandIndex = _animSkeleton->nameToJointIndex("LeftHand"); + AnimPose rootPose = _animNode->getRootPose(leftHandIndex); if (params.isLeftEnabled) { - _animVars.set("leftHandPosition", yFlipHACK * params.leftPosition); - _animVars.set("leftHandRotation", yFlipHACK * params.leftOrientation); + _animVars.set("leftHandPosition", rootPose.trans + rootPose.rot * yFlipHACK * params.leftPosition); + _animVars.set("leftHandRotation", rootPose.rot * yFlipHACK * params.leftOrientation); } else { _animVars.unset("leftHandPosition"); _animVars.unset("leftHandRotation"); } if (params.isRightEnabled) { - _animVars.set("rightHandPosition", yFlipHACK * params.rightPosition); - _animVars.set("rightHandRotation", yFlipHACK * params.rightOrientation); + _animVars.set("rightHandPosition", rootPose.trans + rootPose.rot * yFlipHACK * params.rightPosition); + _animVars.set("rightHandRotation", rootPose.rot * yFlipHACK * params.rightOrientation); } else { _animVars.unset("rightHandPosition"); _animVars.unset("rightHandRotation"); From 3869887610e10e69ecfb7d2368448905f9b4776b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 21 Sep 2015 17:53:59 -0700 Subject: [PATCH 2/4] splitting AnimNode implementation into two files --- libraries/animation/src/AnimNode.cpp | 54 ++++++++++++++++++++++++++++ libraries/animation/src/AnimNode.h | 48 ++++--------------------- 2 files changed, 61 insertions(+), 41 deletions(-) create mode 100644 libraries/animation/src/AnimNode.cpp diff --git a/libraries/animation/src/AnimNode.cpp b/libraries/animation/src/AnimNode.cpp new file mode 100644 index 0000000000..02d0e1b283 --- /dev/null +++ b/libraries/animation/src/AnimNode.cpp @@ -0,0 +1,54 @@ +// +// AnimNode.cpp +// +// Created by Anthony J. Thibault on 9/2/15. +// Copyright (c) 2015 High Fidelity, Inc. All rights reserved. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "AnimNode.h" + +void AnimNode::removeChild(AnimNode::Pointer child) { + auto iter = std::find(_children.begin(), _children.end(), child); + if (iter != _children.end()) { + _children.erase(iter); + } +} + +AnimNode::Pointer AnimNode::getChild(int i) const { + assert(i >= 0 && i < (int)_children.size()); + return _children[i]; +} + +void AnimNode::setSkeleton(const AnimSkeleton::Pointer skeleton) { + setSkeletonInternal(skeleton); + for (auto&& child : _children) { + child->setSkeleton(skeleton); + } +} + +const AnimPose AnimNode::getRootPose(int jointIndex) const { + AnimPose pose = AnimPose::identity; + if (_skeleton && jointIndex != -1) { + const AnimPoseVec& poses = getPosesInternal(); + int numJoints = (int)(poses.size()); + if (jointIndex < numJoints) { + int parentIndex = _skeleton->getParentIndex(jointIndex); + while (parentIndex != -1 && parentIndex < numJoints) { + jointIndex = parentIndex; + parentIndex = _skeleton->getParentIndex(jointIndex); + } + pose = poses[jointIndex]; + } + } + return pose; +} + +void AnimNode::setCurrentFrame(float frame) { + setCurrentFrameInternal(frame); + for (auto&& child : _children) { + child->setCurrentFrameInternal(frame); + } +} diff --git a/libraries/animation/src/AnimNode.h b/libraries/animation/src/AnimNode.h index b6f9987f33..d5da552a0f 100644 --- a/libraries/animation/src/AnimNode.h +++ b/libraries/animation/src/AnimNode.h @@ -60,25 +60,13 @@ public: // hierarchy accessors void addChild(Pointer child) { _children.push_back(child); } - void removeChild(Pointer child) { - auto iter = std::find(_children.begin(), _children.end(), child); - if (iter != _children.end()) { - _children.erase(iter); - } - } - Pointer getChild(int i) const { - assert(i >= 0 && i < (int)_children.size()); - return _children[i]; - } + void removeChild(Pointer child); + + Pointer getChild(int i) const; int getChildCount() const { return (int)_children.size(); } // pair this AnimNode graph with a skeleton. - void setSkeleton(const AnimSkeleton::Pointer skeleton) { - setSkeletonInternal(skeleton); - for (auto&& child : _children) { - child->setSkeleton(skeleton); - } - } + void setSkeleton(const AnimSkeleton::Pointer skeleton); AnimSkeleton::ConstPointer getSkeleton() const { return _skeleton; } @@ -87,36 +75,14 @@ public: return evaluate(animVars, dt, triggersOut); } - const AnimPose getRootPose(int jointIndex) const { - AnimPose pose = AnimPose::identity; - if (_skeleton && jointIndex != -1) { - const AnimPoseVec& poses = getPosesInternal(); - int numJoints = (int)(poses.size()); - if (jointIndex < numJoints) { - int parentIndex = _skeleton->getParentIndex(jointIndex); - while (parentIndex != -1 && parentIndex < numJoints) { - jointIndex = parentIndex; - parentIndex = _skeleton->getParentIndex(jointIndex); - } - pose = poses[jointIndex]; - } - } - return pose; - } + const AnimPose getRootPose(int jointIndex) const; protected: - void setCurrentFrame(float frame) { - setCurrentFrameInternal(frame); - for (auto&& child : _children) { - child->setCurrentFrameInternal(frame); - } - } + void setCurrentFrame(float frame); virtual void setCurrentFrameInternal(float frame) {} - virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) { - _skeleton = skeleton; - } + virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) { _skeleton = skeleton; } // for AnimDebugDraw rendering virtual const AnimPoseVec& getPosesInternal() const = 0; From 8f1dde69cc1e98d98fc6ef237a09268f8e4cea5f Mon Sep 17 00:00:00 2001 From: Howard Stearns Date: Tue, 22 Sep 2015 10:10:29 -0700 Subject: [PATCH 3/4] Always keep targets, even when both position and rotation are unset. (Get from underpose.) Filtering these was necessary before when the underpose coordinate was wrong, but now that we have that working, there shouldn't be any need to filter. --- .../animation/src/AnimInverseKinematics.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 1ccf984815..57459abacb 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -99,15 +99,13 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: } } 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); - 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); - } + IKTarget target; + AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, _relativePoses); + 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); } } From 7e52d38870d296ead5bf9b3053cfbcaf6686c0fa Mon Sep 17 00:00:00 2001 From: Howard Stearns Date: Tue, 22 Sep 2015 10:12:59 -0700 Subject: [PATCH 4/4] Don't include the root rot, because it seems that this is already accounted for in the head params. Restore the hmd conditional on setting head position. This had been removed when failing to pin it cause lean. I believe that lean was being caused by coordinate system issues that are now addressed by the above and Andrew's big cleanup. --- libraries/animation/src/Rig.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 29d0bc011b..a74560114a 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -993,11 +993,13 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { glm::angleAxis(glm::radians(-params.localHeadPitch), X_AXIS)); _animVars.set("headRotation", realLocalHeadOrientation); - // There's a theory that when not in hmd, we should _animVars.unset("headPosition"). - // However, until that works well, let's always request head be positioned where requested by hmd, camera, or default. - int headIndex = _animSkeleton->nameToJointIndex("Head"); - AnimPose rootPose = _animNode->getRootPose(headIndex); - _animVars.set("headPosition", rootPose.trans + rootPose.rot * params.localHeadPosition); + if (params.isInHMD) { + int headIndex = _animSkeleton->nameToJointIndex("Head"); + AnimPose rootPose = _animNode->getRootPose(headIndex); + _animVars.set("headPosition", rootPose.trans + params.localHeadPosition); // rootPose.rot is handled in params?d + } else { + _animVars.unset("headPosition"); + } } else if (!_enableAnimGraph) { auto& state = _jointStates[index];