Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-25 20:16:16 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #13713 from hyperlogic/bug-fix/leg-ik-bug-fixes-master

Browse source 
Leg IK fixes
This commit is contained in:
John Conklin II 2018-08-01 11:17:10 -07:00 committed by GitHub
commit c67ef57ad3
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
34 changed files with 3518 additions and 1230 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2265
interface/resources/avatar/avatar-animation.json
View file

File diff suppressed because it is too large Load diff

1228
interface/resources/avatar/old-avatar-animation.json Normal file
View file

File diff suppressed because it is too large Load diff

7
libraries/animation/src/AnimBlendLinear.cpp
View file

@ -24,7 +24,7 @@ AnimBlendLinear::~AnimBlendLinear() {
} }
const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
_alpha = animVars.lookup(_alphaVar, _alpha); _alpha = animVars.lookup(_alphaVar, _alpha);
@ -43,6 +43,9 @@ const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, con
evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, dt); evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, dt);
} }
processOutputJoints(triggersOut);
return _poses; return _poses;
} }
@ -51,7 +54,7 @@ const AnimPoseVec& AnimBlendLinear::getPosesInternal() const {
return _poses; return _poses;
} }
void AnimBlendLinear::evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha, void AnimBlendLinear::evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, AnimVariantMap& triggersOut, float alpha,
size_t prevPoseIndex, size_t nextPoseIndex, float dt) { size_t prevPoseIndex, size_t nextPoseIndex, float dt) {
if (prevPoseIndex == nextPoseIndex) { if (prevPoseIndex == nextPoseIndex) {
// this can happen if alpha is on an integer boundary // this can happen if alpha is on an integer boundary

4
libraries/animation/src/AnimBlendLinear.h
View file

@ -30,7 +30,7 @@ public:
AnimBlendLinear(const QString& id, float alpha); AnimBlendLinear(const QString& id, float alpha);
virtual ~AnimBlendLinear() override; virtual ~AnimBlendLinear() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; } void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; }
@ -38,7 +38,7 @@ protected:
// for AnimDebugDraw rendering // for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override; virtual const AnimPoseVec& getPosesInternal() const override;
void evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha, void evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, AnimVariantMap& triggersOut, float alpha,
size_t prevPoseIndex, size_t nextPoseIndex, float dt); size_t prevPoseIndex, size_t nextPoseIndex, float dt);
AnimPoseVec _poses; AnimPoseVec _poses;

11
libraries/animation/src/AnimBlendLinearMove.cpp
View file

@ -26,7 +26,7 @@ AnimBlendLinearMove::~AnimBlendLinearMove() {
} }
const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
assert(_children.size() == _characteristicSpeeds.size()); assert(_children.size() == _characteristicSpeeds.size());
@ -54,6 +54,9 @@ const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars,
setFrameAndPhase(dt, alpha, prevPoseIndex, nextPoseIndex, &prevDeltaTime, &nextDeltaTime, triggersOut); setFrameAndPhase(dt, alpha, prevPoseIndex, nextPoseIndex, &prevDeltaTime, &nextDeltaTime, triggersOut);
evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime); evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime);
} }
processOutputJoints(triggersOut);
return _poses; return _poses;
} }
@ -62,7 +65,7 @@ const AnimPoseVec& AnimBlendLinearMove::getPosesInternal() const {
return _poses; return _poses;
} }
void AnimBlendLinearMove::evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha, void AnimBlendLinearMove::evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, AnimVariantMap& triggersOut, float alpha,
size_t prevPoseIndex, size_t nextPoseIndex, size_t prevPoseIndex, size_t nextPoseIndex,
float prevDeltaTime, float nextDeltaTime) { float prevDeltaTime, float nextDeltaTime) {
if (prevPoseIndex == nextPoseIndex) { if (prevPoseIndex == nextPoseIndex) {
@ -82,7 +85,7 @@ void AnimBlendLinearMove::evaluateAndBlendChildren(const AnimVariantMap& animVar
} }
void AnimBlendLinearMove::setFrameAndPhase(float dt, float alpha, int prevPoseIndex, int nextPoseIndex, void AnimBlendLinearMove::setFrameAndPhase(float dt, float alpha, int prevPoseIndex, int nextPoseIndex,
float* prevDeltaTimeOut, float* nextDeltaTimeOut, Triggers& triggersOut) { float* prevDeltaTimeOut, float* nextDeltaTimeOut, AnimVariantMap& triggersOut) {
const float FRAMES_PER_SECOND = 30.0f; const float FRAMES_PER_SECOND = 30.0f;
auto prevClipNode = std::dynamic_pointer_cast<AnimClip>(_children[prevPoseIndex]); auto prevClipNode = std::dynamic_pointer_cast<AnimClip>(_children[prevPoseIndex]);
@ -109,7 +112,7 @@ void AnimBlendLinearMove::setFrameAndPhase(float dt, float alpha, int prevPoseIn
// detect loop trigger events // detect loop trigger events
if (_phase >= 1.0f) { if (_phase >= 1.0f) {
triggersOut.push_back(_id + "Loop"); triggersOut.setTrigger(_id + "Loop");
_phase = glm::fract(_phase); _phase = glm::fract(_phase);
} }

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

@ -39,7 +39,7 @@ public:
AnimBlendLinearMove(const QString& id, float alpha, float desiredSpeed, const std::vector<float>& characteristicSpeeds); AnimBlendLinearMove(const QString& id, float alpha, float desiredSpeed, const std::vector<float>& characteristicSpeeds);
virtual ~AnimBlendLinearMove() override; virtual ~AnimBlendLinearMove() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; } void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; }
void setDesiredSpeedVar(const QString& desiredSpeedVar) { _desiredSpeedVar = desiredSpeedVar; } void setDesiredSpeedVar(const QString& desiredSpeedVar) { _desiredSpeedVar = desiredSpeedVar; }
@ -48,12 +48,12 @@ protected:
// for AnimDebugDraw rendering // for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override; virtual const AnimPoseVec& getPosesInternal() const override;
void evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha, void evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, AnimVariantMap& triggersOut, float alpha,
size_t prevPoseIndex, size_t nextPoseIndex, size_t prevPoseIndex, size_t nextPoseIndex,
float prevDeltaTime, float nextDeltaTime); float prevDeltaTime, float nextDeltaTime);
void setFrameAndPhase(float dt, float alpha, int prevPoseIndex, int nextPoseIndex, void setFrameAndPhase(float dt, float alpha, int prevPoseIndex, int nextPoseIndex,
float* prevDeltaTimeOut, float* nextDeltaTimeOut, Triggers& triggersOut); float* prevDeltaTimeOut, float* nextDeltaTimeOut, AnimVariantMap& triggersOut);
virtual void setCurrentFrameInternal(float frame) override; virtual void setCurrentFrameInternal(float frame) override;

159
libraries/animation/src/AnimChain.h Normal file
View file

@ -0,0 +1,159 @@
//
// AnimChain.h
//
// Created by Anthony J. Thibault on 7/16/2018.
// Copyright (c) 2018 High Fidelity, Inc. All rights reserved.
//
// 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_AnimChain
#define hifi_AnimChain
#include <vector>
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <DebugDraw.h>
template <int N>
class AnimChainT {
public:
AnimChainT() {}
AnimChainT(const AnimChainT& orig) {
_top = orig._top;
for (int i = 0; i < _top; i++) {
_chain[i] = orig._chain[i];
}
}
AnimChainT& operator=(const AnimChainT& orig) {
_top = orig._top;
for (int i = 0; i < _top; i++) {
_chain[i] = orig._chain[i];
}
return *this;
}
bool buildFromRelativePoses(const AnimSkeleton::ConstPointer& skeleton, const AnimPoseVec& relativePoses, int tipIndex) {
_top = 0;
// iterate through the skeleton parents, from the tip to the base, copying over relativePoses into the chain.
for (int jointIndex = tipIndex; jointIndex != -1; jointIndex = skeleton->getParentIndex(jointIndex)) {
if (_top >= N) {
assert(_top < N);
// stack overflow
return false;
}
_chain[_top].relativePose = relativePoses[jointIndex];
_chain[_top].jointIndex = jointIndex;
_chain[_top].dirty = true;
_top++;
}
buildDirtyAbsolutePoses();
return true;
}
const AnimPose& getAbsolutePoseFromJointIndex(int jointIndex) const {
for (int i = 0; i < _top; i++) {
if (_chain[i].jointIndex == jointIndex) {
return _chain[i].absolutePose;
}
}
return AnimPose::identity;
}
bool setRelativePoseAtJointIndex(int jointIndex, const AnimPose& relativePose) {
bool foundIndex = false;
for (int i = _top - 1; i >= 0; i--) {
if (_chain[i].jointIndex == jointIndex) {
_chain[i].relativePose = relativePose;
foundIndex = true;
}
// all child absolute poses are now dirty
if (foundIndex) {
_chain[i].dirty = true;
}
}
return foundIndex;
}
void buildDirtyAbsolutePoses() {
// the relative and absolute pose is the same for the base of the chain.
_chain[_top - 1].absolutePose = _chain[_top - 1].relativePose;
_chain[_top - 1].dirty = false;
// iterate chain from base to tip, concatinating the relative poses to build the absolute poses.
for (int i = _top - 1; i > 0; i--) {
AnimChainElem& parent = _chain[i];
AnimChainElem& child = _chain[i - 1];
if (child.dirty) {
child.absolutePose = parent.absolutePose * child.relativePose;
child.dirty = false;
}
}
}
void blend(const AnimChainT& srcChain, float alpha) {
// make sure chains have same lengths
assert(srcChain._top == _top);
if (srcChain._top != _top) {
return;
}
// only blend the relative poses
for (int i = 0; i < _top; i++) {
_chain[i].relativePose.blend(srcChain._chain[i].relativePose, alpha);
_chain[i].dirty = true;
}
}
int size() const {
return _top;
}
void outputRelativePoses(AnimPoseVec& relativePoses) {
for (int i = 0; i < _top; i++) {
relativePoses[_chain[i].jointIndex] = _chain[i].relativePose;
}
}
void debugDraw(const glm::mat4& geomToWorldMat, const glm::vec4& color) const {
for (int i = 1; i < _top; i++) {
glm::vec3 start = transformPoint(geomToWorldMat, _chain[i - 1].absolutePose.trans());
glm::vec3 end = transformPoint(geomToWorldMat, _chain[i].absolutePose.trans());
DebugDraw::getInstance().drawRay(start, end, color);
}
}
void dump() const {
for (int i = 0; i < _top; i++) {
qWarning() << "AJT: AnimPoseElem[" << i << "]";
qWarning() << "AJT: relPose =" << _chain[i].relativePose;
qWarning() << "AJT: absPose =" << _chain[i].absolutePose;
qWarning() << "AJT: jointIndex =" << _chain[i].jointIndex;
qWarning() << "AJT: dirty =" << _chain[i].dirty;
}
}
protected:
struct AnimChainElem {
AnimPose relativePose;
AnimPose absolutePose;
int jointIndex { -1 };
bool dirty { true };
};
AnimChainElem _chain[N];
int _top { 0 };
};
using AnimChain = AnimChainT<10>;
#endif

6
libraries/animation/src/AnimClip.cpp
View file

@ -30,7 +30,7 @@ AnimClip::~AnimClip() {
} }
const AnimPoseVec& AnimClip::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { const AnimPoseVec& AnimClip::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
// lookup parameters from animVars, using current instance variables as defaults. // lookup parameters from animVars, using current instance variables as defaults.
_startFrame = animVars.lookup(_startFrameVar, _startFrame); _startFrame = animVars.lookup(_startFrameVar, _startFrame);
@ -77,6 +77,8 @@ const AnimPoseVec& AnimClip::evaluate(const AnimVariantMap& animVars, const Anim
::blend(_poses.size(), &prevFrame[0], &nextFrame[0], alpha, &_poses[0]); ::blend(_poses.size(), &prevFrame[0], &nextFrame[0], alpha, &_poses[0]);
} }
processOutputJoints(triggersOut);
return _poses; return _poses;
} }
@ -89,7 +91,7 @@ void AnimClip::loadURL(const QString& url) {
void AnimClip::setCurrentFrameInternal(float frame) { void AnimClip::setCurrentFrameInternal(float frame) {
// because dt is 0, we should not encounter any triggers // because dt is 0, we should not encounter any triggers
const float dt = 0.0f; const float dt = 0.0f;
Triggers triggers; AnimVariantMap triggers;
_frame = ::accumulateTime(_startFrame, _endFrame, _timeScale, frame + _startFrame, dt, _loopFlag, _id, triggers); _frame = ::accumulateTime(_startFrame, _endFrame, _timeScale, frame + _startFrame, dt, _loopFlag, _id, triggers);
} }

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

@ -28,7 +28,7 @@ public:
AnimClip(const QString& id, const QString& url, float startFrame, float endFrame, float timeScale, bool loopFlag, bool mirrorFlag); AnimClip(const QString& id, const QString& url, float startFrame, float endFrame, float timeScale, bool loopFlag, bool mirrorFlag);
virtual ~AnimClip() override; virtual ~AnimClip() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
void setStartFrameVar(const QString& startFrameVar) { _startFrameVar = startFrameVar; } void setStartFrameVar(const QString& startFrameVar) { _startFrameVar = startFrameVar; }
void setEndFrameVar(const QString& endFrameVar) { _endFrameVar = endFrameVar; } void setEndFrameVar(const QString& endFrameVar) { _endFrameVar = endFrameVar; }

5
libraries/animation/src/AnimDefaultPose.cpp
View file

@ -20,12 +20,15 @@ AnimDefaultPose::~AnimDefaultPose() {
} }
const AnimPoseVec& AnimDefaultPose::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { const AnimPoseVec& AnimDefaultPose::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
if (_skeleton) { if (_skeleton) {
_poses = _skeleton->getRelativeDefaultPoses(); _poses = _skeleton->getRelativeDefaultPoses();
} else { } else {
_poses.clear(); _poses.clear();
} }
processOutputJoints(triggersOut);
return _poses; return _poses;
} }

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

@ -21,7 +21,7 @@ public:
AnimDefaultPose(const QString& id); AnimDefaultPose(const QString& id);
virtual ~AnimDefaultPose() override; virtual ~AnimDefaultPose() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
protected: protected:
// for AnimDebugDraw rendering // for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override; virtual const AnimPoseVec& getPosesInternal() const override;

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

@ -259,14 +259,6 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector<
jointChainInfoVec[i].jointInfoVec[j].rot = safeMix(_prevJointChainInfoVec[i].jointInfoVec[j].rot, jointChainInfoVec[i].jointInfoVec[j].rot, alpha); jointChainInfoVec[i].jointInfoVec[j].rot = safeMix(_prevJointChainInfoVec[i].jointInfoVec[j].rot, jointChainInfoVec[i].jointInfoVec[j].rot, alpha);
jointChainInfoVec[i].jointInfoVec[j].trans = lerp(_prevJointChainInfoVec[i].jointInfoVec[j].trans, jointChainInfoVec[i].jointInfoVec[j].trans, alpha); jointChainInfoVec[i].jointInfoVec[j].trans = lerp(_prevJointChainInfoVec[i].jointInfoVec[j].trans, jointChainInfoVec[i].jointInfoVec[j].trans, alpha);
} }
// if joint chain was just disabled, ramp the weight toward zero.
if (_prevJointChainInfoVec[i].target.getType() != IKTarget::Type::Unknown &&
jointChainInfoVec[i].target.getType() == IKTarget::Type::Unknown) {
IKTarget newTarget = _prevJointChainInfoVec[i].target;
newTarget.setWeight((1.0f - alpha) * _prevJointChainInfoVec[i].target.getWeight());
jointChainInfoVec[i].target = newTarget;
}
} }
} }
} }
@ -874,14 +866,14 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
} }
//virtual //virtual
const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) { const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
// don't call this function, call overlay() instead // don't call this function, call overlay() instead
assert(false); assert(false);
return _relativePoses; return _relativePoses;
} }
//virtual //virtual
const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut, const AnimPoseVec& underPoses) {
#ifdef Q_OS_ANDROID #ifdef Q_OS_ANDROID
// disable IK on android // disable IK on android
return underPoses; return underPoses;
@ -961,6 +953,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0); PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0);
if (_hipsTargetIndex >= 0) { if (_hipsTargetIndex >= 0) {
assert(_hipsTargetIndex < (int)targets.size()); assert(_hipsTargetIndex < (int)targets.size());
// slam the hips to match the _hipsTarget // slam the hips to match the _hipsTarget
@ -1045,6 +1038,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0);
setSecondaryTargets(context); setSecondaryTargets(context);
preconditionRelativePosesToAvoidLimbLock(context, targets); preconditionRelativePosesToAvoidLimbLock(context, targets);
solve(context, targets, dt, jointChainInfoVec); solve(context, targets, dt, jointChainInfoVec);
@ -1056,6 +1050,8 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
} }
} }
processOutputJoints(triggersOut);
return _relativePoses; return _relativePoses;
} }
@ -1750,7 +1746,7 @@ void AnimInverseKinematics::preconditionRelativePosesToAvoidLimbLock(const AnimC
const float MIN_AXIS_LENGTH = 1.0e-4f; const float MIN_AXIS_LENGTH = 1.0e-4f;
for (auto& target : targets) { for (auto& target : targets) {
if (target.getIndex() != -1) { if (target.getIndex() != -1 && target.getType() == IKTarget::Type::RotationAndPosition) {
for (int i = 0; i < NUM_LIMBS; i++) { for (int i = 0; i < NUM_LIMBS; i++) {
if (limbs[i].first == target.getIndex()) { if (limbs[i].first == target.getIndex()) {
int tipIndex = limbs[i].first; int tipIndex = limbs[i].first;
@ -1843,6 +1839,10 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s
default: default:
case SolutionSource::RelaxToUnderPoses: case SolutionSource::RelaxToUnderPoses:
blendToPoses(underPoses, underPoses, RELAX_BLEND_FACTOR); blendToPoses(underPoses, underPoses, RELAX_BLEND_FACTOR);
// special case for hips: don't dampen hip motion from underposes
if (_hipsIndex >= 0 && _hipsIndex < (int)_relativePoses.size()) {
_relativePoses[_hipsIndex] = underPoses[_hipsIndex];
}
break; break;
case SolutionSource::RelaxToLimitCenterPoses: case SolutionSource::RelaxToLimitCenterPoses:
blendToPoses(_limitCenterPoses, underPoses, RELAX_BLEND_FACTOR); blendToPoses(_limitCenterPoses, underPoses, RELAX_BLEND_FACTOR);

4
libraries/animation/src/AnimInverseKinematics.h
View file

@ -52,8 +52,8 @@ public:
const QString& typeVar, const QString& weightVar, float weight, const std::vector<float>& flexCoefficients, const QString& typeVar, const QString& weightVar, float weight, const std::vector<float>& flexCoefficients,
const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar); const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar);
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) override; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut, const AnimPoseVec& underPoses) override;
void clearIKJointLimitHistory(); void clearIKJointLimitHistory();

6
libraries/animation/src/AnimManipulator.cpp
View file

@ -32,11 +32,11 @@ AnimManipulator::~AnimManipulator() {
} }
const AnimPoseVec& AnimManipulator::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { const AnimPoseVec& AnimManipulator::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
return overlay(animVars, context, dt, triggersOut, _skeleton->getRelativeDefaultPoses()); return overlay(animVars, context, dt, triggersOut, _skeleton->getRelativeDefaultPoses());
} }
const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut, const AnimPoseVec& underPoses) {
_alpha = animVars.lookup(_alphaVar, _alpha); _alpha = animVars.lookup(_alphaVar, _alpha);
_poses = underPoses; _poses = underPoses;
@ -74,6 +74,8 @@ const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, cons
} }
} }
processOutputJoints(triggersOut);
return _poses; return _poses;
} }

4
libraries/animation/src/AnimManipulator.h
View file

@ -22,8 +22,8 @@ public:
AnimManipulator(const QString& id, float alpha); AnimManipulator(const QString& id, float alpha);
virtual ~AnimManipulator() override; virtual ~AnimManipulator() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut, const AnimPoseVec& underPoses) override;
void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; } void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; }

16
libraries/animation/src/AnimNode.cpp
View file

@ -59,3 +59,19 @@ void AnimNode::setCurrentFrame(float frame) {
child->setCurrentFrameInternal(frame); child->setCurrentFrameInternal(frame);
} }
} }
void AnimNode::processOutputJoints(AnimVariantMap& triggersOut) const {
if (!_skeleton) {
return;
}
for (auto&& jointName : _outputJointNames) {
// TODO: cache the jointIndices
int jointIndex = _skeleton->nameToJointIndex(jointName);
if (jointIndex >= 0) {
AnimPose pose = _skeleton->getAbsolutePose(jointIndex, getPosesInternal());
triggersOut.set(_id + jointName + "Rotation", pose.rot());
triggersOut.set(_id + jointName + "Position", pose.trans());
}
}
}

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

@ -45,11 +45,12 @@ public:
Manipulator, Manipulator,
InverseKinematics, InverseKinematics,
DefaultPose, DefaultPose,
TwoBoneIK,
PoleVectorConstraint,
NumTypes NumTypes
}; };
using Pointer = std::shared_ptr<AnimNode>; using Pointer = std::shared_ptr<AnimNode>;
using ConstPointer = std::shared_ptr<const AnimNode>; using ConstPointer = std::shared_ptr<const AnimNode>;
using Triggers = std::vector<QString>;
friend class AnimDebugDraw; friend class AnimDebugDraw;
friend void buildChildMap(std::map<QString, Pointer>& map, Pointer node); friend void buildChildMap(std::map<QString, Pointer>& map, Pointer node);
@ -61,6 +62,8 @@ public:
const QString& getID() const { return _id; } const QString& getID() const { return _id; }
Type getType() const { return _type; } Type getType() const { return _type; }
void addOutputJoint(const QString& outputJointName) { _outputJointNames.push_back(outputJointName); }
// hierarchy accessors // hierarchy accessors
Pointer getParent(); Pointer getParent();
void addChild(Pointer child); void addChild(Pointer child);
@ -74,8 +77,8 @@ public:
AnimSkeleton::ConstPointer getSkeleton() const { return _skeleton; } AnimSkeleton::ConstPointer getSkeleton() const { return _skeleton; }
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) = 0; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) = 0;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut,
const AnimPoseVec& underPoses) { const AnimPoseVec& underPoses) {
return evaluate(animVars, context, dt, triggersOut); return evaluate(animVars, context, dt, triggersOut);
} }
@ -114,11 +117,14 @@ protected:
// for AnimDebugDraw rendering // for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const = 0; virtual const AnimPoseVec& getPosesInternal() const = 0;
void processOutputJoints(AnimVariantMap& triggersOut) const;
Type _type; Type _type;
QString _id; QString _id;
std::vector<AnimNode::Pointer> _children; std::vector<AnimNode::Pointer> _children;
AnimSkeleton::ConstPointer _skeleton; AnimSkeleton::ConstPointer _skeleton;
std::weak_ptr<AnimNode> _parent; std::weak_ptr<AnimNode> _parent;
std::vector<QString> _outputJointNames;
// no copies // no copies
AnimNode(const AnimNode&) = delete; AnimNode(const AnimNode&) = delete;

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

@ -25,6 +25,8 @@
#include "AnimManipulator.h" #include "AnimManipulator.h"
#include "AnimInverseKinematics.h" #include "AnimInverseKinematics.h"
#include "AnimDefaultPose.h" #include "AnimDefaultPose.h"
#include "AnimTwoBoneIK.h"
#include "AnimPoleVectorConstraint.h"
using NodeLoaderFunc = AnimNode::Pointer (*)(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl); using NodeLoaderFunc = AnimNode::Pointer (*)(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
using NodeProcessFunc = bool (*)(AnimNode::Pointer node, const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl); using NodeProcessFunc = bool (*)(AnimNode::Pointer node, const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
@ -38,6 +40,8 @@ static AnimNode::Pointer loadStateMachineNode(const QJsonObject& jsonObj, const
static AnimNode::Pointer loadManipulatorNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl); static AnimNode::Pointer loadManipulatorNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
static AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl); static AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
static AnimNode::Pointer loadDefaultPoseNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl); static AnimNode::Pointer loadDefaultPoseNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
static AnimNode::Pointer loadTwoBoneIKNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
static AnimNode::Pointer loadPoleVectorConstraintNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl);
static const float ANIM_GRAPH_LOAD_PRIORITY = 10.0f; static const float ANIM_GRAPH_LOAD_PRIORITY = 10.0f;
@ -56,6 +60,8 @@ static const char* animNodeTypeToString(AnimNode::Type type) {
case AnimNode::Type::Manipulator: return "manipulator"; case AnimNode::Type::Manipulator: return "manipulator";
case AnimNode::Type::InverseKinematics: return "inverseKinematics"; case AnimNode::Type::InverseKinematics: return "inverseKinematics";
case AnimNode::Type::DefaultPose: return "defaultPose"; case AnimNode::Type::DefaultPose: return "defaultPose";
case AnimNode::Type::TwoBoneIK: return "twoBoneIK";
case AnimNode::Type::PoleVectorConstraint: return "poleVectorConstraint";
case AnimNode::Type::NumTypes: return nullptr; case AnimNode::Type::NumTypes: return nullptr;
}; };
return nullptr; return nullptr;
@ -116,6 +122,8 @@ static NodeLoaderFunc animNodeTypeToLoaderFunc(AnimNode::Type type) {
case AnimNode::Type::Manipulator: return loadManipulatorNode; case AnimNode::Type::Manipulator: return loadManipulatorNode;
case AnimNode::Type::InverseKinematics: return loadInverseKinematicsNode; case AnimNode::Type::InverseKinematics: return loadInverseKinematicsNode;
case AnimNode::Type::DefaultPose: return loadDefaultPoseNode; case AnimNode::Type::DefaultPose: return loadDefaultPoseNode;
case AnimNode::Type::TwoBoneIK: return loadTwoBoneIKNode;
case AnimNode::Type::PoleVectorConstraint: return loadPoleVectorConstraintNode;
case AnimNode::Type::NumTypes: return nullptr; case AnimNode::Type::NumTypes: return nullptr;
}; };
return nullptr; return nullptr;
@ -131,6 +139,8 @@ static NodeProcessFunc animNodeTypeToProcessFunc(AnimNode::Type type) {
case AnimNode::Type::Manipulator: return processDoNothing; case AnimNode::Type::Manipulator: return processDoNothing;
case AnimNode::Type::InverseKinematics: return processDoNothing; case AnimNode::Type::InverseKinematics: return processDoNothing;
case AnimNode::Type::DefaultPose: return processDoNothing; case AnimNode::Type::DefaultPose: return processDoNothing;
case AnimNode::Type::TwoBoneIK: return processDoNothing;
case AnimNode::Type::PoleVectorConstraint: return processDoNothing;
case AnimNode::Type::NumTypes: return nullptr; case AnimNode::Type::NumTypes: return nullptr;
}; };
return nullptr; return nullptr;
@ -189,6 +199,25 @@ static NodeProcessFunc animNodeTypeToProcessFunc(AnimNode::Type type) {
} \ } \
do {} while (0) do {} while (0)
#define READ_VEC3(NAME, JSON_OBJ, ID, URL, ERROR_RETURN) \
auto NAME##_VAL = JSON_OBJ.value(#NAME); \
if (!NAME##_VAL.isArray()) { \
qCCritical(animation) << "AnimNodeLoader, error reading vector" \
<< #NAME << "id =" << ID \
<< ", url =" << URL.toDisplayString(); \
return ERROR_RETURN; \
} \
QJsonArray NAME##_ARRAY = NAME##_VAL.toArray(); \
if (NAME##_ARRAY.size() != 3) { \
qCCritical(animation) << "AnimNodeLoader, vector size != 3" \
<< #NAME << "id =" << ID \
<< ", url =" << URL.toDisplayString(); \
return ERROR_RETURN; \
} \
glm::vec3 NAME((float)NAME##_ARRAY.at(0).toDouble(), \
(float)NAME##_ARRAY.at(1).toDouble(), \
(float)NAME##_ARRAY.at(2).toDouble())
static AnimNode::Pointer loadNode(const QJsonObject& jsonObj, const QUrl& jsonUrl) { static AnimNode::Pointer loadNode(const QJsonObject& jsonObj, const QUrl& jsonUrl) {
auto idVal = jsonObj.value("id"); auto idVal = jsonObj.value("id");
if (!idVal.isString()) { if (!idVal.isString()) {
@ -216,6 +245,16 @@ static AnimNode::Pointer loadNode(const QJsonObject& jsonObj, const QUrl& jsonUr
} }
auto dataObj = dataValue.toObject(); auto dataObj = dataValue.toObject();
std::vector<QString> outputJoints;
auto outputJoints_VAL = dataObj.value("outputJoints");
if (outputJoints_VAL.isArray()) {
QJsonArray outputJoints_ARRAY = outputJoints_VAL.toArray();
for (int i = 0; i < outputJoints_ARRAY.size(); i++) {
outputJoints.push_back(outputJoints_ARRAY.at(i).toString());
}
}
assert((int)type >= 0 && type < AnimNode::Type::NumTypes); assert((int)type >= 0 && type < AnimNode::Type::NumTypes);
auto node = (animNodeTypeToLoaderFunc(type))(dataObj, id, jsonUrl); auto node = (animNodeTypeToLoaderFunc(type))(dataObj, id, jsonUrl);
if (!node) { if (!node) {
@ -242,6 +281,9 @@ static AnimNode::Pointer loadNode(const QJsonObject& jsonObj, const QUrl& jsonUr
} }
if ((animNodeTypeToProcessFunc(type))(node, dataObj, id, jsonUrl)) { if ((animNodeTypeToProcessFunc(type))(node, dataObj, id, jsonUrl)) {
for (auto&& outputJoint : outputJoints) {
node->addOutputJoint(outputJoint);
}
return node; return node;
} else { } else {
return nullptr; return nullptr;
@ -531,6 +573,41 @@ static AnimNode::Pointer loadDefaultPoseNode(const QJsonObject& jsonObj, const Q
return node; return node;
} }
static AnimNode::Pointer loadTwoBoneIKNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl) {
READ_FLOAT(alpha, jsonObj, id, jsonUrl, nullptr);
READ_BOOL(enabled, jsonObj, id, jsonUrl, nullptr);
READ_FLOAT(interpDuration, jsonObj, id, jsonUrl, nullptr);
READ_STRING(baseJointName, jsonObj, id, jsonUrl, nullptr);
READ_STRING(midJointName, jsonObj, id, jsonUrl, nullptr);
READ_STRING(tipJointName, jsonObj, id, jsonUrl, nullptr);
READ_VEC3(midHingeAxis, jsonObj, id, jsonUrl, nullptr);
READ_STRING(alphaVar, jsonObj, id, jsonUrl, nullptr);
READ_STRING(enabledVar, jsonObj, id, jsonUrl, nullptr);
READ_STRING(endEffectorRotationVarVar, jsonObj, id, jsonUrl, nullptr);
READ_STRING(endEffectorPositionVarVar, jsonObj, id, jsonUrl, nullptr);
auto node = std::make_shared<AnimTwoBoneIK>(id, alpha, enabled, interpDuration,
baseJointName, midJointName, tipJointName, midHingeAxis,
alphaVar, enabledVar,
endEffectorRotationVarVar, endEffectorPositionVarVar);
return node;
}
static AnimNode::Pointer loadPoleVectorConstraintNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl) {
READ_VEC3(referenceVector, jsonObj, id, jsonUrl, nullptr);
READ_BOOL(enabled, jsonObj, id, jsonUrl, nullptr);
READ_STRING(baseJointName, jsonObj, id, jsonUrl, nullptr);
READ_STRING(midJointName, jsonObj, id, jsonUrl, nullptr);
READ_STRING(tipJointName, jsonObj, id, jsonUrl, nullptr);
READ_STRING(enabledVar, jsonObj, id, jsonUrl, nullptr);
READ_STRING(poleVectorVar, jsonObj, id, jsonUrl, nullptr);
auto node = std::make_shared<AnimPoleVectorConstraint>(id, enabled, referenceVector,
baseJointName, midJointName, tipJointName,
enabledVar, poleVectorVar);
return node;
}
void buildChildMap(std::map<QString, int>& map, AnimNode::Pointer node) { void buildChildMap(std::map<QString, int>& map, AnimNode::Pointer node) {
for (int i = 0; i < (int)node->getChildCount(); ++i) { for (int i = 0; i < (int)node->getChildCount(); ++i) {
map.insert(std::pair<QString, int>(node->getChild(i)->getID(), i)); map.insert(std::pair<QString, int>(node->getChild(i)->getID(), i));
@ -682,7 +759,8 @@ AnimNode::Pointer AnimNodeLoader::load(const QByteArray& contents, const QUrl& j
QString version = versionVal.toString(); QString version = versionVal.toString();
// check version // check version
if (version != "1.0") { // AJT: TODO version check
if (version != "1.0" && version != "1.1") {
qCCritical(animation) << "AnimNodeLoader, bad version number" << version << "expected \"1.0\", url =" << jsonUrl.toDisplayString(); qCCritical(animation) << "AnimNodeLoader, bad version number" << version << "expected \"1.0\", url =" << jsonUrl.toDisplayString();
return nullptr; return nullptr;
} }

5
libraries/animation/src/AnimOverlay.cpp
View file

@ -41,7 +41,7 @@ void AnimOverlay::buildBoneSet(BoneSet boneSet) {
} }
} }
const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
// lookup parameters from animVars, using current instance variables as defaults. // lookup parameters from animVars, using current instance variables as defaults.
// NOTE: switching bonesets can be an expensive operation, let's try to avoid it. // NOTE: switching bonesets can be an expensive operation, let's try to avoid it.
@ -66,6 +66,9 @@ const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, const A
} }
} }
} }
processOutputJoints(triggersOut);
return _poses; return _poses;
} }

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

@ -45,7 +45,7 @@ public:
AnimOverlay(const QString& id, BoneSet boneSet, float alpha); AnimOverlay(const QString& id, BoneSet boneSet, float alpha);
virtual ~AnimOverlay() override; virtual ~AnimOverlay() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
void setBoneSetVar(const QString& boneSetVar) { _boneSetVar = boneSetVar; } void setBoneSetVar(const QString& boneSetVar) { _boneSetVar = boneSetVar; }
void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; } void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; }

244
libraries/animation/src/AnimPoleVectorConstraint.cpp Normal file
View file

@ -0,0 +1,244 @@
//
// AnimPoleVectorConstraint.cpp
//
// Created by Anthony J. Thibault on 5/12/18.
// Copyright (c) 2018 High Fidelity, Inc. All rights reserved.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include "AnimPoleVectorConstraint.h"
#include "AnimationLogging.h"
#include "AnimUtil.h"
#include "GLMHelpers.h"
const float FRAMES_PER_SECOND = 30.0f;
const float INTERP_DURATION = 6.0f;
AnimPoleVectorConstraint::AnimPoleVectorConstraint(const QString& id, bool enabled, glm::vec3 referenceVector,
const QString& baseJointName, const QString& midJointName, const QString& tipJointName,
const QString& enabledVar, const QString& poleVectorVar) :
AnimNode(AnimNode::Type::PoleVectorConstraint, id),
_enabled(enabled),
_referenceVector(referenceVector),
_baseJointName(baseJointName),
_midJointName(midJointName),
_tipJointName(tipJointName),
_enabledVar(enabledVar),
_poleVectorVar(poleVectorVar) {
}
AnimPoleVectorConstraint::~AnimPoleVectorConstraint() {
}
const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
assert(_children.size() == 1);
if (_children.size() != 1) {
return _poses;
}
// evalute underPoses
AnimPoseVec underPoses = _children[0]->evaluate(animVars, context, dt, triggersOut);
// if we don't have a skeleton, or jointName lookup failed.
if (!_skeleton || _baseJointIndex == -1 || _midJointIndex == -1 || _tipJointIndex == -1 || underPoses.size() == 0) {
// pass underPoses through unmodified.
_poses = underPoses;
return _poses;
}
// guard against size changes
if (underPoses.size() != _poses.size()) {
_poses = underPoses;
}
// Look up poleVector from animVars, make sure to convert into geom space.
glm::vec3 poleVector = animVars.lookupRigToGeometryVector(_poleVectorVar, Vectors::UNIT_Z);
// determine if we should interpolate
bool enabled = animVars.lookup(_enabledVar, _enabled);
const float MIN_LENGTH = 1.0e-4f;
if (glm::length(poleVector) < MIN_LENGTH) {
enabled = false;
}
if (enabled != _enabled) {
AnimChain poseChain;
poseChain.buildFromRelativePoses(_skeleton, _poses, _tipJointIndex);
if (enabled) {
beginInterp(InterpType::SnapshotToSolve, poseChain);
} else {
beginInterp(InterpType::SnapshotToUnderPoses, poseChain);
}
}
_enabled = enabled;
// don't build chains or do IK if we are disbled & not interping.
if (_interpType == InterpType::None && !enabled) {
_poses = underPoses;
return _poses;
}
// compute chain
AnimChain underChain;
underChain.buildFromRelativePoses(_skeleton, underPoses, _tipJointIndex);
AnimChain ikChain = underChain;
AnimPose baseParentPose = ikChain.getAbsolutePoseFromJointIndex(_baseParentJointIndex);
AnimPose basePose = ikChain.getAbsolutePoseFromJointIndex(_baseJointIndex);
AnimPose midPose = ikChain.getAbsolutePoseFromJointIndex(_midJointIndex);
AnimPose tipPose = ikChain.getAbsolutePoseFromJointIndex(_tipJointIndex);
// Look up refVector from animVars, make sure to convert into geom space.
glm::vec3 refVector = midPose.xformVectorFast(_referenceVector);
float refVectorLength = glm::length(refVector);
glm::vec3 axis = basePose.trans() - tipPose.trans();
float axisLength = glm::length(axis);
glm::vec3 unitAxis = axis / axisLength;
glm::vec3 sideVector = glm::cross(unitAxis, refVector);
float sideVectorLength = glm::length(sideVector);
// project refVector onto axis plane
glm::vec3 refVectorProj = refVector - glm::dot(refVector, unitAxis) * unitAxis;
float refVectorProjLength = glm::length(refVectorProj);
// project poleVector on plane formed by axis.
glm::vec3 poleVectorProj = poleVector - glm::dot(poleVector, unitAxis) * unitAxis;
float poleVectorProjLength = glm::length(poleVectorProj);
// double check for zero length vectors or vectors parallel to rotaiton axis.
if (axisLength > MIN_LENGTH && refVectorLength > MIN_LENGTH && sideVectorLength > MIN_LENGTH &&
refVectorProjLength > MIN_LENGTH && poleVectorProjLength > MIN_LENGTH) {
float dot = glm::clamp(glm::dot(refVectorProj / refVectorProjLength, poleVectorProj / poleVectorProjLength), 0.0f, 1.0f);
float sideDot = glm::dot(poleVector, sideVector);
float theta = copysignf(1.0f, sideDot) * acosf(dot);
glm::quat deltaRot = glm::angleAxis(theta, unitAxis);
// transform result back into parent relative frame.
glm::quat relBaseRot = glm::inverse(baseParentPose.rot()) * deltaRot * basePose.rot();
ikChain.setRelativePoseAtJointIndex(_baseJointIndex, AnimPose(relBaseRot, underPoses[_baseJointIndex].trans()));
glm::quat relTipRot = glm::inverse(midPose.rot()) * glm::inverse(deltaRot) * tipPose.rot();
ikChain.setRelativePoseAtJointIndex(_tipJointIndex, AnimPose(relTipRot, underPoses[_tipJointIndex].trans()));
}
// start off by initializing output poses with the underPoses
_poses = underPoses;
// apply smooth interpolation
if (_interpType != InterpType::None) {
_interpAlpha += _interpAlphaVel * dt;
if (_interpAlpha < 1.0f) {
AnimChain interpChain;
if (_interpType == InterpType::SnapshotToUnderPoses) {
interpChain = underChain;
interpChain.blend(_snapshotChain, _interpAlpha);
} else if (_interpType == InterpType::SnapshotToSolve) {
interpChain = ikChain;
interpChain.blend(_snapshotChain, _interpAlpha);
}
// copy interpChain into _poses
interpChain.outputRelativePoses(_poses);
} else {
// interpolation complete
_interpType = InterpType::None;
}
}
if (_interpType == InterpType::None) {
if (enabled) {
// copy chain into _poses
ikChain.outputRelativePoses(_poses);
} else {
// copy under chain into _poses
underChain.outputRelativePoses(_poses);
}
}
if (context.getEnableDebugDrawIKChains()) {
if (_interpType == InterpType::None && enabled) {
const vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f);
ikChain.debugDraw(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix(), BLUE);
}
}
if (context.getEnableDebugDrawIKChains()) {
if (enabled) {
const glm::vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
const glm::vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
const glm::vec4 CYAN(0.0f, 1.0f, 1.0f, 1.0f);
const glm::vec4 YELLOW(1.0f, 0.0f, 1.0f, 1.0f);
const float VECTOR_LENGTH = 0.5f;
glm::mat4 geomToWorld = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix();
// draw the pole
glm::vec3 start = transformPoint(geomToWorld, basePose.trans());
glm::vec3 end = transformPoint(geomToWorld, tipPose.trans());
DebugDraw::getInstance().drawRay(start, end, CYAN);
// draw the poleVector
glm::vec3 midPoint = 0.5f * (start + end);
glm::vec3 poleVectorEnd = midPoint + VECTOR_LENGTH * glm::normalize(transformVectorFast(geomToWorld, poleVector));
DebugDraw::getInstance().drawRay(midPoint, poleVectorEnd, GREEN);
// draw the refVector
glm::vec3 refVectorEnd = midPoint + VECTOR_LENGTH * glm::normalize(transformVectorFast(geomToWorld, refVector));
DebugDraw::getInstance().drawRay(midPoint, refVectorEnd, RED);
// draw the sideVector
glm::vec3 sideVector = glm::cross(poleVector, refVector);
glm::vec3 sideVectorEnd = midPoint + VECTOR_LENGTH * glm::normalize(transformVectorFast(geomToWorld, sideVector));
DebugDraw::getInstance().drawRay(midPoint, sideVectorEnd, YELLOW);
}
}
processOutputJoints(triggersOut);
return _poses;
}
// for AnimDebugDraw rendering
const AnimPoseVec& AnimPoleVectorConstraint::getPosesInternal() const {
return _poses;
}
void AnimPoleVectorConstraint::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
AnimNode::setSkeletonInternal(skeleton);
lookUpIndices();
}
void AnimPoleVectorConstraint::lookUpIndices() {
assert(_skeleton);
// look up bone indices by name
std::vector<int> indices = _skeleton->lookUpJointIndices({_baseJointName, _midJointName, _tipJointName});
// cache the results
_baseJointIndex = indices[0];
_midJointIndex = indices[1];
_tipJointIndex = indices[2];
if (_baseJointIndex != -1) {
_baseParentJointIndex = _skeleton->getParentIndex(_baseJointIndex);
}
}
void AnimPoleVectorConstraint::beginInterp(InterpType interpType, const AnimChain& chain) {
// capture the current poses in a snapshot.
_snapshotChain = chain;
_interpType = interpType;
_interpAlphaVel = FRAMES_PER_SECOND / INTERP_DURATION;
_interpAlpha = 0.0f;
}

74
libraries/animation/src/AnimPoleVectorConstraint.h Normal file
View file

@ -0,0 +1,74 @@
//
// AnimPoleVectorConstraint.h
//
// Created by Anthony J. Thibault on 5/25/18.
// Copyright (c) 2018 High Fidelity, Inc. All rights reserved.
//
// 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_AnimPoleVectorConstraint_h
#define hifi_AnimPoleVectorConstraint_h
#include "AnimNode.h"
#include "AnimChain.h"
// Three bone IK chain
class AnimPoleVectorConstraint : public AnimNode {
public:
friend class AnimTests;
AnimPoleVectorConstraint(const QString& id, bool enabled, glm::vec3 referenceVector,
const QString& baseJointName, const QString& midJointName, const QString& tipJointName,
const QString& enabledVar, const QString& poleVectorVar);
virtual ~AnimPoleVectorConstraint() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
protected:
enum class InterpType {
None = 0,
SnapshotToUnderPoses,
SnapshotToSolve,
NumTypes
};
// for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override;
virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
void lookUpIndices();
void beginInterp(InterpType interpType, const AnimChain& chain);
AnimPoseVec _poses;
bool _enabled;
glm::vec3 _referenceVector;
QString _baseJointName;
QString _midJointName;
QString _tipJointName;
QString _enabledVar;
QString _poleVectorVar;
int _baseParentJointIndex { -1 };
int _baseJointIndex { -1 };
int _midJointIndex { -1 };
int _tipJointIndex { -1 };
InterpType _interpType { InterpType::None };
float _interpAlphaVel { 0.0f };
float _interpAlpha { 0.0f };
AnimChain _snapshotChain;
// no copies
AnimPoleVectorConstraint(const AnimPoleVectorConstraint&) = delete;
AnimPoleVectorConstraint& operator=(const AnimPoleVectorConstraint&) = delete;
};
#endif // hifi_AnimPoleVectorConstraint_h

7
libraries/animation/src/AnimPose.cpp
View file

@ -12,6 +12,7 @@
#include <GLMHelpers.h> #include <GLMHelpers.h>
#include <algorithm> #include <algorithm>
#include <glm/gtc/matrix_transform.hpp> #include <glm/gtc/matrix_transform.hpp>
#include "AnimUtil.h"
const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f), const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f),
glm::quat(), glm::quat(),
@ -77,4 +78,8 @@ AnimPose::operator glm::mat4() const {
glm::vec4(zAxis, 0.0f), glm::vec4(_trans, 1.0f)); glm::vec4(zAxis, 0.0f), glm::vec4(_trans, 1.0f));
} }
void AnimPose::blend(const AnimPose& srcPose, float alpha) {
_scale = lerp(srcPose._scale, _scale, alpha);
_rot = safeLerp(srcPose._rot, _rot, alpha);
_trans = lerp(srcPose._trans, _trans, alpha);
}

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

@ -46,6 +46,8 @@ public:
const glm::vec3& trans() const { return _trans; } const glm::vec3& trans() const { return _trans; }
glm::vec3& trans() { return _trans; } glm::vec3& trans() { return _trans; }
void blend(const AnimPose& srcPose, float alpha);
private: private:
friend QDebug operator<<(QDebug debug, const AnimPose& pose); friend QDebug operator<<(QDebug debug, const AnimPose& pose);
glm::vec3 _scale { 1.0f }; glm::vec3 _scale { 1.0f };

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

@ -282,3 +282,17 @@ void AnimSkeleton::dump(const AnimPoseVec& poses) const {
qCDebug(animation) << "]"; qCDebug(animation) << "]";
} }
std::vector<int> AnimSkeleton::lookUpJointIndices(const std::vector<QString>& jointNames) const {
std::vector<int> result;
result.reserve(jointNames.size());
for (auto& name : jointNames) {
int index = nameToJointIndex(name);
if (index == -1) {
qWarning(animation) << "AnimSkeleton::lookUpJointIndices(): could not find bone with named " << name;
}
result.push_back(index);
}
return result;
}

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

@ -61,6 +61,8 @@ public:
void dump(bool verbose) const; void dump(bool verbose) const;
void dump(const AnimPoseVec& poses) const; void dump(const AnimPoseVec& poses) const;
std::vector<int> lookUpJointIndices(const std::vector<QString>& jointNames) const;
protected: protected:
void buildSkeletonFromJoints(const std::vector<FBXJoint>& joints); void buildSkeletonFromJoints(const std::vector<FBXJoint>& joints);

7
libraries/animation/src/AnimStateMachine.cpp
View file

@ -21,7 +21,7 @@ AnimStateMachine::~AnimStateMachine() {
} }
const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) { const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
QString desiredStateID = animVars.lookup(_currentStateVar, _currentState->getID()); QString desiredStateID = animVars.lookup(_currentStateVar, _currentState->getID());
if (_currentState->getID() != desiredStateID) { if (_currentState->getID() != desiredStateID) {
@ -81,6 +81,9 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, co
if (!_duringInterp) { if (!_duringInterp) {
_poses = currentStateNode->evaluate(animVars, context, dt, triggersOut); _poses = currentStateNode->evaluate(animVars, context, dt, triggersOut);
} }
processOutputJoints(triggersOut);
return _poses; return _poses;
} }
@ -107,7 +110,7 @@ void AnimStateMachine::switchState(const AnimVariantMap& animVars, const AnimCon
// because dt is 0, we should not encounter any triggers // because dt is 0, we should not encounter any triggers
const float dt = 0.0f; const float dt = 0.0f;
Triggers triggers; AnimVariantMap triggers;
if (_interpType == InterpType::SnapshotBoth) { if (_interpType == InterpType::SnapshotBoth) {
// snapshot previous pose. // snapshot previous pose.

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

@ -113,7 +113,7 @@ public:
explicit AnimStateMachine(const QString& id); explicit AnimStateMachine(const QString& id);
virtual ~AnimStateMachine() override; virtual ~AnimStateMachine() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override; virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
void setCurrentStateVar(QString& currentStateVar) { _currentStateVar = currentStateVar; } void setCurrentStateVar(QString& currentStateVar) { _currentStateVar = currentStateVar; }

293
libraries/animation/src/AnimTwoBoneIK.cpp Normal file
View file

@ -0,0 +1,293 @@
//
// AnimTwoBoneIK.cpp
//
// Created by Anthony J. Thibault on 5/12/18.
// Copyright (c) 2018 High Fidelity, Inc. All rights reserved.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include "AnimTwoBoneIK.h"
#include <DebugDraw.h>
#include "AnimationLogging.h"
#include "AnimUtil.h"
const float FRAMES_PER_SECOND = 30.0f;
AnimTwoBoneIK::AnimTwoBoneIK(const QString& id, float alpha, bool enabled, float interpDuration,
const QString& baseJointName, const QString& midJointName,
const QString& tipJointName, const glm::vec3& midHingeAxis,
const QString& alphaVar, const QString& enabledVar,
const QString& endEffectorRotationVarVar, const QString& endEffectorPositionVarVar) :
AnimNode(AnimNode::Type::TwoBoneIK, id),
_alpha(alpha),
_enabled(enabled),
_interpDuration(interpDuration),
_baseJointName(baseJointName),
_midJointName(midJointName),
_tipJointName(tipJointName),
_midHingeAxis(glm::normalize(midHingeAxis)),
_alphaVar(alphaVar),
_enabledVar(enabledVar),
_endEffectorRotationVarVar(endEffectorRotationVarVar),
_endEffectorPositionVarVar(endEffectorPositionVarVar),
_prevEndEffectorRotationVar(),
_prevEndEffectorPositionVar()
{
}
AnimTwoBoneIK::~AnimTwoBoneIK() {
}
const AnimPoseVec& AnimTwoBoneIK::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {
assert(_children.size() == 1);
if (_children.size() != 1) {
return _poses;
}
// evalute underPoses
AnimPoseVec underPoses = _children[0]->evaluate(animVars, context, dt, triggersOut);
// if we don't have a skeleton, or jointName lookup failed.
if (!_skeleton || _baseJointIndex == -1 || _midJointIndex == -1 || _tipJointIndex == -1 || underPoses.size() == 0) {
// pass underPoses through unmodified.
_poses = underPoses;
return _poses;
}
// guard against size changes
if (underPoses.size() != _poses.size()) {
_poses = underPoses;
}
const float MIN_ALPHA = 0.0f;
const float MAX_ALPHA = 1.0f;
float alpha = glm::clamp(animVars.lookup(_alphaVar, _alpha), MIN_ALPHA, MAX_ALPHA);
// don't perform IK if we have bad indices, or alpha is zero
if (_tipJointIndex == -1 || _midJointIndex == -1 || _baseJointIndex == -1 || alpha == 0.0f) {
_poses = underPoses;
return _poses;
}
// determine if we should interpolate
bool enabled = animVars.lookup(_enabledVar, _enabled);
if (enabled != _enabled) {
AnimChain poseChain;
poseChain.buildFromRelativePoses(_skeleton, _poses, _tipJointIndex);
if (enabled) {
beginInterp(InterpType::SnapshotToSolve, poseChain);
} else {
beginInterp(InterpType::SnapshotToUnderPoses, poseChain);
}
}
_enabled = enabled;
// don't build chains or do IK if we are disbled & not interping.
if (_interpType == InterpType::None && !enabled) {
_poses = underPoses;
return _poses;
}
// compute chain
AnimChain underChain;
underChain.buildFromRelativePoses(_skeleton, underPoses, _tipJointIndex);
AnimChain ikChain = underChain;
AnimPose baseParentPose = ikChain.getAbsolutePoseFromJointIndex(_baseParentJointIndex);
AnimPose basePose = ikChain.getAbsolutePoseFromJointIndex(_baseJointIndex);
AnimPose midPose = ikChain.getAbsolutePoseFromJointIndex(_midJointIndex);
AnimPose tipPose = ikChain.getAbsolutePoseFromJointIndex(_tipJointIndex);
QString endEffectorRotationVar = animVars.lookup(_endEffectorRotationVarVar, QString(""));
QString endEffectorPositionVar = animVars.lookup(_endEffectorPositionVarVar, QString(""));
// if either of the endEffectorVars have changed
if ((!_prevEndEffectorRotationVar.isEmpty() && (_prevEndEffectorRotationVar != endEffectorRotationVar)) ||
(!_prevEndEffectorPositionVar.isEmpty() && (_prevEndEffectorPositionVar != endEffectorPositionVar))) {
// begin interp to smooth out transition between prev and new end effector.
AnimChain poseChain;
poseChain.buildFromRelativePoses(_skeleton, _poses, _tipJointIndex);
beginInterp(InterpType::SnapshotToSolve, poseChain);
}
// Look up end effector from animVars, make sure to convert into geom space.
// First look in the triggers then look in the animVars, so we can follow output joints underneath us in the anim graph
AnimPose targetPose(tipPose);
if (triggersOut.hasKey(endEffectorRotationVar)) {
targetPose.rot() = triggersOut.lookupRigToGeometry(endEffectorRotationVar, tipPose.rot());
} else if (animVars.hasKey(endEffectorRotationVar)) {
targetPose.rot() = animVars.lookupRigToGeometry(endEffectorRotationVar, tipPose.rot());
}
if (triggersOut.hasKey(endEffectorPositionVar)) {
targetPose.trans() = triggersOut.lookupRigToGeometry(endEffectorPositionVar, tipPose.trans());
} else if (animVars.hasKey(endEffectorRotationVar)) {
targetPose.trans() = animVars.lookupRigToGeometry(endEffectorPositionVar, tipPose.trans());
}
_prevEndEffectorRotationVar = endEffectorRotationVar;
_prevEndEffectorPositionVar = endEffectorPositionVar;
glm::vec3 bicepVector = midPose.trans() - basePose.trans();
float r0 = glm::length(bicepVector);
bicepVector = bicepVector / r0;
glm::vec3 forearmVector = tipPose.trans() - midPose.trans();
float r1 = glm::length(forearmVector);
forearmVector = forearmVector / r1;
float d = glm::length(targetPose.trans() - basePose.trans());
// http://mathworld.wolfram.com/Circle-CircleIntersection.html
float midAngle = 0.0f;
if (d < r0 + r1) {
float y = sqrtf((-d + r1 - r0) * (-d - r1 + r0) * (-d + r1 + r0) * (d + r1 + r0)) / (2.0f * d);
midAngle = PI - (acosf(y / r0) + acosf(y / r1));
}
// compute midJoint rotation
glm::quat relMidRot = glm::angleAxis(midAngle, _midHingeAxis);
// insert new relative pose into the chain and rebuild it.
ikChain.setRelativePoseAtJointIndex(_midJointIndex, AnimPose(relMidRot, underPoses[_midJointIndex].trans()));
ikChain.buildDirtyAbsolutePoses();
// recompute tip pose after mid joint has been rotated
AnimPose newTipPose = ikChain.getAbsolutePoseFromJointIndex(_tipJointIndex);
glm::vec3 leverArm = newTipPose.trans() - basePose.trans();
glm::vec3 targetLine = targetPose.trans() - basePose.trans();
// compute delta rotation that brings leverArm parallel to targetLine
glm::vec3 axis = glm::cross(leverArm, targetLine);
float axisLength = glm::length(axis);
const float MIN_AXIS_LENGTH = 1.0e-4f;
if (axisLength > MIN_AXIS_LENGTH) {
axis /= axisLength;
float cosAngle = glm::clamp(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)), -1.0f, 1.0f);
float angle = acosf(cosAngle);
glm::quat deltaRot = glm::angleAxis(angle, axis);
// combine deltaRot with basePose.
glm::quat absRot = deltaRot * basePose.rot();
// transform result back into parent relative frame.
glm::quat relBaseRot = glm::inverse(baseParentPose.rot()) * absRot;
ikChain.setRelativePoseAtJointIndex(_baseJointIndex, AnimPose(relBaseRot, underPoses[_baseJointIndex].trans()));
}
// recompute midJoint pose after base has been rotated.
ikChain.buildDirtyAbsolutePoses();
AnimPose midJointPose = ikChain.getAbsolutePoseFromJointIndex(_midJointIndex);
// transform target rotation in to parent relative frame.
glm::quat relTipRot = glm::inverse(midJointPose.rot()) * targetPose.rot();
ikChain.setRelativePoseAtJointIndex(_tipJointIndex, AnimPose(relTipRot, underPoses[_tipJointIndex].trans()));
// blend with the underChain
ikChain.blend(underChain, alpha);
// start off by initializing output poses with the underPoses
_poses = underPoses;
// apply smooth interpolation
if (_interpType != InterpType::None) {
_interpAlpha += _interpAlphaVel * dt;
if (_interpAlpha < 1.0f) {
AnimChain interpChain;
if (_interpType == InterpType::SnapshotToUnderPoses) {
interpChain = underChain;
interpChain.blend(_snapshotChain, _interpAlpha);
} else if (_interpType == InterpType::SnapshotToSolve) {
interpChain = ikChain;
interpChain.blend(_snapshotChain, _interpAlpha);
}
// copy interpChain into _poses
interpChain.outputRelativePoses(_poses);
} else {
// interpolation complete
_interpType = InterpType::None;
}
}
if (_interpType == InterpType::None) {
if (enabled) {
// copy chain into _poses
ikChain.outputRelativePoses(_poses);
} else {
// copy under chain into _poses
underChain.outputRelativePoses(_poses);
}
}
if (context.getEnableDebugDrawIKTargets()) {
const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3());
glm::mat4 geomTargetMat = createMatFromQuatAndPos(targetPose.rot(), targetPose.trans());
glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat;
QString name = QString("%1_target").arg(_id);
DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat),
extractTranslation(avatarTargetMat), _enabled ? GREEN : RED);
} else if (_lastEnableDebugDrawIKTargets) {
QString name = QString("%1_target").arg(_id);
DebugDraw::getInstance().removeMyAvatarMarker(name);
}
_lastEnableDebugDrawIKTargets = context.getEnableDebugDrawIKTargets();
if (context.getEnableDebugDrawIKChains()) {
if (_interpType == InterpType::None && enabled) {
const vec4 CYAN(0.0f, 1.0f, 1.0f, 1.0f);
ikChain.debugDraw(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix(), CYAN);
}
}
processOutputJoints(triggersOut);
return _poses;
}
// for AnimDebugDraw rendering
const AnimPoseVec& AnimTwoBoneIK::getPosesInternal() const {
return _poses;
}
void AnimTwoBoneIK::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
AnimNode::setSkeletonInternal(skeleton);
lookUpIndices();
}
void AnimTwoBoneIK::lookUpIndices() {
assert(_skeleton);
// look up bone indices by name
std::vector<int> indices = _skeleton->lookUpJointIndices({_baseJointName, _midJointName, _tipJointName});
// cache the results
_baseJointIndex = indices[0];
_midJointIndex = indices[1];
_tipJointIndex = indices[2];
if (_baseJointIndex != -1) {
_baseParentJointIndex = _skeleton->getParentIndex(_baseJointIndex);
}
}
void AnimTwoBoneIK::beginInterp(InterpType interpType, const AnimChain& chain) {
// capture the current poses in a snapshot.
_snapshotChain = chain;
_interpType = interpType;
_interpAlphaVel = FRAMES_PER_SECOND / _interpDuration;
_interpAlpha = 0.0f;
}

83
libraries/animation/src/AnimTwoBoneIK.h Normal file
View file

@ -0,0 +1,83 @@
//
// AnimTwoBoneIK.h
//
// Created by Anthony J. Thibault on 5/12/18.
// Copyright (c) 2018 High Fidelity, Inc. All rights reserved.
//
// 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_AnimTwoBoneIK_h
#define hifi_AnimTwoBoneIK_h
#include "AnimNode.h"
#include "AnimChain.h"
// Simple two bone IK chain
class AnimTwoBoneIK : public AnimNode {
public:
friend class AnimTests;
AnimTwoBoneIK(const QString& id, float alpha, bool enabled, float interpDuration,
const QString& baseJointName, const QString& midJointName,
const QString& tipJointName, const glm::vec3& midHingeAxis,
const QString& alphaVar, const QString& enabledVar,
const QString& endEffectorRotationVarVar, const QString& endEffectorPositionVarVar);
virtual ~AnimTwoBoneIK() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) override;
protected:
enum class InterpType {
None = 0,
SnapshotToUnderPoses,
SnapshotToSolve,
NumTypes
};
// for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override;
virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
void lookUpIndices();
void beginInterp(InterpType interpType, const AnimChain& chain);
AnimPoseVec _poses;
float _alpha;
bool _enabled;
float _interpDuration; // in frames (1/30 sec)
QString _baseJointName;
QString _midJointName;
QString _tipJointName;
glm::vec3 _midHingeAxis; // in baseJoint relative frame, should be normalized
int _baseParentJointIndex { -1 };
int _baseJointIndex { -1 };
int _midJointIndex { -1 };
int _tipJointIndex { -1 };
QString _alphaVar; // float - (0, 1) 0 means underPoses only, 1 means IK only.
QString _enabledVar; // bool
QString _endEffectorRotationVarVar; // string
QString _endEffectorPositionVarVar; // string
QString _prevEndEffectorRotationVar;
QString _prevEndEffectorPositionVar;
InterpType _interpType { InterpType::None };
float _interpAlphaVel { 0.0f };
float _interpAlpha { 0.0f };
AnimChain _snapshotChain;
bool _lastEnableDebugDrawIKTargets { false };
// no copies
AnimTwoBoneIK(const AnimTwoBoneIK&) = delete;
AnimTwoBoneIK& operator=(const AnimTwoBoneIK&) = delete;
};
#endif // hifi_AnimTwoBoneIK_h

14
libraries/animation/src/AnimUtil.cpp
View file

@ -21,14 +21,6 @@ void blend(size_t numPoses, const AnimPose* a, const AnimPose* b, float alpha, A
const AnimPose& aPose = a[i]; const AnimPose& aPose = a[i];
const AnimPose& bPose = b[i]; const AnimPose& bPose = b[i];
// adjust signs if necessary
const glm::quat& q1 = aPose.rot();
glm::quat q2 = bPose.rot();
float dot = glm::dot(q1, q2);
if (dot < 0.0f) {
q2 = -q2;
}
result[i].scale() = lerp(aPose.scale(), bPose.scale(), alpha); result[i].scale() = lerp(aPose.scale(), bPose.scale(), alpha);
result[i].rot() = safeLerp(aPose.rot(), bPose.rot(), alpha); result[i].rot() = safeLerp(aPose.rot(), bPose.rot(), alpha);
result[i].trans() = lerp(aPose.trans(), bPose.trans(), alpha); result[i].trans() = lerp(aPose.trans(), bPose.trans(), alpha);
@ -53,7 +45,7 @@ glm::quat averageQuats(size_t numQuats, const glm::quat* quats) {
} }
float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag, float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag,
const QString& id, AnimNode::Triggers& triggersOut) { const QString& id, AnimVariantMap& triggersOut) {
const float EPSILON = 0.0001f; const float EPSILON = 0.0001f;
float frame = currentFrame; float frame = currentFrame;
@ -79,12 +71,12 @@ float accumulateTime(float startFrame, float endFrame, float timeScale, float cu
if (framesRemaining >= framesTillEnd) { if (framesRemaining >= framesTillEnd) {
if (loopFlag) { if (loopFlag) {
// anim loop // anim loop
triggersOut.push_back(id + "OnLoop"); triggersOut.setTrigger(id + "OnLoop");
framesRemaining -= framesTillEnd; framesRemaining -= framesTillEnd;
frame = clampedStartFrame; frame = clampedStartFrame;
} else { } else {
// anim end // anim end
triggersOut.push_back(id + "OnDone"); triggersOut.setTrigger(id + "OnDone");
frame = endFrame; frame = endFrame;
framesRemaining = 0.0f; framesRemaining = 0.0f;
} }

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

@ -19,7 +19,7 @@ void blend(size_t numPoses, const AnimPose* a, const AnimPose* b, float alpha, A
glm::quat averageQuats(size_t numQuats, const glm::quat* quats); glm::quat averageQuats(size_t numQuats, const glm::quat* quats);
float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag, float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag,
const QString& id, AnimNode::Triggers& triggersOut); const QString& id, AnimVariantMap& triggersOut);
inline glm::quat safeLerp(const glm::quat& a, const glm::quat& b, float alpha) { inline glm::quat safeLerp(const glm::quat& a, const glm::quat& b, float alpha) {
// adjust signs if necessary // adjust signs if necessary

149
libraries/animation/src/Rig.cpp
View file

@ -59,6 +59,21 @@ const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 0.9f, 0.0f);
const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f); const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f);
const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f); const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f);
static const QString LEFT_FOOT_POSITION("leftFootPosition");
static const QString LEFT_FOOT_ROTATION("leftFootRotation");
static const QString LEFT_FOOT_IK_POSITION_VAR("leftFootIKPositionVar");
static const QString LEFT_FOOT_IK_ROTATION_VAR("leftFootIKRotationVar");
static const QString MAIN_STATE_MACHINE_LEFT_FOOT_POSITION("mainStateMachineLeftFootPosition");
static const QString MAIN_STATE_MACHINE_LEFT_FOOT_ROTATION("mainStateMachineLeftFootRotation");
static const QString RIGHT_FOOT_POSITION("rightFootPosition");
static const QString RIGHT_FOOT_ROTATION("rightFootRotation");
static const QString RIGHT_FOOT_IK_POSITION_VAR("rightFootIKPositionVar");
static const QString RIGHT_FOOT_IK_ROTATION_VAR("rightFootIKRotationVar");
static const QString MAIN_STATE_MACHINE_RIGHT_FOOT_ROTATION("mainStateMachineRightFootRotation");
static const QString MAIN_STATE_MACHINE_RIGHT_FOOT_POSITION("mainStateMachineRightFootPosition");
Rig::Rig() { Rig::Rig() {
// Ensure thread-safe access to the rigRegistry. // Ensure thread-safe access to the rigRegistry.
std::lock_guard<std::mutex> guard(rigRegistryMutex); std::lock_guard<std::mutex> guard(rigRegistryMutex);
@ -1049,7 +1064,7 @@ void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, cons
getGeometryToRigTransform(), rigToWorldTransform); getGeometryToRigTransform(), rigToWorldTransform);
// evaluate the animation // evaluate the animation
AnimNode::Triggers triggersOut; AnimVariantMap triggersOut;
_internalPoseSet._relativePoses = _animNode->evaluate(_animVars, context, deltaTime, triggersOut); _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, context, deltaTime, triggersOut);
if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) { if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) {
@ -1057,9 +1072,7 @@ void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, cons
_internalPoseSet._relativePoses = _animSkeleton->getRelativeDefaultPoses(); _internalPoseSet._relativePoses = _animSkeleton->getRelativeDefaultPoses();
} }
_animVars.clearTriggers(); _animVars.clearTriggers();
for (auto& trigger : triggersOut) { _animVars = triggersOut;
_animVars.setTrigger(trigger);
}
} }
applyOverridePoses(); applyOverridePoses();
buildAbsoluteRigPoses(_internalPoseSet._relativePoses, _internalPoseSet._absolutePoses); buildAbsoluteRigPoses(_internalPoseSet._relativePoses, _internalPoseSet._absolutePoses);
@ -1241,7 +1254,7 @@ glm::vec3 Rig::deflectHandFromTorso(const glm::vec3& handPosition, const FBXJoin
} }
void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool hipsEstimated, void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool hipsEstimated,
bool leftArmEnabled, bool rightArmEnabled, float dt, bool leftArmEnabled, bool rightArmEnabled, bool headEnabled, float dt,
const AnimPose& leftHandPose, const AnimPose& rightHandPose, const AnimPose& leftHandPose, const AnimPose& rightHandPose,
const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo,
const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo, const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo,
@ -1305,7 +1318,13 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
_animVars.unset("leftHandPosition"); _animVars.unset("leftHandPosition");
_animVars.unset("leftHandRotation"); _animVars.unset("leftHandRotation");
_animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
if (headEnabled) {
_animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
} else {
// disable hand IK for desktop mode
_animVars.set("leftHandType", (int)IKTarget::Type::Unknown);
}
} }
if (rightHandEnabled) { if (rightHandEnabled) {
@ -1364,21 +1383,41 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
_animVars.unset("rightHandPosition"); _animVars.unset("rightHandPosition");
_animVars.unset("rightHandRotation"); _animVars.unset("rightHandRotation");
_animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
if (headEnabled) {
_animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
} else {
// disable hand IK for desktop mode
_animVars.set("rightHandType", (int)IKTarget::Type::Unknown);
}
} }
} }
void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose, void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, bool headEnabled,
const AnimPose& leftFootPose, const AnimPose& rightFootPose,
const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix) { const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix) {
const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.95f;
int hipsIndex = indexOfJoint("Hips"); int hipsIndex = indexOfJoint("Hips");
const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.85f;
if (headEnabled) {
// always do IK if head is enabled
_animVars.set("leftFootIKEnabled", true);
_animVars.set("rightFootIKEnabled", true);
} else {
// only do IK if we have a valid foot.
_animVars.set("leftFootIKEnabled", leftFootEnabled);
_animVars.set("rightFootIKEnabled", rightFootEnabled);
}
if (leftFootEnabled) { if (leftFootEnabled) {
_animVars.set("leftFootPosition", leftFootPose.trans());
_animVars.set("leftFootRotation", leftFootPose.rot()); _animVars.set(LEFT_FOOT_POSITION, leftFootPose.trans());
_animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); _animVars.set(LEFT_FOOT_ROTATION, leftFootPose.rot());
// We want to drive the IK directly from the trackers.
_animVars.set(LEFT_FOOT_IK_POSITION_VAR, LEFT_FOOT_POSITION);
_animVars.set(LEFT_FOOT_IK_ROTATION_VAR, LEFT_FOOT_ROTATION);
int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot");
int kneeJointIndex = _animSkeleton->nameToJointIndex("LeftLeg"); int kneeJointIndex = _animSkeleton->nameToJointIndex("LeftLeg");
@ -1396,20 +1435,25 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose
_prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector; _prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector;
_animVars.set("leftFootPoleVectorEnabled", true); _animVars.set("leftFootPoleVectorEnabled", true);
_animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z);
_animVars.set("leftFootPoleVector", transformVectorFast(sensorToRigMatrix, _prevLeftFootPoleVector)); _animVars.set("leftFootPoleVector", transformVectorFast(sensorToRigMatrix, _prevLeftFootPoleVector));
} else { } else {
_animVars.unset("leftFootPosition"); // We want to drive the IK from the underlying animation.
_animVars.unset("leftFootRotation"); // This gives us the ability to squat while in the HMD, without the feet from dipping under the floor.
_animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); _animVars.set(LEFT_FOOT_IK_POSITION_VAR, MAIN_STATE_MACHINE_LEFT_FOOT_POSITION);
_animVars.set(LEFT_FOOT_IK_ROTATION_VAR, MAIN_STATE_MACHINE_LEFT_FOOT_ROTATION);
// We want to match the animated knee pose as close as possible, so don't use poleVectors
_animVars.set("leftFootPoleVectorEnabled", false); _animVars.set("leftFootPoleVectorEnabled", false);
_prevLeftFootPoleVectorValid = false; _prevLeftFootPoleVectorValid = false;
} }
if (rightFootEnabled) { if (rightFootEnabled) {
_animVars.set("rightFootPosition", rightFootPose.trans()); _animVars.set(RIGHT_FOOT_POSITION, rightFootPose.trans());
_animVars.set("rightFootRotation", rightFootPose.rot()); _animVars.set(RIGHT_FOOT_ROTATION, rightFootPose.rot());
_animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
// We want to drive the IK directly from the trackers.
_animVars.set(RIGHT_FOOT_IK_POSITION_VAR, RIGHT_FOOT_POSITION);
_animVars.set(RIGHT_FOOT_IK_ROTATION_VAR, RIGHT_FOOT_ROTATION);
int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot");
int kneeJointIndex = _animSkeleton->nameToJointIndex("RightLeg"); int kneeJointIndex = _animSkeleton->nameToJointIndex("RightLeg");
@ -1427,13 +1471,16 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose
_prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector; _prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector;
_animVars.set("rightFootPoleVectorEnabled", true); _animVars.set("rightFootPoleVectorEnabled", true);
_animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z);
_animVars.set("rightFootPoleVector", transformVectorFast(sensorToRigMatrix, _prevRightFootPoleVector)); _animVars.set("rightFootPoleVector", transformVectorFast(sensorToRigMatrix, _prevRightFootPoleVector));
} else { } else {
_animVars.unset("rightFootPosition"); // We want to drive the IK from the underlying animation.
_animVars.unset("rightFootRotation"); // This gives us the ability to squat while in the HMD, without the feet from dipping under the floor.
_animVars.set(RIGHT_FOOT_IK_POSITION_VAR, MAIN_STATE_MACHINE_RIGHT_FOOT_POSITION);
_animVars.set(RIGHT_FOOT_IK_ROTATION_VAR, MAIN_STATE_MACHINE_RIGHT_FOOT_ROTATION);
// We want to match the animated knee pose as close as possible, so don't use poleVectors
_animVars.set("rightFootPoleVectorEnabled", false); _animVars.set("rightFootPoleVectorEnabled", false);
_animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); _prevRightFootPoleVectorValid = false;
} }
} }
@ -1475,23 +1522,12 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
for (int i = 0; i < (int)children.size(); i++) { for (int i = 0; i < (int)children.size(); i++) {
int jointIndex = children[i]; int jointIndex = children[i];
int parentIndex = _animSkeleton->getParentIndex(jointIndex); int parentIndex = _animSkeleton->getParentIndex(jointIndex);
_internalPoseSet._absolutePoses[jointIndex] = _internalPoseSet._absolutePoses[jointIndex] =
_internalPoseSet._absolutePoses[parentIndex] * _internalPoseSet._relativePoses[jointIndex]; _internalPoseSet._absolutePoses[parentIndex] * _internalPoseSet._relativePoses[jointIndex];
} }
} }
} }
static glm::quat quatLerp(const glm::quat& q1, const glm::quat& q2, float alpha) {
float dot = glm::dot(q1, q2);
glm::quat temp;
if (dot < 0.0f) {
temp = -q2;
} else {
temp = q2;
}
return glm::normalize(glm::lerp(q1, temp, alpha));
}
bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& poleVector) const { bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& poleVector) const {
// The resulting Pole Vector is calculated as the sum of a three vectors. // The resulting Pole Vector is calculated as the sum of a three vectors.
// The first is the vector with direction shoulder-hand. The module of this vector is inversely proportional to the strength of the resulting Pole Vector. // The first is the vector with direction shoulder-hand. The module of this vector is inversely proportional to the strength of the resulting Pole Vector.
@ -1510,7 +1546,7 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex,
glm::vec3 backVector = oppositeArmPose.trans() - armPose.trans(); glm::vec3 backVector = oppositeArmPose.trans() - armPose.trans();
glm::vec3 backCenter = armPose.trans() + 0.5f * backVector; glm::vec3 backCenter = armPose.trans() + 0.5f * backVector;
const float OVER_BACK_HEAD_PERCENTAGE = 0.2f; const float OVER_BACK_HEAD_PERCENTAGE = 0.2f;
glm::vec3 headCenter = backCenter + glm::vec3(0, OVER_BACK_HEAD_PERCENTAGE * backVector.length(), 0); glm::vec3 headCenter = backCenter + glm::vec3(0, OVER_BACK_HEAD_PERCENTAGE * backVector.length(), 0);
@ -1522,7 +1558,7 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex,
glm::vec3 headForward = headCenter + horizontalModule * frontVector; glm::vec3 headForward = headCenter + horizontalModule * frontVector;
glm::vec3 armToHead = headForward - armPose.trans(); glm::vec3 armToHead = headForward - armPose.trans();
float armToHandDistance = glm::length(armToHand); float armToHandDistance = glm::length(armToHand);
float armToElbowDistance = glm::length(armToElbow); float armToElbowDistance = glm::length(armToElbow);
float elbowToHandDistance = glm::length(elbowToHand); float elbowToHandDistance = glm::length(elbowToHand);
@ -1533,7 +1569,7 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex,
// How much the hand is reaching for the opposite side // How much the hand is reaching for the opposite side
float oppositeProjection = glm::dot(armToHandDir, glm::normalize(backVector)); float oppositeProjection = glm::dot(armToHandDir, glm::normalize(backVector));
// Don't use pole vector when the hands are behind // Don't use pole vector when the hands are behind
if (glm::dot(frontVector, armToHand) < 0 && oppositeProjection < 0.5f * armTotalDistance) { if (glm::dot(frontVector, armToHand) < 0 && oppositeProjection < 0.5f * armTotalDistance) {
return false; return false;
@ -1552,7 +1588,7 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex,
const float FORWARD_CORRECTOR_WEIGHT = 3.0f; const float FORWARD_CORRECTOR_WEIGHT = 3.0f;
float elbowForwardTrigger = FORWARD_TRIGGER_PERCENTAGE * armToHandDistance; float elbowForwardTrigger = FORWARD_TRIGGER_PERCENTAGE * armToHandDistance;
if (oppositeProjection > -elbowForwardTrigger) { if (oppositeProjection > -elbowForwardTrigger) {
float forwardAmount = FORWARD_CORRECTOR_WEIGHT * (elbowForwardTrigger + oppositeProjection); float forwardAmount = FORWARD_CORRECTOR_WEIGHT * (elbowForwardTrigger + oppositeProjection);
correctionVector = forwardAmount * frontVector; correctionVector = forwardAmount * frontVector;
@ -1561,31 +1597,18 @@ bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex,
return true; return true;
} }
// returns a poleVector for the knees that is a blend of the foot and the hips.
// targetFootPose is in rig space
// result poleVector is also in rig space.
glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const { glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const {
const float FOOT_THETA = 0.8969f; // 51.39 degrees
const glm::vec3 localFootForward(0.0f, cosf(FOOT_THETA), sinf(FOOT_THETA));
glm::vec3 footForward = targetFootPose.rot() * localFootForward;
AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex]; AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex];
AnimPose footPose = targetFootPose; glm::vec3 hipsForward = hipsPose.rot() * Vectors::UNIT_Z;
AnimPose kneePose = _externalPoseSet._absolutePoses[kneeIndex];
AnimPose upLegPose = _externalPoseSet._absolutePoses[upLegIndex];
// ray from foot to upLeg return glm::normalize(lerp(hipsForward, footForward, 0.75f));
glm::vec3 d = glm::normalize(footPose.trans() - upLegPose.trans());
// form a plane normal to the hips x-axis
glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X;
// project d onto this plane
glm::vec3 dProj = d - glm::dot(d, n) * n;
// rotate dProj by 90 degrees to get the poleVector.
glm::vec3 poleVector = glm::angleAxis(-PI / 2.0f, n) * dProj;
// blend the foot oreintation into the pole vector
glm::quat kneeToFootDelta = footPose.rot() * glm::inverse(kneePose.rot());
const float WRIST_POLE_ADJUST_FACTOR = 0.5f;
glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, kneeToFootDelta, WRIST_POLE_ADJUST_FACTOR);
return glm::normalize(poleAdjust * poleVector);
} }
void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) { void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) {
@ -1610,12 +1633,12 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo
updateHead(headEnabled, hipsEnabled, params.primaryControllerPoses[PrimaryControllerType_Head]); updateHead(headEnabled, hipsEnabled, params.primaryControllerPoses[PrimaryControllerType_Head]);
updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, hipsEstimated, leftArmEnabled, rightArmEnabled, dt, updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, hipsEstimated, leftArmEnabled, rightArmEnabled, headEnabled, dt,
params.primaryControllerPoses[PrimaryControllerType_LeftHand], params.primaryControllerPoses[PrimaryControllerType_RightHand], params.primaryControllerPoses[PrimaryControllerType_LeftHand], params.primaryControllerPoses[PrimaryControllerType_RightHand],
params.hipsShapeInfo, params.spineShapeInfo, params.spine1ShapeInfo, params.spine2ShapeInfo, params.hipsShapeInfo, params.spineShapeInfo, params.spine1ShapeInfo, params.spine2ShapeInfo,
params.rigToSensorMatrix, sensorToRigMatrix); params.rigToSensorMatrix, sensorToRigMatrix);
updateFeet(leftFootEnabled, rightFootEnabled, updateFeet(leftFootEnabled, rightFootEnabled, headEnabled,
params.primaryControllerPoses[PrimaryControllerType_LeftFoot], params.primaryControllerPoses[PrimaryControllerType_RightFoot], params.primaryControllerPoses[PrimaryControllerType_LeftFoot], params.primaryControllerPoses[PrimaryControllerType_RightFoot],
params.rigToSensorMatrix, sensorToRigMatrix); params.rigToSensorMatrix, sensorToRigMatrix);

9
libraries/animation/src/Rig.h
View file

@ -75,6 +75,10 @@ public:
}; };
struct ControllerParameters { struct ControllerParameters {
ControllerParameters() {
memset(primaryControllerFlags, 0, NumPrimaryControllerTypes);
memset(secondaryControllerFlags, 0, NumPrimaryControllerTypes);
}
glm::mat4 rigToSensorMatrix; glm::mat4 rigToSensorMatrix;
AnimPose primaryControllerPoses[NumPrimaryControllerTypes]; // rig space AnimPose primaryControllerPoses[NumPrimaryControllerTypes]; // rig space
uint8_t primaryControllerFlags[NumPrimaryControllerTypes]; uint8_t primaryControllerFlags[NumPrimaryControllerTypes];
@ -229,12 +233,13 @@ protected:
void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix); void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix);
void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool hipsEstimated, void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool hipsEstimated,
bool leftArmEnabled, bool rightArmEnabled, float dt, bool leftArmEnabled, bool rightArmEnabled, bool headEnabled, float dt,
const AnimPose& leftHandPose, const AnimPose& rightHandPose, const AnimPose& leftHandPose, const AnimPose& rightHandPose,
const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo,
const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo, const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo,
const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix); const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix);
void updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose, void updateFeet(bool leftFootEnabled, bool rightFootEnabled, bool headEnabled,
const AnimPose& leftFootPose, const AnimPose& rightFootPose,
const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix); const glm::mat4& rigToSensorMatrix, const glm::mat4& sensorToRigMatrix);
void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade);