diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp new file mode 100644 index 0000000000..0f21c6282e --- /dev/null +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -0,0 +1,639 @@ +// +// AnimInverseKinematics.cpp +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "AnimInverseKinematics.h" + +#include +#include +#include // adebug + +#include "ElbowConstraint.h" +#include "SwingTwistConstraint.h" + +AnimInverseKinematics::AnimInverseKinematics(const std::string& id) : AnimNode(AnimNode::Type::InverseKinematics, id) { +} + +AnimInverseKinematics::~AnimInverseKinematics() { + clearConstraints(); +} + +void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) { + _defaultRelativePoses = poses; + assert(_skeleton && _skeleton->getNumJoints() == (int)poses.size()); + /* + // BUG: sometimes poses are in centimeters but we expect meters. + // HACK WORKAROUND: check for very large translation of hips and scale down as necessary + int hipsIndex = _skeleton->nameToJointIndex("Hips"); + if (hipsIndex > -1 && + glm::length(_defaultRelativePoses[hipsIndex].trans) > 10.0f && + glm::length(_defaultRelativePoses[hipsIndex].scale) > 0.1f) { + _defaultRelativePoses[hipsIndex].scale = glm::vec3(0.01f); + _defaultRelativePoses[hipsIndex].trans *= 0.01f; + } + */ +} + +void AnimInverseKinematics::loadPoses(const AnimPoseVec& poses) { + assert(_skeleton && _skeleton->getNumJoints() == (int)poses.size()); + if (_skeleton->getNumJoints() == (int)poses.size()) { + _relativePoses = poses; + } else { + _relativePoses.clear(); + } + /* + // BUG: sometimes poses are in centimeters but we expect meters. + // HACK WORKAROUND: check for very large translation of hips and scale down as necessary + int hipsIndex = _skeleton->nameToJointIndex("Hips"); + if (hipsIndex > -1 && + glm::length(_relativePoses[hipsIndex].trans) > 10.0f && + glm::length(_relativePoses[hipsIndex].scale) > 0.1f) { + _relativePoses[hipsIndex].scale = glm::vec3(0.01f); + _relativePoses[hipsIndex].trans *= 0.01f; + } + */ +} + +void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) const { + int numJoints = (int)_relativePoses.size(); + absolutePoses.clear(); + absolutePoses.reserve(numJoints); + assert(numJoints <= _skeleton->getNumJoints()); + for (int i = 0; i < numJoints; ++i) { + int parentIndex = _skeleton->getParentIndex(i); + if (parentIndex < 0) { + absolutePoses[i] = _relativePoses[i]; + } else { + absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i]; + } + } +} + +void AnimInverseKinematics::updateTarget(int index, const glm::vec3& position, const glm::quat& rotation) { + std::map::iterator targetItr = _absoluteTargets.find(index); + if (targetItr != _absoluteTargets.end()) { + // update existing target + targetItr->second.pose.rot = rotation; + targetItr->second.pose.trans = position; + } else { + // add new target + assert(index >= 0 && index < _skeleton->getNumJoints()); + + IKTarget target; + target.pose = AnimPose(glm::vec3(1.0f), rotation, position); + + // walk down the skeleton hierarchy to find the joint's root + int rootIndex = -1; + int parentIndex = _skeleton->getParentIndex(index); + while (parentIndex != -1) { + rootIndex = parentIndex; + parentIndex = _skeleton->getParentIndex(parentIndex); + } + target.rootIndex = rootIndex; + std::cout << "adebug adding target for end-effector " << index << " with rootIndex " << rootIndex << std::endl; // adebug + + _absoluteTargets[index] = target; + if (index > _maxTargetIndex) { + _maxTargetIndex = index; + } + } +} + +void AnimInverseKinematics::updateTarget(const QString& name, const glm::vec3& position, const glm::quat& rotation) { + int index = _skeleton->nameToJointIndex(name); + if (index != -1) { + updateTarget(index, position, rotation); + } +} + +void AnimInverseKinematics::clearTarget(int index) { + _absoluteTargets.erase(index); + + // recompute _maxTargetIndex + _maxTargetIndex = 0; + for (auto& targetPair: _absoluteTargets) { + int targetIndex = targetPair.first; + if (targetIndex < _maxTargetIndex) { + _maxTargetIndex = targetIndex; + } + } +} + +void AnimInverseKinematics::clearAllTargets() { + _absoluteTargets.clear(); + _maxTargetIndex = 0; +} + +//virtual +const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) { + static int adebug = 0; ++adebug; + // NOTE: we assume that _relativePoses are up to date (e.g. loadPoses() was just called) + if (_relativePoses.empty()) { + return _relativePoses; + } + + relaxTowardDefaults(dt); + + if (_absoluteTargets.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; +// glm::quat oldRotation = rotation; // adebug +// bool appliedConstraint = constraintItr->second->apply(rotation); +// if (0 == (adebug % 100) && appliedConstraint) { +// float angle = glm::angle(rotation * glm::inverse(oldRotation)); // adebug +// std::cout << "adebug 001 applied constraint to index " << index << std::endl; // adebug +// } + _relativePoses[index].rot = rotation; + ++constraintItr; + } + } else { + // compute absolute poses that correspond to relative target poses + AnimPoseVec absolutePoses; + computeAbsolutePoses(absolutePoses); + + 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 loopStart = usecTimestampNow(); + quint64 expiry = usecTimestampNow() + MAX_IK_TIME; + do { + largestError = 0.0f; + for (auto& targetPair: _absoluteTargets) { + int lowestMovedIndex = _relativePoses.size() - 1; + int tipIndex = targetPair.first; + AnimPose targetPose = targetPair.second.pose; + int rootIndex = targetPair.second.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); + if (error < ACCEPTABLE_RELATIVE_ERROR) { + if (largestError < error) { + largestError = error; + } + // this targetPose has been met + // finally set the relative rotation of the tip to agree with absolute target rotation + int parentIndex = _skeleton->getParentIndex(tipIndex); + if (parentIndex != -1) { + // 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) { +// bool appliedConstraint = false; + if (0 == (adebug % 5)) { // && appliedConstraint) { + //std::cout << "adebug 001 applied constraint to index " << tipIndex << std::endl; // adebug + constraint->applyVerbose(newRelativeRotation); + } else { + 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. + + // TODO: setting the absolutePose.rot is not so simple if the relative rotation had to be constrained + //absolutePoses[tipIndex].rot = targetPose.rot; + } + */ + _relativePoses[tipIndex].rot = newRelativeRotation; + } + + break; + } + + // descend toward root, rotating each joint to get tip closer to target + int index = _skeleton->getParentIndex(tipIndex); + while (index != -1 && error > ACCEPTABLE_RELATIVE_ERROR) { + // compute the two lines that should be aligned + glm::vec3 jointPosition = absolutePoses[index].trans; + glm::vec3 leverArm = tip - jointPosition; + glm::vec3 pivotLine = targetPose.trans - jointPosition; + + // compute the axis of the rotation that would align them + glm::vec3 axis = glm::cross(leverArm, pivotLine); + 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, pivotLine) / (glm::length(leverArm) * glm::length(pivotLine))); + + // 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) { + glm::quat deltaRotation = glm::angleAxis(angle, axis); + + int parentIndex = _skeleton->getParentIndex(index); + if (parentIndex == -1) { + // accumulate any delta at the root's relative +// glm::quat newRot = glm::normalize(deltaRotation * _relativePoses[index].rot); +// _relativePoses[index].rot = newRot; +// absolutePoses[index].rot = newRot; + // 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); + if (constraint) { +// glm::quat oldRot = newRot; // adebug +// bool appliedConstraint = // adebug + constraint->apply(newRot); +// if (0 == (adebug % 100) && appliedConstraint) { +// float angle = glm::angle(newRot * glm::inverse(oldRot)); // adebug +// std::cout << "adebug 001 applied constraint to index " << index << " angle = " << angle << std::endl; // adebug +// } + } + _relativePoses[index].rot = newRot; + } + // this joint has been changed so we check to see if it has the lowest index + if (index < lowestMovedIndex) { + lowestMovedIndex = index; + } + + // keep track of tip's new position as we descend towards root + tip = jointPosition + deltaRotation * leverArm; + error = glm::length(targetPose.trans - tip); + } + } + index = _skeleton->getParentIndex(index); + } + if (largestError < error) { + largestError = error; + } + + if (lowestMovedIndex <= _maxTargetIndex && lowestMovedIndex < tipIndex) { + // only update the absolutePoses that matter: 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]; + } + } + } + + // finally set the relative rotation of the tip to agree with absolute target rotation + int parentIndex = _skeleton->getParentIndex(tipIndex); + if (parentIndex != -1) { + // 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) { +// bool appliedConstraint = false; + if (0 == (adebug % 5)) { // && appliedConstraint) { + //std::cout << "adebug 001 applied constraint to index " << tipIndex << std::endl; // adebug + constraint->applyVerbose(newRelativeRotation); + } else { + 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; + // TODO: setting the absolutePose.rot is not so simple if the relative rotation had to be constrained + absolutePoses[tipIndex].rot = targetPose.rot; + } + } + ++numLoops; + } while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry); + /* + if (0 == (adebug % 20)) { + std::cout << "adebug" + << " l = " << numLoops + << " t = " << float(usecTimestampNow() - loopStart) + << " e = " << largestError + << std::endl; // adebug + } + */ + } + return _relativePoses; +} + +RotationConstraint* AnimInverseKinematics::getConstraint(int index) { + RotationConstraint* constraint = nullptr; + std::map::iterator constraintItr = _constraints.find(index); + if (constraintItr != _constraints.end()) { + constraint = constraintItr->second; + } + return constraint; +} + +void AnimInverseKinematics::clearConstraints() { + std::map::iterator constraintItr = _constraints.begin(); + while (constraintItr != _constraints.end()) { + delete constraintItr->second; + ++constraintItr; + } + _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; + } + // We create constraints for the joints shown here + // (and their Left counterparts if applicable). + // + // O RightHand + // / + // O / + // | O RightForeArm + // Neck O / + // | / + // O-------O-------O----O----O RightArm + // Spine2| + // | + // O Spine1 + // | + // | + // O Spine + // | + // | + // O---O---O RightUpLeg + // | | + // | | + // | | + // O O RightLeg + // | | + // y | | + // | | | + // | O O RightFoot + // z | / / + // \ | O--O O--O + // \| + // x -----+ + + loadDefaultPoses(_skeleton->getRelativeBindPoses()); + + // compute corresponding absolute poses + int numJoints = (int)_defaultRelativePoses.size(); + AnimPoseVec absolutePoses; + absolutePoses.reserve(numJoints); + for (int i = 0; i < numJoints; ++i) { + int parentIndex = _skeleton->getParentIndex(i); + if (parentIndex < 0) { + absolutePoses[i] = _defaultRelativePoses[i]; + } else { + absolutePoses[i] = absolutePoses[parentIndex] * _defaultRelativePoses[i]; + } + } + + _constraints.clear(); + for (int i = 0; i < numJoints; ++i) { + QString name = _skeleton->getJointName(i); + bool isLeft = name.startsWith("Left", Qt::CaseInsensitive); + float mirror = isLeft ? -1.0f : 1.0f; + if (isLeft) { + //name.remove(0, 4); + } else if (name.startsWith("Right", Qt::CaseInsensitive)) { + //name.remove(0, 5); + } + + RotationConstraint* constraint = nullptr; + if (0 == name.compare("Arm", Qt::CaseInsensitive)) { + SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); + stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); + stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f); + + // these directions are approximate swing limits in root-frame + // NOTE: they don't need to be normalized + std::vector swungDirections; + swungDirections.push_back(glm::vec3(mirror * 1.0f, 1.0f, 1.0f)); + swungDirections.push_back(glm::vec3(mirror * 1.0f, 0.0f, 1.0f)); + swungDirections.push_back(glm::vec3(mirror * 1.0f, -1.0f, 0.5f)); + swungDirections.push_back(glm::vec3(mirror * 0.0f, -1.0f, 0.0f)); + swungDirections.push_back(glm::vec3(mirror * 0.0f, -1.0f, -1.0f)); + swungDirections.push_back(glm::vec3(mirror * -0.5f, 0.0f, -1.0f)); + swungDirections.push_back(glm::vec3(mirror * 0.0f, 1.0f, -1.0f)); + swungDirections.push_back(glm::vec3(mirror * 0.0f, 1.0f, 0.0f)); + + // rotate directions into joint-frame + glm::quat invAbsoluteRotation = glm::inverse(absolutePoses[i].rot); + int numDirections = (int)swungDirections.size(); + for (int j = 0; j < numDirections; ++j) { + swungDirections[j] = invAbsoluteRotation * swungDirections[j]; + } + stConstraint->setSwingLimits(swungDirections); + + constraint = static_cast(stConstraint); + } else if (0 == name.compare("UpLeg", Qt::CaseInsensitive)) { + SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); + stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); + stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f); + + // these directions are approximate swing limits in root-frame + // NOTE: they don't need to be normalized + std::vector swungDirections; + swungDirections.push_back(glm::vec3(mirror * 0.25f, 0.0f, 1.0f)); + swungDirections.push_back(glm::vec3(mirror * -0.5f, 0.0f, 1.0f)); + swungDirections.push_back(glm::vec3(mirror * -1.0f, 0.0f, 1.0f)); + swungDirections.push_back(glm::vec3(mirror * -1.0f, 0.0f, 0.0f)); + swungDirections.push_back(glm::vec3(mirror * -0.5f, -0.5f, -1.0f)); + swungDirections.push_back(glm::vec3(mirror * 0.0f, -0.75f, -1.0f)); + swungDirections.push_back(glm::vec3(mirror * 0.25f, -1.0f, 0.0f)); + swungDirections.push_back(glm::vec3(mirror * 0.25f, -1.0f, 1.0f)); + + // rotate directions into joint-frame + glm::quat invAbsoluteRotation = glm::inverse(absolutePoses[i].rot); + int numDirections = (int)swungDirections.size(); + for (int j = 0; j < numDirections; ++j) { + swungDirections[j] = invAbsoluteRotation * swungDirections[j]; + } + stConstraint->setSwingLimits(swungDirections); + + constraint = static_cast(stConstraint); + } else if (0 == name.compare("RightHand", Qt::CaseInsensitive)) { + std::cout << "adebug creating constraint for RightHand" << std::endl; // adebug + SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); + stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); + const float MAX_HAND_TWIST = PI / 2.0f; + stConstraint->setTwistLimits(-MAX_HAND_TWIST, MAX_HAND_TWIST); + + // these directions are approximate swing limits in parent-frame + // NOTE: they don't need to be normalized + std::vector swungDirections; + swungDirections.push_back(glm::vec3(1.0f, 1.0f, 0.0f)); + swungDirections.push_back(glm::vec3(0.75f, 1.0f, -1.0f)); + swungDirections.push_back(glm::vec3(-0.75f, 1.0f, -1.0f)); + swungDirections.push_back(glm::vec3(-1.0f, 1.0f, 0.0f)); + swungDirections.push_back(glm::vec3(-0.75f, 1.0f, 1.0f)); + swungDirections.push_back(glm::vec3(0.75f, 1.0f, 1.0f)); + + // rotate directions into joint-frame + glm::quat invRelativeRotation = glm::inverse(_defaultRelativePoses[i].rot); + int numDirections = (int)swungDirections.size(); + for (int j = 0; j < numDirections; ++j) { + swungDirections[j] = invRelativeRotation * swungDirections[j]; + } + stConstraint->setSwingLimits(swungDirections); + + /* + std::vector minDots; + const float MAX_HAND_SWING = PI / 3.0f; + minDots.push_back(cosf(MAX_HAND_SWING)); + stConstraint->setSwingLimits(minDots); + */ + constraint = static_cast(stConstraint); + } else if (name.startsWith("SpineXXX", 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 == name.compare("NeckXXX", Qt::CaseInsensitive)) { + SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); + stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); + const float MAX_NECK_TWIST = PI / 2.0f; + stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST); + + std::vector minDots; + const float MAX_NECK_SWING = PI / 3.0f; + minDots.push_back(cosf(MAX_NECK_SWING)); + stConstraint->setSwingLimits(minDots); + + constraint = static_cast(stConstraint); + } else if (0 == name.compare("ForeArm", Qt::CaseInsensitive)) { + // 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; + eConstraint->setReferenceRotation(referenceRotation); + + // 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; + const float MIN_ELBOW_ANGLE = 0.0f; + const float MAX_ELBOW_ANGLE = 7.0f * PI / 8.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; + + // 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); + float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis)); + if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) { + minAngle = - minAngle; + } + float maxAngle = acosf(glm::dot(projectedYAxis, maxSwingAxis)); + if (glm::dot(hingeAxis, glm::cross(projectedYAxis, maxSwingAxis)) < 0.0f) { + maxAngle = - maxAngle; + } + eConstraint->setAngleLimits(minAngle, maxAngle); + + constraint = static_cast(eConstraint); + } else if (0 == name.compare("Leg", Qt::CaseInsensitive)) { + // The knee joint rotates about the parent-frame's -xAxis. + ElbowConstraint* eConstraint = new ElbowConstraint(); + glm::quat referenceRotation = _defaultRelativePoses[i].rot; + eConstraint->setReferenceRotation(referenceRotation); + glm::vec3 hingeAxis = -1.0f * xAxis; + + // 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; + + // 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); + float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis)); + if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) { + minAngle = - minAngle; + } + float maxAngle = acosf(glm::dot(projectedYAxis, maxSwingAxis)); + if (glm::dot(hingeAxis, glm::cross(projectedYAxis, maxSwingAxis)) < 0.0f) { + maxAngle = - maxAngle; + } + eConstraint->setAngleLimits(minAngle, maxAngle); + + constraint = static_cast(eConstraint); + } else if (0 == name.compare("Foot", Qt::CaseInsensitive)) { + SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); + stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); + stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f); + + // 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(glm::vec3(1.0f, 1.0f, 1.0f)); + swungDirections.push_back(glm::vec3(1.0f, 1.0f, -1.0f)); + + // rotate directions into joint-frame + glm::quat invRelativeRotation = glm::inverse(_defaultRelativePoses[i].rot); + int numDirections = (int)swungDirections.size(); + for (int j = 0; j < numDirections; ++j) { + swungDirections[j] = invRelativeRotation * swungDirections[j]; + } + stConstraint->setSwingLimits(swungDirections); + + constraint = static_cast(stConstraint); + } + if (constraint) { + _constraints[i] = constraint; + std::cout << "adebug " << i << " '" << _skeleton->getJointName(i).toStdString() << "' constraint = " << (void*)(constraint) << std::endl; // adebug + } + } +} + +void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) { + AnimNode::setSkeletonInternal(skeleton); + if (skeleton) { + initConstraints(); + } else { + clearConstraints(); + } +} + +void AnimInverseKinematics::relaxTowardDefaults(float dt) { + // NOTE: for now we just use a single relaxation timescale for all joints, but in the future + // we could vary the timescale on a per-joint basis or do other fancy things. + + // for each joint: lerp towards the default pose + const float RELAXATION_TIMESCALE = 0.25f; + const float alpha = glm::clamp(dt / RELAXATION_TIMESCALE, 0.0f, 1.0f); + int numJoints = (int)_relativePoses.size(); + for (int i = 0; i < numJoints; ++i) { + float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot, _defaultRelativePoses[i].rot)); + _relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * _defaultRelativePoses[i].rot, alpha)); + } +} + diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h new file mode 100644 index 0000000000..03ca5640eb --- /dev/null +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -0,0 +1,76 @@ +// +// AnimInverseKinematics.h +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_AnimInverseKinematics_h +#define hifi_AnimInverseKinematics_h + +#include + +#include "AnimNode.h" + +class RotationConstraint; + +class AnimInverseKinematics : public AnimNode { +public: + + AnimInverseKinematics(const std::string& id); + virtual ~AnimInverseKinematics() override; + + void loadDefaultPoses(const AnimPoseVec& poses); + void loadPoses(const AnimPoseVec& poses); + const AnimPoseVec& getRelativePoses() const { return _relativePoses; } + void computeAbsolutePoses(AnimPoseVec& absolutePoses) const; + + /// \param index index of end effector + /// \param position target position in the frame of the end-effector's root (not the parent!) + /// \param rotation target rotation in the frame of the end-effector's root (not the parent!) + void updateTarget(int index, const glm::vec3& position, const glm::quat& rotation); + + /// \param name name of end effector + /// \param position target position in the frame of the end-effector's root (not the parent!) + /// \param rotation target rotation in the frame of the end-effector's root (not the parent!) + void updateTarget(const QString& name, const glm::vec3& position, const glm::quat& rotation); + + void clearTarget(int index); + void clearAllTargets(); + + virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) override; + +protected: + virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton); + + // for AnimDebugDraw rendering + virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; } + + void relaxTowardDefaults(float dt); + + RotationConstraint* getConstraint(int index); + void clearConstraints(); + void initConstraints(); + + struct IKTarget { + AnimPose pose; // in root-frame + int rootIndex; // cached root index + }; + + std::map _constraints; + std::map _absoluteTargets; // IK targets of end-points + AnimPoseVec _defaultRelativePoses; // poses of the relaxed state + AnimPoseVec _relativePoses; // current relative poses + + // no copies + AnimInverseKinematics(const AnimInverseKinematics&) = delete; + AnimInverseKinematics& operator=(const AnimInverseKinematics&) = delete; + + // _maxTargetIndex is tracked to help optimize the recalculation of absolute poses + // during the the cyclic coordinate descent algorithm + int _maxTargetIndex = 0; +}; + +#endif // hifi_AnimInverseKinematics_h diff --git a/tests/animation/src/AnimInverseKinematicsTests.cpp b/tests/animation/src/AnimInverseKinematicsTests.cpp new file mode 100644 index 0000000000..3958db1242 --- /dev/null +++ b/tests/animation/src/AnimInverseKinematicsTests.cpp @@ -0,0 +1,269 @@ +// +// AnimInverseKinematicsTests.cpp +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "AnimInverseKinematicsTests.h" + +#include + +#include +#include +#include +#include +#include + +#include "../QTestExtensions.h" + +QTEST_MAIN(AnimInverseKinematicsTests) + +const glm::vec3 origin(0.0f); +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); +const glm::quat identity = glm::quat(); +const glm::quat quaterTurnAroundZ = glm::angleAxis(0.5f * PI, zAxis); + + +void makeTestFBXJoints(std::vector& fbxJoints) { + FBXJoint joint; + joint.isFree = false; + joint.freeLineage.clear(); + joint.parentIndex = -1; + joint.distanceToParent = 1.0f; + joint.boneRadius = 1.0f; + + joint.translation = origin; // T + joint.preTransform = glm::mat4(); // Roff * Rp + joint.preRotation = identity; // Rpre + joint.rotation = identity; // R + joint.postRotation = identity; // Rpost + joint.postTransform = glm::mat4(); // Rp-1 * Soff * Sp * S * Sp-1 + + // World = ParentWorld * T * (Roff * Rp) * Rpre * R * Rpost * (Rp-1 * Soff * Sp * S * Sp-1) + joint.transform = glm::mat4(); + joint.rotationMin = glm::vec3(-PI); + joint.rotationMax = glm::vec3(PI); + joint.inverseDefaultRotation = identity; + joint.inverseBindRotation = identity; + joint.bindTransform = glm::mat4(); + + joint.name = ""; + joint.isSkeletonJoint = false; + + // we make a list of joints that look like this: + // + // A------>B------>C------>D + + joint.name = "A"; + joint.parentIndex = -1; + joint.translation = origin; + fbxJoints.push_back(joint); + + joint.name = "B"; + joint.parentIndex = 0; + joint.translation = xAxis; + fbxJoints.push_back(joint); + + joint.name = "C"; + joint.parentIndex = 1; + joint.translation = xAxis; + fbxJoints.push_back(joint); + + joint.name = "D"; + joint.parentIndex = 2; + joint.translation = xAxis; + fbxJoints.push_back(joint); + + // compute each joint's transform + for (int i = 1; i < (int)fbxJoints.size(); ++i) { + FBXJoint& j = fbxJoints[i]; + int parentIndex = j.parentIndex; + // World = ParentWorld * T * (Roff * Rp) * Rpre * R * Rpost * (Rp-1 * Soff * Sp * S * Sp-1) + j.transform = fbxJoints[parentIndex].transform * + glm::translate(j.translation) * + j.preTransform * + glm::mat4_cast(j.preRotation * j.rotation * j.postRotation) * + j.postTransform; + j.inverseBindRotation = identity; + j.bindTransform = j.transform; + } +} + +void AnimInverseKinematicsTests::testSingleChain() { + std::vector fbxJoints; + AnimPose offset; + makeTestFBXJoints(fbxJoints, offset); + + // create a skeleton and doll + AnimSkeleton* skeleton = new AnimSkeleton(fbxJoints); + AnimSkeleton::Pointer skeletonPtr(skeleton); + AnimInverseKinematics ikDoll("doll"); + ikDoll.setSkeleton(skeletonPtr); + + { // easy test IK of joint C + // load intial poses that look like this: + // + // A------>B------>C------>D + AnimPose pose; + pose.scale = glm::vec3(1.0f); + pose.rot = identity; + pose.trans = origin; + + std::vector poses; + poses.push_back(pose); + + pose.trans = xAxis; + for (int i = 1; i < (int)fbxJoints.size(); ++i) { + poses.push_back(pose); + } + ikDoll.loadPoses(poses); + + // we'll put a target t on D for <2, 1, 0> like this: + // + // t + // + // + // A------>B------>C------>D + // + int indexD = 3; + glm::vec3 targetPosition(2.0f, 1.0f, 0.0f); + glm::quat targetRotation = glm::angleAxis(PI / 2.0f, zAxis); + ikDoll.updateTarget(indexD, targetPosition, targetRotation); + + // the IK solution should be: + // + // D + // | + // | + // A------>B------>C + // + float dt = 1.0f; + ikDoll.evaluate(dt); + + // verify absolute results + // NOTE: since we expect this solution to converge very quickly (one loop) + // we can impose very tight error thresholds. + std::vector absolutePoses; + ikDoll.computeAbsolutePoses(absolutePoses); + float acceptableAngle = 0.0001f; + QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle); + QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle); + QCOMPARE_QUATS(absolutePoses[2].rot, quaterTurnAroundZ, acceptableAngle); + QCOMPARE_QUATS(absolutePoses[3].rot, quaterTurnAroundZ, acceptableAngle); + + QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, EPSILON); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, EPSILON); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, targetPosition, EPSILON); + + // verify relative results + const std::vector& relativePoses = ikDoll.getRelativePoses(); + QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle); + QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle); + QCOMPARE_QUATS(relativePoses[2].rot, quaterTurnAroundZ, acceptableAngle); + QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle); + + QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON); + QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON); + QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON); + QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON); + } + + { // hard test IK of joint C + // load intial poses that look like this: + // + // D<------C + // | + // | + // A------>B + // + AnimPose pose; + pose.scale = glm::vec3(1.0f); + pose.rot = identity; + pose.trans = origin; + + std::vector poses; + poses.push_back(pose); + pose.trans = xAxis; + + pose.rot = quaterTurnAroundZ; + poses.push_back(pose); + poses.push_back(pose); + poses.push_back(pose); + ikDoll.loadPoses(poses); + + // we'll put a target t on D for <3, 0, 0> like this: + // + // D<------C + // | + // | + // A------>B t + // + int indexD = 3; + glm::vec3 targetPosition(3.0f, 0.0f, 0.0f); + glm::quat targetRotation = identity; + ikDoll.updateTarget(indexD, targetPosition, targetRotation); + + // the IK solution should be: + // + // A------>B------>C------>D + // + float dt = 1.0f; + ikDoll.evaluate(dt); + + // verify absolute results + // NOTE: the IK algorithm doesn't converge very fast for full-reach targets, + // so we'll consider the poses as good if they are closer than they started. + // + // NOTE: constraints may help speed up convergence since some joints may get clamped + // to maximum extension. TODO: experiment with tightening the error thresholds when + // constraints are working. + std::vector absolutePoses; + ikDoll.computeAbsolutePoses(absolutePoses); + float acceptableAngle = 0.1f; // radians + QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle); + QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle); + QCOMPARE_QUATS(absolutePoses[2].rot, identity, acceptableAngle); + QCOMPARE_QUATS(absolutePoses[3].rot, identity, acceptableAngle); + + float acceptableDistance = 0.4f; + QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, acceptableDistance); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, acceptableDistance); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, 3.0f * xAxis, acceptableDistance); + + // verify relative results + const std::vector& relativePoses = ikDoll.getRelativePoses(); + QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle); + QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle); + QCOMPARE_QUATS(relativePoses[2].rot, identity, acceptableAngle); + QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle); + + QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON); + QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON); + QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON); + QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON); + } +} + +void AnimInverseKinematicsTests::testBar() { + // test AnimPose math + // TODO: move this to other test file + glm::vec3 transA = glm::vec3(1.0f, 2.0f, 3.0f); + glm::vec3 transB = glm::vec3(5.0f, 7.0f, 9.0f); + glm::quat rot = identity; + glm::vec3 scale = glm::vec3(1.0f); + + AnimPose poseA(scale, rot, transA); + AnimPose poseB(scale, rot, transB); + AnimPose poseC = poseA * poseB; + + glm::vec3 expectedTransC = transA + transB; + QCOMPARE_WITH_ABS_ERROR(expectedTransC, poseC.trans, EPSILON); +} + diff --git a/tests/animation/src/AnimInverseKinematicsTests.h b/tests/animation/src/AnimInverseKinematicsTests.h new file mode 100644 index 0000000000..dd1b18e9ae --- /dev/null +++ b/tests/animation/src/AnimInverseKinematicsTests.h @@ -0,0 +1,27 @@ +// +// AnimInverseKinematicsTests.h +// +// Copyright 2015 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_AnimInverseKinematicsTests_h +#define hifi_AnimInverseKinematicsTests_h + +#include +#include + +inline float getErrorDifference(float a, float b) { + return fabs(a - b); +} + +class AnimInverseKinematicsTests : public QObject { + Q_OBJECT +private slots: + void testSingleChain(); + void testBar(); +}; + +#endif // hifi_AnimInverseKinematicsTests_h