Addition of CubicHermiteSpline helper classes.

libraries/animation/src/AnimInverseKinematics.cpp

@@ -20,6 +20,7 @@
 #include "ElbowConstraint.h"
 #include "SwingTwistConstraint.h"
 #include "AnimationLogging.h"
+#include "CubicHermiteSpline.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) :
@@ -457,10 +458,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
     // allows solutionSource to be overridden by an animVar
     auto solutionSource = animVars.lookup(_solutionSourceVar, (int)_solutionSource);
 
-    if (context.getEnableDebugDrawIKConstraints()) {
-        debugDrawConstraints(context);
-    }
-
     const float MAX_OVERLAY_DT = 1.0f / 30.0f; // what to clamp delta-time to in AnimInverseKinematics::overlay
     if (dt > MAX_OVERLAY_DT) {
         dt = MAX_OVERLAY_DT;
@@ -581,6 +578,11 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
         }
     }
 
+    if (context.getEnableDebugDrawIKConstraints()) {
+        debugDrawConstraints(context);
+    }
+    debugDrawSpineSpline(context);
+
     return _relativePoses;
 }
 
@@ -1316,3 +1318,60 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s
         break;
     }
 }
+
+void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context) const {
+    AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix());
+
+    AnimPose hipsPose = geomToWorldPose * _skeleton->getAbsolutePose(_hipsIndex, _relativePoses);
+    AnimPose headPose = geomToWorldPose * _skeleton->getAbsolutePose(_headIndex, _relativePoses);
+
+    float d = glm::length(hipsPose.trans() - headPose.trans());
+
+    const float BACK_GAIN = 1.0f;
+    const float NECK_GAIN = 0.33f;
+    glm::vec3 cp0 = hipsPose.trans();
+    glm::vec3 cm0 = BACK_GAIN * d * (hipsPose.rot() * Vectors::UNIT_Y);
+    glm::vec3 cp1 = headPose.trans();
+    glm::vec3 cm1 = NECK_GAIN * d * (headPose.rot() * Vectors::UNIT_Y);
+
+    CubicHermiteSplineFunctorWithArcLength hermiteFunc(cp0, cm0, cp1, cm1);
+
+    const glm::vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f);
+    const glm::vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f);
+    const glm::vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
+    const glm::vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
+    const glm::vec4 CYAN(0.0f, 1.0f, 1.0f, 1.0f);
+
+    int NUM_SUBDIVISIONS = 20;
+    float totalArcLength = hermiteFunc.arcLength(1.0f);
+    const float dArcLength = totalArcLength / NUM_SUBDIVISIONS;
+    float arcLength = 0.0f;
+    for (int i = 0; i < NUM_SUBDIVISIONS; i++) {
+        float prevT = hermiteFunc.arcLengthInverse(arcLength);
+        float nextT = hermiteFunc.arcLengthInverse(arcLength + dArcLength);
+        DebugDraw::getInstance().drawRay(hermiteFunc(prevT), hermiteFunc(nextT), (i % 2) == 0 ? RED : WHITE);
+        arcLength += dArcLength;
+    }
+
+    /*
+    AnimPose p0 = hipsPose;
+    AnimPose p1 = AnimPose(glm::vec3(1.0f), glm::normalize(glm::lerp(hipsPose.rot(), headPose.rot(), 0.25f)), hermiteSpline(cp0, cm0, cp1, cm1, 0.25f));
+    AnimPose p2 = AnimPose(glm::vec3(1.0f), glm::normalize(glm::lerp(hipsPose.rot(), headPose.rot(), 0.75f)), hermiteSpline(cp0, cm0, cp1, cm1, 0.75f));
+    AnimPose p3 = headPose;
+
+    DebugDraw::getInstance().drawRay(cp0, cp0 + cm0, GREEN);
+    DebugDraw::getInstance().drawRay(cp0 + cm0, cp1, CYAN);
+    DebugDraw::getInstance().drawRay(cp1, cp1 + cm1, GREEN);
+
+    // draw the spline itself
+    int NUM_SUBDIVISIONS = 20;
+    float D_ALPHA = 1.0f / NUM_SUBDIVISIONS;
+    float alpha = 0.0f;
+    for (int i = 0; i < NUM_SUBDIVISIONS; i++) {
+        DebugDraw::getInstance().drawRay(hermiteSpline(cp0, cm0, cp1, cm1, alpha),
+                                         hermiteSpline(cp0, cm0, cp1, cm1, alpha + D_ALPHA),
+                                         (i % 2) == 0 ? RED : WHITE);
+        alpha += D_ALPHA;
+    }
+    */
+}

libraries/animation/src/AnimInverseKinematics.h

@@ -70,6 +70,7 @@ protected:
     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;
     void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose);
     void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor);
 

libraries/shared/src/CubicHermiteSpline.h

@@ -0,0 +1,90 @@
+//
+//  CubicHermiteSpline.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_CubicHermiteSpline_h
+#define hifi_CubicHermiteSpline_h
+
+#include "GLMHelpers.h"
+
+class CubicHermiteSplineFunctor {
+public:
+    CubicHermiteSplineFunctor(const glm::vec3& p0, const glm::vec3& m0, const glm::vec3& p1, const glm::vec3& m1) : _p0(p0), _m0(m0), _p1(p1), _m1(m1) {}
+
+    CubicHermiteSplineFunctor(const CubicHermiteSplineFunctor& orig) : _p0(orig._p0), _m0(orig._m0), _p1(orig._p1), _m1(orig._m1) {}
+
+    virtual ~CubicHermiteSplineFunctor() {}
+
+    // evalute the hermite curve at parameter t (0..1)
+    glm::vec3 operator()(float t) const {
+        float t3 = t * t * t;
+        float t2 = t * t;
+        float w0 = 2.0f * t3 - 3.0f * t2 + 1.0f;
+        float w1 = t3 - 2.0f * t2 + t;
+        float w2 = -2.0f * t3 + 3.0f * t2;
+        float w3 = t3 - t2;
+        return w0 * _p0 + w1 * _m0 + w2 * _p1 + w3 * _m1;
+    }
+
+protected:
+    glm::vec3 _p0;
+    glm::vec3 _m0;
+    glm::vec3 _p1;
+    glm::vec3 _m1;
+};
+
+class CubicHermiteSplineFunctorWithArcLength : public CubicHermiteSplineFunctor {
+public:
+    enum Constants { NUM_SUBDIVISIONS = 30 };
+
+    CubicHermiteSplineFunctorWithArcLength(const glm::vec3& p0, const glm::vec3& m0, const glm::vec3& p1, const glm::vec3& m1) : CubicHermiteSplineFunctor(p0, m0, p1, m1) {
+        // initialize _values with the accumulated arcLength along the spline.
+        const float DELTA = 1.0f / NUM_SUBDIVISIONS;
+        float alpha = 0.0f;
+        float accum = 0.0f;
+        _values[0] = 0.0f;
+        for (int i = 0; i < NUM_SUBDIVISIONS; i++) {
+            accum += glm::distance(this->operator()(alpha),
+                                   this->operator()(alpha + DELTA));
+            alpha += DELTA;
+            _values[i + 1] = accum;
+        }
+    }
+
+    CubicHermiteSplineFunctorWithArcLength(const CubicHermiteSplineFunctorWithArcLength& orig) : CubicHermiteSplineFunctor(orig) {
+        memcpy(_values, orig._values, sizeof(float) * NUM_SUBDIVISIONS + 1);
+    }
+
+    // given the spline parameter (0..1) output the arcLength of the spline up to that point.
+    float arcLength(float t) const {
+        float index = t * NUM_SUBDIVISIONS;
+        int prevIndex = std::min(std::max(0, (int)glm::floor(index)), (int)NUM_SUBDIVISIONS);
+        int nextIndex = std::min(std::max(0, (int)glm::ceil(index)), (int)NUM_SUBDIVISIONS);
+        float alpha = glm::fract(index);
+        return lerp(_values[prevIndex], _values[nextIndex], alpha);
+    }
+
+    // given an arcLength compute the spline parameter (0..1) that cooresponds to that arcLength.
+    float arcLengthInverse(float s) const {
+        // find first item in _values that is > s.
+        int nextIndex;
+        for (nextIndex = 0; nextIndex < NUM_SUBDIVISIONS; nextIndex++) {
+            if (_values[nextIndex] > s) {
+                break;
+            }
+        }
+        int prevIndex = std::min(std::max(0, nextIndex - 1), (int)NUM_SUBDIVISIONS);
+        float alpha = glm::clamp((s - _values[prevIndex]) / (_values[nextIndex] - _values[prevIndex]), 0.0f, 1.0f);
+        const float DELTA = 1.0f / NUM_SUBDIVISIONS;
+        return lerp(prevIndex * DELTA, nextIndex * DELTA, alpha);
+    }
+protected:
+    float _values[NUM_SUBDIVISIONS + 1];
+};
+
+#endif // hifi_CubicHermiteSpline_h

tests/shared/src/CubicHermiteSplineTests.cpp

@@ -0,0 +1,77 @@
+//
+// CubicHermiteSplineTests.cpp
+// tests/shared/src
+//
+// 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
+//
+
+#include "CubicHermiteSplineTests.h"
+#include "../QTestExtensions.h"
+#include <QtCore/QDebug>
+#include "CubicHermiteSpline.h"
+
+QTEST_MAIN(CubicHermiteSplineTests)
+
+void CubicHermiteSplineTests::testCubicHermiteSplineFunctor() {
+    glm::vec3 p0(0.0f, 0.0f, 0.0f);
+    glm::vec3 m0(1.0f, 0.0f, 0.0f);
+    glm::vec3 p1(1.0f, 1.0f, 0.0f);
+    glm::vec3 m1(2.0f, 0.0f, 0.0f);
+    CubicHermiteSplineFunctor hermiteSpline(p0, m0, p1, m1);
+
+    const float EPSILON = 0.0001f;
+
+    QCOMPARE_WITH_ABS_ERROR(p0, hermiteSpline(0.0f), EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(p1, hermiteSpline(1.0f), EPSILON);
+
+    // these values were computed offline.
+    const glm::vec3 oneFourth(0.203125f, 0.15625f, 0.0f);
+    const glm::vec3 oneHalf(0.375f, 0.5f, 0.0f);
+    const glm::vec3 threeFourths(0.609375f, 0.84375f, 0.0f);
+
+    QCOMPARE_WITH_ABS_ERROR(oneFourth, hermiteSpline(0.25f), EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(oneHalf, hermiteSpline(0.5f), EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(threeFourths, hermiteSpline(0.75f), EPSILON);
+}
+
+void CubicHermiteSplineTests::testCubicHermiteSplineFunctorWithArcLength() {
+    glm::vec3 p0(0.0f, 0.0f, 0.0f);
+    glm::vec3 m0(1.0f, 0.0f, 0.0f);
+    glm::vec3 p1(1.0f, 1.0f, 0.0f);
+    glm::vec3 m1(2.0f, 0.0f, 0.0f);
+    CubicHermiteSplineFunctorWithArcLength hermiteSpline(p0, m0, p1, m1);
+
+    const float EPSILON = 0.001f;
+
+    float arcLengths[5] = {
+        hermiteSpline.arcLength(0.0f),
+        hermiteSpline.arcLength(0.25f),
+        hermiteSpline.arcLength(0.5f),
+        hermiteSpline.arcLength(0.75f),
+        hermiteSpline.arcLength(1.0f)
+    };
+
+    // these values were computed offline
+    float referenceArcLengths[5] = {
+        0.0f,
+        0.268317f,
+        0.652788f,
+        1.07096f,
+        1.50267f
+    };
+
+    QCOMPARE_WITH_ABS_ERROR(arcLengths[0], referenceArcLengths[0], EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(arcLengths[1], referenceArcLengths[1], EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(arcLengths[2], referenceArcLengths[2], EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(arcLengths[3], referenceArcLengths[3], EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(arcLengths[4], referenceArcLengths[4], EPSILON);
+
+    QCOMPARE_WITH_ABS_ERROR(0.0f, hermiteSpline.arcLengthInverse(referenceArcLengths[0]), EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(0.25f, hermiteSpline.arcLengthInverse(referenceArcLengths[1]), EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(0.5f, hermiteSpline.arcLengthInverse(referenceArcLengths[2]), EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(0.75f, hermiteSpline.arcLengthInverse(referenceArcLengths[3]), EPSILON);
+    QCOMPARE_WITH_ABS_ERROR(1.0f, hermiteSpline.arcLengthInverse(referenceArcLengths[4]), EPSILON);
+}

tests/shared/src/CubicHermiteSplineTests.h

@@ -0,0 +1,23 @@
+//
+// CubicHermiteSplineTests.h
+// tests/shared/src
+//
+// 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_CubicHermiteSplineTests_h
+#define hifi_CubicHermiteSplineTests_h
+
+#include <QtTest/QtTest>
+
+class CubicHermiteSplineTests : public QObject {
+    Q_OBJECT
+private slots:
+    void testCubicHermiteSplineFunctor();
+    void testCubicHermiteSplineFunctorWithArcLength();
+};
+
+#endif // hifi_TransformTests_h