From 5f1068c404facaf50389606946413b2b75265b2b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Sat, 10 Oct 2015 15:53:03 -0700 Subject: [PATCH] cleanup and optimization of IK loop --- .../animation/src/AnimInverseKinematics.cpp | 183 +++++++++--------- 1 file changed, 90 insertions(+), 93 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index c9f0960a4f..0d1868d955 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -143,106 +143,114 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vectorgetParentIndex(tipIndex); + if (pivotIndex == -1) { + continue; + } + int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex); + if (pivotsParentIndex == -1) { + // TODO?: handle case where tip's parent is root? + continue; + } + glm::vec3 tipPosition = absolutePoses[tipIndex].trans; - glm::quat tipRotation = absolutePoses[tipIndex].rot; + //glm::quat tipRotation = absolutePoses[tipIndex].rot; // cache tip's parent's absolute rotation so we can recompute the tip's parent-relative // as we proceed walking down the joint chain - int pivotIndex = _skeleton->getParentIndex(tipIndex); - glm::quat tipParentRotation; - if (pivotIndex != -1) { - tipParentRotation = absolutePoses[pivotIndex].rot; - } + glm::quat tipParentRotation = absolutePoses[pivotIndex].rot; // descend toward root, pivoting each joint to get tip closer to target - int ancestorCount = 1; - while (pivotIndex != -1) { + while (pivotsParentIndex != -1) { // compute the two lines that should be aligned glm::vec3 jointPosition = absolutePoses[pivotIndex].trans; glm::vec3 leverArm = tipPosition - jointPosition; - glm::vec3 targetLine = target.getTranslation() - jointPosition; - // compute the swing that would get get tip closer - glm::vec3 axis = glm::cross(leverArm, targetLine); - float axisLength = glm::length(axis); glm::quat deltaRotation; - 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 (reduces IK swing contribution of this joint) - angle /= (float)ancestorCount; - 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 rotation and its target. We compute that delta here and rotate the tipJoint accordingly. - glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentRotation) * 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 violates its constraint - // so we try to twist this pivot to compensate. - glm::quat constrainedTipRotation = deltaRotation * tipParentRotation * 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); - deltaRotation = twistPart * deltaRotation; - } - // we update the tip rotation here to rotate it as close to its target orientation as possible - // before moving on to next pivot - tipRotation = tipParentRotation * tipRelativeRotation; - } - } - ++ancestorCount; - - 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 after swing - // 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); - - // enforce pivot's constraint - 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); + if (targetType == IKTarget::Type::RotationAndPosition) { + // 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 rotation and its target. This is the final parent-relative rotation that the tip joint have + // make to achieve its target rotation. + glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentRotation) * 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 * tipParentRotation * 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; + } + } } } - - // store the rotation change in the accumulator - _accumulators[pivotIndex].add(newRot); + } else if (targetType == IKTarget::Type::HmdHead) { + /* TODO: implement this + // 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(tipRotation); + */ } + + // 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 movement of the tip so we have to compute the modified + // 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); + // this joint has been changed so we check to see if it has the lowest index if (pivotIndex < lowestMovedIndex) { lowestMovedIndex = pivotIndex; @@ -250,10 +258,10 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vectorgetParentIndex(pivotIndex); + pivotIndex = pivotsParentIndex; + pivotsParentIndex = _skeleton->getParentIndex(pivotIndex); } } ++numLoops; @@ -276,17 +284,6 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector