Mirrors/overte-JulianGro
3
0
Fork 0
mirror of https://github.com/JulianGro/overte.git synced 2025-04-09 13:12:40 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

add AnimInverseKinematics class

Browse source
This commit is contained in:
Andrew Meadows 2015-09-08 18:20:22 -07:00
parent 3d661095dc
commit ee265aba4a
4 changed files with 1011 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

639
libraries/animation/src/AnimInverseKinematics.cpp Normal file
View file

@ -0,0 +1,639 @@
//
// AnimInverseKinematics.cpp
//
// Copyright 2015 High Fidelity, Inc.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include "AnimInverseKinematics.h"
#include <NumericalConstants.h>
#include <SharedUtil.h>
#include <StreamUtils.h> // adebug
#include "ElbowConstraint.h"
#include "SwingTwistConstraint.h"
AnimInverseKinematics::AnimInverseKinematics(const std::string& id) : AnimNode(AnimNode::Type::InverseKinematics, id) {
}
AnimInverseKinematics::~AnimInverseKinematics() {
clearConstraints();
}
void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) {
_defaultRelativePoses = poses;
assert(_skeleton && _skeleton->getNumJoints() == (int)poses.size());
/*
// BUG: sometimes poses are in centimeters but we expect meters.
// HACK WORKAROUND: check for very large translation of hips and scale down as necessary
int hipsIndex = _skeleton->nameToJointIndex("Hips");
if (hipsIndex > -1 &&
glm::length(_defaultRelativePoses[hipsIndex].trans) > 10.0f &&
glm::length(_defaultRelativePoses[hipsIndex].scale) > 0.1f) {
_defaultRelativePoses[hipsIndex].scale = glm::vec3(0.01f);
_defaultRelativePoses[hipsIndex].trans *= 0.01f;
}
*/
}
void AnimInverseKinematics::loadPoses(const AnimPoseVec& poses) {
assert(_skeleton && _skeleton->getNumJoints() == (int)poses.size());
if (_skeleton->getNumJoints() == (int)poses.size()) {
_relativePoses = poses;
} else {
_relativePoses.clear();
}
/*
// BUG: sometimes poses are in centimeters but we expect meters.
// HACK WORKAROUND: check for very large translation of hips and scale down as necessary
int hipsIndex = _skeleton->nameToJointIndex("Hips");
if (hipsIndex > -1 &&
glm::length(_relativePoses[hipsIndex].trans) > 10.0f &&
glm::length(_relativePoses[hipsIndex].scale) > 0.1f) {
_relativePoses[hipsIndex].scale = glm::vec3(0.01f);
_relativePoses[hipsIndex].trans *= 0.01f;
}
*/
}
void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) const {
int numJoints = (int)_relativePoses.size();
absolutePoses.clear();
absolutePoses.reserve(numJoints);
assert(numJoints <= _skeleton->getNumJoints());
for (int i = 0; i < numJoints; ++i) {
int parentIndex = _skeleton->getParentIndex(i);
if (parentIndex < 0) {
absolutePoses[i] = _relativePoses[i];
} else {
absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
}
}
}
void AnimInverseKinematics::updateTarget(int index, const glm::vec3& position, const glm::quat& rotation) {
std::map<int, IKTarget>::iterator targetItr = _absoluteTargets.find(index);
if (targetItr != _absoluteTargets.end()) {
// update existing target
targetItr->second.pose.rot = rotation;
targetItr->second.pose.trans = position;
} else {
// add new target
assert(index >= 0 && index < _skeleton->getNumJoints());
IKTarget target;
target.pose = AnimPose(glm::vec3(1.0f), rotation, position);
// walk down the skeleton hierarchy to find the joint's root
int rootIndex = -1;
int parentIndex = _skeleton->getParentIndex(index);
while (parentIndex != -1) {
rootIndex = parentIndex;
parentIndex = _skeleton->getParentIndex(parentIndex);
}
target.rootIndex = rootIndex;
std::cout << "adebug adding target for end-effector " << index << " with rootIndex " << rootIndex << std::endl; // adebug
_absoluteTargets[index] = target;
if (index > _maxTargetIndex) {
_maxTargetIndex = index;
}
}
}
void AnimInverseKinematics::updateTarget(const QString& name, const glm::vec3& position, const glm::quat& rotation) {
int index = _skeleton->nameToJointIndex(name);
if (index != -1) {
updateTarget(index, position, rotation);
}
}
void AnimInverseKinematics::clearTarget(int index) {
_absoluteTargets.erase(index);
// recompute _maxTargetIndex
_maxTargetIndex = 0;
for (auto& targetPair: _absoluteTargets) {
int targetIndex = targetPair.first;
if (targetIndex < _maxTargetIndex) {
_maxTargetIndex = targetIndex;
}
}
}
void AnimInverseKinematics::clearAllTargets() {
_absoluteTargets.clear();
_maxTargetIndex = 0;
}
//virtual
const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
static int adebug = 0; ++adebug;
// NOTE: we assume that _relativePoses are up to date (e.g. loadPoses() was just called)
if (_relativePoses.empty()) {
return _relativePoses;
}
relaxTowardDefaults(dt);
if (_absoluteTargets.empty()) {
// no IK targets but still need to enforce constraints
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
while (constraintItr != _constraints.end()) {
int index = constraintItr->first;
glm::quat rotation = _relativePoses[index].rot;
// glm::quat oldRotation = rotation; // adebug
// bool appliedConstraint = constraintItr->second->apply(rotation);
// if (0 == (adebug % 100) && appliedConstraint) {
// float angle = glm::angle(rotation * glm::inverse(oldRotation)); // adebug
// std::cout << "adebug 001 applied constraint to index " << index << std::endl; // adebug
// }
_relativePoses[index].rot = rotation;
++constraintItr;
}
} else {
// compute absolute poses that correspond to relative target poses
AnimPoseVec absolutePoses;
computeAbsolutePoses(absolutePoses);
float largestError = 0.0f;
const float ACCEPTABLE_RELATIVE_ERROR = 1.0e-3f;
int numLoops = 0;
const int MAX_IK_LOOPS = 16;
const quint64 MAX_IK_TIME = 10 * USECS_PER_MSEC;
// quint64 loopStart = usecTimestampNow();
quint64 expiry = usecTimestampNow() + MAX_IK_TIME;
do {
largestError = 0.0f;
for (auto& targetPair: _absoluteTargets) {
int lowestMovedIndex = _relativePoses.size() - 1;
int tipIndex = targetPair.first;
AnimPose targetPose = targetPair.second.pose;
int rootIndex = targetPair.second.rootIndex;
if (rootIndex != -1) {
// transform targetPose into skeleton's absolute frame
AnimPose& rootPose = _relativePoses[rootIndex];
targetPose.trans = rootPose.trans + rootPose.rot * targetPose.trans;
targetPose.rot = rootPose.rot * targetPose.rot;
}
glm::vec3 tip = absolutePoses[tipIndex].trans;
float error = glm::length(targetPose.trans - tip);
if (error < ACCEPTABLE_RELATIVE_ERROR) {
if (largestError < error) {
largestError = error;
}
// this targetPose has been met
// finally set the relative rotation of the tip to agree with absolute target rotation
int parentIndex = _skeleton->getParentIndex(tipIndex);
if (parentIndex != -1) {
// compute tip's new parent-relative rotation
// Q = Qp * q --> q' = Qp^ * Q
glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetPose.rot;
/*
RotationConstraint* constraint = getConstraint(tipIndex);
if (constraint) {
// bool appliedConstraint = false;
if (0 == (adebug % 5)) { // && appliedConstraint) {
//std::cout << "adebug 001 applied constraint to index " << tipIndex << std::endl; // adebug
constraint->applyVerbose(newRelativeRotation);
} else {
constraint->apply(newRelativeRotation);
}
// TODO: ATM the final rotation target just fails but we need to provide
// feedback to the IK system so that it can adjust the bones up the skeleton
// to help this rotation target get met.
// TODO: setting the absolutePose.rot is not so simple if the relative rotation had to be constrained
//absolutePoses[tipIndex].rot = targetPose.rot;
}
*/
_relativePoses[tipIndex].rot = newRelativeRotation;
}
break;
}
// descend toward root, rotating each joint to get tip closer to target
int index = _skeleton->getParentIndex(tipIndex);
while (index != -1 && error > ACCEPTABLE_RELATIVE_ERROR) {
// compute the two lines that should be aligned
glm::vec3 jointPosition = absolutePoses[index].trans;
glm::vec3 leverArm = tip - jointPosition;
glm::vec3 pivotLine = targetPose.trans - jointPosition;
// compute the axis of the rotation that would align them
glm::vec3 axis = glm::cross(leverArm, pivotLine);
float axisLength = glm::length(axis);
if (axisLength > EPSILON) {
// compute deltaRotation for alignment (brings tip closer to target)
axis /= axisLength;
float angle = acosf(glm::dot(leverArm, pivotLine) / (glm::length(leverArm) * glm::length(pivotLine)));
// NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is
// still possible for the angle to be zero so we also check that to avoid unnecessary calculations.
if (angle > EPSILON) {
glm::quat deltaRotation = glm::angleAxis(angle, axis);
int parentIndex = _skeleton->getParentIndex(index);
if (parentIndex == -1) {
// accumulate any delta at the root's relative
// glm::quat newRot = glm::normalize(deltaRotation * _relativePoses[index].rot);
// _relativePoses[index].rot = newRot;
// absolutePoses[index].rot = newRot;
// 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);
if (constraint) {
// glm::quat oldRot = newRot; // adebug
// bool appliedConstraint = // adebug
constraint->apply(newRot);
// if (0 == (adebug % 100) && appliedConstraint) {
// float angle = glm::angle(newRot * glm::inverse(oldRot)); // adebug
// std::cout << "adebug 001 applied constraint to index " << index << " angle = " << angle << std::endl; // adebug
// }
}
_relativePoses[index].rot = newRot;
}
// this joint has been changed so we check to see if it has the lowest index
if (index < lowestMovedIndex) {
lowestMovedIndex = index;
}
// keep track of tip's new position as we descend towards root
tip = jointPosition + deltaRotation * leverArm;
error = glm::length(targetPose.trans - tip);
}
}
index = _skeleton->getParentIndex(index);
}
if (largestError < error) {
largestError = error;
}
if (lowestMovedIndex <= _maxTargetIndex && lowestMovedIndex < tipIndex) {
// only update the absolutePoses that matter: those between lowestMovedIndex and _maxTargetIndex
for (int i = lowestMovedIndex; i <= _maxTargetIndex; ++i) {
int parentIndex = _skeleton->getParentIndex(i);
if (parentIndex != -1) {
absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
}
}
}
// finally set the relative rotation of the tip to agree with absolute target rotation
int parentIndex = _skeleton->getParentIndex(tipIndex);
if (parentIndex != -1) {
// compute tip's new parent-relative rotation
// Q = Qp * q --> q' = Qp^ * Q
glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetPose.rot;
/*
RotationConstraint* constraint = getConstraint(tipIndex);
if (constraint) {
// bool appliedConstraint = false;
if (0 == (adebug % 5)) { // && appliedConstraint) {
//std::cout << "adebug 001 applied constraint to index " << tipIndex << std::endl; // adebug
constraint->applyVerbose(newRelativeRotation);
} else {
constraint->apply(newRelativeRotation);
}
// TODO: ATM the final rotation target just fails but we need to provide
// feedback to the IK system so that it can adjust the bones up the skeleton
// to help this rotation target get met.
}
*/
_relativePoses[tipIndex].rot = newRelativeRotation;
// TODO: setting the absolutePose.rot is not so simple if the relative rotation had to be constrained
absolutePoses[tipIndex].rot = targetPose.rot;
}
}
++numLoops;
} while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry);
/*
if (0 == (adebug % 20)) {
std::cout << "adebug"
<< " l = " << numLoops
<< " t = " << float(usecTimestampNow() - loopStart)
<< " e = " << largestError
<< std::endl; // adebug
}
*/
}
return _relativePoses;
}
RotationConstraint* AnimInverseKinematics::getConstraint(int index) {
RotationConstraint* constraint = nullptr;
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.find(index);
if (constraintItr != _constraints.end()) {
constraint = constraintItr->second;
}
return constraint;
}
void AnimInverseKinematics::clearConstraints() {
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
while (constraintItr != _constraints.end()) {
delete constraintItr->second;
++constraintItr;
}
_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;
}
// We create constraints for the joints shown here
// (and their Left counterparts if applicable).
//
// O RightHand
// /
// O /
// | O RightForeArm
// Neck O /
// | /
// O-------O-------O----O----O RightArm
// Spine2|
// |
// O Spine1
// |
// |
// O Spine
// |
// |
// O---O---O RightUpLeg
// | |
// | |
// | |
// O O RightLeg
// | |
// y | |
// | | |
// | O O RightFoot
// z | / /
// \ | O--O O--O
// \|
// x -----+
loadDefaultPoses(_skeleton->getRelativeBindPoses());
// compute corresponding absolute poses
int numJoints = (int)_defaultRelativePoses.size();
AnimPoseVec absolutePoses;
absolutePoses.reserve(numJoints);
for (int i = 0; i < numJoints; ++i) {
int parentIndex = _skeleton->getParentIndex(i);
if (parentIndex < 0) {
absolutePoses[i] = _defaultRelativePoses[i];
} else {
absolutePoses[i] = absolutePoses[parentIndex] * _defaultRelativePoses[i];
}
}
_constraints.clear();
for (int i = 0; i < numJoints; ++i) {
QString name = _skeleton->getJointName(i);
bool isLeft = name.startsWith("Left", Qt::CaseInsensitive);
float mirror = isLeft ? -1.0f : 1.0f;
if (isLeft) {
//name.remove(0, 4);
} else if (name.startsWith("Right", Qt::CaseInsensitive)) {
//name.remove(0, 5);
}
RotationConstraint* constraint = nullptr;
if (0 == name.compare("Arm", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f);
// these directions are approximate swing limits in root-frame
// NOTE: they don't need to be normalized
std::vector<glm::vec3> swungDirections;
swungDirections.push_back(glm::vec3(mirror * 1.0f, 1.0f, 1.0f));
swungDirections.push_back(glm::vec3(mirror * 1.0f, 0.0f, 1.0f));
swungDirections.push_back(glm::vec3(mirror * 1.0f, -1.0f, 0.5f));
swungDirections.push_back(glm::vec3(mirror * 0.0f, -1.0f, 0.0f));
swungDirections.push_back(glm::vec3(mirror * 0.0f, -1.0f, -1.0f));
swungDirections.push_back(glm::vec3(mirror * -0.5f, 0.0f, -1.0f));
swungDirections.push_back(glm::vec3(mirror * 0.0f, 1.0f, -1.0f));
swungDirections.push_back(glm::vec3(mirror * 0.0f, 1.0f, 0.0f));
// rotate directions into joint-frame
glm::quat invAbsoluteRotation = glm::inverse(absolutePoses[i].rot);
int numDirections = (int)swungDirections.size();
for (int j = 0; j < numDirections; ++j) {
swungDirections[j] = invAbsoluteRotation * swungDirections[j];
}
stConstraint->setSwingLimits(swungDirections);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == name.compare("UpLeg", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
// these directions are approximate swing limits in root-frame
// NOTE: they don't need to be normalized
std::vector<glm::vec3> swungDirections;
swungDirections.push_back(glm::vec3(mirror * 0.25f, 0.0f, 1.0f));
swungDirections.push_back(glm::vec3(mirror * -0.5f, 0.0f, 1.0f));
swungDirections.push_back(glm::vec3(mirror * -1.0f, 0.0f, 1.0f));
swungDirections.push_back(glm::vec3(mirror * -1.0f, 0.0f, 0.0f));
swungDirections.push_back(glm::vec3(mirror * -0.5f, -0.5f, -1.0f));
swungDirections.push_back(glm::vec3(mirror * 0.0f, -0.75f, -1.0f));
swungDirections.push_back(glm::vec3(mirror * 0.25f, -1.0f, 0.0f));
swungDirections.push_back(glm::vec3(mirror * 0.25f, -1.0f, 1.0f));
// rotate directions into joint-frame
glm::quat invAbsoluteRotation = glm::inverse(absolutePoses[i].rot);
int numDirections = (int)swungDirections.size();
for (int j = 0; j < numDirections; ++j) {
swungDirections[j] = invAbsoluteRotation * swungDirections[j];
}
stConstraint->setSwingLimits(swungDirections);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == name.compare("RightHand", Qt::CaseInsensitive)) {
std::cout << "adebug creating constraint for RightHand" << std::endl; // adebug
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_HAND_TWIST = PI / 2.0f;
stConstraint->setTwistLimits(-MAX_HAND_TWIST, MAX_HAND_TWIST);
// 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(glm::vec3(1.0f, 1.0f, 0.0f));
swungDirections.push_back(glm::vec3(0.75f, 1.0f, -1.0f));
swungDirections.push_back(glm::vec3(-0.75f, 1.0f, -1.0f));
swungDirections.push_back(glm::vec3(-1.0f, 1.0f, 0.0f));
swungDirections.push_back(glm::vec3(-0.75f, 1.0f, 1.0f));
swungDirections.push_back(glm::vec3(0.75f, 1.0f, 1.0f));
// rotate directions into joint-frame
glm::quat invRelativeRotation = glm::inverse(_defaultRelativePoses[i].rot);
int numDirections = (int)swungDirections.size();
for (int j = 0; j < numDirections; ++j) {
swungDirections[j] = invRelativeRotation * swungDirections[j];
}
stConstraint->setSwingLimits(swungDirections);
/*
std::vector<float> minDots;
const float MAX_HAND_SWING = PI / 3.0f;
minDots.push_back(cosf(MAX_HAND_SWING));
stConstraint->setSwingLimits(minDots);
*/
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (name.startsWith("SpineXXX", 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 == name.compare("NeckXXX", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
const float MAX_NECK_TWIST = PI / 2.0f;
stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST);
std::vector<float> minDots;
const float MAX_NECK_SWING = PI / 3.0f;
minDots.push_back(cosf(MAX_NECK_SWING));
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == name.compare("ForeArm", Qt::CaseInsensitive)) {
// The elbow joint rotates about the parent-frame's zAxis (-zAxis) for the Right (Left) arm.
ElbowConstraint* eConstraint = new ElbowConstraint();
glm::quat referenceRotation = _defaultRelativePoses[i].rot;
eConstraint->setReferenceRotation(referenceRotation);
// 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;
const float MIN_ELBOW_ANGLE = 0.0f;
const float MAX_ELBOW_ANGLE = 7.0f * PI / 8.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;
// 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);
float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis));
if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) {
minAngle = - minAngle;
}
float maxAngle = acosf(glm::dot(projectedYAxis, maxSwingAxis));
if (glm::dot(hingeAxis, glm::cross(projectedYAxis, maxSwingAxis)) < 0.0f) {
maxAngle = - maxAngle;
}
eConstraint->setAngleLimits(minAngle, maxAngle);
constraint = static_cast<RotationConstraint*>(eConstraint);
} else if (0 == name.compare("Leg", Qt::CaseInsensitive)) {
// The knee joint rotates about the parent-frame's -xAxis.
ElbowConstraint* eConstraint = new ElbowConstraint();
glm::quat referenceRotation = _defaultRelativePoses[i].rot;
eConstraint->setReferenceRotation(referenceRotation);
glm::vec3 hingeAxis = -1.0f * xAxis;
// 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;
// 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);
float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis));
if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) {
minAngle = - minAngle;
}
float maxAngle = acosf(glm::dot(projectedYAxis, maxSwingAxis));
if (glm::dot(hingeAxis, glm::cross(projectedYAxis, maxSwingAxis)) < 0.0f) {
maxAngle = - maxAngle;
}
eConstraint->setAngleLimits(minAngle, maxAngle);
constraint = static_cast<RotationConstraint*>(eConstraint);
} else if (0 == name.compare("Foot", Qt::CaseInsensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
// 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(glm::vec3(1.0f, 1.0f, 1.0f));
swungDirections.push_back(glm::vec3(1.0f, 1.0f, -1.0f));
// rotate directions into joint-frame
glm::quat invRelativeRotation = glm::inverse(_defaultRelativePoses[i].rot);
int numDirections = (int)swungDirections.size();
for (int j = 0; j < numDirections; ++j) {
swungDirections[j] = invRelativeRotation * swungDirections[j];
}
stConstraint->setSwingLimits(swungDirections);
constraint = static_cast<RotationConstraint*>(stConstraint);
}
if (constraint) {
_constraints[i] = constraint;
std::cout << "adebug " << i << " '" << _skeleton->getJointName(i).toStdString() << "' constraint = " << (void*)(constraint) << std::endl; // adebug
}
}
}
void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
AnimNode::setSkeletonInternal(skeleton);
if (skeleton) {
initConstraints();
} else {
clearConstraints();
}
}
void AnimInverseKinematics::relaxTowardDefaults(float dt) {
// NOTE: for now we just use a single relaxation timescale for all joints, but in the future
// we could vary the timescale on a per-joint basis or do other fancy things.
// for each joint: lerp towards the default pose
const float RELAXATION_TIMESCALE = 0.25f;
const float alpha = glm::clamp(dt / RELAXATION_TIMESCALE, 0.0f, 1.0f);
int numJoints = (int)_relativePoses.size();
for (int i = 0; i < numJoints; ++i) {
float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot, _defaultRelativePoses[i].rot));
_relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * _defaultRelativePoses[i].rot, alpha));
}
}

76
libraries/animation/src/AnimInverseKinematics.h Normal file
View file

@ -0,0 +1,76 @@
//
// AnimInverseKinematics.h
//
// Copyright 2015 High Fidelity, Inc.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#ifndef hifi_AnimInverseKinematics_h
#define hifi_AnimInverseKinematics_h
#include <string>
#include "AnimNode.h"
class RotationConstraint;
class AnimInverseKinematics : public AnimNode {
public:
AnimInverseKinematics(const std::string& id);
virtual ~AnimInverseKinematics() override;
void loadDefaultPoses(const AnimPoseVec& poses);
void loadPoses(const AnimPoseVec& poses);
const AnimPoseVec& getRelativePoses() const { return _relativePoses; }
void computeAbsolutePoses(AnimPoseVec& absolutePoses) const;
/// \param index index of end effector
/// \param position target position in the frame of the end-effector's root (not the parent!)
/// \param rotation target rotation in the frame of the end-effector's root (not the parent!)
void updateTarget(int index, const glm::vec3& position, const glm::quat& rotation);
/// \param name name of end effector
/// \param position target position in the frame of the end-effector's root (not the parent!)
/// \param rotation target rotation in the frame of the end-effector's root (not the parent!)
void updateTarget(const QString& name, const glm::vec3& position, const glm::quat& rotation);
void clearTarget(int index);
void clearAllTargets();
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) override;
protected:
virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton);
// for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; }
void relaxTowardDefaults(float dt);
RotationConstraint* getConstraint(int index);
void clearConstraints();
void initConstraints();
struct IKTarget {
AnimPose pose; // in root-frame
int rootIndex; // cached root index
};
std::map<int, RotationConstraint*> _constraints;
std::map<int, IKTarget> _absoluteTargets; // IK targets of end-points
AnimPoseVec _defaultRelativePoses; // poses of the relaxed state
AnimPoseVec _relativePoses; // current relative poses
// no copies
AnimInverseKinematics(const AnimInverseKinematics&) = delete;
AnimInverseKinematics& operator=(const AnimInverseKinematics&) = delete;
// _maxTargetIndex is tracked to help optimize the recalculation of absolute poses
// during the the cyclic coordinate descent algorithm
int _maxTargetIndex = 0;
};
#endif // hifi_AnimInverseKinematics_h

269
tests/animation/src/AnimInverseKinematicsTests.cpp Normal file
View file

@ -0,0 +1,269 @@
//
// AnimInverseKinematicsTests.cpp
//
// Copyright 2015 High Fidelity, Inc.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include "AnimInverseKinematicsTests.h"
#include <glm/gtx/transform.hpp>
#include <AnimNodeLoader.h>
#include <AnimInverseKinematics.h>
#include <AnimBlendLinear.h>
#include <AnimationLogging.h>
#include <NumericalConstants.h>
#include "../QTestExtensions.h"
QTEST_MAIN(AnimInverseKinematicsTests)
const glm::vec3 origin(0.0f);
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);
const glm::quat identity = glm::quat();
const glm::quat quaterTurnAroundZ = glm::angleAxis(0.5f * PI, zAxis);
void makeTestFBXJoints(std::vector<FBXJoint>& fbxJoints) {
FBXJoint joint;
joint.isFree = false;
joint.freeLineage.clear();
joint.parentIndex = -1;
joint.distanceToParent = 1.0f;
joint.boneRadius = 1.0f;
joint.translation = origin; // T
joint.preTransform = glm::mat4(); // Roff * Rp
joint.preRotation = identity; // Rpre
joint.rotation = identity; // R
joint.postRotation = identity; // Rpost
joint.postTransform = glm::mat4(); // Rp-1 * Soff * Sp * S * Sp-1
// World = ParentWorld * T * (Roff * Rp) * Rpre * R * Rpost * (Rp-1 * Soff * Sp * S * Sp-1)
joint.transform = glm::mat4();
joint.rotationMin = glm::vec3(-PI);
joint.rotationMax = glm::vec3(PI);
joint.inverseDefaultRotation = identity;
joint.inverseBindRotation = identity;
joint.bindTransform = glm::mat4();
joint.name = "";
joint.isSkeletonJoint = false;
// we make a list of joints that look like this:
//
// A------>B------>C------>D
joint.name = "A";
joint.parentIndex = -1;
joint.translation = origin;
fbxJoints.push_back(joint);
joint.name = "B";
joint.parentIndex = 0;
joint.translation = xAxis;
fbxJoints.push_back(joint);
joint.name = "C";
joint.parentIndex = 1;
joint.translation = xAxis;
fbxJoints.push_back(joint);
joint.name = "D";
joint.parentIndex = 2;
joint.translation = xAxis;
fbxJoints.push_back(joint);
// compute each joint's transform
for (int i = 1; i < (int)fbxJoints.size(); ++i) {
FBXJoint& j = fbxJoints[i];
int parentIndex = j.parentIndex;
// World = ParentWorld * T * (Roff * Rp) * Rpre * R * Rpost * (Rp-1 * Soff * Sp * S * Sp-1)
j.transform = fbxJoints[parentIndex].transform *
glm::translate(j.translation) *
j.preTransform *
glm::mat4_cast(j.preRotation * j.rotation * j.postRotation) *
j.postTransform;
j.inverseBindRotation = identity;
j.bindTransform = j.transform;
}
}
void AnimInverseKinematicsTests::testSingleChain() {
std::vector<FBXJoint> fbxJoints;
AnimPose offset;
makeTestFBXJoints(fbxJoints, offset);
// create a skeleton and doll
AnimSkeleton* skeleton = new AnimSkeleton(fbxJoints);
AnimSkeleton::Pointer skeletonPtr(skeleton);
AnimInverseKinematics ikDoll("doll");
ikDoll.setSkeleton(skeletonPtr);
{ // easy test IK of joint C
// load intial poses that look like this:
//
// A------>B------>C------>D
AnimPose pose;
pose.scale = glm::vec3(1.0f);
pose.rot = identity;
pose.trans = origin;
std::vector<AnimPose> poses;
poses.push_back(pose);
pose.trans = xAxis;
for (int i = 1; i < (int)fbxJoints.size(); ++i) {
poses.push_back(pose);
}
ikDoll.loadPoses(poses);
// we'll put a target t on D for <2, 1, 0> like this:
//
// t
//
//
// A------>B------>C------>D
//
int indexD = 3;
glm::vec3 targetPosition(2.0f, 1.0f, 0.0f);
glm::quat targetRotation = glm::angleAxis(PI / 2.0f, zAxis);
ikDoll.updateTarget(indexD, targetPosition, targetRotation);
// the IK solution should be:
//
// D
// |
// |
// A------>B------>C
//
float dt = 1.0f;
ikDoll.evaluate(dt);
// verify absolute results
// NOTE: since we expect this solution to converge very quickly (one loop)
// we can impose very tight error thresholds.
std::vector<AnimPose> absolutePoses;
ikDoll.computeAbsolutePoses(absolutePoses);
float acceptableAngle = 0.0001f;
QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[2].rot, quaterTurnAroundZ, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[3].rot, quaterTurnAroundZ, acceptableAngle);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, targetPosition, EPSILON);
// verify relative results
const std::vector<AnimPose>& relativePoses = ikDoll.getRelativePoses();
QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[2].rot, quaterTurnAroundZ, acceptableAngle);
QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle);
QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON);
}
{ // hard test IK of joint C
// load intial poses that look like this:
//
// D<------C
// |
// |
// A------>B
//
AnimPose pose;
pose.scale = glm::vec3(1.0f);
pose.rot = identity;
pose.trans = origin;
std::vector<AnimPose> poses;
poses.push_back(pose);
pose.trans = xAxis;
pose.rot = quaterTurnAroundZ;
poses.push_back(pose);
poses.push_back(pose);
poses.push_back(pose);
ikDoll.loadPoses(poses);
// we'll put a target t on D for <3, 0, 0> like this:
//
// D<------C
// |
// |
// A------>B t
//
int indexD = 3;
glm::vec3 targetPosition(3.0f, 0.0f, 0.0f);
glm::quat targetRotation = identity;
ikDoll.updateTarget(indexD, targetPosition, targetRotation);
// the IK solution should be:
//
// A------>B------>C------>D
//
float dt = 1.0f;
ikDoll.evaluate(dt);
// verify absolute results
// NOTE: the IK algorithm doesn't converge very fast for full-reach targets,
// so we'll consider the poses as good if they are closer than they started.
//
// NOTE: constraints may help speed up convergence since some joints may get clamped
// to maximum extension. TODO: experiment with tightening the error thresholds when
// constraints are working.
std::vector<AnimPose> absolutePoses;
ikDoll.computeAbsolutePoses(absolutePoses);
float acceptableAngle = 0.1f; // radians
QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[2].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[3].rot, identity, acceptableAngle);
float acceptableDistance = 0.4f;
QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, acceptableDistance);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, acceptableDistance);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, 3.0f * xAxis, acceptableDistance);
// verify relative results
const std::vector<AnimPose>& relativePoses = ikDoll.getRelativePoses();
QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[2].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle);
QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON);
}
}
void AnimInverseKinematicsTests::testBar() {
// test AnimPose math
// TODO: move this to other test file
glm::vec3 transA = glm::vec3(1.0f, 2.0f, 3.0f);
glm::vec3 transB = glm::vec3(5.0f, 7.0f, 9.0f);
glm::quat rot = identity;
glm::vec3 scale = glm::vec3(1.0f);
AnimPose poseA(scale, rot, transA);
AnimPose poseB(scale, rot, transB);
AnimPose poseC = poseA * poseB;
glm::vec3 expectedTransC = transA + transB;
QCOMPARE_WITH_ABS_ERROR(expectedTransC, poseC.trans, EPSILON);
}

27
tests/animation/src/AnimInverseKinematicsTests.h Normal file
View file

@ -0,0 +1,27 @@
//
// AnimInverseKinematicsTests.h
//
// Copyright 2015 High Fidelity, Inc.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#ifndef hifi_AnimInverseKinematicsTests_h
#define hifi_AnimInverseKinematicsTests_h
#include <QtTest/QtTest>
#include <glm/glm.hpp>
inline float getErrorDifference(float a, float b) {
return fabs(a - b);
}
class AnimInverseKinematicsTests : public QObject {
Q_OBJECT
private slots:
void testSingleChain();
void testBar();
};
#endif // hifi_AnimInverseKinematicsTests_h