From c3e16d33c1aed162218e1db370167bfd19c26df2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 8 Sep 2015 18:18:50 -0700 Subject: [PATCH 1/7] add InverseKinematics type --- libraries/animation/src/AnimNode.h | 1 + libraries/animation/src/AnimNodeLoader.cpp | 3 +++ 2 files changed, 4 insertions(+) diff --git a/libraries/animation/src/AnimNode.h b/libraries/animation/src/AnimNode.h index 4675ae358f..5c92dd0ecc 100644 --- a/libraries/animation/src/AnimNode.h +++ b/libraries/animation/src/AnimNode.h @@ -40,6 +40,7 @@ public: BlendLinear, Overlay, StateMachine, + InverseKinematics, NumTypes }; using Pointer = std::shared_ptr; diff --git a/libraries/animation/src/AnimNodeLoader.cpp b/libraries/animation/src/AnimNodeLoader.cpp index 866f443005..59d06442ab 100644 --- a/libraries/animation/src/AnimNodeLoader.cpp +++ b/libraries/animation/src/AnimNodeLoader.cpp @@ -42,6 +42,7 @@ static const char* animNodeTypeToString(AnimNode::Type type) { case AnimNode::Type::BlendLinear: return "blendLinear"; case AnimNode::Type::Overlay: return "overlay"; case AnimNode::Type::StateMachine: return "stateMachine"; + case AnimNode::Type::InverseKinematics: return nullptr; case AnimNode::Type::NumTypes: return nullptr; }; return nullptr; @@ -53,6 +54,7 @@ static NodeLoaderFunc animNodeTypeToLoaderFunc(AnimNode::Type type) { case AnimNode::Type::BlendLinear: return loadBlendLinearNode; case AnimNode::Type::Overlay: return loadOverlayNode; case AnimNode::Type::StateMachine: return loadStateMachineNode; + case AnimNode::Type::InverseKinematics: return nullptr; case AnimNode::Type::NumTypes: return nullptr; }; return nullptr; @@ -64,6 +66,7 @@ static NodeProcessFunc animNodeTypeToProcessFunc(AnimNode::Type type) { case AnimNode::Type::BlendLinear: return processBlendLinearNode; case AnimNode::Type::Overlay: return processOverlayNode; case AnimNode::Type::StateMachine: return processStateMachineNode; + case AnimNode::Type::InverseKinematics: return nullptr; case AnimNode::Type::NumTypes: return nullptr; }; return nullptr; From 00c446d1b01320b39cb5caca0e0d56a0e5e87d5c Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 8 Sep 2015 18:19:18 -0700 Subject: [PATCH 2/7] sort includes --- libraries/animation/src/AnimSkeleton.cpp | 8 +++++--- libraries/animation/src/AnimSkeleton.h | 5 ++++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp index 5dfed265a2..a112eb648d 100644 --- a/libraries/animation/src/AnimSkeleton.cpp +++ b/libraries/animation/src/AnimSkeleton.cpp @@ -9,10 +9,12 @@ // #include "AnimSkeleton.h" -#include "AnimationLogging.h" -#include "GLMHelpers.h" + #include -#include + +#include + +#include "AnimationLogging.h" const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f), glm::quat(), diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h index 4a6c3c0087..e1976d929b 100644 --- a/libraries/animation/src/AnimSkeleton.h +++ b/libraries/animation/src/AnimSkeleton.h @@ -12,8 +12,10 @@ #define hifi_AnimSkeleton #include +#include +#include -#include "FBXReader.h" +#include struct AnimPose { AnimPose() {} @@ -58,6 +60,7 @@ public: // relative to parent pose const AnimPose& getRelativeBindPose(int jointIndex) const; + const AnimPoseVec& getRelativeBindPoses() const { return _relativeBindPoses; } int getParentIndex(int jointIndex) const; From b6cef3d1a91b9413af0728208f7658cb32a47bf2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 8 Sep 2015 18:19:33 -0700 Subject: [PATCH 3/7] fix Qt includes --- libraries/animation/src/AnimNodeLoader.h | 2 +- libraries/animation/src/AnimationCache.h | 6 +++--- libraries/networking/src/ResourceCache.h | 22 +++++++++++----------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/animation/src/AnimNodeLoader.h b/libraries/animation/src/AnimNodeLoader.h index 713d980f06..bc7d574c39 100644 --- a/libraries/animation/src/AnimNodeLoader.h +++ b/libraries/animation/src/AnimNodeLoader.h @@ -15,7 +15,7 @@ #include #include -#include +#include #include "AnimNode.h" diff --git a/libraries/animation/src/AnimationCache.h b/libraries/animation/src/AnimationCache.h index 50184862eb..3a8fbf3a61 100644 --- a/libraries/animation/src/AnimationCache.h +++ b/libraries/animation/src/AnimationCache.h @@ -12,9 +12,9 @@ #ifndef hifi_AnimationCache_h #define hifi_AnimationCache_h -#include -#include -#include +#include +#include +#include #include #include diff --git a/libraries/networking/src/ResourceCache.h b/libraries/networking/src/ResourceCache.h index a11ee30174..3bae7f5b9d 100644 --- a/libraries/networking/src/ResourceCache.h +++ b/libraries/networking/src/ResourceCache.h @@ -12,17 +12,17 @@ #ifndef hifi_ResourceCache_h #define hifi_ResourceCache_h -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include From 3d661095dcf82f47750db895569190bca043e25b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 8 Sep 2015 18:20:03 -0700 Subject: [PATCH 4/7] fix some bugs in RotationConstraints --- libraries/animation/src/ElbowConstraint.cpp | 1 - libraries/animation/src/ElbowConstraint.h | 2 - libraries/animation/src/RotationConstraint.h | 12 +- .../animation/src/SwingTwistConstraint.cpp | 108 +++++++++++++++--- .../animation/src/SwingTwistConstraint.h | 15 ++- 5 files changed, 113 insertions(+), 25 deletions(-) diff --git a/libraries/animation/src/ElbowConstraint.cpp b/libraries/animation/src/ElbowConstraint.cpp index a6b2210644..6833c1762e 100644 --- a/libraries/animation/src/ElbowConstraint.cpp +++ b/libraries/animation/src/ElbowConstraint.cpp @@ -15,7 +15,6 @@ #include ElbowConstraint::ElbowConstraint() : - _referenceRotation(), _minAngle(-PI), _maxAngle(PI) { diff --git a/libraries/animation/src/ElbowConstraint.h b/libraries/animation/src/ElbowConstraint.h index 3c4481cfa8..21288715b5 100644 --- a/libraries/animation/src/ElbowConstraint.h +++ b/libraries/animation/src/ElbowConstraint.h @@ -15,12 +15,10 @@ class ElbowConstraint : public RotationConstraint { public: ElbowConstraint(); - virtual void setReferenceRotation(const glm::quat& rotation) override { _referenceRotation = rotation; } void setHingeAxis(const glm::vec3& axis); void setAngleLimits(float minAngle, float maxAngle); virtual bool apply(glm::quat& rotation) const override; protected: - glm::quat _referenceRotation; glm::vec3 _axis; glm::vec3 _perpAxis; float _minAngle; diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h index a05c0d0526..6926302ec8 100644 --- a/libraries/animation/src/RotationConstraint.h +++ b/libraries/animation/src/RotationConstraint.h @@ -15,15 +15,21 @@ class RotationConstraint { public: - RotationConstraint() {} + RotationConstraint() : _referenceRotation() {} virtual ~RotationConstraint() {} - /// \param rotation the default rotation that represents - virtual void setReferenceRotation(const glm::quat& rotation) = 0; + /// \param referenceRotation the rotation from which rotation changes are measured. + virtual void setReferenceRotation(const glm::quat& rotation) { _referenceRotation = rotation; } + + /// \return the rotation from which rotation changes are measured. + const glm::quat& getReferenceRotation() const { return _referenceRotation; } /// \param rotation rotation to clamp /// \return true if rotation is clamped virtual bool apply(glm::quat& rotation) const = 0; + +protected: + glm::quat _referenceRotation = glm::quat(); }; #endif // hifi_RotationConstraint_h diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index 96140f4deb..cd50588f02 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -32,14 +32,20 @@ void SwingTwistConstraint::SwingLimitFunction::setCone(float maxAngle) { } void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector& minDots) { - int numDots = minDots.size(); + uint32_t numDots = minDots.size(); _minDots.clear(); - _minDots.reserve(numDots); - for (int i = 0; i < numDots; ++i) { - _minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT)); + if (numDots == 0) { + // push two copies of MIN_MINDOT + _minDots.push_back(MIN_MINDOT); + _minDots.push_back(MIN_MINDOT); + } else { + _minDots.reserve(numDots); + for (uint32_t i = 0; i < numDots; ++i) { + _minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT)); + } + // push the first value to the back to establish cyclic boundary conditions + _minDots.push_back(_minDots[0]); } - // push the first value to the back to establish cyclic boundary conditions - _minDots.push_back(_minDots[0]); } float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const { @@ -58,8 +64,8 @@ float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const { } SwingTwistConstraint::SwingTwistConstraint() : + RotationConstraint(), _swingLimitFunction(), - _referenceRotation(), _minTwist(-PI), _maxTwist(PI) { @@ -69,6 +75,85 @@ void SwingTwistConstraint::setSwingLimits(std::vector minDots) { _swingLimitFunction.setMinDots(minDots); } +void SwingTwistConstraint::setSwingLimits(const std::vector& swungDirections) { + struct SwingLimitData { + SwingLimitData() : _theta(0.0f), _minDot(1.0f) {} + SwingLimitData(float theta, float minDot) : _theta(theta), _minDot(minDot) {} + float _theta; + float _minDot; + bool operator<(const SwingLimitData& other) const { return _theta < other._theta; } + }; + std::vector limits; + + uint32_t numLimits = swungDirections.size(); + limits.reserve(numLimits); + + // compute the limit pairs: + const glm::vec3 yAxis = glm::vec3(0.0f, 1.0f, 0.0f); + for (uint32_t i = 0; i < numLimits; ++i) { + float directionLength = glm::length(swungDirections[i]); + if (directionLength > EPSILON) { + glm::vec3 swingAxis = glm::cross(yAxis, swungDirections[i]); + float theta = atan2f(-swingAxis.z, swingAxis.x); + if (theta < 0.0f) { + theta += TWO_PI; + } + limits.push_back(SwingLimitData(theta, swungDirections[i].y / directionLength)); + } + } + + std::vector minDots; + numLimits = limits.size(); + if (numLimits == 0) { + // trivial case: nearly free constraint + std::vector minDots; + _swingLimitFunction.setMinDots(minDots); + } else if (numLimits == 1) { + // trivial case: uniform conical constraint + std::vector minDots; + minDots.push_back(limits[0]._minDot); + _swingLimitFunction.setMinDots(minDots); + } else { + // interesting case: potentially non-uniform constraints + + // 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; + for (uint32_t i = 0; i < numLimits; ++i) { + float theta = (float)i * deltaTheta; + uint32_t leftIndex = (rightIndex - 1) % numLimits; + while (rightIndex < numLimits && theta > limits[rightIndex]._theta) { + leftIndex = rightIndex++; + } + + if (leftIndex == numLimits - 1) { + // we straddle the boundary + rightIndex = 0; + } + + float rightTheta = limits[rightIndex]._theta; + float leftTheta = limits[leftIndex]._theta; + if (leftTheta > rightTheta) { + // we straddle the boundary, but we need to figure out which way to stride + // in order to keep theta between left and right + if (leftTheta > theta) { + leftTheta -= TWO_PI; + } else { + rightTheta += TWO_PI; + } + } + + // blend between the left and right minDots to get the value that corresponds to this theta + float rightWeight = (theta - leftTheta) / (rightTheta - leftTheta); + minDots.push_back((1.0f - rightWeight) * limits[leftIndex]._minDot + rightWeight * limits[rightIndex]._minDot); + } + } + _swingLimitFunction.setMinDots(minDots); +} + void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) { // NOTE: min/maxTwist angles should be in the range [-PI, PI] _minTwist = glm::min(minTwist, maxTwist); @@ -107,13 +192,10 @@ 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 use the maxAngle to supply a new rotation. - float maxAngle = acosf(minDot); - if (minDot < 0.0f) { - maxAngle = PI - maxAngle; - } + // 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(maxAngle, swingAxis); + swingRotation = glm::angleAxis(acosf(minDot), swingAxis); swingWasClamped = true; } } diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index c5e6d9e569..b516e984b4 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -16,7 +16,7 @@ #include -class SwingTwistConstraint : RotationConstraint { +class SwingTwistConstraint : public RotationConstraint { public: // 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 @@ -25,10 +25,7 @@ public: SwingTwistConstraint(); - /// \param referenceRotation the rotation from which rotation changes are measured. - virtual void setReferenceRotation(const glm::quat& referenceRotation) override { _referenceRotation = referenceRotation; } - - /// \param minDots vector of minimum dot products between the twist and swung axes. + /// \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 /// 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) @@ -38,6 +35,13 @@ public: /// description of how this works. void setSwingLimits(std::vector minDots); + /// \param swungDirections vector of directions that lie on the swing limit boundary + /// \brief For each swungDirection we compute the corresponding [theta, minDot] pair. + /// We expect the values of theta to NOT be uniformly spaced around the range [0, TWO_PI] + /// so we'll use the input set to extrapolate a lookup function of evenly spaced values. + void setSwingLimits(const std::vector& swungDirections); + + /// \param minTwist the minimum angle of rotation about the twist axis /// \param maxTwist the maximum angle of rotation about the twist axis void setTwistLimits(float minTwist, float maxTwist); @@ -71,7 +75,6 @@ public: protected: SwingLimitFunction _swingLimitFunction; - glm::quat _referenceRotation; float _minTwist; float _maxTwist; }; From ee265aba4a82bc82670ec2e4a98c03fceb4d2484 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 8 Sep 2015 18:20:22 -0700 Subject: [PATCH 5/7] add AnimInverseKinematics class --- .../animation/src/AnimInverseKinematics.cpp | 639 ++++++++++++++++++ .../animation/src/AnimInverseKinematics.h | 76 +++ .../src/AnimInverseKinematicsTests.cpp | 269 ++++++++ .../src/AnimInverseKinematicsTests.h | 27 + 4 files changed, 1011 insertions(+) create mode 100644 libraries/animation/src/AnimInverseKinematics.cpp create mode 100644 libraries/animation/src/AnimInverseKinematics.h create mode 100644 tests/animation/src/AnimInverseKinematicsTests.cpp create mode 100644 tests/animation/src/AnimInverseKinematicsTests.h 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 From 381828dac39b5683c532fb92635c575d215bee19 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 8 Sep 2015 19:02:16 -0700 Subject: [PATCH 6/7] remove hackery for bad length units in AnimSkeleton --- .../animation/src/AnimInverseKinematics.cpp | 22 ------------------- 1 file changed, 22 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 0f21c6282e..4a50fc61a5 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -26,17 +26,6 @@ AnimInverseKinematics::~AnimInverseKinematics() { 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) { @@ -46,17 +35,6 @@ void AnimInverseKinematics::loadPoses(const AnimPoseVec& 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 { From e86e7601188b89403a7ad5d0562144753d7c80a0 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 8 Sep 2015 19:14:31 -0700 Subject: [PATCH 7/7] remove debug stuff and nerfed constriants --- .../animation/src/AnimInverseKinematics.cpp | 61 ++----------------- 1 file changed, 5 insertions(+), 56 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 4a50fc61a5..ce6499557e 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -11,7 +11,6 @@ #include #include -#include // adebug #include "ElbowConstraint.h" #include "SwingTwistConstraint.h" @@ -73,7 +72,6 @@ void AnimInverseKinematics::updateTarget(int index, const glm::vec3& position, c 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) { @@ -109,7 +107,6 @@ void AnimInverseKinematics::clearAllTargets() { //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; @@ -123,12 +120,7 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar 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 -// } + constraintItr->second->apply(rotation); _relativePoses[index].rot = rotation; ++constraintItr; } @@ -142,7 +134,6 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar 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; @@ -171,27 +162,15 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar // 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 + constraint->apply(newRelativeRotation); + // TODO: ATM the final rotation target may 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; } @@ -218,25 +197,15 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar 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 + // 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; } @@ -272,37 +241,19 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar // 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); - } + 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; } @@ -445,7 +396,6 @@ void AnimInverseKinematics::initConstraints() { 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; @@ -587,7 +537,6 @@ void AnimInverseKinematics::initConstraints() { } if (constraint) { _constraints[i] = constraint; - std::cout << "adebug " << i << " '" << _skeleton->getJointName(i).toStdString() << "' constraint = " << (void*)(constraint) << std::endl; // adebug } } }