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

Merge remote-tracking branch 'andrew/europium' into tony/ik-and-controllers

Browse source
This commit is contained in:
Anthony J. Thibault 2015-09-09 09:40:55 -07:00
commit 146836452f
16 changed files with 1079 additions and 44 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -0,0 +1,566 @@
//
// 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 "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());
}
void AnimInverseKinematics::loadPoses(const AnimPoseVec& poses) {
assert(_skeleton && _skeleton->getNumJoints() == (int)poses.size());
if (_skeleton->getNumJoints() == (int)poses.size()) {
_relativePoses = poses;
} else {
_relativePoses.clear();
}
}
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;
_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) {
// 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;
constraintItr->second->apply(rotation);
_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 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) {
constraint->apply(newRelativeRotation);
// TODO: ATM the final rotation target may 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;
}
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) {
// 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) {
constraint->apply(newRot);
}
_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) {
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;
absolutePoses[tipIndex].rot = targetPose.rot;
}
}
++numLoops;
} while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry);
}
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)) {
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;
}
}
}
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

1
libraries/animation/src/AnimNode.h
View file

@ -41,6 +41,7 @@ public:
Overlay,
StateMachine,
Controller,
InverseKinematics,
NumTypes
};
using Pointer = std::shared_ptr<AnimNode>;

3
libraries/animation/src/AnimNodeLoader.cpp
View file

@ -47,6 +47,7 @@ static const char* animNodeTypeToString(AnimNode::Type type) {
case AnimNode::Type::Overlay: return "overlay";
case AnimNode::Type::StateMachine: return "stateMachine";
case AnimNode::Type::Controller: return "controller";
case AnimNode::Type::InverseKinematics: return nullptr;
case AnimNode::Type::NumTypes: return nullptr;
};
return nullptr;
@ -59,6 +60,7 @@ static NodeLoaderFunc animNodeTypeToLoaderFunc(AnimNode::Type type) {
case AnimNode::Type::Overlay: return loadOverlayNode;
case AnimNode::Type::StateMachine: return loadStateMachineNode;
case AnimNode::Type::Controller: return loadControllerNode;
case AnimNode::Type::InverseKinematics: return nullptr;
case AnimNode::Type::NumTypes: return nullptr;
};
return nullptr;
@ -71,6 +73,7 @@ static NodeProcessFunc animNodeTypeToProcessFunc(AnimNode::Type type) {
case AnimNode::Type::Overlay: return processOverlayNode;
case AnimNode::Type::StateMachine: return processStateMachineNode;
case AnimNode::Type::Controller: return processControllerNode;
case AnimNode::Type::InverseKinematics: return nullptr;
case AnimNode::Type::NumTypes: return nullptr;
};
return nullptr;

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

@ -15,7 +15,7 @@
#include <QString>
#include <QUrl>
#include <QNetworkReply>
#include <QtNetwork/QNetworkReply>
#include "AnimNode.h"

8
libraries/animation/src/AnimSkeleton.cpp
View file

@ -9,10 +9,12 @@
//
#include "AnimSkeleton.h"
#include "AnimationLogging.h"
#include "GLMHelpers.h"
#include <glm/gtx/transform.hpp>
#include <glm/gtc/quaternion.hpp>
#include <GLMHelpers.h>
#include "AnimationLogging.h"
const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f),
glm::quat(),

5
libraries/animation/src/AnimSkeleton.h
View file

@ -12,8 +12,10 @@
#define hifi_AnimSkeleton
#include <vector>
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include "FBXReader.h"
#include <FBXReader.h>
struct AnimPose {
AnimPose() {}
@ -58,6 +60,7 @@ public:
// relative to parent pose
const AnimPose& getRelativeBindPose(int jointIndex) const;
const AnimPoseVec& getRelativeBindPoses() const { return _relativeBindPoses; }
int getParentIndex(int jointIndex) const;

6
libraries/animation/src/AnimationCache.h
View file

@ -12,9 +12,9 @@
#ifndef hifi_AnimationCache_h
#define hifi_AnimationCache_h
#include <QRunnable>
#include <QScriptEngine>
#include <QScriptValue>
#include <QtCore/QRunnable>
#include <QtScript/QScriptEngine>
#include <QtScript/QScriptValue>
#include <DependencyManager.h>
#include <FBXReader.h>

1
libraries/animation/src/ElbowConstraint.cpp
View file

@ -15,7 +15,6 @@
#include <NumericalConstants.h>
ElbowConstraint::ElbowConstraint() :
_referenceRotation(),
_minAngle(-PI),
_maxAngle(PI)
{

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

@ -15,12 +15,10 @@
class ElbowConstraint : public RotationConstraint {
public:
ElbowConstraint();
virtual void setReferenceRotation(const glm::quat& rotation) override { _referenceRotation = rotation; }
void setHingeAxis(const glm::vec3& axis);
void setAngleLimits(float minAngle, float maxAngle);
virtual bool apply(glm::quat& rotation) const override;
protected:
glm::quat _referenceRotation;
glm::vec3 _axis;
glm::vec3 _perpAxis;
float _minAngle;

12
libraries/animation/src/RotationConstraint.h
View file

@ -15,15 +15,21 @@
class RotationConstraint {
public:
RotationConstraint() {}
RotationConstraint() : _referenceRotation() {}
virtual ~RotationConstraint() {}
/// \param rotation the default rotation that represents
virtual void setReferenceRotation(const glm::quat& rotation) = 0;
/// \param referenceRotation the rotation from which rotation changes are measured.
virtual void setReferenceRotation(const glm::quat& rotation) { _referenceRotation = rotation; }
/// \return the rotation from which rotation changes are measured.
const glm::quat& getReferenceRotation() const { return _referenceRotation; }
/// \param rotation rotation to clamp
/// \return true if rotation is clamped
virtual bool apply(glm::quat& rotation) const = 0;
protected:
glm::quat _referenceRotation = glm::quat();
};
#endif // hifi_RotationConstraint_h

108
libraries/animation/src/SwingTwistConstraint.cpp
View file

@ -32,14 +32,20 @@ void SwingTwistConstraint::SwingLimitFunction::setCone(float maxAngle) {
}
void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector<float>& minDots) {
int numDots = minDots.size();
uint32_t numDots = minDots.size();
_minDots.clear();
_minDots.reserve(numDots);
for (int i = 0; i < numDots; ++i) {
_minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT));
if (numDots == 0) {
// push two copies of MIN_MINDOT
_minDots.push_back(MIN_MINDOT);
_minDots.push_back(MIN_MINDOT);
} else {
_minDots.reserve(numDots);
for (uint32_t i = 0; i < numDots; ++i) {
_minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT));
}
// push the first value to the back to establish cyclic boundary conditions
_minDots.push_back(_minDots[0]);
}
// push the first value to the back to establish cyclic boundary conditions
_minDots.push_back(_minDots[0]);
}
float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const {
@ -58,8 +64,8 @@ float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const {
}
SwingTwistConstraint::SwingTwistConstraint() :
RotationConstraint(),
_swingLimitFunction(),
_referenceRotation(),
_minTwist(-PI),
_maxTwist(PI)
{
@ -69,6 +75,85 @@ void SwingTwistConstraint::setSwingLimits(std::vector<float> minDots) {
_swingLimitFunction.setMinDots(minDots);
}
void SwingTwistConstraint::setSwingLimits(const std::vector<glm::vec3>& swungDirections) {
struct SwingLimitData {
SwingLimitData() : _theta(0.0f), _minDot(1.0f) {}
SwingLimitData(float theta, float minDot) : _theta(theta), _minDot(minDot) {}
float _theta;
float _minDot;
bool operator<(const SwingLimitData& other) const { return _theta < other._theta; }
};
std::vector<SwingLimitData> limits;
uint32_t numLimits = swungDirections.size();
limits.reserve(numLimits);
// compute the limit pairs: <theta, minDot>
const glm::vec3 yAxis = glm::vec3(0.0f, 1.0f, 0.0f);
for (uint32_t i = 0; i < numLimits; ++i) {
float directionLength = glm::length(swungDirections[i]);
if (directionLength > EPSILON) {
glm::vec3 swingAxis = glm::cross(yAxis, swungDirections[i]);
float theta = atan2f(-swingAxis.z, swingAxis.x);
if (theta < 0.0f) {
theta += TWO_PI;
}
limits.push_back(SwingLimitData(theta, swungDirections[i].y / directionLength));
}
}
std::vector<float> minDots;
numLimits = limits.size();
if (numLimits == 0) {
// trivial case: nearly free constraint
std::vector<float> minDots;
_swingLimitFunction.setMinDots(minDots);
} else if (numLimits == 1) {
// trivial case: uniform conical constraint
std::vector<float> minDots;
minDots.push_back(limits[0]._minDot);
_swingLimitFunction.setMinDots(minDots);
} else {
// interesting case: potentially non-uniform constraints
// sort limits by theta
std::sort(limits.begin(), limits.end());
// extrapolate evenly distributed limits for fast lookup table
float deltaTheta = TWO_PI / (float)(numLimits);
uint32_t rightIndex = 0;
for (uint32_t i = 0; i < numLimits; ++i) {
float theta = (float)i * deltaTheta;
uint32_t leftIndex = (rightIndex - 1) % numLimits;
while (rightIndex < numLimits && theta > limits[rightIndex]._theta) {
leftIndex = rightIndex++;
}
if (leftIndex == numLimits - 1) {
// we straddle the boundary
rightIndex = 0;
}
float rightTheta = limits[rightIndex]._theta;
float leftTheta = limits[leftIndex]._theta;
if (leftTheta > rightTheta) {
// we straddle the boundary, but we need to figure out which way to stride
// in order to keep theta between left and right
if (leftTheta > theta) {
leftTheta -= TWO_PI;
} else {
rightTheta += TWO_PI;
}
}
// blend between the left and right minDots to get the value that corresponds to this theta
float rightWeight = (theta - leftTheta) / (rightTheta - leftTheta);
minDots.push_back((1.0f - rightWeight) * limits[leftIndex]._minDot + rightWeight * limits[rightIndex]._minDot);
}
}
_swingLimitFunction.setMinDots(minDots);
}
void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) {
// NOTE: min/maxTwist angles should be in the range [-PI, PI]
_minTwist = glm::min(minTwist, maxTwist);
@ -107,13 +192,10 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
float theta = atan2f(-swingAxis.z, swingAxis.x);
float minDot = _swingLimitFunction.getMinDot(theta);
if (glm::dot(swungY, yAxis) < minDot) {
// The swing limits are violated so we use the maxAngle to supply a new rotation.
float maxAngle = acosf(minDot);
if (minDot < 0.0f) {
maxAngle = PI - maxAngle;
}
// The swing limits are violated so we extract the angle from midDot and
// use it to supply a new rotation.
swingAxis /= axisLength;
swingRotation = glm::angleAxis(maxAngle, swingAxis);
swingRotation = glm::angleAxis(acosf(minDot), swingAxis);
swingWasClamped = true;
}
}

15
libraries/animation/src/SwingTwistConstraint.h
View file

@ -16,7 +16,7 @@
#include <math.h>
class SwingTwistConstraint : RotationConstraint {
class SwingTwistConstraint : public RotationConstraint {
public:
// The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist
// about the yAxis followed by a swing about some axis that lies in the XZ plane, such that the twist
@ -25,10 +25,7 @@ public:
SwingTwistConstraint();
/// \param referenceRotation the rotation from which rotation changes are measured.
virtual void setReferenceRotation(const glm::quat& referenceRotation) override { _referenceRotation = referenceRotation; }
/// \param minDots vector of minimum dot products between the twist and swung axes.
/// \param minDots vector of minimum dot products between the twist and swung axes
/// \brief The values are minimum dot-products between the twist axis and the swung axes
/// that correspond to swing axes equally spaced around the XZ plane. Another way to
/// think about it is that the dot-products correspond to correspond to angles (theta)
@ -38,6 +35,13 @@ public:
/// description of how this works.
void setSwingLimits(std::vector<float> minDots);
/// \param swungDirections vector of directions that lie on the swing limit boundary
/// \brief For each swungDirection we compute the corresponding [theta, minDot] pair.
/// We expect the values of theta to NOT be uniformly spaced around the range [0, TWO_PI]
/// so we'll use the input set to extrapolate a lookup function of evenly spaced values.
void setSwingLimits(const std::vector<glm::vec3>& swungDirections);
/// \param minTwist the minimum angle of rotation about the twist axis
/// \param maxTwist the maximum angle of rotation about the twist axis
void setTwistLimits(float minTwist, float maxTwist);
@ -71,7 +75,6 @@ public:
protected:
SwingLimitFunction _swingLimitFunction;
glm::quat _referenceRotation;
float _minTwist;
float _maxTwist;
};

22
libraries/networking/src/ResourceCache.h
View file

@ -12,17 +12,17 @@
#ifndef hifi_ResourceCache_h
#define hifi_ResourceCache_h
#include <QHash>
#include <QList>
#include <QNetworkReply>
#include <QNetworkRequest>
#include <QObject>
#include <QPointer>
#include <QSharedPointer>
#include <QUrl>
#include <QWeakPointer>
#include <QReadWriteLock>
#include <QQueue>
#include <QtCore/QHash>
#include <QtCore/QList>
#include <QtCore/QObject>
#include <QtCore/QPointer>
#include <QtCore/QSharedPointer>
#include <QtCore/QUrl>
#include <QtCore/QWeakPointer>
#include <QtCore/QReadWriteLock>
#include <QtCore/QQueue>
#include <QtNetwork/QNetworkReply>
#include <QtNetwork/QNetworkRequest>
#include <DependencyManager.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