From 7b3f688a17241893d563947fa6a904d2822ac08f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 28 Jan 2016 13:18:03 -0800 Subject: [PATCH 1/7] pull CCD IK solution pass into protected method --- .../animation/src/AnimInverseKinematics.cpp | 334 ++++++++++-------- .../animation/src/AnimInverseKinematics.h | 1 + 2 files changed, 183 insertions(+), 152 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index f9c8873779..e616b2ef09 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -105,7 +105,12 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: } target.setPose(rotation, translation); target.setIndex(targetVar.jointIndex); - targets.push_back(target); + if (target.getType() == IKTarget::Type::HmdHead) { + // HmdHead target always goes to beginning of the list + targets.insert(targets.begin(), target); + } else { + targets.push_back(target); + } if (targetVar.jointIndex > _maxTargetIndex) { _maxTargetIndex = targetVar.jointIndex; } @@ -137,6 +142,18 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector= (int)_relativePoses.size()) { continue; } - int tipIndex = target.getIndex(); - int pivotIndex = _skeleton->getParentIndex(tipIndex); - if (pivotIndex == -1 || pivotIndex == _hipsIndex) { - continue; - } - int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex); - if (pivotsParentIndex == -1) { - // TODO?: handle case where tip's parent is root? - continue; - } - - // cache tip's absolute orientation - glm::quat tipOrientation = absolutePoses[tipIndex].rot; - - // also cache tip's parent's absolute orientation so we can recompute - // the tip's parent-relative as we proceed up the chain - glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot; - - if (targetType == IKTarget::Type::HmdHead) { - // rotate tip directly to target orientation - tipOrientation = target.getRotation(); - - // enforce tip's constraint - RotationConstraint* constraint = getConstraint(tipIndex); - if (constraint) { - glm::quat tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation)); - bool constrained = constraint->apply(tipRelativeRotation); - if (constrained) { - tipOrientation = glm::normalize(tipRelativeRotation * tipParentOrientation); - } + // harvest accumulated rotations and apply the average + const int numJoints = (int)_accumulators.size(); + for (int i = 0; i < numJoints; ++i) { + if (_accumulators[i].size() > 0) { + _relativePoses[i].rot = _accumulators[i].getAverage(); + _accumulators[i].clear(); } } - // cache tip absolute position - glm::vec3 tipPosition = absolutePoses[tipIndex].trans; - - // descend toward root, pivoting each joint to get tip closer to target - while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) { - // compute the two lines that should be aligned - glm::vec3 jointPosition = absolutePoses[pivotIndex].trans; - glm::vec3 leverArm = tipPosition - jointPosition; - - glm::quat deltaRotation; - if (targetType == IKTarget::Type::RotationAndPosition || - targetType == IKTarget::Type::HipsRelativeRotationAndPosition) { - // compute the swing that would get get tip closer - glm::vec3 targetLine = target.getTranslation() - jointPosition; - glm::vec3 axis = glm::cross(leverArm, targetLine); - float axisLength = glm::length(axis); - const float MIN_AXIS_LENGTH = 1.0e-4f; - if (axisLength > MIN_AXIS_LENGTH) { - // compute deltaRotation for alignment (swings 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. - const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f; - if (angle > MIN_ADJUSTMENT_ANGLE) { - // reduce angle by a fraction (for stability) - const float fraction = 0.5f; - angle *= fraction; - deltaRotation = glm::angleAxis(angle, axis); - - // The swing will re-orient the tip but there will tend to be be a non-zero delta between the tip's - // new orientation and its target. This is the final parent-relative orientation that the tip joint have - // make to achieve its target orientation. - glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentOrientation) * target.getRotation(); - - // enforce tip's constraint - RotationConstraint* constraint = getConstraint(tipIndex); - if (constraint) { - bool constrained = constraint->apply(tipRelativeRotation); - if (constrained) { - // The tip's final parent-relative rotation would violate its constraint - // so we try to pre-twist this pivot to compensate. - glm::quat constrainedTipRotation = deltaRotation * tipParentOrientation * tipRelativeRotation; - glm::quat missingRotation = target.getRotation() * glm::inverse(constrainedTipRotation); - glm::quat swingPart; - glm::quat twistPart; - glm::vec3 axis = glm::normalize(deltaRotation * leverArm); - swingTwistDecomposition(missingRotation, axis, swingPart, twistPart); - float dotSign = copysignf(1.0f, twistPart.w); - deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * twistPart, fraction)) * deltaRotation; - } - } - } - } - } else if (targetType == IKTarget::Type::HmdHead) { - // An HmdHead target slaves the orientation of the end-effector by distributing rotation - // deltas up the hierarchy. Its target position is enforced later by shifting the hips. - deltaRotation = target.getRotation() * glm::inverse(tipOrientation); - float dotSign = copysignf(1.0f, deltaRotation.w); - const float ANGLE_DISTRIBUTION_FACTOR = 0.45f; - deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR)); + // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex + for (auto i = lowestMovedIndex; i <= _maxTargetIndex; ++i) { + auto parentIndex = _skeleton->getParentIndex((int)i); + if (parentIndex != -1) { + absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i]; } - - // compute joint's new parent-relative rotation after swing - // Q' = dQ * Q and Q = Qp * q --> q' = Qp^ * dQ * Q - glm::quat newRot = glm::normalize(glm::inverse( - absolutePoses[pivotsParentIndex].rot) * - deltaRotation * - absolutePoses[pivotIndex].rot); - - // enforce pivot's constraint - RotationConstraint* constraint = getConstraint(pivotIndex); - if (constraint) { - bool constrained = constraint->apply(newRot); - if (constrained) { - // the constraint will modify the local rotation of the tip so we must - // compute the corresponding model-frame deltaRotation - // Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^ - deltaRotation = absolutePoses[pivotsParentIndex].rot * - newRot * glm::inverse(absolutePoses[pivotIndex].rot); - } - } - - // store the rotation change in the accumulator - _accumulators[pivotIndex].add(newRot, target.getWeight()); - - // 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 transform as we descend towards root - tipPosition = jointPosition + deltaRotation * leverArm; - tipOrientation = glm::normalize(deltaRotation * tipOrientation); - tipParentOrientation = glm::normalize(deltaRotation * tipParentOrientation); - - pivotIndex = pivotsParentIndex; - pivotsParentIndex = _skeleton->getParentIndex(pivotIndex); } } - ++numLoops; - - // harvest accumulated rotations and apply the average - const int numJoints = (int)_accumulators.size(); - for (int i = 0; i < numJoints; ++i) { - if (_accumulators[i].size() > 0) { - _relativePoses[i].rot = _accumulators[i].getAverage(); - _accumulators[i].clear(); - } - } - - // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex - for (auto i = lowestMovedIndex; i <= _maxTargetIndex; ++i) { - auto parentIndex = _skeleton->getParentIndex((int)i); - if (parentIndex != -1) { - absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i]; - } - } - } while (numLoops < MAX_IK_LOOPS); + } // finally set the relative rotation of each tip to agree with absolute target rotation for (auto& target: targets) { @@ -329,6 +212,153 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vectorgetParentIndex(tipIndex); + if (pivotIndex == -1 || pivotIndex == _hipsIndex) { + return lowestMovedIndex; + } + int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex); + if (pivotsParentIndex == -1) { + // TODO?: handle case where tip's parent is root? + return lowestMovedIndex; + } + + // cache tip's absolute orientation + glm::quat tipOrientation = absolutePoses[tipIndex].rot; + + // also cache tip's parent's absolute orientation so we can recompute + // the tip's parent-relative as we proceed up the chain + glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot; + + if (targetType == IKTarget::Type::HmdHead) { + // rotate tip directly to target orientation + tipOrientation = target.getRotation(); + glm::quat tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation)); + + // enforce tip's constraint + RotationConstraint* constraint = getConstraint(tipIndex); + if (constraint) { + bool constrained = constraint->apply(tipRelativeRotation); + if (constrained) { + tipOrientation = glm::normalize(tipRelativeRotation * tipParentOrientation); + tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation)); + } + } + // store the relative rotation change in the accumulator + _accumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); + } + + // cache tip absolute position + glm::vec3 tipPosition = absolutePoses[tipIndex].trans; + + // descend toward root, pivoting each joint to get tip closer to target position + while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) { + // compute the two lines that should be aligned + glm::vec3 jointPosition = absolutePoses[pivotIndex].trans; + glm::vec3 leverArm = tipPosition - jointPosition; + + glm::quat deltaRotation; + if (targetType == IKTarget::Type::RotationAndPosition || + targetType == IKTarget::Type::HipsRelativeRotationAndPosition) { + // compute the swing that would get get tip closer + glm::vec3 targetLine = target.getTranslation() - jointPosition; + glm::vec3 axis = glm::cross(leverArm, targetLine); + float axisLength = glm::length(axis); + const float MIN_AXIS_LENGTH = 1.0e-4f; + if (axisLength > MIN_AXIS_LENGTH) { + // compute deltaRotation for alignment (swings 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. + const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f; + if (angle > MIN_ADJUSTMENT_ANGLE) { + // reduce angle by a fraction (for stability) + const float fraction = 0.5f; + angle *= fraction; + deltaRotation = glm::angleAxis(angle, axis); + + // The swing will re-orient the tip but there will tend to be be a non-zero delta between the tip's + // new orientation and its target. This is the final parent-relative orientation that the tip joint have + // make to achieve its target orientation. + glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentOrientation) * target.getRotation(); + + // enforce tip's constraint + RotationConstraint* constraint = getConstraint(tipIndex); + if (constraint) { + bool constrained = constraint->apply(tipRelativeRotation); + if (constrained) { + // The tip's final parent-relative rotation would violate its constraint + // so we try to pre-twist this pivot to compensate. + glm::quat constrainedTipRotation = deltaRotation * tipParentOrientation * tipRelativeRotation; + glm::quat missingRotation = target.getRotation() * glm::inverse(constrainedTipRotation); + glm::quat swingPart; + glm::quat twistPart; + glm::vec3 axis = glm::normalize(deltaRotation * leverArm); + swingTwistDecomposition(missingRotation, axis, swingPart, twistPart); + float dotSign = copysignf(1.0f, twistPart.w); + deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * twistPart, fraction)) * deltaRotation; + } + } + } + } + } else if (targetType == IKTarget::Type::HmdHead) { + // An HmdHead target slaves the orientation of the end-effector by distributing rotation + // deltas up the hierarchy. Its target position is enforced later (by shifting the hips). + deltaRotation = target.getRotation() * glm::inverse(tipOrientation); + float dotSign = copysignf(1.0f, deltaRotation.w); + const float ANGLE_DISTRIBUTION_FACTOR = 0.45f; + deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR)); + } + + // compute joint's new parent-relative rotation after swing + // Q' = dQ * Q and Q = Qp * q --> q' = Qp^ * dQ * Q + glm::quat newRot = glm::normalize(glm::inverse( + absolutePoses[pivotsParentIndex].rot) * + deltaRotation * + absolutePoses[pivotIndex].rot); + + // enforce pivot's constraint + RotationConstraint* constraint = getConstraint(pivotIndex); + if (constraint) { + bool constrained = constraint->apply(newRot); + if (constrained) { + // the constraint will modify the local rotation of the tip so we must + // compute the corresponding model-frame deltaRotation + // Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^ + deltaRotation = absolutePoses[pivotsParentIndex].rot * newRot * glm::inverse(absolutePoses[pivotIndex].rot); + } + } + + // store the relative rotation change in the accumulator + _accumulators[pivotIndex].add(newRot, target.getWeight()); + + // 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 transform as we descend towards root + tipPosition = jointPosition + deltaRotation * leverArm; + tipOrientation = glm::normalize(deltaRotation * tipOrientation); + tipParentOrientation = glm::normalize(deltaRotation * tipParentOrientation); + + pivotIndex = pivotsParentIndex; + pivotsParentIndex = _skeleton->getParentIndex(pivotIndex); + } + return lowestMovedIndex; +} + //virtual const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) { // don't call this function, call overlay() instead diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 825577b1ae..aeb718668a 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -40,6 +40,7 @@ public: protected: void computeTargets(const AnimVariantMap& animVars, std::vector& targets, const AnimPoseVec& underPoses); void solveWithCyclicCoordinateDescent(const std::vector& targets); + int solveTargetWithCCD(const IKTarget& target, AnimPoseVec& absolutePoses); virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override; // for AnimDebugDraw rendering From 48f6a9c05f69db5060d557ad68077111979de27d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 29 Jan 2016 10:19:12 -0800 Subject: [PATCH 2/7] fix IK looping error after minor refactor --- .../animation/src/AnimInverseKinematics.cpp | 59 ++++++++----------- 1 file changed, 24 insertions(+), 35 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index e616b2ef09..6558ba5f1d 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -142,18 +142,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector= (int)_relativePoses.size()) { - continue; + // harvest accumulated rotations and apply the average + const int numJoints = (int)_accumulators.size(); + for (int i = 0; i < numJoints; ++i) { + if (_accumulators[i].size() > 0) { + _relativePoses[i].rot = _accumulators[i].getAverage(); + _accumulators[i].clear(); } + } - // harvest accumulated rotations and apply the average - const int numJoints = (int)_accumulators.size(); - for (int i = 0; i < numJoints; ++i) { - if (_accumulators[i].size() > 0) { - _relativePoses[i].rot = _accumulators[i].getAverage(); - _accumulators[i].clear(); - } - } - - // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex - for (auto i = lowestMovedIndex; i <= _maxTargetIndex; ++i) { - auto parentIndex = _skeleton->getParentIndex((int)i); - if (parentIndex != -1) { - absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i]; - } + // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex + for (auto i = lowestMovedIndex; i <= _maxTargetIndex; ++i) { + auto parentIndex = _skeleton->getParentIndex((int)i); + if (parentIndex != -1) { + absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i]; } } } @@ -371,7 +358,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars if (_relativePoses.size() != underPoses.size()) { loadPoses(underPoses); } else { - // relax toward underpose + // relax toward underPoses // 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 @@ -382,8 +369,10 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars for (int i = 0; i < numJoints; ++i) { float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot, underPoses[i].rot)); if (_accumulators[i].isDirty()) { + // this joint is affected by IK --> blend toward underPose rotation _relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * underPoses[i].rot, blend)); } else { + // this joint is NOT affected by IK --> slam to underPose rotation _relativePoses[i].rot = underPoses[i].rot; } _relativePoses[i].trans = underPoses[i].trans; @@ -406,7 +395,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars ++constraintItr; } } else { - // shift the everything according to the _hipsOffset from the previous frame + // shift hips according to the _hipsOffset from the previous frame float offsetLength = glm::length(_hipsOffset); const float MIN_HIPS_OFFSET_LENGTH = 0.03f; if (offsetLength > MIN_HIPS_OFFSET_LENGTH) { @@ -423,14 +412,14 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars hipsFrameRotation *= _relativePoses[index].rot; index = _skeleton->getParentIndex(index); } - _relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans + _relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans + glm::inverse(glm::normalize(hipsFrameRotation)) * (scaleFactor * _hipsOffset); } } solveWithCyclicCoordinateDescent(targets); - // compute the new target hips offset (for next frame) + // measure new _hipsOffset for next frame // by looking for discrepancies between where a targeted endEffector is // and where it wants to be (after IK solutions are done) glm::vec3 newHipsOffset = Vectors::ZERO; @@ -439,7 +428,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars if (targetIndex == _headIndex && _headIndex != -1) { // special handling for headTarget if (target.getType() == IKTarget::Type::RotationOnly) { - // we want to shift the hips to bring the underpose closer + // we want to shift the hips to bring the underPose closer // to where the head happens to be (overpose) glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans; glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans; From 449d566d2aa13a5a34a7a052bf3b3887686ababe Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 29 Jan 2016 10:31:00 -0800 Subject: [PATCH 3/7] properly track lowestMovedIndex --- libraries/animation/src/AnimInverseKinematics.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 6558ba5f1d..6a6abff84c 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -156,7 +156,10 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector Date: Fri, 29 Jan 2016 10:36:22 -0800 Subject: [PATCH 4/7] minor IK optimization: changed accumulators only --- libraries/animation/src/AnimInverseKinematics.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 6a6abff84c..d46cb5b60a 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -164,14 +164,14 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector 0) { _relativePoses[i].rot = _accumulators[i].getAverage(); _accumulators[i].clear(); } } - // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex + // update the absolutePoses that need it (from lowestMovedIndex to _maxTargetIndex) for (auto i = lowestMovedIndex; i <= _maxTargetIndex; ++i) { auto parentIndex = _skeleton->getParentIndex((int)i); if (parentIndex != -1) { From 14ec1b62954073b8d218da2af34ed9d768c22149 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 29 Jan 2016 14:26:50 -0800 Subject: [PATCH 5/7] reduce hand IK coupling to hip position --- .../animation/src/AnimInverseKinematics.cpp | 29 +++++++++++++++---- libraries/animation/src/RotationConstraint.h | 3 ++ .../animation/src/SwingTwistConstraint.cpp | 6 ++-- .../animation/src/SwingTwistConstraint.h | 24 ++++++++------- 4 files changed, 44 insertions(+), 18 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index d46cb5b60a..b62eaca7fd 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -163,7 +163,6 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector 0) { _relativePoses[i].rot = _accumulators[i].getAverage(); @@ -261,16 +260,32 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe targetType == IKTarget::Type::HipsRelativeRotationAndPosition) { // compute the swing that would get get tip closer glm::vec3 targetLine = target.getTranslation() - jointPosition; + + const float MIN_AXIS_LENGTH = 1.0e-4f; + RotationConstraint* constraint = getConstraint(pivotIndex); + if (constraint && constraint->isLowerSpine()) { + // for these types of targets we only allow twist at the lower-spine + // (this prevents the hand targets from bending the spine too much and thereby driving the hips too far) + glm::vec3 twistAxis = absolutePoses[pivotIndex].trans - absolutePoses[pivotsParentIndex].trans; + float twistAxisLength = glm::length(twistAxis); + if (twistAxisLength > MIN_AXIS_LENGTH) { + // project leverArm and targetLine to the plane + twistAxis /= twistAxisLength; + leverArm -= glm::dot(leverArm, twistAxis) * twistAxis; + targetLine -= glm::dot(targetLine, twistAxis) * twistAxis; + } else { + leverArm = Vectors::ZERO; + targetLine = Vectors::ZERO; + } + } + glm::vec3 axis = glm::cross(leverArm, targetLine); float axisLength = glm::length(axis); - const float MIN_AXIS_LENGTH = 1.0e-4f; if (axisLength > MIN_AXIS_LENGTH) { - // compute deltaRotation for alignment (swings tip closer to target) + // compute angle of rotation that 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. const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f; if (angle > MIN_ADJUSTMENT_ANGLE) { // reduce angle by a fraction (for stability) @@ -663,6 +678,10 @@ void AnimInverseKinematics::initConstraints() { const float MAX_SPINE_SWING = PI / 14.0f; minDots.push_back(cosf(MAX_SPINE_SWING)); stConstraint->setSwingLimits(minDots); + if (0 == baseName.compare("Spine1", Qt::CaseInsensitive) + || 0 == baseName.compare("Spine", Qt::CaseInsensitive)) { + stConstraint->setLowerSpine(true); + } constraint = static_cast(stConstraint); } else if (baseName.startsWith("Hips2", Qt::CaseInsensitive)) { diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h index 6926302ec8..0745500582 100644 --- a/libraries/animation/src/RotationConstraint.h +++ b/libraries/animation/src/RotationConstraint.h @@ -28,6 +28,9 @@ public: /// \return true if rotation is clamped virtual bool apply(glm::quat& rotation) const = 0; + /// \return true if this constraint is part of lower spine + virtual bool isLowerSpine() const { return false; } + protected: glm::quat _referenceRotation = glm::quat(); }; diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index c29f75202c..7386fb2bcd 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -123,7 +123,7 @@ void SwingTwistConstraint::setSwingLimits(const std::vector& swungDir // sort limits by theta std::sort(limits.begin(), limits.end()); - + // extrapolate evenly distributed limits for fast lookup table float deltaTheta = TWO_PI / (float)(numLimits); uint32_t rightIndex = 0; @@ -219,7 +219,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { } else { _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY; } - + // clamp the swing // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame). glm::vec3 swungY = swingRotation * yAxis; @@ -232,7 +232,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { float theta = atan2f(-swingAxis.z, swingAxis.x); float minDot = _swingLimitFunction.getMinDot(theta); if (glm::dot(swungY, yAxis) < minDot) { - // The swing limits are violated so we extract the angle from midDot and + // The swing limits are violated so we extract the angle from midDot and // use it to supply a new rotation. swingAxis /= axisLength; swingRotation = glm::angleAxis(acosf(minDot), swingAxis); diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index 155f72a518..f36dc851ea 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -18,20 +18,20 @@ class SwingTwistConstraint : public RotationConstraint { public: - // The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist + // The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist // about the yAxis followed by a swing about some axis that lies in the XZ plane, such that the twist - // and swing combine to produce the rotation. Each partial rotation is constrained within limits + // and swing combine to produce the rotation. Each partial rotation is constrained within limits // then used to construct the new final rotation. SwingTwistConstraint(); /// \param minDots vector of minimum dot products between the twist and swung axes - /// \brief The values are minimum dot-products between the twist axis and the swung axes + /// \brief The values are minimum dot-products between the twist axis and the swung axes /// that correspond to swing axes equally spaced around the XZ plane. Another way to - /// think about it is that the dot-products correspond to correspond to angles (theta) - /// about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary + /// think about it is that the dot-products correspond to correspond to angles (theta) + /// about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary /// conditions are handled internally, so don't duplicate the dot-product at 2PI). - /// See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed + /// See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed /// description of how this works. void setSwingLimits(std::vector minDots); @@ -50,21 +50,24 @@ public: /// \return true if rotation is changed virtual bool apply(glm::quat& rotation) const override; + void setLowerSpine(bool lowerSpine) { _lowerSpine = lowerSpine; } + virtual bool isLowerSpine() const { return _lowerSpine; } + // SwingLimitFunction is an implementation of the constraint check described in the paper: // "The Parameterization of Joint Rotation with the Unit Quaternion" by Quang Liu and Edmond C. Prakash class SwingLimitFunction { public: SwingLimitFunction(); - + /// \brief use a uniform conical swing limit void setCone(float maxAngle); - + /// \brief use a vector of lookup values for swing limits void setMinDots(const std::vector& minDots); - + /// \return minimum dotProduct between reference and swung axes float getMinDot(float theta) const; - + protected: // the limits are stored in a lookup table with cyclic boundary conditions std::vector _minDots; @@ -84,6 +87,7 @@ protected: // We want to remember the LAST clamped boundary, so we an use it even when the far boundary is closer. // This reduces "pops" when the input twist angle goes far beyond and wraps around toward the far boundary. mutable int _lastTwistBoundary; + bool _lowerSpine { false }; }; #endif // hifi_SwingTwistConstraint_h From de54a0ac4bc86c867b693b168eb6702af2853d3a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 29 Jan 2016 14:51:24 -0800 Subject: [PATCH 6/7] remove cruft --- libraries/animation/src/AnimInverseKinematics.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index b62eaca7fd..48287d5c7e 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -105,12 +105,7 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: } target.setPose(rotation, translation); target.setIndex(targetVar.jointIndex); - if (target.getType() == IKTarget::Type::HmdHead) { - // HmdHead target always goes to beginning of the list - targets.insert(targets.begin(), target); - } else { - targets.push_back(target); - } + targets.push_back(target); if (targetVar.jointIndex > _maxTargetIndex) { _maxTargetIndex = targetVar.jointIndex; } @@ -142,7 +137,6 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector Date: Mon, 1 Feb 2016 09:50:01 -0800 Subject: [PATCH 7/7] change string comparisons to be CaseSensitive --- .../animation/src/AnimInverseKinematics.cpp | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 48287d5c7e..9f08ce455a 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -542,16 +542,16 @@ void AnimInverseKinematics::initConstraints() { for (int i = 0; i < numJoints; ++i) { // 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); + bool isLeft = baseName.startsWith("Left", Qt::CaseSensitive); float mirror = isLeft ? -1.0f : 1.0f; if (isLeft) { baseName.remove(0, 4); - } else if (baseName.startsWith("Right", Qt::CaseInsensitive)) { + } else if (baseName.startsWith("Right", Qt::CaseSensitive)) { baseName.remove(0, 5); } RotationConstraint* constraint = nullptr; - if (0 == baseName.compare("Arm", Qt::CaseInsensitive)) { + if (0 == baseName.compare("Arm", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f); @@ -585,7 +585,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setSwingLimits(minDots); constraint = static_cast(stConstraint); - } else if (0 == baseName.compare("UpLeg", Qt::CaseInsensitive)) { + } 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); @@ -611,7 +611,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setSwingLimits(swungDirections); constraint = static_cast(stConstraint); - } else if (0 == baseName.compare("Hand", Qt::CaseInsensitive)) { + } else if (0 == baseName.compare("Hand", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); const float MAX_HAND_TWIST = 3.0f * PI / 5.0f; @@ -650,7 +650,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setSwingLimits(minDots); constraint = static_cast(stConstraint); - } else if (baseName.startsWith("Shoulder", Qt::CaseInsensitive)) { + } else if (baseName.startsWith("Shoulder", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); const float MAX_SHOULDER_TWIST = PI / 20.0f; @@ -662,7 +662,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setSwingLimits(minDots); constraint = static_cast(stConstraint); - } else if (baseName.startsWith("Spine", Qt::CaseInsensitive)) { + } else if (baseName.startsWith("Spine", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); const float MAX_SPINE_TWIST = PI / 12.0f; @@ -672,13 +672,13 @@ void AnimInverseKinematics::initConstraints() { const float MAX_SPINE_SWING = PI / 14.0f; minDots.push_back(cosf(MAX_SPINE_SWING)); stConstraint->setSwingLimits(minDots); - if (0 == baseName.compare("Spine1", Qt::CaseInsensitive) - || 0 == baseName.compare("Spine", Qt::CaseInsensitive)) { + if (0 == baseName.compare("Spine1", Qt::CaseSensitive) + || 0 == baseName.compare("Spine", Qt::CaseSensitive)) { stConstraint->setLowerSpine(true); } constraint = static_cast(stConstraint); - } else if (baseName.startsWith("Hips2", Qt::CaseInsensitive)) { + } else if (baseName.startsWith("Hips2", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); const float MAX_SPINE_TWIST = PI / 8.0f; @@ -690,7 +690,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setSwingLimits(minDots); constraint = static_cast(stConstraint); - } else if (0 == baseName.compare("Neck", Qt::CaseInsensitive)) { + } else if (0 == baseName.compare("Neck", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); const float MAX_NECK_TWIST = PI / 9.0f; @@ -702,7 +702,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setSwingLimits(minDots); constraint = static_cast(stConstraint); - } else if (0 == baseName.compare("Head", Qt::CaseInsensitive)) { + } else if (0 == baseName.compare("Head", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); const float MAX_HEAD_TWIST = PI / 9.0f; @@ -714,7 +714,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setSwingLimits(minDots); constraint = static_cast(stConstraint); - } else if (0 == baseName.compare("ForeArm", Qt::CaseInsensitive)) { + } else if (0 == baseName.compare("ForeArm", Qt::CaseSensitive)) { // The elbow joint rotates about the parent-frame's zAxis (-zAxis) for the Right (Left) arm. ElbowConstraint* eConstraint = new ElbowConstraint(); glm::quat referenceRotation = _defaultRelativePoses[i].rot; @@ -745,7 +745,7 @@ void AnimInverseKinematics::initConstraints() { eConstraint->setAngleLimits(minAngle, maxAngle); constraint = static_cast(eConstraint); - } else if (0 == baseName.compare("Leg", Qt::CaseInsensitive)) { + } else if (0 == baseName.compare("Leg", Qt::CaseSensitive)) { // The knee joint rotates about the parent-frame's -xAxis. ElbowConstraint* eConstraint = new ElbowConstraint(); glm::quat referenceRotation = _defaultRelativePoses[i].rot; @@ -776,7 +776,7 @@ void AnimInverseKinematics::initConstraints() { eConstraint->setAngleLimits(minAngle, maxAngle); constraint = static_cast(eConstraint); - } else if (0 == baseName.compare("Foot", Qt::CaseInsensitive)) { + } else if (0 == baseName.compare("Foot", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);