diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 9efe3dd29b..eb8403634a 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -49,6 +49,8 @@ "id": "ik", "type": "inverseKinematics", "data": { + "solutionSource": "relaxToUnderPoses", + "solutionSourceVar": "solutionSource", "targets": [ { "jointName": "Hips", diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 297f6069c7..f5f248602d 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -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()), diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 72f823d3bd..b1f69a28d3 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -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..."; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 333c33ba3b..fd547e39e0 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -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()); } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 04fa37cb1d..cbd51aaaf9 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -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; diff --git a/libraries/animation/src/AnimContext.cpp b/libraries/animation/src/AnimContext.cpp index c8d3e7bcda..70ca3764b0 100644 --- a/libraries/animation/src/AnimContext.cpp +++ b/libraries/animation/src/AnimContext.cpp @@ -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) +{ } diff --git a/libraries/animation/src/AnimContext.h b/libraries/animation/src/AnimContext.h index 3170911e14..f68535005c 100644 --- a/libraries/animation/src/AnimContext.h +++ b/libraries/animation/src/AnimContext.h @@ -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 diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 6edd969568..5e6927afcb 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -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::iterator constraintItr = _constraints.find(index); + std::map::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 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 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 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(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(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(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; + } +} diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index c91b7aa9c4..f73ed95935 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -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& targets, const AnimPoseVec& underPoses); void solveWithCyclicCoordinateDescent(const std::vector& 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& targets, const AnimPoseVec& underPoses, float dt); // no copies @@ -85,6 +101,7 @@ protected: std::vector _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 diff --git a/libraries/animation/src/AnimNodeLoader.cpp b/libraries/animation/src/AnimNodeLoader.cpp index bda4541f36..6bc7342d7f 100644 --- a/libraries/animation/src/AnimNodeLoader.cpp +++ b/libraries/animation/src/AnimNodeLoader.cpp @@ -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; } diff --git a/libraries/animation/src/AnimPose.cpp b/libraries/animation/src/AnimPose.cpp index e1c8528e0b..470bbab8b6 100644 --- a/libraries/animation/src/AnimPose.cpp +++ b/libraries/animation/src/AnimPose.cpp @@ -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); diff --git a/libraries/animation/src/AnimPose.h b/libraries/animation/src/AnimPose.h index 893a5c1382..a2e22a24be 100644 --- a/libraries/animation/src/AnimPose.h +++ b/libraries/animation/src/AnimPose.h @@ -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; diff --git a/libraries/animation/src/AnimUtil.cpp b/libraries/animation/src/AnimUtil.cpp index c5643034e5..a4659f1e76 100644 --- a/libraries/animation/src/AnimUtil.cpp +++ b/libraries/animation/src/AnimUtil.cpp @@ -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) { diff --git a/libraries/animation/src/AnimUtil.h b/libraries/animation/src/AnimUtil.h index 6d394be882..055fd630eb 100644 --- a/libraries/animation/src/AnimUtil.h +++ b/libraries/animation/src/AnimUtil.h @@ -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 - - diff --git a/libraries/animation/src/ElbowConstraint.cpp b/libraries/animation/src/ElbowConstraint.cpp index 6833c1762e..17c6bb2da6 100644 --- a/libraries/animation/src/ElbowConstraint.cpp +++ b/libraries/animation/src/ElbowConstraint.cpp @@ -13,6 +13,7 @@ #include #include +#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); +} diff --git a/libraries/animation/src/ElbowConstraint.h b/libraries/animation/src/ElbowConstraint.h index 21288715b5..d3f080374a 100644 --- a/libraries/animation/src/ElbowConstraint.h +++ b/libraries/animation/src/ElbowConstraint.h @@ -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; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 700761b248..cb5aebe930 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -305,30 +305,35 @@ void Rig::clearJointAnimationPriority(int index) { } } -void Rig::clearIKJointLimitHistory() { +std::shared_ptr Rig::getAnimInverseKinematicsNode() const { + std::shared_ptr result; if (_animNode) { _animNode->traverse([&](AnimNode::Pointer node) { // only report clip nodes as valid roles. auto ikNode = std::dynamic_pointer_cast(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(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()); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 2d024628f5..33b66f91ea 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -26,6 +26,7 @@ #include "SimpleMovingAverage.h" class Rig; +class AnimInverseKinematics; typedef std::shared_ptr RigPointer; // Rig instances are reentrant. @@ -111,6 +112,8 @@ public: void clearJointStates(); void clearJointAnimationPriority(int index); + std::shared_ptr 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 _stateHandlers; diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h index 277e5293c6..e4a5334d41 100644 --- a/libraries/animation/src/RotationConstraint.h +++ b/libraries/animation/src/RotationConstraint.h @@ -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(); }; diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index 12d7e618e5..212343d4eb 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -15,6 +15,7 @@ #include #include #include +#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 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; +} diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index 06afd64232..a41664d353 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -58,7 +58,7 @@ public: virtual void dynamicallyAdjustLimits(const glm::quat& rotation) override; // for testing purposes - const std::vector& getMinDots() { return _swingLimitFunction.getMinDots(); } + const std::vector& 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& getMinDots() { return _minDots; } + const std::vector& 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; diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index acc84646c5..766a584b85 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -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() {