From fc12d7547a45434f784412c029acedc1e127acc9 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 24 May 2017 19:06:49 -0700 Subject: [PATCH 01/14] 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 From 39c23bfe2f60721bf8d251927cfc553f93935f47 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 26 May 2017 17:09:30 -0700 Subject: [PATCH 02/14] revision of spine spline rotation calculation --- .../animation/src/AnimInverseKinematics.cpp | 68 +++++++++++-------- libraries/shared/src/CubicHermiteSpline.h | 19 ++++++ libraries/shared/src/GLMHelpers.cpp | 2 +- 3 files changed, 59 insertions(+), 30 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 0684cdbd8e..69a224c686 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1319,6 +1319,13 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s } } +struct SplineJointInfo { + int index; + float ratio; + AnimPose defaultPose; + AnimPose offsetPose; +}; + void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context) const { AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); @@ -1327,21 +1334,22 @@ void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context) con 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); + const float BACK_GAIN = 0.5f; + const float NECK_GAIN = 1.0f; + glm::vec3 p0 = hipsPose.trans(); + glm::vec3 m0 = BACK_GAIN * d * (hipsPose.rot() * Vectors::UNIT_Y); + glm::vec3 p1 = headPose.trans(); + glm::vec3 m1 = NECK_GAIN * d * (headPose.rot() * Vectors::UNIT_Y); - CubicHermiteSplineFunctorWithArcLength hermiteFunc(cp0, cm0, cp1, cm1); + CubicHermiteSplineFunctorWithArcLength hermiteFunc(p0, m0, p1, m1); - 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 WHITE(1.0f, 1.0f, 1.0f, 1.0f); + const glm::vec4 BLUE(0.0f, 0.0f, 1.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); + // draw red and white stripped spline, parameterized by arc length. + // i.e. each stripe should be the same length. int NUM_SUBDIVISIONS = 20; float totalArcLength = hermiteFunc.arcLength(1.0f); const float dArcLength = totalArcLength / NUM_SUBDIVISIONS; @@ -1353,25 +1361,27 @@ void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context) con 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; + AnimPose defaultHipsPose = _skeleton->getAbsoluteDefaultPose(_hipsIndex); + AnimPose defaultHeadPose = _skeleton->getAbsoluteDefaultPose(_headIndex); + float hipsToHeadHeight = defaultHeadPose.trans().y - defaultHipsPose.trans().y; - 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; + // iterate down the chain and build the splineJointInfoVec + std::vector splineJointInfoVec; + int index = _headIndex; + int endIndex = _skeleton->getParentIndex(_hipsIndex); + while (index != endIndex) { + AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(index); + float ratio = (defaultPose.trans().y - defaultHipsPose.trans().y) / hipsToHeadHeight; + SplineJointInfo splineJointInfo = { index, ratio, defaultPose, AnimPose() }; + splineJointInfoVec.push_back(splineJointInfo); + index = _skeleton->getParentIndex(index); } - */ + + for (auto& splineJointInfo : splineJointInfoVec) { + float t = hermiteFunc.arcLengthInverse(splineJointInfo.ratio * totalArcLength); + glm::vec3 trans = hermiteFunc(t); + glm::quat rot = glm::normalize(glm::lerp(hipsPose.rot(), headPose.rot(), t)); + DebugDraw::getInstance().addMarker(QString("splineJoint%1").arg(splineJointInfo.index), rot, trans, BLUE); + } + } diff --git a/libraries/shared/src/CubicHermiteSpline.h b/libraries/shared/src/CubicHermiteSpline.h index 310dedb1af..e563fc2b73 100644 --- a/libraries/shared/src/CubicHermiteSpline.h +++ b/libraries/shared/src/CubicHermiteSpline.h @@ -31,6 +31,25 @@ public: return w0 * _p0 + w1 * _m0 + w2 * _p1 + w3 * _m1; } + // evaulate the first derivative of the hermite curve at parameter t (0..1) + glm::vec3 d(float t) const { + float t2 = t * t; + float w0 = -6.0f * t + 6.0f * t2; + float w1 = 1.0f - 4.0f * t + 3.0f * t2; + float w2 = 6.0 * t - 6.0f * t2; + float w3 = -2.0f * t + 3.0f * t2; + return w0 * _p0 + w1 * _m0 + w2 * _p1 + w3 * _m1; + } + + // evaulate the second derivative of the hermite curve at paramter t (0..1) + glm::vec3 d2(float t) const { + float w0 = -6.0f + 12.0f * t; + float w1 = -4.0f + 6.0f * t; + float w2 = 6.0f - 12.0f * t; + float w3 = -2.0f + 6.0f * t; + return w0 + _p0 + w1 * _m0 + w2 * _p1 + w3 * _m1; + } + protected: glm::vec3 _p0; glm::vec3 _m0; diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index 70237e8ff6..f4989d61f5 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -540,7 +540,7 @@ void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& seconda // if secondaryAxis is parallel with the primaryAxis, pick another axis. const float EPSILON = 1.0e-4f; - if (fabsf(fabsf(glm::dot(uAxisOut, secondaryAxis)) - 1.0f) > EPSILON) { + if (fabsf(fabsf(glm::dot(uAxisOut, secondaryAxis)) - 1.0f) > EPSILON) { // pick a better secondaryAxis. normSecondary = glm::vec3(1.0f, 0.0f, 0.0f); if (fabsf(fabsf(glm::dot(uAxisOut, secondaryAxis)) - 1.0f) > EPSILON) { From d4dbd94a3564c2bc020fceeb5723eaaff8547f17 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 30 May 2017 14:22:03 -0700 Subject: [PATCH 03/14] Compute defaultSpineSplineto defaultPose offset --- .../animation/src/AnimInverseKinematics.cpp | 110 +++++++++++++----- .../animation/src/AnimInverseKinematics.h | 11 +- 2 files changed, 92 insertions(+), 29 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 69a224c686..03f1208600 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -576,12 +576,12 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars _hipsOffset = Vectors::ZERO; } } - } - if (context.getEnableDebugDrawIKConstraints()) { - debugDrawConstraints(context); + if (context.getEnableDebugDrawIKConstraints()) { + debugDrawConstraints(context); + } + debugDrawSpineSpline(context, targets); } - debugDrawSpineSpline(context); return _relativePoses; } @@ -1319,14 +1319,77 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s } } -struct SplineJointInfo { - int index; - float ratio; - AnimPose defaultPose; - AnimPose offsetPose; -}; +void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext& context, int targetIndex, const IKTarget& target) { + std::vector splineJointInfoVec; + + // build default spline + AnimPose geomToRigPose(context.getGeometryToRigMatrix()); + AnimPose tipPose = geomToRigPose * _skeleton->getAbsoluteDefaultPose(target.getIndex()); + AnimPose basePose = geomToRigPose * _skeleton->getAbsoluteDefaultPose(_hipsIndex); + + 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); + + // AJT: FIXME: TODO: this won't work for horizontal splines... + float baseToTipHeight = tipPose.trans().y - basePose.trans().y; + + CubicHermiteSplineFunctorWithArcLength spline(p0, m0, p1, m1); + float totalArcLength = spline.arcLength(1.0f); + + int index = _headIndex; + int endIndex = _skeleton->getParentIndex(_hipsIndex); + while (index != endIndex) { + AnimPose defaultPose = geomToRigPose * _skeleton->getAbsoluteDefaultPose(index); + + // AJT: FIXME: TODO: this wont work for horizontal splines... + float ratio = (defaultPose.trans().y - basePose.trans().y) / baseToTipHeight; + + // 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; + + SplineJointInfo splineJointInfo = { index, ratio, defaultPose, offset }; + splineJointInfoVec.push_back(splineJointInfo); + index = _skeleton->getParentIndex(index); + } + + _splineJointInfoMap[targetIndex] = splineJointInfoVec; +} + +void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context, const std::vector& targets) { + + // find head target + int headTargetIndex = -1; + for (int i = 0; i < (int)targets.size(); i++) { + if (targets[i].getIndex() == _headIndex) { + headTargetIndex = i; + break; + } + } + + // find or create splineJointInfo for the head target + const std::vector* splineJointInfoVec = nullptr; + if (headTargetIndex != -1) { + auto iter = _splineJointInfoMap.find(headTargetIndex); + if (iter != _splineJointInfoMap.end()) { + splineJointInfoVec = &(iter->second); + } else { + computeSplineJointInfosForIKTarget(context, headTargetIndex, targets[headTargetIndex]); + auto iter = _splineJointInfoMap.find(headTargetIndex); + if (iter != _splineJointInfoMap.end()) { + splineJointInfoVec = &(iter->second); + } + } + } + + // AJT: TODO: render using splineJointInfoVec offset -void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context) const { AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); AnimPose hipsPose = geomToWorldPose * _skeleton->getAbsolutePose(_hipsIndex, _relativePoses); @@ -1365,23 +1428,14 @@ void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context) con AnimPose defaultHeadPose = _skeleton->getAbsoluteDefaultPose(_headIndex); float hipsToHeadHeight = defaultHeadPose.trans().y - defaultHipsPose.trans().y; - // iterate down the chain and build the splineJointInfoVec - std::vector splineJointInfoVec; - int index = _headIndex; - int endIndex = _skeleton->getParentIndex(_hipsIndex); - while (index != endIndex) { - AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(index); - float ratio = (defaultPose.trans().y - defaultHipsPose.trans().y) / hipsToHeadHeight; - SplineJointInfo splineJointInfo = { index, ratio, defaultPose, AnimPose() }; - splineJointInfoVec.push_back(splineJointInfo); - index = _skeleton->getParentIndex(index); - } - - for (auto& splineJointInfo : splineJointInfoVec) { - float t = hermiteFunc.arcLengthInverse(splineJointInfo.ratio * totalArcLength); - glm::vec3 trans = hermiteFunc(t); - glm::quat rot = glm::normalize(glm::lerp(hipsPose.rot(), headPose.rot(), t)); - DebugDraw::getInstance().addMarker(QString("splineJoint%1").arg(splineJointInfo.index), rot, trans, BLUE); + if (splineJointInfoVec) { + for (auto& splineJointInfo : *splineJointInfoVec) { + float t = hermiteFunc.arcLengthInverse(splineJointInfo.ratio * totalArcLength); + glm::vec3 trans = hermiteFunc(t); + glm::quat rot = glm::normalize(glm::lerp(hipsPose.rot(), headPose.rot(), t)); + AnimPose pose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose; + DebugDraw::getInstance().addMarker(QString("splineJoint%1").arg(splineJointInfo.jointIndex), pose.rot(), pose.trans(), BLUE); + } } } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 22ebee4dec..2b2535ac79 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -70,7 +70,8 @@ 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 debugDrawSpineSpline(const AnimContext& context, const std::vector& targets); + void computeSplineJointInfosForIKTarget(const AnimContext& context, int targetIndex, const IKTarget& target); void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose); void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor); @@ -112,6 +113,14 @@ protected: AnimPoseVec _relativePoses; // current relative poses AnimPoseVec _limitCenterPoses; // relative + struct SplineJointInfo { + int jointIndex; + float ratio; + AnimPose defaultPose; + AnimPose offsetPose; + }; + std::map> _splineJointInfoMap; + // experimental data for moving hips during IK glm::vec3 _hipsOffset { Vectors::ZERO }; float _maxHipsOffsetLength{ FLT_MAX }; From cff42ab9b0a5b92aac8275d7db20850f3b23f98b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 30 May 2017 19:01:52 -0700 Subject: [PATCH 04/14] Working spline spline. --- .../animation/src/AnimInverseKinematics.cpp | 142 ++++++++++++++---- .../animation/src/AnimInverseKinematics.h | 13 +- libraries/animation/src/IKTarget.cpp | 3 + libraries/animation/src/IKTarget.h | 1 + libraries/animation/src/Rig.cpp | 8 +- .../animation/src/RotationAccumulator.cpp | 4 +- .../animation/src/TranslationAccumulator.cpp | 34 +++++ .../animation/src/TranslationAccumulator.h | 42 ++++++ 8 files changed, 207 insertions(+), 40 deletions(-) create mode 100644 libraries/animation/src/TranslationAccumulator.cpp create mode 100644 libraries/animation/src/TranslationAccumulator.h diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 03f1208600..31f7bc63d2 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/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& targets) { +void AnimInverseKinematics::solve(const AnimContext& context, const std::vector& 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 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* 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& 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 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& 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); } } - + */ } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 2b2535ac79..42e84530cc 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/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& targets, const AnimPoseVec& underPoses); - void solveWithCyclicCoordinateDescent(const AnimContext& context, const std::vector& targets); + void solve(const AnimContext& context, const std::vector& 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& 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& 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 _constraints; - std::vector _accumulators; + std::vector _rotationAccumulators; + std::vector _translationAccumulators; std::vector _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> _splineJointInfoMap; diff --git a/libraries/animation/src/IKTarget.cpp b/libraries/animation/src/IKTarget.cpp index 2fe767b08d..41cac62fa3 100644 --- a/libraries/animation/src/IKTarget.cpp +++ b/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; } diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index 4f464c103c..011175aedf 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -21,6 +21,7 @@ public: RotationOnly, HmdHead, HipsRelativeRotationAndPosition, + Spline, Unknown }; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index add3a461af..dd98993688 100644 --- a/libraries/animation/src/Rig.cpp +++ b/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, diff --git a/libraries/animation/src/RotationAccumulator.cpp b/libraries/animation/src/RotationAccumulator.cpp index a4940e1989..2b8fc7d66a 100644 --- a/libraries/animation/src/RotationAccumulator.cpp +++ b/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; } diff --git a/libraries/animation/src/TranslationAccumulator.cpp b/libraries/animation/src/TranslationAccumulator.cpp new file mode 100644 index 0000000000..4b110b1564 --- /dev/null +++ b/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; +} diff --git a/libraries/animation/src/TranslationAccumulator.h b/libraries/animation/src/TranslationAccumulator.h new file mode 100644 index 0000000000..65f8a0dc97 --- /dev/null +++ b/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 + +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 From d3ca34956d23b65a60f1ff9bb460cec5de09af4d Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 31 May 2017 16:58:17 -0700 Subject: [PATCH 05/14] Fix spline interpolation the wrong way when bending backward. --- .../animation/src/AnimInverseKinematics.cpp | 118 +++++++----------- .../animation/src/AnimInverseKinematics.h | 2 +- 2 files changed, 46 insertions(+), 74 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 31f7bc63d2..911d966057 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -499,6 +499,13 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co } } + // 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)); + if (glm::dot(halfRot * Vectors::UNIT_Z, basePose.rot() * Vectors::UNIT_Z) < 0.0f) { + tipPose.rot() = -tipPose.rot(); + } + if (splineJointInfoVec && splineJointInfoVec->size() > 0) { const int baseParentIndex = _skeleton->getParentIndex(baseIndex); AnimPose parentAbsPose = (baseParentIndex >= 0) ? absolutePoses[baseParentIndex] : AnimPose(); @@ -662,7 +669,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars if (context.getEnableDebugDrawIKConstraints()) { debugDrawConstraints(context); } - debugDrawSpineSpline(context, targets); } return _relativePoses; @@ -1447,79 +1453,45 @@ void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext _splineJointInfoMap[target.getIndex()] = splineJointInfoVec; } -void AnimInverseKinematics::debugDrawSpineSpline(const AnimContext& context, const std::vector& targets) { +void AnimInverseKinematics::debugDrawSpineSplines(const AnimContext& context, const std::vector& targets) const { - /* - // find head target - int headTargetIndex = -1; - for (int i = 0; i < (int)targets.size(); i++) { - if (targets[i].getIndex() == _headIndex) { - headTargetIndex = i; - break; + for (auto& target : targets) { + + if (target.getType() != IKTarget::Type::Spline) { + continue; + } + + const int baseIndex = _hipsIndex; + + // build spline + AnimPose tipPose = AnimPose(glm::vec3(1.0f), target.getRotation(), target.getTranslation()); + AnimPose basePose = _skeleton->getAbsolutePose(baseIndex, _relativePoses); + + 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); + + const glm::vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); + const glm::vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f); + + // draw red and white stripped spline, parameterized by arc length. + // i.e. each stripe should be the same length. + AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); + int NUM_SUBDIVISIONS = 20; + const float dArcLength = totalArcLength / NUM_SUBDIVISIONS; + float arcLength = 0.0f; + for (int i = 0; i < NUM_SUBDIVISIONS; i++) { + float prevT = spline.arcLengthInverse(arcLength); + float nextT = spline.arcLengthInverse(arcLength + dArcLength); + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(spline(prevT)), geomToWorldPose.xformPoint(spline(nextT)), (i % 2) == 0 ? RED : WHITE); + arcLength += dArcLength; } } - - // find or create splineJointInfo for the head target - const std::vector* splineJointInfoVec = nullptr; - if (headTargetIndex != -1) { - auto iter = _splineJointInfoMap.find(headTargetIndex); - if (iter != _splineJointInfoMap.end()) { - splineJointInfoVec = &(iter->second); - } else { - computeSplineJointInfosForIKTarget(context, headTargetIndex, targets[headTargetIndex]); - auto iter = _splineJointInfoMap.find(headTargetIndex); - if (iter != _splineJointInfoMap.end()) { - splineJointInfoVec = &(iter->second); - } - } - } - - 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 = 0.5f; - const float NECK_GAIN = 1.0f; - glm::vec3 p0 = hipsPose.trans(); - glm::vec3 m0 = BACK_GAIN * d * (hipsPose.rot() * Vectors::UNIT_Y); - glm::vec3 p1 = headPose.trans(); - glm::vec3 m1 = NECK_GAIN * d * (headPose.rot() * Vectors::UNIT_Y); - - CubicHermiteSplineFunctorWithArcLength hermiteFunc(p0, m0, p1, m1); - - const glm::vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); - const glm::vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f); - const glm::vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f); - const glm::vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f); - - // draw red and white stripped spline, parameterized by arc length. - // i.e. each stripe should be the same length. - 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 defaultHipsPose = _skeleton->getAbsoluteDefaultPose(_hipsIndex); - AnimPose defaultHeadPose = _skeleton->getAbsoluteDefaultPose(_headIndex); - float hipsToHeadHeight = defaultHeadPose.trans().y - defaultHipsPose.trans().y; - - if (splineJointInfoVec) { - for (auto& splineJointInfo : *splineJointInfoVec) { - float t = hermiteFunc.arcLengthInverse(splineJointInfo.ratio * totalArcLength); - glm::vec3 trans = hermiteFunc(t); - glm::quat rot = glm::normalize(glm::lerp(hipsPose.rot(), headPose.rot(), t)); - AnimPose pose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose; - DebugDraw::getInstance().addMarker(QString("splineJoint%1").arg(splineJointInfo.jointIndex), pose.rot(), pose.trans(), BLUE); - } - } - */ } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 42e84530cc..459b37d0b6 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -73,7 +73,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 std::vector& targets); + void debugDrawSpineSplines(const AnimContext& context, const std::vector& 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); From 813feeb8fdeb8f6414703b8e73ed5f3a454813c9 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 1 Jun 2017 08:56:42 -0700 Subject: [PATCH 06/14] Limit spine spline compression/stretch to 15% --- .../animation/src/AnimInverseKinematics.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 911d966057..4fd769bec6 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -519,10 +519,26 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co AnimPose absPose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose; AnimPose relPose = parentAbsPose.inverse() * absPose; _rotationAccumulators[splineJointInfo.jointIndex].add(relPose.rot(), target.getWeight()); + + // constrain the amount the spine can stretch or compress + float length = glm::length(relPose.trans()); + 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); + bool constrained = false; + 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; + } + _translationAccumulators[splineJointInfo.jointIndex].add(relPose.trans(), target.getWeight()); if (debug) { - debugJointMap[splineJointInfo.jointIndex] = DebugJoint(relPose.rot(), relPose.trans(), false); + debugJointMap[splineJointInfo.jointIndex] = DebugJoint(relPose.rot(), relPose.trans(), constrained); } parentAbsPose = absPose; From 226855b2b922f72440f1b10c5b3979356d38f038 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 1 Jun 2017 13:20:50 -0700 Subject: [PATCH 07/14] Bug fixes and cleanup * Bug fix for problem when chest target is enabled but hips target is not. * centralized the two computeSplineFromTipAndBase functions into one. * Removed dead code --- .../animation/src/AnimInverseKinematics.cpp | 100 +++++++++++------- libraries/animation/src/Rig.cpp | 14 +-- libraries/animation/src/Rig.h | 4 - libraries/shared/src/CubicHermiteSpline.h | 4 + 4 files changed, 65 insertions(+), 57 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 4fd769bec6..5c0127334b 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -465,6 +465,16 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } } +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 AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug) { std::map debugJointMap; @@ -475,15 +485,15 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co 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); + 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); // find or create splineJointInfo for the head target @@ -515,24 +525,32 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co 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)); + + // for head splines, preform most rotation 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)); AnimPose absPose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose; AnimPose relPose = parentAbsPose.inverse() * absPose; _rotationAccumulators[splineJointInfo.jointIndex].add(relPose.rot(), target.getWeight()); - // constrain the amount the spine can stretch or compress - float length = glm::length(relPose.trans()); - 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); bool constrained = false; - 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; + if (splineJointInfo.jointIndex != _hipsIndex) { + // constrain the amount the spine can stretch or compress + float length = glm::length(relPose.trans()); + 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; + } } _translationAccumulators[splineJointInfo.jointIndex].add(relPose.trans(), target.getWeight()); @@ -1434,20 +1452,20 @@ void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext AnimPose tipPose = _skeleton->getAbsoluteDefaultPose(target.getIndex()); AnimPose basePose = _skeleton->getAbsoluteDefaultPose(_hipsIndex); - 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; + 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); // AJT: FIXME: TODO: this won't work for horizontal splines... float baseToTipHeight = tipPose.trans().y - basePose.trans().y; - CubicHermiteSplineFunctorWithArcLength spline(p0, m0, p1, m1); - float totalArcLength = spline.arcLength(1.0f); - int index = target.getIndex(); int endIndex = _skeleton->getParentIndex(_hipsIndex); while (index != endIndex) { @@ -1483,15 +1501,15 @@ void AnimInverseKinematics::debugDrawSpineSplines(const AnimContext& context, co AnimPose tipPose = AnimPose(glm::vec3(1.0f), target.getRotation(), target.getTranslation()); AnimPose basePose = _skeleton->getAbsolutePose(baseIndex, _relativePoses); - 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); + 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); const glm::vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index dd98993688..175c834d5d 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -401,16 +401,6 @@ void Rig::setJointRotation(int index, bool valid, const glm::quat& rotation, flo } } -void Rig::restoreJointRotation(int index, float fraction, float priority) { - // AJT: DEAD CODE? - ASSERT(false); -} - -void Rig::restoreJointTranslation(int index, float fraction, float priority) { - // AJT: DEAD CODE? - ASSERT(false); -} - bool Rig::getJointPositionInWorldFrame(int jointIndex, glm::vec3& position, glm::vec3 translation, glm::quat rotation) const { if (isIndexValid(jointIndex)) { position = (rotation * _internalPoseSet._absolutePoses[jointIndex].trans()) + translation; @@ -1040,7 +1030,7 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { _animVars.set("hipsType", (int)IKTarget::Type::Unknown); } - if (params.spine2Enabled) { + if (params.hipsEnabled && params.spine2Enabled) { _animVars.set("spine2Type", (int)IKTarget::Type::Spline); _animVars.set("spine2Position", extractTranslation(params.spine2Matrix)); _animVars.set("spine2Rotation", glmExtractRotation(params.spine2Matrix)); @@ -1051,7 +1041,7 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { if (params.leftArmEnabled) { _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition); _animVars.set("leftArmPosition", params.leftArmPosition); - _animVars.set("leftArmRotation", params.leftArmRotation); + _animVars.set("leftArmRotation", params.leftArmRotation); } else { _animVars.set("leftArmType", (int)IKTarget::Type::Unknown); } diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index c9d52d8c72..7166717792 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -134,10 +134,6 @@ public: void setJointTranslation(int index, bool valid, const glm::vec3& translation, float priority); void setJointRotation(int index, bool valid, const glm::quat& rotation, float priority); - // legacy - void restoreJointRotation(int index, float fraction, float priority); - void restoreJointTranslation(int index, float fraction, float priority); - // if translation and rotation is identity, position will be in rig space bool getJointPositionInWorldFrame(int jointIndex, glm::vec3& position, glm::vec3 translation, glm::quat rotation) const; diff --git a/libraries/shared/src/CubicHermiteSpline.h b/libraries/shared/src/CubicHermiteSpline.h index e563fc2b73..17b4fda991 100644 --- a/libraries/shared/src/CubicHermiteSpline.h +++ b/libraries/shared/src/CubicHermiteSpline.h @@ -14,6 +14,7 @@ class CubicHermiteSplineFunctor { public: + CubicHermiteSplineFunctor() : _p0(), _m0(), _p1(), _m1() {} 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) {} @@ -61,6 +62,9 @@ class CubicHermiteSplineFunctorWithArcLength : public CubicHermiteSplineFunctor public: enum Constants { NUM_SUBDIVISIONS = 30 }; + CubicHermiteSplineFunctorWithArcLength() : CubicHermiteSplineFunctor() { + memset(_values, 0, sizeof(float) * NUM_SUBDIVISIONS + 1); + } 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; From b9bf6f4c054a459fca4265c98e9bdbce3b300390 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 1 Jun 2017 13:30:35 -0700 Subject: [PATCH 08/14] whitespace --- libraries/shared/src/GLMHelpers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index f4989d61f5..70237e8ff6 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -540,7 +540,7 @@ void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& seconda // if secondaryAxis is parallel with the primaryAxis, pick another axis. const float EPSILON = 1.0e-4f; - if (fabsf(fabsf(glm::dot(uAxisOut, secondaryAxis)) - 1.0f) > EPSILON) { + if (fabsf(fabsf(glm::dot(uAxisOut, secondaryAxis)) - 1.0f) > EPSILON) { // pick a better secondaryAxis. normSecondary = glm::vec3(1.0f, 0.0f, 0.0f); if (fabsf(fabsf(glm::dot(uAxisOut, secondaryAxis)) - 1.0f) > EPSILON) { From f99b579c14ed38ac0a4dff551c42b2bd2925e3b3 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 1 Jun 2017 13:38:18 -0700 Subject: [PATCH 09/14] added some docs --- libraries/animation/src/AnimInverseKinematics.cpp | 11 +++++++---- libraries/animation/src/AnimInverseKinematics.h | 7 ++++--- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 5c0127334b..524d08de5b 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1445,10 +1445,11 @@ 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 splineJointInfoVec; - // build default spline + // build spline between the default poses. AnimPose tipPose = _skeleton->getAbsoluteDefaultPose(target.getIndex()); AnimPose basePose = _skeleton->getAbsoluteDefaultPose(_hipsIndex); @@ -1461,9 +1462,11 @@ void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext } else { spline = computeSplineFromTipAndBase(tipPose, basePose); } + + // measure the total arc length along the spline float totalArcLength = spline.arcLength(1.0f); - // AJT: FIXME: TODO: this won't work for horizontal splines... + // FIXME: TODO: this won't work for horizontal splines... float baseToTipHeight = tipPose.trans().y - basePose.trans().y; int index = target.getIndex(); @@ -1471,10 +1474,10 @@ void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext while (index != endIndex) { AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(index); - // AJT: FIXME: TODO: this wont work for horizontal splines... + // FIXME: TODO: this wont work for horizontal splines... float ratio = (defaultPose.trans().y - basePose.trans().y) / baseToTipHeight; - // compute offset from default spline pose to default pose. + // 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; diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 459b37d0b6..ff1ab9115f 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -117,10 +117,11 @@ 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; - float ratio; - AnimPose offsetPose; + 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> _splineJointInfoMap; From 551426f46e1d645049e344dbb1c5309fc0800771 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 1 Jun 2017 13:56:06 -0700 Subject: [PATCH 10/14] Made computeSplineJointInfosForIKTarget more general It should now work for non-vertical oriented splines. --- libraries/animation/src/AnimInverseKinematics.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 524d08de5b..12d480a578 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1466,16 +1466,16 @@ void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext // measure the total arc length along the spline float totalArcLength = spline.arcLength(1.0f); - // FIXME: TODO: this won't work for horizontal splines... - float baseToTipHeight = tipPose.trans().y - basePose.trans().y; + 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); - // FIXME: TODO: this wont work for horizontal splines... - float ratio = (defaultPose.trans().y - basePose.trans().y) / baseToTipHeight; + float ratio = glm::dot(defaultPose.trans() - basePose.trans(), baseToTipNormal) / baseToTipLength; // compute offset from spline to the default pose. float t = spline.arcLengthInverse(ratio * totalArcLength); From 3710637254a01e8b48e83f720d26973de9430338 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 1 Jun 2017 15:17:02 -0700 Subject: [PATCH 11/14] warning fix --- libraries/shared/src/CubicHermiteSpline.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/shared/src/CubicHermiteSpline.h b/libraries/shared/src/CubicHermiteSpline.h index 17b4fda991..d03136867a 100644 --- a/libraries/shared/src/CubicHermiteSpline.h +++ b/libraries/shared/src/CubicHermiteSpline.h @@ -37,7 +37,7 @@ public: float t2 = t * t; float w0 = -6.0f * t + 6.0f * t2; float w1 = 1.0f - 4.0f * t + 3.0f * t2; - float w2 = 6.0 * t - 6.0f * t2; + float w2 = 6.0f * t - 6.0f * t2; float w3 = -2.0f * t + 3.0f * t2; return w0 * _p0 + w1 * _m0 + w2 * _p1 + w3 * _m1; } From 2422c7e1bb73e76b3a1a53bb106aff74b4c17d03 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 2 Jun 2017 15:47:51 -0700 Subject: [PATCH 12/14] code review feedback --- .../animation/src/AnimInverseKinematics.cpp | 31 +++++++++++-------- .../animation/src/TranslationAccumulator.h | 10 +++--- libraries/shared/src/CubicHermiteSpline.h | 6 ++-- 3 files changed, 26 insertions(+), 21 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 12d480a578..c155f4733f 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -540,16 +540,21 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co if (splineJointInfo.jointIndex != _hipsIndex) { // constrain the amount the spine can stretch or compress float length = glm::length(relPose.trans()); - 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; + 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); } } @@ -1521,10 +1526,10 @@ void AnimInverseKinematics::debugDrawSpineSplines(const AnimContext& context, co // draw red and white stripped spline, parameterized by arc length. // i.e. each stripe should be the same length. AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); - int NUM_SUBDIVISIONS = 20; - const float dArcLength = totalArcLength / NUM_SUBDIVISIONS; + const int NUM_SEGMENTS = 20; + const float dArcLength = totalArcLength / NUM_SEGMENTS; float arcLength = 0.0f; - for (int i = 0; i < NUM_SUBDIVISIONS; i++) { + for (int i = 0; i < NUM_SEGMENTS; i++) { float prevT = spline.arcLengthInverse(arcLength); float nextT = spline.arcLengthInverse(arcLength + dArcLength); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(spline(prevT)), geomToWorldPose.xformPoint(spline(nextT)), (i % 2) == 0 ? RED : WHITE); diff --git a/libraries/animation/src/TranslationAccumulator.h b/libraries/animation/src/TranslationAccumulator.h index 65f8a0dc97..18cac5ec7a 100644 --- a/libraries/animation/src/TranslationAccumulator.h +++ b/libraries/animation/src/TranslationAccumulator.h @@ -18,19 +18,19 @@ public: int size() const { return _totalWeight > 0.0f; } - /// \param rotation rotation to add - /// \param weight contribution factor of this rotation to total accumulation + /// \param translation translation to add + /// \param weight contribution factor of this translation to total accumulation void add(const glm::vec3& translation, float weight = 1.0f); glm::vec3 getAverage(); - /// \return true if any rotations were accumulated + /// \return true if any translation were accumulated bool isDirty() const { return _isDirty; } - /// \brief clear accumulated rotation but don't change _isDirty + /// \brief clear accumulated translation but don't change _isDirty void clear(); - /// \brief clear accumulated rotation and set _isDirty to false + /// \brief clear accumulated translation and set _isDirty to false void clearAndClean(); private: diff --git a/libraries/shared/src/CubicHermiteSpline.h b/libraries/shared/src/CubicHermiteSpline.h index d03136867a..a393e4c51f 100644 --- a/libraries/shared/src/CubicHermiteSpline.h +++ b/libraries/shared/src/CubicHermiteSpline.h @@ -23,8 +23,8 @@ public: // 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 t3 = t2 * 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; @@ -63,7 +63,7 @@ public: enum Constants { NUM_SUBDIVISIONS = 30 }; CubicHermiteSplineFunctorWithArcLength() : CubicHermiteSplineFunctor() { - memset(_values, 0, sizeof(float) * NUM_SUBDIVISIONS + 1); + memset(_values, 0, sizeof(float) * (NUM_SUBDIVISIONS + 1)); } 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. @@ -80,7 +80,7 @@ public: } CubicHermiteSplineFunctorWithArcLength(const CubicHermiteSplineFunctorWithArcLength& orig) : CubicHermiteSplineFunctor(orig) { - memcpy(_values, orig._values, sizeof(float) * NUM_SUBDIVISIONS + 1); + 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. From 1a8f6b8de00c379f6efb35167382b7734afd8ea8 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 2 Jun 2017 18:16:14 -0700 Subject: [PATCH 13/14] Readability improvement --- libraries/shared/src/CubicHermiteSpline.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/shared/src/CubicHermiteSpline.h b/libraries/shared/src/CubicHermiteSpline.h index a393e4c51f..da2ed26de4 100644 --- a/libraries/shared/src/CubicHermiteSpline.h +++ b/libraries/shared/src/CubicHermiteSpline.h @@ -71,11 +71,11 @@ public: float alpha = 0.0f; float accum = 0.0f; _values[0] = 0.0f; - for (int i = 0; i < NUM_SUBDIVISIONS; i++) { + for (int i = 1; i < NUM_SUBDIVISIONS + 1; i++) { accum += glm::distance(this->operator()(alpha), this->operator()(alpha + DELTA)); alpha += DELTA; - _values[i + 1] = accum; + _values[i] = accum; } } From 8334dff610f1c206a1ef4c1f9b731a272bf03786 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 8 Jun 2017 15:00:12 -0700 Subject: [PATCH 14/14] compute rotation from derivative of spline This should fix bad rotation values for the spine during bowing/touching toes. --- .../resources/avatar/avatar-animation.json | 6 +- .../animation/src/AnimInverseKinematics.cpp | 159 +++++++++++------- .../animation/src/AnimInverseKinematics.h | 16 +- 3 files changed, 106 insertions(+), 75 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 35f2d4b9af..1412b45968 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -103,8 +103,8 @@ "rotationVar": "spine2Rotation", "typeVar": "spine2Type", "weightVar": "spine2Weight", - "weight": 1.0, - "flexCoefficients": [1.0, 0.5, 0.5] + "weight": 2.0, + "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", diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index c155f4733f..ed61058f6c 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/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& 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 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::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 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* 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* 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)); - AnimPose absPose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose; - AnimPose relPose = parentAbsPose.inverse() * absPose; + glm::quat twistRot = glm::normalize(glm::lerp(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; _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 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& targets) const { for (auto& target : targets) { diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index ff1ab9115f..352238d1e4 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/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& 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* 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> _splineJointInfoMap; // experimental data for moving hips during IK