From abe19310da55400b615f9d36284fb47eb3a0d988 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 17 May 2017 18:33:49 -0700 Subject: [PATCH] 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; }