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

Merge pull request #10419 from hyperlogic/feature/ik-solver-init-config

Browse source 
More stable IK when hips, feet and chest are controlled via external input
This commit is contained in:
anshuman64 2017-05-12 17:22:29 -07:00 committed by GitHub
commit 49d4cf4e68
22 changed files with 413 additions and 67 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -49,6 +49,8 @@
"id": "ik",
"type": "inverseKinematics",
"data": {
"solutionSource": "relaxToUnderPoses",
"solutionSourceVar": "solutionSource",
"targets": [
{
"jointName": "Hips",

2
interface/src/Menu.cpp
View file

@ -523,6 +523,8 @@ Menu::Menu() {
avatar.get(), SLOT(setEnableDebugDrawSensorToWorldMatrix(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKTargets, 0, false,
avatar.get(), SLOT(setEnableDebugDrawIKTargets(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKConstraints, 0, false,
avatar.get(), SLOT(setEnableDebugDrawIKConstraints(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::ActionMotorControl,
Qt::CTRL | Qt::SHIFT | Qt::Key_K, true, avatar.get(), SLOT(updateMotionBehaviorFromMenu()),

1
interface/src/Menu.h
View file

@ -161,6 +161,7 @@ namespace MenuOption {
const QString RenderResolutionQuarter = "1/4";
const QString RenderSensorToWorldMatrix = "Show SensorToWorld Matrix";
const QString RenderIKTargets = "Show IK Targets";
const QString RenderIKConstraints = "Show IK Constraints";
const QString ResetAvatarSize = "Reset Avatar Size";
const QString ResetSensors = "Reset Sensors";
const QString RunningScripts = "Running Scripts...";

6
interface/src/avatar/MyAvatar.cpp
View file

@ -38,6 +38,7 @@
#include <UserActivityLogger.h>
#include <AnimDebugDraw.h>
#include <AnimClip.h>
#include <AnimInverseKinematics.h>
#include <recording/Deck.h>
#include <recording/Recorder.h>
#include <recording/Clip.h>
@ -504,6 +505,7 @@ void MyAvatar::simulate(float deltaTime) {
if (_rig) {
_rig->setEnableDebugDrawIKTargets(_enableDebugDrawIKTargets);
_rig->setEnableDebugDrawIKConstraints(_enableDebugDrawIKConstraints);
}
_skeletonModel->simulate(deltaTime);
@ -927,6 +929,10 @@ void MyAvatar::setEnableDebugDrawIKTargets(bool isEnabled) {
_enableDebugDrawIKTargets = isEnabled;
}
void MyAvatar::setEnableDebugDrawIKConstraints(bool isEnabled) {
_enableDebugDrawIKConstraints = isEnabled;
}
void MyAvatar::setEnableMeshVisible(bool isEnabled) {
_skeletonModel->setVisibleInScene(isEnabled, qApp->getMain3DScene());
}

2
interface/src/avatar/MyAvatar.h
View file

@ -521,6 +521,7 @@ public slots:
void setEnableDebugDrawHandControllers(bool isEnabled);
void setEnableDebugDrawSensorToWorldMatrix(bool isEnabled);
void setEnableDebugDrawIKTargets(bool isEnabled);
void setEnableDebugDrawIKConstraints(bool isEnabled);
bool getEnableMeshVisible() const { return _skeletonModel->isVisible(); }
void setEnableMeshVisible(bool isEnabled);
void setUseAnimPreAndPostRotations(bool isEnabled);
@ -706,6 +707,7 @@ private:
bool _enableDebugDrawHandControllers { false };
bool _enableDebugDrawSensorToWorldMatrix { false };
bool _enableDebugDrawIKTargets { false };
bool _enableDebugDrawIKConstraints { false };
AudioListenerMode _audioListenerMode;
glm::vec3 _customListenPosition;

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

@ -10,7 +10,11 @@
#include "AnimContext.h"
AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix) :
AnimContext::AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints,
const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix) :
_enableDebugDrawIKTargets(enableDebugDrawIKTargets),
_geometryToRigMatrix(geometryToRigMatrix) {
_enableDebugDrawIKConstraints(enableDebugDrawIKConstraints),
_geometryToRigMatrix(geometryToRigMatrix),
_rigToWorldMatrix(rigToWorldMatrix)
{
}

7
libraries/animation/src/AnimContext.h
View file

@ -16,15 +16,20 @@
class AnimContext {
public:
AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix);
AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints,
const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix);
bool getEnableDebugDrawIKTargets() const { return _enableDebugDrawIKTargets; }
bool getEnableDebugDrawIKConstraints() const { return _enableDebugDrawIKConstraints; }
const glm::mat4& getGeometryToRigMatrix() const { return _geometryToRigMatrix; }
const glm::mat4& getRigToWorldMatrix() const { return _rigToWorldMatrix; }
protected:
bool _enableDebugDrawIKTargets { false };
bool _enableDebugDrawIKConstraints{ false };
glm::mat4 _geometryToRigMatrix;
glm::mat4 _rigToWorldMatrix;
};
#endif // hifi_AnimContext_h

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

@ -399,6 +399,13 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
//virtual
const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
// allows solutionSource to be overridden by an animVar
auto solutionSource = animVars.lookup(_solutionSourceVar, (int)_solutionSource);
if (context.getEnableDebugDrawIKConstraints()) {
debugDrawConstraints(context);
}
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;
@ -410,25 +417,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
PROFILE_RANGE_EX(simulation_animation, "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();
}
initRelativePosesFromSolutionSource((SolutionSource)solutionSource, underPoses);
if (!underPoses.empty()) {
// Sometimes the underpose itself can violate the constraints. Rather than
@ -604,9 +593,9 @@ void AnimInverseKinematics::clearIKJointLimitHistory() {
}
}
RotationConstraint* AnimInverseKinematics::getConstraint(int index) {
RotationConstraint* AnimInverseKinematics::getConstraint(int index) const {
RotationConstraint* constraint = nullptr;
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.find(index);
std::map<int, RotationConstraint*>::const_iterator constraintItr = _constraints.find(index);
if (constraintItr != _constraints.end()) {
constraint = constraintItr->second;
}
@ -622,17 +611,19 @@ void AnimInverseKinematics::clearConstraints() {
_constraints.clear();
}
// set up swing limits around a swingTwistConstraint in an ellipse, where lateralSwingTheta is the swing limit for lateral swings (side to side)
// anteriorSwingTheta is swing limit for forward and backward swings. (where x-axis of reference rotation is sideways and -z-axis is forward)
static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float lateralSwingTheta, float anteriorSwingTheta) {
// set up swing limits around a swingTwistConstraint in an ellipse, where lateralSwingPhi is the swing limit for lateral swings (side to side)
// anteriorSwingPhi is swing limit for forward and backward swings. (where x-axis of reference rotation is sideways and -z-axis is forward)
static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float lateralSwingPhi, float anteriorSwingPhi) {
assert(stConstraint);
const int NUM_SUBDIVISIONS = 8;
const int NUM_SUBDIVISIONS = 16;
std::vector<float> minDots;
minDots.reserve(NUM_SUBDIVISIONS);
float dTheta = TWO_PI / NUM_SUBDIVISIONS;
float theta = 0.0f;
for (int i = 0; i < NUM_SUBDIVISIONS; i++) {
minDots.push_back(cosf(glm::length(glm::vec2(anteriorSwingTheta * cosf(theta), lateralSwingTheta * sinf(theta)))));
float theta_prime = atanf((lateralSwingPhi / anteriorSwingPhi) * tanf(theta));
float phi = (cosf(2.0f * theta_prime) * ((lateralSwingPhi - anteriorSwingPhi) / 2.0f)) + ((lateralSwingPhi + anteriorSwingPhi) / 2.0f);
minDots.push_back(cosf(phi));
theta += dTheta;
}
stConstraint->setSwingLimits(minDots);
@ -640,7 +631,6 @@ static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float l
void AnimInverseKinematics::initConstraints() {
if (!_skeleton) {
return;
}
// We create constraints for the joints shown here
// (and their Left counterparts if applicable).
@ -744,30 +734,27 @@ void AnimInverseKinematics::initConstraints() {
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)));
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
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)));
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)));
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
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)));
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];
std::vector<float> minDots;
for (size_t i = 0; i < swungDirections.size(); i++) {
minDots.push_back(glm::dot(glm::normalize(swungDirections[i]), Vectors::UNIT_Y));
}
stConstraint->setSwingLimits(swungDirections);
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == baseName.compare("Hand", Qt::CaseSensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
@ -957,6 +944,32 @@ void AnimInverseKinematics::initConstraints() {
}
}
void AnimInverseKinematics::initLimitCenterPoses() {
assert(_skeleton);
_limitCenterPoses.reserve(_skeleton->getNumJoints());
for (int i = 0; i < _skeleton->getNumJoints(); i++) {
AnimPose pose = _skeleton->getRelativeDefaultPose(i);
RotationConstraint* constraint = getConstraint(i);
if (constraint) {
pose.rot() = constraint->computeCenterRotation();
}
_limitCenterPoses.push_back(pose);
}
// The limit center rotations for the LeftArm and RightArm form a t-pose.
// In order for the elbows to look more natural, we rotate them down by the avatar's sides
const float UPPER_ARM_THETA = PI / 3.0f; // 60 deg
int leftArmIndex = _skeleton->nameToJointIndex("LeftArm");
const glm::quat armRot = glm::angleAxis(UPPER_ARM_THETA, Vectors::UNIT_X);
if (leftArmIndex >= 0 && leftArmIndex < (int)_limitCenterPoses.size()) {
_limitCenterPoses[leftArmIndex].rot() = _limitCenterPoses[leftArmIndex].rot() * armRot;
}
int rightArmIndex = _skeleton->nameToJointIndex("RightArm");
if (rightArmIndex >= 0 && rightArmIndex < (int)_limitCenterPoses.size()) {
_limitCenterPoses[rightArmIndex].rot() = _limitCenterPoses[rightArmIndex].rot() * armRot;
}
}
void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
AnimNode::setSkeletonInternal(skeleton);
@ -973,6 +986,7 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
if (skeleton) {
initConstraints();
initLimitCenterPoses();
_headIndex = _skeleton->nameToJointIndex("Head");
_hipsIndex = _skeleton->nameToJointIndex("Hips");
@ -989,3 +1003,170 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
_hipsParentIndex = -1;
}
}
static glm::vec3 sphericalToCartesian(float phi, float theta) {
float cos_phi = cosf(phi);
float sin_phi = sinf(phi);
return glm::vec3(sin_phi * cosf(theta), cos_phi, -sin_phi * sinf(theta));
}
void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) const {
if (_skeleton) {
const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
const vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f);
const vec4 PURPLE(0.5f, 0.0f, 1.0f, 1.0f);
const vec4 CYAN(0.0f, 1.0f, 1.0f, 1.0f);
const vec4 GRAY(0.2f, 0.2f, 0.2f, 1.0f);
const vec4 MAGENTA(1.0f, 0.0f, 1.0f, 1.0f);
const float AXIS_LENGTH = 2.0f; // cm
const float TWIST_LENGTH = 4.0f; // cm
const float HINGE_LENGTH = 6.0f; // cm
const float SWING_LENGTH = 5.0f; // cm
AnimPoseVec poses = _skeleton->getRelativeDefaultPoses();
// copy reference rotations into the relative poses
for (int i = 0; i < (int)poses.size(); i++) {
const RotationConstraint* constraint = getConstraint(i);
if (constraint) {
poses[i].rot() = constraint->getReferenceRotation();
}
}
// convert relative poses to absolute
_skeleton->convertRelativePosesToAbsolute(poses);
mat4 geomToWorldMatrix = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix();
// draw each pose and constraint
for (int i = 0; i < (int)poses.size(); i++) {
// transform local axes into world space.
auto pose = poses[i];
glm::vec3 xAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_X);
glm::vec3 yAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_Y);
glm::vec3 zAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_Z);
glm::vec3 pos = transformPoint(geomToWorldMatrix, pose.trans());
DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * xAxis, RED);
DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * yAxis, GREEN);
DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * zAxis, BLUE);
// draw line to parent
int parentIndex = _skeleton->getParentIndex(i);
if (parentIndex != -1) {
glm::vec3 parentPos = transformPoint(geomToWorldMatrix, poses[parentIndex].trans());
DebugDraw::getInstance().drawRay(pos, parentPos, GRAY);
}
glm::quat parentAbsRot;
if (parentIndex != -1) {
parentAbsRot = poses[parentIndex].rot();
}
const RotationConstraint* constraint = getConstraint(i);
if (constraint) {
glm::quat refRot = constraint->getReferenceRotation();
const ElbowConstraint* elbowConstraint = dynamic_cast<const ElbowConstraint*>(constraint);
if (elbowConstraint) {
glm::vec3 hingeAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * elbowConstraint->getHingeAxis());
DebugDraw::getInstance().drawRay(pos, pos + HINGE_LENGTH * hingeAxis, MAGENTA);
// draw elbow constraints
glm::quat minRot = glm::angleAxis(elbowConstraint->getMinAngle(), elbowConstraint->getHingeAxis());
glm::quat maxRot = glm::angleAxis(elbowConstraint->getMaxAngle(), elbowConstraint->getHingeAxis());
const int NUM_SWING_STEPS = 10;
for (int i = 0; i < NUM_SWING_STEPS + 1; i++) {
glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS)));
glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_Y);
DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN);
}
} else {
const SwingTwistConstraint* swingTwistConstraint = dynamic_cast<const SwingTwistConstraint*>(constraint);
if (swingTwistConstraint) {
// twist constraints
glm::vec3 hingeAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * Vectors::UNIT_Y);
DebugDraw::getInstance().drawRay(pos, pos + HINGE_LENGTH * hingeAxis, MAGENTA);
glm::quat minRot = glm::angleAxis(swingTwistConstraint->getMinTwist(), Vectors::UNIT_Y);
glm::quat maxRot = glm::angleAxis(swingTwistConstraint->getMaxTwist(), Vectors::UNIT_Y);
const int NUM_SWING_STEPS = 10;
for (int i = 0; i < NUM_SWING_STEPS + 1; i++) {
glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS)));
glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_X);
DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN);
}
// draw swing constraints.
const size_t NUM_MIN_DOTS = swingTwistConstraint->getMinDots().size();
const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1);
float theta = 0.0f;
for (size_t i = 0, j = NUM_MIN_DOTS - 2; i < NUM_MIN_DOTS - 1; j = i, i++, theta += D_THETA) {
// compute swing rotation from theta and phi angles.
float phi = acosf(swingTwistConstraint->getMinDots()[i]);
glm::vec3 swungAxis = sphericalToCartesian(phi, theta);
glm::vec3 worldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * swungAxis);
glm::vec3 swingTip = pos + SWING_LENGTH * worldSwungAxis;
float prevPhi = acos(swingTwistConstraint->getMinDots()[j]);
float prevTheta = theta - D_THETA;
glm::vec3 prevSwungAxis = sphericalToCartesian(prevPhi, prevTheta);
glm::vec3 prevWorldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * prevSwungAxis);
glm::vec3 prevSwingTip = pos + SWING_LENGTH * prevWorldSwungAxis;
DebugDraw::getInstance().drawRay(pos, swingTip, PURPLE);
DebugDraw::getInstance().drawRay(prevSwingTip, swingTip, PURPLE);
}
}
}
pose.rot() = constraint->computeCenterRotation();
}
}
}
}
// for bones under IK, blend between previous solution (_relativePoses) to targetPoses
// for bones NOT under IK, copy directly from underPoses.
// mutates _relativePoses.
void AnimInverseKinematics::blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPoses, float blendFactor) {
// relax toward poses
int numJoints = (int)_relativePoses.size();
for (int i = 0; i < numJoints; ++i) {
float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot(), targetPoses[i].rot()));
if (_accumulators[i].isDirty()) {
// this joint is affected by IK --> blend toward the targetPoses rotation
_relativePoses[i].rot() = glm::normalize(glm::lerp(_relativePoses[i].rot(), dotSign * targetPoses[i].rot(), blendFactor));
} else {
// this joint is NOT affected by IK --> slam to underPoses rotation
_relativePoses[i].rot() = underPoses[i].rot();
}
_relativePoses[i].trans() = underPoses[i].trans();
}
}
void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPoses) {
const float RELAX_BLEND_FACTOR = (1.0f / 16.0f);
const float COPY_BLEND_FACTOR = 1.0f;
switch (solutionSource) {
default:
case SolutionSource::RelaxToUnderPoses:
blendToPoses(underPoses, underPoses, RELAX_BLEND_FACTOR);
break;
case SolutionSource::RelaxToLimitCenterPoses:
blendToPoses(_limitCenterPoses, underPoses, RELAX_BLEND_FACTOR);
break;
case SolutionSource::PreviousSolution:
// do nothing... _relativePoses is already the previous solution
break;
case SolutionSource::UnderPoses:
_relativePoses = underPoses;
break;
case SolutionSource::LimitCenterPoses:
// essentially copy limitCenterPoses over to _relativePoses.
blendToPoses(_limitCenterPoses, underPoses, COPY_BLEND_FACTOR);
break;
}
}

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

@ -43,18 +43,34 @@ public:
float getMaxErrorOnLastSolve() { return _maxErrorOnLastSolve; }
enum class SolutionSource {
RelaxToUnderPoses = 0,
RelaxToLimitCenterPoses,
PreviousSolution,
UnderPoses,
LimitCenterPoses,
NumSolutionSources,
};
void setSolutionSource(SolutionSource solutionSource) { _solutionSource = solutionSource; }
void setSolutionSourceVar(const QString& solutionSourceVar) { _solutionSourceVar = solutionSourceVar; }
protected:
void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
int solveTargetWithCCD(const IKTarget& target, AnimPoseVec& absolutePoses);
virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
void debugDrawConstraints(const AnimContext& context) const;
void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose);
void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor);
// for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; }
RotationConstraint* getConstraint(int index);
RotationConstraint* getConstraint(int index) const;
void clearConstraints();
void initConstraints();
void initLimitCenterPoses();
void computeHipsOffset(const std::vector<IKTarget>& targets, const AnimPoseVec& underPoses, float dt);
// no copies
@ -85,6 +101,7 @@ protected:
std::vector<IKTargetVar> _targetVarVec;
AnimPoseVec _defaultRelativePoses; // poses of the relaxed state
AnimPoseVec _relativePoses; // current relative poses
AnimPoseVec _limitCenterPoses; // relative
// experimental data for moving hips during IK
glm::vec3 _hipsOffset { Vectors::ZERO };
@ -100,6 +117,8 @@ protected:
float _maxErrorOnLastSolve { FLT_MAX };
bool _previousEnableDebugIKTargets { false };
SolutionSource _solutionSource { SolutionSource::RelaxToUnderPoses };
QString _solutionSourceVar;
};
#endif // hifi_AnimInverseKinematics_h

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

@ -352,6 +352,23 @@ static AnimOverlay::BoneSet stringToBoneSetEnum(const QString& str) {
return AnimOverlay::NumBoneSets;
}
static const char* solutionSourceStrings[(int)AnimInverseKinematics::SolutionSource::NumSolutionSources] = {
"relaxToUnderPoses",
"relaxToLimitCenterPoses",
"previousSolution",
"underPoses",
"limitCenterPoses"
};
static AnimInverseKinematics::SolutionSource stringToSolutionSourceEnum(const QString& str) {
for (int i = 0; i < (int)AnimInverseKinematics::SolutionSource::NumSolutionSources; i++) {
if (str == solutionSourceStrings[i]) {
return (AnimInverseKinematics::SolutionSource)i;
}
}
return AnimInverseKinematics::SolutionSource::NumSolutionSources;
}
static AnimNode::Pointer loadOverlayNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl) {
READ_STRING(boneSet, jsonObj, id, jsonUrl, nullptr);
@ -457,6 +474,23 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS
node->setTargetVars(jointName, positionVar, rotationVar, typeVar);
};
READ_OPTIONAL_STRING(solutionSource, jsonObj);
if (!solutionSource.isEmpty()) {
AnimInverseKinematics::SolutionSource solutionSourceType = stringToSolutionSourceEnum(solutionSource);
if (solutionSourceType != AnimInverseKinematics::SolutionSource::NumSolutionSources) {
node->setSolutionSource(solutionSourceType);
} else {
qCWarning(animation) << "AnimNodeLoader, bad solutionSourceType in \"solutionSource\", id = " << id << ", url = " << jsonUrl.toDisplayString();
}
}
READ_OPTIONAL_STRING(solutionSourceVar, jsonObj);
if (!solutionSourceVar.isEmpty()) {
node->setSolutionSourceVar(solutionSourceVar);
}
return node;
}

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

@ -39,7 +39,7 @@ glm::vec3 AnimPose::xformPoint(const glm::vec3& rhs) const {
return *this * rhs;
}
// really slow
// really slow, but accurate for transforms with non-uniform scale
glm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const {
glm::vec3 xAxis = _rot * glm::vec3(_scale.x, 0.0f, 0.0f);
glm::vec3 yAxis = _rot * glm::vec3(0.0f, _scale.y, 0.0f);
@ -49,6 +49,11 @@ glm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const {
return transInvMat * rhs;
}
// faster, but does not handle non-uniform scale correctly.
glm::vec3 AnimPose::xformVectorFast(const glm::vec3& rhs) const {
return _rot * (_scale * rhs);
}
AnimPose AnimPose::operator*(const AnimPose& rhs) const {
glm::mat4 result;
glm_mat4u_mul(*this, rhs, result);

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

@ -25,7 +25,8 @@ public:
static const AnimPose identity;
glm::vec3 xformPoint(const glm::vec3& rhs) const;
glm::vec3 xformVector(const glm::vec3& rhs) const; // really slow
glm::vec3 xformVector(const glm::vec3& rhs) const; // really slow, but accurate for transforms with non-uniform scale
glm::vec3 xformVectorFast(const glm::vec3& rhs) const; // faster, but does not handle non-uniform scale correctly.
glm::vec3 operator*(const glm::vec3& rhs) const; // same as xformPoint
AnimPose operator*(const AnimPose& rhs) const;

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

@ -33,6 +33,23 @@ void blend(size_t numPoses, const AnimPose* a, const AnimPose* b, float alpha, A
}
}
glm::quat averageQuats(size_t numQuats, const glm::quat* quats) {
if (numQuats == 0) {
return glm::quat();
}
glm::quat accum = quats[0];
glm::quat firstRot = quats[0];
for (size_t i = 1; i < numQuats; i++) {
glm::quat rot = quats[i];
float dot = glm::dot(firstRot, rot);
if (dot < 0.0f) {
rot = -rot;
}
accum += rot;
}
return glm::normalize(accum);
}
float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag,
const QString& id, AnimNode::Triggers& triggersOut) {

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

@ -16,9 +16,9 @@
// this is where the magic happens
void blend(size_t numPoses, const AnimPose* a, const AnimPose* b, float alpha, AnimPose* result);
glm::quat averageQuats(size_t numQuats, const glm::quat* quats);
float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag,
const QString& id, AnimNode::Triggers& triggersOut);
#endif

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

@ -13,6 +13,7 @@
#include <GeometryUtil.h>
#include <NumericalConstants.h>
#include "AnimUtil.h"
ElbowConstraint::ElbowConstraint() :
_minAngle(-PI),
@ -77,3 +78,10 @@ bool ElbowConstraint::apply(glm::quat& rotation) const {
return false;
}
glm::quat ElbowConstraint::computeCenterRotation() const {
const size_t NUM_LIMITS = 2;
glm::quat limits[NUM_LIMITS];
limits[0] = glm::angleAxis(_minAngle, _axis) * _referenceRotation;
limits[1] = glm::angleAxis(_maxAngle, _axis) * _referenceRotation;
return averageQuats(NUM_LIMITS, limits);
}

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

@ -18,6 +18,12 @@ public:
void setHingeAxis(const glm::vec3& axis);
void setAngleLimits(float minAngle, float maxAngle);
virtual bool apply(glm::quat& rotation) const override;
virtual glm::quat computeCenterRotation() const override;
glm::vec3 getHingeAxis() const { return _axis; }
float getMinAngle() const { return _minAngle; }
float getMaxAngle() const { return _maxAngle; }
protected:
glm::vec3 _axis;
glm::vec3 _perpAxis;

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

@ -305,30 +305,35 @@ void Rig::clearJointAnimationPriority(int index) {
}
}
void Rig::clearIKJointLimitHistory() {
std::shared_ptr<AnimInverseKinematics> Rig::getAnimInverseKinematicsNode() const {
std::shared_ptr<AnimInverseKinematics> result;
if (_animNode) {
_animNode->traverse([&](AnimNode::Pointer node) {
// only report clip nodes as valid roles.
auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
if (ikNode) {
ikNode->clearIKJointLimitHistory();
result = ikNode;
return false;
} else {
return true;
}
return true;
});
}
return result;
}
void Rig::clearIKJointLimitHistory() {
auto ikNode = getAnimInverseKinematicsNode();
if (ikNode) {
ikNode->clearIKJointLimitHistory();
}
}
void Rig::setMaxHipsOffsetLength(float maxLength) {
_maxHipsOffsetLength = maxLength;
if (_animNode) {
_animNode->traverse([&](AnimNode::Pointer node) {
auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
if (ikNode) {
ikNode->setMaxHipsOffsetLength(_maxHipsOffsetLength);
}
return true;
});
auto ikNode = getAnimInverseKinematicsNode();
if (ikNode) {
ikNode->setMaxHipsOffsetLength(_maxHipsOffsetLength);
}
}
@ -936,7 +941,7 @@ void Rig::updateAnimationStateHandlers() { // called on avatar update thread (wh
}
}
void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, const glm::mat4& rigToWorldTransform) {
PROFILE_RANGE_EX(simulation_animation_detail, __FUNCTION__, 0xffff00ff, 0);
PerformanceTimer perfTimer("updateAnimations");
@ -949,7 +954,8 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
updateAnimationStateHandlers();
_animVars.setRigToGeometryTransform(_rigToGeometryTransform);
AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform());
AnimContext context(_enableDebugDrawIKTargets, _enableDebugDrawIKConstraints,
getGeometryToRigTransform(), rigToWorldTransform);
// evaluate the animation
AnimNode::Triggers triggersOut;
@ -1025,10 +1031,12 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) {
_animVars.set("notIsTalking", !params.isTalking);
if (params.hipsEnabled) {
_animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToLimitCenterPoses);
_animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition);
_animVars.set("hipsPosition", extractTranslation(params.hipsMatrix));
_animVars.set("hipsRotation", glmExtractRotation(params.hipsMatrix));
} else {
_animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToUnderPoses);
_animVars.set("hipsType", (int)IKTarget::Type::Unknown);
}
@ -1440,7 +1448,7 @@ void Rig::computeAvatarBoundingCapsule(
// call overlay twice: once to verify AnimPoseVec joints and again to do the IK
AnimNode::Triggers triggersOut;
AnimContext context(false, glm::mat4());
AnimContext context(false, false, glm::mat4(), glm::mat4());
float dt = 1.0f; // the value of this does not matter
ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
AnimPoseVec finalPoses = ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());

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

@ -26,6 +26,7 @@
#include "SimpleMovingAverage.h"
class Rig;
class AnimInverseKinematics;
typedef std::shared_ptr<Rig> RigPointer;
// Rig instances are reentrant.
@ -111,6 +112,8 @@ public:
void clearJointStates();
void clearJointAnimationPriority(int index);
std::shared_ptr<AnimInverseKinematics> getAnimInverseKinematicsNode() const;
void clearIKJointLimitHistory();
void setMaxHipsOffsetLength(float maxLength);
float getMaxHipsOffsetLength() const;
@ -159,7 +162,7 @@ public:
void computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState);
// Regardless of who started the animations or how many, update the joints.
void updateAnimations(float deltaTime, glm::mat4 rootTransform);
void updateAnimations(float deltaTime, const glm::mat4& rootTransform, const glm::mat4& rigToWorldTransform);
// legacy
void inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority,
@ -228,6 +231,7 @@ public:
const glm::mat4& getGeometryToRigTransform() const { return _geometryToRigTransform; }
void setEnableDebugDrawIKTargets(bool enableDebugDrawIKTargets) { _enableDebugDrawIKTargets = enableDebugDrawIKTargets; }
void setEnableDebugDrawIKConstraints(bool enableDebugDrawIKConstraints) { _enableDebugDrawIKConstraints = enableDebugDrawIKConstraints; }
// input assumed to be in rig space
void computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const;
@ -338,6 +342,7 @@ protected:
float _maxHipsOffsetLength { 1.0f };
bool _enableDebugDrawIKTargets { false };
bool _enableDebugDrawIKConstraints { false };
private:
QMap<int, StateHandler> _stateHandlers;

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

@ -38,6 +38,9 @@ public:
/// \brief reset any remembered joint limit history
virtual void clearHistory() {};
/// \brief return the rotation that lies at the "center" of all the joint limits.
virtual glm::quat computeCenterRotation() const = 0;
protected:
glm::quat _referenceRotation = glm::quat();
};

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

@ -15,6 +15,7 @@
#include <GeometryUtil.h>
#include <GLMHelpers.h>
#include <NumericalConstants.h>
#include "AnimUtil.h"
const float MIN_MINDOT = -0.999f;
@ -430,3 +431,33 @@ void SwingTwistConstraint::dynamicallyAdjustLimits(const glm::quat& rotation) {
void SwingTwistConstraint::clearHistory() {
_lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
}
glm::quat SwingTwistConstraint::computeCenterRotation() const {
const size_t NUM_TWIST_LIMITS = 2;
const size_t NUM_MIN_DOTS = getMinDots().size();
std::vector<glm::quat> swingLimits;
swingLimits.reserve(NUM_MIN_DOTS);
glm::quat twistLimits[NUM_TWIST_LIMITS];
if (_minTwist != _maxTwist) {
// to ensure that twists do not flip the center rotation, we devide twist angle by 2.
twistLimits[0] = glm::angleAxis(_minTwist / 2.0f, _referenceRotation * Vectors::UNIT_Y);
twistLimits[1] = glm::angleAxis(_maxTwist / 2.0f, _referenceRotation * Vectors::UNIT_Y);
}
const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1);
float theta = 0.0f;
for (size_t i = 0; i < NUM_MIN_DOTS - 1; i++, theta += D_THETA) {
// compute swing rotation from theta and phi angles.
float phi = acos(getMinDots()[i]);
float cos_phi = getMinDots()[i];
float sin_phi = sinf(phi);
glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, -sin_phi * sinf(theta));
// to ensure that swings > 90 degrees do not flip the center rotation, we devide phi / 2
glm::quat swing = glm::angleAxis(phi / 2, glm::normalize(glm::cross(Vectors::UNIT_Y, swungAxis)));
swingLimits.push_back(swing);
}
glm::quat averageSwing = averageQuats(swingLimits.size(), &swingLimits[0]);
glm::quat averageTwist = averageQuats(2, twistLimits);
return averageSwing * averageTwist * _referenceRotation;
}

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

@ -58,7 +58,7 @@ public:
virtual void dynamicallyAdjustLimits(const glm::quat& rotation) override;
// for testing purposes
const std::vector<float>& getMinDots() { return _swingLimitFunction.getMinDots(); }
const std::vector<float>& getMinDots() const { return _swingLimitFunction.getMinDots(); }
// SwingLimitFunction is an implementation of the constraint check described in the paper:
// "The Parameterization of Joint Rotation with the Unit Quaternion" by Quang Liu and Edmond C. Prakash
@ -81,7 +81,7 @@ public:
float getMinDot(float theta) const;
// for testing purposes
const std::vector<float>& getMinDots() { return _minDots; }
const std::vector<float>& getMinDots() const { return _minDots; }
private:
// the limits are stored in a lookup table with cyclic boundary conditions
@ -99,6 +99,11 @@ public:
void clearHistory() override;
virtual glm::quat computeCenterRotation() const override;
float getMinTwist() const { return _minTwist; }
float getMaxTwist() const { return _maxTwist; }
private:
float handleTwistBoundaryConditions(float twistAngle) const;

3
libraries/render-utils/src/Model.cpp
View file

@ -1046,7 +1046,8 @@ void Model::simulate(float deltaTime, bool fullUpdate) {
//virtual
void Model::updateRig(float deltaTime, glm::mat4 parentTransform) {
_needsUpdateClusterMatrices = true;
_rig->updateAnimations(deltaTime, parentTransform);
glm::mat4 rigToWorldTransform = createMatFromQuatAndPos(getRotation(), getTranslation());
_rig->updateAnimations(deltaTime, parentTransform, rigToWorldTransform);
}
void Model::computeMeshPartLocalBounds() {