From c7e4bf931b49e3dd04c8f4595572e8395efe1850 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Tue, 27 Jun 2017 19:23:59 -0700
Subject: [PATCH 01/12] WIP: first steps toward smoothing ik chains
---
.../animation/src/AnimInverseKinematics.cpp | 109 +++++++++++-------
.../animation/src/AnimInverseKinematics.h | 8 +-
libraries/animation/src/AnimSkeleton.cpp | 14 +++
libraries/animation/src/AnimSkeleton.h | 1 +
4 files changed, 85 insertions(+), 47 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index d7076a443e..4ec10b9faf 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -23,18 +23,18 @@
#include "CubicHermiteSpline.h"
#include "AnimUtil.h"
-static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointChainInfos, size_t numJointChainInfos,
+static void lookupJointChainInfo(const std::vector<AnimInverseKinematics::JointChainInfo>& jointChainInfoVec,
int indexA, int indexB,
- AnimInverseKinematics::JointChainInfo** jointChainInfoA,
- AnimInverseKinematics::JointChainInfo** jointChainInfoB) {
+ const AnimInverseKinematics::JointChainInfo** jointChainInfoA,
+ const AnimInverseKinematics::JointChainInfo** jointChainInfoB) {
*jointChainInfoA = nullptr;
*jointChainInfoB = nullptr;
- for (size_t i = 0; i < numJointChainInfos; i++) {
- if (jointChainInfos[i].jointIndex == indexA) {
- *jointChainInfoA = jointChainInfos + i;
+ for (size_t i = 0; i < jointChainInfoVec.size(); i++) {
+ if (jointChainInfoVec[i].jointIndex == indexA) {
+ *jointChainInfoA = &jointChainInfoVec[i];
}
- if (jointChainInfos[i].jointIndex == indexB) {
- *jointChainInfoB = jointChainInfos + i;
+ if (jointChainInfoVec[i].jointIndex == indexB) {
+ *jointChainInfoB = &jointChainInfoVec[i];
}
if (*jointChainInfoA && *jointChainInfoB) {
break;
@@ -234,6 +234,17 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
accumulator.clearAndClean();
}
+ // initialize jointChainInfoVecVec, this holds the results for one iteration of each ik chain.
+ JointChainInfo defaultJointChainInfo = { glm::quat(), glm::vec3(), 0.0f, -1, false };
+ std::vector<std::vector<JointChainInfo>> jointChainInfoVecVec(targets.size());
+ for (size_t i = 0; i < targets.size(); i++) {
+ int chainDepth = _skeleton->getChainDepth(targets[i].getIndex());
+ jointChainInfoVecVec[i].reserve(chainDepth);
+ for (size_t j = 0; j < chainDepth; j++) {
+ jointChainInfoVecVec[i].push_back(defaultJointChainInfo);
+ }
+ }
+
float maxError = FLT_MAX;
int numLoops = 0;
const int MAX_IK_LOOPS = 16;
@@ -244,11 +255,25 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
bool debug = context.getEnableDebugDrawIKChains() && numLoops == MAX_IK_LOOPS;
// solve all targets
- for (auto& target: targets) {
- if (target.getType() == IKTarget::Type::Spline) {
- solveTargetWithSpline(context, target, absolutePoses, debug);
+ for (size_t i = 0; i < targets.size(); i++) {
+ if (targets[i].getType() == IKTarget::Type::Spline) {
+ solveTargetWithSpline(context, targets[i], absolutePoses, debug, jointChainInfoVecVec[i]);
} else {
- solveTargetWithCCD(context, target, absolutePoses, debug);
+ solveTargetWithCCD(context, targets[i], absolutePoses, debug, jointChainInfoVecVec[i]);
+ }
+ }
+
+ // TODO: do smooth interpolation of joint chains here, if necessary.
+
+ // copy jointChainInfoVecs into accumulators
+ for (size_t i = 0; i < targets.size(); i++) {
+ const std::vector<JointChainInfo>& jointChainInfoVec = jointChainInfoVecVec[i];
+ for (size_t j = 0; j < jointChainInfoVec.size(); j++) {
+ const JointChainInfo& info = jointChainInfoVec[j];
+ if (info.jointIndex >= 0) {
+ _rotationAccumulators[info.jointIndex].add(info.relRot, info.weight);
+ _translationAccumulators[info.jointIndex].add(info.relTrans, info.weight);
+ }
}
}
@@ -310,7 +335,8 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
}
}
-void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug) {
+void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
+ bool debug, std::vector<JointChainInfo>& jointChainInfoVec) const {
size_t chainDepth = 0;
IKTarget::Type targetType = target.getType();
@@ -338,9 +364,6 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
// the tip's parent-relative as we proceed up the chain
glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot();
- const size_t MAX_CHAIN_DEPTH = 30;
- JointChainInfo jointChainInfos[MAX_CHAIN_DEPTH];
-
// NOTE: if this code is removed, the head will remain rigid, causing the spine/hips to thrust forward backward
// as the head is nodded.
if (targetType == IKTarget::Type::HmdHead ||
@@ -368,7 +391,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
}
glm::vec3 tipRelativeTranslation = _relativePoses[target.getIndex()].trans();
- jointChainInfos[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, target.getWeight(), tipIndex, constrained };
+ jointChainInfoVec[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, target.getWeight(), tipIndex, constrained };
}
// cache tip absolute position
@@ -379,7 +402,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
// descend toward root, pivoting each joint to get tip closer to target position
while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) {
- assert(chainDepth < MAX_CHAIN_DEPTH);
+ assert(chainDepth < jointChainInfoVec.size());
// compute the two lines that should be aligned
glm::vec3 jointPosition = absolutePoses[pivotIndex].trans();
@@ -480,7 +503,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
}
glm::vec3 newTrans = _relativePoses[pivotIndex].trans();
- jointChainInfos[chainDepth] = { newRot, newTrans, target.getWeight(), pivotIndex, constrained };
+ jointChainInfoVec[chainDepth] = { newRot, newTrans, target.getWeight(), pivotIndex, constrained };
// keep track of tip's new transform as we descend towards root
tipPosition = jointPosition + deltaRotation * (tipPosition - jointPosition);
@@ -502,24 +525,25 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
int baseParentJointIndex = _skeleton->getParentIndex(baseJointIndex);
AnimPose topPose, midPose, basePose;
int topChainIndex = -1, baseChainIndex = -1;
+ const size_t MAX_CHAIN_DEPTH = 30;
AnimPose postAbsPoses[MAX_CHAIN_DEPTH];
AnimPose accum = absolutePoses[_hipsIndex];
AnimPose baseParentPose = absolutePoses[_hipsIndex];
for (int i = (int)chainDepth - 1; i >= 0; i--) {
- accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfos[i].relRot, jointChainInfos[i].relTrans);
+ accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfoVec[i].relRot, jointChainInfoVec[i].relTrans);
postAbsPoses[i] = accum;
- if (jointChainInfos[i].jointIndex == topJointIndex) {
+ if (jointChainInfoVec[i].jointIndex == topJointIndex) {
topChainIndex = i;
topPose = accum;
}
- if (jointChainInfos[i].jointIndex == midJointIndex) {
+ if (jointChainInfoVec[i].jointIndex == midJointIndex) {
midPose = accum;
}
- if (jointChainInfos[i].jointIndex == baseJointIndex) {
+ if (jointChainInfoVec[i].jointIndex == baseJointIndex) {
baseChainIndex = i;
basePose = accum;
}
- if (jointChainInfos[i].jointIndex == baseParentJointIndex) {
+ if (jointChainInfoVec[i].jointIndex == baseParentJointIndex) {
baseParentPose = accum;
}
}
@@ -599,21 +623,16 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
}
glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot();
- jointChainInfos[baseChainIndex].relRot = newBaseRelRot;
+ jointChainInfoVec[baseChainIndex].relRot = newBaseRelRot;
glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot();
- jointChainInfos[topChainIndex].relRot = newTopRelRot;
+ jointChainInfoVec[topChainIndex].relRot = newTopRelRot;
}
}
}
- for (size_t i = 0; i < chainDepth; i++) {
- _rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight);
- _translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight);
- }
-
if (debug) {
- debugDrawIKChain(jointChainInfos, chainDepth, context);
+ debugDrawIKChain(jointChainInfoVec, context);
}
}
@@ -697,10 +716,9 @@ const std::vector<AnimInverseKinematics::SplineJointInfo>* AnimInverseKinematics
return nullptr;
}
-void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug) {
-
- const size_t MAX_CHAIN_DEPTH = 30;
- JointChainInfo jointChainInfos[MAX_CHAIN_DEPTH];
+// AJT: TODO: make this const someday
+void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
+ bool debug, std::vector<JointChainInfo>& jointChainInfoVec) {
const int baseIndex = _hipsIndex;
@@ -783,19 +801,22 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
}
}
- jointChainInfos[i] = { relPose.rot(), relPose.trans(), target.getWeight(), splineJointInfo.jointIndex, constrained };
+ jointChainInfoVec[i] = { relPose.rot(), relPose.trans(), target.getWeight(), splineJointInfo.jointIndex, constrained };
parentAbsPose = flexedAbsPose;
}
}
+ // AJT: REMOVE
+ /*
for (size_t i = 0; i < splineJointInfoVec->size(); i++) {
_rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight);
_translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight);
}
+ */
if (debug) {
- debugDrawIKChain(jointChainInfos, splineJointInfoVec->size(), context);
+ debugDrawIKChain(jointChainInfoVec, context);
}
}
@@ -1495,12 +1516,12 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c
}
}
-void AnimInverseKinematics::debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, const AnimContext& context) const {
+void AnimInverseKinematics::debugDrawIKChain(const std::vector<JointChainInfo>& jointChainInfoVec, const AnimContext& context) const {
AnimPoseVec poses = _relativePoses;
// copy debug joint rotations into the relative poses
- for (size_t i = 0; i < numJointChainInfos; i++) {
- const JointChainInfo& info = jointChainInfos[i];
+ for (size_t i = 0; i < jointChainInfoVec.size(); i++) {
+ const JointChainInfo& info = jointChainInfoVec[i];
poses[info.jointIndex].rot() = info.relRot;
poses[info.jointIndex].trans() = info.relTrans;
}
@@ -1519,9 +1540,9 @@ void AnimInverseKinematics::debugDrawIKChain(JointChainInfo* jointChainInfos, si
// draw each pose
for (int i = 0; i < (int)poses.size(); i++) {
int parentIndex = _skeleton->getParentIndex(i);
- JointChainInfo* jointInfo = nullptr;
- JointChainInfo* parentJointInfo = nullptr;
- lookupJointChainInfo(jointChainInfos, numJointChainInfos, i, parentIndex, &jointInfo, &parentJointInfo);
+ const JointChainInfo* jointInfo = nullptr;
+ const JointChainInfo* parentJointInfo = nullptr;
+ lookupJointChainInfo(jointChainInfoVec, i, parentIndex, &jointInfo, &parentJointInfo);
if (jointInfo && parentJointInfo) {
// transform local axes into world space.
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index d473ae3698..152034b596 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -73,10 +73,12 @@ public:
protected:
void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
void solve(const AnimContext& context, const std::vector<IKTarget>& 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);
+ void solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
+ bool debug, std::vector<JointChainInfo>& jointChainInfoVec) const;
+ void solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
+ bool debug, std::vector<JointChainInfo>& jointChainInfoVec);
virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
- void debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, const AnimContext& context) const;
+ void debugDrawIKChain(const std::vector<JointChainInfo>& jointChainInfoVec, const AnimContext& context) const;
void debugDrawRelativePoses(const AnimContext& context) const;
void debugDrawConstraints(const AnimContext& context) const;
void debugDrawSpineSplines(const AnimContext& context, const std::vector<IKTarget>& targets) const;
diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp
index 062e016660..804ffb0583 100644
--- a/libraries/animation/src/AnimSkeleton.cpp
+++ b/libraries/animation/src/AnimSkeleton.cpp
@@ -42,6 +42,20 @@ int AnimSkeleton::getNumJoints() const {
return _jointsSize;
}
+int AnimSkeleton::getChainDepth(int jointIndex) const {
+ if (jointIndex >= 0) {
+ int chainDepth = 0;
+ int index = jointIndex;
+ do {
+ chainDepth++;
+ index = _joints[index].parentIndex;
+ } while (index != -1);
+ return chainDepth;
+ } else {
+ return 0;
+ }
+}
+
const AnimPose& AnimSkeleton::getAbsoluteBindPose(int jointIndex) const {
return _absoluteBindPoses[jointIndex];
}
diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h
index 6315f2d62b..99c9a148f7 100644
--- a/libraries/animation/src/AnimSkeleton.h
+++ b/libraries/animation/src/AnimSkeleton.h
@@ -28,6 +28,7 @@ public:
int nameToJointIndex(const QString& jointName) const;
const QString& getJointName(int jointIndex) const;
int getNumJoints() const;
+ int getChainDepth(int jointIndex) const;
// absolute pose, not relative to parent
const AnimPose& getAbsoluteBindPose(int jointIndex) const;
From 75e1a4a1e6f389da13861b2b2854f6e0e948efb4 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Wed, 28 Jun 2017 13:31:15 -0700
Subject: [PATCH 02/12] Refactor of JointChainInfo data structure
---
.../animation/src/AnimInverseKinematics.cpp | 103 +++++++++---------
.../animation/src/AnimInverseKinematics.h | 20 ++--
2 files changed, 62 insertions(+), 61 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 4ec10b9faf..ea87fab4c0 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -23,20 +23,21 @@
#include "CubicHermiteSpline.h"
#include "AnimUtil.h"
-static void lookupJointChainInfo(const std::vector<AnimInverseKinematics::JointChainInfo>& jointChainInfoVec,
+static void lookupJointInfo(const AnimInverseKinematics::JointChainInfo& jointChainInfo,
int indexA, int indexB,
- const AnimInverseKinematics::JointChainInfo** jointChainInfoA,
- const AnimInverseKinematics::JointChainInfo** jointChainInfoB) {
- *jointChainInfoA = nullptr;
- *jointChainInfoB = nullptr;
- for (size_t i = 0; i < jointChainInfoVec.size(); i++) {
- if (jointChainInfoVec[i].jointIndex == indexA) {
- *jointChainInfoA = &jointChainInfoVec[i];
+ const AnimInverseKinematics::JointInfo** jointInfoA,
+ const AnimInverseKinematics::JointInfo** jointInfoB) {
+ *jointInfoA = nullptr;
+ *jointInfoB = nullptr;
+ for (size_t i = 0; i < jointChainInfo.jointInfoVec.size(); i++) {
+ const AnimInverseKinematics::JointInfo* jointInfo = &jointChainInfo.jointInfoVec[i];
+ if (jointInfo->jointIndex == indexA) {
+ *jointInfoA = jointInfo;
}
- if (jointChainInfoVec[i].jointIndex == indexB) {
- *jointChainInfoB = &jointChainInfoVec[i];
+ if (jointInfo->jointIndex == indexB) {
+ *jointInfoB = jointInfo;
}
- if (*jointChainInfoA && *jointChainInfoB) {
+ if (*jointInfoA && *jointInfoB) {
break;
}
}
@@ -234,14 +235,15 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
accumulator.clearAndClean();
}
- // initialize jointChainInfoVecVec, this holds the results for one iteration of each ik chain.
- JointChainInfo defaultJointChainInfo = { glm::quat(), glm::vec3(), 0.0f, -1, false };
- std::vector<std::vector<JointChainInfo>> jointChainInfoVecVec(targets.size());
+ // initialize a new jointChainInfoVec, this will holds the results for solving each ik chain.
+ JointInfo defaultJointInfo = { glm::quat(), glm::vec3(), -1, false };
+ JointChainInfoVec jointChainInfoVec(targets.size());
for (size_t i = 0; i < targets.size(); i++) {
int chainDepth = _skeleton->getChainDepth(targets[i].getIndex());
- jointChainInfoVecVec[i].reserve(chainDepth);
+ jointChainInfoVec[i].jointInfoVec.reserve(chainDepth);
+ jointChainInfoVec[i].target = targets[i];
for (size_t j = 0; j < chainDepth; j++) {
- jointChainInfoVecVec[i].push_back(defaultJointChainInfo);
+ jointChainInfoVec[i].jointInfoVec.push_back(defaultJointInfo);
}
}
@@ -257,9 +259,9 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
// solve all targets
for (size_t i = 0; i < targets.size(); i++) {
if (targets[i].getType() == IKTarget::Type::Spline) {
- solveTargetWithSpline(context, targets[i], absolutePoses, debug, jointChainInfoVecVec[i]);
+ solveTargetWithSpline(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]);
} else {
- solveTargetWithCCD(context, targets[i], absolutePoses, debug, jointChainInfoVecVec[i]);
+ solveTargetWithCCD(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]);
}
}
@@ -267,12 +269,13 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
// copy jointChainInfoVecs into accumulators
for (size_t i = 0; i < targets.size(); i++) {
- const std::vector<JointChainInfo>& jointChainInfoVec = jointChainInfoVecVec[i];
- for (size_t j = 0; j < jointChainInfoVec.size(); j++) {
- const JointChainInfo& info = jointChainInfoVec[j];
+ const std::vector<JointInfo>& jointInfoVec = jointChainInfoVec[i].jointInfoVec;
+ float weight = jointChainInfoVec[i].target.getWeight();
+ for (size_t j = 0; j < jointInfoVec.size(); j++) {
+ const JointInfo& info = jointInfoVec[j];
if (info.jointIndex >= 0) {
- _rotationAccumulators[info.jointIndex].add(info.relRot, info.weight);
- _translationAccumulators[info.jointIndex].add(info.relTrans, info.weight);
+ _rotationAccumulators[info.jointIndex].add(info.rot, weight);
+ _translationAccumulators[info.jointIndex].add(info.trans, weight);
}
}
}
@@ -336,7 +339,7 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
}
void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
- bool debug, std::vector<JointChainInfo>& jointChainInfoVec) const {
+ bool debug, JointChainInfo& jointChainInfoOut) const {
size_t chainDepth = 0;
IKTarget::Type targetType = target.getType();
@@ -391,7 +394,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
}
glm::vec3 tipRelativeTranslation = _relativePoses[target.getIndex()].trans();
- jointChainInfoVec[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, target.getWeight(), tipIndex, constrained };
+ jointChainInfoOut.jointInfoVec[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, tipIndex, constrained };
}
// cache tip absolute position
@@ -402,7 +405,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
// descend toward root, pivoting each joint to get tip closer to target position
while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) {
- assert(chainDepth < jointChainInfoVec.size());
+ assert(chainDepth < jointChainInfoOut.jointInfoVec.size());
// compute the two lines that should be aligned
glm::vec3 jointPosition = absolutePoses[pivotIndex].trans();
@@ -503,7 +506,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
}
glm::vec3 newTrans = _relativePoses[pivotIndex].trans();
- jointChainInfoVec[chainDepth] = { newRot, newTrans, target.getWeight(), pivotIndex, constrained };
+ jointChainInfoOut.jointInfoVec[chainDepth] = { newRot, newTrans, pivotIndex, constrained };
// keep track of tip's new transform as we descend towards root
tipPosition = jointPosition + deltaRotation * (tipPosition - jointPosition);
@@ -530,20 +533,20 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
AnimPose accum = absolutePoses[_hipsIndex];
AnimPose baseParentPose = absolutePoses[_hipsIndex];
for (int i = (int)chainDepth - 1; i >= 0; i--) {
- accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfoVec[i].relRot, jointChainInfoVec[i].relTrans);
+ accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfoOut.jointInfoVec[i].rot, jointChainInfoOut.jointInfoVec[i].trans);
postAbsPoses[i] = accum;
- if (jointChainInfoVec[i].jointIndex == topJointIndex) {
+ if (jointChainInfoOut.jointInfoVec[i].jointIndex == topJointIndex) {
topChainIndex = i;
topPose = accum;
}
- if (jointChainInfoVec[i].jointIndex == midJointIndex) {
+ if (jointChainInfoOut.jointInfoVec[i].jointIndex == midJointIndex) {
midPose = accum;
}
- if (jointChainInfoVec[i].jointIndex == baseJointIndex) {
+ if (jointChainInfoOut.jointInfoVec[i].jointIndex == baseJointIndex) {
baseChainIndex = i;
basePose = accum;
}
- if (jointChainInfoVec[i].jointIndex == baseParentJointIndex) {
+ if (jointChainInfoOut.jointInfoVec[i].jointIndex == baseParentJointIndex) {
baseParentPose = accum;
}
}
@@ -623,16 +626,16 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
}
glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot();
- jointChainInfoVec[baseChainIndex].relRot = newBaseRelRot;
+ jointChainInfoOut.jointInfoVec[baseChainIndex].rot = newBaseRelRot;
glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot();
- jointChainInfoVec[topChainIndex].relRot = newTopRelRot;
+ jointChainInfoOut.jointInfoVec[topChainIndex].rot = newTopRelRot;
}
}
}
if (debug) {
- debugDrawIKChain(jointChainInfoVec, context);
+ debugDrawIKChain(jointChainInfoOut, context);
}
}
@@ -718,7 +721,7 @@ const std::vector<AnimInverseKinematics::SplineJointInfo>* AnimInverseKinematics
// AJT: TODO: make this const someday
void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
- bool debug, std::vector<JointChainInfo>& jointChainInfoVec) {
+ bool debug, JointChainInfo& jointChainInfoOut) {
const int baseIndex = _hipsIndex;
@@ -801,22 +804,14 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
}
}
- jointChainInfoVec[i] = { relPose.rot(), relPose.trans(), target.getWeight(), splineJointInfo.jointIndex, constrained };
+ jointChainInfoOut.jointInfoVec[i] = { relPose.rot(), relPose.trans(), splineJointInfo.jointIndex, constrained };
parentAbsPose = flexedAbsPose;
}
}
- // AJT: REMOVE
- /*
- for (size_t i = 0; i < splineJointInfoVec->size(); i++) {
- _rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight);
- _translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight);
- }
- */
-
if (debug) {
- debugDrawIKChain(jointChainInfoVec, context);
+ debugDrawIKChain(jointChainInfoOut, context);
}
}
@@ -1516,14 +1511,14 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c
}
}
-void AnimInverseKinematics::debugDrawIKChain(const std::vector<JointChainInfo>& jointChainInfoVec, const AnimContext& context) const {
+void AnimInverseKinematics::debugDrawIKChain(const JointChainInfo& jointChainInfo, const AnimContext& context) const {
AnimPoseVec poses = _relativePoses;
// copy debug joint rotations into the relative poses
- for (size_t i = 0; i < jointChainInfoVec.size(); i++) {
- const JointChainInfo& info = jointChainInfoVec[i];
- poses[info.jointIndex].rot() = info.relRot;
- poses[info.jointIndex].trans() = info.relTrans;
+ for (size_t i = 0; i < jointChainInfo.jointInfoVec.size(); i++) {
+ const JointInfo& info = jointChainInfo.jointInfoVec[i];
+ poses[info.jointIndex].rot() = info.rot;
+ poses[info.jointIndex].trans() = info.trans;
}
// convert relative poses to absolute
@@ -1540,9 +1535,9 @@ void AnimInverseKinematics::debugDrawIKChain(const std::vector<JointChainInfo>&
// draw each pose
for (int i = 0; i < (int)poses.size(); i++) {
int parentIndex = _skeleton->getParentIndex(i);
- const JointChainInfo* jointInfo = nullptr;
- const JointChainInfo* parentJointInfo = nullptr;
- lookupJointChainInfo(jointChainInfoVec, i, parentIndex, &jointInfo, &parentJointInfo);
+ const JointInfo* jointInfo = nullptr;
+ const JointInfo* parentJointInfo = nullptr;
+ lookupJointInfo(jointChainInfo, i, parentIndex, &jointInfo, &parentJointInfo);
if (jointInfo && parentJointInfo) {
// transform local axes into world space.
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index 152034b596..38288aa288 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -26,14 +26,20 @@ class RotationConstraint;
class AnimInverseKinematics : public AnimNode {
public:
- struct JointChainInfo {
- glm::quat relRot;
- glm::vec3 relTrans;
- float weight;
+ struct JointInfo {
+ glm::quat rot;
+ glm::vec3 trans;
int jointIndex;
bool constrained;
};
+ struct JointChainInfo {
+ std::vector<JointInfo> jointInfoVec;
+ IKTarget target;
+ };
+
+ using JointChainInfoVec = std::vector<JointChainInfo>;
+
explicit AnimInverseKinematics(const QString& id);
virtual ~AnimInverseKinematics() override;
@@ -74,11 +80,11 @@ protected:
void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
void solve(const AnimContext& context, const std::vector<IKTarget>& targets);
void solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
- bool debug, std::vector<JointChainInfo>& jointChainInfoVec) const;
+ bool debug, JointChainInfo& jointChainInfoOut) const;
void solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
- bool debug, std::vector<JointChainInfo>& jointChainInfoVec);
+ bool debug, JointChainInfo& jointChainInfoOut);
virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
- void debugDrawIKChain(const std::vector<JointChainInfo>& jointChainInfoVec, const AnimContext& context) const;
+ void debugDrawIKChain(const JointChainInfo& jointChainInfo, const AnimContext& context) const;
void debugDrawRelativePoses(const AnimContext& context) const;
void debugDrawConstraints(const AnimContext& context) const;
void debugDrawSpineSplines(const AnimContext& context, const std::vector<IKTarget>& targets) const;
From 237872e4779ad9a6c0e3007e1b3e9dae3ca04c25 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Fri, 30 Jun 2017 12:47:01 -0700
Subject: [PATCH 03/12] sizes and order of IKTargetVarVec and IKTargetVec are
now the same.
Also, A change in how the bone name to bone index lookup occurs exposed a bug
in Rig::computeAvatarBoundingCapsule(), basically it was not actually preforming IK,
and the ik targets were in the wrong coordinate frame. So when IK was actually
performed it would give bad results. This bug is now fixed.
---
.../animation/src/AnimInverseKinematics.cpp | 56 +++++++------------
.../animation/src/AnimInverseKinematics.h | 4 --
libraries/animation/src/IKTarget.h | 2 +-
libraries/animation/src/Rig.cpp | 18 +++---
4 files changed, 32 insertions(+), 48 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index ea87fab4c0..5ea628d1f4 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -150,24 +150,26 @@ void AnimInverseKinematics::setTargetVars(const QString& jointName, const QStrin
}
void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses) {
- // build a list of valid targets from _targetVarVec and animVars
- _maxTargetIndex = -1;
+
_hipsTargetIndex = -1;
- bool removeUnfoundJoints = false;
+
+ targets.reserve(_targetVarVec.size());
for (auto& targetVar : _targetVarVec) {
+
+ // update targetVar jointIndex cache
if (targetVar.jointIndex == -1) {
- // this targetVar hasn't been validated yet...
int jointIndex = _skeleton->nameToJointIndex(targetVar.jointName);
if (jointIndex >= 0) {
// this targetVar has a valid joint --> cache the indices
targetVar.jointIndex = jointIndex;
} else {
qCWarning(animation) << "AnimInverseKinematics could not find jointName" << targetVar.jointName << "in skeleton";
- removeUnfoundJoints = true;
}
- } else {
- IKTarget target;
+ }
+
+ IKTarget target;
+ if (targetVar.jointIndex != -1) {
target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition));
if (target.getType() != IKTarget::Type::Unknown) {
AnimPose absPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);
@@ -189,35 +191,16 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
glm::vec3 poleReferenceVector = animVars.lookupRigToGeometryVector(targetVar.poleReferenceVectorVar, Vectors::UNIT_Z);
target.setPoleReferenceVector(glm::normalize(poleReferenceVector));
- targets.push_back(target);
-
- if (targetVar.jointIndex > _maxTargetIndex) {
- _maxTargetIndex = targetVar.jointIndex;
- }
-
// record the index of the hips ik target.
if (target.getIndex() == _hipsIndex) {
- _hipsTargetIndex = (int)targets.size() - 1;
+ _hipsTargetIndex = (int)targets.size();
}
}
+ } else {
+ target.setType((int)IKTarget::Type::Unknown);
}
- }
- if (removeUnfoundJoints) {
- int numVars = (int)_targetVarVec.size();
- int i = 0;
- while (i < numVars) {
- if (_targetVarVec[i].jointIndex == -1) {
- if (numVars > 1) {
- // swap i for last element
- _targetVarVec[i] = _targetVarVec[numVars - 1];
- }
- _targetVarVec.pop_back();
- --numVars;
- } else {
- ++i;
- }
- }
+ targets.push_back(target);
}
}
@@ -258,10 +241,15 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
// solve all targets
for (size_t i = 0; i < targets.size(); i++) {
- if (targets[i].getType() == IKTarget::Type::Spline) {
+ switch (targets[i].getType()) {
+ case IKTarget::Type::Unknown:
+ break;
+ case IKTarget::Type::Spline:
solveTargetWithSpline(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]);
- } else {
+ break;
+ default:
solveTargetWithCCD(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]);
+ break;
}
}
@@ -317,7 +305,7 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
// finally set the relative rotation of each tip to agree with absolute target rotation
for (auto& target: targets) {
int tipIndex = target.getIndex();
- int parentIndex = _skeleton->getParentIndex(tipIndex);
+ int parentIndex = (tipIndex >= 0) ? _skeleton->getParentIndex(tipIndex) : -1;
// update rotationOnly targets that don't lie on the ik chain of other ik targets.
if (parentIndex != -1 && !_rotationAccumulators[tipIndex].isDirty() && target.getType() == IKTarget::Type::RotationOnly) {
@@ -1430,8 +1418,6 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
targetVar.jointIndex = -1;
}
- _maxTargetIndex = -1;
-
for (auto& accumulator: _rotationAccumulators) {
accumulator.clearAndClean();
}
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index 38288aa288..fb462cbf50 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -156,10 +156,6 @@ protected:
int _leftHandIndex { -1 };
int _rightHandIndex { -1 };
- // _maxTargetIndex is tracked to help optimize the recalculation of absolute poses
- // during the the cyclic coordinate descent algorithm
- int _maxTargetIndex { 0 };
-
float _maxErrorOnLastSolve { FLT_MAX };
bool _previousEnableDebugIKTargets { false };
SolutionSource _solutionSource { SolutionSource::RelaxToUnderPoses };
diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h
index 5567539659..a86ae0ca8b 100644
--- a/libraries/animation/src/IKTarget.h
+++ b/libraries/animation/src/IKTarget.h
@@ -57,7 +57,7 @@ private:
bool _poleVectorEnabled { false };
int _index { -1 };
Type _type { Type::RotationAndPosition };
- float _weight;
+ float _weight { 0.0f };
float _flexCoefficients[MAX_FLEX_COEFFICIENTS];
size_t _numFlexCoefficients;
};
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 3d04b0b26f..7b11465062 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1707,36 +1707,38 @@ void Rig::computeAvatarBoundingCapsule(
AnimPose geometryToRig = _modelOffset * _geometryOffset;
- AnimPose hips(glm::vec3(1), glm::quat(), glm::vec3());
+ glm::vec3 hipsPosition(0.0f);
int hipsIndex = indexOfJoint("Hips");
if (hipsIndex >= 0) {
- hips = geometryToRig * _animSkeleton->getAbsoluteBindPose(hipsIndex);
+ hipsPosition = transformPoint(_geometryToRigTransform, _animSkeleton->getAbsoluteDefaultPose(hipsIndex).trans());
}
AnimVariantMap animVars;
+ animVars.setRigToGeometryTransform(_rigToGeometryTransform);
glm::quat handRotation = glm::angleAxis(PI, Vectors::UNIT_X);
- animVars.set("leftHandPosition", hips.trans());
+ animVars.set("leftHandPosition", hipsPosition);
animVars.set("leftHandRotation", handRotation);
animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
- animVars.set("rightHandPosition", hips.trans());
+ animVars.set("rightHandPosition", hipsPosition);
animVars.set("rightHandRotation", handRotation);
animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
int rightFootIndex = indexOfJoint("RightFoot");
int leftFootIndex = indexOfJoint("LeftFoot");
if (rightFootIndex != -1 && leftFootIndex != -1) {
- glm::vec3 foot = Vectors::ZERO;
+ glm::vec3 geomFootPosition = glm::vec3(0.0f, _animSkeleton->getAbsoluteDefaultPose(rightFootIndex).trans().y, 0.0f);
+ glm::vec3 footPosition = transformPoint(_geometryToRigTransform, geomFootPosition);
glm::quat footRotation = glm::angleAxis(0.5f * PI, Vectors::UNIT_X);
- animVars.set("leftFootPosition", foot);
+ animVars.set("leftFootPosition", footPosition);
animVars.set("leftFootRotation", footRotation);
animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
- animVars.set("rightFootPosition", foot);
+ animVars.set("rightFootPosition", footPosition);
animVars.set("rightFootRotation", footRotation);
animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
}
// call overlay twice: once to verify AnimPoseVec joints and again to do the IK
AnimNode::Triggers triggersOut;
- AnimContext context(false, false, false, glm::mat4(), glm::mat4());
+ AnimContext context(false, false, false, _geometryToRigTransform, _rigToGeometryTransform);
float dt = 1.0f; // the value of this does not matter
ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
AnimPoseVec finalPoses = ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
From aba164b26e2e08ad6ea1a9e48b55d509c183f27a Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Fri, 30 Jun 2017 13:27:53 -0700
Subject: [PATCH 04/12] more clean up of Rig::computeAvatarBoundingCapsule
---
libraries/animation/src/Rig.cpp | 10 ++++------
1 file changed, 4 insertions(+), 6 deletions(-)
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 7b11465062..0dfeb43f3d 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1704,9 +1704,6 @@ void Rig::computeAvatarBoundingCapsule(
ikNode.setTargetVars("RightFoot", "rightFootPosition", "rightFootRotation",
"rightFootType", "rightFootWeight", 1.0f, {},
QString(), QString(), QString());
-
- AnimPose geometryToRig = _modelOffset * _geometryOffset;
-
glm::vec3 hipsPosition(0.0f);
int hipsIndex = indexOfJoint("Hips");
if (hipsIndex >= 0) {
@@ -1771,14 +1768,15 @@ void Rig::computeAvatarBoundingCapsule(
// compute bounding shape parameters
// NOTE: we assume that the longest side of totalExtents is the yAxis...
- glm::vec3 diagonal = (geometryToRig * totalExtents.maximum) - (geometryToRig * totalExtents.minimum);
+ glm::vec3 diagonal = (transformPoint(_geometryToRigTransform, totalExtents.maximum) -
+ transformPoint(_geometryToRigTransform, totalExtents.minimum));
// ... and assume the radiusOut is half the RMS of the X and Z sides:
radiusOut = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
heightOut = diagonal.y - 2.0f * radiusOut;
glm::vec3 rootPosition = finalPoses[geometry.rootJointIndex].trans();
- glm::vec3 rigCenter = (geometryToRig * (0.5f * (totalExtents.maximum + totalExtents.minimum)));
- localOffsetOut = rigCenter - (geometryToRig * rootPosition);
+ glm::vec3 rigCenter = transformPoint(_geometryToRigTransform, (0.5f * (totalExtents.maximum + totalExtents.minimum)));
+ localOffsetOut = rigCenter - transformPoint(_geometryToRigTransform, rootPosition);
}
bool Rig::transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand,
From 2f6a37ee53fc5e1546a1592259d94d7d71d4ce21 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 3 Jul 2017 16:31:05 -0700
Subject: [PATCH 05/12] Removed interpolation of hand controllers
---
libraries/animation/src/Rig.cpp | 79 +++------------------------------
libraries/animation/src/Rig.h | 9 ----
2 files changed, 6 insertions(+), 82 deletions(-)
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 0dfeb43f3d..c505353174 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1093,25 +1093,10 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f;
if (leftHandEnabled) {
- if (!_isLeftHandControlled) {
- _leftHandControlTimeRemaining = CONTROL_DURATION;
- _isLeftHandControlled = true;
- }
glm::vec3 handPosition = leftHandPose.trans();
glm::quat handRotation = leftHandPose.rot();
- if (_leftHandControlTimeRemaining > 0.0f) {
- // Move hand from non-controlled position to controlled position.
- _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f);
- AnimPose handPose(Vectors::ONE, handRotation, handPosition);
- if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose,
- LEFT_HAND, TO_CONTROLLED, handPose)) {
- handPosition = handPose.trans();
- handRotation = handPose.rot();
- }
- }
-
if (!hipsEnabled) {
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 displacement;
@@ -1124,9 +1109,6 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
_animVars.set("leftHandRotation", handRotation);
_animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
- _lastLeftHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition);
- _isLeftHandControlled = true;
-
// compute pole vector
int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand");
int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm");
@@ -1154,47 +1136,17 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
_prevLeftHandPoleVectorValid = false;
_animVars.set("leftHandPoleVectorEnabled", false);
- if (_isLeftHandControlled) {
- _leftHandRelaxTimeRemaining = RELAX_DURATION;
- _isLeftHandControlled = false;
- }
+ _animVars.unset("leftHandPosition");
+ _animVars.unset("leftHandRotation");
+ _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
- if (_leftHandRelaxTimeRemaining > 0.0f) {
- // Move hand from controlled position to non-controlled position.
- _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f);
- AnimPose handPose;
- if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose,
- LEFT_HAND, FROM_CONTROLLED, handPose)) {
- _animVars.set("leftHandPosition", handPose.trans());
- _animVars.set("leftHandRotation", handPose.rot());
- _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
- }
- } else {
- _animVars.unset("leftHandPosition");
- _animVars.unset("leftHandRotation");
- _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
- }
}
if (rightHandEnabled) {
- if (!_isRightHandControlled) {
- _rightHandControlTimeRemaining = CONTROL_DURATION;
- _isRightHandControlled = true;
- }
glm::vec3 handPosition = rightHandPose.trans();
glm::quat handRotation = rightHandPose.rot();
- if (_rightHandControlTimeRemaining > 0.0f) {
- // Move hand from non-controlled position to controlled position.
- _rightHandControlTimeRemaining = std::max(_rightHandControlTimeRemaining - dt, 0.0f);
- AnimPose handPose(Vectors::ONE, handRotation, handPosition);
- if (transitionHandPose(_rightHandControlTimeRemaining, CONTROL_DURATION, handPose, RIGHT_HAND, TO_CONTROLLED, handPose)) {
- handPosition = handPose.trans();
- handRotation = handPose.rot();
- }
- }
-
if (!hipsEnabled) {
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 displacement;
@@ -1207,9 +1159,6 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
_animVars.set("rightHandRotation", handRotation);
_animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
- _lastRightHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition);
- _isRightHandControlled = true;
-
// compute pole vector
int handJointIndex = _animSkeleton->nameToJointIndex("RightHand");
int armJointIndex = _animSkeleton->nameToJointIndex("RightArm");
@@ -1237,25 +1186,9 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
_prevRightHandPoleVectorValid = false;
_animVars.set("rightHandPoleVectorEnabled", false);
- if (_isRightHandControlled) {
- _rightHandRelaxTimeRemaining = RELAX_DURATION;
- _isRightHandControlled = false;
- }
-
- if (_rightHandRelaxTimeRemaining > 0.0f) {
- // Move hand from controlled position to non-controlled position.
- _rightHandRelaxTimeRemaining = std::max(_rightHandRelaxTimeRemaining - dt, 0.0f);
- AnimPose handPose;
- if (transitionHandPose(_rightHandRelaxTimeRemaining, RELAX_DURATION, _lastRightHandControlledPose, RIGHT_HAND, FROM_CONTROLLED, handPose)) {
- _animVars.set("rightHandPosition", handPose.trans());
- _animVars.set("rightHandRotation", handPose.rot());
- _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
- }
- } else {
- _animVars.unset("rightHandPosition");
- _animVars.unset("rightHandRotation");
- _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
- }
+ _animVars.unset("rightHandPosition");
+ _animVars.unset("rightHandRotation");
+ _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
}
}
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index c17a7b9c8f..ec13d98613 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -343,15 +343,6 @@ protected:
bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand,
bool isToControlled, AnimPose& returnHandPose);
- bool _isLeftHandControlled { false };
- bool _isRightHandControlled { false };
- float _leftHandControlTimeRemaining { 0.0f };
- float _rightHandControlTimeRemaining { 0.0f };
- float _leftHandRelaxTimeRemaining { 0.0f };
- float _rightHandRelaxTimeRemaining { 0.0f };
- AnimPose _lastLeftHandControlledPose;
- AnimPose _lastRightHandControlledPose;
-
glm::vec3 _prevRightFootPoleVector { Vectors::UNIT_Z };
bool _prevRightFootPoleVectorValid { false };
From 7ed1382ac98b4ff0dc0479713126e3832c9b554a Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 3 Jul 2017 16:32:46 -0700
Subject: [PATCH 06/12] ik level interpolation of incoming targets
---
.../animation/src/AnimInverseKinematics.cpp | 140 +++++++++++++-----
.../animation/src/AnimInverseKinematics.h | 13 +-
2 files changed, 114 insertions(+), 39 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 5ea628d1f4..dd13279786 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -23,6 +23,8 @@
#include "CubicHermiteSpline.h"
#include "AnimUtil.h"
+static const float JOINT_CHAIN_INTERP_TIME = 0.25f;
+
static void lookupJointInfo(const AnimInverseKinematics::JointChainInfo& jointChainInfo,
int indexA, int indexB,
const AnimInverseKinematics::JointInfo** jointInfoA,
@@ -171,6 +173,7 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
IKTarget target;
if (targetVar.jointIndex != -1) {
target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition));
+ target.setIndex(targetVar.jointIndex);
if (target.getType() != IKTarget::Type::Unknown) {
AnimPose absPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);
glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, absPose.rot());
@@ -178,7 +181,6 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
float weight = animVars.lookup(targetVar.weightVar, targetVar.weight);
target.setPose(rotation, translation);
- target.setIndex(targetVar.jointIndex);
target.setWeight(weight);
target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients);
@@ -204,7 +206,7 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
}
}
-void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<IKTarget>& targets) {
+void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<IKTarget>& targets, float dt, JointChainInfoVec& jointChainInfoVec) {
// compute absolute poses that correspond to relative target poses
AnimPoseVec absolutePoses;
absolutePoses.resize(_relativePoses.size());
@@ -218,23 +220,10 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
accumulator.clearAndClean();
}
- // initialize a new jointChainInfoVec, this will holds the results for solving each ik chain.
- JointInfo defaultJointInfo = { glm::quat(), glm::vec3(), -1, false };
- JointChainInfoVec jointChainInfoVec(targets.size());
- for (size_t i = 0; i < targets.size(); i++) {
- int chainDepth = _skeleton->getChainDepth(targets[i].getIndex());
- jointChainInfoVec[i].jointInfoVec.reserve(chainDepth);
- jointChainInfoVec[i].target = targets[i];
- for (size_t j = 0; j < chainDepth; j++) {
- jointChainInfoVec[i].jointInfoVec.push_back(defaultJointInfo);
- }
- }
-
- float maxError = FLT_MAX;
+ float maxError = 0.0f;
int numLoops = 0;
const int MAX_IK_LOOPS = 16;
- const float MAX_ERROR_TOLERANCE = 0.1f; // cm
- while (maxError > MAX_ERROR_TOLERANCE && numLoops < MAX_IK_LOOPS) {
+ while (numLoops < MAX_IK_LOOPS) {
++numLoops;
bool debug = context.getEnableDebugDrawIKChains() && numLoops == MAX_IK_LOOPS;
@@ -253,23 +242,45 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
}
}
- // TODO: do smooth interpolation of joint chains here, if necessary.
+ // on last iteration, interpolate jointChains, if necessary
+ if (numLoops == MAX_IK_LOOPS) {
+ for (size_t i = 0; i < _prevJointChainInfoVec.size(); i++) {
+ if (_prevJointChainInfoVec[i].timer > 0.0f) {
+ float alpha = (JOINT_CHAIN_INTERP_TIME - _prevJointChainInfoVec[i].timer) / JOINT_CHAIN_INTERP_TIME;
+ size_t chainSize = std::min(_prevJointChainInfoVec[i].jointInfoVec.size(), jointChainInfoVec[i].jointInfoVec.size());
+ for (size_t j = 0; j < chainSize; j++) {
+ jointChainInfoVec[i].jointInfoVec[j].rot = safeMix(_prevJointChainInfoVec[i].jointInfoVec[j].rot, jointChainInfoVec[i].jointInfoVec[j].rot, alpha);
+ jointChainInfoVec[i].jointInfoVec[j].trans = lerp(_prevJointChainInfoVec[i].jointInfoVec[j].trans, jointChainInfoVec[i].jointInfoVec[j].trans, alpha);
+ }
+ }
+ }
+ }
// copy jointChainInfoVecs into accumulators
for (size_t i = 0; i < targets.size(); i++) {
const std::vector<JointInfo>& jointInfoVec = jointChainInfoVec[i].jointInfoVec;
- float weight = jointChainInfoVec[i].target.getWeight();
- for (size_t j = 0; j < jointInfoVec.size(); j++) {
- const JointInfo& info = jointInfoVec[j];
- if (info.jointIndex >= 0) {
- _rotationAccumulators[info.jointIndex].add(info.rot, weight);
- _translationAccumulators[info.jointIndex].add(info.trans, weight);
+
+ // don't accumulate disabled or rotation only ik targets.
+ IKTarget::Type type = jointChainInfoVec[i].target.getType();
+ if (type != IKTarget::Type::Unknown && type != IKTarget::Type::RotationOnly) {
+ float weight = jointChainInfoVec[i].target.getWeight();
+ if (weight > 0.0f) {
+ for (size_t j = 0; j < jointInfoVec.size(); j++) {
+ const JointInfo& info = jointInfoVec[j];
+ if (info.jointIndex >= 0) {
+ _rotationAccumulators[info.jointIndex].add(info.rot, weight);
+ _translationAccumulators[info.jointIndex].add(info.trans, weight);
+ }
+ }
}
}
}
// harvest accumulated rotations and apply the average
for (int i = 0; i < (int)_relativePoses.size(); ++i) {
+ if (i == _hipsIndex) {
+ continue; // don't apply accumulators to hips
+ }
if (_rotationAccumulators[i].size() > 0) {
_relativePoses[i].rot() = _rotationAccumulators[i].getAverage();
_rotationAccumulators[i].clear();
@@ -324,6 +335,28 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
absolutePoses[tipIndex].rot() = targetRotation;
}
}
+
+ for (size_t i = 0; i < jointChainInfoVec.size(); i++) {
+ _prevJointChainInfoVec[i].timer = _prevJointChainInfoVec[i].timer - dt;
+ if (_prevJointChainInfoVec[i].timer <= 0.0f) {
+ _prevJointChainInfoVec[i] = jointChainInfoVec[i];
+ // store relative poses into unknown/rotation only joint chains.
+ // so we have something to interpolate from if this chain is activated.
+ IKTarget::Type type = _prevJointChainInfoVec[i].target.getType();
+ if (type == IKTarget::Type::Unknown || type == IKTarget::Type::RotationOnly) {
+ for (size_t j = 0; j < _prevJointChainInfoVec[i].jointInfoVec.size(); j++) {
+ auto& info = _prevJointChainInfoVec[i].jointInfoVec[j];
+ if (info.jointIndex >= 0) {
+ info.rot = _relativePoses[info.jointIndex].rot();
+ info.trans = _relativePoses[info.jointIndex].trans();
+ } else {
+ info.rot = Quaternions::IDENTITY;
+ info.trans = glm::vec3(0.0f);
+ }
+ }
+ }
+ }
+ }
}
void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
@@ -638,7 +671,7 @@ static CubicHermiteSplineFunctorWithArcLength computeSplineFromTipAndBase(const
}
// pre-compute information about each joint influeced by this spline IK target.
-void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) {
+void AnimInverseKinematics::computeAndCacheSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) const {
std::vector<SplineJointInfo> splineJointInfoVec;
// build spline between the default poses.
@@ -691,13 +724,13 @@ void AnimInverseKinematics::computeSplineJointInfosForIKTarget(const AnimContext
_splineJointInfoMap[target.getIndex()] = splineJointInfoVec;
}
-const std::vector<AnimInverseKinematics::SplineJointInfo>* AnimInverseKinematics::findOrCreateSplineJointInfo(const AnimContext& context, const IKTarget& target) {
+const std::vector<AnimInverseKinematics::SplineJointInfo>* AnimInverseKinematics::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 {
- computeSplineJointInfosForIKTarget(context, target);
+ computeAndCacheSplineJointInfosForIKTarget(context, target);
auto iter = _splineJointInfoMap.find(target.getIndex());
if (iter != _splineJointInfoMap.end()) {
return &(iter->second);
@@ -707,9 +740,8 @@ const std::vector<AnimInverseKinematics::SplineJointInfo>* AnimInverseKinematics
return nullptr;
}
-// AJT: TODO: make this const someday
void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
- bool debug, JointChainInfo& jointChainInfoOut) {
+ bool debug, JointChainInfo& jointChainInfoOut) const {
const int baseIndex = _hipsIndex;
@@ -854,18 +886,57 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
_relativePoses = underPoses;
} else {
+ JointChainInfoVec jointChainInfoVec(targets.size());
+ {
+ PROFILE_RANGE_EX(simulation_animation, "ik/jointChainInfo", 0xffff00ff, 0);
+
+ // initialize a new jointChainInfoVec, this will holds the results for solving each ik chain.
+ JointInfo defaultJointInfo = { glm::quat(), glm::vec3(), -1, false };
+ for (size_t i = 0; i < targets.size(); i++) {
+ int chainDepth = _skeleton->getChainDepth(targets[i].getIndex());
+ jointChainInfoVec[i].jointInfoVec.reserve(chainDepth);
+ jointChainInfoVec[i].target = targets[i];
+ int index = targets[i].getIndex();
+ for (size_t j = 0; j < chainDepth; j++) {
+ jointChainInfoVec[i].jointInfoVec.push_back(defaultJointInfo);
+ jointChainInfoVec[i].jointInfoVec[j].jointIndex = index;
+ index = _skeleton->getParentIndex(index);
+ }
+ }
+
+ _prevJointChainInfoVec.resize(jointChainInfoVec.size());
+ for (size_t i = 0; i < _prevJointChainInfoVec.size(); i++) {
+ if (_prevJointChainInfoVec[i].timer <= 0.0f &&
+ (jointChainInfoVec[i].target.getType() != _prevJointChainInfoVec[i].target.getType() ||
+ jointChainInfoVec[i].target.getPoleVectorEnabled() != _prevJointChainInfoVec[i].target.getPoleVectorEnabled())) {
+ _prevJointChainInfoVec[i].timer = JOINT_CHAIN_INTERP_TIME;
+ }
+ }
+ }
+
{
PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0);
if (_hipsTargetIndex >= 0 && _hipsTargetIndex < (int)targets.size()) {
// slam the hips to match the _hipsTarget
+
AnimPose absPose = targets[_hipsTargetIndex].getPose();
+
int parentIndex = _skeleton->getParentIndex(targets[_hipsTargetIndex].getIndex());
- if (parentIndex != -1) {
- _relativePoses[_hipsIndex] = _skeleton->getAbsolutePose(parentIndex, _relativePoses).inverse() * absPose;
- } else {
- _relativePoses[_hipsIndex] = absPose;
+ AnimPose parentAbsPose = _skeleton->getAbsolutePose(parentIndex, _relativePoses);
+
+ // do smooth interpolation of hips here, if necessary.
+ if (_prevJointChainInfoVec[_hipsTargetIndex].timer > 0.0f) {
+ float alpha = (JOINT_CHAIN_INTERP_TIME - _prevJointChainInfoVec[_hipsTargetIndex].timer) / JOINT_CHAIN_INTERP_TIME;
+
+ auto& info = _prevJointChainInfoVec[_hipsTargetIndex].jointInfoVec[0];
+ AnimPose prevHipsRelPose(info.rot, info.trans);
+ AnimPose prevHipsAbsPose = parentAbsPose * prevHipsRelPose;
+ ::blend(1, &prevHipsAbsPose, &absPose, alpha, &absPose);
}
+
+ _relativePoses[_hipsIndex] = parentAbsPose.inverse() * absPose;
+
} else {
// if there is no hips target, shift hips according to the _hipsOffset from the previous frame
float offsetLength = glm::length(_hipsOffset);
@@ -924,8 +995,9 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
{
PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0);
+
preconditionRelativePosesToAvoidLimbLock(context, targets);
- solve(context, targets);
+ solve(context, targets, dt, jointChainInfoVec);
}
if (_hipsTargetIndex < 0) {
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index fb462cbf50..d5fc5a6a8c 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -36,6 +36,7 @@ public:
struct JointChainInfo {
std::vector<JointInfo> jointInfoVec;
IKTarget target;
+ float timer { 0.0f };
};
using JointChainInfoVec = std::vector<JointChainInfo>;
@@ -78,11 +79,11 @@ public:
protected:
void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
- void solve(const AnimContext& context, const std::vector<IKTarget>& targets);
+ void solve(const AnimContext& context, const std::vector<IKTarget>& targets, float dt, JointChainInfoVec& jointChainInfoVec);
void solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
bool debug, JointChainInfo& jointChainInfoOut) const;
void solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
- bool debug, JointChainInfo& jointChainInfoOut);
+ bool debug, JointChainInfo& jointChainInfoOut) const;
virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
void debugDrawIKChain(const JointChainInfo& jointChainInfo, const AnimContext& context) const;
void debugDrawRelativePoses(const AnimContext& context) const;
@@ -99,8 +100,8 @@ protected:
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);
+ void computeAndCacheSplineJointInfosForIKTarget(const AnimContext& context, const IKTarget& target) const;
+ const std::vector<SplineJointInfo>* findOrCreateSplineJointInfo(const AnimContext& context, const IKTarget& target) const;
// for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; }
@@ -144,7 +145,7 @@ protected:
AnimPoseVec _relativePoses; // current relative poses
AnimPoseVec _limitCenterPoses; // relative
- std::map<int, std::vector<SplineJointInfo>> _splineJointInfoMap;
+ mutable std::map<int, std::vector<SplineJointInfo>> _splineJointInfoMap;
// experimental data for moving hips during IK
glm::vec3 _hipsOffset { Vectors::ZERO };
@@ -164,6 +165,8 @@ protected:
AnimPose _uncontrolledLeftHandPose { AnimPose() };
AnimPose _uncontrolledRightHandPose { AnimPose() };
AnimPose _uncontrolledHipsPose { AnimPose() };
+
+ JointChainInfoVec _prevJointChainInfoVec;
};
#endif // hifi_AnimInverseKinematics_h
From 1a24d4d8ec060f87ac29e8a3bf6d119bdb513ccd Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Wed, 5 Jul 2017 09:31:02 -0700
Subject: [PATCH 07/12] added safeLerp, shortest angle quat lerp with post
normalize
---
.../animation/src/AnimInverseKinematics.cpp | 17 +++++++----------
libraries/animation/src/AnimUtil.cpp | 2 +-
libraries/animation/src/AnimUtil.h | 10 ++++++++++
3 files changed, 18 insertions(+), 11 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index dd13279786..f0cd0dea98 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -491,9 +491,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
glm::quat twistPart;
glm::vec3 axis = glm::normalize(deltaRotation * leverArm);
swingTwistDecomposition(missingRotation, axis, swingPart, twistPart);
- float dotSign = copysignf(1.0f, twistPart.w);
const float LIMIT_LEAK_FRACTION = 0.1f;
- deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * twistPart, LIMIT_LEAK_FRACTION)) * deltaRotation;
+ deltaRotation = safeLerp(glm::quat(), twistPart, LIMIT_LEAK_FRACTION);
}
}
}
@@ -502,9 +501,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
// An HmdHead target slaves the orientation of the end-effector by distributing rotation
// deltas up the hierarchy. Its target position is enforced later (by shifting the hips).
deltaRotation = target.getRotation() * glm::inverse(tipOrientation);
- float dotSign = copysignf(1.0f, deltaRotation.w);
const float ANGLE_DISTRIBUTION_FACTOR = 0.45f;
- deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR));
+ deltaRotation = safeLerp(glm::quat(), deltaRotation, ANGLE_DISTRIBUTION_FACTOR);
}
// compute joint's new parent-relative rotation after swing
@@ -761,7 +759,7 @@ 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));
+ 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();
}
@@ -784,7 +782,7 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
if (target.getIndex() == _headIndex) {
rotT = t * t;
}
- glm::quat twistRot = glm::normalize(glm::lerp(basePose.rot(), tipPose.rot(), rotT));
+ 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));
@@ -1682,7 +1680,7 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con
const int NUM_SWING_STEPS = 10;
for (int i = 0; i < NUM_SWING_STEPS + 1; i++) {
- glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS)));
+ glm::quat rot = safeLerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS));
glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_Y);
DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN);
}
@@ -1700,7 +1698,7 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con
const int NUM_SWING_STEPS = 10;
for (int i = 0; i < NUM_SWING_STEPS + 1; i++) {
- glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS)));
+ glm::quat rot = safeLerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS));
glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_X);
DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN);
}
@@ -1740,10 +1738,9 @@ void AnimInverseKinematics::blendToPoses(const AnimPoseVec& targetPoses, const A
// relax toward poses
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 (_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));
+ _relativePoses[i].rot() = safeLerp(_relativePoses[i].rot(), targetPoses[i].rot(), blendFactor);
} else {
// this joint is NOT affected by IK --> slam to underPoses rotation
_relativePoses[i].rot() = underPoses[i].rot();
diff --git a/libraries/animation/src/AnimUtil.cpp b/libraries/animation/src/AnimUtil.cpp
index a4659f1e76..bcf30642e8 100644
--- a/libraries/animation/src/AnimUtil.cpp
+++ b/libraries/animation/src/AnimUtil.cpp
@@ -28,7 +28,7 @@ void blend(size_t numPoses, const AnimPose* a, const AnimPose* b, float alpha, A
}
result[i].scale() = lerp(aPose.scale(), bPose.scale(), alpha);
- result[i].rot() = glm::normalize(glm::lerp(aPose.rot(), q2, alpha));
+ result[i].rot() = safeLerp(aPose.rot(), bPose.rot(), alpha);
result[i].trans() = lerp(aPose.trans(), bPose.trans(), alpha);
}
}
diff --git a/libraries/animation/src/AnimUtil.h b/libraries/animation/src/AnimUtil.h
index 055fd630eb..d215fdc654 100644
--- a/libraries/animation/src/AnimUtil.h
+++ b/libraries/animation/src/AnimUtil.h
@@ -21,4 +21,14 @@ glm::quat averageQuats(size_t numQuats, const glm::quat* quats);
float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag,
const QString& id, AnimNode::Triggers& triggersOut);
+inline glm::quat safeLerp(const glm::quat& a, const glm::quat& b, float alpha) {
+ // adjust signs if necessary
+ glm::quat bTemp = b;
+ float dot = glm::dot(a, bTemp);
+ if (dot < 0.0f) {
+ bTemp = -bTemp;
+ }
+ return glm::normalize(glm::lerp(a, bTemp, alpha));
+}
+
#endif
From 06d512dab90f2444bc5d968ea5f6b21d4a665b53 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Wed, 5 Jul 2017 10:43:24 -0700
Subject: [PATCH 08/12] Warning fixes
---
libraries/animation/src/AnimInverseKinematics.cpp | 2 +-
libraries/animation/src/Rig.cpp | 8 --------
2 files changed, 1 insertion(+), 9 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index f0cd0dea98..7e13c13650 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -891,7 +891,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
// initialize a new jointChainInfoVec, this will holds the results for solving each ik chain.
JointInfo defaultJointInfo = { glm::quat(), glm::vec3(), -1, false };
for (size_t i = 0; i < targets.size(); i++) {
- int chainDepth = _skeleton->getChainDepth(targets[i].getIndex());
+ size_t chainDepth = (size_t)_skeleton->getChainDepth(targets[i].getIndex());
jointChainInfoVec[i].jointInfoVec.reserve(chainDepth);
jointChainInfoVec[i].target = targets[i];
int index = targets[i].getIndex();
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index c505353174..9cc09addb3 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1082,14 +1082,6 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, bodyCapsuleHalfHeight, 0);
const float HAND_RADIUS = 0.05f;
-
- const float RELAX_DURATION = 0.6f;
- const float CONTROL_DURATION = 0.4f;
- const bool TO_CONTROLLED = true;
- const bool FROM_CONTROLLED = false;
- const bool LEFT_HAND = true;
- const bool RIGHT_HAND = false;
-
const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f;
if (leftHandEnabled) {
From bd8d6280a8a4636648b840c2fcfb8d80abe756b8 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Fri, 7 Jul 2017 09:29:57 -0700
Subject: [PATCH 09/12] Interpolate out of ik chains when they are disabled
---
libraries/animation/src/AnimInverseKinematics.cpp | 14 ++++++++++++--
libraries/animation/src/IKTarget.h | 2 +-
2 files changed, 13 insertions(+), 3 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 7e13c13650..a0cb8432d9 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -252,6 +252,14 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
jointChainInfoVec[i].jointInfoVec[j].rot = safeMix(_prevJointChainInfoVec[i].jointInfoVec[j].rot, jointChainInfoVec[i].jointInfoVec[j].rot, alpha);
jointChainInfoVec[i].jointInfoVec[j].trans = lerp(_prevJointChainInfoVec[i].jointInfoVec[j].trans, jointChainInfoVec[i].jointInfoVec[j].trans, alpha);
}
+
+ // if joint chain was just disabled, ramp the weight toward zero.
+ if (_prevJointChainInfoVec[i].target.getType() != IKTarget::Type::Unknown &&
+ jointChainInfoVec[i].target.getType() == IKTarget::Type::Unknown) {
+ IKTarget newTarget = _prevJointChainInfoVec[i].target;
+ newTarget.setWeight(alpha);
+ jointChainInfoVec[i].target = newTarget;
+ }
}
}
}
@@ -336,6 +344,7 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
}
}
+ // copy jointChainInfoVec into _prevJointChainInfoVec, and update timers
for (size_t i = 0; i < jointChainInfoVec.size(); i++) {
_prevJointChainInfoVec[i].timer = _prevJointChainInfoVec[i].timer - dt;
if (_prevJointChainInfoVec[i].timer <= 0.0f) {
@@ -888,7 +897,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
{
PROFILE_RANGE_EX(simulation_animation, "ik/jointChainInfo", 0xffff00ff, 0);
- // initialize a new jointChainInfoVec, this will holds the results for solving each ik chain.
+ // initialize a new jointChainInfoVec, this will hold the results for solving each ik chain.
JointInfo defaultJointInfo = { glm::quat(), glm::vec3(), -1, false };
for (size_t i = 0; i < targets.size(); i++) {
size_t chainDepth = (size_t)_skeleton->getChainDepth(targets[i].getIndex());
@@ -902,6 +911,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
}
}
+ // identity joint chains that have changed types this frame.
_prevJointChainInfoVec.resize(jointChainInfoVec.size());
for (size_t i = 0; i < _prevJointChainInfoVec.size(); i++) {
if (_prevJointChainInfoVec[i].timer <= 0.0f &&
@@ -923,7 +933,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
int parentIndex = _skeleton->getParentIndex(targets[_hipsTargetIndex].getIndex());
AnimPose parentAbsPose = _skeleton->getAbsolutePose(parentIndex, _relativePoses);
- // do smooth interpolation of hips here, if necessary.
+ // do smooth interpolation of hips, if necessary.
if (_prevJointChainInfoVec[_hipsTargetIndex].timer > 0.0f) {
float alpha = (JOINT_CHAIN_INTERP_TIME - _prevJointChainInfoVec[_hipsTargetIndex].timer) / JOINT_CHAIN_INTERP_TIME;
diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h
index a86ae0ca8b..325a1b40b6 100644
--- a/libraries/animation/src/IKTarget.h
+++ b/libraries/animation/src/IKTarget.h
@@ -56,7 +56,7 @@ private:
glm::vec3 _poleReferenceVector;
bool _poleVectorEnabled { false };
int _index { -1 };
- Type _type { Type::RotationAndPosition };
+ Type _type { Type::Unknown };
float _weight { 0.0f };
float _flexCoefficients[MAX_FLEX_COEFFICIENTS];
size_t _numFlexCoefficients;
From 1cdc0071f3cfb489831f90d04171c6418d9d4906 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 10 Jul 2017 16:17:25 -0700
Subject: [PATCH 10/12] Fixed issue with hips and chest not ramping off
properly.
---
.../animation/src/AnimInverseKinematics.cpp | 99 +++++++++++--------
.../animation/src/AnimInverseKinematics.h | 11 +--
libraries/animation/src/Rig.cpp | 22 -----
libraries/animation/src/Rig.h | 3 -
4 files changed, 62 insertions(+), 73 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index a0cb8432d9..8c86ada43c 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -257,7 +257,7 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
if (_prevJointChainInfoVec[i].target.getType() != IKTarget::Type::Unknown &&
jointChainInfoVec[i].target.getType() == IKTarget::Type::Unknown) {
IKTarget newTarget = _prevJointChainInfoVec[i].target;
- newTarget.setWeight(alpha);
+ newTarget.setWeight((1.0f - alpha) * _prevJointChainInfoVec[i].target.getWeight());
jointChainInfoVec[i].target = newTarget;
}
}
@@ -349,6 +349,7 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
_prevJointChainInfoVec[i].timer = _prevJointChainInfoVec[i].timer - dt;
if (_prevJointChainInfoVec[i].timer <= 0.0f) {
_prevJointChainInfoVec[i] = jointChainInfoVec[i];
+ _prevJointChainInfoVec[i].target = targets[i];
// store relative poses into unknown/rotation only joint chains.
// so we have something to interpolate from if this chain is activated.
IKTarget::Type type = _prevJointChainInfoVec[i].target.getType();
@@ -849,6 +850,24 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
return _relativePoses;
}
+AnimPose AnimInverseKinematics::applyHipsOffset() const {
+ glm::vec3 hipsOffset = _hipsOffset;
+ AnimPose relHipsPose = _relativePoses[_hipsIndex];
+ float offsetLength = glm::length(hipsOffset);
+ const float MIN_HIPS_OFFSET_LENGTH = 0.03f;
+ if (offsetLength > MIN_HIPS_OFFSET_LENGTH) {
+ float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength);
+ glm::vec3 scaledHipsOffset = scaleFactor * hipsOffset;
+ if (_hipsParentIndex == -1) {
+ relHipsPose.trans() = _relativePoses[_hipsIndex].trans() + scaledHipsOffset;
+ } else {
+ AnimPose absHipsPose = _skeleton->getAbsolutePose(_hipsIndex, _relativePoses);
+ absHipsPose.trans() += scaledHipsOffset;
+ relHipsPose = _skeleton->getAbsolutePose(_hipsParentIndex, _relativePoses).inverse() * absHipsPose;
+ }
+ }
+ return relHipsPose;
+}
//virtual
const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
@@ -925,7 +944,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
{
PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0);
- if (_hipsTargetIndex >= 0 && _hipsTargetIndex < (int)targets.size()) {
+ if (_hipsTargetIndex >= 0 && _hipsTargetIndex >= 0 && _hipsTargetIndex < (int)targets.size()) {
// slam the hips to match the _hipsTarget
AnimPose absPose = targets[_hipsTargetIndex].getPose();
@@ -934,7 +953,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
AnimPose parentAbsPose = _skeleton->getAbsolutePose(parentIndex, _relativePoses);
// do smooth interpolation of hips, if necessary.
- if (_prevJointChainInfoVec[_hipsTargetIndex].timer > 0.0f) {
+ if (_prevJointChainInfoVec[_hipsTargetIndex].timer > 0.0f && _prevJointChainInfoVec[_hipsTargetIndex].jointInfoVec.size() > 0) {
float alpha = (JOINT_CHAIN_INTERP_TIME - _prevJointChainInfoVec[_hipsTargetIndex].timer) / JOINT_CHAIN_INTERP_TIME;
auto& info = _prevJointChainInfoVec[_hipsTargetIndex].jointInfoVec[0];
@@ -944,22 +963,36 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
}
_relativePoses[_hipsIndex] = parentAbsPose.inverse() * absPose;
+ _relativePoses[_hipsIndex].scale() = glm::vec3(1.0f);
+ _hipsOffset = Vectors::ZERO;
+
+ } else if (_hipsIndex >= 0) {
- } else {
// if there is no hips target, shift hips according to the _hipsOffset from the previous frame
- float offsetLength = glm::length(_hipsOffset);
- const float MIN_HIPS_OFFSET_LENGTH = 0.03f;
- if (offsetLength > MIN_HIPS_OFFSET_LENGTH && _hipsIndex >= 0) {
- float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength);
- glm::vec3 hipsOffset = scaleFactor * _hipsOffset;
- if (_hipsParentIndex == -1) {
- _relativePoses[_hipsIndex].trans() = _relativePoses[_hipsIndex].trans() + hipsOffset;
- } else {
- auto absHipsPose = _skeleton->getAbsolutePose(_hipsIndex, _relativePoses);
- absHipsPose.trans() += hipsOffset;
- _relativePoses[_hipsIndex] = _skeleton->getAbsolutePose(_hipsParentIndex, _relativePoses).inverse() * absHipsPose;
+ AnimPose relHipsPose = applyHipsOffset();
+
+ // determine if we should begin interpolating the hips.
+ for (size_t i = 0; i < targets.size(); i++) {
+ if (_prevJointChainInfoVec[i].target.getIndex() == _hipsIndex) {
+ if (_prevJointChainInfoVec[i].timer > 0.0f) {
+ // smoothly lerp in hipsOffset
+ float alpha = (JOINT_CHAIN_INTERP_TIME - _prevJointChainInfoVec[i].timer) / JOINT_CHAIN_INTERP_TIME;
+ AnimPose prevRelHipsPose(_prevJointChainInfoVec[i].jointInfoVec[0].rot, _prevJointChainInfoVec[i].jointInfoVec[0].trans);
+ ::blend(1, &prevRelHipsPose, &relHipsPose, alpha, &relHipsPose);
+ }
+ break;
}
}
+
+ _relativePoses[_hipsIndex] = relHipsPose;
+ }
+
+ // if there is an active jointChainInfo for the hips store the post shifted hips into it.
+ // This is so we have a valid pose to interplate from when the hips target is disabled.
+ if (_hipsTargetIndex >= 0) {
+ // AJT: TODO: WILL THIS WORK if hips aren't the root of skeleton?
+ jointChainInfoVec[_hipsTargetIndex].jointInfoVec[0].rot = _relativePoses[_hipsIndex].rot();
+ jointChainInfoVec[_hipsTargetIndex].jointInfoVec[0].trans = _relativePoses[_hipsIndex].trans();
}
// update all HipsRelative targets to account for the hips shift/ik target.
@@ -1010,9 +1043,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
if (_hipsTargetIndex < 0) {
PROFILE_RANGE_EX(simulation_animation, "ik/measureHipsOffset", 0xffff00ff, 0);
- computeHipsOffset(targets, underPoses, dt);
- } else {
- _hipsOffset = Vectors::ZERO;
+ _hipsOffset = computeHipsOffset(targets, underPoses, dt, _hipsOffset);
}
}
@@ -1021,23 +1052,15 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
}
}
- if (_leftHandIndex > -1) {
- _uncontrolledLeftHandPose = _skeleton->getAbsolutePose(_leftHandIndex, underPoses);
- }
- if (_rightHandIndex > -1) {
- _uncontrolledRightHandPose = _skeleton->getAbsolutePose(_rightHandIndex, underPoses);
- }
- if (_hipsIndex > -1) {
- _uncontrolledHipsPose = _skeleton->getAbsolutePose(_hipsIndex, underPoses);
- }
-
return _relativePoses;
}
-void AnimInverseKinematics::computeHipsOffset(const std::vector<IKTarget>& targets, const AnimPoseVec& underPoses, float dt) {
+glm::vec3 AnimInverseKinematics::computeHipsOffset(const std::vector<IKTarget>& targets, const AnimPoseVec& underPoses, float dt, glm::vec3 prevHipsOffset) const {
+
// measure new _hipsOffset for next frame
// by looking for discrepancies between where a targeted endEffector is
// and where it wants to be (after IK solutions are done)
+ glm::vec3 hipsOffset = prevHipsOffset;
glm::vec3 newHipsOffset = Vectors::ZERO;
for (auto& target: targets) {
int targetIndex = target.getIndex();
@@ -1053,9 +1076,9 @@ void AnimInverseKinematics::computeHipsOffset(const std::vector<IKTarget>& targe
} else if (target.getType() == IKTarget::Type::HmdHead) {
// we want to shift the hips to bring the head to its designated position
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans();
- _hipsOffset += target.getTranslation() - actual;
+ hipsOffset += target.getTranslation() - actual;
// and ignore all other targets
- newHipsOffset = _hipsOffset;
+ newHipsOffset = hipsOffset;
break;
} else if (target.getType() == IKTarget::Type::RotationAndPosition) {
glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans();
@@ -1075,16 +1098,18 @@ void AnimInverseKinematics::computeHipsOffset(const std::vector<IKTarget>& targe
}
}
- // smooth transitions by relaxing _hipsOffset toward the new value
+ // smooth transitions by relaxing hipsOffset toward the new value
const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f;
float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
- _hipsOffset += (newHipsOffset - _hipsOffset) * tau;
+ hipsOffset += (newHipsOffset - hipsOffset) * tau;
// clamp the hips offset
- float hipsOffsetLength = glm::length(_hipsOffset);
+ float hipsOffsetLength = glm::length(hipsOffset);
if (hipsOffsetLength > _maxHipsOffsetLength) {
- _hipsOffset *= _maxHipsOffsetLength / hipsOffsetLength;
+ hipsOffset *= _maxHipsOffsetLength / hipsOffsetLength;
}
+
+ return hipsOffset;
}
void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) {
@@ -1528,10 +1553,6 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
_leftHandIndex = -1;
_rightHandIndex = -1;
}
-
- _uncontrolledLeftHandPose = AnimPose();
- _uncontrolledRightHandPose = AnimPose();
- _uncontrolledHipsPose = AnimPose();
}
static glm::vec3 sphericalToCartesian(float phi, float theta) {
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index d5fc5a6a8c..7f7640aa24 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -73,10 +73,6 @@ public:
void setSolutionSource(SolutionSource solutionSource) { _solutionSource = solutionSource; }
void setSolutionSourceVar(const QString& solutionSourceVar) { _solutionSourceVar = solutionSourceVar; }
- const AnimPose& getUncontrolledLeftHandPose() { return _uncontrolledLeftHandPose; }
- const AnimPose& getUncontrolledRightHandPose() { return _uncontrolledRightHandPose; }
- const AnimPose& getUncontrolledHipPose() { return _uncontrolledHipsPose; }
-
protected:
void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
void solve(const AnimContext& context, const std::vector<IKTarget>& targets, float dt, JointChainInfoVec& jointChainInfoVec);
@@ -92,6 +88,7 @@ protected:
void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose);
void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor);
void preconditionRelativePosesToAvoidLimbLock(const AnimContext& context, const std::vector<IKTarget>& targets);
+ AnimPose applyHipsOffset() const;
// used to pre-compute information about each joint influeced by a spline IK target.
struct SplineJointInfo {
@@ -110,7 +107,7 @@ protected:
void clearConstraints();
void initConstraints();
void initLimitCenterPoses();
- void computeHipsOffset(const std::vector<IKTarget>& targets, const AnimPoseVec& underPoses, float dt);
+ glm::vec3 computeHipsOffset(const std::vector<IKTarget>& targets, const AnimPoseVec& underPoses, float dt, glm::vec3 prevHipsOffset) const;
// no copies
AnimInverseKinematics(const AnimInverseKinematics&) = delete;
@@ -162,10 +159,6 @@ protected:
SolutionSource _solutionSource { SolutionSource::RelaxToUnderPoses };
QString _solutionSourceVar;
- AnimPose _uncontrolledLeftHandPose { AnimPose() };
- AnimPose _uncontrolledRightHandPose { AnimPose() };
- AnimPose _uncontrolledHipsPose { AnimPose() };
-
JointChainInfoVec _prevJointChainInfoVec;
};
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 9cc09addb3..aa080dfa86 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1703,25 +1703,3 @@ void Rig::computeAvatarBoundingCapsule(
glm::vec3 rigCenter = transformPoint(_geometryToRigTransform, (0.5f * (totalExtents.maximum + totalExtents.minimum)));
localOffsetOut = rigCenter - transformPoint(_geometryToRigTransform, rootPosition);
}
-
-bool Rig::transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand,
- bool isToControlled, AnimPose& returnHandPose) {
- auto ikNode = getAnimInverseKinematicsNode();
- if (ikNode) {
- float alpha = 1.0f - deltaTime / totalDuration;
- const AnimPose geometryToRigTransform(_geometryToRigTransform);
- AnimPose uncontrolledHandPose;
- if (isLeftHand) {
- uncontrolledHandPose = geometryToRigTransform * ikNode->getUncontrolledLeftHandPose();
- } else {
- uncontrolledHandPose = geometryToRigTransform * ikNode->getUncontrolledRightHandPose();
- }
- if (isToControlled) {
- ::blend(1, &uncontrolledHandPose, &controlledHandPose, alpha, &returnHandPose);
- } else {
- ::blend(1, &controlledHandPose, &uncontrolledHandPose, alpha, &returnHandPose);
- }
- return true;
- }
- return false;
-}
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index ec13d98613..5293fa1fe7 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -340,9 +340,6 @@ protected:
int _nextStateHandlerId { 0 };
QMutex _stateMutex;
- bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand,
- bool isToControlled, AnimPose& returnHandPose);
-
glm::vec3 _prevRightFootPoleVector { Vectors::UNIT_Z };
bool _prevRightFootPoleVectorValid { false };
From b0177c25221d59c065a385b6287ca6a48b94e965 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 10 Jul 2017 16:25:37 -0700
Subject: [PATCH 11/12] remove comment, it does indeed work
---
libraries/animation/src/AnimInverseKinematics.cpp | 1 -
1 file changed, 1 deletion(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 8c86ada43c..20b62c2724 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -990,7 +990,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
// if there is an active jointChainInfo for the hips store the post shifted hips into it.
// This is so we have a valid pose to interplate from when the hips target is disabled.
if (_hipsTargetIndex >= 0) {
- // AJT: TODO: WILL THIS WORK if hips aren't the root of skeleton?
jointChainInfoVec[_hipsTargetIndex].jointInfoVec[0].rot = _relativePoses[_hipsIndex].rot();
jointChainInfoVec[_hipsTargetIndex].jointInfoVec[0].trans = _relativePoses[_hipsIndex].trans();
}
From de199bff9df9f42c164bf77ef501811eb2b0bbf4 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Fri, 14 Jul 2017 09:47:37 -0700
Subject: [PATCH 12/12] code review feedback
---
libraries/animation/src/AnimInverseKinematics.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 20b62c2724..57c00e7183 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -930,7 +930,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
}
}
- // identity joint chains that have changed types this frame.
+ // identify joint chains that have changed types this frame.
_prevJointChainInfoVec.resize(jointChainInfoVec.size());
for (size_t i = 0; i < _prevJointChainInfoVec.size(); i++) {
if (_prevJointChainInfoVec[i].timer <= 0.0f &&
@@ -944,7 +944,9 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
{
PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0);
- if (_hipsTargetIndex >= 0 && _hipsTargetIndex >= 0 && _hipsTargetIndex < (int)targets.size()) {
+ if (_hipsTargetIndex >= 0) {
+ assert(_hipsTargetIndex < (int)targets.size());
+
// slam the hips to match the _hipsTarget
AnimPose absPose = targets[_hipsTargetIndex].getPose();