From 8334dff610f1c206a1ef4c1f9b731a272bf03786 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Thu, 8 Jun 2017 15:00:12 -0700
Subject: [PATCH] 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<float>& 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<SplineJointInfo> 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::SplineJointInfo>* 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<int, DebugJoint> 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<SplineJointInfo>* 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<SplineJointInfo>* 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<SplineJointInfo> 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<IKTarget>& 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<IKTarget>& 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<SplineJointInfo>* 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<int, std::vector<SplineJointInfo>> _splineJointInfoMap;
// experimental data for moving hips during IK