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

cleanup and optimization of IK loop

Browse source
This commit is contained in:
Andrew Meadows 2015-10-10 15:53:03 -07:00
parent 6a96d5f0c5
commit 5f1068c404
1 changed files with 90 additions and 93 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -143,36 +143,43 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
do { do {
int lowestMovedIndex = _relativePoses.size(); int lowestMovedIndex = _relativePoses.size();
for (auto& target: targets) { for (auto& target: targets) {
if (target.getType() == IKTarget::Type::RotationOnly) { IKTarget::Type targetType = target.getType();
if (targetType == IKTarget::Type::RotationOnly) {
// the final rotation will be enforced after the iterations // the final rotation will be enforced after the iterations
continue; continue;
} }
// cache tip absolute transform // cache tip absolute transform
int tipIndex = target.getIndex(); int tipIndex = target.getIndex();
int pivotIndex = _skeleton->getParentIndex(tipIndex);
if (pivotIndex == -1) {
continue;
}
int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
if (pivotsParentIndex == -1) {
// TODO?: handle case where tip's parent is root?
continue;
}
glm::vec3 tipPosition = absolutePoses[tipIndex].trans; glm::vec3 tipPosition = absolutePoses[tipIndex].trans;
glm::quat tipRotation = absolutePoses[tipIndex].rot; //glm::quat tipRotation = absolutePoses[tipIndex].rot;
// cache tip's parent's absolute rotation so we can recompute the tip's parent-relative // cache tip's parent's absolute rotation so we can recompute the tip's parent-relative
// as we proceed walking down the joint chain // as we proceed walking down the joint chain
int pivotIndex = _skeleton->getParentIndex(tipIndex); glm::quat tipParentRotation = absolutePoses[pivotIndex].rot;
glm::quat tipParentRotation;
if (pivotIndex != -1) {
tipParentRotation = absolutePoses[pivotIndex].rot;
}
// descend toward root, pivoting each joint to get tip closer to target // descend toward root, pivoting each joint to get tip closer to target
int ancestorCount = 1; while (pivotsParentIndex != -1) {
while (pivotIndex != -1) {
// compute the two lines that should be aligned // compute the two lines that should be aligned
glm::vec3 jointPosition = absolutePoses[pivotIndex].trans; glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
glm::vec3 leverArm = tipPosition - jointPosition; glm::vec3 leverArm = tipPosition - jointPosition;
glm::vec3 targetLine = target.getTranslation() - jointPosition;
glm::quat deltaRotation;
if (targetType == IKTarget::Type::RotationAndPosition) {
// compute the swing that would get get tip closer // compute the swing that would get get tip closer
glm::vec3 targetLine = target.getTranslation() - jointPosition;
glm::vec3 axis = glm::cross(leverArm, targetLine); glm::vec3 axis = glm::cross(leverArm, targetLine);
float axisLength = glm::length(axis); float axisLength = glm::length(axis);
glm::quat deltaRotation;
const float MIN_AXIS_LENGTH = 1.0e-4f; const float MIN_AXIS_LENGTH = 1.0e-4f;
if (axisLength > MIN_AXIS_LENGTH) { if (axisLength > MIN_AXIS_LENGTH) {
// compute deltaRotation for alignment (swings tip closer to target) // compute deltaRotation for alignment (swings tip closer to target)
@ -183,13 +190,14 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
// still possible for the angle to be zero so we also check that to avoid unnecessary calculations. // still possible for the angle to be zero so we also check that to avoid unnecessary calculations.
const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f; const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f;
if (angle > MIN_ADJUSTMENT_ANGLE) { if (angle > MIN_ADJUSTMENT_ANGLE) {
// reduce angle by a fraction (reduces IK swing contribution of this joint) // reduce angle by a fraction (for stability)
angle /= (float)ancestorCount; const float fraction = 0.5f;
angle *= fraction;
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. We compute that delta here and rotate the tipJoint accordingly. // new rotation and its target. This is the final parent-relative rotation that the tip joint have
// make to achieve its target rotation.
glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentRotation) * target.getRotation(); glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentRotation) * target.getRotation();
// enforce tip's constraint // enforce tip's constraint
@ -197,32 +205,32 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
if (constraint) { if (constraint) {
bool constrained = constraint->apply(tipRelativeRotation); bool constrained = constraint->apply(tipRelativeRotation);
if (constrained) { if (constrained) {
// The tip's final parent-relative rotation violates its constraint // The tip's final parent-relative rotation would violate its constraint
// so we try to 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 * tipParentRotation * 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;
glm::vec3 axis = glm::normalize(deltaRotation * leverArm); glm::vec3 axis = glm::normalize(deltaRotation * leverArm);
swingTwistDecomposition(missingRotation, axis, swingPart, twistPart); swingTwistDecomposition(missingRotation, axis, swingPart, twistPart);
deltaRotation = twistPart * deltaRotation; float dotSign = copysignf(1.0f, twistPart.w);
} deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * twistPart, fraction)) * deltaRotation;
// we update the tip rotation here to rotate it as close to its target orientation as possible
// before moving on to next pivot
tipRotation = tipParentRotation * tipRelativeRotation;
} }
} }
++ancestorCount; }
}
} else if (targetType == IKTarget::Type::HmdHead) {
/* TODO: implement this
// 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(tipRotation);
*/
}
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 after swing // compute joint's new parent-relative rotation after swing
// Q' = dQ * Q and Q = Qp * q --> q' = Qp^ * dQ * Q // Q' = dQ * Q and Q = Qp * q --> q' = Qp^ * dQ * Q
glm::quat newRot = glm::normalize(glm::inverse( glm::quat newRot = glm::normalize(glm::inverse(
absolutePoses[parentIndex].rot) * absolutePoses[pivotsParentIndex].rot) *
deltaRotation * deltaRotation *
absolutePoses[pivotIndex].rot); absolutePoses[pivotIndex].rot);
@ -234,7 +242,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
// the constraint will modify the movement of the tip so we have to compute the modified // the constraint will modify the movement of the tip so we have to compute the modified
// model-frame deltaRotation // model-frame deltaRotation
// Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^ // Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^
deltaRotation = absolutePoses[parentIndex].rot * deltaRotation = absolutePoses[pivotsParentIndex].rot *
newRot * newRot *
glm::inverse(absolutePoses[pivotIndex].rot); glm::inverse(absolutePoses[pivotIndex].rot);
} }
@ -242,7 +250,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
// store the rotation change in the accumulator // store the rotation change in the accumulator
_accumulators[pivotIndex].add(newRot); _accumulators[pivotIndex].add(newRot);
}
// this joint has been changed so we check to see if it has the lowest index // this joint has been changed so we check to see if it has the lowest index
if (pivotIndex < lowestMovedIndex) { if (pivotIndex < lowestMovedIndex) {
lowestMovedIndex = pivotIndex; lowestMovedIndex = pivotIndex;
@ -250,10 +258,10 @@ 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);
tipParentRotation = glm::normalize(deltaRotation * tipParentRotation); tipParentRotation = glm::normalize(deltaRotation * tipParentRotation);
pivotIndex = _skeleton->getParentIndex(pivotIndex); pivotIndex = pivotsParentIndex;
pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
} }
} }
++numLoops; ++numLoops;
@ -276,17 +284,6 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
} }
} while (numLoops < MAX_IK_LOOPS); } while (numLoops < MAX_IK_LOOPS);
/* KEEP: example code for measuring endeffector error of IK solution
for (uint32_t i = 0; i < targets.size(); ++i) {
auto& target = targets[i];
if (target.getType() == IKTarget::Type::RotationOnly) {
continue;
}
glm::vec3 tipPosition = absolutePoses[target.index].trans;
std::cout << i << " IK error = " << glm::distance(tipPosition, target.getTranslation()) << std::endl;
}
*/
// finally set the relative rotation of each tip to agree with absolute target rotation // finally set the relative rotation of each tip to agree with absolute target rotation
for (auto& target: targets) { for (auto& target: targets) {
int tipIndex = target.getIndex(); int tipIndex = target.getIndex();