From 33ff5188c11b949fff7371c9ec155edffae0d52e Mon Sep 17 00:00:00 2001 From: amantley Date: Tue, 15 Jan 2019 18:28:29 -0800 Subject: [PATCH] adding the spline code to the splineik class --- libraries/animation/src/AnimSplineIK.cpp | 230 ++++++++++++++++++++++- libraries/animation/src/AnimSplineIK.h | 17 ++ 2 files changed, 246 insertions(+), 1 deletion(-) diff --git a/libraries/animation/src/AnimSplineIK.cpp b/libraries/animation/src/AnimSplineIK.cpp index db6b6864b1..65d7bfc821 100644 --- a/libraries/animation/src/AnimSplineIK.cpp +++ b/libraries/animation/src/AnimSplineIK.cpp @@ -44,10 +44,46 @@ const AnimPoseVec& AnimSplineIK::evaluate(const AnimVariantMap& animVars, const if (_children.size() != 1) { return _poses; } - // evalute underPoses AnimPoseVec underPoses = _children[0]->evaluate(animVars, context, dt, triggersOut); _poses = underPoses; + + // check to see if we actually need absolute poses. + AnimPoseVec absolutePoses; + absolutePoses.resize(_poses.size()); + computeAbsolutePoses(absolutePoses); + + IKTarget target; + int jointIndex = _skeleton->nameToJointIndex("Head"); + if (jointIndex != -1) { + target.setType(animVars.lookup("HeadType", (int)IKTarget::Type::RotationAndPosition)); + target.setIndex(jointIndex); + AnimPose absPose = _skeleton->getAbsolutePose(jointIndex, _poses); + glm::quat rotation = animVars.lookupRigToGeometry("headRotation", absPose.rot()); + glm::vec3 translation = animVars.lookupRigToGeometry("headPosition", absPose.trans()); + float weight = animVars.lookup("headWeight", "4.0"); + + target.setPose(rotation, translation); + target.setWeight(weight); + const float* flexCoefficients = new float[5]{ 1.0f, 0.5f, 0.25f, 0.2f, 0.1f }; + target.setFlexCoefficients(4, flexCoefficients); + + // record the index of the hips ik target. + if (target.getIndex() == _hipsIndex) { + _hipsTargetIndex = 1; + } + } + if (_poses.size() > 0) { + AnimChain jointChain; + jointChain.buildFromRelativePoses(_skeleton, _poses, target.getIndex()); + + // for each target solve target with spline + + solveTargetWithSpline(context, target, absolutePoses, false, jointChain); + qCDebug(animation) << "made it past the spline solve code"; + } + // we need to blend the old joint chain with the current joint chain, otherwise known as: _snapShotChain + /**/ return _poses; } @@ -66,6 +102,20 @@ void AnimSplineIK::lookUpIndices() { } } +void AnimSplineIK::computeAbsolutePoses(AnimPoseVec& absolutePoses) const { + int numJoints = (int)_poses.size(); + assert(numJoints <= _skeleton->getNumJoints()); + assert(numJoints == (int)absolutePoses.size()); + for (int i = 0; i < numJoints; ++i) { + int parentIndex = _skeleton->getParentIndex(i); + if (parentIndex < 0) { + absolutePoses[i] = _poses[i]; + } else { + absolutePoses[i] = absolutePoses[parentIndex] * _poses[i]; + } + } +} + // for AnimDebugDraw rendering const AnimPoseVec& AnimSplineIK::getPosesInternal() const { return _poses; @@ -73,5 +123,183 @@ const AnimPoseVec& AnimSplineIK::getPosesInternal() const { void AnimSplineIK::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) { AnimNode::setSkeletonInternal(skeleton); + _headIndex = _skeleton->nameToJointIndex("Head"); + _hipsIndex = _skeleton->nameToJointIndex("Hips"); lookUpIndices(); +} + +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 AnimSplineIK::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug, AnimChain& chainInfoOut) const { + + const int baseIndex = _hipsIndex; + + // 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. + 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); + + // 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 = safeLerp(basePose.rot(), tipPose.rot(), 0.5f); + if (glm::dot(halfRot * Vectors::UNIT_Z, basePose.rot() * Vectors::UNIT_Z) < 0.0f) { + tipPose.rot() = -tipPose.rot(); + } + qCDebug(animation) << "spot 1"; + // 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(); + qCDebug(animation) << "spot 2"; + // 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); + + // 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 twistRot = safeLerp(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; + + bool constrained = false; + if (splineJointInfo.jointIndex != _hipsIndex) { + // constrain the amount the spine can stretch or compress + float length = glm::length(relPose.trans()); + 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); + } + } + // note we are ignoring the constrained info for now. + if (!chainInfoOut.setRelativePoseAtJointIndex(splineJointInfo.jointIndex, relPose)) { + qCDebug(animation) << "we didn't find the joint index for the spline!!!!"; + } + + parentAbsPose = flexedAbsPose; + } + } + + if (debug) { + //debugDrawIKChain(jointChainInfoOut, context); + } +} + +const std::vector* AnimSplineIK::findOrCreateSplineJointInfo(const AnimContext& context, const IKTarget& target) const { + // find or create splineJointInfo for this target + auto iter = _splineJointInfoMap.find(target.getIndex()); + if (iter != _splineJointInfoMap.end()) { + return &(iter->second); + } else { + computeAndCacheSplineJointInfosForIKTarget(context, target); + auto iter = _splineJointInfoMap.find(target.getIndex()); + if (iter != _splineJointInfoMap.end()) { + return &(iter->second); + } + } + + return nullptr; +} + +// pre-compute information about each joint influenced by this spline IK target. +void AnimSplineIK::computeAndCacheSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) const { + 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; } \ No newline at end of file diff --git a/libraries/animation/src/AnimSplineIK.h b/libraries/animation/src/AnimSplineIK.h index 36a29971a0..79f012365a 100644 --- a/libraries/animation/src/AnimSplineIK.h +++ b/libraries/animation/src/AnimSplineIK.h @@ -12,6 +12,7 @@ #define hifi_AnimSplineIK_h #include "AnimNode.h" +#include "IKTarget.h" #include "AnimChain.h" // Spline IK for the spine @@ -34,6 +35,8 @@ protected: NumTypes }; + void computeAbsolutePoses(AnimPoseVec& absolutePoses) const; + // for AnimDebugDraw rendering virtual const AnimPoseVec& getPosesInternal() const override; virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override; @@ -51,6 +54,9 @@ protected: int _baseParentJointIndex{ -1 }; int _baseJointIndex{ -1 }; int _tipJointIndex{ -1 }; + int _headIndex{ -1 }; + int _hipsIndex{ -1 }; + int _hipsTargetIndex{ -1 }; QString _alphaVar; // float - (0, 1) 0 means underPoses only, 1 means IK only. QString _enabledVar; // bool @@ -66,7 +72,18 @@ protected: AnimChain _snapshotChain; + // 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. + }; + bool _lastEnableDebugDrawIKTargets{ false }; + void AnimSplineIK::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug, AnimChain& chainInfoOut) const; + void computeAndCacheSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) const; + const std::vector* findOrCreateSplineJointInfo(const AnimContext& context, const IKTarget& target) const; + mutable std::map> _splineJointInfoMap; // no copies AnimSplineIK(const AnimSplineIK&) = delete;