mirror of
https://github.com/HifiExperiments/overte.git
synced 2025-07-15 12:56:40 +02:00
a028d3ba58
A AnimContext class was introduced. This context is passed into every node during evaluation/overlay. It holds non-animVar "global" data passed from the application.
963 lines
45 KiB
C++
963 lines
45 KiB
C++
//
|
|
// 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 <GeometryUtil.h>
|
|
#include <GLMHelpers.h>
|
|
#include <NumericalConstants.h>
|
|
#include <SharedUtil.h>
|
|
#include <shared/NsightHelpers.h>
|
|
#include <DebugDraw.h>
|
|
#include "Rig.h"
|
|
|
|
#include "ElbowConstraint.h"
|
|
#include "SwingTwistConstraint.h"
|
|
#include "AnimationLogging.h"
|
|
|
|
AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimNode::Type::InverseKinematics, id) {
|
|
}
|
|
|
|
AnimInverseKinematics::~AnimInverseKinematics() {
|
|
clearConstraints();
|
|
_accumulators.clear();
|
|
_targetVarVec.clear();
|
|
}
|
|
|
|
void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) {
|
|
_defaultRelativePoses = poses;
|
|
assert(_skeleton && _skeleton->getNumJoints() == (int)poses.size());
|
|
}
|
|
|
|
void AnimInverseKinematics::loadPoses(const AnimPoseVec& poses) {
|
|
assert(_skeleton && ((poses.size() == 0) || (_skeleton->getNumJoints() == (int)poses.size())));
|
|
if (_skeleton->getNumJoints() == (int)poses.size()) {
|
|
_relativePoses = poses;
|
|
_accumulators.resize(_relativePoses.size());
|
|
} else {
|
|
_relativePoses.clear();
|
|
_accumulators.clear();
|
|
}
|
|
}
|
|
|
|
void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) const {
|
|
int numJoints = (int)_relativePoses.size();
|
|
assert(numJoints <= _skeleton->getNumJoints());
|
|
assert(numJoints == (int)absolutePoses.size());
|
|
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::setTargetVars(
|
|
const QString& jointName,
|
|
const QString& positionVar,
|
|
const QString& rotationVar,
|
|
const QString& typeVar) {
|
|
// if there are dups, last one wins.
|
|
bool found = false;
|
|
for (auto& targetVar: _targetVarVec) {
|
|
if (targetVar.jointName == jointName) {
|
|
// update existing targetVar
|
|
targetVar.positionVar = positionVar;
|
|
targetVar.rotationVar = rotationVar;
|
|
targetVar.typeVar = typeVar;
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found) {
|
|
// create a new entry
|
|
_targetVarVec.push_back(IKTargetVar(jointName, positionVar, rotationVar, typeVar));
|
|
}
|
|
}
|
|
|
|
void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses) {
|
|
// build a list of valid targets from _targetVarVec and animVars
|
|
_maxTargetIndex = -1;
|
|
bool removeUnfoundJoints = false;
|
|
for (auto& targetVar : _targetVarVec) {
|
|
if (targetVar.jointIndex == -1) {
|
|
// this targetVar hasn't been validated yet...
|
|
int jointIndex = _skeleton->nameToJointIndex(targetVar.jointName);
|
|
if (jointIndex >= 0) {
|
|
// this targetVar has a valid joint --> cache the indices
|
|
targetVar.jointIndex = jointIndex;
|
|
} else {
|
|
qCWarning(animation) << "AnimInverseKinematics could not find jointName" << targetVar.jointName << "in skeleton";
|
|
removeUnfoundJoints = true;
|
|
}
|
|
} else {
|
|
IKTarget target;
|
|
target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition));
|
|
if (target.getType() != IKTarget::Type::Unknown) {
|
|
AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);
|
|
glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, defaultPose.rot);
|
|
glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, defaultPose.trans);
|
|
if (target.getType() == IKTarget::Type::HipsRelativeRotationAndPosition) {
|
|
translation += _hipsOffset;
|
|
}
|
|
target.setPose(rotation, translation);
|
|
target.setIndex(targetVar.jointIndex);
|
|
targets.push_back(target);
|
|
if (targetVar.jointIndex > _maxTargetIndex) {
|
|
_maxTargetIndex = targetVar.jointIndex;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (removeUnfoundJoints) {
|
|
int numVars = (int)_targetVarVec.size();
|
|
int i = 0;
|
|
while (i < numVars) {
|
|
if (_targetVarVec[i].jointIndex == -1) {
|
|
if (numVars > 1) {
|
|
// swap i for last element
|
|
_targetVarVec[i] = _targetVarVec[numVars - 1];
|
|
}
|
|
_targetVarVec.pop_back();
|
|
--numVars;
|
|
} else {
|
|
++i;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets) {
|
|
// compute absolute poses that correspond to relative target poses
|
|
AnimPoseVec absolutePoses;
|
|
absolutePoses.resize(_relativePoses.size());
|
|
computeAbsolutePoses(absolutePoses);
|
|
|
|
// clear the accumulators before we start the IK solver
|
|
for (auto& accumulator: _accumulators) {
|
|
accumulator.clearAndClean();
|
|
}
|
|
|
|
float maxError = FLT_MAX;
|
|
int numLoops = 0;
|
|
const int MAX_IK_LOOPS = 16;
|
|
const float MAX_ERROR_TOLERANCE = 0.1f; // cm
|
|
while (maxError > MAX_ERROR_TOLERANCE && numLoops < MAX_IK_LOOPS) {
|
|
++numLoops;
|
|
|
|
// solve all targets
|
|
int lowestMovedIndex = (int)_relativePoses.size();
|
|
for (auto& target: targets) {
|
|
int lowIndex = solveTargetWithCCD(target, absolutePoses);
|
|
if (lowIndex < lowestMovedIndex) {
|
|
lowestMovedIndex = lowIndex;
|
|
}
|
|
}
|
|
|
|
// harvest accumulated rotations and apply the average
|
|
for (int i = lowestMovedIndex; i < _maxTargetIndex; ++i) {
|
|
if (_accumulators[i].size() > 0) {
|
|
_relativePoses[i].rot = _accumulators[i].getAverage();
|
|
_accumulators[i].clear();
|
|
}
|
|
}
|
|
|
|
// update the absolutePoses that need it (from lowestMovedIndex to _maxTargetIndex)
|
|
for (auto i = lowestMovedIndex; i <= _maxTargetIndex; ++i) {
|
|
auto parentIndex = _skeleton->getParentIndex((int)i);
|
|
if (parentIndex != -1) {
|
|
absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
|
|
}
|
|
}
|
|
|
|
// compute maxError
|
|
maxError = 0.0f;
|
|
for (size_t i = 0; i < targets.size(); i++) {
|
|
if (targets[i].getType() == IKTarget::Type::RotationAndPosition || targets[i].getType() == IKTarget::Type::HmdHead ||
|
|
targets[i].getType() == IKTarget::Type::HipsRelativeRotationAndPosition) {
|
|
float error = glm::length(absolutePoses[targets[i].getIndex()].trans - targets[i].getTranslation());
|
|
if (error > maxError) {
|
|
maxError = error;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// finally set the relative rotation of each tip to agree with absolute target rotation
|
|
for (auto& target: targets) {
|
|
int tipIndex = target.getIndex();
|
|
int parentIndex = _skeleton->getParentIndex(tipIndex);
|
|
if (parentIndex != -1) {
|
|
const glm::quat& targetRotation = target.getRotation();
|
|
// compute tip's new parent-relative rotation
|
|
// Q = Qp * q --> q' = Qp^ * Q
|
|
glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetRotation;
|
|
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 = targetRotation;
|
|
}
|
|
}
|
|
}
|
|
|
|
int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVec& absolutePoses) {
|
|
int lowestMovedIndex = (int)_relativePoses.size();
|
|
IKTarget::Type targetType = target.getType();
|
|
if (targetType == IKTarget::Type::RotationOnly) {
|
|
// the final rotation will be enforced after the iterations
|
|
// TODO: solve this correctly
|
|
return lowestMovedIndex;
|
|
}
|
|
|
|
int tipIndex = target.getIndex();
|
|
int pivotIndex = _skeleton->getParentIndex(tipIndex);
|
|
if (pivotIndex == -1 || pivotIndex == _hipsIndex) {
|
|
return lowestMovedIndex;
|
|
}
|
|
int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
|
|
if (pivotsParentIndex == -1) {
|
|
// TODO?: handle case where tip's parent is root?
|
|
return lowestMovedIndex;
|
|
}
|
|
|
|
// cache tip's absolute orientation
|
|
glm::quat tipOrientation = absolutePoses[tipIndex].rot;
|
|
|
|
// also cache tip's parent's absolute orientation so we can recompute
|
|
// the tip's parent-relative as we proceed up the chain
|
|
glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot;
|
|
|
|
if (targetType == IKTarget::Type::HmdHead) {
|
|
// rotate tip directly to target orientation
|
|
tipOrientation = target.getRotation();
|
|
glm::quat tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation));
|
|
|
|
// enforce tip's constraint
|
|
RotationConstraint* constraint = getConstraint(tipIndex);
|
|
if (constraint) {
|
|
bool constrained = constraint->apply(tipRelativeRotation);
|
|
if (constrained) {
|
|
tipOrientation = glm::normalize(tipRelativeRotation * tipParentOrientation);
|
|
tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation));
|
|
}
|
|
}
|
|
// store the relative rotation change in the accumulator
|
|
_accumulators[tipIndex].add(tipRelativeRotation, target.getWeight());
|
|
}
|
|
|
|
// cache tip absolute position
|
|
glm::vec3 tipPosition = absolutePoses[tipIndex].trans;
|
|
|
|
// descend toward root, pivoting each joint to get tip closer to target position
|
|
while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) {
|
|
// compute the two lines that should be aligned
|
|
glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
|
|
glm::vec3 leverArm = tipPosition - jointPosition;
|
|
|
|
glm::quat deltaRotation;
|
|
if (targetType == IKTarget::Type::RotationAndPosition ||
|
|
targetType == IKTarget::Type::HipsRelativeRotationAndPosition) {
|
|
// compute the swing that would get get tip closer
|
|
glm::vec3 targetLine = target.getTranslation() - jointPosition;
|
|
|
|
const float MIN_AXIS_LENGTH = 1.0e-4f;
|
|
RotationConstraint* constraint = getConstraint(pivotIndex);
|
|
if (constraint && constraint->isLowerSpine()) {
|
|
// for these types of targets we only allow twist at the lower-spine
|
|
// (this prevents the hand targets from bending the spine too much and thereby driving the hips too far)
|
|
glm::vec3 twistAxis = absolutePoses[pivotIndex].trans - absolutePoses[pivotsParentIndex].trans;
|
|
float twistAxisLength = glm::length(twistAxis);
|
|
if (twistAxisLength > MIN_AXIS_LENGTH) {
|
|
// project leverArm and targetLine to the plane
|
|
twistAxis /= twistAxisLength;
|
|
leverArm -= glm::dot(leverArm, twistAxis) * twistAxis;
|
|
targetLine -= glm::dot(targetLine, twistAxis) * twistAxis;
|
|
} else {
|
|
leverArm = Vectors::ZERO;
|
|
targetLine = Vectors::ZERO;
|
|
}
|
|
}
|
|
|
|
glm::vec3 axis = glm::cross(leverArm, targetLine);
|
|
float axisLength = glm::length(axis);
|
|
if (axisLength > MIN_AXIS_LENGTH) {
|
|
// compute angle of rotation that brings tip closer to target
|
|
axis /= axisLength;
|
|
float cosAngle = glm::clamp(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)), -1.0f, 1.0f);
|
|
float angle = acosf(cosAngle);
|
|
const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f;
|
|
if (angle > MIN_ADJUSTMENT_ANGLE) {
|
|
// reduce angle by a fraction (for stability)
|
|
const float FRACTION = 0.5f;
|
|
angle *= FRACTION;
|
|
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
|
|
// new orientation and its target. This is the final parent-relative orientation that the tip joint have
|
|
// make to achieve its target orientation.
|
|
glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentOrientation) * target.getRotation();
|
|
|
|
// enforce tip's constraint
|
|
RotationConstraint* constraint = getConstraint(tipIndex);
|
|
if (constraint) {
|
|
bool constrained = constraint->apply(tipRelativeRotation);
|
|
if (constrained) {
|
|
// The tip's final parent-relative rotation would violate its constraint
|
|
// so we try to pre-twist this pivot to compensate.
|
|
glm::quat constrainedTipRotation = deltaRotation * tipParentOrientation * tipRelativeRotation;
|
|
glm::quat missingRotation = target.getRotation() * glm::inverse(constrainedTipRotation);
|
|
glm::quat swingPart;
|
|
glm::quat twistPart;
|
|
glm::vec3 axis = glm::normalize(deltaRotation * leverArm);
|
|
swingTwistDecomposition(missingRotation, axis, swingPart, twistPart);
|
|
float dotSign = copysignf(1.0f, twistPart.w);
|
|
deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * twistPart, FRACTION)) * deltaRotation;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} else if (targetType == IKTarget::Type::HmdHead) {
|
|
// 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(tipOrientation);
|
|
float dotSign = copysignf(1.0f, deltaRotation.w);
|
|
const float ANGLE_DISTRIBUTION_FACTOR = 0.45f;
|
|
deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR));
|
|
}
|
|
|
|
// compute joint's new parent-relative rotation after swing
|
|
// Q' = dQ * Q and Q = Qp * q --> q' = Qp^ * dQ * Q
|
|
glm::quat newRot = glm::normalize(glm::inverse(
|
|
absolutePoses[pivotsParentIndex].rot) *
|
|
deltaRotation *
|
|
absolutePoses[pivotIndex].rot);
|
|
|
|
// enforce pivot's constraint
|
|
RotationConstraint* constraint = getConstraint(pivotIndex);
|
|
if (constraint) {
|
|
bool constrained = constraint->apply(newRot);
|
|
if (constrained) {
|
|
// the constraint will modify the local rotation of the tip so we must
|
|
// compute the corresponding model-frame deltaRotation
|
|
// Q' = Qp^ * dQ * Q --> dQ = Qp * Q' * Q^
|
|
deltaRotation = absolutePoses[pivotsParentIndex].rot * newRot * glm::inverse(absolutePoses[pivotIndex].rot);
|
|
}
|
|
}
|
|
|
|
// store the relative rotation change in the accumulator
|
|
_accumulators[pivotIndex].add(newRot, target.getWeight());
|
|
|
|
// this joint has been changed so we check to see if it has the lowest index
|
|
if (pivotIndex < lowestMovedIndex) {
|
|
lowestMovedIndex = pivotIndex;
|
|
}
|
|
|
|
// keep track of tip's new transform as we descend towards root
|
|
tipPosition = jointPosition + deltaRotation * (tipPosition - jointPosition);
|
|
tipOrientation = glm::normalize(deltaRotation * tipOrientation);
|
|
tipParentOrientation = glm::normalize(deltaRotation * tipParentOrientation);
|
|
|
|
pivotIndex = pivotsParentIndex;
|
|
pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
|
|
}
|
|
return lowestMovedIndex;
|
|
}
|
|
|
|
//virtual
|
|
const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) {
|
|
// don't call this function, call overlay() instead
|
|
assert(false);
|
|
return _relativePoses;
|
|
}
|
|
|
|
//virtual
|
|
const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
|
|
|
|
const float MAX_OVERLAY_DT = 1.0f / 30.0f; // what to clamp delta-time to in AnimInverseKinematics::overlay
|
|
if (dt > MAX_OVERLAY_DT) {
|
|
dt = MAX_OVERLAY_DT;
|
|
}
|
|
|
|
if (_relativePoses.size() != underPoses.size()) {
|
|
loadPoses(underPoses);
|
|
} else {
|
|
|
|
PROFILE_RANGE_EX("ik/relax", 0xffff00ff, 0);
|
|
|
|
// relax toward underPoses
|
|
// HACK: this relaxation needs to be constant per-frame rather than per-realtime
|
|
// in order to prevent IK "flutter" for bad FPS. The bad news is that the good parts
|
|
// of this relaxation will be FPS dependent (low FPS will make the limbs align slower
|
|
// in real-time), however most people will not notice this and this problem is less
|
|
// annoying than the flutter.
|
|
const float blend = (1.0f / 60.0f) / (0.25f); // effectively: dt / RELAXATION_TIMESCALE
|
|
int numJoints = (int)_relativePoses.size();
|
|
for (int i = 0; i < numJoints; ++i) {
|
|
float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot, underPoses[i].rot));
|
|
if (_accumulators[i].isDirty()) {
|
|
// this joint is affected by IK --> blend toward underPose rotation
|
|
_relativePoses[i].rot = glm::normalize(glm::lerp(_relativePoses[i].rot, dotSign * underPoses[i].rot, blend));
|
|
} else {
|
|
// this joint is NOT affected by IK --> slam to underPose rotation
|
|
_relativePoses[i].rot = underPoses[i].rot;
|
|
}
|
|
_relativePoses[i].trans = underPoses[i].trans;
|
|
}
|
|
|
|
if (!_relativePoses.empty()) {
|
|
// Sometimes the underpose itself can violate the constraints. Rather than
|
|
// clamp the animation we dynamically expand each constraint to accomodate it.
|
|
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
|
|
while (constraintItr != _constraints.end()) {
|
|
int index = constraintItr->first;
|
|
constraintItr->second->dynamicallyAdjustLimits(_relativePoses[index].rot);
|
|
++constraintItr;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!_relativePoses.empty()) {
|
|
|
|
// build a list of targets from _targetVarVec
|
|
std::vector<IKTarget> targets;
|
|
{
|
|
PROFILE_RANGE_EX("ik/computeTargets", 0xffff00ff, 0);
|
|
computeTargets(animVars, targets, underPoses);
|
|
}
|
|
|
|
// debug render ik targets
|
|
if (context.getEnableDebugDrawIKTargets()) {
|
|
const vec4 WHITE(1.0f);
|
|
glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3());
|
|
|
|
for (auto& target : targets) {
|
|
glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation());
|
|
glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat;
|
|
|
|
std::string name = "ikTarget" + std::to_string(target.getIndex());
|
|
DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE);
|
|
}
|
|
} else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) {
|
|
// remove markers if they were added last frame.
|
|
for (auto& target : targets) {
|
|
std::string name = "ikTarget" + std::to_string(target.getIndex());
|
|
DebugDraw::getInstance().removeMyAvatarMarker(name);
|
|
}
|
|
}
|
|
|
|
_previousEnableDebugIKTargets = context.getEnableDebugDrawIKTargets();
|
|
|
|
if (targets.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 {
|
|
|
|
{
|
|
PROFILE_RANGE_EX("ik/shiftHips", 0xffff00ff, 0);
|
|
|
|
// shift hips according to the _hipsOffset from the previous frame
|
|
float offsetLength = glm::length(_hipsOffset);
|
|
const float MIN_HIPS_OFFSET_LENGTH = 0.03f;
|
|
if (offsetLength > MIN_HIPS_OFFSET_LENGTH && _hipsIndex >= 0) {
|
|
// but only if offset is long enough
|
|
float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength);
|
|
if (_hipsParentIndex == -1) {
|
|
// the hips are the root so _hipsOffset is in the correct frame
|
|
_relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans + scaleFactor * _hipsOffset;
|
|
} else {
|
|
// the hips are NOT the root so we need to transform _hipsOffset into hips local-frame
|
|
glm::quat hipsFrameRotation = _relativePoses[_hipsParentIndex].rot;
|
|
int index = _skeleton->getParentIndex(_hipsParentIndex);
|
|
while (index != -1) {
|
|
hipsFrameRotation *= _relativePoses[index].rot;
|
|
index = _skeleton->getParentIndex(index);
|
|
}
|
|
_relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans
|
|
+ glm::inverse(glm::normalize(hipsFrameRotation)) * (scaleFactor * _hipsOffset);
|
|
}
|
|
}
|
|
}
|
|
|
|
{
|
|
PROFILE_RANGE_EX("ik/ccd", 0xffff00ff, 0);
|
|
solveWithCyclicCoordinateDescent(targets);
|
|
}
|
|
|
|
{
|
|
PROFILE_RANGE_EX("ik/measureHipsOffset", 0xffff00ff, 0);
|
|
|
|
// measure new _hipsOffset for next frame
|
|
// by looking for discrepancies between where a targeted endEffector is
|
|
// and where it wants to be (after IK solutions are done)
|
|
|
|
// OUTOFBODY_HACK:use weighted average between HMD and other targets
|
|
float HMD_WEIGHT = 10.0f;
|
|
float OTHER_WEIGHT = 1.0f;
|
|
float totalWeight = 0.0f;
|
|
|
|
glm::vec3 newHipsOffset = Vectors::ZERO;
|
|
for (auto& target: targets) {
|
|
int targetIndex = target.getIndex();
|
|
if (targetIndex == _headIndex && _headIndex != -1) {
|
|
// special handling for headTarget
|
|
if (target.getType() == IKTarget::Type::RotationOnly) {
|
|
// we want to shift the hips to bring the underPose closer
|
|
// to where the head happens to be (overpose)
|
|
glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans;
|
|
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
|
|
const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
|
|
newHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (actual - under);
|
|
totalWeight += OTHER_WEIGHT;
|
|
} else if (target.getType() == IKTarget::Type::HmdHead) {
|
|
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
|
|
glm::vec3 thisOffset = target.getTranslation() - actual;
|
|
glm::vec3 futureHipsOffset = _hipsOffset + thisOffset;
|
|
if (glm::length(futureHipsOffset) < _maxHipsOffsetLength) {
|
|
// it is imperative to shift the hips and bring the head to its designated position
|
|
// so we slam newHipsOffset here and ignore all other targets
|
|
newHipsOffset = futureHipsOffset;
|
|
totalWeight = 0.0f;
|
|
break;
|
|
} else {
|
|
newHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual);
|
|
totalWeight += HMD_WEIGHT;
|
|
}
|
|
}
|
|
} else if (target.getType() == IKTarget::Type::RotationAndPosition) {
|
|
glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans;
|
|
glm::vec3 targetPosition = target.getTranslation();
|
|
newHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition);
|
|
totalWeight += OTHER_WEIGHT;
|
|
}
|
|
}
|
|
if (totalWeight > 1.0f) {
|
|
newHipsOffset /= totalWeight;
|
|
}
|
|
|
|
// smooth transitions by relaxing _hipsOffset toward the new value
|
|
const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f;
|
|
float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
|
|
float newOffsetLength = glm::length(newHipsOffset);
|
|
if (newOffsetLength > _maxHipsOffsetLength) {
|
|
// clamp the hips offset
|
|
newHipsOffset *= _maxHipsOffsetLength / newOffsetLength;
|
|
}
|
|
_hipsOffset += (newHipsOffset - _hipsOffset) * tau;
|
|
}
|
|
}
|
|
}
|
|
return _relativePoses;
|
|
}
|
|
|
|
void AnimInverseKinematics::clearIKJointLimitHistory() {
|
|
for (auto& pair : _constraints) {
|
|
pair.second->clearHistory();
|
|
}
|
|
}
|
|
|
|
void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) {
|
|
// OUTOFBODY_HACK: manually adjust scale here
|
|
const float METERS_TO_CENTIMETERS = 100.0f;
|
|
_maxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength;
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
void AnimInverseKinematics::initConstraints() {
|
|
if (!_skeleton) {
|
|
return;
|
|
}
|
|
// We create constraints for the joints shown here
|
|
// (and their Left counterparts if applicable).
|
|
//
|
|
//
|
|
// O RightHand
|
|
// Head /
|
|
// O /
|
|
// Neck| O RightForeArm
|
|
// O /
|
|
// O | O / RightShoulder
|
|
// O-------O-------O' \|/ 'O
|
|
// Spine2 O RightArm
|
|
// |
|
|
// |
|
|
// Spine1 O
|
|
// |
|
|
// |
|
|
// Spine O
|
|
// y |
|
|
// | |
|
|
// | O---O---O RightUpLeg
|
|
// z | | Hips2 |
|
|
// \ | | |
|
|
// \| | |
|
|
// x -----+ O O RightLeg
|
|
// | |
|
|
// | |
|
|
// | |
|
|
// O O RightFoot
|
|
// / /
|
|
// O--O O--O
|
|
|
|
loadDefaultPoses(_skeleton->getRelativeBindPoses());
|
|
|
|
// compute corresponding absolute poses
|
|
int numJoints = (int)_defaultRelativePoses.size();
|
|
AnimPoseVec absolutePoses;
|
|
absolutePoses.resize(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];
|
|
}
|
|
}
|
|
|
|
clearConstraints();
|
|
for (int i = 0; i < numJoints; ++i) {
|
|
// compute the joint's baseName and remember whether its prefix was "Left" or not
|
|
QString baseName = _skeleton->getJointName(i);
|
|
bool isLeft = baseName.startsWith("Left", Qt::CaseSensitive);
|
|
float mirror = isLeft ? -1.0f : 1.0f;
|
|
if (isLeft) {
|
|
baseName.remove(0, 4);
|
|
} else if (baseName.startsWith("Right", Qt::CaseSensitive)) {
|
|
baseName.remove(0, 5);
|
|
}
|
|
|
|
RotationConstraint* constraint = nullptr;
|
|
if (0 == baseName.compare("Arm", Qt::CaseSensitive)) {
|
|
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
|
|
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
|
|
stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f);
|
|
|
|
/* KEEP THIS CODE for future experimentation
|
|
// 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);
|
|
*/
|
|
|
|
// simple cone
|
|
std::vector<float> minDots;
|
|
const float MAX_HAND_SWING = PI / 2.0f;
|
|
minDots.push_back(cosf(MAX_HAND_SWING));
|
|
stConstraint->setSwingLimits(minDots);
|
|
|
|
constraint = static_cast<RotationConstraint*>(stConstraint);
|
|
} else if (0 == baseName.compare("UpLeg", Qt::CaseSensitive)) {
|
|
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
|
|
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
|
|
stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
|
|
|
|
std::vector<glm::vec3> swungDirections;
|
|
float deltaTheta = PI / 4.0f;
|
|
float theta = 0.0f;
|
|
swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.25f, sinf(theta)));
|
|
theta += deltaTheta;
|
|
swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.0f, sinf(theta)));
|
|
theta += deltaTheta;
|
|
swungDirections.push_back(glm::vec3(mirror * cosf(theta), -0.25f, sinf(theta))); // posterior
|
|
theta += deltaTheta;
|
|
swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.0f, sinf(theta)));
|
|
theta += deltaTheta;
|
|
swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.25f, sinf(theta)));
|
|
theta += deltaTheta;
|
|
swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.5f, sinf(theta)));
|
|
theta += deltaTheta;
|
|
swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.5f, sinf(theta))); // anterior
|
|
theta += deltaTheta;
|
|
swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.5f, sinf(theta)));
|
|
|
|
// 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 == baseName.compare("Hand", Qt::CaseSensitive)) {
|
|
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
|
|
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
|
|
stConstraint->setTwistLimits(0.0f, 0.0f); // max == min, disables twist limits
|
|
|
|
/* KEEP THIS CODE for future experimentation -- twist limits for hands
|
|
const float MAX_HAND_TWIST = 3.0f * PI / 5.0f;
|
|
const float MIN_HAND_TWIST = -PI / 2.0f;
|
|
if (isLeft) {
|
|
stConstraint->setTwistLimits(-MAX_HAND_TWIST, -MIN_HAND_TWIST);
|
|
} else {
|
|
stConstraint->setTwistLimits(MIN_HAND_TWIST, MAX_HAND_TWIST);
|
|
}
|
|
*/
|
|
|
|
/* KEEP THIS CODE for future experimentation -- non-symmetrical swing limits for wrist
|
|
* a more complicated wrist with asymmetric cone
|
|
// 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);
|
|
*/
|
|
|
|
// simple cone
|
|
std::vector<float> minDots;
|
|
const float MAX_HAND_SWING = PI / 2.0f;
|
|
minDots.push_back(cosf(MAX_HAND_SWING));
|
|
stConstraint->setSwingLimits(minDots);
|
|
|
|
constraint = static_cast<RotationConstraint*>(stConstraint);
|
|
} else if (baseName.startsWith("Shoulder", Qt::CaseSensitive)) {
|
|
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
|
|
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
|
|
const float MAX_SHOULDER_TWIST = PI / 20.0f;
|
|
stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST);
|
|
|
|
std::vector<float> minDots;
|
|
const float MAX_SHOULDER_SWING = PI / 20.0f;
|
|
minDots.push_back(cosf(MAX_SHOULDER_SWING));
|
|
stConstraint->setSwingLimits(minDots);
|
|
|
|
constraint = static_cast<RotationConstraint*>(stConstraint);
|
|
} else if (baseName.startsWith("Spine", Qt::CaseSensitive)) {
|
|
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
|
|
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
|
|
const float MAX_SPINE_TWIST = PI / 12.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);
|
|
if (0 == baseName.compare("Spine1", Qt::CaseSensitive)
|
|
|| 0 == baseName.compare("Spine", Qt::CaseSensitive)) {
|
|
stConstraint->setLowerSpine(true);
|
|
}
|
|
|
|
constraint = static_cast<RotationConstraint*>(stConstraint);
|
|
} else if (baseName.startsWith("Hips2", Qt::CaseSensitive)) {
|
|
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
|
|
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
|
|
const float MAX_SPINE_TWIST = PI / 8.0f;
|
|
stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST);
|
|
|
|
std::vector<float> minDots;
|
|
const float MAX_SPINE_SWING = PI / 14.0f;
|
|
minDots.push_back(cosf(MAX_SPINE_SWING));
|
|
stConstraint->setSwingLimits(minDots);
|
|
|
|
constraint = static_cast<RotationConstraint*>(stConstraint);
|
|
} else if (0 == baseName.compare("Neck", Qt::CaseSensitive)) {
|
|
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
|
|
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
|
|
const float MAX_NECK_TWIST = PI / 9.0f;
|
|
stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST);
|
|
|
|
std::vector<float> minDots;
|
|
const float MAX_NECK_SWING = PI / 8.0f;
|
|
minDots.push_back(cosf(MAX_NECK_SWING));
|
|
stConstraint->setSwingLimits(minDots);
|
|
|
|
constraint = static_cast<RotationConstraint*>(stConstraint);
|
|
} else if (0 == baseName.compare("Head", Qt::CaseSensitive)) {
|
|
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
|
|
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
|
|
const float MAX_HEAD_TWIST = PI / 9.0f;
|
|
stConstraint->setTwistLimits(-MAX_HEAD_TWIST, MAX_HEAD_TWIST);
|
|
|
|
std::vector<float> minDots;
|
|
const float MAX_HEAD_SWING = PI / 10.0f;
|
|
minDots.push_back(cosf(MAX_HEAD_SWING));
|
|
stConstraint->setSwingLimits(minDots);
|
|
|
|
constraint = static_cast<RotationConstraint*>(stConstraint);
|
|
} else if (0 == baseName.compare("ForeArm", Qt::CaseSensitive)) {
|
|
// 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 * Vectors::UNIT_Z;
|
|
const float MIN_ELBOW_ANGLE = 0.05f;
|
|
const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f;
|
|
glm::quat invReferenceRotation = glm::inverse(referenceRotation);
|
|
glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y;
|
|
glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y;
|
|
|
|
// for the rest of the math we rotate hingeAxis into the child frame
|
|
hingeAxis = referenceRotation * hingeAxis;
|
|
eConstraint->setHingeAxis(hingeAxis);
|
|
|
|
glm::vec3 projectedYAxis = glm::normalize(Vectors::UNIT_Y - glm::dot(Vectors::UNIT_Y, hingeAxis) * hingeAxis);
|
|
float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis));
|
|
if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) {
|
|
minAngle = - minAngle;
|
|
}
|
|
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 == baseName.compare("Leg", Qt::CaseSensitive)) {
|
|
// 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 * Vectors::UNIT_X;
|
|
|
|
// we determine the max/min angles by rotating the swing limit lines from parent- to child-frame
|
|
// then measure the angles to swing the yAxis into alignment
|
|
const float MIN_KNEE_ANGLE = 0.0f;
|
|
const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f;
|
|
glm::quat invReferenceRotation = glm::inverse(referenceRotation);
|
|
glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y;
|
|
glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y;
|
|
|
|
// for the rest of the math we rotate hingeAxis into the child frame
|
|
hingeAxis = referenceRotation * hingeAxis;
|
|
eConstraint->setHingeAxis(hingeAxis);
|
|
|
|
glm::vec3 projectedYAxis = glm::normalize(Vectors::UNIT_Y - glm::dot(Vectors::UNIT_Y, hingeAxis) * hingeAxis);
|
|
float minAngle = acosf(glm::dot(projectedYAxis, minSwingAxis));
|
|
if (glm::dot(hingeAxis, glm::cross(projectedYAxis, minSwingAxis)) < 0.0f) {
|
|
minAngle = - minAngle;
|
|
}
|
|
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 == baseName.compare("Foot", Qt::CaseSensitive)) {
|
|
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(Vectors::UNIT_Y);
|
|
swungDirections.push_back(Vectors::UNIT_X);
|
|
swungDirections.push_back(glm::vec3(1.0f, 1.0f, 1.0f));
|
|
swungDirections.push_back(glm::vec3(1.0f, 1.0f, -1.0f));
|
|
|
|
// 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);
|
|
|
|
// invalidate all targetVars
|
|
for (auto& targetVar: _targetVarVec) {
|
|
targetVar.jointIndex = -1;
|
|
}
|
|
|
|
_maxTargetIndex = -1;
|
|
|
|
for (auto& accumulator: _accumulators) {
|
|
accumulator.clearAndClean();
|
|
}
|
|
|
|
if (skeleton) {
|
|
initConstraints();
|
|
_headIndex = _skeleton->nameToJointIndex("Head");
|
|
_hipsIndex = _skeleton->nameToJointIndex("Hips");
|
|
|
|
// also cache the _hipsParentIndex for later
|
|
if (_hipsIndex >= 0) {
|
|
_hipsParentIndex = _skeleton->getParentIndex(_hipsIndex);
|
|
} else {
|
|
_hipsParentIndex = -1;
|
|
}
|
|
} else {
|
|
clearConstraints();
|
|
_headIndex = -1;
|
|
_hipsIndex = -1;
|
|
_hipsParentIndex = -1;
|
|
}
|
|
}
|