Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-20 03:44:02 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #5826 from AndrewMeadows/ik-repairs-000

Browse source 
prevent initial bad IK targets for hands
This commit is contained in:
Howard Stearns 2015-09-16 18:49:46 -07:00
commit 5eef0e21ad
3 changed files with 118 additions and 78 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

184
libraries/animation/src/AnimInverseKinematics.cpp
View file

@ -9,6 +9,7 @@
#include "AnimInverseKinematics.h"
#include <GLMHelpers.h>
#include <NumericalConstants.h>
#include <SharedUtil.h>
@ -54,7 +55,20 @@ void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) con
void AnimInverseKinematics::setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar) {
// if there are dups, last one wins.
_targetVarVec.push_back(IKTargetVar(jointName, positionVar.toStdString(), rotationVar.toStdString()));
bool found = false;
for (auto& targetVar: _targetVarVec) {
if (targetVar.jointName == jointName) {
// update existing targetVar
targetVar.positionVar = positionVar.toStdString();
targetVar.rotationVar = rotationVar.toStdString();
found = true;
break;
}
}
if (!found) {
// create a new entry
_targetVarVec.push_back(IKTargetVar(jointName, positionVar.toStdString(), rotationVar.toStdString()));
}
}
static int findRootJointInSkeleton(AnimSkeleton::ConstPointer skeleton, int index) {
@ -68,6 +82,12 @@ static int findRootJointInSkeleton(AnimSkeleton::ConstPointer skeleton, int inde
return rootIndex;
}
struct IKTarget {
AnimPose pose;
int index;
int rootIndex;
};
//virtual
const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
@ -76,43 +96,53 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
return _relativePoses;
}
// evaluate target vars
// build a list of targets from _targetVarVec
std::vector<IKTarget> targets;
bool removeUnfoundJoints = false;
for (auto& targetVar : _targetVarVec) {
// lazy look up of jointIndices and insertion into _absoluteTargets map
if (!targetVar.hasPerformedJointLookup) {
targetVar.jointIndex = _skeleton->nameToJointIndex(targetVar.jointName);
if (targetVar.jointIndex >= 0) {
// insert into _absoluteTargets map
IKTarget target;
target.pose = AnimPose::identity;
target.rootIndex = findRootJointInSkeleton(_skeleton, targetVar.jointIndex);
_absoluteTargets[targetVar.jointIndex] = target;
if (targetVar.jointIndex > _maxTargetIndex) {
_maxTargetIndex = targetVar.jointIndex;
}
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;
targetVar.rootIndex = findRootJointInSkeleton(_skeleton, jointIndex);
} else {
qCWarning(animation) << "AnimInverseKinematics could not find jointName" << targetVar.jointName << "in skeleton";
removeUnfoundJoints = true;
}
targetVar.hasPerformedJointLookup = true;
}
if (targetVar.jointIndex >= 0) {
// update pose in _absoluteTargets map
auto iter = _absoluteTargets.find(targetVar.jointIndex);
if (iter != _absoluteTargets.end()) {
} else {
// TODO: get this done without a double-lookup of each var in animVars
if (animVars.hasKey(targetVar.positionVar) || animVars.hasKey(targetVar.rotationVar)) {
IKTarget target;
AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, _relativePoses);
iter->second.pose.trans = animVars.lookup(targetVar.positionVar, defaultPose.trans);
iter->second.pose.rot = animVars.lookup(targetVar.rotationVar, defaultPose.rot);
target.pose.trans = animVars.lookup(targetVar.positionVar, defaultPose.trans);
target.pose.rot = animVars.lookup(targetVar.rotationVar, defaultPose.rot);
target.rootIndex = targetVar.rootIndex;
target.index = targetVar.jointIndex;
targets.push_back(target);
}
}
}
// RELAX! Don't do it.
// relaxTowardDefaults(dt);
if (removeUnfoundJoints) {
int numVars = _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;
}
}
}
if (_absoluteTargets.empty()) {
if (targets.empty()) {
// no IK targets but still need to enforce constraints
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
while (constraintItr != _constraints.end()) {
@ -135,11 +165,11 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
quint64 expiry = usecTimestampNow() + MAX_IK_TIME;
do {
largestError = 0.0f;
for (auto& targetPair: _absoluteTargets) {
for (auto& target: targets) {
int lowestMovedIndex = _relativePoses.size() - 1;
int tipIndex = targetPair.first;
AnimPose targetPose = targetPair.second.pose;
int rootIndex = targetPair.second.rootIndex;
int tipIndex = target.index;
AnimPose targetPose = target.pose;
int rootIndex = target.rootIndex;
if (rootIndex != -1) {
// transform targetPose into skeleton's absolute frame
AnimPose& rootPose = _relativePoses[rootIndex];
@ -150,11 +180,11 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
glm::vec3 tip = absolutePoses[tipIndex].trans;
float error = glm::length(targetPose.trans - tip);
// descend toward root, rotating each joint to get tip closer to target
int index = _skeleton->getParentIndex(tipIndex);
while (index != -1 && error > ACCEPTABLE_RELATIVE_ERROR) {
// descend toward root, pivoting each joint to get tip closer to target
int pivotIndex = _skeleton->getParentIndex(tipIndex);
while (pivotIndex != -1 && error > ACCEPTABLE_RELATIVE_ERROR) {
// compute the two lines that should be aligned
glm::vec3 jointPosition = absolutePoses[index].trans;
glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
glm::vec3 leverArm = tip - jointPosition;
glm::vec3 targetLine = targetPose.trans - jointPosition;
@ -173,29 +203,34 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
angle = 0.5f * angle;
glm::quat deltaRotation = glm::angleAxis(angle, axis);
int parentIndex = _skeleton->getParentIndex(index);
int parentIndex = _skeleton->getParentIndex(pivotIndex);
if (parentIndex == -1) {
// TODO? apply constraints to root?
// TODO? harvest the root's transform as movement of entire skeleton?
} else {
// compute joint's new parent-relative rotation
// Q' = dQ * Q and Q = Qp * q --> q' = Qp^ * dQ * Q
glm::quat newRot = glm::normalize(glm::inverse(absolutePoses[parentIndex].rot) * deltaRotation * absolutePoses[index].rot);
RotationConstraint* constraint = getConstraint(index);
glm::quat newRot = glm::normalize(glm::inverse(
absolutePoses[parentIndex].rot) *
deltaRotation *
absolutePoses[pivotIndex].rot);
RotationConstraint* constraint = getConstraint(pivotIndex);
if (constraint) {
bool constrained = constraint->apply(newRot);
if (constrained) {
// the constraint will modify the movement of the tip so we have to compute the modified
// model-frame deltaRotation
// Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^
deltaRotation = absolutePoses[parentIndex].rot * newRot * glm::inverse(absolutePoses[index].rot);
deltaRotation = absolutePoses[parentIndex].rot *
newRot *
glm::inverse(absolutePoses[pivotIndex].rot);
}
}
_relativePoses[index].rot = newRot;
_relativePoses[pivotIndex].rot = newRot;
}
// this joint has been changed so we check to see if it has the lowest index
if (index < lowestMovedIndex) {
lowestMovedIndex = index;
if (pivotIndex < lowestMovedIndex) {
lowestMovedIndex = pivotIndex;
}
// keep track of tip's new position as we descend towards root
@ -203,7 +238,7 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
error = glm::length(targetPose.trans - tip);
}
}
index = _skeleton->getParentIndex(index);
pivotIndex = _skeleton->getParentIndex(pivotIndex);
}
if (largestError < error) {
largestError = error;
@ -248,12 +283,16 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
loadPoses(underPoses);
} else {
// relax toward underpose
const float RELAXATION_TIMESCALE = 0.125f;
const float alpha = glm::clamp(dt / RELAXATION_TIMESCALE, 0.0f, 1.0f);
// HACK: this relaxation needs to be constant per-frame rather than per-realtime
// in order to prevent IK "flutter" for bad FPS. The bad news is that the good parts
// of this relaxation will be FPS dependent (low FPS will make the limbs align slower
// in real-time), however most people will not notice this and this problem is less
// annoying than the flutter.
const float blend = (1.0f / 60.0f) / (0.25f); // effectively: dt / RELAXATION_TIMESCALE
int numJoints = (int)_relativePoses.size();
for (int i = 0; i < numJoints; ++i) {
float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot, underPoses[i].rot));
_relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * underPoses[i].rot, alpha));
_relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * underPoses[i].rot, blend));
}
}
return evaluate(animVars, dt, triggersOut);
@ -277,10 +316,6 @@ void AnimInverseKinematics::clearConstraints() {
_constraints.clear();
}
const glm::vec3 xAxis(1.0f, 0.0f, 0.0f);
const glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
const glm::vec3 zAxis(0.0f, 0.0f, 1.0f);
void AnimInverseKinematics::initConstraints() {
if (!_skeleton) {
return;
@ -288,7 +323,7 @@ void AnimInverseKinematics::initConstraints() {
// We create constraints for the joints shown here
// (and their Left counterparts if applicable).
//
//
//
// O RightHand
// Head /
// O /
@ -306,7 +341,7 @@ void AnimInverseKinematics::initConstraints() {
// y |
// | |
// | O---O---O RightUpLeg
// z | | |
// z | | Hips2 |
// \ | | |
// \| | |
// x -----+ O O RightLeg
@ -334,7 +369,7 @@ void AnimInverseKinematics::initConstraints() {
_constraints.clear();
for (int i = 0; i < numJoints; ++i) {
// compute the joint's baseName and remember if it was Left or not
// compute the joint's baseName and remember whether its prefix was "Left" or not
QString baseName = _skeleton->getJointName(i);
bool isLeft = baseName.startsWith("Left", Qt::CaseInsensitive);
float mirror = isLeft ? -1.0f : 1.0f;
@ -442,7 +477,7 @@ void AnimInverseKinematics::initConstraints() {
const float MAX_HAND_SWING = PI / 2.0f;
minDots.push_back(cosf(MAX_HAND_SWING));
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (baseName.startsWith("Shoulder", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
@ -467,6 +502,18 @@ void AnimInverseKinematics::initConstraints() {
minDots.push_back(cosf(MAX_SPINE_SWING));
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (baseName.startsWith("Hips2", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_SPINE_TWIST = PI / 8.0f;
stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST);
std::vector<float> minDots;
const float MAX_SPINE_SWING = PI / 14.0f;
minDots.push_back(cosf(MAX_SPINE_SWING));
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == baseName.compare("Neck", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
@ -488,18 +535,18 @@ void AnimInverseKinematics::initConstraints() {
// we determine the max/min angles by rotating the swing limit lines from parent- to child-frame
// then measure the angles to swing the yAxis into alignment
glm::vec3 hingeAxis = - mirror * zAxis;
glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z;
const float MIN_ELBOW_ANGLE = 0.05f;
const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f;
glm::quat invReferenceRotation = glm::inverse(referenceRotation);
glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * yAxis;
glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_ELBOW_ANGLE, hingeAxis) * yAxis;
glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y;
glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y;
// for the rest of the math we rotate hingeAxis into the child frame
hingeAxis = referenceRotation * hingeAxis;
eConstraint->setHingeAxis(hingeAxis);
glm::vec3 projectedYAxis = glm::normalize(yAxis - glm::dot(yAxis, hingeAxis) * hingeAxis);
glm::vec3 projectedYAxis = glm::normalize(Vectors::UNIT_Y - glm::dot(Vectors::UNIT_Y, hingeAxis) * hingeAxis);
float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis));
if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) {
minAngle = - minAngle;
@ -516,21 +563,21 @@ void AnimInverseKinematics::initConstraints() {
ElbowConstraint* eConstraint = new ElbowConstraint();
glm::quat referenceRotation = _defaultRelativePoses[i].rot;
eConstraint->setReferenceRotation(referenceRotation);
glm::vec3 hingeAxis = -1.0f * xAxis;
glm::vec3 hingeAxis = -1.0f * Vectors::UNIT_X;
// we determine the max/min angles by rotating the swing limit lines from parent- to child-frame
// then measure the angles to swing the yAxis into alignment
const float MIN_KNEE_ANGLE = 0.0f;
const float MAX_KNEE_ANGLE = 3.0f * PI / 4.0f;
glm::quat invReferenceRotation = glm::inverse(referenceRotation);
glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * yAxis;
glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_KNEE_ANGLE, hingeAxis) * yAxis;
glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y;
glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y;
// for the rest of the math we rotate hingeAxis into the child frame
hingeAxis = referenceRotation * hingeAxis;
eConstraint->setHingeAxis(hingeAxis);
glm::vec3 projectedYAxis = glm::normalize(yAxis - glm::dot(yAxis, hingeAxis) * hingeAxis);
glm::vec3 projectedYAxis = glm::normalize(Vectors::UNIT_Y - glm::dot(Vectors::UNIT_Y, hingeAxis) * hingeAxis);
float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis));
if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) {
minAngle = - minAngle;
@ -550,8 +597,8 @@ void AnimInverseKinematics::initConstraints() {
// these directions are approximate swing limits in parent-frame
// NOTE: they don't need to be normalized
std::vector<glm::vec3> swungDirections;
swungDirections.push_back(yAxis);
swungDirections.push_back(xAxis);
swungDirections.push_back(Vectors::UNIT_Y);
swungDirections.push_back(Vectors::UNIT_X);
swungDirections.push_back(glm::vec3(1.0f, 1.0f, 1.0f));
swungDirections.push_back(glm::vec3(1.0f, 1.0f, -1.0f));
@ -575,13 +622,10 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
AnimNode::setSkeletonInternal(skeleton);
// invalidate all targetVars
for (auto& targetVar : _targetVarVec) {
targetVar.hasPerformedJointLookup = false;
for (auto& targetVar: _targetVarVec) {
targetVar.jointIndex = -1;
}
// invalidate all targets
_absoluteTargets.clear();
_maxTargetIndex = 0;
if (skeleton) {

10
libraries/animation/src/AnimInverseKinematics.h
View file

@ -50,23 +50,17 @@ protected:
rotationVar(rotationVarIn),
jointName(jointNameIn),
jointIndex(-1),
hasPerformedJointLookup(false) {}
rootIndex(-1) {}
std::string positionVar;
std::string rotationVar;
QString jointName;
int jointIndex; // cached joint index
bool hasPerformedJointLookup = false;
};
struct IKTarget {
AnimPose pose;
int rootIndex;
int rootIndex; // cached root index
};
std::map<int, RotationConstraint*> _constraints;
std::vector<IKTargetVar> _targetVarVec;
std::map<int, IKTarget> _absoluteTargets; // IK targets of end-points
AnimPoseVec _defaultRelativePoses; // poses of the relaxed state
AnimPoseVec _relativePoses; // current relative poses

2
libraries/animation/src/AnimVariant.h
View file

@ -154,6 +154,8 @@ public:
void setTrigger(const std::string& key) { _triggers.insert(key); }
void clearTriggers() { _triggers.clear(); }
bool hasKey(const std::string& key) const { return _map.find(key) != _map.end(); }
protected:
std::map<std::string, AnimVariant> _map;
std::set<std::string> _triggers;