From 7af93f9fea98ac0538d414adc4a9fa88ff724043 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 4 May 2017 17:16:22 -0700 Subject: [PATCH] Hooked up IK constraint rendering --- interface/src/Menu.cpp | 2 + interface/src/Menu.h | 1 + interface/src/avatar/MyAvatar.cpp | 5 + interface/src/avatar/MyAvatar.h | 2 + libraries/animation/src/AnimContext.cpp | 4 +- libraries/animation/src/AnimContext.h | 5 +- .../animation/src/AnimInverseKinematics.cpp | 102 ++++++++++-------- libraries/animation/src/Rig.cpp | 5 +- libraries/animation/src/Rig.h | 2 + .../animation/src/SwingTwistConstraint.cpp | 15 +-- 10 files changed, 89 insertions(+), 54 deletions(-) diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 9688694287..560fe7582d 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 250d2241ac..1231e0c72d 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 526b80e3f1..368b403910 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -505,6 +505,7 @@ void MyAvatar::simulate(float deltaTime) { if (_rig) { _rig->setEnableDebugDrawIKTargets(_enableDebugDrawIKTargets); + _rig->setEnableDebugDrawIKConstraints(_enableDebugDrawIKConstraints); } _skeletonModel->simulate(deltaTime); @@ -930,6 +931,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 7c510f0556..5a7bd7c79c 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -518,6 +518,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); @@ -703,6 +704,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 c59c75b191..70ca3764b0 100644 --- a/libraries/animation/src/AnimContext.cpp +++ b/libraries/animation/src/AnimContext.cpp @@ -10,8 +10,10 @@ #include "AnimContext.h" -AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix) : +AnimContext::AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints, + const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix) : _enableDebugDrawIKTargets(enableDebugDrawIKTargets), + _enableDebugDrawIKConstraints(enableDebugDrawIKConstraints), _geometryToRigMatrix(geometryToRigMatrix), _rigToWorldMatrix(rigToWorldMatrix) { diff --git a/libraries/animation/src/AnimContext.h b/libraries/animation/src/AnimContext.h index 067e64026a..f68535005c 100644 --- a/libraries/animation/src/AnimContext.h +++ b/libraries/animation/src/AnimContext.h @@ -16,15 +16,18 @@ class AnimContext { public: - AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix); + 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; }; diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index ae2ca94d66..5dcbfcafad 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -399,7 +399,9 @@ 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) { - debugDrawConstraints(context); + 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) { @@ -624,17 +626,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); @@ -642,7 +646,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). @@ -746,30 +749,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(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(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(cosf(theta), 0.25f, sinf(theta))); // posterior theta += deltaTheta; - swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.0f, sinf(theta))); + swungDirections.push_back(glm::vec3(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(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(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(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(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 (int 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(); @@ -1006,8 +1006,13 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele } } -void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) const { +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); @@ -1020,13 +1025,26 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con const float TWIST_LENGTH = 4.0f; // cm const float HINGE_LENGTH = 6.0f; // cm const float SWING_LENGTH = 5.0f; // cm - AnimPoseVec absPoses = /*_limitCenterPoses;*/ _skeleton->getRelativeDefaultPoses(); - _skeleton->convertRelativePosesToAbsolute(absPoses); + + AnimPoseVec poses = _skeleton->getRelativeDefaultPoses(); + + // copy reference rotations into the relative poses + for (int i = 0; i < 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(); - for (int i = 0; i < absPoses.size(); i++) { + + // draw each pose and constraint + for (int i = 0; i < poses.size(); i++) { // transform local axes into world space. - auto pose = absPoses[i]; + 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); @@ -1038,13 +1056,13 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con // draw line to parent int parentIndex = _skeleton->getParentIndex(i); if (parentIndex != -1) { - glm::vec3 parentPos = transformPoint(geomToWorldMatrix, absPoses[parentIndex].trans()); + glm::vec3 parentPos = transformPoint(geomToWorldMatrix, poses[parentIndex].trans()); DebugDraw::getInstance().drawRay(pos, parentPos, GRAY); } glm::quat parentAbsRot; if (parentIndex != -1) { - parentAbsRot = absPoses[parentIndex].rot(); + parentAbsRot = poses[parentIndex].rot(); } const RotationConstraint* constraint = getConstraint(i); @@ -1091,26 +1109,24 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con } // draw swing constraints. - glm::vec3 previousSwingTip; const size_t NUM_MIN_DOTS = swingTwistConstraint->getMinDots().size(); - const float D_THETA = TWO_PI / NUM_MIN_DOTS; + const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1); float theta = 0.0f; - for (size_t i = 0; i < NUM_MIN_DOTS; i++, theta += D_THETA) { + 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 = acos(swingTwistConstraint->getMinDots()[i]); - float cos_phi = swingTwistConstraint->getMinDots()[i]; - float sin_phi = sinf(phi); - glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, sin_phi * sinf(theta)); + 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; - DebugDraw::getInstance().drawRay(pos, swingTip, PURPLE); - if (previousSwingTipValid) { - DebugDraw::getInstance().drawRay(previousSwingTip, swingTip, PURPLE); - } - previousSwingTip = swingTip; - previousSwingTipValid = true; + 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); } } } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 2689fe5be8..2bcd71d5c3 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -954,7 +954,8 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform, glm::mat4 r updateAnimationStateHandlers(); _animVars.setRigToGeometryTransform(_rigToGeometryTransform); - AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform(), rigToWorldTransform); + AnimContext context(_enableDebugDrawIKTargets, _enableDebugDrawIKConstraints, + getGeometryToRigTransform(), rigToWorldTransform); // evaluate the animation AnimNode::Triggers triggersOut; @@ -1445,7 +1446,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(), 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 f8ae0bdfae..396e68d633 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -231,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; @@ -341,6 +342,7 @@ protected: float _maxHipsOffsetLength { 1.0f }; bool _enableDebugDrawIKTargets { false }; + bool _enableDebugDrawIKConstraints { false }; private: QMap _stateHandlers; diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index 475ee6a59e..aba0516b2a 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -443,19 +443,20 @@ glm::quat SwingTwistConstraint::computeCenterRotation() const { twistLimits[0] = glm::angleAxis(_minTwist, _referenceRotation * Vectors::UNIT_Y); twistLimits[1] = glm::angleAxis(_maxTwist, _referenceRotation * Vectors::UNIT_Y); } - const float D_THETA = TWO_PI / NUM_MIN_DOTS; + const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1); float theta = 0.0f; - for (size_t i = 0; i < NUM_MIN_DOTS; i++, theta += D_THETA) { + 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)); - glm::quat swing = glm::angleAxis(phi, glm::normalize(glm::cross(Vectors::UNIT_Y, swungAxis))); + + // 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 limits[2]; - limits[0] = averageQuats(swingLimits.size(), &swingLimits[0]); - limits[1] = averageQuats(2, twistLimits); - return averageQuats(2, limits); + glm::quat averageSwing = averageQuats(swingLimits.size(), &swingLimits[0]); + glm::quat averageTwist = averageQuats(2, twistLimits); + return averageSwing * averageTwist * _referenceRotation; }