adding the spline code to the splineik class

libraries/animation/src/AnimSplineIK.cpp

@@ -44,10 +44,46 @@ const AnimPoseVec& AnimSplineIK::evaluate(const AnimVariantMap& animVars, const
     if (_children.size() != 1) {
         return _poses;
     }
-
     // evalute underPoses
     AnimPoseVec underPoses = _children[0]->evaluate(animVars, context, dt, triggersOut);
     _poses = underPoses;
+    
+    // check to see if we actually need absolute poses.
+    AnimPoseVec absolutePoses;
+    absolutePoses.resize(_poses.size());
+    computeAbsolutePoses(absolutePoses);
+
+    IKTarget target;
+    int jointIndex = _skeleton->nameToJointIndex("Head");
+    if (jointIndex != -1) {
+        target.setType(animVars.lookup("HeadType", (int)IKTarget::Type::RotationAndPosition));
+        target.setIndex(jointIndex);
+        AnimPose absPose = _skeleton->getAbsolutePose(jointIndex, _poses);
+        glm::quat rotation = animVars.lookupRigToGeometry("headRotation", absPose.rot());
+        glm::vec3 translation = animVars.lookupRigToGeometry("headPosition", absPose.trans());
+        float weight = animVars.lookup("headWeight", "4.0");
+
+        target.setPose(rotation, translation);
+        target.setWeight(weight);
+        const float* flexCoefficients = new float[5]{ 1.0f, 0.5f, 0.25f, 0.2f, 0.1f };
+        target.setFlexCoefficients(4, flexCoefficients);
+
+        // record the index of the hips ik target.
+        if (target.getIndex() == _hipsIndex) {
+            _hipsTargetIndex = 1;
+        }
+    }
+    if (_poses.size() > 0) {
+    AnimChain jointChain;
+    jointChain.buildFromRelativePoses(_skeleton, _poses, target.getIndex());
+
+    // for each target solve target with spline
+    
+        solveTargetWithSpline(context, target, absolutePoses, false, jointChain);
+        qCDebug(animation) << "made it past the spline solve code";
+    }
+    // we need to blend the old joint chain with the current joint chain, otherwise known as: _snapShotChain
+    /**/
     return _poses;
 }
 
@@ -66,6 +102,20 @@ void AnimSplineIK::lookUpIndices() {
     }
 }
 
+void AnimSplineIK::computeAbsolutePoses(AnimPoseVec& absolutePoses) const {
+    int numJoints = (int)_poses.size();
+    assert(numJoints <= _skeleton->getNumJoints());
+    assert(numJoints == (int)absolutePoses.size());
+    for (int i = 0; i < numJoints; ++i) {
+        int parentIndex = _skeleton->getParentIndex(i);
+        if (parentIndex < 0) {
+            absolutePoses[i] = _poses[i];
+        } else {
+            absolutePoses[i] = absolutePoses[parentIndex] * _poses[i];
+        }
+    }
+}
+
 // for AnimDebugDraw rendering
 const AnimPoseVec& AnimSplineIK::getPosesInternal() const {
     return _poses;
@@ -73,5 +123,183 @@ const AnimPoseVec& AnimSplineIK::getPosesInternal() const {
 
 void AnimSplineIK::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
     AnimNode::setSkeletonInternal(skeleton);
+    _headIndex = _skeleton->nameToJointIndex("Head");
+    _hipsIndex = _skeleton->nameToJointIndex("Hips");
     lookUpIndices();
 }
+
+static CubicHermiteSplineFunctorWithArcLength computeSplineFromTipAndBase(const AnimPose& tipPose, const AnimPose& basePose, float baseGain = 1.0f, float tipGain = 1.0f) {
+    float linearDistance = glm::length(basePose.trans() - tipPose.trans());
+    glm::vec3 p0 = basePose.trans();
+    glm::vec3 m0 = baseGain * linearDistance * (basePose.rot() * Vectors::UNIT_Y);
+    glm::vec3 p1 = tipPose.trans();
+    glm::vec3 m1 = tipGain * linearDistance * (tipPose.rot() * Vectors::UNIT_Y);
+
+    return CubicHermiteSplineFunctorWithArcLength(p0, m0, p1, m1);
+}
+
+void AnimSplineIK::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug, AnimChain& chainInfoOut) const {
+
+    const int baseIndex = _hipsIndex;
+
+    // build spline from tip to base
+    AnimPose tipPose = AnimPose(glm::vec3(1.0f), target.getRotation(), target.getTranslation());
+    AnimPose basePose = absolutePoses[baseIndex];
+    CubicHermiteSplineFunctorWithArcLength spline;
+    if (target.getIndex() == _headIndex) {
+        // set gain factors so that more curvature occurs near the tip of the spline.
+        const float HIPS_GAIN = 0.5f;
+        const float HEAD_GAIN = 1.0f;
+        spline = computeSplineFromTipAndBase(tipPose, basePose, HIPS_GAIN, HEAD_GAIN);
+    } else {
+        spline = computeSplineFromTipAndBase(tipPose, basePose);
+    }
+    float totalArcLength = spline.arcLength(1.0f);
+
+    // This prevents the rotation interpolation from rotating the wrong physical way (but correct mathematical way)
+    // when the head is arched backwards very far.
+    glm::quat halfRot = safeLerp(basePose.rot(), tipPose.rot(), 0.5f);
+    if (glm::dot(halfRot * Vectors::UNIT_Z, basePose.rot() * Vectors::UNIT_Z) < 0.0f) {
+        tipPose.rot() = -tipPose.rot();
+    }
+    qCDebug(animation) << "spot 1";
+    // find or create splineJointInfo for this target
+    const std::vector<SplineJointInfo>* splineJointInfoVec = findOrCreateSplineJointInfo(context, target);
+
+    if (splineJointInfoVec && splineJointInfoVec->size() > 0) {
+        const int baseParentIndex = _skeleton->getParentIndex(baseIndex);
+        AnimPose parentAbsPose = (baseParentIndex >= 0) ? absolutePoses[baseParentIndex] : AnimPose();
+        qCDebug(animation) << "spot 2";
+        // 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);
+
+            // for head splines, preform most twist toward the tip by using ease in function. t^2
+            float rotT = t;
+            if (target.getIndex() == _headIndex) {
+                rotT = t * t;
+            }
+            glm::quat twistRot = safeLerp(basePose.rot(), tipPose.rot(), rotT);
+
+            // compute the rotation by using the derivative of the spline as the y-axis, and the twistRot x-axis
+            glm::vec3 y = glm::normalize(spline.d(t));
+            glm::vec3 x = twistRot * Vectors::UNIT_X;
+            glm::vec3 u, v, w;
+            generateBasisVectors(y, x, v, u, w);
+            glm::mat3 m(u, v, glm::cross(u, v));
+            glm::quat rot = glm::normalize(glm::quat_cast(m));
+
+            AnimPose desiredAbsPose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose;
+
+            // apply flex coefficent
+            AnimPose flexedAbsPose;
+            ::blend(1, &absolutePoses[splineJointInfo.jointIndex], &desiredAbsPose, target.getFlexCoefficient(i), &flexedAbsPose);
+
+            AnimPose relPose = parentAbsPose.inverse() * flexedAbsPose;
+
+            bool constrained = false;
+            if (splineJointInfo.jointIndex != _hipsIndex) {
+                // constrain the amount the spine can stretch or compress
+                float length = glm::length(relPose.trans());
+                const float EPSILON = 0.0001f;
+                if (length > EPSILON) {
+                    float defaultLength = glm::length(_skeleton->getRelativeDefaultPose(splineJointInfo.jointIndex).trans());
+                    const float STRETCH_COMPRESS_PERCENTAGE = 0.15f;
+                    const float MAX_LENGTH = defaultLength * (1.0f + STRETCH_COMPRESS_PERCENTAGE);
+                    const float MIN_LENGTH = defaultLength * (1.0f - STRETCH_COMPRESS_PERCENTAGE);
+                    if (length > MAX_LENGTH) {
+                        relPose.trans() = (relPose.trans() / length) * MAX_LENGTH;
+                        constrained = true;
+                    } else if (length < MIN_LENGTH) {
+                        relPose.trans() = (relPose.trans() / length) * MIN_LENGTH;
+                        constrained = true;
+                    }
+                } else {
+                    relPose.trans() = glm::vec3(0.0f);
+                }
+            }
+            // note we are ignoring the constrained info for now.  
+            if (!chainInfoOut.setRelativePoseAtJointIndex(splineJointInfo.jointIndex, relPose)) {
+                qCDebug(animation) << "we didn't find the joint index for the spline!!!!";
+            }
+
+            parentAbsPose = flexedAbsPose;
+        }
+    }
+
+    if (debug) {
+        //debugDrawIKChain(jointChainInfoOut, context);
+    }
+}
+
+const std::vector<AnimSplineIK::SplineJointInfo>* AnimSplineIK::findOrCreateSplineJointInfo(const AnimContext& context, const IKTarget& target) const {
+    // find or create splineJointInfo for this target
+    auto iter = _splineJointInfoMap.find(target.getIndex());
+    if (iter != _splineJointInfoMap.end()) {
+        return &(iter->second);
+    } else {
+        computeAndCacheSplineJointInfosForIKTarget(context, target);
+        auto iter = _splineJointInfoMap.find(target.getIndex());
+        if (iter != _splineJointInfoMap.end()) {
+            return &(iter->second);
+        }
+    }
+
+    return nullptr;
+}
+
+// pre-compute information about each joint influenced by this spline IK target.
+void AnimSplineIK::computeAndCacheSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) const {
+    std::vector<SplineJointInfo> splineJointInfoVec;
+
+    // build spline between the default poses.
+    AnimPose tipPose = _skeleton->getAbsoluteDefaultPose(target.getIndex());
+    AnimPose basePose = _skeleton->getAbsoluteDefaultPose(_hipsIndex);
+
+    CubicHermiteSplineFunctorWithArcLength spline;
+    if (target.getIndex() == _headIndex) {
+        // set gain factors so that more curvature occurs near the tip of the spline.
+        const float HIPS_GAIN = 0.5f;
+        const float HEAD_GAIN = 1.0f;
+        spline = computeSplineFromTipAndBase(tipPose, basePose, HIPS_GAIN, HEAD_GAIN);
+    } else {
+        spline = computeSplineFromTipAndBase(tipPose, basePose);
+    }
+
+    // measure the total arc length along the spline
+    float totalArcLength = spline.arcLength(1.0f);
+
+    glm::vec3 baseToTip = tipPose.trans() - basePose.trans();
+    float baseToTipLength = glm::length(baseToTip);
+    glm::vec3 baseToTipNormal = baseToTip / baseToTipLength;
+
+    int index = target.getIndex();
+    int endIndex = _skeleton->getParentIndex(_hipsIndex);
+    while (index != endIndex) {
+        AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(index);
+
+        float ratio = glm::dot(defaultPose.trans() - basePose.trans(), baseToTipNormal) / baseToTipLength;
+
+        // compute offset from spline to the default pose.
+        float t = spline.arcLengthInverse(ratio * totalArcLength);
+
+        // compute the rotation by using the derivative of the spline as the y-axis, and the defaultPose x-axis
+        glm::vec3 y = glm::normalize(spline.d(t));
+        glm::vec3 x = defaultPose.rot() * Vectors::UNIT_X;
+        glm::vec3 u, v, w;
+        generateBasisVectors(y, x, v, u, w);
+        glm::mat3 m(u, v, glm::cross(u, v));
+        glm::quat rot = glm::normalize(glm::quat_cast(m));
+
+        AnimPose pose(glm::vec3(1.0f), rot, spline(t));
+        AnimPose offsetPose = pose.inverse() * defaultPose;
+
+        SplineJointInfo splineJointInfo = { index, ratio, offsetPose };
+        splineJointInfoVec.push_back(splineJointInfo);
+        index = _skeleton->getParentIndex(index);
+    }
+
+    _splineJointInfoMap[target.getIndex()] = splineJointInfoVec;
+}

libraries/animation/src/AnimSplineIK.h

@@ -12,6 +12,7 @@
 #define hifi_AnimSplineIK_h
 
 #include "AnimNode.h"
+#include "IKTarget.h"
 #include "AnimChain.h"
 
 // Spline IK for the spine
@@ -34,6 +35,8 @@ protected:
         NumTypes
     };
 
+    void computeAbsolutePoses(AnimPoseVec& absolutePoses) const;
+
     // for AnimDebugDraw rendering
     virtual const AnimPoseVec& getPosesInternal() const override;
     virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
@@ -51,6 +54,9 @@ protected:
     int _baseParentJointIndex{ -1 };
     int _baseJointIndex{ -1 };
     int _tipJointIndex{ -1 };
+    int _headIndex{ -1 };
+    int _hipsIndex{ -1 };
+    int _hipsTargetIndex{ -1 };
 
     QString _alphaVar;  // float - (0, 1) 0 means underPoses only, 1 means IK only.
     QString _enabledVar;  // bool
@@ -66,7 +72,18 @@ protected:
 
     AnimChain _snapshotChain;
 
+    // used to pre-compute information about each joint influeced by a spline IK target.
+    struct SplineJointInfo {
+        int jointIndex;       // joint in the skeleton that this information pertains to.
+        float ratio;          // percentage (0..1) along the spline for this joint.
+        AnimPose offsetPose;  // local offset from the spline to the joint.
+    };
+
     bool _lastEnableDebugDrawIKTargets{ false };
+    void AnimSplineIK::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug, AnimChain& chainInfoOut) const;
+    void computeAndCacheSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) const;
+    const std::vector<SplineJointInfo>* findOrCreateSplineJointInfo(const AnimContext& context, const IKTarget& target) const;
+    mutable std::map<int, std::vector<SplineJointInfo>> _splineJointInfoMap;
 
     // no copies
     AnimSplineIK(const AnimSplineIK&) = delete;