diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 55f86f7f44..b0d27ee04b 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -113,7 +113,7 @@ "typeVar": "headType", "weightVar": "headWeight", "weight": 4.0, - "flexCoefficients": [1, 0.35, 0.5, 0.35, 0.25] + "flexCoefficients": [1, 0.05, 0.25, 0.25, 0.25] } ] }, diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index f5f248602d..8461b1d42a 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -525,6 +525,8 @@ Menu::Menu() { avatar.get(), SLOT(setEnableDebugDrawIKTargets(bool))); addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKConstraints, 0, false, avatar.get(), SLOT(setEnableDebugDrawIKConstraints(bool))); + addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKChains, 0, false, + avatar.get(), SLOT(setEnableDebugDrawIKChains(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 b1f69a28d3..b6d72f5446 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -162,6 +162,7 @@ namespace MenuOption { const QString RenderSensorToWorldMatrix = "Show SensorToWorld Matrix"; const QString RenderIKTargets = "Show IK Targets"; const QString RenderIKConstraints = "Show IK Constraints"; + const QString RenderIKChains = "Show IK Chains"; 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 9cf8e7747b..b8d6a4cfa4 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -531,6 +531,7 @@ void MyAvatar::simulate(float deltaTime) { if (_rig) { _rig->setEnableDebugDrawIKTargets(_enableDebugDrawIKTargets); _rig->setEnableDebugDrawIKConstraints(_enableDebugDrawIKConstraints); + _rig->setEnableDebugDrawIKChains(_enableDebugDrawIKChains); } _skeletonModel->simulate(deltaTime); @@ -958,6 +959,10 @@ void MyAvatar::setEnableDebugDrawIKConstraints(bool isEnabled) { _enableDebugDrawIKConstraints = isEnabled; } +void MyAvatar::setEnableDebugDrawIKChains(bool isEnabled) { + _enableDebugDrawIKChains = 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 394d6b2ac7..95643f3bd4 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -524,6 +524,7 @@ public slots: void setEnableDebugDrawSensorToWorldMatrix(bool isEnabled); void setEnableDebugDrawIKTargets(bool isEnabled); void setEnableDebugDrawIKConstraints(bool isEnabled); + void setEnableDebugDrawIKChains(bool isEnabled); bool getEnableMeshVisible() const { return _skeletonModel->isVisible(); } void setEnableMeshVisible(bool isEnabled); void setUseAnimPreAndPostRotations(bool isEnabled); @@ -718,6 +719,7 @@ private: bool _enableDebugDrawSensorToWorldMatrix { false }; bool _enableDebugDrawIKTargets { false }; bool _enableDebugDrawIKConstraints { false }; + bool _enableDebugDrawIKChains { false }; AudioListenerMode _audioListenerMode; glm::vec3 _customListenPosition; diff --git a/libraries/animation/src/AnimContext.cpp b/libraries/animation/src/AnimContext.cpp index 70ca3764b0..c8efd83318 100644 --- a/libraries/animation/src/AnimContext.cpp +++ b/libraries/animation/src/AnimContext.cpp @@ -10,10 +10,11 @@ #include "AnimContext.h" -AnimContext::AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints, +AnimContext::AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints, bool enableDebugDrawIKChains, const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix) : _enableDebugDrawIKTargets(enableDebugDrawIKTargets), _enableDebugDrawIKConstraints(enableDebugDrawIKConstraints), + _enableDebugDrawIKChains(enableDebugDrawIKChains), _geometryToRigMatrix(geometryToRigMatrix), _rigToWorldMatrix(rigToWorldMatrix) { diff --git a/libraries/animation/src/AnimContext.h b/libraries/animation/src/AnimContext.h index f68535005c..e8bf5c34eb 100644 --- a/libraries/animation/src/AnimContext.h +++ b/libraries/animation/src/AnimContext.h @@ -16,18 +16,20 @@ class AnimContext { public: - AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints, + AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints, bool enableDebugDrawIKChains, const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix); bool getEnableDebugDrawIKTargets() const { return _enableDebugDrawIKTargets; } bool getEnableDebugDrawIKConstraints() const { return _enableDebugDrawIKConstraints; } + bool getEnableDebugDrawIKChains() const { return _enableDebugDrawIKChains; } const glm::mat4& getGeometryToRigMatrix() const { return _geometryToRigMatrix; } const glm::mat4& getRigToWorldMatrix() const { return _rigToWorldMatrix; } protected: bool _enableDebugDrawIKTargets { false }; - bool _enableDebugDrawIKConstraints{ false }; + bool _enableDebugDrawIKConstraints { false }; + bool _enableDebugDrawIKChains { false }; glm::mat4 _geometryToRigMatrix; glm::mat4 _rigToWorldMatrix; }; diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 70c47c6609..d613e42866 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -193,9 +193,10 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const AnimContext& while (maxError > MAX_ERROR_TOLERANCE && numLoops < MAX_IK_LOOPS) { ++numLoops; + bool debug = context.getEnableDebugDrawIKChains() && numLoops == MAX_IK_LOOPS; + // solve all targets for (auto& target: targets) { - bool debug = false; solveTargetWithCCD(context, target, absolutePoses, debug); } @@ -292,15 +293,11 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const // rotate tip toward target orientation glm::quat deltaRot = target.getRotation() * glm::inverse(tipOrientation); - // decompose deltaRot into axis angle - glm::vec3 axis = glm::axis(deltaRot); - float angle = glm::angle(deltaRot); - - // apply flexCoefficent and re-compose quat - glm::quat deltaRotation = glm::angleAxis(angle * target.getFlexCoefficient(chainDepth), axis); + deltaRot *= target.getFlexCoefficient(chainDepth); + glm::normalize(deltaRot); // compute parent relative rotation - glm::quat tipRelativeRotation = glm::inverse(tipParentOrientation) * deltaRotation * tipOrientation; + glm::quat tipRelativeRotation = glm::inverse(tipParentOrientation) * deltaRot * tipOrientation; // then enforce tip's constraint RotationConstraint* constraint = getConstraint(tipIndex); @@ -680,8 +677,8 @@ static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float l float dTheta = TWO_PI / NUM_SUBDIVISIONS; float theta = 0.0f; for (int i = 0; i < NUM_SUBDIVISIONS; i++) { - float theta_prime = atanf((lateralSwingPhi / anteriorSwingPhi) * tanf(theta)); - float phi = (cosf(2.0f * theta_prime) * ((lateralSwingPhi - anteriorSwingPhi) / 2.0f)) + ((lateralSwingPhi + anteriorSwingPhi) / 2.0f); + float theta_prime = atanf((anteriorSwingPhi / lateralSwingPhi) * tanf(theta)); + float phi = (cosf(2.0f * theta_prime) * ((anteriorSwingPhi - lateralSwingPhi) / 2.0f)) + ((anteriorSwingPhi + lateralSwingPhi) / 2.0f); minDots.push_back(cosf(phi)); theta += dTheta; } @@ -793,27 +790,36 @@ 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), 1.0f, sinf(theta))); // posterior theta += deltaTheta; - swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.0f, 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.25f, sinf(theta))); // posterior + 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))); + swungDirections.push_back(glm::vec3(mirror * cosf(theta), -1.5f, sinf(theta))); theta += deltaTheta; - swungDirections.push_back(glm::vec3(mirror * cosf(theta), -0.25f, sinf(theta))); + swungDirections.push_back(glm::vec3(mirror * cosf(theta), -3.0f, 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), -1.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.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))); 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(minDots); + + /* + // simple cone + std::vector minDots; + const float MAX_HAND_SWING = 2.9f; // 170 deg //2 * PI / 3.0f; + minDots.push_back(cosf(MAX_HAND_SWING)); + stConstraint->setSwingLimits(minDots); + */ + constraint = static_cast(stConstraint); } else if (0 == baseName.compare("Hand", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); @@ -861,7 +867,7 @@ void AnimInverseKinematics::initConstraints() { } else if (baseName.startsWith("Shoulder", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - const float MAX_SHOULDER_TWIST = PI / 20.0f; + const float MAX_SHOULDER_TWIST = PI / 10.0f; stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST); std::vector minDots; @@ -895,8 +901,8 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST); // limit lateral swings more then forward-backward swings - const float MAX_NECK_LATERAL_SWING = PI / 8.0f; - const float MAX_NECK_ANTERIOR_SWING = PI / 6.0f; + const float MAX_NECK_LATERAL_SWING = PI / 12.0f; + const float MAX_NECK_ANTERIOR_SWING = PI / 10.0f; setEllipticalSwingLimits(stConstraint, MAX_NECK_LATERAL_SWING, MAX_NECK_ANTERIOR_SWING); constraint = static_cast(stConstraint); @@ -906,10 +912,10 @@ void AnimInverseKinematics::initConstraints() { const float MAX_HEAD_TWIST = PI / 6.0f; stConstraint->setTwistLimits(-MAX_HEAD_TWIST, MAX_HEAD_TWIST); - std::vector minDots; - const float MAX_HEAD_SWING = PI / 6.0f; - minDots.push_back(cosf(MAX_HEAD_SWING)); - stConstraint->setSwingLimits(minDots); + // limit lateral swings more then forward-backward swings + const float MAX_NECK_LATERAL_SWING = PI / 4.0f; + const float MAX_NECK_ANTERIOR_SWING = PI / 3.0f; + setEllipticalSwingLimits(stConstraint, MAX_NECK_LATERAL_SWING, MAX_NECK_ANTERIOR_SWING); constraint = static_cast(stConstraint); } else if (0 == baseName.compare("ForeArm", Qt::CaseSensitive)) { @@ -1066,7 +1072,7 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele 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)); + return glm::vec3(sin_phi * cosf(theta), cos_phi, sin_phi * sinf(theta)); } void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) const { @@ -1167,20 +1173,12 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con 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 AXIS_LENGTH = 5.0f; // cm const float TWIST_LENGTH = 4.0f; // cm - const float HINGE_LENGTH = 6.0f; // cm - const float SWING_LENGTH = 5.0f; // cm + const float HINGE_LENGTH = 4.0f; // cm + const float SWING_LENGTH = 4.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(); - } - } + AnimPoseVec poses = _relativePoses; // convert relative poses to absolute _skeleton->convertRelativePosesToAbsolute(poses); @@ -1238,8 +1236,8 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con 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); + glm::quat minRot = glm::angleAxis(swingTwistConstraint->getMinTwist(), refRot * Vectors::UNIT_Y); + glm::quat maxRot = glm::angleAxis(swingTwistConstraint->getMaxTwist(), refRot * Vectors::UNIT_Y); const int NUM_SWING_STEPS = 10; for (int i = 0; i < NUM_SWING_STEPS + 1; i++) { @@ -1251,17 +1249,18 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con // draw swing constraints. const size_t NUM_MIN_DOTS = swingTwistConstraint->getMinDots().size(); const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1); + const float PI_2 = PI / 2.0f; 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 swungAxis = sphericalToCartesian(phi, theta - PI_2); 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 prevSwungAxis = sphericalToCartesian(prevPhi, prevTheta - PI_2); glm::vec3 prevWorldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * prevSwungAxis); glm::vec3 prevSwingTip = pos + SWING_LENGTH * prevWorldSwungAxis; @@ -1270,7 +1269,6 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con } } } - pose.rot() = constraint->computeCenterRotation(); } } } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 23db05eb73..fe1a3bfab2 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -954,7 +954,7 @@ void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, cons updateAnimationStateHandlers(); _animVars.setRigToGeometryTransform(_rigToGeometryTransform); - AnimContext context(_enableDebugDrawIKTargets, _enableDebugDrawIKConstraints, + AnimContext context(_enableDebugDrawIKTargets, _enableDebugDrawIKConstraints, _enableDebugDrawIKChains, getGeometryToRigTransform(), rigToWorldTransform); // evaluate the animation @@ -1452,7 +1452,7 @@ void Rig::computeAvatarBoundingCapsule( // call overlay twice: once to verify AnimPoseVec joints and again to do the IK AnimNode::Triggers triggersOut; - AnimContext context(false, false, glm::mat4(), glm::mat4()); + AnimContext context(false, 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 33b66f91ea..b5ff172f22 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -232,6 +232,7 @@ public: void setEnableDebugDrawIKTargets(bool enableDebugDrawIKTargets) { _enableDebugDrawIKTargets = enableDebugDrawIKTargets; } void setEnableDebugDrawIKConstraints(bool enableDebugDrawIKConstraints) { _enableDebugDrawIKConstraints = enableDebugDrawIKConstraints; } + void setEnableDebugDrawIKChains(bool enableDebugDrawIKChains) { _enableDebugDrawIKChains = enableDebugDrawIKChains; } // input assumed to be in rig space void computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const; @@ -343,6 +344,7 @@ protected: bool _enableDebugDrawIKTargets { false }; bool _enableDebugDrawIKConstraints { false }; + bool _enableDebugDrawIKChains { false }; private: QMap _stateHandlers;