From fc12d7547a45434f784412c029acedc1e127acc9 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 24 May 2017 19:06:49 -0700 Subject: [PATCH] Addition of CubicHermiteSpline helper classes. --- .../animation/src/AnimInverseKinematics.cpp | 67 +++++++++++++- .../animation/src/AnimInverseKinematics.h | 1 + libraries/shared/src/CubicHermiteSpline.h | 90 +++++++++++++++++++ tests/shared/src/CubicHermiteSplineTests.cpp | 77 ++++++++++++++++ tests/shared/src/CubicHermiteSplineTests.h | 23 +++++ 5 files changed, 254 insertions(+), 4 deletions(-) create mode 100644 libraries/shared/src/CubicHermiteSpline.h create mode 100644 tests/shared/src/CubicHermiteSplineTests.cpp create mode 100644 tests/shared/src/CubicHermiteSplineTests.h diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index d613e42866..0684cdbd8e 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/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& 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; + } + */ +} diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 0267f14650..22ebee4dec 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -70,6 +70,7 @@ protected: 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; void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose); void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor); diff --git a/libraries/shared/src/CubicHermiteSpline.h b/libraries/shared/src/CubicHermiteSpline.h new file mode 100644 index 0000000000..310dedb1af --- /dev/null +++ b/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 diff --git a/tests/shared/src/CubicHermiteSplineTests.cpp b/tests/shared/src/CubicHermiteSplineTests.cpp new file mode 100644 index 0000000000..53a4ba8f6b --- /dev/null +++ b/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 +#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); +} diff --git a/tests/shared/src/CubicHermiteSplineTests.h b/tests/shared/src/CubicHermiteSplineTests.h new file mode 100644 index 0000000000..1828d1aa02 --- /dev/null +++ b/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 + +class CubicHermiteSplineTests : public QObject { + Q_OBJECT +private slots: + void testCubicHermiteSplineFunctor(); + void testCubicHermiteSplineFunctorWithArcLength(); +}; + +#endif // hifi_TransformTests_h