implementing the spline ik for the spline as an anim node

libraries/animation/src/AnimContext.h

@@ -28,6 +28,7 @@ enum class AnimNodeType {
     InverseKinematics,
     DefaultPose,
     TwoBoneIK,
+    SplineIK,
     PoleVectorConstraint,
     NumTypes
 };

libraries/animation/src/AnimNodeLoader.cpp

@@ -26,6 +26,7 @@
 #include "AnimInverseKinematics.h"
 #include "AnimDefaultPose.h"
 #include "AnimTwoBoneIK.h"
+#include "AnimSplineIK.h"
 #include "AnimPoleVectorConstraint.h"
 
 using NodeLoaderFunc = AnimNode::Pointer (*)(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
@@ -41,6 +42,7 @@ static AnimNode::Pointer loadManipulatorNode(const QJsonObject& jsonObj, const Q
 static AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
 static AnimNode::Pointer loadDefaultPoseNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
 static AnimNode::Pointer loadTwoBoneIKNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
+static AnimNode::Pointer loadSplineIKNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
 static AnimNode::Pointer loadPoleVectorConstraintNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
 
 static const float ANIM_GRAPH_LOAD_PRIORITY = 10.0f;
@@ -123,6 +125,7 @@ static NodeLoaderFunc animNodeTypeToLoaderFunc(AnimNode::Type type) {
     case AnimNode::Type::InverseKinematics: return loadInverseKinematicsNode;
     case AnimNode::Type::DefaultPose: return loadDefaultPoseNode;
     case AnimNode::Type::TwoBoneIK: return loadTwoBoneIKNode;
+    case AnimNode::Type::SplineIK: return loadSplineIKNode;
     case AnimNode::Type::PoleVectorConstraint: return loadPoleVectorConstraintNode;
     case AnimNode::Type::NumTypes: return nullptr;
     };
@@ -140,6 +143,7 @@ static NodeProcessFunc animNodeTypeToProcessFunc(AnimNode::Type type) {
     case AnimNode::Type::InverseKinematics: return processDoNothing;
     case AnimNode::Type::DefaultPose: return processDoNothing;
     case AnimNode::Type::TwoBoneIK: return processDoNothing;
+    case AnimNode::Type::SplineIK: return processDoNothing;
     case AnimNode::Type::PoleVectorConstraint: return processDoNothing;
     case AnimNode::Type::NumTypes: return nullptr;
     };
@@ -574,6 +578,24 @@ static AnimNode::Pointer loadDefaultPoseNode(const QJsonObject& jsonObj, const Q
     return node;
 }
 
+static AnimNode::Pointer loadSplineIKNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl) {
+    READ_FLOAT(alpha, jsonObj, id, jsonUrl, nullptr);
+    READ_BOOL(enabled, jsonObj, id, jsonUrl, nullptr);
+    READ_FLOAT(interpDuration, jsonObj, id, jsonUrl, nullptr);
+    READ_STRING(baseJointName, jsonObj, id, jsonUrl, nullptr);
+    READ_STRING(tipJointName, jsonObj, id, jsonUrl, nullptr);
+    READ_STRING(alphaVar, jsonObj, id, jsonUrl, nullptr);
+    READ_STRING(enabledVar, jsonObj, id, jsonUrl, nullptr);
+    READ_STRING(endEffectorRotationVarVar, jsonObj, id, jsonUrl, nullptr);
+    READ_STRING(endEffectorPositionVarVar, jsonObj, id, jsonUrl, nullptr);
+
+    auto node = std::make_shared<AnimSplineIK>(id, alpha, enabled, interpDuration,
+        baseJointName, tipJointName,
+        alphaVar, enabledVar,
+        endEffectorRotationVarVar, endEffectorPositionVarVar);
+    return node;
+}
+
 static AnimNode::Pointer loadTwoBoneIKNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl) {
     READ_FLOAT(alpha, jsonObj, id, jsonUrl, nullptr);
     READ_BOOL(enabled, jsonObj, id, jsonUrl, nullptr);

libraries/animation/src/AnimSplineIK.cpp

@@ -0,0 +1,77 @@
+//
+//  AnimSplineIK.cpp
+//
+//  Created by Angus Antley on 1/7/19.
+//  Copyright (c) 2019 High Fidelity, Inc. All rights reserved.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "AnimSplineIK.h"
+#include "AnimationLogging.h"
+#include "CubicHermiteSpline.h"
+#include <DebugDraw.h>
+#include "AnimUtil.h"
+
+AnimSplineIK::AnimSplineIK(const QString& id, float alpha, bool enabled, float interpDuration,
+    const QString& baseJointName,
+    const QString& tipJointName,
+    const QString& alphaVar, const QString& enabledVar,
+    const QString& endEffectorRotationVarVar, const QString& endEffectorPositionVarVar) :
+    AnimNode(AnimNode::Type::SplineIK, id),
+    _alpha(alpha),
+    _enabled(enabled),
+    _interpDuration(interpDuration),
+    _baseJointName(baseJointName),
+    _tipJointName(tipJointName),
+    _alphaVar(alphaVar),
+    _enabledVar(enabledVar),
+    _endEffectorRotationVarVar(endEffectorRotationVarVar),
+    _endEffectorPositionVarVar(endEffectorPositionVarVar),
+    _prevEndEffectorRotationVar(),
+    _prevEndEffectorPositionVar() {
+
+}
+
+AnimSplineIK::~AnimSplineIK() {
+
+}
+
+const AnimPoseVec& AnimSplineIK::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
+    qCDebug(animation) << "evaluating the spline function";
+    assert(_children.size() == 1);
+    if (_children.size() != 1) {
+        return _poses;
+    }
+
+    // evalute underPoses
+    AnimPoseVec underPoses = _children[0]->evaluate(animVars, context, dt, triggersOut);
+    _poses = underPoses;
+    return _poses;
+}
+
+void AnimSplineIK::lookUpIndices() {
+    assert(_skeleton);
+
+    // look up bone indices by name
+    std::vector<int> indices = _skeleton->lookUpJointIndices({ _baseJointName, _tipJointName });
+
+    // cache the results
+    _baseJointIndex = indices[0];
+    _tipJointIndex = indices[1];
+
+    if (_baseJointIndex != -1) {
+        _baseParentJointIndex = _skeleton->getParentIndex(_baseJointIndex);
+    }
+}
+
+// for AnimDebugDraw rendering
+const AnimPoseVec& AnimSplineIK::getPosesInternal() const {
+    return _poses;
+}
+
+void AnimSplineIK::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
+    AnimNode::setSkeletonInternal(skeleton);
+    lookUpIndices();
+}

libraries/animation/src/AnimSplineIK.h

@@ -0,0 +1,76 @@
+//
+//  AnimSplineIK.h
+//
+//  Created by Angus Antley on 1/7/19.
+//  Copyright (c) 2019 High Fidelity, Inc. All rights reserved.
+//
+//  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_AnimSplineIK_h
+#define hifi_AnimSplineIK_h
+
+#include "AnimNode.h"
+#include "AnimChain.h"
+
+// Spline IK for the spine
+class AnimSplineIK : public AnimNode {
+public:
+    AnimSplineIK(const QString& id, float alpha, bool enabled, float interpDuration,
+        const QString& baseJointName, const QString& tipJointName,
+        const QString& alphaVar, const QString& enabledVar,
+        const QString& endEffectorRotationVarVar, const QString& endEffectorPositionVarVar);
+
+	virtual ~AnimSplineIK() override;
+    virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
+
+protected:
+
+    enum class InterpType {
+        None = 0,
+        SnapshotToUnderPoses,
+        SnapshotToSolve,
+        NumTypes
+    };
+
+    // for AnimDebugDraw rendering
+    virtual const AnimPoseVec& getPosesInternal() const override;
+    virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
+
+    void lookUpIndices();
+
+    AnimPoseVec _poses;
+
+    float _alpha;
+    bool _enabled;
+    float _interpDuration;
+    QString _baseJointName;
+    QString _tipJointName;
+
+    int _baseParentJointIndex{ -1 };
+    int _baseJointIndex{ -1 };
+    int _tipJointIndex{ -1 };
+
+    QString _alphaVar;  // float - (0, 1) 0 means underPoses only, 1 means IK only.
+    QString _enabledVar;  // bool
+    QString _endEffectorRotationVarVar; // string
+    QString _endEffectorPositionVarVar; // string
+
+    QString _prevEndEffectorRotationVar;
+    QString _prevEndEffectorPositionVar;
+
+    InterpType _interpType{ InterpType::None };
+    float _interpAlphaVel{ 0.0f };
+    float _interpAlpha{ 0.0f };
+
+    AnimChain _snapshotChain;
+
+    bool _lastEnableDebugDrawIKTargets{ false };
+
+    // no copies
+    AnimSplineIK(const AnimSplineIK&) = delete;
+    AnimSplineIK&  operator=(const AnimSplineIK&) = delete;
+
+};
+#endif // hifi_AnimSplineIK_h