mirror of
https://github.com/overte-org/overte.git
synced 2025-04-08 07:12:40 +02:00
Merge pull request #10881 from hyperlogic/feature/smooth-ik-chains
Smooth IK when trackers are disabled / enabled.
This commit is contained in:
Andrew Meadows
GitHub
commit
5a4f56388b
9 changed files with 322 additions and 303 deletions
|
|
@ -23,20 +23,23 @@
|
|||
#include "CubicHermiteSpline.h"
|
||||
#include "AnimUtil.h"
|
||||
|
||||
static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointChainInfos, size_t numJointChainInfos,
|
||||
static const float JOINT_CHAIN_INTERP_TIME = 0.25f;
|
||||
|
||||
static void lookupJointInfo(const AnimInverseKinematics::JointChainInfo& jointChainInfo,
|
||||
int indexA, int indexB,
|
||||
AnimInverseKinematics::JointChainInfo** jointChainInfoA,
|
||||
AnimInverseKinematics::JointChainInfo** jointChainInfoB) {
|
||||
*jointChainInfoA = nullptr;
|
||||
*jointChainInfoB = nullptr;
|
||||
for (size_t i = 0; i < numJointChainInfos; i++) {
|
||||
if (jointChainInfos[i].jointIndex == indexA) {
|
||||
*jointChainInfoA = jointChainInfos + 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 (jointChainInfos[i].jointIndex == indexB) {
|
||||
*jointChainInfoB = jointChainInfos + i;
|
||||
if (jointInfo->jointIndex == indexB) {
|
||||
*jointInfoB = jointInfo;
|
||||
}
|
||||
if (*jointChainInfoA && *jointChainInfoB) {
|
||||
if (*jointInfoA && *jointInfoB) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -149,25 +152,28 @@ 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));
|
||||
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());
|
||||
|
|
@ -175,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);
|
||||
|
||||
|
|
@ -188,39 +193,20 @@ 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);
|
||||
}
|
||||
}
|
||||
|
||||
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());
|
||||
|
|
@ -234,26 +220,75 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
|
|||
accumulator.clearAndClean();
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
// solve all targets
|
||||
for (auto& target: targets) {
|
||||
if (target.getType() == IKTarget::Type::Spline) {
|
||||
solveTargetWithSpline(context, target, absolutePoses, debug);
|
||||
} else {
|
||||
solveTargetWithCCD(context, target, absolutePoses, debug);
|
||||
for (size_t i = 0; i < targets.size(); i++) {
|
||||
switch (targets[i].getType()) {
|
||||
case IKTarget::Type::Unknown:
|
||||
break;
|
||||
case IKTarget::Type::Spline:
|
||||
solveTargetWithSpline(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]);
|
||||
break;
|
||||
default:
|
||||
solveTargetWithCCD(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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((1.0f - alpha) * _prevJointChainInfoVec[i].target.getWeight());
|
||||
jointChainInfoVec[i].target = newTarget;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// copy jointChainInfoVecs into accumulators
|
||||
for (size_t i = 0; i < targets.size(); i++) {
|
||||
const std::vector<JointInfo>& jointInfoVec = jointChainInfoVec[i].jointInfoVec;
|
||||
|
||||
// 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();
|
||||
|
|
@ -289,7 +324,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) {
|
||||
|
|
@ -308,9 +343,34 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
|
|||
absolutePoses[tipIndex].rot() = targetRotation;
|
||||
}
|
||||
}
|
||||
|
||||
// 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) {
|
||||
_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();
|
||||
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, bool debug) {
|
||||
void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
|
||||
bool debug, JointChainInfo& jointChainInfoOut) const {
|
||||
size_t chainDepth = 0;
|
||||
|
||||
IKTarget::Type targetType = target.getType();
|
||||
|
|
@ -338,9 +398,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 +425,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
|
|||
}
|
||||
|
||||
glm::vec3 tipRelativeTranslation = _relativePoses[target.getIndex()].trans();
|
||||
jointChainInfos[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, target.getWeight(), tipIndex, constrained };
|
||||
jointChainInfoOut.jointInfoVec[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, tipIndex, constrained };
|
||||
}
|
||||
|
||||
// cache tip absolute position
|
||||
|
|
@ -379,7 +436,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 < jointChainInfoOut.jointInfoVec.size());
|
||||
|
||||
// compute the two lines that should be aligned
|
||||
glm::vec3 jointPosition = absolutePoses[pivotIndex].trans();
|
||||
|
|
@ -444,9 +501,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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -455,9 +511,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
|
||||
|
|
@ -480,7 +535,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
|
|||
}
|
||||
|
||||
glm::vec3 newTrans = _relativePoses[pivotIndex].trans();
|
||||
jointChainInfos[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);
|
||||
|
|
@ -502,24 +557,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), jointChainInfoOut.jointInfoVec[i].rot, jointChainInfoOut.jointInfoVec[i].trans);
|
||||
postAbsPoses[i] = accum;
|
||||
if (jointChainInfos[i].jointIndex == topJointIndex) {
|
||||
if (jointChainInfoOut.jointInfoVec[i].jointIndex == topJointIndex) {
|
||||
topChainIndex = i;
|
||||
topPose = accum;
|
||||
}
|
||||
if (jointChainInfos[i].jointIndex == midJointIndex) {
|
||||
if (jointChainInfoOut.jointInfoVec[i].jointIndex == midJointIndex) {
|
||||
midPose = accum;
|
||||
}
|
||||
if (jointChainInfos[i].jointIndex == baseJointIndex) {
|
||||
if (jointChainInfoOut.jointInfoVec[i].jointIndex == baseJointIndex) {
|
||||
baseChainIndex = i;
|
||||
basePose = accum;
|
||||
}
|
||||
if (jointChainInfos[i].jointIndex == baseParentJointIndex) {
|
||||
if (jointChainInfoOut.jointInfoVec[i].jointIndex == baseParentJointIndex) {
|
||||
baseParentPose = accum;
|
||||
}
|
||||
}
|
||||
|
|
@ -599,21 +655,16 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
|
|||
}
|
||||
|
||||
glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot();
|
||||
jointChainInfos[baseChainIndex].relRot = newBaseRelRot;
|
||||
jointChainInfoOut.jointInfoVec[baseChainIndex].rot = newBaseRelRot;
|
||||
|
||||
glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot();
|
||||
jointChainInfos[topChainIndex].relRot = newTopRelRot;
|
||||
jointChainInfoOut.jointInfoVec[topChainIndex].rot = 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(jointChainInfoOut, context);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -628,7 +679,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.
|
||||
|
|
@ -681,13 +732,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);
|
||||
|
|
@ -697,10 +748,8 @@ 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];
|
||||
void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,
|
||||
bool debug, JointChainInfo& jointChainInfoOut) const {
|
||||
|
||||
const int baseIndex = _hipsIndex;
|
||||
|
||||
|
|
@ -720,7 +769,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();
|
||||
}
|
||||
|
|
@ -743,7 +792,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));
|
||||
|
|
@ -783,19 +832,14 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
|
|||
}
|
||||
}
|
||||
|
||||
jointChainInfos[i] = { relPose.rot(), relPose.trans(), target.getWeight(), splineJointInfo.jointIndex, constrained };
|
||||
jointChainInfoOut.jointInfoVec[i] = { relPose.rot(), relPose.trans(), splineJointInfo.jointIndex, constrained };
|
||||
|
||||
parentAbsPose = flexedAbsPose;
|
||||
}
|
||||
}
|
||||
|
||||
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(jointChainInfoOut, context);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -806,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) {
|
||||
|
|
@ -850,33 +912,88 @@ 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 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());
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// 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 &&
|
||||
(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()) {
|
||||
if (_hipsTargetIndex >= 0) {
|
||||
assert(_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, if necessary.
|
||||
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];
|
||||
AnimPose prevHipsRelPose(info.rot, info.trans);
|
||||
AnimPose prevHipsAbsPose = parentAbsPose * prevHipsRelPose;
|
||||
::blend(1, &prevHipsAbsPose, &absPose, alpha, &absPose);
|
||||
}
|
||||
} else {
|
||||
|
||||
_relativePoses[_hipsIndex] = parentAbsPose.inverse() * absPose;
|
||||
_relativePoses[_hipsIndex].scale() = glm::vec3(1.0f);
|
||||
_hipsOffset = Vectors::ZERO;
|
||||
|
||||
} else if (_hipsIndex >= 0) {
|
||||
|
||||
// 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) {
|
||||
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.
|
||||
|
|
@ -920,15 +1037,14 @@ 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) {
|
||||
PROFILE_RANGE_EX(simulation_animation, "ik/measureHipsOffset", 0xffff00ff, 0);
|
||||
computeHipsOffset(targets, underPoses, dt);
|
||||
} else {
|
||||
_hipsOffset = Vectors::ZERO;
|
||||
_hipsOffset = computeHipsOffset(targets, underPoses, dt, _hipsOffset);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -937,23 +1053,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();
|
||||
|
|
@ -969,9 +1077,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();
|
||||
|
|
@ -991,16 +1099,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) {
|
||||
|
|
@ -1414,8 +1524,6 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
|
|||
targetVar.jointIndex = -1;
|
||||
}
|
||||
|
||||
_maxTargetIndex = -1;
|
||||
|
||||
for (auto& accumulator: _rotationAccumulators) {
|
||||
accumulator.clearAndClean();
|
||||
}
|
||||
|
|
@ -1446,10 +1554,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) {
|
||||
|
|
@ -1495,14 +1599,14 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c
|
|||
}
|
||||
}
|
||||
|
||||
void AnimInverseKinematics::debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, 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 < numJointChainInfos; i++) {
|
||||
const JointChainInfo& info = jointChainInfos[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
|
||||
|
|
@ -1519,9 +1623,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 JointInfo* jointInfo = nullptr;
|
||||
const JointInfo* parentJointInfo = nullptr;
|
||||
lookupJointInfo(jointChainInfo, i, parentIndex, &jointInfo, &parentJointInfo);
|
||||
if (jointInfo && parentJointInfo) {
|
||||
|
||||
// transform local axes into world space.
|
||||
|
|
@ -1608,7 +1712,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);
|
||||
}
|
||||
|
|
@ -1626,7 +1730,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);
|
||||
}
|
||||
|
|
@ -1666,10 +1770,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();
|
||||
|
|
|
|||
|
|
@ -26,14 +26,21 @@ 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;
|
||||
float timer { 0.0f };
|
||||
};
|
||||
|
||||
using JointChainInfoVec = std::vector<JointChainInfo>;
|
||||
|
||||
explicit AnimInverseKinematics(const QString& id);
|
||||
virtual ~AnimInverseKinematics() override;
|
||||
|
||||
|
|
@ -66,23 +73,22 @@ 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);
|
||||
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 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) const;
|
||||
virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
|
||||
void debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, 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;
|
||||
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 {
|
||||
|
|
@ -91,8 +97,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; }
|
||||
|
|
@ -101,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;
|
||||
|
|
@ -136,7 +142,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 };
|
||||
|
|
@ -148,18 +154,12 @@ 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 };
|
||||
QString _solutionSourceVar;
|
||||
|
||||
AnimPose _uncontrolledLeftHandPose { AnimPose() };
|
||||
AnimPose _uncontrolledRightHandPose { AnimPose() };
|
||||
AnimPose _uncontrolledHipsPose { AnimPose() };
|
||||
JointChainInfoVec _prevJointChainInfoVec;
|
||||
};
|
||||
|
||||
#endif // hifi_AnimInverseKinematics_h
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -56,8 +56,8 @@ private:
|
|||
glm::vec3 _poleReferenceVector;
|
||||
bool _poleVectorEnabled { false };
|
||||
int _index { -1 };
|
||||
Type _type { Type::RotationAndPosition };
|
||||
float _weight;
|
||||
Type _type { Type::Unknown };
|
||||
float _weight { 0.0f };
|
||||
float _flexCoefficients[MAX_FLEX_COEFFICIENTS];
|
||||
size_t _numFlexCoefficients;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1115,36 +1115,13 @@ 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) {
|
||||
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;
|
||||
|
|
@ -1157,9 +1134,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");
|
||||
|
|
@ -1187,47 +1161,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;
|
||||
|
|
@ -1240,9 +1184,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");
|
||||
|
|
@ -1270,25 +1211,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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1737,39 +1662,38 @@ void Rig::computeAvatarBoundingCapsule(
|
|||
ikNode.setTargetVars("RightFoot", "rightFootPosition", "rightFootRotation",
|
||||
"rightFootType", "rightFootWeight", 1.0f, {},
|
||||
QString(), QString(), QString());
|
||||
|
||||
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());
|
||||
|
|
@ -1802,34 +1726,13 @@ 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);
|
||||
}
|
||||
|
||||
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;
|
||||
glm::vec3 rigCenter = transformPoint(_geometryToRigTransform, (0.5f * (totalExtents.maximum + totalExtents.minimum)));
|
||||
localOffsetOut = rigCenter - transformPoint(_geometryToRigTransform, rootPosition);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -340,18 +340,6 @@ protected:
|
|||
int _nextStateHandlerId { 0 };
|
||||
QMutex _stateMutex;
|
||||
|
||||
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 };
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue