compute rotation from derivative of spline

This should fix bad rotation values for the spine during bowing/touching toes.

interface/resources/avatar/avatar-animation.json

@@ -103,8 +103,8 @@
                                     "rotationVar": "spine2Rotation",
                                     "typeVar": "spine2Type",
                                     "weightVar": "spine2Weight",
-                                    "weight": 1.0,
+                                    "weight": 2.0,
-                                    "flexCoefficients": [1.0, 0.5, 0.5]
+                                    "flexCoefficients": [1.0, 0.5, 0.25]
                                 },
                                 {
                                     "jointName": "Head",
@@ -113,7 +113,7 @@
                                     "typeVar": "headType",
                                     "weightVar": "headWeight",
                                     "weight": 4.0,
-                                    "flexCoefficients": [1, 0.05, 0.25, 0.25, 0.25]
+                                    "flexCoefficients": [1, 0.5, 0.25, 0.2, 0.1]
                                 },
                                 {
                                     "jointName": "LeftArm",

libraries/animation/src/AnimInverseKinematics.cpp

@@ -21,6 +21,7 @@
 #include "SwingTwistConstraint.h"
 #include "AnimationLogging.h"
 #include "CubicHermiteSpline.h"
+#include "AnimUtil.h"
 
 AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn,
                                                 const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector<float>& flexCoefficientsIn) :
@@ -475,16 +476,85 @@ static CubicHermiteSplineFunctorWithArcLength computeSplineFromTipAndBase(const
     return CubicHermiteSplineFunctorWithArcLength(p0, m0, p1, m1);
 }
 
+// pre-compute information about each joint influeced by this spline IK target.
+void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) {
+    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;
+}
+
+const std::vector<AnimInverseKinematics::SplineJointInfo>* AnimInverseKinematics::findOrCreateSplineJointInfo(const AnimContext& context, const IKTarget& target) {
+    // find or create splineJointInfo for this target
+    auto iter = _splineJointInfoMap.find(target.getIndex());
+    if (iter != _splineJointInfoMap.end()) {
+        return &(iter->second);
+    } else {
+        computeSplineJointInfosForIKTarget(context, target);
+        auto iter = _splineJointInfoMap.find(target.getIndex());
+        if (iter != _splineJointInfoMap.end()) {
+            return &(iter->second);
+        }
+    }
+
+    return nullptr;
+}
+
 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
+    // 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.
@@ -496,19 +566,6 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
     }
     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);
-        }
-    }
-
     // 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 = glm::normalize(glm::lerp(basePose.rot(), tipPose.rot(), 0.5f));
@@ -516,6 +573,9 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
         tipPose.rot() = -tipPose.rot();
     }
 
+    // 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();
@@ -526,14 +586,28 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
             float t = spline.arcLengthInverse(splineJointInfo.ratio * totalArcLength);
             glm::vec3 trans = spline(t);
 
-            // for head splines, preform most rotation toward the tip by using ease in function. t^2
+            // 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 rot = glm::normalize(glm::lerp(basePose.rot(), tipPose.rot(), rotT));
+            glm::quat twistRot = glm::normalize(glm::lerp(basePose.rot(), tipPose.rot(), rotT));
-            AnimPose absPose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose;
+
-            AnimPose relPose = parentAbsPose.inverse() * absPose;
+            // 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;
             _rotationAccumulators[splineJointInfo.jointIndex].add(relPose.rot(), target.getWeight());
 
             bool constrained = false;
@@ -564,7 +638,7 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
                 debugJointMap[splineJointInfo.jointIndex] = DebugJoint(relPose.rot(), relPose.trans(), constrained);
             }
 
-            parentAbsPose = absPose;
+            parentAbsPose = flexedAbsPose;
         }
     }
 
@@ -1450,51 +1524,6 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s
     }
 }
 
-// pre-compute information about each joint influeced by this spline IK target.
-void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) {
-    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);
-        AnimPose pose(glm::vec3(1.0f), glm::normalize(glm::lerp(basePose.rot(), tipPose.rot(), t)), spline(t));
-        AnimPose offsetPose = pose.inverse() * defaultPose;
-
-        SplineJointInfo splineJointInfo = { index, ratio, offsetPose };
-        splineJointInfoVec.push_back(splineJointInfo);
-        index = _skeleton->getParentIndex(index);
-    }
-
-    _splineJointInfoMap[target.getIndex()] = splineJointInfoVec;
-}
-
 void AnimInverseKinematics::debugDrawSpineSplines(const AnimContext& context, const std::vector<IKTarget>& targets) const {
 
     for (auto& target : targets) {

libraries/animation/src/AnimInverseKinematics.h

@@ -74,10 +74,18 @@ protected:
     void debugDrawRelativePoses(const AnimContext& context) const;
     void debugDrawConstraints(const AnimContext& context) const;
     void debugDrawSpineSplines(const AnimContext& context, const std::vector<IKTarget>& targets) const;
-    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);
 
+    // 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.
+    };
+
+    void computeSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target);
+    const std::vector<SplineJointInfo>* findOrCreateSplineJointInfo(const AnimContext& context, const IKTarget& target);
 
     // for AnimDebugDraw rendering
     virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; }
@@ -117,12 +125,6 @@ protected:
     AnimPoseVec _relativePoses; // current relative poses
     AnimPoseVec _limitCenterPoses;  // relative
 
-    // 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.
-    };
     std::map<int, std::vector<SplineJointInfo>> _splineJointInfoMap;
 
     // experimental data for moving hips during IK