From cff42ab9b0a5b92aac8275d7db20850f3b23f98b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 30 May 2017 19:01:52 -0700 Subject: [PATCH] Working spline spline. --- .../animation/src/AnimInverseKinematics.cpp | 142 ++++++++++++++---- .../animation/src/AnimInverseKinematics.h | 13 +- libraries/animation/src/IKTarget.cpp | 3 + libraries/animation/src/IKTarget.h | 1 + libraries/animation/src/Rig.cpp | 8 +- .../animation/src/RotationAccumulator.cpp | 4 +- .../animation/src/TranslationAccumulator.cpp | 34 +++++ .../animation/src/TranslationAccumulator.h | 42 ++++++ 8 files changed, 207 insertions(+), 40 deletions(-) create mode 100644 libraries/animation/src/TranslationAccumulator.cpp create mode 100644 libraries/animation/src/TranslationAccumulator.h diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 03f1208600..31f7bc63d2 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -60,7 +60,8 @@ AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimN AnimInverseKinematics::~AnimInverseKinematics() { clearConstraints(); - _accumulators.clear(); + _rotationAccumulators.clear(); + _translationAccumulators.clear(); _targetVarVec.clear(); } @@ -73,10 +74,12 @@ void AnimInverseKinematics::loadPoses(const AnimPoseVec& poses) { assert(_skeleton && ((poses.size() == 0) || (_skeleton->getNumJoints() == (int)poses.size()))); if (_skeleton->getNumJoints() == (int)poses.size()) { _relativePoses = poses; - _accumulators.resize(_relativePoses.size()); + _rotationAccumulators.resize(_relativePoses.size()); + _translationAccumulators.resize(_relativePoses.size()); } else { _relativePoses.clear(); - _accumulators.clear(); + _rotationAccumulators.clear(); + _translationAccumulators.clear(); } } @@ -176,14 +179,17 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: } } -void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const AnimContext& context, const std::vector& targets) { +void AnimInverseKinematics::solve(const AnimContext& context, const std::vector& targets) { // compute absolute poses that correspond to relative target poses AnimPoseVec absolutePoses; absolutePoses.resize(_relativePoses.size()); computeAbsolutePoses(absolutePoses); // clear the accumulators before we start the IK solver - for (auto& accumulator: _accumulators) { + for (auto& accumulator : _rotationAccumulators) { + accumulator.clearAndClean(); + } + for (auto& accumulator : _translationAccumulators) { accumulator.clearAndClean(); } @@ -198,14 +204,22 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const AnimContext& // solve all targets for (auto& target: targets) { - solveTargetWithCCD(context, target, absolutePoses, debug); + if (target.getType() == IKTarget::Type::Spline) { + solveTargetWithSpline(context, target, absolutePoses, debug); + } else { + solveTargetWithCCD(context, target, absolutePoses, debug); + } } // harvest accumulated rotations and apply the average for (int i = 0; i < (int)_relativePoses.size(); ++i) { - if (_accumulators[i].size() > 0) { - _relativePoses[i].rot() = _accumulators[i].getAverage(); - _accumulators[i].clear(); + if (_rotationAccumulators[i].size() > 0) { + _relativePoses[i].rot() = _rotationAccumulators[i].getAverage(); + _rotationAccumulators[i].clear(); + } + if (_translationAccumulators[i].size() > 0) { + _relativePoses[i].trans() = _translationAccumulators[i].getAverage(); + _translationAccumulators[i].clear(); } } @@ -237,7 +251,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const AnimContext& int parentIndex = _skeleton->getParentIndex(tipIndex); // update rotationOnly targets that don't lie on the ik chain of other ik targets. - if (parentIndex != -1 && !_accumulators[tipIndex].isDirty() && target.getType() == IKTarget::Type::RotationOnly) { + if (parentIndex != -1 && !_rotationAccumulators[tipIndex].isDirty() && target.getType() == IKTarget::Type::RotationOnly) { const glm::quat& targetRotation = target.getRotation(); // compute tip's new parent-relative rotation // Q = Qp * q --> q' = Qp^ * Q @@ -312,10 +326,13 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } // store the relative rotation change in the accumulator - _accumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); + _rotationAccumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); + + glm::vec3 tipRelativeTranslation = _relativePoses[target.getIndex()].trans(); + _translationAccumulators[tipIndex].add(tipRelativeTranslation); if (debug) { - debugJointMap[tipIndex] = DebugJoint(tipRelativeRotation, constrained); + debugJointMap[tipIndex] = DebugJoint(tipRelativeRotation, tipRelativeTranslation, constrained); } } @@ -423,10 +440,13 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } // store the relative rotation change in the accumulator - _accumulators[pivotIndex].add(newRot, target.getWeight()); + _rotationAccumulators[pivotIndex].add(newRot, target.getWeight()); + + glm::vec3 newTrans = _relativePoses[pivotIndex].trans(); + _translationAccumulators[pivotIndex].add(newTrans); if (debug) { - debugJointMap[pivotIndex] = DebugJoint(newRot, constrained); + debugJointMap[pivotIndex] = DebugJoint(newRot, newTrans, constrained); } // keep track of tip's new transform as we descend towards root @@ -445,6 +465,68 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } } +void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug) { + + std::map debugJointMap; + + const int baseIndex = _hipsIndex; + + // build spline + AnimPose tipPose = AnimPose(glm::vec3(1.0f), target.getRotation(), target.getTranslation()); + AnimPose basePose = absolutePoses[baseIndex]; + + const float BASE_GAIN = 0.5f; + const float TIP_GAIN = 1.0f; + float d = glm::length(basePose.trans() - tipPose.trans()); + glm::vec3 p0 = basePose.trans(); + glm::vec3 m0 = BASE_GAIN * d * (basePose.rot() * Vectors::UNIT_Y); + glm::vec3 p1 = tipPose.trans(); + glm::vec3 m1 = TIP_GAIN * d * (tipPose.rot() * Vectors::UNIT_Y); + + CubicHermiteSplineFunctorWithArcLength spline(p0, m0, p1, m1); + float totalArcLength = spline.arcLength(1.0f); + + // find or create splineJointInfo for the head target + const std::vector* splineJointInfoVec = nullptr; + auto iter = _splineJointInfoMap.find(target.getIndex()); + if (iter != _splineJointInfoMap.end()) { + splineJointInfoVec = &(iter->second); + } else { + computeSplineJointInfosForIKTarget(context, target); + auto iter = _splineJointInfoMap.find(target.getIndex()); + if (iter != _splineJointInfoMap.end()) { + splineJointInfoVec = &(iter->second); + } + } + + if (splineJointInfoVec && splineJointInfoVec->size() > 0) { + const int baseParentIndex = _skeleton->getParentIndex(baseIndex); + AnimPose parentAbsPose = (baseParentIndex >= 0) ? absolutePoses[baseParentIndex] : AnimPose(); + + // go thru splineJointInfoVec backwards (base to tip) + for (int i = (int)splineJointInfoVec->size() - 1; i >= 0; i--) { + const SplineJointInfo& splineJointInfo = (*splineJointInfoVec)[i]; + float t = spline.arcLengthInverse(splineJointInfo.ratio * totalArcLength); + glm::vec3 trans = spline(t); + glm::quat rot = glm::normalize(glm::lerp(basePose.rot(), tipPose.rot(), t)); + AnimPose absPose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose; + AnimPose relPose = parentAbsPose.inverse() * absPose; + _rotationAccumulators[splineJointInfo.jointIndex].add(relPose.rot(), target.getWeight()); + _translationAccumulators[splineJointInfo.jointIndex].add(relPose.trans(), target.getWeight()); + + if (debug) { + debugJointMap[splineJointInfo.jointIndex] = DebugJoint(relPose.rot(), relPose.trans(), false); + } + + parentAbsPose = absPose; + } + } + + if (debug) { + debugDrawIKChain(debugJointMap, context); + } +} + //virtual const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) { // don't call this function, call overlay() instead @@ -566,7 +648,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); - solveWithCyclicCoordinateDescent(context, targets); + solve(context, targets); } if (_hipsTargetIndex < 0) { @@ -1047,7 +1129,10 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele _maxTargetIndex = -1; - for (auto& accumulator: _accumulators) { + for (auto& accumulator: _rotationAccumulators) { + accumulator.clearAndClean(); + } + for (auto& accumulator: _translationAccumulators) { accumulator.clearAndClean(); } @@ -1119,6 +1204,7 @@ void AnimInverseKinematics::debugDrawIKChain(std::map& debugJoi // copy debug joint rotations into the relative poses for (auto& debugJoint : debugJointMap) { poses[debugJoint.first].rot() = debugJoint.second.relRot; + poses[debugJoint.first].trans() = debugJoint.second.relTrans; } // convert relative poses to absolute @@ -1284,7 +1370,7 @@ void AnimInverseKinematics::blendToPoses(const AnimPoseVec& targetPoses, const A int numJoints = (int)_relativePoses.size(); for (int i = 0; i < numJoints; ++i) { float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot(), targetPoses[i].rot())); - if (_accumulators[i].isDirty()) { + if (_rotationAccumulators[i].isDirty()) { // this joint is affected by IK --> blend toward the targetPoses rotation _relativePoses[i].rot() = glm::normalize(glm::lerp(_relativePoses[i].rot(), dotSign * targetPoses[i].rot(), blendFactor)); } else { @@ -1319,13 +1405,12 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s } } -void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext& context, int targetIndex, const IKTarget& target) { +void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) { std::vector splineJointInfoVec; // build default spline - AnimPose geomToRigPose(context.getGeometryToRigMatrix()); - AnimPose tipPose = geomToRigPose * _skeleton->getAbsoluteDefaultPose(target.getIndex()); - AnimPose basePose = geomToRigPose * _skeleton->getAbsoluteDefaultPose(_hipsIndex); + AnimPose tipPose = _skeleton->getAbsoluteDefaultPose(target.getIndex()); + AnimPose basePose = _skeleton->getAbsoluteDefaultPose(_hipsIndex); const float BASE_GAIN = 0.5f; const float TIP_GAIN = 1.0f; @@ -1341,10 +1426,10 @@ void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext CubicHermiteSplineFunctorWithArcLength spline(p0, m0, p1, m1); float totalArcLength = spline.arcLength(1.0f); - int index = _headIndex; + int index = target.getIndex(); int endIndex = _skeleton->getParentIndex(_hipsIndex); while (index != endIndex) { - AnimPose defaultPose = geomToRigPose * _skeleton->getAbsoluteDefaultPose(index); + AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(index); // AJT: FIXME: TODO: this wont work for horizontal splines... float ratio = (defaultPose.trans().y - basePose.trans().y) / baseToTipHeight; @@ -1352,18 +1437,19 @@ void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext // compute offset from default spline pose to default pose. float t = spline.arcLengthInverse(ratio * totalArcLength); AnimPose pose(glm::vec3(1.0f), glm::normalize(glm::lerp(basePose.rot(), tipPose.rot(), t)), spline(t)); - AnimPose offset = pose.inverse() * defaultPose; + AnimPose offsetPose = pose.inverse() * defaultPose; - SplineJointInfo splineJointInfo = { index, ratio, defaultPose, offset }; + SplineJointInfo splineJointInfo = { index, ratio, offsetPose }; splineJointInfoVec.push_back(splineJointInfo); index = _skeleton->getParentIndex(index); } - _splineJointInfoMap[targetIndex] = splineJointInfoVec; + _splineJointInfoMap[target.getIndex()] = splineJointInfoVec; } void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context, const std::vector& targets) { + /* // find head target int headTargetIndex = -1; for (int i = 0; i < (int)targets.size(); i++) { @@ -1388,8 +1474,6 @@ void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context, con } } - // AJT: TODO: render using splineJointInfoVec offset - AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); AnimPose hipsPose = geomToWorldPose * _skeleton->getAbsolutePose(_hipsIndex, _relativePoses); @@ -1437,5 +1521,5 @@ void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context, con DebugDraw::getInstance().addMarker(QString("splineJoint%1").arg(splineJointInfo.jointIndex), pose.rot(), pose.trans(), BLUE); } } - + */ } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 2b2535ac79..42e84530cc 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -19,6 +19,7 @@ #include "IKTarget.h" #include "RotationAccumulator.h" +#include "TranslationAccumulator.h" class RotationConstraint; @@ -58,20 +59,22 @@ public: protected: void computeTargets(const AnimVariantMap& animVars, std::vector& targets, const AnimPoseVec& underPoses); - void solveWithCyclicCoordinateDescent(const AnimContext& context, const std::vector& targets); + void solve(const AnimContext& context, const std::vector& targets); void solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug); + void solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug); virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override; struct DebugJoint { DebugJoint() : relRot(), constrained(false) {} - DebugJoint(const glm::quat& relRotIn, bool constrainedIn) : relRot(relRotIn), constrained(constrainedIn) {} + DebugJoint(const glm::quat& relRotIn, const glm::vec3& relTransIn, bool constrainedIn) : relRot(relRotIn), relTrans(relTransIn), constrained(constrainedIn) {} glm::quat relRot; + glm::vec3 relTrans; bool constrained; }; void debugDrawIKChain(std::map& debugJointMap, const AnimContext& context) const; void debugDrawRelativePoses(const AnimContext& context) const; void debugDrawConstraints(const AnimContext& context) const; void debugDrawSpineSpline(const AnimContext& context, const std::vector& targets); - void computeSplineJointInfosForIKTarget(const AnimContext& context, int targetIndex, const IKTarget& target); + void computeSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target); void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose); void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor); @@ -107,7 +110,8 @@ protected: }; std::map _constraints; - std::vector _accumulators; + std::vector _rotationAccumulators; + std::vector _translationAccumulators; std::vector _targetVarVec; AnimPoseVec _defaultRelativePoses; // poses of the relaxed state AnimPoseVec _relativePoses; // current relative poses @@ -116,7 +120,6 @@ protected: struct SplineJointInfo { int jointIndex; float ratio; - AnimPose defaultPose; AnimPose offsetPose; }; std::map> _splineJointInfoMap; diff --git a/libraries/animation/src/IKTarget.cpp b/libraries/animation/src/IKTarget.cpp index 2fe767b08d..41cac62fa3 100644 --- a/libraries/animation/src/IKTarget.cpp +++ b/libraries/animation/src/IKTarget.cpp @@ -44,6 +44,9 @@ void IKTarget::setType(int type) { case (int)Type::HipsRelativeRotationAndPosition: _type = Type::HipsRelativeRotationAndPosition; break; + case (int)Type::Spline: + _type = Type::Spline; + break; default: _type = Type::Unknown; } diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index 4f464c103c..011175aedf 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -21,6 +21,7 @@ public: RotationOnly, HmdHead, HipsRelativeRotationAndPosition, + Spline, Unknown }; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index add3a461af..dd98993688 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1041,7 +1041,7 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { } if (params.spine2Enabled) { - _animVars.set("spine2Type", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("spine2Type", (int)IKTarget::Type::Spline); _animVars.set("spine2Position", extractTranslation(params.spine2Matrix)); _animVars.set("spine2Rotation", glmExtractRotation(params.spine2Matrix)); } else { @@ -1101,9 +1101,9 @@ void Rig::updateHeadAnimVars(const HeadParameters& params) { _animVars.set("headPosition", params.rigHeadPosition); _animVars.set("headRotation", params.rigHeadOrientation); if (params.hipsEnabled) { - // Since there is an explicit hips ik target, switch the head to use the more generic RotationAndPosition IK chain type. - // this will allow the spine to bend more, ensuring that it can reach the head target position. - _animVars.set("headType", (int)IKTarget::Type::RotationAndPosition); + // Since there is an explicit hips ik target, switch the head to use the more flexible Spline IK chain type. + // this will allow the spine to compress/expand and bend more natrually, ensuring that it can reach the head target position. + _animVars.set("headType", (int)IKTarget::Type::Spline); _animVars.unset("headWeight"); // use the default weight for this target. } else { // When there is no hips IK target, use the HmdHead IK chain type. This will make the spine very stiff, diff --git a/libraries/animation/src/RotationAccumulator.cpp b/libraries/animation/src/RotationAccumulator.cpp index a4940e1989..2b8fc7d66a 100644 --- a/libraries/animation/src/RotationAccumulator.cpp +++ b/libraries/animation/src/RotationAccumulator.cpp @@ -1,5 +1,5 @@ // -// RotationAccumulator.h +// RotationAccumulator.cpp // // Copyright 2015 High Fidelity, Inc. // @@ -27,7 +27,7 @@ void RotationAccumulator::clear() { _numRotations = 0; } -void RotationAccumulator::clearAndClean() { +void RotationAccumulator::clearAndClean() { clear(); _isDirty = false; } diff --git a/libraries/animation/src/TranslationAccumulator.cpp b/libraries/animation/src/TranslationAccumulator.cpp new file mode 100644 index 0000000000..4b110b1564 --- /dev/null +++ b/libraries/animation/src/TranslationAccumulator.cpp @@ -0,0 +1,34 @@ +// +// TranslationAccumulator.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 "TranslationAccumulator.h" + +void TranslationAccumulator::add(const glm::vec3& translation, float weight) { + _accum += weight * translation; + _totalWeight += weight; + _isDirty = true; +} + +glm::vec3 TranslationAccumulator::getAverage() { + if (_totalWeight > 0.0f) { + return _accum / _totalWeight; + } else { + return glm::vec3(); + } +} + +void TranslationAccumulator::clear() { + _accum *= 0.0f; + _totalWeight = 0.0f; +} + +void TranslationAccumulator::clearAndClean() { + clear(); + _isDirty = false; +} diff --git a/libraries/animation/src/TranslationAccumulator.h b/libraries/animation/src/TranslationAccumulator.h new file mode 100644 index 0000000000..65f8a0dc97 --- /dev/null +++ b/libraries/animation/src/TranslationAccumulator.h @@ -0,0 +1,42 @@ +// +// TranslationAccumulator.h +// +// Copyright 2017 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_TranslationAccumulator_h +#define hifi_TranslationAccumulator_h + +#include + +class TranslationAccumulator { +public: + TranslationAccumulator() : _accum(0.0f, 0.0f, 0.0f), _totalWeight(0), _isDirty(false) { } + + int size() const { return _totalWeight > 0.0f; } + + /// \param rotation rotation to add + /// \param weight contribution factor of this rotation to total accumulation + void add(const glm::vec3& translation, float weight = 1.0f); + + glm::vec3 getAverage(); + + /// \return true if any rotations were accumulated + bool isDirty() const { return _isDirty; } + + /// \brief clear accumulated rotation but don't change _isDirty + void clear(); + + /// \brief clear accumulated rotation and set _isDirty to false + void clearAndClean(); + +private: + glm::vec3 _accum; + float _totalWeight; + bool _isDirty; +}; + +#endif // hifi_TranslationAccumulator_h