From abe19310da55400b615f9d36284fb47eb3a0d988 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 17 May 2017 18:33:49 -0700 Subject: [PATCH 1/5] WIP: checkpoint of ik changes --- .../resources/avatar/avatar-animation.json | 6 +- .../animation/src/AnimInverseKinematics.cpp | 165 ++++++++++++++---- .../animation/src/AnimInverseKinematics.h | 13 +- 3 files changed, 146 insertions(+), 38 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index b18599d8a9..56d761ed53 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -68,7 +68,7 @@ "typeVar": "rightHandType", "weightVar": "rightHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.25, 0.1, 0.05, 0.01, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.00, 0.00, 0.00, 0.0, 0.0] }, { "jointName": "LeftHand", @@ -77,7 +77,7 @@ "typeVar": "leftHandType", "weightVar": "leftHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.25, 0.1, 0.05, 0.01, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0] }, { "jointName": "RightFoot", @@ -113,7 +113,7 @@ "typeVar": "headType", "weightVar": "headWeight", "weight": 4.0, - "flexCoefficients": [1, 0.5, 0.5, 0.5, 0.5] + "flexCoefficients": [1, 0.35, 0.5, 0.35, 0.25] } ] }, diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 4471f11857..5ef1a168e7 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -175,7 +175,7 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: } } -void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector& targets) { +void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const AnimContext& context, const std::vector& targets) { // compute absolute poses that correspond to relative target poses AnimPoseVec absolutePoses; absolutePoses.resize(_relativePoses.size()); @@ -194,24 +194,21 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector 0) { _relativePoses[i].rot() = _accumulators[i].getAverage(); _accumulators[i].clear(); } } - // update the absolutePoses that need it (from lowestMovedIndex to _maxTargetIndex) - for (auto i = lowestMovedIndex; i <= _maxTargetIndex; ++i) { + // update the absolutePoses + for (int i = 0; i < (int)_relativePoses.size(); ++i) { auto parentIndex = _skeleton->getParentIndex((int)i); if (parentIndex != -1) { absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i]; @@ -236,7 +233,9 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vectorgetParentIndex(tipIndex); - if (parentIndex != -1) { + + // update rotationOnly targets that don't lie on the ik chain of other ik targets. + if (parentIndex != -1 && !_accumulators[tipIndex].isDirty() && target.getType() == IKTarget::Type::RotationOnly) { const glm::quat& targetRotation = target.getRotation(); // compute tip's new parent-relative rotation // Q = Qp * q --> q' = Qp^ * Q @@ -254,24 +253,23 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vectorgetParentIndex(tipIndex); if (pivotIndex == -1 || pivotIndex == _hipsIndex) { - return lowestMovedIndex; + return; } int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex); if (pivotsParentIndex == -1) { // TODO?: handle case where tip's parent is root? - return lowestMovedIndex; + return; } // cache tip's absolute orientation @@ -281,9 +279,13 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // the tip's parent-relative as we proceed up the chain glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot(); + std::map debugJointMap; + // NOTE: if this code is removed, the head will remain rigid, causing the spine/hips to thrust forward backward // as the head is nodded. - if (targetType == IKTarget::Type::HmdHead) { + if (targetType == IKTarget::Type::HmdHead || + targetType == IKTarget::Type::RotationAndPosition || + targetType == IKTarget::Type::HipsRelativeRotationAndPosition) { // rotate tip directly to target orientation tipOrientation = target.getRotation(); @@ -291,8 +293,9 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // then enforce tip's constraint RotationConstraint* constraint = getConstraint(tipIndex); + bool constrained = false; if (constraint) { - bool constrained = constraint->apply(tipRelativeRotation); + constrained = constraint->apply(tipRelativeRotation); if (constrained) { tipOrientation = tipParentOrientation * tipRelativeRotation; tipRelativeRotation = tipRelativeRotation; @@ -300,6 +303,10 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe } // store the relative rotation change in the accumulator _accumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); + + if (debug) { + debugJointMap[tipIndex] = DebugJoint(tipRelativeRotation, constrained); + } } // cache tip absolute position @@ -388,15 +395,15 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // compute joint's new parent-relative rotation after swing // Q' = dQ * Q and Q = Qp * q --> q' = Qp^ * dQ * Q - glm::quat newRot = glm::normalize(glm::inverse( - absolutePoses[pivotsParentIndex].rot()) * - deltaRotation * - absolutePoses[pivotIndex].rot()); + glm::quat newRot = glm::normalize(glm::inverse(absolutePoses[pivotsParentIndex].rot()) * + deltaRotation * + absolutePoses[pivotIndex].rot()); // enforce pivot's constraint RotationConstraint* constraint = getConstraint(pivotIndex); + bool constrained = false; if (constraint) { - bool constrained = constraint->apply(newRot); + constrained = constraint->apply(newRot); if (constrained) { // the constraint will modify the local rotation of the tip so we must // compute the corresponding model-frame deltaRotation @@ -408,9 +415,8 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // store the relative rotation change in the accumulator _accumulators[pivotIndex].add(newRot, target.getWeight()); - // this joint has been changed so we check to see if it has the lowest index - if (pivotIndex < lowestMovedIndex) { - lowestMovedIndex = pivotIndex; + if (debug) { + debugJointMap[pivotIndex] = DebugJoint(newRot, constrained); } // keep track of tip's new transform as we descend towards root @@ -423,7 +429,10 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe chainDepth++; } - return lowestMovedIndex; + + if (debug) { + debugDrawIKChain(debugJointMap, context); + } } //virtual @@ -551,7 +560,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); - solveWithCyclicCoordinateDescent(targets); + solveWithCyclicCoordinateDescent(context, targets); } if (_hipsTargetIndex < 0) { @@ -562,6 +571,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars } } } + return _relativePoses; } @@ -843,7 +853,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST); std::vector minDots; - const float MAX_SHOULDER_SWING = PI / 16.0f; + const float MAX_SHOULDER_SWING = PI / 12.0f; minDots.push_back(cosf(MAX_SHOULDER_SWING)); stConstraint->setSwingLimits(minDots); @@ -855,8 +865,8 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST); // limit lateral swings more then forward-backward swings - const float MAX_SPINE_LATERAL_SWING = PI / 30.0f; - const float MAX_SPINE_ANTERIOR_SWING = PI / 20.0f; + const float MAX_SPINE_LATERAL_SWING = PI / 15.0f; + const float MAX_SPINE_ANTERIOR_SWING = PI / 10.0f; setEllipticalSwingLimits(stConstraint, MAX_SPINE_LATERAL_SWING, MAX_SPINE_ANTERIOR_SWING); if (0 == baseName.compare("Spine1", Qt::CaseSensitive) @@ -869,12 +879,12 @@ void AnimInverseKinematics::initConstraints() { } else if (0 == baseName.compare("Neck", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - const float MAX_NECK_TWIST = PI / 10.0f; + const float MAX_NECK_TWIST = PI / 8.0f; stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST); // limit lateral swings more then forward-backward swings - const float MAX_NECK_LATERAL_SWING = PI / 10.0f; - const float MAX_NECK_ANTERIOR_SWING = PI / 8.0f; + const float MAX_NECK_LATERAL_SWING = PI / 8.0f; + const float MAX_NECK_ANTERIOR_SWING = PI / 6.0f; setEllipticalSwingLimits(stConstraint, MAX_NECK_LATERAL_SWING, MAX_NECK_ANTERIOR_SWING); constraint = static_cast(stConstraint); @@ -1047,6 +1057,95 @@ static glm::vec3 sphericalToCartesian(float phi, float theta) { return glm::vec3(sin_phi * cosf(theta), cos_phi, -sin_phi * sinf(theta)); } +void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) const { + AnimPoseVec poses = _relativePoses; + + // convert relative poses to absolute + _skeleton->convertRelativePosesToAbsolute(poses); + + mat4 geomToWorldMatrix = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix(); + + 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 GRAY(0.2f, 0.2f, 0.2f, 1.0f); + const float AXIS_LENGTH = 2.0f; // cm + + // draw each pose + 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); + } + } +} + +void AnimInverseKinematics::debugDrawIKChain(std::map& debugJointMap, const AnimContext& context) const { + AnimPoseVec poses = _relativePoses; + + // copy debug joint rotations into the relative poses + for (auto& debugJoint : debugJointMap) { + poses[debugJoint.first].rot() = debugJoint.second.relRot; + } + + // convert relative poses to absolute + _skeleton->convertRelativePosesToAbsolute(poses); + + mat4 geomToWorldMatrix = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix(); + + 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 GRAY(0.2f, 0.2f, 0.2f, 1.0f); + const float AXIS_LENGTH = 2.0f; // cm + + // draw each pose + for (int i = 0; i < (int)poses.size(); i++) { + + // only draw joints that are actually in debugJointMap, or their parents + auto iter = debugJointMap.find(i); + auto parentIter = debugJointMap.find(_skeleton->getParentIndex(i)); + if (iter != debugJointMap.end() || parentIter != debugJointMap.end()) { + + // 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()); + glm::vec4 color = GRAY; + + // draw constrained joints with a RED link to their parent. + if (parentIter != debugJointMap.end() && parentIter->second.constrained) { + color = RED; + } + DebugDraw::getInstance().drawRay(pos, parentPos, color); + } + } + } +} + void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) const { if (_skeleton) { const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 74face6d0b..0267f14650 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -58,13 +58,22 @@ public: 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); + void solveWithCyclicCoordinateDescent(const AnimContext& context, const std::vector& targets); + void solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug); virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override; + struct DebugJoint { + DebugJoint() : relRot(), constrained(false) {} + DebugJoint(const glm::quat& relRotIn, bool constrainedIn) : relRot(relRotIn), constrained(constrainedIn) {} + glm::quat relRot; + bool constrained; + }; + void debugDrawIKChain(std::map& debugJointMap, const AnimContext& context) const; + void debugDrawRelativePoses(const AnimContext& context) const; 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; } From 87adeb0565d7133cd506cfb6ca88c09e11d2b389 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 18 May 2017 11:24:30 -0700 Subject: [PATCH 2/5] apply flexCoeff to tip of joint chain as well. --- .../resources/avatar/avatar-animation.json | 6 ++--- .../animation/src/AnimInverseKinematics.cpp | 22 ++++++++++++++----- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 56d761ed53..55f86f7f44 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -68,7 +68,7 @@ "typeVar": "rightHandType", "weightVar": "rightHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.00, 0.00, 0.00, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.25, 0.1, 0.05, 0.01, 0.0, 0.0] }, { "jointName": "LeftHand", @@ -77,7 +77,7 @@ "typeVar": "leftHandType", "weightVar": "leftHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.00, 0.00, 0.00, 0.0, 0.0] }, { "jointName": "RightFoot", @@ -104,7 +104,7 @@ "typeVar": "spine2Type", "weightVar": "spine2Weight", "weight": 1.0, - "flexCoefficients": [0.45, 0.45] + "flexCoefficients": [1.0, 0.5, 0.5] }, { "jointName": "Head", diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 5ef1a168e7..70c47c6609 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -195,7 +195,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const AnimContext& // solve all targets for (auto& target: targets) { - bool debug = numLoops == MAX_IK_LOOPS; + bool debug = false; solveTargetWithCCD(context, target, absolutePoses, debug); } @@ -254,6 +254,8 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const AnimContext& } void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug) { + size_t chainDepth = 0; + IKTarget::Type targetType = target.getType(); if (targetType == IKTarget::Type::RotationOnly) { // the final rotation will be enforced after the iterations @@ -287,9 +289,18 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const targetType == IKTarget::Type::RotationAndPosition || targetType == IKTarget::Type::HipsRelativeRotationAndPosition) { - // rotate tip directly to target orientation - tipOrientation = target.getRotation(); - glm::quat tipRelativeRotation = glm::inverse(tipParentOrientation) * tipOrientation; + // 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); + + // compute parent relative rotation + glm::quat tipRelativeRotation = glm::inverse(tipParentOrientation) * deltaRotation * tipOrientation; // then enforce tip's constraint RotationConstraint* constraint = getConstraint(tipIndex); @@ -301,6 +312,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const tipRelativeRotation = tipRelativeRotation; } } + // store the relative rotation change in the accumulator _accumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); @@ -312,7 +324,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const // cache tip absolute position glm::vec3 tipPosition = absolutePoses[tipIndex].trans(); - size_t chainDepth = 1; + chainDepth++; // descend toward root, pivoting each joint to get tip closer to target position while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) { From e99001d86c624b2294d2c0d7a69f80544665708a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 18 May 2017 16:29:55 -0700 Subject: [PATCH 3/5] debug draw ik chains, bug fix for flex coef on tips And opened up hip constraints. --- .../resources/avatar/avatar-animation.json | 2 +- 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 | 3 +- libraries/animation/src/AnimContext.h | 6 +- .../animation/src/AnimInverseKinematics.cpp | 84 +++++++++---------- libraries/animation/src/Rig.cpp | 4 +- libraries/animation/src/Rig.h | 2 + 10 files changed, 62 insertions(+), 49 deletions(-) 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; From 621d94eb91fbfd75da646d0a8e33ecbba7d207fa Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 19 May 2017 10:38:16 -0700 Subject: [PATCH 4/5] removed temp comments --- libraries/animation/src/IKTarget.cpp | 1 - libraries/animation/src/Rig.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/libraries/animation/src/IKTarget.cpp b/libraries/animation/src/IKTarget.cpp index c67c0621c3..2fe767b08d 100644 --- a/libraries/animation/src/IKTarget.cpp +++ b/libraries/animation/src/IKTarget.cpp @@ -23,7 +23,6 @@ void IKTarget::setFlexCoefficients(size_t numFlexCoefficientsIn, const float* fl float IKTarget::getFlexCoefficient(size_t chainDepth) const { const float DEFAULT_FLEX_COEFFICIENT = 0.5f; - if (chainDepth < _numFlexCoefficients) { return _flexCoefficients[chainDepth]; } else { diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index fe1a3bfab2..99d2deb323 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1403,7 +1403,6 @@ void Rig::computeAvatarBoundingCapsule( AnimInverseKinematics ikNode("boundingShape"); ikNode.setSkeleton(_animSkeleton); - // AJT: FIX ME!!!!! ensure that empty weights vector does something reasonable.... ikNode.setTargetVars("LeftHand", "leftHandPosition", "leftHandRotation", From 6ae2e6778e33dd1b27ac9797659b0dcbd3798c92 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 19 May 2017 10:51:09 -0700 Subject: [PATCH 5/5] ensure left and right arm flexCoeff are the same. --- interface/resources/avatar/avatar-animation.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index b0d27ee04b..f88e322d99 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -68,7 +68,7 @@ "typeVar": "rightHandType", "weightVar": "rightHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.25, 0.1, 0.05, 0.01, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0] }, { "jointName": "LeftHand", @@ -77,7 +77,7 @@ "typeVar": "leftHandType", "weightVar": "leftHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.00, 0.00, 0.00, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0] }, { "jointName": "RightFoot",