Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-08 11:37:58 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

fix bug in IK and tune head/neck constraints

Browse source
This commit is contained in:
U-GAPOS\andrew 2016-01-26 09:37:33 -08:00
parent e2e7a60c0f
commit 2728290ffb
1 changed files with 38 additions and 22 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -153,7 +153,6 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
continue; continue;
} }
// cache tip absolute transform
int tipIndex = target.getIndex(); int tipIndex = target.getIndex();
int pivotIndex = _skeleton->getParentIndex(tipIndex); int pivotIndex = _skeleton->getParentIndex(tipIndex);
if (pivotIndex == -1 || pivotIndex == _hipsIndex) { if (pivotIndex == -1 || pivotIndex == _hipsIndex) {
@ -165,12 +164,30 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
continue; continue;
} }
glm::vec3 tipPosition = absolutePoses[tipIndex].trans; // cache tip's absolute orientation
glm::quat tipRotation = absolutePoses[tipIndex].rot; glm::quat tipOrientation = absolutePoses[tipIndex].rot;
// cache tip's parent's absolute rotation so we can recompute the tip's parent-relative // also cache tip's parent's absolute orientation so we can recompute
// as we proceed walking down the joint chain // the tip's parent-relative as we proceed up the chain
glm::quat tipParentRotation = absolutePoses[pivotIndex].rot; glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot;
if (targetType == IKTarget::Type::HmdHead) {
// rotate tip directly to target orientation
tipOrientation = target.getRotation();
// enforce tip's constraint
RotationConstraint* constraint = getConstraint(tipIndex);
if (constraint) {
glm::quat tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation));
bool constrained = constraint->apply(tipRelativeRotation);
if (constrained) {
tipOrientation = glm::normalize(tipRelativeRotation * tipParentOrientation);
}
}
}
// cache tip absolute position
glm::vec3 tipPosition = absolutePoses[tipIndex].trans;
// descend toward root, pivoting each joint to get tip closer to target // descend toward root, pivoting each joint to get tip closer to target
while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) { while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) {
@ -201,9 +218,9 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
deltaRotation = glm::angleAxis(angle, axis); deltaRotation = glm::angleAxis(angle, axis);
// The swing will re-orient the tip but there will tend to be be a non-zero delta between the tip's // The swing will re-orient the tip but there will tend to be be a non-zero delta between the tip's
// new rotation and its target. This is the final parent-relative rotation that the tip joint have // new orientation and its target. This is the final parent-relative orientation that the tip joint have
// make to achieve its target rotation. // make to achieve its target orientation.
glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentRotation) * target.getRotation(); glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentOrientation) * target.getRotation();
// enforce tip's constraint // enforce tip's constraint
RotationConstraint* constraint = getConstraint(tipIndex); RotationConstraint* constraint = getConstraint(tipIndex);
@ -212,7 +229,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
if (constrained) { if (constrained) {
// The tip's final parent-relative rotation would violate its constraint // The tip's final parent-relative rotation would violate its constraint
// so we try to pre-twist this pivot to compensate. // so we try to pre-twist this pivot to compensate.
glm::quat constrainedTipRotation = deltaRotation * tipParentRotation * tipRelativeRotation; glm::quat constrainedTipRotation = deltaRotation * tipParentOrientation * tipRelativeRotation;
glm::quat missingRotation = target.getRotation() * glm::inverse(constrainedTipRotation); glm::quat missingRotation = target.getRotation() * glm::inverse(constrainedTipRotation);
glm::quat swingPart; glm::quat swingPart;
glm::quat twistPart; glm::quat twistPart;
@ -227,9 +244,9 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
} else if (targetType == IKTarget::Type::HmdHead) { } else if (targetType == IKTarget::Type::HmdHead) {
// An HmdHead target slaves the orientation of the end-effector by distributing rotation // 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. // deltas up the hierarchy. Its target position is enforced later by shifting the hips.
deltaRotation = target.getRotation() * glm::inverse(tipRotation); deltaRotation = target.getRotation() * glm::inverse(tipOrientation);
float dotSign = copysignf(1.0f, deltaRotation.w); float dotSign = copysignf(1.0f, deltaRotation.w);
const float ANGLE_DISTRIBUTION_FACTOR = 0.15f; const float ANGLE_DISTRIBUTION_FACTOR = 0.35f;
deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR)); deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR));
} }
@ -245,12 +262,11 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
if (constraint) { if (constraint) {
bool constrained = constraint->apply(newRot); bool constrained = constraint->apply(newRot);
if (constrained) { if (constrained) {
// the constraint will modify the movement of the tip so we have to compute the modified // the constraint will modify the local rotation of the tip so we must
// model-frame deltaRotation // compute the corresponding model-frame deltaRotation
// Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^ // Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^
deltaRotation = absolutePoses[pivotsParentIndex].rot * deltaRotation = absolutePoses[pivotsParentIndex].rot *
newRot * newRot * glm::inverse(absolutePoses[pivotIndex].rot);
glm::inverse(absolutePoses[pivotIndex].rot);
} }
} }
@ -264,8 +280,8 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
// keep track of tip's new transform as we descend towards root // keep track of tip's new transform as we descend towards root
tipPosition = jointPosition + deltaRotation * leverArm; tipPosition = jointPosition + deltaRotation * leverArm;
tipRotation = glm::normalize(deltaRotation * tipRotation); tipOrientation = glm::normalize(deltaRotation * tipOrientation);
tipParentRotation = glm::normalize(deltaRotation * tipParentRotation); tipParentOrientation = glm::normalize(deltaRotation * tipParentOrientation);
pivotIndex = pivotsParentIndex; pivotIndex = pivotsParentIndex;
pivotsParentIndex = _skeleton->getParentIndex(pivotIndex); pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
@ -629,11 +645,11 @@ void AnimInverseKinematics::initConstraints() {
} else if (0 == baseName.compare("Neck", Qt::CaseInsensitive)) { } else if (0 == baseName.compare("Neck", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_NECK_TWIST = PI / 4.0f; const float MAX_NECK_TWIST = PI / 6.0f;
stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST); stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST);
std::vector<float> minDots; std::vector<float> minDots;
const float MAX_NECK_SWING = PI / 3.0f; const float MAX_NECK_SWING = PI / 4.0f;
minDots.push_back(cosf(MAX_NECK_SWING)); minDots.push_back(cosf(MAX_NECK_SWING));
stConstraint->setSwingLimits(minDots); stConstraint->setSwingLimits(minDots);
@ -641,11 +657,11 @@ void AnimInverseKinematics::initConstraints() {
} else if (0 == baseName.compare("Head", Qt::CaseInsensitive)) { } else if (0 == baseName.compare("Head", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_HEAD_TWIST = PI / 4.0f; const float MAX_HEAD_TWIST = PI / 8.0f;
stConstraint->setTwistLimits(-MAX_HEAD_TWIST, MAX_HEAD_TWIST); stConstraint->setTwistLimits(-MAX_HEAD_TWIST, MAX_HEAD_TWIST);
std::vector<float> minDots; std::vector<float> minDots;
const float MAX_HEAD_SWING = PI / 4.0f; const float MAX_HEAD_SWING = PI / 6.0f;
minDots.push_back(cosf(MAX_HEAD_SWING)); minDots.push_back(cosf(MAX_HEAD_SWING));
stConstraint->setSwingLimits(minDots); stConstraint->setSwingLimits(minDots);