Working spline spline.

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<IKTarget>& targets) {
+void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<IKTarget>& 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<int, DebugJoint> 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<SplineJointInfo>* 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<int, DebugJoint>& 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<SplineJointInfo> 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<IKTarget>& 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);
         }
     }
-
+    */
 }

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<IKTarget>& targets, const AnimPoseVec& underPoses);
-    void solveWithCyclicCoordinateDescent(const AnimContext& context, const std::vector<IKTarget>& targets);
+    void solve(const AnimContext& context, const std::vector<IKTarget>& 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<int, DebugJoint>& 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<IKTarget>& 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<int, RotationConstraint*> _constraints;
-    std::vector<RotationAccumulator> _accumulators;
+    std::vector<RotationAccumulator> _rotationAccumulators;
+    std::vector<TranslationAccumulator> _translationAccumulators;
     std::vector<IKTargetVar> _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<int, std::vector<SplineJointInfo>> _splineJointInfoMap;

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;
     }

libraries/animation/src/IKTarget.h

@@ -21,6 +21,7 @@ public:
         RotationOnly,
         HmdHead,
         HipsRelativeRotationAndPosition,
+        Spline,
         Unknown
     };
 

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,

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;
 }

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;
+}

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 <glm/glm.hpp>
+
+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